多路径下航向预估的无人车路径跟踪算法研究

鲁棒棒，皮大伟，谢伯元，王显会

(南京理工大学，江苏 南京 210094)

1 引言

作为当今汽车行业的热门，无人驾驶车辆已广泛应用于安保巡逻、物流配送等方面。对此，无人驾驶车辆在较低车速下的路径跟踪研究显得尤为关键。在低速下的路径跟踪研究中最为关键的是设计精度高、稳定、适应性好的路径跟踪控制器[1-2]。目前，研究路径跟踪问题的控制算法有很多，主要为纯点跟踪算法、斯坦利算法、模型预测算法[3-6]等。国内外学者也对此做了大量研究[7-9]，这方面面临的主要问题是纵向车速和方向的解耦降低了算法的精度和稳定性，目前的路径跟踪控制算法在设计控制器的时候大多忽略纵向车速的影响，以某一恒定的经验来确定控制器的参数，导致纵向车速变化后控制精度和稳定性都有所降低。为解决这一问题，以一辆无人物流车为研究对象，以GPS采集的离散数表形式的数据建立多路径下的预期道路模型，通过引入GPS获取的车辆位置、惯性导航系统、前轮转角、车速等位姿反馈量，建立二自由度车辆模型、航向预估模型及附加航向模型，计算横向位置偏差、航向预估量及附加航向，以航向预估量、附加航向、车辆当前航向、前轮转角为航向反馈量，设计多路径下基于航向预估的模糊PID控制器，并通过SIMULINK仿真和对应的实车实对算法进行验证。

控制架构如图1。本文旨在设计多路径下适应纵向车速的基于航向预估与模糊控制的PID控制器，安排如下：第二节着重于预期道路模型、航向预估模型、附加航向模型的建立。在第三节，设计了多路径下适应纵向车速的基于航向预估的模糊PID控制器。第四节通过SINULINK仿真和对应的实车实验对提出的路径跟踪算法进行验证。最后，在第五节中给出简短的结论。

图1 控制架构

2 动力学与数学模型

2.1 道路模型

本文设计的控制器建立在预先获取的理想道路模型的前提下。无人车希望跟随的“道路”千变万化，错综复杂[10]。考虑到本文的道路模型通过GPS采集的数据获取，采用离散的数表形式描述道路，如图2。根据道路曲率的大小确定该段道路离散点的数量，二者成正比，道路曲率越大，数量越多，点越密集。

图2 预期道路的描述

2.2 二自由度车辆模型

无人驾驶车辆在进行路径跟踪的过程中，横向位置偏差y随纵向车速、横摆角速度等车辆状态参数改变而变化。无人驾驶车辆作为高度非线性的复杂系统，横向车速、横摆角速度等参数无法通过测量获取。为更精确地计算由车辆当前位置与预期道路产生的横向位置偏差y，引入只有侧向和横摆两个自由度的车辆二自由度模型。

车辆二自由度模型微分方程如下：

(1)

(2)

其中：

m为无人驾驶车辆整车质量(kg)；

Iz为车辆绕z轴的转动惯量(kg.m2)；

kf、kr分别为前后轮胎侧偏系数(N/rad)；

u、ν分别为汽车纵向、横向速度(m/s)；

a、b分别为质心到前后轴距离(m)；

ω为汽车横摆角速度(rad/s)；

β为质心侧偏角(rad)；

δ为前轮转角(rad)；

考虑车辆在转向时转向机构具有一定的迟滞性，视为一阶惯性环节，则有:

(3)

假设车辆在行驶过程中横向车速v远远小于纵向车速u，即u≫v。如图3，有

(4)

联立上式(1)(2)(3)(4)可获得以横摆角速度、横向车速、横向位置偏差、前轮转角为状态量的系统状态空间表达式

(5)

2.3 附加航向模型

附加航向模型如图3。在大地直角坐标系下，(X0，Y0)为GPS获取的车辆在当前时刻质心位置的坐标，(Xk+1，Yk+1)为预瞄点rk+1的坐标，ψ为大地坐标系下通过惯性导航系统获得的车辆的当前航向，ψd为预瞄点rk+1在大地直角坐标系下无人驾驶车辆希望跟随的期望航向。任何对无人驾驶车辆的控制需要转换至车辆坐标系下进行，对预瞄点rk+1进行坐标转换

(6)

无人驾驶车辆当前时刻的实际航向与预期道路下期望航向的航向差值φ为

φ=ψd-ψ

(7)

车辆质心至预瞄点rk+1的距离l为

(8)

由于横向位置偏差y而产生的附加航向φΔ为

(9)

图3 附加航向模型

2.3 航向预估模型

航向预估模型就是基于纵向车速、前轮转角预测出车辆的航向变化趋势，以此为航向反馈并计入控制偏差来影响控制器输出的前轮转角，减小因航向偏差滞后带来的控制效果变差的影响[11]。本文提出的航向与估算法基于简化的车辆二自由度模型，预估模型如图4。O为无人驾驶车辆转向时的瞬时转向中心，r为无人驾驶车辆转向时绕O的转向半径，G为无人驾驶车辆的质心，δ为前轮转过的角度，a、b分别为G到车辆前轴与后轴的距离。

图4 航向预估模型

设控制器的采样周期为T，无人驾驶车辆的前轮车速近似等于纵向车速u，车辆在T内的航向变化量即航向预估量Δθ近似为

Δθ=uT/r

(10)

前轮绕转向中心O的转向半径r近似为

r=(a+b)/sinδ

(11)

联立上面两式可得

Δθ=uTsinδ/(a+b)

(12)

3 控制器设计

本文的控制策略建立在航向跟踪的基础上，在无人驾驶车辆的控制过程中，产生的横向位置偏差和航向偏差全部转化为航向偏差进行控制。为解决直道下车辆纵向车速变化时控制算法精度和安全性下降、弯道的航向偏差大、驶入弯道时转向不足、驶出弯道时过度转向等问题，考虑车速和前轮转角和横向位置对航向变化的影响，提出多路径下基于航向预估的模糊PID控制算法。控制器分为两层，上层为多路径控制器，计算直道和弯道下对应的航向偏差量，确保车辆在多路径、纵向车速变化下行驶时具有良好的跟踪性能，下层为模糊PID控制器，计算下发至车辆的期望前轮转角，控制器架构如图5。

图5 控制器架构

3.1 多路径控制器

设计多路径控制器的目的是解决因路径(直道和弯道)不同和纵向车速变化而导致的跟踪算法精度和稳定性下降的问题。为此引入路径标志变量signal、0-1损失函数和代价变量kψ、kψd、kΔθ、kφΔ、kδ，控制器结构如图6。

变量signal为衡量车辆处于直道或者弯道的标志，若处于直道，signal≥0；处于弯道时，signal<0。

各代价变量为

(13)

控制器输出量航向偏差E为

E=kψdψd-kψψ-kΔθΔθ-kφΔφΔ-kδδ

(14)

图6 多路径控制器结构

3.2 模糊PID控制器

考虑到车辆纵向和横摆运动之间的耦合关系，下层控制器器是基于模糊和PID控制理论设计的，控制框图如图7所示。模糊PID控制器可以计算下发至车辆的前轮期望转角。

图7 模糊PID控制框图

模糊PID控制器由模糊控制和PID控制两部分组成。模糊控制器的输出量为PID控制器的三个参数。然后PID控制器计算下发至车辆的期望转角。通过模糊控制对PID控制器的Kp、Ki、Kd三个参数自整定，可提高系统的鲁棒性、稳健性和适应性。

上层多路径控制器的输出量为航向偏差E，因此模糊控制器的输入量为航向偏差E及航向偏差变化率EC，模糊控制器的输出为PID控制器的三个可调参数调整量ΔKp、ΔKi、ΔKd。

在模糊化过程中，输入变量E、EC输出模糊集合分别为NB(负大)，NM(负中)，NS(负小)，ZO(零)，PS(正小)，PM(正中)，PB(正大)，E及EC的模糊论域为[-3，3]。隶属度函数采用较为简单的三角函数，量化因子为1.1和2.0。ΔKp、ΔKi、ΔKd的输出论域分别为[-3，3]，[-1，1]，[-0.2，0.2]。

将车辆行驶工况分成下述三个典型工况进行分析，以确定Kp，Ki，Kd的选取规则。

规则1：if E is PB and EC is PB，then ΔKpis NB，ΔKiis PB，ΔKdis PS

在这种工况下，车辆当前实际航向远远小于希望跟随的期望航向，车辆处于严重转向不足状态。为了使车辆能够迅速跟随上期望航向，应取较大的Kp，增大前轮转角，同时为避免转向机构超过极限位置，应取较小的Kd，Ki应取为0，防止车辆实际航向超过期望航向。

规则2：if E is NS and EC is NB，then ΔKpis PM，ΔKiis NB，ΔKdis Z0

在这种工况下，车辆当前实际航向略大于希望跟随的期望航向。为提高跟踪精度，减小车辆在稳定后与期望航向的差值，应取较大的Kp，Ki，同时，为减少车辆进入稳定状态的调节时间，应取较小的Kd。

规则3：if E is PM and EC is NM，then ΔKpis Z0，ΔKiis Z0，ΔKdis NS

在这种工况下，车辆当前实际航向相比于希望跟随的期望航向小一些。为避免车辆在跟踪过程中产生航向超调，应取较小的Kp，同时，为减少车辆进入稳定状态的调节时间，Ki应取适中的值，应取较小的Kd并保持不变。由此构成的模糊规则如表2所示。

表1 模糊控制规则表

PID控制器的输入变量为航向偏差值E，输出变量为期望转角δd，δd为

(15)

模糊PID控制器的参数如表2。

表2 模糊PID参数

4 仿真与实车实验

为验证本文提出的算法的有效性，本文进行了SIMULINK仿真和实车实验，为表现所提出的多路径下基于航向预估的模糊PID控制器(以下简称航向预估算法)的跟踪性能，与只以实际航向和附加航向为反馈量的增量式PID算法控制器进行比较。

4.1 仿真研究

仿真在MATLAB/SIMULINK环境下进行，实验车辆前轮转角的最大值为45°，为与其一致，将仿真中控制器输出中加入最大值为0.785rad。

考虑到GPS等传感器的采样周期，将控制器采样周期设为100ms，实验车速为2-4m/s，仿真时系统参数如表4所示。

表3 车辆参数

以实验场地中的一段直道加弯道的实际路径作为仿真路径，仿真初始位置偏差为1.1m，航向偏差为1°，直道时车速为4m/s，进入弯道时减速为2m/s，过弯后再加速至4m/s，仿真结果见图8、图9、图10。

图8 路径跟踪曲线

图9 航向跟踪曲线

图10 横向位置误差曲线

图11 纵向车速曲线

图8表明，航向预估算法的跟踪效果比常规PID控制算更好法。从图9可以看出，在直道时，纵向车速由0.5增加至4m/s时，航向预估算法对应的航向偏差变化较为平缓，在第16.5s时航向偏差趋于0，车辆进入稳定状态，常规PID控制算法航向变化较为剧烈，在第23.3s时航向偏差趋于0，调整时间较长；在弯道时，航向预估算法航向跟踪效果比较理想，始终与理想航向保持一定的偏差，而常规控制算法在第47-52s内时航向超调，在第62s时，驶出弯道重新驶入直道，车辆出现航向超调现象。图10表明，当纵向车速变化时，航向预估算法的横向位置误差较小，保持在0.1m以内，常规控制算法出现较为剧烈的震荡现象；综合比较图8、图9、图10和图11，航向预估算法的控制效果、稳定性以及对车速的适应性优于常规算法。

4.2 实车实验

实验平台为无人驾驶车辆，车辆位置由差分GPS系统给出，车辆航向由惯性导航系统给出，实验路径由一段直道加弯道和过弯后的一段直道组成，路径初始位置偏差为0.5m，航向偏差为0°，直道时车速为4m/s，进入弯道时减速为2m/s，过弯后再加速至4m/s，实验结果见图12、图13、图14。

图12 路径跟踪曲线

图13 横向位置偏差曲线

图14 纵向车速曲线

从图12可以看出，航向预估算法能够较好地跟随上路径，而常规控制算法出现明显的震荡，控制的路径沿理想路径左右摇摆；图13表明，航向预估算法的横向位置偏差超调量较小，最大为0.1m，车辆进入稳定状态的调节时间为13s，在驶入和驶出弯道时横向位置偏差保持在0.1m以内，而常规PID控制算法的横向位置偏差最大达到了0.2m，车辆进入稳定状态的调节时间为18s，且出现明显的震荡，相比于常规控制算法，横向位置偏差减少了0.1m，调节时间缩短了27.7%。

5 结论

针对低速下无人车的路径跟踪问题，设计了多路径下适应纵向车速的航向预估模糊PID控制器。通过引入航向预估量、前轮转角、附加航向、实际航向等反馈量，解决多路径下、纵向车速变化等因素下引起的跟踪精度与稳定性下降的问题。在未增加计算复杂性的前提下，提出的算法与常规算法相比，提高了路径跟踪的精度(横向位置偏差减少至0.1m以内)，算法的稳定有所提升(调节时间减少27.7%)。

提出的算法只在低速下对纵向车速等变化因素只具有一定的自适应能力，应用于全车速下的自适应路径跟踪算法仍待研究。