对于野外移动机器人，利用实时定位与地图(simultaneous localization and mapping, SLAM)技术和多源感知信息建立三维环境模型和自主定位，是移动机器人在未知环境中自主导航的关键。野外环境地形复杂、特征稀疏等因素对传统视觉测量有不利影响，而基于三维激光雷达(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 ground-optimized 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)回环检测，利用间隔关键帧构成的小回环为转角提供更多约束，可用于提升激光里程计的鲁棒性，而大回环则可以通过建立闭环约束并修正全局累计误差；5)基于紧耦合的数据融合，包含LiDAR里程计、IMU预积分和地图配准等，进而得到机器人全局一致的位姿。本文所提算法的流程图如图1所示，其中标号为文中对应公式。

通过融合LiDAR点云描述和机器人运动学信息，进一步分析移动平台颠簸对LiDAR点云畸变和视觉图像模糊的影响。具体方案如下：

1) 针对机器人的姿态变化引起的点云图像扰动，采用组合导航系统信息对坐标转换中的旋转矩阵 ${{R}}$ 和平移 ${{T}}$ 进行初始化，当导航坐标系、载体坐标系和第 $i$ 个传感器坐标系分别表示为{n}、{b}、{S_i}，则环境传感器测量点 $ {P}^{{S}_{i}} $ 转换到导航系可以表示为式(1)：

${n_p} = f(\varPsi ,\varPhi ,{P^{{S_i}}}) = {{R}}_b^n({{R}}_{{s_i}}^b{{{P}}^{{S_i}}} + {{T}}_{{s_i}}^b) + {{T}}_b^n$

(1)

式中： $\varPsi ,\varPhi $ 分别为机器人在导航坐标 $ \left\{n\right\},\left\{b\right\},\left\{{S}_{i}\right\} $ 系下的位姿和第 $ i $ 传感器相对于 $b$ 系的位姿。

2) 对三维点云数据的不确定性和量测点误差进行研究并建立对应误差模型，将量测点投影到二维平面，则其分布在栅格 ${{\rm{g}} _{ij}}$ 的概率可用黎曼分布近似表示为式(2)：

$\begin{array}{c} P\left( {({n_{{p_x}}},{n_{{p_y}}}) \in {{\rm{g}} _{ij}}} \right) \approx \\ ({x_{i + }} - {x_{i - }})({y_{i + }} - {y_{i - }}) \cdot P\left( {\dfrac{{{x_{i + }} + {x_{i - }}}}{2},\dfrac{{{y_{i + }} + {y_{i - }}}}{2}} \right) \\ \end{array} $

(2)

利用式(3)对多层环境栅格地图的每个栅格平均高程值进行更新：

${\hat \xi _N} = \frac{{\displaystyle\sum\nolimits_{n = 1}^N {{P_n}{{\hat e}_n}} }}{{\displaystyle\sum\nolimits_{n = 1}^N {{P_n}} }}$

(3)

式中： ${P_n}$ 为第n高程值在栅格内的概率； ${\hat e_n}$ 为高程的均值。

3) 针对环境特征点稀少引起的近似误差增大和目标函数陷入极小值点的问题，可以采用闭环检测等方式抑制漂移的影响。在此基础上，利用高斯过程模型对LiDAR运动速度进行估计并利用惯性信息修正位姿，进而对环境模型更新进行优化，实现机器人环境特征的完整映射。

2 LiDAR/INS数据融合方法 2.1 数据预处理

IMU在 $t$ 时刻的角速度和加速度观测模型可表示为

$ \begin{array}{c} {\hat \omega _t} = {\omega _t} + b_t^\omega + n_t^\omega \\ {\hat a_t} = {{C}}_t^{bn}({a_t} - g) + b_t^a + n_t^a \end{array} $

式中： ${\hat \omega _t}$ 和 ${\hat a_t}$ 分别为IMU原始数据； $b_t^\omega $ 、 $b_t^a$ 和 $n_t^\omega $ 、 $n_t^a$ 分别表示对应输出信号中的漂移和高斯白噪声； ${{C}}_t^{bn}$ 为坐标转换矩阵。移动机器人状态定义为

${{{x}}_i} = \left[ {\begin{array}{*{20}{c}} {{{{R}}_i}}&{{{{p}}_i}}&{{{{v}}_i}}&{{b_i}} \end{array}} \right]$

式中： ${{{R}}_i} \in SO(3)$ 为旋转矩阵； ${{{p}}_i} \in {\rm{R} ^3}$ 表示位置向量； ${{{v}}_i}$ 和 ${b_i}$ 分别为载体速度和IMU漂移； ${{T}} \in SE(3)$ 表示从载体系到导航系的转换矩阵，即 ${{T}} = \left[ {{{{R}}_i} {{{p}}_i} } \right]$ 。

基于上述分析，移动机器人在 $t + \Delta t$ 时刻的速度、位置和旋转可以分别表示为

${{{v}}_{t + \Delta t}} = {{{v}}_t}{{ + g}}\Delta t{\rm{ + }}{{C}}_t^{bn}({\hat a_t} - b_t^a - n_t^a)\Delta t$

${{{p}}_{t + \Delta t}} = {{{p}}_t}{\rm{ + }}{{{v}}_t}\Delta t{\rm{ + }}\frac{1}{2}{{g}}\Delta {t^2}{\rm{ + }}\frac{1}{2}{{C}}_t^{bn}({\hat a_t} - b_t^a - n_t^a)\Delta {t^2}$

${{{R}}_{t + \Delta t}} = {{C}}_t^{bn}\exp ({\hat \omega _t} - b_t^\omega - n_t^\omega )\Delta t$

利用IMU预积分理论计算相邻 $i$ 和 $j$ 时刻机器人相对位姿变化量， $\Delta {{{v}}}_{ij}{\text{、}}\Delta {{{p}}}_{ij}{\text{、}}\Delta {{{R}}}_{ij}$ 分别表示为

$\begin{array}{c} \Delta {{{v}}_{ij}} = {{R}}_i^{\rm{T}}({{{v}}_j} - {{{v}}_i} - g\Delta {t_{ij}}) \\ \Delta {{{p}}_{ij}} = {{R}}_i^{\rm{T}}({{{p}}_j} - {{{p}}_i} - {{{v}}_i}\Delta {t_{ij}} - \frac{1}{2}g\Delta t_{ij}^2) \\ \Delta {{{R}}_{ij}} = {{R}}_i^{\rm{T}}{{R}}_j^{} \\ \end{array} $

针对车体快速转弯等运动情形，考虑在一个优化任务里执行点云畸变补偿和点云匹配，计算激光点采集时刻和相对于该帧点云初始时刻的姿态变化量，在点云配准时将畸变补偿的残差作为误差函数的一部分，对LiDAR位姿之间的IMU运动信息进行积分，并将积分得到的相对位姿于因子图中的匹配位姿实施图优化，实验结果表明，该方法可以进一步提高定位和建图的精度。

2.2 点云分割与特征提取

本文在点云分割和特征提取阶段，考虑加入基于区域生长的分割方法和地面提取算法，根据被分割物体的点云个数判断其是否为稳定特征，进而对地面提取之后剩余的点云进行聚类分析，可以在一定程度上减小动态物体对建图的影响，同时可以有效降低 $Z$ 轴方向的漂移误差。在激光里程计模块，基于提取的特征点构建scan-to-scan的约束关系，经过两次Levenberg-Marquardt优化得到姿态变换矩阵。

假设在 $t$ 时刻获取的激光点云为 ${{{P}}_t} = \{ {p_1},{p_2}, \cdots , {p_n} \}$ ，VLP-16线激光雷达的水平和垂直方向角分辨率为0.2°和2°。将地面和目标分割后，在特征提取流程中，根据点云各点深度 ${r_i}$ 计算在其同行邻域 $S$ 中的粗糙度 $c$ :

$c = \frac{1}{{\left| S \right| \cdot \left\| {{r_i}} \right\|}}\left\| {\sum\limits_{j \in S,j \ne i} {({r_j} - {r_i})} } \right\|$

其中，粗糙度值较大的非地面点定义为边缘点 $F_i^e$ ，较小的为平面点 $F_i^p$ 。但在实际应用中，由于数量过大的点云数据特征会对系统实时性有一定影响，因此在数据处理时仅选取若干关键帧作为优化估计的状态量。提取LiDAR第 $i$ 个关键帧的特征 ${{{F}}_i} = \left\{ {F_i^e,F_i^p} \right\}$ ，当相邻时刻机器人位姿变化超出预设阈值，则保存与机器人更新状态节点 ${x_{i + 1}}$ 相关的关键帧 ${{{F}}_{i{\rm{ + }}1}}$ 。根据实验条件，位移和旋转阈值可设置为1 ${\rm{m}}$ 和10°。当接收到新的关键帧时，利用前 $n + 1$ 时刻关键帧的特征集合 $\{ {{{F}}_{i - n}},{{{F}}_{i - n{\rm{ + }}1}} ,\cdots , {{{F}}_i}\}$ 和位姿估计 $\left\{ {{{{T}}_{i - n}},{{{T}}_{i - n{\rm{ + }}1}} ,\cdots ,{{{T}}_i}} \right\}$ 构建局部地图 ${{{M}}_i}$

${{{M}}_i} = \left\{ {M_i^e,M_i^p} \right\}$

式中： $M_i^e$ 和 $M_i^p$ 分别由世界坐标系下边缘特征 $\left\{ {' F_{i - n}^e,^\prime F_{i-n+1}^p, \cdots ,^\prime F_i^e} \right\}$ 与平面特征 $\left\{ {^\prime F_{i - n}^p,^\prime F_{i - n+1}^p, \cdots ,^\prime F_i^p} \right\}$ 集合构成。

2.3 地图匹配与回环检测

帧间匹配的算法主要有基于迭代临近点和基于正态分布变换，针对野外大范围空旷场景特征稀少等引起的LOAM出现退化现象，参考CPFG (closet probability and feature grid) SLAM方法，将基于ICP平面特征提取和基于NDT的点特征结合，进而从雷达单帧和拼接的多帧点云数据中提取得到环境特征。利用基于距离阈值判断关键帧的方法对地图进行保存，从而保证了所建立地图的稀疏性。在后端加入基于ICP算法的闭环检测，可以在检测到闭环时降低累积漂移误差，使所建地图达到全局一致性的效果。

针对车体快速转弯等引起点云变形等问题，本文采用IMU的预积分因子与激光的scan-to-map两项构成优化函数，更新计算每帧激光的位姿，并将IMU的预测量作为优化的初值。对于边缘特征 $^\prime F_i^e$ 中的更新点 $p_{i + 1,k}^e$ ，以及构成边缘线的 $p_{i,u}^e$ 和 $p_{i,v}^e$ ，平面特征 $^\prime F_{i + 1}^p$ 中的 $p_{i + 1,k}^p$ 和构成平面的 $p_{i,u}^p$ 、 $p_{i,v}^p$ 和 $p_{i,w}^p$ 。点到边缘线的距离 ${d_e}$ 表示为

${d_e} = \frac{{\left| {(p_{i + 1,k}^e - p_{i,u}^e) \times (p_{i + 1,k}^e - p_{i,v}^e)} \right|}}{{(p_{i,u}^e - p_{i,v}^e)}}$

点到平面的距离 ${d_p}$ 表示为

${d_p} = \frac{{\left| {(p_{i + 1,k}^p - p_{i,u}^p) \cdot (p_{i,u}^p - p_{i,v}^p) \times (p_{i,u}^p - p_{i,w}^p)} \right|}}{{\left| {(p_{i,u}^p - p_{i,v}^p) \times (p_{i,u}^p - p_{i,w}^p)} \right|}}$

计算式(4)最小值并根据Gauss-Newton方法得到最优的转换矩阵：

$\mathop {{\rm{min}}}\limits_{{T_{i + 1}}} \left\{ {\sum\limits_{p_{i + 1,k}^e \in ^\prime F_{i + 1}^e} {{d_e} + \sum\limits_{p_{i + 1,k}^p \in ^\prime F_{i + 1}^p} {{d_p}} } } \right\}$

(4)

进一步得到 ${{{x}}_i}$ 与 ${{{x}}_{i{\rm{ + }}1}}$ 之间的旋转变化量 $\Delta {{{T}}_{i,i + 1}}$ 。

2.4 多传感耦合算法

本文采用基于迭代误差状态卡尔曼滤波(error state Kalman filter, ESKF)方法对最小化非线性约束求解，进而更新状态，根据得到的全局位姿信息进行地图构建。迭代ESKF在量测更新中增加一步迭代优化，进而得到状态收敛时的最优解，可以有效降低传统EKF舍弃高阶误差项带来的算法误差。

在基于LIO紧耦合模型，将状态估计误差定义为 ${\text{δ}} {{x}} = [{\text{δ}} \theta ,{\text{δ}} p,{\text{δ}} v,{\text{δ}} b]$ ， ${\text{δ}} \theta$ 为角度误差，假设前一时刻状态为 ${}^ - {{x}}_{{{k}} + 1}^{{k}}$ ，则 ${{x}}_{{{k}} + 1}^{{k}}$ 可表示为

${{x}}_{k + 1}^k{{ = h(}}{}^ - {{x}}_{k + 1}^k{\rm{,}}{\text{δ}} {{x}}{\rm{)}} = \left[ {\begin{array}{*{20}{c}} {{}^ - q_{k + 1}^k \otimes \exp ({\text{δ}} \theta )} \\ {{}^ - p_{k + 1}^k + {\text{δ}} p} \\ {{}^ - v_{k + 1}^k + {\text{δ}} v} \\ {{}^ - b + {\text{δ}} b} \end{array}} \right]$

其中 $ \otimes $ 表示将角度向量变换到旋转矩阵。当IMU相邻时间间隔为 $\Delta t$ ,则在迭代ESKF中，状态更新可转换为式(5)的优化问题：

$\mathop {\min }\limits_{{\text{δ}} {{x}}} {\left\| {{\text{δ}} {{x}}} \right\|_{{{({P_k})}^{ - 1}}}} + {\left\| {f({{x}}_{k{\rm{ + }}1}^k)} \right\|_{{{({{{J}}_k}{{{M}}_k}{{J}}_k^{\rm{T}})}^{ - 1}}}}$

(5)

$\left\| \cdot \right\|$ 表示Mahalanobis范数， $f( \cdot )$ 为基于ICP的残差函数， ${{{J}}_k}$ 为残差函数的雅克比矩阵， ${{{M}}_k}$ 为量测噪声的协方差矩阵。求解式(6)得到ESKF的增益方程和状态更新方程：

${{{K}}_{k,j}} = {{{P}}_k}{{H}}_{k,j}^{\rm{T}}{({{H}}_{k,j}^{}{{{P}}_k}{{H}}_{k,j}^{\rm{T}} + {{{J}}_{k,j}}{{{M}}_k}{{J}}_{k,j}^{\rm{T}})^{ - 1}}$

${\text{δ}} {{{x}}_{j + 1}} = {\text{δ}} {{{x}}_j} + {{{K}}_{k,j}}({{H}}_{k,j}^{}{\text{δ}} {{{x}}_j} - f({{x}}_{k + 1}^k))$

(6)

在每次迭代过程中，根据更新的匹配边缘和平面，进一步最小化度量误差，进而计算更新的雅克比矩阵和增益。

当迭代n步后终止，更新的协方差方程表示为

$\begin{array}{c} {{{P}}_{k + 1}} = ({{I}} - {{{K}}_{k,n}}{{{H}}_{k,n}}){{{P}}_k}{({{I}} - {{{K}}_{k,n}}{{{H}}_{k,n}})^{\rm{T}}} + {{{K}}_{k,n}}{{{M}}_k}{{K}}_{k,n}^{\rm{T}} \end{array} $

经过ESKF迭代，根据得到 $ \delta {{{x}}}_{j+1} $ 可以对 ${{x}}_{k + 1}^k$ 进行更新，并将其应用于下一时刻的信息更新。由于位姿信息是在以机器人为中心的坐标系下计算得到的，经过状态合成可将其转换到全局坐标系。

3 实验与结果分析 3.1 实验平台

基于上述对移动机器人环境建模与定位研究的分析，搭建如图2所示的野外移动机器人实验平台，搭载有Xsens MTi-G-710 IMU 和Velodyne VLP-16线LiDAR，数据输出频率分别为200 Hz和10 Hz。在校园开阔环境开展了移动机器人环境建模和自主导航实验，移动机器人的大致运动轨迹如图3所示，从红色曲线可知机器人在运动过程中存在急转弯等情形。

3.2 实验结果分析

从图4可知当机器人急转弯运动时，基于LeGO-LOAM的三维激光点云地图出现畸变，红色标识为机器人急转弯时出现的点云畸变，本文采用IMU的预积分因子与激光的scan-to-map 2项构成优化函数，用于计算每帧激光的位姿，并且将IMU的预测量作为优化的初值，可以保证在急速旋转的情况下仍可以为优化提供良好的初值估计，实验结果如图5所示。蓝色标识为算法优化后的机器人路径。基于LOAM、LeGO-LOAM和本文提出的方法对移动机器人定位结果如图6所示，在机器人直线行驶阶段LOAM与所提方法的计算结果近似，在如图5所示蓝色框标识区域，即机器人完成快速转弯等运动时，本文方法相对于LOAM和LeGo-LOAM具有更好的稳定性。

从结果可以看出本文方法能够很好地表示机器人的运动轨迹，尤其小范围内的转动运动。由于机器人在进入小范围空间时，LOAM和LeGO-LOAM的精度迅速下降，计算不同算法定位结果的均方根误差如表1所示，进一步验证了本文方法的有效性。

4 总结与展望

针对野外移动机器人环境建模和定位问题，本文从数据预处理、点云分割与特征提取、激光里程计和信息融合等方法对组合系统进行分析，结合移动机器人运动特性分析激光点云畸变等问题，提出基于IMU的预积分与scan-to-map的融合方法，通过构建优化函数并对每帧激光的位姿、优化初值进行更新，实验结果表明本文方法相对于LeGO-LOAM、LOAM具有更好的环境建模和位置估计精度。此外，高精度的定位和地图构建对于运载体的智能行为有重要影响，地图中可能存在的移动物体会将建图问题进一步复杂化，下一步考虑融合环境的语义信息，根据点云语义分割识别动态物体，将带有语义标签的物体进行数据关联，利用几何信息构建约束关系，进而提高机器人的建图和定位精度。