多传感器融合定位八-惯性导航解算及误差分析其二
Reference:
文章跳转:
- 多传感器融合定位一-3D激光里程计其一:ICP
- 多传感器融合定位二-3D激光里程计其二:NDT
- 多传感器融合定位三-3D激光里程计其三:点云畸变补偿
- 多传感器融合定位四-3D激光里程计其四:点云线面特征提取
- 多传感器融合定位五-点云地图构建及定位
- 多传感器融合定位六-惯性导航原理及误差分析
- 多传感器融合定位七-惯性导航解算及误差分析其一
- 多传感器融合定位八-惯性导航解算及误差分析其二
- 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
- 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
- 多传感器融合定位十一-基于滤波的融合方法Ⅱ
- 多传感器融合定位十二-基于图优化的建图方法其一
- 多传感器融合定位十三-基于图优化的建图方法其二
- 多传感器融合定位十四-基于图优化的定位方法
- 多传感器融合定位十五-多传感器时空标定(综述)
3. 惯性导航解算
3.1 惯性导航解算概述
-
目的: 利用IMU测量的角速度、加速度,根据上一时刻导航信息,推算出当前时刻导航信息,包括姿态解算、速度解算、位置解算。(根据角速度+加速度,推算位姿+速度)
-
方法: 由姿态、速度、位置的微分方程,推导出它们的解,并转变成离散时间下的近似形式,从而可以在离散时间采样下,完成导航信息求解。
-
优点: IMU不受外界信号干扰,可以得到稳定、平滑的导航结果。
-
缺点: 误差随时间累计,一般时间越长,累计误差越大。因此需要融合,而导航解算(本小节)及其误差分析(下一小节),是融合的基础。
因此通常用于与其他传感器做一个优势互补,IMU 提供一个连续、稳定的位姿解算;而其他传感器提供不随时间发散,但是有噪声的观测。
3.2 姿态更新
3.2.1 基于旋转矩阵的姿态更新
根据前面推导,旋转矩阵的微分方程为:
R
˙
w
b
=
R
w
b
[
ω
]
×
\dot{\boldsymbol{R}}_{w b}=\boldsymbol{R}_{w b}[\boldsymbol{\omega}]_{\times}
R˙wb=Rwb[ω]×根据微分方程的求解方法,可以写出由
k
−
1
k-1
k−1 时刻求解
k
k
k 时刻旋转矩阵的公式为:
R
w
b
k
=
R
w
b
k
−
1
e
∫
t
k
−
1
t
k
[
ω
]
×
d
τ
\boldsymbol{R}_{w b_k}=\boldsymbol{R}_{w b_{k-1}} e^{\int_{t_{k-1}}^{t_k}[\boldsymbol{\omega}]_{\times} d \tau}
Rwbk=Rwbk−1e∫tk−1tk[ω]×dτ指数上的积分结果其实就是
k
−
1
\mathrm{k}-1
k−1 时刻到
k
k
k 时刻之间的相对旋转对应的等效旋转矢量构成的反对称矩阵。
因此上式可以重新写为:
R
w
b
k
=
R
w
b
k
−
1
e
ϕ
×
\boldsymbol{R}_{w b_k}=\boldsymbol{R}_{w b_{k-1}} e^{\boldsymbol{\phi}_\times}
Rwbk=Rwbk−1eϕ×由于反对称矩阵的指数函数前面已经推导完毕,因此上式又可以写为:
R
w
b
k
=
R
w
b
k
−
1
R
b
k
−
1
b
k
\boldsymbol{R}_{w b_k}=\boldsymbol{R}_{w b_{k-1}} \boldsymbol{R}_{b_{k-1} b_k}
Rwbk=Rwbk−1Rbk−1bk其中(罗德里格斯again)
R
b
k
−
1
b
k
=
I
+
sin
ϕ
ϕ
(
ϕ
×
)
+
1
−
cos
ϕ
ϕ
2
(
ϕ
×
)
2
\boldsymbol{R}_{b_{k-1} b_k}=I+\frac{\sin \phi}{\phi}(\phi_\times)+\frac{1-\cos \phi}{\phi^2}(\phi_\times)^2
Rbk−1bk=I+ϕsinϕ(ϕ×)+ϕ21−cosϕ(ϕ×)2可以看出,解算过程中需要求解等效旋转矢量
ϕ
\phi
ϕ,需要借助其微分方程实现。
在旋转矢量的微分方程
ϕ
˙
≈
ω
+
1
2
ϕ
×
ω
\dot{\phi} \approx \omega+\frac{1}{2} \phi \times \omega
ϕ˙≈ω+21ϕ×ω 中(可见2.3节),对于右侧第二项,根据叉乘的定义可知,当叉乘的两个矢量之间方向重合时,则结果为
0
0
0,在
k
−
1
k-1
k−1 时刻到
k
k
k 时刻的极短时间内,可认为接近重合,因此,旋转矢量微分方程可进一步化简为:
ϕ
˙
≈
ω
⇒
ϕ
≈
∫
t
k
−
1
t
k
ω
(
τ
)
d
τ
\dot{\phi} \approx \boldsymbol{\omega} \Rightarrow \phi \approx \int_{t_{k-1}}^{t_k} \boldsymbol{\omega}(\tau) d \tau
ϕ˙≈ω⇒ϕ≈∫tk−1tkω(τ)dτa. 欧拉法
:
ϕ
=
ω
k
−
1
(
t
k
−
t
k
−
1
)
\phi=\boldsymbol{\omega}_{k-1}\left(t_k-t_{k-1}\right)
ϕ=ωk−1(tk−tk−1)
b. 中值法
:
ϕ
=
ω
k
−
1
+
ω
k
2
(
t
k
−
t
k
−
1
)
\phi=\frac{\omega_{k-1}+\omega_k}{2}\left(t_k-t_{k-1}\right)
ϕ=2ωk−1+ωk(tk−tk−1)
中值法精度大于欧拉法,后续本课程解算全部采用中值法进行。
另有四阶龙格库塔法
,在IMU采样频率较高(>100HZ)时,收益不大,此处不做介绍。
3.2.2 基于四元数的姿态更新
根据前面推导,四元数的微分方程为:
q
˙
w
b
=
q
w
b
⊗
1
2
[
0
ω
]
\dot{\boldsymbol{q}}_{w b}=\boldsymbol{q}_{w b} \otimes \frac{1}{2}\left[\begin{array}{c} 0 \\ \boldsymbol{\omega} \end{array}\right]
q˙wb=qwb⊗21[0ω]微分方程带着
⊗
\otimes
⊗ 不太好解,转成以下矩阵形式:
q
˙
w
b
=
1
2
[
0
ω
]
R
q
w
b
\dot{\boldsymbol{q}}_{w b}=\frac{1}{2}\left[\begin{array}{c} 0 \\ \boldsymbol{\omega} \end{array}\right]_R \boldsymbol{q}_{w b}
q˙wb=21[0ω]Rqwb同样按照解微分方程的方法,并对指数项做积分,可以得到:
q
˙
w
b
=
e
1
2
Θ
q
w
b
\dot{\boldsymbol{q}}_{w b}=\mathrm{e}^{\frac{1}{2} \Theta} \boldsymbol{q}_{w b}
q˙wb=e21Θqwb其中:
Θ
=
[
0
−
ϕ
x
−
ϕ
y
−
ϕ
z
ϕ
x
0
ϕ
z
−
ϕ
y
ϕ
y
−
ϕ
z
0
ϕ
x
ϕ
z
ϕ
y
−
ϕ
x
0
]
\boldsymbol{\Theta}=\left[\begin{array}{cccc} 0 & -\phi_x & -\phi_y & -\phi_z \\ \phi_x & 0 & \phi_z & -\phi_y \\ \phi_y & -\phi_z & 0 & \phi_x \\ \phi_z & \phi_y & -\phi_x & 0 \end{array}\right]
Θ=
0ϕxϕyϕz−ϕx0−ϕzϕy−ϕyϕz0−ϕx−ϕz−ϕyϕx0
为了求解该方程,要对指数函数进行泰勒展开,同样包含高次幂,展开方法与前述方法相同(可以自行推导,此处直接给出结论):
e
1
2
Θ
(
T
)
=
I
cos
ϕ
2
+
Θ
ϕ
sin
ϕ
2
\mathrm{e}^{\frac{1}{2} \Theta(T)}=I \cos \frac{\phi}{2}+\frac{\Theta}{\phi} \sin \frac{\phi}{2}
e21Θ(T)=Icos2ϕ+ϕΘsin2ϕ因此有:
q
w
b
k
=
[
I
cos
ϕ
2
+
Θ
ϕ
sin
ϕ
2
]
q
w
b
k
−
1
\boldsymbol{q}_{w b_k}=\left[I \cos \frac{\phi}{2}+\frac{\Theta}{\phi} \sin \frac{\phi}{2}\right] \boldsymbol{q}_{w b_{k-1}}
qwbk=[Icos2ϕ+ϕΘsin2ϕ]qwbk−1由于:
[
cos
ϕ
2
ϕ
ϕ
sin
ϕ
2
]
R
=
[
I
cos
ϕ
2
+
Θ
ϕ
sin
ϕ
2
]
\left[\begin{array}{c} \cos \frac{\phi}{2} \\ \frac{\boldsymbol{\phi}}{\phi} \sin \frac{\phi}{2} \end{array}\right]_R=\left[I \cos \frac{\phi}{2}+\frac{\Theta}{\phi} \sin \frac{\phi}{2}\right]
[cos2ϕϕϕsin2ϕ]R=[Icos2ϕ+ϕΘsin2ϕ]令:
q
b
k
−
1
b
k
=
[
cos
ϕ
2
ϕ
ϕ
sin
ϕ
2
]
\boldsymbol{q}_{b_{k-1} b_k}=\left[\begin{array}{c} \cos \frac{\phi}{2} \\ \frac{\boldsymbol{\phi}}{\phi} \sin \frac{\phi}{2} \end{array}\right]
qbk−1bk=[cos2ϕϕϕsin2ϕ]则
q
w
b
k
=
q
w
b
k
−
1
⊗
q
b
k
−
1
b
k
\boldsymbol{q}_{w b_k}=\boldsymbol{q}_{w b_{k-1}} \otimes \boldsymbol{q}_{b_{k-1} b_k}
qwbk=qwbk−1⊗qbk−1bk
旋转矢量
ϕ
\phi
ϕ 的计算仍采用中值法进行,即
ϕ
=
ω
k
−
1
+
ω
k
2
(
t
k
−
t
k
−
1
)
\boldsymbol{\phi}=\frac{\boldsymbol{\omega}_{k-1}+\omega_k}{2}\left(t_k-t_{k-1}\right)
ϕ=2ωk−1+ωk(tk−tk−1)。注意
q
b
k
−
1
b
k
\boldsymbol{q}_{b_{k-1} b_k}
qbk−1bk 需要归一化,即求 norm()。
3.3 速度更新
速度微分方程为:
v
˙
=
R
w
b
a
−
g
\dot{\boldsymbol{v}}=\boldsymbol{R}_{w b} \boldsymbol{a}-\boldsymbol{g}
v˙=Rwba−g其中
a
=
[
a
x
a
y
a
z
]
\boldsymbol{a}=\left[\begin{array}{lll}a_x & a_y & a_z\end{array}\right]
a=[axayaz] 为测量加速度,
g
=
[
0
0
g
0
]
\boldsymbol{g}=\left[\begin{array}{lll}0 & 0 & g_0\end{array}\right]
g=[00g0] 为重力加速度。
该微分方程的通解形式为:
Δ
v
=
(
R
w
b
a
−
g
)
Δ
t
\Delta \boldsymbol{v}=\left(\boldsymbol{R}_{w b} \boldsymbol{a}-\boldsymbol{g}\right) \Delta t
Δv=(Rwba−g)Δt对应的基于中值法的速度更新形式为:
v
k
=
v
k
−
1
+
(
R
w
b
k
a
k
+
R
w
b
k
−
1
a
k
−
1
2
−
g
)
(
t
k
−
t
k
−
1
)
\boldsymbol{v}_k=\boldsymbol{v}_{k-1}+\left(\frac{\boldsymbol{R}_{w b_k} \boldsymbol{a}_k+\boldsymbol{R}_{w b_{k-1}} \boldsymbol{a}_{k-1}}{2}-\boldsymbol{g}\right)\left(t_k-t_{k-1}\right)
vk=vk−1+(2Rwbkak+Rwbk−1ak−1−g)(tk−tk−1)
3.4 位置更新
位置微分方程为:
p
˙
=
v
\dot{\boldsymbol{p}}=\boldsymbol{v}
p˙=v其通解形式为
Δ
p
=
v
Δ
t
\Delta \boldsymbol{p}=\boldsymbol{v} \Delta t
Δp=vΔt
此处
v
\boldsymbol{v}
v 指的是该时间段内的平均速度,该形式对应的基于中值法的离散形式为:
p
k
=
p
k
−
1
+
v
k
+
v
k
−
1
2
(
t
k
−
t
k
−
1
)
\boldsymbol{p}_k=\boldsymbol{p}_{k-1}+\frac{\boldsymbol{v}_k+\boldsymbol{v}_{k-1}}{2}\left(t_k-t_{k-1}\right)
pk=pk−1+2vk+vk−1(tk−tk−1)另外,通解还可以写为:
Δ
p
=
v
Δ
t
+
1
2
a
Δ
t
2
\Delta \boldsymbol{p}=\boldsymbol{v} \Delta t+\frac{1}{2} \boldsymbol{a} \Delta t^2
Δp=vΔt+21aΔt2需要注意的是,此处的
v
\boldsymbol{v}
v 指的是该时间段起始时刻速度(上下两个
v
\boldsymbol{v}
v表示的东西不太一样),
该形式对应的基于中值法的离散形式为:
p
k
=
p
k
−
1
+
v
k
−
1
(
t
k
−
t
k
−
1
)
+
1
2
(
R
w
b
k
a
k
+
R
w
b
k
−
1
a
k
−
1
2
−
g
)
(
t
k
−
t
k
−
1
)
2
\begin{aligned} \boldsymbol{p}_k= & \boldsymbol{p}_{k-1}+\boldsymbol{v}_{k-1}\left(t_k-t_{k-1}\right) \\ & +\frac{1}{2}\left(\frac{\boldsymbol{R}_{w b_k} \boldsymbol{a}_k+\boldsymbol{R}_{w b_{k-1}} \boldsymbol{a}_{k-1}}{2}-\boldsymbol{g}\right)\left(t_k-t_{k-1}\right)^2 \end{aligned}
pk=pk−1+vk−1(tk−tk−1)+21(2Rwbkak+Rwbk−1ak−1−g)(tk−tk−1)2 这个公式的好处在于,不需要提前求
v
k
v_k
vk,求
v
k
−
1
v_{k-1}
vk−1 就可以了。
3.5 惯性导航解算总结
3.5.1 姿态解算
a. 基于旋转矩阵
R
w
b
k
=
R
w
b
k
−
1
(
I
+
sin
ϕ
ϕ
(
ϕ
×
)
+
1
−
cos
ϕ
ϕ
2
(
ϕ
×
)
2
)
其中
ϕ
=
ω
k
−
1
+
ω
k
2
(
t
k
−
t
k
−
1
)
\boldsymbol{R}_{w b_k}=\boldsymbol{R}_{w b_{k-1}}\left(I+\frac{\sin \phi}{\phi}(\phi \times)+\frac{1-\cos \phi}{\phi^2}(\phi \times)^2\right) \quad \text { 其中 } \boldsymbol{\phi}=\frac{\boldsymbol{\omega}_{k-1}+\omega_k}{2}\left(t_k-t_{k-1}\right)
Rwbk=Rwbk−1(I+ϕsinϕ(ϕ×)+ϕ21−cosϕ(ϕ×)2) 其中 ϕ=2ωk−1+ωk(tk−tk−1)b. 基于四元数
q
w
b
k
=
q
w
b
k
−
1
⊗
[
cos
ϕ
2
ϕ
ϕ
sin
ϕ
2
]
其中
ϕ
=
ω
k
−
1
+
ω
k
2
(
t
k
−
t
k
−
1
)
\boldsymbol{q}_{w b_k}=\boldsymbol{q}_{w b_{k-1}} \otimes\left[\begin{array}{c} \cos \frac{\phi}{2} \\ \frac{\boldsymbol{\phi}}{\phi} \sin \frac{\phi}{2} \end{array}\right] \quad \text { 其中 } \quad \boldsymbol{\phi}=\frac{\boldsymbol{\omega}_{k-1}+\boldsymbol{\omega}_k}{2}\left(t_k-t_{k-1}\right)
qwbk=qwbk−1⊗[cos2ϕϕϕsin2ϕ] 其中 ϕ=2ωk−1+ωk(tk−tk−1)
3.5.2 速度解算
v k = v k − 1 + ( R w b k a k + R w b k − 1 a k − 1 2 − g ) ( t k − t k − 1 ) \boldsymbol{v}_k=\boldsymbol{v}_{k-1}+\left(\frac{\boldsymbol{R}_{\boldsymbol{w} b_k} \boldsymbol{a}_k+\boldsymbol{R}_{\boldsymbol{w} b_{k-1}} \boldsymbol{a}_{k-1}}{2}-\boldsymbol{g}\right)\left(t_k-t_{k-1}\right) vk=vk−1+(2Rwbkak+Rwbk−1ak−1−g)(tk−tk−1)
3.5.3 位置解算
p k = p k − 1 + v k + v k − 1 2 ( t k − t k − 1 ) 或 p k = p k − 1 + v k − 1 ( t k − t k − 1 ) + 1 2 ( R w b k a k + R w b k − 1 a k − 1 2 − g ) ( t k − t k − 1 ) 2 \boldsymbol{p}_k=\boldsymbol{p}_{k-1}+\frac{\boldsymbol{v}_k+\boldsymbol{v}_{k-1}}{2}\left(t_k-t_{k-1}\right) \text { 或 } \boldsymbol{p}_k=\boldsymbol{p}_{k-1}+\boldsymbol{v}_{k-1}\left(t_k-t_{k-1}\right)+\frac{1}{2}\left(\frac{\boldsymbol{R}_{w b_k} \boldsymbol{a}_k+\boldsymbol{R}_{w b_{k-1}} \boldsymbol{a}_{k-1}}{2}-\boldsymbol{g}\right)\left(t_k-t_{k-1}\right)^2 pk=pk−1+2vk+vk−1(tk−tk−1) 或 pk=pk−1+vk−1(tk−tk−1)+21(2Rwbkak+Rwbk−1ak−1−g)(tk−tk−1)2
4. 惯性导航误差分析
4.1 误差方程推导方法
误差分析的输出形式为误差方程。
误差方程
:状态量(速度误差、位置误差、姿态误差、bias误差等)误差形式的表示。误差方程及其推导方法,是后续滤波、图优化等融合方案的基础,是重中之重。误差方程的推导有固定的套路,举例说明具体步骤:
- 写出不考虑误差时的微分方程
z ˙ = x + y \dot{z}=x+y z˙=x+y - 写出考虑误差时的微分方程
z ~ ˙ = x ~ + y ~ \dot{\tilde{z}}=\tilde{x}+\tilde{y} z~˙=x~+y~ - 写出真实值与理想值之间的关系(这里的
δ
\delta
δ 表示误差量)
z ~ = z + δ z x ~ = x + δ x y ~ = y + δ y \begin{array}{l} \tilde{z}=z+\delta z \\ \tilde{x}=x+\delta x \\ \tilde{y}=y+\delta y \end{array} z~=z+δzx~=x+δxy~=y+δy - 把 3) 中的关系,带入 2)
z ˙ + δ z ˙ = x + δ x + y + δ y \dot{z}+\delta \dot{z}=x+\delta x+y+\delta y z˙+δz˙=x+δx+y+δy - 把 1) 中的关系,带入 4)
x + y + δ z ˙ = x + δ x + y + δ y x+y+\delta \dot{z}=x+\delta x+y+\delta y x+y+δz˙=x+δx+y+δy - 化简方程
δ z ˙ = δ x + δ y \delta \dot{z}=\delta x+\delta y δz˙=δx+δy
我们把这个简单的例子抽象成六个步骤,后面无论有多么复杂的问题,都可以用这六个步骤解决。
4.2 姿态误差方程
-
写出不考虑误差的微分方程
q ˙ t = 1 2 q t ⊗ [ 0 ω t − b ω t ] \dot{\boldsymbol{q}}_t=\frac{1}{2} \boldsymbol{q}_t \otimes\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_t-\boldsymbol{b}_{\omega_t} \end{array}\right] q˙t=21qt⊗[0ωt−bωt]这里考虑了零偏 b ω t \boldsymbol{b}_{\omega_t} bωt,所以在测量的 ω t \boldsymbol{\omega}_t ωt 上,要减去一个 b ω t \boldsymbol{b}_{\omega_t} bωt。 -
写出考虑误差的微分方程
q t ~ ˙ = 1 2 q ~ t ⊗ [ 0 ω ~ t − b ~ ω t ] \dot{\tilde{{\boldsymbol{q}}_t}}=\frac{1}{2} \tilde{\boldsymbol{q}}_t \otimes\left[\begin{array}{c} 0 \\ \tilde{\boldsymbol{\omega}}_t-\tilde{\boldsymbol{b}}_{\omega_t} \end{array}\right] qt~˙=21q~t⊗[0ω~t−b~ωt]每个量上面都有误差。 -
写出带误差的值与理想值之间的关系
q ~ t = q t ⊗ δ q ω ~ t = ω t + n ω b ~ ω t = b ω t + δ b ω \begin{array}{l} \tilde{\boldsymbol{q}}_t=\boldsymbol{q}_t \otimes \delta \boldsymbol{q} \\ \tilde{\boldsymbol{\omega}}_t=\boldsymbol{\omega}_t+\boldsymbol{n}_\omega \\ \tilde{\boldsymbol{b}}_{\omega_t}=\boldsymbol{b}_{\omega_t}+\delta \boldsymbol{b}_\omega \end{array} q~t=qt⊗δqω~t=ωt+nωb~ωt=bωt+δbω其中 n ω \boldsymbol{n}_\omega nω 为陀螺仪白噪声。其中
δ q = [ cos ( ∣ δ θ ∣ 2 ) δ θ ∣ δ θ ∣ sin ( ∣ δ θ ∣ 2 ) ] ≈ [ 1 δ θ 2 ] \delta \boldsymbol{q}=\left[\begin{array}{c} \cos \left(\frac{|\delta \theta|}{2}\right) \\ \frac{\delta \boldsymbol{\theta}}{|\delta \theta|} \sin \left(\frac{|\delta \theta|}{2}\right) \end{array}\right] \approx\left[\begin{array}{c} 1 \\ \frac{\delta \boldsymbol{\theta}}{2} \end{array}\right] δq= cos(2∣δθ∣)∣δθ∣δθsin(2∣δθ∣) ≈[12δθ] δ θ \delta \boldsymbol{\theta} δθ 是姿态误差对应的旋转矢量
(有时被称为失准角
),因为 δ θ \delta \boldsymbol{\theta} δθ 是个非常小的量,所以 cos ( ∣ δ θ ∣ 2 ) \cos \left(\frac{|\delta \theta|}{2}\right) cos(2∣δθ∣) 约等于 1 1 1; sin ( ∣ δ θ ∣ 2 ) \sin \left(\frac{|\delta \theta|}{2}\right) sin(2∣δθ∣) 约等于它本身。 δ θ \delta \boldsymbol{\theta} δθ 代表的是一个旋转的大小,需要注意的是,它是用等效旋转矢量表示的,它绕固定轴旋转,旋转一定角度,用一个矢量表示,矢量方向就是轴的方向,矢量的大小就是轴的大小。 -
将 3) 中的关系带入 2)
( q t ⊗ δ q ) ˙ = 1 2 q t ⊗ δ q ⊗ [ 0 ω t + n ω − b ω t − δ b ω ] \dot{\left(\boldsymbol{q}_t {\otimes} \delta \boldsymbol{q}\right)}=\frac{1}{2} \boldsymbol{q}_t \otimes \delta \boldsymbol{q} \otimes\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_t+\boldsymbol{n}_\omega-\boldsymbol{b}_{\omega_t}-\delta \boldsymbol{b}_\omega \end{array}\right] (qt⊗δq)˙=21qt⊗δq⊗[0ωt+nω−bωt−δbω] -
把 1) 中的关系带入 4)
( q t ⊗ δ q ) ˙ = q ˙ t ⊗ δ q + q t ⊗ δ q ˙ = 1 2 q t ⊗ [ 0 ω t − b ω t ] ⊗ δ q + q t ⊗ δ q ˙ = 1 2 q t ⊗ δ q ⊗ [ 0 ω t + n ω − b ω t − δ b ω ] \begin{aligned} & \dot{\left(\boldsymbol{q}_t {\otimes} \delta \boldsymbol{q}\right)} \\ = & \dot{\boldsymbol{q}}_t \otimes \delta \boldsymbol{q}+\boldsymbol{q}_t \otimes \delta \dot{\boldsymbol{q}} \\ = & \frac{1}{2} \boldsymbol{q}_t \otimes\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_t-\boldsymbol{b}_{\omega_t} \end{array}\right] \otimes \delta \boldsymbol{q}+\boldsymbol{q}_t \otimes \delta \dot{\boldsymbol{q}} \\ = & \frac{1}{2} \boldsymbol{q}_t \otimes \delta \boldsymbol{q} \otimes\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_t+\boldsymbol{n}_\omega-\boldsymbol{b}_{\omega_t}-\delta \boldsymbol{b}_\omega \end{array}\right] \end{aligned} ===(qt⊗δq)˙q˙t⊗δq+qt⊗δq˙21qt⊗[0ωt−bωt]⊗δq+qt⊗δq˙21qt⊗δq⊗[0ωt+nω−bωt−δbω]最后一行为公式 4) 所得 -
化简方程
首先把 5) 中最后两行左乘 ( q t ) − 1 \left(\boldsymbol{q}_t\right)^{-1} (qt)−1 并移项可得:
δ q ˙ = 1 2 δ q ⊗ [ 0 ω t + n ω − b ω t − δ b ω ] − 1 2 [ 0 ω t − b ω t ] ⊗ δ q \begin{aligned} \delta \dot{\boldsymbol{q}}= & \frac{1}{2} \delta \boldsymbol{q} \otimes\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_t+\boldsymbol{n}_\omega-\boldsymbol{b}_{\omega_t}-\delta \boldsymbol{b}_\omega \end{array}\right] \\ & -\frac{1}{2}\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_t-\boldsymbol{b}_{\omega_t} \end{array}\right] \otimes \delta \boldsymbol{q} \end{aligned} δq˙=21δq⊗[0ωt+nω−bωt−δbω]−21[0ωt−bωt]⊗δq根据前面所讲,四元数相乘可以转换成矩阵与向量相乘:
p ⊗ q = [ p ] L q = [ q ] R p [ p ] L = [ p 0 − p v T p v p 0 I + [ p v ] × ] [ q ] R = [ q 0 − q v T q v q 0 I − [ q v ] × ] \begin{array}{c} &\boldsymbol{p} \otimes \boldsymbol{q}=[\boldsymbol{p}]_L \boldsymbol{q}=[\boldsymbol{q}]_R \boldsymbol{p} \\ &{[\boldsymbol{p}]_L=\left[\begin{array}{cc} p_0 & -\boldsymbol{p}_v^{\mathrm{T}} \\ \boldsymbol{p}_v & p_0 \boldsymbol{I}+\left[\boldsymbol{p}_v\right]_{\times} \end{array}\right]} \\ &{[\boldsymbol{q}]_R=\left[\begin{array}{cc} q_0 & -\boldsymbol{q}_v^{\mathrm{T}} \\ \boldsymbol{q}_v & q_0 \boldsymbol{I}-\left[\boldsymbol{q}_v\right]_{\times} \end{array}\right]} \end{array} p⊗q=[p]Lq=[q]Rp[p]L=[p0pv−pvTp0I+[pv]×][q]R=[q0qv−qvTq0I−[qv]×]令:
ω 1 = ω t + n ω − b ω t − δ b ω ω 2 = ω t − b ω \begin{array}{l} \boldsymbol{\omega}_1=\boldsymbol{\omega}_t+\boldsymbol{n}_\omega-\boldsymbol{b}_{\omega_t}-\delta \boldsymbol{b}_\omega \\ \boldsymbol{\omega}_2=\boldsymbol{\omega}_t-\boldsymbol{b}_\omega \end{array} ω1=ωt+nω−bωt−δbωω2=ωt−bω则:
δ q ˙ = 1 2 [ 0 ω 1 ] R δ q − 1 2 [ 0 ω 2 ] L δ q = 1 2 [ 0 ( ω 2 − ω 1 ) T ( ω 1 − ω 2 ) − [ ω 1 + ω 2 ] × ] δ q \begin{aligned} \delta \dot{\boldsymbol{q}} & =\frac{1}{2}\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_1 \end{array}\right]_R \delta \boldsymbol{q}-\frac{1}{2}\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_2 \end{array}\right]_L \delta \boldsymbol{q} \\ & =\frac{1}{2}\left[\begin{array}{cc} 0 & \left(\boldsymbol{\omega}_2-\boldsymbol{\omega}_1\right)^T \\ \left(\boldsymbol{\omega}_1-\boldsymbol{\omega}_2\right) & -\left[\boldsymbol{\omega}_1+\boldsymbol{\omega}_2\right]_{\times} \end{array}\right] \delta \boldsymbol{q} \end{aligned} δq˙=21[0ω1]Rδq−21[0ω2]Lδq=21[0(ω1−ω2)(ω2−ω1)T−[ω1+ω2]×]δq由于融合时,状态量中往往不是使用四元数,而是使用失准角,因此要把上式转成失准角的微分形式。
由于:
δ q ≈ [ 1 δ θ 2 ] ⇒ δ q ˙ ≈ [ 0 δ θ ˙ 2 ] \delta \boldsymbol{q} \approx\left[\begin{array}{c} 1 \\ \frac{\delta \boldsymbol{\theta}}{2} \end{array}\right] \Rightarrow \delta \dot{q} \approx\left[\begin{array}{c} 0 \\ \frac{\delta \dot{\boldsymbol{\theta}}}{2} \end{array}\right] δq≈[12δθ]⇒δq˙≈[02δθ˙]把它代入上式,又可以得到:
δ θ ˙ = − [ ω 1 + ω 2 ] × δ θ 2 + ( ω 1 − ω 2 ) = − [ 2 ω t + n ω − 2 b ω t − δ b ω ] × δ θ 2 + n ω − δ b ω \begin{aligned} \delta \dot{\boldsymbol{\theta}}= & -\left[\boldsymbol{\omega}_1+\boldsymbol{\omega}_2\right]_{\times} \frac{\delta \boldsymbol{\theta}}{2}+\left(\boldsymbol{\omega}_1-\boldsymbol{\omega}_2\right) \\ = & -\left[2 \boldsymbol{\omega}_t+\boldsymbol{n}_\omega-2 \boldsymbol{b}_{\omega_t}-\delta \boldsymbol{b}_\omega\right]_{\times} \frac{\delta \boldsymbol{\theta}}{2} \\ & +\boldsymbol{n}_\omega-\delta \boldsymbol{b}_\omega \end{aligned} δθ˙==−[ω1+ω2]×2δθ+(ω1−ω2)−[2ωt+nω−2bωt−δbω]×2δθ+nω−δbω忽略其中的二阶小项,可得:
δ θ ˙ = − [ ω t − b ω t ] × δ θ + n ω − δ b ω \delta \dot{\boldsymbol{\theta}}=-\left[\boldsymbol{\omega}_t-\boldsymbol{b}_{\omega_t}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{n}_\omega-\delta \boldsymbol{b}_\omega δθ˙=−[ωt−bωt]×δθ+nω−δbω
4.3 速度误差方程
- 写出不考虑误差时的微分方程
v ˙ t = R t ( a t − b a t ) \dot{\boldsymbol{v}}_t=\boldsymbol{R}_t\left(\boldsymbol{a}_t-\boldsymbol{b}_{a_t}\right) v˙t=Rt(at−bat) - 写出考虑误差时的微分方程
v ~ ˙ t = R ~ t ( a ~ t − b ~ a t ) \dot{\tilde{\boldsymbol{v}}}_t=\tilde{\boldsymbol{R}}_{\boldsymbol{t}}\left(\tilde{\boldsymbol{a}}_t-\tilde{\boldsymbol{b}}_{a_t}\right) v~˙t=R~t(a~t−b~at) - 写出真实值和理想值之间的关系
v ~ = v + δ v R ~ t = R t exp ( [ δ θ ] × ) ≈ R t ( I + [ δ θ ] × ) a ~ t = a t + n a b ~ a t = b a t + δ b a \begin{aligned} \tilde{\boldsymbol{v}} & =\boldsymbol{v}+\delta \boldsymbol{v} \\ \tilde{\boldsymbol{R}}_t & =\boldsymbol{R}_t \exp \left([\delta \boldsymbol{\theta}]_{\times}\right) \\ & \approx \boldsymbol{R}_t\left(\boldsymbol{I}+[\delta \boldsymbol{\theta}]_{\times}\right) \\ \tilde{\boldsymbol{a}}_t & =\boldsymbol{a}_t+\boldsymbol{n}_a \\ \tilde{\boldsymbol{b}}_{a_t} & =\boldsymbol{b}_{a_t}+\delta \boldsymbol{b}_a \end{aligned} v~R~ta~tb~at=v+δv=Rtexp([δθ]×)≈Rt(I+[δθ]×)=at+na=bat+δba其中 n a \boldsymbol{n}_a na 为加速度计白噪声。 - 将3)中的关系带入2)
v ˙ + δ v ˙ = R t ( I + [ δ θ ] × ) ( a t + n a − b a t − δ b a ) \begin{aligned} & \dot{\boldsymbol{v}}+\delta \dot{\boldsymbol{v}} \\ = & \boldsymbol{R}_t\left(\boldsymbol{I}+[\delta \boldsymbol{\theta}]_{\times}\right)\left(\boldsymbol{a}_t+\boldsymbol{n}_a-\boldsymbol{b}_{a_t}-\delta \boldsymbol{b}_a\right) \end{aligned} =v˙+δv˙Rt(I+[δθ]×)(at+na−bat−δba) - 将1)中的关系带入4)
R t ( a t − b a t ) + δ v ˙ = R t ( I + [ δ ] × ) ( a t + n a − b a t − δ b a ) \begin{aligned} & \boldsymbol{R}_t\left(\boldsymbol{a}_t-\boldsymbol{b}_{a_t}\right)+\delta \dot{\boldsymbol{v}} \\ = & \boldsymbol{R}_t\left(\boldsymbol{I}+[\delta]_{\times}\right)\left(\boldsymbol{a}_t+\boldsymbol{n}_a-\boldsymbol{b}_{a_t}-\delta \boldsymbol{b}_a\right) \end{aligned} =Rt(at−bat)+δv˙Rt(I+[δ]×)(at+na−bat−δba) - 化简方程(忽略二阶小项)
δ v ˙ = R t [ δ θ ] × ( a t − b a t ) + R t ( n a − δ b a ) = − R t [ a t − b a t ] × δ θ + R t ( n a − δ b a ) \begin{aligned} & \delta \dot{\boldsymbol{v}} \\ = & \boldsymbol{R}_t[\delta \boldsymbol{\theta}]_{\times}\left(\boldsymbol{a}_t-\boldsymbol{b}_{a_t}\right)+\boldsymbol{R}_t\left(\boldsymbol{n}_a-\delta \boldsymbol{b}_a\right) \\ = & -\boldsymbol{R}_t\left[\boldsymbol{a}_t-\boldsymbol{b}_{a_t}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{R}_t\left(\boldsymbol{n}_a-\delta \boldsymbol{b}_a\right) \end{aligned} ==δv˙Rt[δθ]×(at−bat)+Rt(na−δba)−Rt[at−bat]×δθ+Rt(na−δba)最后一步使用的性质为: [ V 1 ] × V 2 = − [ V 2 ] × V 1 [V_1]_\times V_2=-[V_2]_\times V_1 [V1]×V2=−[V2]×V1
4.4 位置误差方程
- 写出不考虑误差时的微分方程:
p ˙ = v \dot{p}=\boldsymbol{v} p˙=v - 写出考虑误差时的微分方程:
p ~ ˙ = v ~ \dot{\tilde{\boldsymbol{p}}}=\tilde{\boldsymbol{v}} p~˙=v~ - 写出真实值与理想值之间的关系:
v ~ = v + δ v p ~ = p + δ p \begin{array}{l} \tilde{\boldsymbol{v}}=\boldsymbol{v}+\delta \boldsymbol{v} \\ \tilde{\boldsymbol{p}}=\boldsymbol{p}+\delta \boldsymbol{p} \end{array} v~=v+δvp~=p+δp - 将 3) 中的关系带入 2):
p ˙ + δ p ˙ = v + δ v \dot{\boldsymbol{p}}+\delta \dot{\boldsymbol{p}}=\boldsymbol{v}+\delta \boldsymbol{v} p˙+δp˙=v+δv - 把 1) 中的关系带入 4):
v + δ p ˙ = v + δ v \boldsymbol{v}+\delta \dot{\boldsymbol{p}}=\boldsymbol{v}+\delta \boldsymbol{v} v+δp˙=v+δv - 化简方程:
δ p ˙ = δ v \delta \dot{\boldsymbol{p}}=\delta \boldsymbol{v} δp˙=δv
4.5 bias误差方程
在IMU精度较高时,bias认为是常值,即有:
δ
b
˙
a
=
0
δ
b
˙
ω
=
0
\begin{aligned} \delta \dot{\boldsymbol{b}}_a & =0 \\ \delta \dot{\boldsymbol{b}}_\omega & =0 \end{aligned}
δb˙aδb˙ω=0=0但自动驾驶和机器人领域所用的mems多数达不到这种精度,因为角速度随机游走和加速度随机游走较大,因此误差方程常写为:
δ
b
˙
a
=
n
b
a
δ
b
˙
ω
=
n
b
ω
\begin{aligned} \delta \dot{\boldsymbol{b}}_a & =\boldsymbol{n}_{b_a} \\ \delta \dot{\boldsymbol{b}}_\omega & =\boldsymbol{n}_{b \omega} \end{aligned}
δb˙aδb˙ω=nba=nbω其中
n
b
a
\boldsymbol{n}_{b_a}
nba 和
n
b
ω
\boldsymbol{n}_{b \omega}
nbω 分别为加速度计和陀螺仪的随机游走对应的白噪声。
以上两种形式都很常见,没有绝对的对和错。
4.6 惯性导航误差分析总结
δ p ˙ = δ v δ v ˙ = − R t [ a t − b a t ] × δ θ + R t ( n a − δ b a ) δ θ ˙ = − [ ω t − b ω t ] × δ θ + n ω − δ b ω δ b ˙ a = n b a 或 δ b ˙ a = 0 δ b ˙ ω = n b ω 或 δ b ˙ ω = 0 \begin{array}{l} \delta \dot{\boldsymbol{p}}=\delta \boldsymbol{v} \\ \delta \dot{\boldsymbol{v}}=-\boldsymbol{R}_t\left[\boldsymbol{a}_t-\boldsymbol{b}_{a_t}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{R}_t\left(\boldsymbol{n}_a-\delta \boldsymbol{b}_a\right) \\ \delta \dot{\boldsymbol{\theta}}=-\left[\boldsymbol{\omega}_t-\boldsymbol{b}_{\omega_t}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{n}_\omega-\delta \boldsymbol{b}_\omega \\ \delta \dot{\boldsymbol{b}}_a=\boldsymbol{n}_{b_a} \quad \text { 或 } \quad \delta \dot{\boldsymbol{b}}_a=0 \\ \delta \dot{\boldsymbol{b}}_\omega=\boldsymbol{n}_{b \omega} \quad \text {或 } \quad \delta \dot{\boldsymbol{b}}_\omega=0 \\ \end{array} δp˙=δvδv˙=−Rt[at−bat]×δθ+Rt(na−δba)δθ˙=−[ωt−bωt]×δθ+nω−δbωδb˙a=nba 或 δb˙a=0δb˙ω=nbω或 δb˙ω=0