Mahony互补滤波器是多旋翼姿态解算中常用的一种简单又高效的方法。
在介绍之前，首先来看一些关于传感器的基础知识：
陀螺仪
用于测量角速度，高频特性好，它是一个间接测量角度的器件；测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。
偏移
偏移就是在陀螺没有转动的时候却又输出，这个输出量的大小和供电电压以及温度有关，该偏移可以在陀螺仪上电时通过一小段时间的测量来修正。
漂移
它是由于在时间的积累下偏移和噪声相互影响的结果，例如有一个偏置（offset）0.1dps加在上面，于是测量出来是0.1dps，积分一秒之后，得到的角度是0.1度，1分钟之后是6度，还能忍受，一小时之后是360度，转了一圈，也就是说，陀螺仪在短时间内有很大的参考价值。
白噪声
电信号的测量中，一定会带有白噪声，陀螺仪数据的测量也不例外。所以获得的陀螺仪数据中也会带有白噪声，而且这种白噪声会随着积分而累加。
积分误差
对陀螺仪角速度的积分是离散的，长时间的积分会出现漂移的情况。所以要考虑积分误差的问题。
加速度计
加速度计的低频特性好，可以测量低速的静态加速度。在无人机上就是对重力加速度g的测量和分析。当把加速度计拿在手上随意转动时，看的是重力加速度在三个轴上的分量值。加速度计在自由落体时，其输出为0。为什么会这样呢？这里涉及到加速度计的设计原理：加速度计测量加速度是通过比力来测量(比力方程，秦永元老师的书中有介绍)，而不是通过加速度。加速度计仅仅测量的是重力加速度,而重力加速度与地理坐标系是固连的,通过这种关系,可以得到加速度计所在平面与地面的角度关系。速度计若是绕着重力加速度的轴转动，则测量值不会改变，也就是说加速度计无法感知这种水平旋转**。
使用公式描述就是：
{
θ
m
=
arcsin
(
a
x
b
m
g
)
ϕ
m
=
−
arcsin
(
a
y
b
m
g
cos
θ
m
)
\left\{\begin{array}{c}{\theta_{\mathrm{m}}=\arcsin \left(\frac{a_{x_{\mathrm{b}} \mathrm{m}}}{g}\right)} \\ {\phi_{\mathrm{m}}=-\arcsin \left(\frac{a_{y_{\mathrm{b}} \mathrm{m}}}{g \cos \theta_{\mathrm{m}}}\right)}\end{array}\right.
⎩⎨⎧θm=arcsin(gaxbm)ϕm=−arcsin(gcosθmaybm)
加速度计只能得到俯仰和滚转信息。
磁力计
用于测量磁场，在没有其他磁场的情况下，仅仅测量的是地球的磁场，而地磁也是和地理坐标系固连的，通过这种关系，可以得到平面A和地平面的关系。
使用公式描述便是：
{
m
x
c
=
m
x
b
cos
θ
m
+
m
y
b
sin
ϕ
m
sin
θ
m
+
m
z
b
cos
ϕ
m
sin
θ
m
m
y
c
=
m
y
b
cos
ϕ
m
−
m
z
b
sin
ϕ
m
\left\{\begin{array}{l}{m_{x_{c}}=m_{x_{b}} \cos \theta_{m}+m_{y_{b}} \sin \phi_{m} \sin \theta_{m}+m_{z_{b}} \cos \phi_{m} \sin \theta_{m}} \\ {m_{y_{c}}=m_{y_{b}} \cos \phi_{m}-m_{z_{b}} \sin \phi_{m}}\end{array}\right.
{mxc=mxbcosθm+mybsinϕmsinθm+mzbcosϕmsinθmmyc=mybcosϕm−mzbsinϕm
ψ
m
a
g
=
arctan
2
(
m
y
c
,
m
x
e
)
\psi_{\mathrm{mag}}=\arctan 2\left(m_{y_{c}}, m_{x_{\mathrm{e}}}\right)
ψmag=arctan2(myc,mxe)
即磁力计只能感知偏航信息。
综合考虑，加速度计和磁传感器都是极易受外部干扰的传感器，都只能得到部分角度关系，但是测量值随时间的变化相对较小，低频特性好，结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系，动态性能好，受外部干扰小，但测量值随时间变化比较大。可以看出，它们优缺点互补，结合起来才能有好的效果。
关于数据融合
现在有了三个传感器，都能在一定程度上测量角度关系，但是究竟相信谁？根据刚才的分析，应该是在短时间内更加相信陀螺仪，隔三差五的问问加速度计和磁力计，角度飘了多少了？有一点必须非常明确，陀螺仪是主角，加速度计和磁力计的作用是修正陀螺仪三轴的漂移。同时应该明确加速度计无法对航向角进行修正，修正航向角需要磁力计。
Mahony互补滤波的算法流程
预测过程
与Kalman滤波相似，Mahony滤波也是一个不断预测和校正的过程；
在预测环节，首先根据由陀螺仪测得的机体角速度，利用下式得到姿态四元数的一步预测：
[ q ˙ 0 q ˙ 1 q ˙ 2 q ˙ 3 ] = 1 2 [ 0 − ω x b − ω y b − ω z b ω x b 0 ω z b − ω y b ω y b − ω z b 0 ω x b ω z b ω y b − ω x b 0 ] [ q 0 q 1 q 2 q 3 ] \left[\begin{array}{c}{\dot{q}_{0}} \\ {\dot{q}_{1}} \\ {\dot{q}_{2}} \\ {\dot{q}_{3}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{0} & {-\omega_{x}^{b}} & {-\omega_{y}^{b}} & {-\omega_{z}^{b}} \\ {\omega_{x}^{b}} & {0} & {\omega_{z}^{b}} & {-\omega_{y}^{b}} \\ {\omega_{y}^{b}} & {-\omega_{z}^{b}} & {0} & {\omega_{x}^{b}} \\ {\omega_{z}^{b}} & {\omega_{y}^{b}} & {-\omega_{x}^{b}} & {0}\end{array}\right]\left[\begin{array}{c}{q_{0}} \\ {q_{1}} \\ {q_{2}} \\ {q_{3}}\end{array}\right] ⎣⎢⎢⎡q˙0q˙1q˙2q˙3⎦⎥⎥⎤=21⎣⎢⎢⎡0ωxbωybωzb−ωxb0−ωzbωyb−ωybωzb0−ωxb−ωzb−ωybωxb0⎦⎥⎥⎤⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤
校正过程
在预测环节得到的姿态四元数需要通过加速度计和磁力计的值进行校正，通常由如下两个环节组成：
通过加速度计校正姿态四元数中的俯仰和滚转分量
此处先假设多旋翼无人机始终处于低速，小范围机动的状态下，这样就可以将加速度计的测量值视为机体的重力加速度（实际在多旋翼飞控系统中可以使用GPS测量得到的运动加速度来补偿加速度计，使得用于姿态解算的加速度计测量值更接近实际的重力加速度）；我们知道重力加速度在地理系下的表示始终是
A c c n = [ 0 0 g ] T Acc_n=\left[\begin{array}{lll}{0} & {0} & {g}\end{array}\right]^{\mathrm{T}} Accn=[00g]T，
单位化之后即为
A c c n ‾ = [ 0 0 1 ] T \overline {Acc_n}=\left[\begin{array}{lll}{0} & {0} & {1}\end{array}\right]^{\mathrm{T}} Accn=[001]T，
机体系下的加速度计测量值为
A c c b = [ a x a y a z ] T Acc_b=\left[\begin{array}{lll}{a_x} & {a_y} & {a_z} \end {array}\right]^{\mathrm{T}} Accb=[axayaz]T
单位化后可得
A c c b ‾ = [ a x n o r m a a y n o r m a a z n o r m a ] T \overline {Acc_b}=\left[\begin{array}{lll}{\frac{a_x}{norm_a}} & {\frac{a_y}{norm_a}} & {\frac{a_z}{norm_a}} \end {array}\right]^{\mathrm{T}} Accb=[normaaxnormaaynormaaz]T
其中 n o r m a = a x 2 + a y 2 + a z 2 norm_a = \sqrt{ a_x ^2 + a_y ^2 + a_z ^2} norma=ax2+ay2+az2
重力加速度在机体下的表示可根据下式计算：
v = [ v x v y v z ] = C n b [ 0 0 1 ] = [ 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] v = \left[\begin{array}{c}{v_{x}} \\ {v_{y}} \\ {v_{z}}\end{array}\right] =C_{n}^{b}\left[\begin{array}{l}{0} \\ {0} \\ {1}\end{array}\right] = \left[\begin{array}{c}{2\left(q_{1} q_{3}-q_{0} q_{2}\right)} \\ {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} \\ {q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2}}\end{array}\right] v=⎣⎡vxvyvz⎦⎤=Cnb⎣⎡001⎦⎤=⎣⎡2(q1q3−q0q2)2(q2q3+q0q1)q02−q12−q22+q32⎦⎤
其中$ C_{n}^{b}$表示由地理坐标系到机体坐标系的四元数形式的旋转矩阵
C n b = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 e ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] C_{n}^{b} = \left[\begin{array}{ccc}{q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2}} & {2\left(q_{1} q_{2}-q_{0} q_{3}\right)} & {2\left(q_{1} q_{3}+q_{0} q_{2}\right)} \\ {2\left(q_{1} q_{2}+q_{0} q_{3}\right)} & {q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2}} & {2\left(q_{2} q_{3}-q_{0} q_{1}e\right)} \\ {2\left(q_{1} q_{3}-q_{0} q_{2}\right)} & {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} & {q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2}}\end{array}\right] Cnb=⎣⎡q02+q12−q22−q322(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)q02−q12+q22−q322(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1e)q02−q12−q22+q32⎦⎤
假如我们在预测环节得到的姿态四元数是准确的，那么通过坐标变换将地理系下的重力加速度转换到机体系之后应该等于加速度的测量值；而两个向量的误差可以用向量叉积来计算，即
e a c c = A c c b ‾ × v e_{acc}=\overline {Acc_b} \times v eacc=Accb×v
值得注意的是，两个物理意义相同的量，理论上叉积应该为零；该误差表示了由陀螺仪积分得到的姿态四元数中的俯仰和滚转误差。
通过磁力计校正姿态四元数中的偏航分量
首先对机体系下的磁力计测量值进行规范化
M a g b ‾ = [ m x n o r m m m y n o r m m m z n o r m m ] T \overline {Mag_b}=\left[\begin{array}{lll}{\frac{m_x}{norm_ m }} & {\frac{ m _y}{norm_ m }} & {\dfrac{ m _z}{norm_ m }} \end {array}\right]^{\mathrm{T}} Magb=[normmmxnormmmynormmmz]T
其中 n o r m m = m a g x 2 + m a g y 2 + m a g z 2 norm_ m = \sqrt{ mag _x ^2 + mag _y ^2 + mag _z ^2} normm=magx2+magy2+magz2
接着便是求磁力矢量在机体系下的表示，首先我们假定地理坐标系的 O − N O-N O−N轴与地磁北极对齐(即忽略地磁偏角)，那么地理系下的磁力矢量便可表示为(单位化后)：
[
b
x
0
b
z
]
\left[\begin{array}{c}{b_{x}} \\ {0} \\ {b_{z}}\end{array}\right]
⎣⎡bx0bz⎦⎤
上式中的值可以通过下面步骤来计算：
首先将机体系下的磁力计测量值通过旋转矩阵转换到地理坐标系；
[
h
x
h
y
h
z
]
=
C
b
n
M
a
g
b
‾
\left[\begin{array}{l}{h_{x}} \\ {h_{y}} \\ {h_{z}}\end{array}\right]=C_{b}^{n} \overline{Mag_b}
⎣⎡hxhyhz⎦⎤=CbnMagb
即可得到磁力矢量的水平分量:
b
x
=
h
x
2
+
h
y
2
b_{x}=\sqrt{h_{x}^{2}+h_{y}^{2}}
bx=hx2+hy2
以及竖直分量:
b
z
=
h
z
b_{z}=h_{z}
bz=hz
再次变换回机体系：
[ w x w y w z ] = C n b [ b x 0 b z ] \left[\begin{array}{c}{w_{x}} \\ {w_{y}} \\ {w_{z}}\end{array}\right]=C_{n}^{b}\left[\begin{array}{c}{b_{x}} \\ {0} \\ {b_{z}}\end{array}\right] ⎣⎡wxwywz⎦⎤=Cnb⎣⎡bx0bz⎦⎤
同样使用向量叉积得到误差
e m a g = M a g b ‾ × w e_{mag}=\overline {Mag_b} \times w emag=Magb×w
该误差表示了由陀螺仪积分得到的姿态四元数中的偏航误差。
四元数更新
由加速度计和磁力计校正得到的误差值为：
e = e a c c + e m a g e=e_{a c c}+e_{m a g} e=eacc+emag
由误差得到的修正量(PI环节可以有效消除常值偏移和累计误差)
δ
=
K
p
⋅
e
+
K
i
⋅
∫
e
d
t
\delta=K_p \cdot e+K_{i} \cdot \int e d t
δ=Kp⋅e+Ki⋅∫edt
陀螺仪修正
ω
=
ω
g
y
r
o
+
δ
\omega=\omega_{g y r o}+\delta
ω=ωgyro+δ
四元数更新
[ q ˙ 0 q ˙ 1 q ˙ 2 q ˙ 3 ] = 1 2 [ 0 − ω x b − ω y b − ω z b ω x b 0 ω z b − ω y b ω y b − ω z b 0 ω x b ω z y ω y b − ω x b 0 ] [ q 0 q 1 q 2 q 3 ] \left[\begin{array}{c}{\dot{q}_{0}} \\ {\dot{q}_{1}} \\ {\dot{q}_{2}} \\ {\dot{q}_{3}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{0} & {-\omega_{x}^{b}} & {-\omega_{y}^{b}} & {-\omega_{z}^{b}} \\ {\omega_{x}^{b}} & {0} & {\omega_{z}^{b}} & {-\omega_{y}^{b}} \\ {\omega_{y}^{b}} & {-\omega_{z}^{b}} & {0} & {\omega_{x}^{b}} \\ {\omega_{z}^{y}} & {\omega_{y}^{b}} & {-\omega_{x}^{b}} & {0}\end{array}\right]\left[\begin{array}{c}{q_{0}} \\ {q_{1}} \\ {q_{2}} \\ {q_{3}}\end{array}\right] ⎣⎢⎢⎡q˙0q˙1q˙2q˙3⎦⎥⎥⎤=21⎣⎢⎢⎡0ωxbωybωzy−ωxb0−ωzbωyb−ωybωzb0−ωxb−ωzb−ωybωxb0⎦⎥⎥⎤⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤
四元数规范化
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
q
0
2
+
q
1
2
+
q
2
2
+
q
3
2
q=\frac{q_{0}+q_{1} i+q_{2} j+q_{3} k}{\sqrt{q_{0}^{2}+q_{1}^{2}+q_{2}^{2}+q_{3}^{2}}}
q=q02+q12+q22+q32q0+q1i+q2j+q3k
一次迭代完毕，进入下一次迭代；
嵌入式代码
这里列写了Mahony互补滤波算法的部分代码
float norm;
float vx, vy, vz;
float ex, ey, ez;
//陀螺仪偏差修正
gx = gx + gyro_offsetx;
gy = gy + gyro_offsety;
gz = gz + gyro_offsetz;
//定义辅助变量以减少重复计算乘法的次数
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
//加速度计数据的规范化
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
//提取旋转矩阵的z轴分量，即重力加速度在机体系下的表示
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
//计算向量叉积，表示了陀螺仪积分得到的重力加速度与实际重力加速度的误差
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
//误差积分
exInt = exInt + ex*Ki*DT;
eyInt = eyInt + ey*Ki*DT;
ezInt = ezInt + ez*Ki*DT;
//误差的比例积分用来修正陀螺仪
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
//四元数的二阶递推算法
float delta = (DT*gx) * (DT*gx) + (DT*gy) * (DT*gy) + (DT*gz) * (DT*gz);
float tmp0 = (1.0f - delta / 8.0f) * q0 + 0.5f * (-q1*gx - q2*gy - q3*gz) * DT;
float tmp1 = (1.0f - delta / 8.0f) * q1 + 0.5f * ( q0*gx + q2*gz - q3*gy) * DT;
float tmp2 = (1.0f - delta / 8.0f) * q2 + 0.5f * ( q0*gy - q1*gz + q3*gx) * DT;
float tmp3 = (1.0f - delta / 8.0f) * q3 + 0.5f * ( q0*gz + q1*gy - q2*gx) * DT;
q0=tmp0;
q1=tmp1;
q2=tmp2;
q3=tmp3;
//姿态四元数的规范化
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
*pitch = asin(-2 * q1 * q3 + 2 * q0 * q2)* 57.3f; // pitch
*roll = fast_atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1)* 57.3f; // roll
//*yaw = fast_atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3f; //yaw几点注意：
实际使用过程中只加入了加速度计对陀螺仪的校正，并未使用磁力计对陀螺仪进行校正，同时偏航角使用的高低通滤波的方式进行解算。原因：由于磁力计本身的噪声过大，导致融合了磁力计的Mahony算法的姿态输出不稳定(稳态值不准且有较大的噪声)，且磁力计本身的更新频率远低于陀螺仪和加速度计，导致融合后偏航角的补偿效果并不明显。
扫一扫
4万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
