基于时滞补偿的纯追踪控制预瞄距离优化方法

汤新华，成宇庆，潘树国，鲍亚川，黄 璐，蔚保国

（1.东南大学 仪器科学与工程学院，南京 210096：2.微惯性仪表与先进导航技术教育部重点实验室，南京 210096：3.卫星导航系统与装备技术国家重点实验室，石家庄 050081）

近年来，随着交通迅速发展以及汽车数量的持续上升，道路拥堵以及疲劳驾驶等人为因素造成的交通事故日益增加，作为未来改善道路交通与驾驶安全最具潜力的技术，自动驾驶技术的研发备受瞩目。自动驾驶技术主要包括导航定位、环境感知、决策规划和控制执行四大技术模块[1]，如图1 所示。车辆通过全球卫星导航系统（Global Navigation Satellite System,GNSS）/惯性导航系统（Inertial Navigation System,INS）等组合导航系统获取自身车辆位姿信息[2]，并利用视觉、激光雷达等相关传感器对环境信息进行采集与处理。综合位置信息和环境信息，车辆根据目标任务完成规划、自主避障等决策，并下发至车辆执行机构[3]，其中规划根据是否包含时间信息分为路径规划和轨迹规划。控制执行是在导航定位、环境感知、决策规划的基础之上，根据规划决策的目标轨迹，对车辆进行纵向控制和横向控制，使车辆沿着规划的目标轨迹行驶，保证车辆行驶的安全性与操纵稳定性[4]。

图1 自动驾驶系统结构图Fig.1 Structure diagram of autopilot system

目前主流的控制算法包括纯追踪算法（Pure Pursuit,PP）、Stanley 算法、比例积分微分（Proportion Integration Differentiation,PID）算法、线性二次型调节器（Linear Quadratic Regulator,LQR）算法和模型预测控制（Model Predictive Control,MPC）算法等。纯追踪算法由于具有几何模型简单、鲁棒性高且算法实现复杂度低的特点，被广泛用于解决无人驾驶领域中路径跟踪问题，但在实际应用中，纯追踪算法需要结合具体应用场景进行进一步优化以保证控制精度和可靠性。其中，预瞄距离的选择会在很大程度上影响纯追踪算法的性能。预瞄距离越小，对目标路径的收敛速度就越快，从而能够更快地到达目标点，但容易引起振荡现象，导致跟踪精度低、车辆舒适度差。预瞄距离较大可以避免明显的振荡现象，但在初始跟踪阶段或转弯阶段存在较大的跟踪误差。因此，优化预瞄距离是保证纯追踪算法在实际应用中可靠运行的重要因素。目前已有大量文献针对预瞄距离优化做了相关研究。其中，Horváth E 等人提出了一种多目标优化纯追踪，利用车辆运动学约束计算离散曲率，从而选择最佳转向角[5]。Chen Y 等人提出了一种利用PID 控制器和低通滤波器对纯追踪控制器的转向角进行拟合补偿，弱化预瞄距离对跟踪精度的影响[6]。Ibrahim A 等人将路径曲率分为直线和圆弧部分，提取曲线信息自适应修正预瞄距离[7]。唐小涛等人提出了模糊自适应纯追踪，通过先验路径特性、速度和跟踪误差自适应地调整预瞄距离[8]。另一方面，也有部分学者针对车辆模型时延优化做出了相关研究，Tao H 等人通过更新卡尔曼滤波器改变GPS 接收信号，解决了延时反馈和慢反馈的问题[9]。赵建辉等人通过预测车辆延迟后的位置和方向，减小延迟对路径跟踪控制精度的影响[10]。Pornthiwa C 等人通过改变MPC 内部结构，输出新的转向角输出[11]。Xu S 等人提出了一种考虑前向状态预测器和延迟增强的优化算法，利用预瞄控制理论解决时滞问题[12]。

综上所述，针对预瞄距离优化的研究缺少对实际车辆模型时延的考虑，本文结合实际车辆分析控制回路时延和转向系统迟滞对车辆模型及其稳定性的影响，提出了一种基于时滞补偿的纯追踪预瞄距离优化方法，图2 为控制算法的总流程图。通过实际的车辆实验，验证了基于时滞补偿的纯追踪预瞄距离优化算法与实际路径跟踪效果的一致性和可靠性。

图2 控制总流程图Fig.2 Flow chart of the autonomous control process

1 纯追踪及ABMSSA 算法

1.1 纯追踪算法

纯追踪算法及其变式是无人驾驶领域中路径跟踪问题最常用的方法，该方法在几何层面上计算行驶路径上车辆后轴与前方目标点的圆弧曲率，根据后轴位置到所需路径的预瞄距离dL确定目标点，通过目标点位置以及车辆航向矢量和超前矢量之间的角度α确定车辆的转向角δ，达到路径追踪的目的。纯追踪的示意图如图3 所示。

图3 纯追踪示意图Fig.3 Diagram of the pure pursuit algorithm

图3 中，R是车辆转弯半径，d是横向误差，(gx,gy)是目标点。

应用正弦定理可以得到α与δ的关系：

以γ表示圆弧的曲率，得到曲率表达式：

根据车辆几何模型，转向角可写为：

其中，L为车辆轴距。

利用式(2)(3)，纯追踪的控制律可以写作：

其中，δu为纯追踪期望转向角。从式(4)可以看出，预瞄距离Ld影响纯追踪的控制精度。若预瞄距离过大，则车辆从某一偏离车道位置到达目标点的响应时间变长，路径跟踪偏差变大；若预瞄距离过小，会使车辆驾驶时发生振荡，甚至会使车辆失控。因此，在利用纯追踪进行路径跟踪时，预瞄距离决定其跟踪精度和跟踪效果，选取合适的预瞄距离尤为重要。

在一般工况下，使用纯追踪预瞄距离与速度一次线性关系的经验公式[5]：

其中，k为比例系数；v为车辆速度；Lf为初始预瞄距离或预瞄距离下限。

1.2 ABMSSA 算法

ABMSSA（Adaptive Brownian Motion Salp Swarm Algorithm）是一种改进的樽海鞘智能优化算法，通过在SSA 的领导者位置更新公式中引入布朗运动，在SSA 跟随者位置更新公式中增加两个自适应权重，使SSA 的全局优化能力和局部优化能力得到联合调整，加快了ABMSSA 算法的收敛速度，同时满足了路径跟踪精度和收敛速度的要求[13]。

布朗运动N(0,h) 是粒子的随机运动机制，粒子运动的步长h是正态分布的。对于一维的布朗运动，假设Wt是概率空间上布朗运动实值过程，ΔWt是增量，t为当前时刻。布朗运动实值更新公式为：

领导者的更新公式为：

图4 c1 变化曲线图Fig.4 Plot ofthevariation of parameter c1

图5 直线路径时滞系统稳定阈值Fig.5 Stability threshold of straight path time-delay systems

自适应权重的更新公式为：

跟随者的位置更新公式为：

为使预瞄距离最优化，实现不同车速下预瞄距离自适应地寻优，将预瞄距离设置为樽海鞘种群。综合考虑车辆的跟踪精度和车辆舒适性，设计目标函数为：

其中，ai(i=1,2)为权重系数；J1为车辆跟踪误差指标；J2为车辆航向误差指标。J1、J2的计算公式为：

其中，xi和yi分别为车辆在第i时刻的横向位置和纵向位置；和分别车辆在第i时刻的参考横向位置和参考纵向位置；θi和θi-1分别车辆在第i时刻和第i-1时刻的车辆航向角。在本文中，a1和a2分别设置为0.7 和0.3。

通过ABMSSA 算法寻找使目标函数JABMSSA达到最小的预瞄距离，即为该速度该工况下的最优预瞄距离。

2 基于时滞补偿的预瞄距离优化

2.1 时滞车辆模型优化

在实际车辆平台运行过程中，一方面由于离散控制系统状态观测与闭环控制存在周期时延，导致车辆执行机构存在一定的动作滞后，即指令由决策层到执行层不会被瞬间执行，而是存在一定的控制时延；另一方面由于机械间隙、轮胎侧向力等因素的存在，车辆转向机构系统接收到指令后，在采样时间内无法准确输出指令转向角，具有一定的惯性迟滞。

本文引入控制回路时延和转向系统迟滞，对车辆模型进行更新。车辆方向盘由步进电机驱动，整个转向系统可以看作为一阶系统[14]：

其中，T为转向系统时间常数。

引入控制回路时延，将角度转动视为微小转动，则车辆模型更新为：

2.2 时滞补偿模型的稳定阈值选择

在时滞补偿模型下，选择合适的ABMSSA 寻优下界阈值lb，对模型的稳定性尤为重要。本文对时滞补偿模型直线路径稳定性（γ=0 时）进行了分析与选择[14]。时滞补偿模型在平衡位置(x=0，φ=)进行线性化，可以从线性系统的稳定性中确定非线性系统在平衡时的局部稳定性：

设G(s) 为时滞补偿模型系统特征多项式[14]，则：

令sj=ω，整理非线性方程组得到：

3 实验验证

为了验证基于时滞补偿的纯追踪控制预瞄距离优化的有效性，本节分为两部分：1）不考虑时滞补偿，使用传统车辆模型对比分析ABMSSA 算法，得到预瞄距离的最优性，并通过拟合得到传统车辆模型预瞄距离与速度的线性关系；2）使用时滞补偿模型，通过拟合得到优化后的车辆模型预瞄距离与速度的线性关系，并进行实车验证。设定车速为1 m/s，车辆参考路径共计 200 个参考轨迹点，车辆初始位置为(0 m,0.15 m)，航向角为北偏东5°。仿真参考路径设置为直线路径，如图6 所示。

图6 仿真实验参考路径Fig.6 Reference path in the simulation experiment

3.1 不考虑时滞补偿

按照式(7)和式(9)对樽海鞘智能优化算法进行更新，比较适应度值。利用ABMSSA 算法优化得到最优预瞄距离为Ld=0.3695 m，为验证Ld为最优预瞄距离，分别取Ld=0.3695 m、0.15 m、1 m 得到不同预瞄距离下路径跟踪和横向误差对比，如图7 所示。

图7 无时滞补偿预瞄距离下路径跟踪和横向误差对比Fig.7 Comparison of path tracking and lateral errors using different look-ahead distance with no time delay compensation

1）图7(a)中，当所选预瞄距离在Ld=0.15m 时，瞬时收敛速度快，但整体路径跟踪控制表现出不稳定的震荡现象。

2）图7(b)中，预瞄距离Ld=0.3695 m 与Ld=1 m相比，收敛速度更快，总体路径误差最小。

采用ABMSSA 算法对不同速度、不同路况下的预瞄距离进行最小二乘拟合，得到预瞄距离和速度的一次线性关系。为了得到合理的最优预瞄曲线，本节速度设置为定速，即初始速度和目标速度一致，不设置加速度变量。速度作为自变量分别取0.6 m/s、0.8 m/s、1.0 m/s、1.2 m/s、1.4 m/s，得到无时滞补偿的预瞄距离拟合曲线如图8 所示。

图8 无时滞补偿的预瞄距离拟合曲线Fig.8 Fitting curve of the relationship between look-ahead distance and velocity with no delay compensation

采用最小二乘法直线拟合得到无时滞补偿的预瞄距离Ld与速度v的线性关系为：

3.2 考虑时滞补偿

3.2.1 仿真实验

在实验车辆平台中，转向系统迟滞T约为0.25 s，控制回路延时τ约为0.4 s，在该系统下选取稳定阈值预瞄下限lb=1.1511v，利用ABMSSA 算法优化得到最优预瞄距离为Ld=2.7176 m。为验证Ld为最优预瞄距离，分别取Ld=2.7176 m、2m、3 m 得到不同预瞄距离下的路径跟踪和横向误差对比，如图9 所示。

图9 时滞补偿预瞄距离下路径跟踪和横向误差对比Fig.9 Comparison of path tracking and lateral errors using different look-ahead distance with time delay compensation

1）图9(a)中，当预瞄距离为Ld=2 m时，具有较快的上升速度，但收敛速度慢，稳定性差。

2）图 9(b)中，本文选择的优化预瞄距离Ld=2.7176 m在跟踪误差和稳定性方面优于Ld=2 m 和Ld=3 m 。

考虑时滞补偿后得到各速度对应的最优预瞄距离，如表1 所示。

表1 考虑时滞补偿的各速度下最优预瞄距离Tab.1 Optimal look-ahead distance under different velocity with time delay compensation

考虑时滞补偿的预瞄距离拟合曲线如图10 所示。

图10 考虑时滞补偿的预瞄距离拟合曲线Fig.10 Fitting curve of the relationship between look-ahead distance and velocity with time delay compensation

采用最小二乘法直线拟合得到时滞补偿下预瞄距离Ld与速度v的线性关系为：

3.2.2 实车实验

为了进一步验证基于时滞补偿所得最优预瞄结果的合理性，在实验小车平台上进行实验。实车路径跟踪具体框架分为感知层、决策层和执行层。感知层通过实时动态定位技术（Real Time Kinematic,RTK）和惯性测量单元（Inertial Measurement Unit,IMU）进行组合导航接受定位信息；与机器人操作系统（Robot Operating System,ROS）以10 HZ 的频率通信，传输定位数据到决策层。决策层通过CAN 总线输出控制命令，对执行层进行实时控制。执行层由自动转向机构、制动和加速模块构成。为了减少潜在的耦合误差，本文只涉及转向机构的横向控制，不对车辆进行纵向速度控制。实验小车测试平台的基本参数如表2 所示。

表2 车辆参数Tab.2 Vehicle parameters

图11 为实际测试场地环境，本次测试主要进行了直线跟踪测试(γ=0)，利用导航系统选取起点与终点，根据起点和终点作一条直线，将其设置为参考轨迹。测试距离设置为40 m 左右，验证直线路况下纯追踪ABMSSA 离线模型预瞄距离寻优算法得到最优预瞄距离的合理性和准确性。

在直线路径(γ=0) 下，通过计算实时跟踪路径与参考路径的误差，选取最小的跟踪误差作为该速度对应的最优预瞄距离，实验结果如表3 所示。

表3 实车各速度下最优预瞄距离Tab.3 Optimal look-ahead distances under different velocity in the vehicle

表4 给出了不同模型各速度对应的最优预瞄距离。从表4 可以看出，在相同速度下，加入转向系统迟滞和控制回路时延的车辆模型对车辆预瞄距离寻优结果更贴合实际情况，不考虑时滞模型的预瞄距离平均误差为83.49%，考虑时滞模型的预瞄距离平均误差为15.94%。上述实验结果表明，相比于传统模型，加入时滞补偿所得最优预瞄准确率提高67.55%，验证了时滞车辆模型的合理性。

表4 不同模型各速度下的最优预瞄距离对比Tab.4 Comparison of optimal look-ahead distances for the two different system models at different velocities

4 结论

本文对基于时滞补偿的预瞄距离优化进行了深入分析。首先，针对实际车辆转向机构迟滞和控制回路时延带来的路径跟踪控制精度降低问题，引入了时滞补偿。其次，从理论上分析时滞模型的稳定性，对ABMSSA 算法的阈值下限进行优化，并分别在无时滞和有时滞补偿情况下，计算得到不同的最优预瞄距离。最后，在实际车辆平台上，对所提出的基于时滞补偿模型的纯追踪预瞄距离进行实验验证。实验结果表明，相比于传统模型，加入时滞补偿所得最优预瞄与实验测试所得最优预瞄准确率提高了67.55%。

本文对直线路径工况下的时滞补偿模型进行了讨论，并证明时滞补偿模型在纯追踪预瞄距离优化的合理性和有效性。针对更复杂的实际路况，后续将进一步研究对常规定曲率和变曲率工况下的无人驾驶路径跟踪控制效果，提高模型优化的泛化能力。