-
摘要: 本文以实现移动小型智能化系统的实时自主定位为目标, 针对激光里程计误差累计大, 旋转估计不稳定, 以及观测信息利用不充分等问题, 提出一种LiDAR/IMU紧耦合的实时定位方法 — Inertial-LOAM. 数据预处理部分, 对IMU数据预积分, 降低优化变量维度, 并为点云畸变校正提供参考. 提出一种基于角度图像的快速点云分割方法, 筛选结构性显著的点作为特征点, 降低点云规模, 保证激光里程计的效率; 针对地图构建部分存在的地图匹配点搜索效率低和离散点云地图的不完整性问题, 提出传感器中心的多尺度地图模型, 利用环形容器保持地图点恒定, 并结合多尺度格网保证地图模型中点的均匀分布. 数据融合部分, 提出LiDAR/IMU紧耦合的优化方法, 将IMU和LiDAR构成的预积分因子、配准因子、闭环因子插入全局因子图中, 采用基于贝叶斯树的因子图优化算法对变量节点进行增量式优化估计, 实现数据融合. 最后, 采用实测数据评估Inertial-LOAM的性能并与LeGO-LOAM, LOAM和Cartographer对比. 结果表明, Inertial-LOAM在不明显增加运算负担的前提下大幅降低连续配准误差造成的误差累计, 具有良好的实时性; 在结构性特征明显的室内环境, 定位精度达厘米级, 与对比方法持平; 在开阔的室外环境, 定位精度达分米级, 而对比方法均存在不同程度的漂移.Abstract: Aiming at the problem of the high-drift of LiDAR odometry, the unstability of purely rotation estimation, and the underutilization of sensor observation, a LiDAR/IMU tightly coupled real-time localization method, Inertial-LOAM, is presented in the paper, to achieve real-time and autonomous localization for small intelligence system, such as mobile robot. In data preprocessing section, we conduct IMU pre-integration to address the rapid growth of the number of variables in the optimizer and provide reference for point cloud deskewing. A fast point cloud segmentation approach based on angle image is proposed to screen out featured points and downsize the magnitude of point cloud, which increase the accuracy and efficiency of LiDAR odometry. In mapping section, a sensor-centered multi-resolution map model is proposed to solve the problem of the costly searching for correspondences among local map, and the incomplete representation of accumulated point cloud map. We keep the number of points in local map constant by restoring them in a circular vector, and use multi-resolution grids to keep map points uniform, which promote the increasing of efficiency and accuracy of scan-to-map registration. In data fusion section, we present a LiDAR/IMU tightly coupled optimization method by adding pre-integration factors, registration factors and loop closure factors into the global factor graph, and applying optimization algorithm based on Bayesian tree to estimate all variable nodes, which achieves deeply integration of multi-source information. Finally, we evaluate and compare the performance of Inertial-LOAM with that of LeGO-LOAM, LOAM, and Cartographer using test data gathered by mobile robot. It shows that Inertial-LOAM, with the help of optimization back-end, could dramatically reduce the impact from wrong registrations of point cloud without increasing calculation burden, and has good real time performance. It can achieve centimeter-level positioning accuracy in structured indoor environments, which is similar to other counterparts. And decimeter-level positioning accuracy could be achieved in open and structureless outdoor environments, while all other solutions have different extent of drift.
-
Key words:
- SLAM /
- LiDAR odometry /
- pre-integration /
- data fusion /
- factor graph optimization
-
载体利用所搭载传感器的观测信息, 在未知环境中进行实时自主定位并增量式构建环境地图的能力是无人系统的关键技术之一[1]. 激光雷达(LiDAR)因其分辨率高、抗干扰强、不受光照条件影响等特点被广泛应用于无人系统中, 被称为“无人车的眼睛”. 惯性导航技术(Inertial navigation system, INS)是一种不受外界因素干扰的自主导航手段, 可以在卫星导航信号失锁情况下提供载体的位姿信息. 因此将二者结合实现自主定位与地图构建已成为学术界与工业界的共识. 然而, 对于诸如无人机、智能机器人等运算资源有限的小型智能系统, 实现实时的6自由度(Degree of freedom, DoF)位姿估计并增量式构建环境地图是十分困难的. 一方面, 由于定位与地图构建相互依赖. 位姿估计可以为地图构建提供必要的先验信息, 而对已构建地图的重复观测又可以消除位姿估计的不确定度[1], 二者的交织使得实时定位与地图构建(Simultaneous localization and mapping, SLAM)问题具有极高的复杂度. 另一方面, 由于数据流中包含了数据量庞大的点云, 以及高频采样的惯性测量单元(Inertial measurement unit, IMU)数据, 如何高效且充分地融合点云与IMU数据也是一个难点.
点云配准是利用连续点云进行位姿推估的核心方法, 迭代最邻近点算法(Iterative closest point, ICP)[2-3]是应用最为广泛的点云配准算法. Surmann等将2D LiDAR安装在编码器和转台上获取三维点云, 并采用ICP算法估计相LiDAR的运动[4]. Moosmann等提出基于3D LiDAR和ICP的解决方案, 介绍了一种基于匀速运动模型的点云扭曲的补偿方法并对补偿结果进行了分析[5]. 由于这两种方法均采用ICP算法直接对大规模点云进行处理, 配准效率低, 实时性差; 另外, 这种增量式递推方法缺少对点云配准误差累计的优化后端. Droeschel等提出多分辨率格网地图(Multi-resolution, MRS)的环境表达方式, 和一种基于点云表面元模型的概率配准算法[6], 并在此基础上提出了一种分层优化的后端图优化策略[7], 实现对连续轨迹和地图的优化. 但由于该方法侧重强调地图构建的一致性, 因此将地图作为变量节点一并纳入后端优化中, 运算量较大, 在小型智能系统上难以达到实时性. Zhang等提出的激光雷达里程计与地图构建方法(LiDAR odometry and mapping, LOAM)[8-9]代表该领域的最高水准, 该方法在KITTI (Karlsruhe Institute of Technology and Toyota Technological Institute)[10]评测中排名第二1. LOAM采用点到边和点到面的联合配准方法估计LiDAR的相对运动; 为保证实时性, 在两线程上分别运行高频的激光里程计模块和低频的地图构建模块, 低频线程接收高频线程的位姿估计结果. Shan等在[11]中指出LOAM在计算资源有限的条件下效果大大下降, 并基于此提出了面向无人车(Unmanned ground vehicle, UGV)的轻量级LOAM方法(Lightweight and ground-optimized LOAM, LeGO-LOAM). 该方法增加点云分割处理模块降低点云规模, 采用两步法配准点云, 并增加闭环检测线程实现对累计误差的在线修正. 但这两种方法都只是利用IMU提供的姿态信息为点云配准和点云畸变消除提供运动估计的先验信息, 在优化计算时并未将其作为观测约束进行整体优化, 数据利用不够充分. 此外, 仅依靠点云配准进行位姿推估和地图构建在实际应用中误差累计较大, 且极易出现配准错误, 尤其在较为缺乏结构性特征的室外环境下或LiDAR快速旋转时. 这对缺乏重定位能力的系统而言是致命的, 将导致定位和地图构建的失败. Google 2016年发布的开源SLAM库Cartographer[12]采用一种分层优化的思路. 前端采用无损卡尔曼滤波(Unscented Kalman filter, UKF)算法融合多源数据进行位姿推估并构建子地图. 后端以子地图为基本单元构建优化问题, 并提出分支界定算法加速子地图间约束的构建. 该系统仅在前端进行了较为松散的数据耦合, 而在后端优化时仍是仅以点云的匹配为约束, 数据融合不够紧密.
多传感器数据紧融合算法整体可分为滤波算法和平滑算法两类[13-14]. 滤波算法[15-16]数据处理简单, 实时性强, 在工业界得到了广泛应用. 但由于其本身的递推性质, 线性化误差将不断累积, 导致精度下降. 平滑算法包括全平滑(Full smoothing)和固定滞延平滑(Fixed-lag smoothing)两种. 全平滑[17]对所有变量整体优化, 精度高, 但在SLAM问题中随着轨迹扩张变量个数激增, 无法满足实时性. 固定滞延平滑[18-19]是一种滤波和全优化的折中方式, 它在时间轴上设置一个随时间滑动的固定窗口, 每次仅优化窗口内的变量, 并边缘化其余变量. 由于优化迭代时所有变量都将被重新线性化, 因此线性化累计误差较小, 精度得到保证; 同时由于窗口大小固定, 优化变量个数基本不变, 实时性可以满足. 该算法的缺点是变量边缘化时协方差矩阵变稠密, 这一定程度上增加了计算负担, 影响了计算效率. Kaess等用因子图(Fator graph)表示变量间的关系, 提出一种增量式平滑的全优化算法[20-21], 代表当前最先进的优化方法. 该方法将因子图保存在贝叶斯树中[22], 当有新的因子节点加入时, 识别被影响的变量节点并仅对它们进行优化更新, 从而维持全优化的稀疏特性. 目前基于因子图全优化算法的IMU数据紧融合研究主要集中在相机与IMU的组合中[23-24], 而在LiDAR与IMU的组合中还鲜有涉及.
综上所述, 针对小型智能系统的实时定位研究具有重要的现实意义, LiDAR/IMU的紧耦合方案仍需进一步探讨. 本文在LOAM[9]和LeGO-LOAM[11]的基础上并结合前期工作[25], 提出一种在未知环境下, 基于LiDAR/IMU紧耦合的实时定位方法 — Inertial-LOAM, 通过在预处理、配准和后端优化等多层次的数据融合, 实现多源数据的紧密耦合, 降低轨迹漂移, 增强系统在室外开阔环境和快速转动时的鲁棒性. 实验结果表明本文方法可以满足小型智能的实时定位需求, 定位性能显著提升.
1. 系统概述
系统接收3D LiDAR的原始点云和IMU输出的角速度及加速度数据, 输出连续位姿估计和环境地图. 需要说明的是, LiDAR/IMU的外参数需预先标定, 在后续数据处理中作为已知量. 传感器需时间对齐, 一般可由IMU向LiDAR输出PPS(Pulse per second)脉冲和推荐定位信息(GPRMC), 实现同步触发; 也可利用IMU的高频输出特性, 对带有时间戳的数据进行滤波和内插实现.
系统整体分为五个部分: 1) 数据预处理. 将原始点云投影为深度图像, 并进行快速的地面点及目标分割剔除野点, 并从分割后的点云中提取特征点; 同时, 利用IMU积分得到的相对运动估值对特征点进行畸变校正; 2) 激光里程计. 将连续时刻的特征点配准, 估计LiDAR相对运动, 并据此再次校正特征点; 3) 地图构建. 将校正后的特征点与局部地图配准, 优化位姿估计结果, 并更新局部地图; 4) 闭合回环. 检测轨迹是否闭合, 并将闭合处的点云配准结果作为闭环约束关系; 5) 因子图优化. 系统维护一个全局的因子图, 各模块向因子图中插入IMU预积分因子、配准因子和闭环因子, 每当插入新的因子节点, 优化计算一次. 为保证实时性, 系统在三个线程上并行运行, 其中预处理和激光里程计占用主线程, 地图构建和闭合回环各占一个分线程. Inertial-LOAM系统框架图如图1.
2. LiDAR/IMU数据融合方法
2.1 基于因子图优化的状态估计
载体坐标系定义为
$ {{\rm{B}}} $ , 与IMU坐标系保持一致; 世界坐标系定义为$ {{\rm{W}}} $ , 原点为系统初始化时的载体系中心,$ {{z}} $ 轴方向与世界坐标系下的重力方向对齐; 假设LiDAR第$ i $ 次扫描的起始时刻为$ {t_i} $ , 扫描得到的全部点云为$ {{\cal P}_i} $ , 其中任意一点记为$ {}_{\mathop{{{\rm{B}}}}\nolimits} {{{p}}_n} \in {{\cal P}_i} $ ; IMU在$ \left[ {{t_i},{t_{i + 1}}} \right] $ 内采集的数据为$ {{\cal I}_{\left( {i,i{\rm{ + }}1} \right)}} $ , 由于IMU输出频率高于LiDAR,$ {{\cal I}_{\left( {i,i{\rm{ + }}1} \right)}} $ 中包含了$ N $ 组$ {{\rm{B}}} $ 下的加速度和角速度$\left[ {{}_{\mathop{{{\rm{B}}}}\nolimits} {\tilde{ a}}\left( {t_i^n} \right),{}_{\mathop{{{\rm{B}}}}\nolimits} {{{\tilde{ \omega }}}_{{\rm{WB}}}}\left( {t_i^n} \right)} \right],n \in {\bf N}$ . 系统在$ {t_i} $ 时刻的状态包括姿态, 位置, 速度和IMU零偏$$ {{x}}_i = \left[ {{{{R}}_i},{{{t}}_i},{{{v}}_i},{{{b}}_i}} \right] $$ (1) 其中, 位姿变换构成特殊欧氏群[26]
$ \left[ {{{{R}}_i},{{{t}}_i}} \right] \in SE\left( 3 \right) $ ; 速度为欧氏空间下的三维向量${{{v}}_i} \in {{{{\bf{R}}}}^3}$ ; 零偏项由陀螺仪零偏${{b}}_i^g \in {{{{\bf{R}}}}^3}$ 和加速度计零偏${{b}}_i^a \in {{{{\bf{R}}}}^3}$ 构成${{{b}}_i} =$ $\left[ {{{b}}_i^g,{{b}}_i^a} \right] \in {{{\bf{R}}}^6}$ . 需要说明的是, 本文侧重于研究系统的定位方法, 因此地图点不作为状态量进行优化, 以保证后端优化的运算效率. 此外, 所有LiDAR的状态量过于庞大, 且部分状态量十分冗余(如: LiDAR静止), 若将其全部作为状态量进行优化, 一方面很快就会超出计算机的运算能力, 另一方面也会造成运算资源浪费. 因此, 在数据处理中仅选取具有代表性的关键帧作为优化估计的状态量. 定义$ {{\cal K}_k} $ 为$ {t_k} $ 时刻的所有关键帧, 其对应的状态量为${{\cal X}_k} = {\left\{ {{{{{x}}}_i}} \right\}_{i \in {{\cal K}_k}}} $ ; 其对应的所有观测量为$ {{\cal Z}_k} = {\left\{ {{{\cal P}_i},{{\cal I}_{\left( {i,j} \right)}}} \right\}_{\left( {i,j} \right) \in {{\cal K}_k}}} $ .在这些假设和定义下, 状态估计问题可描述为: 在给定观测信息
$ {\cal Z}_k $ 和先验信息$ {\mathop{p}\nolimits} \left( {{{\cal X}_0}} \right) $ 的条件下, 估计$ {\cal X}_0 $ 的后验概率问题, 即$$ \begin{split}& {\mathop{p}\nolimits} \left( {{{\cal X}_k}\left| {{{\cal Z}_k}} \right.} \right) \propto {\mathop{p}\nolimits} \left( {{{\cal X}_0}} \right){\mathop{p}\nolimits} \left( {{{\cal Z}_k}\left| {{{\cal X}_k}} \right.} \right)= \\ & \qquad {\mathop{p}\nolimits} \left( {{{\cal X}_0}} \right)\prod\limits_{\left( {i,j} \right) \in {{\cal K}_k}} {{\mathop{p}\nolimits} \left( {{{\cal P}_i},{{\cal I}_{\left( {i,j} \right)}}\left| {{{\cal X}_k}} \right.} \right)} \end{split} $$ (2) 由于观测量已知, 在联合概率分布中将其作为参数而非随机变量. 根据马尔科夫性质,
$ {{\cal P}_i} $ 仅与$ {t_i} $ 时刻的状态$ {{{x}}_i} $ 有关, 则式(2)可分解为$$ \begin{split} &{{\mathop{p}\nolimits} \left( {{{\cal X}_0}} \right)} \prod \limits_{\left( {i,j} \right) \in {{\cal K}_k}} {\mathop{p}\nolimits} \left( {{{\cal P}_i},{{\cal I}_{\left( {i,j} \right)}}\left| {{{\cal X}_k}} \right.} \right)= \\ &\;\;\quad{\mathop{p}\nolimits} \left( {{{\cal X}_0}} \right)\prod\limits_{\left( {i,j} \right) \in {{\cal K}_k}} {\mathop{p}\nolimits} \left( {{{\cal I}_{\left( {i,j} \right)}}\left| {{{{{x}}}_i},{{{{x}}}_j}} \right.} \right) \prod\limits_{i \in {{\cal K}_k}} {{\mathop{p}\nolimits} \left( {{{\cal P}_i}\left| {{{{{x}}}_i}} \right.} \right)} \\[-17pt] \end{split} $$ (3) 变量因子的最大后验概率(Maximum a posteriori, MAP)可由式(3)推得
$$ \begin{aligned} & {{\hat {\cal X}}_k}= \mathop {\arg \min }\limits_{\ \ \ \ \ \ {{\cal X}_k}} \left( \!\!{ - \ln {\mathop{p}\nolimits} \left( {{{\cal X}_k}\left| {{{\cal Z}_k}} \right.} \right)} \right) =\\ & \quad\;\;\mathop {\arg \min }\limits_{\ \ \ \ \ \ {{\cal X}_k}} \left( \begin{array}{l} \!\! \left\| {{{{{r}}}_0}} \right\|_{{{{\varSigma }}_0}}^2 \!\!+\!\! \sum\limits_{\left( {i,j} \right) \in {{\cal K}_k}} \!\!{\left\| {{{{{r}}}_{{{\cal I}_{\left( {i,j} \right)}}}}} \right\|_{{{{\varSigma }}_{{{\cal I}_{\left( {i,j} \right)}}}}}^2} \\ \!\!+\!\! \sum\limits_{i \in {{\cal K}_k}} {\left\| {{{{{r}}}_{{{\cal P}_i}}}} \right\|_{{{{\varSigma }}_{{{\cal P}_i}}}}^2} \!\!+\!\! \sum\limits_{\left( {i,j} \right) \in {{\cal K}_k}} \!\!{\left\| {{{{{r}}}_{{{\cal P}_{\left( {i,j} \right)}}}}} \right\|_{{{{\varSigma }}_{{{\cal P}_{\left( {i,j}\!\!\!\!\!\! \right)}}}}}^2} \end{array} \right) \end{aligned} $$ (4) 式中
$ {{r}} $ 表示观测模型与实际观测的残差, 是关于状态量$ {\cal X}_k $ 的函数,${{\varSigma }}$ 为对应的协方差矩阵. 后文将对式(4)中各项的具体形式做详细说明.2.2 数据预处理
1) 点云数据处理
本文采用前期工作[25]中的运动模型估计方法校正点云, 即根据预积分得到的相对运动估计将点云统一至同一时刻载体坐标系下. 图2为KITTI数据集[10]的处理实例. 点云的特征提取主要分为以下步骤:
步骤 1. 将无序的点云
$ {\cal{P}} $ 转为有序的深度图$ \mathop{{D}}\limits_{r \times c} $ , 以提升点云搜索速度.$ \mathop{{D}}\limits_{r \times c} $ 的行数$ r $ 为LiDAR线阵的行数, 列数$ c $ 根据深度图水平角分辨率设定.$ \mathop{{D}}\limits_{r \times c} $ 中每个像素对应一个或几个点云中的点, 像素值取最远点的深度值. 如图2 (c), 深度图大小为$ 64 \times 860 $ , 每个像素约包含5个点.步骤 2. 不同于LeGO-LOAM[11]中的地面点分割方法, 本文采用基于角度图像的分割策略. 首先根据垂直方向和水平方向的坐标差
$ \Delta x $ 和$ \Delta z $ 计算深度图上同一列相邻行的两点$ A $ 和$ B $ 间的垂直夹角$ \alpha $ (如图2 (d)), 构成夹角图像, 并对其采用Savitsky-Golay滤波算法平滑处理得到如图2 (e)所示的平滑后夹角图像$ \mathop {{A}}\limits_{\left( {r - 1} \right) \times c} $ . 对于车载点云, 一般可以认为满足$ \alpha $ 接近于0的像素是地面点所对应的区域; 采用广度优先算法(Breadth-first search, BFS)算法在$ \mathop {{A}}\limits_{\left( {r - 1} \right) \times c} $ 上搜索小于阈值的点, 将其标记为地面点$ \mathop {{G}}\limits_{r \times c} $ .步骤 3. 对剔除地面点后的深度图进行目标分割. 当某一激光光束
$ {{OP}} $ 与两激光点连线$ {{PQ} }$ 所构成的夹角较小时, 认为两激光点$ {{P}} $ 和$ {{Q}} $ 位于不同目标上. 循环搜索生成如图2 (f)的分割图像$ \mathop {{L}}\limits_{r \times c} $ , 并将$ \mathop {{L}}\limits_{r \times c} $ 中点数较少的目标作为野点剔除.步骤 4. 将地面和目标分割后的部分作为特征图
$ \mathop {{F}}\limits_{r \times c} = \mathop {{G}}\limits_{r \times c} + \mathop {{L}}\limits_{r \times c} $ , 并在其上进行特征提取, 提取方法与文献[8]中相同. 根据各点深度值$ {d_i} $ 计算某一点在其同行邻域点$ {\cal S} $ 中的粗糙度$ \theta $ :$$ \theta = \frac{1}{{\left| {\cal S} \right| \cdot \left\| {{d_i}} \right\|}} \cdot \left\| {\sum\limits_{j \in {\cal S},j \ne i} {\left( {{d_j} - {d_i}} \right)} } \right\| $$ (5) 并将
$ \theta $ 较大的非地面点标记为边缘点$ {\cal P}^E $ , 将$ \theta $ 较小的标记为平面点$ {\cal P}^F $ . 从$ {\cal P}^E $ 中选取$ n_E $ 个$ \theta $ 最大的点构成$ {{\cal P}^{{E_{max}}}} \in {{\cal P}^E} $ , 从$ {\cal P}^F $ 中选取$ n_F $ 个$ \theta $ 最小且为地面点的点构成$ {{\cal P}^{{F_{min}}}} \in {{\cal P}^F} $ .2) IMU数据处理
IMU的观测模型为:
$$ {}_{\mathop{{{\rm{B}}}}\nolimits} {{\tilde{ \omega }}_{{\rm{WB}}}}\left( {t_i^n} \right) = {}_{\mathop{{{\rm{B}}}}\nolimits} {{{\omega }}_{{\rm{WB}}}}\left( {t_i^n} \right) + {{{b}}^g}\left( {t_i^n} \right) + {{{\eta }}^g}\qquad \quad \;\;\; \;$$ (6) $$ {}_{\mathop{{{\rm{B}}}}\nolimits} {\tilde{ a}}\left( {t_i^n} \right) = {{R}}_{{\rm{WB}}}^{{\rm{T}}} \left( {t_i^n} \right)\left( {{}_{\rm{W}}{{a}}\left( {t_i^n} \right) - {}_{\rm{W}}{{g}}} \right) + {{{b}}^a}\left( {t_i^n} \right) + {{\bf{\eta }}^a} $$ (7) 其中,
$ {{\eta }} \sim {\cal N}\left( {{\bf{0}},{{\Sigma }}} \right) $ 表示观测噪声,${{R}}_{{\rm{WB}}}^{{\rm{T}}}$ 为$ {{\rm{B}}} $ 到$ {{\rm{W}}} $ 的旋转矩阵,${}_{\rm{W}}{{g}}$ 为重力加速度. 根据IMU的动力学模型, 采用离散化积分方法对${}_{\mathop{{{\rm{B}}}}\nolimits} {{\bf{\omega }}_{{\rm{WB}}}}\left( {t_i^n} \right)$ 和${}_{\rm{W}}{{a}}\left( {t_i^n} \right)$ 在IMU采样间隔时间$\Delta t$ 内积分:$$ \begin{split} &{{{R}}_{{\rm{WB}}}}\left( {t_i^{n + 1}} \right) = {{{R}}_{{\rm{WB}}}}\left( {t_i^n} \right) \cdot \\ & \quad \exp \left[ {{{\left( {\left( {{}_{\mathop{{{\rm{B}}}}\nolimits} {{{\tilde{ \omega }}}_{{\rm{WB}}}}\left( {t_i^n} \right) - {{{b}}^g}\left( {t_i^n} \right) - {{{\eta }}^g}} \right) \cdot \Delta t} \right)}^ {\wedge} }} \right] \end{split} \;\;\;$$ (8) $$ \begin{split} &{}_{\rm{W}}{{v}}\left( {t_i^{n + 1}} \right) = {}_{\rm{W}}{{v}}\left( {t_i^n} \right) + {}_{\rm{W}}{{g}} \cdot \Delta t+ \\ &\qquad {{{R}}_{{\rm{WB}}}}\left( {t_i^n} \right)\left( {{}_{\mathop{{{\rm{B}}}}\nolimits} {\tilde{ a}}\left( {t_i^n} \right) - {{{b}}^a}\left( {t_i^n} \right) - {{{\eta }}^a}} \right) \cdot \Delta t \end{split} \quad $$ (9) $$ \begin{split} &{}_{\rm{W}}{{t}}\left( {t_i^{n + 1}} \right) = {}_{\rm{W}}{{t}}\left( {t_i^n} \right) + {}_{\rm{W}}{{v}}\left( {t_i^n} \right) \cdot \Delta t +\displaystyle \frac{1}{2}{}_{\rm{W}}{{g}} \cdot \Delta {t^2}+\\ &\qquad \frac{1}{2}{{{R}}_{{\rm{WB}}}}\left( {t_i^n} \right)\left( {{}_{\mathop{{{\rm{B}}}}\nolimits} {\tilde{ a}}\left( {t_i^n} \right) - {{{b}}^a}\left( {t_i^n} \right) - {{{\eta }}^a}} \right) \cdot \Delta {t^2}\\[-13pt] \end{split} $$ (10) 式中‘
$\wedge $ ’符号表示将三维向量映射为反对称矩阵, 由于IMU输出频率很高, 状态估计时若直接将IMU采样时刻对应的全部位姿作为变量节点插入因子图进行优化, 无疑是不现实的[21]. 通常通过预积分处理, 将高频输出的加速度和角速度观测量转化为状态量间的位姿变换, 构成关键帧状态量之间的约束因子[23], 从而将所有IMU观测量转化为一个预积分观测量, 传感器采样频率与IMU因子关系如图3.根据式(8) ~ (10)定义关键帧
$ {{{x}}}_i $ 与$ {{{x}}}_j $ 间的运动增量:$$ \begin{split} & \Delta {{{R}}_{ij}} = {{R}}_i^{{\rm{T}}} {{{R}}_j}=\\ &\qquad\quad\;\; \prod\limits_{k = i}^{j - 1} {\exp \left( {{{\left( {\left( {{{{\tilde{ \omega }}}_k} - {{b}}_k^g - {{{\eta }}^g}} \right) \cdot \Delta t} \right)}^ {\wedge} }} \right)} \end{split} $$ (11) $$ \begin{split}& \Delta {{{v}}_{ij}} = {{R}}_i^{{\rm{T}}} \left( {{{{v}}_j} - {{{v}}_i} - {{g}} \cdot \Delta {t_{ij}}} \right) = \\ &\quad\;\;\qquad\sum\limits_{k = i}^{j - 1} {\Delta {{{R}}_{ik}}\left( {{{{\tilde{ a}}}_k} - {{b}}_k^a - {{{\eta }}^a}} \right) \cdot \Delta t} \end{split} \quad \;\;\;\;$$ (12) $$ \begin{aligned}& \Delta {{{t}}_{ij}} = {{R}}_i^{{\rm{T}}} \left( {{{{t}}_j} - {{{t}}_i} - {{{v}}_i}\Delta {t_{ij}} -\displaystyle \frac{1}{2}\sum\limits_{k = i}^{j - 1} {{{g}}(\Delta t)^2} } \right)= \\ &\quad\;\; \qquad\sum\limits_{k = i}^{j - 1} {\left( \!\!{\Delta {{{v}}_{ik}}\Delta t +\displaystyle \frac{1}{2}\Delta {{{R}}_{ik}}\left( {{{{\tilde{ a}}}_k} \!-\! {{b}}_k^a \!-\! {{{\eta }}^a}} \right)(\Delta t)^2} \!\!\right)} \end{aligned} $$ (13) 为方便书写保证公式简洁, 上式省略了左下标的坐标系标注, 并将
$ \left( \cdot \right) \left( t_i \right) $ 简写为$ {\left( \cdot \right)_i} $ ; 其中$ \Delta {t_{ij}} =$ $\sum\nolimits_{k = i}^{j - 1} {\Delta t}$ . 假设关键帧$ {{{{x}}}_i} $ 与$ {{{{x}}}_j} $ 间的IMU零偏保持不变, 则可由式(11) ~ (13)可得预积分观测模型[13]:$$ \Delta {{\tilde{ R}}_{ij}} = {{R}}_i^{{\rm{T}}} {{{R}}_j}\exp \left( {\delta \phi _{ij}^ {\wedge} } \right) \qquad \qquad \qquad \quad \;\;\;\;$$ (14) $$ \Delta {{\tilde{ v}}_{ij}} = {{R}}_i^{{\rm{T}}} \left( {{{{v}}_j} - {{{v}}_i} - {{g}} \cdot \Delta {t_{ij}}} \right) + \delta {{{v}}_{ij}} \qquad \quad$$ (15) $$ \Delta {{\tilde{ t}}_{ij}} = {{R}}_i^{{\rm{T}}} \left( {{{{t}}_j} - {{{t}}_i} - {{{v}}_i}\Delta {t_{ij}} - \frac{1}{2}{{g}}\Delta {t_{ij}}^2} \right) + \delta {{{t}}_{ij}} $$ (16) 上式中
${\left[ {\delta \phi _{ij}^{{\rm{T}}}}\;\;\;\;{\delta {{v}}_{ij}^{{\rm{T}}}}\;\;\;\;{\delta {{t}}_{ij}^{{\rm{T}}}} \right]^{{\rm{T}}}}$ 均为随机变量, 其协方差矩阵${{{\varSigma }}_{ij}}$ 可由误差传播定律推导. 由式(14)~(16)即可直接列出式(4)中的残差项${{{{r}}}_{{{\cal I}_{\left( {i,j} \right)}}}} = $ ${\left[ {{{{r}}}_{\Delta {{{R}}_{ij}}}^{{\rm{T}}}}\;\;{{{{r}}}_{\Delta {{{v}}_{ij}}}^{{\rm{T}}}}\;\;{{{{r}}}_{\Delta {{{t}}_{ij}}}^{{\rm{T}}}} \right]^{{\rm{T}}}}$ .2.3 激光里程计
激光里程计模块利用连续的特征点云序列
$\left\{ {\cal P}_{k + 1}^{E_{\rm{max}}},{\cal P}_{k + 1}^{F_{\rm{min}} } \right\}$ ,$ \left\{ {{\cal P}_k^E,{\cal P}_k^F} \right\} $ 和IMU预积分结果$ \int {{{\cal I}_{\left( {k,k{\rm{ + }}1} \right)}}} $ 估计载体的相对运动, 输出频率与LiDAR的采样频率保持一致.首先, 利用
$ \int {{{\cal I}_{\left( {k,k{\rm{ + }}1} \right)}}} $ 对$\left\{ {{\cal P}_{k + 1}^{{E_{\max}}},{\cal P}_{k + 1}^{{F_{\min}}}} \right\}$ 进行坐标变换, 使之与$ \left\{ {{\cal P}_k^E,{\cal P}_k^F} \right\} $ 坐标系统一. 分别在$ {\cal P}_k^E $ 和$ {\cal P}_k^F $ 中搜索${\cal P}_{k + 1}^{{E_{\max}}}$ 和${\cal P}_{k + 1}^{{F_{\min}}}$ 的匹配点.${}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i}\in $ ${\cal P}_{k + 1}^{{E_{\max}}}$ 的匹配点由最邻近点$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} \in {\cal P}_k^E $ 和次临近点$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l} \in {\cal P}_k^E $ 组成, 可利用KD树搜索得到. 点$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} $ 到直线$ \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right) $ 的距离$ {{d}}_E $ 为:$$ {{{{d}}}_E} = \frac{{\left\| {\left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j}} \right) \times \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right)} \right\|}}{{\left\| {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right\|}} $$ (17) ${}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} \!\in\! {\cal P}_{k + 1}^{{F_{max}}}$ 的匹配点由$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j}\! \in\! {\cal P}_k^F $ ,$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l} \!\in\! {\cal P}_k^F $ 和$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m} \in {\cal P}_k^F $ 组成, 点$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} $ 到平面$( {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}, $ $ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m} )$ 的距离$ {{{{d}}}_F} $ 为:$$ \begin{split} &{{{d}}_F} =\\ &\quad\frac{{\left\| {\left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} \!-\! {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j}} \right) \!\cdot\! \left( {\left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} \!-\! {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right) \!\times\! \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} \!-\! {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m}} \right)} \right)} \right\|}}{{\left\| {\left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right) \times \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m}} \right)} \right\|}} \end{split} $$ (18) 其中
$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} $ 为利用KD树搜索到的目标点最邻近点,$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l} $ 为相邻线上的最邻近点, 而$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m} $ 为同一线上的次邻近点. 由于$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_i} \in {{\cal P}_k} $ 与$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} \in {{\cal P}_{k + 1}} $ 存在三维变换关系:$$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} = {{\tilde{ R}}_{\left( {k + 1,k} \right)}} \cdot {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_i} + {}_{{B_k}}{{\tilde{ t}}_{\left( {k + 1,k} \right)}} $$ (19) 因此, 将式(19)分别代入式(17)和式(18), 得:
$$ f\left( {{{{\tilde{ R}}}_{\left( {k + 1,k} \right)}},{}_{{B_k}}{{{\tilde{ t}}}_{\left( {k + 1,k} \right)}}} \right) = {{{d}}} $$ (20) 令
$ {{d}} $ 最小, 利用Levenberg-Maquardt算法即可求解旋转量和平移量$ {{\bar{ T}}_{\left( {k + 1,k} \right)}} = \left[ {{{{\bar{ R}}}_{\left( {k + 1,k} \right)}},{}_{{B_k}}{{{\bar{ t}}}_{\left( {k + 1,k} \right)}}} \right] $ 的估值.需要说明的是: 1)为提高配准精度, 搜索匹配点时, 点对
$ \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right) $ 或$ \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m}} \right) $ 在分割图像$ \mathop {{L}}\limits_{r \times c} $ 中应属于同一目标, 否则舍弃该匹配. 2) 结合UGV特点和地面点云的几何属性, 采用LeGO-LOAM中的两步法估计载体的6DoF运动. 先利用$ {\cal P}_{k + 1}^{{F_{min}}} $ 估计载体的横滚角, 俯仰角及垂直方向的平移量, 将估值带入后再利用$ {\cal P}_{k + 1}^{{E_{max}}} $ 估计载体的偏航角及水平方向平移量. 地面点云对横滚角, 俯仰角及垂直方向具有很好的约束, 而对偏航角及水平方向平移量没有约束, 非地面点云与之相反. 经测试验证, 采用这种方法可以加快配准迭代的收敛速度, 在达到相同配准精度的前提下, 计算速度提升35 %[11].2.4 地图构建
地图构建模块通过将特征点云
$ \left\{ {{\cal P}_{k + 1}^{{E_{max}}},{\cal P}_{k + 1}^{{F_{min}}}} \right\} $ 与地图$ {{\cal M}_k} $ 配准, 优化激光雷达里程计模块估计的位姿${{\bar{ T}}_{\left( {k + 1,k} \right)}} = \left[ {{{{\bar{ R}}}_{\left( {k + 1,k} \right)}},{}_{{{\rm{B}}_k}}{{{\bar{ t}}}_{\left( {k + 1,k} \right)}}} \right]$ , 同时更新地图$ {{\cal M}_{k + 1}} $ . 由于地图配准需要占用大量运算资源, 为保证实时性, 该模块低频运行. 点到地图的配准方法与上节相同.LOAM和LeGO-LOAM中并未定义局部地图, 只是根据垂直角可视范围, 以传感器为中心搜索与索引特征点可能存在匹配的地图点. 这种粗略的搜索方法过于依赖阈值: 搜索得到的地图点过多, 则配准效率低下; 若地图点过少, 则可能造成地图点与特征点对周围环境表达的不一致, 部分可观测的地图点缺失, 导致配准精度下降.
与文献[27-28]类似, 本文提出采用以传感器为中心的多尺度地图模型建立局部地图
$ {{\cal M}_k} $ , 并定义$ {{\cal M}_k} $ 的参考系与当前关键帧一致$ {{{m}}_k} = {{{{x}}}_i} $ , 地图构建模块每运行一次, 便插入一个关键帧. 局部地图共分三层, 各层栅格大小不一, 相互嵌套. 栅格中的点存储于环形容器中, 保持局部地图中点个数不变, 从而保持地图配准模块的处理速度相对恒定. 点坐标保存为点在栅格内的相对位置, 以便于局部地图坐标系变化时点坐标更新. 局部地图的示意图如图4, 大小不一的三种栅格分别表示不同分辨率的格网地图; 栅格中的点表示该格网中存储的点云; 当载体运动时局部地图随之移动, 局部地图的中心由$ {{{m}}_{k - 1}} $ 的原点${O_{{\rm{old}}}}$ 向$ {{{m}}_{k}} $ 的原点${O_{{\rm{new}}}}$ 移动, 局部地图向前进方向添加栅格, 并剔除远离方向的栅格.这种局部地图表示方法有两个特点: 1) 由于局部地图始终保持以载体为中心, 使得地图中的点大部分位于LiDAR的可视范围内, 避免了对不可视地图点的搜索. 2) 多分辨率格网地图更符合LiDAR放射状采样的性质, 近处点云密集, 而远处相对稀疏. 随着移动LiDAR的移动高分辨率栅格内的点逐渐增加并稳定, 能够更准确地刻画环境的真实面貌, 避免地图点与特征点不一致性的产生. 这也是与其他格网地图表达点云方法[27-28]的最主要区别.
2.5 闭合回环
闭合回环模块检测载体的轨迹是否形成闭合, 即是检测载体是否回到之前的位置. 若构成闭环则利用GICP算法将当前的特征点云与闭环处的特征点云配准, 得到相对位姿关系
$ {\bar{ T}} $ , 构成闭环约束因子, 并将其插入因子图中. 闭环检测根据当前关键帧位置与其余关键帧间的距离判断: 将关键帧列表$ {{\cal X}_k} = {\left\{ {{{{{x}}}_i}} \right\}_{i \in {{\cal K}_k}}} $ 保存于KD树中, 以半径$ R $ 搜索当前关键帧$ {{{{x}}}_{{{\cal K}_{k + 1}}}} $ 的相邻关键帧, 并根据采样时间判断是否为相邻时刻的关键帧.本文提出采用大闭环与小闭环结合的方式, 大闭环表示机器人回到之前的位置, 可以建立闭环约束修正全局误差累计; 而小闭环则表示间隔关键帧之间的共视关系, 可以为转角位置提供更多约束, 从而提升激光里程计对LiDAR旋转运动的鲁棒性.
2.6 因子图优化
因子图优化模块在系统中维护一个全局因子图, 因子图结构如图5. 图中右侧为系统实际运行时构建的因子图, 左侧是对其结构的抽象化说明. 假设给定初始状态
$ {{\cal X}_0} $ , 对应式(4)中的$\left\| {{{{{r}}}_0}} \right\|_{{{{\varSigma }}_0}}^2$ . 地图构建模块向因子图插入配准因子, 对应式(4)中的$\left\| {{{{{r}}}_{{{\cal P}_i}}}} \right\|_{{{{\varSigma }}_{{{\cal P}_i}}}}^2$ :$$ {{{{r}}}_{{{\cal P}_i}}} = \left( {{{{{x}}}_i} \ominus {{{m}}_{i - 1}}} \right) \ominus {{\bar{ T}}_{\left( {i,i - 1} \right)}} $$ (21) 数据预处理模块计算相邻关键帧之间的IMU预积分, 并向因子图插入IMU预积分因子, 对应式(4)中的
$\left\| {{{{{r}}}_{{{\cal I}_{\left( {i,j} \right)}}}}} \right\|_{{{{\varSigma }}_{{{\cal I}_{\left( {i,j} \right)}}}}}^2$ :$$ {{{{r}}}_{{{\cal I}_{\left( {i,j} \right)}}}} = \left( {{{{{x}}}_i} \ominus {{{{x}}}_j}} \right) \ominus \int {{{\cal I}_{\left( {i,j} \right)}}}{\rm{d}}t $$ (22) 其中
$ {{{{r}}}_{{{\cal P}_i}}} $ 与$ {{{{r}}}_{{{\cal I}_{\left( {i,j} \right)}}}} $ 是一一对应的. 闭环检测模块向因子图插入闭环因子, 对应式(4)中的$\left\| {{{\bf{r}}_{{{\cal P}_{\left( {i,j} \right)}}}}} \right\|_{{{{\varSigma }}_{{{\cal P}_{\left( {i,j} \right)}}}}}^2$ :$$ {{{{r}}}_{{{\cal P}_{\left( {i,j} \right)}}}} = \left( {{{{x}}_i} \ominus {{{{x}}}_j}} \right) \ominus {{\bar{ T}}_{\left( {i,j} \right)}} $$ (23) 式中
$ \ominus $ 表示$ {\mathfrak{s}}{\mathfrak{e}}\left( 3 \right) $ 中位姿变换的逆运算. 每当因子图中插入新的节点, 就对整个因子图优化计算一次, 更新当前时刻的位姿估计, 实现点云数据和IMU数据的深度融合. 公式推导参考文献[13].3. 实验与分析
3.1 实验及设备介绍
为客观全面地评估本文所提方法的性能, 共进行两组实验: 1) 采用文献[11]公开的两组数据集1#和3#对系统运行速度、闭环优化效果进行评价和对比; 2) 分别采用文献[11]公开的数据集2#和实测数据, 根据回到起始点的位姿偏差定量评估系统定位精度. 数据采集平台为搭载了Xsens MTi-G-710 IMU和Velodyne VLP-16 LiDAR的地面移动机器人(如图6), 数据采集频率分别为200 Hz和10 Hz, 两传感器采用软同步方式进行时间对齐. 数据采集环境为室内地下停车场和室外开阔广场(如图10 (c)和10 (d)), 数据采集时间约为1000 s, 平台运动速度约为1 m/s, 总距离约为1 km. 系统采用C++实现, 在机器人操作系统(Robot operation system, ROS)中运行. 小型智能地面机器人的数据处理器为4 GB内存并搭载Intel i5 1.8 GHz CPU的终端(如图6).
在数据融合前, 需要对系统进行初始化, 其主要目的是: 1) 获得IMU与LiDAR之间的相对位姿
${{T}}_{\rm{L}}^{\rm{B}}$ , 即传感器外参数; 2) 计算载体系$ {\rm{B}} $ 下的重力加速度${}_{\rm{B}}{{g}}$ , 从而得到载体坐标系$ {\rm{B}} $ 到世界坐标系$ {\rm{W}} $ 的转换关系${{T}}_{\rm{B}}^{\rm{W}}$ . 结合本文对坐标系统的定义, 在本实验中,${} _{\rm{B}}{{g}}$ 采用文献[23]中所述方法计算得到, 而后将其旋转至$ {\rm{W}} $ 系下得到旋转关系${{R}}_{\rm{B}}^{\rm{W}}$ ; 为保证精度, LiDAR/IMU外参数${{T}}_{\rm{L}}^{\rm{B}}$ 的标定在结构性较好且设置了特殊靶标的实验室预先进行, 数据处理中作为已知量. 基本原理是分别采用激光雷达里程计和IMU预积分得到一段时间内相邻状态间的约束${}_{\rm{L}}{{C}}_i^{i + 1}$ 和${}_{\rm{B}}{{C}}_i^{i + 1}$ , 从而构建关于${{T}}_{\rm{L}}^{\rm{B}}$ 的优化方程:$$ {\hat{ T}}_{\rm{L}}^{\rm{B}} = \mathop {\arg \min }\limits_{{{T}}_{\rm{L}}^{\rm{B}}} \left( {{{\left( {{}_{\rm{B}}{{C}}_i^{i + 1}} \right)}^{ - 1}} \cdot {{T}}_{\rm{L}}^{\rm{B}} \cdot {}_{\rm{L}}{{C}}_i^{i + 1}} \right) $$ (24) 用特殊氏式群[26]
$ \left[ {{{{R}}_i},{{{t}}_i}} \right] \in SE\left( 3 \right) $ 表达位姿变换, 即可在流形上利用L-M算法求解外参的估值${\hat{ T}}_{\rm{L}}^{\rm{B}}$ . 一般而言, 传感器外参数在数据采集过程中保持不变, 因此在后续数据处理中可将其作为固定量.3.2 运行效率对比及闭环优化结果分析
分别统计LeGO-LOAM和本文Inertial-LOAM对1# 和3# 两组数据集的单帧数据处理时间, 包括数据预处理, 激光里程计, 地图构建, 优化计算等各模块及系统总体的单帧处理时长(如图7), 比较并分析两系统数据处理的速度和效率.
从图中可以看出: 两系统总处理时间的95 %以上都小于LiDAR的采样间隔0.1
${\rm{s}}$ , 说明系统满足实时处理的要求; 由于Inertial-LOAM采用多分辨率局部地图, 地图构建模块避免了对全局地图点的搜索, 因此平均处理速度更快. 而在数据预处理时, 由于增加了IMU数据的预积分计算, 该模块处理时间略有增加; 另外, Inertial-LOAM的优化计算时间并未明显增加, 说明融合IMU数据后并未过多增加系统的计算负担.根据闭环优化前后的地图偏差定性评价闭环优化对系统定位和建图精度的提升作用, 1# 和3# 两组数据闭环优化前后所构建的地图及轨迹如图7. 从图7 (a)可以明显看出, 闭环优化可以有效处理大闭环造成的累计误差, 对轨迹精度和地图一致性的提升具有重要作用; 图7(b)的激光里程计误差累计不明显, 因此闭环优化作用效果不甚显著. 采用Inertial-LOAM方法和文献[11]中1# 和3# 数据, 构建的地图和轨迹的结果如图10 (a)和10 (b).
3.3 定位精度分析
在IMU和LiDAR数据融合时, 对传感器观测量的信任度由观测量的质量决定. 因此需要预先标定IMU, 统计其阿伦方差量, 以确定对角速度和加速度观测值的信任程度.
实验时, 在起点附近架设高精度全站仪, 并在机器人上粘贴标志点. 每当测量平台回到起点附近时, 用高精度全站仪观测标志点, 并以其位姿变换作为相对变换的参考值
${}_{\rm{G}}{{T}}$ . 估计的位姿${}_{\rm{W}}{{{x}}}$ 的精度由累计误差的均方根(Root mean square error, RMSE)表示:$$ {{{e}}_n} = \left( {{}_{\rm{W}}{{{{x}}}_n} \ominus {}_{\rm{W}}{{{{x}}}_0}} \right) \ominus {}_{\mathop{G}\nolimits} {{T}} $$ (25) $$ {\mathop{{\rm{RMSE}}}\nolimits} \left( {{{{e}}_{1:n}}} \right) = {\left( {\frac{1}{N}\sum\limits_{n = 1}^N {{{\left\| {{{{e}}_i}} \right\|}^2}} } \right)^{\frac{1}{2}}}$$ (26) 其中,
$ \ominus $ 表示位姿变换的逆运算. 采用起终点相同的2# 数据, 分别对Cartographer[12], LeGO-LOAM[11], LOAM[8], IMU和本文所提出的Inertial-LOAM (以下分别简称为C, LL, L, I, 和IL)进行精度对比, 并说明IMU在数据融合中的作用; 采用实测的室内停车场、室外开阔广场数据对比IL, LL, L和C的精度, 分析本文方法的性能和优势. 误差累计结果如表1. 需要说明的是, 为客观评价系统漂移程度, 实验结果均未进行闭环优化, LL, L和C均为加入IMU观测后的结果, 误差累计均为各系统处理5次的平均值.表 1 累计误差结果Table 1 Error accumulation result场景 方法 横滚 (°) 俯仰 (°) 航向 (°) 角度偏差 (°) X方向 (m) Y方向 (m) Z方向 (m) 位置偏差 (m) 2# 数据[11] IMU 0.748 1.018 0.598 1.398 35.095 84.652 −665.782 672.059 Cartographer 0.113 −0.709 0.989 1.222 0.405 1.317 0.670 1.532 LOAM 0.016 0.141 0.925 0.936 0.316 0.349 0.025 0.471 LeGO-LOAM 0.061 0.081 0.916 0.921 0.068 0.338 0.115 0.364 Inertial-LOAM 0.013 0.026 0.917 0.918 0.061 0.258 0.023 0.266 室内环境 Cartographer 0.003 −0.001 0.017 0.017 0.023 0.037 0.028 0.052 LOAM 0.001 0.004 0.068 0.068 0.032 0.083 0.032 0.095 LeGO-LOAM −0.006 −0.002 −0.021 0.022 0.016 0.047 −0.032 0.059 Inertial-LOAM −0.008 0.001 −0.020 0.021 0.021 0.043 0.027 0.055 室外环境 Cartographer 0.075 −0.024 0.081 0.113 1.747 2.592 −0.449 3.158 LOAM −0.031 0.006 0.096 0.101 0.0467 2.368 −0.065 2.353 LeGO-LOAM −0.024 −0.543 0.041 0.545 −19.857 −14.914 −0.355 24.836 Inertial-LOAM 0.006 −0.080 0.003 0.080 −0.310 −0.100 −0.030 0.328 2# 数据的采集环境与3#相同, 特征丰富但机器人运动较快. 由表1可以看出, 仅利用IMU积分的位姿推估误差累计严重, 无法得到正确的结果. 因此, 在后续实验中不再将其作为对比对象. 尽管IMU位置漂移严重, 但姿态偏差相对较小. 说明IMU能提供一个较好的姿态约束, 而这正是LiDAR里程计所缺少的, 间接反映了激光里程计与IMU耦合的优势.
在室外特征丰富但运动较快的情况下, L、LL和IL均能得到分米级的定位结果而C精度较差. LL和IL在数据预处理阶段对特征点云进行了目标分割, 剔除了大量主要特征以外的野点, 避免其对配准精度的影响, 因此较L的精度有所提升, 其中IL最优. 由于C将三维点云投影至二维格网中, 它对类似墙面等结构性较强的环境有较好的适应性, 而室外诸如树木、车辆等特征不能保证较好的配准精度. 因此, 即便在前端融合了IMU数据, 最终仍产生较大误差.
在相对狭小的室内环境中, C、LL与IL的位姿估计精度基本都在0.05
${\rm{m}}$ 左右, 其中C方法最优, 较L提升近一倍, 四种方法的定位精度均能达到厘米量级. 由于室内相对狭小, 路面平坦, 且包含大量的墙面及边沿线等明显的结构特征, 点云配准精度高, 在IMU/LiDAR数据融合解算时占据更大权重, 因此IMU对定位结果的作用较小. 室内环境中IL地图和轨迹结果如图10 (c).在室外开阔且运动相对平缓的条件下, LL随运行时间增加, 误差累计愈发严重, 无法得到可靠的位姿估计结果; L和C性能优于LL, 但仍存在一定误差累计. 在不采用闭环优化的前提下, IL通过融合IMU信息, 显著降低定位漂移, 精度仍能达到分米量级. 四者轨迹和地图结果对比如图9. 室外空间较为开阔且结构性特征较少, 路面不够平整造成移动机器人运动时产生剧烈的震动, 因此点云包含较多噪点. 由于未在后端优化中融入IMU观测约束, 且LL为了适应轻量级终端, 提取的特征点云较为稀疏, 导致配准误差较大, 出现明显的误差累计; 尽管L的运行效率不及LL, 但相对稠密的点云保证了其配准的精度, 但轨迹仍不可避免的存在漂移; 虽然C在前端融合了IMU数据, 但本质上只是利用融合的数据构建更为精确的局部地图, 以此保证更准确的前端匹配, 导致数据融合的作用在后端优化中体现不明显, 不足以弥补连续配准引起的轨迹发散问题. 本文提出的IL通过充分融合LiDAR和IMU的观测信息对轨迹进行整体优化, 有效降低了轨迹漂移, 提升了位姿估计的精度. 从图9的对比中还可以看出, 融合IMU信息后系统在快速直角转弯处的表现显著提升, 反映了IMU良好的姿态约束的作用. 采用室外数据和IL方法所构建的地图和轨迹结果如图10 (d).
图 9 室外开阔环境IL/LL/L/C轨迹结果对比Fig. 9 Comparison of pose estimation of IL/LL/L/C in outdoor environment4. 结论
本文面向小型智能平台, 针对现有的LiDAR/IMU SLAM方法数据融合不充分, 对开阔环境和快速旋转鲁棒性差, 地图构建缺乏一致性的问题, 提出了一种LiDAR/IMU紧耦合的实时定位方法— Inertial-LOAM. 首先在数据预处理阶段采用地面分割效果更好的基于夹角图像的地面点云分割方法, 并对IMU数据进行预积分处理. 其次, 在地图构建阶段定义了一个以传感器为中心的MRS局部地图, 提升地图匹配的速度和精度. 最后, 在系统中增加因子图优化模块, 对LiDAR和IMU观测数据进行整体优化, 实现多源观测值的紧密耦合.
采用实测数据对系统进行全面测试和评估, 可以得到以下结论:
1) IMU预积分计算略微增加了数据预处理时间, 同时IMU预积分因子的加入使因子图结构更为复杂, 但整体而言对系统的运算负担增加不大, 可以满足实时性要求; 另外由于定义了MRS局部地图, 避免了对全局地图的搜索, 地图构建模块速度更快.
2) 闭合回环对系统精度的提升具有重要作用. 通过对整体误差的修正, 能够有效降低误差累计对定位结果带来的影响, 提升构建地图的一致性.
3) 在结构特征不明显的开阔室外环境, LeGO-LOAM等方法无法进行可靠的位姿估计, 而Inertial-LOAM仍能得到准确的定位结果; 同时, 在快速直角转弯处, Inertial-LOAM较依赖点云配准的LeGO-LOAM具有更为稳定可靠的表现.
-
图 9 室外开阔环境IL/LL/L/C轨迹结果对比
Fig. 9 Comparison of pose estimation of IL/LL/L/C in outdoor environment
表 1 累计误差结果
Table 1 Error accumulation result
场景 方法 横滚 (°) 俯仰 (°) 航向 (°) 角度偏差 (°) X方向 (m) Y方向 (m) Z方向 (m) 位置偏差 (m) 2# 数据[11] IMU 0.748 1.018 0.598 1.398 35.095 84.652 −665.782 672.059 Cartographer 0.113 −0.709 0.989 1.222 0.405 1.317 0.670 1.532 LOAM 0.016 0.141 0.925 0.936 0.316 0.349 0.025 0.471 LeGO-LOAM 0.061 0.081 0.916 0.921 0.068 0.338 0.115 0.364 Inertial-LOAM 0.013 0.026 0.917 0.918 0.061 0.258 0.023 0.266 室内环境 Cartographer 0.003 −0.001 0.017 0.017 0.023 0.037 0.028 0.052 LOAM 0.001 0.004 0.068 0.068 0.032 0.083 0.032 0.095 LeGO-LOAM −0.006 −0.002 −0.021 0.022 0.016 0.047 −0.032 0.059 Inertial-LOAM −0.008 0.001 −0.020 0.021 0.021 0.043 0.027 0.055 室外环境 Cartographer 0.075 −0.024 0.081 0.113 1.747 2.592 −0.449 3.158 LOAM −0.031 0.006 0.096 0.101 0.0467 2.368 −0.065 2.353 LeGO-LOAM −0.024 −0.543 0.041 0.545 −19.857 −14.914 −0.355 24.836 Inertial-LOAM 0.006 −0.080 0.003 0.080 −0.310 −0.100 −0.030 0.328 
下载: 导出CSV
-
[1] 李帅鑫. 激光雷达/相机组合的3D SLAM技术研究 [硕士学位论文], 战略支援部队信息工程大学, 中国, 2018Li Shuai-Xin. Research on 3D SLAM Based on Lidar/Camera Coupled System [Master thesis], PLA Strategic Support Force Information Engineering University, China, 2018 [2] Besl P J, McKay N D. Method for registration of 3-D shapes. In: Proceedings Volume 1611, Sensor Fusion IV: Control Paradigms and Data Structures. Boston, MA, USA: SPIE, 1992. 586−606 [3] Pomerleau F, Colas F, Siegwart R, Magnenat S. Comparing ICP variants on real-world data sets. Autonomous Robots, 2013, 34(3): 133−148 doi: 10.1007/s10514-013-9327-2 [4] Surmann H, Nűchter A, Lingemann K, Hertzberg J. 6D SLAM-preliminary report on closing the loop in six dimensions. IFAC Proceedings Volumes, 2004, 37(8): 197−202 doi: 10.1016/S1474-6670(17)31975-4 [5] Moosmann F, Stiller C. Velodyne SLAM. In: Proceedings of the 2011 IEEE Intelligent Vehicles Symposium (IV). Baden-Baden, Germany: IEEE, 2011. 393−398 [6] Droeschel D, Schwarz M, Behnke S. Continuous mapping and localization for autonomous navigation in rough terrain using a 3D laser scanner. Robotics and Autonomous Systems, 2017, 88: 104−115 doi: 10.1016/j.robot.2016.10.017 [7] Droeschel D, Behnke S. Efficient continuous-time SLAM for 3D lidar-based online mapping. In: Proceedings of the 2018 IEEE International Conference on Robotics and Automation (ICRA). Brisbane, QLD, Australia: IEEE, 2018. 5000−5007 [8] Zhang J, Singh S. LOAM: Lidar odometry and mapping in real-time. In: Proceedings of Robotics: Science and Systems. Berkeley, CA, USA: 2014. [9] Zhang J, Singh S. Low-drift and real-time lidar odometry and mapping. Autonomous Robots, 2017, 41(2): 401−416 doi: 10.1007/s10514-016-9548-2 [10] Geiger A, Lenz P, Urtasun R. Are we ready for autonomous driving? The KITTI vision benchmark suite. In: Proceedings of the 2012 IEEE Conference on Computer Vision and Pattern Recognition. Providence, RI, USA: IEEE, 2012. 3354−3361 [11] Shan T X, Englot B. LeGO-LOAM: Lightweight and ground-optimized lidar odometry and mapping on variable terrain. In: Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Madrid, Spain: IEEE, 2018. 4758−4765 [12] Hess W, Kohler D, Rapp H, Andor D. Real-time loop closure in 2D LIDAR SLAM. In: Proceedings of the 2016 IEEE International Conference on Robotics and Automation (ICRA). Stockholm, Sweden: IEEE, 2016. 1271−1278 [13] Forster C, Carlone L, Dellaert F, Scaramuzza D. On-manifold preintegration for real-time visual-inertial odometry. IEEE Transactions on Robotics, 2017, 33(1): 1−21 doi: 10.1109/TRO.2016.2597321 [14] Sarvrood Y B, Hosseinyalamdary S, Gao Y. Visual-LiDAR odometry aided by reduced IMU. ISPRS International Journal of Geo-Information, 2016, 5(1): 3 doi: 10.3390/ijgi5010003 [15] Thrun S, Burgard W, Fox D. Probabilistic Robotics. Cambridge, MA: MIT Press, 2005. [16] Hening S, Ippolito C A, Krishnakumar K S, Stepanyan V, Teodorescu M. 3D LIDAR SLAM integration with GPS/INS for UAVs in urban GPS-degraded environments. In: AIAA Information Systems-AIAA Infotech@Aerospace. Grapevine, Texas: AIAA, 2017. [17] Dellaert F, Kaess M. Factor graphs for robot perception. Foundations and Trends® in Robotics, 2017, 6(1-2): 1−139 [18] Leutenegger S, Lynen S, Bosse M, Siegwart R, Furgale P. Keyframe-based visual-inertial odometry using nonlinear optimization. The International Journal of Robotics Research, 2015, 34(3): 314−334 doi: 10.1177/0278364914554813 [19] Konolige K, Grisetti G, Kűmmerle R, Burgard W, Limketkai B, Vincent R. Efficient sparse pose adjustment for 2D mapping. In: Proceedings of the 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. Taipei, China: IEEE, 2010. 22−29 [20] Kaess M, Ranganathan A, Dellaert F. ISAM: Incremental smoothing and mapping. IEEE Transactions on Robotics, 2008, 24(6): 1365−1378 doi: 10.1109/TRO.2008.2006706 [21] Indelman V, Williams S, Kaess M, Dellaert F. Factor graph based incremental smoothing in inertial navigation systems. In: Proceedings of the 15th International Conference on Information Fusion. Singapore: IEEE, 2012. 2154-2161 [22] Kaess M, Johannsson H, Roberts R, Ila V, Leonard J J, Dellaert F. iSAM2: Incremental smoothing and mapping using the Bayes tree. The International Journal of Robotics Research, 2012, 31(2): 216−235 doi: 10.1177/0278364911430419 [23] Qin T, Li P L, Shen S J. VINS-mono: A robust and versatile monocular visual-inertial state estimator. IEEE Transactions on Robotics, 2018, 34(4): 1004−1020 doi: 10.1109/TRO.2018.2853729 [24] Qin T, Shen S J. Online temporal calibration for monocular visual-inertial systems. In: Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Madrid, Spain: IEEE, 2018. 3662−3669 [25] Li S X, Li G Y, Zhou Y L, Wang L, Fu J Y. Real-time dead reckoning and mapping approach based on three-dimensional point cloud. In: proceedings of the 2018 China Satellite Navigation Conference. Harbin, China: Springer, 2018. 643−662 [26] Barfoot T D. State Estimation for Robotics. Cambridge: Cambridge University Press, 2017. [27] Zhang J, Singh S. Laser-visual-inertial odometry and mapping with high robustness and low drift. Journal of Field Robotics, 2018, 35(8): 1242−1264 doi: 10.1002/rob.21809 [28] Behley J, Stachniss C. Efficient surfel-based SLAM using 3D laser range data in urban environments. In: Proceedings of Robotics: Science and Systems. Pittsburgh, Pennsylvania, 2018. 期刊类型引用(20)
1. 李岩，施忠臣，侯燕青，戚煜华，谢良，陈伟，陈洪波，闫野，印二威. 行人惯性定位新动态:基于神经网络的方法、性能与展望. 自动化学报. 2025(02): 271-286 . 
本站查看2. 汪方斌，曹锟，龚雪，朱达荣，汪萍. 基于因子图优化的光伏电站场景激光雷达SLAM方法. 激光与光电子学进展. 2024(14): 165-173 . 百度学术3. 李佳益，马智亮，陈礼杰，季鑫霖. 面向施工机器人定位的多模态数据融合方法研究综述. 计算机工程与应用. 2024(15): 11-23 . 百度学术4. 汪湘川，张辉，陈波，周熙栋. 基于扫描上下文优化的紧耦合激光SLAM方法. 控制与决策. 2024(10): 3234-3242 . 百度学术5. 张瑞亮，吕刚，方明，李兵，罗宗源. 基于slam技术与位置融合的换流站主设备虚拟现实巡检方法. 计算技术与自动化. 2024(03): 153-158+172 . 百度学术6. 罗钒睿，刘振宇，任佳辉，李笑宇，程阳. 基于改进卡尔曼滤波的轻量级激光惯性里程计. 浙江大学学报(工学版). 2024(11): 2280-2289 . 百度学术7. 薛光辉，李瑞雪，张钲昊，刘睿. 基于3D激光雷达的SLAM算法研究现状与发展趋势. 信息与控制. 2023(01): 18-36 . 百度学术8. 朱东莹，钟勇，杨观赐，李杨. 动态环境下视觉定位与建图的运动分割研究进展. 计算机应用. 2023(08): 2537-2545 . 百度学术9. 刘宇，袁正，陈燕苹，彭慧. 基于鲁棒增强的因子图多源信息融合算法. 电子质量. 2023(08): 27-32 . 百度学术10. 钱闯，张红娟，李文卓，刘晖，李必军. LiDAR标签和栅格占有图结合的LiDAR/IMU空间标定方法. 测绘学报. 2023(09): 1469-1479 . 百度学术11. 崔玉明，刘送永，吕振礼，李洪盛，王崧全. 基于级联优化和强度特征的地下退化环境机器人自主精准定位. 仪器仪表学报. 2023(12): 208-216 . 百度学术12. 徐爱功，孟祥鹤，高嵩，陈志键. UWB/LiDAR紧组合室内定位方法. 测绘科学. 2022(04): 1-9+18 . 百度学术13. 刘雷，柏艳红，孙志毅，赵兵洋. 融合IMU的多帧点云场景配准方法. 激光与红外. 2022(08): 1246-1250 . 百度学术14. 李楠，向文豪. 城市环境中无人机作战导航定位研究现状综述. 无人系统技术. 2022(04): 75-87 . 百度学术15. 徐爱功，程安莉，隋心，陈志键，王思语，高佳鑫. 室内退化场景下UWB双基站辅助LiDAR里程计的定位方法. 导航定位学报. 2022(05): 1-9 . 百度学术16. 李景文，韦晶闪，陆妍玲，姜建武，朱明，叶波，张英南. 激光雷达和行人航迹推算融合的室内目标定位方法. 科学技术与工程. 2022(25): 11068-11074 . 百度学术17. 常耀辉，陈年生，饶蕾，程松林，范光宇，宋晓勇，杨定裕. 动态环境下具有旋转和平移不变性的激光雷达点云描述子. 光学学报. 2022(24): 63-73 . 百度学术18. 庞帆，危双丰，师现杰，陈凯. 激光雷达惯导耦合的里程计与建图方法. 计算机应用研究. 2021(07): 2188-2193+2199 . 百度学术19. 孙喜亮，关宏灿，苏艳军，徐光彩，郭庆华. 面向高精度城市测绘的激光紧耦合SLAM方法. 测绘学报. 2021(11): 1585-1593 . 百度学术20. 李帅鑫，李九人，田滨，陈龙，王力，李广云. 面向点云退化的隧道环境的无人车激光SLAM方法. 测绘学报. 2021(11): 1487-1499 . 百度学术其他类型引用(35)
-
下载:
计量
- 文章访问数: 8916
- HTML全文浏览量: 4342
- PDF下载量: 672
- 被引次数: 55
