欢迎访问《控制理论与应用》期刊网站！

基于孪生网络的场景编码与智能车多尺度激光定位

doi: 10.7641/CTA.2024.30182

1. 武汉理工大学智能交通系统研究中心, 湖北武汉 430063

2. 武汉理工大学重庆研究院, 重庆 401120

基金项目: 国家重点研发计划项目(2022YFB2502904), 湖北省重点研发计划项目(2022BAA082), 武汉市人工智能创新专项项目(2022010702040064), 重庆市科技创新重大研发项目(CSTB2022TIAD–STX0003)资助.

详细信息

作者简介

陶倩文博士研究生,目前研究方向为3D计算机视觉和智能车定位等,E-mail: tqw@whut.edu.cn;

胡钊政教授,博士,目前研究方向为3D计算机视觉、车路协同、智能网联汽车等,E-mail: zzhu@whut.edu.cn;

孙勋培硕士研究生,目前研究方向为智能车定位和多传感器融合等,E-mail: asunxunp@whut.edu.cn;

万金杰硕士研究生,目前研究方向为机器学习和智能车定位等,E-mail: jinjiewan@whut.edu.cn;

陈琪莉硕士研究生,目前研究方向为车路协同和智能车定位等,E-mail: cqlll@whut.edu.cn.

通信作者

胡钊政，E-mail: zzhu@whut.edu.cn;Tel.:+8615102764879.

Scene coding for multi-scale LiDAR-based localization of intelligent vehicle with Siamese network

TAO Qian-wen¹ ， HU Zhao-zheng^1,2 ， SUN Xun-pei¹ ， WAN Jin-jie¹ ， CHEN Qi-li¹

1. Intelligent Transportation Systems Research Center, Wuhan University of Technology, Wuhan Hubei 430063 , China

2. Chongqing Research Institute of Wuhan University of Technology, Chongqing 401120 , China

Funds: Supported by the National Key Research and Development Program of China (2022YFB2502904), the Key Research and Development Program of Hubei Province (2022BAA082), the Artificial Intelligence Innovation Program of Wuhan (2022010702040064) and the Natural Science Foundation of Chongqing Province, China (CSTB2022TIAD–STX0003).

摘要

基于激光点云地图的智能车定位方法存在地图数据量大、匹配准确率低等问题, 为此, 本文提出了基于孪生网络的场景编码与智能车多尺度激光定位方法. 首先, 构建了基于场景编码的激光极化地图, 该地图以节点形式表征, 每个节点均包含点云极化图、点云场景编码以及全局位姿; 然后, 提出了基于激光极化地图的智能车多尺度定位方法, 包括基于GPS与运动模型的粗定位、基于直方图滤波的节点级定位和基于快速的GICP算法的度量级定位; 最后, 使用现场采集的校园道路数据集和公共KITTI数据集对所提出的算法进行测试验证. 实验结果表明, 基于所构建地图的节点级定位准确率分别为99.6%和96.9%, 平均定位误差分别为0.34 m和0.21 m, 且对不同类型的激光雷达传感器和不同的环境具有较强的鲁棒性.

关键词

智能车 / 激光定位 / 地图匹配 / 孪生网络

Abstract

Intelligent vehicle localization methods based on LiDAR point cloud maps have problems such as large amounts of map data and low matching accuracy, hence, a scene coding method and a multi-scale LiDAR-based localization method of intelligent vehicles based on Siamese network are proposed. Firstly, a polarized LiDAR map is constructed based on scene coding, which is represented with nodes, and each node contains a polarized LiDAR image, a point cloud scene coding, and a global pose. Secondly, a multi-scale localization of intelligent vehicles is readily realized based on the polarized LiDAR map, which contains coarse localization based on global positioning system (GPS) and motion model, node-level localization based on a histogram filter, and metric-level localization based on GICP algorithm. Finally, the proposed method is verified by the campus road dataset and the public KITTI dataset. The experimental results demonstrate that the accuracies of the node-level localization are 99.6% and 96.9%, the average localization errors are 0.34 m and 0.21 m, and the proposed method has strong robustness to different types of LiDAR sensors and different environments.

Keywords

intelligent vehicle / LiDAR localization / map matching / Siamese network

1 引言 2 激光极化地图构建 2.1 三维点云极化表征 2.2 面向激光点云场景编码的孪生网络构建 2.3 基于场景编码的激光极化地图构建 3 智能车多尺度定位 3.1 基于普通GPS/运动模型的粗定位 3.2 基于直方图滤波的节点级定位 3.3 基于三维点云配准的度量级定位 4 实验及分析 4.1 某校园道路场景实验结果与分析 4.2 KITTI数据集实验结果与分析 5 结论

1 引言

高精度的车辆定位技术作为智能汽车发展的基础与核心，现有定位方法根据传感器类型可以分为基于全球定位系统（global positioning system，GPS）、视觉和激光的定位方法. 基于GPS的智能车定位存在信号盲区的问题，虽然结合惯导系统（inertial navigation system，INS）可解决导航卫星信号短期丢失下的定位问题，但无法实现在长期丢失下的定位 ^[1] . 基于视觉 ^[2] 的智能车定位方法存在易受环境光照影响导致定位失败的问题，且由于三维重建导致定位精度较差. 而基于激光雷达 ^[3] 的智能车定位方法可直接获取环境三维结构信息，无需三维重建且定位精度高，并随着成本降低成为了智能车定位领域的研究热点.

基于激光雷达的智能车定位方法主要分为基于激光即时建图与定位 ^[4]（simultaneous localization and mapping，SLAM）的智能车定位和基于激光地图 ^[5] 的智能车定位. LeGO-LOAM ^[3] 算法是一种常见的激光 SLAM算法，在实时激光雷达里程计的基础上，通过回环检测以改善累积误差矫正定位轨迹，但是基于点云配准的回环检测准确率低且耗时长，无法精确地消除累积误差和优化定位轨迹. 基于激光SLAM的定位方法，由于累积误差问题，不适用于大范围GPS盲区下的智能车定位.

基于激光地图的智能车定位通过点云数据与高精度地图进行匹配获取先验信息，实现智能车定位，具有定位精度高且鲁棒的特点. Koide等 ^[5] 利用正太分布变化（normal distributions transform，NDT）算法，将当前点云与预先构建的3D点云地图进行匹配，并通过无迹卡尔曼滤波融合INS定位结果. 由于3D点云地图数据结构无序导致检索复杂、计算效率低，该方法在地图匹配阶段需要高精度INS定位结果协助检索地图. 一些研究人员在存储3D点云地图数据之前对其进行处理，如将其转换为栅格地图. Wan等 ^[3] 构建栅格地图存储激光反射强度和高度的统计信息，通过融合激光雷达、惯性测量单元、GPS数据实现智能车定位. 但是该方法同样需要精确的GPS作为初值，否则会陷入复杂的地图检索过程中，且占用栅格图对点云进行降维，地图丢失了环境三维结构信息. 另一部分学者利用车道级的矢量地图实现地图定位，地图中包含车道线、交通指示牌等定位地标. Zhang等 ^[6] 从激光雷达点云中提取车道线、停车线和杆状物的语义信息，与车道级矢量地图之间的异构数据关联起来，将定位表征为非线性最小二乘问题. 但是车道级地图缺乏全面的道路信息，且由于道路标志的不连续，无法为车辆定位提供持续可靠的参考信息.

另有学者提出了节点地图的概念，即地图由一系列节点组成，每个节点中可能包含点云的特征、全局描述符和全局位姿等数据. 在节点地图匹配上，Kim 等 ^[7] 提出了一种激光点云三维结构的非直方图全局描述符，在重定位过程中查询速度快，但匹配精度较低，容易定位失败. 为增强地图节点表征的唯一性，相关文献利用深度神经网络提取激光点云的特征. Chen 等 ^[8] 提出了OverlapNet网络利用点云深度等信息提取特征，并估计点云之间的重叠度和航向角. Yin ^[9] 等利用LocNet网络提取点云的特征向量，并将当前特征向量与基于节点的激光雷达地图进行匹配实现全局定位. 但上述方法仍需在每个地图节点中保存3D激光点云与特征，需要较大的存储空间. Tao等 ^[10] 提出了利用三通道图像分别保存点云距离与反射率信息，解算成点云的精度更高. 该方法利用三通道的点云极化图像构建轻量级激光节点地图，并使用基于隐马尔科夫模型的地图匹配方法完成智能车定位. 但节点特征提取较为单一，导致节点级定位匹配精度与鲁棒性较低.

综上所述，基于激光地图的智能车定位的关键在于激光地图的紧凑且轻量、节点特征描述的高效性以及地图匹配的准确性. 因此，本文研究了一种基于孪生网络场景编码的激光极化地图构建方法，图1为该方法的流程图，其创新和贡献主要体现为:

1）提出了一种基于场景编码的激光极化地图构建方法. 该地图由节点组成，节点中包含点云极化图、点云场景编码以及全局位姿. 点云极化图由三维激光点云极化表征得到，具有无损和轻量的特点. 点云场景编码通过孪生网络对极化图学习得到，可将极化图匹配转换为基于孪生网络场景编码的相似概率计算，提高了节点表征的效率与鲁棒性.

2）提出了一种基于直方图滤波的智能车多尺度定位方法，包含基于GPS/运动模型的粗定位、基于直方图滤波的节点级定位和基于三维点云配准的度量级定位. 其中节点级定位将地图节点级定位问题转化为基于直方图滤波框架的最佳状态估计问题，将状态定义为待定位地图节点，并通过孪生网络与视觉特征算子计算节点联合概率，提高了地图匹配的准确性与鲁棒性.

图1算法流程图

Fig.1Illustration of the proposed method

2 激光极化地图构建

在激光极化地图构建过程中，首先将激光点云作极化表征，然后利用孪生网络提取场景编码. 轻量化的激光极化地图由一系列数据采集节点组成，每个地图节点由3个元素描述: 点云极化图、点云场景编码和全局位姿.

2.1 三维点云极化表征

激光点云由数万个无序的点组成，每个点通常保存了三维坐标以及反射率信息. 无序点云数据处理复杂、计算量大且耗时，难以进行快速的相似性评估. 因此，本文提出了一种点云极化表征方法，将无序的三维点云转换为二维极化图像，更简洁、直观、结构化地表征三维点云.

如图2所示，激光雷达坐标系

O_{1} - X_{1} Y_{1} Z_{1}

下的三维点

p (p_{x} ， p_{y} ， p_{z})

可以通过点到激光雷达的距离、所处垂直激光通道和水平方位角来表示，即极坐标. 其所处垂直激光通道和水平方位角组成极化图的像素坐标

（ u ， v ）

由下式计算:

u = \frac{\arctan \frac{p_{y}}{p_{x}}}{θ_{res_h}}, v = \frac{\arcsin \frac{p_{z}}{d}}{θ_{res_v}},

(1)

式中:

θ_{res_h} 与 θ_{res_v}

分别为激光雷达水平角分辨率与垂直角分辨率; d为点到激光雷达的距离，由下式计算:

d = \sqrt{p_{x}^{2} + p_{y}^{2} + p_{z}^{2}} .

(2)

图2点云极化表征示意图

Fig.2Schematic of polarized LiDAR representation

本文利用三通道RGB图像存储三维点云信息，其 R和G通道分别存储点的距离的整数部分和小数部分，B通道存储点的反射强度信息. 一帧三维点云的全部信息就通过极化表征储存在点云极化图中，将无序的三维点云结构转化为有序的二维图像结构，同时极大程度上缩小了激光地图存储空间.

点云极化图的大小H × W由式（3）决定，以激光雷达Velodyne VLP-16为例，其激光扫描线数、水平视场角和水平角分辨率分别为16，360^◦，0.2^◦，故其采集点云生成的点云极化图大小为16 × 1800，同样地，根据不同的激光雷达特性可以得到不同大小的二维点云极化图，即

W = \frac{φ}{θ_{res - h}}, H = N

(3)

式中: N为激光扫描线数; φ为水平视场角; W为极化图的宽度; H为极化图的高度. 为了方便显示，本文将图像分割成10个均等大小的图像块，然后将图像块从上到下叠加，最终合成一个图像大小为（N × 10）×（W/10）的点云极化图.

2.2 面向激光点云场景编码的孪生网络构建

基于激光地图的智能车定位的关键在于如何将待定位节点的激光点云与地图节点中的激光点云进行匹配，获取与待定位节点最近的地图节点. 本文利用深度学习方法解决该问题，即使用孪生网络从点云极化图提取场景编码，并进行相似性建模，提高激光地图匹配的效率.

首先，本文将激光点云表征为点云极化图形式，然后构建孪生网络学习点云极化图之间的相似性，如图3所示. 孪生网络由上下两侧两个相同的主干网络构成（文中使用VGG16（visual geometry group）作为主干网络），并且两侧网络权重共享. 三维点云C₁，C₂通过极化表征方法转化为点云极化图I₁，I₂作为网络输入，通过主干网络进行特征提取获得特征向量h₁，h₂，称之为点云场景编码. 由于两侧主干网络权重共享，场景编码可在同一空间下进行比较，本文计算不同场景编码h₁与h₂之间的绝对差，并且通过使用sigmoid激活函数 ^[11] 将其映射到区间[0，1]之间，转化为一个相似概率S（I₁，I₂），即场景编码之间的相似度，从而实现激光点云之间的匹配，即

S (I_{1}, I_{2}) = s i g m o i d ({‖h_{1} - h_{2}‖}_{1}) .

(4)

假设激光点云C₁与C₂采集时传感器位姿分别为 x₁与x₂，计算x₁与x₂之间的相对位姿，当两帧激光点云之间的距离小于阈值ρ时，认为它们是在同一个位置被采集的，视为一对正样本，标签Y = 1. 反之，当它们之间的距离大于阈值ρ，则视为一对负样本，标签 Y = 0. 因此训练数据被自动标注为

Y = \{\begin{matrix} 1, {‖x_{1} - x_{2}‖}_{2} ⩽ ρ, \\ 0, {‖x_{1} - x_{2}‖}_{2} > ρ . \end{matrix}

(5)

利用上述标注方法，在车辆运动过程中可以产生大量的数据，简单快速，满足神经网络训练的要求. 在本文中选取的阈值ρ为1.5 m. 本文使用二分类交叉熵作为孪生网络的损失函数，从而训练网络中的可学习部分. 损失函数可表示为

\begin{matrix} L (I_{1}, I_{2}, Y) = Y \cdot l o g (S (I_{1}, I_{2})) + (1 - Y) l o g (1 - \\ S (I_{1}, I_{2})) + λ^{T} ‖ ω ‖_{2}, \end{matrix}

(6)

式中

λ^{T} ‖ ω ‖_{2}

为一个L₂权重衰减项，可提高网络的泛化能力.

2.3 基于场景编码的激光极化地图构建

基于场景编码的激光极化地图由一系列数据采集节点组成. 在每个采集点，数据采集车辆搭载激光雷达和组合惯导采集激光点云和高精度惯导数据. 其中组合惯导仅用于地图构建阶段，保证地图精度. 出于地图存储大小与读取效率的考虑，每个节点中只包含 3个元素:

1）点云极化图: 点云极化图是利用点云极化表征方法将激光点云表示为三通道RGB图像形式进行储存，占用存储空间小;

2）点云场景编码: 点云场景编码是利用孪生网络中的主干网络，从极化图中提取出的一种独特且可识别的场景表征，可作为已知信息保存进地图当中，在定位过程中，可直接提取待定位节点的场景编码与地图中所保存的场景编码进行相似概率计算，从而实现地图匹配，降低孪生网络的匹配耗时;

3）全局位姿: 全局位姿是车辆在GPS坐标系下的位姿表示P^g，可为度量级定位提供高精度的全局坐标参考. 由融合差分GPS与高精度惯导的激光SLAM计算得到，通过图优化实现全局位姿优化.

图3面向点云极化表征与场景编码的孪生网络设计

Fig.3Siamese network design for polarized LiDAR representation and scene coding

3 智能车多尺度定位

基于所构建的激光极化地图，本文利用“由粗到精”的多尺度定位方法，实现智能车高精度定位.

3.1 基于普通GPS/运动模型的粗定位

在定位初始阶段，利用普通GPS定位结果获得一定范围内的地图节点作为候选，因普通GPS定位精度较低，大约为3∼10 m，故选取其附近10 m范围内的地图节点作为地图节点候选，以保证候选中包含距离车辆当前位置最近的地图节点. 进一步地，在已有前两个时刻的定位结果时，根据匀速运动模型得到车辆 t 时刻预测位置P_t，即

P_{t} = 2 P_{t - 1} - P_{t - 2},

(7)

式中P_t₋₁和P_t₋₂为车辆在t − 1和t − 2时刻的定位结果. 选取P_t附近5 m范围内的地图节点作为地图节点候选. 将由普通GPS定位结果获得的地图节点候选与由运动模型预测获得的地图节点候选的交集作为粗定位结果，通过粗定位缩小后续直方图滤波的 Bin 检索数量，加快定位速度.

3.2 基于直方图滤波的节点级定位

本文将地图节点定义为待匹配状态x_t，将当前点云作为观测 y_t，激光极化地图则作为状态空间，即

M_{1 : K} = \{M_{1} ， M_{2} ， \dots ， M_{k} ， \dots ， M_{K}\} ， 1 ⩽ k ⩽ K ，

K是地图中节点总数，从而将地图节点级定位问题转化为最佳状态估计问题，即

x^{*} = \underset{x_{t}}{\arg m a x} p (x_{t} ∣ y_{1 : t}),

(8)

其中p（x_t|y_1:_t）表示在获取观测序列y_1:_t的条件下状态 x_t的概率.

本文采用直方图滤波框架求解上式，如图4所示，每个状态节点均定义为直方图的Bin，每个时刻的状态空间对应一个直方图. 从而通过Bin与Bin之间的状态转移可以完成t时刻的先验估计，即

\bar{b e l} (x_{t, i}) = \sum_{j = 1}^{K} p (x_{t, i} ∣ x_{t - 1, j}) b e l (x_{t - 1, j}), i \in [1, K],

(9)

式中:

\bar{b e l} (x_{t ， i})

为t时刻下第i个Bin的先验估计; p（x_t_，_i| x_t₋₁，j）为前后时刻任意两个状态之间的转移概率; bel（x_t₋₁，j）为直方图滤波t − 1时刻下第j个Bin的后验概率，也是下一时刻迭代的输入.

直方图滤波中的状态转移概率定义如下:

\begin{matrix} p (x_{t, i} ∣ x_{t - 1, j}) \propto e x p (- \frac{1}{2 σ^{2}} ‖ d (x_{t, i}, x_{t - 1, j}) - \\ v_{t} Δ t ‖^{2}) \end{matrix}

(10)

式中: d（x_t_，_i，x_t₋₁，j）为前后两个历史之间的距离; v_t为车辆的行驶速度，可通过车辆的历史位置计算得出; ∆t为前后时刻的时间差; σ为运动模型方差，表示运动不确定性.

图4基于直方图滤波的激光极化地图匹配

Fig.4Polarized LiDAR map matching based on histogram filter

通过对先验估计进行似然概率更新，即

b e l (x_{t, i}) \propto p (y_{t} ∣ x_{t, i}) \bar{b e l} (x_{t, i}),

(11)

式中: bel（x_t_，_i）为t时刻下第i个Bin的后验估计; p（y_t| x_t_，_i）为似然概率，y_t为t时刻下的观测. 观测由两部分特征组成，分别为场景编码特征和点特征，假定这两类特征之间相互独立，则有

p (y_{t} ∣ x_{t, i}) = p (y_{t, s} ∣ x_{t, i}) p (y_{t, m} ∣ x_{t, i}),

(12)

式中p（y_t_，_s|x_t_，_i）为由式（4）计算得到的待定位节点与地图节点之间孪生网络场景编码的相似概率，即场景编码之间的相似度为

p (y_{t, s} ∣ x_{t, i}) = S (y_{t, s}, x_{t, i}),

(13)

式中p（y_t_，_m|x_t_，_i）为待定位节点极化图与地图节点极化图之间的似然概率. 利用加速鲁棒特征（speeded-up robust features，SURF）对待定位节点极化图与地图节点极化图进行匹配，并采用随机样本一致性（random sample consensus，RANSAC）算法去除异常值，匹配点对数越多，定位节点极化图与地图节点极化图的相似度越高，似然概率越大. 因此，通过高斯模型构建基于SURF点特征匹配的似然概率模型为

p (y_{t, m} ∣ x_{t, i}) \propto e x p (- \frac{1}{2 σ_{m}^{2}} {‖m (I_{t}^{x}, I_{t}^{y}) - μ‖}^{2}),

(14)

式中:

m (I_{t}^{x} ， I_{t}^{y})

代表t时刻下待定位节点极化图

I_{t}^{y}

与地图节点极化图

I_{t}^{x}

的匹配点对数，µ为两幅极化图所提取的最大特征点数目，σ_m为方差.

3.3 基于三维点云配准的度量级定位

通过节点级定位从地图中匹配出与待定位节点最近的地图节点后，本文通过三维点云配准算法计算出待定位节点与最相似地图节点之间的相对位姿，实现度量级定位.

首先根据点云极化图的像素坐标和像素值信息解码恢复出最相似地图节点中的三维激光点云数据. 然后使用快速的GICP（generalized iterative closet point）算法 ^[12] 计算出待定位节点三维点云与最相似地图节点三维点云之间的相对位姿变换P^t . 最后利用地图节点中存储的全局位姿信息P^g，通过坐标系变换可获得待定位车辆在GPS全局坐标系下位姿P^l，即

P^{l} = P^{g} P^{t} .

(15)

4 实验及分析

为了验证所提算法的有效性，本文分别使用两种不同的数据集进行实验测试. 一种为智能车搭载传感器在某高校采集的校园道路数据集. 实验平台和实验场景如图5所示，智能车搭载激光雷达Velodyne VLP16和组合惯导系统，以20∼40 km/h的速度行驶在实验场景中进行数据采集. 激光雷达和组合惯导的采集频率均为10 Hz，并且时间上与空间上均已实现同步. 在数据采集过程中，本文分别在3个不同的时间段对实验场景进行多次数据采集，从而获得该场景下的实验数据集. 该路线全长504 m，覆盖面积1.5万m². 另一种为KITTI数据集 ^[13] . KITTI数据集是一个用于自动驾驶的公共数据集，它提供了由激光雷达Velodyne HDL-64E所采集的激光点云数据和由DGPS（differential global positioning system）所采集的位姿真值.

图5校园道路实验平台与场景

Fig.5Experimental platform and scenario on campus road

两个数据集搭载的激光雷达传感器参数如表1所示，通过激光雷达传感器参数可将该激光雷达采集的点云数据转换为极化图，图6分别给出了2个分别由 Velodyne VLP-16和Velodyne HDL-64E激光雷达采集的点云数据转化的极化图示例. 需要说明的是，尽管 Velodyne HDL-64E激光雷达扫描线之间的垂直分辨率不是均匀的，但是每个扫描线的垂直角度是固定的，因此仍可以通过查表法获取每个扫描线的垂直角度，从而将三维点云数据转化为二维极化图. 激光雷达 Velodyne VLP-16采集的一帧激光点云数据的平均大小为484.3 KB，而通过极化表征方法生成的极化图的平均大小为44.1 KB，数据存储压缩率为90.1%. 激光雷达Velodyne HDL-64E所采集的一帧激光点云数据平均大小为1961.3 KB，而对应的极化图的平均大小为384.7 KB，数据存储压缩率为80.4%. 这表明通过存储一帧极化图来替代原始激光点云数据，极大的缩小了激光地图的大小.

表1不同激光雷达传感器参数及相应的极化图大小

Table1LiDAR parameters and the size of polarized LiDAR images of different LiDAR sensors

图6不同激光雷达传感器产生的点云极化图

Fig.6Polar image from different LiDAR sensors

在这两个数据集上对本文所提算法进行评估，可测试该算法对不同激光雷达配置和不同智能车平台的兼容性. 本文的实验计算平台为具有Intel i7-9750H 2.60 GHz CPU，8 G RAM和NVIDIA GeForce GTX 1660Ti6 G的笔记本电脑.

4.1 某校园道路场景实验结果与分析

如图5（b）所示，智能车搭载传感器在闭环路线上采集了3个不同时段的数据，每个数据节点包含同步的一帧激光点云数据、一帧普通GPS数据和一帧高精度惯导数据. 首先，需要选取数据标注样本训练孪生网络，本文以第2个时间段和第3个时间段采集的数据作为训练数据，按照上文所介绍的样本自动标注方法从中生成了19000个正样本和42000个负样本对孪生网络进行训练. 然后使用第1个时间段采集的数据进行测试，该时间段所采集的数据中包含该闭环路线的 3圈数据，本文以第1圈数据构建激光极化地图，以第2圈数据进行定位实验. 定位数据中包含该闭环路线上451个待定位节点，相邻节点之间的平均间距约为 1.1 m. 值得注意的是，高精度惯导数据在实验中只有两个作用: 一是在激光极化地图构建过程中用于优化激化SLAM轨迹，获得更加精确的地图位姿信息; 另一个是在定位过程中作为待定位节点的真值信息，实验真值的获取方法与地图构建过程中全局位姿计算方法一致.

基于所构建的激光极化地图，首先利用GPS与运动模型进行粗定位，获取地图节点候选; 然后在地图节点候选的基础上进行节点级定位. 观测来源于地图节点候选的场景编码特征和SURF特征点，通过直方图滤波得到最佳地图节点匹配结果. 由于车辆通常在两个地图节点之间，本文认为若通过节点级定位能获得与待定位节点的高精度惯导真值最近的两个地图节点之一，则认为节点级定位成功.

实验结果如表2所示，当地图分辨率为 1.0 m 和 2.0 m 时，本文算法的节点级定位的准确率分别为 97.6%和96.2%. 当地图分辨率为2.0 m时，准确率最低，此时地图分辨率较大，地图节点较为稀疏，待定位节点与地图节点间距较远，导致相应激光点云之间的相似性降低，从而使得匹配准确率降低. 而当地图分辨率为1.5 m时，本文算法的节点级定位的准确率最高为99.6%，正确定位最近节点个数为449个. 图7进一步分析了仅孪生网络匹配的正确率与仅SURF点特征匹配的正确率，该正确率以匹配到的最大概率节点是实际地图中距离定位节点最近第n个（n = 1，2，3）节点的数目占所有定位节点比例计算得到. 实验结果表明，融合2个观测的正确率较仅单独一个观测的正确率得到了很大的提高.

本文选取文献 ^[7-8，10] 中提出的匹配算法在粗定位基础上进行节点级定位作为对比. 实验结果表明，与其他节点级定位方法相比，本文算法具有较小的节点级地图大小，且节点级定位具有更好的匹配准确率，较快的地图匹配速度，在不同地图分辨率下有良好的鲁棒性，能够为智能车定位提供准确的匹配节点.

考虑到地图存储空间的大小和节点级定位的准确率，本文选取极化地图的分辨率为1.5 m，完成并分析后续的度量级定位实验. 在度量级定位时，通过快速 GICP算法计算待定位节点与最相似地图节点之间的相对位姿，完成度量级定位，并且根据地图节点的全局位姿信息得到待定位节点的全局位姿轨迹，实现智能车全局定位. 此外，本文进行了多种对比算法实验验证最终的定位效果: 一是基于文献 ^[7-8，10] 的节点级定位结果进行快速的GICP点云配准获取度量级定位结果; 二是采用NDT算法 ^[14] 在本文节点级算法的基础上进行点云配准计算度量级定位结果; 三是采用 LeGO-LOAM算法 ^[4] 作为度量级定位的对比算法. 对比实验结果如表3所示，定位轨迹及其累计误差见图8. 误差是根据车辆定位位姿与实验真值位姿之间的绝对距离计算得到. 实验结果证明，与地图定位算法相比，LeGO-LOAM算法的定位误差会随着定位的距离增长累计增加，并且在定位结束后未能成功回环优化定位轨迹，导致最终的定位误差过大. 本文算法平均定位误差为0.34 m，定位误差小于1.0 m的节点占所有待定位节点的97.6%，且能实现约14 Hz的定位频率，高于激光雷达扫描频率. 与其他地图定位算法相比，本文算法具有更高精度的地图匹配效率与较快的定位频率. 本文算法采用GICP算法 ^[12] 进行度量级定位，较NDT算法 ^[14] 定位频率更高，定位精度与鲁棒性更好. 需要说明的是，尽管文献 ^[7] 算法节点级定位准确率较高，但由于其中存在18.0%的节点匹配到了次近的地图节点，导致其度量级定位的误差较大.

表2校园道路数据集节点级定位结果

Table2Node-level localization results of campus road dataset

4.2 KITTI数据集实验结果与分析

与校园数据集实验类似，本文从序列00和08中生成了8000个正样本和13000个负样本训练孪生网络，并同时使用序列07和05进行测试. 首先取间隔均匀分布的数据采集节点做地图节点，其余节点做定位测试节点. 其中序列 07全长 683 m，待定位节点有 411个，相邻节点之间的平均间距约为1.6 m，地图节点有417 个，地图分辨率为1.6 m. 与07序列不同的是，序列05 存在多个重合道路，但是在选取地图节点时，只需在同一道路上选取一条均匀分布的地图节点路线即可. 序列05全长2190 m，待定位节点地图节点有1246个，相邻节点之间的平均间距约为 1.8 m，地图节点各有 1041个，地图分辨率为2.1 m.

图7不同观测的正确率

Fig.7Accuracy form different observations

表3校园道路数据集度量级定位结果

Table3Metric localization results of campus road dataset

节点级定位结果如表4所示，07数据集中，本文所提算法正确定位地图节点个数为399个，节点级定位的准确率为97.1%. 05数据集中，本文所提算法正确定位地图节点个数为1215个，节点级定位的准确率为 96.9%. 与其他节点级定位方法相比，本文算法具有较小的节点级地图大小，且节点级定位具有更好的匹配准确率与较快的地图匹配速度，并对不同的场景地图匹配均有良好的鲁棒性，能够为智能车定位提供准确的匹配节点.

度量级定位结果如表5所示，07数据集中，定位轨迹如图9（a）所示，定位误差如图9（b）所示. 本文所提算法平均定位误差为0.22 m，定位误差小于0.7 m的节点占所有待定位节点的99.5%. 05数据集中，定位轨迹如图10（a）所示，定位误差如图10（b）所示. 本文所提算法平均定位误差为0.19 m，定位误差小于1.0 m的节点占所有待定位节点的97.7%. 基于Velodyne64线激光雷达，本文算法能实现约7 Hz的定位频率. 在对比实验中，由于KITTI数据集实使用的激光点云数据为64线激光雷达所采集，通过GICP算法 ^[12] 得到的度量级定位误差较小，较NDT算法 ^[14] 定位精度更高更鲁棒. 虽然文献 ^[7-8，10] 算法有些定位节点匹配到了较远的地图节点，但采用GICP算法也能实现较高的定位精度. 需要说明的是，文献 ^[8] 算法有 64个定位节点（占比5.1 %）在节点级定位中未能匹配地图节点，在计算度量级定位误差时没有计算这些定位失败的节点. 与其他地图定位算法相比，本文算法仍具有更高精度的地图匹配效率与较快的定位频率.

图8校园道路数据集度量级定位结果

Fig.8Metric localization results of campus road dataset

表4KITTI数据集节点级定位结果

Table4Node-level localization results of KITTI dataset

表5KITTI数据集度量级定位结果

Table5Metric localization results of KITTI dataset

5 结论

本文研究了一种基于场景编码的智能车多尺度激光定位方法. 利用极化表征方法对激光点云进行降维表征，通过孪生网络对极化图进行相似性建模并提取场景编码，构建激光极化地图. 在粗定位基础上，通过直方图滤波计算最优匹配地图节点，最后利用三维点云配准完成度量级定位. 利用校园道路数据集和KITTI数据集进行测试与评估，可得出以下结论:

1）将节点级地图匹配问题转换为基于直方图滤波框架的最佳状态估计问题，以地图节点为状态，将场景编码匹配和SURF点特征匹配计算节点联合概率作为观测，提高了节点级定位的准确率与地图匹配的效率. 校园道路数据集节点定位准确率为 99.6%，KITTI数据集节点定位准确率为96.9%.

2）通过基于场景编码的轻量级激光极化地图实现“由粗到精”的智能车多尺度定位，校园道路数据集平均定位误差为 0.34 m，定位频率约为14 Hz; KITTI 数据集平均定位误差为0.21 m，定位频率约为7 Hz，证明本文算法在不同场景、激光雷达和智能车平台中具有良好的鲁棒性.

图9KITTI 07数据集度量级定位结果

Fig.9Metric localization results of KITTI 07 dataset

图10KITTI 05数据集度量级定位结果

Fig.10Metric localization results of KITTI 05 dataset

图1算法流程图

Fig.1Illustration of the proposed method

下载: 全尺寸图片

图2点云极化表征示意图

Fig.2Schematic of polarized LiDAR representation

下载: 全尺寸图片

图3面向点云极化表征与场景编码的孪生网络设计

Fig.3Siamese network design for polarized LiDAR representation and scene coding

下载: 全尺寸图片

图4基于直方图滤波的激光极化地图匹配

Fig.4Polarized LiDAR map matching based on histogram filter

下载: 全尺寸图片

图5校园道路实验平台与场景

Fig.5Experimental platform and scenario on campus road

下载: 全尺寸图片

图6不同激光雷达传感器产生的点云极化图

Fig.6Polar image from different LiDAR sensors

下载: 全尺寸图片

图7不同观测的正确率

Fig.7Accuracy form different observations

下载: 全尺寸图片

图8校园道路数据集度量级定位结果

Fig.8Metric localization results of campus road dataset

下载: 全尺寸图片

图9KITTI 07数据集度量级定位结果

Fig.9Metric localization results of KITTI 07 dataset

下载: 全尺寸图片

图10KITTI 05数据集度量级定位结果

Fig.10Metric localization results of KITTI 05 dataset

下载: 全尺寸图片

表1不同激光雷达传感器参数及相应的极化图大小

Table1LiDAR parameters and the size of polarized LiDAR images of different LiDAR sensors

下载: 全尺寸图片

表2校园道路数据集节点级定位结果

Table2Node-level localization results of campus road dataset

下载: 全尺寸图片

表3校园道路数据集度量级定位结果

Table3Metric localization results of campus road dataset

下载: 全尺寸图片

表4KITTI数据集节点级定位结果

Table4Node-level localization results of KITTI dataset

下载: 全尺寸图片

表5KITTI数据集度量级定位结果

Table5Metric localization results of KITTI dataset

下载: 全尺寸图片

图1算法流程图

Fig.1Illustration of the proposed method

图2点云极化表征示意图

Fig.2Schematic of polarized LiDAR representation

图3面向点云极化表征与场景编码的孪生网络设计

Fig.3Siamese network design for polarized LiDAR representation and scene coding

图4基于直方图滤波的激光极化地图匹配

Fig.4Polarized LiDAR map matching based on histogram filter

图5校园道路实验平台与场景

Fig.5Experimental platform and scenario on campus road

图6不同激光雷达传感器产生的点云极化图

Fig.6Polar image from different LiDAR sensors

图7不同观测的正确率

Fig.7Accuracy form different observations

图8校园道路数据集度量级定位结果

Fig.8Metric localization results of campus road dataset

图9KITTI 07数据集度量级定位结果

Fig.9Metric localization results of KITTI 07 dataset

图10KITTI 05数据集度量级定位结果

Fig.10Metric localization results of KITTI 05 dataset

表1不同激光雷达传感器参数及相应的极化图大小

Table1LiDAR parameters and the size of polarized LiDAR images of different LiDAR sensors

表2校园道路数据集节点级定位结果

Table2Node-level localization results of campus road dataset

表3校园道路数据集度量级定位结果

Table3Metric localization results of campus road dataset

表4KITTI数据集节点级定位结果

Table4Node-level localization results of KITTI dataset

表5KITTI数据集度量级定位结果

Table5Metric localization results of KITTI dataset

图(10) / 表(5)

引用本文

陶倩文, 胡钊政, 孙勋培, 等. 基于孪生网络的场景编码与智能车多尺度激光定位. 控制理论与应用, 2025, 42(3): 521 – 530

复制

TAO Qianwen, HU Zhaozheng, SUN Xunpei, et al. Scene coding for multi-scale LiDAR-based localization of intelligent vehicle with Siamese network. Control Theory & Applications, 2025, 42(3): 521 – 530

Copy

计量

图1算法流程图

Fig.1Illustration of the proposed method

图2点云极化表征示意图

Fig.2Schematic of polarized LiDAR representation

图3面向点云极化表征与场景编码的孪生网络设计

Fig.3Siamese network design for polarized LiDAR representation and scene coding

图4基于直方图滤波的激光极化地图匹配

Fig.4Polarized LiDAR map matching based on histogram filter

图5校园道路实验平台与场景

Fig.5Experimental platform and scenario on campus road

图6不同激光雷达传感器产生的点云极化图

Fig.6Polar image from different LiDAR sensors

图7不同观测的正确率

Fig.7Accuracy form different observations

图8校园道路数据集度量级定位结果

Fig.8Metric localization results of campus road dataset

图9KITTI 07数据集度量级定位结果

Fig.9Metric localization results of KITTI 07 dataset

图10KITTI 05数据集度量级定位结果

Fig.10Metric localization results of KITTI 05 dataset

表1不同激光雷达传感器参数及相应的极化图大小

Table1LiDAR parameters and the size of polarized LiDAR images of different LiDAR sensors

表2校园道路数据集节点级定位结果

Table2Node-level localization results of campus road dataset

表3校园道路数据集度量级定位结果

Table3Metric localization results of campus road dataset

表4KITTI数据集节点级定位结果

Table4Node-level localization results of KITTI dataset

表5KITTI数据集度量级定位结果

Table5Metric localization results of KITTI dataset

LIU J, GUO G, ZHANG R. Residual-based fault detection and exclusion with enhanced localization integrity. IEEE Transactions on Vehicular Technology,2023,72(5):5798-5808.

XIAO Z, YANG D, WEN T,et al. Monocular localization with vector HD map(MLVHM): A low-cost method for commercial IVs. Sensors,2020,20(7):1870.

WAN G, YANG X, CAI R,et al. Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes. IEEE International Conference on Robotics and Automation. Piscataway, USA: IEEE,2018:4670-4677.

SHAN T, ENGLOT B. LeGO-LOAM: Lightweight and groundoptimized LiDAR odometry and mapping on variable terrain. IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE,2018:4758-4765.

KOIDE K, MIURA J, MENEGATTI E. A portable three-dimensional LiDAR-based system for long-term and wide-area people behavior measurement. International Journal of Advanced Robotic Systems,2019,16(2):1-16.

ZHANG C, LIU L, XUE Z,et al. Robust LiDAR localization on an HD vector map without a separate localization layer.2021 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS). Prague, Czech Republic: IEEE,2021:5536-5543.

KIM G, KIM A. Scan context: Egocentric spatial descriptor for place recognition within 3D point cloud map.2018 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS). Madrid, Spain: IEEE,2018:4802-4809.

CHEN X, LABE T, MILIOTO A,et al. OverlapNet: A siamese net-¨ work for computing LiDAR scan similarity with applications to loop closing and localization. Autonomous Robots,2022,46(1):61-81.

YIN H, WANG Y, DING X,et al.3d LiDAR-based global localization using Siamese neural network. IEEE Transactions on Intelligent Transportation Systems,2019,21(4):1380-1392.

TAO Q, HU Z, ZHOU Z,et al. SeqPolar: Sequence matching of polarized LiDAR map with HMM for intelligent vehicle localization. IEEE Transactions on Vehicular Technology,2022,71(7):7071-7083.

KOCH G, ZEMEL R, SALAKHUTDINOV R. Siamese neural networks for one-shot image recognition. ICML Deep Learning Workshop. New York, USA: ACM,2015:1-8.

SEGAL A, HAEHNEL D, THRUN S. Generalized-ICP. Robotics: Science and Systems. Pittsburgh, PA, USA: MIT Press,2009:1-8.

GEIGER A, LENZ P, URTASUN R. Are we ready for autonomous driving?The KITTI vision benchmark suite. IEEE Conference on Computer Vision and Pattern Recognition. Piscataway, USA: IEEE,2012:3354-3361.

MAGNUSSON M, LILIENTHAL A, DUCKETT T. Scan registration for autonomous mining vehicles using 3D-NDT. Journal of Field Robotics,2007,24(10):803-827.