基于惯导系统的智能汽车定位技术应用

文／宁予 张智清 刘明春

高精度定位对无人驾驶汽车的重要性不言而喻。而常用的定位方式若单独使用则不能长期稳定地为无人驾驶车辆提供高精度的定位信息。本文详细介绍了惯导系统的工作原理，把它作为无人驾驶汽车的定位系统，能够实现较好的定位效果。

惯导系统是车辆定位的核心设备，包括惯性传感器（IMU）和全球定位系统（GNSS）。前者用于检测车辆加速度与旋转运动，实时估计车辆运动状态，信息更新频率高但存在偏差与噪音；后者通过卫星定位，可提供三维坐标和速度，但信息更新频率低且信号易丢失；两者结合取长补短，能达到较好的定位效果。

一、惯导数据协议和语句格式

目前GNSS 接收机上使用最广泛的协议是NMEA-0183 协议，大 多 数 常 见 的 GNSS 接 收机、GNSS 数据处理软件、导航软件都遵守或者至少兼容该协议。NMEA-0183 协议定义的语句格式众多，常用的语句格式有$XXGGA、$XXGSA、$XXGSV、$XXRMC、$XXVTG 等，随着各种卫星系统增多，每种报文的报头不一样，如GPS 的报文头为GP（即上述语句格式中的XX 为GP，则 $XXGGA 对应为 GPGGA），GLONASS 的报文头为GL，中国北斗卫星导航（BDS）的报文头为BD，对于多系统联合定位（双星或者多星）的报文头为GN。各语句中包含时间、经纬度、航向角、海拔高度等定位信息，并按照指定数据格式输出。以$GPGGA 语句格式为例，其语句中的数据内容如下：

$GPGGA,utc,lat,latdir,lon,londir,QF,sat(No.),hdop,alt,aunits,age,stn(ID),★xx,[CR][LF]

上述语句中各数据段的含义如下表所示：

如某时刻惯导系统输出的一条完整$GPGGA 数据为：

$GPGGA,011716.00,3120.998 2885,N,12117.5452325,E,4,23,1.1,26.4000,M,10.305,M,01,1496★6D

编号 名称 描述格式1 $GPGGA Log header 2 utc UTC时间 (时/分/秒)hhmmss.ss 3 lat 纬度：-90～90度 纬度ddmm.mmmm（度分） 格式 ddmm.mmm 4 latdir 纬度方向：N：北；S：南N/S 5 lon 经度：-180～180度 纬度ddmm.mmmm（度分） 格式 ddmm.mmmm 6 londir 经度方向：E：东；W：西E/W 7 QF解状态0：无效解； 1：单点定位解； 2：伪距差分；4：固定解； 5：浮动解；n 8 sat No.卫星数n x.x 10 alt 高程 h.h 11 a-units 高程单位 M 12 age 差分延迟 dd 13 stn ID 基站号：0000-1023，单机时：AAAA xxxx 14 ＊xx Checksum ＊hh 15 [CR][LF] Sentence terminator [CR][LF]9 hdop 水平DOP值

则 可 知 该 时 刻 经 度为 12117.5452325，纬 度 为3120.9982885。 在 车 辆 行 驶 过程中，惯导系统连续采集车辆位置信息（惯导系统数据采集周期20ms-50ms），数据处理算法根据相应的语句格式，提取其中的经度、纬度和航向角数据，便可得到车辆连续的位置信息。

二、惯导数据处理

目前GNSS 接收机普遍采用串口输出方式。使用工控机、PC、嵌入式控制器等计算平台，通过串口与惯导设备连接，读取惯导设备输出的串口数据，根据数据格式协议进行解析，选取所需的数据（如经纬度、航向角等），通过坐标转换，得到平面坐标系下的坐标信息（X、Y）。

（一）坐标系

大地坐标系是大地测量中以参考椭球面为基准面建立起来的坐标系。地面点的位置用大地经度、大地纬度和大地高度表示。主要涉及三类常用的大地坐标系统，即参心坐标系统（1980 西安坐标系、1954 北京坐标系）、地心坐标系统（WGS-84）和地方独立坐标系统。

平面坐标系，UTM（UNIVERSAL TR ANSVERSE MERCARTOR GRID SYSTEM,通用横墨卡托格网系统）坐标是一种平面直角坐标，通常基于WGS84 椭球。在北纬84°与南纬80°之间共有60 个经度带，它们是6 度分带。为了避免边界的经度变形，使用了相交柱面进行投影。

高斯平面坐标系，以中央子午线与赤道的交点作为坐标原点，以中央子午线的投影为纵坐标轴X，规定X 轴向北为正，以赤道的投影为横坐标轴Y，Y 轴向东为正，形成的坐标系。

（二）坐标系转换

图1 惯导系统应用框图

GNSS 接收机输出的数据通常包含经、纬度信息，为方便且直观的得到位置信息，利用坐标转换公式，即可将大地坐标系的经纬度信息转换为平面坐标系的坐标值。以WGS84 坐标系转UTM坐标系为例：只针对北半球，中国地区计算方法，经纬度分别为单位为弧度，UTM 坐 标 为（E,N），单 位 为k m，k0=0.9 9 9 6，E0=5 0 0 k m，e=0.0818192。转换公式如下：

利用上述公式并进行转换，编写代码，其中L 为经度，B 为纬度，最后得到X 为平面坐标系横轴，Y为平面坐标系纵轴。

三、基于惯导系统的车辆行驶轨迹采集和处理

智能车辆在行驶过程中需要实时输出其位置信息，并输出实时的行驶轨迹，以作为车辆决策、规划和控制的依据。车辆的位置信息和行驶轨迹通过对惯导系统采集的数据进行处理得到。

（一）基于ROS 生成实时行驶轨迹

ROS（Robot Operating System）机器人操作系统，是用于编写机器人软件程序的一种具有高度灵活性的软件架构。ROS 提供一些标准操作系统服务，例如硬件抽象，底层设备控制，常用功能实现，进程间消息以及数据包管理。ROS 是基于一种图状架构，从而不同节点的进程能接受、发布、聚合各种信息（例如传感，控制，状态，规划等等）。基于ROS 生成车辆实时行驶轨迹的过程如下：

1.初始化ROS 节点，发布话题；

2.设置串口参数，并判断串口状态；

3.读取串口数据，根据惯导数据格式，判断是否为需要处理的数据，如对比数据标识符是否为$GPGGA；

4.提取经纬度、航向角等信息，通过坐标转换，将经纬度位置信息换算为大地坐标系的位置信息；

5.发布位置坐标到RVIZ（机器人操作系统3D 可视化工具）进行显示，并保存数据。

图2 坐标系转换公式

图3 根据坐标系转换公式编写代码

（二）轨迹数据后处理

生成行驶轨迹时，由于各种因素（如有行人、障碍物在道路上）导致录制的轨迹非理想状态，此时进行适当的数据处理使得录制的参考轨迹更加合理、真实。

1.轨迹点等间距化

惯导系统输出的行驶轨迹，本质上为一连串的位置坐标数据，由于行驶过程中速度变化，导致采集的坐标数据间距不均匀，甚至某一位置存在大量的数据，不仅数据量多，且在自动驾驶时匹配参考轨迹点时不易匹配。因此将录制的轨迹数据进行等间距化处理，使轨迹点间距相等，得到更加均匀分布的轨迹数据，有利于为自动驾驶系统提供更好的参考轨迹。

2.弯道曲线拟合

惯导系统在采集车辆位置数据，以及后续生成的路径轨迹中，车辆在直角转弯等转弯半径较小的情况下，存在行驶轨迹不平滑的问题，导致自动驾驶系统匹配参考轨迹时易偏离。因此需要对转弯半径较小的轨迹数据进行曲线拟合（如多项式拟合），拟合为平滑的圆弧，降低参考轨迹匹配偏移，提升行驶平稳性。

（三）测试与结果分析

1.测试方案

本文的实车测试在汉腾汽车技术研究院的园区内进行的，该测试场地空旷，大部分区域卫星信号较好，能保证GPS/IMU 系统具备良好的定位准确性，在测试中，惯导系统的主机安装于车辆后轴中心处，通过串口与工控机连接实现通讯。

2.测试结果分析

为验证惯导系统定位的精准性及基于ROS 生成实时行驶轨迹，智能汽车按照规划的路线行驶，可在RVIZ 界面观察到车辆位置以及行驶的轨迹，并将采集的数据进行后处理。经对比可发现：经处理后的数据所生成轨迹，数据量极大的压缩，定位点间隔基本一致，且在弯道处的轨迹平滑，可以较合理地反映车辆的行驶轨迹，并作为自动驾驶车辆的参考行驶路径。