python 路径优化（1）多项式曲线优化

• python 路径多项式曲线优化
• 代码
• 测试效果

通过多项式曲线优化路线，让原本曲率不连续的路径变得曲率连续
参考大佬

优点
曲率连续
（代码简单）

缺点
无法固定起点终点的曲率
无法控制最大曲率
在路径规划中无法控制优化出来的路径是否与障碍物碰撞
需要控制多项式次数来调整曲线

import math

import matplotlib.pyplot as plt
import numpy as np

class PolynomialSmooth():
    def fit(self,route_x,route_y,polynomial_times=3):
        z1 = np.polyfit(route_x,route_y,polynomial_times) # 用polynomial_times次多项式拟合
        p1 = np.poly1d(z1)
        return p1

    def smooth(self,route_x,route_y,route_theta=None,polynomial_times=3):
        p1=self.fit(route_x, route_y,polynomial_times=polynomial_times)
        new_route_y=p1(route_x)
        theta_p=p1.deriv(1)
        dy=theta_p(route_x)
        new_route_theta=[]
        for i in dy:
            new_route_theta.append(math.atan2(i,1))
        for i in range(len(route_x)):
            print(route_x[i],new_route_y[i],new_route_theta[i]*180/math.pi)
        return route_x,new_route_y,new_route_theta

from polynomial_smooth import PolynomialSmooth
import matplotlib.pyplot as plt
import numpy as np

def new_test():

    x = np.arange(1, 17, 1)
    y = np.array([4.00, 6.40, 8.00, 8.80, 9.22, 9.50, 9.70, 9.86, 10.00, 10.20, 10.32, 10.42, 10.50, 10.55, 10.58, 10.60])
    poly=PolynomialSmooth()

    x,yvals,theta=poly.smooth(x,y,theta,4)


    plot1=plt.plot(x, y, '*',label='original values')
    plot2=plt.plot(x, yvals, '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()