坐标轨迹计算_无人驾驶轨迹拟合和二次轨迹计算

在轨迹重规划算法中，将目标函数设定为有限时域内与参考点的距离偏差最小，所规划出的轨迹是以预测时域内离散的点给出的。随着预测时域的增加、这些局部参考轨迹点的数量也会随之增加。直接将这些点输入控制层，占用了过多的控制器输入接口，不利于控制器的规范化设计。另外，由于规划层与控制层的控制周期并不一致，控制层很难根据离散的参考轨迹点完成轨迹跟踪的任务。

综合上述因素，有必要对轨迹重规划算法所规划出的局部参考路径进行处理，实现规划层与控制层的顺利对接。曲线拟合是对离散点处理的主要方式，根据所采用曲线的不同，有样条曲线拟合、多项式拟合以及幂指数拟合等。由于车辆存在运动学约束，如车辆位置连续要求曲线是连续的，横摆角连续要求曲线是一阶连续，而加速度约束则要求曲线二阶连续。因此，本书选择5次多项式作为拟合曲线，形式如下∶

设规划层预测时域N，=16，拟合结果如图所示。图中，星号为轨迹重规划算法在预测时域内规划出的离散点，曲线为拟合后的轨迹曲线。

以拟合后各数据点残差的范数来表征拟合质量，对整个轨迹重规划过程中的201次规划数据进行统计，统计结果如图所示。从图中可以看出，车辆位置横坐标Y的拟合残差最大值为0.003，车体航向角g的拟合残差最大值不超过7.50×10³，具备非常高的拟合精度。

在用S函数实现基于MPC的轨迹规划器时首先需要确定S函数的输入与输出量，如图所示。基于MPC的轨迹规划器将CarSim的输出作为输入，CaSim输出的参数为：

分别代表车辆在Y和X坐标方向上的速度、航向角及航向角速度、在Y和X坐标轴下的坐标等，共6个参数。基于MPC的轨迹规划器的输出是规划好的期望轨迹方程的参数，因为选择了5次曲线进行曲线拟合，所以这里的输出量为10个。

第1部分为主函数。通过此函数完成程序的初始化，并设置S函数模块的输入、输出和状态变量的个数等。sys变量在不同的阶段表示不同的意义。

function[sys,x0,str,ts]=MPC_TrajPlanner(t,x,u,flag)该程序功能∶用点质量模型设计规划期，能够规避障碍物8程序版本V1.0，Matlab版本∶R201la，采用S函数的标准形式，%状态量=【y_dot，x_dot，phi，，YX】，控制量为前轮偏角ay
switch flag,
case0flag=0表示处于初始化状态，此时用函数mdlIni-
tializesizes进行初始化
[sys,x0,str,ts]=mdlInitializeSizes; Initializa