第15卷第4期 智能系统学报 Vol.15 No.4 2020年7月 CAAI Transactions on Intelligent Systems Jul.2020 D0L:10.11992tis.202008026 基于LDAR/NS的野外移动机器人组合导航方法 宋锐，方勇纯，刘辉 (南开大学人工智能学院，天津300350) 摘要：移动机器人在地形复杂等野外环境跨区域运动时，机器人运动特性和环境特征变化更为明显，由此引 起的点云畸变和特征点稀疏等问题尤为突出，有必要结合传感器标定误差、车轮打滑和车体颠簸等因素进一步 改进机器人的位姿估计精度。本文对基于LDAR/NS的移动机器人环境建模和自主导航方法展开研究，针对 LGO-LOAM等在处理车体姿态快速变化时的性能退化问题，提出一种适用于野外移动机器人运动特性的点 云特征分析和多传感融合方法，利用IMU的预积分与LiDAR的scan-to-ma即构成优化函数，进而迭代更新机器 人的位姿。野外环境实验结果表明，当机器人以较高速度做转弯运动或在短时间内多次转向时，本文所提方法 仍可以为优化提供良好的初值估计，相比LGO-LOAM等方法具有更高的位姿估计精度。 关键词：移动机器人；同步定位与建图；位姿估计；紧耦合；非线性；惯性导航；组合导航系统：数据融合 中图分类号：TP242文献标志码：A文章编号：1673-4785(2020)04-0804-07 中文引用格式：宋锐，方勇纯，刘辉.基于LiDAR/INS的野外移动机器人组合导航方法IJ.智能系统学报，2020,15(4)： 804-810. 英文引用格式：SONG Rui,,FANG Yongchun,LIU Hui.Integrated navigation approach for the field mobile robot based on LiDAR/INSIJI.CAAI transactions on intelligent systems,2020,15(4):804-810. Integrated navigation approach for the field mobile robot based on LiDAR/INS SONG Rui,FANG Yongchun,LIU Hui (College of Artificial Intelligence,Nankai University,Tianjin 300350,China) Abstract:When the mobile robot moves in the large scale field with complex terrain,the motion and environmental characteristics changes dramatically,and the problems of motion distortion in point clouds and the sparse of feature points become prominently.Hence,it is necessary to improve the estimation accuracy of position and states from as- pects of the calibration of sensors'error,wheel-slip and bumpy terrain.The environmental modeling and autonomous navigation of the field mobile robot based on LiDAR/INS(Inertial Navigation System)are studied in this paper.Aiming at the degradation problem of LeGO-LOAM when the vehicle state changes dramatically,the strategy of point cloud feature extraction and sensor fusion are proposed to adapt the motion characteristics of the field mobile robot,the optim- ization function is composed of IMU pre-integration and scan-to-map of LiDAR,then the state of robot is updated.Fi- nally,the field experiments are implemented when the robot makes a fast turning or steering in limited time,the pro- posed method are validated to provide more accurate estimation of initial values and states compared to LeGO-LOAM related approaches. Keywords:mobile robot;SLAM(simultaneous localization and mapping);pose estimation;tightly coupled;nonlinear; inertial navigation;integrated navigation system;data fusion 收稿日期：2020-08-20. 对于野外移动机器人，利用实时定位与地图 基金项目：国家自然科学基金项目(61903202)，国家重点研发 (simultaneous localization and mapping,SLAM) 计划项目(2018YFB1307503.2018YFB1309003). 通信作者：方勇纯.E-mail:fangyc@nankai.edu.cn. 和多源感知信息建立三维环境模型和自主定位
DOI: 10.11992/tis.202008026 基于 LiDAR/INS 的野外移动机器人组合导航方法 宋锐，方勇纯，刘辉 （南开大学 人工智能学院，天津 300350） 摘 要：移动机器人在地形复杂等野外环境跨区域运动时，机器人运动特性和环境特征变化更为明显，由此引 起的点云畸变和特征点稀疏等问题尤为突出，有必要结合传感器标定误差、车轮打滑和车体颠簸等因素进一步 改进机器人的位姿估计精度。本文对基于 LiDAR/INS 的移动机器人环境建模和自主导航方法展开研究，针对 LeGO-LOAM 等在处理车体姿态快速变化时的性能退化问题，提出一种适用于野外移动机器人运动特性的点 云特征分析和多传感融合方法，利用 IMU 的预积分与 LiDAR 的 scan-to-map 构成优化函数，进而迭代更新机器 人的位姿。野外环境实验结果表明，当机器人以较高速度做转弯运动或在短时间内多次转向时，本文所提方法 仍可以为优化提供良好的初值估计，相比 LeGO-LOAM 等方法具有更高的位姿估计精度。 关键词：移动机器人；同步定位与建图；位姿估计；紧耦合；非线性；惯性导航；组合导航系统；数据融合 中图分类号：TP242 文献标志码：A 文章编号：1673−4785(2020)04−0804−07 中文引用格式：宋锐, 方勇纯, 刘辉. 基于 LiDAR/INS 的野外移动机器人组合导航方法 [J]. 智能系统学报, 2020, 15(4): 804–810. 英文引用格式：SONG Rui, FANG Yongchun, LIU Hui. Integrated navigation approach for the field mobile robot based on LiDAR/INS[J]. CAAI transactions on intelligent systems, 2020, 15(4): 804–810. Integrated navigation approach for the field mobile robot based on LiDAR/INS SONG Rui，FANG Yongchun，LIU Hui (College of Artificial Intelligence, Nankai University, Tianjin 300350, China) Abstract: When the mobile robot moves in the large scale field with complex terrain, the motion and environmental characteristics changes dramatically, and the problems of motion distortion in point clouds and the sparse of feature points become prominently. Hence, it is necessary to improve the estimation accuracy of position and states from aspects of the calibration of sensors’ error, wheel-slip and bumpy terrain. The environmental modeling and autonomous navigation of the field mobile robot based on LiDAR/INS (Inertial Navigation System) are studied in this paper. Aiming at the degradation problem of LeGO-LOAM when the vehicle state changes dramatically, the strategy of point cloud feature extraction and sensor fusion are proposed to adapt the motion characteristics of the field mobile robot, the optimization function is composed of IMU pre-integration and scan-to-map of LiDAR, then the state of robot is updated. Finally, the field experiments are implemented when the robot makes a fast turning or steering in limited time, the proposed method are validated to provide more accurate estimation of initial values and states compared to LeGO-LOAM related approaches. Keywords: mobile robot; SLAM (simultaneous localization and mapping); pose estimation; tightly coupled; nonlinear; inertial navigation; integrated navigation system; data fusion 对于野外移动机器人，利用实时定位与地图 (simultaneous localization and mapping, SLAM) 技术 和多源感知信息建立三维环境模型和自主定位， 收稿日期：2020−08−20. 基金项目：国家自然科学基金项目 (61903202)，国家重点研发 计划项目 (2018YFB1307503，2018YFB1309003). 通信作者：方勇纯. E-mail：fangyc@nankai.edu.cn. 第 15 卷第 4 期 智 能 系 统 学 报 Vol.15 No.4 2020 年 7 月 CAAI Transactions on Intelligent Systems Jul. 2020
第4期 宋锐，等：基于LiDAR/INS的野外移动机器人组合导航方法 ·805· 是移动机器人在未知环境中自主导航的关键。野 步提高系统的性能4 外环境地形复杂、特征稀疏等因素对传统视觉测 Zhao等16提出鲁棒激光惯性里程计与建图 量有不利影响，而基于三维激光雷达((light detec- (laser-inertial odometry and mapping,LIOM)方法对 tion and ranging,LiDAR)可以有效应对环境光照 LDAR和INS测量数据联合优化，但由于数据量 和纹理的变化。在结构化环境模型更新过程中 过大，实时性难以有效保证。Liu Ming团队1剧 机器人低速运动产生的累计误差可以近似求解。 提出基于紧耦合框架的激光雷达惯性状态估计 随着野外作业任务对机器人时空覆盖率要求的不 器((Iidar--inertial state estimator,LNS)。Shan等ug 断提高，移动机器人的平均速度将比现阶段有较 采用因子图优化方法，并将帧一局部地图用于帧 大提升，由于机器人快速移动引起的本体剧烈振 间匹配，实验结果表明其具有更高定位精度和鲁 动和姿态变化，对车体稳定性和颠簸等状态下的 棒性。点云配准算法利用连续点云对机器人位姿 位姿估计带来了更严峻挑战。 进行估计，其中迭代最邻近点(iterative closest 用于移动机器人环境感知的传感器可分为主 point,,ICP)已得到广泛运用，但对于大规模点云情 动式和被动式，其中激光雷达具有宽视场(ield 形，存在配准效率低、实时性差等问题。此外，当 of view,FOV)、不受光照影响等优点，在SLAM等 机器人以大转角运动时，两帧数据之间变化剧 任务中已经得到广泛运用，在视觉退化等场景中 烈，点云配准算法可能发生失效等问题02。 可以提供更为鲁棒和准确的位姿估计。由陀螺仪 多传感器数据融合算法可以分成基于滤波和 和加速度计等构成的惯性导航系统(inertial navig- 优化两种，其中滤波算法复杂度较低，在处理非 ation system,NS),可以根据测量得到的载体三轴 线性误差方面仍需改进。基于优化的算法具有精 加速度和三轴角速度等信息，利用惯导解算算法 度高的特点，但在实时性方面存在不足。Kaess 计算载体位置和姿态，具有自主性好、短时精度 等提出基于因子图(factor graph)的增量式平滑 高等优点。在大范围场景建图实践应用中，根据 的全优化算法，每次仅优化更新被影响的变量节 实时动态差分(real-time kinematic,RTK)与NS的 点，可以维持全优化的稀疏特性。研究表明32刈 组合导航结果，将估计的姿态作为先验因子进 采用基于贝叶斯树的因子图优化算法，有效降低 行后端优化。在基于互补滤波的NS姿态估计 了连续配准造成的累计误差，在开阔野外环境定 中，由于估计精度受制于载体的动态条件，主要 位精度达到分米级。 应用于融合定位等场景。 针对融合LiDAR和INS的野外移动机器人 由激光雷达与惯性导航系统构成的组合导航 自主导航，目前有关机器人匀速移动等假设具有 系统，利用信息融合方式可以实现姿态信息的校 一定局限性，本文根据机器人快速转弯和移动时 正，称为激光雷达惯性里程计(LiDAR-inertial odo 车体振动和姿态变化等情形，从以下几个方面展 mety,LIO)向，但由于图优化计算具有代价高等问 开分析：首先讨论组合系统的架构，针对现有研 题，使得LO在一些情形下的实时性难以满足要 究方法性能退化问题，利用MU的姿态信息为点 求。Zhang等m提出的激光雷达里程计与地图构 云配准和点云畸变消除提供先验信息，将其作为 建方法(lidar odometry and mapping,.LOAM),采用 观测约束在优化计算中进行考虑。在特征提取和 点到边和点到面的联合配准方法估计LDAR的 信息融合方面提出改进思路，利用MU的预积分 相对运动，在双线程上分别运行高频率的激光里 与LiDAR的scan-to-map构成优化函数，结合误差 程计和低频率的地图构建模块，可以满足实时性 状态卡尔曼滤波器(error state Kalman filter,. 需求，但后端缺少回环检测对漂移进行修正，且 ESKF)迭代修正机器人的位姿，结合实验数据分 未将GNSS等测量数据用于位姿估计。Tian等 析了方法的有效性。 进一步提出轻量级LOAM((lightweight and ground-. 1 组合导航算法概述 optimized LOAM,LeGO-LOAM),通过增加点云分 割处理模块降低点云规模并存储关键帧的特征。 多传感紧耦合算法流程一般可分为5个部 针对缺乏结构化特征的野外环境，基于单纯点云 分：1)数据预处理：利用MU预积分得到的相对 配准的位姿估计和地图构建误差累计较大9山。 运动估计对特征点畸变进行校正；2)将连续时刻 在Cartographer等应用中l2.l),前端数据的松耦合 的激光里程计特征点进行匹配，估计LiDAR的相 和后端以点云匹配为约束的优化仍属于松耦合的 对运动；3)基于校正后的特征点与局部地图配 框架，有必要结合前后端分层优化的思路，进一 准，更新局部地图；4)回环检测，利用间隔关键帧
是移动机器人在未知环境中自主导航的关键。野 外环境地形复杂、特征稀疏等因素对传统视觉测 量有不利影响，而基于三维激光雷达 (light detection and ranging, LiDAR) 可以有效应对环境光照 和纹理的变化。在结构化环境模型更新过程中， 机器人低速运动产生的累计误差可以近似求解[1]。 随着野外作业任务对机器人时空覆盖率要求的不 断提高，移动机器人的平均速度将比现阶段有较 大提升，由于机器人快速移动引起的本体剧烈振 动和姿态变化，对车体稳定性和颠簸等状态下的 位姿估计带来了更严峻挑战[2-3]。 用于移动机器人环境感知的传感器可分为主 动式和被动式[4] ，其中激光雷达具有宽视场 (field of view, FOV)、不受光照影响等优点，在 SLAM 等 任务中已经得到广泛运用，在视觉退化等场景中 可以提供更为鲁棒和准确的位姿估计。由陀螺仪 和加速度计等构成的惯性导航系统 (inertial navigation system, INS)，可以根据测量得到的载体三轴 加速度和三轴角速度等信息，利用惯导解算算法 计算载体位置和姿态，具有自主性好、短时精度 高等优点。在大范围场景建图实践应用中，根据 实时动态差分 (real-time kinematic, RTK) 与 INS 的 组合导航结果[5] ，将估计的姿态作为先验因子进 行后端优化。在基于互补滤波的 INS 姿态估计 中，由于估计精度受制于载体的动态条件，主要 应用于融合定位等场景。 由激光雷达与惯性导航系统构成的组合导航 系统，利用信息融合方式可以实现姿态信息的校 正，称为激光雷达惯性里程计 (LiDAR-inertial odometry, LIO) [6] ，但由于图优化计算具有代价高等问 题，使得 LIO 在一些情形下的实时性难以满足要 求。Zhang 等 [7] 提出的激光雷达里程计与地图构 建方法 (lidar odometry and mapping, LOAM)，采用 点到边和点到面的联合配准方法估计 LiDAR 的 相对运动，在双线程上分别运行高频率的激光里 程计和低频率的地图构建模块，可以满足实时性 需求，但后端缺少回环检测对漂移进行修正，且 未将 GNSS 等测量数据用于位姿估计。Tian 等 [8] 进一步提出轻量级 LOAM (lightweight and groundoptimized LOAM, LeGO-LOAM)，通过增加点云分 割处理模块降低点云规模并存储关键帧的特征。 针对缺乏结构化特征的野外环境，基于单纯点云 配准的位姿估计和地图构建误差累计较大[9-11]。 在 Cartographer 等应用中[12-13] ，前端数据的松耦合 和后端以点云匹配为约束的优化仍属于松耦合的 框架，有必要结合前后端分层优化的思路，进一 步提高系统的性能[14-15]。 Zhao 等 [16] 提出鲁棒激光惯性里程计与建图 (laser-inertial odometry and mapping, LIOM) 方法对 LiDAR 和 INS 测量数据联合优化，但由于数据量 过大，实时性难以有效保证。Liu Ming 团队[17-18] 提出基于紧耦合框架的激光雷达惯性状态估计 器 (lidar-inertial state estimator, LINS)。Shan 等 [19] 采用因子图优化方法，并将帧—局部地图用于帧 间匹配，实验结果表明其具有更高定位精度和鲁 棒性。点云配准算法利用连续点云对机器人位姿 进行估计，其中迭代最邻近点 (iterative closest point, ICP) 已得到广泛运用，但对于大规模点云情 形，存在配准效率低、实时性差等问题。此外，当 机器人以大转角运动时，两帧数据之间变化剧 烈，点云配准算法可能发生失效等问题[20-21]。 多传感器数据融合算法可以分成基于滤波和 优化两种，其中滤波算法复杂度较低，在处理非 线性误差方面仍需改进。基于优化的算法具有精 度高的特点，但在实时性方面存在不足。Kaess 等 [22] 提出基于因子图 (factor graph) 的增量式平滑 的全优化算法，每次仅优化更新被影响的变量节 点，可以维持全优化的稀疏特性。研究表明[23-24] 采用基于贝叶斯树的因子图优化算法，有效降低 了连续配准造成的累计误差，在开阔野外环境定 位精度达到分米级。 针对融合 LiDAR 和 INS 的野外移动机器人 自主导航，目前有关机器人匀速移动等假设具有 一定局限性，本文根据机器人快速转弯和移动时 车体振动和姿态变化等情形，从以下几个方面展 开分析：首先讨论组合系统的架构，针对现有研 究方法性能退化问题，利用 IMU 的姿态信息为点 云配准和点云畸变消除提供先验信息，将其作为 观测约束在优化计算中进行考虑。在特征提取和 信息融合方面提出改进思路，利用 IMU 的预积分 与 LiDAR 的 scan-to-map 构成优化函数，结合误差 状态卡尔曼滤波器 (error state Kalman filter, ESKF) 迭代修正机器人的位姿，结合实验数据分 析了方法的有效性。 1 组合导航算法概述 多传感紧耦合算法流程一般可分为 5 个部 分：1) 数据预处理：利用 IMU 预积分得到的相对 运动估计对特征点畸变进行校正；2) 将连续时刻 的激光里程计特征点进行匹配，估计 LiDAR 的相 对运动；3) 基于校正后的特征点与局部地图配 准，更新局部地图；4) 回环检测，利用间隔关键帧 第 4 期 宋锐，等：基于 LiDAR/INS 的野外移动机器人组合导航方法 ·805·
·806· 智能系统学报 第15卷 构成的小回环为转角提供更多约束，可用于提升 2 LiDAR/INS数据融合方法 激光里程计的鲁棒性，而大回环则可以通过建立 闭环约束并修正全局累计误差；5)基于紧耦合的 2.1数据预处理 数据融合，包含LiDAR里程计、IMU预积分和地 MU在t时刻的角速度和加速度观测模型可 图配准等，进而得到机器人全局一致的位姿。本 表示为 文所提算法的流程图如图1所示，其中标号为文 ù，=+b+n à，=C(a,-g)+b+n 中对应公式。 式中：o,和a,分别为MU原始数据；b、b和、 IMU +预积分 运动估计 分别表示对应输出信号中的漂移和高斯白噪 特征点云 位姿 声；C为坐标转换矩阵。移动机器人状态定义为 畸变校正 →点云配准 估计紧耦合 算法 x;=R:p.v.bi LDAR+特征分割 地图配淮→局部地图 式中：R∈SO(3)为旋转矩阵；p,∈R3表示位置向 量；，和b:分别为载体速度和IMU漂移；T∈SE(3) 图1算法流程图 表示从载体系到导航系的转换矩阵，即T=[RP。 Fig.1 The flowchart of the algorithm 基于上述分析，移动机器人在t+△：时刻的速 通过融合LiDAR点云描述和机器人运动学 度、位置和旋转可以分别表示为 信息，进一步分析移动平台颠簸对LiDAR点云畸 v+=y,+g△1+C(a,-b-n)△ 变和视觉图像模糊的影响。具体方案如下： 1)针对机器人的姿态变化引起的点云图像扰 Aw=R+w,A+58Ar+C宁a-所-g9ar 动，采用组合导航系统信息对坐标转换中的旋转 R+=Cexp(o-b-n)△1 矩阵R和平移T进行初始化，当导航坐标系、载 利用IMU预积分理论计算相邻i和j时刻 体坐标系和第i个传感器坐标系分别表示为{}、 机器人相对位姿变化量，△、△P、△R;分别表 {b}、{S},则环境传感器测量点P,转换到导航系 示为 可以表示为式(1)： △v=R(yj--g△) np=f(Ψ，Φ，p)=Rg(RP+T)+T6 (1) △P=R(p-p.-△t-8A) △R=RR 式中：平，Φ分别为机器人在导航坐标{m,b1,{S 针对车体快速转弯等运动情形，考虑在一个 系下的位姿和第i传感器相对于b系的位姿。 优化任务里执行点云畸变补偿和点云匹配，计算 2)对三维点云数据的不确定性和量测点误差 激光点采集时刻和相对于该帧点云初始时刻的姿 进行研究并建立对应误差模型，将量测点投影到 态变化量，在点云配准时将畸变补偿的残差作为 二维平面，则其分布在栅格的概率可用黎曼分 误差函数的一部分，对LiDAR位姿之间的IMU 布近似表示为式(2)： 运动信息进行积分，并将积分得到的相对位姿于 P(np.,nn,)eg)≈ 因子图中的匹配位姿实施图优化，实验结果表 --0:-)P54+,4+4） (2) 2 2 明，该方法可以进一步提高定位和建图的精度。 利用式(3)对多层环境栅格地图的每个栅格 2.2点云分割与特征提取 平均高程值进行更新： 本文在点云分割和特征提取阶段，考虑加入 aen 基于区域生长的分割方法和地面提取算法，根据 N二1 m=1 (3) 被分割物体的点云个数判断其是否为稳定特征， ∑P. 进而对地面提取之后剩余的点云进行聚类分析， 式中：P.为第n高程值在栅格内的概率；en为高 可以在一定程度上减小动态物体对建图的影响， 程的均值。 同时可以有效降低Z轴方向的漂移误差。在激 3)针对环境特征点稀少引起的近似误差增大 光里程计模块，基于提取的特征点构建scan-to- 和目标函数陷入极小值点的问题，可以采用闭环 scan的约束关系，经过两次Levenberg-Marquardt 检测等方式抑制漂移的影响。在此基础上，利用 优化得到姿态变换矩阵。 高斯过程模型对LiDAR运动速度进行估计并利 假设在1时刻获取的激光点云为P,={p1,P2,…, 用惯性信息修正位姿，进而对环境模型更新进行 P,VLP-l6线激光雷达的水平和垂直方向角分辨 优化，实现机器人环境特征的完整映射。 率为0.2°和2°。将地面和目标分割后，在特征提
构成的小回环为转角提供更多约束，可用于提升 激光里程计的鲁棒性，而大回环则可以通过建立 闭环约束并修正全局累计误差；5) 基于紧耦合的 数据融合，包含 LiDAR 里程计、IMU 预积分和地 图配准等，进而得到机器人全局一致的位姿。本 文所提算法的流程图如图 1 所示，其中标号为文 中对应公式。 IMU LiDAR 预积分 畸变校正 特征分割 地图配准 局部地图 点云配准 紧耦合 算法 运动估计 特征点云 位姿 估计 图 1 算法流程图 Fig. 1 The flowchart of the algorithm 通过融合 LiDAR 点云描述和机器人运动学 信息，进一步分析移动平台颠簸对 LiDAR 点云畸 变和视觉图像模糊的影响。具体方案如下： R T i P S i 1) 针对机器人的姿态变化引起的点云图像扰 动，采用组合导航系统信息对坐标转换中的旋转 矩阵 和平移 进行初始化，当导航坐标系、载 体坐标系和第 个传感器坐标系分别表示为{n}、 {b}、{Si}，则环境传感器测量点 转换到导航系 可以表示为式 (1)： np = f(Ψ,Φ,P S i ) = R n b (R b si P S i +T b si )+T n b (1) Ψ,Φ {n},{b},{S i} i b 式中： 分别为机器人在导航坐标 系下的位姿和第 传感器相对于 系的位姿。 gi j 2) 对三维点云数据的不确定性和量测点误差 进行研究并建立对应误差模型，将量测点投影到 二维平面，则其分布在栅格 的概率可用黎曼分 布近似表示为式 (2)： P ( (npx ,npy ) ∈ gi j) ≈ (xi+ − xi−)(yi+ −yi−)· P ( xi+ + xi− 2 , yi+ +yi− 2 ) (2) 利用式 (3) 对多层环境栅格地图的每个栅格 平均高程值进行更新： ξˆ N = ∑N n=1 Pneˆn ∑N n=1 Pn (3) 式中： Pn 为第 n 高程值在栅格内的概率；eˆn 为高 程的均值。 3) 针对环境特征点稀少引起的近似误差增大 和目标函数陷入极小值点的问题，可以采用闭环 检测等方式抑制漂移的影响。在此基础上，利用 高斯过程模型对 LiDAR 运动速度进行估计并利 用惯性信息修正位姿，进而对环境模型更新进行 优化，实现机器人环境特征的完整映射。 2 LiDAR/INS 数据融合方法 2.1 数据预处理 IMU 在 t 时刻的角速度和加速度观测模型可 表示为 ωˆ t = ωt +b ω t +n ω t aˆt = C bn t (at −g)+b a t +n a t ωˆ t aˆt b ω t b a t n ω t n a t C bn t 式中： 和 分别为 IMU 原始数据； 、 和 、 分别表示对应输出信号中的漂移和高斯白噪 声； 为坐标转换矩阵。移动机器人状态定义为 xi = [ Ri pi vi bi ] Ri ∈ S O(3) pi ∈ R 3 vi bi T ∈ S E(3) T = [ Ri pi ] 式中： 为旋转矩阵； 表示位置向 量； 和 分别为载体速度和 IMU 漂移； 表示从载体系到导航系的转换矩阵，即 。 基于上述分析，移动机器人在 t+ ∆t 时刻的速 度、位置和旋转可以分别表示为 vt+∆t = vt+g∆t+C bn t (ˆat −b a t −n a t )∆t pt+∆t = pt+vt∆t+ 1 2 g∆t 2+ 1 2 C bn t (ˆat −b a t −n a t )∆t 2 Rt+∆t = C bn t exp( ˆωt −b ω t −n ω t )∆t i j ∆vi j、∆pi j、∆Ri j 利用 IMU 预积分理论计算相邻 和 时刻 机器人相对位姿变化量， 分别表 示为 ∆vi j = R T i (vj −vi −g∆ti j) ∆pi j = R T i (pj − pi −vi∆ti j − 1 2 g∆t 2 i j) ∆Ri j = R T i Rj 针对车体快速转弯等运动情形，考虑在一个 优化任务里执行点云畸变补偿和点云匹配，计算 激光点采集时刻和相对于该帧点云初始时刻的姿 态变化量，在点云配准时将畸变补偿的残差作为 误差函数的一部分，对 LiDAR 位姿之间的 IMU 运动信息进行积分，并将积分得到的相对位姿于 因子图中的匹配位姿实施图优化，实验结果表 明，该方法可以进一步提高定位和建图的精度。 2.2 点云分割与特征提取 Z 本文在点云分割和特征提取阶段，考虑加入 基于区域生长的分割方法和地面提取算法，根据 被分割物体的点云个数判断其是否为稳定特征， 进而对地面提取之后剩余的点云进行聚类分析， 可以在一定程度上减小动态物体对建图的影响， 同时可以有效降低 轴方向的漂移误差。在激 光里程计模块，基于提取的特征点构建 scan-toscan 的约束关系，经过两次 Levenberg-Marquardt 优化得到姿态变换矩阵。 t Pt = {p1, p2,··· , pn} 假设在 时刻获取的激光点云为 ，VLP-16 线激光雷达的水平和垂直方向角分辨 率为 0.2°和 2°。将地面和目标分割后，在特征提 ·806· 智 能 系 统 学 报 第 15 卷
第4期 宋锐，等：基于LiDAR/NS的野外移动机器人组合导航方法 ·807· 取流程中，根据点云各点深度，计算在其同行邻 域S中的粗糙度c: min ∑d.+∑d, (4) T41 pueF c BF-vi 进一步得到x:与x1之间的旋转变化量 △T+lo 其中，粗糙度值较大的非地面点定义为边缘点 2.4多传感耦合算法 F,较小的为平面点F。但在实际应用中，由于 本文采用基于迭代误差状态卡尔曼滤波(er- 数量过大的点云数据特征会对系统实时性有一定 ror state Kalman filter,.ESKF)方法对最小化非线性 影响，因此在数据处理时仅选取若干关键帧作为 约束求解，进而更新状态，根据得到的全局位姿 优化估计的状态量。提取LDAR第i个关键帧的 信息进行地图构建。迭代ESKF在量测更新中增 特征F:={F,F},当相邻时刻机器人位姿变化超 加一步迭代优化，进而得到状态收敛时的最优 出预设阈值，则保存与机器人更新状态节点x+1 解，可以有效降低传统EKF舍弃高阶误差项带来 相关的关键帧F+1。根据实验条件，位移和旋转 的算法误差。 阈值可设置为1m和10°。当接收到新的关键帧 在基于LIO紧耦合模型，将状态估计误差定 时，利用前n+1时刻关键帧的特征集合{F-,F-+1,…, 义为δx=[80,8p,8m,δbl,δ0为角度误差，假设前一 F}和位姿估计{T-,T-+1,…,T}构建局部地图M 时刻状态为，则x,可表示为 M:={M5,Mg\ 4+:⑧exp(δ0 式中：M和Mg分别由世界坐标系下边缘特征 F-n'Fn1…,'F与平面特征Fn'Fn1…,'F} 4=h(x41,δ)= P41+δp -吹1+δv 集合构成。 -b+8b 2.3地图匹配与回环检测 其中⑧表示将角度向量变换到旋转矩阵。当 帧间匹配的算法主要有基于迭代临近点和基 MU相邻时间间隔为△：，则在迭代ESKF中，状态 于正态分布变换，针对野外大范围空旷场景特征 更新可转换为式（⑤）的优化问题： 稀少等引起的LOAM出现退化现象，参考CPFG minf( (5) (closet probability and feature grid)SLAM方法，将 基于ICP平面特征提取和基于NDT的点特征结 ll表示Mahalanobis范数，f)为基于ICP的 合，进而从雷达单帧和拼接的多帧点云数据中提 残差函数，J:为残差函数的雅克比矩阵，M为量 取得到环境特征。利用基于距离阈值判断关键帧 测噪声的协方差矩阵。求解式(6)得到ESKF的 增益方程和状态更新方程： 的方法对地图进行保存，从而保证了所建立地图 的稀疏性。在后端加入基于ICP算法的闭环检 K,=PHE(HPHE+JMJE）- 测，可以在检测到闭环时降低累积漂移误差，使 δx+1=δx,+K(Hδx,-fr) (6) 所建地图达到全局一致性的效果。 在每次迭代过程中，根据更新的匹配边缘和 针对车体快速转弯等引起点云变形等问题， 平面，进一步最小化度量误差，进而计算更新的 本文采用IMU的预积分因子与激光的scan-to- 雅克比矩阵和增益。 map两项构成优化函数，更新计算每帧激光的位 当迭代n步后终止，更新的协方差方程表示为 姿，并将MU的预测量作为优化的初值。对于边 P =(I-KHin)P(I-KHin)+KnMKi 缘特征'F中的更新点pk,以及构成边缘线的 经过ESKF迭代，根据得到6x1可以对x4 p和p,平面特征F,中的k和构成平面的 进行更新，并将其应用于下一时刻的信息更新。 p、p和p。点到边缘线的距离d。表示为 由于位姿信息是在以机器人为中心的坐标系下计 d-l-pE)xu-吃) 算得到的，经过状态合成可将其转换到全局坐标系。 (P-p, 点到平面的距离d。表示为 3实验与结果分析 d,-k-)--)x-pk 3.1实验平台 (pip)x (p-p 基于上述对移动机器人环境建模与定位研究 计算式(4)最小值并根据Gauss-.Newton方法 的分析，搭建如图2所示的野外移动机器人实验 得到最优的转换矩阵： 平台，搭载有Xsens MTi-G-710IMU和Velodyne
ri S c 取流程中，根据点云各点深度 计算在其同行邻 域 中的粗糙度 : c = 1 |S | ·∥ri∥ ∑ j∈S, j,i (rj −ri) F e i F p i i Fi = { F e i ,F p i } xi+1 Fi+1 m n+1 {Fi−n,Fi−n+1,··· , Fi} {Ti−n,Ti−n+1,··· ,Ti} Mi 其中，粗糙度值较大的非地面点定义为边缘点 ，较小的为平面点 。但在实际应用中，由于 数量过大的点云数据特征会对系统实时性有一定 影响，因此在数据处理时仅选取若干关键帧作为 优化估计的状态量。提取 LiDAR 第 个关键帧的 特征 ，当相邻时刻机器人位姿变化超 出预设阈值，则保存与机器人更新状态节点 相关的关键帧 。根据实验条件，位移和旋转 阈值可设置为 1 和 10°。当接收到新的关键帧 时，利用前 时刻关键帧的特征集合 和位姿估计 构建局部地图 Mi = { Me i , M p i } Me i M p i { ′F e i−n , ′ F p i−n+1 ,··· , ′ F e i } { ′F p i−n , ′ F p i−n+1 ,··· , ′ F p i } 式中： 和 分别由世界坐标系下边缘特征 与平面特征 集合构成。 2.3 地图匹配与回环检测 帧间匹配的算法主要有基于迭代临近点和基 于正态分布变换，针对野外大范围空旷场景特征 稀少等引起的 LOAM 出现退化现象，参考 CPFG (closet probability and feature grid) SLAM 方法，将 基于 ICP 平面特征提取和基于 NDT 的点特征结 合，进而从雷达单帧和拼接的多帧点云数据中提 取得到环境特征。利用基于距离阈值判断关键帧 的方法对地图进行保存，从而保证了所建立地图 的稀疏性。在后端加入基于 ICP 算法的闭环检 测，可以在检测到闭环时降低累积漂移误差，使 所建地图达到全局一致性的效果。 ′F e i p e i+1,k p e i,u p e i,v ′F p i+1 p p i+1,k p p i,u p p i,v p p i,w de 针对车体快速转弯等引起点云变形等问题， 本文采用 IMU 的预积分因子与激光的 scan-tomap 两项构成优化函数，更新计算每帧激光的位 姿，并将 IMU 的预测量作为优化的初值。对于边 缘特征 中的更新点 ，以及构成边缘线的 和 ，平面特征 中的 和构成平面的 、 和 。点到边缘线的距离 表示为 de = (p e i+1,k − p e i,u )×(p e i+1,k − p e i,v ) (p e i,u − p e i,v ) 点到平面的距离 dp 表示为 dp = (p p i+1,k − p p i,u )·(p p i,u − p p i,v )×(p p i,u − p p i,w ) (p p i,u − p p i,v )×(p p i,u − p p i,w ) 计算式 (4) 最小值并根据 Gauss-Newton 方法 得到最优的转换矩阵： min Ti+1 ∑ p e i+1,k ∈ ′F e i+1 de + ∑ p p i+1,k ∈ ′F p i+1 dp (4) xi xi+1 ∆Ti,i+1 进一步得到 与 之间的旋转变化量 。 2.4 多传感耦合算法 本文采用基于迭代误差状态卡尔曼滤波 (error state Kalman filter, ESKF) 方法对最小化非线性 约束求解，进而更新状态，根据得到的全局位姿 信息进行地图构建。迭代 ESKF 在量测更新中增 加一步迭代优化，进而得到状态收敛时的最优 解，可以有效降低传统 EKF 舍弃高阶误差项带来 的算法误差。 δx = [δθ,δp,δv,δb] δθ − x k k+1 x k k+1 在基于 LIO 紧耦合模型，将状态估计误差定 义为 ， 为角度误差，假设前一 时刻状态为 ，则 可表示为 x k k+1= h( − x k k+1 ,δx) = −q k k+1 ⊗exp(δθ) − p k k+1 +δp − v k k+1 +δv −b+δb ⊗ ∆t 其中 表示将角度向量变换到旋转矩阵。当 IMU 相邻时间间隔为 ,则在迭代 ESKF 中，状态 更新可转换为式 (5) 的优化问题： min δx ∥δx∥(Pk ) −1 + f(x k k+1 ) (Jk Mk J T k ) −1 (5) ∥·∥ f(·) Jk Mk 表示 Mahalanobis 范数， 为基于 ICP 的 残差函数， 为残差函数的雅克比矩阵， 为量 测噪声的协方差矩阵。求解式 (6) 得到 ESKF 的 增益方程和状态更新方程： Kk, j = PkH T k, j (Hk, jPkH T k, j + Jk, jMk J T k, j ) −1 δxj+1 = δxj + Kk, j(Hk, j δxj − f(x k k+1 )) (6) 在每次迭代过程中，根据更新的匹配边缘和 平面，进一步最小化度量误差，进而计算更新的 雅克比矩阵和增益。 当迭代 n 步后终止，更新的协方差方程表示为 Pk+1 = (I− Kk,nHk,n)Pk(I− Kk,nHk,n) T + Kk,nMkK T k,n δxj+1 x k 经过 ESKF 迭代，根据得到 可以对 k+1 进行更新，并将其应用于下一时刻的信息更新。 由于位姿信息是在以机器人为中心的坐标系下计 算得到的，经过状态合成可将其转换到全局坐标系。 3 实验与结果分析 3.1 实验平台 基于上述对移动机器人环境建模与定位研究 的分析，搭建如图 2 所示的野外移动机器人实验 平台，搭载有 Xsens MTi-G-710 IMU 和 Velodyne 第 4 期 宋锐，等：基于 LiDAR/INS 的野外移动机器人组合导航方法 ·807·
·808· 智能系统学报 第15卷 VLP-16线LiDAR,数据输出频率分别为200Hz 和10Hz。在校园开阔环境开展了移动机器人环 境建模和自主导航实验，移动机器人的大致运动 轨迹如图3所示，从红色曲线可知机器人在运动 过程中存在急转弯等情形。 Velodyne 16 屏幕 IMU 图4基于LeGO-LOAM的三维激光点云地图 Fig.4 3D LiDAR point cloud map based on LeGO-LOAM 图2野外移动机器人实验平台 Fig.2 Experimental setup of the field mobile robot 图5基于改进LeGO-LOAM的三维激光点云地图 图3校园环境移动机器人运动路线图 Fig.5 3D LiDAR point clouds map based on optimized Fig.3 The trajectory of the field mobile robot in campus LeGO-LOAM(The optimized trajectories are environment marked in blue) 3.2实验结果分析 分 从图4可知当机器人急转弯运动时，基于 0 LeGO-LOAM的三维激光点云地图出现畸变，红 LeGO-LOAM 色标识为机器人急转弯时出现的点云畸变，本文 -50 LOAM scan-map 采用IMU的预积分因子与激光的scan-to-map -100 2项构成优化函数，用于计算每帧激光的位姿，并 -150 且将MU的预测量作为优化的初值，可以保证在 急速旋转的情况下仍可以为优化提供良好的初值 20050 200 150100 50 0-50 估计，实验结果如图5所示。蓝色标识为算法优 X/m 化后的机器人路径。基于LOAM、LeGO-LOAM 图6 基于LOAM,LeGO-LOAM和scan-to-map(以scan- 和本文提出的方法对移动机器人定位结果如图6 map表示)优化方法的实验结果比较 所示，在机器人直线行驶阶段LOAM与所提方法 Fig.6 The comparison of experimental results obtained by LOAM,LeGO-LOAM and the optimized scan-to- 的计算结果近似，在如图5所示蓝色框标识区域， map(represented by scan-map)method 即机器人完成快速转弯等运动时，本文方法相对 从结果可以看出本文方法能够很好地表示机 于LOAM和LeGo-LOAM具有更好的稳定性。 器人的运动轨迹，尤其小范围内的转动运动。由
VLP-16 线 LiDAR，数据输出频率分别为 200 Hz 和 10 Hz。在校园开阔环境开展了移动机器人环 境建模和自主导航实验，移动机器人的大致运动 轨迹如图 3 所示，从红色曲线可知机器人在运动 过程中存在急转弯等情形。 Velodyne 16 IMU 屏幕 图 2 野外移动机器人实验平台 Fig. 2 Experimental setup of the field mobile robot 图 3 校园环境移动机器人运动路线图 Fig. 3 The trajectory of the field mobile robot in campus environment 3.2 实验结果分析 从图 4 可知当机器人急转弯运动时，基于 LeGO-LOAM 的三维激光点云地图出现畸变，红 色标识为机器人急转弯时出现的点云畸变，本文 采用 IMU 的预积分因子与激光的 scan-to-map 2 项构成优化函数，用于计算每帧激光的位姿，并 且将 IMU 的预测量作为优化的初值，可以保证在 急速旋转的情况下仍可以为优化提供良好的初值 估计，实验结果如图 5 所示。蓝色标识为算法优 化后的机器人路径。基于 LOAM、LeGO-LOAM 和本文提出的方法对移动机器人定位结果如图 6 所示，在机器人直线行驶阶段 LOAM 与所提方法 的计算结果近似，在如图 5 所示蓝色框标识区域， 即机器人完成快速转弯等运动时，本文方法相对 于 LOAM 和 LeGo-LOAM 具有更好的稳定性。 图 4 基于 LeGO-LOAM 的三维激光点云地图 Fig. 4 3D LiDAR point cloud map based on LeGO-LOAM 图 5 基于改进 LeGO-LOAM 的三维激光点云地图 Fig. 5 3D LiDAR point clouds map based on optimized LeGO-LOAM (The optimized trajectories are marked in blue) 50 0 −50 −100 −150 −200 Y/m X/m 250 200 150 100 50 0 −50 LeGO-LOAM LOAM scan-map 图 6 基于 LOAM, LeGO-LOAM 和 scan-to-map(以 scanmap 表示) 优化方法的实验结果比较 Fig. 6 The comparison of experimental results obtained by LOAM, LeGO-LOAM and the optimized scan-tomap (represented by scan-map) method 从结果可以看出本文方法能够很好地表示机 器人的运动轨迹，尤其小范围内的转动运动。由 ·808· 智 能 系 统 学 报 第 15 卷
第4期 宋锐，等：基于LiDAR/INS的野外移动机器人组合导航方法 ·809· 于机器人在进入小范围空间时，LOAM和LeGO SLAM based on 3D Lidar and RTK fusion[J].Manufactur LOAM的精度迅速下降，计算不同算法定位结果 ing automation,2020,42(7):51-54. 的均方根误差如表1所示，进一步验证了本文方 [5]CADENA C,CARLONE L,CARRILLO H,et al.Past, 法的有效性。 present,and future of simultaneous localization and map- ping:toward the robust-perception age[J].IEEE transac- 表1基于不同算法的实验结果均方根误差比较 tions on robotics,2016,32(6):1309-1332 Table 1 RMSE of the experimental results by different al- gorithms [6]YE H,CHEN Y,LIU M.Tightly coupled 3D lidar inertial odometry and mapping[C]//Proceedings of 2019 IEEE In- 算法 LeGO-LOAM LOAM scan-to-map ternational Conference on Robotics and Automation. X方向m 32.44 5.76 1.65 Montreal,Canada,2019:3144-3150. Y方向/m 25.54 5.04 1.03 [7]ZHANG J,SINGH S.Laser-visual-inertial odometry and mapping with high robustness and low drift[J].Journal of field robotics,2018,35(8):1242-1264 4总结与展望 [8]SHAN T,ENGLOT B.LeGO-LOAM:lightweight and 针对野外移动机器人环境建模和定位问题， ground-optimized lidar odometry and mapping on variable 本文从数据预处理、点云分割与特征提取、激光 terrain[Cl//Proceedings of 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems.Madrid. 里程计和信息融合等方法对组合系统进行分析， Spain,2018:4758-4765 结合移动机器人运动特性分析激光点云畸变等问 [9]HESS W,KOHLER D,RAPP H,et al.Real-time loop 题，提出基于IMU的预积分与scan-to-map的融合 closure in 2D LIDAR SLAM[C]//Proceedings of 2016 方法，通过构建优化函数并对每帧激光的位姿、 IEEE International Conference on Robotics and Automa- 优化初值进行更新，实验结果表明本文方法相对 tion.Stockholm,Sweden,2016:1271-1278. 于LeGO-LOAM、LOAM具有更好的环境建模和 [10]FORSTER C.CARLONE L,DELLAERT F,et al.On- 位置估计精度。此外，高精度的定位和地图构建 manifold preintegration for real-time visual-inertial odo- 对于运载体的智能行为有重要影响，地图中可能 metry[J].IEEE transactions on robotics,2017,33(1): 存在的移动物体会将建图问题进一步复杂化，下 1-21 一步考虑融合环境的语义信息，根据点云语义分 [11]BOSSE M.ZLOT R.FLICK P.Zebedee:design of a spring-mounted 3-D range sensor with application to mo- 割识别动态物体，将带有语义标签的物体进行数 bile mapping[J].IEEE transactions on robotics,2012, 据关联，利用几何信息构建约束关系，进而提高 28(5):11041119. 机器人的建图和定位精度。 [12]李帅鑫，李广云，王力，等.Lidar/IMU紧耦合的实时定 参考文献： 位方法[J/OL].自动化学报，https:/doi.org/10 16383/1.aas.c190424. [1]HIDALGO-CARRI6 J.POULAKIS P.KIRCHNER F.Ad- LI Shuaixin,LI Guangyun,WANG Li,et al.Lidar/IMU aptive localization and mapping with application to planet- tightly coupled real-time localization method[J/OL].Acta ary rovers[J].Journal of field robotics,2018,35(6): automatica sinica,https://doi.org/10.16383/j.aas.c190424. 961-987. [13]CHEN C.HE Y.GU F.et al.A Real-time relative probab- [2]YOUNG S H.MAZZUCHI T A.SARKANI S.A frame- ilistic mapping algorithm for high-speed off-road work for predicting future system performance in autonom- autonomous driving[C]//Proceedings of 2015 IEEE/RSJ ous unmanned ground vehicles[J].IEEE transactions on International Conference on Intelligent Robots and Sys- systems,man,and cybernetics:systems,2017,47(7): tems.Hamburg,Germany,2015:6252-6258. 1192-1206. [14]GENTIL CL,VIDAL-CALLEJA T,HUANG S.3D lid- [3]陈成，何玉庆，卜春光，等.基于四阶贝塞尔曲线的无人 ar-IMU calibration based on upsampled preintegrated 车可行轨迹规划[.自动化学报，2015,41(3)：486-496. measurements for motion distortion correction[Cl//Pro- CHEN Cheng,HE Yuging,BU Chunguang,et al.Feasible ceedings of 2018 IEEE International Conference on Ro- trajectory generation for autonomous vehicles based on botics and Automation.Brisbane,Australia,2018: quartic bezier curve[J].acta automatica sinica,2015,41(3): 2149-2155. 486-496. [15]GAO H.ZHANG X.YUAN J,et al.A novel global local- [4]倪志康，厉茂海，林容，等.基于三维激光雷达与RTK融 ization approach based on structural unit encoding and 合的SLAM研究)制造业自动化，2020,42(7)：51-54 multiple hypothesis tracking[J].IEEE transactions on in- NI Zhikang,LI Maohai,LIN Rui,et al.Research on strumentation and measurement,2019,68(11):
于机器人在进入小范围空间时，LOAM 和 LeGOLOAM 的精度迅速下降，计算不同算法定位结果 的均方根误差如表 1 所示，进一步验证了本文方 法的有效性。 表 1 基于不同算法的实验结果均方根误差比较 Table 1 RMSE of the experimental results by different algorithms 算法 LeGO-LOAM LOAM scan-to-map X方向/m 32.44 5.76 1.65 Y方向/m 25.54 5.04 1.03 4 总结与展望 针对野外移动机器人环境建模和定位问题， 本文从数据预处理、点云分割与特征提取、激光 里程计和信息融合等方法对组合系统进行分析， 结合移动机器人运动特性分析激光点云畸变等问 题，提出基于 IMU 的预积分与 scan-to-map 的融合 方法，通过构建优化函数并对每帧激光的位姿、 优化初值进行更新，实验结果表明本文方法相对 于 LeGO-LOAM、LOAM 具有更好的环境建模和 位置估计精度。此外，高精度的定位和地图构建 对于运载体的智能行为有重要影响，地图中可能 存在的移动物体会将建图问题进一步复杂化，下 一步考虑融合环境的语义信息，根据点云语义分 割识别动态物体，将带有语义标签的物体进行数 据关联，利用几何信息构建约束关系，进而提高 机器人的建图和定位精度。 参考文献： HIDALGO-CARRIó J, POULAKIS P, KIRCHNER F. Adaptive localization and mapping with application to planetary rovers[J]. Journal of field robotics, 2018, 35(6): 961–987. [1] YOUNG S H, MAZZUCHI T A, SARKANI S. A framework for predicting future system performance in autonomous unmanned ground vehicles[J]. IEEE transactions on systems, man, and cybernetics: systems, 2017, 47(7): 1192–1206. [2] 陈成, 何玉庆, 卜春光, 等. 基于四阶贝塞尔曲线的无人 车可行轨迹规划 [J]. 自动化学报, 2015, 41(3): 486–496. CHEN Cheng, HE Yuqing, BU Chunguang, et al. Feasible trajectory generation for autonomous vehicles based on quartic bezier curve[J]. acta automatica sinica, 2015, 41(3): 486–496. [3] 倪志康, 厉茂海, 林睿, 等. 基于三维激光雷达与 RTK 融 合的 SLAM 研究 [J]. 制造业自动化, 2020, 42(7): 51–54. NI Zhikang, LI Maohai, LIN Rui, et al. Research on [4] SLAM based on 3D Lidar and RTK fusion[J]. Manufacturing automation, 2020, 42(7): 51–54. CADENA C, CARLONE L, CARRILLO H, et al. Past, present, and future of simultaneous localization and mapping: toward the robust-perception age[J]. IEEE transactions on robotics, 2016, 32(6): 1309–1332. [5] YE H, CHEN Y, LIU M. Tightly coupled 3D lidar inertial odometry and mapping[C]//Proceedings of 2019 IEEE International Conference on Robotics and Automation. Montreal, Canada, 2019: 3144−3150. [6] ZHANG J, SINGH S. Laser-visual-inertial odometry and mapping with high robustness and low drift[J]. Journal of field robotics, 2018, 35(8): 1242–1264. [7] SHAN T, ENGLOT B. LeGO-LOAM: lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]//Proceedings of 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid, Spain, 2018: 4758−4765. [8] HESS W, KOHLER D, RAPP H, et al. Real-time loop closure in 2D LIDAR SLAM[C]//Proceedings of 2016 IEEE International Conference on Robotics and Automation. Stockholm, Sweden, 2016: 1271−1278. [9] FORSTER C, CARLONE L, DELLAERT F, et al. Onmanifold preintegration for real-time visual-inertial odometry[J]. IEEE transactions on robotics, 2017, 33(1): 1–21. [10] BOSSE M, ZLOT R, FLICK P. Zebedee: design of a spring-mounted 3-D range sensor with application to mobile mapping[J]. IEEE transactions on robotics, 2012, 28(5): 1104–1119. [11] 李帅鑫, 李广云, 王力, 等. Lidar/IMU 紧耦合的实时定 位方法 [J/OL]. 自动化学报, https://doi.org/10. 16383/j.aas.c190424. LI Shuaixin, LI Guangyun, WANG Li, et al. Lidar/IMU tightly coupled real-time localization method[J/OL]. Acta automatica sinica, https://doi.org/10.16383/j.aas.c190424. [12] CHEN C, HE Y, GU F, et al. A Real-time relative probabilistic mapping algorithm for high-speed off-road autonomous driving[C]//Proceedings of 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems. Hamburg, Germany, 2015: 6252−6258. [13] GENTIL C L, VIDAL-CALLEJA T, HUANG S. 3D lidar-IMU calibration based on upsampled preintegrated measurements for motion distortion correction[C]//Proceedings of 2018 IEEE International Conference on Robotics and Automation. Brisbane, Australia, 2018: 2149−2155. [14] GAO H, ZHANG X, YUAN J, et al. A novel global localization approach based on structural unit encoding and multiple hypothesis tracking[J]. IEEE transactions on instrumentation and measurement, 2019, 68(11): [15] 第 4 期 宋锐，等：基于 LiDAR/INS 的野外移动机器人组合导航方法 ·809·
·810· 智能系统学报 第15卷 4427-4442 taneous localization and mapping based on LIDAR in off- [16]ZHAO S,FANG Z,LI H,et al.A robust laser-inertial road environment[C]//Proceedings of 2018 IEEE Intelli- odometry and mapping method for large-scale highway gent Vehicles Symposium (IV).Changshu,China,2018: environments[C]//Proceedings of 2019 IEEE/RSJ Interna- 650-655. tional Conference on Intelligent Robots and Systems.Ma- [27]HANTEN R,KUHLMANN P,OTTE S,et al.Robust cau,China2019:1285-1292. real-time 3D person detection for indoor and outdoor ap- [17]NEUHAUS F,TILMAN K,ROBERT K,et al. plications[C]//Proceedings of 2018 IEEE International MC2SLAM:Real-time inertial lidar odometry using two- Conference on Robotics and Automation.Brisbane,Aus- scan motion compensation[C]//Proceedings of 2018 Ger- tralia,2018:2000-2006. man Conference on Pattern Recognition,Stuttgart,Ger- [28]SHAO W,VIJAYARANGAN S,LI C,et al.Stereo visu- many,2018:60-72. al inertial LiDAR simultaneous localization and [18]张雪涛，方勇纯，张雪波，等.基于误差状态卡尔曼滤波 mapping[C]//Proceedings of 2019 IEEE/RSJ Internation- 估计的旋翼无人机输入饱和控制[.机器人，2020 al Conference on Intelligent Robots and Systems.Macau, 42(4:394_405 China,2019:370-377. ZHANG Xuetao,FANG Yongchun,ZHANG Xuebo,et [29]PANG F.CHEN Z.PU L.et al.Depth enhanced visual- al.Error state Kalman filter estimator based input satur- inertial odometry based on multi-state constraint Kalman ated control for rotorcraft unmanned aerial vehicle[J].ro- bot,2020,42(4):394-405. filter[C]//Proceedings of 2017 IEEE/RSJ International [19]SHAN T.ENGLOT B,MEYERS D,et al.LIO-SAM: Conference on Intelligent Robots and Systems.Van- Tightly-coupled Lidar Inertial Odometry via Smoothing couver,Canada,2017:1761-1767. and Mapping[Cl//Proceedings of 2020 IEEE/RSJ Interna- [30]LEUTENEGGER S,LYNEN S,BOSSE M,et al.Key- tional Conference on Intelligent Robots and Systems.Las frame-based visual-inertial odometry using nonlinear op- Vegas,USA,2020:1-8. timization[J].The international journal of robotics re- [20]KAESS M.RANGANATHAN A.DELLAERT F.iSAM: search,2014,343):314-334 incremental smoothing and mapping[J].IEEE transac- 作者简介： tions on robotics,2008,24(6):1365-78. 宋锐，助理研究员，博土，主要研 [21]BLOESCH M,BURRI M,OMARI S,et al.Iterated ex- 究方向为环境感知与移动机器人 tended Kalman filter based visual-inertial odometry using SLAM。主持国家自然科学基金项目 direct photometric feedback[].The international journal 1项，发表学术论文10余篇。 of robotics research,2017.36(10):1053-72 [22]MERRIAUX P.DUPUIS Y.BOUTTEAU R,et al.Ro- bust robot localization in a complex oil and gas industrial environment[J].Journal of field robotics,2018,35(2): 方勇纯，教授，博土生导师，中国 213-30 自动化学会理事，南开大学人工智能 [23]HEMANN G,SINGH S,KAESS M.Long-range GPS- 学院院长，人工智能学会智库首批专 denied aerial inertial navigation with LIDAR localization[Cl 家。主要研究方向为机器人视觉控 Proceedings of 2016 IEEE/RSJ International Conference 制、欠驱动吊运系统控制、仿生机器人 on Intelligent Robots and Systems.Daejeon,Korea,2016: 运动控制和微纳米操作。主持国家重 1659-1666 点研发计划项目、国家基金重点项目、 [24]WAN G,YANG X,CAI R,et al.Robust and precise “十二五”国家技术支撑计划课题、国家基金仪器专项等项 vehicle localization based on multi-sensor fusion in di- 目。获吴文俊人工智能自然科学奖一等奖、天津市专利奖金 verse city scenes[C]//Proceedings of 2018 IEEE Interna- 奖、天津市自然科学一等奖、高等教育教学成果一等奖等多 tional Conference on Robotics and Automation.Brisbane, 项奖励，发表学术论文100余篇。 Australia,2018:4670-4677. 刘辉，硕士研究生，主要研究方向 [25]SHAMSELDIN T,MANERIKAR A,ELBAHNASAWY 为移动机器人SLAM。 M.et al.SLAM-based Pseudo-GNSS/INS localization system for indoor liDAR mobile mapping systems[C]// Proceedings of 2018 IEEE/ION Position,Location and Navigation Symposium.Monterey,USA,2018:197-208. [26]JI K.CHEN H.DI H.et al.CPFG-SLAM:a robust simul
4427–4442. ZHAO S, FANG Z, LI H, et al. A robust laser-inertial odometry and mapping method for large-scale highway environments[C]//Proceedings of 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems. Macau, China, 2019: 1285−1292. [16] NEUHAUS F, TILMAN K, ROBERT K, et al. MC2SLAM: Real-time inertial lidar odometry using twoscan motion compensation[C]//Proceedings of 2018 German Conference on Pattern Recognition, Stuttgart, Germany, 2018: 60−72. [17] 张雪涛, 方勇纯, 张雪波, 等. 基于误差状态卡尔曼滤波 估计的旋翼无人机输入饱和控制 [J]. 机器人, 2020, 42(4): 394–405. ZHANG Xuetao, FANG Yongchun, ZHANG Xuebo, et al. Error state Kalman filter estimator based input saturated control for rotorcraft unmanned aerial vehicle[J]. robot, 2020, 42(4): 394–405. [18] SHAN T, ENGLOT B, MEYERS D, et al. LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping[C]//Proceedings of 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems. Las Vegas, USA, 2020: 1−8. [19] KAESS M, RANGANATHAN A, DELLAERT F. iSAM: incremental smoothing and mapping[J]. IEEE transactions on robotics, 2008, 24(6): 1365–78. [20] BLOESCH M, BURRI M, OMARI S, et al. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback[J]. The international journal of robotics research, 2017, 36(10): 1053–72. [21] MERRIAUX P, DUPUIS Y, BOUTTEAU R, et al. Robust robot localization in a complex oil and gas industrial environment[J]. Journal of field robotics, 2018, 35(2): 213–30. [22] HEMANN G, SINGH S, KAESS M. Long-range GPSdenied aerial inertial navigation with LIDAR localization[C]// Proceedings of 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems. Daejeon, Korea, 2016: 1659−1666. [23] WAN G, YANG X, CAI R, et al. Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes[C]//Proceedings of 2018 IEEE International Conference on Robotics and Automation. Brisbane, Australia, 2018: 4670−4677. [24] SHAMSELDIN T, MANERIKAR A, ELBAHNASAWY M, et al. SLAM-based Pseudo-GNSS/INS localization system for indoor liDAR mobile mapping systems[C]// Proceedings of 2018 IEEE/ION Position, Location and Navigation Symposium. Monterey, USA, 2018: 197−208. [25] [26] JI K, CHEN H, DI H, et al. CPFG-SLAM: a robust simultaneous localization and mapping based on LIDAR in offroad environment[C]//Proceedings of 2018 IEEE Intelligent Vehicles Symposium (IV). Changshu, China, 2018: 650−655. HANTEN R, KUHLMANN P, OTTE S, et al. Robust real-time 3D person detection for indoor and outdoor applications[C]//Proceedings of 2018 IEEE International Conference on Robotics and Automation. Brisbane, Australia, 2018: 2000−2006. [27] SHAO W, VIJAYARANGAN S, LI C, et al. Stereo visual inertial LiDAR simultaneous localization and mapping[C]//Proceedings of 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems. Macau, China, 2019: 370−377. [28] PANG F, CHEN Z, PU L, et al. Depth enhanced visualinertial odometry based on multi-state constraint Kalman filter[C]//Proceedings of 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vancouver, Canada, 2017: 1761−1767. [29] LEUTENEGGER S, LYNEN S, BOSSE M, et al. Keyframe-based visual-inertial odometry using nonlinear optimization[J]. The international journal of robotics research, 2014, 34(3): 314–334. [30] 作者简介： 宋锐，助理研究员，博士，主要研 究方向为环境感知与移动机器人 SLAM。主持国家自然科学基金项目 1 项，发表学术论文 10 余篇。 方勇纯，教授，博士生导师，中国 自动化学会理事，南开大学人工智能 学院院长，人工智能学会智库首批专 家。主要研究方向为机器人视觉控 制、欠驱动吊运系统控制、仿生机器人 运动控制和微纳米操作。主持国家重 点研发计划项目、国家基金重点项目、 “十二五”国家技术支撑计划课题、国家基金仪器专项等项 目。获吴文俊人工智能自然科学奖一等奖、天津市专利奖金 奖、天津市自然科学一等奖、高等教育教学成果一等奖等多 项奖励，发表学术论文 100 余篇。 刘辉，硕士研究生，主要研究方向 为移动机器人 SLAM。 ·810· 智 能 系 统 学 报 第 15 卷