基于时变局部模型的无人驾驶车辆路径跟踪

白国星，周 蕾，孟 宇✉，刘 立，顾 青，王国栋

1) 北京科技大学机械工程学院，北京 100083 2) 北京科技大学顺德研究生院，顺德 528399

路径跟踪控制常用于无人驾驶车辆、移动机器人等移动装备的自动行驶系统，其作用是控制移动装备沿着给定的参考路径行驶[1-2].由于存在较为成熟的无人驾驶车辆数学建模方法，目前基于反馈线性化控制、最优控制、模型预测控制等有模型控制方法的路径跟踪控制研究十分常见[2]，基于无需精确系统模型的滑模控制等方法的研究工作也常采用基于名义模型的设计方法[3].

按照建立数学模型采用的坐标系，可以将上述有模型的路径跟踪控制方法分为两种.其中一种是基于全局模型的路径跟踪控制，如Kang等提出的移动机器人自适应路径跟踪控制方法[4]，赵景波等提出的四轮转向车辆路径跟踪控制方法[5]，Li等提出的履带车路径跟踪控制方法[6]，白国星等提出的以非线性模型预测控制（Nonlinear model predictive control, NMPC）为基础的无人驾驶车辆、移动机器人路径跟踪控制方法[7-11]，Tang等提出的结合模型预测控制与PID（Proportion integral differential）控制的无人驾驶车辆路径跟踪控制方法[12]，邓海鹏等提出的紧急避障路径跟踪控制系统[13]和Klauer等提出的用于城市路径跟踪的控制方法[14]等.基于全局模型的路径跟踪控制方法能够在参考路径曲率较大时正常运行，但是在采用这种方法的路径跟踪控制系统中，通常采用横坐标和纵坐标方向的位移误差作为惩罚项，而根据运动学特性可知移动装备的行驶速度与这两个方向的位移误差之间存在耦合关系[10]且耦合程度还随航向角变化，因此在这种系统中很难通过主动控制行驶速度来提高路径跟踪控制的精确性.

另一种是基于局部模型的路径跟踪控制，如Wang等提出的可变预测时域的路径跟踪控制方法[15]，王玉琼等提出的高速无人驾驶车辆路径跟踪控制系统[16]，林洪振等提出的水田植保机路径跟踪控制方法[17]，Cheng等提出的能够在行驶速度波动的工况下运行的路径跟踪控制系统[18]，Zhang等提出的基于自适应神经网络的路径跟踪控制系统[19]，Hang和Chen提出的四轮转向车辆路径跟踪控制方法[20]，Wang等提出的转向制动一体式路径跟踪控制方法[21]，Liu等提出的采用自适应预瞄距离的路径跟踪控制方法[22]，蔡英凤等提出的结合模型预测控制和PID控制的智能汽车路径跟踪控制方法[23]和孟宇等提出的基于最优控制的铰接式车辆路径跟踪控制系统[24]等.由于在固结于移动装备的局部坐标系中移动装备的航向角通常较小，因此在基于局部模型的路径跟踪控制系统中行驶速度与位移误差之间的耦合关系相对固定.不过上述路径跟踪控制方法大多采用的是以横向误差和航向误差为状态量的数学模型，而采用这种模型的路径跟踪控制方法在参考路径曲率变化幅度较大时精确性较差[11,25]，因此现有的基于局部模型的控制方法较难保证上述工况下路径跟踪控制的精确性.

针对上述研究现状，以前轮转向无人驾驶车辆为研究对象，基于非线性模型预测控制滚动优化的原理以及无人驾驶车辆在局部坐标系和全局坐标系中运动特性不变的特点，提出一种基于时变局部模型的无人驾驶车辆路径跟踪控制方法.该方法通过MATLAB与Carsim联合仿真进行了验证，并与基于全局模型的路径跟踪控制方法、基于局部模型的路径跟踪控制方法以及Stanley路径跟踪控制方法进行了比较研究.

1 时变局部模型

路径跟踪控制系统由全局坐标系向时变局部坐标系转变的过程如图1所示.

图1上部为无人驾驶车辆在全局坐标系中的运动关系，在全局坐标系中，无人驾驶车辆的运动学模型为

图1 坐标转换示意图Fig.1 Coordinate conversion diagram

其中，XGC、YGC为全局坐标系下的横、纵坐标值，m； θGC为全局坐标系下的航向角，rad；vx为纵向速度，m·s-1；vy为横向速度，m·s-1； ω 为航向角速度，也可称为横摆角速度，rad·s-1.

完成坐标转换的第一个步骤如图1中部所示，其目的是将能够满足非线性模型预测控制在一个控制周期内使用的一段参考路径转入局部坐标系.

每个控制周期内的参考路径选取分为两步.第一步是在整个参考路径点列中寻找一个点作为基准点，该基准点可以是参考路径上距离无人驾驶车辆最近的点，具体的选取方法可见文献[26].第二步是在该基准点的前方，选择一系列点作为该控制周期内的参考路径点列，其数量至少与预测步数Np相等，且点列中每两个点之间的弧长为

其中 ，T为迭代周期，s；vxset为期望纵向速度，m·s-1.

该参考路径点列在全局坐标系下可以表示为

其中，XrefGC(t+i|t)为t时刻的第i个参考点的状态向量，XrefGC(t+i|t)为t时刻的第i个参考点的横坐标，m；YrefGC(t+i|t)为t时刻的第i个参考点的纵坐标，m；θrefGC(t+i|t)为t时刻的第i个参考点的航向角，rad.

由于在路径跟踪过程中的每一个确定时刻，都可以测得无人驾驶车辆在全局坐标系中的状态信息，可以表示为

其中，XGC(t|t)为t时刻车辆在全局坐标系中的状态向量，XGC(t|t)为t时刻车辆在全局坐标系中的横坐标，m；YGC(t|t)为t时刻车辆在全局坐标系中的纵坐标，m； θGC(t|t)为t时刻车辆在全局坐标系中的航向角，rad.

通过坐标转换即可得该时刻参考路径点列在以无人驾驶车辆为原点的局部坐标系中的状态信息

其中，Xref(t+i|t)为t时刻的第i个参考点在局部坐标系中的横坐标，m，Yref(t+i|t)为t时刻的第i个参考点在局部坐标系中的纵坐标，m， θref(t+i|t)为t时刻的第i个参考点在局部坐标系中的航向角，rad.

上述参考路径点列的状态信息可以改写为矢量形式

完成坐标转换的第二个步骤如图1下部所示，其目的是给出局部坐标系下的无人驾驶车辆运动学模型.

考虑到坐标转换并不会改变无人驾驶车辆的运动学特性，且根据非线性模型预测控制的滚动优化原理，在每一个控制周期内无人驾驶车辆预测模型的起始位置均不变化，即局部坐标系均不变化，因此局部坐标系中的运动学模型可以表示为

其中 ，X、Y为局部坐标系下的横、纵坐标值，θ为局部坐标系下的航向角，X、Y和 θ的初始值均为0.

在完成一个控制周期后，局部坐标系以无人驾驶车辆在全局坐标系中的位置为原点重新建立，从而形成时变局部坐标系.

在此基础上，可以将上述时变局部坐标系中的运动学模型与车辆动力学模型结合，建立无人驾驶车辆时变局部模型.

对于一般的无人驾驶车辆，通常具有专门的纵向速度控制模块，在路径跟踪控制系统中可以直接采用纵向速度作为控制输入，且左、右侧车轮受到的侧向力基本相同，因此可以采用如图2所示的二自由度车辆单轨模型作为车辆动力学模型.图2中OG表示车辆质心.

图2 车辆受力图Fig.2 Vehicle force diagram

根据图2中的受力关系，可以建立如下动力学微分方程

其中，Fyf、Fyr为前、后轴车轮侧向力，N；δ为车辆前轮等效转角，rad；Lf、Lr为前、后轴与车辆质心的距离，m；m为 车辆质量，kg；Iz为车辆绕垂直方向的转动惯量，kg·m2.

侧向力可以通过魔术公式模型计算

其中，Fy表示侧向力，N；µ表示地面附着系数；Fz表示轮胎垂向载荷，N；α表示侧偏角，rad；a0～a7是魔术公式的参数.

魔术公式也可以表达为函数形式，即

其中，fMF为魔术公式抽象表达式.

前、后轴的垂向载荷为

其中，g是重力加速度，9.8 m·s-2.

前、后轴轮胎侧偏角为

将上述公式联立，并写为矢量的抽象函数形式

其中

2 非线性模型预测控制器

非线性模型预测控制器的设计过程与文献[7-11]中的设计过程相似.非线性模型预测控制器的预测模型通常表达为

其中，Nc为 控制步数，X(t+i|t)为t时刻的第i组预测状态，U(t+i|t)为t时刻的第i组可行的控制输入.

在上述模型中，i为0时X(t+i|t)是t时刻无人驾驶车辆的起始位置，在时变局部坐标系下

由于参考路径点列中仅包含三维信息，因此可以设置上述模型的输出向量为

输出向量与预测状态的关系为

非线性模型预测控制器的优化目标函数可以设计为

其中，Q1为误差惩罚项权重矩阵，Q2为侧向速度惩罚项权重矩阵，Q3为控制量增量惩罚项权重矩阵.

进一步地，权重矩阵Q1可以写为

其中 ，qx、qy和qθ为横坐标方向位移误差权重、纵坐标方向位移误差权重和航向误差权重.为保证跟踪不同航向的参考路径时具有相同的精确性，基于全局模型的路径跟踪控制器中的qx与qy通常相等.而在基于时变局部模型的路径跟踪控制器中，由于航向角通常很小，因此qx与行驶速度之间的耦合度较高，而与横向位移误差的耦合度较低，qy则与横向位移误差的耦合度较高，因此为了控制移动装备尽可能靠近参考路径，可以将qx设置为较小值，而将qy设置为较大值.

在优化目标函数中，还需加入系统的约束条件.对于路径跟踪控制，系统约束条件通常包含前轮转角约束 δlim和前轮转角增量约束 Δ δlim.此外由于基于时变局部模型的路径跟踪控制器引入了无人驾驶车辆纵向速度作为控制输入，因此还需加入纵向速度增量约束 Δvxlim.至此，路径跟踪控制可以等效为求解以下多约束优化问题

求解获得的第一组控制输入即路径跟踪控制器在下一控制周期内的实际控制输入.

3 仿真结果

为验证提出的路径跟踪控制算法，在处理器为Intel(R) Core(TM) i5-10200H CPU @ 2.40GHz的Dell G15 5510型计算机上进行了联合仿真测试.仿真系统中的控制器以S-Function模块的形式搭建于MATLAB R2020a内部的Simulink平台，控制周期设置为50 ms.为了参照比较，除提出的控制器外，仿真中还测试了基于全局模型的路径跟踪控制器、基于局部模型的路径跟踪控制器以及Stanley路径跟踪控制器.三种模型预测控制器的权重系数如表1所示，预测步数均为10，控制步数均为2，Stanley控制器的增益为5.仿真系统中的被控车辆模型由Carsim 2019提供，采用的是“DLC, Constant speed target”模板，其中车辆型号设定为“B-class hatchback 2017 (no ABS)”，参数如表2所示，轮胎模型为185/65 R15，对应的魔术公式参数如表3所示.对于车辆模型的系统约束，考虑到一般无人驾驶车辆前轮转角上限为30°，线控转向系统的前轮转角速度上限可达30°·s-1，因此前轮转角在每个控制周期内的增量上限设置为(30T)°，此外考虑到速度剧烈变化的紧急制动控制通常比路径跟踪控制优先级更高，路径跟踪控制系统的纵向速度约束无需考虑紧急制动，再加上加速度较小（如在2 m·s-2以内）时能够更好地保障乘坐舒适性，因此将纵向速度在每个迭代周期内的增量上限设置为(30T) m·s-1.对于Carsim 2019的环境参数，“Driver controls”部分采用“Constant target speed”模式给定初始速度，“Braking”、“Shifting control”和“Steering”均空置，“Start and stop conditions”部分采用“Run forever”模式.此外，Carsim 2019 中数学模型和输出文件的输出步长均设置为1 ms，控制输入设置为前轮转角和车速，控制输入模式均设置为替换.为了完备地测试控制器的性能，设置了三组仿真.第一组仿真为低速高附着路面仿真，第二组仿真为低速低附着路面仿真，第三组仿真为高速低附着路面仿真.

表1 控制器权重系数Table 1 Weight coefficient of controllers

表2 无人驾驶车辆模型参数Table 2 Parameters of unmanned vehicle model

表3 魔术公式轮胎模型参数Table 3 Parameters of the magic formula tire model

3.1 低速高附着路面仿真

低速高附着路面仿真主要用于模拟晴好天气的城市道路工况，根据我国交通法规，车辆在城市道路的最高行驶速度通常为40 km·h-1，因此本组仿真中无人驾驶车辆的行驶速度初始值设定为40 km·h-1.此外地面附着系数设定为0.8，参考路径设置为U形弯道，弯道半径为25 m.

低速高附着路面仿真结果如图3～图9所示，图中三角形为仿真起始位置，PC表示提出的控制器，GC表示基于全局模型的控制器，LC表示基于局部模型的控制器，SC表示Stanley控制器.由图3可知，在低速高附着工况下，基于时变局部模型和基于全局模型的控制器均可控制无人驾驶车辆跟踪给定的参考路径，基于局部模型的控制器和Stanley控制器无法完成路径跟踪.基于局部模型的控制器性能较差的原因是这种以误差模型作为预测模型的控制器在参考路径曲率变化幅度较大时精确性较差[11,25].Stanley控制器性能较差的原因可能是该控制方法在存在系统约束的情况下性能较差.为了验证该推论，增加了一组删除车辆系统约束后的Stanley控制器仿真实验，并在图3～图9中标记为无约束的Stanley控制器（Stanley controller without constraints）或 SC-C.由图3(f)可知，在没有系统约束的情况下，Stanley控制器可以控制无人驾驶车辆跟踪给定的参考路径.然而考虑到实际车辆的系统约束不可能被轻易删除，因此可以推论提出的基于时变局部模型的控制器相比Stanley控制器具有能够在系统约束影响下实现路径跟踪控制的优势.图4显示了各控制器的位移误差，基于时变局部模型的控制器的位移误差绝对值不超过0.0934 m，基于全局模型的控制器的位移误差最大绝对值为0.1148 m.图5显示了各控制器的航向误差，基于时变局部模型的控制器的航向误差绝对值不超过0.0869 rad，基于全局模型的控制器的航向误差最大绝对值为0.0883 rad.图6显示了无人驾驶车辆的横向速度，各模型预测控制器控制下的横向速度值均较小，Stanley控制器未将横向速度纳入考虑，因此在该控制器控制下的横向速度相对较大.

图3 低速高附着路面仿真轨迹.(a)参考路径; (b)提出的控制器;(c)基于全局模型的控制器; (d)基于局部模型的控制器; (e)Stanley控制器; (f)无约束的Stanley控制器Fig.3 Low-speed and high-adhesion road simulation trajectory: (a) reference path; (b) proposed controller; (c) controller based global model;(d) controller based on local model; (e) Stanley controller; (f) Stanley controller without constraints

图4 低速高附着路面仿真位移误差.(a) PC、GC和SC-C的位移误差; (b) LC和SC的位移误差Fig.4 Displacement error of the low-speed and high-adhesion road simulation: (a) displacement error of PC, GC, and SC-C; (b) displacement error of LC and SC

图5 低速高附着路面仿真航向误差.(a) PC、GC和SC-C的航向误差; (b) LC和SC的航向误差Fig.5 Heading error of the low-speed and high-adhesion road simulation: (a) heading error of PC, GC, and SC-C; (b) heading error of LC and SC

图6 低速高附着路面仿真横向速度.(a) PC、GC和LC的横向速度;(b) SC和SC-C的横向速度Fig.6 Lateral speed of the low-speed and high-adhesion road simulation: (a) lateral speed of PC, GC, and LC; (b) lateral speed of SC and SC-C

图7显示了控制器输出的前轮转角，由图可知各模型预测控制器输出的控制量较为平滑且都在系统约束范围内，而Stanley控制器输出的控制量存在较大幅度的振荡，且由图8可知，Stanley控制器输出的前轮转角显然超出了车辆的前轮转角约束和前轮转角速度约束.图9显示了各模型预测控制器输出的纵向速度，由图可知基于时变局部模型的控制器可以较大幅度主动调节纵向速度.Stanley控制器是一种恒速路径跟踪控制器，在本组仿真中其纵向速度为恒定的40 km·h-1.图10显示了各模型预测控制器的实时性指标，由图可知各控制器在每个控制周期内的时间成本最大值均小于控制周期，因此这三种控制器均可用于实时的无人驾驶车辆路径跟踪控制.Stanley控制器无需在线寻优，其时间成本接近于零，与模型预测控制器的时间成本无可比性，因此未在图中予以比较.

图7 低速高附着路面仿真控制器输出前轮转角.(a) PC、GC和LC的前轮转角; (b) SC和SC-C的前轮转角Fig.7 Front wheel angle output by the controller of the low-speed and high-adhesion road simulation: (a) front wheel angle of PC, GC, and LC;(b) front wheel angle of SC and SC-C

图8 Stanley控制器输出的前轮转角和执行器执行的前轮转角Fig.8 Front wheel angle output by the Stanley controller and the front wheel angle act by the actuator

图9 低速高附着路面仿真控制器输出纵向速度Fig.9 Longitudinal speed output by the controller of the low-speed and high-adhesion road simulation

图10 低速高附着路面仿真时间成本Fig.10 Time cost of the low-speed and high-adhesion road simulation

3.2 低速低附着路面仿真

低速低附着路面仿真主要用于模拟雨雪天气的城市道路工况，仿真中无人驾驶车辆的行驶速度初始值设定为40 km·h-1，地面附着系数为0.5，参考路径为U形弯道，弯道半径为25 m.考虑到在低速高附着工况下已经证明了基于局部模型的控制器和Stanley控制器性能不佳，而本组仿真的工况更加趋于极限，因此本组仿真不再将基于局部模型的控制器和Stanley控制器纳入比较.

低速低附着路面仿真结果如图11～图13所示，各控制器在图中的标记与在低速高附着路面仿真结果中的标记相同.图11显示了各控制器的位移误差和航向误差.基于时变局部模型的控制器的位移误差绝对值不超过0.1707 m，基于全局模型的控制器的位移误差最大绝对值为0.3199 m.即相比基于全局模型的控制器，基于时变局部模型的控制器将位移误差最大绝对值减小了46.64%.基于时变局部模型的控制器的航向误差绝对值不超过0.0913 rad，基于全局模型的控制器的航向误差最大绝对值为0.0942 rad.图12显示了控制器输出的前轮转角和纵向速度，由图可知基于时变局部模型的控制器可以较大幅度主动调节纵向速度，而基于全局模型的控制器则只能在较小范围内进行调节.

图11 低速低附着路面仿真位移误差和航向误差.(a)位移误差;(b)航向误差Fig.11 Displacement and heading errors of the low-speed and lowadhesion road simulation: (a) displacement error; (b) heading error

图12 低速低附着路面仿真控制器输出前轮转角和纵向速度.(a)前轮转角; (b)纵向速度Fig.12 Front wheel angle and longitudinal speed output by the controller of the low-speed and low-adhesion road simulation: (a) front wheel angle; (b) longitudinal speed

图13 低速低附着路面仿真横向速度Fig.13 Lateral speed of the low-speed and low-adhesion road simulation

图13显示了无人驾驶车辆的横向速度，各控制器控制下的横向速度较低速高附着工况下的横向速度略大，且有较明显的增加趋势，不过基于时变局部模型的控制器和基于全局模型的控制器均能对该趋势起到一定的遏制作用，其中基于时变局部模型的控制器抑制作用更加明显.在实时性方面，各控制器的时间成本与在低速高附着工况下的时间成本基本一致，此处不再赘述.

3.3 高速低附着路面仿真

为了进一步测试控制器的性能，在高速低附着工况下也进行了仿真试验.在本组仿真中，无人驾驶车辆的行驶速度初始值设定为60 km·h-1，地面附着系数为0.5，参考路径为U型弯道.考虑到前述低速低附着工况已经是一种极限工况，如果在不改变参考路径的前提下提高车速，车辆的离心加速度会超过地面附着极限，此时所有控制器均无法完成路径跟踪，因此在本组仿真中弯道半径改为了55 m.此外由于前轮转角速度约束会导致车辆出现转向不足的现象[1,27]，车速越高该约束对路径跟踪控制精确性的影响越大，而前两组仿真中控制器的权重系数主要考虑在低速极限工况下通过惩罚侧向速度来调节纵向速度，未考虑前轮转角速度约束对路径跟踪控制精确性的影响，因此在本组仿真中对权重系数进行了面向高速工况的适应性调整，基于时变局部模型的控制器和基于全局模型的控制器的纵坐标方向位移误差权重均改为100，航向权重均改为10.

高速低附着路面仿真结果如图14～图16所示，各控制器在图中的标记与在低速高附着路面仿真结果中的标记相同.在高速低附着路面仿真中，基于时变局部模型的控制器可以控制无人驾驶车辆跟踪给定的参考路径，而基于全局模型的控制器则无法完成路径跟踪.图14显示了各控制器的位移误差和航向误差，基于时变局部模型的控制器的位移误差绝对值不超过0.3342 m，航向误差绝对值不超过0.0505 rad.图15显示了控制器输出的前轮转角和纵向速度，这些状态的变化趋势与前两组仿真结果中的变化趋势一致.图16显示了各控制器控制下无人驾驶车辆的横向速度，各控制器控制下的横向速度值均较小.在实时性方面，各控制器表现正常，此处不再赘述.

图14 高速低附着路面仿真位移误差和航向误差.(a)位移误差;(b)航向误差Fig.14 Displacement and heading errors of the high-speed lowadhesion road simulation: (a) displacement error; (b) heading error

图15 高速低附着路面仿真控制器输出前轮转角和纵向速度.(a)前轮转角; (b)纵向速度Fig.15 Front wheel angle and longitudinal speed output by the controller of the high-speed low-adhesion road simulation: (a) front wheel angle; (b) longitudinal speed

图16 高速低附着路面仿真横向速度Fig.16 Lateral speed of the high-speed low-adhesion road simulation

4 结论

（1）本文提出的基于时变局部模型的路径跟踪控制器具有较高的精确性，在本文的所有仿真结果中，该控制器的位移误差绝对值不超过0.3342 m，航向误差绝对值不超过0.0913 rad.

（2）相比基于局部模型的路径跟踪控制器和Stanley路径跟踪控制器，基于时变局部模型的路径跟踪控制器具有能够在系统约束下跟踪曲率变化幅度较大的参考路径的优势，在低速高附着路面仿真中，基于局部模型的路径跟踪控制器和Stanley路径跟踪控制器均控制失败，而基于时变局部模型的路径跟踪控制器性能较佳，位移误差最大绝对值仅为0.0934 m.

（3）时变局部模型有助于解决路径跟踪控制中纵向速度与位移误差之间的耦合关系影响精确性的问题，在低速低附着路面仿真中，相比基于全局模型的路径跟踪控制器，基于时变局部模型的路径跟踪控制器可以将位移误差的最大绝对值减小46.64%，在高速低附着路面仿真中，基于时变局部模型的路径跟踪控制器可以完成路径跟踪，而基于全局模型的路径跟踪控制器控制失败.