基于机理 + 数据补偿模型的车辆轨迹跟踪控制

Vehicle Trajectory Tracking Control Based on Mechanism + Data Compensation Model

摘要: 针对无人驾驶车辆在制造和装配过程中的精度限制导致车辆的实际物理参数和设计值不一致性的问题。本文提出了一种机理与数据驱动补偿模型的线性二次调节器(LQR)控制策略。通过线性回归误差补偿对观光车机理模型进行优化，提高了模型的准确性和稳定性。基于补偿后的模型，设计了LQR路径跟踪控制器，通过优化二次型性能指标确定系统最优控制序列。仿真结果证实，与未补偿模型相比，该策略有效提高路径跟踪精度和维持系统稳定性，展示了机理与数据驱动补偿的LQR控制策略在无人驾驶车辆路径跟踪控制中的有效性和优越性。

Abstract: Aiming at the problem of inconsistencies between actual physical parameters and design values caused by precision limitations in the manufacturing and assembly process of unmanned vehicles. In this paper, a linear quadratic regulator (LQR) control strategy combining mechanism model and data-driven compensation is proposed. Through linear regression error compensation, the sightseeing vehicle mechanism model is optimized, and the accuracy and stability of the model are improved. Based on the compensated model, the LQR path tracking controller is designed, and the optimal control sequence is determined by optimizing the quadratic performance index. Simulation results confirm that compared with the uncompensated model, the proposed strategy effectively improves the path tracking accuracy and maintains the system stability, demonstrating the effectiveness and superiority of the LQR control strategy based on the fusion mechanism and data-driven compensation in the path tracking control of unmanned vehicles.

1. 引言

随着人工智能、物联网、大数据等技术的飞速发展，一些无人驾驶控制系统在许多领域逐渐发展为主力，无人驾驶观光车的出现，不仅提升了游客的出行体验，还为景区管理带来了革命性的变化。它们在无需人工驾驶的情况下，能够安全、高效地穿梭于景区之间，提供给乘客全新的游览方式。

无人驾驶系统由环境感知、决策、规划及控制四部分组成，路径跟踪是无人驾驶汽车运动控制模块的基本组成部分，其控制算法直接影响着路径跟踪效果。而车辆建模作为控制策略的基础，不仅涉及对车辆行驶轨迹、转向和速度变化等的精确描述，更是车辆控制策略设计、路径规划和避障算法开发的基础。文献[1]中提出模型预测控制和转角补偿结合的轨迹跟踪控制策略，设计策略控制智能汽车的换道轨迹跟踪效果更好，且横摆角速度和质心侧偏角峰值明显降低，稳定性更好。文献[2]中针对高速列车运行中建模简化等产生误差的问题，提出了一种基于模型补偿的状态反馈预测控制方法，提高了控制精度。

路径跟踪控制技术可大致分为三类：基于经典控制理论的控制方法、基于智能控制理论的控制方法和基于现代控制理论的控制方法[3]。文献[4]中分析了不同工况下路径跟踪算法的原理、优缺点、应用及目前的研究进展。目前常用的控制算法有比例–积分–微分(PID)控制、滑模控制(SMC)、模型预测控制(MPC)、LQR等。经典控制方法的特点是简单易于实现和调试，但是应对非线性和复杂的横向控制时表现不佳；滑膜控制具有较好的鲁棒性，但是设计过程较为复杂；模型预测控制可以处理系统的不确定性和约束，具有较好的实时性和鲁棒性，但是计算复杂度较高，实际应用可能受限[5]。MPC在轮式机器人的路径跟踪控制中得到了积极的研究，参见[6]进行全面综述。文献[7]中设计鲁棒控制律并构建上层多目标约束RMPC控制器，提高了跟踪精度。Jie Wang等[8]将基于高斯过程(GP)的学习和反馈线性化(FBL)与模型预测控制(MPC)相结合(GP-FBLMPC)，可泛化地减少不同路径的路径跟踪误差。王文娟等[9]人则提出了一种结合径向基神经网络(RBF)和滑模控制的自适应控制律，通过降低切换增益来提高移动机器人和车辆平台路径跟踪控制的稳定性。文献[10]设计了一种自适应滑模控制器，减少了传统滑模控制的高频振荡现象，提高了控制器的平滑性和稳定性。文献[11]在此基础上设计了前馈 + 反馈的LQR转向控制器,结果表明跟踪精度有所提高。文献[12]设计了一种自适应预测时间的前馈LQR控制器，提高了车辆在不同车速的稳定性。文献[13]提出了一种模糊比例积分-LQR的内外环控制算法，提升了无人驾驶农机装备转向的精准控制。文献[14]中基于二自由度横向动力学模型，利用LQR将路径跟踪问题转化为带有动态干扰的最优控制问题提高了跟踪精度。

针对以上的内容，展现出了在解决系统模型不确定性以及提高控制精度和增强稳定性的创新控制策略和方法，但是模型的不确定性以及动态变化仍然制约着其控制性能的上限，模型补偿作为提高控制鲁棒性和适应性的有效手段，可以减少模型与实际系统之间的偏差，为提升控制策略的适应性提供重要支撑。本文针对无人驾驶车辆实际物理参数与设计值之间存在结构性误差的问题，提出了机理 + 数据驱动补偿模型的LQR控制策略，通过仿真结果验证了所提工作的有效性。

2. 基于机理与数据驱动的车辆运动模型

2.1. 车辆机理模型

由于景区内无人驾驶观光车的运动速度较低，且后轮为非转向轮，不易出现侧滑现象，因此选取后轴中点的运动轨迹来代替汽车的运动轨迹，建立如图1所示的两驱车模型示意图。

Figure 1. Schematic diagram of two-wheel drive mechanism model

图1. 两驱车机理模型示意图

图1中，x和y分别表示车辆后轴中点的横坐标和纵坐标，v表示车辆速度，$\psi$ 表示车身角，${\delta }_f$ 表示汽车前轮转角，L表示前后轮之间的轴距。车辆机理模型方程可表示为：

$\begin{array}{l}\dot{x}=v\cos \left(\psi \right)\\ \dot{y}=v\sin \left(\psi \right)\\ \dot{\psi }=\frac{v\tan \left({\delta }_f\right)}{L}\end{array}$ (1)

因实际应用中，微处理器处理信号都是数字信号，因此需要将模型(1)离散化处理为：

$\begin{array}{l}x\left(k+1\right)=x\left(k\right)+v\cdot \cos \left(\psi \left(k\right)\right)T\\ y\left(k+1\right)=y\left(k\right)+v\cdot \sin \left(\psi \left(k\right)\right)T\\ \psi \left(k+1\right)=\psi \left(k\right)+\frac{v\cdot \left(\tan \left({\delta }_f\left(k\right)\right)\right)T}{L}\end{array}$ (2)

式中：T表示控制周期。

上述车辆机理模型亦可表示为：

$Y\left(k+1\right)=\tilde{A}\left(k\right)\cdot Y\left(k\right)+\tilde{B}\left(k\right)\cdot U\left(k\right)$ (3)

由车辆运动学模型离散化后，针对其中非线性项进行线性化可表示为：

$Y\left(k+1\right)=Y\left(k\right)+\left[\begin{array}{cc}T\cos \left(\psi \left(k\right)\right)& 0\\ T\sin \left(\psi \left(k\right)\right)& 0\\ \frac{T\tan \left({\delta }_f\left(k-1\right)\right)}{L}& \frac{v\left(k-1\right)T\left(1-{\tan }^2\left({\delta }_f\left(k-1\right)\right)\right)}{L}\end{array}\right]U\left(k\right)$ (4)

其中：$Y\left(k\right)={\left[x\left(k\right),y\left(k\right),\psi \left(k\right)\right]}^{\text{T}}$ ，为k时刻输出，$U\left(k\right)={\left[v\left(k\right),{\delta }_f\left(k\right)\right]}^{\text{T}}$ ，为k时刻输入，$Y\left(k+1\right)$ 为k时刻车辆机理模型预测输出，$\tilde{A}\left(k\right)$ ，$\tilde{B}\left(k\right)$ 为模型线性化后的参数矩阵：

$\tilde{A}\left(k\right)=\left[I\right]$ , $\tilde{B}\left(k\right)=\left[\begin{array}{cc}T\cos \left(\psi \left(k\right)\right)& 0\\ T\sin \left(\psi \left(k\right)\right)& 0\\ \frac{T\tan \left({\delta }_f\left(k-1\right)\right)}{L}& \frac{v\left(k-1\right)T\left(1-{\tan }^2\left({\delta }_f\left(k-1\right)\right)\right)}{L}\end{array}\right]$

由于上述机理模型建立作了一些假设，忽略了一些因素，且需要离散化，线性化等处理，因此车辆机理模型与实车之间存在误差。本文采用车辆机理模型与数据驱动补偿相结合的方式，采用数据驱动的方法对该误差进行补偿。

2.2. 数据驱动补偿模型

采用线性回归模型对所建车辆运动学机理模型进行误差补偿，使其变得更加精准，若$\widehat{Y}\left(k\right)$ 为k时刻基于机理与数据驱动补偿的运动车辆模型输出，$\overline{Y}\left(k+1\right)$ 为k + 1时刻真实轨迹点以及航向角，$\widehat{Y}\left(k+1\right)$ 为基于机理与数据驱动补偿的运动车辆模型的预测输出，所建车辆机理与数据驱动补偿模型与实车输出误差模型的一般结构如下图2表示：

Figure 2. Mechanism and error compensation model structure diagram

图2. 机理 + 误差补偿模型结构图

$\Delta Y\left(k+1\right)=\overline{Y}\left(k+1\right)-\widehat{Y}\left(k+1\right)=\Delta \tilde{A}\left(k\right)\cdot \widehat{Y}\left(k\right)+\Delta \tilde{B}\left(k\right)\cdot U\left(k\right)$ (5)

式中：$\Delta \tilde{A}\left(k\right)$ 为$3\times 3$ 矩阵，$\Delta \tilde{B}\left(k\right)$ 为$3\times 2$ 矩阵：

$\Delta \tilde{A}\left(k\right)=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right]$ ，$\Delta \tilde{B}\left(k\right)=\left[\begin{array}{cc}{b}_{11}& b_{12}\\ b_{21}& b_{22}\\ b_{31}& b_{32}\end{array}\right]$ ，$\Delta \tilde{A}\left(k\right)$ ，$\Delta \tilde{B}\left(k\right)$ 均为未知参数。

将$\widehat{Y}\left(k\right)$ 代入上述公式(3)中得到车辆机理模型的输出结果：

$Y\left(k+1\right)=\tilde{A}\left(k\right)\cdot \widehat{Y}\left(k\right)+\tilde{B}\left(k\right)\cdot U\left(k\right)$ (6)

将机理模型输出和数据补偿模型输出相结合如下：

$\widehat{Y}\left(k+1\right)=\left(\tilde{A}\left(k\right)+\Delta \tilde{A}\left(k\right)\right)\cdot \widehat{Y}\left(k\right)+\left(\tilde{B}\left(k\right)+\Delta \tilde{B}\left(k\right)\right)\cdot U\left(k\right)$ (7)

式中：$\widehat{Y}\left(k+1\right)$ 为k时刻基于机理与数据驱动补偿的运动车辆模型的预测输出。

将上述式子整理为：

$\widehat{Y}\left(k+1\right)=A\left(k\right)\widehat{Y}\left(k\right)+B\left(k\right)U\left(k\right)$ (8)

式中：$A\left(k\right)=\tilde{A}\left(k\right)+\Delta \tilde{A}\left(k\right)$ ，$B\left(k\right)=\tilde{B}\left(k\right)+\Delta \tilde{B}\left(k\right)$ 。

3. LQR控制器设计

LQR控制算法通过最小化一个包含状态偏差和控制输入的二次成本函数来设计控制器，使得系统在给定的性能指标下达到最佳控制效果，且同时可以对不稳定系统进行整定，所以该方法在各个领域都有着广泛的应用。

根据上述的状态空间表达式(8)直接利用LQR算法计算出最优的控制输入，使得下列性能指标函数达到最小值：

$J={\displaystyle \sum _{k=0}^{\infty }\left(\widehat{Y}{\left(k\right)}^{\text{T}}Q\left(k\right)\widehat{Y}\left(k\right)+U{\left(k\right)}^{\text{T}}R\left(k\right)U\left(k\right)\right)}$ (9)

其中，加权阵$Q\left(k\right)$ 和$R\left(k\right)$ 是用来平衡状态向量和输入向量的权重，$Q\left(k\right)$ 为状态向量加权的半正定矩阵，$R\left(k\right)$ 为控制输入加权的正定矩阵。

结合公式(8)和最优控制理论可知，式(9)达到最小值的最优控制律为：

$U\left(k\right)=-K\left(k\right)\widehat{Y}\left(k\right)$ (10)

式中$K\left(k\right)$ 是线性最优反馈增益矩阵，通过求解下式Riccati方程的解得到。

$\begin{array}{l}P\left(k\right)=Q\left(k\right)+A{\left(k\right)}^{\text{T}}P\left(k+1\right)A\left(k\right)\\ -A{\left(k\right)}^{\text{T}}P\left(k+1\right)B\left(k\right){\left(R\left(k\right)+B{\left(k\right)}^{\text{T}}P\left(k+1\right)B\left(k\right)\right)}^{-1}B{\left(k\right)}^{\text{T}}P\left(k+1\right)A\left(k\right)\end{array}$ (11)

$P\left(k\right)$ 是时间步 k的对称正定矩阵，$P\left(k+1\right)$ 是时间步k + 1的对称正定矩阵。

在求得$P\left(k\right)$ 之后，反馈增益矩阵$K\left(k\right)$ 可以通过以下公式计算：

$K\left(k\right)={\left(R\left(k\right)+B{\left(k\right)}^{\text{T}}P\left(k+1\right)B\left(k\right)\right)}^{-1}B{\left(k\right)}^{\text{T}}P\left(k+1\right)A\left(k\right)$ (12)

4. 仿真验证

4.1. 工况设置

为了验证机理 + 数据补偿模型的LQR控制器的跟踪控制效果，本文使用Matlab进行仿真验证。本文综合考虑了路径跟踪过程中的控制精度和车辆物理约束，考虑到无人驾驶观光车的舒适性以及观光路径，车的速度一般被限制在10 km/h到20 km/h之间，角度限制在30˚左右，这样既能保证观光体验，又能确保安全，车辆具体参数如表1所示。根据大量的实验效果来设计LQR控制器合适的加权阵的值，设置Q为5的对角矩阵，R为单位矩阵，来进行控制器的设计。

Table 1. Simulation experiment control system parameters

表1. 仿真实验控制系统参数

参数	数值
车距L/m	1.5
车速km/h	10
时间步长/s	0.1
最大转向角/˚	27
Q	5
R	1

4.2. 跟踪控制效果对比

为了更好的了解本设计的控制方法，分两个实验进行比较。

实验一为考察车辆模型轴距和设计值不一致时，此处仿真实验中相差0.01 m，模型补偿后通过LQR控制器的跟踪性能，为了更好对比控制器的跟踪效果，设置不同的车辆起始点和参考路径点，参考路径的起始点设置为$\left(x^{*}\left(1\right),y^{*}\left(1\right)\right)=\left(0,0\right)$ ，车辆跟踪初始点设置为$\left(x\left(1\right),y\left(1\right)\right)=\left(0,-1\right)$ ，起始航向角都设置为0˚，仿真得到的跟踪效果和前轮摆向角，如下图3所示。

Figure 3. Trajectory tracking effect diagram of wheelbase compensation

图3. 轴距补偿轨迹跟踪效果图

如图3(a)、图3(b)所示的直线和曲线的路径跟踪效果，可以看出：轴距不一致的模型控制器跟踪效果与参考轨迹存在一定稳态误差，机理加数据补偿的模型可以有效地消除由于模型误差导致的稳态误差，提高了控制系统跟踪效果精度。图3(c)、图3(d)所示的直线和曲线跟踪控制前轮摆向角，可以看出机理加数据补偿的模型跟踪控制中的摆向角和未补偿的相差不大，不改变控制系统转向的稳定性。

实验二为考察车辆因为安装产生航向角偏差时，模型补偿后通过LQR控制器的跟踪性能，为了更好对比控制器的跟踪效果，设置相同的车辆起始点和参考路径点，不同的航向角。起始点设置为 $\left(x^{*}\left(1\right),y^{*}\left(1\right)\right)=\left(0,0\right)$ ，起始航向角参考路径设置为0˚，车辆起始航向角为1˚，且在实验中对仿真所测得航向角数据均增加1˚的航向角偏差。仿真对比没有误差的真实模型、增加了航向角误差的模型和补偿后的模型三者进行跟踪控制得到的跟踪效果、前轮摆向角和车辆航向角，如下图4所示。

Figure 4. Trajectory tracking effect diagram course angle compensation

图4. 航向角补偿轨迹跟踪效果图

如图4(a)、图4(b)所示的直线和曲线的路径跟踪效果，可以看出：存在航向角偏差的模型控制器跟踪效果与参考轨迹存在一定稳态误差，机理加数据补偿的模型可以有效地消除由于模型误差导致的稳态误差，提高了控制系统跟踪效果精度。图4(c)、图4(d)所示的直线和曲线跟踪控制前轮摆向角，可以看出机理加数据补偿的模型跟踪控制中的摆向角和未补偿的摆向角相比，初始100 m内有3度之间的波动，100 m后基本稳定两者相差不大，基本能维持控制系统转向的稳定性。图4(c)、图4(d)所示的直线和曲线跟踪控制航向角，因为要补偿稳态偏差可以看出带有航向角偏差的补偿模型的控制航向角稳定较慢，在200 m前会有2度以内的波动，200 m后基本趋于平滑稳定。

通过以上测试结果和数据，验证了机理 + 数据补偿模型的LQR控制器的路径跟踪效果达到了最初设计的目的。针对带有误差的模型可以进行有效的补偿，给LQR控制器提供更准确的模型，可以有效提高车辆的自适应性，减小车辆的跟踪误差，提高车辆的路径跟踪精度，同时基本具有同样的稳定性。

5. 结论

针对无人驾驶车辆在制造和装配过程中的精度限制导致车辆的实际物理参数和设计值不一致性的问题，本文设计了一种机理模型与数据驱动补偿的LQR控制策略。首先，通过线性回归误差补偿方法对观光车的机理模型进行修正，显著提高了模型的准确性和稳定性。其次，基于补偿后的机理模型，设计了LQR路径跟踪控制器，通过优化二次型性能指标，求解系统的最优控制序列。最后，仿真结果对比显示，针对带有误差的模型可以进行有效的补偿，给LQR控制器提供更准确的模型，减小因模型不准导致的跟踪误差，有效提高了车辆的自适应性和路径跟踪精度。

基金项目

山东省交通运输部重点科技项目(No. 2022-MS6-159)；山东省自然科学基金项目(No. ZR2022MF345)。