基于预瞄的智能车位姿补偿轨迹跟踪算法

王军雷 李健明 吴学易

摘 要：针对单点预瞄、模糊控制等轨迹跟踪算法鲁棒性低以及模型预测控制算法计算量大、参数不易确定的问题，提出基于预瞄的智能车位姿补偿轨迹跟踪算法。通过建立目标车辆的运动学模型，在轨迹跟踪建模时进行位姿补偿，降低执行机构延时;使用预瞄算法，构建智能车转向模型，求解最优方向盘转角。实车测试结果表明，所提轨迹跟踪算法通过对位姿进行实时补偿，可以进一步减小横向误差，具有实时性高，鲁棒性好的特点，能够满足智能网联汽车实际运行需要。

关键词：轨迹跟踪;两点预瞄;位姿预测;位姿补偿;最优转向角;智能网联汽车

中图分类号：U462  文献标识码：A  文章编号：1671-7988（2020）21-40-06

Abstract： Aiming at the problems of low robustness of trajectory tracking algorithms such as single-point preview and fuzzy control， large amount of calculation and difficult determination of parameters of model predictive control algorithm， an intelligent vehicle position and attitude compensation trajectory tracking algorithm based on preview is proposed. The kinematic model of the target vehicle is established， and the position and attitude compensation is carried out during the track tracking modeling to reduce the actuator delay. Using preview algorithm， the steering model of intelligent vehicle is built to solve the optimal steering wheel angle. The experimental results show that the proposed trajectory tracking algorithm can further reduce the lateral error by real-time compensation of the pose. It has the characteristics of high real-time performance and good robustness， and can meet the actual operation needs of the intelligent connected vehicle.

Keywords： Trajectory tracking; Two-point preview; Pose prediction; Pose compensation; Optimum steering angle; Intelligent connected vehicle

CLC NO.： U462  Document Code： A  Article ID： 1671-7988（2020）21-40-06

引言

轨迹跟踪作为智能网联汽车软件架构中重要组成部分，直接关系到智能网联汽车是否按照轨迹规划层规划出的轨迹进行行驶，以及是否会与其它道路交通参与者发生碰撞。轨迹跟踪效果的好坏不仅体现在实际轨迹与期望轨迹之间的误差，而且体现在智能网联汽车的安全性、平顺性以及舒适性。常用轨迹跟踪有纯轨迹跟踪、单点预瞄、双点预瞄、模型预测控制、模糊控制、深度学习等方法。秦万军等人基于无人车轮胎受力情况建立二自由度模型，从而进行轨迹跟踪。刁勤晴等人提出一种基于双预瞄点的智能车大曲率路径横向模糊控制方法。梁政涛等人基于车辆运动学模型，采用模型预测控制方法进行车辆横向控制。伏雨旋等人使用时变参数的人工势场与PID协调控制的轨迹跟踪方法，系统使用横向误差和航向误差的时变参数人工势场力对前轮转角进行控制。Chen等人使用端到端的深度学习方式进行智能车控制，系统直接输入图片，输出结果直接为汽车横向控制参数。Norouzi Armin等人提出了一种基于元启发式优化算法的新型李雅普诺夫鲁棒控制器。上述方法虽取得一定研究成果，但在轨迹跟踪效果方面还存在一定的提升空间。

为更好的智能网联汽车轨迹跟踪问题，本文提出基于滚动优化的智能车轨迹跟踪算法。算法分为三步骤，首先，通过建立汽车运动学模型，对执行机构延时产生的位姿变化量进行补偿;其次，使用改进的预瞄算法，构建智能车转向模型并求解最优方向盘转角;最后，使用PID控制进行误差调节，使得实际方向盘转角快速稳定的跟踪上期望方向盘转角。实车实验结果表明，本文使用算法实时性高，鲁棒性好，满足实际测试需要。

1 汽车位姿补偿

无人驾驶汽车在轨迹跟踪过程中，由于执行机构延时问题，控制程序发出执行命令到执行机构开始响应相关控制命令时，无人驾驶汽车已经移动一段距离，当无人驾驶汽车高速行驶时，其延时问题对汽车平顺性和控制精度的影响尤为明显。为解决执行机构延时问题，本文建立汽车运动学模型，研究在延时时间段无人驾驶汽车位姿的改变量，使用当前位置加上位姿改变量量得到一个虚拟位姿，使用虚拟位姿计算当前需要跟踪参考轨迹的控制量，示意图如图1所示。S为无人驾驶汽车参考轨迹，Pt为无人驾驶汽车t时刻在参考轨迹上选择的预瞄目标点，Xt（xt， yt， ψt）为t时刻无人驾驶汽车的姿态信息，Xt'（xt'， yt'， ψt'）执行机构开始执行t时刻控制指令时无人驾驶汽车的姿态信息。无人驾驶汽车执行机构执行延时时间假设为t0，在t0时间段无人驾驶汽车前进的距离△dt'為：

为得到无人驾驶汽车在t0时间段其位姿改变量，建立如图2所示的智能车运动学简化模型，简化模型假设无人驾驶汽车在运动过程中前后轴左右轮侧偏刚度保持一致，且在短时间内无人驾驶汽车可以看作一恒定加速度在运动。因此，将无人驾驶汽车四轮简化成如图所示的两轮自行车模型。δf、δr分别为前后轮转角，lf、lr分别为汽车重心到前后轴之间的距离，υt为智能车t时刻速度，ψt为智能车t时刻航向角，β为智能车速度方向与智能车纵轴方向間的夹角，at为智能车t时刻加速度，dt为时间改变量。

2 轨迹跟踪

在轨迹跟踪控制过程中，最优的控制目标是无人驾驶汽车当前位置与参考轨迹之间无横向误差，如图3所示，Pt为参考轨迹S到无人驾驶汽车当前位置之间横向距离最小参考点，最小距离用△dt表示，且参考轨迹在参考点Pt点的方向与无人驾驶汽车当前航向角ψt之间的误差尽可能小。然而由于执行机构延时问题的存在，因此，当前t时刻计算控制量应计算如图4所示的无人驾驶汽车位姿Xt'所对应的控制量，Xt到Xt'的位姿变化量由式2-式6给出。许多学者对轨迹跟踪多有研究，文献[6]为最为经典的单点预瞄控制模型，但单点预瞄没有考虑汽车运动学模型对控制的影响，且没有预见性，一般适用于车速较低的场景。模型预测控制基于汽车运动学模型，求解汽车未来一段时间内的姿态，根据姿态量和参考轨迹构建优化函数，模型预测都遵循预测模型、滚动优化和反馈控制，从而得到最优的控制量，但模型预测控制计算量大，对计算资源要求较高。本文提出基于两点预瞄的控制模型，如图4所示，基于位姿补偿Xt'，给出两个预瞄点Q1、Q2，预瞄点Q2到Xt'的直线距离dt1为：

其中T0为预瞄时间，υt'为t时刻位姿补偿后的智能车速度。

预瞄点Q2为参考轨迹最大曲率控制点，如图5所示，Pt'为Xt'到参考轨迹距离最小的点，Pt''为实时参考轨迹约束点，将Pt'点至Pt''点间参考轨迹作为t时刻研究对象，Pt'点至Pt''点间距离为：

其中T1为参考轨迹选择时间长度。

将Pt'点至Pt''点连接成直线，构成直线方程，得到Pt'点至Pt''点间参考轨迹偏离直线方程最大的轨迹点Q2。如图5所示，在参考轨迹Q2附近截取一段距离，构成点集，使用最小二乘法拟合成圆，得到圆心Ot（x，y）。

公式9至公式17中，xi、yi为点集中点的横纵坐标，xo、yoi、R分别为拟合圆的圆心坐标和半径。根据式9-式16可得到拟合圆的中心的半径。M11、M12、M21、M22、H1、H2、A、B、C为中间变量，无实际含义。

υQ2为根据Q2点处根据拟合圆半径R得到的最大车速， kQ2为半径调节系数，dQ2为智能车位姿补偿后点Xt'到Q2点之间的距离。当t时刻时刻实际车速大于安全车速时，系统应向驾驶员提供预警;实际车速小于安全车速时，无人驾驶汽车执行速度规划层规划的速度。

根据“Ackermann”几何关系。汽车轨迹曲率与方向盘转角成正比：

式中，θ为方向盘转角，i为方向盘传动比，L为智能车轴距，R't为t时刻位姿补偿后智能车转弯半径，为智能车侧向加速度。

根据图6，经过时间T0之后，则无人驾驶汽车的横向位移关系式为：

式中为智能车侧向速度。

由式21-式23可知：

“Aeklman”几何关系的最优方向盘转角为：

在单点预瞄运动分析模型中，将无人驾驶汽车简化为一个点，无人驾驶汽车轨迹跟踪的优劣由到跟踪轨迹的最小横向误差决定，然而由于无人驾驶汽车与跟踪轨迹之间存在航向误差，即使无横向误差，航向误差的存在将时汽车在下一时刻偏离跟踪轨迹，因此，最优方向盘转角应考虑航向误差的影响，本文建立如式25所示的最优方向盘模型：

k2为“Aeklman”几何关系的最优方向盘转角权重，k3为航向误差权重。

然而由于执行机构执行精度的问题，最优方向盘转角与实际方向盘转角之间存在误差，因此，通过PID控制进行误差补偿。

式中：kp、ki、kd分别为比例、积分、微分环节的系数， e（t）为t时刻最优方向盘转角与实际方向盘转角之间的误差，μ（t）为t时刻发送给方向盘的实际控制量，T为考虑的历史数据时长。

根据式25-式27，可求得每一时刻的请求方向盘转角。

3 仿真实验

本文为验证基于预瞄的智能车位姿补偿轨迹跟踪算法，使用MATALB和Carsim进行联合仿真，并对比纯跟踪算法、本文所提算法、模型预测控制算法（MFC）在跟踪效果和实时性等方面的因素。如图7、8、9所示分别为纯跟踪算法、本文所提算法、MPC算法仿真效果，图中黑色点划线为参考轨迹，红色实线为跟踪参考轨迹时的跟踪效果，蓝色实线为横向误差，从图7-图9中可以看出，本文所提算法横向误差控制在0.7m范围内，满足智能车对轨迹跟踪横向误差精度要求。表1为三种控制方法的仿真时长，设置车速为30km/h，仿真参数一致，从图和表中可知，本文所提算法实时性较高，控制精度好，MPC虽然横向误差更小，但是实时性较差。

为验证本文所提局部路径规划算法，使用东风柳汽S50EV电动车作为实车实验平台，实验车搭载一套GNSS定位系统根，一个64线激光雷达，两个16线激光雷达，一个4线激光雷达一个毫米波雷达和2个不同焦距的摄像头，实车图像如图10所示。

本文实车实验时设定巡航速度为30km/h，实车测试结果如图11所示，图中黑色实线为参考轨迹，参考轨迹为事先采集的GPS数据，黑色虚线为没有进行位姿补偿时轨迹跟踪效果，黑色点划线为进行位姿补偿后的轨迹跟踪效果，图12中点划线为没有进行位姿补偿时轨迹跟踪横向误差，实线为进行位姿补偿后轨迹跟踪横向误差，图13为每个GPS点对应的速度规划结果。

从图11-图13可以看出，本文提出基于预瞄的智能车位姿补偿轨迹跟踪算法，在实时对位姿进行补偿后，横向误差进一步缩小，满足轨迹跟踪要求，且满足汽车实时性要求，并且在进入弯道前提前减速，驶出弯道都能够立即加速，速度响应快，控制精度高。

4 结语

本文针对智能网联汽车的轨迹跟踪问题，提出了一种基于预瞄的智能车位姿补偿轨跟踪算法。算法首先利用车辆动力学模型预测车辆在执行机构延时时间内的位姿变化量，在轨迹建模时对车辆进行位姿补偿以降低执行机构延时问题;其次，本文利用预瞄算法，在车辆轨迹上搜索转弯半径最小的参考点，在其附近截取一段参考点进行圆拟合，得到转弯半径，使用转弯半径和到其点的距离求解当前时刻期望速度。根据当前车速选择轨迹上一个预瞄点，构建智能车转向模型，根据转向模型求解最优方向盘转角;最后利用PID控制对车辆实际方向盘转角进行误差调节，使其能高效的跟踪期望方向盘转角。实验结果表明，本文所提算法既能够满足轨迹跟踪的实时性与准确性要求，又具有控制精度高，响应速度快的特点。

参考文献

[1] 秦万军，徐友春，李明喜，等.基于二自由度模型的无人驾驶车辆轨迹跟踪控制研究[J].军事交通学院学报，2014，16（11）：31-35.

[2] 刁勤晴，张雅妮，朱凌云.双点预瞄式智能车大曲率路径的横向控制[J].重庆理工大学学报（自然科学），2019，33（04）：32-40.

[3] 梁政焘，赵克刚，裴锋，等.基于MPC的智能车轨迹跟踪算法[J].机械与电子，2019，37（01）：66-70.

[4] 伏雨旋，李孝银，吴鹏.基于时变人工势场PID的智能汽车轨迹跟踪控制[J].时代汽车，2019（07）：17-18.

[5] Norouzi Armin， Masoumi Milad， Barari Ali， et al. Lateral control of an autonomous vehicle using integrated backstepping and sliding mode controller[J]. Proceedings of the Institution of Mechanical Engineers，2019，233（1）：141-151.

[6] 郭孔辉.驾驶员—汽车闭环系统操纵运动的预瞄最优曲率模型[J].汽车工程，1984（03）：1-16.

[7] Hongde Qin，Chengpeng Li， Yanchao Sun， et al. Finite-time trajectory tracking control of unmanned surface vessel with error constraints and input saturations[J]. Journal of the Franklin Institute，2019.

[8] Guo Zhao，Sun Jiantao，Ling Jie，et al.Robust Trajectory Tracking Control for Variable Stiffness Actuators With Model Perturbations [J].Frontiers in Neurorobotics， 2019， 13：35.

[9] Qun Cao， Zhongqi Sun， Yuanqing Xia， et al. Self-triggered MPC for trajectory tracking of unicycle-type robots with external disturbance [J]. Journal of the Franklin Institute， 2019， 356（11）： 5593-5610.

[10] Aleksandr Andreev， Olga Peregudova. Trajectory tracking control for robot manipulators using only position measurements[J]. Inter -national Journal of Control，2019，92（7）：1490-1496.

[11] Ma Tianyu，Song Zhibin， Xiang Zhongxia， et al. Trajectory Tracking Control for Flexible-Joint Robot Based on Extended Kalman Filter and PD Control[J]. Frontiers in Neurorobotics， 2019， 13：25.

[12] Hang Peng， Chen Xinbo. Integrated chassis control algorithm design for path tracking based on four-wheel steering and direct yaw-moment control[J].Proceedings of the Institution of Mechani -cal Engineers， Part I： Journal of Systems and Control Engineering， 2019， 233（6）： 625-641.

[13] Ning Wang，Xinxiang Pan，Shun-Feng Su.Finite-Time Fault-Tolerant Trajectory Tracking Control of an Autonomous Surface Vehicle[J]. Journal of the Franklin Institute，2019.

[14] Souad Bezzaoucha Reba?，Holger Voos，Mohamed Darouach. Attack- tolerant control and observer-based trajectory tracking for Cyber- Physical Systems[J]. European Journal of Control，2019，47：30-46.

[15] Zecai Lin，Xin Wang，Jian Yang. Trajectory tracking control of robotic transcranial magnetic stimulation[J]. International Journal of Intelligent Computing and Cybernetics，2019，12（2）：245-259.

[16] 张炳力，吕敏煜，程进，等.两点预瞄轨迹跟踪横向控制系统[J].电子测量与仪器学报，2019，33（05）：158-163.

[17] 秦霞，李德釗，邓华.基于模糊PD型输入迭代学习的工业机器人轨迹跟踪控制[J].传感器与微系统，2019，38（04）：66-68+72.

[18] 沈智鹏，张晓玲.带扰动补偿的移动机器人轨迹跟踪反演控制[J].控制工程，2019，26（03）：398-404.

[19] K. Chaib Draa， A. Zemouche， M. Alma，H. Voos， et al. A Nonlinear observer-based trajectory tracking method applied to an anaerobic digestion process[J]. Journal of Process Control，2019，75：120-135.