【运动传感器】Madgwick算法（下）
【运动传感器】Madgwick算法（下）
在上篇中，讲解了分别从角速度计、加速度计、磁力计估计姿态的方法。本篇文章讲解他们的融合，对误差的处理，以及标定实验。
融合
这一部分在文章中称为filter。
在上一篇文章中，我们能够通过角速度计的读数
ω
\omega
ω，加速度计或磁力计的读数
d
S
d_S
dS在每个采样间隔更新姿态四元数
q
q
q：<br>
q
t
+
1
=
q
t
+
1
2
q
t
×
ω
t
Δ
t
q_{t+1} = q_{t} + \frac{1}{2}q_t \times \omega_t \Delta t
qt+1=qt+21qt×ωtΔt
q
t
+
1
=
q
t
−
μ
∇
f
∣
∣
∇
f
∣
∣
q_{t+1}=q_t - \mu \frac{\nabla f}{||\nabla f||}
qt+1=qt−μ∣∣∇f∣∣∇f
其中
f
f
f是姿态拟合误差，可以通过当前姿态
q
q
q和读数
d
S
d_S
dS计算得到。<br> 两者融合有：
q
t
+
1
=
q
t
+
1
2
q
t
×
ω
t
Δ
t
−
β
∇
f
∣
∣
∇
f
∣
∣
Δ
t
q_{t+1} = q_{t} + \frac{1}{2}q_t \times \omega_t \Delta t - \beta \frac{\nabla f}{||\nabla f||}\Delta_t
qt+1=qt+21qt×ωtΔt−β∣∣∇f∣∣∇fΔt
增量由两部分构成：
第一部分
1
2
q
t
×
ω
t
\frac{1}{2}q_t \times \omega_t
21qt×ωt为通过角速度计获取的结果。<br> 如果角速度计是完美的，则只需这部分即可。
第二部分
∇
f
∣
∣
∇
f
∣
∣
\frac{\nabla f}{||\nabla f||}
∣∣∇f∣∣∇f为通过加速度计/磁力计获取的结果。其权重
β
\beta
β表示角速度计的误差。<br> 如果误差为0，则不需要此项；否则，因为角速度计不完美，当前姿态
q
q
q有误差，导致拟合误差
f
f
f没有达到极小，
∇
f
\nabla f
∇f不为0矢量，需要沿着误差下降方向对姿态
q
q
q进行修正，修正幅度和角速度计误差成正比。
本部分证明原文参见Madgwick Internal Report3.3节。原始证明感觉就是凑的尚存疑，此处根据个人理解书写。
磁力计偏转
根据前文推导，世界坐标系下磁力矢量
d
E
=
[
0
,
b
x
,
0
,
b
z
]
d_E = [0,b_x,0,b_z]
dE=[0,bx,0,bz]和传感器坐标系下磁力矢量
d
S
=
[
0
,
m
x
,
m
y
,
m
z
]
d_S = [0,m_x,m_y,m_z]
dS=[0,mx,my,mz]应满足如下关系：
[
0
,
b
x
,
0
,
b
z
]
T
=
q
×
[
0
,
m
x
,
m
y
,
m
z
]
T
×
q
−
1
[0,b_x,0,b_z]^T = q \times [0,m_x,m_y,m_z]^T \times q^{-1}
[0,bx,0,bz]T=q×[0,mx,my,mz]T×q−1
但由于误差存在（电子仪器干扰，磁铁干扰，地磁偏转等），变换后的y坐标可能不为零，得到结果为
[
0
,
b
x
′
,
b
y
′
,
b
z
′
]
[0,b_x',b_y',b_z']
[0,bx′,by′,bz′]。
[
0
,
b
x
′
,
b
y
′
,
b
z
′
]
T
=
q
×
[
0
,
m
x
,
m
y
,
m
z
]
T
×
q
−
1
[0,b_x',b_y',b_z']^T = q \times [0,m_x,m_y,m_z]^T \times q^{-1}
[0,bx′,by′,bz′]T=q×[0,mx,my,mz]T×q−1
在计算误差
e
e
e和Jacobian矩阵
J
J
J时，可以人工强制消除此误差：
d
E
=
[
0
,
b
x
′
2
+
b
y
′
2
,
0
,
b
z
′
]
d_E = [0,\sqrt{b_x'^2+b_y'^2},0,b_z']
dE=[0,bx′2+by′2
<svg width="400em" height="1.8800000000000001em" viewbox="0 0 400000 1944" preserveaspectratio="xMinYMin slice">
<path d="M1001,80H400000v40H1013.1s-83.4,268,-264.1,840c-180.7,
572,-277,876.3,-289,913c-4.7,4.7,-12.7,7,-24,7s-12,0,-12,0c-1.3,-3.3,-3.7,-11.7, -7,-25c-35.3,-125.3,-106.7,-373.3,-214,-744c-10,12,-21,25,-33,39s-32,39,-32,39 c-6,-5.3,-15,-14,-27,-26s25,-30,25,-30c26.7,-32.7,52,-63,76,-91s52,-60,52,-60 s208,722,208,722c56,-175.3,126.3,-397.3,211,-666c84.7,-268.7,153.8,-488.2,207.5, -658.5c53.7,-170.3,84.5,-266.8,92.5,-289.5c4,-6.7,10,-10,18,-10z M1001 80H400000v40H1013z"></path> </svg>,0,bz′]
这样做的另一个好处是，x轴正方向不必强制指北，指向平面任意方向即可。
角速度计误差
存疑：这部分道理不明，作者网站的源码中也没有实现，只简略记下流程。
角速度计误差由两部分组成
漂移（bias drift）：
ω
ζ
˙
\dot{\omega_{\zeta}}
ωζ˙，单位rad/s/s。<br> 增益（error）：
ω
β
，
\omega_{\beta}，
ωβ，单位rad/s。
角速度计的三个示数和真实值的关系为：
ω
t
r
u
e
=
ω
β
ω
r
e
a
d
+
ω
ζ
˙
\omega_{true} = \omega_{\beta} \omega_{read} + \dot{\omega_{\zeta}}
ωtrue=ωβωread+ωζ˙
两个传感器参数可以通过后面的标定算法测量而知。
进一步可以得到两个系统参数：
β
=
∣
∣
[
0
,
ω
β
,
ω
β
,
ω
β
]
∣
∣
=
3
4
ω
β
\beta = ||[0,\omega_{\beta},\omega_{\beta},\omega_{\beta}]|| = \sqrt{\frac{3}{4}}\omega_{\beta}
β=∣∣[0,ωβ,ωβ,ωβ]∣∣=43
<svg width="400em" height="2.48em" viewbox="0 0 400000 2592" preserveaspectratio="xMinYMin slice">
<path d="M424,2478c-1.3,-0.7,-38.5,-172,-111.5,-514c-73,
-342,-109.8,-513.3,-110.5,-514c0,-2,-10.7,14.3,-32,49c-4.7,7.3,-9.8,15.7,-15.5, 25c-5.7,9.3,-9.8,16,-12.5,20s-5,7,-5,7c-4,-3.3,-8.3,-7.7,-13,-13s-13,-13,-13, -13s76,-122,76,-122s77,-121,77,-121s209,968,209,968c0,-2,84.7,-361.7,254,-1079 c169.3,-717.3,254.7,-1077.7,256,-1081c4,-6.7,10,-10,18,-10H400000v40H1014.6 s-87.3,378.7,-272.6,1166c-185.3,787.3,-279.3,1182.3,-282,1185c-2,6,-10,9,-24,9 c-8,0,-12,-0.7,-12,-2z M1001 80H400000v40H1014z"></path> </svg>ωβ
ζ
=
∣
∣
[
0
,
ω
ζ
˙
,
ω
ζ
˙
,
ω
ζ
˙
]
∣
∣
=
3
4
ω
ζ
˙
\zeta = ||[0,\dot{\omega_{\zeta}},\dot{\omega_{\zeta}},\dot{\omega_{\zeta}}]||=\sqrt{\frac{3}{4}}\dot{\omega_{\zeta}}
ζ=∣∣[0,ωζ˙,ωζ˙,ωζ˙]∣∣=43
<svg width="400em" height="2.48em" viewbox="0 0 400000 2592" preserveaspectratio="xMinYMin slice">
<path d="M424,2478c-1.3,-0.7,-38.5,-172,-111.5,-514c-73,
-342,-109.8,-513.3,-110.5,-514c0,-2,-10.7,14.3,-32,49c-4.7,7.3,-9.8,15.7,-15.5, 25c-5.7,9.3,-9.8,16,-12.5,20s-5,7,-5,7c-4,-3.3,-8.3,-7.7,-13,-13s-13,-13,-13, -13s76,-122,76,-122s77,-121,77,-121s209,968,209,968c0,-2,84.7,-361.7,254,-1079 c169.3,-717.3,254.7,-1077.7,256,-1081c4,-6.7,10,-10,18,-10H400000v40H1014.6 s-87.3,378.7,-272.6,1166c-185.3,787.3,-279.3,1182.3,-282,1185c-2,6,-10,9,-24,9 c-8,0,-12,-0.7,-12,-2z M1001 80H400000v40H1014z"></path> </svg>ωζ˙
其中
β
\beta
β就是前节融合算法中，加速度计/磁力计的权重参数。<br> 而
ζ
\zeta
ζ则用来直接修正加速度计中的
ω
\omega
ω。
ω
′
=
ω
−
ζ
∫
2
q
t
−
1
×
∇
f
∣
∣
∇
f
∣
∣
d
t
\omega' = \omega - \zeta \int 2q_t^{-1}\times \frac{\nabla f}{||\nabla f||}dt
ω′=ω−ζ∫2qt−1×∣∣∇f∣∣∇fdt
其中
∇
f
∣
∣
∇
f
∣
∣
\frac{\nabla f}{||\nabla f||}
∣∣∇f∣∣∇f表示
q
q
q的误差随时间变化率。经过反变换，积分号内部分为
ω
\omega
ω的误差随时间变化率。其对时间的积分表示均值不为零的部分。
标定
使用8台高速（120Hz）红外摄像机，拍摄带有红外标记（杆顶端的亮点）的支架，获取标定支架（三条杆）上传感器（橘黄盒子）的真实姿态。
涉及四个坐标系统：世界（E），摄像机（C），支架（M），传感器（S）。各个坐标系统之间的转换按如下方式确定。
世界(E)
↔
\leftrightarrow
↔摄像机©
使用棉线悬挂重物，垂线方向的单位向量
c
Z
c_Z
cZ记为**摄像机坐标系**下**世界坐标系**的z轴。<br> <img src="https://img-blog.csdnimg.cn/img_convert/0c68c3a0dbf3bcd7b82757218bc349eb.png" alt="这里写图片描述">
使用棉线悬挂一根磁铁杆，杆方向的单位向量
c
X
c_X
cX记为**摄像机坐标系**下**世界坐标系**的x轴。<br> <img src="https://img-blog.csdnimg.cn/img_convert/45adac8a96976f1b6b6d52d3572977f4.png" alt="这里写图片描述">
垂直轴
c
Z
c_Z
cZ一般较准确，但由于摩擦力等因素，水平轴
c
X
c_X
cX可能和z不垂直（不够平）。用以下方法进行修正：<br>
c
X
=
c
X
−
(
c
X
T
c
Z
)
c
Z
c_X = c_X - (c_X^Tc_Z) c_Z
cX=cX−(cXTcZ)cZ
c
X
=
c
X
/
∣
∣
c
X
∣
∣
c_X = c_X / ||c_X||
cX=cX/∣∣cX∣∣<br> 即，将
c
X
c_X
cX向z轴垂直方向投影，之后再归一化。 y轴由xz轴叉乘得到。
2
×
3
2\times3
2×3矩阵
R
=
[
c
X
,
c
Y
,
c
Z
]
R=[c_X,c_Y,c_Z]
R=[cX,cY,cZ]即为从摄像机到世界的旋转矩阵，可以将其转换为四元数
E
C
q
^C_Eq
ECq。
摄像机©
↔
\leftrightarrow
↔支架(M)
设拍摄图像中，三个杆方向的单位向量（参看前图）为
c
X
,
c
Y
,
c
Z
c_X,c_Y,c_Z
cX,cY,cZ。并排构成如下
2
×
3
2\times3
2×3的矩阵：$R=[c_X, c_Y,c_Z]=\left( r_{ij}\right)_{i=1:2,j=1:3} $。<br> 构造如下的
4
×
4
4\times4
4×4的矩阵： 该矩阵最大特征值对应的特征向量即为**摄像机坐标系**下**支架**的姿态四元数，换言之，得到的是从摄像机到支架的转换
M
C
q
^C_Mq
MCq。<br> 此部分证明超出本文范围，可以参看Bar-Itzhack的论文<sup class="footnote-ref">[1](#fn1)</sup>。
支架(M)
↔
\leftrightarrow
↔传感器(S)
两者之间的相对关系无法测量，必须借助第三方的姿态估计算法。此处使用Kalman算法[2](#fn2)，在静止情况下，获得世界坐标系中传感器的姿态四元数
E
S
q
K
a
l
m
a
n
^S_Eq_{Kalman}
ESqKalman，将其认为是真实值。从传感器到支架的转换可以通过下式获取：
M
S
q
=
M
C
q
×
C
E
q
×
E
S
q
K
a
l
m
a
n
^S_Mq = {^C_Mq} \times {^E_Cq} \times {^S_Eq_{Kalman}}
MSq=MCq×CEq×ESqKalman
综合三个结果，可以通过摄像机获取世界坐标系下的传感器姿态：
E
S
q
=
E
C
q
×
C
M
q
×
M
S
q
^S_Eq = {^C_Eq}\times {^M_Cq}\times {^S_Mq}
ESq=ECq×CMq×MSq
结论
比较摄像机获取的姿态和前述算法估计的姿态，可以对算法进行评估。
作者在自己公司的网站上给出了Madgwick算法的源码。网站上还有许多实际应用例子。
Madgwick算法的一个显著优点是通用性：不需要对运动做任何假设，可以直接套用。但是，在实际应用中，还应该尽量利用先验知识，对加速度、速度或者位置进行重置，避免随时间的漂移。
- Itzhack Y Bar-Itzhack. New method for extracting the quaternion from a rotation matrix. AIAA Journal of Guidance, Control and Dynamics, 23(6):10851087, Nov.Dec 2000. (Engineering Note). ↩︎ 1. R. E. Kalman. A new approach to linear ltering and prediction problems. Journal of Basic Engineering, 82:35{45, 1960. ↩︎
浙公网安备 33010602011771号