1. 貝塞爾曲線

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

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

????

?1.3 三階表達(dá)式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時(shí)得到的點(diǎn)替代三點(diǎn)中的中間點(diǎn)，從而實(shí)現(xiàn)路徑平滑 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樣條曲線

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

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

def prune(self, x, y, z, k):# n表示曲線的點(diǎn)數(shù)減1，因?yàn)橄聵?biāo)從0開(kāi)始；也表示B樣條的基函數(shù)要計(jì)算到nn = len(x) - 1# u_base表示準(zhǔn)均勻集合的分母u_base = n + 1 - k# U表示準(zhǔn)均勻集合，重復(fù)k階個(gè)起始點(diǎn)和結(jié)束點(diǎn)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是自變量，控制了樣條基函數(shù)，這邊設(shè)置間隔為0.02u = 0.0while u <= 1.0:tmp_x = 0tmp_y = 0tmp_z = 0# 累加各個(gè)原始點(diǎn)和樣條基函數(shù)的乘積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# 計(jì)算完一組，就放入擬合曲線的集合中prune_x.append(tmp_x)prune_y.append(tmp_y)prune_z.append(tmp_z)u += 0.02return prune_x, prune_y, prune_z# 樣條基函數(shù)的遞歸計(jì)算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

擬合結(jié)果如下圖，紅色是原先存在拐點(diǎn)的路徑，藍(lán)色是使用B樣條2階擬合的路徑。

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。