路径规划后对路径进行平滑处理

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样条的基函数要计算到n
        n = 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_base
        for 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.02
        u = 0.0
        while u <= 1.0:
            tmp_x = 0
            tmp_y = 0
            tmp_z = 0
            # 累加各个原始点和样条基函数的乘积
            for i in range(len(x)):
                Nik_u = self.B_spline(i, u, k, U)
                tmp_x += x[i] * Nik_u
                tmp_y += y[i] * Nik_u
                tmp_z += z[i] * Nik_u
            # 计算完一组，就放入拟合曲线的集合中
            prune_x.append(tmp_x)
            prune_y.append(tmp_y)
            prune_z.append(tmp_z)
            u += 0.02
        return prune_x, prune_y, prune_z


    # 样条基函数的递归计算
    def B_spline(self, i, u, k, U):
        Nik_u = 0.0
        if k==0:
            if u >= U[i] and u < U[i+1]:
                Nik_u = 1.0
        else:
            base1 = U[i+k] - U[i]
            base2 = U[i+k+1] - U[i+1]
            if base1 == 0.0:
                base1 = 1.0
            if base2 == 0.0:
                base2 = 1.0
            Nik_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阶拟合的路径。