自适应双时域参数MPC的智能车辆路径规划与跟踪控制

李耀华，范吉康，刘 洋，何 杰，李泽田，潘绍飞

（1. 长安大学汽车学院，西安710064，中国；2. 上海汽车集团股份有限公司 技术中心，上海 201804，中国）

路径规划是指在存在障碍物的行驶环境中，按照一定标准规划出一条从初始位置到目标位置的无碰撞路径，可分为全局路径规划和局部路径规划[1-3]。全局路径规划是根据地图信息规划出从起点到终点的最优行驶路径。局部路径规划根据全局路径，同时结合传感器检测到的车辆周围信息，实时规划出未来一段时间的局部路径。路径跟踪是自动驾驶技术中的重要部分，其主要任务是根据决策规划层输出的参考路径，并结合当前车辆状态，控制车辆转向从而跟踪参考路径[4-7]。

针对局部路径规划问题，文献[8]对传统快速扩展随机树(rapidly-exploring random trees, RRT)算法进行改进，采用样条曲线拟合RRT生成的路径，提高规划路径的平滑度。文献[9]选择5次多项式进行换道路径规划，实现车辆侧向加速度的减小，提高换道时的驾驶舒适性。文献[10]将四次Bézier曲线与蚁群算法结合，规划出符合自动驾驶车辆行驶曲率的避障局部路径。文献[11]采用一种改进势场模型，以类椭圆形斥力场代替传统斥力场，同时添加道路边界斥力场和障碍物速度势场，保证了规划路径的行驶安全性和舒适性。文献[12]研究了基于凸二次规划的模型预测控制，将车辆的形状考虑成由线性约束而非单个点定义的凸多边形区域，试验证明能够改进系统的实时性。

针对路径跟踪问题，文献[13]通过比例-积分-微分(proportion integration differentiation, PID)控制器得到车辆方向盘转角，采用死区补偿法来提高路径跟踪过程的稳定性。文献[14]以跟踪偏差作为目标函数，结合 线性二次型调节器 (linear quadratic regulator，LQR)控制算法，提出了一种多点预瞄路径跟踪控制算法。文献[15]针对四轮独立转向驱动电动汽车路径跟踪问题，采用模型预测控制(model predictive control,MPC)算法，设计包含航向角偏差、纵向偏差以及控制量增量的目标函数。文献[16]对比了最优预瞄控制和MPC 2种路径跟踪策略，指出MPC的路径跟踪精度更高，对车速和路面附着系数变化具有较好的适应性。MPC可通过目标函数综合考虑不同的控制目标和约束条件，实现传统控制难以实现的控制功能，具有较大的柔性。但MPC算法需要求解非线性规划以获得最优解，计算量较大，算法实时性差。因此，MPC算法在实际工程应用与芯片处理器水平、软件算法优化等密切相关。随着芯片处理能力的提升，模型预测控制算法有望在未来实现实际工程应用。

本文以障碍物场景下的自动驾驶车辆作为研究对象，针对局部避障路径规划问题，基于车辆点质量模型，采用非线性MPC算法进行局部路径规划，设计包含新型避障功能函数的目标函数，解决传统避障功能函数过度避障的局限性。针对路径跟踪问题，基于二自由度车辆动力学模型，采用线性时变MPC算法进行路径跟踪控制，分析预测时域和控制时域对控制性能的影响，建立考虑控制精度和行驶稳定性的路径跟踪性能综合评价指标，得到不同车速下的最优预测时域和控制时域参数，设计了自适应时域参数路径跟踪控制器。对局部路径规划器与路径跟踪控制器集成设计，并通过CarSim软件与Simulink软件进行联合仿真集成验证。

1 局部避障路径规划

基于非线性MPC的局部避障路径规划器的基本思想是通过将最优局部路径的选取原则融入MPC优化问题中进行求解，将所求最优控制序列作用于“假想”车辆，进而获得“假想”车辆的唯一行驶路径，该路径即为满足路径选取原则最优局部路径。

1.1 规划层车辆预测模型建立

由于非线性MPC算法本身计算量相对较大，为保证实时性，模型不应该过于复杂，选择车辆点质量模型作为规划层车辆模型。若令 (x, y) 为车辆在车身坐标系下的位置，(X, Y)为车辆在大地坐标系下的位置，车辆质心为车身坐标系原点，ay为车辆侧向加速度，φ为车辆横摆角；则该模型为：

式（1） 亦可看作一个状态量为 ，控制量为 的系统，可简化为

假设当前时刻为k，局部规划层的采样时间为Tp，预测时域为Np，控制时域为Nc，并且一般Np＞Nc。采用前向Euler法将式（2）离散化，可得：

其 中：规 定u(k+i) =u(k+Nc- 1),i=Nc,Nc+1,…,Np-1，即预测时域内控制时域外的控制量保持不变。

若定义系统输出量为 ，则有根据式（3）及状态量与输出量之间的关系，可求得系统输出量。

1.2 避障功能函数设计

避障功能函数来描述局部路径与障碍物之间的相近程度，根据MPC目标函数的要求可知，当局部路径与障碍物的距离越近时，避障功能函数值应越大。若令Sob为权重系数，v0为车辆行驶速度，(xob,n,yob,n)为在车身坐标系下第n个障碍物点的位置，(x0,y0)为车辆质心坐标，ζ为一个较小的正数以防止分母为零，N为障碍物点总个数；则避障功能函数为[17]

传统避障功能函数将障碍物点与车辆质心点之间的距离作为唯一标准来描述车辆与障碍物发生碰撞的可能性。当障碍物与车辆实际不可能发生碰撞时，此时计算得到的Jobs会对模型预测控制产生干扰，可能发生没有必要的过度避障。障碍物与车辆的位置如图1所示。

此时障碍物1不存在与车辆发生碰撞的可能性，但传统避障函数计算得到的Jobs会对目标函数产生干扰，使得规划路径产生不必要的避障规划，从而增大碰撞风险，提高避障难度，影响驾驶舒适性。

针对以上问题，本文定义当障碍物点与车辆之间的距离超过安全距离范围，则直接令其对应的距离为固定的极大值，从而消除其在目标函数中的影响。车辆与障碍物的关系如图2所示，图2中a、b、c为车身尺寸参数。

车辆与障碍物点之间的距离（dmin, n）和车辆与障碍物点之间的距离（dmin）分别为：

式中：W为一个极大常数，取1×107；cs为横向安全距离阈值，cs与车速的关系为正相关[18]，即：

由式（4）—（6），可得新型避障功能函数为

避障函数为分段函数，在2段函数的切换点处（横向安全阈值），新型避障功能函数值会发生突变。如果实际横向距离恰好在阈值附近，会因传感器误差等原因造成横向距离微小波动，使得避障功能在进入与退出状态频繁切换，导致驾乘体验变差。为了避免出现频繁切换的抖动，本文采用滞环控制以消除因传感器误差等原因造成的抖动。

1.3 优化问题构建与求解

系统预测时域内局部避障路径规划层的目标是安全避开障碍物，同时保证规划的路径与全局参考路径偏差不要过大。路径规划层的目标函数为

式中：U(t)为所求控制量序列，ηref为局部参考路径，ηref=[φref,Yref]T，Q为输出量偏差权重矩阵，R为控制量权重矩阵，Jobs,i为第i时刻避障功能函数值。

为了使规划路径能保证车辆行驶稳定性，在规划层目标函数中添加相应约束。因此，局部避障路径规划层转化为求解如式（9）所示的优化问题。

通过求解式（9）得到预测时域内最优控制序列，将最优控制序列带入式（3），获得预测时域内的离散最优局部避障路径点，通过五次多项式曲线进行路径离散点拟合，则可获得局部避障规划路径。

2 路径跟踪控制

路径跟踪控制问题本质上是车辆转向盘转角控制问题，也称为车辆横向控制问题，在路径规划层输入的参考路径信息的基础上，根据车辆当前状态，计算出最优控制量，输出给执行层。最终通过执行机构的准确执行，完成车辆的路径跟踪控制。

2.1 控制层车辆模型建立

为兼顾控制层的控制精度和实时性，选择二自由度车辆动力学模型作为控制层的车辆模型，即：

式中：Cf和Cr分别为车辆前后轮侧偏刚度，m为车辆整备质量，a、b分别为质心到前、后轴的距离，δf为前轮转角，φ为车辆横摆角，Iz为车辆绕z轴转动惯量。

上述模型可简写为

对于路径跟踪控制层来说，车辆模型更为复杂，且采样周期更短。为提高控制实时性，将式（10）进行近似线性化，可得线性时变方程为

式中：A(t)、B(t)分别为f对于ξ和u的Jacobi矩阵，ξ0(t)为参考状态量，u0(t)为参考控制量。

采用前向Euler法对式（12）进行离散化，可得：

式中，Tc为控制层采样周期。

2.2 优化问题构建与求解

系统路径跟踪控制层的目标是保证控制车辆使其实际行驶路径与规划层输入的局部避障路径的偏差尽量小，同时为保证行驶稳定性尽量减小控制增量。路径跟踪控制层的模型预测控制器的目标函数为

式中： ΔU(t)为所求控制增量序列，ε为松弛因子，Q为输出量偏差权重矩阵，R为控制量权重矩阵，ρ为松弛因子权重因子。

在建立目标函数并添加约束条件后，路径跟踪控制即转化为求解如式（15）所示的优化问题。

式中：Δumin、Δumax为控制增量上下限；umin、umax为控制量上下限；ηmin、ηmax为输出量上下限；Lp为p维列向量。

将式（14）转化为二次规划问题，通过求解获得控制时域内的控制增量序列ΔU(t)，并将第一个量作为实际控制增量，根据上一控制周期输入被控系统的控制量确定本次控制周期的控制量。在每个控制周期都重复上述求解过程，获得实时反馈控制量，从而实现车辆的路径跟踪控制。

2.3 最优时域参数选择

MPC控制器时域参数有预测时域为Np和控制时域为Nc，两者对控制效果均有较大的影响。

由式（14）可知，在其它控制器参数不变时，预测时域Np越大，控制器能够预测更远的位置，获得更多的车辆状态信息，但是Np过大会增大考虑距离车辆当前位置较远处输出量偏差的控制，减弱距离车辆当前位置较近处输出量偏差的控制，导致求解目标函数得到的最优控制量对于当前位置较近处并不是最优控制量，导致较近处的跟踪偏差增大。当控制器进入下一个采样周期重新求解下一个周期的最优控制量，则会继续增大下一个较近处的跟踪偏差，最终通过累加导致整体的跟踪偏差增大。同时过大的Np也会增加控制器的计算时间，降低控制器实时性。当Np过小时，控制器的实时性虽会提高，但是预测的未来车辆状态信息过少，在存在控制量与控制增量约束的前提下，前轮无法及时转向，也会导致跟踪偏差的增大，甚至导致车辆失控。在其它控制器参数不变时，Nc越大，控制灵敏度和精度越高，但是系统稳定性和实时性也随之下降。Nc变小，控制器稳定性与实时性更好，但是控制精度下降。例如：当Nc= 1时，在预测时域内，控制器仅做一次控制动作，为保证预测时域内平均偏差最小，求解的控制量会牺牲当前位置附近处输出量的精度，如图3所示。

为获得不同车速下的最优时域参数，基于Carsim/Simulink搭建联合仿真平台，选择双移线工况作为参考路径，进行不同行驶车速下的仿真试验。车辆轮胎型号235/45 R18，整车其他参数见表1，控制器参数见表2。

表1 整车参数

表2 控制器参数

采用最大横向位置偏差Edmax、平均横向位置偏差Edm和平均横摆角偏差Eφm，来衡量实际跟踪路径与参考路径的偏差，分别为：

式中：Ed为车辆与参考路径的横向偏差，N为总测量次数，φ为实际横摆角，φr为参考横摆角。

路径跟踪控制不仅要满足跟踪偏差要求，而且要保证车辆路径跟踪时的行驶稳定性。通过约束车辆最大质心侧偏角和最大横摆角速度，可防止车辆失稳。车辆最大质心侧偏角βmax和最大横摆角速度ωmax约束条件分别为[19-21]：

式中：μ为路面附着因数、g为重力加速度，取9.8 m/s-2。

以10 km/h的间隔选取30～70 km/h的行驶车速作为仿真工况条件，Np取值范围为1至40，Nc取值范围为1至(Np-1)，共遍历3 900种组合。试验发现，在部分时域参数组合条件下，出现车辆失控等无效情况。为了保证控制器时域参数的有效性，制定时域参数筛选标准如下：

1） 设置的时域参数能够保证控制器可运行，保证车辆不发生失控导致路径跟踪失败；

2） 设置的时域参数能够保证车辆的行驶安全性，保证质心侧偏角、横摆角速度等参数的最大值不超过约束上限；

3） 设置的时域参数能够保证控制器的实时性，保证求解时间小于采样周期。根据时域参数筛选策略，剔除无效时域参数后，有效时域参数组合的仿真得到3 900组数据，选择有代表性的数据， 最大横向偏差Edmax、平均横向偏差Edm、平均横摆角偏差Eφm、最大质心侧偏角βmax、ωmax最大横摆角速度，如表3所示。

表3 双时域参数仿真结果

仿真结果表明：车辆的控制精度与行驶稳定性存在一定的冲突。为了选择最优预测时域参数与控制时域参数，平衡控制器对车辆的控制精度与行驶稳定性，综合考虑控制精度与行驶稳定性，提出一个路径跟踪性能综合评价指标，对不同双时域参数设置下的不同仿真结果参数进行加权计算，将其作为双时域参数优劣的综合评价指标。

式中：Edmax为最大横向偏差；Edm为平均横向偏差；Eφm为平均横摆角偏差；βmax为最大质心侧偏角；ωmax为最大横摆角速度；Q1、Q2、Q3、R1、R2为相应的权重系数。

本文重点研究避障行驶工况，侧重于控制精度，尤其是最大横向偏差参数。根据侧重点不同，同时考虑到以上参数的量纲不同，经过多次试验，确定权重系数值分别为200、400、40、20和1。

在建立评价函数后，将双时域参数试验结果代入式（20）进行得分计算，选择最低综合评价指标Sc值作为对应车速下的最优预测时域参数Np与控制时域参数Nc，结果如表4所示。

表4 不同车速下的最优双时域参数

在获得不同车速下的最优双时域参数后，制定自适应双时域参数控制策略，如式（22）所示。

2.4 控制器性能验证

为验证最优双时域参数选择方法，将车速分别设置为25、35、45、55、65 km/h，对固定双时域参数控制器与自适应双时域参数控制器进行双移线工况仿真试验，其中固定双时域参数设置为Np= 25，Nc= 1。不同车速下，固定双时域和自适应双时域参数控制器的控制性能如表5所示。部分车速下的仿真结果如图4—图6所示。

表5 不同MPC控制器的控制性能

仿真结果表明：在不同车速下，自适应时域参数控制器的前轮转角、前轮转角增量、最大质心侧偏角、最大横摆角速度均满足约束条件。与固定时域参数控制器相比，自适应时域参数控制器均具有更小的最大横向偏差、平均横向偏差和平均横摆角偏差及综合评价指标Sc，且随着车速增加两者的差异性越明显。当车速达到65 km/h时，固定时域参数控制器的横向偏差为0.368 m，自适应时域参数控制器的最大横向偏差仅为0.199 m，相差0.169 m，改善效果显著。此时，固定时域参数控制器最大横摆角速度为20.591 (°)/s，已经接近理论上限值21 (°)/s，车辆行驶稳定性变差，而自适应时域参数控制器最大横摆角速度为17.395 (°)/s，行驶稳定性得到明显改善，说明自适应时域参数控制器可用于提高极限工况的行驶安全性。

3 局部避障路径规划与路径跟踪集成验证

将局部路径规划MPC控制器规划层与路径跟踪层MPC控制器集成，形成规划层与控制层集成控制器，如图7所示，其中路径规划层的输出规划路径即为路径跟踪层输入-需要跟踪的参考轨迹。

对上述集成控制器进行仿真验证，其中路径规划MPC控制器参数设置如表6所示。

表6 规划器参数

设置仿真工况如下：总长100 m、宽度3.8 m的直线道路右侧存在一长50 m、宽2 m的静态障碍物，车辆沿右车道以60 km/h的车速直线行驶，全局参考路径为右车道中心线，以验证新型避障功能函数消除过度避障的性能。此时，传统和新型避障函数下的避障功能函数及规划路径仿真结果分别如图8—图9所示。图9中，红色矩形表示静态障碍物。

仿真结果表明：由于传统避障功能函数的设置，使得车辆行驶至障碍物附近时避障功能函数不为零，从而导致局部路径规划器规划出向内的避障路径，带来轻微的横向偏差，最大横向偏差达到0.276 m，存在过度避障，使得行驶稳定性和驾驶舒适性下降。如果此时左侧车道出现车辆，则增大碰撞的可能性。而新型避障功能函数始终为零，使得局部路径规划器在车辆行驶至障碍物附近时规划出的局部路径和全局参考路径重合，车辆没有产生避障动作，表明采用新型避障功能函数的局部路径规划器能够避免过度避障。

相较于传统避障功能函数，新型避障功能函数需增加对障碍物点类型判断的过程，下文对采用STM32H743单片机硬件平台对两者的实时性进行验证。设置实时性验证工况如图12所示。在车载传感器可检测范围内存在3个尺寸为4 m × 2 m物，其中心位置处的车身坐标分别为(6，1)，(11，-2)和(2，-4)，每个障碍物采用60个障碍物点来模拟车载传感器获取的障碍物边缘位置信息。情况1假设存在障碍物1，情况2假设存在障碍物1和2，情况3为假设存在障碍物1、2和3，对应的总障碍物点个数N分别为60、120和180。

分别计算3情况下2种避障功能函数的计算时间。不同避障情况下，计算100次避障功能函数的实时性验证结果如表7所示。

表7 实时性验证结果

由表7可知，在计算100次的情况下，新型避障功能函数的计算时间略高于传统避障功能函数，当障碍物点总个数为180时仅多0.294 ms，但由于规划层采样周期为100 ms，且采样周期内的避障功能函数计算为Np次，远小于100次。因此，新型避障功能函数可在保证实时性的同时，避免过度避障，提高车辆行驶稳定性。

为了验证局部路径规划与控制集成控制器的性能，本文进行多障碍物避障工况仿真验证。仿真工况设置如下：总长150 m、宽度3.8 m的直线道路存在4个尺寸为5 m × 2 m的静态障碍物，其位置坐标分别为(35,0)，(70, 3.8)，(105, 0)和(140, 3.8)。车辆以30 km/h的速度匀速行驶。车辆初始位置为大地坐标系原点。

避障工况示意图如图10所示。规划层规划路径和跟踪层实际路径如图11所示。车辆质心侧偏角和横摆角速度如图12和图13所示。实际路径与规划路径的最大横向偏差、平均横向偏差、平均横摆角偏差、最大质心侧偏角和最大横摆角速度如表8所示。

表8 避障工况仿真结果

由仿真结果可知，车辆在实际行驶过程中能够安全避开所有障碍物，车辆行驶稳定。车辆实际路径与参考路径的横向偏差最大仅为0.293 m，平均横向偏差与平均横摆角偏差均较小。由于初始速度、初始位置与航向设置为零，导致车辆起始质心侧偏角、横摆角速度有较大波动，但在合理范围内。仿真结果表明：局部路径规划与控制器集成控制器能够使车辆在多障碍物工况下规划出安全参考路径，并实现精准跟踪，实现车辆避障功能。

为了验证局部路径规划与控制集成控制器在动态障碍物工况下的性能，本文进行动态避障工况仿真验证。仿真工况设置如下：总长100 m、宽度3.8 m的直线道路存在尺寸为10 m×2 m的动态障碍物，以30 km/h的速度在右侧车道匀速行驶，以模拟行驶中的大型商用车，其初始位置中心坐标为(22.5，0)。车辆以60 km/h的速度匀速行驶，初始位置为大地坐标系原点，其示意图如图14所示。部分时间点时，规划层规划路径、跟踪层实际路径及动态障碍物实时位置如图15所示。

由仿真结果可知：车辆能够安全避开动态障碍物，车辆实际路径与参考路径横向偏差最大仅为8.8 cm。仿真结果表明：局部路径规划与控制集成控制器能够使车辆在动态障碍物工况下规划出安全参考路径，并实现精准跟踪，实现车辆对动态障碍物的避障功能。

4 结 论

根据车辆与障碍物的位置关系设计新型避障功能函数，并基于非线性模型预测控制算法设计局部路径规划器，实现障碍物场景下局部路径的实时规划。理论分析并结合仿真试验获得不同车速下的最优时域参数，设计自适应时域参数路径跟踪控制器，实现车辆对参考路径的精确跟踪。搭建联合仿真平台，对规划层和控制层的集成结构进行仿真验证。

仿真结果表明：新型避障功能函数可在保证实时性的同时，避免过度避障，提高车辆行驶稳定性。自适应时域参数路径跟踪控制器的综合控制性能更佳，可用于提高极限工况的行驶安全性。局部避障路径规划与路径跟踪控制集成结构能够控制车辆在多静态障碍物场景与动态障碍物场景下实时规划出局部避障路径并精确跟踪，实现车辆的安全稳定行驶。