激光雷达与惯性测量单元同步融合下的园区三维建图

针对自动驾驶三维建图中存在的建图不准确以及位姿飘移的问题，利用激光雷达里程计消除惯性测量单元（Inertial Measurement Unit， nIMU）累计误差并通过IMU预积分去除激光雷达点云畸变，形成激光雷达与IMU的紧耦合建图系统；通过增加回环检测因子、激光雷达里程计因子以及IMU预积分因子进行后端图优化，旨在提升定位建图的全局一致性，减小位姿估计误差，降低累计漂移误差。最后，在学校园区实地场景以及利用开源数据集KITTI进行实验验证，实验表明，在选取的学校园区实地场景下，改进算法APE误差均值相较于原算法降低了11.04%，APE均方根误差较于原算法降低了17.35%；改进算法在KITTI数据集场景下平均APE误差下降了10.04%，均方误差方面相较于原算法平均下降了12.04%。研究结果表明，改进的建图方法能够有效提高建图的位姿估计精度与地图构建精度。

Abstract

To address the issues of inaccurate mapping and position drift in 3D autonomous driving maps， LIDAR odometry was utilized to counteract cumulative errors of the inertial measurement unit（IMU）， and corrections for LIDAR point cloud distortions were made through IMU pre-integration. This approach enabled the creation of a mapping system where LIDAR and IMU were tightly integrated. Subsequently， the back-end map was enhanced by the incorporation of loopback detection， LIDAR odometry， and IMU pre-integration factors， aiming to bolster the global consistency of the positioning map and minimize cumulative drift errors. The optimization of the back-end map sought to enhance global localization consistency， reduce positioning errors， and curtail cumulative drift. Experimental validation was conducted in a school campus environment and with the use of the KITTI open-source dataset. The results demonstrate that in the school campus scenario， an 11.04% reduction in average APE error and a 17.35% decrease in RMSE are achieved by the refined algorithm compared to the baseline algorithm. For the KITTI dataset scenario， a reduction of 10.04% in both average APE error and RMSE， and a 12.04% decrease in mean square error are observed， underscoring the efficacy of the enhanced mapping technique in elevating position estimation and map construction precision.

1 引言

随着自动驾驶技术的不断发展，同步定位与建图（Simultaneous Localization and Mapping， SLAM）技术逐步成熟。SLAM技术是指通过车辆在未知环境中动态构建的空间特征从而进行自身位姿估计，实现同步定位和增量式建图的目标。

随着一个在未知位置和未知环境中移动的机器人能否在建立环境地图的同时确定他自身的位置的问题被提出，SLAM技术不断发展，已经从单传感器发展为多传感器SLAM。SLAM技术应用的传感器主要包括激光雷达与视觉传感器^［1］。陈龙等^［2］通过对比激光雷达SLAM与视觉SLAM的定位建图效果，发现激光SLAM比视觉SLAM更具备稳定性。有研究发现惯性测量单元（Inertial Measurement Unit， IMU）与3D激光雷达有很好的互补性^［3］。Ji等^［4］提出的LOAM算法框架通过低频高精的后端与高频低精的前端来提升SLAM效能，但LOAM算法无法使用IMU进行整个系统的优化。Gilmar等^［5］将LOAM算法进行改进，提出了适用性更好的EKF-SAM算法，但它无法进行回环检测，也无法融入绝对测量数据进行位姿校正，而且LOAM存在大场景测试中偏移的问题。Michael等^［6］提出了激光雷达与IMU融合的Fast-SLAM 2.0算法，该方法增强了设备位于不平坦区域的建图能力。黄粒等^［7］通过改进后端优化提高计算效率，取得了较低的轨迹误差、绝对轨迹误差和相对位姿误差，提高了同步定位与建图的精度。杨智宇等^［8］利用因子图联合优化共同提高位姿估计精度。王挺等^［9］运用GR-LOAM算法，通过激光雷达和IMU等多传感器融合来提高建图效果，由于传感器本身存在精度误差，多传感器融合容易导致累计误差的产生。尹芳等^［10］针对模型未知的空间非合作旋转目标的模型重建和位姿估计问题，利用激光雷达采集的3D点云，提出一种基于位姿图优化的SLAM技术框架，以解决跟踪过程中产生的累积误差问题。杨林等^［11］提出了一种基于激光惯性的融合SLAM算法，实现了移动机器人全局一致的同步定位与地图构建。针对激光雷达与IMU融合，一些改进SLAM算法，如Fast-LIO^［12］与Fast-LIO2^［13］被提出。徐伟等^［14］通过评估Fast-LIO2算法以及其数据结构iKD-tree，体现Fast-LIO2算法的优越性。Fast-LIO2在Fast-LIO的基础上在前端采用增量KD-tree（iKD-tree），后端采用迭代误差状态Kalman Filter，但iKD-tree是一个静态数据结构，当需要频繁进行数据集更新时，需要重新构建整个iKD-tree，算法运行效率会降低。高翔等^［15］在Fast-LIO2的基础上进行改进，用增量体素（ivoxel）取代iKD-tree，实现了同等的定位和建图精度，并取得了更快的运行速度。马艾强等^［16］提出了一种面向煤矿巷道环境的激光雷达与IMU融合的实时定位与建图方法。徐晓苏等^［17］提出了一种面向室外环境的基于快速回环检测的SLAM算法，提升了回环检测的精度与速度。陈志强等^［18］提出了一个完整的激光雷达SLAM框架SCL-SLAM，将回环检测模块与扫描邻近帧集成到紧密耦合的激光雷达惯性里程计FAST-LIO2中。目前，有些优秀算法缺少后端优化与回环检测。如果没有回环检测来修正轨迹估计中的误差，SLAM算法的轨迹会使机器人的位姿估计逐渐偏离真实轨迹，导致SLAM系统的性能逐渐下降，甚至无法有效地完成定位和建图任务。

激光SLAM与IMU在同步定位与构图导航应用方面各有优势，有着较为良好的互补性，但同时定位与建图的误差问题始终存在，部分算法缺少后端优化与回环检测，导致算法存在全局一致性较弱或者累计误差过大的问题。本文提出一种改进的激光雷达与IMU融合SLAM方法用以提高位姿估计精度和地图构建精度。

2 激光雷达IMU融合的建图系统分析

2.1　SLAM模型分析

SLAM数学模型可以使用贝叶斯滤波理论来描述^［19］。激光雷达的测量模型为距离-方位角-俯仰角模型（即距离-方位角-俯仰角模型），如式（1）所示：

(\begin{array}{l} R \\ α \\ ω \end{array}) = (\begin{array}{l} \sqrt[]{{(x - l_{x})}^{2} + {(y - l_{y})}^{2} + {(z - l_{z})}^{2}} \\ a r c t a n 2 (l_{y} - y, l_{x} - x) \\ a r c t a n 2 (l_{z} - z, \sqrt[]{{(x - l_{x})}^{2} + {(y - l_{y})}^{2}}) \end{array})

其中： $(\begin{matrix} x, y, z, α, ω \end{matrix})$ 表示机器人位姿， $(\begin{matrix} l_{x}, l_{y}, l_{z} \end{matrix})$ 表示地标点位置， $α$ 表示测量的地标点相对于机器人的方位角， $ω$ 表示激光雷达测量的地标点相对于机器人的俯仰角。该测量模型是一种非线性模型，需要通过优化算法来估计机器人位姿和地标点的位置。

IMU用于测量当前时刻的角速度和加速度值。IMU加速度计模型及陀螺仪模型如式（2）表示：

\{\begin{array}{l} a_{m} = R_{b}^{m} (a_{b} - b_{a}) + n_{a} \\ ω_{m} = R_{b}^{m} (ω_{b} - b_{ω}) + n_{ω} \end{array}

其中： $b_{a}$ 和 $b_{ω}$ 表示加速度计和陀螺仪的偏置向量， $R_{b}^{m}$ 表示从物体坐标系到IMU安装坐标系的旋转矩阵， $n_{a}$ 和 $n_{ω}$ 表示加速度计和陀螺仪的随机噪声向量。

2.2　Fast-LIO2算法

Fast-LIO2以高效紧耦合的迭代卡尔曼滤波器（Extended Kalman Filter， EKF）为基础，将原始点云配准在地图中，利用环境中的细微特征提高数据的准确性，通过增量数据结构iKD-tree来维护地图。对于状态转移模型而言，以第一帧IMU帧作为系统初始化输入，其初始化全局坐标系为G，并且以 $T_{L} = (R_{L}, P_{L})$ 代表激光雷达和IMU之间的外参，其运动学方程如公式（3）所示：

\{\begin{array}{l} {\dot{R}}_{I} = R_{I} ⌊ω_{m} - b_{ω} - n_{ω}⌋ \\ {\dot{v}}_{I} = R_{I} (a_{m} - b_{a} - n_{a}) + g \\ {\dot{b}}_{ω} = n_{b ω}, {\dot{b}}_{a} = n_{b a}, {\dot{p}}_{I} = v \end{array}

其中：P_I和R_I分别表示IMU在全局坐标系下的位置和姿态， $n_{a}$ 和 $n_{ω}$ 分别表示测量值 $a_{m}$ 和 $ω_{m}$ 中的噪声； $n_{b a}$ 和 $b_{a}$ 表示 $a_{m}$ 中的零偏， $b_{ω}$ 和 $n_{b w}$ 表示 $ω_{m}$ 的零偏，其离散方程为：

\{\begin{array}{l} x_{i + 1} = x_{i} \oplus (Δ t f (x_{i}, u_{i}, w_{i})) \\ x = {[\begin{array}{l} R_{I}^{T} & p_{I}^{T} & v_{I}^{T} & b_{ω}^{T} & b_{a}^{T} & g^{T} & R_{L}^{T} & p_{L}^{T} \end{array}]}^{T} \in M \\ w = {[\begin{array}{l} n_{ω}^{T} & n_{a}^{T} & n_{b ω}^{T} & n_{b a}^{T} \end{array}]}^{T} \\ u = {[\begin{array}{l} ω_{m}^{T} & a_{m}^{T} \end{array}]}^{T} \end{array}

其中：x表示状态，u表示输入，w表示噪声。利用反向传播修正扫描帧的运动，见式（5）所示：

\{\begin{array}{l} p_{j}^{g t} = p_{j} + n_{j} \\ u_{j}^{T} (T_{I_{k}} T_{L} (p_{j} + n_{j}) - q_{j}) = 0 \end{array}

其中：k表示激光雷达扫描帧中的索引；p_j表示第k个扫描帧在激光雷达局部坐标系以及扫描帧结束时间的采样点，每个测量点p_j都包含了n_j的距离和方向；剔除噪声后即是激光雷达局部坐标系中真正的点 $p_{j}^{g t}$ ；T_Lk表示相应的激光雷达姿态，T_L表示外参； $u_{j}^{T}$ 表示对应平面的法向量；q_j表示该平面的一个点。

基于上述的状态转移模型和观测模型，使用迭代卡尔曼滤波。当IMU数据到来时则前向传播，过程噪声 $w_{i} = 0$ ，前向传播状态和协方差如下：

\{\begin{array}{l} {\hat{x}}_{i + 1} = {\hat{X}}_{i} \oplus (Δ t f ({\hat{x}}_{i}, u_{i}, 0)); {\hat{x}}_{0} = {\bar{x}}_{k - 1} \\ {\hat{P}}_{i + 1} = F_{x_{i}} {\hat{P}}_{i} F_{x_{i}}^{T} + F_{w_{i}} Q_{i} F_{x_{i}}^{T}; {\hat{P}}_{0} = {\bar{P}}_{k - 1} \\ F_{x_{i}^{'}} = \frac{\partial (x_{i + 1} - {\hat{x}}_{i + 1})}{\partial x_{i}^{'}} |_{x_{i}^{'} = 0, w_{i} = 0} \\ F_{w_{i}} = \frac{\partial (x_{i + 1} - {\hat{x}}_{i + 1})}{\partial w_{i}} |_{x_{i} = 0, w_{i} = 0} \end{array}

其中： $\hat{x}$ 表示预测状态，Q_i表示w_i协方差， ${\bar{P}}_{k - 1}$ 表示协方差矩阵， ${\hat{P}}_{i}$ 表示真值x_k与状态传播值 ${\hat{x}}_{k}$ 之间误差的协方差， $F_{x^{'}}$ 表示状态转移矩阵， $F_{w_{i}}$ 表示噪声转移矩阵。将观测模型进行泰勒展开得到：

\{\begin{array}{l} h_{j} (x_{k},^{L} n_{j}) ≃ h_{j} ({\hat{x}}_{k}^{k}, 0) + H_{j}^{k} x_{k}^{k} + v_{j} = z_{j}^{κ} + H_{j}^{κ} x_{k}^{κ} + v_{j} \\ z_{j}^{κ} = h_{j} ({\hat{x}}_{k}^{k}, 0) = u_{j}^{T} (^{G} {\hat{T}}_{I_{k}}^{k I} {\hat{T}}_{L_{k}}^{k}^{L} p_{j} -^{G} q_{j}) \end{array}

其中： $x^{'}_{k}^{k} = x_{k} - {\hat{x}}_{k}^{k}$ ，表示 $H_{j}^{k}$ 在 $H_{j} ({\hat{x}}_{k}^{k} \oplus x^{'}_{k}^{k},^{L} n_{j})$ 的雅可比矩阵，近似于0， $z_{j}^{κ}$ 表示残差。上述处理过程不断重复直到收敛。收敛后，最优状态为 ${\bar{x}}_{k} = x^{'}_{k}^{k + 1}$ ，协方差估计为 ${\bar{P}}_{k} = (I - K H) P$ 。基于最优状态 ${\bar{x}}_{k}$ ，在第k个扫描帧中的每个激光雷达点，p_i变换到全局坐标系下，得到：

{\bar{p}}_{j} = {\bar{T}}_{_{L}}^{- 1} {\bar{T}}_{I_{j}} {\bar{T}}_{L} p_{j}; j = 1, \dots, m

其中： ${\bar{T}}_{I_{j}}$ 表示激光雷达p_j时刻与t_k时刻的相对位姿。Fast-LIO2算法存在累计误差无法消除的缺陷，不能适应太大场景下的建图工作。此外，该算法主要关注局部的点云数据和地图信息，对于全局一致性的建模和定位存在一定的局限性。在长时间和大范围的移动中，Fast-LIO2算法难以保持全局一致性和准确性，因此改进时沿用Fast-LIO2优秀的前端点云提取策略，在算法后端中构建因子引入GTSAM图，优化增强SLAM构图的全局一致性；变更iKD-tree地图数据维护类型为iVox结构，增强其增量式地图维护速度；最终引入ICP回环检测，对所建点云地图进行回环修正消除累计误差。

3 改进的激光雷达IMU融合建图算法

3.1　Fast-LIO2算法的后端优化

对Fast-LIO2算法引入GTSAM后端图优化，增强激光雷达与IMU的融合精度以提高SLAM构图的全局一致性。GTSAM为解决SLAM中出现的非线性优化问题提供了一个框架，其优化过程为调用因子图以及相应图节点的值，计算因子（观测约束）的残差，因子对相关图节点的雅可比矩阵，完成迭代优化，以提高自动驾驶车辆的定位精度和稳定性。改进算法的因子图结构如图1所示。

图 1. 改进算法的因子图结构

Fig. 1. Graph structure improved algorithm factor

下载图片查看所有图片

在GTSAM中，IMU预积分因子是一种重要的优化因子，车辆在 $t + Δ t$ 时刻的速度 $v_{t + Δ t}$ 、位置 $p_{t + Δ t}$ 和旋转 $R_{t + Δ t}$ 的计算公式如下：

\{\begin{array}{l} v_{t + Δ t} = v_{t} + g Δ t + R_{t} ({\hat{a}}_{t} - b_{t}^{a} - n_{t}^{a}) \\ Δ t p_{t + Δ t} = p_{t} + v_{t} Δ t + \frac{1}{2} g Δ t^{2} + \frac{1}{2} R_{t} ({\hat{a}}_{t} - b_{t}^{a} - n_{t}^{a}) \\ Δ t^{2} R_{t + Δ t} = R_{t} e x p (({\hat{ω}}_{t} - b_{t}^{ω} - n_{t}^{ω}) Δ t) \end{array}

其中： ${\hat{a}}_{t}$ 表示IMU在t时刻的原始测量值， $b_{t}^{a}$ 表示零漂， $n_{t}^{a}$ 表示白噪声， $R_{t}$ 表示t时刻世界坐标系到机体坐标系的旋转矩阵，g表示世界坐标系下的重力加速度。在积分过程中，基于IMU的预积分获取两个时间戳之间的相对位姿。在两个时刻差 $Δ t$ 之间，预积分 $Δ v_{i j}$ ， $Δ p_{i j}$ 和 $Δ R_{i j}$ 分别为：

\{\begin{array}{l} Δ v_{i j} = R_{i}^{T} (v_{j} - v_{i} - g Δ t_{i j}) \\ Δ p_{i j} = R_{i}^{T} (p_{j} - p_{i} - v_{i} Δ t_{i j} - \frac{1}{2} g Δ t_{i j}^{2}) \\ Δ R_{i j} = R_{i}^{T} R_{j} \end{array}

IMU预积分提供IMU预积分因子，建立IMU预积分因子首先建立IMU模型，如式（11）所示：

{ω^{'}}^{b} = ω^{b} + b^{g} + n^{g} {a^{'}}^{b} = q_{b w} (a^{w} + g^{w}) + b^{a} + n^{a}

其中： $ω^{b}$ 表示IMU坐标系中角速度的真实值， $a^{w}$ 表示加速度真实值， $b^{g}$ ， $b^{a}$ 表示IMU偏置， $n^{g}$ ， $n^{a}$ 表示测量值的噪声，p表示位置，v表示速度，q表示姿态。根据积分关系，得到迭代公式：

\{\begin{array}{l} p_{w b_{j}} = p_{w b_{i}} + v_{i}^{w} Δ t - \frac{1}{2} g^{w} Δ t^{2} + q_{w b_{i}} \iint_{t \in [i, j]}^{} (q_{b_{i} b_{t}} a^{b_{t}}) δ t^{2} \\ v_{j}^{w} = v_{i}^{w} - g^{w} Δ t + q_{w b_{i}} \int_{t \in [i, j]} (q_{b_{i} b_{t}} a^{b_{t}}) δ t \\ q_{w b_{j}} = q_{w b_{i}} \int_{t \in [i, j]} q_{b_{i} b_{t}} \otimes [\begin{matrix} 0 \\ \frac{1}{2} ω^{b_{t}} \end{matrix}] δ t \end{array}

根据i时刻的位置 $p_{w b_{i}}$ ，速度 $v_{i}^{w}$ 以及姿态 $q_{w b_{i}}$ 进行积分得到j时刻 $p_{w b_{j}}$ ， $v_{j}^{w}$ 以及 $q_{w b_{j}}$ 。定义预积分量并且移项整理，构建残差并离散化，根据EKF进行泰勒展开可得：

\{\begin{array}{l} x_{k} = f (x_{k - 1}, u_{k - 1}) \\ {\hat{x}}_{k} + δ x_{k} = f ({\hat{x}}_{k - 1} + δ x_{k - 1}, {\hat{u}}_{k - 1} + n_{k - 1}) \\ {\hat{x}}_{k} + δ x_{k} = f ({\hat{x}}_{k - 1}, u_{k - 1}) + F δ x_{k - 1} + G n_{k - 1} \end{array}

其中：F，G分别表示方程 $f (x_{k - 1}, u_{k - 1})$ 在 ${\hat{x}}_{k - 1}$ 处噪声项为0处的雅可比矩阵。预积分量的协方差矩阵可以通过F，G雅可比矩阵进行传播。

激光里程因子是一种基于激光数据的因子，对于特征点云与其对应的边缘或平面特征之间的距离为：

\{\begin{array}{l} d_{e_{k}} = \frac{|(p_{i + 1, k}^{e} - p_{i, u}^{e}) \times (p_{i + 1, k}^{e} - p_{i, v}^{e})|}{|p_{i, u}^{e} - p_{i, v}^{e}|} \\ d_{p_{k}} = \frac{|\begin{matrix} (p_{i + 1, k}^{p} - p_{i, u}^{p}) \\ (p_{i, u}^{p} - p_{i, v}^{p}) \times (p_{i, u}^{p} - p_{i, w}^{p}) \end{matrix}|}{|(p_{i, u}^{p} - p_{i, v}^{p}) \times (p_{i, u}^{p} - p_{i, w}^{p})|} \end{array}

使用主成分分析（Pricipal Component Analysis， PCA）提取点、线和平面特征。定义几何特征的局部线性度 $σ_{1 D}$ 、平面度 $σ_{2 D}$ 和曲率 $σ_{c}$ 分别为：

\{\begin{array}{l} σ_{1 D} = \frac{σ_{1} - σ_{2}}{σ_{1}} \\ σ_{2 D} = \frac{σ_{2} - σ_{3}}{σ_{1}} \\ σ_{c} = \frac{σ_{3}}{σ_{1} + σ_{2} + σ_{3}} \end{array}

其中 $σ_{i} = \sqrt[]{λ_{i}}$ ， $λ_{i}$ 表示PCA正常计算的特征值。根据式（15）及原理，对法线方向，点、线和平面特征 $F_{i + 1} = \{F_{i + 1}^{p o}, F_{i + 1}^{l i}, F_{i + 1}^{p l}\}$ 进行分类。期望估计联合最小化点到点、点到线和点到平面距离误差度量的最佳变换，如下：

\{\begin{array}{l} e_{i}^{p o \to p o} = q_{i} - (R p_{i} + t), & q_{i}, p_{i} \in F_{i + 1}^{p o} \\ e_{j}^{p o \to l i} = v_{j} \times (q_{j} - (R p_{j} + t)), & q_{j}, p_{j} \in F_{i + 1}^{l i} \\ e_{k}^{p o \to p l} = n_{k}^{T} (q_{k} - (R p_{k} + t)), & q_{k}, p_{k} \in F_{i + 1}^{p l} \end{array}

其中： $e_{i}^{p o \to p o}$ ， $e_{j}^{p o \to l}$ ， $e_{k}^{p o \to p l}$ 分别表示点到点、点到直线以及点到平面的距离；v_j，n_k表示特征的相应线特征方向向量和面特征法线方向； $t$ 表示期望估计的最佳变换。利用莱文贝格-马夸特算法（Levenberg-Marquardt Algorithm， LM）最小化式（16）来求解最佳变换得到：

m i n_{T_{i + 1}} \{\begin{array}{l} \sum_{p \in F_{i + 1}^{p o}} w_{i} e_{i + 1}^{p o \to p o} + \sum_{p \in F_{i + 1}^{l i}} w_{i} e_{i + 1}^{p o \to l i} + \\ \sum_{p \in F_{i + 1}^{p l}} w_{i} e_{i + 1}^{p o \to p l} + \sum_{(i, i + 1) \in B} w_{i} e^{i m u} + e_{i m u o d o m}^{p r i o r} \end{array}\}

其中： $e_{i m u o d o m}^{p r i o r}$ ， $e^{i m u}$ 表示IMU里程计提供的预测姿态约束和IMU预积分因子。假设车辆在时间步t和t+k分别观测到同一地点X_i，则可以构建一个回环检测因子，如下：

e_{i, t, t + k} = ‖{T_{t}}^{- 1} T_{t + k} X_{i} - X_{i}‖

其中： $T_{t}$ 和 $T_{t + k}$ 分别表示车辆在时间步 $t$ 和 $t + k$ 时的位姿变换矩阵， $X_{i}$ 表示地图上的点。

基于稀疏体素的近邻结构（incremental Voxels， iVox）与LIO算法更加匹配^［15］，而iVox中伪希尔伯特空间曲线（Pseudo-Hilbert Curve， PHC）在降低点云配准耗时的同时不影响SLAM的精度表现。因此，通过给iVox局部地图添加最近最少使用缓存（Least Recently Used， LRU），以克服遍历整个iVox带来的时效问题。通过调整iVox局部地图的容量阈值，并在添加新地图数据时筛选出使用频率较小的体素以移除，以降低iVox遍历的复杂度。

3.2　SLAM算法评价指标

视觉里程计（Estimation， Validation， and Optimization， EVO）评价工具是众多学者广泛认可的SLAM系统性能评价工具^［20］，主要评价参数为绝对位姿误差（Absolute Pose Error， APE）与相对位姿误差（Relative Pose Error， RPE）。APE可以直观地反映SLAM算法精度和轨迹的全局一致性。如下：

\{\begin{array}{l} E_{g, i} = l n {(T_{e, i + Δ} \cdot T_{t, i}^{- 1})}^{V} = - l n {(T_{t, i}^{- 1} \cdot T_{t, i + Δ})}^{V} \\ E_{A P} = \sqrt[]{\frac{1}{N} \sum_{i = 1}^{N} {‖l n {(T_{t, i}^{- 1} \cdot T_{e, i})}^{V}‖}_{2}^{2}} \end{array}

其中：V表示位姿对之间的相对平移向量，N表示位姿个数， $T_{t, i}^{}$ 和 $T_{e, i}$ 分别表示真实轨迹和SLAM系统估计出的轨迹，E_g，i表示为误差求解出不包含位姿旋转角的APE参数，如下：

E_{A P} = \sqrt[]{\frac{1}{N} \sum_{i = 1}^{N} {‖t r a n s (T_{t, i}^{- 1} \cdot T_{e, i})‖}_{2}^{2}}

其中trans表示变量平移部分。RPE反映SLAM算法的局部精度，计算相隔 $Δ t$ 时间的两帧之间位姿之差，代表相应轨迹段的相对误差，如下：

{E_{R P}}_{} = \sqrt[]{\frac{1}{N - Δ t} \sum_{i = 1}^{N - Δ t} ‖ l n ({(T_{t, i}^{- 1} \cdot T_{g i, i + Δ})}^{- 1} {(T_{e, i}^{- 1} \cdot T_{e, i + Δ})}^{V} ‖_{2}^{2}}

其中：E_AP误差的均方根误差（Root Mean Squared Error， RMSE）、误差均值（E_Mean）、最大误差（E_Max）以及最小误差（E_Min）用于评价预测值与真实值之间的差异，分别为：

\{\begin{array}{l} E_{R M S} = \sqrt[]{\frac{1}{N} \sum_{i = 1}^{N} {‖t r a n s (T_{t, i}^{- 1} \cdot T_{e, i})‖}_{2}^{2}} \\ E_{M e a n} = \sqrt[]{\frac{1}{N} \sum_{i = 1}^{N} (|E_{A P}|)} \\ E_{M a x} = m a x (|E_{A P}|) \\ E_{M i n} = m i n (|E_{A P}|) \end{array}

此外有学者提出众多类似于点云在距离真值一定距离处的点的累积百分比或离散程度来评价点云建图性能^［4］。

4 实验及结果分析

4.1　激光雷达IMU融合建图

实验平台为Apollo D-KIT-lite自动驾驶开发套件，采用的开发套件以及建图算法框架如图2所示。实验场地选取为学校园区第一教学楼外。改进算法建图过程与生成点云地图的部分细节如图3所示。改进后生成的总体地图效果优异。同一数据集分别采用原始算法与改进算法进行建图，生成的点云地图与地图细节对比如图4所示。

图 2. 激光雷达IMU融合建图实验平台与系统框架

Fig. 2. Experimental platform and system framework for mapping based on fusion of lidar and IMU

下载图片查看所有图片

图 3. 改进算法的建图过程与效果

Fig. 3. Improved algorithm building process and renderings

下载图片查看所有图片

图 4. Fast-LIO2改进前后的建图对比

Fig. 4. Comparison of rendering before and after improvement of Fast-LIO2

下载图片查看所有图片

对比图4（a）与图4（b）可以明显看出，改进算法的整体建图效果优于原算法。如图4（c）所示，原算法点云重叠导致地图边缘点云厚度异常，改进算法的轮廓纤细清晰，如图4（d）所示。在地面点云上，图4（e）出现点云运动畸变，图4（f）地面平整。

EVO对SLAM效果进行对比，如图5所示（彩图见期刊电子版）。数据集中的GPS定位信息为时间戳经纬度高程信息，需要通过解算与对齐（所有传感器数据通过相应的TF变化到同一坐标系）后才能用于地面真值作为后续SLAM算法位姿估计的基准。图5（b）、图5（c）与图5（d）中虚线为GPS差分得到的真实轨迹参考、蓝色为改进前Fast-LIO2算法、绿色与红色为改进后算法（其中绿色未将回环检测因子加入图优化、红色为加入回环检测），可以发现改进算法与原算法相比，无论是否增加回环检测，改进算法的轨迹估计更接近GPS提供的轨迹真值。

图 5. SLAM位姿估计

Fig. 5. SLAM pose estimation

下载图片查看所有图片

4.2　实验评价分析

通过APE与RPE对改进前后的融合算法进行对比分析，结果如图6所示。

图 6. SLAM轨迹估计的误差分析

Fig. 6. Error analysis of SLAM trajectory estimation

下载图片查看所有图片

对比图6（a）和图6（b），改进后算法的位姿估计误差变低。图6（c）和图6（d）表明，改进算法将绝对误差的Mean、RMSE、误差中位数（midian）以及标准差（std）限定在更小的范围内；而图6（e）和图6（f）表明，RPE误差所代表的局部误差在算法改进前后没有明显区别。

为了更好地增强园区的三维建图效果，通过添加回环检测对点云地图进行回环修正消除累计误差，利用APE与RPE对添加回环检测的改进后融合算法进行分析，结果如图7所示。

图 7. 回环检测误差分析

Fig. 7. Loopback detection error analysis

下载图片查看所有图片

对比图6和图7，在改进算法中添加回环检测后位姿估计误差变低，Mean，RMSE，midian以及std范围变小。根据APE误差进行分布对比分析与误差箱型图，得到具体的误差分布规律，APE误差分布如图8所示。

图 8. 绝对误差分布

Fig. 8. Absolute error distribution

下载图片查看所有图片

由图8（a）可知，改进算法比原算法的APE误差分布更为平均，没有出现极其突出的误差。由图8（b）可知，改进算法没有存在异常值并将误差分布限制在更小的范围内。对录制的数据集进行bag包的倍速播放再进行重复的误差分析，如图9所示。

图 9. 倍速实验的APE误差对比

Fig. 9. Comparison of APE error of double speed experiment

下载图片查看所有图片

图9分析得到，改进前算法的各项误差受倍速实验干扰时的影响较大，而改进后算法的误差都较为稳定。以z轴高度为颜色映射能直观地看出建图的高程效果，如图10所示。以z轴为分割映射，调整点云颜色赋值为z轴并将蓝色设置为点云z轴坐标最低于1.5 m处的点云，总体上随着高度变高颜色由蓝色变为红色，这样的操作可以确定蓝色部分为地面及其接近地面的点云，红色部分为其他高处环境点云，改进后算法点云在水平基准0 m存在更多分布（SLAM开始时，以起点处为高程为0）。

图 10. 点云地图及Z轴映射分析结果

Fig. 10. Point cloud map and Z-axis map analysis result

下载图片查看所有图片

采用Cloudcompare对改进前后算法生成的点云地图进行ICP配准后，选取基准面并将截取的基准面点云分割切片，操作流程如图11所示。将点云信息输出，去除反射强度时间戳等信息后只保留位置坐标信息进行分析，拟合线点云离散分布如图12所示（彩图见期刊电子版）。

图 11. 拟合线信息提取过程

Fig. 11. Process of fitting line information extraction

下载图片查看所有图片

图 12. 点云的离散性分布分析

Fig. 12. Discrete distribution analysis of point cloud

下载图片查看所有图片

点云的拟合误差（位置坐标误差）是指拟合后的点云与实际点云之间的位置差异。位置坐标误差是一个相对值，它表示拟合后的点云与实际点云之间的差异。根据图12的拟合线点云集合可以看出，大部分改进后的拟合线周围点云（蓝色）比改进前算法点云（红色）在它们各自拟合线附近有更多分布，同时分布区间更小更密集，集中在拟合线附近。通过分析误差也可以得出同样的结论，拟合线点云分布的各类误差与离散程度参数见表1。

表 1. 拟合线误差的分析结果

Table 1. Analysis result of fitting line error

序号	S（σ）	S（σ）'	RSS	RSS'	R²	R²'
拟合线1	0.000 995	0.000 872	1.739 34	1. 53 151	0.819 28	0.822 63
拟合线2	0.003 280	0.003 030	4.385 92	3.936 75	0.633 21	0.659 77
拟合线3	0.005 830	0.005 240	4.308 21	3.901 56	0.336 33	0.366 64
拟合线4	0.010 340	0.007 250	19.496 27	17.328 15	0.789 32	0.879 45
拟合线5	0.017 540	0.006 990	15.454 81	7.481 31	0.998 07	0.999 21
拟合线6	0.000 720	0.000 654	2.527 03	2.238 36	0.963 55	0.970 43

查看所有表

如表1所示，改进后的算法在各个拟合线上的点云偏离拟合线的均方差（S（σ）'）和残差平方和（RSS'）相比于原算法的S（σ）和RSS上都有改善，同时在评价偏离程度的参数上，改进算法的R²更接近1，表明在建图细节上改进算法比原算法的点云地图更加平整规律，一致性更好。

选取KITTI数据集00，05，07，09序号数据集进行仿真，所有场景对比APE情况如表2所示。在4个KITTI数据集上改进算法均优于其他算法，缩小了APE的误差区间，改进算法的Mean相较于原Fast-LIO2算法，在KITTI数据集00，05，07和09号场景下分别降低了12.09%，8.36%，7.50%和12.21%，平均下降了10.04%，改进算法的RMSE相较于原算法在00，05，07和09号场景下分别降低了12.56%，14.56%，8.85%和12.19%，平均下降了12.04%。在选取的学校园区实地场景下，改进算法的Mean较原算法降低了11.04%，改进算法的RMSE'较原算法降低了17.35%。

表 2. 算法APE误差对比

Table 2. Comparison of APE error

场景	Min	Min'	Max	Max'	Mean	Mean'	RMSE	RMSE'
Seq.00	0.525	0.455	8.245	8.110	4.356	3.829	4.721	4.128
Seq.05	0.606	0.543	4.462	4.281	3.049	2.794	3.522	3.009
Seq.07	0.583	0.501	4.169	3.432	2.372	2.194	2.518	2.295
Seq.09	0.543	0.461	6.234	4.515	2.849	2.501	3.067	2.693
实地	0.521	0.443	9.627	6.289	3.423	3.045	4.027	3.328

查看所有表

5 结论

本文将激光雷达与IMU融合进行建图，通过添加IMU因子和激光雷达里程计因子进行后端优化，引入ICP回环检测对点云地图进行回环修正消除累计误差，利用iVox结构改进Fast-LIO2采用的iKD-tree结构，通过APE与RPE进行评价分析。选取开源数据集与实地场景进行建图实验，改进算法在KITTI数据集00，05，07和09号场景下的平均误差分别降低了12.09%，8.36%，7.50%和12.21%，平均下降了10.04%，均方误差方面相较于原算法分别降低了12.56%，14.56%，8.85%和12.19%，平均下降了12.04%。在选取的学校园区实地场景下，改进算法误差均值较原算法降低了11.04%，均方根误差较原算法降低了17.35%。

参考文献

[1] 贾晓雪，赵冬青，张乐添，等. 基于自适应惯导辅助特征匹配的视觉SLAM算法［J］. 光学精密工程， 2023， 31（5）： 621-630. doi: 10.37188/OPE.20233105.0621JIAX X， ZHAOD Q， ZHANGL T， et al. A visual SLAM algorithm based on adaptive inertial navigation assistant feature matching［J］. Opt. Precision Eng.， 2023， 31（5）： 621-630.（in Chinese）. doi: 10.37188/OPE.20233105.0621

[2] ZOU Q, SUN Q, CHEN L, et al. A comparative analysis of LiDAR SLAM-based indoor navigation for autonomous vehicles[J]. IEEE Transactions on Intelligent Transportation Systems, 2022, 23(7): 6907-6921.

[3] 张福斌，王凯，廖伟飞，等. 激光雷达/MEMS IMU/里程计紧组合导航算法［J］. 仪器仪表学报， 2022， 43（7）： 139-148.ZHANGF B， WANGK， LIAOW F， et al. Lidar/MEMS IMU/Odometer integrated tightly navigation algorithm［J］. Chinese Journal of Scientific Instrument， 2022， 43（7）： 139-148.（in Chinese）

[4] ZHANGJ， SINGHS. LOAM： lidar odometry and mapping in real-time［C］. Robotics： Science and Systems X. Robotics： Science and Systems Foundation， 2014，15（5）：9-25.

[5] JÚNIOR G P C, REZENDE A M C, MIRANDA V R F, et al. EKF-LOAM： an adaptive fusion of LiDAR SLAM with wheel odometry and inertial data for confined spaces with few geometric features[J]. IEEE Transactions on Automation Science and Engineering, 2022, 19(3): 1458-1471.

[6] MICHAEL M, SEBASTIAN T, DAPHNE K, et al. Fast-slam 2.0： An improved particle filtering algorithm for simultaneous localization and mapping that provably converges[J]. Artificial Intelligence Journal, 2003, 7(8): 593-606.

[7] 黄粒，张宏立，李新凯. 基于更新策略的SLAM后端优化算法研究［J］. 组合机床与自动化加工技术， 2023（6）： 14-17. doi: 10.3390/s22239362HUANGL， ZHANGH L， LIX K. Research on SLAM back-end optimization algorithm based on update strategy［J］. Modular Machine Tool & Automatic Manufacturing Technique， 2023（6）： 14-17.（in Chinese）. doi: 10.3390/s22239362

[8] 杨智宇，陆佳红，杜力，等. 多分辨率去动点的IMU激光雷达定位方法［J］. 传感器与微系统， 2022， 41（12）： 113-117.YANGZ Y， LUJ H， DUL， et al. IMU LiDAR localization method based on multi-resolution dynamic point removal［J］. Transducer and Microsystem Technologies， 2022， 41（12）： 113-117.（in Chinese）

[9] SU Y, WANG T, SHAO S L, et al. GR-LOAM： LiDAR-based sensor fusion SLAM for ground robots on complex terrain[J]. Robotics and Autonomous Systems, 2021, 140: 103759.

[10] 尹芳，吴云. 空间非合作旋转目标的模型重建与位姿优化［J］. 光学精密工程， 2019， 27（8）： 1854-1862. doi: 10.3788/ope.20192708.1854YINF， WUY. Model reconstruction and pose optimization of non-cooperative rotating space target［J］. Opt. Precision Eng.， 2019， 27（8）： 1854-1862.（in Chinese）. doi: 10.3788/ope.20192708.1854

[11] 杨林，马宏伟，王岩. 基于激光惯性融合的煤矿井下移动机器人SLAM算法［J］. 煤炭学报， 2022， 47（9）： 3523-3534.YANGL， MAH W， WANGY. LiDAR-Inertial SLAM for mobile robot in underground coal mine［J］. Journal of China Coal Society， 2022， 47（9）： 3523-3534.（in Chinese）

[12] XU W, ZHANG F. FAST-LIO： a fast， robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 3317-3324.

[13] JEONG， J， LEE， Fast-LIOJ, JEONG， J， LEE， Fast-LIOJ, JEONG， J， LEE， Fast-LIOJ, JEONG， J， LEE， Fast-LIOJ. 2： Efficient Lidar-Inertial Odometry for 6-DoF Pose Estimation[J]. IEEE Transactions on Robotics, 2020, 36(1): 156-167.

[14] XU W, CAI Y X, HE D J, et al. FAST-LIO2： fast direct LiDAR-inertial odometry[J]. IEEE Transactions on Robotics, 2022, 38(4): 2053-2073.

[15] BAI C G, XIAO T, CHEN Y J, et al. Faster-LIO： lightweight tightly coupled lidar-inertial odometry using parallel sparse incremental voxels[J]. IEEE Robotics and Automation Letters, 2022, 7(2): 4861-4868.

[16] 马艾强，姚顽强，蔺小虎，等. 面向煤矿巷道环境的LiDAR与IMU融合定位与建图方法［J］. 工矿自动化， 2022， 48（12）： 49-56.MAA Q， YAOW Q， LINX H， et al. Coal mine roadway environment-oriented LiDAR and IMU fusion positioning and mapping method［J］. Journal of Mine Automation， 2022， 48（12）： 49-56.（in Chinese）

[17] 徐晓苏，李诺，姚逸卿. 基于快速回环检测的室外环境下激光雷达SLAM算法［J］. 中国惯性技术学报， 2022， 30（6）： 716-722.XUX S， LIN， YAOY Q. Lidar SLAM algorithm in outdoor environment based on fast loop detection［J］. Journal of Chinese Inertial Technology， 2022， 30（6）： 716-722.（in Chinese）

[18] CHENZ Q， QIY H， ZHONGS P， et al. SCL-SLAM： a scan context-enabled LiDAR SLAM using factor graph-based optimization［C］. 2022 IEEE International Conference on Unmanned Systems （ICUS）. Guangzhou， China. IEEE， 2022： 1264-1269. doi: 10.1109/icus55513.2022.9987005

[19] ZHENG S R, WANG J L, RIZOS C, et al. Simultaneous localization and mapping （SLAM） for autonomous driving： concept and analysis[J]. Remote Sensing, 2023, 15(4): 1156.

[20] REBECQ H, HORSTSCHAEFER T, GALLEGO G, et al. EVO： a geometric approach to event-based 6-DOF parallel tracking and mapping in real time[J]. IEEE Robotics and Automation Letters, 2017, 2(2): 593-600.

马庆禄, 汪军豪, 张杰, 邹政. 激光雷达与惯性测量单元同步融合下的园区三维建图[J]. 光学精密工程, 2024, 32(3): 422. Qinglu MA, Junhao WANG, Jie ZHANG, Zheng ZOU. Three-dimensional mapping of park based on synchronous fusion of lidar and inertial measurement unit[J]. Optics and Precision Engineering, 2024, 32(3): 422.