基于模型预测控制无人驾驶车辆避障轨迹跟踪器设计

杨朝阳，陈恳，彭丽

（湖北汽车工业学院 汽车动力传动与电子控制湖北省重点实验室，湖北 十堰442002）

无人车在未来的智能交通中扮演着关键角色［1］，在行驶中实现路径跟踪和避障功能是无人车安全性的基本要求。良好的控制算法是实现无人车轨迹跟踪和避障的前提，国内外学者对此做了大量的研究。关于轨迹跟踪问题，文献［2］提出了一种强化学习自适应PID路径跟踪控制算法，相较于传统PID算法达到了优化系统动态性能的目的。文献［3］提出了一种随机扩展数算法，实现了无人车的局部规划。关于车辆避障问题，文献［4］提出了一种改进的速度障碍算法来实现无人车避障功能。其他的控制方法如遗传算法［5］、人工势场法［6］和神经网络［7］等在无人车避障问题上都有所应用。与上述研究中的控制算法相比，模型预测控制算法能处理复杂约束问题，预测模型更灵活，传统的传递函数和状态方程都能作为预测模型。模型预测控制可以监测控制器的实际输出，根据实际输出对预测的结果进行校正，然后再进行优化，不断的反馈、校正、优化控制结果，使控制指标达到最优。

1 轨迹跟踪控制器设计

轨迹跟踪控制器主要实现无人车以设定的速度跟踪既定的行驶轨迹。文中设计的轨迹跟踪控制器不涉及环境感知和车辆状态的估计，因此对控制器做出限定：1）知道可行驶道路的路面特征和几何描述；2）知道车辆横向速度、加速度和轮速等车辆状态参数。

1.1 无人车动力学模型

建立无人车动力学模型如图1 所示。在小角度假设条件下，计算轮胎力时，三角函数满足以下近似条件

式中：θ为各种小角度，在计算轮胎力时轮胎的前轮转角、侧偏角等角度均为小角度。用式（1）简化后的轮胎侧偏角为

式中：Ccf和Ccr分别为前/后轮轮胎侧偏刚度。前/后轮纵向力为

式中：Clf和Clr分别为前/后轮纵向刚度；sf和sr分别为前/后轮滑移率。根据运动学模型建立无人车非线性动力学方程：

图1 动力学模型

式中：m为车辆整备质量；φ为车身横摆角；为横摆角速度；Iz为车辆绕z轴的转动惯量和̇为惯性坐标系下车辆在X轴和Y轴的速度̈和̈为车辆坐标系下车辆在x轴和y轴的加速度。

1.2 预测模型设计

轨迹跟踪控制中采用车辆的非线性动力学模型作为预测模型，在轨迹跟踪过程中只对车轮的前轮偏角进行控制。将式（5）转化为状态方程：

式中：ξdyn状态量；udyn为控制量。在轨迹线性化过程中，状态轨迹是通过不断给系统施加持续不变的控制量得到的，通过状态轨迹和实际状态量的偏差来设计模型预测算法，得到线性时变方程：

式中：Bdyn由fdyn对udyn求偏导得到；Adyn由fdyn对ξdyn求偏导得到，

通过一阶差商法将式（7）离散化处理得到离散的状态空间方程：

式中：I为单位矩阵；T为采样周期。

1.3 约束条件设计

在设计过程中为了使控制器达到最好的控制状态，将控制器的控制量和控制增量都限定在相应控制时域内的允许范围。无人车在行驶过程中外界环境在不断变化，因此需要考虑车辆动力学参数、路面状态和轮胎的约束。文中添加无人车质心侧偏角、车辆附着条件和轮胎侧偏角的约束。

1）质心侧偏角约束 为保证无人车行驶的稳定性，在附着条件良好的路面质心侧偏角约束范围为-12～12°，在附着条件较差的路面质心侧偏角约束范围为-2～2°［8］。

2）附着条件约束 为了保证车辆的动力性，对附着条件进行约束。当车辆纵向匀速行驶时，车辆的横向加速度与附着系数的关系为

式中：ay为车辆的横向加速度；μ为附着系数；g 为重力加速度。横向加速度过大时，车内人员的舒适性较差，约束条件太窄会导致求解失败，综合2 个因素将该约束选定为软约束。根据每个周期的求解情况，通过松弛因子动态调整该约束条件：

式中：ay,min和ay,max分别为横向加速度的最小值和最大值；ε为松弛因子。

3）轮胎侧偏角约束 在动力学建模过程中没有将轮胎的侧偏角作为状态量，因此将轮胎侧偏角约束在-2.5～2.5°。

1.4 目标函数设计

预测模型采用的车辆运动学模型中加入了约束条件，可能在计算过程中出现无解的情况。为了解决这一问题，在目标函数中加入松弛因子，目标函数表达式为

式中：Q和R为权重矩阵；ρ为权重系数；NP为预测时域；Nc为控制时域。综合约束和目标函数，轨迹跟踪控制器在每个周期内要优化的问题为

式中：yhc和ysc分别为硬约束和软约束；A为比例系数。在每个周期求解完成后，得到一系列的控制输入增量和松弛因子：

将该控制序列中的第1 个元素作为实际的控制输入增量作用于系统：

在每个周期内重复上述过程，实现对期望轨迹的跟踪控制。

2 避障控制器设计

无人车的避障功能是在路径跟踪基础上建立的，在轨迹跟踪过程中，当设定的路径上存在障碍物时，无人车需要重新规划路径避开障碍物。当成功避开障碍物后，无人车继续按照期望轨迹行驶，避障控制器就要实现上述功能。

在避障功能设计过程中，由于避障算法自身的计算量较大，为了减少计算量，采用的车辆模型只要满足规划结果达到车辆动力学和运动学约束即可。通过文献［9］中的对比实验可知，在避障控制器采用较低精度的模型而在轨迹跟踪控制器采用较高精度的模型，能很好地兼顾控制性能和计算速度。因此，在轨迹跟踪控制器中采用的是高精度的车辆非线性动力学模型，在避障控制器中采用忽略无人车车身尺寸低精度的点质量模型。

2.1 点质量模型

点质量模型是将车辆看作1个有质量的点，忽略车身尺寸，如图2所示。点质量动力学方程为

式中：ξ为状态量矩阵。

图2 点质量模型

2.2 避障功能设计

避障控制器是以点质量模型为基础设计的，由于点质量模型没有考虑车身尺寸，可能出现车辆质心没有碰撞到障碍物但车身部分碰到障碍物的情况。为解决这一问题，将障碍物膨胀化处理，并将膨胀化后的障碍物看作一系列的障碍物点集（图3），避免车辆从障碍物穿越的现象发生。

避障功能函数针对每个障碍物点进行设计，惩罚函数通过障碍物点和目标之间的距离偏差来调节函数的大小，距离越近函数值越大，其大小与车辆速度有关。避障功能函数为

图3 障碍物膨化及点处理图

式中：(x0,y0)为车辆质心坐标；(xi,yi)为障碍物点在车辆坐标系下的坐标；Sobs为权重系数；β为很小的正数，防止分母为0。

避障控制器要在避开障碍物的同时，减小车辆行驶轨迹与期望路径之间的偏差。避障模型预测控制器的模型为

避障控制器给出的是离散后的点，将其用多项式拟合法拟合成一条曲线，得到规划后车辆轨迹。

3 仿真分析

3.1 仿真工况和主要参数

采用Carsim和Matlab联合仿真，在Simulink中编写算法，车辆模型采用Carsim 中B-Class 下的Hatchback轿车模型。选择双移线工况作为仿真工况，道路附着系数为0.8，分别验证车速在低速20km·h-1、中速60km·h-1和高速90km·h-1情况下控制器的性能。车辆的主要参数见表1，控制器参数如表2所示。

表1 车辆的主要参数

表2 控制器参数

3.2 结果分析

3.2.1轨迹跟踪性能仿真分析

首先验证了在没有障碍物的情况下，轨迹跟踪控制器的性能，仿真结果如图4所示。

图4 控制器轨迹跟踪控制仿真结果

由图4 a～4 b 可知在不同的车速下，车辆位移和横摆角都能很好地跟随参考路径位移和参考横摆角，而且跟踪性能良好。由图4 c～4 f 可知，在不同车速下车辆的动力学参数变化较大，速度越快参数变化越大。前轮转角的变化范围在-4～4°，符合小角度转向，说明仿真结果在合理范围内。由图4d可以看出，仿真结果轮胎侧偏角在-0.8～1.2°，满足约束条件。仿真时地面附着系数为0.8，根据式（9）可以得到侧向加速度应该在-0.8 g～0.8 g，根据图4e可知侧向加速度也在约束范围之内。良好路面上质心侧偏角约束在-12～12°，由图4f可知质心侧偏角远小于约束。

根据避障仿真结果可以看出，在低速、中速、高速3 种状态下，控制的轨迹跟踪性能良好，车辆各种动力学参数均在约束范围之内，控制器稳定性和轨迹跟踪性能满足设计要求。

3.2.2避障性能分析

在参考路径上放置1 个尺寸为5m×0.5m 的矩形障碍物，障碍物左下角顶点的坐标为（30，0.5），不同车速下避障仿真结果如图5所示。

图5 车辆在不同速度下的避障路径

由图5 可知在不同车速下车辆从原点位置出发，没有发现障碍物时车辆会按照期望轨迹行驶。在20m 处车辆发现障碍物，重新规划行驶轨迹来避免与障碍物接触，成功躲避障碍物之后再次规划路径向期望轨迹靠近，最后重新跟踪上期望轨迹，并按照期望轨迹行驶。由图5 a～5 b 可知车辆在中、低速行驶时，车辆在避障后能较好地重新跟踪期望路径，而图5 c车辆在高速行驶时，避障成功后车辆在重新跟踪期望路径的过程出现了偏差，但偏差较小并且最终跟踪上了期望路径。

根据仿真结果可以看出，在低、中、高车速下，车辆都能成功躲避障碍物并能重新跟踪上期望路径，控制避障和跟踪性能良好，控制器稳定性高。

4 结论

1）基于模型预测控制算法设计了避障轨迹跟踪控制器，实现轨迹跟踪和避障功能。

2）将车辆运动学模型作为预测模型，在此基础上设计了轨迹跟踪控制器。将轨迹跟踪转化为带有约束条件的二次规划问题，以点质量模型和惩罚函数设计避障功能。

3）联合仿真结果表明：当参考路径上没有障碍物时，在低、中、高车速下车辆能实现对参考路径的追踪，并且车辆状态均在约束范围之内。当参考路径存在障碍物时，车辆能够成功躲避障碍物，并在躲避障碍物后重新跟踪上参考路径。所设计的控制器，在不同车速下能很好地实现轨迹跟踪和避障功能，控制效果良好，稳定性较高。