NeuroSLAM 论文解析
本文对NeuroSLAM论文进行翻译和总结,之后可能会有关于代码的补充。
论文链接:NeuroSLAM: a brain-inspired SLAM system for 3D environments
摘要:
- 本文基于3D网格单元和多层头部方向单元的计算模型,提出了一种新型的受神经启发的4DoF(自由度)SLAM系统,名为NeuroSLAM,并与提供外部视觉提示和自我运动提示的视觉系统集成在一起;
- 采用多层经验图松弛算法对闭环后的路径积分累积误差进行修正;
- 使用合成和真实世界的数据集,包括复杂的、多层的室内室外,证明了NeuroSLAM生成正确的拓扑地图。
1 introduction
本文的主要贡献:
- 提出了一个新的神经启发模型,用于在大型的、真实的三维环境中进行建图和定位;
- 建立了一个由三维网格细胞和多层头朝向细胞组成的联合位姿细胞的计算模型,用于表示4DoF位姿(x, y, z, yaw);
- 提出了结合局部视觉细胞、三维栅格细胞、多层头朝向细胞以及三维视觉里程计的多层graphical experience map;
- 提出使用三种3D mapping 真实世界和合成数据集,包括室内外场景,评估了NeuroSLAM的性能。
2 三维环境中的SLAM算法
三维SLAM系统使机器人能从任意的初始三维位置探索位置的三维环境。解决3D SLAM问题的方法大致可分为两类:
- 主要的方法通常是几何性质的,由优化或概率滤波器驱动,例如,图优化、粒子滤波器或扩展卡尔曼滤波器(EKF) ;
- SLAM的第二套方法是基于生物制图和定位系统的灵感。这些受生物学启发的方法也分为两类:
- 一种方法是基于蚂蚁、蜜蜂和昆虫的导航行为策略;
- 另一种方法是基于神经导航机制。
本文主要研究基于三维神经导航机制的导航方法。
2.1 传统的三维视觉SLAM
- 传统的视觉SLAM和三位视觉里程计的方法有:ORBSLAM、PTAM、LSD-SLAM和DSO等。
- 一些基于生物模拟事件相机的方法被提出,有EVO和基于事件相机的SLAM;
- 关注SLAM回环问题的方法有:FrameSLAM,FAB-MAP,SeqSLAM和CA T-SLAM等;
2.2 基于脑启发的SLAM
- 基础版本:大规模持久导航的仿生SLAM模型,RatSLAM ;
- 扩展方法:
- 使用声纳传感模态的BatSLAM(Steckel J, Peremans H (2013) BatSLAM);
- 基于RatSLAM解决2.5D环境下的slam问题(Feature-based visual odometry and featureless place recognition for SLAM in 2.5 d environments);
- 使用三维位置细胞对RatSLAM进行扩展,探索水下三维环境中的SLAM问题(Silveira L, Guth F, Duarte-Filho N, Botelho S (2015) An open-source bio-inspired solution to underwater SLAM)。
- 利用连续吸引子神经网络(CANN)、深度神经网络(DNN)和峰值神经网络(SNN)等不同类型的神经网络,在位置细胞(PC)、头部方向细胞(HDC)和网格细胞(GC)基础上发展了一些新的模型和方法,如下表格所示:
3 哺乳动物的三维空间表示
- 3D place cells :当哺乳动物经过某个3D空间位置时,该3D位置细胞会选择性放电,形成所有三维的度量地图;
- 3D head direction cells:三维头方向细胞响应方位角x俯仰的特定组合,从而表示三维空间中头矢量的方向;
- 3D grid cells:三维网格细胞将呈现规则的三维网格模式,用于三维路径集成所需的三维位置、方向和度量信息。
Jeffery et al.(2015)和Hayman et al.(2015) 提出了这些三维空间神经细胞的几种数学模型,并分析了它们在表示三维空间时的特性和约束条件。
本文将三维栅格细胞和头向细胞模型相结合,实现了4DoF的位姿。
3.1 三维栅格细胞
- 栅格细胞具有周期性的六边形放电场,与动物运动的方向和速度无关;
- 栅格细胞可以提供用于导航的度量空间表示;
- 栅格细胞可能会基于自身运动累积地进行路径整合;
Kim和Maguire(2019)提出使用六边形紧密堆积(HCP)和面心立方晶格(FCC)模型来组织三维网格单元。在本文中,我们使用3D网格细胞模型来表示3D路径积分的3D位置和度量信息。
3.2 头方向细胞
三维头部方向细胞可以在3D空间中以roll,yaw,pitch或其组合来表示动物的方向。本文中,我们只考虑方位角来表示4DoF位姿,并使用多层头部方向细胞模型来表示机器人方向。
3.3 多维度连续吸引子网络
多维连续吸引子网络(MD-CAN)是空间神经细胞建模的重要方法。主要有以下特性:
- MD-CAN是一种具有加权兴奋和抑制连接的神经网络;
- MD-CAN有很多循环连接,这些连接导致网络在没有外部输入的情况下,随时间推移收敛到某些稳定状态;
- MD-CAN通过更新神经活动进行操作;
- MD-CAN中的每个神经单元在0到1之间有一个连续的激活值,当机器人接近空间位置时,相关神经单元的激活值增大。
在本文,二维的MD-CAN和三维的MD-CAN分别用于表示多层头方向细胞模型和三维网格细胞模型。
4 NeuroSLAM框架
NeuroSLAM系统包括:联合位姿细胞模型、多层经验图以及视觉模块。
4.1 系统框架
NeuroSLAM系统的结构如下图所示。(补充材料中的算法1)
- 机器人在三维环境中的4DoF位姿(x, y,z,偏航)状态由三维网格单元网络和多层头部方向单元网络中的活动联合表示;
- 联合位姿细胞网络基于自运动线索进行路径积分,并基于视觉线索进行校正;
- 将局部视觉细胞、联合位姿细胞和三维视觉里程计相结合,实现多层图形化experience map的创建和优化。
联合位姿细胞包括三维栅格细胞和多层头朝向细胞。根据自运动线索可以更新联合位姿细胞的活性,当遇到熟悉场景时,通过局部视觉线索可以校准联合位姿细胞的活性。在MD-CAN中,通过将活性注入联合位姿细胞来实现路径积分。因此,当前的活动包会根据三维视觉里程计进行移位。当遇到熟悉视觉线索,局部视觉细胞被激活,通过联想学习将该活动从注入特定的联合位姿细胞中,从而实现路径积分。
多层经验图用于表示三维空间experience。一个特定的三维空间experience通过conjunctive code结合一系列的视觉细胞,多层头朝向细胞、三维栅格细胞以及自运动线索信息编码实现。我们通过局部视图细胞和联合位姿细胞的活动模式定义了一个独立的三维空间experience,当模式发生改变时,我们会创建一个新的三维空间experience,同时,会创建一个从旧的experience到新experience的新的transition,这个transition包含两个experience的4DoF位姿的变化。当机器人在一个新环境中移动时,新的三维experience和transition会不断形成。当机器人重新回到一个熟悉的场景,如果conjunctive code与之前存储的相同,就会产生回环。多层地图的松弛被用来保持拓扑的一致性。
4.2 联合位姿细胞模型
我们使用由三维栅格细胞和多层头部方向细胞组成的联合位姿细胞来表示一个4DoF位姿(x, y, z, yaw)。位置细胞的功能特征被编码在三维网格细胞网络和三维experience map中。三维位置(x, y, z)由三维栅格细胞使用3D MD-CAN表示;方向(yaw)由多层头朝向细胞使用2D MD-CAN。
4.2.1 三维栅格细胞模型
三维栅格单元网络是一个三维MD-CAN网络,模拟了哺乳动物大脑中的三维空间神经表征,如下图2所示。该网络具有规则的三维晶格模式,该模式表示三维路径集成的三维位置、方向和度量信息。它可以在没有外部感官信号输入的情况下保持机器人的4DoF姿态。
该模型可以基于自运动线索进行3D路径积分。局部视图细胞与特定的3D网格细胞相关联,当机器人看到熟悉的场景时,3D MD-CAN可以进行3D位置矫正。(补充材料中的算法2)
三维栅格细胞表示三维空间中的绝对位置(x, y, z),活性矩阵
P
g
c
P^{gc}
Pgc描述了三维栅格细胞的活性,通过以下三个过程更新活性:
- 首先,利用具有兴奋和抑制的吸引子动力学更新活性;
- 然后,三维视觉里程计提供的平移速度和旋转速度,通过三维路径积分对活动包进行移动;
- 最后,当机器人看到熟悉的场景时,通过局部视图校准更新活动。
(1) Attractor dynamics(吸引子动力学)
三维MD-CAN模型的内吸引子动力学过程包括三个阶段:
-
首先,对部分三维栅格细胞进行局部激发;
使用三维高斯分布创建了兴奋权重矩阵 ε u , v , w g c \varepsilon^{gc}_{u,v,w} εu,v,wgc ,其中 u u u、 v v v、 w w w分别表示不同方向的距离指数, δ x δ_x δx、 δ y δ_y δy、 δ z δ_z δz为三维空间分布的方差常数。
ε u , v , w g c = 1 δ x ( 2 π ) e − u 2 / ( 2 δ x 2 ) ⋅ 1 δ y ( 2 π ) e − v 2 / ( 2 δ y 2 ) ⋅ 1 δ z ( 2 π ) e − w 2 / ( 2 δ z 2 ) \varepsilon^{gc}_{u,v,w} = \frac{1}{δ_x\sqrt(2π)}e^{-u^2/(2δ_x^2)}\cdot\frac{1}{δ_y\sqrt(2π)}e^{-v^2/(2δ_y^2)}\cdot\frac{1}{δ_z\sqrt(2π)}e^{-w^2/(2δ_z^2)} εu,v,wgc=δx(2π)1e−u2/(2δx2)⋅δy(2π)1e−v2/(2δy2)⋅δz(2π)1e−w2/(2δz2)一个三维栅格细胞的活性变化表示为:
Δ P x , y , z g c = ∑ i n x ∑ j n y ∑ k n z P i , j , k g c ε u , v , w g c \Delta P^{gc}_{x,y,z} = \sum_i^{n_x} \sum_j^{n_y} \sum_k^{n_z}P^{gc}_{i,j,k}\varepsilon^{gc}_{u,v,w} ΔPx,y,zgc=i∑nxj∑nyk∑nzPi,j,kgcεu,v,wgc
n x n_x nx, n y n_y ny, n z n_z nz分别表示矩阵的三个维度,距离指数通过以下公式计算:
u = ( x − i ) ( m o d n x ) v = ( y − j ) ( m o d n y ) w = ( z − k ) ( m o d n z ) u = (x-i)(mod n_x) \\ v = (y-j)(mod n_y)\\ w = (z-k)(mod n_z) u=(x−i)(modnx)v=(y−j)(modny)w=(z−k)(modnz) -
然后,对所有三维栅格细胞进行全局抑制;
每个三维栅格细胞通过局部抑制过程抑制邻近细胞,我们创建了一个抑制矩阵 ψ x , y , z g c ψ^{gc}_{x,y,z} ψx,y,zgc来更新局部抑制过程中的活性,然后通过全局抑制 φ \varphi φ平均更新所有三维栅格细胞的活性。全局抑制和局部抑制的过程可通过以下公式计算,其中 P g c P^{gc} Pgc为非负值:
Δ P x , y , z g c = ∑ i n x ∑ j n y ∑ k n z P i , j , k g c ψ u , v , w g c − φ \Delta P^{gc}_{x,y,z} = \sum_i^{n_x} \sum_j^{n_y} \sum_k^{n_z}P^{gc}_{i,j,k}ψ^{gc}_{u,v,w} - \varphi ΔPx,y,zgc=i∑nxj∑nyk∑nzPi,j,kgcψu,v,wgc−φ -
最后,对三维栅格细胞的活动进行归一化。
三维栅格细胞的总活性通过活性归一化保持为1,归一化后的活性 P x , y , z g c ′ P^{gc'}_{x,y,z} Px,y,zgc′表示为:
P x , y , z g c ′ = P x , y , z g c ∑ i n x ∑ j n y ∑ k n z P i , j , k g c P^{gc'}_{x,y,z} = \frac{P^{gc}_{x,y,z}}{\sum_i^{n_x} \sum_j^{n_y} \sum_k^{n_z}P^{gc}_{i,j,k}} Px,y,zgc′=∑inx∑jny∑knzPi,j,kgcPx,y,zgc
(2) Path integration(路径积分)
路径积分将三维栅格细胞活动映射到邻近的细胞中。当yaw角为θ时,根据x、y、z轴的平动速度和高度变化速度,活动分别在x、y平面和z维度上进行移动,活动变化
Δ
U
l
m
n
g
c
ΔU^{gc}_{lmn}
ΔUlmngc可以被计算:
Δ
U
l
m
n
g
c
=
∑
x
=
δ
x
0
δ
x
0
+
1
∑
y
=
δ
y
0
δ
y
0
+
1
∑
z
=
δ
z
0
δ
z
0
+
1
γ
U
(
l
+
x
)
(
m
+
y
)
(
n
+
z
)
g
c
ΔU^{gc}_{lmn} = \sum_{x=\delta_{x_0}}^{\delta_{x_0}+1}\sum_{y=\delta_{y_0}}^{\delta_{y_0}+1}\sum_{z=\delta_{z_0}}^{\delta_{z_0}+1}\gamma U^{gc}_{(l+x)(m+y)(n+z)}
ΔUlmngc=x=δx0∑δx0+1y=δy0∑δy0+1z=δz0∑δz0+1γU(l+x)(m+y)(n+z)gc
注入的活动量通过两个输入决定,一个是sending unit
U
g
c
U^{gc}
Ugc,另一个是残差
γ
\gamma
γ。残差通过偏移量的小数部分
δ
x
f
,
δ
y
f
,
δ
z
f
\delta_{x_f},\delta_{y_f},\delta_{z_f}
δxf,δyf,δzf计算得到:
[
δ
x
0
δ
y
0
δ
z
0
]
=
[
⌊
k
x
v
c
o
s
θ
⌋
⌊
k
y
v
s
i
n
θ
⌋
⌊
k
z
v
h
⌋
]
,
[
δ
x
f
δ
y
f
δ
z
f
]
=
[
⌊
k
x
v
c
o
s
θ
−
δ
x
0
⌋
⌊
k
y
v
s
i
n
θ
−
δ
y
0
⌋
⌊
k
z
v
h
−
δ
z
0
⌋
]
\begin{bmatrix} \delta_{x_0} \\ \delta_{y_0} \\ \delta_{z_0} \end{bmatrix} = \begin{bmatrix} \lfloor k_xvcos\theta \rfloor \\ \lfloor k_yvsin\theta \rfloor \\ \lfloor k_zv_h \rfloor \\ \end{bmatrix},\\ \\ \begin{bmatrix} \delta_{x_f} \\ \delta_{y_f} \\ \delta_{z_f} \end{bmatrix} = \begin{bmatrix} \lfloor k_xvcos\theta - \delta x_0 \rfloor \\ \lfloor k_yvsin\theta- \delta y_0 \rfloor \\ \lfloor k_zv_h - \delta z_0\rfloor \\ \end{bmatrix}
⎣⎡δx0δy0δz0⎦⎤=⎣⎡⌊kxvcosθ⌋⌊kyvsinθ⌋⌊kzvh⌋⎦⎤,⎣⎡δxfδyfδzf⎦⎤=⎣⎡⌊kxvcosθ−δx0⌋⌊kyvsinθ−δy0⌋⌊kzvh−δz0⌋⎦⎤
其中,
k
x
,
k
y
,
k
z
k_x,k_y,k_z
kx,ky,kz是三维路径积分的常数,残差通过以下公式可以被计算:
γ
=
f
(
δ
x
f
,
x
−
δ
x
0
)
f
(
δ
y
f
,
y
−
δ
y
0
)
f
(
δ
z
f
,
x
−
δ
z
0
)
f
(
a
,
b
)
=
{
a
,
i
f
b
=
1
1
−
a
,
i
f
b
=
0
\gamma =f(\delta_{x_f}, x-\delta_{x_0}) f(\delta_{y_f}, y-\delta_{y_0}) f(\delta_{z_f}, x-\delta_{z_0}) \\ f(a,b) =\begin{cases} a, & if\ b=1 \\ 1-a, & if \ b=0 \end{cases}
γ=f(δxf,x−δx0)f(δyf,y−δy0)f(δzf,x−δz0)f(a,b)={a,1−a,if b=1if b=0
(3) Local view calibration(局部视图校正)
局部视图校正根据三维视觉里程计提供的平移速度和旋转速度对路径积分的累计误差进行reset。将局部视图细胞与三维栅格细胞和多层头朝向细胞相关联,当机器人看到熟悉的场景,之前的关联会被recalled。我们使用向量 V V V表示局部视图细胞的活性,当当前视图和之前出现的视图相似,相关联的局部视图细胞被激活。
连接矩阵
Ψ
Ψ
Ψ存储了局部视图细胞向量、三维栅格细胞矩阵以及多层头朝向细胞矩阵的学习连接,我们使用改进版的Hebb法则用于学习连接:
Ψ
i
,
x
,
y
,
z
,
θ
t
+
1
=
m
a
x
(
τ
V
i
P
x
,
y
,
z
,
θ
,
Ψ
i
,
x
,
y
,
z
,
θ
t
)
Ψ^{t+1}_{i,x,y,z,\theta} = max(\tau V_iP_{x,y,z,\theta},Ψ^{t}_{i,x,y,z,\theta})
Ψi,x,y,z,θt+1=max(τViPx,y,z,θ,Ψi,x,y,z,θt)
其中,
τ
\tau
τ表示学习率,三维栅格细胞和多层头朝向细胞的活性可通过以下公式计算:
Δ
P
x
,
y
,
z
,
θ
=
δ
n
a
c
t
∑
i
V
i
Ψ
i
,
x
,
y
,
z
,
θ
\Delta P_{x,y,z,\theta} = \frac{\delta}{n_{act}}\sum_i V_iΨ_{i,x,y,z,\theta}
ΔPx,y,z,θ=nactδi∑ViΨi,x,y,z,θ
其中,常数
δ
\delta
δ控制局部视图校准的强度,
n
a
c
t
n_{act}
nact表示活动的局部视图细胞的数量。
4.2.2 多层头朝向细胞模型
多层头朝向相信细胞网络采用MD-CAN,模拟哺乳动物三维方向认知的神经机制,如下图示。MD-CAN可以在三维空间中保持机器人的头朝向,每个神经元负责在特定的垂直区域绘制特定的方向(偏航角)。为了有效的表示4DoF位姿,我们采用多层头朝向细胞模型表示三维垂直空间中的方位角。(补充材料中的算法3)
细胞活性矩阵
P
h
d
c
P^{hdc}
Phdc用来描述多层头朝向细胞的活性。该活性通过以下三个过程来更新:(更新过程与栅格细胞类似)
- 首先,MD-CAN的活性通过吸引子动力学来更新;
- 其次,MD-CAN基于方向变化和高度变化进行头朝向的更新,该变化是根据旋转速度、高度变化速度和平移速度进行路径积分后三维栅格细胞网络得到的;
- 最后,当机器人看到一个熟悉的视图时,MD-CAN中的活性由局部视图校准过程更新。
(1) Attractor dynamics(吸引子动力学)
MD-CAN模型的内吸引子动力学过程包括三个阶段:
- 首先,对多层头朝向细胞的部分区域进行局部激发;
使用一个二维高斯分布创建兴奋权重矩阵 ε u , v h d c \varepsilon^{hdc}_{u,v} εu,vhdc,分别使用 u u u和 v v v表示 ( θ , h ) (θ,h) (θ,h)单位间的距离。权重可以表示为:
ε u , v h d c = 1 δ θ ( 2 π ) e − u 2 / ( 2 δ θ 2 ) ⋅ 1 δ h ( 2 π ) e − v 2 / ( 2 δ h 2 ) \varepsilon^{hdc}_{u,v} = \frac{1}{δ_\theta\sqrt(2π)}e^{-u^2/(2δ_\theta^2)}\cdot\frac{1}{δ_h\sqrt(2π)}e^{-v^2/(2δ_h^2)} εu,vhdc=δθ(2π)1e−u2/(2δθ2)⋅δh(2π)1e−v2/(2δh2)
其中, δ θ δ_\theta δθ和 δ h δ_h δh表示方向和高度分别的方差常数。一个多层头朝向细胞的活性变化表示为:
Δ P θ , h h d c = ∑ i n θ ∑ j n h P i , j h d c ε u , v h d c \Delta P^{hdc}_{\theta,h} = \sum_i^{n_\theta} \sum_j^{n_h} P^{hdc}_{i,j} \varepsilon^{hdc}_{u,v} ΔPθ,hhdc=i∑nθj∑nhPi,jhdcεu,vhdc
n θ n_\theta nθ, n h n_h nh分别表示多层头朝向细胞的矩阵维度,距离指数通过以下公式计算:
u = ( θ − i ) ( m o d n θ ) v = ( h − j ) ( m o d n h ) u = (\theta-i)(mod n_\theta) \\ v = (h-j)(mod n_h) u=(θ−i)(modnθ)v=(h−j)(modnh) - 然后,对所有多层头朝向细胞进行全局抑制;
每个多层头向细胞通过局部抑制过程对邻近细胞进行抑制。然后,所有多层头向单元均被全局抑制常数 φ \varphi φ抑制,局部抑制和全局抑制的过程表示为:
Δ P θ , h h d c = ∑ i n θ ∑ j n h P i , j h d c ψ u , v h d c − φ \Delta P^{hdc}_{\theta,h} = \sum_i^{n_\theta} \sum_j^{n_h} P^{hdc}_{i,j}ψ^{hdc}_{u,v} - \varphi ΔPθ,hhdc=i∑nθj∑nhPi,jhdcψu,vhdc−φ - 最后,对多层头朝向细胞的活动进行归一化。
通过活性归一化操作,多层头部方向细胞的总活性保持为1,归一化后的活性 P θ , h h d c ′ P^{hdc'}_{\theta,h} Pθ,hhdc′表示为:
P θ , h h d c ′ = P h d c ∑ i = 1 n θ ∑ j = 1 n h P i , j h d c P^{hdc'}_{\theta,h} = \frac{P^{hdc}}{\sum_{i=1}^{n_\theta} \sum_{j=1}^{n_h} P^{hdc}_{i,j}} Pθ,hhdc′=∑i=1nθ∑j=1nhPi,jhdcPhdc
(2) 头朝向更新
通过将当前激活的细胞的活性投射到附近的细胞中改变方向和高度,从而使得多层头朝向细胞的MD-CAN进行头朝向的更新。根据旋转方向变化
ω
θ
ω_θ
ωθ和高度变化
v
h
v_h
vh分别对偏航和高度矩阵的活性进行移位。注入的活性有两个输入,一个是sending unit
U
h
d
c
U^{hdc}
Uhdc,另一个是残差
η
\eta
η,活性的变化通过以下公式计算:
Δ
U
l
m
h
d
c
=
∑
θ
=
δ
θ
0
δ
θ
0
+
1
∑
h
=
δ
h
0
δ
h
0
+
1
η
U
(
l
+
θ
)
(
m
+
h
)
h
d
c
[
δ
θ
0
δ
h
0
]
=
[
⌊
k
θ
w
θ
⌋
⌊
k
h
v
h
⌋
]
,
[
δ
θ
f
δ
h
f
]
=
[
⌊
k
θ
w
θ
−
δ
θ
0
⌋
⌊
k
h
v
h
−
δ
h
0
⌋
]
ΔU^{hdc}_{lm} = \sum_{\theta=\delta_{\theta_0}}^{\delta_{\theta_0}+1}\sum_{h=\delta_{h_0}}^{\delta_{h_0}+1}\eta U^{hdc}_{(l+\theta)(m+h)} \\ \begin{bmatrix} \delta_{\theta_0} \\ \delta_{h_0} \end{bmatrix} = \begin{bmatrix} \lfloor k_\theta w_\theta \rfloor \\ \lfloor k_hv_h \rfloor \end{bmatrix},\\ \\ \begin{bmatrix} \delta_{\theta_f} \\ \delta_{h_f} \end{bmatrix} = \begin{bmatrix} \lfloor k_\theta w_\theta - \delta \theta_0 \rfloor \\ \lfloor k_hv_h- \delta h_0 \rfloor \end{bmatrix}
ΔUlmhdc=θ=δθ0∑δθ0+1h=δh0∑δh0+1ηU(l+θ)(m+h)hdc[δθ0δh0]=[⌊kθwθ⌋⌊khvh⌋],[δθfδhf]=[⌊kθwθ−δθ0⌋⌊khvh−δh0⌋]
其中,
k
θ
,
k
h
k_\theta,k_h
kθ,kh是头朝向的常数,残差
η
\eta
η通过以下公式可以被计算:
η
=
f
(
δ
θ
f
,
θ
−
δ
θ
0
)
f
(
δ
h
f
,
h
−
δ
h
0
)
f
(
a
,
b
)
=
{
a
,
i
f
b
=
1
1
−
a
,
i
f
b
=
0
\eta =f(\delta_{\theta_f}, \theta-\delta_{\theta_0}) f(\delta_{h_f}, h-\delta_{h_0}) \\ f(a,b) =\begin{cases} a, & if\ b=1 \\ 1-a, & if \ b=0 \end{cases}
η=f(δθf,θ−δθ0)f(δhf,h−δh0)f(a,b)={a,1−a,if b=1if b=0
4.3 多层经验图
多层经验图包含许多三维空间experiences
E
i
E_i
Ei,相关的experiences通过transitions
T
T
T进行连接。experience的创建是通过局部视图细胞、三维栅格细胞以及多层头朝向细胞的活性驱动的。我们通过与其相关联的局部视图细胞code
V
i
l
v
V^{lv}_i
Vilv、三维栅格细胞code
P
i
g
c
P^{gc}_i
Pigc以及多层头朝向细胞code
P
i
h
d
c
P^{hdc}_i
Pihdc的 conjunctive code定义了experience
E
i
E_i
Ei。经验图的位姿
P
i
e
x
p
P^{exp}_i
Piexp是通过视觉里程计估计得到的。我们定义一个三维空间experience
E
i
E_i
Ei为一个turtle。
E
i
=
{
V
i
l
v
,
P
i
g
c
,
P
i
h
d
c
,
P
i
e
x
p
}
E_i = \{V^{lv}_i, P^{gc}_i, P^{hdc}_i,P^{exp}_i\}
Ei={Vilv,Pigc,Pihdc,Piexp}
多层经验图的联合坐标空间如下图所示。
(
x
,
y
,
z
,
θ
)
(x, y, z, θ)
(x,y,z,θ)表示在特定experience上三维栅格细胞的三维location和多层头朝向细胞的方向,并且每一个局部视图都与特性的experience相关,每一个空间experience都有一个三维空间状态
(
x
′
,
y
′
,
z
′
,
θ
′
)
(x', y', z', θ')
(x′,y′,z′,θ′)。上图中的联合坐标系描述的是experience在三维空间中的位置和方向。我们设置初始experience的4DoF的位姿为(0, 0, 0, 0),通过旋转和平移速度可以估计experience的位姿。(补充材料中的算法4)
4.3.1 多层经验图的创建
我们使用一个分数度量
S
i
S_i
Si将由局部视图code
V
i
l
v
V^{lv}_i
Vilv、三维栅格细胞code
P
i
g
c
P^{gc}_i
Pigc以及多层头朝向细胞code
P
i
h
d
c
P^{hdc}_i
Pihdc组成的当前experience union codes与所有已存在的experience union codes进行比较,其中
i
i
i表示当前experience的索引。
S
i
=
μ
l
v
∣
V
i
l
v
−
V
l
v
∣
+
μ
g
c
∣
P
i
g
c
−
P
g
c
∣
+
μ
h
d
c
∣
P
i
h
d
c
−
P
h
d
c
∣
S_i = \mu^{lv}|V^{lv}_i - V^{lv}| + \mu^{gc}|P^{gc}_i - P^{gc}| + \mu^{hdc}|P^{hdc}_i - P^{hdc}|
Si=μlv∣Vilv−Vlv∣+μgc∣Pigc−Pgc∣+μhdc∣Pihdc−Phdc∣
其中,
μ
l
v
、
μ
g
c
、
μ
h
d
c
μ^{lv}、μ^{gc}、μ^{hdc}
μlv、μgc、μhdc分别表示局部视图细胞、三维栅格细胞和多层头部方向细胞的权重。
如果 S S S超过已有三维空间experience的最大值 S m a x S_{max} Smax,我们就会创建一个新的三维空间experience和transition,如果所有匹配的分数小于 S S S,我们选取分数最低的三维空间experience作为当前活动的三维空间experience。在三维空间中,活动的三维空间experience在三维空间中表示当前机器人的4DoF位姿。
transition link存储了4DoF的变化和一个experience与另一个experience的距离,经验图的校正和relaxation是通过该信息进行的。经验
E
i
E_i
Ei到经验
E
j
E_j
Ej的过渡(transition)如下图所示。
从该图中看,transition
T
i
j
T_{ij}
Tij存储了4DoF位姿的变化
δ
P
i
j
e
x
p
\delta_{P^{exp}_{ij}}
δPijexp和距离
δ
d
i
j
\delta_{d_{ij}}
δdij,公式表示为:
T
i
j
=
{
δ
P
i
j
e
x
p
,
δ
d
i
j
}
δ
P
i
j
e
x
p
=
P
j
e
x
p
−
P
i
e
x
p
=
[
x
j
y
j
z
j
θ
j
]
−
[
x
i
y
i
z
i
θ
i
]
=
[
δ
x
i
j
δ
y
i
j
δ
z
i
j
δ
θ
i
j
]
T_{ij} = \{\delta_{P^{exp}_{ij}}, \delta_{d_{ij}} \} \\ \delta_{P^{exp}_{ij}} = P^{exp}_{j} - P^{exp}_{i} = \begin{bmatrix} x_j \\ y_j\\ z_j \\ \theta_j \end{bmatrix} - \begin{bmatrix} x_i \\ y_i\\ z_i \\ \theta_i \end{bmatrix} = \begin{bmatrix} \delta_{x_{ij}} \\ \delta_{y_{ij}} \\ \delta_{z_{ij}} \\ \delta_{\theta_{ij}} \end{bmatrix}
Tij={δPijexp,δdij}δPijexp=Pjexp−Piexp=⎣⎢⎢⎡xjyjzjθj⎦⎥⎥⎤−⎣⎢⎢⎡xiyiziθi⎦⎥⎥⎤=⎣⎢⎢⎡δxijδyijδzijδθij⎦⎥⎥⎤
其中,新的experience
E
j
E_j
Ej的4DoF位姿
P
j
e
x
p
P^{exp}_j
Pjexp可以根据先前的4DoF位姿
P
i
e
x
p
P^{exp}_i
Piexp和运动计算得到。
E
j
=
{
V
j
l
v
,
P
j
g
c
,
P
j
h
d
c
,
P
i
e
x
p
+
δ
P
i
j
e
x
p
}
E_j = \{V^{lv}_j, P^{gc}_j, P^{hdc}_j,P^{exp}_i+ \delta_{P^{exp}_{ij}} \}
Ej={Vjlv,Pjgc,Pjhdc,Piexp+δPijexp}
4.3.2 多层经验图的优化
当机器人看到一个熟悉的场景,我们将视觉活性注入与该场景相关的局部视觉细胞、三维栅格细胞和多层头朝向细胞中,使得机器人的4DoF位姿被重定位。因此机器人4DoF的位姿由新的experience变成已存在的experience,同时新的(transition link)连接被学习。但是,由于三维视觉里程计的累计误差,新的transition和旧的transition的相对4DoF位姿的变化是不同的。
为了减少相对4DoF位姿估计的误差,根据旧连接(old transition)到新连接(new transition)的位姿变化信息,采用具有适当校正率
ψ
ψ
ψ的多层经验图校正和优化的迭代方法。4DoF位姿的变化
δ
P
i
δ_{P_i}
δPi可以计算:
δ
P
i
=
ψ
[
∑
k
=
1
N
t
(
P
k
−
P
i
−
δ
k
i
o
l
d
)
+
∑
j
=
1
N
f
(
P
j
−
P
i
−
δ
i
j
o
l
d
)
]
δ_{P_i} = ψ\begin{bmatrix} \sum_{k=1}^{N_t}(P_k-P_i-δ_{ki}^{old}) + \sum_{j=1}^{N_f}(P_j-P_i-δ_{ij}^{old}) \end{bmatrix}
δPi=ψ[∑k=1Nt(Pk−Pi−δkiold)+∑j=1Nf(Pj−Pi−δijold)]
其中,
N
f
,
N
t
N_f,N_t
Nf,Nt表示
E
i
E_i
Ei到其他三维空间experiences和从其他三维空间experiences到
E
i
E_i
Ei的连接的数量。
4.4 视觉模块
局部视觉线索和自运动线索都是通过视觉模块提供的,如下图所示:
三维视觉里程计使用aidvo算法,该算法基于对比一组连续图像的平均强度差。我们还实验了一些限制比较少的视觉历程计方法,比如VINS-mono等。
4.4.1 图像获取
将图像的分辨率降低,裁剪成四个区域,分别用于估计偏航旋转速度、平移速度、高度变化速度和视觉模板匹配。区域的选择规则参考RatSLAM算法。为了提高视觉模板匹配的鲁棒性,采用了patch归一化的方法。因此,像素的强度可计算:
I
x
y
′
=
I
x
y
−
μ
x
y
δ
x
y
I'_{xy} = \frac{I_{xy}-\mu_{xy}}{\delta_{xy}}
Ixy′=δxyIxy−μxy
其中,
μ
x
y
\mu_{xy}
μxy表示均值,
δ
x
y
\delta_{xy}
δxy表示标准差。
视觉处理方法采用 scanline intensity profile,是由图像组成的一维向量(就是将图像按列或者按行相加,组成一维向量)。使用这个profile可以计算三维旋转和平移速度以及视觉模板匹配。
(1) 估计偏航旋转速度
偏航旋转速度可以通过一系列的连续图像计算得到,如下图示:
- 将前后两帧图像转换成一维列向量形式,且 I i + 1 I^{i+1} Ii+1与 I i I^{i} Ii相差 s h s^h sh个平移维度;
- 计算前后两帧图像的平均强度差:
d ( I i , I i + 1 , s h ) = 1 w − ∣ s h ∣ ( ∑ n = 1 w − ∣ s h ∣ ∣ I n + m a x ( s h , 0 ) i + 1 − I n − m i n ( s h , 0 ) i ∣ ) d(I^i, I^{i+1}, s^h) = \frac{1}{w-|s^h|}(\sum_{n=1}^{w-|s^h|}|I^{i+1}_{n+max(s^h,0)} - I^{i}_{n-min(s^h,0)}|) d(Ii,Ii+1,sh)=w−∣sh∣1(n=1∑w−∣sh∣∣In+max(sh,0)i+1−In−min(sh,0)i∣)
其中 s h s^h sh表示profile在列维度的平移量, w w w表示图像宽度。 - 设置不同的平移量
s
h
s^h
sh,计算前后两帧最小的平均强度差对应的平移量:
s m h = a r g m i n s ∈ [ ρ h − w , w − ρ h ] d ( I i , I i + 1 , s h ) s_m^h = arg min_{s∈[\rho^h-w, w-\rho^h]}d(I^i, I^{i+1}, s^h) smh=argmins∈[ρh−w,w−ρh]d(Ii,Ii+1,sh) - 旋转速度
Δ
θ
\Delta \theta
Δθ可以通过
s
m
h
s_m^h
smh乘以常数
δ
h
\delta^h
δh计算得到,
δ
h
\delta^h
δh是经验值。
Δ θ = δ h s m h \Delta \theta = \delta^h s_m^h Δθ=δhsmh
(2) 估计平移速度
平移速度的计算与旋转速度类似,平移速度是通过最小平均强度差乘以经验值
μ
μ
μ得到:
v
=
m
i
n
[
μ
d
(
I
i
,
I
i
+
1
,
s
h
)
,
v
m
a
x
]
v = min[μd(I^i, I^{i+1}, s^h), v_{max}]
v=min[μd(Ii,Ii+1,sh),vmax]
其中
μ
μ
μ表示转换到物理空间的尺度,设置
v
m
a
x
v_{max}
vmax用于过滤速度异常值。
(3) 估计高度变化速度
基于一系列连续图像,通过比较图像的行维度的profile difference,估计出图像的高度变化速度。如下图示:
- 为了减少偏航旋转的影响,将最适合估计偏航角计算的偏移量 s m h s^h_m smh裁剪掉,如上图红色虚线框;
- 前后两帧图像的强度差值通过以下公式计算,其中
I
I
I指的是将图像组成一行向量;
d ( I i , I i + 1 , s m h , s v ) = 1 h − ∣ s v ∣ ( ∑ m = 1 h − ∣ s v ∣ ∣ I m + m a x ( s v , 0 ) i + 1 − I m − m i n ( s v , 0 ) i ∣ ) I = ∑ j = 1 w − ∣ s m h ∣ I j ′ d(I^i, I^{i+1}, s^h_m, s^v) = \frac{1}{h-|s^v|}(\sum_{m=1}^{h-|s^v|}|I^{i+1}_{m+max(s^v,0)} - I^{i}_{m-min(s^v,0)}|) \\ I =\sum_{j=1}^{w-|s^h_m|} I'_j d(Ii,Ii+1,smh,sv)=h−∣sv∣1(m=1∑h−∣sv∣∣Im+max(sv,0)i+1−Im−min(sv,0)i∣)I=j=1∑w−∣smh∣Ij′
其中, s m h , s v s^h_m, s^v smh,sv分别表示列维度和行维度的偏移量, h h h表示图像的高度, w w w表示图像的宽度。 - 计算一系列连续图像在最小偏移量上的强度差:
d m = m i n s m h ∈ [ ρ h − w , w − ρ h ] , s v ∈ [ ρ v − h , h − ρ v ] d ( I i , I i + 1 , s m h , s v ) v h = m i n [ μ d m , v m a x h ] d_m = min_{s^h_m∈[\rho^h-w, w-\rho^h], s^v∈[\rho^v-h, h-\rho^v]}d(I^i, I^{i+1}, s^h_m,s^v) \\ v_h = min[\mu d_m, v_{max}^h] dm=minsmh∈[ρh−w,w−ρh],sv∈[ρv−h,h−ρv]d(Ii,Ii+1,smh,sv)vh=min[μdm,vmaxh]
其中,常数 μ \mu μ为经验值,用来调整物理速度。
(4) 局部视觉细胞计算
使用 scanline intensity profile计算和存储可视化模板,如下图所示:
我们设置了一个阈值来控制视觉模板的生成,如果profile 差异小于阈值,则匹配到当前模板,否则,将创建一个新模板。
5 实验设置
我们使用四组数据集评估了NeuroSLAM的性能。如下表所示:
5.1 合成数据集的获取
合成的三维城市环境的数据如下图所示:
图中b和c显示了SynPerData数据集的相机轨迹;a显示了单目相机在平面和垂直空间中通过路径时,SynPerData数据集中的一些图像;d显示了全景相机在三维空间中移动时,SynPanData数据集的一些图像。
5.2 真实数据集的获取
真实的三维环境数据我们采用大学校园内由室内室外组成的两层停车场数据QUTCarParkData。
图中,c和d显示了拓扑图和三维轨迹图;a和e分别显示了一层和二层的图像;b和f是通过连接两层的斜坡时的图像。
5.3 实验参数设置
参考文章中的表格。
5.4 轨迹评估指标
为了定量评估我们方法的建图性能,我们通过计算均方根误差(RMSE),使用绝对轨迹误差(ATE)和相对误差(RE)作为评估指标。
在计算平移误差之前,我们需要将不同的轨迹与真值进行对齐并匹配,为了在估计轨迹和真值之间建立正确的位姿对应关系,我们采用关键位置匹配方法,如下图所示,我们按顺序在转弯处随机提取正定数量的关键位置,然后计算每组关键位置的均值,最后,基于每组关键点的均值计算ATE和RE。
此外,我们还计算了平移误差来评估每对关键位置的准确性。关键位置的ATE计算:
A
T
E
p
o
s
=
(
1
n
∑
i
=
1
n
(
Δ
p
i
)
2
)
1
/
2
ATE_{pos} = (\frac{1}{n} \sum_{i=1}^n (\Delta p_i)^2)^{1/2}
ATEpos=(n1i=1∑n(Δpi)2)1/2
其中,
Δ
p
i
\Delta p_i
Δpi表示三维距离的差值。
关键位置的RE计算:
R
E
p
o
s
=
(
1
m
−
k
+
1
∑
i
=
k
m
(
Δ
p
i
)
2
)
1
/
2
RE_{pos} = (\frac{1}{m-k+1} \sum_{i=k}^m (\Delta p_i)^2)^{1/2}
REpos=(m−k+11i=k∑m(Δpi)2)1/2
6 结果
我们介绍了整个三维建图的结果以及系统中每个模块的性能,包括多层经验图,局部视图细胞活性,active experience,三维栅格细胞活性、多层头朝向细胞活性以及视觉历程计。此外,我们将NeuroSLAM与先进的三维视觉SLAM系统ORB-SLAM和LDSO进行了比较。最后,我们将NeuroSLAM和VINS-MONO相结合显示建图效果。
6.1 NeuroSLAM结果
6.1.1 多层经验图
NeuroSLAM不生成严格意义上的三维笛卡尔空间地图,它创建了一个具有局部度量信息的拓扑上抑制的三维空间表示。为了分析经验图在全局坐标系下的拓扑一致性,我们根据地图的真值调整了经验图的比例,而不改变其拓扑。
(1) 基于SynPerData数据集的三维建图结果
图12将多层经验图、里程计地图以及真值进行对比,可以发现构建了正确且一致的拓扑地图。多层经验图的总体拓扑与真值保持一致。由于里程计的漂移误差,里程计的拓扑误差随着距离的增加而增大,如图(b)所示。
此外,我们通过计算ATE和RE提供了定量评估。首先,我们依次提供了19组转弯处的关键位置,每组有5个关键位置,随机进行五次实验;估计每组关键位置的均值,如下图所示,NeuroSLAM中大多数的关键位置靠近真值估计的关键位置。
图13显示了多层经验图、里程计轨迹和真值地图之间每组关键位置的平移误差。经验图的误差和里程计轨迹在某些关键位置相似,因为经验图的位姿是基于里程计估计得到的。由于里程计的累积误差,aidvo的平移误差随着距离的增大而增大。NeuroSLAM在15-20之间的关键位置更为精确,是因为回到熟悉场景会进行回环处理以及地图优化。
为了评估总体轨迹的准确性,我们分别评估了NeuroSLAM地图、aidvo地图以及真值地图之间每组关键位置的ATE,如下图(a)所示。每个框表示一组关键位置的RMSE,代表每条轨迹的平移误差。NeuroSLAM在大多数关键位置精度高于aidvo。
此外,我们评估了NeuroSLAM地图、aidvo地图以及真值地图之间每个子轨迹的RE,如下图(b)所示,NeuroSLAM在大部分关键位置表现优于aidvo。(箱线图显示了一组数据分散情况的统计图)
(2) 基于SynPanData数据集的三维建图结果
图15给出了SynPanData数据集的三维建图结果,多层经验图的拓扑结构与真值地图保持一致。底层拓扑误差小于上层拓扑误差是因为上层数据天空较多,里程计漂移误差较大,当回到熟悉场景,多层经验图的回环和地图优化效果较好。
图16表示每组关键位置的平移误差,与图13类似,不做详解。图17和图14类似,不做详解。
(3) 基于QUTCarparkData数据集的三维建图结果
图18给出了QUTCarparkData数据集的三维建图结果,从图中可以看出总体拓扑结构图和图11所示的平面图是一致的。由于缺少真值数据,我们没有进行定量评估,但可以看到在重新访问熟悉场景时,多层经验图使用回环和地图优化保持了一致的拓扑结构。
6.1.2 局部视图细胞活性
图19显示使用三组数据集生成的可视化模板,虚线表示回环期间的开始帧和结束帧。没有添加新模板的时间段表示机器人移动在熟悉的环境中。
6.1.3 Activate experiences
图20显示了当机器人移动到熟悉环境,激活的经验被学习或者回调。三组数据的地图中经验的数量接近视觉模板的数量。
6.1.4 三维栅格细胞活性
图21显示了活动包从初始位置向右移动,然后向左缠绕,(b)和©中显示了当相机在垂直空间移动时,各个垂直区域的细胞被激活。
图22显示了三组数据集在建图中最活跃的细胞的移动过程,由于机器人长时间在三维空间运动,两个平面的组多细胞被反复激活,当机器人在垂直空间移动时,不同高度的细胞被激活。
6.1.5 多层头朝向细胞活性
图23显示了机器人在一个特定的平面移动,高度相对稳定,激活的细胞在偏航维度上移动,如果累积角度大于360或者小鱼-360,具有缠绕连接的相对边界细胞被激活。
图24显示了最活跃的多层头朝向细胞的移动,当机器人在不同高度平面移动时,这些平面的细胞被激活,如图中水平线所示,当相机垂直移动时,不同高度的细胞被激活,如图中垂线所示。
6.2 Comparison with state-of-the-art 3D SLAM
为了评估NeuroSLAM系统,我们将算法与ORB-SLAM和LDSO算法使用SynPerData数据集进行比较。
图25显示了这几种方法生成的三维地图,这些地图的拓扑结构与真实地图保持一致。然而,LDSO生成的地图在向上或向下运动时误差较大,因此,LDSO的结果在一些子轨迹上存在漂移误差。从图中可以看出,NeuroSLAM可以构建类似于ORB-SLAM的高质量三维经验图。
此外,我们增加了定量分析,类似于图13和图16,在转弯处提取了19组关键位置,每组由5个关键位置组成,随机实验5次。计算每组关键位置的均值,如下图所示。
图26分别给出了估计地图和真实地图之间每组关键位置的平移误差。由于视觉里程计的累计误差,aidvo的平移误差随着距离增大而变大。
每个算法构建的地图和真实地图之间的总体轨迹和子轨迹的ATE和RE如图27所示。NeuroSLAM方法具有较高且稳定的集合精度,随着距离的增加,几种方法的RMSE都会增加,然而我们的方法具有更高的几何精度。
6.3 Demonstration of integration with visual inertial odometry
为了证明三维建图的性能,我们将Neuroslam算法和视觉惯性里程计(VINS-mono)结合在一起。我们使用maplab数据集的cla-floor-f数据来进行评估,这个数据是在多层建筑中采集的,包含单目图像数据和IMU传感器数据。
图28显示了多层经验图的拓扑结构和该数据的平面图一致,并且基于视觉惯性里程计的多层经验图比纯视觉里程计的多层经验图具有更高的几何精度。
此外,我们增加了几何精度的定量分析。在转弯处依次提取了八组关键位置,每组由五个关键位置组成,随机进行5次实验。结果表明,NeuroSLAM轨迹的关键位置与VINS-mono轨迹对应关键位置非常接近。
从图29和30中我们也可以看出基于VINS-mono,NeuroSLAM具有较高的几何精度。