基于改进Stanley算法的目标假车路径跟踪控制

李文礼，易 帆，封坤，王 戡，张智勇

（1.重庆理工大学汽车零部件先进制造技术教育部重点实验室，重庆 400054；2.招商局检测车辆技术研究院有限公司，重庆 401122）

0 引言

近几年，自动驾驶产业发展迅猛，自动驾驶功能测试需求也在不断增长，在智能车场地测试中，基于安全性和测试效率的考虑，通常使用软目标车作为目标车辆参与测试，软目标车由目标假车与目标假车搭载平板组成［1］。

国内外软目标车是随着ADAS测试需求的增长而不断发展的。2013年6月，Euro NCAP指定的AEB系统测试规程中规定了软目标车选用标准以及对AEB测试场景的要求，如图1所示Euro NCAP最初使用的软目标车是一种具有车辆外形的气球车［2］，该软目标车主要用来模拟前车尾部，具有一定的柔韧性，在一定的测试范围内可以避免对测试车辆造成破坏。由导航车提供动力，在发生碰撞时导轨能按照预定轨迹滑行，以此吸收碰撞产生的能量和冲击。为提升测试效率，英国Moshon Data公司在软目标车上加装了拉绳和拉杆以提升软目标车安装速度。2018年5月，Euro NCAP提高了软目标车的测试要求［3］。如图2所示为美国DRI公司生产的软目标车［4］，相比于EVT，美国DRI公司生产的软目标车的动力系统由底部的移动平台车提供，该移动平台车装置由高精度GPS设备和控制装置组成，能够实现高精度的路径跟踪，且当测试车辆与软目标车发生碰撞时（如图3所示），被试车辆能够很容易碾压上移动平台车，保护被试车辆不与软目标车金属结构发生碰撞，从而避免对被试车辆造成损坏。

图2 DRI公司生产的软目标车

图3 被试车辆与软目标车发生碰撞

软目标车众多技术指标中最重要的一环就是能够在场地测试中较真实地模拟出各种测试场景和工况，即目标假车搭载平台能够配合搭载的3D软体目标假车，通过相应的控制策略使软目标车能够按照预设的轨迹，完成交叉路口穿行、切入切出等多种复杂的测试工况。

软目标车的路径跟踪主要包括纵向的速度跟随控制以及横向的轨迹跟随控制。在纵向控制方面，常用的控制方法有比例积分微分控制（PID）、滑模控制（SMC）和模型预测控制（MPC）等。其中，PID控制因其简单实用且不需要依赖具体模型的优点，广泛应用于智能车的纵向控制。尹燕莉等［5］提出一种基于PID与Q-Learning的混合动力汽车队列分层控制策略，并通过在线仿真验证了该控制策略的有效性和工况适应性；Zennir等［6］在控制器设计中引入车辆横摆角速度，显著提高了控制性能；Han等［7］提出一种自适应PID神经网络路径跟踪控制策略，通过遗忘因子最小二乘算法辨识模型参数，通过神经网络算法调控控制器参数。在横向控制方面，常用的控制方法可大致分为3类：第1类是以几何学模型为基础的控制算法，有Stanley算法、纯跟踪算法等，第二类是以运动学为基础的控制算法，有模型预测控制、反馈线性化等，第3类则是以运动学与动力学为基础的控制算法，有滑模控制、线性二次调节以及模型预测控制等［8］。靳欣宇［9］综合考虑车辆横向误差、航向角偏差以及侧向加速度等因素，提出了一种基于Stanley的自适应最优预瞄模型；王鑫等［10］在Stanley算法模型的基础上提出了一种融合纯跟踪计算前轮转角方式的融合算法，该算法基于航向角、横向偏差、车辆轴距、前视距离等得到前轮转角信息；Shen等［11］基于运动学和动力学模型，设计了基于LPV的模型预测控制（MPC）和基于线性矩阵不等式（LMI）的线性二次型调节器（LQR），分别以运动学变量和动力学变量的形式跟踪期望路径。

面向智能车场地测试开发了一种软目标车，用来代替实车作为目标车辆参与测试，用于评估ADAS车辆的主动安全性。同时，对软目标车的轨迹跟踪控制方法进行了研究，在纵向控制方面，采用粒子群优化算法优化PID控制器，得到最优控制参数，对软目标车速度跟踪效果进行研究；在横向控制方面，以横向误差与前轮转角控制量组成适应度函数，采用遗传算法优化Stanley控制器参数，得到最优知识库，采用模糊控制的方法来自适应调节控制器参数以提高路径跟踪的精准性与稳定性，并进行了软目标车实车实验，以验证算法的有效性。

1 软目标车轨迹跟踪控制方法研究

软目标车的轨迹跟踪控制根据上位机软件制定并下发的目标路径信息规定软目标车的运行轨迹。按照软目标车通过惯导等传感器获得的自身位姿信息，通过优化算法调整车辆输入即前轮转角与驱动电机扭矩的大小，通过CAN通讯将计算所得的期望前轮转角与电机扭矩传递至执行机构，实现对目标轨迹的跟踪。

1.1 目标假车搭载平台转向系统建模

智能车的路径跟踪首先需要对车辆模型进行分析，按照当前车辆参数，给予一定的优化算法控制调节转向执行系统，使智能车能够沿着规划好的期望路径平稳行驶，同时尽可能地减少误差，确保控制精度。复杂的模型并不一定是最好的选择，对车辆运动学模型进行合理简化并且选择行使工况的约束条件才是研究的重点，能够表达控制对象的真实物理限制即可，因此，需要对软目标车的车辆模型进行相应的模型简化。

结合软目标车的结构特点，做以下假设：1）软目标车通过转向推杆来控制转向轮，假定转向轮的左右侧车轮转角大致相同，左右轮等效为单个轮子，将软目标车模型简化为两轮模型；2）忽略软目标车在紧急制动及急加速等情况下造成的轴荷转移；3）软目标车后轮无法偏转，仅通过前轮控制转向，因此，后轮转角δr近似零；4）不考虑软目标车在垂直方向的运动，假设车辆的运动是一个二维平面上的运动；简化后软目标车转向模型如图4所示。

图4 软目标车转向模型示意图

结合后轮转角δr输入大致为零，软目标车对于转角的输入反馈到前轮，前轮转角可表达为

式中：L为软目标车的轴距，R为软目标车的转弯半径。

1.2 软目标车横向控制算法研究

1.2.1 Stanley算法

Stanley算法最开始被应用到斯坦福大学的智能驾驶车上，是一种基于横向跟踪误差的非线性反馈函数，也被称为前轮反馈控制，能够实现跟踪误差指数收敛于0。

Stanley算法原理如图5所示，输出的前轮转角的控制量由横向误差引起的转角θe与航向误差引起的转角δδe两部分组成。

图5 Stanley原理示意图

在不考虑横向误差ey的前提下，要使车辆能够沿着期望路径行驶，则前轮转角应与参考路径点的切线方向一致。则在不考虑横向误差的情况下，有：

式中：θe为车辆航向与期望路径最近路径点切线方向之间的夹角。

在不考虑航向误差的前提下，横向跟踪误差越大，即ey越大，则控制量转角越大。假设经过d（t）距离，车辆与期望路径重合，则根据图中几何关系得出如下非线性比例函数：

综合考虑航向及横向误差，可得前轮转角：

由图5可知横向误差变化率：

当横向误差ey很小时，由上式可以得：

因此，横向误差收敛于指数e（t），增益参数k决定了收敛速度，是系统收敛的关键参数。

1.2.2 基于遗传算法的横向控制器参数优化

如上文所分析，说明了增益参数k会对控制器的跟踪效果具有重要影响，增益参数k的值越大，控制精度越高，但过高时会引起剧烈的摆动。针对Stanley控制器，选择合适的增益参数k至关重要。因此，引入全局搜索能力较高的遗传算法来解决对增益参数k的优化问题。

1）遗传算法概述

遗传算法（genetic algorithm，GA）是受自然进化理论启发由进化论和遗传学机理而产生的搜索算法，从任一初始种群出发，通过随机选择、交叉和变异操作，产生一群更适合环境的个体，使群体进化到搜索空间中越来越好的区域，这样一代一代的不断繁衍进化，最后收敛到一群最适应环境的个体，从而求得问题的优质解［12］，其流程如图6所示。

图6 遗传算法优化流程示意图

遗传算法主要有以下4个操作步骤：

①在规定的区域内，随机产生初试初始种群；

②计算当前种群内的个体是否适合当前的环境，即得到适应度值；

③依据得到的适应度值选择、淘汰一些个体，再通过交叉以及变异等操作得到新的种群；

④重复②和③步骤，直到完成迭代次数。

2）基于遗传算法的参数优化方法

由于遗传算法无法直接处理控制模型中的待优化参数，因此需要对实际问题进行编码，将待优化参数进行编码处理。常用编码方式有二进制编码法与浮点数编码法（实数编码法）。相比于二进制编码的方式，实数编码法能够有效地改善遗传算法的计算复杂性，适用于求解精度要求较高的问题［13］，考虑到路径跟踪对于计算精度要求较高，所以选择以实数编码的方式产生初始种群。

遗传算法基本不利用外部信息，仅通过适应值来确定目标个体是否适应当前环境，是否判定为优秀，并基于适应度值作为优胜劣汰的指标，因此，适应度函数选择很大程度上影响了遗传算法的综合性能。

在遗传算法的适应度函数设计上考虑横向误差的同时也要兼顾智能车的操纵稳定性，因此，建立以下性能指标：

式中：α和β均为系数，且满足α＋β＝1；为跟踪横向误差的平均值；为输出转交的标准差。

基于性能指标J，将其转化为适应度函数用来评价个体的优劣程度。当J的取值越小时，则适应度值应当越大，此时算法的跟踪效果最优，因此，适应度函数可以表示为

遗传算法主要参数设置如表1所示。

表1 遗传算法参数

1.2.3 自适应模糊控制系统设计

如上文所述，Stanley算法的控制效果受增益参数k的影响，在不同的道路与速度下，需要匹配不同的控制参数k才能达到最好的跟踪效果。因此，针对软目标车需要能够以不同车速在不同曲率的道路下行驶的工作需求，本文中设计以道路曲率与软目标车车速为输入，增益参数k为输出的模糊控制器，并通过遗传算法获得模糊控制参数表，以实现增益参数k的自适应调整。

将输入量定义为5个子集表示｛左中，左小，零，右小，右中｝，对应到模糊语言集｛NM，NS，ZO，PS，PM｝（模糊子集对应含义分别为NM（negative middle）＝左中，NS（negative small）＝左小，ZO（zero）＝0，PS（positive small）＝右小，PM（positivemiddle）＝右中）。

为降低模糊控制器的整体复杂程度，选择三角函数与梯形函数2种曲线形式的隶属度函数作为输入与输出变量的隶属度函数，得到各输入与输出的隶属度函数，如图7所示。基于遗传算法优化寻找不同输入下最优的增益参数，得到相应模糊控制的规则表如表2所示。

表2 模糊控制规则

图7 模糊控制隶属度函数

1.3 软目标车纵向控制方法研究

1.3.1 PID控制器

PID控制器通过比较被控对象的实时信息与目标值的误差的比例、积分和微分获得控制量，实现对受控对象的控制。

PID控制器的控制效果会受到kp、ki、kd影响［14］，但传统的人工经验调整参数的方法，无法确保PID控制器速度跟随的效果，且费时费力，基于此采用粒子群优化的方法优化PID参数。

1.3.2 粒子群优化算法

粒子群优化算法（particle swarm optimization，PSO），模拟鸟类捕食过程中的群体行为，最终找到群体最优捕食点。用一个粒子模拟鸟类个体，该粒子的当前位置即为对应优化问题的一个候选解，飞行过程即为该个体的搜寻过程，飞行速度会根据粒子的历史最优位置与种群中的粒子最优位置进行动态调整。不断迭代，更新速度和位置，得到最优解［15］。PSO优化流程如图8所示。

图8 PSO优化流程示意图

a）初始化粒子群参数：设定粒子种群个数N＝30，PID控制器待优化参数为3个，因此解空间维度为三维，对粒子进行初始化，包括每个粒子的随机速度和位置，并设置中止条件。

b）更新粒子的适应度：依据适应度函数更新粒子的当前适应度。

取ITAE指标作为PSO算法的适应度函数，即时间乘以系统误差的绝对值作为适应度函数。

c）更新个体极值：根据计算所得适应度值，比较当前粒子与个体最优位置适应度，若更优，则更新当前个体最优位置。

d）更新全局极值：根据计算所得适应度值，比较当前粒子与全局最优位置适应度，若更优，则更新当前全局最优位置。

e）更新速度和位置：通过式（12）和式（13）更新每个粒子的速度与位置。

f）检查是否满足结束条件：重复b）—f）步骤，直到达成终止条件。

2 仿真结果和分析

为验证本文中提出的软目标车轨迹跟踪方法的可行性、有效性和适应性，搭建Carsim-Matlab／Simulink联合仿真模型，由Carsim提供车辆动力学模型，Matlab／Simulink搭建车辆横纵向控制算法模型。设计了3种典型工况，分别在恒定参考车速下控制不同路段以验证算法的有效性，设计有多曲率综合工况，以验证模糊控制算法的自适应性。

2.1 换道工况仿真分析

在换道的仿真实验中，将期望车速设置为10 m／s的恒定车速。图9（a）展示了模糊Stanley算法和传统的Stanley算法在全局换道路径下的追踪效果，结果显示：2种算法在车速为10 m／s时均能较好地完成车辆换道的路径追踪任务。但是如图9（b）横向误差对比图所示，传统Stanley算法的最大横向误差绝对值约为0.17 m，而模糊Stanley算法的最大横向误差绝对值约为0.05 m，横向偏离的程度与幅度均要小于传统Stanley算法。在速度跟踪上，如图9（c）所示经过粒子群优化后的PID算法在收敛速度与稳定性上均要优于未进行优化的PID算法。

图9 换道路径仿真结果

综上所述，模糊Stanley算法在跟踪换道路径时的性能要优于传统的Stanley算法，经过粒子群优化后的PID算法也能够更好地实现对速度的跟踪。

2.2 S弯工况仿真分析

在S弯工况的仿真实验中，分别考虑了5、10 m／s以及15 m／s车速下模糊Stanley控制算法的轨迹跟踪效果，以验证不同车速下在面对曲率方向突然变化时模糊Stanley的自适应性，并以10 m／s为例对比了模糊Stanley与传统Stanley的横向误差与前轮转角。图10（a）展示了不同车速下模糊Stanley算法和车速为10 m／s的传统Stanley算法在全局换道路径下的追踪效果，在曲率发生突变时的跟随误差会随着车速的增加而变大，当车速为15 m／s时，此时的最大误差约为0.56 m。

图10 S型道路仿真结果

图10（b）与图9（c）则为车速为10 m／s时模糊Stanley与传统Stanley的横向误差与前轮转角。从图中可以看出，相比于模糊Stanley算法，固定权重的Stanley算法在曲率发生突变时，受前轮转角突变影响，其横向偏差会出现一定程度的振荡现象，且误差收敛时间较长，而经过模糊调节的Stanley控制器可以快速收敛跟踪误差，无较大浮动的振荡现象，最大的横向偏差约为0.06 m，路径跟踪的精度与稳定性都比较好，能够满足精度要求。

2.3 U型弯工况仿真分析

选取U型弯工况来模拟真实驾驶环境中的掉头动作，验证控制算法在较大曲率轨迹下跟随目标参考轨迹的能力。将目标车速设置为3 m／s，图11（a）、（b）分别为期望路径为U型弯时的全局路径与横向误差。相较于Stanley控制器，在跟踪大区率路径时模糊Stanley控制器具有较好的路径跟踪性能。

图11 U型弯路径仿真结果

2.4 综合工况仿真分析

设计包含有直线、换道、直角弯、S弯以及U型弯的多曲率工况，曲率如图12（a）所示，以验证算法的自适应性。

图12 综合工况仿真结果

图12（b）为车速跟随。经过粒子群优化后的PID控制器在车速发生变化时能够更快地收敛到期望车速，超调量较小，控制过程平稳。

从图12（c）与图12（d）可以看出，2种控制器的控制误差都在一定范围内，其中模糊Stanley控制器的横向误差控制在较小的范围内，最大误差大约为0.1 m，整体控制效果要优于Stanley控制器，并且在曲率突变处模糊Stanley能够较快地达到收敛。从图12（f）航向角可以看出，模糊Stanley控制器的航向角曲线较为平滑，这也表明模糊Stanley控制器的稳定性要高于Stanley算法。

3 智能车场地测试用软目标车实车试验

3.1 软目标车试验平台搭建

3.1.1 软目标车硬件部分

所开发的软目标车由目标假车与目标假车搭载平台两部分组成，如图13所示。为避免在车辆测试中损伤被试车辆，目标假车采用泡沫板材质及雷达反射材料制成，在与被试车辆发生碰撞时，目标假车就会被推开或散开，防止或减轻对被试车辆造成的损坏，且在视觉和雷达等传感器识别特性上与实车一致［16－17］。

图13 开发的软目标车

假车搭载平台通过装载假车目标物按照规定的行驶条件（速度、路径、减速度、加速度等）运动，模拟道路实车目标，构建复杂的道路测试场景，满足汽车的ADAS功能和自动驾驶功能测试。

为保证最低的雷达反射，同时避免被试车辆与目标假车搭载平台的金属结构发生碰撞，目标假车搭载平台的设计高度较低，且目标假车搭载平台的外部框架的裙部采用倾斜设计，以减轻被试车辆通过时的冲击，当被试车辆碾压到目标假车搭载平台上后，搭载平台内置的悬架系统会受到压力作用而缩进平台内部，保证在一定的测试范围内不会对被试车辆造成破坏。

图14所示为假车搭载平台的整体结构，包括惯性导航系统、驱动系统、转向系统、刹车总成、动力电池、无线通讯系统以及控制系统等。假车搭载平台的驱动轮被串联在同一根转轴上，形成三轮结构，避免了转速差对转向系统的干扰，降低方向控制的难度，且由2个高性能伺服电机通过链传动带动车轮转动，为软目标车提供动力；该平台车转向轮由一套高精度单轴驱动器带动，并通过其连杆机构实现车轮转向的控制；在制动方面，设计时为每个轮子都设计有独立的刹车机构，确保测试时能够提供足够的制动力矩。

图14 目标假车搭载平台整体结构示意图

3.1.2 软目标车系统软件开发

要想实现软目标车的路径跟踪效果，还需要在软目标车的上位机、下位机（即工控机）上开发软件，以实现系统的各部分协调运行。

上位机控制操作平台主要实现期望路径设置与下发、与下位机之间的数据交互，以及能够在上位机中的控制操纵平台中实时显示并存储下位机传输的数据，以便对实验过程中出现的异常状态进行检测。

下位机控制操作软件平台主要结构如图15所示，该软件平台能够以多线程多任务的方式通过串口、CAN通信以及Wi－Fi通信的方式与定位导航定位模块，底层控制模块以及上位机之间进行通信。下位机平台通过惯导数据得到精确的车辆实时状态，并将结合车辆的实时状态与上位机获得的期望路径数据输出前轮转角、加速度等控制信号，之后通过CAN通信实现对转向推杆、刹车电机与驱动器电机的控制。

图15 软目标车下位机部分主要结构示意图

3.2 试验结果分析

考虑仿真环境与真实环境的差异性，进行实车试验来验证所设计的路径跟踪控制器在实际应用中的控制性能，实车试验分为直线路径跟踪试验与曲线路径跟踪试验，受场地限制且基于安全性考虑，直线试验车速为36、43.2、54 km／h，而曲线试验的最大车速则不超过30 km／h。

3.2.1 直线路径跟踪试验

期望路径为RT Range所建立的本地坐标系下从点（0，0）到点（150，0）的直线路径，图16（a）即为直线路径实车跟随结果，图16（b）为跟踪误差定量分析示意图。车速为36、43.2 km／h时的实际路径与期望轨迹一致，其中车速为36 km／h时的平均误差大约为0.024 m，跟踪误差极值为0.044 m；车速为43.2 km／h时的平均误差大约为0.029 m，跟踪误差极值为0.054 m。车速为54 km／h时，在开始阶段车辆和目标路径存在偏差，经过一段距离的行驶后误差逐渐减小，平均误差大约为0.041 m，跟踪误差极值为0.123 m，整体跟踪效果较差，但总体偏差仍在有效范围内。

图16 直线路径实车跟踪结果和误差分析

3.2.2 曲线路径跟踪试验

曲线路径实车跟踪结果如图17所示，选择的期望曲线路径有5次多项式生成的换道曲线与S型类sin曲线。由图17（a）换道曲线实车跟踪结果与图17（e）S型曲线实车跟踪结果可知，软目标车在进行曲线路径跟踪时，整体跟踪效果良好，但在曲率较大或曲率突变处跟踪误差较大。

图17 曲线路径实车跟踪结果和误差分析

如图17（b）换道曲线实车跟踪误差与图17（c）跟踪误差定量分析示意图所示，当车速为10、20、30 km／h时的换道曲线跟踪误差平均值分别为0.035、0.040、0.068 m，跟踪误差极值分别为0.07、0.10、0.21 m，极值点均出现在换道结束后的曲率突变处，但随后误差便逐渐消除；如图17（e）S型曲线实车跟踪误差与图17（f）跟踪误差定量分析示意图所示，当软目标车车速为10、20、30 km／h时的跟踪误差平均值分别为0.060、0.069、0.104 m，跟踪误差极值分别为0.083、0.110、0.233 m，极值点出现在曲率最大处。2种曲线路径跟踪误差极值均在路径跟踪的接受范围内，且随后便能迅速消除跟踪误差。

由以上试验结果可知，本文中所提的路径跟踪控制方法具有一定的可行性和有效性，能够满足软目标车场地测试需求。

4 结论

针对智能汽车封闭场地测试需求，开发了一种用于智能汽车场地测试的软目标车，采用横纵向综合控制策略解决软目标车的路径跟踪精度问题。通过粒子群优化纵向PID控制器调整速度误差，控制软目标车驱动和制动；在横向控制方面，提出一种改进的模糊Stanley路径跟踪控制算法，基于遗传算法得到Stanley控制最优参数及最优知识库，利用模糊控制算法实现Stanley控制参数自适应控制的效果。仿真结果与实车试验结果表明：所提出的方法有效地提高了软目标车按照预设的GPS轨迹行驶的跟踪精度，满足软目标车场地测试需求。由于场地原因及时间限制等因素，本研究仍存在一些需要改进的地方，如在构造增益参数的适应度函数时，仅考虑了跟踪误差与转向角，在未来的工作中可考虑构建多维度的适应度函数，同时考虑纵横向控制系统之间的耦合，对横纵向综合控制展开研究，并在条件允许的情况下进行更高车速的软目标车实车试验。