赞
踩
论文:https://arxiv.org/abs/1904.06064
代码:https://github.com/mbrossar/ai-imu-dr
a. 本文主要提出了一种方法,能够仅仅使用IMU的航位推算得到较为精准的轨迹,如下图所示:
b. 该方法只适用于像汽车一样只能向前跑的系统,因为这样的系统在进行kalman滤波器的时候可以加入动力模型的辅助信息,在这里,辅助信息主要是指系统在侧向 y l a t y^{lat} ylat和与地面垂直的方向 y u p y^{up} yup上是没有移动的,文中称之为伪测量;
c. 论文的重要改进点在于,(b)中辅助信息的均值可以认为是0,但是协方差是否一直都是一个值呢?显然,在系统进行转弯的时候,水平方向的协方差要比直线方向的协方差是要大很多的,同理,在爬坡的时候,垂直地面方向的协方差也应该比直线的时候协方差是要大的;鉴于一般的滤波器都是人为设计协方差矩阵,同时设计好的协方差不能修改,因此文章希望使用CNN(并非RNN,作者说RNN不是很好)对观测的协方差进行动态的更新,进一步的,作者使用CNN也同时预测出来了上述的伪测量的值(这里没有给出伪量测的值)。
一般情况下,IMU的模型建立为真值+零偏+高斯白噪声,如下:
(1)
w
n
I
M
U
=
w
n
+
b
n
w
+
w
n
w
a
n
I
M
U
=
a
n
+
b
n
a
+
w
n
a
其中零偏一般是一个随机游走的“准常量”(quasi-constant)(一般来说与温度有关),因此对零偏进行建模,其模型如下:
(2)
b
n
+
1
w
=
b
n
w
+
w
n
b
w
b
n
+
1
a
=
b
n
a
+
w
n
b
a
其中
w
n
w
,
w
n
a
,
w
n
b
w
,
w
n
b
a
w_{n}^{w}, w_{n}^{a}, w_{n}^{b_w}, w_{n}^{b_a}
wnw,wna,wnbw,wnba为高斯分布的白噪声。
IMU的运动模型可以定义为如下形式:
(3)
R
n
+
1
I
M
U
=
R
n
I
M
U
exp
(
(
ω
n
d
t
)
×
)
v
n
+
1
I
M
U
=
v
n
I
M
U
+
(
R
n
I
M
U
a
n
+
g
)
d
t
p
n
+
1
I
M
U
=
p
n
I
M
U
+
v
n
I
M
U
d
t
其中的所有变量都是真值形式,没有考虑到偏差等等的因素,如果把带有偏差的值运用到上面动态模型中,则误差会十分大,甚至比直接使用轮子的积分来的更差。
状态变量选择为:
x
n
:
=
(
R
n
I
M
U
,
v
n
I
M
U
,
p
n
I
M
U
,
b
n
ω
,
b
n
a
,
R
n
c
,
p
n
c
)
\mathbf{x}_{n} :=\left(\mathbf{R}_{n}^{\mathrm{IMU}}, \mathbf{v}_{n}^{\mathrm{IMU}}, \mathbf{p}_{n}^{\mathrm{IMU}}, \mathbf{b}_{n}^{\omega}, \mathbf{b}_{n}^{\mathrm{a}}, \mathbf{R}_{n}^{\mathrm{c}}, \mathbf{p}_{n}^{\mathrm{c}}\right)
xn:=(RnIMU,vnIMU,pnIMU,bnω,bna,Rnc,pnc)
其中系统的输入为IMU的测量值,
R
n
I
M
U
,
v
n
I
M
U
,
p
n
I
M
U
\mathbf{R}_{n}^{\mathrm{IMU}},\mathbf{v}_{n}^{\mathrm{IMU}},\mathbf{p}_{n}^{\mathrm{IMU}}
RnIMU,vnIMU,pnIMU是载体在第n步(或者理解为t时刻)的姿态,速度和位置,其中的参考系都是世界坐标系,坐标系的信息如下图:
可以看到每个变量都是从载体系到世界坐标系的转换,不要被变量上面的IMU所迷惑了; b n ω , b n a \mathbf{b}_{n}^{\omega}, \mathbf{b}_{n}^{\mathrm{a}} bnω,bna是IMU的零偏, R n c , p n c \mathbf{R}_{n}^{\mathrm{c}}, \mathbf{p}_{n}^{\mathrm{c}} Rnc,pnc是IMU到载体系的外参矩阵,这里也做作为一个更新量进行了估计。
同正常的kalman滤波步骤一样,我们首先建立预测方程(文章中把预测阶段称为传播propagate)和更新方程:
(5)
x
n
+
1
=
f
(
x
n
,
u
n
)
+
w
n
y
n
=
h
(
x
n
)
+
n
n
其中正如上面所说, y n y_n yn是辅助变量,理想情况下,我们的设置值为[0, 0],即 h ( x n ) ≈ 0 h\left(\mathbf{x}_{n}\right)\approx0 h(xn)≈0
这部分更类似于观测方程的建立,主要是把状态估计中的速度(载体在世界坐标系下的速度,即
W
V
n
^WV_n
WVn)转移到载体系下
v
n
c
=
[
v
n
f
o
r
v
n
l
a
t
v
n
u
p
]
=
(
R
n
c
)
T
(
R
n
I
M
U
)
T
v
n
I
M
U
+
(
ω
n
)
×
p
n
c
\mathbf{v}_{n}^{\mathrm{c}}=\left[
对于汽车等只能向前的物体,上述的
v
n
l
a
t
,
v
n
u
p
v_n^{lat}, v_n^{up}
vnlat,vnup的值应该近似于0,因此伪量测为
y
n
=
[
y
n
l
a
t
y
n
u
p
]
=
[
h
l
a
t
(
x
n
)
+
n
n
l
a
t
h
u
p
(
x
n
)
+
n
n
u
p
]
=
[
v
n
l
a
t
v
n
u
p
]
+
n
n
y_n=\left[
其中
n
n
n_n
nn为噪声,这里算作一个松散的约束,这样的效果往往比强硬的把伪测量看做0要好的多。
整体结构如下图:
整体而言,系统使用IEKF进行状态的估计,而使用神经网络根据IMU的测量值产生出伪量测及其协方差,整个算法中预测阶段的系统噪声的协方差 Q Q Q是固定的,因为都是高斯白噪声。笔者还是比较看好这样的slam系统的,因为个人认为神经网络不是很适合slam这种几何学的问题,而作为辅助的话还是很看好的。
从系统结构图上可以看到,系统使用的是IMU的量测值,其为基于时间的一组序列,作者选择其中N组数据进行卷积,之后产生对应的测量协方差:
N
n
+
1
=
C
N
N
(
{
ω
i
I
M
U
,
a
i
I
M
U
}
i
=
n
−
N
n
)
\mathbf{N}_{n+1}=\mathrm{CNN}\left(\left\{\omega_{i}^{\mathrm{IMU}}, \mathbf{a}_{i}^{\mathrm{IMU}}\right\}_{i=n-N}^{n}\right)
Nn+1=CNN({ωiIMU,aiIMU}i=n−Nn)
作者说道设计中的三个动机如下:
网络最终的输出为一个2维向量
z
n
=
[
z
n
l
a
t
,
z
n
u
p
]
T
∈
R
2
z_n=[z_n^{lat}, z_n^{up}]^T \in R^2
zn=[znlat,znup]T∈R2,随后作者引入一个超参数
β
\beta
β用于控制协方差的扩张上下限,最终的协方差值取做:
N
n
+
1
=
diag
(
σ
lat
2
1
0
β
tanh
(
z
n
lat
)
,
σ
up
2
1
0
β
tanh
(
z
n
up
)
)
\mathbf{N}_{n+1}=\operatorname{diag}\left(\sigma_{\text { lat }}^{2} 10^{\beta \tanh \left(z_{n}^{\text { lat }}\right)}, \sigma_{\text { up }}^{2} 10^{\beta \tanh \left(z_{n}^{\text { up }}\right)}\right)
Nn+1=diag(σ lat 210βtanh(zn lat ),σ up 210βtanh(zn up ))
因此协方差的动态变化范围为
1
0
−
β
~
1
0
β
10^{-\beta}~10^{\beta}
10−β~10β。
网络采用1维卷积的方式,共2层,第一层卷积核的大小为5,输出的深度为32,膨胀系数为1,第二层卷积核大小为5,输出深度为32,膨胀系数为3,最后还有一个全连接层将特征输出综合输出为2,取得滑动窗口的大小为N=15,最终一共产生的参数量为6210(这个参数量笔者计算的稍微不太对,按照stride=2来计算的话最终的结果是6304)。
在实现的一些初始参数这里就不过多解释了,需要注意的是,用于不同的机器人的时候,需要对这些参数稍加修改,具体还是参照论文中的部分。
网络的训练步骤就是:
具体的一些超参数这里就不赘述了。
论文最终对比了IMLS(以激光雷达数据为基础),双目ORB-SLAM2和纯IMU进行积分的方法,结果如下:
结论还是很强势的,毕竟只是使用IMU的数据进行航位推算,最终能达到比ORB-SLAM2的效果还要还一点也确实很不错了。
除此之外,还记得作者设计网络时候的三个动机吗?作者也把其中第二个动机进行了验证,结果如下图:
恩~果然在转弯的时候,方差出现了变化。
笔者最后把github上的代码下载下来运行了一下,几个测试数据集的效果还是不错的,是一个足以期待的方向。
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。