本文介绍一篇 IMU 和 GPS
融合的惯性导航论文,重点是理解本文提出的:Dynamical constraints update
、Roll and pitch updates
和 Position and heading updates
。
论文链接为:https://www.sciencedirect.com/science/article/pii/S1665642314700963
文章目录
1. Method Description
1.1 Vector state and system specification
首先系统状态量
x
^
\hat{\mathrm{x}}
x^(22维) :
x
^
=
[
q
n
b
ω
b
x
g
r
n
v
n
a
n
x
a
]
′
\hat{\mathrm{x}}=\left[\begin{array}{lllllll} q^{n b} & \omega^{b} & \mathrm{x}_{g} & r^{n} & \mathrm{v}^{n} & a^{n} & \mathrm{x}_{a} \end{array}\right]^{\prime}
x^=[qnbωbxgrnvnanxa]′
其中:
- q n b = [ q 1 , q 2 , q 3 , q 4 ] q^{n b}=\left[\mathrm{q}_{1}, \mathrm{q}_{2}, \mathrm{q}_{3}, \mathrm{q}_{4}\right] qnb=[q1,q2,q3,q4] 为单位四元数,表示导航坐标到载体坐标的坐标变换。
- ω b = [ ω x , ω y , ω z ] \omega^{b}=\left[\omega_{\mathrm{x}}, \omega_{\mathrm{y}}, \omega_{\mathrm{z}}\right] ωb=[ωx,ωy,ωz] 是偏差补偿后的载体旋转角速度。
- x g = [ x g _ x , g _ y , x g _ z ] \mathrm{x}_{g}=\left[\mathrm{x}_{g\_\mathrm{x}}, \mathrm{}_{g\_\mathrm{y}},\mathrm{x}_{g\_\mathrm{z}}\right] xg=[xg_x,g_y,xg_z] 是角速度偏差。
- r n = [ x v , y v , z v ] r^{n}=\left[\mathrm{x}_{\mathrm{v}}, \mathrm{y}_{\mathrm{v}}, \mathrm{z}_{\mathrm{v}}\right] rn=[xv,yv,zv] 是载体坐标原点在导航坐标下的位置。
- v n = [ v x , v y , v z ] \mathrm{v}^{n}=\left[\mathrm{v}_{\mathrm{x}}, \mathrm{v}_{\mathrm{y}}, \mathrm{v}_{\mathrm{z}}\right] vn=[vx,vy,vz] 是载体在导航坐标下的速度。
- a n = [ a x , a y , a z ] {a}^{n}=\left[{a}_{\mathrm{x}}, {a}_{\mathrm{y}}, {a}_{\mathrm{z}}\right] an=[ax,ay,az] 是载体在导航坐标下偏差补偿后的加速度。
- x a = [ x a _ x , x a _ y , x a _ z ] \mathrm{x}_{a}=\left[\mathrm{x}_{{a}\_\mathrm{x}}, \mathrm{x}_{{a}\_\mathrm{y}},\mathrm{x}_{{a}\_\mathrm{z}}\right] xa=[xa_x,xa_y,xa_z] 是加速度偏差。
下图说明了载体坐标与导航坐标之间的关系,这里导航坐标为NED
坐标。
在惯性导航系统中,需要考虑两种测量:
- 高频测量(IMU)。
角速度测量模型
为: y g = ω b + x g + ν g \mathrm{y}_g=\omega^b+\mathrm{x}_g+\nu_g yg=ωb+xg+νg,其中 y g \mathrm{y}_g yg 为测量值, x g \mathrm{x}_g xg 为角速度偏差, ν g \nu_g νg 为角速度高斯白噪声, ω b \omega^b ωb 为角速度真值。加速度测量模型
为: y a = a b + g b + x a + ν a \mathrm{y}_a=a^b+g^b+\mathrm{x}_a+\nu_a ya=ab+gb+xa+νa,其中 g b g^b gb 为载体坐标下的重力向量, x a \mathrm{x}_a xa 是加速度偏差, ν a \nu_a νa 是加速度高斯白噪声。 - 低频测量(GPS)。其位置
z
r
\mathrm{z}_r
zr 和 航向角
z
θ
\mathrm{z}_{\theta}
zθ
测量模型
为: [ z r n z θ n ] = [ r n + ν r θ n + ν θ ] \left[\begin{array}{c} z_{r}^{n} \\ z_{\theta}^{n} \end{array}\right]=\left[\begin{array}{c} r^{n}+\nu_{r} \\ \theta^{n}+\nu_{\theta} \end{array}\right] [zrnzθn]=[rn+νrθn+νθ]。其中 r n r^n rn 为导航位置, ν r \nu_r νr 是位置高斯白噪声, θ n \theta^n θn 是航向角, ν θ \nu_{\theta} νθ 是航向角高斯白噪声。这里需要注意的是,处理时需要将GPS测量的WGS坐标转换成NED坐标
。
1.2 Architecture of the system
系统结构如下图所示,包含预测和更新两部份。使用IMU数据进行预测,更新由三个子系统组成。
1.3 System prediction
当IMU数据到来时,系统进行预测,预测方程为:
Attitude
{
q
(
k
+
1
)
n
b
=
q
(
k
)
n
b
×
q
(
R
b
n
[
ω
(
k
+
1
)
b
Δ
t
]
′
)
ω
(
k
+
1
)
b
=
−
(
y
g
(
k
)
−
x
g
(
k
)
)
x
g
(
k
+
1
)
=
(
1
−
λ
x
g
Δ
t
)
x
g
(
k
)
Position
{
r
(
k
+
1
)
n
=
r
(
k
)
n
+
(
v
(
k
+
1
)
n
Δ
t
)
+
(
a
(
k
+
1
)
n
Δ
t
2
/
2
)
v
(
k
+
1
)
n
=
v
(
k
)
n
+
(
a
(
k
+
1
)
n
Δ
t
)
a
(
k
+
1
)
n
=
R
b
n
[
y
a
(
k
)
−
x
a
(
k
)
]
+
g
x
a
(
k
+
1
)
=
(
1
−
λ
x
a
Δ
t
)
x
a
(
k
)
\begin{array}{l} \text { Attitude }\left\{\begin{array}{l} q_{(k+1)}^{n b}=q_{(k)}^{n b} \times q\left(R^{b n}\left[\omega_{(k+1)}^{b} \Delta t\right]^{\prime}\right) \\ \omega_{(k+1)}^{b}=-\left(y_{g(k)}-\mathrm{x}_{g(k)}\right) \\ \mathrm{x}_{\mathrm{g}(\mathrm{k}+1)}=\left(1-\lambda_{x g} \Delta t\right) \mathrm{x}_{\mathrm{g}(\mathrm{k})} \end{array}\right. \\ \text { Position }\left\{\begin{array}{l} r_{(k+1)}^{n}=r_{(k)}^{n}+\left(\mathrm{v}_{(k+1)}^{n} \Delta t\right)+\left(a_{(k+1)}^{n} \Delta t^{2} / 2\right) \\ \mathrm{v}_{(k+1)}^{n}=\mathrm{v}_{(k)}^{n}+\left(a_{(k+1)}^{n} \Delta t\right) \\ a_{(k+1)}^{n}=R^{b n}\left[y_{a(k)}-\mathrm{x}_{a(k)}\right]+g \\ \mathrm{x}_{a(k+1)}=\left(1-\lambda_{x a} \Delta t\right) \mathrm{x}_{a(\mathrm{k})} \end{array}\right. \end{array}
Attitude ⎩⎪⎪⎨⎪⎪⎧q(k+1)nb=q(k)nb×q(Rbn[ω(k+1)bΔt]′)ω(k+1)b=−(yg(k)−xg(k))xg(k+1)=(1−λxgΔt)xg(k) Position ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧r(k+1)n=r(k)n+(v(k+1)nΔt)+(a(k+1)nΔt2/2)v(k+1)n=v(k)n+(a(k+1)nΔt)a(k+1)n=Rbn[ya(k)−xa(k)]+gxa(k+1)=(1−λxaΔt)xa(k)
其中, λ x g , λ x a \lambda_{\mathrm{xg}},\lambda_{\mathrm{xa}} λxg,λxa 是加速度和角速度随时间变化的衰减系数, Δ t \Delta t Δt 是IMU采样时间, g g g 是重力向量。
状态协方差
P
P
P 更新方程为:
P
(
k
+
1
)
=
∇
F
x
P
(
k
)
∇
F
x
′
+
∇
F
u
U
∇
F
u
′
P_{(k+1)}=\nabla F_{\mathrm{x}} P_{(k)} \nabla F_{\mathrm{x}}^{\prime}+\nabla F_{\mathrm{u}} U \nabla F_{\mathrm{u}}^{\prime}
P(k+1)=∇FxP(k)∇Fx′+∇FuU∇Fu′
其中,矩阵 U U U 为状态转移过程协方差矩阵,包含加速度和角速度噪声与偏差噪声,即 U = diag [ σ g 2 I 3 × 3 σ x g 2 I 3 × 3 σ a 2 I 3 × 3 σ x a 2 I 3 × 3 ] U=\operatorname{diag}\left[\begin{array}{llll} \sigma_{g}{ }^{2} I_{3 \times 3} & \sigma_{x g}{ }^{2} I_{3 \times 3} & \sigma_{a}^{2} I_{3 \times 3} & \sigma_{x a}{ }^{2} I_{3 \times 3} \end{array}\right] U=diag[σg2I3×3σxg2I3×3σa2I3×3σxa2I3×3]
状态转移雅克比矩阵
∇
F
x
\nabla F_{\mathrm{x}}
∇Fx 和系统输入雅可比矩阵
∇
F
u
\nabla F_{\mathrm{u}}
∇Fu 为:
∇
F
x
=
[
∂
f
q
n
b
∂
q
n
b
∂
f
q
n
b
∂
ω
b
0
0
0
0
0
0
0
∂
f
ω
b
∂
x
g
0
0
0
0
0
0
∂
f
x
g
∂
x
g
0
0
0
0
0
0
0
∂
f
r
n
∂
r
n
∂
f
r
n
∂
v
n
∂
f
r
n
∂
a
n
0
0
0
0
0
∂
f
v
n
∂
v
n
∂
f
v
n
∂
a
n
0
∂
f
a
n
∂
q
n
b
0
0
0
0
0
∂
f
a
n
∂
x
a
0
0
0
0
0
0
∂
f
x
a
∂
x
a
]
\nabla F_{\mathrm{x}}=\left[\begin{array}{ccccccc} \frac{\partial fq^{nb}}{\partial q^{nb}} & \frac{\partial fq^{nb}}{\partial \omega^{b}} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & \frac{\partial f\omega^{b}}{\partial \mathrm{x}_{g}} & 0 & 0 & 0 & 0 \\ 0 & 0 & \frac{\partial f\mathrm{x}_{g}}{\partial \mathrm{x}_{g}} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \frac{\partial fr^{n}}{\partial r^{n}} & \frac{\partial fr^{n}}{\partial \mathrm{v}^{n}} & \frac{\partial fr^{n}}{\partial a^{n}} & 0 \\ 0 & 0 & 0 & 0 & \frac{\partial f\mathrm{v}^{n}}{\partial \mathrm{v}^{n}} & \frac{\partial f\mathrm{v}^{n}}{\partial a^{n}} & 0 \\ \frac{\partial fa^{n}}{\partial q^{n b}} & 0 & 0 & 0 & 0 & 0 & \frac{\partial fa^{n}}{\partial \mathrm{x}_{a}} \\ 0 & 0 & 0 & 0 & 0 & 0 & \frac{\partial f\mathrm{x}_{a}}{\partial \mathrm{x}_{a}} \end{array}\right]
∇Fx=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡∂qnb∂fqnb0000∂qnb∂fan0∂ωb∂fqnb0000000∂xg∂fωb∂xg∂fxg0000000∂rn∂frn000000∂vn∂frn∂vn∂fvn00000∂an∂frn∂an∂fvn0000000∂xa∂fan∂xa∂fxa⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
∇ F u = [ 0 0 0 0 ∂ f ω b ∂ y g 0 0 0 0 ∂ f x g ∂ ν x g 0 0 0 0 0 0 0 0 0 0 0 0 ∂ f a n ∂ y a 0 0 0 0 ∂ f x a ∂ ν x a ] \nabla F_{\mathrm{u}}=\left[\begin{array}{ccccccc} 0&0&0&0 \\ \frac{\partial f\omega^{b}}{\partial \mathrm{y}_{g}} & 0 & 0 & 0 \\ 0&\frac{\partial f\mathrm{x}_g}{\partial \mathrm{\nu}_{\mathrm{xg}}}&0&0\\ 0&0&0&0 \\ 0&0&0&0 \\ 0 & 0 & \frac{\partial fa^n}{\partial \mathrm{y}_{a}} & 0 \\ 0 & 0 & 0 & \frac{\partial f\mathrm{x}_{a}}{\partial \nu_{\mathrm{xa}}} \end{array}\right] ∇Fu=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡0∂yg∂fωb0000000∂νxg∂fxg000000000∂ya∂fan0000000∂νxa∂fxa⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
1.4 System update
EKF更新方程如下(作者这里写的形式可能有误):
x
^
k
=
x
^
k
+
1
+
W
(
z
i
−
h
i
)
P
k
=
P
k
+
1
−
W
S
i
W
′
\hat{\mathrm{x}}_{k}=\hat{\mathrm{x}}_{k+1}+W(\mathrm{z}_i-h_i)\\ P_{k}=P_{k+1}-WS_iW'
x^k=x^k+1+W(zi−hi)Pk=Pk+1−WSiW′
其中,
z
i
\mathrm{z}_i
zi 是当前时刻测量值,
h
i
=
h
(
x
^
)
h_i=h(\hat{\mathrm{x}})
hi=h(x^) 是预测测量,
W
W
W 是卡尔曼增益,计算方程为:
W
=
P
k
+
1
∇
H
i
′
S
i
−
1
W=P_{k+1}\nabla H_i^{'}S_i^{-1}
W=Pk+1∇Hi′Si−1,
S
i
S_i
Si为:
S
i
=
∇
H
i
P
k
+
1
∇
H
i
′
+
R
i
S_i=\nabla H_i P_{k+1}\nabla H_i^{'} + R_i
Si=∇HiPk+1∇Hi′+Ri。其中,
∇
H
i
\nabla H_i
∇Hi 是预测测量
h
(
x
^
)
h(\hat{\mathrm{x}})
h(x^) 对系统状态量
x
^
\hat{\mathrm{x}}
x^ 的测量雅可比矩阵,
R
i
R_i
Ri 是测量噪声协方差矩阵。根据以上方程可以发现,只要知道测量值、预测测量、测量雅克比矩阵、噪声
即可进行状态更新。
1.4.1 Dynamical constraints update
首先是非完整性约束,如果 IMU的
x
\mathrm{x}
x 轴和陆地汽车的
x
\mathrm{x}
x 轴平行的话,则载体沿
y
\mathrm{y}
y 和
z
\mathrm{z}
z 轴的速度可以建模为:
[
v
y
b
v
z
b
]
=
[
0
+
ν
v
0
+
ν
v
]
\left[\begin{array}{c} \mathrm{v}_{\mathrm{y}}^{b} \\ \mathrm{v}_{\mathrm{z}}^{b} \end{array}\right]=\left[\begin{array}{c} 0+\nu_{\mathrm{v}} \\ 0+\nu_{\mathrm{v}} \end{array}\right]
[vybvzb]=[0+νv0+νv]
载体速度和导航速度关系为:
[
v
x
b
,
v
y
b
,
v
z
b
]
′
=
[
R
n
b
v
n
]
\left[\begin{array}{c} \mathrm{v}_{\mathrm{x}}^{b},\mathrm{v}_{\mathrm{y}}^{b}, \mathrm{v}_{\mathrm{z}}^{b} \end{array}\right]'=\left[\begin{array}{c} R^{nb}\mathrm{v}^n\end{array}\right]
[vxb,vyb,vzb]′=[Rnbvn]
则现在已知测量值
z
i
=
[
0
,
0
]
′
\mathrm{z}_i=[0,0]'
zi=[0,0]′,预测测量
h
i
=
[
v
y
b
,
v
z
b
]
h_i=[\mathrm{v}_{\mathrm{y}}^b,\mathrm{v}_{\mathrm{z}}^b]
hi=[vyb,vzb],噪声
R
=
I
2
×
2
σ
v
2
R=\mathbf{I}_{2\times2}\sigma_{\mathrm{v}}^2
R=I2×2σv2,雅可比矩阵
为
∇
H
i
=
∂
h
i
/
∂
x
^
\nabla H_i=\partial h_{i} / \partial \hat{\mathrm{x}}
∇Hi=∂hi/∂x^,代入卡尔曼滤波,即可进行状态更新。
1.4.2 Roll and pitch updates
当载体加速度很小时( a b ≈ 0 a^b\approx0 ab≈0),则加速度模型可以写成: y a ≈ g b + ν a \mathrm{y}_a \approx g^b+\nu_a ya≈gb+νa (忽略加速度偏差)。此时,加速度测量值为重力向量在载体坐标的分量,可以根据重力向量进行俯仰角修正。(不过在进行俯仰角修正时,需要使用零速检测算法进行静止判断)。
此时,重力向量测量值为:
h
g
=
R
n
b
[
0
,
0
,
g
c
]
′
h_g=R^{nb}[0,0,g_c]'
hg=Rnb[0,0,gc]′
其中,
g
c
g_c
gc 为重力向量常数,现在已知测量值
z
i
=
y
a
\mathrm{z}_i=\mathrm{y}_a
zi=ya,预测测量
h
i
=
h
g
h_i=h_g
hi=hg,噪声
R
=
I
3
×
3
σ
a
2
R=\mathbf{I}_{3\times3}\sigma_{a}^2
R=I3×3σa2,雅可比矩阵
为
∇
H
i
=
∂
h
g
/
∂
x
^
\nabla H_i=\partial h_{g} / \partial \hat{\mathrm{x}}
∇Hi=∂hg/∂x^,代入卡尔曼滤波,即可进行状态更新。
1.4.3 Position and heading updates
位置更新
,测量值
z
i
=
z
r
n
\mathrm{z}_i=\mathrm{z}_r^n
zi=zrn,预测测量
h
i
=
h
r
=
[
0
3
×
10
,
I
3
×
3
,
0
3
×
9
]
x
^
h_i=h_r=[0_{3\times10},\mathrm{I}_{3\times3},0_{3\times9}]\hat{\mathrm{x}}
hi=hr=[03×10,I3×3,03×9]x^,噪声
R
=
I
3
×
3
σ
r
2
R=\mathbf{I}_{3\times3}\sigma_{r}^2
R=I3×3σr2,雅可比矩阵
为
∇
H
i
=
[
0
3
×
10
,
I
3
×
3
,
0
3
×
9
]
\nabla H_i=[0_{3\times10},\mathrm{I}_{3\times3},0_{3\times9}]
∇Hi=[03×10,I3×3,03×9],代入卡尔曼滤波,即可进行状态更新。
航向角更新
,测量值
z
i
=
z
θ
n
\mathrm{z}_i=\mathrm{z}_\theta^n
zi=zθn,预测测量
h
i
=
h
θ
=
a
t
a
n
2
(
2
(
q
2
q
3
−
q
1
q
4
)
,
1
−
2
(
q
3
2
+
q
4
2
)
)
h_i=h_{\theta}=\mathrm{atan2(2(q_2q_3-q_1q_4),1-2(q_3^2+q_4^2))}
hi=hθ=atan2(2(q2q3−q1q4),1−2(q32+q42)),噪声
R
=
σ
θ
2
R=\sigma_{\theta}^2
R=σθ2,雅可比矩阵
为
∇
H
i
=
∂
h
θ
/
∂
x
^
\nabla H_i=\partial h_{\theta} / \partial \hat{\mathrm{x}}
∇Hi=∂hθ/∂x^,代入卡尔曼滤波,即可进行状态更新。
2. Experimental results
下面是实验参数设置和结果: