基于多模态控制的智能车辆路径跟踪

基于多模态控制的智能车辆路径跟踪
Intelligent vehicle path tracking based on multi-modal control
刘飞飞1，陈臻阳2，彭辉辉2，代云勇2，陈文涛2
LIU Fei-fei 1, CHEN Zhen-yang 2, PENG Hui-hui 2, DAI Yun-yong 2, CHEN Wen-tao 2
（1.江西理工大学 电气工程学院，赣州 341000；2.江西理工大学 机电工程学院，赣州 341000）摘 要：为了使智能车辆的路径跟踪精度更加准确，设计了一种基于MPC控制和LQR控制相结合的多模态控制器，通过对跟踪路径曲率的判断，控制器选择相对应的控制模式，并解析车辆当前状
态量和跟踪路径上相对应参考点状态量的偏差，输出控制集，最终完成路径跟踪。

MATLAB 仿真结果表明，这种多模态控制算法能精确快速的跟踪上参考路径，并保持良好的稳定性。

关键词：智能车辆；MPC；LQR；多模态控制
中图分类号：TP202+.1 文献标识码：A 文章编号：1009-0134(2020)06-0099-04
收稿日期：2019-02-18
作者简介：刘飞飞（1962 -），男，教授，博士，研究方向为智能装备及其自动化、特种装备机器人和检测技术。

0 引言
随着科技的迅速发展，智能车辆的自动驾驶技术有着越来越重要的地位[1]。

路径跟踪作为自动驾驶技术中非常关键的控制环节，主要的目的是让智能小车始终能沿着期望的路径行驶[2]。

由于车辆是一个明显的非线性、高度耦合的复杂系统，难以建立精确的动力学模型，所以智能车辆的路径跟踪控制一直是一个难题[3]。

文献[4,5]采用了PID 和模糊PID 控制器，根据人类的驾驶经验建立决策系统，对车辆的前轮偏角进行控制。

文献[6]通过BP 神经网络控制器对前视距离进行动态调节，实现车辆自主路径跟踪控制。

文献[7,8]分别使用纯跟踪算法和基于时间延迟动态预测的纯路径跟踪控制算法，来实现路径跟踪控制。

要实现模拟人的自动驾驶路径跟踪控制过程十分复杂。

直至目前，对路径跟踪的控制多以单一的控制模式来实现，而单一的模态控制很难适应多变的路况。

因此，笔者设计了一种基于模型预测控制（Model Predictive Control ，MPC ）和线性二次型调节器（Linear Quadratic Regulator ，LQR ）相结合的多模态控制方式，通过对路况曲率的判断，来选择对应的控制模式。

直线路径时选择模型预测控制，减少系统的超调量，提高系统的稳定性。

而进入到曲线路段时，则切换到LQR 控制方式，使车辆紧随路径过弯，提高算法实时性和适用性。

1 车辆运动学建模
本文选用Kelly [9]提出简化的车辆运动学的两轮自行车模型来代替车辆的四轮运动学模型，将轮胎视为刚性轮，忽略轮胎和地面的侧滑等因素。

如图1所示，0XY 为大地坐标系，智能车辆后轮中心点Oc 坐标为（X1，
Y1），δ为前轮偏角，θ为车体的航向角，R 为后轮转向半径，C 为车辆瞬时的转向中心。

V 为车辆后轮的速度，L
为车辆前后轮轴距。

图1 车辆运动学模型
由图1可得：
(1)
式中ω为车辆横摆角速度。

由运动学分析可以得如下运动学模型：
(2)
2 多模态控制器的设计
2.1 概述
传统的单模态控制器都有其应用的局限性，而现实
中的路况复杂多变，很难建立一个对这种复杂多变系统各类状况均适应的模型。

解决这个矛盾的一种方法是对人类开车时的情景进行模仿，根据路况来判断并选择控制方式。

因此可以设计一个多模态的控制器，它集合了MPC和LQR两种控制模式。

在直线路段时选择超调量小，控制稳定的MPC控制模式。

当进入弯道时，则选用LQR控制模式，实时性更强，使系统更精准的跟踪弯道路径。

⁑ 䘹
MPC
㹼
LQR
图2 多模态控制器
图2中θ
周期路径跟踪点的倾斜角。

设ER为某一阈值，当倾斜角差值△θ小于ER时，路径平缓，采用MPC控制模式，当倾斜角差值△θ大于ER时，路径弯曲，采用LQR控制模式。

2.2 MPC控制器的设计
模型预测控制（Model Predictive Control，MPC）是一种现代控制理论，它不依赖于系统精确的数学模型，使用滚动优化方法代替传统的最优控制，有较强的鲁棒性，且算法实现相对简单[10]。

选取系统状态量，控制量。

则参考系统的任意时刻的状态和控制量都有如下关系：
(3)
在任意点（X, u）处进行泰勒展开线性化可得：
(4)
A(t)为F相对于X的雅
可比矩阵，B(t)为F相对于u的雅可比矩阵。

连续的状态方程不能直接用于模型预测控制器的设
计，固对其离散化得：
(5)
式中k为时间步长。

对式(5)做如下转换：
(6)
可得新的状态空间表达式：
)
(
)
(
)1
(
,
,
t k
U
B
t k
A
t
k
t k
t k
∆
+
=
+ξ
ξ
)
(
)
(
,
t k
C
t k
t k
ξ
η=(7)
式
，
，
n为状态量维
度，m为控制量维度。

设定系统预测时域为N
p
，控制时域为N
c
，则车辆各
时间点的状态能通过式(7)迭代的方式来预测：
(8)
联立式(7)和式(8)可得：
)
(
)
(
)
(t k
U
t k
t k
ZΘ∆
+
Ψ
=ξ(9)
式中：
=
Θ
−
−
−
−B
A
C
B
A
C
B
A
C
B
C
B A
C
B
C
Nc
Np
Np
Np1
2
1
L
M
O
M
M
采用如下形式的目标函数：
(10)
式中Q 和R 为权重矩阵。

求解目标行数，能得到预测时域内的控制序列。

将控制序列中第一个元素作为实际的控制输入增量，即：
t u t u t u ∆+−=)1()(
(11)
2.3 LQR 控制器的设计
线性二次型调节器（Linear Quadratic Regulator ，LQR ）是针对以状态空间模型给出的线性系统。

在车辆路径跟踪中，利用LQR 的前馈和反馈控制系统，通过状态的反馈来实现闭环最优控制[10]。

由车辆动力学模型线性化可得：
k k k k k u B x A x ∆+∆=∆+1
(12)
式中x k 表示车辆实际状态量和参考点状态量的偏差，A k =J x f （x k ，u k ），B k =J u f （x k ，u k ）。

其中J x 为函数f 关于x 的雅可比矩阵，J u 为函数f 关于u 的雅可比矩阵。

通过LQR 控制方法消除车辆当前状态与参考路径上参考点的状态偏差，首先定义如下评价函数：
))(()(2
1
)]()()()[(2101
N N T N N k k T
k k N k k k T k k P x t Q P x u u R u u P x Q P x J −−+∆+∆++
−−=∑−= (13)
式中Q 为状态量误差的权重矩阵，R 为控制量的权重矩阵，Q 0为终端状态权重矩阵，x k 为无人驾驶车辆在第k 时刻的预测状态，P k 为参考路径上第k 个参考点的状态，u k +u k 为第k 时刻的控制量。

通过引入拉格朗日算子，构造无约束的最优化问题，如下：
)
())(()(1
)]()()()[(2111
01
++−=∆−∆+∆+−−+∆+∆++−−=
∑k k k k k T k N N T N N k k T k k N k k k T k k x u B x A P x t Q P x u u R u u P x Q P x V λ
(14)
构造Hamilton 函数，令：
)]
()()()()[(2
1
1
k k k k T k k k T k k k k T k k u B x A u u R u u P x Q P x V ∆+∆+
∆+∆++
−−=+λ
(15)
联立式(14)、式(15)可得：
]
)([1))(()(21
1
1
00∑−=∆−+
∆++−−=
N k k T k
k
N T
N N N T N N x H x H P x t Q P x V λ
λ (16)
当函数V 满足如下条件时：
(17)
函数V 取最小值，由式(17)可得：
N
N k T
k k k k
k k k x Q B u u R A x Q λλλλ=∆=+∆+=+∆++0110)(
(18)
假设：
k k k k v x S +∆=λ
(19)
由评价函数式(13)和式(18)，式(19)可得：
k
T k k k k k k k T K k T k k k T k v k k T k u k k T
k k k T k Ru K v K B A v Q
K B A S A S B R B S B K R
R B S B K A S B R B S B K −−=+−=+=+=+=++−+−++−+111111111)()()()()( (20)
已知SN=Q 0，v N =0和终端状态x N ，通过向后迭代可得反馈控制的最优控制序列，如式(21)所示：
11++−−∆−=∆k v k u k k v K u K x K u
(21)
所以最终控制量为：
k k u u u ∆+=
(22)
式中u k 为前馈控制量，u k 为反馈控制量。

3 仿真分析
在MATLAB 软件中模拟车辆对已知路径进行跟踪仿真。

设置模拟仿真的初始条件：已知路径由直线路段和曲线路段组成。

道路起点位置为（0，2），终点位置为（10，7），车辆的初始位置为（0，0），航向角θ为π/3，车辆轴距为1m ，后轮速度为1m/s ，控制采样周
期为0.05s。

仿真的跟踪效果如图3～图6
所示。

图3 路径跟踪全局地图
从图3的路径跟踪结果中可以看出，车辆在MPC控制器的作用下，快速的跟踪上期望路径，超调量很少，之后的直线路段，保持着对参考路径良好的跟随，系统稳定性强。

从直线路段进入弯曲路段时，控制器自动切换到LQR控制模式，使车辆依然能够保持着对弯曲路径
的准确追踪，没有过多的超调量。

图4 x
轴方向偏差图
图5 y轴方向偏差图
由图4～图6可以看出，系统在初始状态设置时设置了比较大的偏差量，在控制器的作用下，也能很快的跟踪上参考路径并保持稳定。

直线路径转弯曲路径的过度地方也没有出现状态波动，说明系统能很好的切换这两种控制方式。

除初始状态故意设置的较大偏差外，车辆跟踪上参考路径后各状态偏差均很小，满足车辆自动驾驶时路径跟踪的要求。

4 结语
本文通过对智能车辆路径跟踪问题的分析，设计了一种基于多模态控制的路径跟踪控制器，通过建立车辆运动学模型，以及对控制器参数的调节，并在MATLAB环境中对直线路径和弯曲路径均进行了仿真。

仿真结果表明，本文所设计的多模态控制器可以保障智能车辆在低速时具备良好的稳定性和跟踪精度，精度满足自动驾驶时路径跟踪的要求。

随后的研究将针对车辆在中高车速时，对车辆路径跟踪控制器进一步完善，提高系统的稳定性和可靠性。

参考文献：
[1] Snider J M.Automatic Steering Methods for Autonomous
Automobile Path Tracking[J].Robotics Institute,2009.
[2] 赵熙俊,陈慧岩.智能车辆路径跟踪横向控制方法的研究[J].汽
车工程,2011,(5):382-387.
[3] SETLUR P,WAGNER J R,DAWSON D M,eta1.A trajectory
tracking steer-by-wire control system for ground vehicles[J].IEEE Transactions on Vehicular Technology,2006,55(1):76-85.
[4] Garcia-Perez L,Garcia—Alegre M C,Ribeiro A,et a1.An agent of
behaviour architecture for unmanned control of a farming vehicle[J].
Computers and Electronics in Agriculture,2008,60(1):39-48.
[5] 张卫波,张麒麟,马宁,吴乙万.基于模糊PID的智能车辆路径跟踪
控制技术研究[J].机械制造与自动化,2018,47(02):167-170,174.
[6] 黄沛琛,罗锡文,张智刚.改进纯追踪模型的农业机械地头转向
控制方法[J].计算机工程与应用,2010,46(21):216-219.
[7] Amidi O, Thorpe C E. Integrated Mobile Robot Control[C].Fibers '91,
Boston, MA. International Society for Optics and Photonics, 1991:504-523.[J].ComputersandElectronicsinAgriculture,2008,60(1):39-48.
[8] 赵建辉,高洪波,张新钰,张颖麟.基于时间延迟动态预测的自动
驾驶控制[J].清华大学学报(自然科学版),2018,58(04):432-437.
[9] KEIIY A.A feedforward controlapproach to the local navigation
problem for autonomous vehicles:Technical Report,CMU-RI-TR-94-17JR].Pittsburgh,PA:Robotics Institute,CarnegieM ellon University,1994.
[10] 龚建伟,姜岩,徐威.无人驾驶车辆模型预测控制[M].北京:北京
理工大学出版社
,2014.
图6 车辆航向角偏差图。