轮式差速转向车辆轨迹跟踪模型预测控制方法

Trajectory Tracking Model Predictive Control Method for Wheeled Differential Steering Vehicles

摘要: 针对全地形轮式差速转向车辆，设计了基于改进模型预测控制算法的轨迹跟踪控制器。首先对差速转向车辆的运动学和动力学进行分析，建立MPC轨迹跟踪误差模型；然后以实际跟踪误差为依据，设计控制器权重系数调节方法，对MPC的状态量矩阵权重和控制量矩阵权重进行动态调节；最后通过仿真实验对改进轨迹跟踪算法性能进行验证。仿真结果表明，相比经典MPC方法，改进算法对参考轨迹的横向跟踪误差降低35.3%，航向跟踪误差降低3%，跟踪精度有较大提高。

Abstract: A trajectory tracking controller based on an improved model predictive control algorithm is de-signed for all-terrain wheeled differential steering vehicles. Firstly, the kinematics and dynamics of differential steering vehicles are analyzed, and the MPC trajectory tracking error model is es-tablished. Then, according to the actual tracking error, the controller weight coefficient adjust-ment method is designed to dynamically adjust the MPC state quantity matrix weight and control quantity matrix weight. Finally, the performance of the improved trajectory tracking algorithm is verified by simulation experiments. The simulation results show that compared with the classical MPC method, the lateral tracking error of the reference trajectory is reduced by 35.3%, the course tracking error is reduced by 3%, and the tracking accuracy is greatly improved.

1. 引言

不同于阿克曼转向方式，差速转向车辆对两侧车轮施加不同驱动和制动力，通过控制两侧车轮的速度差来实现转向功能。最初，差速转向系统主要用于履带式车辆，后来，凭借简单、紧凑、可靠、高效、机动性强以及能够实现原地转向等优点，差速转向系统逐渐在轮式车辆中得到广泛应用。随着自动驾驶技术的发展，无人驾驶的轮式差速转向车辆在军事、抢险救灾等领域展现了广阔的应用前景，受到国内外学者的普遍关注。

在无人驾驶轮式差速转向车辆研究领域，轨迹跟踪控制是关键研究方向之一，尤其是在复杂野外环境下，实现高精度轨迹跟踪是确保全地形车辆安全行驶和顺利完成任务的基础 [1] 。文献 [2] 对差速转向轮式车辆进行动力学分析，设计了纵向力矩与转向两侧轮速差解耦控制的控制器，完成对参考轨迹的跟踪。文献 [3] 对四轮差速转向车进行运动学分析，通过考虑内外侧车轮的真实速度与转弯半径间的关系，对控制器进行优化设计。文献 [4] 对电传动履带车的运动特性进行分析，并实现了差速转向控制。文献 [5] 对模型预测控制在无人驾驶车辆轨迹跟踪问题中的应用进行了全面综述。文献 [6] 提出了基于非线性模型预测控制的滚动预测范围估计方法，有效地提高了跟踪精度。文献 [7] 在线性车辆模型的基础上建立了MPC路径跟踪控制器，并对前轮偏角进行约束，提高了车辆稳定性。文献 [8] 根据模糊算法调节MPC的权重值，提高了不同曲率下的路径跟踪算法的鲁棒性。文献 [9] 考虑路面附着系数对控制器的影响，设计了自适应预测时域，在添加侧偏角约束后取得了较好效果。文献 [10] 设计了自适应采样周期的路径跟踪控制器，对采样周期进行动态规划，很好地兼顾了实时性和跟踪精度。

在经典模型预测控制算法中，目标函数的状态量权重和控制量权重系数一般为定常值，当轨迹跟踪误差变大时，无法及时做出响应，不利于车辆参考轨迹的高精度跟踪。为解决这个问题，对目标车辆进行运动学和动力学分析，并将动力学分析结果作为MPC目标函数的约束条件。此外，提出一种目标函数状态权重与控制权重参数动态调节方法，以轨迹跟踪误差为依据，不断调整状态量和控制量权重，以提高跟踪精度。最后的仿真实验验证了改进MPC轨迹跟踪控制器的高精度与可靠性。

2. 差速车辆模型

2.1. 运动学模型

如图1所示，水陆两栖救灾差速转向八轮车的同一侧车轮由同一个电机驱动，具有相同转速。

图中：OXY为大地惯性坐标系；差动车的位置和姿态可以定义为OXY坐标系中的 $\left(x,y,\theta \right)$ ，θ为车辆的航向角；假设质心位于车辆几何中心o，oxy为车体参考坐标系 $\left(v_x,v_y,{\omega }_z\right)$ 分别为差动车辆的纵向速度、横向速度、横摆角速度； $v_{\text{r}},v_{\text{l}}$ 分别为左右两侧车轮速度；B为两侧车轮中心距。则车辆的运动学微分方程可表示为：

Figure 1. Kinematic model of differential vehicle

图1. 差速车辆运动学模型

$\left[\begin{array}{c}\stackrel{\dot{}}{x}\\ \stackrel{\dot{}}{y}\\ \stackrel{\dot{}}{\theta }\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ {\omega }_z\end{array}\right]$ (1)

根据所选择的控制量，以及差速转向特性可化简为：

$\left[\begin{array}{c}\stackrel{\dot{}}{x}\\ \stackrel{\dot{}}{y}\\ \stackrel{\dot{}}{\theta }\end{array}\right]=\left[\begin{array}{cc}\frac{1}{2}\cos \theta & \frac{1}{2}\cos \theta \\ \frac{1}{2}\sin \theta & \frac{1}{2}\sin \theta \\ \frac{1}{l}& -\frac{1}{l}\end{array}\right]\left[\begin{array}{c}{v}_{\text{r}}\\ v_{\text{l}}\end{array}\right]$ (2)

式中： $l=B/2$ 。

2.2. 动力学模型

为了简化车辆的动力学模型，假设车辆行驶在绝对平坦的道路上并仅受到道路作用在车辆上的力，忽略由空气动力学产生的各种力与力矩作用。动力学模型如图2所示。

根据车体受力分析得到车身动力学微分方程：

$m{\stackrel{\dot{}}{v}}_x=F_{x\text{l}1}+F_{x\text{r}1}+F_{x\text{l}2}+F_{x\text{r}2}+F_{x\text{l}3}+F_{x\text{r}3}+F_{x\text{l}4}+F_{x\text{r}4}+m{\omega }_z{v}_y$ (3)

$m{\stackrel{\dot{}}{v}}_y=F_{y\text{l}1}+F_{y\text{r}1}+F_{y\text{l}2}+F_{y\text{r}2}+F_{y\text{l}3}+F_{y\text{r}3}+F_{y\text{l}4}+F_{y\text{r}4}-m{\omega }_z{v}_x$ (4)

$I_z{\stackrel{\dot{}}{\omega }}_z=\left({\displaystyle \underset{i=1}{\overset{4}{\sum }}{F}_{x\text{r}i}-{\displaystyle \underset{i=1}{\overset{4}{\sum }}{F}_{x\text{l}i}}}\right)\frac{B}{2}+\left(F_{y\text{r}1}-F_{y\text{l}1}\right)l_1+\left(F_{y\text{r}2}-F_{y\text{l}2}\right)l_2+\left(F_{y\text{r}3}-F_{y\text{l}3}\right)l_3+\left(F_{y\text{r}4}-F_{y\text{l}4}\right)l_4$ (5)

式中：m为整车质量； $v_x$ 、 $v_y$ 、 ${\omega }_z$ 分别为差动车辆的纵向速度、横向速度、横摆角速度； $F_{x\text{r}i}$ 、 $F_{x\text{l}i}$ 、 $F_{y\text{r}i}$ 、 $F_{y\text{l}i}$ 分别代表第i轴的左、右轮接地处所受到的纵向和横向力； $I_z$ 为绕z轴的转动惯量；B为左右车轮的轮距； $l_1$ 、 $l_2$ 、 $l_3$ 、 $l_4$ 分别为各轮轴距离质心的距离，在质心位置前的为正，在质心后的为负。

Figure 2. Vehicle body dynamic model

图2. 车身动力学模型

对车轮进行受力分析，假设每个车轮与地面的接触情况相同，车轮动力学模型如图3。

Figure 3. Wheel dynamic model

图3. 车轮动力学模型

以一侧车轮建立动态运动微分方程，如下所示：

$4J_{\tau }{\stackrel{\dot{}}{\tau }}_j=i_{\text{tr}}T-i_{\text{tr}}{T}_{\text{b}}-r{\displaystyle \underset{i=1}{\overset{4}{\sum }}{F}_{xji}}\left(\lambda \right)$ (6)

式中： $\tau$ 为车轮的转动角速度； $j=\text{l},\text{r}$ 分别表示左、右侧车轮；r为车轮的有效半径； $i_{\text{tr}}$ 为电机到车轮的传动比； $\lambda$ 为轮胎滑移率；T为驱动力转矩； $T_{\text{b}}$ 为制动力转矩， $J_{\tau }$ 为单个车轮的转动惯量。

车辆受到的力最终可以归于车轮与地面之间的相互产生的轮胎力，在理想条件下，不考虑离心力对其的影响，对于全驱动车的附着力为：

$F_x=mg\mu \cos \alpha$ (7)

式中： $F_x$ 为车辆附着力；g为重力加速度；μ为路面附着系数；α为坡度角(在此忽略不计)。

3. 基于模型预测控制的轨迹跟踪

3.1. 轨迹跟踪控制器设计

模型预测控制器的核心思想是将实际控制问题转化为一个目标函数，在满足一系列约束条件的情况下，通过最优化求解方法，计算出未来一段时间内的最佳控制序列。随后，将该控制序列的首个控制输入应用于被控系统，以实现对系统的精确控制。无人差速转向车辆系统可以抽象为：

$\stackrel{\dot{}}{\xi }=f\left(\xi ,u\right)$ (8)

其中， $\xi ={\left[x_0,y_0,{\theta }_0\right]}^{\text{T}}$ 为系统状态量， $\left(x_0,y_0\right)$ 表示车辆几何中心o在大地坐标系下的位置， $u={\left[v_{\text{r}},v_{\text{l}}\right]}^{\text{T}}$ 为控制量。定义跟踪误差 ${\xi }_e=\xi -{\xi }_{\text{r}}$ ， ${\xi }_{\text{r}}={\left[x_{\text{r}},y_{\text{r}},{\theta }_{\text{r}}\right]}^{\text{T}}$ ， $\left(x_{\text{r}},y_{\text{r}}\right)$ 为参考路径给出的目标位置， ${\theta }_{\text{r}}$ 为期望航向角。经过前向欧拉法离散后，误差跟踪模型为：

${\xi }_e\left(k+1\right)=\xi \left(k\right)-{\xi }_{\text{r}}\left(k+1\right)+f\left(\xi \left(k\right),u\left(k\right)\right)\cdot t$ (9)

式中：k表示第k个求解时刻；t为离散时间步长。

用公式(1)的运动学方程表示离散后的误差跟踪模型为：

${\xi }_e\left(k+1\right)=\left[\begin{array}{ccc}1& 0& -\frac{1}{2}t\left(v_{\text{lr}}+v_{\text{rr}}\right)\sin {\theta }_{\text{r}}\\ 0& 1& \frac{1}{2}t\left(v_{\text{lr}}+v_{\text{rr}}\right)\cos {\theta }_{\text{r}}\\ 0& 0& 1\end{array}\right]{\xi }_e\left(k\right)+\left[\begin{array}{cc}\frac{1}{2}t\cos {\theta }_{\text{r}}& \frac{1}{2}t\cos {\theta }_{\text{r}}\\ \frac{1}{2}t\sin {\theta }_{\text{r}}& \frac{1}{2}t\sin {\theta }_{\text{r}}\\ \frac{t}{l}& -\frac{t}{l}\end{array}\right]u\left(k\right)$ (10)

设计的目标函数需要确保目标车辆能够快速、平稳、精确地跟踪规划模块提供的参考轨迹。为此，需要将系统状态偏差以及控制输入纳入目标函数中，其表达方式如下：

$J={\displaystyle \underset{j=1}{\overset{N_p}{\sum }}{\xi }_e^{\text{T}}\left(k+j|k\right)Q{\xi }_e\left(k+j\right)}+\Delta u^{\text{T}}\left(k+j-1\right)R\Delta u\left(k+j-1\right)$ (11)

其中，

$J={\left[\begin{array}{c}{\xi }_e{}_1\\ {\xi }_e{}_2\\ ⋮\\ {\xi }_e{}_{N_p-1}\\ {\xi }_e{}_{N_p}\end{array}\right]}^{\text{T}}\left[\begin{array}{ccccc}Q& 0& 0& \cdots & 0\\ 0& Q& 0& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& \cdots & 0& Q& 0\\ 0& 0& \cdots & 0& Q\end{array}\right]\left[\begin{array}{c}{\xi }_e{}_1\\ {\xi }_e{}_2\\ ⋮\\ {\xi }_e{}_{N_p-1}\\ {\xi }_e{}_{N_p}\end{array}\right]+{\left[\begin{array}{c}\Delta u_0\\ \Delta u_1\\ ⋮\\ \Delta u_{N_p-1}\end{array}\right]}^{\text{T}}\left[\begin{array}{cccc}R& 0& \cdots & 0\\ 0& R& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& \cdots & 0& R\end{array}\right]\left[\begin{array}{c}\Delta u_0\\ \Delta u_1\\ ⋮\\ \Delta u_{N_p-1}\end{array}\right]$ (12)

式中：Q和R为权重矩阵， $\Delta u$ 为控制增量， $N_p$ 为预测时域。状态误差 $\psi =\left[\begin{array}{cccc}{\xi }_e{}_1& {\xi }_e{}_2& \cdots & {\xi }_e{}_{N_p}\end{array}\right]$ 。

式(11)右侧的第一项表示系统在跟踪参考轨迹方面的性能，而第二项则表示对控制输入变化的约束。这个目标函数的优点在于其容易转化为标准的二次规划形式。

3.2. 约束条件

约束条件可以分为两类：等式约束和不等式约束。等式约束通常用于描述系统的预测模型，即用于表达相邻时刻之间的状态转移关系。而不等式约束则一般基于车辆的执行能力来限定控制量、控制量增量等的取值范围。其对控制量约束表示为：

$u_{\min }\le u\left(k+i|k\right)\le u_{\max },0\le i\le N_{\text{c}}-1$ (13)

对控制增量约束表示为：

$\Delta u_{\min }\le \Delta u\left(k+i|k\right)\le \Delta u_{\max },0\le i\le N_{\text{c}}-1$ (14)

式中： $u_{\min }$ 、 $u_{\max }$ 分别为控制量的下限值、上限值； $\Delta u_{\min }$ 、 $\Delta u_{\max }$ 分别为控制增量的下限值、上限值。控制量与控制增量的约束通过式(3)~(7)所示的动力学模型进行计算。

为了便于控制器设计，本文基于以下假设对车辆动力学模型进行简化：1) 车辆的纵向速度保持恒定不变；2) 假设车辆的轮胎工作在线性区域内；3) 仅考虑车辆在平面内做平面运动，不考虑立体运动；4) 在分析中忽略了空气阻力、滚动阻力等阻力因素的影响；5) 不考虑车辆的侧倾和俯仰运动；6) 假设车辆质心处纵向速度等于车辆两侧轮胎接地点的纵向速度均值；7) 忽略传动比和传动损失。控制增量限制的表达式如下：

$\left|\Delta u\right|\le \frac{\mu mg-\left|{\displaystyle \underset{i=1}{\overset{4}{\sum }}{F}_{xri}}\right|-\left|{\displaystyle \underset{i=1}{\overset{4}{\sum }}{F}_{xli}}\right|}{m}$ (15)

式中：μ为路面附着系数， $F_{x\text{r}},F_{x\text{l}}$ ，为左右侧车轮纵向力。

则目标函数转化为标准二次规划形式：

$\begin{array}{l}\underset{\Delta u}{\min }\frac{1}{2}\Delta u^{\text{T}}H\Delta u+G^{\text{T}}\Delta u\\ \text{s}\text{.t}\text{.}\Delta u_{\min }\le \Delta u\le \Delta u\\ u_{\min }\le u\le u_{\max }\end{array}$ (16)

式中： $H=\psi Q{\psi }^{\text{T}}+R$ ， $G=\psi QE$ 。

3.3. 控制器参数优化

在MPC轨迹跟踪控制器中，Q矩阵用于衡量系统状态与参考轨迹之间的误差对性能指标的影响，R矩阵用于衡量控制输入变化对性能指标的影响。

通过调整Q和R矩阵的参数，可以使MPC控制器在实时运行中更好地满足性能要求。如果Q矩阵中的元素较大，控制器将更注重减小状态误差，但这可能导致控制输入变化较大。相反，如果R矩阵中的元素较大，控制器将更注重减小控制输入的变化，但可能导致状态误差相对较大。因此，通过调整这些参数，可以平衡状态误差和控制输入，以实现更好的控制性能。

Q和R矩阵动态参数调整规则如式(17)所示：

$q=\{\begin{array}{l}\phi \times a\left(\phi \ge x\right)\\ \phi \times b\left(\phi \le -x\right)\end{array}$ ， $r=\{\begin{array}{l}\phi \times c\left(\phi <x\right)\\ \phi \times d\left(\phi >-x\right)\end{array}$ (17)

式中：q、r分别代表Q和R矩阵的权重参数； $\phi$ 为实际轨迹与参考轨迹之间的误差(这里指与参考轨迹的欧式距离)；x为最小误差参考量；a、b、c、d分别为给定比例系数。可以看出：以实际轨迹和规划轨迹的误差为参考量，当误差大于一定值时提高状态矩阵的权重，以达到提高跟踪精度的目的；当误差小于一定值时，提高控制矩阵的权重，可以使控制量更加趋于稳定。

4. 仿真实验与分析

为了验证算法的有效性与跟踪精度，在MatLab平台下进行仿真实验，差速转向车辆的具体参数如表1所示，MPC控制器算法参数如表2所示。将MPC控制器设计问题转化为标准二次规划问题，并使用MatLab提供的quadprog函数在线求解。

Table 1. Simulate vehicle model parameters

表1. 仿真车辆模型参数

Table 2. MPC controller parameters

表2. MPC控制器参数

式(17)关于动态调节参数设置为， $x=0.05$ ， $a=c=1000$ ， $b=1$ ， $d=500$ 。以双移线为参考轨迹，参考速度为10 m/s，路面附着系数为0.8。分别使用改进MPC方法与经典MPC方法进行相同工况下的对比仿真实验。

跟踪结果如图4所示：

Figure 4. Trajectory tracking effect diagram

图4. 轨迹跟踪效果图

横向跟踪误差与航向角误差分别如图5(a)和图5(b)所示：

(a) (b)

Figure 5. Lateral error diagram and heading error diagram

图5. 横向误差与航向误差

表3所示为轨迹跟踪仿真结果。

Table 3. Simulation experiment effect statistics

表3. 仿真实验效果统计

通过图4、图5和表3可以看出：通过动态调节权重参数，针对差速转向车辆设计的改进MPC控制器比经典MPC控制器的横向跟踪误差降低35.3%，航向跟踪误差降低3%，跟踪精度有明显提高。

5. 结论

针对自动驾驶水陆两栖救灾车辆的轨迹跟踪问题，建立了差速转向八轮车运动学模型和动力学模型，并结合动力学约束设计了改进MPC轨迹跟踪控制器，该控制器以车辆实时跟踪误差为依据对目标函数中的状态量与控制量权重矩阵系数进行动态调节，有效提高了参考轨迹的横向及航向跟踪精度。通过验证可得出，动态的调节控制器参数可以有效地提高控制精度。