王鑫 凌铭 饶启鹏 刘畅 翟树龙

（上海工程技术大学，上海 201620）

主题词：无人驾驶 路径跟踪 纯跟踪算法 斯坦利算法 融合算法

1 前言

近年来，随着自动驾驶和无人驾驶技术的发展，车辆自身控制的安全性和稳定性研究一直备受关注。路径跟踪是其中的重要环节之一，直接决定了车辆自身控制的安全性和稳定性。

自1980 年卡内基梅隆大学无人驾驶车出现至今，路径跟踪控制模型的研究有了长足的发展。早期的控制算法以PID 反馈控制为主，后来出现了多种控制算法，如：模型预测控制（Model Predictive Control，MPC）算法能够较为有效地解决线性和非线性控制系统中的约束问题，线性二次型调节器（Linear Quadratic Regulator，LQR）算法也能利用模型进行行为预测，然后实现对预测结果的跟踪。也有学者提出基于几何关系的模型控制算法：文献[9]提出了一种基于斯坦利（Stanley）算法的自适应最优预瞄模型，文献[10]利用GPS 获取位置信息，采用纯跟踪（Pure Pursuit）算法实现路径跟踪。以上控制算法均以控制跟踪精度为主要目标，忽略了无人车对跟踪平滑性的要求，乘坐舒适性有待提高。

本文基于纯跟踪算法在路径跟踪时对车轮转角的计算原理，在Stanley算法的控制模型上进行改进，提出一种融合算法，在不同车速下，依据横向距离、航向角、合适的预瞄距离等车辆参数，得到当前状态下合适的车轮转角，在保证跟踪精确度的情况下，能够保证良好的跟踪路径平滑性，以提高乘坐舒适性。

2 转向几何模型

2.1 自行车运动模型

无人车系统的运动学模型相对于无人机或机械臂运动模型来说更简洁，但并非线性系统，应用于具体的控制算法时，有必要对原始的运动学模型进行线性化。本文使用自行车运动学模型，如图1所示。

图1 自行车运动学模型

图1中，为车辆前轮的转向角，为轴距，为后轮对应的转弯半径。该运动模型实际上由阿克曼转向几何简化得到，将四轮车辆模型简化为两轮单轨模型。该运动模型一般采取以下假设：车辆只在水平面上运动，不考虑空气阻力、轮胎的侧滑和车辆的俯仰角，车辆的前轮转向时，内、外车轮转动相同的角度。采用自行车模型能够简化无人车的前轮转向角与后轮转向曲率之间的几何关系，其关系可表示为：

2.2 纯跟踪控制模型

纯追踪算法的运动模型基于自行车模型得到，如图2所示。

该算法的核心思想是：以车辆后轮中心点为参考点，通过控制前轮的转向角度来控制车辆沿一段圆弧行驶，使后轮的中心点经过路径上的预瞄点。图2中，()为预瞄点坐标，为车辆后轴心与预瞄点的连线与当前航向之间的夹角，为车辆后轴中心点到预瞄点的距离，即前视距离。根据正弦定理可得：

图2 纯跟踪算法运动学模型

式中，为后轮转向时所对应的曲率。

由自行车运动模型可得：

由此建立了与、、之间的关系。该算法主要影响参数为，其与车速总体呈正相关，且需约束其取值范围。

2.3 Stanley控制模型

Stanley算法的控制模型原理如图3所示。

图3 Stanley算法运动学模型

该算法核心思想也是对车辆前轮进行控制，通过消除横向偏差来实现路径跟踪。其控制原理可表示为：

式中，为航向角，是车辆的前后轴心连线方向与距离车前轮轴心最近的轨迹点的切线方向之间的夹角；为车速；为横向误差；为可进行调节的控制量增益。

由式（6）可看出，当横向误差不存在或者无穷小时，有：

当航向角偏差不存在或者无穷小时，有：

由此可见，此算法对于被控量的影响主要在于航向角和横向误差。在值一定，车速不变的前提下，横向误差和航向角增大，则算法会增大车轮的转角控制量，进而减小误差实现轨迹跟踪。

Stanley 方法的特点是跟踪效果主要受到增益的影响，并且拥有前馈控制，能够提前补足转弯过程中的车辆转角，可根据设计需求对前馈值进一步增加增益系数。但当在合适取值范围内且速度不变时，与跟踪精度正相关，与平滑度负相关。

3 融合控制算法

3.1 融合算法原理

融合算法的控制模型原理如图4所示。

图4 融合算法运动学模型

对模型的车辆行驶环境进行如下假设：不考虑阿克曼转向几何，忽略轮胎硬度和侧滑，不考虑车辆悬架特性、道路阻力以及俯仰角，近似认为汽车自身驱动与制动之间的相互作用是线性平滑的理想状态。

分析Stanley 算法和纯跟踪算法可知，式（4）和式（6）中的车轮转角都是由于航向角所引起的控制量的变化。控制量的影响因素相同，则为两者分配不同的权重、，且+=1，只要找到合适的、即可使效果叠加后，既能保证车轮转角的输出结果不变或变化甚微，又能使平滑性相对可控。

当横向误差不存在或者无穷小时，有：

当航向角偏差不存在或者无穷小时，的计算与式（7）相同。

则融合算法的车轮转角为：

由Stanley 算法可知，增大以提高精确度，但平滑度降低；由纯跟踪算法可知，增大可增加平滑度，但精确度会降低。则式（9）可以通过同时增大和使平滑性和精确度能够相互得到补偿，并调整影响因子和的值使平滑性和精确度维持在相对平衡的水平上，在不损失精度或精度损失甚微的情况下，保证较好的平滑性。同时由式（6）和式（8）可知，融合算法能够减弱航向角在检测过程中因存在较大误差导致车轮转角发生突变的负面影响。

3.2 融合算法仿真

由于该算法受横向误差影响较大，当路径点不连续时，跟踪路径的平滑性会受到更大影响，对于外界刺激较为敏感。为此，直接以影响较大的点路径（Point Way）作为试验对象，测试该融合算法的性能。

基于点路径分别测试原Stanley 算法和融合算法在相同路径和速度下的车辆的最优跟踪效果，比较分析不同车速下2 种算法在最优跟踪效果前提下的跟踪精确度和平滑性的优劣，在20～80 km/h 车速范围内，车速每变化10 km/h 进行一次仿真，部分仿真跟踪结果如图5～图8 所示。

图5 车速20 km/h仿真结果

图6 车速40 km/h仿真结果

图7 车速60 km/h仿真结果

图8 车速80 km/h仿真结果

在原Stanley 算法和融合算法均处于最优跟踪效果的前提下，当速度不超过40 km/h 时，Stanley 算法中的值随速度提高呈增长趋势，但速度在40 km/h 以上时，的值随着速度的提高而减小。当速度不同时，融合算法中值并无明显变化规律，但前视距离与速度正相关。图9 所示为不同车速下各参数的变化情况。

图9 k、Ld与速度的关系

融合算法中随着速度增大而减小，随着速度增大而增大，二者变化较为规律，车速每变化10 km/h时，和的取值相应变化约0.05，如图10所示。

图10 权重与速度的关系

仿真结果表明，2种算法均处于最精确的跟踪条件时，不同速度下的仿真结果平滑性差异较大。Stanley算法在不连续的路径点之间的跟踪路径呈折线化趋势，振荡频率较高，点与点之间的过渡并不理想，想要维持较好的平滑性稍有困难。而在曲率较大的路径段，平滑效果更差，甚至出现跟踪精确度损失较大的情况。速度越高时，这一趋势越明显。而改进后的融合算法在相同速度和相同路径段能够在维持其精确度的前提下，或者以微小的精确度损失为补偿，使平滑性得到明显提升。在曲率较大的路径段，过渡更为自然平滑，精确度也极大改善。并且在路径跟随的初始阶段，融合算法在减小振荡的功能方面也能保持良好效果，也能够快速进入稳定阶段。

4 实车试验

试验采用的车辆如图11 所示。车辆配有CAN 总线、工控机、摄像头等。车辆的轴距为2.9 m，通过使用摄像头采集路况信息，提取视频流中采集到的车道线信息，得到融合算法中的航向角、横向偏差等参数，然后通过融合算法得到实时车轮转角，将转角信息和速度信息通过CAN 总线发送给工控机，由工控机对转向系统发送转向指令，以实现路径跟随。

图11 试验车辆

在直线道路上分别对Stanley 算法和改进后的融合算法路径跟踪效果进行试验。使车辆在车道线内行驶，完成车道保持，车辆在跟踪路径过程中，质心始终在车道线中心线附近，车辆未偏离车道线，如图12 所示。

图12 记录轨迹示意

记录每个时刻得到的车轮转角，其正、负分别代表左、右转向，统计其正负值交替次数，若在一定时间内交替次数多，则说明车辆在跟随路径时振荡频率高，摇摆次数多，平滑性不佳。通过测量横向偏差来判断车辆的跟踪精确度，正值表示车辆质心在车道中心线左侧，反之在右侧。同时，在车辆稳定跟踪后，车轮转角的变化范围也能侧面反映跟踪精确度，变化范围越小，说明横向误差越小，跟踪越精确。受试验场地车速限制，并考虑无人车的道路安全性，试验速度设为20 km/h。Stanley 算法和融合算法每秒末的试验结果分别如表1、表2 所示，2 次试验的道路段相同，车辆初始位置相同。

表1 20 km/h车速下Stanley算法的试验结果

表2 20 km/h车速下融合算法的试验结果

实车试验时和的实际值与仿真值存在差异，如表3所示。

表3 20 km/h车速下的实车参数

试验结果表明，2种算法均存在一些车轮转角突变值，这是由于摄像头在采集车道信息时的一些外在因素，如车道线磨损严重等，导致识别不到较为准确的车道线，影响了航向角的判断。为了抑制突变产生的突然转向，设置转角变化范围超过6°的转角不会发送到控制端。

由表1 和表2 可知：试验前3 s 内，在车辆启动的初始阶段，由于车速不高，且车辆质心偏离车道中心线较远，车轮校正角度较大，车轮转角随着车辆向车道中心线附近移动而变小；在第3 s后，车辆质心首次越过车道中心线，车轮转角开始反向变化，以反馈控制的方式来修正跟踪路径，不会使车辆过于偏离预定轨迹，以达到精确跟踪的目的。且车轮转角在第5 s 后，融合算法的横向偏差的绝对值明显较原Stanley算法小，且车轮转角变化幅度明显减小，说明车辆质心与车道中心线的距离较小，精确度较好。由此可见，改进后的融合算法在维持跟踪精确度方面表现较好。

但在第5 s 后，2 种算法在平滑度方面差异较大。Stanley 算法实车试验中除车轮转角突变引起的车轮转角正负值交替外，总有效次数为11次，而改进后的融合算法总有效次数仅为5 次。且在后5 s 的时间段内，车轮转角突变只出现3次。由此可知，改进后的融合算法相比原Stanley算法减弱了航向角检测的大误差直接带来的车轮转角的变化影响，不仅在一定程度上抑制了车轮转角突变，而且平滑度有了显著提升。

在实车试验过程中，为了得到较为清晰的车辆轨迹，通过在车辆右后轮的轮胎上撒水的方式来记录轨迹，如图13、图14所示。其中，路段为校正阶段，路段为稳定跟踪阶段。

图13 轨迹示意

图14 实测轨迹

由图14 可看出，改进后的融合算法车辆轨迹振荡频率小，相对平滑且跟踪精确。在较远的路径中，也能维持一定的精度，不会很快脱离轨道。仿真结果和实车试验结果均能说明改进后融合算法的可行性和优越性。

5 结束语

本文通过分析Stanley 算法中航向角和横向偏差对车轮转角的影响，在斯坦利控制模型的基础上，采用纯跟踪算法中航向角对车轮转角的计算方式，提出了一种融合算法，基于航向角、横向偏差、车辆轴距、前视距离来得到合适的车轮转角信息。仿真和实车试验结果表明，改进后的融合算法在精确度和平滑性方面均有更好的表现，在保证安全性的同时，提升了乘坐舒适性。