1. 貝塞爾曲線

1.1 一階表達式B(u) = (1-u)*P0 + u*P1，0 <= u?<= 1。隨著u的增大，插值點慢慢從P0點擬合到P1點。

1.2 二階表達式B(u) = (1-u)^2 * P0 + 2u(1-u) * P1 + u^2 * P2，0 <= u?<= 1。同理，隨著u的增大，插值點慢慢從P0點擬合到P2點。

????

?1.3 三階表達式B(u) = (1-u)^3 * P0 + 3u(1-u)^2 * P1 + 3u^2 * (1-u)P2 + u^3 * P3，0 <= u?<= 1。

# 二階，取u=0.5時得到的點替代三點中的中間點，從而實現路徑平滑 prune_x = [] prune_y = [] prune_z = [] for i in range(len(x)):if(i ==0 or i== len(x)-1):prune_x.append(x[i])prune_y.append(y[i])prune_z.append(z[i])else:prune_x.append(0.25*x[i-1] + 0.5*x[i] + 0.25*x[i+1])prune_y.append(0.25*y[i-1] + 0.5*y[i] + 0.25*y[i+1])prune_z.append(0.25*z[i-1] + 0.5*z[i] + 0.25*z[i+1])

2. B樣條曲線

擬合后的曲線表達式為，其中di表示需要被擬合的曲線坐標點，Ni,k(u)表示B樣條曲線的基函數，遞推公式如下圖；k表示階數；u是個自變量，一般為[0, 1]區間。

其中ui來自于集合U，分為均勻集合(如[0, 1/4, 2/4, 3/4, 1])和準均勻集合(如[0, 0, 0, 1/4, 2/4, 3/4, 1,? 1, 1])，準均勻集合在集合開始和結尾有一定的重復度，重復次數取決于最終采用的階數，如果是2階，那么重復兩次。

def prune(self, x, y, z, k):# n表示曲線的點數減1，因為下標從0開始；也表示B樣條的基函數要計算到nn = len(x) - 1# u_base表示準均勻集合的分母u_base = n + 1 - k# U表示準均勻集合，重復k階個起始點和結束點U = []for i in range(k):U.append(0)u_basic = 1.0/u_basefor i in range(u_base+1):U.append(i * u_basic)for i in range(k):U.append(1)prune_x = []prune_y = []prune_z = []# u是自變量，控制了樣條基函數，這邊設置間隔為0.02u = 0.0while u <= 1.0:tmp_x = 0tmp_y = 0tmp_z = 0# 累加各個原始點和樣條基函數的乘積for i in range(len(x)):Nik_u = self.B_spline(i, u, k, U)tmp_x += x[i] * Nik_utmp_y += y[i] * Nik_utmp_z += z[i] * Nik_u# 計算完一組，就放入擬合曲線的集合中prune_x.append(tmp_x)prune_y.append(tmp_y)prune_z.append(tmp_z)u += 0.02return prune_x, prune_y, prune_z# 樣條基函數的遞歸計算def B_spline(self, i, u, k, U):Nik_u = 0.0if k==0:if u >= U[i] and u < U[i+1]:Nik_u = 1.0else:base1 = U[i+k] - U[i]base2 = U[i+k+1] - U[i+1]if base1 == 0.0:base1 = 1.0if base2 == 0.0:base2 = 1.0Nik_u = (u - U[i]) / base1 * self.B_spline(i, u, k-1, U) + (U[i+k+1] - u) / base2 * self.B_spline(i+1, u, k-1, U)return Nik_u

擬合結果如下圖，紅色是原先存在拐點的路徑，藍色是使用B樣條2階擬合的路徑。

總結

以上是生活随笔為你收集整理的路径规划后对路径进行平滑处理的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。