VIO第1讲:IMU传感器模型与误差模型
文章目录
1 旋转运动学-刚体坐标系
定义几个常用的坐标系,b
代表本体body
坐标系,也就是IMU
所在的坐标系;I
代表世界world
坐标系(惯性系inertial
);有些论文会这样描述the rotation from the inertial frame to the body frame
,所以要搞清楚这些坐标系之间的关系。
质量块在body坐标系下的坐标为:
r
B
=
(
x
1
,
x
2
,
x
3
)
⊤
\mathbf{r}_B=(x_1,x_2,x_3)^\top
rB=(x1,x2,x3)⊤
把这个坐标旋转到惯性系(世界系)
r
I
(
t
)
=
x
1
(
t
)
i
+
x
2
(
t
)
j
+
x
3
(
t
)
k
=
R
I
B
r
B
\boldsymbol{r}_I(t)=x_1(t)\boldsymbol{i}+x_2(t)\boldsymbol{j}+x_3(t)\boldsymbol{k}=\mathbf{R}_{IB}\mathbf{r}_B
rI(t)=x1(t)i+x2(t)j+x3(t)k=RIBrB
① 位置的导数:速度
对位置求导,得到惯性系下的速度
v
I
v_I
vI
r
˙
I
=
R
I
B
r
˙
B
+
R
˙
I
B
r
B
=
R
I
B
r
˙
B
+
[
R
I
B
ω
b
]
×
r
I
=
R
I
B
v
B
+
ω
×
r
I
=
v
I
\begin{aligned} &\dot{\mathbf{r}}_{I} =\mathbf{R}_{IB}\dot{\mathbf{r}}_B+\dot{\mathbf{R}}_{IB}\mathbf{r}_B \\ &=\mathbf{R}_{IB}\dot{\mathbf{r}}_B+[\mathbf{R}_{IB}\omega_b]_\times\mathbf{r}_I \\ &=\mathbf{R}_{IB}\mathbf{v}_B+\boldsymbol{\omega}\times\mathbf{r}_I \\ &=\mathbf{v}_I \end{aligned}
r˙I=RIBr˙B+R˙IBrB=RIBr˙B+[RIBωb]×rI=RIBvB+ω×rI=vI
其中,对旋转矩阵R的导数右下面推出,这里用到了泰勒展开和
R
p
∧
R
T
=
(
R
p
)
∧
Rp^\wedge R^\mathrm{T}=(Rp)^\wedge
Rp∧RT=(Rp)∧这一条性质。最后一行因为反对称矩阵也是叉积。
R
˙
I
B
r
B
=
lim
Δ
t
→
0
R
I
B
exp
(
[
ω
B
B
′
Δ
t
]
∧
)
r
B
−
R
I
B
r
B
Δ
t
≈
(
R
I
B
(
I
+
[
ω
B
B
′
Δ
t
]
∧
)
r
B
−
R
I
B
r
B
)
/
Δ
t
≈
R
I
B
(
ω
B
B
′
)
∧
r
B
=
(
R
I
B
ω
B
B
′
)
∧
R
I
B
r
B
=
ω
×
r
I
\begin{aligned} \dot{\mathbf{R}}_{IB}\mathbf{r}_{B}& =\lim_{\Delta t\to0}\frac{\mathbf{R}_{IB}\exp([\omega_{BB^{\prime}}\Delta t]^{\wedge})\mathbf{r}_B-\mathbf{R}_{IB}\mathbf{r}_B}{\Delta t} \\ &\approx ({\mathbf{R}}_{IB}(I + [\omega_{BB^{\prime}}\Delta t]^{\wedge})\mathbf{r}_B -{R}_{IB}{r}_B ) /{\Delta t} \\ &\approx\mathbf{R}_{IB}(\omega_{BB^{\prime}})^{\wedge} {r}_B \\ &= (\mathbf{R}_{IB}\omega_{BB^{\prime}})^{\wedge}\mathbf{R}_{IB}\mathbf{r}_B=\boldsymbol{\omega}\times\mathbf{r}_I \end{aligned}
R˙IBrB=Δt→0limΔtRIBexp([ωBB′Δt]∧)rB−RIBrB≈(RIB(I+[ωBB′Δt]∧)rB−RIBrB)/Δt≈RIB(ωBB′)∧rB=(RIBωBB′)∧RIBrB=ω×rI
这里有一个非常重要的结论,
r
I
(
t
)
=
R
I
B
r
B
\boldsymbol{r}_I(t)=\mathbf{R}_{IB}\mathbf{r}_B
rI(t)=RIBrB,但是
v
I
!
=
R
I
B
v
B
v_{I} \ !=\mathbf{R}_{IB}\mathbf{v}_B
vI !=RIBvB,也就是说,任一个点在两个坐标系下的速度矢量都是不同的,不是同一个矢量在不同坐标系下的表达!下面加速度同理。
v
I
≡
R
I
B
v
B
+
ω
×
r
I
⇔
R
I
B
v
B
≡
v
I
−
ω
×
r
I
\begin{aligned}&\mathbf{v}_I \equiv\mathbf{R}_{IB}\mathbf{v}_B+\mathbf{\omega}\times\mathbf{r}_I\Leftrightarrow\mathbf{R}_{IB}\mathbf{v}_B\equiv\mathbf{v}_I-\mathbf{\omega}\times\mathbf{r}_I\end{aligned}
vI≡RIBvB+ω×rI⇔RIBvB≡vI−ω×rI
② 速度的导数:加速度
r ¨ = R I B v ˙ B + R ˙ I B v B + ω × r ˙ I + [ R ˙ I B ω b + R I B ω ˙ b ] × r I = R I B v ˙ B + R ˙ I B v B + ω × ( v + ω × r I ) + [ R I B ω b ] × r I = R I B a B + 2 ω × v + ω × ( ω × r I ) + ω ˙ × r I ⇒ a = a I − 2 ω × v ⏟ 科氏力 − ω ˙ × r I ⏟ 欧拉力 − ω × ( ω I × r I ) ⏟ 离心力 \begin{aligned} \ddot{\mathbf{r}}&=\mathbf{R}_{IB}\dot{\mathbf{v}}_B+\dot{\mathbf{R}}_{IB}\mathbf{v}_B+\boldsymbol{\omega}\times\dot{\mathbf{r}}_I+[\dot{\mathbf{R}}_{IB}\boldsymbol{\omega}_b+\mathbf{R}_{IB}\dot{\boldsymbol{\omega}}_b]_\times\mathbf{r}_I \\ &=\mathbf{R}_{IB}\dot{\mathbf{v}}_B+\dot{\mathbf{R}}_{IB}\mathbf{v}_B+\boldsymbol{\omega}\times(\mathbf{v}+\boldsymbol{\omega}\times\mathbf{r}_I)+[\mathbf{R}_{IB}\boldsymbol{\omega}_b]_\times\mathbf{r}_I \\ &=\mathbf{R}_{IB}\mathbf{a}_B+2\boldsymbol{\omega}\times\mathbf{v}+\boldsymbol{\omega}\times(\boldsymbol{\omega}\times\mathbf{r}_I)+\dot{\boldsymbol{\omega}}\times\mathbf{r}_I \\ \Rightarrow\mathbf{a}& =\mathbf{a}_I-\underbrace{2\boldsymbol{\omega}\times\mathbf{v}}_\text{科氏力}-\underbrace{\dot{\boldsymbol{\omega}}\times\mathbf{r}_I}_\text{欧拉力}-\underbrace{\boldsymbol{\omega}\times(\boldsymbol{\omega}_I\times\mathbf{r}_I)}_\text{离心力} \end{aligned} r¨⇒a=RIBv˙B+R˙IBvB+ω×r˙I+[R˙IBωb+RIBω˙b]×rI=RIBv˙B+R˙IBvB+ω×(v+ω×rI)+[RIBωb]×rI=RIBaB+2ω×v+ω×(ω×rI)+ω˙×rI=aI−科氏力 2ω×v−欧拉力 ω˙×rI−离心力 ω×(ωI×rI)
③ 关于速度、加速度、角速度在不同坐标系间关系
实际上,如果把IMU放在车辆中心,我们可以忽略掉科氏力等的影响,认为IMU所测的加速度即车体系加速度,即世界系下加速度矢量转换到车辆坐标系下的结果。
总体来讲,速度和加速度都是世界系下矢量转换到车辆坐标系下的结果,而不是车辆坐标系中的矢量,因为对于车辆自身坐标系来说,其速度一直为0,更不可能有加速度了!
注意IMU测量的加速度和角速度都是body系下的变量!比如下面误差模型中的左侧的测量值,右上角b表示body系下变量!(这个是我一开始疑惑的地方,IMU测量的数据到底是那个系下的变量,主要是把模型和实际搞反)
实际中,我们得到的是左边的测量值,而不是右边的值!我们实际使用两个变量,角速度不需要变换到世界系下!(至于为什么在下面的误差模型中没有变换角速度,这个涉及了绕轴旋转,旋转一个微小的角度,是用body
系下的角速度计算的。)
ω
~
b
=
ω
b
+
b
g
+
η
g
a
~
b
=
R
b
w
(
a
w
−
g
w
)
+
b
a
+
η
a
\begin{aligned}\tilde{\boldsymbol{\omega}}^b&=\mathbf{\omega}^b+\mathbf{b}_g+\mathbf{\eta}_g\\\tilde{\mathbf{a}}^b&=\mathbf{R}_{bw}(\mathbf{a}^w{-}\mathbf{g}^w)+\mathbf{b}_a+\mathbf{\eta}_a\end{aligned}
ω~ba~b=ωb+bg+ηg=Rbw(aw−gw)+ba+ηa
在运动系统中,角速度与加速度是如何测量的。这种思路是仿真的思路,或者说是描述已知系统的思路。也就是说,我们可以先假设物体发生了什么运动,再去考虑这种运动下应该产生什么样的 IMU 测量。但是,实际情况是反过来的。我们能够读取到 IMU 传感器的读数,但必须根据 IMU 和其他传感器的读数来推断系统的运动,而不能直接获取系统的各种速度、加速度信息。
我们要用测量得到的IMU
数据去进行航迹推算,下面的式子忽略了噪声
R
˙
w
b
=
R
w
b
(
ω
~
b
−
b
g
)
∧
,
或
q
˙
=
q
[
0
,
1
2
(
ω
~
−
b
g
)
]
p
˙
w
=
v
w
,
v
˙
w
=
R
w
b
(
a
~
b
−
b
a
)
+
g
w
=
a
w
.
\begin{aligned}\dot{\boldsymbol{R}}_{wb}&=\boldsymbol{R}_{wb}(\tilde{\boldsymbol{\omega}}^{b}-\boldsymbol{b}_g)^\wedge,\quad\text{或 }\dot{\boldsymbol{q}}=\boldsymbol{q}\left[0,\frac12\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_g\right)\right]\\\dot{\boldsymbol{p}}^{w}&=\boldsymbol{v}^{w},\\\dot{\boldsymbol{v}}^{w}&=\boldsymbol{R}_{wb}(\tilde{\boldsymbol{a}}^{b}-\boldsymbol{b}_a)+\boldsymbol{g}^{w} = a^w.\end{aligned}
R˙wbp˙wv˙w=Rwb(ω~b−bg)∧,或 q˙=q[0,21(ω~−bg)]=vw,=Rwb(a~b−ba)+gw=aw.
通过欧拉法进行积分(省略了上下标),除此以外,还有中值积分、梯形积分以及高阶插值法-龙格库塔法(比如MSCKF)。
R
(
t
+
Δ
t
)
=
R
(
t
)
E
x
p
(
(
ω
~
−
b
g
)
Δ
t
)
,
或
q
(
t
+
Δ
t
)
=
q
(
t
)
⌊
1
,
1
2
(
ω
~
−
b
g
)
Δ
t
⌋
,
p
(
t
+
Δ
t
)
=
p
(
t
)
+
v
Δ
t
+
1
2
(
R
(
t
)
(
a
~
−
b
a
)
)
Δ
t
2
+
1
2
g
Δ
t
2
,
v
(
t
+
Δ
t
)
=
v
(
t
)
+
R
(
t
)
(
a
~
−
b
a
)
Δ
t
+
g
Δ
t
.
\begin{aligned} &\boldsymbol{R}(t+\Delta t) =\boldsymbol{R}(t)\mathrm{Exp}\left((\tilde{\omega}-\boldsymbol{b}_g)\Delta t\right),\quad\text{或 }q(t+\Delta t)=\boldsymbol{q}(t)\left\lfloor1,\frac12\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_g\right)\Delta t\right\rfloor, \\ &\boldsymbol{p}(t+\Delta t) =\boldsymbol{p}(t)+\boldsymbol{v}\Delta t+\frac12\left(\boldsymbol{R}(t)(\tilde{\boldsymbol{a}}-\boldsymbol{b}_a)\right)\Delta t^2+\frac12\boldsymbol{g}\Delta t^2, \\ &\boldsymbol{v}(t+\Delta t) =\boldsymbol{v}(t)+\boldsymbol{R}(t)(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a})\Delta t+\boldsymbol{g}\Delta t. \end{aligned}
R(t+Δt)=R(t)Exp((ω~−bg)Δt),或 q(t+Δt)=q(t)⌊1,21(ω~−bg)Δt⌋,p(t+Δt)=p(t)+vΔt+21(R(t)(a~−ba))Δt2+21gΔt2,v(t+Δt)=v(t)+R(t)(a~−ba)Δt+gΔt.
上式也可以进一步累积,比如从 i
时刻一直递推到 j
时刻
R
j
=
R
i
∏
k
=
i
j
−
1
Exp
(
(
ω
~
k
−
b
g
,
k
)
Δ
t
)
或
q
j
=
q
i
∏
k
=
i
j
−
1
[
1
,
1
2
(
ω
~
k
−
b
g
,
k
)
Δ
t
]
,
p
j
=
p
k
+
∑
k
=
i
j
−
1
[
v
k
Δ
t
+
1
2
g
Δ
t
2
]
+
1
2
∑
k
=
i
j
−
1
R
k
(
a
~
k
−
b
a
,
k
)
Δ
t
2
,
v
j
=
v
i
+
∑
k
=
i
j
−
1
[
R
k
(
a
~
k
−
b
a
,
k
)
Δ
t
+
g
Δ
t
]
.
\begin{aligned} &R_{j} =R_i\prod_{k=i}^{j-1}\operatorname{Exp}\left(\left(\tilde{\omega}_k-b_{g,k}\right)\Delta t\right)\quad\text{或 }\boldsymbol{q}_j=\boldsymbol{q}_i\prod_{k=i}^{j-1}\left[1,\frac12\left(\tilde{\omega}_k-\boldsymbol{b}_{g,k}\right)\Delta t\right], \\ &\boldsymbol{p}_{j} =\boldsymbol{p}_{k}+\sum\limits_{k=i}^{j-1}\left[\boldsymbol{v}_{k}\Delta t+\frac{1}{2}\boldsymbol{g}\Delta t^{2}\right]+\frac{1}{2}\sum\limits_{k=i}^{j-1}\boldsymbol{R}_{k}\left(\widetilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{a,k}\right)\Delta t^{2}, \\ &\boldsymbol{v}_{j} =\boldsymbol{v}_i+\sum_{k=i}^{j-1}\left[\boldsymbol{R}_k\left(\tilde{\boldsymbol{a}}_k-\boldsymbol{b}_{a,k}\right)\Delta t+\boldsymbol{g}\Delta t\right]. \end{aligned}
Rj=Rik=i∏j−1Exp((ω~k−bg,k)Δt)或 qj=qik=i∏j−1[1,21(ω~k−bg,k)Δt],pj=pk+k=i∑j−1[vkΔt+21gΔt2]+21k=i∑j−1Rk(a
k−ba,k)Δt2,vj=vi+k=i∑j−1[Rk(a~k−ba,k)Δt+gΔt].
2 IMU工作原理
IMU
由陀螺仪+加速度计组成,绝⼤多数场景下认为他们是同⼀坐标系,使用时认为他们是捷联的(就是相对固定的)
2.1 加速度计工作原理
测量原理可以用一个简单的质量块 + 弹簧 + 指示计来表示,加速度计测量值 a m a_m am为弹簧拉力对应的加速度, f f f 弹簧拉力, a a a 物体在惯性系下的加速度, g g g 重力加速度。
a
m
=
f
m
=
a
−
g
\mathbf{a}_m=\frac{\mathbf{f}}{m}=\mathbf{a}-\mathbf{g}
am=mf=a−g
加速度计经常使用的坐标系:东北天坐标系 (ENU)
静止时,合力为0
a
=
0
a
m
=
−
g
\begin{aligned}\mathbf{a}&=0\\\mathbf{a}_m&=-\mathbf{g}\end{aligned}
aam=0=−g
自由落体时:合力即重力,所以
a
=
g
a = g
a=g
a
=
g
a
m
=
0
\begin{aligned}\mathbf{a}&=\mathbf{g}\\\mathbf{a}_m&=0\end{aligned}
aam=g=0
2.2 陀螺仪测量原理
陀螺仪主要用来测量物体的旋转角速度,按测量原理分有振动陀螺,光纤陀螺等。MEMS 陀螺仪:一个主动运动轴 + 一个敏感轴。在旋转坐标系中,运动的物体受到科氏力作用
3 IMU误差模型
一般来讲,IMU存在以下的两种误差:确定性误差和随机误差。值得注意,确定性误差bias会随时间发生随机的改变!也就是形成随机误差中的bias
随机游走,随机游走的本质是指导数为高斯过程的随机过程!
3.1 确定性误差
3.1.1 Bias
理论上,当没有外部作用时,
I
M
U
IMU
IMU 传感器的输出应该为 0。但是,实际数据存在一个偏置
b
b
b。加速度计
b
i
a
s
bias
bias 对位姿估计的影响:
v
e
r
r
=
b
a
t
,
p
e
r
r
=
1
2
b
a
t
2
\mathbf{v}_{err}=\mathbf{b}_at,\quad\mathbf{p}_{err}=\frac{1}{2}\mathbf{b}_at^2
verr=bat,perr=21bat2
3.1.2 Scale
真实值和传感器测量值之间存在尺度关系,但不为1.
工艺问题导致的误差
scale + Misalignment(尺度 + 失真)
对角线元素表示尺度,其它的表示由于xyz轴不垂直给其他轴产生的失真影响。
[
l
a
x
l
a
y
l
a
z
]
=
[
s
x
x
m
x
y
m
x
z
m
y
x
s
y
y
m
y
z
m
z
x
m
z
y
s
z
z
]
[
a
x
a
y
a
z
]
\begin{bmatrix}l_{ax}\\l_{ay}\\l_{az}\end{bmatrix}=\begin{bmatrix}s_{xx}&m_{xy}&m_{xz}\\m_{yx}&s_{yy}&m_{yz}\\m_{zx}&m_{zy}&s_{zz}\end{bmatrix}\begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}
laxlaylaz
=
sxxmyxmzxmxysyymzymxzmyzszz
axayaz
3.2 确定性误差标定—六面法
3.2.1 六面法标定加速度
首先确定上面的确定性误差模型,实际值
l
l
l = 尺度*测量 + 偏差。所以我们在标定的时候,需要求解出这12个变量。
[
l
a
x
l
a
y
l
a
z
]
=
[
s
x
x
m
x
y
m
x
z
m
y
x
s
y
y
m
y
z
m
z
x
m
z
y
s
z
z
]
[
a
x
a
y
a
z
]
+
[
b
a
x
b
a
y
b
a
z
]
\begin{bmatrix}l_{ax}\\l_{ay}\\l_{az}\end{bmatrix}=\begin{bmatrix}s_{xx}&m_{xy}&m_{xz}\\m_{yx}&s_{yy}&m_{yz}\\m_{zx}&m_{zy}&s_{zz}\end{bmatrix}\begin{bmatrix}a_{x}\\a_{y}\\a_{z}\end{bmatrix}+\begin{bmatrix}b_{ax}\\b_{ay}\\b_{az}\end{bmatrix}
laxlaylaz
=
sxxmyxmzxmxysyymzymxzmyzszz
axayaz
+
baxbaybaz
六面法的具体原理如下:
3.2.2 六面法标定陀螺仪
和加速度计六面法不同的是,陀螺仪的真实值由高精度转台提供,这里的 6 面是指各个轴顺时针和逆时针旋转。
具体方法同上。
3.3 IMU随机误差
3.3.1 高斯过程
在高斯过程Gaussian process
中,连续输入空间中每个点都是与一个正态分布的随机变量相关联。此外,这些随机变量的每个有限集合都有一个多元正态分布,换句话说他们的任意有限线性组合是一个正态分布。高斯过程的分布是所有那些(无限多个)随机变量的联合分布,正因如此,它是连续域(例如时间或空间)上函数的分布。
可以把高斯过程看成多元正态分布的无限维广义延伸,每一个采样时刻的变量对应一个维度。
3.3.2 高斯白噪声
IMU
数据连续时间上受到一个均值为 0,方差为
σ
σ
σ,各时刻之间相互独立的高斯过程
n
(
t
)
n(t)
n(t)
E
[
n
(
t
)
]
≡
0
E
[
n
(
t
1
)
n
(
t
2
)
]
=
σ
2
δ
(
t
1
−
t
2
)
\begin{aligned} &E[n(t)]\equiv0 \\ &E\left[n\left(t_{1}\right)n\left(t_{2}\right)\right]=\sigma^{2}\delta\left(t_{1}-t_{2}\right) \end{aligned}
E[n(t)]≡0E[n(t1)n(t2)]=σ2δ(t1−t2)
但是IMU
传感器获取的数据为离散采样,离散和连续高斯白噪声的方差之间存在如下转换关系:
n
d
[
k
]
≜
n
(
t
0
+
Δ
t
)
≃
1
Δ
t
∫
t
0
t
0
+
Δ
t
n
(
τ
)
d
t
E
(
n
d
[
k
]
2
)
=
E
(
1
Δ
t
2
∫
t
0
t
0
+
Δ
t
∫
t
0
t
0
+
Δ
t
n
(
τ
)
n
(
t
)
d
τ
d
t
)
=
E
(
σ
2
Δ
t
2
∫
t
0
t
0
+
Δ
t
∫
t
0
t
0
+
Δ
t
δ
(
t
−
τ
)
d
τ
d
t
)
=
E
(
σ
2
Δ
t
)
\begin{aligned} n_{d}[k]& \triangleq n\left(t_{0}+\Delta t\right)\simeq\frac1{\Delta t}\int_{t_{0}}^{t_{0}+\Delta t}n(\tau)dt \\ E\left(n_d[k]^2\right)& =E\left(\frac1{\Delta t^2}\int_{t_0}^{t_0+\Delta t}\int_{t_0}^{t_0+\Delta t}n(\tau)n(t)d\tau dt\right) \\ &=E\left(\frac{\sigma^2}{\Delta t^2}\int_{t_0}^{t_0+\Delta t}\int_{t_0}^{t_0+\Delta t}\delta(t-\tau)d\tau dt\right) \\ &=E\left(\frac{\sigma^2}{\Delta t}\right) \end{aligned}
nd[k]E(nd[k]2)≜n(t0+Δt)≃Δt1∫t0t0+Δtn(τ)dt=E(Δt21∫t0t0+Δt∫t0t0+Δtn(τ)n(t)dτdt)=E(Δt2σ2∫t0t0+Δt∫t0t0+Δtδ(t−τ)dτdt)=E(Δtσ2)
所以我们可以推出连续和离散的高斯白噪声标准差相差一个
1
Δ
t
\frac{1}{\sqrt{\Delta t}}
Δt1,其中
w
[
k
]
∼
N
(
0
,
1
)
w[k]\sim\mathcal{N}(0,1)
w[k]∼N(0,1)
n
d
[
k
]
=
σ
d
w
[
k
]
=
σ
1
Δ
t
w
[
k
]
n_d[k]=\sigma_dw[k] = \sigma\frac{1}{\sqrt{\Delta t}}w[k]
nd[k]=σdw[k]=σΔt1w[k]
3.3.3 Bias随机游走
在上面确定性误差bias中,bias会随时间的改变而改变!
通常用维纳过程(wiener process)来建模 bias 随时间连续变化的过程,离散时间下称之为随机游走,其中 w 是方差为 1 的白噪声。换句话说,随机游走就是导数为高斯过程的随机过程!
b
˙
(
t
)
=
n
(
t
)
=
σ
b
w
(
t
)
\dot{b}(t)=n(t)=\sigma_bw(t)
b˙(t)=n(t)=σbw(t)
bias
随机游走的噪声标准差从连续时间到离散之间需要乘以
Δ
t
\sqrt{\Delta t}
Δt
b
d
[
k
]
≜
b
(
t
0
)
+
∫
t
0
t
0
+
Δ
t
n
(
t
)
d
t
E
(
(
b
d
[
k
]
−
b
d
[
k
−
1
]
)
2
)
=
E
(
∫
t
0
+
Δ
t
t
0
+
Δ
t
∫
t
0
t
0
+
Δ
t
n
(
t
)
n
(
τ
)
d
τ
d
t
)
=
E
(
σ
b
2
∫
t
0
t
0
+
Δ
t
∫
t
0
t
0
+
Δ
t
δ
(
t
−
τ
)
d
τ
d
t
)
=
E
(
σ
b
2
Δ
t
)
\begin{aligned} b_{d}[k]\triangleq & b\left(t_{0}\right)+\int_{t_{0}}^{t_{0}+\Delta t}n(t)dt \\ E\left(\left(b_{d}[k]-b_{d}[k-1]\right)^{2}\right)& =E\left(\int_{t_{0}+\Delta t}^{t_{0}+\Delta t}\int_{t_{0}}^{t_{0}+\Delta t}n(t)n(\tau)d\tau dt\right) \\ &=E\left(\sigma_{b}^{2}\int_{t_{0}}^{t_{0}+\Delta t}\int_{t_{0}}^{t_{0}+\Delta t}\delta(t-\tau)d\tau dt\right) \\ &=E\left(\sigma_{b}^{2}\Delta t\right) \end{aligned}
bd[k]≜E((bd[k]−bd[k−1])2)b(t0)+∫t0t0+Δtn(t)dt=E(∫t0+Δtt0+Δt∫t0t0+Δtn(t)n(τ)dτdt)=E(σb2∫t0t0+Δt∫t0t0+Δtδ(t−τ)dτdt)=E(σb2Δt)
即
b
d
[
k
]
=
b
d
[
k
−
1
]
+
σ
b
d
w
[
k
]
b_{d}[k]=b_{d}[k-1]+\sigma_{bd}w[k]
bd[k]=bd[k−1]+σbdw[k]
其中:
w
[
k
]
∼
N
(
0
,
1
)
σ
b
d
=
σ
b
Δ
t
\begin{array}{l}w[k]\sim\mathcal{N}(0,1)\\\sigma_{bd}=\sigma_b\sqrt{\Delta t}\end{array}
w[k]∼N(0,1)σbd=σbΔt
3.3.4 总结
因为 IMU
传感器按照固定频率进行采样,不妨设每次采样间隔为 ∆t
,那么对于噪声来说,陀螺仪和加速度计的离散测量噪声可以简化地描述为
η
g
(
k
)
∼
N
(
0
,
1
Δ
t
C
o
v
(
η
g
)
)
,
η
a
(
k
)
∼
N
(
0
,
1
Δ
t
C
o
v
(
η
a
)
)
.
\begin{aligned}\eta_g(k)&\sim\mathcal{N}(0,\frac{1}{\Delta t}\mathrm{Cov}(\eta_g)),\\\eta_a(k)&\sim\mathcal{N}(0,\frac{1}{\Delta t}\mathrm{Cov}(\eta_a)).\end{aligned}
ηg(k)ηa(k)∼N(0,Δt1Cov(ηg)),∼N(0,Δt1Cov(ηa)).
对于零偏部分,则可以写:
b
g
(
k
+
1
)
−
b
g
(
k
)
∼
N
(
0
,
Δ
t
C
o
v
(
b
g
)
)
,
b
a
(
k
+
1
)
−
b
a
(
k
)
∼
N
(
0
,
Δ
t
C
o
v
(
b
a
)
)
.
\begin{aligned}\boldsymbol{b}_g(k+1)-\boldsymbol{b}_g(k)&\sim\mathcal{N}(\boldsymbol{0},\Delta t\mathrm{Cov}(\boldsymbol{b}_g)),\\\boldsymbol{b}_a(k+1)-\boldsymbol{b}_a(k)&\sim\mathcal{N}(\boldsymbol{0},\Delta t\mathrm{Cov}(\boldsymbol{b}_a)).\end{aligned}
bg(k+1)−bg(k)ba(k+1)−ba(k)∼N(0,ΔtCov(bg)),∼N(0,ΔtCov(ba)).
σ
g
,
σ
a
σ_g ,σ_a
σg,σa 的参数来表达 IMU
的噪声标准差,用
σ
b
g
,
σ
b
a
σ_bg ,σ_ba
σbg,σba参数来表达零偏游走的标准差,下面表明了离散与连续之间的转换关系(一般标定都是连续,程序中使用离散需要进行转换)
σ
g
(
k
)
=
1
Δ
t
σ
g
,
σ
a
(
k
)
=
1
Δ
t
σ
a
,
σ
b
g
(
k
)
=
Δ
t
σ
b
g
,
σ
b
a
(
k
)
=
Δ
t
σ
b
a
.
\begin{aligned}\sigma_g(k)&=\frac{1}{\sqrt{\Delta t}}\sigma_g,\quad\sigma_a(k)&=\frac{1}{\sqrt{\Delta t}}\sigma_a,\\\sigma_{bg}(k)&=\sqrt{\Delta t}\sigma_{bg},\quad\sigma_{ba}(k)&=\sqrt{\Delta t}\sigma_{ba}.\end{aligned}
σg(k)σbg(k)=Δt1σg,σa(k)=Δtσbg,σba(k)=Δt1σa,=Δtσba.
离散时间的噪声和零偏是直接加在被测的物理量上的,很容易确定它们的物理单位
σ
g
(
k
)
→
r
a
d
s
,
σ
a
(
k
)
→
m
s
2
,
σ
b
g
(
k
)
→
r
a
d
s
,
σ
b
a
(
k
)
→
m
s
2
.
\sigma_g(k)\to\frac{rad}{s},\quad\sigma_a(k)\to\frac{m}{s^2},\quad\sigma_{bg}(k)\to\frac{rad}{s},\quad\sigma_{ba}(k)\to\frac{m}{s^2}.
σg(k)→srad,σa(k)→s2m,σbg(k)→srad,σba(k)→s2m.
连续时间的方差需要在离散方差上乘或除以一个开方时间单位
σ
g
→
r
a
d
s
,
σ
a
→
m
s
s
,
σ
b
g
→
r
a
d
s
s
,
σ
b
a
→
m
s
2
s
.
\sigma_{g}\to\frac{rad}{\sqrt{s}},\quad\sigma_{a}\to\frac{m}{s\sqrt{s}},\quad\sigma_{bg}\to\frac{rad}{s\sqrt{s}},\quad\sigma_{ba}\to\frac{m}{s^{2}\sqrt{s}}.
σg→srad,σa→ssm,σbg→ssrad,σba→s2sm.
3.4 随机误差标定—Allan 方差法
Allan
方差法要保持传感器绝对静止获取数据。
标定拿到的都是连续时间下白噪声的标准差,但是程序里面的是离散的,所以需要算⼀下离散的标准差(根据上面的公式)
要知道每一段表示什么,角速度和加速度都有一个这样的图。这个就是真实坐标,不是什么对数坐标!
- 角度、速度随机游走/白噪声强度
角度随机游走对应到双对数曲线中斜率为−0.5
的直线,取值可以直接读取t=1
处的值。(一般这个计算比较正确)参考论文报告An introduction to inertial navigation
: obtained by fitting a straight line through the slope and reading its value at t = 1
- 零偏不稳定性
零偏不稳定性对应双对数曲线中斜率为0的直线,也就是双对数曲线中最小值位置。
- 速率随机游走(偏置随机游走)
速率随机游走对应双对数曲线中斜率为0.5的直线,取值时取该直线与t=3
交点。(这个实际上标定可能差了数量级,不是很准确)
注意横坐标数量级可能很大,1就是10的0次幂(红色线那个直角处),3离得不远(蓝色线那个直角处)。参考链接。
3.5 IMU误差模型总结
S
a
{S}_a
Sa为尺度,
n
a
{n}_a
na为高斯白噪声,
b
a
{b}_a
ba为偏差(随机游走的确定性偏差)
a
m
B
=
S
a
R
B
G
(
a
G
−
g
G
)
+
η
a
+
b
a
\mathbf{a}_m^B=\mathbf{S}_a\mathbf{R}_{BG}\left(\mathbf{a}^G-\mathbf{g}^G\right)+\mathbf{\eta}_a+\mathbf{b}_a
amB=SaRBG(aG−gG)+ηa+ba
低端IMU会受到科氏力等影响,所以还要考虑加速度
a
B
{a}^B
aB对陀螺仪的影响。
ω
m
B
=
S
g
ω
B
+
s
g
a
a
B
+
η
g
+
b
g
\mathbf{\omega}_m^B=\mathbf{S}_g\mathbf{\omega}^B+\mathbf{s}_{ga}\mathbf{a}^B+\mathbf{\eta}_g+\mathbf{b}_g
ωmB=SgωB+sgaaB+ηg+bg
- 简化的IMU误差模型
很多时候都会忽略尺度的影响,以及如果IMU安装在车体中心,就可以忽略科氏力等的影响!
ω
~
b
=
ω
b
+
b
g
+
η
g
a
~
b
=
R
b
w
(
a
w
−
g
w
)
+
b
a
+
η
a
\begin{aligned}\tilde{\boldsymbol{\omega}}^b&=\mathbf{\omega}^b+\mathbf{b}_g+\mathbf{\eta}_g\\\tilde{\mathbf{a}}^b&=\mathbf{R}_{bw}(\mathbf{a}^w{-}\mathbf{g}^w)+\mathbf{b}_a+\mathbf{\eta}_a\end{aligned}
ω~ba~b=ωb+bg+ηg=Rbw(aw−gw)+ba+ηa