Mahony算法 AHRS系统
简介
AHRS(Attitude and heading reference system,也就是航姿参考系统。在互补滤波算法中传感器主要采用了IMU(陀螺仪、加速度计)和磁力计。
AHRS的基本思想是,在载体没有平移运动的情况下,通过加速度感知重力分量,可以计算出载体的俯仰和横滚;磁力计可以感知磁北方向,因此可以计算载体的磁北航向;而陀螺仪测量输出载体的旋转角速度,通过积分可以计算得到横滚、俯仰、航向增量,但由于陀螺输出值含有误差,采用积分计算,误差会随着时间累积。陀螺仪动态响应特性良好,但计算姿态时会产生累积误差, 磁力计和加速度计测量姿态没有累积误差,但动态响应较差,那么采用加速度计和磁力计的即时输出值对陀螺进行修正,则可以达到优势互补的效果,提高测量精度和系统的动态性能。
1、陀螺仪
陀螺仪,测量角速度,具有高动态特性,它是一个间接测量角度的器件。它测量的是角度的导数,即角速度,要将角速度对时间积分才能得到角度。
陀螺仪就是内部有一个陀螺,它的轴由于陀螺效应始终与初始方向平行,这样就可以通过与初始方向的偏差计算出旋转方向和角度。传感器MPU6050实际上是一个结构非常精密的芯片,内部包含超微小的陀螺。
如果这个世界是理想的,美好的,那我们的问题到此就解决了,从理论上讲只用陀螺仪是可以完成姿态导航的任务的。只需要对3个轴的陀螺仪角速度进行积分,得到3个方向上的旋转角度,也就是姿态数据。这也就是说的快速融合。
不过很遗憾,现实是残酷的,由于误差噪声等的存在,对陀螺仪积分并不能够得到完全准确的姿态,尤其是运转一段时间以后,积分误差的累加会让得到的姿态和实际的相差甚远。
那么哪些原因会使陀螺仪得到的姿态结果不准确呢?下面列举除常见的几种:
1.1 零点漂移
假设陀螺仪固定不动,理想角速度值是0dps(degree per second),但是存在零点漂移,例如有一个偏置0.1dps加在上面,于是测量出来是0.1dps,积分一秒之后,得到的角度是0.1度,1分钟之后是6度,还能忍受,一小时之后是360度,转了一圈,也就是说,陀螺仪在短时间内有很大的参考价值。
1.2 白噪声
电信号的测量中,一定会带有白噪声,陀螺仪数据的测量也不例外。所以获得的陀螺仪数据中也会带有白噪声,而且这种白噪声会随着积分而累加。
1.3 温度/加速度计影响
陀螺仪是一个温度和加速度敏感的元器件。例如对于加速度,多轴飞行器中的马达一般会带来较强烈的振动,一旦减震控制不好,就会在飞行过程中产生很大的加速度,必会带来陀螺输出的变化,引入误差。这就是在陀螺仪数据手册上常见的deg/sec/g指标。
1.4 积分误差
对陀螺仪角速度的积分是离散的,长时间的积分会出现漂移的情况。所以要考虑积分误差的问题。
这是由于陀螺仪测量姿态存在这么多的误差,所以我们必须要使用其它传感器辅助校正,其中最重要的就是下面的加速度传感器。
2、加速度计
加速度计的低频特性好,可以测量低速的静态加速度。在我们的飞行器上,就是对重力加速度g(也就是前面说的静态加速度)的测量和分析,其它瞬间加速度可以忽略。记住这一点对姿态解算融合理解非常重要。
当我们把加速度计拿在手上随意转动时,我们看的是重力加速度在三个轴上的分量值。加速度计在自由落体时,其输出为0。为什么会这样呢?这里涉及到加速度计的设计原理:加速度计测量加速度是通过比力来测量,而不是通过加速度。
加速度计仅仅测量的是重力加速度,3轴加速度计输出重力加速度在加速度计所在机体坐标系3个轴上的分量大小。重力加速度的方向和大小是固定的。通过这种关系,可以得到加速度计所在平面与地面的角度关系.
加速度计若是绕着重力加速度的轴转动,则测量值不会改变,也就是说加速度计无法感知这种水平旋转。
有关陀螺仪和加速度计和关系,姿态解算融合的原理,再把下面这个比喻放到这里一遍。
机体好似一条船,姿态就是航向(船头的方位),重力是灯塔,陀螺(角速度积分)是舵手,加速度计是瞭望手。舵手负责估计和把稳航向,他相信自己,本来船向北开的,就一定会一直往北开,觉得转了90度弯,那就会往东开。当然如果舵手很牛逼,也许能估计很准确,维持很长时间。不过只信任舵手,肯定会迷路,所以一般都有瞭望手来观察误差。
瞭望手根据地图灯塔方位和船的当前航向,算出灯塔理论上应该在船的X方位。然而看到实际灯塔在船的Y方位,那肯定船的当前航向有偏差了,偏差就是ERR=X-Y。舵手收到瞭望手给的ERR报告,觉得可靠,那就听个90%ERR,觉得天气不好、地图误差大,那就听个10%ERR,根据这个来纠正估算航向。
3、坐标系定义
载体坐标系使用“右-前-上”坐标系,地理坐标系采用“东-北-天”坐标系。
4、算法流程
4.1 初始状态确定
在初始静止情况下,加速度计数据可以根据重力矢量在三轴的分量计算出,这里加速度计单位为g。
[
f
x
f
y
f
z
]
=
C
n
b
[
0
0
1
]
=
[
cos
ϕ
cos
ψ
+
sin
ϕ
sin
θ
sin
ψ
−
cos
ϕ
sin
ψ
+
sin
ϕ
cos
ψ
sin
θ
−
sin
ϕ
cos
θ
sin
ψ
cos
θ
cos
ψ
cos
θ
sin
θ
sin
ϕ
cos
ψ
−
cos
ϕ
sin
θ
sin
ψ
−
sin
ϕ
sin
ψ
−
cos
ϕ
cos
ψ
sin
θ
cos
ϕ
cos
θ
]
[
0
0
1
]
\left[\begin{array}{l} f_{x} \\ f_{y} \\ f_{z} \end{array}\right]=C_{n}^{b}\left[\begin{array}{l} 0 \\ 0 \\ 1 \end{array}\right]=\left[\begin{array}{ccc} \cos \phi \cos \psi+\sin \phi \sin \theta \sin \psi & -\cos \phi \sin \psi+\sin \phi \cos \psi \sin \theta & -\sin \phi \cos \theta \\ \sin \psi \cos \theta & \cos \psi \cos \theta & \sin \theta \\ \sin \phi \cos \psi-\cos \phi \sin \theta \sin \psi & -\sin \phi \sin \psi-\cos \phi \cos \psi \sin \theta & \cos \phi \cos \theta \end{array}\right]\left[\begin{array}{l} 0 \\ 0 \\ 1 \end{array}\right]
⎣⎡fxfyfz⎦⎤=Cnb⎣⎡001⎦⎤=⎣⎡cosϕcosψ+sinϕsinθsinψsinψcosθsinϕcosψ−cosϕsinθsinψ−cosϕsinψ+sinϕcosψsinθcosψcosθ−sinϕsinψ−cosϕcosψsinθ−sinϕcosθsinθcosϕcosθ⎦⎤⎣⎡001⎦⎤
即:
[
f
x
f
y
f
z
]
=
[
−
sin
ϕ
cos
θ
sin
θ
cos
ϕ
cos
θ
]
\left[\begin{array}{l} f_{x} \\ f_{y} \\ f_{z} \end{array}\right]=\left[\begin{array}{c}\\ -\sin \phi \cos \theta \\ \sin \theta \\ \cos \phi \cos \theta \end{array}\right]
⎣⎡fxfyfz⎦⎤=⎣⎡−sinϕcosθsinθcosϕcosθ⎦⎤
可得:
θ
=
arcsin
f
y
ϕ
=
−
arctan
f
x
f
z
\begin{array}{c} \theta=\arcsin f_{y} \\ \phi=-\arctan \frac{f_{x}}{f_{z}} \end{array}
θ=arcsinfyϕ=−arctanfzfx
之后利用磁传感器计算偏航角。
ψ
=
arctan
m
x
n
m
y
n
\psi=\arctan \frac{m_{x}^{n}}{m_{y}^{n}}
ψ=arctanmynmxn
4.2 加速度计修正
利用四元数得到姿态变换矩阵
C
n
b
C_{n}^{b}
Cnb
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
)
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
)
]
\boldsymbol{C}_{n}^{b}=\left[\begin{array}{ccc} \left(q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2}\right) & 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) & \left(q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2}\right) & 2\left(q_{2} q_{3}+q_{0} q_{1}\right) \\ 2\left(q_{1} q_{3}+q_{0} q_{2}\right) & 2\left(q_{2} q_{3}-q_{0} q_{1}\right) & \left(q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2}\right) \end{array}\right]
Cnb=⎣⎡(q02+q12−q22−q32)2(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)(q02−q12+q22−q32)2(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)(q02−q12−q22+q32)⎦⎤
而重力矢量为[0;0;1]实际解算出的加速度计估计值即为姿态变换矩阵的最后一列[vx;vy;vz]。
将[vx;vy;vz]与加速度计测量值做向量积得到加速度计误差向量。
e
a
c
c
=
[
a
y
∗
v
z
−
a
z
∗
v
y
a
z
∗
v
x
−
a
x
∗
v
z
a
x
∗
v
y
−
a
y
∗
v
x
]
e_{a c c}=\left[\begin{array}{lllllll} a y & * & v z & -a z & * & v y \\ a z & * & v x & -a x & * & v z \\ a x & * & v y & -a y & * & v x \end{array}\right]
eacc=⎣⎡ayazax∗∗∗vzvxvy−az−ax−ay∗∗∗vyvzvx⎦⎤
4.3 磁力计修正
由于加速度计所参考的重力加速度矢量垂直向下,当加速度计沿z轴旋转时,其测量值不会发生变化,导致加速度计只能对滚转角和俯仰角的值进行修正,无法对偏航角进行感知,在偏航角上存在误差。利用地磁传感器可以对偏航角进行修正。
首先利用
C
b
n
C_{b}^{n}
Cbn为
C
n
b
C_{n}^{b}
Cnb逆矩阵,得到
C
b
n
C_{b}^{n}
Cbn,然后利用载体坐标系的地磁传感器的读数得到[hx;hy;hz]向量。
[
h
x
h
y
h
z
]
=
C
b
n
[
m
x
m
y
m
z
]
\left[\begin{array}{l} h_{x} \\ h_{y} \\ h_{z} \end{array}\right]=C_{b}^{n}\left[\begin{array}{l} m_{x} \\ m_{y} \\ m_{z} \end{array}\right]
⎣⎡hxhyhz⎦⎤=Cbn⎣⎡mxmymz⎦⎤
加速度计在静止时测量的是重力加速度,是有大小和方向的;同理,地磁计同样测量的是地球磁场的大小和方向,只不过这个方向不再是竖直向下,而是与x轴(或者y轴)呈一个角度,与z轴呈一个角度。由于在水平方向,无论
C
b
n
C_{b}^{n}
Cbn在航向上有没有误差,转换后水平方向矢量和应该相等。
这里由于使用东北天坐标系,x轴指向东方,所以将bx设为0,by为hx和hy的乘方开根。再将向量[0;by;bz]通过
C
n
b
C_{n}^{b}
Cnb转换到载体坐标系,得到地磁估计矢量:
[
w
x
w
y
w
z
]
=
C
n
b
[
0
b
y
b
z
]
\left[\begin{array}{l} w_{x} \\ w_{y} \\ w_{z} \end{array}\right]=C_{n}^{b}\left[\begin{array}{c} 0 \\ b_{y} \\ b_{z} \end{array}\right]
⎣⎡wxwywz⎦⎤=Cnb⎣⎡0bybz⎦⎤
之后求得到的地磁估计值和实际读数向量的向量积:
e
mag
=
[
m
y
∗
w
z
−
m
z
∗
w
y
m
z
∗
w
x
−
m
x
∗
w
z
m
x
∗
w
y
−
m
y
∗
w
x
]
e_{\text {mag }}=\left[\begin{array}{l} m y * w z-m z * w y \\ m z * w x-m x * w z \\ m x * w y-m y * w x \end{array}\right]
emag =⎣⎡my∗wz−mz∗wymz∗wx−mx∗wzmx∗wy−my∗wx⎦⎤
4.4 对陀螺仪进行修正
根据pi调节,设置对陀螺测量值的修正量:
δ
=
k
p
(
e
a
c
c
+
e
m
a
g
)
+
k
i
∫
(
e
a
c
c
+
e
m
a
g
)
\delta=k_{p}\left(e_{a c c}+e_{m a g}\right)+k_{i} \int\left(e_{a c c}+e_{m a g}\right)
δ=kp(eacc+emag)+ki∫(eacc+emag)
对陀螺仪测量值进行修正:
[
g
x
′
g
y
′
g
z
′
]
=
[
g
x
g
y
g
z
]
+
δ
\left[\begin{array}{l} g_{x}^{\prime} \\ g_{y}^{\prime} \\ g_{z}^{\prime} \end{array}\right]=\left[\begin{array}{l} g_{x} \\ g_{y} \\ g_{z} \end{array}\right]+\delta
⎣⎡gx′gy′gz′⎦⎤=⎣⎡gxgygz⎦⎤+δ
参考资料
[1] AHRS互补滤波(Mahony)算法及开源代码
[2]陀螺仪加速度计MPU6050
[3] Mahony姿态解算算法笔记(一)