多传感器融合的激光雷达点云矫正与定位方法

针对单一传感器难以解决激光雷达在运动场景中因为点云畸变和误差累积产生的运动失真与定位精度差的问题，提出一种融合惯性测量单元数据和轮速计数据的激光雷达点云畸变矫正与定位方法。首先，以激光雷达数据为时刻基准，利用积分的方法对惯性测量单元和轮速计的数据进行预处理；之后，将融合数据与激光雷达数据融合，以矫正产生畸变的激光点云；最后，利用线性插值的方式来保证传感器间数据的时间同步，并将计算的位姿作为里程计迭代计算的初值，降低计算复杂度并提高里程计的定位精度。实验结果表明，相比没有采用多传感器融合的传统方案（LOAM、F-LOAM），在公开数据集实验中，所提方法的定位均方根误差分别降低了81.11%和21.54%，在自测数据集实验中，定位均方根误差分别降低了52.76%和24.29%。

Abstract

It is difficult to solve motion distortion and poor positioning accuracy caused by point cloud distortion and error accumulation in a LiDAR moving scene using a single sensor. To address this problem, a LiDAR point cloud distortion correction and positioning method that combines inertial measurement unit data and wheel tachometer data is proposed. First, the data of the inertial measurement unit and the wheel tachometer are preprocessed by an integration method based on the time of the LiDAR data. Next, the fusion data and the LiDAR point cloud data are fused to correct the position and pose of the laser point cloud distorted by motion. Finally, the linear interpolation method is used to ensure the time synchronization and availability of data between sensors and ultimately improve the positioning accuracy of the odometer; the calculated pose was used as the optimal initial value of the odometer iteration. The experimental results show that compared with the traditional method that does not use multisensor fusion (LOAM and F-LOAM), the proposed method's root mean square error of positioning on the open data set experiment is reduced by 81.11% and 21.54%, respectively, the root mean square error of positioning of the proposed method on the self-testing data concentration period is reduced by 52.76% and 24.29%, respectively.

1　引言

近些年来，随着社会智能化的不断进步，机器人、自动驾驶等领域都取得了长足的发展^［1］。智能体的进一步应用和推广都需要在无任何人工的操控下，在复杂、多变的室内外场景下执行任务。而精准地感知自身的位置和周围场景是执行各种任务的必要条件，同时定位与建图（SLAM）技术正是当下智能体定位技术的核心。SLAM技术指：承载特定传感器的主体在不知道外界环境的任何信息的情况下，对自己本身位姿进行估计，并且能够建立环境信息。对于SLAM技术，传感器是系统获取环境信息的最重要工具^［2］。激光雷达以激光束作为信息载体，可以提供高精度的环境深度信息，同时具有很高的角分辨率、速度分辨率和距离分辨率，在机器人定位领域存在大量应用^［3］。但是激光雷达在动态环境下存在点云畸变的问题，并且里程计误差会在匹配过程中随距离增加不断累积，造成定位精度降低。

目前，国内外学者针对如上问题已经进行了相关研究。文献［4］中的方法利用匀速模型计算一帧点云中各激光点的具体位姿，该方法在复杂运动场景中无法起到较好的效果。文献［5］提出了一种基于激光扫描角度固定模型的改进方法，该方法利用激光数据的时间间隔估算点云位姿，但是利用的二次插值假设依然过于简单，无法适应复杂运动环境。文献［6］利用相干调频连续波激光雷达技术对点云畸变进行补偿，但对激光的线宽、线性调频脉冲的频率范围等条件要求相对较高。文献［7］提出具有旋转和平移不变性的描述子，利用非线性优化直接补偿畸变造成的定位误差，但算法对计算实时性要求较高。

以上研究表明利用数学模型解决点云畸变问题的效果有限，利用优化等其他方式，使用条件相对苛刻。研究人员开始考虑利用多传感器的方式解决畸变问题。Shan等^［8］考虑到惯性测量单元（IMU）的可用性，在IMU不可用时利用匀速模型进行点云位姿估计，在IMU可用时利用其获得运动线性加速度，优化匀速模型消除畸变。但是IMU本身也存在误差累积的问题，算法效果并不理想。文献［9］利用多传感器辅助激光雷达进行自补偿和自校正，提高了激光雷达自身的定位精度。文献［10］利用视觉里程计消除点云畸变，但在高速运动场景下不能保证计算的准确性。文献［11］提出一种基于无迹卡尔曼滤波（UKF）算法的点云补偿方案，利用雷达位姿和IMU位姿融合进行畸变去除，该方案需要大量的计算工作，当环境特征结构不清晰或者发生变化时，无法有效消除累积误差。

综上，学界的研究表明，利用激光雷达一种传感器，模拟运动模型的方案无法适应复杂运动场景，很难消除点云畸变，需要利用多传感器融合解决此问题；同时需要减少复杂计算，保证系统整体在复杂动态场景下的鲁棒性和定位准确性。

为了解决上述问题，本文提出一种融合惯性测量单元数据和轮速计数据的激光雷达点云畸变矫正和定位方法。针对运动状态中时刻发生的点云畸变，利用IMU和轮速计分别具有较高的角速度测量精度和较高的局部位移测量精度的特点，准确矫正激光雷达因运动产生的点云畸变，并将矫正过程中计算的位姿作为雷达里程计迭代计算的初值，减小计算复杂度，提高系统整体的定位精度与实时性。

2　激光雷达点云畸变失真与补偿原理

2.1　激光雷达点云畸变原理

激光雷达由内部电机驱动，每一帧激光雷达点云数据都由一定数量的激光束所获得的数据信息封装组成，默认一帧数据获得的时刻是相同的，但实际上，这些数据并不是瞬时获得的。当激光扫描伴随着空间运动时，一帧数据中的每个数据被认为皆处于同一坐标系下，但实际上，它们被获取时的移动坐标系各不相同，这会造成各点间坐标系的转换错误，导致点云出现畸变，这种畸变会在雷达运动过程中不断发生。如图1（a）所示， $a_{1} 、 a_{2} 、 a_{3}$ 表示激光雷达依次扫描的三个位置点，它们在实际场景中基本处于共线的位置；但由于激光雷达的运动，如图1（b）所示，激光雷达扫过这三个点时处于三个不同的姿态；在最终点云中，三个点的坐标也发生了变化，不再处于同一直线上，如图1（c）所示。

图 1. 激光雷达点云畸变产生示意图。（a）初始时刻雷达扫描情况；（b）运动状态的雷达扫描情况；（c）失真的点云坐标情况

Fig. 1. Schematic of LiDAR point cloud distortion generation. (a) LiDAR scanning at initial time; (b) LiDAR scanning at motion state; (c) point cloud coordinates with distortion

下载图片查看所有图片

2.2　激光雷达点云畸变失真补偿原理

激光雷达每旋转一周封装一帧数据，每一帧数据中有许多的雷达点云数据。对于畸变补偿，需要将一帧数据中的所有点云数据都投影到初始时刻的点云数据所处的坐标系下。每个激光点的坐标矩阵可表达为

ξ_{i}^{j} = {[\begin{matrix} p_{x} & p_{y} & p_{z} \end{matrix}]}^{T}

，（1）

式中： $ξ_{i}^{j}$ 为在雷达坐标系中第 $i$ 帧中第 $j$ 个点的三轴坐标； $p_{x}, p_{y}, p_{z}$ 分别为激光点的 $x, y, z$ 轴的具体坐标值。2D问题中， $z$ 轴坐标值为不变的定值，而对于 $x, y$ 轴的坐标，计算的方法为

\{\begin{array}{l} p_{x} = l_{i}^{j} \cdot c o s θ_{i}^{j} \\ p_{y} = l_{i}^{j} \cdot s i n θ_{i}^{j} \\ θ_{i}^{j} = θ_{i}^{j - 1} + Δ θ \end{array}

，（2）

式中： $l_{i}^{j}$ 为第 $i$ 帧中第 $j$ 个激光点测得的实际距离； $θ_{i}^{j}$ 为第 $i$ 帧中第 $j$ 个激光点对应的旋转角； $Δ θ$ 为激光雷达点云数据的角分辨率。假设一帧点云中，起始时刻雷达位姿的转换矩阵为

I_{i}^{0} = [\begin{matrix} R_{i}^{0} & t_{i}^{0} \\ 0 & 1 \end{matrix}]

，（3）

式中： $R 和 t$ 分别为雷达位姿对应的旋转矩阵和平移矩阵。第 $j$ 个激光点采集时，雷达的转换矩阵为

I_{i}^{j} = [\begin{matrix} R_{i}^{j} & t_{i}^{j} \\ 0 & 1 \end{matrix}]

。（4）

第 $j$ 个激光点补偿畸变后的坐标变换过程应表达为

{\tilde{ξ}}_{i}^{j} = {I_{i}^{0}}^{^{- 1}} \cdot I_{i}^{j} \cdot ξ_{i}^{j}

，（5）

式中： ${\tilde{ξ}}_{i}^{j}$ 为补偿后的第 $i$ 帧中第 $j$ 个点的三轴坐标。利用式（5），计算点与点之间的坐标系转换关系，从而完成点云补偿。为了充分发挥里程计局部位移测量精度高的优势，平移矩阵由里程计数据求得；利用IMU数据计算旋转矩阵，发挥其局部角速度测量精度高的优势，从而提高整体定位精度。但激光雷达的数据更新频率一般保持在5~10 Hz，IMU的更新频率却通常保持在100~200 Hz，这使得雷达数据、IMU数据和轮速计数据在时间上不能严格对齐，数据量也不一致，需要通过线性插值的方式进行调整和补充。

一帧雷达数据具有初始时刻和终止时刻两个时间节点，可以通过预积分的方法计算两个时刻间的IMU数据的相对位姿，之后利用插值，计算出雷达对应时刻的旋转量。对于里程计信息，计算两个时刻间的平移变化后，再通过插值计算对应时刻的平移量。对于IMU数据，求解旋转的过程可以表示为

\{\begin{matrix} φ_{x}^{t} = φ_{x}^{t - 1} + ω_{x}^{t} \cdot Δ t \\ φ_{y}^{t} = φ_{y}^{t - 1} + ω_{y}^{t} \cdot Δ t \\ φ_{z}^{t} = φ_{z}^{t - 1} + ω_{z}^{t} \cdot Δ t \end{matrix}

，（6）

式中： $φ_{x}^{t}, φ_{y}^{t}, φ_{z}^{t}$ 分别为 $x, y, z$ 轴方向上第 $t$ 时刻IMU测得的旋转量； $ω_{x}^{t}, ω_{y}^{t}, ω_{z}^{t}$ 分别为 $x, y, z$ 轴方向上第 $t$ 时刻测得的角速度； $Δ t$ 为两组IMU数据之间的时间差； $φ^{t}$ 的集合为IMU在第 $t$ 时刻的位姿队列。对于里程计数据，求解平移量的过程可以表示为

\{\begin{matrix} λ_{x} = d_{s}^{x} - d_{e}^{x} \\ λ_{y} = d_{s}^{y} - d_{e}^{y} \\ λ_{z} = d_{s}^{z} - d_{e}^{z} \end{matrix}

，（7）

式中： $λ_{x}, λ_{y}, λ_{z}$ 分别为 $x, y, z$ 轴方向上激光雷达数据帧起止时刻间里程计测得的平移数据； $d_{s}^{x}, d_{s}^{y}, d_{s}^{z}, d_{e}^{x},$ $d_{e}^{y}, d_{e}^{z}$ 分别为 $x, y, z$ 轴方向上在起止时刻测得的里程计数据； $λ$ 的集合为里程计位姿队列，其和IMU位姿队列 $φ^{t}$ 统一表示为融合位姿队列 $Q_{F U S E}$ 。

因为IMU和轮速计的更新频率相对较高，可以利用插值的方式对其与激光雷达数据的时刻进行对齐。假设激光数据在 $l 和 k$ 时刻有对应的解算位姿，但是在 $s$ 时刻没有，并且 $s$ 小于 $k$ 且大于 $l$ ，则可以利用二次插值方法对 $s$ 时刻的位姿进行求解^［12-13］，求解过程可表示为

\{\begin{matrix} q_{l} = Q_{F U S E} [i] \\ q_{k} = Q_{F U S E} [j] \\ q_{s} = q_{l} + \frac{q_{k} - q_{l}}{k - l} (s - l) \end{matrix}

，（8）

式中： $q_{l}, q_{s}, q_{k}$ 分别为在 $l, s, k$ 时刻下解算激光数据后得到的位姿信息； $Q_{F U S E} [i] 和 Q_{F U S E} [j]$ 分别为融合位姿队列中的第 $i$ 组数据和第 $j$ 组数据。针对具体数据，求解过程为

\{\begin{matrix} R_{x (i)}^{j} = φ_{x (i)}^{j} \cdot \frac{r_{i}^{j} - t_{i}^{j - 1}}{t_{i}^{j} - t_{i}^{j - 1}} + φ_{x (i)}^{j - 1} \cdot \frac{t_{i}^{j} - r_{i}^{j}}{t_{i}^{j} - t_{i}^{j - 1}} \\ R_{y (i)}^{j} = φ_{y (i)}^{j} \cdot \frac{r_{i}^{j} - t_{i}^{j - 1}}{t_{i}^{j} - t_{i}^{j - 1}} + φ_{y (i)}^{j - 1} \cdot \frac{t_{i}^{j} - r_{i}^{j}}{t_{i}^{j} - t_{i}^{j - 1}} \\ R_{z (i)}^{j} = φ_{z (i)}^{j} \cdot \frac{r_{i}^{j} - t_{i}^{j - 1}}{t_{i}^{j} - t_{i}^{j - 1}} + φ_{z (i)}^{j - 1} \cdot \frac{t_{i}^{j} - r_{i}^{j}}{t_{i}^{j} - t_{i}^{j - 1}} \end{matrix}

，（9）

式中： $R_{x (i)}^{j}, R_{y (i)}^{j}, R_{z (i)}^{j}$ 分别为 $x, y, z$ 轴方向上激光雷达在第 $i$ 帧第 $j$ 个激光点数据需要补偿的旋转量； $r_{i}^{j}$ 为激光雷达在第 $i$ 帧第 $j$ 个激光点数据需要进行插值计算的时刻； $t_{i}^{j}$ 为激光雷达在第 $i$ 帧时进行插值前后的起止时刻。对平移补偿的求解为

\{\begin{matrix} T_{x (i)}^{j} = λ_{x (i)}^{j} \cdot \frac{r_{i}^{j} - t_{i}^{s}}{t_{i}^{e} - t_{i}^{s}} \\ T_{y (i)}^{j} = λ_{y (i)}^{j} \cdot \frac{r_{i}^{j} - t_{i}^{s}}{t_{i}^{e} - t_{i}^{s}} \\ T_{z (i)}^{j} = λ_{z (i)}^{j} \cdot \frac{r_{i}^{j} - t_{i}^{s}}{t_{i}^{e} - t_{i}^{s}} \end{matrix}

，（10）

式中： $T_{x (i)}^{j}, T_{y (i)}^{j}, T_{z (i)}^{j}$ 分别为 $x, y, z$ 轴方向上激光雷达在第 $i$ 帧第 $j$ 个激光点数据需要补偿的平移量； $r_{i}^{j}$ 为激光雷达在第 $i$ 帧第 $j$ 个激光点数据需要进行插值计算的时刻； $t_{i}^{s} 和 t_{t}^{e}$ 分别为激光雷达在第 $i$ 帧时进行插值前后的起止时刻。将式（9）和式（10）计算完成后的结果代回式（5），完成对点云的补偿计算。计算出的旋转矩阵和平移矩阵也可以在里程计改进的环节再次使用。

3　基于点云畸变失真补偿的激光雷达里程计改进方法

利用IMU和轮速计在局部角速度和局部位移上的精度优势，将它们计算得到的位姿作为激光里程计迭代计算的初值，可以减少迭代次数并补偿激光里程计本身存在的累积误差，以达到降低计算复杂度、提高定位精度的目的^［14-15］。将需要匹配的两对点云位姿矩阵集合记为

X = \{x_{1}, x_{2}, \dots, x_{N (x)}\} \begin{matrix} , & Y = \{y_{1}, y_{2}, \dots, y_{N (y)}\} \end{matrix}

。（11）

对于每一个空间栅格，均值和方差分别为

\{\begin{array}{l} μ = \frac{1}{N_{x}} \sum_{i = 1}^{N_{x}} x_{i} \\ \sum = \frac{1}{N_{x} - 1} \sum_{i = 1}^{N_{x}} (x_{i} - μ) (x_{i} {- μ)}^{T} \end{array}

，（12）

式中： $N_{x}$ 为点云 $X$ 的总数量； $x_{i}$ 为点云 $X$ 中第 $i$ 个点云的位姿数据。根据预测的位姿，可计算对所有点的联合概率，取对数，化简为

m a x ψ = m i n {\sum_{i = 1}^{N_{y}} {(y_{i}^{'} - μ)}^{T}}^{- 1} \sum (y_{i}^{'} - μ)

，（13）

式中： $ψ$ 为对所有点求联合概率并取对数后的表达式； $y_{i}^{'}$ 为点云 $Y$ 中的第 $i$ 个点云位姿数据经过预测后的点云位姿。对于 $y_{i}^{'}$ ，假设各点的坐标矩阵 $w$ 为

\{\begin{array}{l} w = {[\begin{matrix} e_{x} & e_{y} & α_{z} \end{matrix}]}^{T} \\ y_{i}^{'} = T (w, y_{i}) \end{array}

，（14）

式中： $e_{x} 、 e_{y} 和 α_{z}$ 分别为 $w$ 中的 $x$ 坐标、 $y$ 坐标和姿态角； $y_{i}$ 为点云 $Y$ 中的第 $i$ 个点云的位姿数据； $T (w, y_{i})$ 为点云间的变换函数。构建残差函数 $f_{i} (w) = y_{i}^{'} - μ$ ，只需要计算残差函数关于待求参数的雅克比，就可以完成迭代优化^［16］，定义残差函数雅克比矩阵为

J_{i} = \frac{d f_{i} (w)}{d w} = [\begin{matrix} 1 & 0 & - y_{i} s i n α_{z} - y_{i} c o s α_{z} \\ 0 & 1 & y_{i} c o s α_{z} - y_{i} s i n α_{z} \end{matrix}]

。（15）

对于每次迭代的初始位姿 $w$ ，如果不进行修正，其误差会随距离增加不断累积，每次用以匹配的初值不够准确，必然会造成定位精度下降^［17］。对于部分基于帧间直接匹配的里程计构建方法，对迭代初值的准确性往往都有较高要求，如果初值准确度较差，会导致迭代次数过多，影响计算实时性；同时，随着定位误差的不断累积，匹配过程还容易陷入局部最优，导致里程计无法准确定位机器人的位置。因而需要融合IMU和轮速计的数据，用其计算的雷达位姿作为激光里程计迭代计算的初值，以起到修正累积误差的效果。同时更高精度的初值可以有效地减少迭代次数，减小计算复杂度，提高计算速度，最终提高整体定位精度。系统整体流程如图2所示。

图 2. 算法流程示意图

Fig. 2. Schematic of algorithm flow

下载图片查看所有图片

4　分析与讨论

为了验证所提方法的有效性和可行性，利用公开数据集与实地室内退化环境进行实验验证，并分析实验结果。

4.1　激光雷达点云畸变去除验证

为了验证所提方法对点云畸变去除的效果，需要在静态环境中完成实验。机器人在静态环境中做周期性的旋转和平移运动，使得采集到的坐标也呈周期性，用以定量比较。在静止条件下，将获得的无畸变的点云坐标作为真值；在运动条件下点云会不断发生畸变，记录所提算法修正后的点云坐标，对其与LOAM方法和F-LOAM方法的畸变去除情况进行定量对比。LOAM算法在畸变去除环节采用简单的匀速模型对畸变点云进行补偿，F-LOAM算法在此基础上进行改进，在位姿解算环节再进行一次点云畸变去除的工作，获得了相对更好的畸变去除效果，但两者均没有采用多传感器的方案进行畸变补偿。三种方法的实验结果如图3所示。

图 3. 激光雷达点云畸变修正结果。（a）x方向坐标修正情况；（b）y方向坐标修正情况

Fig. 3. Schematic of correction results of LiDAR point cloud distortion. (a) Diagram of correction of x-direction coordinates; (b) diagram of correction of y-direction coordinates

下载图片查看所有图片

图3描述了实验中坐标的畸变与修正的情况。由图3可知，所提方法通过IMU和轮速计的信息融合，去除畸变后的坐标值基本与真值一致，而另外两种方法的结果相对真值都还有一定的偏差，结果表明所提方法更有效地消除了点云畸变。进一步的，所提方法修正后，点云坐标在 $x 和 y$ 方向上因畸变产生的误差分别为0.189 m和0.128 m。表1具体量化了三种方法在 $x 和 y$ 方向上的误差修正情况。

表 1. 三种算法在两个方向上的误差结果对比

Table 1. Comparison of error results in two directions of the three algorithms

Algorithm	Average error alongx-direction /m	Average error alongy-direction /m
LOAM	0.281	0.213
F-LOAM	0.221	0.152
Proposed algorithm	0.189	0.128

查看所有表

由表1可知，相比LOAM和F-LOAM方法，所提方法通过利用多传感器融合的方案确实提高了激光雷达点云畸变的补偿精度：相比LOAM方法，所提方法补偿畸变误差后， $x$ 方向上的畸变误差下降了32.7%， $y$ 方向上的畸变误差下降了39.9%；相比F-LOAM方法，所提方法补偿畸变误差后， $x$ 方向上的畸变误差下降了14.5%， $y$ 方向上的畸变误差下降了15.8%。为了直观地验证点云畸变去除的效果，分别在走廊和楼宇两个实际场景进行实验，对点云情况进行可视化，点云视图如图4和图5所示。

图 4. 走廊环境下不同点云畸变矫正方法的点云视图。（a）未进行畸变补偿的点云视图；（b）LOAM方法；（c）F-LOAM方法；（d）所提方法

Fig. 4. Point cloud views of different point cloud distortion correction methods in corridor environment. (a) Point cloud view without distortion compensation; (b) LOAM method; (c) F-LOAM method; (d) proposed method

下载图片查看所有图片

图 5. 楼宇环境下不同点云畸变矫正方法的点云视图。（a）未进行畸变补偿的点云视图；（b）LOAM方法；（c）F-LOAM方法；（d）所提方法

Fig. 5. Point cloud views of different point cloud distortion correction methods in building environment. (a) Point cloud view without distortion compensation; (b) LOAM method; (c) F-LOAM method; (d) proposed method

下载图片查看所有图片

从图4可以看出：在未进行点云畸变矫正的点云中，平面的墙结构因为畸变出现了凸起和重影；LOAM方法和F-LOAM方法在去除畸变后，效果相对有所改进，但平面结构仍然出现了点云的重叠和不平整；对于所提方法处理的点云视图，平面结构基本平滑，说明了畸变去除方法的有效性。

从图5可以看出：在室外环境中，未进行点云畸变的点云视图出现了非常严重的错位和重影现象；使用LOAM和F-LOAM方法进行畸变去除后，点云地图相对规则，但是直线信息处仍然出现了部分重影，线段也不够平滑；使用所提方法进行畸变去除后，线段的重影基本没有出现，直线段也相对更平滑。实验结果也进一步证明了所提方法对点云畸变去除的作用。

4.2　数据集验证

KITTI数据集由德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合创办，该数据集用于评测激光里程计等算法在车载环境下的性能。实验平台的传感器组成包括64线激光雷达和组合导航系统。组合导航系统可以输出惯性导航单元和载波相位差分定位系统（RTK）组合导航后的结果^［18］，本文以此数据（GNSS）作为真值进行对比。

对所提方法与传统方法LOAM和F-LOAM进行定量对比。除了在畸变去除环节不同，两种方法并没有在里程计迭代计算环节代入更优初值以减小计算复杂度、消除累积误差。几种算法的对比实验均在内存为16G的英伟达Jetson TX2工控机上进行。图6是所提方法和两种经典方案在数据集kitti_2011_10_03_drive_0027_synced场景下的定位轨迹对比。所提方法在轨迹前半段与真实轨迹基本重合，另外两种方案皆存在相对明显的定位误差；后半段随着误差累积，所提方法与真实轨迹之间也出现一定偏差，但是偏差程度仍小于另外两种方法。结果说明了所提方法对定位精度的提高有帮助。

图 6. 不同算法的轨迹对比

Fig. 6. Trajectory comparison between different algorithms

下载图片查看所有图片

图7展示了3种方法在三个不同方向上的位置误差随时间变化的情况。由图7可知：在三轴方向上，所提算法在前半段能保持很好的定位精度，与真实值基本一致，因为IMU和轮速计提供的初值较好地补偿了激光里程计自身的累积误差，同时点云畸变的去除也保证了定位的准确性，其他方案都与真实值存在较大偏差；随着IMU自身累积误差的不断增加，所提方法在后半段也出现了一定误差，同时因为在畸变去除环节简化了z轴方向的计算，z轴方向的精度在后半部分相对较差，但是整体定位精度依然优于另外两种方案。

图 7. 三轴方向误差对比。（a）x轴方向；（b）y轴方向；（c）z轴方向

Fig. 7. Comparison of errors in three axes. (a) Along the x axis; (b) along the y axis; (c) along the z axis

下载图片查看所有图片

表2给出了三种方法在公开数据集下定位轨迹和真实轨迹的对比结果，对比指标包括平均误差、均方根误差（RMSE）、误差平方和、标准差和平均迭代时间。其中平均误差和均方根误差衡量了观测值和真值间的偏差大小；误差平方和用以衡量误差量的总体大小规模；标准差用以衡量数据的收敛与离散程度；平均迭代时间指雷达点云经过迭代优化计算后完成匹配所用的平均时间。从表2可知，所提算法比LOAM和F-LOAM方法的定位精度高，均方根误差分别降低了81.11%和21.54%，里程计迭代计算的平均耗时分别减少了7.3%和2.0%。

表 2. 三种算法在KITTI数据集上的定位结果对比

Table 2. Comparison of positioning results of three algorithms on KITTI dataset

Algorithm	Average error /m	RMSE /m	Error sum of squares /m²	Standard deviation /m	Average iteration time /ms
LOAM	17.861	19.252	4 769 827.822	11.517	0.519
F-LOAM	1.728	4.634	2 254.807	4.230	0.491
Proposed algorithm	1.003	3.636	138.793	3.099	0.481

查看所有表

4.3　真实场景验证

自测数据集的采集场地选在露天走廊。以turtlebot2机器人作为实验平台的主要移动平台，其搭载有NVIDIA Jetson TX2高性能计算平台开发板、RoboSense速腾16线激光雷达、SBG消费级IMU六轴传感器、blicube系列的RTK定位设备等相关组件，为实验提供真值用以对比。实验对比了所提算法和LOAM、F-LOAM方案的定位精度，结果如图8所示。

图 8. 算法间的轨迹在实际场景下的对比

Fig. 8. Comparison of trajectory between algorithms in the actual scene

下载图片查看所有图片

图8是所提方法和LOAM、F-LOAM方法在两段长走廊场景下的定位轨迹对比图。所提算法基本能做到和真实轨迹保持一致，在开始阶段与真实轨迹基本重合，随着误差累积增加轨迹出现一定偏差，但最后终点和起点可以做到基本重合。LOAM和F-LOAM算法在长走廊环境下出现了较为明显的偏差，除了在初始部分路段能保持与真值一致，后续轨迹都与真值发生了较大偏差，起点和终点在最后没有重合。该实验进一步说明了所提方法提高了系统的整体定位精度。

进一步地，表3给出了三种方法在露天走廊环境下各自定位轨迹和真实轨迹的对比结果。从表3可知，所提算法比LOAM和F-LOAM方法的定位精度更高，均方根误差分别降低了52.76%和24.29%，里程计迭代计算的平均耗时分别减少了4.8%和2.4%。

表 3. 三种算法在实际场景下的定位结果对比

Table 3. Comparison of positioning results of the three algorithms in actual scene

Algorithm	Average error /m	RMSE /m	Error sum of squares /m²	Standard deviation /m	Average iteration time /ms
LOAM	3.562	0.851	491.366	0.936	0.604
F-LOAM	1.887	0.531	414.835	0.282	0.589
Proposed algorithm	1.102	0.402	122.233	0.025	0.575

查看所有表

5　结论

提出一种融合IMU数据和轮速计数据的激光雷达点云矫正和定位方法。利用IMU和轮速计具有较高的线性加速度精度和二次积分在局部的较高精度的特点，矫正激光雷达因运动产生的点云畸变。同时，计算出的雷达位姿为激光里程计提供相对最优初值，减少激光里程计的迭代计算次数，降低计算复杂度，并补偿激光里程计随距离增加累积的定位误差，解决在复杂环境下定位精度低的问题。实验结果表明：在KITTI数据集上，所提方法比LOAM算法和F-LOAM算法的定位均方根误差分别降低了81.11%和21.54%，计算平均耗时分别降低了7.3%和2.0%；在露天走廊采集的自测数据集上，所提方法比LOAM算法和F-LOAM算法的定位均方根误差分别降低了52.76%和24.29%，计算平均耗时分别降低了4.8%和2.4%。所提方法不涉及闭环检测和建图功能，增添闭环检测模块进行全局修正可以进一步提升定位精度，增添建图功能可以进行路径规划和基于地图的位置匹配，以适应更广泛的室内场景。

参考文献

[1] 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.

[2] Lopac N, Jurdana I, Brnelić A, et al. Application of laser systems for detection and ranging in the modern road transportation and maritime sector[J]. Sensors, 2022, 22(16): 5946.

[3] MiliotoA, VizzoI, BehleyJ, et al. RangeNet: fast and accurate LiDAR semantic segmentation[C]∥2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), November 3-8, 2019, Macau, China. New York: IEEE Press, 2020: 4213-4220.

[4] HongS, KoH, KimJ. VICP: velocity updating iterative closest point algorithm[C]∥2010 IEEE International Conference on Robotics and Automation, May 3-7, 2010, Anchorage, AK, USA. New York: IEEE Press, 2010: 1893-1898.

[5] BezetO, CherfaouiV. Time error correction for laser range scanner data[C]∥2006 9th International Conference on Information Fusion, July 10-13, 2006, Florence, Italy. New York: IEEE Press, 2007.

[6] 蔡新雨, 孙建锋, 卢智勇, 等. 相干调频连续波激光雷达畸变补偿技术研究[J]. 中国激光, 2020, 47(9): 0910003.

Cai X Y, Sun J F, Lu Z Y, et al. Distortion compensation technology of coherent frequency modulation continuous wave lidar[J]. Chinese Journal of Lasers, 2020, 47(9): 0910003.

[7] 常耀辉, 陈年生, 饶蕾, 等. 动态环境下具有旋转和平移不变性的激光雷达点云描述子[J]. 光学学报, 2022, 42(24): 2401007.

Chang Y H, Chen N S, Rao L, et al. Lidar point cloud descriptor with rotation and translation invariance in dynamic environment[J]. Acta Optica Sinica, 2022, 42(24): 2401007.

[8] ShanT X, EnglotB. LeGO-LOAM: lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]∥2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), October 1-5, 2018, Madrid, Spain. New York: IEEE Press, 2019: 4758-4765.

[9] 李小路, 周依尔, 毕腾飞, 等. 轻量型感知激光雷达关键技术发展综述[J]. 中国激光, 2022, 49(19): 1910002.

Li X L, Zhou Y E, Bi T F, et al. Review on key technologies of lightweight type-aware LiDAR[J]. Chinese Journal of Lasers, 2022, 49(19): 1910002.

[10] Zhang Y. LILO: a novel lidar–IMU SLAM system with loop optimization[J]. IEEE Transactions on Aerospace and Electronic Systems, 2022, 58(4): 2649-2659.

[11] ZhangB, ZhangX Y, WeiB C, et al. A point cloud distortion removing and mapping algorithm based on lidar and IMU UKF fusion[C]∥2019 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), July 8-12, 2019, Hong Kong, China. New York: IEEE Press, 2019: 966-971.

[12] Zhang J, Singh S. Low-drift and real-time lidar odometry and mapping[J]. Autonomous Robots, 2017, 41(2): 401-416.

[13] 张磊, 徐孝彬, 曹晨飞, 等. 基于动态特征剔除的图像与点云融合的机器人位姿估计方法[J]. 中国激光, 2022, 49(6): 0610001.

Zhang L, Xu X B, Cao C F, et al. Robot pose estimation method based on image and point cloud fusion with dynamic feature elimination[J]. Chinese Journal of Lasers, 2022, 49(6): 0610001.

[14] Wu Q X, Meng Q Y, Tian Y Y, et al. A method of calibration for the distortion of LiDAR integrating IMU and odometer[J]. Sensors, 2022, 22(17): 6716.

[15] Tang J, Chen Y W, Niu X J, et al. LiDAR scan matching aided inertial navigation system in GNSS-denied environments[J]. Sensors, 2015, 15(7): 16710-16728.

[16] BiberP, StrasserW. The normal distributions transform: a new approach to laser scan matching[C]∥Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems, October 27-31, 2003, Las Vegas, NV, USA. New York: IEEE Press, 2004: 2743-2748.

[17] ShanT X, EnglotB, MeyersD, et al. LIO-SAM: tightly-coupled lidar inertial odometry via smoothing and mapping[C]∥2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), October 24-January 24, 2021, Las Vegas, NV, USA. New York: IEEE Press, 2021: 5135-5142.

[18] Li Y H, Yang S Z, Xiu X C, et al. A spatiotemporal calibration algorithm for IMU-LiDAR navigation system based on similarity of motion trajectories[J]. Sensors, 2022, 22(19): 7637.

3　基于点云畸变失真补偿的激光雷达里程计改进方法

蒲文浩, 刘锡祥, 陈昊, 徐浩, 刘烨. 多传感器融合的激光雷达点云矫正与定位方法[J]. 激光与光电子学进展, 2023, 60(24): 2428003. Wenhao Pu, Xixiang Liu, Hao Chen, Hao Xu, Ye Liu. LiDAR Point Cloud Correction and Location Based on Multisensor Fusion[J]. Laser & Optoelectronics Progress, 2023, 60(24): 2428003.