标签：定位 Based Localization 特征 位置 Movable LiDAR DMR 点云
论文笔记—3D LiDAR-Based Precision V ehicle Localization with Movable Region Constraints
文章摘要
~~~~ ~~~ 本文讨论了一种基于已知地图信息的高性能相似度测量方法，称为交叉平均绝对差（CMAD）方法。应用常规归一化互相关（NCC）特征配准方法需要足够数量的特征点，这些特征点还必须表现出接近正态的分布。但是，现场扫描和收集的光检测和测距（LiDAR）测距点云数据很少，无法满足近乎正态分布的要求。因此，将NCC特征配准到地图特征时会发生相当大的定位错误。因此，提出了CMAD方法，以有效地提高NCC算法和定位精度。由于定位传感器的不确定性会导致定位过程出现偏差，因此建立了可驱动的移动区域（DMR），以限制位置搜索的范围，滤除不合理的轨迹并提高定位速度和性能。在基于窗口的，DMR–CMAD和DMR–NCC方法的定位结果以及同时定位和建图方法的定位结果之间进行了误差比较。 DMR–CMAD方法的准确性与基于窗口的方法没有太大差异：室内实验的均方根误差不超过10cm，室外实验的均方根误差不超过10–30cm。此外，DMR–CMAD方法是这三种方法中耗时最少的，并且与其他两种方法相比，DMR–NCC产生了更多的定位错误，并且需要更多的定位时间。最后，将DMR–CMAD算法用于汽车的成功现场即时定位。
关键词：
定位; 归一化互相关； 3D激光雷达
导语
~~~~
~~~
自动驾驶技术的开发是为了大大提高驾驶安全性和便利性，从而减轻驾驶员的负担。 未来，全自动汽车将有可能构成智能交通系统的主体，并完全取代人类驾驶员。 但是，以目前的形式，该技术在高级驾驶员辅助系统中得到了较为适度的实施。 自动驾驶技术的一项强制性功能是环境感知，可防止碰撞。 同样重要的是准确的定位，尤其是在城市道路上行驶车辆的城市环境中； 像人类驾驶员一样，自动驾驶汽车必须遵守交通规则。
车辆中现有的自动定位系统依赖于传感器的辅助，并且根据所用传感器的类型将其分为被动或主动传感器系统。无源传感器系统进一步分为全球导航卫星系统，惯性导航系统和立体视觉。全球导航卫星系统的准确性受非视距接收和多径干扰的影响[1]；在室内环境中操作时，它们也可能无法接收到一致的信号，从而影响了其提供位置信息的能力。惯性导航系统可即时提供车辆的准确相对位置，但随着时间的流逝，精度会不断下降[2]。立体视觉涉及基于视觉的车道检测，并通过检测停车线[3]，路缘石[4,5]，箭头[6,7]和交通信号灯[8]提供位置信息。但是，在光线昏暗的环境中，例如室内停车场，立体视觉的定位精度可能会降低；此外，这些系统在没有上述标志的情况下不能在道路上定位车辆。
现有的有源传感器包括激光测距仪和光检测与测距（LiDAR）系统，后者更为常用。 先前对智能汽车的研究主要讨论了二维（2D）LiDAR和V elodyne有源传感器系统的成功实施。 总体而言，有源传感器比无源传感器受到广泛青睐，因为它们简化了基本距离的距离估计过程，并产生了理想的定位结果。
同时定位和建图（SLAM）系统同时说明和更新未知环境的地图并定位代理。 SLAM是机器人导航系统的主要组成部分，并且在过去的二十年中有了长足的发展。顾名思义，SLAM系统同时提供定位和建图功能。定位功能包括基于路边石头，基于道路标记和基于地标建筑的定位。 Hata等。 [9]使用Velodyne传感器开发了一种基于路缘的定位技术，其中路缘被识别为障碍物，并通过LiDAR中的多层激光环压缩进行检测。随后，Hata等人[10]。再次使用V Elodyne传感器，提出了一种基于道路标记的定位技术；在这种方法中，使用LiDAR中的强度信息来捕获路面上的所有路标以进行定位。 Choi [11]随后建立了一种基于地标和建筑物的定位技术，该技术也使用了V Elodyne传感器。他开发了混合地图SLAM系统，通过同时使用由2D和三维（3D）映射组成的网格图和要素图来说明环境。 GraphSLAM算法用于2D建图，被Thrun等人视为最小二乘问题。 [12]。在大规模映射中，GraphSLAM可以处理大量特征并将全球定位系统信息集成到其建图过程中。例如，莱文森[13]结合了全球定位系统，惯性测量单元（IMU），里程表和LiDAR数据，以生成高分辨率的2D表面图。但是，3D建图比2D建图更可靠，更准确。特别是，无特征的道路在使用预先准备的地图进行导航时会受益最大，因为它们减轻了SLAM中的累积误差，SLAM的即时定位结果对于这些道路而言可能是不可取的。
先前的研究已经结合了各种估计技术来解决SLAM中的问题。 Smith和Cheeseman [14,15]开发了扩展的卡尔曼滤波器（EKF）来解决这些问题。但是，当EKF中的路标信息增加时，其协方差矩阵可能会扩大并加重计算负担。简而言之，路标定位错误会升级为EKF中的实质性累积错误。此外，EKF仅适用于求解线性系统。当用于求解非线性系统时，EKF可能会导致收敛缓慢或出现不希望的发散。随后，Montermerlo等。 [16,17]开发了FastSLAM，它是基于粒子过滤器的。 FastSLAM 1.0仅使用基本里程表信息来估计机器人的位置，因此，随着里程表中累积误差的增加，系统的估计精度也会降低。相比之下，FastSLAM 2.0应用了EKF更新的概念和非线性环境信息的线性化，以提高其定位精度。但是，使用EKF更新机器人的位置会增加环境信息的数量和计算成本。
对于3D点云图，可以采用现有方法，包括正态分布变换（NDT）[18-21]，迭代最近点（ICP）[22-24]和蒙特卡洛定位（MCL）[25-31]作为地图匹配模块。 NDT将点云空间划分为几个网格并计算其正态分布，然后通过计算概率分布函数来分析变换的最优解。 NDT比其他方法需要更多的点云数据，因为NDT通过它们的分布来分析变换关系，并且大量数据会导致计算速度变慢。 ICP是当前扫描匹配中最常用的算法。它搜索两个点云之间的最近点，并通过奇异值分解分析它们的变换关系。但是，ICP用于查找局部最优解，并且需要有利的点云数据和初始值才能产生相对令人满意的收敛性。传统的蒙特卡洛定位（MCL）算法[25,26]仍然需要解决三个问题。具体地，当难以确定粒子的数量时，或者当为粒子分配过多的权重时，该算法可能仅生成局部最优解，并且当机器人陷入此类解中后，无法恢复其位置。为了减轻这些问题，已经对MCL进行了改进，例如Kullback-Leibler散度MCL [27,28]和自适应MCL [30,31]。然而，围绕方向估计的问题仍然存在。此外，MCL涉及使用随机粒子，因此不适用于即时定位。为了解决上述问题，可以将预先准备的地图用于即时建图。在这项研究中，使用3D LiDAR进行3D环境感知，准备了一个地图数据库，并设计了一种算法，可以即时准确地进行室内和室外定位。此外，在文献中存在许多距离传感器模型，用于测量测量值与地图之间的互相关[32,33]。一种常见的技术称为地图匹配。地图匹配技术提供了将扫描转换为地图的功能。一旦两个图都在同一个参考系中，就可以使用图互相关函数对它们进行比较。应用常规归一化互相关（NCC）特征配准方法需要足够数量的特征点，这些特征点还必须表现出接近正态的分布。但是，现场扫描和收集的LiDAR测距点云数据很少，无法满足近乎正态分布的要求。因此，将NCC特征配准到地图特征时会发生相当大的定位错误。
早些时候，Chong等。 [34]提出了一种合成的LiDAR，其中根据3D特征扫描合成的2D信息，并通过2D方法解决了定位和建图方面的问题。 使用3D滚动窗口重建3D环境信息，并计算表面法线矢量。 随后，将3D点云分为多个块。 确定阈值以保留垂直于地面的特征点并将其投影到虚拟2D平面，从而完成合成LiDAR模型的构建。
本研究采用合成LiDAR作为其基本框架，提出了一种测量交叉平均绝对差（CMAD）的高鲁棒性相似性测量方法，并将CMAD集成在可驱动运动区域（DMR）中以进行即时定位。这种定位方法比传统的归一化互相关（NCC）方法更令人满意地检测到运动对象。值得注意的是，本文仅采用3D LiDAR，并且未合并其他里程表或IMU数据以提供支持。因此，该搜索方法不具有运动模型，并且不能准确地识别车辆行驶信息。此外，当使用基于窗口的方法进行定位时，估计的位置将突然向侧面或向后移动。因此，增加了DMR以限制不合理的运动区域，过滤不适当的定位轨迹，并提高定位系统的速度和性能。根据结果，尽管DMR方法的定位精度与基于窗口的方法没有显着差异，但与基于窗口的方法相比，它需要更少的时间来定位车辆。另外，当使用粒子过滤器进行定位时，不需要将粒子散布在整个窗口区域中。而是只需要将它们散布到尽可能远的可驱动区域，从而缩短收敛时间并实现即时定位。
方法
2.1 定位算法的程序结构
~~~~
~~~
图1说明了地图定位算法的结构。 对3D点云进行映射，校准和分段，以获得所需的点云信息，然后将该信息转换为网格图。 最后，采用虚拟的LiDAR扫描方法提取环境特征并建立地图数据库。
上述程序也用于LiDAR现场扫描过程。 提取所有特征后，通过平均能量方法估算初始位置和方向，并将DMR纳入特征配准过程以识别车辆的当前位置和方向。 
2.2 校准
~~~~
~~~
在实验中，由于未校准LiDAR的位置，因此偏离了路面，导致出现图2所示的点云信息模式。因此，从点云的
X
–
Z
X–Z
X–Z平面中选择了四个点，分别表示在图2a中，在(1)中选择了两个单独的点进行斜率计算。 然后，使用三角函数反函数
t
a
n
−
1
tan^{-1}
tan−1来计算角度
φ
A
φ_A
φA和
φ
B
φ_B
φB，如式(2)所示。 随后，获得两个角度之间的平均值，以识别旋转角度
θ
θ
θ，如(3)所示，并且对整个3D点云进行了校准，以简化地图数据库的构建。 图2b描绘了LiDAR航向角与车辆头部方向之间的差异。
2.3 分割
~~~~
~~~
对校准后的点云进行分段，以捕获与路面正交的特征。 图3a图示了原始点云的示意图，其中绿色，蓝色，红色和紫色的点分别代表墙壁，道路，车辆和管道。 在实验中只需要环境轮廓； 不需要车辆和管道。 因此，如图3b所示，在大于车辆
(
H
v
e
h
i
c
l
e
)
(H_{vehicle})
(Hvehicle)的高度但小于管道
(
H
l
i
n
e
p
i
l
e
)
(H_{line~pile})
(Hline pile)的高度的范围内对云进行了分段。 但是，如果出现其他点（例如，图中所示的紫色点），则在分割后可能无法完全呈现所需的轮廓。
分割的目的是获得分割点云的法线向量和道路点云的法线向量之间的内积。 因此，必须保留垂直于道路的要素。 内积在阈值之间的范围内，并表示所需的特征点。 计算内积需要分割道路的点云信息（图3d），并计算分割的点云和道路点云的法线向量。 通过将最小二乘平面拟合到局部相邻点来计算向量[35]。
在计算了分割点云和道路点云的法线向量之后(图3c,e)，并获得了道路点云的法线向量的平均值。 随后，计算了分割云的法线向量与上述平均值之间的内积。 最后，为内积确定阈值，并保留在-0.005和0.005之间的点云信息，如图3f,g所示。 图3b也是一样的。 
2.4 将点云转变为网格图
~~~~
~~~
将3D点云转换为2D栅格地图，以提取特征并根据其分割和校准来构建地图数据库。 图4a展示了分段的3D点云信息，该信息通过转换被压缩成网格图(图4b)。 有关墙壁和圆柱的环境信息包含在栅格地图中，其中每个栅格为10×10
c
m
2
cm^2
cm2。 另外，合并了栅格地图以建立如图4c所示的地图数据库，其中绿色区域代表车辆的可能位置； 值得注意的是，该数据库具有x轴，y轴和特征信息。 LiDAR现场扫描过程还采用了上述流程，以现场捕获环境特征，然后将其与特征数据一起注册到地图数据库中，以识别最佳位置和方向。
2.5 特征提取
~~~~
~~~
从栅格地图中提取环境特征，以将其与LiDAR数据进行配准。值得注意的是，使用虚拟LiDAR扫描而非全局扫描来提取特征，因为本地扫描的特征比全局扫描的特征更准确。如图5a所示，从半径为R（最大扫描范围的30％）的圆内扫描特征。初始扫描点(即0°)位于虚拟LiDAR的左侧，并且沿逆时针方向扫描数据。提取的特征表示为距离角数据(图5b)，其中x轴表示角度，y轴表示距离。因此，LiDAR扫描的每个周期都会产生360度的距离角度数据。假设网格数据库显示了N个可能的车辆位置，如图5a中的绿色区域所示，则LiDAR扫描的每个周期都会产生360度的距离角度数据。此外，由于N个位置中的每个位置都具有不同的环境特征，因此数据库总共具有N×360个特征数据。然后将这些数据合并到仅包含位置信息的地图数据库中。
2.6 特征配准
~~~~
~~~
记录地图和LiDAR特征，以在后续定位计算中获得车辆的最佳位置和航向角。 当前流行的模板匹配技术涉及通过NCC方法进行相似度测量[36-38]。
参考信号和测试信号分别表示为t和s，其在测试信号上的运动角度显示为
τ
τ
τ。
s
(
τ
)
s(τ)
s(τ)表示具有
τ
τ
τ角度偏移的测试信号。 NCC相似度表示为
c
(
τ
)
c(τ)
c(τ)，其计算公式如(4)所示： 
其中
m
i
m_i
mi和
m
s
(
τ
)
m_{s(τ)}
ms(τ)分别表示
τ
τ
τ和
s
(
τ
)
s(τ)
s(τ)的平均值；
σ
t
σ_t
σt和
σ
s
(
τ
)
σ_{s(τ)}
σs(τ)分别表示
t
t
t和
s
(
τ
)
s(τ)
s(τ)的标准偏差(SD)。 N代表参考信号的度数（设为360）。 该过程是通过在频域中进行傅立叶变换来降低计算成本的。 具体地说，当
c
(
τ
)
c(τ)
c(τ)最大化时，
τ
τ
τ表示测试和参考信号之间的最佳匹配角。
c
(
τ
)
c(τ)
c(τ)的值介于-1和1之间：值越接近1，则表明测试和参考信号越相似。
应用NCC需要足够数量的匹配点，这些匹配点还必须表现出接近正态的分布。 在此，通过LiDAR扫描的点云信息相对稀少。 因此，当NCC特征与LiDAR和地图的特征匹配时，将导致较大的定位误差。 为了增强NCC算法，应用CMAD方法比较了地图和LiDAR特征之间的相似性。 该过程涉及的参数如下所示 
其中
P
L
i
d
a
r
P_{Lidar}
PLidar代表现场LiDAR特征（图6b），
P
M
a
p
P_{Map}
PMap代表虚拟扫描的LiDAR特征（图6a），W代表在
P
L
i
d
a
r
P_{Lidar}
PLidar和
P
M
a
p
P_{Map}
PMap中扫描的障碍点的集合，
P
D
M
R
P_{DMR}
PDMR代表
D
M
R
DMR
DMR中的位置集合。 而V表示通过轮询机制选择的最佳位置和航向角。
如图6d所示，模板
(
P
L
i
d
a
r
)
(P_{Lidar})
(PLidar)与固定信号
(
P
M
a
p
)
(P_{Map})
(PMap)重叠以进行特征配准。 模板在固定信号上的移动角度表示为
τ
τ
τ，并且模板与信号之间的相似性用
c
(
τ
)
c(τ)
c(τ)表示，其计算公式如下 : 
其中，M代表W的程度，
τ
τ
τ的范围从1º到360º。 当
c
(
τ
)
c(τ)
c(τ)最小时，LiDAR和地图特征最匹配（图6c）。 由于（5）仅涉及一个点的特征配准，而DMR总共包含L个点，因此使用L个点中的最小
c
(
τ
)
c(τ)
c(τ)来确定最佳位置
P
B
e
s
t
P_{Best}
PBest，如（6）所示： 
其中，对应的
τ
τ
τ代表最佳航向角
θ
H
e
a
d
i
n
g
θ_{Heading}
θHeading。
2.7 估计初始位置
~~~~
~~~
特征提取是在本地执行的。 因此，某些角度没有显示任何距离值，因为在那个角度没有扫描障碍物，或者距离值超过了扫描半径R。接下来，通过平均能量方法计算了LiDAR和地图特征，这涉及将所有扫描的能量点的距离值相加并相除能量点总数之和 。 公式（7）测量地图的能量，其中N代表扫描的能量点数量，
d
M
a
p
d_{Map}
dMap代表第
i
i
i度的距离值，方程（8）度量LiDAR的能量，其中M代表能量点数量扫描，而
d
L
i
d
a
r
,
j
d_{Lidar},j
dLidar,j表示第
j
j
j度的距离值。 因此，在地图数据库中建立并实施了地图的能量数据。
平均能量方法也被用于全局搜索车辆的初始位置和航向角。 通过上述平均能量值选择车辆的可能位置，其中
E
L
i
d
a
r
i
s
E_{Lidaris}
ELidaris为LiDAR的能量。 设置了搜索范围以过滤接近
E
L
i
d
a
r
i
s
E_{Lidaris}
ELidaris的地图能量
其中t表示可容忍的误差，该误差在室内较小而在室外较大。 因为地图的点云比LiDAR的现场点云（相对稀少）更全面，所以地图的大多数能量值都比LiDAR高。 从理论上讲，可以通过全局搜索地图和LiDAR特征的最佳配准来获得初始位置。 为了减少计算时间，平均能量方法也被用于全局搜索车辆的粗略初始位置和航向角。 通过上述平均能量值选择车辆的可能位置。 使用平均能量方法获得的选择结果在图7a中用红色表示。 为了获得最佳的初始位置和航向角，CMAD将这些选定位置的特征与在必要时刻扫描的LiDAR特征进行配准（图7b）。 
2.8 基于窗口的定位
~~~~
~~~
在通过平均能量方法确定了车辆的初始位置之后，通过基于窗口的定位确定了下一个时间点的位置。 与用于全局搜索的平均能量方法不同，基于窗口的定位涉及通过窗口搜索方法在本地搜索车辆的可能位置，并且所需的计算时间要少得多。 首先，将通过平均能量法计算出的初始位置设为中心。 创建了一个w×h的窗口，在图8a中用紫色矩形框表示。 其次，根据初始位置的坐标确定窗口中当前时间点车辆的可能位置，并在图8b中显示为蓝点。 最后，将这些位置的特征与并发的LiDAR特征进行配准，以获得车辆的当前位置，如图8c中的橙色点所示。
2.9 基于DMR的定位
~~~~
~~~
由于仅使用3D LiDAR搜索了车辆的位置，而没有附加的里程表或IMU数据，并且由于系统不包含任何运动模型，因此无法识别车辆的运动信息。 因此，还进行了基于DMR(可驱动移动区域)的定位，以计算近似于实际车辆运动的轨迹。 尽管通过基于窗口的定位方式估算的运动轨迹可能会导致偏离正常运动状态的横向或后向偏离（如图9a，b中的红色圆圈所示），但DMR旨在解决此问题并减少了运动次数。 估计的车辆位置。
与基于窗口的定位类似，基于DMR的定位涉及通过平均能量方法估算车辆的初始位置和方向。以该初始位置为中心，建立了具有最大移动距离半径R和旋转半径r的DMR(可驱动移动区域)（在图11a中用紫色圆圈表示）。整个DMR如图10所示，其中
p
o
p_o
po表示先前估计的位置和初始点。
p
i
p_i
pi（附近位置）与
p
o
p_o
po之间的距离应不大于R，并且
p
i
p_i
pi与
(
p
o
+
r
)
(p_o + r)
(po+r)之间以及
p
i
p_i
pi与
(
p
o
−
r
)
(p_o-r)
(po−r)之间的距离必须大于或等于r。满足这两个条件的
p
i
p_i
pi是DMR内的一个点，如（10）所确定。由于已知初始位置的坐标，因此可以计算DMR中的其他位置；车辆可能的当前位置在图11b中用蓝点表示。与基于窗口的估计位置不同，DMR根据车辆前进方向的变化而旋转。因此，将从前一个时间点开始的航向角用作旋转角度，并使用z轴旋转整个搜索区域。最后，将该区域中的位置的特征与并发的LiDAR特征进行配准，以隔离车辆在地图上的当前位置，该位置由图11c中的橙色点标识。 
标签：定位,Based,Localization,特征,位置,Movable,LiDAR,DMR,点云 来源： https://blog.csdn.net/qq_46480130/article/details/117166452
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。