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

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