基于实时局部建图的激光雷达长周期定位方法

非结构化道路中的无人驾驶精确定位大量使用基于激光雷达的simultaneous localization and mapping（SLAM）技术，解决因环境变化导致的预建地图匹配失败，进而引起定位丢失的问题一直是业内难题和热点研究方向之一。针对上述问题，提出一种利用激光雷达和惯性测量单元在normal distribution transform（NDT）定位基础上融合实时局部地图匹配的长周期鲁棒定位方法online location normal distributions transform（OL-NDT）。OL-NDT将NDT获得的定位信息作为测量信息因子输入因子图中优化实时构建的局部地图，并且在其全局定位丢失后采用实时局部地图进行定位。在MulRan数据集上进行定位精度测试，OL-NDT的累计误差占比为0.40%，较现有的传统定位方法降低了1.06个百分点，定位精度得到了有效提升，且在静态结构发生较大变化的场景下也可以精准定位。同时，利用在北京联合大学采集的校园数据验证了在短暂无地图情况下OL-NDT的定位轨迹精度与已知地图时完全匹配。

Abstract

The precise positioning of driverless vehicles on unstructured roads extensively relies on LiDAR-based simultaneous localization and mapping (SLAM). However, the problem of localization loss, caused by the failure of pre-built map matching due to environmental changes, has been an industry challenge and a popular research direction. To address the aforementioned problems, this study proposes a long-term robust localization method, online location normal distributions transform (OL-NDT), which uses LiDAR and inertial measurement units to combine real-time local map matching based on NDT localization. OL-NDT inputs the localization information obtained by NDT as measurement information factors into the factor map to optimize the local maps constructed in real time and uses real-time local maps for localization after NDT localization is lost. OL-NDT is tested on the MulRan dataset and achieves a cumulative error percentage of 0.40%, which is 1.06 percentage points lower than the existing traditional localization methods. This effectively improves localization accuracy and enables accurate localization in scenarios with significant changes in the static structure. Moreover, the campus data collected by Beijing Union University is used to verify that the localization trajectory accuracy of OL-NDT precisely matches the known map, even in cases of short-term missing maps.

1　引言

依托于simultaneous localization and mapping（SLAM）技术的发展，物流配送、automated guided vehicle（AGV）、清扫机器人及园区接驳车等无人驾驶的轮式机器人应用广泛^［1］。此类机器人主要依靠激光雷达和视觉SLAM定位技术长期运行，但是周围环境会随时间的推移发生较大或较小的变化，如周围移动的车辆和行人^［2］、临时出现或消失的货物、因季节变化而改变的树木轮廓和施工造成的建筑物改变等环境变化。这些改变会导致机器人出现定位精度降低，甚至丢失等问题，如何在变化的环境中进行长期、精确、鲁棒的定位被称为SLAM的长生命周期问题^［3］。

解决长生命周期问题的关键是提高对历史位置重识别的鲁棒性，可分为基于规则的方法和基于深度学习的方法。基于规则的方法通过设定参考帧与匹配帧之间的相似规则定位。文献［4］、［5］使用邻域搜索计算帧间距离判断是否回环，但基于特征匹配或基于帧间距离的方法需要环境不发生变化，限制较多。文献［6］、［7］为了提高定位精度且具有更好的鲁棒性，将点云视角及坐标进行转换，手动构建具有旋转不变性的全局空间描述子，提高了位置重识别的精度，同时该方法在具有动态物体时也能取得较好的精度。文献［8］、［9］在使用基于规则的方法检测到环境变化后，对预构建的地图进行局部替换，以保证预构建地图与周围环境的实时匹配。基于深度学习的方法通过学习参考帧和匹配帧获得定位结果。文献［10］、［11］、［12］、［13］将已构建的地图、实时采集的点云帧和采集轨迹输入端到端的网络中学习其特征描述符，再与地图中的描述符进行比较，从而进行定位。文献［14］、［15］采用相机语义信息辅助激光雷达进行定位。虽然深度学习的方法在具有动态物体和静态结构发生改变时具有较高的定位精度但是深度学习的方法需要对预构建地图和点云进行训练，且定位时间相比于上述基于规则的方法更长。

为了应对环境结构较预构建地图发生变化时产生的定位丢失和精度降低问题，本文提出一种利用激光雷达和惯性测量单元（IMU）实时构建局部地图融合normal distribution transform（NDT）定位的长周期鲁棒定位方法OL-NDT，OL-NDT由normal distributions transform localization（NDTL）和online mapping and localization（OLML）两部分组成。OLML与NDTL独立运行且相互补充，NDTL定位丢失时采用OLML输出的优化位姿作为定位结果输出，NDTL定位成功时将NDTL的定位输出位姿作为测量信息因子输入OLML的因子图中对OLML的全局位姿进行优化，该方法可有效修正OLML局部地图在世界坐标系的位姿，提高了定位精度，减少了因环境结构改变产生的定位丢失。

2　OL-NDT框架概述

OL-NDT采用激光雷达作为感知的主要传感器，虽然相机可以获得纹理信息，但是白天到晚上的光线变化和季节的变化会对相机的定位产生影响，造成定位丢失等问题^［16-17］，采用激光雷达作为主要传感器可以避免上述问题。

OL-NDT主要由OLML和NDTL构成，该框架的结构如图1所示。该框架前端接收激光雷达和IMU的输出作为输入，利用IMU输出的欧拉角对激光雷达的每一帧点云进行运动补偿，以矫正畸变，将矫正后的点云帧用于NDTL与OLML。首先利用建图算法对环境进行地图构建，获得预构建全局地图，下文简称全局地图，作为NDTL定位时的参考帧。其次对动态补偿后的点云帧进行地面滤除，将其作为匹配帧。最后采用NDT将其与全局地图进行配准，从而获得当前的定位信息，定位成功后输出位姿信息 $P_{M}^{W}$ ，其中， $W$ 为世界坐标系，表示为 ${W}$ ， $P$ 表示位姿， $M$ 表示NDTL。

图 1. OL-NDT系统框架图

Fig. 1. The system structure of OL-NDT

下载图片查看所有图片

OLML引入滑动时间窗的概念，只对时间窗 $[t - k, t]$ 内的数据进行处理，其中， $t$ 为当前时刻。OLML对运动补偿后的点云帧进行边特征和面特征提取，利用扫描帧到关键帧间地图配准的方法获得当前帧的姿态变换，从而对时间窗 $[t - k, t]$ 内的点云进行实时地图构建，下文简称局部地图。同时将 $[t - k, t]$ 中的关键帧、两个关键帧间的IMU预积分值作为因子节点输入因子图中，用于OLML的位姿优化。将NDTL输出的定位信息 $P_{M}^{W}$ 作为测量真值因子节点， $[t - k, t]$ 时间内NDTL输出的有效定位信息 $P_{M}^{W}$ 保存在队列 $Q_{M p}$ 中，以当前时刻t获得的 $P_{M}^{W}$ 作为全局坐标系的位姿真值输入因子图中， $t - k$ 时刻的位姿作为下次因子图优化的初始位姿，在NDTL输出的定位信息丢失时不将其输入队列 $Q_{M p}$ 中去优化定位信息。最后将优化后的位姿 $P_{F}^{W}$ 输出，在NDTL丢失后使用，其中，F表示OLML。对 $[t - k, t]$ 时间内的数据进行因子图优化，为了节省计算内存、提高运行时间，去除 $t - k$ 时刻前的无用历史节点，保留与当前时间窗节点相关的节点。

在NDTL定位成功且输出位姿 $P_{M}^{W}$ 后，则将 $P_{M}^{W}$ 作为定位结果输出，若NDTL定位丢失不能获得当前时刻 $t$ 的位姿，则将OLML输出的位姿 $P_{F}^{W}$ 作为定位结果输出。

2.1　NDTL

NDTL将运动补偿和地面点滤除后的激光雷达实时采集点云作为匹配帧、全局地图作为参考帧，进行NDT配准，获得当前定位。基于点云地图的NDT扫描匹配是自动驾驶和机器人领域的主流算法之一^［18］，NDT对环境的轻微变化具有适应性，在环境中存在较少的高动态和低动态物体且没有遮挡时仍然可以获得精准定位^［19-20］。NDTL首先对预构建的全局地图进行体素划分，并计算混合高斯分布概率密度函数，其次将处理后的点云帧通过旋转矩阵变换计算对应体素内的概率密度函数得到似然函数，最后利用高斯牛顿优化算法计算最大似然对应的旋转矩阵。将似然函数的对数记为得分值 $V_{s c o r e}$ ，将NTDL定位 $V_{s c o r e}$ 阈值设为0.8。 $V_{s c o r e} < 0.8$ 时表示匹配成功，成功定位，输出定位成功位姿 $P_{M}^{W}$ ；当 $V_{s c o r e} > 0.8$ 时，则认为是无效定位不输出位姿，采用OLML输出位姿 $P_{F}^{W}$ 作为定位结果，当再次定位成功后则输出 $P_{M}^{W}$ 作为定位结果。OL-NDT结合OLML，将NTDL的得分阈值降低从而提高定位精度。

2.2　OLML

OLML是基于稀疏图优化的激光雷达和IMU结合的局部SLAM，通过OLML构建的地图称为局部地图，用 $M_{F}$ 表示， $M_{F}$ 的坐标系与世界坐标系相同，为 ${W}$ 。该局部地图为 $[t - k, t]$ 时间内构建的点云地图，其中， $t$ 为当前时刻， $k$ 为时间窗口的大小，本研究中 $k = 20 s$ 。通过构建局部点云地图，可在全局地图定位失败后，立刻采用局部地图进行定位。

OLML前端对运动补偿后的点云帧进行面、线特征提取，通过特征匹配构建点云地图，局部地图的后端优化由因子图完成，如图2所示。将窗口长度可变的滑动窗引入局部地图的后端优化：在NDTL有效定位时，时间窗口大小为 $[t - k, t]$ ；在NDTL定位丢失时，根据其定位丢失时间确定时间窗口大小。当丢失时间 $t_{l o s s}^{M}$ 大于 $k$ 时，将 $t_{l o s s}^{M} + k$ 作为时间窗口的长度即时间窗口长 $k = m a x \{t_{l o s s}^{M} + k, k\}$ 。同时删除历史时间小于 $t - k$ 的无用节点，保留 $[t - k, t]$ 时间内的相关节点与路标点，这样既保证了因子图不会因为时间累计无限变大，使计算变得复杂，也不会因为删除历史节点后损失因子信息。该时间窗内的因子图包含3种类型的因子：IMU预积分值、激光雷达关键帧以及NDTL输出的有效定位信息 $P_{M}^{W}$ 。在地图构建过程中将上述3种类型的因子作为节点添加在因子图中，利用增量平滑和贝叶斯树优化构建地图。值得一提的是，将 $P_{M}^{W}$ 作为绝对测量值输出到因子图中，可以有效矫正OLML输出的全局位姿。

图 2. 局部因子图优化

Fig. 2. Local factor map optimization structure

下载图片查看所有图片

2.2.1　激光雷达关键帧因子

设定激光雷达一帧扫描需要的时间为 $h$ ，则 $t$ 时刻在雷达坐标系 ${L}$ 下获得的一帧点云数据 $S_{t}$ 实际上由 $[t - h, t]$ 间段内不同时刻获得的数据组成，在 $t - h$ 时刻到 $t$ 时刻车辆仍在运动，则 $t$ 时刻获得的 $S_{t}$ 是车辆在不同位姿下的集合，导致了点云的运动畸变。LOAM^［21］假设激光雷达在 $[t - h, t]$ 时刻内以恒定的角速度和线速度运动，在该假设的前提下可对激光雷达的位姿进行线性插值。设定 $t - h$ 是一帧点云数据 $S_{t}$ 的开始时刻， $O_{[t - h, t]}^{L}$ 是 $[t - h, t]$ 内激光雷达点云位姿的变化量，存在一数据点 $ρ$ ， $ρ \in S_{t}$ ，则 $t_{ρ}$ 表示该点对应的时间戳， $O_{[t - h, t_{ρ}]}^{L}$ 表示的是 $[t - h, t_{ρ}]$ 内的位姿变化。 $O_{[t - h, t]}^{L}$ 和 $O_{[t - h, t_{ρ}]}^{L}$ 的表达式为

O_{[t - h, t]}^{L} = [τ_{x}^{t}, τ_{y}^{t}, τ_{z}^{t}, θ_{x}^{t}, θ_{y}^{t}, θ_{z}^{t}]^{T},

（1）

O_{[t - h, t_{ρ}]}^{L} = \frac{t_{ρ} - (t - h)}{t - (t - h)} O_{[t - h, t]}^{L}

。（2）

两帧点云之间姿态变化大于阈值时为关键帧，设定两帧点云之间角度阈值为5°，距离阈值为0.5 m。该两帧可连续或非连续，初始帧直接设定为关键帧。OLML采取扫描帧到关键帧间地图的匹配方式，首先从运动补偿后的点云中提取边特征和平面特征，其次与关键帧间地图进行匹配获得当前位姿，计算当前点云帧与上一个关键帧的相对变化，当超过设定阈值时则判定当前点云帧为关键帧，并将其作为节点加入因子图中。

2.2.2　惯性测量单元预积分因子

IMU加速度计和陀螺仪分别测量对应方向上的线运动和角运动，IMU的真实值会受零偏和系统噪声影响，IMU在 $t$ 时刻的真实值与测量值之间的关系为

{\hat{ω}}_{t} = ω_{t} + b_{t}^{ω} + n_{t}^{ω},

（3）

{\hat{a}}_{t} = R (a_{t} - g) + b_{t}^{a} + n_{t}^{a}

，（4）

式中： ${\hat{ω}}_{t}$ 、 ${\hat{a}}_{t}$ 是t时刻世界坐标系 ${W}$ 下角速度和加速度的测量值； $ω_{t}$ 、 $a_{t}$ 为真实值； $b_{t}^{}$ 、 $n_{t}^{}$ 为零偏和系统噪声，系统噪声设置为高斯白噪声； $R$ 是机器人坐标系到世界坐标系的旋转矩阵。

由IMU获得时刻 $t$ 的加速度和角速度后，通过积分可知 $t + Δ t$ 时刻的位移 $x$ 、速度 $v$ 和旋转矩阵 $R$ 的表达式为

v_{t + Δ t} = v_{t} + g Δ t + R_{t} ({\hat{a}}_{t} - b_{t}^{a} - n_{t}^{a}) Δ t

，（5）

x_{t + Δ t} = x_{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}

，（6）

R_{t + Δ t} = R_{t} e x p [({\hat{ω}}_{t} - b_{t}^{ω} - n_{t}^{ω}) Δ t]

。（7）

定义IMU在 $[p, q]$ 一段时间内的相对运动量^［22］为 $Δ v_{p q}$ 、 $Δ x_{p q}$ 、 $Δ R_{p q}$ ，其表达式分别为

Δ v_{p q} = R_{p}^{T} (v_{q} - v_{p} - g Δ t_{p q}),

（8）

Δ x_{p q} = R_{p}^{T} (x_{q} - x_{p} - v_{p} Δ t_{p q} - \frac{1}{2} g Δ t_{p q}^{2}),

（9）

Δ R_{p q} = R_{p}^{T} R_{q}

。（10）

将 $Δ v_{p q}$ 、 $Δ x_{p q}$ 、 $Δ R_{p q}$ 作为因子输入因子图中进行优化获得更新后的姿态，使用因子图优化后，只需修改对应的 $Δ v_{p q}$ 、 $Δ x_{p q}$ 、 $Δ R_{p q}$ ，避免了从初值进行计算。

3　实验与分析

3.1　数据集说明

为了验证OL-NDT的定位精度和鲁棒性，采用长周期数据集MulRan^［23］进行实验。该数据集是64线Ouster激光雷达、IMU、GPS和超声波雷达组合构建的城市环境多模态数据集。MulRan数据集多次重复经过同一个地方，具有较长的时间跨度和明显的空间结构变化，可以有效验证算法定位精度和重识别情况，同时该数据集提供了真值可以进行轨迹对比。该数据集包含Riverside、KAIST、DCC和Sejong city等4个不同地点在不同时间采集的数据，本实验采用KAIST和DCC数据。KAIST和DCC数据时间序列和长度信息如表1所示。由表1可知，KAIST地点在2019年8月2日、2019年8月23日和2019年9月3日采集了3组序列，分别记为KAIST01、KAIST02和KAIST03，3组序列的均长为6.1 km。DCC地点在2019年6月20日、2019年8月23日和2019年9月2日采集了3组序列，分别记为DCC01、DCC02和DCC03，3组序列的均长为4.9 km。

表 1. 数据集详细信息

Table 1. Dataset details

Name	Session date			Average length /km
Name	01	02	03	Average length /km
DCC	2019‒08‒02	2019‒08‒23	2019‒09‒03	4.9
KAIST	2019‒06‒20	2019‒08‒23	2019‒09‒02	6.1
BUU	2023‒01‒15	2023‒02‒16		1

查看所有表

为了验证OL-NDT在较短未知路段的定位精度，用旋风智能车平台在北京联合大学校园采集了校园数据集，下文简称为BUU数据。BUU数据由velodyne16线激光雷达和瑞芬TL740D IMU采集数据组成，在2023年1月15日和2023年2月16日采集了2组序列，分别记为BUU01和BUU02。

3.2　大范围静态结构变化定位实验分析

本实验采用DCC01构建的全局地图作为预构建地图，DCC02与DCC03作为测试数据集验证所提方法在大范围静态结构变化下的鲁棒性和定位精度。DCC数据相对KAIST较小但是结构复杂，有广场、狭窄道路、山和十字路口，且在某些环境下静态结构有较大的变化。图3（a）、（b）中框出的部分表现了DCC02较DCC01在静态环境结构上的较大改变，DCC01数据采集时该路边正在施工，路边停着大卡车、吊车以及路边的护栏，并且中间的出入口未修缮完成，相反DCC02数据采集时周围的护栏和施工车辆已经消失，出入口也修缮完工。

图 3. DCC数据静态结构永久变化示例。（a）DCC01正在施工；（b）DCC02已施工完成

Fig. 3. An example of a permanent structural change in DCC. (a) Construction in progress in DCC01; (b) construction completed in DCC02

下载图片查看所有图片

图4（a）为DCC02轨迹真值（ground truth）的3D图，表示重叠路线的先后行驶关系，图4（b）为轨迹对比图，实线为轨迹真值，虚线为OL-NDT，点划线为NDT定位方法。由图4（b）框出部分（A标记框）可知，NDT方法在图3（b）静态结构变化区域出现了定位丢失，长度约为180 m，驶出图3（b）区域后找回定位。OL-NDT未发生定位丢失，且图4（b）轨迹对比表明，OL-NDT较NDT方法定位更准确。图4（c）为OL-NDT和NDT方法激光雷达帧（LiDAR scans）对应的位置绝对误差（abs error）的量化比较，该误差由计算定位结果与真值数据相同帧对应位置的三维欧氏距离得出。由图4（c）可知，在NDT定位成功的帧区间，OL-NDT较NDT绝对误差明显降低，定位精度更为准确。

图 4. 轨迹对比。（a）DCC02的轨迹形状与方向；（b）OL-NDT和NDT与真值的轨迹对比，框A，NDT定位丢失位置；（c）NDT与OL-NDT的绝对误差对比

Fig. 4. Trajectory comparison. (a) The shape and direction of the DCC02 trajectory; (b) comparison of OL-NDT and NDT trajectories with ground truth, box A, lost location of NDT; (c) absolute error comparison of NDT and OL-NDT

下载图片查看所有图片

图5表示DCC02数据经过图3（b）静态结构环境发生变化的地点时，使用OLML定位的情况。图5（a）为OL-NDT的定位轨迹和局部地图与全局地图的关系，图5（b）是NDTL定位消失地点时的局部放大图，其中，标记点B为当前局部地图的起点时刻，在标记点C时NDTL模块发生定位丢失，标记点D的NDTL的定位得分值为0.3，找回定位。标记点B到标记点D时间总长为32 s，说明OL-NDT在NDTL定位丢失时间大于设定时间窗口后，OLML会修改时间窗口持续建图，直到NDTL重新找回定位。图5（b）为OLML在NDTL定位丢失后构建的局部地图，由图5（b）局部地图与全局地图的位置匹配度可看出，在NDTL定位丢失后OLML可有效保证定位准确性。实验结果表明，OL-NDT的定位鲁棒性和精度较NDT有较大的提升。

图 5. OLML定位示意图。（a）OL-NDT定位轨迹和局部地图与全局地图的位置关系；（b）局部放大图

Fig. 5. OLML positioning schematic. (a) OL-NDT trajectory and the positional relationship between local map and global map; (b) partial enlarged map

下载图片查看所有图片

3.3　定位精度对比实验分析

本实验采用KAIST02构建的全局地图作为预构建地图，KASIT01与KASIT03作为测试数据集验证所提方法在环境发生变化时的定位精度。KAIST中没有静态结构变化大的场景，3个不同的时间序列KASIT01、KASIT02、KASIT03中有许多短期的静态结构变化（路边停留的车辆和暂留的行人）和动态障碍物（路上行驶的大小不一的车辆）。图6（a）、（b）、（c）展示了KAIST01、KAIST02和KAIST03在不同时间下同一位置路边的停车情况。

图 6. KAIST数据静态结构短期变化示例。（a）KAIST01路边停车情况；（b）KAIST02路边停车情况；（c）KAIST03路边停车情况

Fig. 6. An example of a short-term structural change in KAIST. (a) On-street parking situation of KAIST01; (b) on-street parking situation of KAIST02; (c) on-street parking situation of KAIST03

下载图片查看所有图片

将OL-NDT与LIO、NDT方法在KAIST01和KAIST02数据集上进行定位精度对比实验。LIO、NDT与所提算法在KAIST01和KAIST02数据集皆完成了全路段定位（由表1知平均长度为6.1 km），没有定位丢失。表2分别列出了平移的平均绝对误差（MAE）和累计误差与总路程长度的比（A/L）。在KAIST01数据集上：OL-NDT平移的MAE值为0.034 m，较LIO降低了0.089 m，较NDT降低了0.071 m；A/L值为0.40%，较LIO降低了1.06个百分点，较NDT降低了0.83个百分点。在KAIST03数据上OL-NDT的定位误差较LIO和NDT仍有下降。表2数据表明，OL-NDT的定位精度较LIO和NDT都有所提高。

表 2. KAIST01和KAIST03 数据上平均绝对误差和累计误差与总路程比

Table 2. MAE and A/L on KAIST01 and KAIST03

Dataset	NDT		LIO		OL-NDT
Dataset	MAE /m	A/L /%	MAE /m	A/L /%	MAE /m	A/L /%
KAIST01	0.105	1.23	0.123	1.46	0.034	0.40
KAIST03	0.099	1.20	0.116	1.31	0.031	0.36

查看所有表

3.4　短暂未知路段定位实验分析

本实验采用BUU01构建的全局地图作为预构建地图，BUU02作为测试数据集验证所提方法在无地图未知路段定位的可靠性和准确性。图7（a）为采集环境的鸟瞰图，图7（b）为BUU01构建的全局地图，框E为未采集数据区域，在实验中作为未知区域，长度约为54 m。

图 7. 北京联合大学校园数据示意图。（a）校园数据鸟瞰图；（b）校园数据全局地图，框E为无地图的场景

Fig. 7. Schematic of the campus data of Beijing Union University. (a) Aerial view of campus data; (b) global map of campus data, box E shows the a scene without a map

下载图片查看所有图片

实验设计行驶轨迹从全局地图中的右上角已知区域出发，到未知区域E后，再次进入到已知区域。图8（a）为OL-NDT定位轨迹、局部地图与全局地图的关系。图8（b）为全局地图未知区域E的局部放大图［较图8（a）逆时针旋转了90°］。图8（c）为OLML构建的局部地图［较图8（a）逆时针旋转了90°］。由图8（b）可看出，全局地图在该区域点云极少，与图8（c）OLML构建的局部地图形成鲜明的对比，且在未知区域的OL-NDT定位轨迹与全局地图匹配较高。

图 8. 未知区域定位结果示意图。（a）OL-NDT定位轨迹和局部地图与全局地图的位置关系；（b）全局地图未知区域E的放大图；（c）局部地图未知区域E的放大图

Fig. 8. Schematic of the result of the no-map position.(a) OL-NDT trajectory and local map relations with global map; (b) an enlarged view of the no-map region E of the global map. (c) an enlarged view of the no-map region E of the local map

下载图片查看所有图片

图9（a）为NDT算法与OL-NDT在相同点云帧下的定位得分 $V_{s c o r e}$ ，其中，97帧~160帧左右为未知区域， $V_{s c o r e}$ 值越小精度越高， $V_{s c o r e}$ 变化越小越可靠。图9（c）为图9（a）中OL-NDT的 $V_{s c o r e}$ 值图，由图9（c）可看出，OL-NDT的 $V_{s c o r e}$ 值稳定在0.8以下，远远小于NDT的 $V_{s c o r e}$ 值。图9（b）为非未知区域H的 $V_{s c o r e}$ 值放大图，表明OL-NDT的 $V_{s c o r e}$ 值和变化量小于NDT，说明OL-NDT定位精度和可靠性高于NDT。实验结果表明，OL-NDT可保证在经过短暂未知路段时的可靠性和准确性。

图 9. 未知区域下定位得分 $V_{s c o r e}$ 对比。（a）OL-NDT与NDT定位 $V_{s c o r e}$ 对比图；（b）框H部分放大图；（c）OL-NDT方法定位 $V_{s c o r e}$

Fig. 9. Comparison of localization score $V_{s c o r e}$ in unknown regions. (a) Comparison of OL-NDT and NDT localization $V_{s c o r e}$ ; (b) an enlarged view of box H part; (c) OL-NDT location $V_{s c o r e}$

下载图片查看所有图片

4　结论

提出结合NDTL和OLML的可长期鲁棒定位的定位方法OL-NDT。OL-NDT在MulRan数据集上表现出在具有动态障碍物变化和静态结构变化下仍具有精确定位的能力，在变化环境中仍可长期、鲁棒运行，累计误差占比较LIO和NDT算法分别降低了1.06个百分点和0.83个百分点。在北京联合大学的校园数据上，在地图缺失的未知区域的定位轨迹可与全局地图匹配。

所提方法在部分位置发生变化时可进行精确定位，但若运行环境结构变化较多，无法由NDTL定位修正OLML的输出位姿时，则需要在检测到地图匹配环境发生变化后，将其对应的变化更新在初始的全局地图上，所以后续的工作将在检测到环境变化后对全局地图进行高效、精准的地图更新。

参考文献

[1] Xu X B, Zhang L, Yang J A, et al. A review of multi-sensor fusion SLAM systems based on 3D LIDAR[J]. Remote Sensing, 2022, 14(12): 2835.

[2] 徐陈, 周怡君, 罗晨. 动态场景下基于光流和实例分割的视觉SLAM方法[J]. 光学学报, 2022, 42(14): 1415002.

Xu C, Zhou Y J, Luo C. Visual SLAM method based on optical flow and instance segmentation for dynamic scenes[J]. Acta Optica Sinica, 2022, 42(14): 1415002.

[3] ShiX S, LiD J, ZhaoP P, et al. Are we ready for service robots? the OpenLORIS-scene datasets for lifelong SLAM[C]∥2020 IEEE International Conference on Robotics and Automation (ICRA), May 31-August 31, 2020, Paris, France. New York: IEEE Press, 2020: 3139-3145.

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

[5] HessW, KohlerD, RappH, et al. Real-time loop closure in 2D LIDAR SLAM[C]∥2016 IEEE International Conference on Robotics and Automation (ICRA), May 16-21, 2016, Stockholm, Sweden. New York: IEEE Press, 2016: 1271-1278.

[6] Kim G, Choi S, Kim A. Scan context: structural place recognition robust to rotation and lateral variations in urban environments[J]. IEEE Transactions on Robotics, 2022, 38(3): 1856-1874.

[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] DingW D, HouS H, GaoH, et al. LiDAR inertial odometry aided robust LiDAR localization system in changing city scenes[C]∥2020 IEEE International Conference on Robotics and Automation (ICRA), May 31-August 31, 2020, Paris, France. New York: IEEE Press, 2020: 4322-4328.

[9] ZhaoM, GuoX, SongL, et al. A general framework for lifelong localization and mapping in changing environment[C]∥2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), September 27-October 1, 2021, Prague, Czech Republic. New York: IEEE Press, 2021: 3305-3312.

[10] LuW X, ZhouY, WanG W, et al. L3-net: towards learning based LiDAR localization for autonomous driving[C]∥2019 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), June 15-20, 2019, Long Beach, CA, USA. New York: IEEE Press, 2020: 6382-6391.

[11] WiesmannL, MarcuzziR, StachnissC, et al. Retriever: point cloud retrieval in compressed 3D maps[C]∥2022 International Conference on Robotics and Automation (ICRA), May 23-27, 2022, Philadelphia, PA, USA. New York: IEEE Press, 2022: 10925-10932.

[12] Luo L, Cao S Y, Han B, et al. BVMatch: lidar-based place recognition using bird’s-eye view images[J]. IEEE Robotics and Automation Letters, 2021, 6(3): 6076-6083.

[13] VidanapathiranaK, RamezaniM, MoghadamP, et al. LoGG3D-net: locally guided global descriptor learning for 3D place recognition[C]∥2022 International Conference on Robotics and Automation (ICRA), May 23-27, 2022, Philadelphia, PA, USA. New York: IEEE Press, 2022: 2215-2221.

[14] 任丽军, 刘元盛, 钟科娣. 基于改进PFPN的语义地图构建方法研究[J]. 激光与光电子学进展, 2022, 59(4): 0428002.

Ren L J, Liu Y S, Zhong K D. Building method of semantic map based on improved PFPN[J]. Laser & Optoelectronics Progress, 2022, 59(4): 0428002.

[15] 蒋林, 刘奇, 雷斌, 等. 激光与视觉融合识别并构建语义地图改善定位性能[J]. 中国激光, 2022, 49(18): 1810003.

Jiang L, Liu Q, Lei B, et al. Fusion of laser and vision to identify and construct semantic map to improve positioning performance[J]. Chinese Journal of Lasers, 2022, 49(18): 1810003.

[16] WangY, ChenH Y, LiuY F, et al. Edge-based monocular thermal-inertial odometry in visually degraded environments[EB/OL]. (2022-10-18)[2023-02-03]. https://arxiv.org/abs/2210.10033.

[17] Kazerouni I A, Fitzgerald L, Dooly G, et al. A survey of state-of-the-art on visual SLAM[J]. Expert Systems With Applications, 2022, 205: 117734.

[18] Javanmardi E, Javanmardi M, Gu Y L, et al. Pre-estimating self-localization error of NDT-based map-matching from map only[J]. IEEE Transactions on Intelligent Transportation Systems, 2021, 22(12): 7652-7666.

[19] AkaiN, MoralesL Y, TakeuchiE, et al. Robust localization using 3D NDT scan matching with experimentally determined uncertainty and road marker matching[C]∥2017 IEEE Intelligent Vehicles Symposium (IV), June 11-14, 2017, Los Angeles, CA, USA. New York: IEEE Press, 2017: 1356-1363.

[20] Zhong K D, Liu Y S, Yang J S, et al. A factor graph optimization mapping based on normaldistributions transform[J]. Turkish Journal of Electrical Engineering and Computer Sciences, 2022, 30(3): 1127-1141.

[21] ZhangJ, SinghS. LOAM: lidar odometry and mapping in real-time[EB/OL]. [2023-02-06]. https://www.roboticsproceedings.org/rss10/p07.pdf.

[22] Forster C, Carlone L, Dellaert F, et al. On-manifold preintegration for real-time visual: inertial odometry[J]. IEEE Transactions on Robotics, 2017, 33(1): 1-21.

[23] KimG, ParkY S, ChoY, et al. MulRan: multimodal range dataset for urban place recognition[C]∥2020 IEEE International Conference on Robotics and Automation (ICRA), May 31-August 31, 2020, Paris, France. New York: IEEE Press, 2020: 6246-6253.

齐含, 刘元盛, 张军, 满恂钰, 张志铭. 基于实时局部建图的激光雷达长周期定位方法[J]. 激光与光电子学进展, 2024, 61(4): 0428001. Han Qi, Yuansheng Liu, Jun Zhang, Xunyu Man, Zhiming Zhang. Long-Period Localization Method for LiDAR Based on Local Mapping[J]. Laser & Optoelectronics Progress, 2024, 61(4): 0428001.