python 路径平滑（3）贝塞尔曲线优化

python 路径平滑（3）贝塞尔曲线优化

python 路径优化（1）多项式曲线优化
python 路径平滑（2）指定函数曲线优化
python 路径平滑（3）贝塞尔曲线优化

贝塞尔曲线图示
这篇博文里有贝塞尔曲线的动态绘制方法，辅助图可以帮助理解贝塞尔曲线
大佬代码实现
我接下来的代码基本上是大量参考(chao）大佬的代码

import matplotlib.pyplot as plt
import numpy as np
import math
import copy
class BezierSmooth():
    # 输入控制点，Points是一个array,num是控制点间的插补个数
    def __init__(self,route_x,route_y,route_theta,InterpolationNum=1000):
        Points=[]
        # for i in range(len(route_x)):
        #     Points.append([route_x[i],route_y[i],route_theta[i]])
        for i in range(len(route_x)):
            Points.append([route_x[i],route_y[i]])
        Points=np.array(Points)
        self.demension=Points.shape[1]   # 点的维度
        self.order=Points.shape[0]-1     # 贝塞尔阶数=控制点个数-1
        self.num=InterpolationNum        # 相邻控制点的插补个数
        self.pointsNum=Points.shape[0]   # 控制点的个数
        self.Points=Points

    def smooth(self,method=1):
        ans=self.getBezierPoints(method)
        new_route_x=[]
        new_route_y=[]
        new_route_theta=[]
        if ans.shape[1]==2:
            for x,y in ans:
                new_route_x.append(x)
                new_route_y.append(y)
        if ans.shape[1]==3:
            for x,y,theta in ans:
                new_route_x.append(x)
                new_route_y.append(y)
                new_route_theta.append(theta)
        return new_route_x,new_route_y,new_route_theta

    # 获取Bezeir所有插补点
    def getBezierPoints(self,method):
        if method==0:
            return self.DigitalAlgo()
        if method==1:
            return self.DeCasteljauAlgo()

    # 数值解法
    def DigitalAlgo(self):
        PB=np.zeros((self.pointsNum,self.demension)) # 求和前各项
        pis =[]                                      # 插补点
        for u in np.arange(0,1+1/self.num,1/self.num):
            for i in range(0,self.pointsNum):
                PB[i]=(math.factorial(self.order)/(math.factorial(i)*math.factorial(self.order-i)))*(u**i)*(1-u)**(self.order-i)*self.Points[i]
            pi=sum(PB).tolist()                      #求和得到一个插补点
            pis.append(pi)
        return np.array(pis)

    # 德卡斯特里奥解法
    def DeCasteljauAlgo(self):
        pis =[]                          # 插补点
        for u in np.arange(0,1+1/self.num,1/self.num):
            Att=copy.deepcopy(self.Points)
            for i in np.arange(0,self.order):
                for j in np.arange(0,self.order-i):
                    Att[j]=(1.0-u)*Att[j]+u*Att[j+1]
            pis.append(Att[0].tolist())

        return np.array(pis)

测试效果

from bezier_smooth import BezierSmooth
import matplotlib.pyplot as plt
import numpy as np

def new_test():

    
    bezier_smooth=BezierSmooth(x,y,theta)
    new_route_x,new_route_y,theta=bezier_smooth.smooth()

    new_route_theta=theta

    for i in range(len(new_route_x)):
        print(new_route_x[i],new_route_y[i],new_route_theta[i]*180/math.pi)



    plot1=plt.plot(x, y, '*',label='original values')
    plot2=plt.plot(new_route_x, new_route_y, 'r',label='polyfit values')
    plt.xlabel('x axis')
    plt.ylabel('y axis')
    plt.legend(loc=4) # 指定legend的位置,读者可以自己help它的用法
    plt.title('polyfitting')
    plt.show()





if __name__ == '__main__':
    new_test()