自适应LOS制导结合MPC控制的车辆循迹优化

刘清河，王泽文，赵立军

(哈尔滨工业大学 汽车工程学院，山东 威海 264209)

近些年，沿规定导航路径行驶的运输车以及园区观光游览车逐步成为企业研究的热点[1]，无人车在商用运输[2-3]以及未来军事领域都有广阔的应用前景。作为自动驾驶核心技术之一，路径制导算法以及控制器的设计具有很强的理论研究价值和现实意义，国内外学者在这个领域开展了广泛而深入研究。目前主流的制导方法有两种：第一种是纯追踪(Pure Pursuit)算法[4]，该方式在车辆以及移动机器人的循迹领域应用广泛，但在高速行驶工况下会出现超调。第二种是Stanley算法，该方法对环境中的随机干扰有较弱的鲁棒性，转弯时极易冲出跑道。目前循迹车辆的控制方法大多基于现代控制理论，主流方法有LQR控制[5]，PID控制[6]和模型预测控制[7](model predictive control,MPC)等。LQR控制选取了车辆运动学模型作为研究对象，但忽略车辆动力学约束，快速转弯时极易侧偏。经典PID控制简化了控制问题，鲁棒性高，控制参数需要反复地修改、调试，耗时较长。模型预测控制能实时处理多变量约束优化问题，权重系数选取灵活多样，在提高循迹实时性和稳定性方面有不错的效果。

针对无人车路径跟踪问题的不足之处，本文总结了文献[8]中传统视线(line-of-sight ,LOS)制导法动态响应慢的缺点，提出了时变前视距离的自适应LOS算法，把目标轨迹的跟踪转化为航向的跟踪，并基于车辆横向动力学模型以及模型预测控制方式求解最优反馈的方向盘控制指令。最后，为验证上述所提跟踪策略的有效性与优越性，在Simulink仿真环境分别进行直线路径和曲线路径追踪测试，验证了改进LOS制导算法能够基于当前的横向误差及调整前视距离，更出色地完成路径跟踪任务。

1 基于时变前视距离的视线(LOS)制导法

1.1 传统LOS的基本原理

视线(line-of-sight，LOS)制导法[9]是在地对空导弹的多目标拦截，自动驾驶循迹领域应用较为广泛的经典制导算法，不仅能追踪曲率k为0的直线路径还可以追踪曲率k>0的曲线路径，但是变化曲率的存在会干扰车辆当前位置和目标位置的横向误差的大小，使得误差不能稳定的收敛。虽然曲率k不为零，但可以把曲线轨迹切分为许多段曲率为0的直线路径，本文选取细分后的直线作为追踪的目标路径。视线制导算法的原理如图1所示。

图1 视线制导法追踪直线的几何图解Fig.1 Geometric illustration of line-of-sight guidance

LOS引导车辆收敛于目标直线Pw-1Pw上的LOS点Plos(xlos,ylos)，其中，Pw-1=[xw-1,yw-1]∈R2,Pw=[xw,yw]∈R2并保持车辆的方位角ψ和目标直线的航向角αw一致。车辆在世界坐标系(OXY)中的位置坐标为P=[x,y]∈R2,则循迹过程中确定Plos(xlos,ylos)的方法为：作车辆位置至期望直线的垂线，Pw-1Pw上与垂足F相距前视距离Δ的位置即为LOS点。设定一个以Pw-1为原点的正切参考坐标系(OXPYP)，此时参考路径在世界坐标系(OXY)中的航向角可以表示成

aw=arctan2(yw-yw-1,xw-xw-1)

(1)

循迹过程中的沿向误差xe和横向误差ye经过坐标转换可以表示为

(2)

式中，RT为世界坐标系(OXY)到参考路径坐标系(OXPYP)的转换矩阵,表示为

(3)

展开公式(2)可得追踪的横向距离误差

ye=-(x-xw-1)sinaw+(y-yw-1)cosaw

(4)

车辆需要快速且稳定地收敛于期望的路径上的LOS点Plos(xlos,ylos)并沿着目标轨迹稳定的追踪，因此，制导系统有两个操纵目标：

1)横向控制：随着车辆接近目标路径，横向误差ye逐渐收敛于0，即

(5)

2)纵向控制：循迹车在纵向的实际速度与期望速度之间的误差逐步减小，即

(6)

式中，ud为沿着车体x坐标轴的纵向期望线速度。

无人车长度为L,接纳圆半径R0与相邻路径的夹角θ成反比关系，设计如下的公式：

(7)

上式中的l数值取决于车辆的操纵性能，确定l变量的具体方法如下所示：

1)取n+2个期望点，生成的轨迹会有n个不同数值的航向夹角，分别为{θ1,θ2,…,θn}。设计期望轨迹时尽可能满足相邻目标直线之间的航向夹角θ彼此不同。

2)应用恒定的接纳圆半径R0进行循迹，计算不同航向夹角下各个目标直线路径的平均横向追踪误差ea，其公式为

(8)

式中，N代表整个循迹过程的总步长，e(i)代表第i个目标直线的横向追踪误差，ea取较小数值时说明追踪效果更为出色。

(9)

式中：

1.2 基于可变前视距离的自适应LOS

传统的视线(LOS)制导法结合无人车实际位置与理想位置之间的状态误差，按照一定的制导算法计算出控制命令，发送给底层控制器，改变车辆运动状态，消除横向和纵向偏差。无人车的控制目标是无限接近于直线路径上目标点Plos(xlos,ylos)，令参考路径方向Pw-1Pw与车辆方位obPlos的夹角ψlos逐渐收敛到零，因此车辆期望航向角度的公式如下:

ψd=aw-ψlos

(10)

式中

(11)

通常情况下，前视距离Δ为一个固定的数值。循迹初始阶段的横向误差ye过大，车辆本应迅速地收敛于LOS点，但是固定的前视距离Δ无法快速调整方位，因此收敛缓慢；随着车辆接近参考路径，ye数值逐渐变小，平稳地追踪成为系统的首要目标，但是固定的Δ使得循迹存在震荡，抖动的问题，因此本文根据横向误差ye的数值大小，在线调整前视距离Δ，提出一种基于可变前视距离的LOS制导法，使得车辆可以更加迅速且平稳地实现路径追踪。

本文所提出的时变前视距离公式如下:

Δ(yc)=(Δmax-Δmin)e-y|ye|+Δmin

(12)

从上述公式(12)可以看出，前视距离Δ为横向距离误差函数，与车速和目标路径曲率无关。其中，Δmin和Δmax为循迹过程中前视距离的阈值，γ为收敛速率。为取得出色的路径追踪效果，前视距离的最小、最大值Δmin与Δmax依据经验分别取车身长度的4倍与8倍，γ取0.1。当车辆远离目标路径时，Δ取较小值，此时收敛速度加快；当车辆靠近目标路径时，Δ取较大值，此时车辆的平稳性提高，有效避免了震荡；上述的远和近的概念都是相对，当设定Δmin、Δmax以及γ的数值时应考虑车辆的动力学约束。

1.3 制导法稳定性分析

车辆在世界坐标系下的运动学方程式可以写成

(13)

式中:u，v为车辆纵向和横向的线速度，ψ是车辆当前时刻的方位角。

对公式(4)两边求导，可得

usin(ψ-aw)+vcos(ψ-aw)≈

Usin(ψ-aw)

(14)

为使LOS所规划的轨迹尽可能贴近实际情况，考虑横向误差ye的运动学约束，接下来对制导系统在平衡点ye=0附近的轨迹收敛稳定性进行分析。假设无人车循迹过程中精准地沿着期望路径行驶，此时实际方位角ψ满足

(15)

在sinψlos取值很小的情况下，sinψlos=ψlos，将公式(15)代入到公式(14)，得到

(16)

本文使用李雅普诺夫(Lyapunov)稳定性判据来证明车辆LOS制导系统的横向跟踪误差收敛可以到平衡性状态ye=0，设置一个正定的李雅普诺夫(Lyapunov)函数

(17)

对横向误差ye求导，并代入公式(16)可得

(18)

(19)

2 自适应LOS制导结合模型预测控制(MPC)的跟踪方法设计

2.1 车辆路径跟踪控制的原理

针对自动驾驶车辆的路径跟踪问题，本文设计了一种基于系统跟踪模型的预测控制方法。原理如下：首先利用自适应LOS把对目标路径的跟踪简化为对航向的跟踪，然后基于系统跟踪模型以及模型预测控制方法实现车辆对期望方位的收敛，跟踪系统的架构如图2所示。

图2 路径跟踪系统原理图Fig.2 Schematic diagram of path tracking system

该跟踪控制系统由3个部分组成，即环境感知子系统，制导子系统以及底层控制子系统 。环境感知系统主要利用激光雷达、GPS等传感器获取无人车的位置，速度以及姿态信息。制导系统按照一定的制导算法生成期望路径与目标方位角。底层控制系统的任务是接受制导系统的规划指令，改变车辆运动状态，消除偏差。

循迹控制的具体过程如下：

1)上位机记录GPS的经纬度坐标并生成参考路径点。一般情况下，参考路径是相邻点连接形成的直线段。

2)自适应LOS制导律依据参考路径，车身姿态，位置等信息计算出车辆与参考路径的横向距离误差以及方位角的相位差。

3)横向控制采用模型预测控制的方式，依据横向距离误差、方位角相位差以及车速等信息计算最优的方向盘指令。纵向控制采用传统比例-积分-微分这种闭环控制方式(PID控制)，生成油门和刹车的指令，实现汽车的定速巡航。

4)将线控油门，方向盘转角指令输送到车辆的底盘。

5)环境感知子系统得到车辆行驶过程中的位置，车速，车身姿态等信息，再把数据及时的传送到上层制导系统，生成期望的状态参数。

2.2 无人车的动力学模型

汽车动力学模型[11]是车辆实现仿真控制的基础，可以更好地分析车辆稳态响应以及瞬态响应。然而，车辆具有非线性的特性，考虑到仿真的计算效率和耗时，本文使用单车模型进行动力学分析并做如下假设：

1)单纯考虑轮胎的线性侧偏特性，忽略轮胎里的横纵耦合关系。

2)忽略纵向空气动力学。

3)假设汽车只有前轮转向，并且忽略悬架对稳定性的影响。

4)忽略汽车载荷左右的转移。

根据上述所列的假设，建立如图3所示的汽车动力学模型，O-XYZ是世界惯性坐标系，o-xyz是车体坐标系 。

基于车辆动力学模型和牛顿第二定律，车辆沿着车体坐标系x轴、y轴和绕z轴的方程分别为：

在x轴方向上

max=Fxf+Fxr

(20)

在y轴方向上

may=Fyf+Fyr

(21)

在z轴方向上

(22)

图3 车辆动力学模型Fig.3 Vehicle dynamics model

其中,m为整车整备质量，Iz为绕z轴的转动惯量，a、b分别为重心到前后轴的距离。经过分析，y轴的加速度ay由两部分组成：分别是y轴方向的位移加速度和绕z轴转动的向心加速度，表示如下：

(23)

代入到公式(21),可得

(24)

轮胎由于受到横向的载荷压力，会产生一个很小的侧偏角，前后轮胎所受横向力Fyf,Fyr与对应侧偏角ρf,ρr的线性关系为

(25)

式中,Cf和Cr分别为汽车前后轮胎的侧偏刚度。当前后轮的侧偏角都取较小值时,tanρ=ρ，ρf和ρr计算公式为

(26)

式中，δ为前轮转角。结合公式(24)～(26)得到横向位移y与方位角ψ的状态空间模型。

(27)

其中：

a11=-(2Cf+2Cr)/mu

a12=-u-(2aCf-2bCr)/mu

a21=(2aCf-2bCr)/Izu

a22=-(2a2Cf+2b2Cr)/Izu

2.2 无人车的路径跟踪系统模型

自适应LOS制导获取车辆实时位置信息并与期望路径作比较，在线计算出期望的方位角度，使用模型预测控制器改变车辆状态。结合公式(14)和(27)，设计跟踪系统的非线性数学模型，可得

(28)

(29)

(30)

2.4 基于自适应模型预测控制的跟踪策略设计

模型预测控制(MPC)使用多步预测[12]，滚动实时优化[13]，反馈校正[14]等策略缩小系统模型中状态变量的误差。本文设计的控制器能估算未来预测步长系统状态，修正车辆方位角误差。为书写方便，公式(30)要表示为状态方程，可得

(31)

式中，x为状态变量，代表无人车当前状态的数值与目标数值的误差量；u为系统输入变量，y为系统输出变量，系统矩阵为

数值计算器需要处理离散数据信息，但是上式的状态方程是连续，有必要对系统进行离散化。本文使用欧拉算法实现状态方程的离散化，可以得到

(32)

式中:Ak以及Bk分别表示经过欧拉算法离散化后的系统状态矩阵，T为离散间隔步长。综合公式(31)和(32)，可得

(33)

为了精确地约束前轮转角的增量，把状态变量x(k)与输入量u(k-1)封装成一个新的状态变量

(34)

更新后的系统模型为

(35)

MPC可以获取系统某未来NP步长内的预测输出，NP预测时域内的离散状态变量方程可表示为

(36)

上式中，NP、NC分别为预测时域和控制时域。NC

Y(k)=Fε(k)+KΔU(k)

(37)

式中，ΔU为控制时域NC内的输入增量，F、K为系统矩阵，它们的表达式为

构建考虑横向误差以及控制量增量的惩罚函数，从控制序列中找最优解，惩罚函数的表达式为

(38)

式中，J为惩罚函数，Q与R分别是预测时域权重系数和控制时域权重系数。由于y(k)已经代表当前状态数值与期望状态数值误差，所以期望状态yd为0。为方便求解带约束二次规划问题，结合公式(37)和(38),惩罚函数转化为标准的二次型，可得

(39)

式中：

实际控制过程中，系统的控制量和状态变量满足以下约束条件：

(40)

上式中，Δumax和Δumin为NC步长内控制增量约束阈值，umax和umin为NC步长内控制量约束阈值，ymax和ymin为系统输出约束阈值。

综上所述，自适应视线LOS制导结合MPC的跟踪优化过程可转化为如下带有约束的二次规划[15](Quadratic Programming,QP)问题。

(41)

对于实际循迹来讲，如何满足MPC控制器对轨迹跟踪精度与行驶稳定性的要求十分重要。传统MPC控制器中的预测时域步长NP通常为一个恒定的数值，但该步长参数应当依据车道的几何形状及时进行调整，从而获得到较好的控制效果。降低预测时域步长NP时，控制器对路径的跟踪精度会有所降低(尤其在曲率特别大的转弯处，追踪精度降低的更加明显)。目前主流的预测步长NP自适应控制器原理如下：选取参考路径曲率的绝对值k来判定道路的几何信息，NP可以依据跟踪轨迹曲率k的变化整定，实现自适应控制，达到较好的跟踪效果。MPC控制器中本身就等同包含前视过程，自适应也约等同于前视自适应，后续仿真会比较变参数NP与自适应LOS的路径追踪效果。

预测步长与曲率之间的关系可表示为

NP=Round(c1|k|+c2)

(42)

式中：c1为曲率增益系数，c2为预测步长NP的最小值，本文取c1=400,c2=5。

3 仿真实验

本文提出了自适应视线(LOS)制导法结合MPC控制的循迹优化策略，为了检测该方法的有效性，需要通过Simulink仿真平台进行模拟：将自适应LOS制导结合传统MPC的路径跟踪策略应用到循迹控制过程中，同时设计了传统LOS +传统MPC以及传统LOS+自适应MPC的路径跟随方法，并将上述三种轨迹追踪仿真结果进行比较。车辆大多数时间沿直线路径行驶，偶尔遇到需要更换车道以及加速的工况。因此，本文分别设计了直线路径和曲线路径工况下的路径追踪仿真。考虑到计算机硬件的运算能力，设定传统MPC控制器的参数为：NP=5,NC=2,T=1 s；纵向PID控制器参数为：KP=0.2,KI=0.1,KD=0，车辆模型参数见表1。

表1 无人车模型数据Tab.1 Parameters of unmanned vehicle model

3.1 直线路径的跟踪结果

车辆追踪直线路径时，设定期望车速28 km/h,起步时的位置坐标为(-10,40)并处于静止状态，横向初始偏差为-20 m，初始航向角度与目标路径方位角的差值为0 rad。仿真实验结果见图3，图3(a)清晰地表明传统LOS+传统MPC，传统LOS+变预测步长MPC以及自适应LOS+传统MPC这三种路径追踪策略均可实现直线的良好跟踪。经过对比，自适应LOS制导律机动灵活，响应迅速，可以更快收敛到目标直线，整体的行驶平顺性也更加出色，主要是因为初始阶段车辆与目标直线距离较大，前视距离Δ相应地缩短，自适应LOS发出更为“激进”的追踪指令。随着车辆接近目标直线，自适应LOS的前视距离相应地增加，让震荡有所缓解，追踪也更加地平稳。考虑到直线参考路径的曲率k=0，两种MPC控制器根据公式(42)取相同的预测步长NP，因此传统LOS+传统MPC与传统LOS+自适应MPC在追踪直线过程中有着相同的轨迹，这两种循迹策略在之后四个评价指标中也有着重合的曲线。图3(b)与图3(c)分别为横向误差以及前视距离随时间变化的对比结果。虽然在车辆到达目标直线附近时，三种追踪策略的横向偏差会有稍许的波动，但是都可以稳定地收敛于0。其中，自适应LOS+传统MPC的横向误差比变预测步长的MPC控制器收敛得更加迅速，延迟相对较小，跟踪精度也更为理想。图3(d)为前轮转角对比图,能够看出自适应LOS的方向盘转向幅度较大，横向误差收敛变快，说明系统起步阶段对跟踪性能更加侧重。由图3(d)可以看出，对于设计的自适应LOS，前期的航向角收敛速度快，达到稳态航向角后的震荡、抖动与自适应MPC相比得到了大幅度改善。图3(f)为纵向速度变化曲线。三种循迹策略的纵向控制都是采用相同的PID控制器，因此，纵向速度收敛曲线重合。车辆由最开始的静止状态经过40 s的加速后平稳地收敛到期望值，有效地避免了超调以及静态误差，纵向车速跟踪效果较好；综上所述，本文所设计的横纵向控制器满足循迹的要求。

3.2 曲线路径的跟踪结果

1)假设车辆在S型无障碍物赛道上行驶，该赛道由曲率半径分别为10 m和15 m的半圆以及其他直线构成。起始位置坐标为(-10,25)，横向初始偏差为5 m，初始航向角为0 rad，循迹车由静止加速到目标车速30 km/h，随后保持纵向的匀速运动。仿真结果见图4，从图4(a)能够看出三种追踪系统都可以在有限的时间内出色地完成曲线路径追踪任务。尽管第二种循迹策略的预测时域NP会随着赛道曲率k的变化自动调节，但自适应LOS在初始阶段的转弯更“激烈”，收敛更快，转弯期间以一个弧度相对更小的曲线沿着圆形赛道行驶，具有更加出色的动态追踪能力。图4(b)为横向误差随时间变化的对比结果。随着转弯处曲率k的增加，第二种循迹策略中控制器的预测步长NP调整变长，追踪精度有所上升，因此比传统LOS+传统MPC策略的横向误差指标表现得更为出色。图4(c)与图4(d)给出了三种策略的前视距离与转向角变化曲线。可以看出当车辆与参考路径误差较大时，自适应LOS对追踪的时效性有着更高的要求，因此误差收敛速度变快；当位置误差变小时，行驶平稳性占更高的权重，此时的前视距离随之增加，车辆的行驶也更加平稳。无论是传统LOS还是自适应LOS，前轮转角一直处于约束阈值内，自适应LOS要求的转向操纵指令更迅速，转向幅度更大。分析图4(e)的航向角变化曲线可得：与传统LOS相比，采用自适应LOS制导算法的航向角前期响应迅速，这意味着更为优秀的动态追踪能力，随着车辆接近参考曲线，车辆的平稳性提高，有效避免了超调。图4(f)中的实际车速与期望车速对比图可看出纵向速度收敛地迅速且平稳，跟踪效果好；再结合图4(f)中的横向误差以及而图4(e)中的航向角均控制在合理范围之内，说明本文的横纵综合控制器可在曲线工况下保证车辆完成追踪任务，再次验证了其可行性与可靠性。

图3 直线路径追踪效果评价指标变化曲线Fig.3 Evaluating indicator of straight path tracking

图4 曲线路径追踪效果评价指标变化曲线Fig.4 Evaluating indicator of curve path tracking

4 结 论

1)围绕着如何提高自主循迹车辆路径跟踪的响应速度和平稳性，本文提出了基于可变前视距离视线(LOS)制导结合模型预测控制(MPC)的思想，为无人车提供充足的操纵和响应时间，保证汽车更加灵活地追踪路径。

2)基于传统LOS制导算法,将前视距离Δ当成一个随横向误差不断优化的时变量，提升追踪的灵活度与稳定性，并基于李雅普诺夫稳定性理论证明LOS制导算法满足横向跟踪误差yc最后趋近于0。

3)采用单车模型作为研究的对象，建立能够分析车辆稳定性与操纵性的动力学模型，并结合自适应LOS的状态方程建立系统追踪控制模型，为模型预测控制器的设计做好准备。

4)基于模型预测控制(MPC)理论设计带约束的惩罚函数，预测系统未来时域内的状态变量，并通过滚动优化以及反馈校正求取方向盘的最优输入量，从而快速且平稳地跟踪参考轨迹。

5)在Simulink仿真环境分别进行直线路径和曲线路径的追踪测试，验证了自适应LOS制导算法能够基于当前的横向误差及时调整前视距离，有效提高无人车路径跟踪的响应速度和平稳性。