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

2023-01-11申建华谭志强杨思佳

河北工业科技 2022年6期

申建华，谭志强，杨思佳

(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，点云数据的运动补偿是关键步骤。