python 路径优化多项式曲线优化

Posted 怪皮蛇皮怪

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了python 路径优化多项式曲线优化相关的知识,希望对你有一定的参考价值。

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

以上是关于python 路径优化多项式曲线优化的主要内容,如果未能解决你的问题,请参考以下文章

python 路径平滑指定函数曲线优化

python 路径平滑指定函数曲线优化

python 路径平滑指定函数曲线优化

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

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

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