基于自适应预瞄路径的非连续曲率轨迹跟踪控制方法

李杭宇，刘冉冉，姜 宇，王文豪，李 峰，郭 威

（江苏理工学院汽车与交通工程学院，江苏 常州 213001）

0 引言

随着人工智能的不断发展，智能驾驶汽车成为现代科技的重要领域，其中智能驾驶的轨迹跟踪性能决定了车辆在智能驾驶过程中的行驶稳定与安全［1］，因此，轨迹跟踪控制层面逐渐成为当前的研究热点［2］，轨迹跟踪技术作为实现智能驾驶的基本要素之一，能够使车辆在无人操纵的情况下，实现车辆按照规划路径行驶，因此，尽可能地提高智能驾驶过程中的跟踪精度是该技术的重要目标［3］。

针对智能驾驶过程中存在的跟踪精度不足问题，国内外研究人员对此进行了大量的探索，在现阶段的研究中，大部分轨迹跟踪控制算法基于预瞄理论或模型拟合理论［4］，其中主要有PID控制法［5］、最优预瞄控制法、纯跟踪控制法［6］、模型预测控制法（MPC）［7－8］及最优控制法（LQR）［9－11］等。其中最优预瞄控制法因计算简单方便而广泛应用于智能驾驶领域。

Hu等［12］根据整车运动响应特性的横向偏差、道路边界和道路边界的优化函数来确定自适应预瞄时间，建立单点预瞄模型，实验表明该方法对速度和黏附系数具有良好的效果和适应性，聂枝根等［13］模仿人类的驾驶习惯，采取预瞄前馈与车辆状态反馈结合的方式设计控制器，实现以平稳且较小的控制量完成智能汽车动态轨迹的跟踪动作；谢辰阳等［14］提出了一种预瞄距离自适应的分层式路径跟踪控制系统，根据车辆自身实时状态进一步调整预瞄距离，提高了智能车辆的轨迹跟踪精度；黄海洋等［15］以预瞄跟踪误差为目标函数，并结合LQR最优控制方法，提出了一种新型的基于多点预瞄的最优轨迹跟踪控制策略，该方法进一步改进了道路模型的预瞄误差和跟踪算法的适应性及实时性；Liu等［16］根据迭代学习理论，设计了一种角度误差的补偿方法，对车辆前轮转向不足的情况进行补偿，提高了车辆侧倾稳定性；邱少林等［17］根据预瞄控制理论，提出了一种最优预瞄轨迹跟踪控制方法，得到了车辆最优转向盘转角，进一步提高了轨迹跟踪精度；杨浩等［18］提出了一套路程预瞄曲率阈值理论，该理论可判断目标路径相对于预瞄点位置关系，建立了一种可根据路程预瞄的有效信息和当前车速改变预瞄距离的自适应跟踪模型，研究结果显示，建立的模型具有较高的路径跟踪精度和合理的速度控制策略；王全等［19］以智能车辆的转向控制特点，设计了一种将模糊控制与预瞄控制相结合的轨迹跟踪控制方法，试验表明，该控制方法能够实时调节预瞄点的个数，从而提高车辆在行驶时的跟踪拟合性和鲁棒性。

上述研究主要通过速度、预瞄时间等因素调节预瞄距离或预瞄误差从而实现轨迹跟踪控制，但忽略了真实工况下道路曲率、转向稳定性对预瞄距离与行驶安全的影响，为此，根据最优预瞄控制，在考虑速度、曲率的综合影响情况下，提出一种基于自适应预瞄路径的非连续曲率轨迹跟踪控制方法，采取自适应预瞄前馈和状态反馈相结合的方式对方向盘转角进行控制；首先根据车体信息建立车辆2自由度动力学模型，依据坐标转换设计了一种改进的预瞄误差模型，并依据转向几何原理设计了一种驾驶员转向模型，同时引入横摆角速度、前轮转角约束，保证车辆运动过程中的稳定性与安全性；其次以预瞄-跟随理论建立了自适应预瞄控制器，通过自适应预瞄距离模块求解实时预瞄距离，并以五次多项式方程拟合方法得到预瞄路径；最后以Matlab／Simulink设计期望轨迹模块、轨迹跟踪控制模块，在Carsim中设定车辆2自由度动力学参数，并于Prescan中构建实时仿真工况、驾驶场景与传感器模型，验证控制器跟踪效果。

1 自适应预瞄跟踪控制系统

自适应预瞄轨迹跟踪控制系统由车辆模型、预瞄误差模型、自适应预瞄控制器、驾驶员转向模型组成，整车控制系统如图1所示，预设期望路径的参考坐标（xr，yr），把质心坐标作为车体坐标，以车辆实时位姿参数为输入，通过自适应预瞄距离模型计算预瞄距离L，并采用五次多项式拟合模块，得到车辆拟合后预瞄路径坐标（x，y）及预瞄点位置道路曲率kd，以（xr，yr）、（x，y）、kd、预瞄点航向角θd及预瞄距离L作为预瞄误差模型的输入变量，计算参考坐标与预瞄路径坐标的跟踪误差ey，同时以ey作为驾驶员转向模型的输入量，计算前轮转角δf，并输出给车辆模型，车辆通过转动方向盘调整转角从而实现路径的跟踪，最后，根据实时车辆位姿参数，反馈调节自适应预瞄控制器，再进一步作用于预瞄误差模型，改善车辆跟踪效果。

由于汽车运动过程中是一个复杂的非线性时变系统，涉及到车辆横纵向及横摆特性，在实现车联跟踪控制的安全与稳定性条件下，忽略以下因素：1）车辆载荷变化及悬架运动的影响；2）空气动力及车辆的垂直方向上运动的影响。因此采用简化的2自由度动力学模型，如图2所示。

图2 2自由度动力学模型

图2中：O点为车辆质心；v为质心速度；vx、vy分别为车辆纵向、侧向速度；δf为车辆前轮转角；lr、lf分别为质心到前轴、后轴的距离；αf、αr分别为车辆前、后轮侧偏角；β为质心侧偏角；vf、vr分别为车辆前后轮运动速度；Fyf、Fyr分别为车辆前、后轮胎侧偏力。假设被控车辆以前轮作为驱动轮，考虑轮胎侧偏力，则2自由度模型的动力学方程可以表示为：

式中：m为整车质量；ay为侧向加速度；φ为横摆角；I为绕轴的转动惯量；M为横摆力矩。故根据车辆轮胎特性［20］有：

式中：Cf、Cr分别为前、后轮侧偏刚度。考虑到模型中前轮转角较小，因此cosδf≈1，且根据刚体运动关系，αf与αr分别为：

式中：表示车辆横摆角速度。联立式（1）—式（6）可得：

式中，K为稳定性系数。

2 预瞄误差模型

为了进一步分析车辆动力学特性，设计了一种改进的预瞄误差模型，如图3所示，以XOY坐标系为基础，取道路中心红色虚线作为期望轨迹，黑色实线为车辆实际运动轨迹；根据车辆位置矢量关系及位姿状态设计预瞄误差方程；其次，基于运动几何原理构建驾驶员转向模型得到前轮转角，实现车辆跟踪控制。假设车辆此时运动位置于A点处时，根据轨迹投影规则，此时车辆投影点为B点；设预瞄距离为L，则车辆预瞄点为图3中C点，将点C投影至期望轨迹上得到点D。分别以点A、B和点D为原点，建立Frenet坐标系，记法向单位向量分别为Νa、Νb、Νd，记切向单位向量分别为Va、Vb、Vd；以Xa、Xb、Xd分别表示点A、B、D处向量。

图3 预瞄误差模型

图3中：θa表示车辆航向角；θb表示B点切线和XOY坐标系横轴夹角；ey表示点C与点D间的距离即横向误差；L表示点A与点C间的距离即预瞄距离；v表示车速；s（t）表示投影点B在期望轨迹上的位移。根据车速v与车体位置A点的位置关系有：

式（10）对时间t求导可得：

同时点A、B、C、D间的矢量关系有：

对式（12）两边同乘Νd，且有Νd·Νd＝1，可得：

进一步求导：

根据D点矢量关系可得：

将式（11）、式（15）代入式（14）可得：

由于Frenet坐标系下有：

式中，kd表示D点位置道路曲率。将式（13）、（17）代入式（16）同时可得：

由于VdΝd＝0则有：

由于θa＝β＋φ，可得：

式中，θa－θd表示向量Va与Vd的夹角，且当车辆在道路上平稳行驶时β较小，即有航向偏差eφ＝θa－θd。根据曲率与航向角的关系可知：

式中，ka表示A点位置道路曲率。当智能车辆在通常的结构化道路上行驶时，可知φ－θd≈eφ且数值较小，可近似取eφ＝sineφ、coseφ＝1，并将式（23）代入式（22），可得预瞄误差模型为：

式中，L为预瞄距离。为进一步分析ey与δf的关系，根据车辆几何运动原理建立了一种驾驶员转向模型，如图4所示。

图4 驾驶员转向模型

图4中：红色XGY为Frenet坐标系；θ为转弯圆心角；G点为车辆当前时刻的质心位置坐标点；Q点为车辆预瞄点；F点为Q点在期望轨迹上的投影点；E点为直线FG的中点。当车辆行驶时，假设被控的橫摆角速度在转弯瞬间保持不变，即此刻汽车运动方向与期望路径相切，经tp时间后，汽车将到达理想的质心位置即点F，即有：

由几何关系对式（25）进一步推导可得：

联立式（7）、（8）、（9）、（26）可得理想的前轮转角方程为：

为了保证车辆的侧向安全性，考虑约束的车辆横摆角速度定义为［21］：

3 自适应预瞄控制器设计

3.1 自适应预瞄控制原理图

自适应预瞄控制器的思想是以车辆实时位姿参数为输入，通过自适应预瞄距离模块实现预瞄距离L实时调整，并根据L进一步结合五次多项式路径拟合模块实现自适应预瞄路径，并控制车辆跟随预瞄路径完成跟踪过程。考虑到不同路面工况下车辆运动的安全性，基于路面附着及道路曲率等因素对车速进行了限制，防止车辆因转向导致车辆失稳。原理如图5所示。

图5 自适应预瞄控制原理

图5中：vc表示车辆安全速度；λ表示驾驶风格影响系数；μ表示路面附着系数；ka表示当前位置道路曲率；vc＝F（λ，μ）为直道安全速度限制函数；vc＝F（λ，μ，ka）表示弯道安全速度限制函数；L＝F（vc，ka）表示自适应预瞄距离函数。

3.2 五次多项式路径拟合

为模仿人类对运动路径的预见性，以五次多项式方程［22］对未来路径进行拟合。针对车辆路径拟合场景，五次多项式求解问题通常可以表示为经过多个点（xi，yi）（i＝1，2，3，…）且xi处五次多项式斜率为wi。待拟合的五次多项式可用式（30）表示，其在任意x处的斜率可表示为式（31）所示，式中at（t＝0，1，…5）为五次多项式方程待求解系数。

进一步：

式（32）为五次多项式方程需要满足的不等式约束，点（xz，yz）（z＝1，2，3，…）和（xj，yj）（j＝1，2，3，…）分别代表左右道路边界线上的点；按位置偏差与斜率偏差平方和最小的原则，求解五次多项式路径拟合问题，则五次多项式拟合问题转化为如式（33）的优化问题。

式（33）取得极小值时满足如下形式，即优化问题的导数为0，式（34）中（t＝0，1，…，5）为G对五次多项式系数的导数。

路径拟合计算步骤如下：①确定当前车辆位置坐标，该坐标即为车辆质心坐标即P0（0，0），当前车辆处切线斜率为w0＝0；②根据预瞄距离L（L＝P0Pd）及此时质心坐标确定期望路径上的预瞄点，即图6中点Pd（xd，yd）所示，进一步确定预瞄点Pd位置处的斜率wd；③直接对式（34）进行求解，将P0、Pd、w0和wd代入式（34）可直接求解得到拟合路径的五次多项式（30）的系数。经过上述步骤从而得到的拟合后的轨迹方程，确定预瞄路径如图6中红色实线所示。则拟合轨迹在车辆预瞄点Pd的曲率及航向角为：

图6 预瞄路径拟合原理

3.3 自适应预瞄距离模型

在智能车辆的预瞄控制中，预瞄距离直接影响着智能驾驶过程中车辆的跟踪精度及稳定性。通过对比不同预瞄距离对控制器的影响，发现预瞄距离过短会导致车辆转角频繁变化，导致车辆的行驶稳定性［23］变差，预瞄距离过长会使系统执行与现实情况下不匹配的控制指令，使控制器性能变差［24］，因此合理的预瞄距离是实现较高轨迹跟踪精度的关键，而现如今大多数研究中只考虑到车辆自身姿态对预瞄距离的影响，并未考虑安全速度范围及道路曲率因素，因此，引入安全车速与道路曲率相结合的方法实现自适应预瞄距离。当道路曲率较小时，预瞄距离受安全车速、道路曲率的影响，且随安全车速的增大而增大、道路曲率增大而减小；当道路曲率较大时，此时预瞄距离与安全制动距离相关。各阶段具体过程如式（36）所示。

式中：vc表示安全车速；ka表示当前道路曲率；ε表示经验系数；amax表示智能汽车在保证驾驶舒适性条件下的最大制动加速度；τ表示车辆系统反应时间，取0.1 s。当车辆在路面上行驶时，为了保证车辆运动的安全性，需根据不同的路面工况，设定安全车速限制，如式（37）、（42）所示。

3.3.1 直道安全车速限制

当被控车辆在直道上运动时即ka＝0，路面附着系数μ对车速的影响较大，因此，根据μ的高中低程度，并考虑不同驾驶风格对车速的影响，直道安全车速表达式为：

式中：vh为期望高速；vm为期望中速；vs为期望低速；λ为驾驶风格系数。

3.3.2 弯道安全车速限制

车辆于弯曲道路上运动时，即ka＞0，弯道内车速达到最大值则车辆离心力等于最大地面附着力。即有：

式中：g为重力常数，取10 N／kg，纵向力Fx与横向力Fy可分别表示为：

将式（38）—式（40）联立可得最高车速表达式为：

考虑不同的驾驶风格，并保证一定的安全系数，弯道安全车速表达式为：

式中：kmin表示最小道路曲率；kmax表示最大道路曲率；ksafe表示为安全系数。

4 仿真实验与结果分析

为了充分验证基于自适应预瞄路径的非连续曲率轨迹跟踪控制策略的有效性，选择小型轿车作为被控汽车，车辆模型参数见表1所示。

表1 车辆模型参数

其中Carsim用于实现整车动力学模型和虚拟场景设定，Prescan用于搭载传感器模型，Matlab／Simulink轨迹跟踪模块作为控制中枢；引入驾驶员风格系数λ对行车的影响，并分别选取双移线工况、非连续曲率工况、连续换道工况为期望轨迹，对所设计控制器即自适应-曲率-速度控制器进行仿真研究。在上述工况下进行自适应曲率-速度控制器下的3种速度对比实验及所设计控制器、自适应-时间控制器［25］、自适应-速度控制器［26］对比实验。观测4种控制器下的跟踪误差、前轮转角、横摆角速度变化情况。通过这3个工况，进一步验证汽车在复杂道路环境下的跟踪能力、稳定性及适应性。

4.1 双移线工况-多速度对比

为了验证自适应曲率-速度控制器在双移线轨迹下对不同行驶速度的适应性，取λ＝0.8即驾驶员在采取平稳型驾驶规则，同时以行驶速度25、35、45 km／h进行对比实验，3种速度下轨迹对比如图7（a）所示。

图7 双移线多速度仿真对比

由图7（b）可知，在25 km／h速度下，当t＝17 s时，跟踪误差最大值为0.183 m，在35 km／h速度下，当t＝14 s时，跟踪误差最大值为0.224 m，在45 km／h速度下，当t＝6 s时，跟踪误差最大值为0.288 m，由此可知，当车速越低时系统跟踪精度越高，由图7（c）与图7（d）可知，25 km／h时车辆横摆角速度与前轮转角最大值分别为13（°）／s、5.5°，35 km／h时车辆横摆角速度与前轮转角最大值为14（°）／s、4°，45 km／h时车辆横摆角速度与前轮转角最大值分别为16（°）／s、4°，均处于在合理范围内，验证了所设计控制器在不同车速条件下均有着良好的适应性。

4.2 双移线工况-多控制器对比

为了验证自适应-曲率-速度控制器下车辆在双移线工况的车辆跟踪能力，将所设计控制器与自适应-时间控制器、自适应-速度控制器进行对比实验，取车速为45 km／h、λ＝0.8。车辆在笛卡尔坐标系下的实际轨迹如图8（a）所示。

图8 双移线多控制器仿真对比

由图8（b）可知，当t＝12 s时，设计控制器达到最大跟踪误差0.288 m，可知该轨迹下所设计控制器跟踪性能明显优于其他控制器，而其他控制器最大跟踪误差达0.648 m，跟踪精度提升55.34%，由图8（c）、图8（d）可知，所设计控制器横摆角速度最大值为16（°）／s，前轮转角最大值为4°，均处于合理范围内，保证了被控车辆的行驶稳定性与安全性，进一步验证了控制器的优越性。

4.3 非连续曲率工况-多速度对比

为验证所设计的控制器在非连续曲率道路工况下对不同行驶速度的适应性，该工况下选取激进型驾驶规则即取驾驶风格影响系数λ＝1.25，分别以行驶速度25、35、45 km／h进行对比实验，3种速度轨迹如图9（a）所示，由图9（b）可知，25 km／h条件下车辆最大跟踪误差为0.272 m，35 km／h条件下车辆最大跟踪误差为0.242 m，45 km／h条件下车辆最大跟踪误差为0.325 m，由图9（c）可知，该工况下道路曲率呈阶跃性变化，由图9（d）、图9（e）可知，所设计控制器3种速度下的最大横摆角速度为38（°）／s，前轮转角最大值为13°。由上述分析知，不同速度条件下，智能汽车依然保持着良好的跟踪精度，且被控车辆横摆角速度与前轮转角均保持安全范围内，进一步验证了曲率-速度控制器在非连续曲率道路工况下对不同车速下的适应性。

图9 非连续曲率多速度仿真对比

4.4 非连续曲率工况-多控制器对比

为了进一步研究控制策略应对道路曲率不连续恶劣路况的能力，设计如图10（a）轨迹工况，此时，车速为45 km／h，且取驾驶风格影响系数λ＝1.25。由图10（b）可知，当t＝17 s时，自适应-曲率-速度控制器最大跟踪误差为0.325 m，其他控制器最大误差达到0.952 m，跟踪精度提升65.86%。

图10 非连续曲率多控制器仿真对比

由图10（c）、图10（d）可知，整个轨迹跟踪过程中，自适应-曲率-速度控制器横摆角速度最大值为38（°）／s，前转角最大值为12°，均处于合理范围内，保证了车辆行驶过程中较高的跟踪精度与稳定性、安全性。

4.5 连续换道工况对比

为充分探究控制器在真实道路环境下的跟踪能力，取驾驶风格影响系数λ＝1.25，在连续换道工况下，将3个控制器跟踪效果进行对比，车辆运动轨迹如图11（a）所示，可知自适应-曲率-速度控制器跟踪轨迹最贴近期望轨迹，该工况下道路曲率大小在0～0.1剧烈变化如图11（b）所示。由图11（c）可知当t＝9 s时所设计控制器的跟踪误差达到最大值0.15 m，其他控制器最大跟踪误差达0.28 m，跟踪精度提升46.00%。由图11（d）、（e）可知，车辆运动过程中，横摆角速度及前轮转角最大值分别为5（°）／s、8°，均为较小量且在安全范围内。

图11 连续换道对比

以上3个工况的试验结果表明，所设计的控制策略能根据外界道路信息迅速调整控制量，应对外界工况激烈变化，同时以较高精度跟踪期望轨迹，且保证了跟踪过程中汽车的行驶稳定性、安全性。

5 结论

1）基于自适应预瞄路径的控制方法考虑了安全速度约束的条件下道路曲率与速度的综合影响，提高了车辆跟踪过程中的跟踪精度；同时引入了真实条件下的车辆横摆角速度、前轮转角约束，进一步保证了运动稳定性与安全性。

2）以双移线、非连续曲率、连续换道轨迹为实验工况，Carsim／Simulink／Prescan联合仿真平台研究结果表明，自适应-曲率-速度控制器对跟踪精度分别提升55.34%、65.86%、46.00%；并在不同的行驶速度下验证了所设计的控制器的适应性。

3）自适应预瞄控制器在设计时未考虑车辆的机械转向系统的转向不足特性及系统稳态误差对系统的影响，下一步将建立相应的反馈模型法，以提高控制系统的鲁棒性。