张永华;杜煜;张汇;杨硕;杜晨
【摘 要】为满足智能车在低速和高速运行时稳定和精确的轨迹跟踪,提出了一种基于几何模型的智能车轨迹跟踪算法.算法首先通过惯性导航系统的航向角参数,计算车辆纵向运动方向和轨迹跟踪点切线方向之间的切向角,再通过横向偏差角进行转向的偏差校正,实现轨迹的实时跟踪.以真实智能车在实际道路环境中对算法进行了20 km/h下的小曲率直道和大曲率路口弯道以及50 km/h下的小曲率直道的轨迹跟踪实验.实验结果表明,在不同的典型路况下,采用该算法的智能车能够实现稳定和精确的轨迹跟踪;与其他轨迹跟踪算法相比,该算法具有较好的性能.
【期刊名称】《北京联合大学学报（自然科学版）》
【年(卷),期】2017(031)004
【总页数】汽车360度7页(P54-60)
【关键词】智能车;几何轨迹跟踪;航向角;切向角
【作 者】张永华;杜煜;张汇;杨硕;杜晨
【作者单位】北京联合大学智慧城市学院,北京100101;北京联合大学机器人学院,北京100101;北京联合大学智慧城市学院,北京100101;北京联合大学智慧城市学院,北京100101;北京联合大学北京市信息服务工程重点实验室,北京 100101
【正文语种】中 文
【中图分类】U462
轨迹跟踪算法是智能车车辆控制的一项关键技术，近年来国内外许多高校和科研院所进行了广泛和深入的研究，该算法通过对车辆转向系统的控制，使车辆按照规划的路线行驶，并满足一定的舒适性、安全性和稳定性。以经典的PID控制理论为基础，早期研究人员提出了由前馈控制器和PID反馈控制器复合而成的非线性PID控制器[1]。在此基础之上，研究人员又进一步提出了具有学习功能的自适应轨迹跟踪算法[2]。为了解决PID控制中参数的鲁棒性问题，一些研究人员在鲁棒PID反馈控制器中引入期望方位偏差估计器来构建新的横向控制系统[3]，一些研究人员则通过前轮转向和直接横摆力矩协调控制，实现了基于广义PI
控制的LPV鲁棒增益调度控制器[4]。此外，基于横向位置误差和横摆率，研究人员提出了嵌套PID控制器[5]，该控制器外回路采用PID控制器，通过横向位置误差求出横摆率的参考值，并将其作为外回路的输入值。内回路利用前轮转向的PI控制器通过跟踪横摆率从而实现车道保持。PID控制器虽然可以处理智能车辆的轨迹跟踪问题，但是由于其控制参数通过试凑法获取，因此难以实现对横向运动的最优控制。除了基于PID控制理论的轨迹跟踪算法以外，针对车辆的运动学和动力学特性，研究人员提出了基于模型预测的轨迹跟踪算法。该类算法包括：通过计算车辆的横摆角、横摆率和横摆角加速度的期望值，采用终端滑模控制器进行轨迹跟踪控制的控制方法[6]；采用模型预测控制和前馈控制相结合的方式，在轨迹已知的前提下，通过控制前轮转向角实现轨迹跟踪的控制方法[7]。在此基础上，研究人员又进一步提出了模型预测控制器和PI控制器相结合的控制方法[8]，该方法将转向和直接横摆力矩相结合，从而在轨迹跟踪的同时，实现了对侧向风干扰的抑制。为了解决模型预测控制中参数的自适应性和鲁棒性问题，研究人员提出了基于线性时变预测模型的自适应模型预测控制方法[9]和基于参数空间法的模型预测控制方法[10]，从而提高了算法的适应性和鲁棒性。
为了应对实际驾驶环境的复杂多变性，需要设计满足实际智能驾驶需要的数学模型简单、
控制精确、参数物理意义清晰、易于调节的轨迹跟踪控制算法，本文以现有的基于几何模型的轨迹跟踪算法[11-13]为基础，提出了一种实用的基于几何模型的轨迹跟踪算法。该算法首先通过使用惯性导航系统的航向角参数，转换计算轨迹跟踪算法中的切向角参数，然后通过横向偏差角进行转向的偏差矫正，实现稳定、精确的轨迹跟踪。
1.1 几何轨迹跟踪算法原理
如图1所示，纵轴左侧曲线为智能车先验地图轨迹线，cx,cy为距前轴中心点最近的轨迹点。δt为车轮实际转角，由横向偏差角θfat和切向角θet两部分构成，即
其中横向偏差角θfat是横向偏差efat的非线性函数，即
横向偏差efat是前轴中心点到轨迹点cx,cy之间的距离。vxt是车辆的实际行驶速度vt的纵向分量，k为增益参数，切向角θet是轨迹点cx,cy的切线正方向与纵轴正方向的夹角。方向盘的转角φt由车轮偏角和车轮转角到方向盘转角的映射函数μt共同得到，即
当增益参数k和纵向速度分量vxt为常数时，公式(1)中横向偏差角θfat在车轮实际转角δt中所占的比重越小，表明轨迹跟踪的精度就越高，此时横向偏差efat就越小。因此可以用横向偏
差efat的绝对值的大小来评估轨迹跟踪的效果。
公式(2)中，在不同纵向速度分量vxt情况下，增益参数k用于调节横向偏差efat对横向偏差角θfat的贡献大小。当纵向速度由低到高逐渐增大时，为了保证智能车在行驶过程中及时、平稳、安全地进行横向误差矫正，k值呈现非线性递增变化。
1.2 惯性导航系统航向角解算
惯性导航系统是以陀螺和加速度计为惯性测量元件，通过陀螺的输出建立导航坐标系，通过加速度计的输出计算运动载体的位置、速度等信息。载体姿态角描述载体坐标系相对于当地地理坐标系的转角关系。如图2所示，载体的纵轴与Z轴的对称平面之间的夹角定义为俯仰角，用α表示。载体横轴与Z轴的对称平面之间的夹角定义为横滚角，用γ表示。载体的纵轴在水平面的投影与X轴的夹角定义为航向角，用θ表示。
航向角解算原理如图3所示，载体相对于惯性空间转动的角速率在载体系中的投影是表示的物理量是载体坐标系中的投影)，导航计算机的角速率是表示的物理量是导航坐标系中的投影)，通过两者的变换求出载体姿态矩阵然后将载体坐标系信号转换为导航坐标系信号经导航计算机解算得到载体的姿态信息，从而求得航向角θ∈0°,360°。
1.3 切向角换算
航向角到切向角换算过程如图4所示。
公式(4)中θ表示高精度先验地图轨迹线的航向角，θp表示车辆实时行驶方向所对应的航向角。由于航向角取值范围为0,360，且以360度为周期进行循环表示。因此，为了解决航向角的周期问题，求得主值区间内的合理切向角，需要采用分段函数形式表示，并通过周期补偿求得正确的切向角取值。
1.4 PID轨迹跟踪算法原理
PID轨迹跟踪算法如图5所示，该算法选取车头正前方先验轨迹线的2n个轨迹点作为候选轨迹点集，然后按照轨迹点距离车头位置的距离将该点集分成两部分，分别称为近端轨迹段和远端轨迹段。其中近端轨迹段用于计算近端区域内先验轨迹线的平均横向偏差devision，如公式(5)所示；远端轨迹段用于计算远端区域内由平均横向偏差引起的偏差角deflection，如公式(6)所示。公式(7)中kd和kp分别为用于调节方向盘输出转角φt的PID参数。
deflection=tan-1()，
φt=kd*deflection+kp*devision。
本文的实验平台为北汽绅宝C70改装的智能车，如图6所示。该车的车头安装一个LUX4激光雷达和一个ESR毫米波雷达，用于检测车前远距离的动态和静态障碍物；车顶安装一个Velodyne64线激光雷达，用于检测车辆周围360度范围内的动态和静态障碍物；车尾安装两个RSDS毫米波雷达用于检测车辆左后和右后方向的动态车辆。该车还安装组合导航系统一套，定位精度达到厘米级。车内安装两个AVT-PIKE相机，用于检测车道线、停止线、斑马线和红绿灯。
该智能车实验平台的车轮转角δt取值范围为±33.7°。由于汽车传动系统的限制，车辆偏角到方向盘转角的传动比函数μδt是车轮偏角δt的非线性函数，因此本文采用离散映射表的方式来近似逼近传动比函数。整个轨迹跟踪算法最终输出的方向盘转角φt的取值范围为±540°。
实验路线如图7所示，实验地点选择在园博西二路、长顺三路、园博西一路和长顺二路组成的环形路线，各个路段分别用L1、L2、L3和L4表示，L1和L2、L2和L3、L3和L4、L4和L1之间的路口分别用C1、C2、C3、C4表示。实验路线中各个路段的信息和路口信息如表1和
表2所示。
本实验平台的惯性导航系统采用光纤陀螺传感器，最高定位精度0.02 m，航向精度0.06°，角速度精度0.01°/s，计算延时3.5 ms，操作温度-10 ℃～50 ℃，抗冲击100 G, 11 ms。其设备的稳定性和精度都很高，从而保证解算得到的航向角具有很高的精度、很好的抗干扰性和稳定性。
由于实际场景中智能车的行驶速度在实时发生变化，因此根据不同的速度，选择一组合适的k值，对于轨迹跟踪的精确度和稳定性至关重要。本文经过实际道路的测试，得到了几何轨迹跟踪算法的k系数离散映射经验值，如表3所示：
根据实验路线的特点，实验分为3个部分，首先对实验路线L1进行了20 km/h直道轨迹跟踪测试，用于测试算法在小曲率直道低速行驶时的表现效果；然后对C1进行了20 km/h直角弯道轨迹跟踪测试，用于测试算法在大曲率弯道低速行驶时的表现效果；最后以L1实验路段为测试路段，进行了50 km/h直道轨迹跟踪测试，用于测试算法在小曲率直道较高速度行驶时的表现效果。
3.1 20 km/h直道轨迹跟踪测试
如图8所示，从图中可以看出，基于航向角方式的轨迹跟踪算法和基于PID方式的轨迹跟踪算法输出的方向盘转角曲线整体上均具有很好的连续性和平滑性。基于航向角方式的轨迹跟踪算法相比基于PID方式的轨迹跟踪算法输出的方向盘转角的波动较小，两者平均波动分别为5°和10°。实验表明，基于航向角方式的轨迹跟踪算法相比基于PID方式的轨迹跟踪算法在小曲率直道低速行驶时跟踪稳定性更好。如图9所示，基于航向角方式的轨迹跟踪算法与基于PID方式的轨迹跟踪算法跟踪误差绝对值的平均值分别为0.029 m和0.044 m，两者最小跟踪误差均为零，最大轨迹跟踪误差分别为0.20 m和0.30 m。实验表明基于航向角方式的轨迹跟踪算法相比基于PID方式的轨迹跟踪算法在小曲率直道低速行驶时跟踪精度更高。
3.2 20 km/h直角弯道轨迹跟踪测试
如图10所示，从图中可以看出，基于航向角方式的轨迹跟踪算法和基于PID方式的轨迹跟踪算法输出的方向盘转角曲线整体上均具有很好的连续性和平滑性。基于航向角方式的轨迹跟踪算法和基于PID方式的轨迹跟踪算法输出的方向盘转角平均波动分别为6°和15°。实验表明，基于航向角方式的轨迹跟踪算法相比基于PID方式的轨迹跟踪算法在大曲率直角弯
道低速度行驶时跟踪稳定性更好。如图11所示，基于航向角方式的轨迹跟踪算法与基于PID方式的轨迹跟踪算法跟踪误差绝对值的平均值分别为0.034 m和0.076 m，两者最小跟踪误差均为零，最大轨迹跟踪误差分别为0.30 m和0.40 m。实验表明，基于航向角方式的轨迹跟踪算法相比基于PID的轨迹跟踪算法在大曲率直角弯道低速行驶时跟踪精度更高。
3.3 50 km/h直道轨迹跟踪测试
如图12所示，从图中可以看出，基于航向角方式的轨迹跟踪算法和基于PID方式的轨迹跟踪算法输出的方向盘转角曲线整体上均具有较好的连续性和平滑性。基于航向角方式的轨迹跟踪算法和基于PID方式的轨迹跟踪算法输出的方向盘转角平均波动分别为13°和20°。实验表明，基于航向角方式的轨迹跟踪算法相比基于PID方式的轨迹跟踪算法在小曲率直道较高速度行驶时跟踪稳定性更好。如图13所示，基于航向角方式的轨迹跟踪算法与基于PID方式的轨迹跟踪算法跟踪误差绝对值的平均值分别为0.192 m和0.254 m，两者最小跟踪误差均为零，最大轨迹跟踪误差分别为
0.30 m和0.50 m。实验表明，基于航向角方式的轨迹跟踪算法相比基于PID方式的轨迹跟踪算法在小曲率直道较高速度行驶时跟踪精度更高。
本文介绍了基于航向角的几何轨迹跟踪算法，并在真实道路环境下进行了实验验证。得出如下结论：在小曲率直道低速行驶、大曲率弯道低速行驶和小曲率直道高速行驶这3种典型场景下，本算法相比其他轨迹跟踪算法的跟踪精度和跟踪误差的收敛性、方向盘转角的平滑性等技术指标都更好。采用该算法的智能车实验平台参加了2016年世界机器人大会，在大曲率直角弯道和S路的典型场景下实现了20 km/h精确的轨迹跟踪，顺利完成了比赛，取得了很好的名次。比赛结果充分证明了该算法的适应性和准确性，但是针对本算法的验证测试还不够充分，需要开展更多、更广泛的测试实验来进一步验证算法的稳定性。
发布评论