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

Posted 怪皮蛇皮怪

tags:

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

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

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

python 路径指定函数曲线优化

通过指定曲线优化路线,让原本曲率不连续的路径变得曲率连续
参考代码:使用非线性最小二乘法拟合

优点
曲率连续
(代码简单)

缺点
无法固定起点终点的曲率
无法控制最大曲率
在路径规划中无法控制优化出来的路径是否与障碍物碰撞

代码

import math
from scipy.optimize import curve_fit
import numpy as np

def function1(x,a,b):
    return a*np.exp(b/x)


class FunctionSmooth():

    def smooth(self,route_x,route_y,route_theta,func=function1):
        popt, pcov = curve_fit(func, route_x,route_y)
        a=popt[0] # popt里面是拟合系数,读者可以自己help其用法
        b=popt[1]
        yvals=func(route_x,a,b)
        new_route_theta=[]
        for i in route_x:
            dx=0.001
            dy=func(i+dx,a,b)-func(i,a,b)
            new_route_theta.append(math.atan2(dy,dx))
        return route_x,yvals,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 路径优化多项式曲线优化