基于高精度定位系统的轨迹跟踪算法研究 - SUSTC-XLAB/FormulaE GitHub Wiki
本文来自 2016 年 10 月 15 日出版的《 机电一体化》，作者是同济大学智能型新能源汽车协同创新中心的俞佳伟和罗峰教授。
基于人工智能的驾驶行为决策、由多种传感器构成的环境感知系统等新技术的出现，使得车辆在部分场景下的无人驾驶成为可能，现已成为互联网行业与汽车行业争相研究的热点。无人车辆是指通过给车辆装备智能软件和多种感应设备，包括车载传感器、雷达、全球定位系统（Global Positioning System，GPS）以及摄像头等，实现车辆的自主安全驾驶，安全高效地到达目的地并达到完全消除交通事故的目标[1]。
在此期间，卫星定位导航技术不断进步，尤其是我国北斗厘米级高精度定位技术的逐渐成熟 [2]，使得高精度定位导航方式成为无人驾驶车辆必不可少的配备之一。本文基于高精度定位系统，提出了一种适用于低速应用场景下的无人驾驶车辆轨迹跟踪算法。
1. 北斗高精度差分定位系统搭建与基本原理
高精度定位系统构成示意图
本文采用局部基站方式进行北斗高精度定位系统搭建，移动站安装于试验车辆，局部基站安装于实验楼顶。局部基站是整个定位系统的基准框架，长期连续跟踪观测卫星信号，通过无线数传电台实时播发基准站差分改正信息，并实时为各车载移动站提供高精度的载波相位差分（Real-time Kinematic，RTK）数据及起算坐标[3]。移动站接收来自空间卫星信号及局部基站数据，进行 RTK 实时解算，求得厘米级的高精度实时坐标。见图 1。
本文采用的高精度定位系统可进行航向精确定向，并具有惯性导航模块，在卫星信号丢失时由惯性导航模块介入提供短时间的惯性平滑定位。输出数据格式为 GPS 导航设备统一的 RTCM 标准协议 NMEA－0183。主要包含 GGA（定位信息）、RMC（推荐定位信息）、VTG（地面速度信息）等，主要参数见表 1。
表 1 高精度接收机性能指标
2. 路点信息采集与试验路线生成
图 2 路点采集与路线生成示意图
以同济大学嘉定校区为试验场地，使用高精度测绘仪沿车道线进行不等间距路点采集。可输出 WGS84 坐标系下的路点的经纬度、高程等信息。路点数据采集完毕后，将 WGS84 椭球坐标向高斯平面投影，由于路点采集是沿车道线采集，需使用 AutoCAD 软件进行路点筛选与偏移处理，拟合出道路中线路点坐标，以数组形式生成电子地图与试验路线（经纬度坐标、高斯投影坐标），并标记关键点包括起点、终点、十字路口停止点等，见图 2。
3. 高斯-克吕格投影方法
经纬度坐标 (L,B) 是测绘学中常用的球面坐标系统，在工程应用中，球面坐标系无法直观表达两点间的相对位置状态，故需要采用地图数学投影变换，将椭球面上的元素（包括坐标、方位和距离）按一定数学法则投影到平面上。常用投影方式有高斯-克吕格投影、横轴墨卡托投影、兰勃脱投影等。
本文采用高斯-克吕格投影方式。由于高斯-克吕格投影为等角投影，转换后的坐标可真实反映路点间的相对夹角，同时长度形变小，在实际应用中可忽略。高斯-克吕格投影特点如下：
投影后中央子午线为直线，长度不变形，离中央子午线越远，长度变形越大。可用分带方法将形变限制在可接受范围内（分带方法见图 3）。
图 3 高斯-克吕格投影分带方法
赤道投影为一直线，并与中央子午线正交； 经纬投影后仍然保持相互垂直的关系，投影后角度无变形。 由于高斯-克吕格投影是以中央子午线为基准展开投影，需先求得中央子午线弧长 X_z。计算式为：
式中，各系数由以下递推关系式获得：
式中，a为椭球长半轴长度 (m)；e为椭球第一偏心率；B为纬度坐标 (°)。 将高斯-克吕格公式通过泰勒级数展开，并将式 (1) 代入，可获得高精度高斯投影正算式（精度 0.001 m）为：
式中，B为纬度坐标 (°)；L 为经度坐标 (°)；L_0为中央子午线经度坐标(°)；l″为经差 (°)；N 为卯酉圈曲率半径 (m)；X_z 为中央子午线弧长 (m)；e' 为椭球第二偏心率；b 为椭球短半轴长度 (m)。 WGS84 椭球参数见表 2。
表 2 WGS84 椭球参数
由于测试点处于上海，位于东经 121° 附近，按照 3° 带东经 120° 中央子午线展开，代入参数至式 (2)、(3) 进行投影变换。列举部分转换数据便于对比验证，见表 3、表 4。
表 3 原始 WGS84 经纬度坐标
表 4 经过高斯投影变换后的平面坐标
4. 坐标系建立
由于高斯-克吕格投影方法所获得的坐标系是以中央子午线与赤道交点为坐标原点，沿赤道以东为 y 轴正向，沿中央子午线以北为 x 轴正向。为便于计算与直观理解，将高斯平面坐标系 (x，y) 以原点为中心逆时针旋转 90°，然后将 x 轴沿 y 轴平面反转，向笛卡尔坐标系 (X,Y) 转换。变换后的笛卡尔平面坐标系 Y 轴指向为正北方向，X 轴指向为正东方向。变换公式为
图 4 高斯平面坐标系与笛卡尔平面坐标系
为减少计算量，将路点转换至车辆坐标系下，再进行跟踪算法处理。以车辆后轴中心为原点，前进方向为车辆坐标 X' 正向，副驾驶指向驾驶员方向为车辆坐标 Y'，建立车辆正交坐标系见图 4。
笛卡尔平面坐标系下的点变换至车辆坐标系下公式为：
式中，θ 为车辆航向角，即与正北方向夹角，顺时针为正。
5. 轨迹跟踪算法原理
图 5 车辆坐标系建立
将车辆任务路点序列进行三次样条曲线光滑连接，形成任务路径。设预瞄距离为 D，以车辆中心点为圆心，预瞄距离 D 为半径，相交于前方轨迹于点 A，称 A 点 (x_a, y_a) 为预瞄点。见图 5。
具体实现方式如下：
根据车辆运动学模型，车辆航向在一个控制周期内的变化量可近似计算为：
前轮绕运动中心的半径Ｒ可由式 (5) 计算获得：
将式 (5) 代入式 (4) 可得：
v和 δ 在当前采样周期内测得，在下一控制周期内会有所变化；但由于其变化的连续性，故计算得到的 Δθ 作为下一控制周期内的预估量是可行的。称 Δθ为航向变化预估量。
图 6 轨迹跟踪算法流程图
在控制算法中，车辆当前航向角与航向变化预估量之和作为航向反馈量，期望航向与航向反馈量的差值则作为经典 PID 控制器的输入偏差计算期望前轮偏角 δ。该预估模型可改善算法在不同速度条件下的适应性 [4]。流程框图见图 6。
6. 仿真与试验验证
由于车辆运行速度较低，建立以车辆后轴中心点为车辆坐标系原点，车速和前轮偏角为输入量的车辆运动学模型：
图 7 轨迹跟踪算法仿真结果分析
设定车速恒定为 5 km/h，选取采用周期为 0.05s，预瞄距离 D 取 5m，进行仿真验证。结果见图 7。
由仿真结果可知，该算法在曲线段具有良好的跟踪表现，仿真路径与期望路径误差在可接受范围之内，达到了预期的效果。
图 8 试验车平台结构示意图
实车试验采用基于嵌入式快速原型开发控制器作为主控制器，测试平台为具有线控驱动、线控转向与线控制动功能的电动清扫车底盘。图 8 为整车试验平台结构示意图。以同济大学新能源汽车工程中心圆形试车场为试验场地，进行实车绕圈跟踪试验。
图 9 基于高精度定位系统的轨迹跟踪实车试验数据分析
图 9a、9b 是由高精度定位系统给出的水平定位精度因子变化与车辆航向角变化。水平精度因子变化幅度较小，车辆航向角变化连续，说明高精度定位系统在水平面上的定位及定向精度状态良好。
无人驾驶车辆自主行驶的轨迹曲线见图 9c，显示的期望路径是根据预先采集路点生成的轨迹序列。从试验结果可知，采用航向变化预估模型的轨迹跟踪算法结果与仿真结果基本一致，说明该方法在低速条件下对无人驾驶车辆的转向控制有效且符合预期。
- 结束语
本文提出了一种基于高精度定位系统的无人驾驶车辆轨迹跟踪方法，通过预瞄的方法获得期望航向角与前轮偏角，根据无人驾驶车辆当前的前轮偏角和纵向速度来计算航向变化量作为预估量，并将其与实际航向之和作为反馈量，进行反馈控制。
仿真结果与试验结果均表明，该方法在低速条件下可实现车辆的无人驾驶循迹控制，算法简单、易于实现，且效果良好，可与路径规划等功能结合形成功能更加完善的无人驾驶控制方法，具有一定的理论价值和工程应用价值。
参考文献
[1] 杨帆. 无人驾驶汽车的发展现状和展望 [J]. 上海汽车. 2014 (3) : 35-40
[2] 施闯，赵齐乐，李敏，等. 北斗卫星导航系统的精密定轨与定位研究 [J]. 中国科学: 地球科学. 2012, 42(6) :854-861
[3] 谢月红，姚定江. 基于高精度 GPS 定位技术的驾考系统 [J]. 计算机科学与应用. 2016， 6(3) : 103-109
[4] 孙银健. 基于模型预测控制的无人驾驶车辆轨迹跟踪控制算法研究 [D]. 北京: 北京理工大学. 2015