一体式车辆避撞轨迹规划与跟踪控制

2022-04-26 06:49王国栋刘立孟宇马智萍郑淏清顾青白国星

交通运输系统工程与信息 2022年2期

王国栋，刘立，孟宇*，马智萍，郑淏清，顾青,4，白国星

(1.北京科技大学，机械工程学院，北京 100083；2.汽车噪声振动和安全技术国家重点实验室，重庆 401122；3.北京青年政治学院，管理系，北京 100102；4.北京科技大学，顺德研究生院，广东佛山 528300)

0 引言

过去10年，全球道路交通拥堵问题持续恶化，道路交通事故逐年增加。缓解交通拥堵，提高道路交通安全，避免和减轻交通事故伤害成为各国迫切需要解决的问题。近年来，快速发展的自动驾驶技术为解决这些问题提供了可能。其中，避撞控制是实现车辆自动驾驶的核心技术[1]，能够在关键时刻拯救驾乘人员生命，保证车辆安全行驶，因此，得到了广泛研究。

避撞控制的实现可分为制动避撞和转向避撞。研究表明，在高速、低附着道路等工况，转向避撞比制动避撞所需的避撞距离更短，避撞效果更好[2]。目前，转向避撞控制通常采用分层式控制方案，将避撞控制问题分为避撞路径规划和路径跟踪两个问题。上层采用基于搜索的方法、人工势场法、多项式法以及Sigmod 函数法等算法规划避撞路径，下层采用模糊控制、滑模控制及模型预测控制(Model Predictive Control,MPC)等算法控制车辆跟踪所规划路径。然而，由于上层规划算法通常没有考虑车辆运动学、动力学和约束，或者仅考虑了简单的车辆运动学、动力学和约束[3-4]，因此，规划出的路径可能不够平滑，或者规划出的路径对下层跟踪控制器而言不可行或不合理。为解决该问题，有研究提出基于数学优化的避撞路径二次规划方法。文献[5]首先采用传统方法规划出初始避撞路径；然后，采用基于简单模型的MPC二次规划初始路径，得到平滑以及满足车辆运动学和约束的路径；最后，采用基于高精度模型的MPC进行路径跟踪。文献[6]和文献[7]通过在MPC 成本函数中加入碰撞风险成本，将路径二次规划和跟踪控制整合成一个MPC 优化问题并解决。然而，在这些研究中，车辆的避撞性能十分依赖初始路径，难以保证避撞路径的全局最优性。此外，这些研究在避撞过程中假设自车的速度保持恒定，不考虑速度规划和控制问题。因此，这些避撞控制算法在实际交通环境中的应用受到限制。在速度规划方面，文献[8]提出在已规划出的路径上进行速度规划，得到含速度曲线的路径，称为轨迹。由于这种方法仍需要假设可以预先得到一条最优路径，因此，并不能解决所有场景的避撞问题[9]。为此，文献[10]提出在时空空间内对路径和速度同时进行搜索的轨迹规划方法。然而，这些研究在轨迹规划时仍鲜有考虑车辆运动学、动力学和约束，并且依然采用分层式规划和控制方案。因此，仍可能规划出不平滑或者不合理的轨迹。

相比分层式控制方案，一体式避撞控制方法，或称同步式控制方法，在处理系统约束、路径平滑性及全局最优性方面具有明显优势。一体式避撞控制方案不需要预先规划出参考路径或轨迹，直接将规划和跟踪控制整合为一个优化问题，并进行在线求解。目前，采用一体式方案的避撞控制研究工作较少，但已公开的研究表明了这种控制方案的可行性和优越性。GUO等[11]基于MPC提出一种避撞路径规划和跟踪同步控制方法，其中，目标路径被参数化为时间的三次函数，然后，通过将避撞终点的横向速度作为中间变量，将控制输入序列引入到MPC 优化问题目标路径的表达式中，得到路径规划和跟踪控制同步进行的避撞控制方法。ZHONG等[12]针对智能车辆最优换道问题，设计了一体式最优换道路径规划和跟踪控制方法，换道问题被描述为一个多约束的非线性优化问题，并基于MPC 设计了最优换道控制器。然而，这些研究依然假设避撞过程中自车的速度保持不变，不考虑速度规划和控制问题。因此，限制了车辆的机动性和可能的避撞行为，导致在一些复杂交通环境中无法完成避撞任务。

为提高智能车辆在实际交通环境中的避撞能力，充分发挥车辆的机动性能和避撞潜力，本文基于现有一体式转向避撞控制方案，在避撞过程中考虑速度规划和控制，并将路径规划、速度规划及跟踪控制整合为一个优化问题，提出一种基于MPC的一体式车辆避撞轨迹规划与跟踪控制方法。本文具有以下特点：避撞控制算法基于一体式控制方案设计，能够保证避撞轨迹的最优性；避撞控制算法在转向控制同时进行制动或驱动控制，能够充分发挥车辆的机动性和避撞潜力，提高智能车辆在实际交通环境中的避撞能力；避撞控制算法基于七自由度车辆动力学模型和复合滑移轮胎模型设计，以前轮转角和制动、驱动力矩作为控制输入，能够保证避撞过程车辆状态符合车辆动力学和约束，以及避撞轨迹和控制动作平滑。此外，本文还设计了基于恒速假设的一体式避撞路径规划和跟踪控制方法，并基于Matlab 和CarSim 联合仿真平台与本文所提方法进行了对比。

1 控制问题描述

1.1 控制问题假设

完整的自动驾驶系统包括：环境感知与预测，行为决策和运动控制等功能。本文仅研究避撞控制问题，认为避撞场景中所有交通车辆的行为和状态都是可测或可预测的。同时，本文假设智能车辆的行为决策系统是可靠和完备的，且行为决策已经完成，具体假设：

(1)交通环境中其他车辆当前行为是确定且已知的，例如，直行或换道等行为；

(2)交通环境中其他车辆当前的状态信息已知，例如，位置，速度等；

(3)智能车辆自车的行为决策已经完成且结果可靠，不考虑“不可避免碰撞”的情况。

实际交通环境中道路结构是不同的，然而，对于避撞控制，自车道和相邻车道的交通参与者是自动驾驶系统最关注的环境信息，因此，本文假设当前道路结构为单向双车道，并且最多只考虑存在其他4个交通参与者的交通场景。

1.2 避障场景分析

当智能车辆自车前方存在障碍物时，若安全距离足够，可以通过自动紧急制动系统完成避撞。但是，在一些场景中，即使完全制动也无法避免碰撞，此时采用转向控制仍有机会避免碰撞。

然而，面对现实中复杂的交通环境，基于恒速假设的转向避撞控制方法往往也无能为力。避障场景如图1所示。

图1(a)的驾驶场景是智能车辆自车前方较近处因突然发生交通事故或出现障碍物需要进行转向避撞。此时，目标车道仍存在其他车辆，若其他车辆的位置和车速与自车接近时，以恒定速度进行转向避撞显然不可行，会引发新的事故。然而，若此时在转向避撞的同时进行加速控制则能够实现避撞。

图1(b)的驾驶场景是智能车辆自车前方较远处出现交通事故，但自车后方较近处和相邻车道存在其他车辆。尽管自车到事故点或障碍物的制动距离足够，但是紧急制动会导致自车与后方车辆发生追尾。虽然此时追尾事故是因后车未保持足够的安全距离所导致，在法律上该事故不会被认定为自车的责任。但是，采用转向配合加速进行避撞是更好的选择。

图1 避障场景Fig.1 Scene of collision avoidance

图1(c)的驾驶场景中，当自车进行转向避撞的目标车道前方存在行驶缓慢的车辆时，在转向避撞过程中进行制动控制成为避免碰撞的唯一选择。

图1(d)的驾驶场景中，尽管自车到事故点的制动距离足够，但是，此时进行紧急制动会导致自车与后方车辆发生追尾事故。同时，由于目标车道前方存在行驶缓慢的车辆，因此，在转向避撞过程中进行制动控制是避免碰撞的最佳选择。

综上，在实际交通环境中进行转向避撞控制的同时进行速度控制非常有必要。

1.3 环境与状态约束

一体式避撞控制问题可以描述为：在满足空间位置、车身位姿、目标状态及稳定性约束等环境和车辆系统约束下的多约束优化问题。在局部交通场景中，自车与其他最多4辆交通车辆的位置和约束如图2所示。

图2 局部环境约束Fig.2 Local environmental constraints

自车当前车道后方和前方车辆分别记为V1和V2，目标车道后方和前方车辆分别记为V3和V4。为保证在避撞控制中自车不与其他交通车辆发生碰撞，需要考虑的约束为

式中：Xsafei为自车与Vi的最小安全位置，i=1,2,3,4；a和b分别为自车质心至车头和车尾的距离；d为自车车宽；Yboundi为车道边界；Xhost和Yhost分别为自车的纵、横坐标。

为了在避撞结束时保证车身姿态正确，完成换道动作，达到目标速度，并确保车辆在避撞过程中的稳定性，需要考虑的约束为

式中：φend和Yend分别为终点横摆角和侧向位置；Yref为目标换道距离；Vx,end为终点车速；Vx,ref为目标终点车速；ay为侧向加速度；μ为路面附着系数；g为重力加速度。

2 控制器建模

本文以前轮转向和四轮驱动、制动智能车辆为研究对象。为实现转向和制动、驱动集成控制以及准确描述车辆和轮胎的动力学特性，本文分别搭建7DOF 四轮车辆动力学模型和复合滑移工况的UniTire轮胎模型。

2.1 车辆模型

为简化控制器设计，在车辆模型建模时进行如下假设：

(1)忽略执行器动力学，直接使用前转向角，驱动、制动力矩作为控制输入；

(2)忽略车辆的垂直、俯仰及侧倾运动，只考虑偏航、横向及纵向运动；

(3)忽略滚动阻力、坡度阻力及空气动力学的影响。

基于以上假设，可以得到考虑车体纵向、侧向、横摆运动以及4个车轮旋转运动的7DOF四轮车辆模型。车辆模型如图3所示。车辆在车体坐标系xoy中的运动表示为

图3 车辆模型Fig.3 Vehicle model

式中：和分别为车辆在x轴和y轴方向的速度变化率；为横摆角加速度。

车辆在大地坐标系XOY中的运动表示为

式中：和分别为车辆在X轴和Y轴方向上的位移变化率；为横摆角变化率。

车轮的旋转运动描述为

式中：为车轮旋转角速度；j={f l,fr,rl,rr}。

本文认为同一车轮上的驱动力矩和制动力矩不同时存在，故式(8)退化为

式(9)表示当求出的力矩Tx,j大于0 时，将其视为驱动力矩；当Tx,j小于0 时，将其视为制动力矩。车轮动力学模型如图4所示。车辆模型所用符号的定义如表1所示。

图4 车轮动力学模型Fig.4 Wheel dynamics model

表1 车辆模型符号Table 1 Vehicle model symbols

2.2 轮胎模型

轮胎力是车辆运动的主要外力来源，具有很强的耦合和非线性特性。因此，转向和制动、驱动集成的避撞控制需要建立复合滑移工况的轮胎模型。常见的复合滑移工况轮胎模型有Magic模型、Fiala 模型及UniTire 模型。其中，UniTire 模型是郭孔辉院士提出的适用于车辆动力学仿真和控制的非线性轮胎模型，能够准确描述轮胎在复杂工况下的力学特性。因此，本文采用UniTire 模型。具体表达式和验证参考文献[13]，本文不再赘述。

3 控制器设计

由于MPC在处理系统约束和多变量优化问题方面的优势，以及独特的滚动预测机制，在智能车辆控制方面得到广泛应用。因此，本文采用MPC设计一体式避撞轨迹规划与跟踪控制器。

3.1 预测模型

将轮胎模型和式(9)代入式(6)，并联立式(7)可以得到一体式避撞轨迹规划与跟踪控制器的预测模型，即

式中：t为时间变量；(t)为时域；为状态变量；为控制输入；为控制输出；h为输出映射，即

采用龙格-库塔法对上述模型进行离散化，转化成增量型模型为

根据模型预测控制理论，在当前k时刻，系统未来状态表示为

式中：P为预测时域；M为控制时域。

基于式(12)可以得到系统未来的预测输出ζ(k+1|k)…ζ(k+P|k)，不再赘述。

定义控制输入和预测输出序列分别为

3.2 空间域预测模型

不同于基于恒速假设的避撞控制方法，本文需要对车速进行控制。因此，在MPC 预测时域中车辆的纵向位置不可知，进而，式(1)～式(4)所示的空间位置约束和终点姿态约束在预测时域中无法准确表达。因此，需要将式(10)所示基于时间域的预测模型转化为空间域预测模型。

将时域预测模型转化成空间域模型，即

式中：s为位置变量；(s)为空间域。

进而，参考式(11)和式(12)可得离散的增量式空间域模型及其预测输出，不再赘述。

3.3 变离散步长设计

当采用固定步长对系统进行离散化时，随着控制窗口逐渐变短，MPC 控制器的预测时域P越来越小。当接近终点位置时，P将变得极小，此时，系统可能变得不稳定。为此，本文设计了变离散步长解决此问题。

假设当自车质心驶过障碍车安全位置时视为避撞完成，如图5所示。因此，避撞时的控制窗口可以表示为

图5 控制窗口Fig.5 Control window

由于自车未来的行为不可知，因此，在控制窗口ECW内自车未来的轨迹亦不可知。根据式(3)和式(17)可知，自车未来在控制窗口内行驶的路程在ECW+Yref,end和之间，如图5中点线所示。因此，自车在控制窗口内行驶的路程可以近似为

式中：Yref,end为参考侧向位置的终点坐标。

因此，对于既定的预测时域P，离散步长为

式中：ρTs为微调系数。

3.4 目标函数设计

车辆避撞的安全性和稳定性以及到达目标点的位置、姿态及速度需求通过式(1)～式(5)的不等式约束和等式约束实现。控制目标可以描述为在满足约束的前提下尽可能保持避撞动作平滑。因此，本文将最小化控制动作的变化量作为优化目标，得到目标函数为

式中：ks为对应于时域中的离散时刻k，描述空间域离散序列的量；n=1,2,…,M；Γu为控制输入权重矩阵，Γu=diag[τu(ks),τu(ks+1),…,τu(ks+M)]；τu=[τδf,τTx,fl,τTx,fr,τTx,rl,τTx,rr]。

此外，为避免执行器饱和，还应对控制输入及其变化率进行约束，即

最终，一体式避撞控制问题描述为

上述优化问题采用MATLAB提供的优化函数“Fmincon”求解，求解算法选择有效集法。

4 仿真试验

本文基于MATLAB/CarSim 联合仿真平台分别设计了加速、减速两种避撞场景对所提出的一体式避撞轨迹规划与跟踪控制方法进行验证，并与未考虑速度规划和控制的一体式避撞控制方法，即基于恒速假设的一体式避撞控制方法，进行了对比。仿真车辆和控制器的主要参数如表2和表3所示。

表2 车辆参数Table 2 Vehicle parameters

表3 控制器参数Table 3 MPC controller parameters

4.1 加速避撞场景试验

加速避撞场景如图6所示。

图6 加速避撞场景Fig.6 Collision avoidance in acceleration scene

定义自车质心所在的位置为局部坐标系的原点，即(Xhost,Yhost)=(0，0)；自车初始车速为90 km·h-1；交通车V1、V2、V3及V4的速度分别为80，0，90，120 km·h-1；交通车V2可以看作事故车辆或其他静止障碍物；Xsafe1、Xsafe2、Xsafe3及Xsafe4分别为-30，100，-10，50 m；目标终点侧向位置Yref为3.5 m。仿真结果如图7～图14所示，其中，恒速避撞的仿真结果以表示，仿真中定义为90 km·h-1。需要说明的是Xsafei与Vi距离的取值范围并不影响本文所提方法的实现，因此，本文不讨论Xsafei与Vi距离的设置，直接给出Xsafei的值。

自车(质心位置)与其他4 辆交通参与车辆(安全位置)的轨迹和相对位置关系如图7所示，图中轨迹起点以实心圆表示，终点以空心圆表示。

图7 车辆轨迹Fig.7 Vehicle trajectory

自车(质心位置)与其他4 辆交通车辆(安全位置)纵向位置随时间的变化曲线如图8所示。

图8 车辆纵向位置Fig.8 Vehicle longitudinal position

自车侧向位置随时间的变化曲线如图9所示。

图9 车辆侧向位置Fig.9 Vehicle lateral position

由图7可知，自车约在Xhost=80 m 处，即3 s 时达到目标终点侧向位置，如图9所示避撞轨迹平滑。由图8可知，自车约在3.5 s 时驶过事故车辆V2，即Xhost=Xsafe2。结合图8和图9可以看到，自车轨迹在空间上和其他4辆交通车辆没有重合，表明自车与其他交通车辆无碰撞，成功完成避撞任务。另外，若自车以90 km·h-1的车速进行匀速避撞，则将与交通车V3发生碰撞。如图8中的短点划线所示，在3.6 s附近与V3的纵向位置重合，同时，图9显示，在该时刻与V3的侧向位置重合，因此，当自车以90 km·h-1的车速进行匀速避撞时，将会与交通车V3发生碰撞。自车在避撞过程中的横摆角变化如图10所示。

图10 横摆角Fig.10 Yaw angle

图10显示在避撞过程中自车的横摆角变化平稳，在避撞终点位置自车的横摆角能够逐渐收敛到0 deg。结果表明，在避撞过程中自车能够保持良好的车身姿态变化，在避撞结束时能够保证车头指向正前方。

MPC控制器求出的自车前轮转角变化如图11所示，显示避撞过程中前轮转角变化平滑，效果良好。

图11 前轮转角Fig.11 Front steering angle

MPC控制器求出的自车驱动力矩输入如图12所示。由于本文控制对象为普通四轮驱动车辆，不考虑四轮差动驱动，且前、后轴动力分配采用等比分配，因此，图12中4 个车轮的驱动力矩大小一致。图12显示车轮的驱动力矩从0开始逐渐增加，达到约389 N·m-1后开始减小，并在3.6 s附近迅速降低，最终保持在53 N·m-1附近。避撞结束后，自车的驱动力矩并不为0，因为，自车仍然需要驱动力以克服传动系统阻力、滚动阻力及风阻等阻力的影响来保持匀速行驶。