基于DBO-LQR和MPC的智能车轨迹跟踪控制

摘要：为提高智能车辆轨迹跟踪的精度和稳定性，提出1种基于蜣螂优化（DBO）算法优化的线性二次型调节器（LQR）与模型预测控制（MPC）的横向、纵向控制策略。构建车辆动力学模型和基于Frenet坐标系下的横向误差模型，设计带有前馈的横向LQR控制器，利用蜣螂优化算法确定LQR控制器权重系数；基于MPC实现纵向速度和位置的跟踪，利用纵向速度联结横向控制器与纵向控制器，同时对车辆的速度和转向进行控制；最后基于CarSim和Matlab/Simulink联合仿真平台在不同道路工况下进行仿真实验，验证所提策略的有效性。结果表明：在城市道路泊车、城市道路换道、高速公路换道3种工况下，车辆的最大横向跟踪误差均小于0.010m、航向偏差在0.0250rad内；横摆角速度及前轮转角变化比较平稳、无明显抖动，所提策略可有效提高车辆跟踪轨迹的精度和稳定性。

关键词：智能车辆；轨迹跟踪；横纵向控制；蜣螂优化算法（DBO）；线性二次型调节器（LQR）；模型预测控制（MPC）中图分类号：TP273文献标志码：A doi：10.12415/j.issn.1671−7872.24015

Intelligent Vehicle Trajectory Tracking Control Based on DBO-LQR and MPC

CHENG Chao1，HE Rongbo1，2，HE Haoran1，YAN Qiang1

（1.School of Electricalamp;Information Engineering，Anhui University of Technology，Maanshan243032;China; 2.Intelligent Vehicle Technology Center，Chery Automobile Co.，Ltd，Wuhu241000，China）

Abstract：To improve the accuracy and stability of intelligent vehicle trajectory tracking，a transverse and longitudinal control strategy based on dung beetle optimization（DBO）algorithm optimized linear quadratic regulator （LQR）with model predictive control（MPC）was proposed.A vehicle dynamics model and atransverse error model based on Frenet coordinate system were constructed，the transverse LQR controller with feedforward was designed，and the DBO algorithm was used to determine the weight coefficient of the LQR controller.The longitudinal speed and position tracking were realized based on MPC，and the longitudinal speed was used to connect lateral and longitudinal controllers，while controlling the speed and steering of the vehicle at the same time.Finally，simulation experiments were conducted on different working conditions using the CarSim and Matlab/Simulink joint simulation platform to verify the effectiveness of the proposed strategy.The results show that the maximum lateral tracking error of the vehicle is less than0.010m and the heading deviation is within0.0250rad under three working conditions of parking on urban road，lane changing on urban roads，and lane changing on highways.The changes of transverse angular velocity and front wheel angle are relatively smooth without obvious jitter，and the proposed strategy can effectively improve the accuracy and stability of the tracking trajectory of the vehicle.

Keywords：intelligent vehicle;trajectory tracking;transverse longitudinal control;DBO;LQR;MPC

随着人们对安全舒适、节能省时交通服务需求的日益增强，众多国内外汽车公司投入大量人力和物力开展智能驾驶车辆的研发。智能车辆结合了智能制造技术和线控电动车技术的特点和优势，不仅可减轻驾驶者面对复杂驾驶环境的压力，同时还可显著提升车辆行驶的安全性和舒适性[1−2]。智能车 横向控制与纵向控制是智能驾驶的核心技术之一，可用于处理规划轨迹、车辆位置信息和输出相应控制命令，确保车辆沿预定轨迹行驶[3]。然而，如何确 保车辆沿规划轨迹精准行驶并达到预期跟踪精度是智能车辆领域的一大难题[4]。因此，研究轨迹跟踪 控制的精度和车辆稳定性，对于保证车辆乘员生命、财产安全和提升乘坐舒适性具有重要意义。

目前，常用的轨迹跟踪控制方法有纯跟踪（pure pursuit）控制[5]、比例微分积分（proportion integration differentiation，PID）控制[6]、滑模控制[7]、模型预测控制（model predictive control，MPC）[8]以及线性二次型调节器（linear quadratic regulator，LQR）控制[9]等，国内外学者基于不同的控制方法对智能车辆轨迹跟踪进行了研究。Chen等[10]提出1种基于自适应模糊PID控制的轨迹跟踪方法，该方法在多种工况下跟踪预定轨迹效果出色，且具有较高的稳定性，但实时性较差；李寿涛等[11]设计了1种滑模变结构控制策略，通过对横摆角速度及质心侧偏角参量的跟踪实现对车辆的横向控制，但车辆的纵向速度存在较大偏差；Attia等[12]针对全自主制导结构下汽车横纵向控制的耦合问题，采用非线性模型预测控制（nonlinear model predictive control，NMPC）进行转向控制，但纵向控制中速度等相关动力学条件难以约束；王柏林等[13]设计1种基于蚁狮优化（ant lion optimization，ALO）算法优化的自适应权重系数LQR控制器，对车辆进行路径跟踪，在不同车速下皆具有较好的鲁棒性，但仅考虑了车辆横向控制，未考虑纵向控制。

LQR控制算法的评价指标为受控线性系统状态变量的二次型积分，通过求解Riccati方程获得最优控制律，实现闭环控制目标[14]。此方法对硬件算力要求低，应用前景广阔。但为保证控制效果，设计LQR横向控制器时会对系统模型进行简化，导致产生一定的稳态误差；LQR控制器的权重系数固定，难以应对不同车速或复杂道路状况，适应性差，跟踪控制精度不高。鉴于此，建立自行车动力学模型和Frenet坐标系下的横向误差模型，采用经蜣螂优化算法（dung beetle optimization，DBO）优化带有前馈的LQR控制器进行横向控制，同时采用考虑距离偏差、航向角偏差及前轮转角的LQR控制器权重系数优化车辆轨迹跟踪效果，以期提高智能车辆轨迹跟踪的精度和稳定性。

1车辆轨迹跟踪误差模型的建立

1.1自行车模型

考虑到车辆行驶工况为低速、中速、高速3种工况，同时考虑到轮胎的侧偏特性，车辆在高速行驶情况下轮胎会发生较大形变，对于控制模型采用动力学模型。将汽车前后两轮各简化为1个车轮，构建二自由度自行车动力学模型，如图1。图中：β为质心侧偏角；vx，vy分别为车辆纵向、横向速度；a，b分别为车辆质心到前后轴的距离；Iz为质心绕z轴的转动惯量；δ为前轮转角；Fyf，Fyr分别为前后轮所受的侧向力；αf，αr分别为前后轮的侧偏角。

假设前轮转角δ很小，则cos δ≈1。据此对模型进行受力分析：

式中：m为整车质量；为车辆沿y轴运动的加速度；vxφ.为车辆向心加速度；φ为车辆横摆角；φ.为车辆横摆角速度；v.y为车辆横向加速度；Cαf，Cαr分别为车辆前后轮的侧偏刚度。由上述假设可求出前后车轮的侧偏角：

将侧偏角方程式（2）代入牛顿第二定律公式可得：

2跟踪控制器的设计

2.1横向控制器

LQR控制算法表现出较高的稳定性，仅需较小的控制量即可使系统误差趋于零，同时具有调整不稳定系统的能力[16]。因此，文中利用LQR控制算法设计车辆横向跟踪控制器。

2.1.1LQR控制

采用前向欧拉法和中点欧拉法使控制量和状态量离散化，得到的离散状态空间方程为

LQR控制的核心在于线性约束条件下寻求控制量与状态量的最优控制规律，为兼顾跟踪误差和能量损耗，定义性能函数为

式中：Q，R分别为性能函数对应的状态权重矩阵与控制权重矩阵。因此，可将路径横向跟踪问题转变为在约束式（11）的条件下求解代价函数式（12）的最小值问题。

LQR控制器的反馈控制规律为

式中：K=（R+BT PB）−1BT PA，为LQR控制器的增益系数；P为使黎卡提（Riccati）方程式（14）成立的正定常量解。

P=Q+ATPA−AT PB（R+BT PB）−1BT PA（14）

引入横纵向误差量e，将前轮转角δ作为控制量，可得到LQR控制器最优前轮转角控制规律：

δ（k）=−K·e（k）（15）

式中K=[k1k2k3k4]，为LQR控制器的最优控制律反馈矩阵。

2.1.2前馈控制器

基于轨迹跟踪误差模型计算出LQR控制器最优前轮转角时，模型简化处理会给被控系统带来无法规避的稳态误差。为消除稳态误差，需通过额外引入前馈转角控制量δf使e尽可能趋向于0，则重新定义全状态反馈控制与误差微分方程：

−Ke+δf）+Cθ（16）

当控制稳定后得到方程式（17）：

{′（A−BK）−1（Bδf+Cθ）（17）

当横向误差ed为零时，解稳态误差方程式（17）得到前轮转角前馈量：

式中k3为LQR控制器反馈矩阵K的第3列。

2.2基于蜣螂优化算法的LQR权重优化策略

LQR控制中Q和R权重系数分别代表对系统状态量和控制量的限制效果，影响车辆轨迹跟踪的效果，可写成以下矩阵形式：

Q=diag[q1，q2，q3，q4]，R=[r]（19）

一般情况下，Q内的元素值增大可使系统响应更快；R内的元素值越大，对控制的量限制越大。对于LQR控制器的权重系数，确定过程复杂且通常靠经验确定。蜣螂优化器是Xue等[17]模拟蜣螂的滚球、跳舞、觅食、偷窃和繁殖等行为提出的1种新的群智能算法，具有收敛迅速、求解精度高的优点。故文中采用蜣螂优化算法确定加权矩阵Q，R对角线的3个参数q1，q3和r。为同时确保行驶平稳性和轨迹跟踪能力，采用对横向距离偏差ed、航向角偏差eφ和前轮转角δ进行加权处理的平方和作为蜣螂算法优化的适应度函数[18]：

E=ψ1e+ψ2e+ψ3（Δδ）2（20）

由于侧重于轨迹跟踪的效果，设置加权系数ψ1=0.4，ψ2=0.35，ψ3=0.25；迭代次数最大为30，蜣螂种群初始的个体数为30。优化算法流程图如图2。

由图2可看出，DBO算法的具体优化步骤如下：

1）蜣螂种群初始化，赋予每个蜣螂初始位置；

2）赋初值给权重系数q1，q3，r；

3）求解出最优控制反馈矩阵K，并求出最优前轮转角，作用于车辆模型，得到性能指标；

4）根据求得的适应度函数值，判断是否满足终止条件，若达到则结束循环，输出最优权重系数q1，q3，r，反之依据式（20）更新蜣螂最优位置，重复上述步骤。

蜣螂优化算法迭代优化LQR控制器参数的结果如图3。

由图3可看出：在迭代优化早期速度较快，最后收敛于第15次迭代，最优权重系数q1，q3，r分别稳定于50，7.2491和3.3549。

为验证DBO优化LQR控制器在路径跟踪中的可行性与优越性，选择一段大曲率S形路径，采用ALO优化LQR控制器、人工固定权重系数LQR控制器与DBO优化LQR控制器使车辆跟踪参考路径，结果如图4，5。

由图4，5可知：与ALO优化LQR控制器和人工固定权重系数LQR控制器相比，DBO优化LQR控制器在跟踪参考路径上表现更好，车辆轨迹与参

考路径的吻合度更高；ALO优化LQR控制器横向跟踪误差维持在0.25m以内，而DBO优化LQR控制器横向跟踪误差显著减小，维持在0.16m以内。最终设置LQR的权重矩阵Q=diag（50，1，7.2491，1），r=3.3549，控制器跟踪目标轨迹效果良好。

2.3纵向跟踪控制器

纵向控制器分为上下2层，上层MPC控制器求解期望加速度，下层控制器根据期望加速度控制执行机构使车辆达到最佳纵向状态。

2.3.1预测模型建立

假设车辆横摆运动幅度较小，则有

以一阶惯性系统来近似建立下层控制器，如式（22）。

式中：τ为惯性时间常数；ax为纵向加速度；ap为期望纵向加速度。车辆纵向运动方程可描述如式（23）。

取状态量ξ=[v，a]T，控制量u=ap，输出量O（t），将式（23）描述为状态空间表达式。

式中t，t+1，T分别为当前采样时间点、下一周期采样时间点和采样周期。

将预测方程写成矩阵形式，如式（25）。

U（t）=[u（t）···u（t+Nc−1）]；Np，Nc分别为预测时域和控制时域。

2.3.2优化求解与反馈

建立预测受控车辆状态模型后，需进行下一步滚动优化。在t时间点的状态优化问题可表述为：确定从该时间点起的n个控制量u（t），u（t+1），u（t+2），···，u（t+m−1），使受控车辆在其影响下，在未来的j个时间点最大限度地逼近期望状态，防止车辆速度的急剧变化，优化性能指标可通过以下向量形式表述：

其中：S，W分别为状态和控制权重矩阵；Yp为输出期望值的向量。此外，受控车辆的期望加速度还需符合如下硬约束：

amin≤a（t）≤amax（27）

式中amin和amax为最小期望加速度和最大期望加速度。

由此，式（26），（27）构成了1个完整的优化目标，对其进行求解，得到受控车辆在未来时刻期望加速度的控制序列。直接求解这些优化性能指标较为复杂，通过矩阵运算将优化性能指标转化为易于解决的二次规划问题，使MPC控制器每一步都能有效进行滚动优化。

n）J2=U（t）T·Ht·U（t）+2G·U（t）（28）

其中Ht=ΘTSΘ+W，Gt=2（φξ一Yp）SΘ。

使用二次规划（QP）求解器对式（27），（28）进行求解，得到Nc内的控制输入增量。

将增量序列的第一个元素作为实际输入增量作用于控制器，实现滚动优化，最后进行反馈校正。受控车辆的状态ξ（t）是可测量的，在每个采样时间点测量的ξ（t）可用来为该时间点预测和优化起始定位，这样预测与优化都是基于受控车辆实时反馈基础上的，反馈校正得以实现。

2.3.3油门刹车标定表的生成

研发自动驾驶系统主要用于电动车辆，因此需在CarSim中构建1个电动机模型，电机最大功率Pmax为125kW，电机最大扭矩Tmax为258N/m。文中油门刹车标定表采用传统办法生成，即通过不同程度的踏板踩压生成不同的速度v和加速度a曲线，将其整合得到v一a曲线。

3轨迹跟踪仿真验证

将横向控制器和纵向控制器耦合形成联合控制结构，实现对轨迹和速度的同时跟踪[19]，横纵向控制流程如图6。为验证本文提出的横纵向控制策略的可行性及有效性，设计基于五次多项式曲线的参考轨迹，设置车辆参数及仿真工况，在CarSim和Matlab/Simulink环境下进行仿真实验。

3.1参考轨迹设置

五次多项式曲线曲率平滑连续、突变较小，文中采用五次多项式曲线设计参考轨迹，以使跟踪效果更理想，车辆纵横向位移可表示为：

式中：x（t），y（t）为车辆纵横向位移；a0，a1，···，a5及b0，b1，···，b5为待定系数。

此外，根据城市道路工程设计规范[20]，参考轨迹需符合起点和终点的限制条件：

其中xend为终点横坐标，通过设定不同的限制条件，求解式（28）对应的待定系数a0，a1，a2，a3，a4，a5，b0，b1，b2，b3，b4，b5；同时，将y（x）转化为y（t），方便纵坐标的求解：

然后求解期望轨迹的切线角θp和曲率kp。

最后参考轨迹还需限定加速度的范围：

3.2车辆参数及仿真工况设置

采用CarSim2019.1版本、Matlab2020b版本搭建CarSim/Matlab仿真平台，在CarSim中选择C级轿车，车辆参数设置如表1。设置的仿真工况为城市道路泊车、城市道路换道、高速公路换道3种，仿真测试时间为16s，具体工况参数如表2。

3.3仿真结果分析

在Matlab/Simulink中构建车辆控制器，由控制模块计算出车辆油门、刹车及方向盘的控制律，将控制律传递给CarSim中的车辆模型，采用DBO-LQR和MPC控制算法对车辆在3种工况下进行横纵向轨迹跟踪控制，结果如图7。

由图7可知：在城市道路泊车工况下，车辆在启动阶段实际车速相比期望车速存在少许超调，车辆实际横向、纵向位移跟踪规划效果较好，最大横向跟踪偏差小于0.010m，航向偏差在0.0250rad内，表明控制器具有高精度的跟踪能力和良好的横向稳定性；前轮转角和横摆角速度未出现明显抖动，变化平稳，表明控制器的鲁棒性较好。在高速公路换道工况下，车辆在0～2s启动阶段实际车速相比期望车速存在少许超调，超调量较城市道路泊车工况减小；车辆实际横向、纵向位移跟踪效果较好，跟踪轨迹横向误差小于0.008m，航向偏差在0.0008rad内，与城市道路泊车工况相比，车辆在此工况的跟踪精度更高、前轮转角和横摆角速度更小，变化平稳。在城市道路换道工况下，车辆在0～2s启动阶段实际车速相比期望车速存在较小偏差，车辆实际横向、纵向位移跟踪规划效果较好，跟踪轨迹的横向误差均小于0.006m，航向偏差在0.0005rad内，与其他2种工况相比，控制器在城市道路换道工况下跟踪精度更高；前轮转角和横摆角速度相较于高速公路换道工况增大，但变化仍然保持平稳。

综上：采用经DBO优化参数后的LQR控制器对车辆横向控制更精确，跟踪参考轨迹精度更高；提出的基于DBO-LQR和MPC的横纵向控制策略，能完成多种工况下的轨迹跟踪，在兼顾车辆稳定性的同时具有较高的跟踪精度。

4结论

为提高智能车辆轨迹跟踪的精度和稳定性，基于二自由度自行车动力学模型和Frenet坐标系下的横向误差模型，提出1种基于DBO-LQR和MPC的横纵向控制策略，控制车辆跟踪参考轨迹，基于CarSim和Matlab/Simulink联合仿真平台在城市道路泊车、城市道路换道、高速公路换道3种工况下进行仿真实验，所得主要结论如下：

1）与ALO优化LQR控制器和人工固定权重参数的LQR控制器相比，采用DBO优化的LQR控制器对车辆横向控制更精确、跟踪参考轨迹的精度更高。

2）采用DBO-LQR和MPC策略可同时对车辆横向、纵向轨迹跟踪进行控制，且在城市道路泊车、城市道路换道、高速公路换道3种工况下跟踪五次多项式规划轨迹的效果均较好，横向误差与航向偏差均较小。提出的策略能完成多种工况下的轨迹跟踪，在兼顾车辆稳定性的同时具有较高的跟踪精度。

参考文献：

[1]白国星，孟宇，刘立，等.无人驾驶车辆路径跟踪控制研究现状[J].工程科学学报，2021，43（4）：475−485.

[2]CHEN L，LI Y，HUANG C，et al.Milestones in autonomous driving and intelligent vehicles：survey of surveys[J].IEEE Transactions on Intelligent Vehicles，2023，8（2）：1046−1056.

[3]徐明泽，刘清河.基于LQR和PID的智能车轨迹跟踪控制算法设计与仿真[J].太原理工大学学报，2022，53（5）：877−885.

[4]熊璐，杨兴，卓桂荣，等.无人驾驶车辆的运动控制发展现状综述[J].机械工程学报，2020，56（10）：127−143.

[5]王瑞.基于纯追踪算法和樽海鞘优化算法的无人驾驶路径跟踪算法研究[D].长春：吉林大学，2021.

[6]ROSOLIA U，BRUYNE SD，ALLEYNE AG.Autonomous vehicle control：a nonconvex approach for obstacle avoidance[J].IEEE Transactions on Control Systems Technology，2017，25（2）：469−484.

[7]张鑫，秦东晨，谢远龙，等.基于双环自适应滑模的移动机器人轨迹跟踪控制[J].合肥工业大学学报（自然科学版），2024，47（1）：13−20.

[8]苑风霞，张华，陈丰，等.基于模型预测和遗传算法的智能车辆轨迹跟踪控制[J].安徽工业大学学报（自然科学版），2021，38（4）：393−400.

[9]HANG P，CHEN XB，LUO FM，et al.Robust control of afour-wheel-independent-steering electric vehicle for path tracking[J].SAE International Journal of Vehicle Dynamics，Stability，and NVH，2017（1）：307−316.

[10]CHEN XB，BAO QL，ZHANG B.Research on4WIS electric vehicle path tracking control based on adaptive fuzzy PID algorithm[C]//Chinese Control Conference（CCC）.Guangzhou：IEEE，2019：6753−6760.

[11]李寿涛，李秋媛，刘辉，等.可实现车辆稳定性控制的滑模变结构策略[J].吉林大学学报（工学版），2019，49（4）：1288−1292.

[12]ATTIA R，BASSET M，ORJUELA R.Combined longitudinal and lateral control for automated vehicle guidance[J].Vehicle System Dynamics：International Journal of Vehicle Mechanics and Mobility，2014，52（2）：260−280.

[13]王柏林，李云伍，赵颖，等.基于蚁狮算法优化的LQR横向跟踪控制策略[J].重庆理工大学学报（自然科学版），2023，37（4）：27−38.

[14]张义奇，牛礼民，朱奋田.基于LQR的车道保持辅助控制策略[J].安徽工业大学学报（自然科学版），2022，39（1）：43−52.

[15]余米森，钱玉宝，黄华宝，等.连续工况下基于PID+LQR算法的自动驾驶车辆横纵向耦合控制[J].科学技术与工程，2022，22（30）：13490−13496.

[16]周卫琪，赵羿寒，刘擎超，等.基于改进LQR的车辆路径跟踪横向控制策略[J].华中科技大学学报（自然科学版），2024，52（3）：135−141.

[17]XUE J，SHEN B.Dung beetle optimizer：a new meta-heuristic algorithm for global optimization[J].The Journal of Super Computing，2023，79：7305−7336.

[18]范秋霞，张珂，徐磊，等.基于蛇算法的主动悬架线性二次型调节器优化设计[J].科学技术与工程，2024，24（9）：3852−3860.

[19]韩天园，沈永俊，鲍琼，等.基于类人决策与横纵向协同的车辆弯道自适应巡航控制策略[J].中国公路学报，2023，36（10）：211−223.

[20]中华人民共和国住房和城乡建设部.城市道路工程设计规范：CJJ37—2012[S].北京：中国建筑工业出版社，2012.

责任编辑：何莉