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

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

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()