在轨迹重规划算法中,将目标函数设定为有限时域内与参考点的距离偏差最小,所规划出的轨迹是以预测时域内离散的点给出的。随着预测时域的增加、这些局部参考轨迹点的数量也会随之增加。直接将这些点输入控制层,占用了过多的控制器输入接口,不利于控制器的规范化设计。另外,由于规划层与控制层的控制周期并不一致,控制层很难根据离散的参考轨迹点完成轨迹跟踪的任务。
综合上述因素,有必要对轨迹重规划算法所规划出的局部参考路径进行处理,实现规划层与控制层的顺利对接。曲线拟合是对离散点处理的主要方式,根据所采用曲线的不同,有样条曲线拟合、多项式拟合以及幂指数拟合等。由于车辆存在运动学约束,如车辆位置连续要求曲线是连续的,横摆角连续要求曲线是一阶连续,而加速度约束则要求曲线二阶连续。因此,本书选择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