Python奇异曲线拟合机器人路径

背景

在机器人路径规划中，有时需要对规划出的曲线进行拟合，通过这种方式增加机器人行驶的平滑性。

常规的曲线拟合方式一般是简单的基于最小二乘法的多项式拟合，三角函数拟合等，还有比较复杂的B样条，三次样条等方法。这些方法对于一般的曲线都可以完成比较好的拟合效果。但是对于一些不规则的，比如一个x对应多个y的波浪形曲线效果并不很好。这时候，需要将曲线分段进行拟合，会比较麻烦。

我在应用中使用的是一种较为简单的方法，即将原本拟合出来应为

的函数分为

来分别进行拟合，最终使用多个相同的t值带入分别计算路径点(x,y)，并将其拼成完整路径。

方法

假设现在需要拟合如下路径

 图中红色点为路径点，黑色曲线为期望拟合出的曲线。

这种曲线就可以将x,y分开拟合，首先，需要将路径点的x和y值分别提取出来：

for i in range(len(path.points)):
    p_x[i] = path.points[i].x
    p_y[i] = path.points[i].y

然后将p_x,p_y中的值都对应到相应的t值，这里需要注意t代表的是点的次序而不是具体的位置，我们可以简单的按数量设置t值，比如要拟合五个点: t = [0, 0.25, 0.5, 0.75, 1]即可。

t_list = np.linspace(0,1,len(phase_x))

之后可以直接使用numpy中自带的曲线拟合工具进行拟合，获得拟合后的曲线后，可以增加t值数量来生成平滑的曲线。

ref_x = np.poly1d(np.polyfit(t_list.flatten(),phase_x.flatten(),3))
ref_y = np.poly1d(np.polyfit(t_list.flatten(),phase_y.flatten(),3))
for i in range(1,len(t_list)):
    t_value.extend(np.linspace(t_list[i-1],t_list[i],4, endpoint=False))
t_value.extend([1])
x_value = ref_x(t_value)
y_value = ref_y(t_value)
for i in range(len(x_value)):
    fit_path.append(Point(x_value[i], y_value[i], 0))