基于最优控制的智能车轨迹跟踪算法

,, ,

(1.华南理工大学汽车零部件技术国家地方联合工程实验室,广东 广州 510640；2.广汽集团汽车工程研究院，广东 广州 511495)

0 引言

轨迹跟踪控制作为智能车车辆控制的一项重要技术，在满足一定的舒适性、安全性和稳定性的前提下，轨迹跟踪控制系统的目的是，通过控制行驶车辆的速度和行驶方向，使其跟踪规划的曲线与路径[1]。早期研究人员以经典 PID 控制为基础，提出了一种前馈加PID反馈复合成非线性PID控制的方法[2]。后研究人员为提高PID控制的鲁棒性，采用了以具有学习功能的自适应[3]、预瞄控制[4]、模糊控制[5]、基于道路势场的车道偏离自动校正自适应控制[6]等方法优化PID算法。虽然PID 控制可以处理智能车辆的轨迹跟踪问题，但是PID控制器的参数需要通过试凑的方法获得，因此最优控制难以实现。除此之外，基于模型预测的轨迹跟踪算法也受到研究人员的关注。比如采用非奇异终端滑模技术设计了偏航率跟踪控制律，实现对变道的理想跟踪性能[7]；在模型预测的基础上，结合前馈- 反馈的方式实现轨迹跟踪控制[8]。进一步，研究人员提出了基于遗传优化的智能车轨迹跟踪模糊控制[9]、基于神经网络滑模的轨迹跟踪控制[10]等智能化控制算法。然而，智能化控制器大大提高了其设计和运用时复杂程度，且控制参数对车辆状态参数识别精度和传感器精度要求高，计算量大，不利于实时运用。例如模糊控制器从一辆汽车移植到另一辆不同参数的汽车时，根据经验制定的模糊规则必须重新建立，因而可移植性较差[11]。

在此，试图寻找一个通用性强的，鲁棒性好的方法。线性二次型最优控制方法是一种模型参数物理意义清晰，易于移植、抗干扰能力强的优化算法，基于此，提出了一种基于线性二次型最优控制的速度自适应最优轨迹跟踪控制算法。通过建立车辆运动学方程，进一步分析车辆轨迹跟踪误差模型，基于一阶泰勒展开对误差模型进行线性化处理，进而设计线性二次型最优控制器，并将算法运用于实验平台，表现出了较强的抗干扰能力。

1 轨迹跟踪算法

一般而言，智能车循迹控制分为2个步骤：路径规划及轨迹跟踪。在此，关注第2个步骤，第1个步骤由更高的决策层生成。

智能车控制结构如图1所示。

图1 智能车控制结构

本文规划的路径作为轨迹跟踪的输入，其格式为一系列带有时间信息的车辆状态[(xt0,yt0,θt0)(xt1,yt1,θt1)…(xt,yt,θt)…(xtf,ytf,θtf)]。(xt,yt)为t时刻车辆的期望位置；θt为t时刻车辆期望的航向角，即车辆行驶方向与横坐标的夹角，顺时针为正。

1.1 车辆模型

本文智能车平台车辆状态传感器为载波相位差分(RTK)GPS，其输出物理量为位置、速度和方向，因此选择汽车运动学模型，以揭示汽车在全局坐标系中的位移与汽车速度、横摆角和前轮转角之间的关系。

在一系列的假设条件下，可以用一个线性二自由度的二轮车模型来代替智能汽车模型[12]。通常的假设包括：省去汽车的转向系统，以前轮转角直接作为转向输入；不考虑悬架，即忽略汽车的俯仰和侧倾运动；车辆纵向车速视作定值；轮胎处于线性区；忽略地面切向力对轮胎的影响等。车辆运动模型如图2所示。

图2 车辆运动模型

汽车前轮中心的坐标与汽车横摆角和前轮转角之间的关系表示为：

(1)

前后轮坐标的关系为：

(2)

车辆后轮轮速的关系为：

(3)

(x,y)和(xf,yf)分别为后轮和前轮中心位置；θ为车辆当前前进方向与横坐标的夹角；δ为前轮转角；L为车辆轴距；v为车辆前进速度。

解以上方程组，可得

(4)

1.2 误差模型

设此时车辆跟踪直线y=0，如图3所示。

图3 误差模型

车辆系统输出为(x,y,θ)。

在给定期望位置和方向(xt,yt,θt)情况下，车辆运行的位置和方向偏差表达为：

e=[exeyeθ]T=[x-xty-ytθ-θt]T

(5)

偏差逆时针为正，顺时针为负。由于跟踪y=0，所以有：

(6)

根据式(4)、式(5)和式(6)，车辆轨迹跟踪误差动力学模型可以表达为：

(7)

为使方程解耦，在不同的速度工况下，我们取v为不同的值，以实现不同速度下的自适应轨迹跟踪。重新定义状态变量

X=[eyeθ]T

(8)

则动力学方程为：

(9)

当δ和θ较小时， 有：

(10)

由式(8)和式(10)，可以得到状态方程为：

(11)

其中

可控性矩阵

因为v>0，Rank(BAB)=2可控性矩阵是非奇异的，所以系统是可控的。

1.3 速度自适应线性二次型最优控制器设计

根据线性二次型最优控制的基本原理，用泛函数表示跟踪控制律性能指标为：

(12)

积分项XTQdt反映了轨迹跟踪控制过程中动态跟踪误差的累积，权矩阵Q能给不同的误差状态变量赋予不同的加权，这一项反映了系统的控制效果；而第二个积分项δTRdt中的R能给控制分量赋予不同的加权。二次型指标式极小即在模型跟踪过程中使跟踪参数动态跟踪误差、控制能量消耗综合最优。

其中

(13)

R=[r]

(14)

q1，q2为权重矩阵Q的元素；r为权重矩阵R的元素。考虑到在速度较低时，系统处在较稳定的运行状态，车辆轮胎转角可以较大，让车辆在安全的前提下快速响应恢复到平衡状态；而在高速工况时，为了防止由于较大侧偏、侧倾现象的出现，从而导致车辆乘适性和安全性降低，车辆轮胎转角应当保持在较小的范围内调整。

因此，对于方向盘控制能量指标权重R，应当与车辆系统速度相适应。本文取

r=kv

(15)

k为调整系数;v为车辆速度。

利用庞德亚金极小值原理，可以使为最小值的最优控制信号δ(t)=-KX(t)；其中最优反馈矩阵K=R-1BTp。p是黎卡提方程(16)的解：

PA+ATP-PBR-1BTP-CTQC=0

(16)

控制规律为：

δ(t)=δf-KX(t)

(17)

δf为前馈量，本文取前馈量为：

δf=arctan(Lρ)

(18)

ρ为轨迹曲率。

轨迹跟踪算法示意如图4所示。

图4 控制模型

2 仿真分析

在车辆受到高斯白噪声干扰的情况下，分别在不同速度工况下(10 km/h，20 km/h，30 km/h)对比基于距离偏差的PID控制和本文的线性二次型最优控制轨迹跟踪算法，结果如图5所示。两者的轨迹跟踪横向偏差的均方差结果如表1所示。

从图5和表1分析可知，在同一工况下，基于线性二次型最优控制的轨迹跟踪算法具有比PID控制更小的均方差值，明显具有更加稳定和精确的轨迹跟踪能力；并且随着速度的增大，两者的均方差值比越来越大，可见基于线性二次型最优控制的速度自适应轨迹跟踪具有更优良的速度适应能力。

根据化曲为直的极限思想，曲线轨迹可以分解为多条直线段，将直线线性二次型最优控制轨迹跟踪控制算法拓展运用于曲线时，跟踪当前位置切向的直线，并使用式(18)进行前馈补偿。图6是车辆跟踪圆弧曲线的仿真轨迹。由图6可知，本文的算法对于曲线轨迹跟踪也有着较好的效果。其跟踪横向偏差均方差为0.011 m，基本实现了稳定精确的轨迹跟踪。

图5 线性二次最优控制和PID直线跟踪效果

表1 线性二次型最优控制和PID跟踪算法均方差对比

图6 曲线跟踪仿真

3 实验验证

3.1 实验平台

本文的实验平台为某自主品牌改装的实验样车，如图 7 所示。车内搭载一个北斗星通载波相位高精度差分GPS接收机，两路天线输入信号独立跟踪，全天候提供毫米级载波相位观测值和厘米级 RTK 定位精度。最高采样频率10 Hz，最高定位精度0.01 m，速度精度0.03 m/s，航向精度 0.06°。由于实验在开阔场地进行，从而保证解算得到的位置和航向角具有很高的精度、较好的抗干扰性和稳定性。实验平台方向盘传感器最小分辨率0.1°，范围为±500°。

图7 试验车平台

3.2 实验数据分析

首先进行了10 km/h，20 km/h，30 km/h直道轨迹跟踪测试，用于测试算法在小曲率直道不同速度下行驶时的表现效果；然后对S形曲线进行了 20 km/h的弯道轨迹跟踪测试，用于测试算法在较大曲率弯道低速行驶时的表现效果。考虑到如果控制效果较差会给实车实验时带来一定危险性，本文并未实施PID控制轨迹跟踪实车实验。

直道轨迹跟踪效果如图8所示。其不同速度工况下的跟踪均方差和误差绝对值平均值如表2所示。

表2 直道跟踪方差和误差均值

从表2分析可知，轨迹跟踪均方差和误差绝对值均值均保持在厘米数量级的水平，算法表现出了良好的鲁棒性和精确的轨迹跟踪能力。从图8分析可知，在低速工况时，车辆轨迹跟踪在较小范围内波动，表现出了快速的响应和收敛能力；随着速度的不断增加，虽然波动的幅度逐渐增加，但仍保持在小范围内，波动趋缓，保证了高速运动时的乘适性和安全性。

图9是车辆在S形曲线以20 km/h的速度跟踪行驶时的轨迹曲线，其跟踪的横向偏差均方差值为0.136 m，可见算法对于曲线工况也有较好的应用效果，在大曲率跟踪时表现出稳定的跟踪性能。

图8 直线路段跟踪曲线

图9 S形路段跟踪曲线

4 结束语

通过在仿真和真实道路环境下进行了轨迹跟踪实验验证，结果表明， 算法能够适用于小曲率直道跟踪和大曲率曲线跟踪等不同场景，并且表现出了稳定和精确的跟踪能力；由于算法对不同速度工况下做了最优反馈的自适应调节，因此在大部分工况特别是高速下表现出安全稳定的轨迹跟踪性能。