Zhang J, Singh S. LOAM: Lidar odometry and mapping in real-time[C]//Robotics: Science and Systems. 2014, 2(9): 1-9.
@inproceedings{zhang2014loam, title={LOAM: Lidar odometry and mapping in real-time.}, author={Zhang, Ji and Singh, Sanjiv}, booktitle={Robotics: Science and Systems}, volume={2}, number={9}, pages={1–9}, year={2014}, organization={Berkeley, CA} }
本文所要解决的问题是利用3D Lidar感知的点云进行自我运动估计,并对穿越的环境建立地图。我们假设lidar是预校准的。我们还假设lidar的角速度和线速度随时间是平滑连续的,没有突变。第二种假设将在第7.2节中通过使用IMU来释放。 本文约定,我们使用右大写表示坐标系统。我们定义我们定义一次扫描(sweep)为完成一次扫描覆盖。我们使用右缀
k
,
k
∈
Z
+
k,k\in Z^+
k,k∈Z+ 表示扫描次数,
P
k
\mathcal{P}_k
Pk 表示扫描第
k
k
k 次时感知到的点云。我们定义了如下两个坐标系:
∙
\bullet
∙ Lidar坐标系
{
L
}
\{L\}
{L} 是一个原点在lidar几何中心的3D坐标系统。
x
x
x-轴指向左侧,
y
y
y-轴指向前方。点
i
,
i
∈
P
k
i,i\in\mathcal{P}_k
i,i∈Pk 在
{
L
k
}
\{L_k\}
{Lk} 中的坐标值被记为
X
k
,
i
L
\pmb{X}_{k,i}^L
Xk,iL。
∙
\bullet
∙ 世界坐标系
{
W
}
\{W\}
{W} 是与
L
{L}
L 的初始位置重合的3D坐标系。点
i
i
i 的坐标值 在
{
W
k
}
\{W_k\}
{Wk} 中的坐标值被记为
W
k
,
i
L
\pmb{W}_{k,i}^L
Wk,iL。 通过假设和记号,我们的lidar里程计和建图问题可以定义为: 问题:给定lidar点云的序列
P
k
,
k
∈
Z
+
P_k,k\in Z^+
Pk,k∈Z+ ,在每次扫描时计算自身运动,并且使用
P
k
\mathcal{P}_k
Pk 建立地图。
图三展示了软件系统。设定
P
^
\hat{\mathcal{P}}
P^ 为雷达扫描获得的点云。在每次扫描过程中,
P
^
\hat{\mathcal{P}}
P^ 被注册在
{
L
}
\{L\}
{L} 中。在第
k
k
k 次扫描中结合的点云形成了
P
k
\mathcal{P}_k
Pk。然后,
P
k
\mathcal{P}_k
Pk 在两个算法中被处理。Lidar里程计获取点云并在两个连续扫描之间计算点云运动。估计出来的位姿被用来校正
P
k
\mathcal{P}_k
Pk。该(里程计)算法以10Hz左右的频率运行。输出通过lidar建图进一步处理,这个过程将校正畸变的点云以1Hz的频率匹配和注册到地图中。最后,由两个算法输出的位姿变换被集成,生成大约10Hz的Lidar相对于地图的变换的输出。第五、六节详细展示了软件框图中的模块。
图3. Lidar里程计和建图软件系统框图
5. Lidar里程计
5.1 特征点提取
我们从在lidar点云
P
k
\mathcal{P}_k
Pk中提取特征点开始。图2中展示的lidar自然地生成了
P
k
\mathcal{P}_k
Pk中不均匀分布的点。从激光扫描仪返回分辨率为## 5.2 找到特征点对应关系°的扫描。这些点位于一个扫描平面上。但是,随着激光扫描仪以180°/s的角速度旋转,以40Hz生成扫描结果,与扫描平面垂直方向的分辨率为180°/40=4.5°。考虑实际情况,特征点从
P
k
\mathcal{P}_k
Pk 中提取,仅使用了来自单个扫描的信息,具有共平面集合关系。 我们在锐利边缘和平面表面块上选择特征点。设置
i
i
i 为
P
k
,
i
∈
P
k
\mathcal{P}_k,i\in\mathcal{P}_k
Pk,i∈Pk 中的一个点,
S
S
S 为激光扫描仪在同一次扫描中返回的连续点的集合。因为激光扫描仪生成的点以CW或CCW顺序返回,
S
S
S 每边包含一半的点,以及两点之间间隔为0.25°。定义如下项以评估局部表面的平滑性:
c
=
1
∣
S
∣
∥
X
(
k
,
i
)
L
∥
∥
∑
j
∈
S
,
j
≠
i
(
X
(
k
,
i
)
L
−
X
(
k
,
j
)
L
)
∥
.
(1)
c=\frac{1}{\vert\mathcal{S}\vert\Vert\pmb{X}_{(k,i)}^L\Vert}\Vert\sum_{j\in\mathcal{S},j\neq i}(\pmb{X}_{(k,i)}^L-\pmb{X}_{(k,j)}^L)\Vert.\tag{1}
c=∣S∣∥X(k,i)L∥1∥j∈S,j=i∑(X(k,i)L−X(k,j)L)∥.(1) 扫描中的点基于
c
c
c值进行排序,
c
c
c值最大的为边缘点,
c
c
c值最小的为平面点。为了环境中均匀分布特征点,我们将扫描分为四个相同的子区域。每个子区域最多可以提供2个边缘点和4个平面点。一个点
i
i
i 只有在它的
c
c
c 值大于或小于一个阈值的时候才能够被选为一个边缘点或一个平面点,并且被选的点数量不能超过最大值。 当选择特征点时,我们想要避免周围点已经被选择的点,或者局部平面上大致与激光束平行的点(如图4(a)中的点B)。这些点通常被认为是不可靠的。同样,我们希望避免在被遮挡区域边界上的点[23]。如图4(b)所示。点
A
A
A是激光雷达云中的一个边缘点,因为它的连接表面(虚线段)被另一个对象挡住了。然而,如果激光雷达移动到另一个视角,被遮挡的区域就会发生变化,变得可观察到。为了避免前面提到的点被选择,我们再次找到点集
S
\mathcal{S}
S。点
i
i
i 只有在
S
\mathcal{S}
S不形成一个与激光束近似平行的表面块并且
S
\mathcal{S}
S 中没有一个点与
i
i
i 在激光束方向上有一个间隙,同时比点
i
i
i 更接近激光雷达(如图4(b)中的点
B
B
B)。 总结来说,从
c
c
c值最大的特征点中选择边缘点,从
c
c
c值最小的特征点中选择平面点,并且如果一个点被选择:
∙
\bullet
∙被选择边缘点、平面点数量不超过子区域最大值。
∙
\bullet
∙它周围的点都没有被选取。
∙
\bullet
∙它不在与激光束近似平行的表面块上,也不在一个被遮挡区域的边缘。 从走廊场景中提取特征点的示例如图5所示。边缘点和平面点分别用黄色和红色标记。
里程计算法在一次扫描中估计lidar的运动。设置
t
k
t_k
tk 为一次扫描
k
k
k 的开始时间。在一次扫描结束时,这次扫描期间感知到的点云为
P
k
\mathcal{P}_k
Pk,被重投影到时间戳
t
k
+
1
t_{k+1}
tk+1 上,如 图6 所示。我们将重投影的点云设为
P
‾
k
\overline{\mathcal{P}}_k
Pk。在下一次扫描
k
+
1
k+1
k+1 时,
P
‾
k
\overline{\mathcal{P}}_k
Pk 将和新接收到的点云
P
k
+
1
\mathcal{P}_{k+1}
Pk+1 一同用以估计lidar运动。
图6. 一次扫描结束重投影的点云
让我们假设
P
‾
k
\overline{\mathcal{P}}_k
Pk 和
P
k
+
1
\mathcal{P}_{k+1}
Pk+1现在都是可用的,并从寻找两个激光雷达云之间的对应关系开始。对于
P
k
+
1
\mathcal{P}_{k+1}
Pk+1,我们使用上一节讨论的方法从激光雷达云中找到边缘点和面点。设
E
k
+
1
\mathcal{E}_{k+1}
Ek+1 为边缘点集合,
H
k
+
1
\mathcal{H}_{k+1}
Hk+1 为平面点集合。我们将找到
P
‾
k
\overline{\mathcal{P}}_k
Pk的边线作为
E
k
+
1
\mathcal{E}_{k+1}
Ek+1 中的点的对应点,平面块作为
H
k
+
1
\mathcal{H}_{k+1}
Hk+1中的点的对应点。
请注意,在扫描
k
+
1
k+1
k+1 开始时,
P
k
+
1
\mathcal{P}_{k+1}
Pk+1是一个空集,它会在在扫描过程中,随着接收到更多的点而增长。lidar里程计在扫描过程中递归估计6自由度运动,并随着
P
k
+
1
\mathcal{P}_{k+1}
Pk+1 的增加逐渐包含更多的点。在每次迭代中,
E
k
+
1
\mathcal{E}_{k+1}
Ek+1 和
H
k
+
1
\mathcal{H}_{k+1}
Hk+1 使用当前估计的变换重新投影到扫描的开始。设
E
~
k
+
1
\tilde{\mathcal{E}}_{k+1}
E~k+1 和
H
~
k
+
1
\tilde{\mathcal{H}}_{k+1}
H~k+1 为重投影点集。对于
E
~
k
+
1
\tilde{\mathcal{E}}_{k+1}
E~k+1 和
H
~
k
+
1
\tilde{\mathcal{H}}_{k+1}
H~k+1 中的每个点,我们将在
P
‾
k
\overline{\mathcal{P}}_k
Pk 中找到最近的邻居点。在这里,
P
‾
k
\overline{\mathcal{P}}_k
Pk 存储在3D KD树[24]中,以便快速索引。
图7(a)表示查找边线作为边缘点的对应点的过程。假设
i
i
i 是
E
~
k
+
1
\tilde{\mathcal{E}}_{k+1}
E~k+1 中的一点,
i
∈
E
~
k
+
1
i\in\tilde{\mathcal{E}}_{k+1}
i∈E~k+1。边线由两点表示。设置
j
j
j 为
i
i
i 在
P
‾
k
\overline{\mathcal{P}}_k
Pk 中最近的邻居点,
j
∈
P
‾
k
j\in\overline{\mathcal{P}}_k
j∈Pk。设置
l
l
l 为
i
i
i 在两次连续扫描中离
j
j
j 的扫描最近的邻居点。
(
j
,
l
)
(j,l)
(j,l) 的形式表示
i
i
i 的对应关系。然后,为了验证
j
j
j 和
l
l
l 都是边缘点,我们基于式(1)检查局部表面平滑度。在这里,我们特别要求
j
j
j 和
l
l
l 来自不同扫描,因为单次扫描不能包含相同边线的多个点。只有一个例外,即边缘线在扫描平面上。但如果是这样,则边缘线退化,在扫描平面上呈现为一条直线,边缘线上的特征点首先就不能提取。
图7. 寻找一条边线作为边缘点的对应点...
图7(b)为平面点对应寻找平面块的过程。假设
i
i
i 是
H
~
k
+
1
\tilde{\mathcal{H}}_{k+1}
H~k+1 上的点,
i
∈
H
~
k
+
1
i\in\tilde{\mathcal{H}}_{k+1}
i∈H~k+1。平面块用三个点表示。与上一段类似,我们在
P
‾
k
\overline{\mathcal{P}}_k
Pk 中找到点
i
i
i 的最近邻居点记为
j
j
j 。然后,我们找到另外两个点
j
j
j 和
m
m
m ,作为
i
i
i 的最近邻居,一个在
j
j
j 的同一扫描中,另一个在
j
j
j 的连续两次扫描中。这保证了这三个点是非共线的。为了验证
j
j
j 、
l
l
l 和
m
m
m 都是平面点,我们根据式(1)再次检查局部曲面的平滑度。
在找到特征点的对应点后,我们推导出计算特征点到对应点的距离的表达式。在下一节中,我们将通过最小化特征点的总体距离来恢复lidar运动。我们从边缘点开始。对于点
i
∈
E
~
k
+
1
i\in\tilde{\mathcal{E}}_{k+1}
i∈E~k+1,如果
(
j
,
l
)
(j,l)
(j,l) 是对应的边线,
j
,
l
∈
P
‾
k
j,l\in\overline{\mathcal{P}}_k
j,l∈Pk,则点到线的距离可计算为:
d
E
=
∣
(
X
~
(
k
+
1
,
i
)
L
−
X
‾
(
k
,
j
)
L
)
×
(
X
~
(
k
+
1
,
i
)
L
−
X
‾
(
k
,
l
)
L
)
∣
∣
X
‾
(
k
,
l
)
L
−
X
‾
(
k
,
j
)
L
∣
(2)
d_{\mathcal{E}}=\frac{\vert(\tilde{\pmb{X}}^L_{(k+1,i)}-\overline{\pmb{X}}^L_{(k,j)})\times(\tilde{\pmb{X}}^L_{(k+1,i)}-\overline{\pmb{X}}^L_{(k,l)})\vert}{\vert\overline{\pmb{X}}^L_{(k,l)}-\overline{\pmb{X}}^L_{(k,j)}\vert}\tag{2}
dE=∣X(k,l)L−X(k,j)L∣∣(X~(k+1,i)L−X(k,j)L)×(X~(k+1,i)L−X(k,l)L)∣(2) 其中
X
~
(
k
+
1
,
i
)
L
,
X
‾
(
k
,
l
)
L
\tilde{\pmb{X}}^L_{(k+1,i)},\overline{\pmb{X}}^L_{(k,l)}
X~(k+1,i)L,X(k,l)L 和
X
‾
(
k
,
j
)
L
\overline{\pmb{X}}^L_{(k,j)}
X(k,j)L是
{
L
}
\{L\}
{L} 中点
i
i
i 、
j
j
j 和
l
l
l 的坐标。然后,对于点
i
∈
H
~
k
+
1
i\in\tilde{\mathcal{H}}_{k+1}
i∈H~k+1,如果
(
j
,
l
,
m
)
(j,l,m)
(j,l,m) 是对应的平面块,
j
,
l
,
m
∈
P
‾
k
j,l,m\in\overline{\mathcal{P}}_k
j,l,m∈Pk,则点到平面的距离可计算为:
d
H
=
∣
X
~
(
k
+
1
,
i
)
L
−
X
‾
(
k
,
j
)
L
(
X
‾
(
k
,
j
)
L
−
X
‾
(
k
,
l
)
L
)
×
(
X
‾
(
k
,
j
)
L
−
X
‾
(
k
,
m
)
L
)
∣
∣
(
X
‾
(
k
,
j
)
L
−
X
‾
(
k
,
l
)
L
)
×
(
X
‾
(
k
,
j
)
L
−
X
‾
(
k
,
m
)
L
)
∣
(3)
d_{\mathcal{H}} =\frac{\left\vert\begin{matrix}\tilde{\pmb{X}}^L_{(k+1,i)}-\overline{\pmb{X}}^L_{(k,j)} \\(\overline{\pmb{X}}^L_{(k,j)}-\overline{\pmb{X}}^L_{(k,l)})\times(\overline{\pmb{X}}^L_{(k,j)}-\overline{\pmb{X}}^L_{(k,m)})\end{matrix}\right\vert} {\vert(\overline{\pmb{X}}^L_{(k,j)}-\overline{\pmb{X}}^L_{(k,l)})\times(\overline{\pmb{X}}^L_{(k,j)}-\overline{\pmb{X}}^L_{(k,m)})\vert}\tag{3}
dH=∣(X(k,j)L−X(k,l)L)×(X(k,j)L−X(k,m)L)∣X~(k+1,i)L−X(k,j)L(X(k,j)L−X(k,l)L)×(X(k,j)L−X(k,m)L)(3) 其中
X
‾
(
k
,
m
)
L
\overline{\pmb{X}}^L_{(k,m)}
X(k,m)L 是
{
L
}
\{L\}
{L} 中点
m
m
m 的坐标。
5.3 运动估计
在扫描过程中,lidar 的运动建模为恒定的角速度和线速度。…
5.4 Lidar里程计算法
lidar里程计算法如算法1所示。该算法以最后一次扫描的点云
P
‾
k
\overline{\mathcal{P}}_k
Pk、当次扫描的增长点云
P
k
+
1
\mathcal{P}_{k+1}
Pk+1,以及最后一次递归的位姿变换
T
k
+
1
L
\pmb{T}^L_{k+1}
Tk+1L 作为输入。如果开始新的扫描,
T
k
+
1
L
\pmb{T}^L_{k+1}
Tk+1L 被设置为零(第4-6行)。然后从
P
k
+
1
\mathcal{P}_{k+1}
Pk+1 中提取特征点,构造第7行
E
k
+
1
\mathcal{E}_{k+1}
Ek+1 和
H
k
+
1
\mathcal{H}_{k+1}
Hk+1。对于每个特征点,我们在
P
‾
k
\overline{\mathcal{P}}_k
Pk 中找到它的对应点(第9-19行)。运动估计被编写到一个鲁棒拟合[27]中。在第15行中,算法为每个特征点分配了一个二方(bisquare)权重。与对应点之间距离较大的特征点被赋予较小的权重,距离大于阈值的特征点被认为是异常值,并赋予零权重。然后,在第16行中,姿势变换在每次迭代中被更新。如果找到收敛点或满足最大迭代次数,非线性优化就终止。如果算法到达扫描的结束,则使用扫描期间估计的运动将
P
k
+
1
\mathcal{P}_{k+1}
Pk+1重新投影到时间戳
t
k
+
2
t_{k+2}
tk+2 上。否则,下一轮递归只返回变换
T
k
+
1
L
\pmb{T}^L_{k+1}
Tk+1L 。
图7. 寻找一条边线作为边缘点的对应点...
6. Lidar建图
建图算法的运行频率比里程计算法低,并且每次扫描只调用一次。在扫描
k
+
1
k+1
k+1 结束时,lidar雷达里程计生成一个未扭曲的点云
P
k
+
1
\mathcal{P}_{k+1}
Pk+1 ,同时生成一个姿态变换
T
k
+
1
L
\pmb{T}^L_{k+1}
Tk+1L,包含扫描期间激光雷达在
[
t
k
+
1
,
t
k
+
2
]
[t_{k+1},t_{k+2}]
[tk+1,tk+2] 之间的运动。建图算法在世界坐标系
{
W
}
\{W\}
{W} 中匹配并注册
P
‾
k
+
1
\overline{P}_{k+1}
Pk+1 ,如图8所示。为了解释这个过程,我们定义
Q
k
\mathcal{Q}_{k}
Qk 为累积到扫描
k
k
k 时地图上的点云,设
T
k
W
\pmb{T}^W_k
TkW 为扫描
k
k
k 结束时刻
t
k
+
1
t_{k+1}
tk+1 时 lidar 在地图上的姿态。根据 lidar 里程计的输出,建图算法将
T
k
W
\pmb{T}^W_k
TkW 从
t
k
+
1
t_{k+1}
tk+1 扩展到
t
k
+
2
t_{k+2}
tk+2,以获得
T
k
+
1
W
\pmb{T}^W_{k+1}
Tk+1W ,并将
P
‾
k
+
1
\overline{\mathcal{P}}_{k+1}
Pk+1 投影到世界坐标
{
W
}
\{W\}
{W},表示为
Q
k
+
1
\mathcal{Q}_{k+1}
Qk+1 。接下来,算法通过优化 lidar 位姿
T
k
+
1
W
\pmb{T}^W_{k+1}
Tk+1W 来匹配
Q
k
+
1
\mathcal{Q}_{k+1}
Qk+1 与
Q
k
\mathcal{Q}_{k}
Qk 。
图8. 建图过程说明...
特征点提取方法与第 5.1 节相同,但使用了10倍的特征点。为了找到特征点的对应关系,我们将点云存储在地图
Q
k
\mathcal{Q}_{k}
Qk 上,在10m立方体区域内。立方体中与
Q
‾
k
+
1
\mathcal{\overline{Q}}_{k+1}
Qk+1 相交的点被提取并存储在3D KD树[24]中。我们在特征点周围的一定区域内找到
Q
k
\mathcal{Q}_{k}
Qk 中的点。设
S
′
\mathcal{S}'
S′ 是周围点的集合。对于一个边缘点,我们只在
S
′
\mathcal{S}'
S′ 的边线上保留点,而对于一个平面点,我们只在平面块上保留点。然后,我们计算
S
′
\mathcal{S}'
S′ 的协方差矩阵,记为
M
\pmb{M}
M ,
M
\pmb{M}
M 的特征值和特征向量,分别记为
V
\pmb{V}
V 和
E
\pmb{E}
E。如果
S
′
\mathcal{S}'
S′ 分布在边缘线上,
V
\pmb{V}
V 包含一个显著大于另外两个的特征值,
E
\pmb{E}
E 中与最大特征值相关联的特征向量表示边线的方向。另一方面,如果
S
′
\mathcal{S}'
S′ 分布在平面块上,
V
\pmb{V}
V 包含两个较大的特征值,第三个特征值明显较小,
E
\pmb{E}
E 中与最小特征值相关的特征向量表示平面块的方向。边缘线或平面块的位置是通过
S
′
\mathcal{S}'
S′ 的几何中心来确定的。
为了计算从特征点到其对应点的距离,我们在边缘线上选择两点,在平面块上选择三个点。这允许使用与式(2)和(3)相同的公式来计算距离。然后,为每个特征点推导出一个方程(9)或(10),但不同之处在于
Q
‾
k
+
1
\mathcal{\overline{Q}}_{k+1}
Qk+1 中的所有点共享相同的时间戳
t
k
+
2
t_{k+2}
tk+2 。通过Levenberg-Marquardt方法[26]进行鲁棒拟合[27]再次求解非线性优化,并在地图上注册
Q
‾
k
+
1
\mathcal{\overline{Q}}_{k+1}
Qk+1 。为了使得点分布均匀,地图云通过体素网格过滤器[28]缩小为5cm大小的体素立方体。
位姿变换的集成过程如图9所示。蓝色区域表示 lidar 建图的位姿输出
T
k
W
\pmb{T}_k^W
TkW ,每次扫描生成一次。橙色区域表示 lidar 里程计的位姿变换输出
T
k
+
1
L
\pmb{T}_{k+1}^L
Tk+1L ,频率约为10Hz。lidar 相对于地图的位姿是两个变换的组合,与 Lidar 里程计频率相同。