基于LiDAR辅助的GNSS/INS组合导航设计

申建华，谭志强，杨思佳

(1.中国电子科技集团公司第五十四研究所，河北石家庄 050081；2.电子信息系统复杂电磁环境效应国家重点实验室，河南洛阳 471001；3.北京卫星导航中心，北京 100094)

随着自动驾驶、智慧城市等研究领域的飞速发展，基于定位服务(location based services，LBS)的行业标准及要求越来越高。全球卫星导航系统(global navigation satellite system,GNSS) 近几年得到飞速发展，主要包括多频多星座的系统拓展，如中国北斗的全球服务应用、美国GPS系统现代化等，同时相关增强系统(包括星级增强(SBAS),地基增强(GBAS)等)的发展大大提高了GNSS系统在开阔场景下的服务能力，满足了大部分应用场景下的作业要求。

但对于挑战环境下的GNSS导航技术仍有待突破，特别是在茂密的森林、高楼林立的城市或室内环境下，GNSS信号因信号衰减、多路径、干扰等因素无法正常到达接收机终端，严重影响定位精度，甚至无法完成定位服务[1-2]。为了增强GNSS拒止场景下的导航定位服务能力，目前主要应用的强化技术包括GNSS反干扰天线、手机通信、Wi-Fi定位、航位推算、天文导航、重力梯度仪、量子导航、伪卫星等技术。基于机会信号(电视信号、手机信号、伪卫星等)的定位技术需要基础设施支撑，往往需要较大的成本，同时也需要保证机会信号的完好性，另一方面，基于量子、重力梯度等前沿技术的导航技术，因成本、技术成熟度等问题短时间内无法落地实现。

在目前常规应用中，仍以自主性较强的航位推算算法为主，以解决GNSS拒止场景下的精度、可靠性问题。其中惯性导航系统(inertial navigation system,INS)是应用较广的自主导航方案，其可以通过自主加速度计、陀螺量测的积分操作来实现短期的航位推算，提供位移、姿态变化信息。由于加速度计、陀螺仪零偏的存在，导致位置、速度、姿态的估计误差会随时间指数级增长，一方面，可以通过高精度惯性量测单元(IMU)来实现长期的高精度航位推算，但通常类似高精度IMU(如导航级)的成本过高，将无法应用于常规应用场景中。另一方面，INS可以通过GNSS不定期的组合纠正误差来抑制误差的积累，从而实现长期可靠的导航。典型的GNSS/INS组合导航系统包括松组合、紧组合、超紧耦合系统[3-8]，考虑到目前可见星数，一般采用松组合系统即可满足常规的定位导航服务。

在室内自主导航定位应用中，可以通过激光雷达(LiDAR)或视觉系统来记录周围环境信息，推算自己的行进轨迹，达到定位目的[9]。SLAM(simultaneous localization and mapping)起源于1986年的ICRA(international conference on robotics and automation)会议，PETER，JIM，DURRANT-WHYTE等几位学者讨论了定位与制图的估计理论与方法。 SMITH和DURRANT-WHYTE等建立了基于相对地标的卡尔曼滤波模型。1966年，DURRANT-WHYTE等系统性地定义了SLAM的结构、收敛结果以及缩写。国外对SLAM技术的研究要比国内早很多，已经有一批相当不错的成果，比如Google的Cartographer背包系统[10]、CSIRO的Zebedee系统，这些系统包括了2D，3D激光和视觉SLAM[11]，可以进行高精度的环境建模，现已应用到各个领域,并结合其他传感器进行了融合定位导航[12-13]。同时国内的SLAM也有了一定的发展[14-15]，但主要是应用研究，受限于LiDAR成本的约束，并没有得到广泛的推广应用。

本文在分析了GNSS定位与激光SLAM技术各自的优缺点之后，对GNSS/INS/LiDAR组合的可行性进行了探究，期望构建出一个能全天候运行、适应各种环境的导航定位算法，并在实验车平台中实现算法的初步验证。

1 GNSS/INS组合系统

全球卫星导航系统和惯性导航系统具有很强的互补特性，GNSS/INS组合系统充分利用了GNSS长期精度高和INS短期精度高、抗干扰能力强的优点，取长补短，获得了比单独使用任一系统时更高的导航性能，也因此在军民领域都得到了越来越广泛的研究和应用。GNSS/INS组合导航系统如图1所示。

图1 GNSS/INS组合导航系统Fig.1 GNSS/INS integration system

GNSS与INS的组合策略通常有3种：松组合、紧组合和深组合[4-5]。其中，松/紧组合系统是系统级组合，GNSS与INS保持相对的独立性，该技术迄今为止已经相对比较成熟。GNSS可见情况下，GNSS/INS组合导航系统是常规的定位、定姿解决方案。在低成本GNSS/INS组合导航系统中，主要有初始化问题。初始姿态需要一定的精度，否则导航系统会慢慢发散或者存有粗差。即便在精准的初始化前提下，由于航向参数的弱观测性，航向估测也会慢慢漂移，在这种情况下，需要其他传感器，如电子罗盘等来对系统航向估测保持长时间的稳定可靠。

如图2所示，IMU组件将通过自身的航位推算来计算位置、速度(主要包括速度更新、位置更新、姿态更新3个步骤)[4]。在本方案中，主要采用了如式(1)和式(2)所示的“单子样+前一周期”算法来补偿不可交换误差。

(1)

(2)

φ(T)为当前时刻的等效旋转矢量。在速度更新中，考虑到IMU精度及航位推算时间长度，在速度更新中并没有加入旋转效应及划桨效应补偿。基于航位推算的位置、速度精度主要取决于IMU器件本身及累积时间，所以不能依靠长时间积累的位置、速度信息。

图2 INS航位推算Fig.2 Dead-reckoning of INS

在GNSS拒止或不可见情况下，GNSS/INS组合系统直接切换为图2所示的纯INS导航系统，由于初始状态零偏、惯性器件零偏等误差因素的影响，在中低精度的IMU传感器情况下，导航的精度会在短时间内发散，无法保障导航系统的联系服务能力。如图3所示，在RTK/INS组合系统测试中，在组合系统稳定之后，通过不同周期的纯INS定位结果来估计INS短时精度，具体精度主要取决于初始零偏、初始姿态等状态信息精度，在图3跑车实验中，可以得出在30 s以上，MEMS惯组的纯DR精度将大打折扣。

图3 基于SBG Ellipse-A-GEA2-B1惯组的纯导航 测试，30 s偏离(5.2 m)40 s 偏移(11.87 m)Fig.3 INS DR test using a MEMS-IMU(SBG Ellipse- A-GEA2-B1),positioning error in 30 s (is 5.2 m), positioning error in 40 s (is 11.87 m)

2 LiDAR辅助的GNSS/INS导航系统

常规LiDAR-SLAM与视觉SLAM的工作流程基本都可以概括为3个环节：1)前端利用连续的点云/影像配准，粗略估计机器人的相对运动；2)将当前接收到的环境信息与之前存储的局部地图进行frame-to-map匹配(或者局部地图与全局地图匹配等)，修正位姿；3)后端建立位姿图进行整体优化，或滤波求解。

如图4所示，主要采用了LOAM框架[16]，将整个LOAM系统分成4个主要部分：1)点云注册(point cloud registration)；2)里程计(LiDAR odometry)；3)建图(LiDAR mapping)；4)位姿整合(transform integration)。

图4 LOAM系统架构Fig.4 Structure of LOAM algorithm

工作流程概括：首先点云注册部分对接收到的LiDAR点云数据进行预处理，包括消除运动畸变、特征点提取等；在里程计部分，通过相邻帧的点云匹配技术，输出一个高频(10 Hz)的位姿；接着建图部分利用新接收的点云帧与建好的局部地图再配准优化高频位姿，输出一个低频位姿(1 Hz)并更新地图；最后位姿整合部分将低频位姿融合到高频位姿并输出。LOAM相比其他激光SLAM算法的优点是它充分发挥了帧到帧(frame-to-frame)和帧到地图(frame-to-map)点云匹配算法的优势，并能针对计算量大小为它们安排合适的任务，LOAM算法能实现的精度可以达到总行驶距离的3%以内[16]。

点云匹配算法可分为帧到帧(frame-to-frame)、帧到地图(frame-to-map)、子图到地图(map-to-map)。其中，frame-to-frame用到的点云数据最少，所以计算量小、精度低。 frame-to-map适中，map-to-map匹配得到的位姿最准确但计算量太大，不适合用在实时环境中。LOAM把定位和建图分开，在定位时，使用frame-to-frame时，因为其计算量小，所以高频输出，达到实时定位的目的；建图时则使用frame-to-map，使得生成的地图和位姿更准确，并以较低的频率执行保证不占用过多的CPU资源[17]；最后再将这2个位姿信息融合。总之就是高频里程计低频建图，这样的方案既保证了里程计信息和建图的准确性，又兼具了实时性。

LiDAR数据扫描过程如图5所示。针对机械式LiDAR，点云数据的运动补偿是关键步骤。

图5 LiDAR数据扫描过程Fig.5 Scanning process of a LIDAR

(3)

(4)

考虑到LiDAR-SLAM与传统GNSS/INS组合系统的优缺点，提出了如图6所示的LiDAR辅助的GNSS/INS组合系统。

图6 LiDAR辅助的GNSS/INS组合系统Fig.6 LiDAR aided GNSS/INS integration system

GNSS/INS组合系统主要的原理是利用LiDAR-SLAM前端里程计短时精度，去弥补短时GNSS拒止情况下的导航定位服务,同时也考虑了激光雷达的完好性分析[16]。

3 实验验证

为了验证LiDAR辅助的GNSS/INS组合系统架构的正确性，在实验车的平台上搭建了验证系统，如图7所示。

图7 实验验证平台Fig.7 Experiment platform

软件：VMware Workstation 14 Player虚拟机，Ubuntu16.04，ROS Kinetic，Opencv-3.3.0，PCL-1.8.1。硬件：1)GNSS/INS使用u-blox的GPS芯片，SBG惯组；2)LiDAR使用速腾聚创的Robosense16线激光雷达。

3.1 室内实验(纯LiDAR)

首先在室内做了LiDAR里程计算法的测试，室内场景实验结果如图8所示。可以明显看到红圈后面的定位结构完全与真实方向存在明显的90°偏离，这是由于在特征匹配问题上造成的。实验中由于在手持LiDAR时，绕该方向的抖动导致LiDAR在x轴的位姿角陡增，而后无法修正的原因。

图8 室内场景实验结果Fig.8 Experiment result in indoor scenario

其次在室外停车场做了3组典型实验，结果如图9所示：1)是沿着停车场直线行走的结果；2)和3)是在转弯处，快速转弯导致特征匹配失效，发生了不同程度的偏离。

图9 停车场实验结果Fig.9 Experiment results in a parking spot

3.2 室外组合实验

在室外组合实验中，如图10所示，点1到点2，点3到点4，点5到点6段，手动将GNSS输入关闭(拒止场景)，通过图6所示的方法，保证了导航定位的连续性服务。

图10 GNSS拒止场景下的组合导航轨迹图Fig.10 Integrated navigation trajectory diagram in GNSS-denied scenario

图11 LiDAR/INS定位与GNSS参考结果对比Fig.11 Comparison between positioning results of LiDAR/INS odometry and GNSS reference

如图11所示，在模拟的4段GNSS拒止场景下(前3段各自为25 s，第4段为35 s)，LiDAR/INS里程计很好地保持了GNSS/INS导航精度，其轨迹与GNSS参考轨迹基本一致，累积距离误差控制在0.5 m以内。LiDAR/INS短期定位误差如表1所示。

表1 LiDAR/INS短期定位误差(以GNSS为标准)Tab.1 Position error statics of LiDAR/INS in short periods (take GNSS as the standard)

实验结果表明，在对设备和算法进行改善后，完全可以胜任为GNSS信号弱的区域提供短时间相对定位任务的。

4 结 语

首先在分析了GNSS定位的局限性后，探讨了一种基于SLAM技术相对定位的补偿方式，并将这两种技术结合在一起期望达到更好的定位效果，并在小车平台上进行验证。从本文实验验证中，可以得出以下结论。

1)纯LiDAR定位、定姿存在一定的漂移，特别在中间出现特征匹配丢失的情况下，直接会引入较大的姿态、位置估计误差。

2)在GNSS/INS定位导航系统工作期间，GNSS的丢失可以依靠LiDAR里程计去弥补并保证整个定位导航系统服务的连续性和可靠性。

从GNSS/INS/LiDAR组合方案及对应的测试结果可以初步证实GNSS挑战环境下的多源异构组合导航系统的优越性。但针对多源融合方式本身的算法结构需要进一步优化和探讨，主要包括多源异构传感之间的深耦合组合及完好性分析，这些将有助于实现高精度、高鲁棒性的多源融合导航系统。