Mahony算法 AHRS系统

简介
- 1、陀螺仪
- - 1.1 零点漂移
  - 1.2 白噪声
  - 1.3 温度/加速度计影响
  - 1.4 积分误差
- 2、加速度计
- 3、坐标系定义
- 4、算法流程
- - 4.1 初始状态确定
  - 4.2 加速度计修正
  - 4.3 磁力计修正
  - 4.4 对陀螺仪进行修正
- 参考资料

简介

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姿态解算算法笔记（一）

Mahony算法 AHRS系统相关推荐

Cordic算法——圆周系统之向量模式
转发博文背景: 在项目中用到了下面的知识:数据都是R+jI 的复数形式,首先要用CORDIC算法将数据转换成Ae^(jωt+φ) 形式.一句话就引出了一个知识点,这篇博文来总结这种功能的CORDIC算 ...
考虑页面置换算法，系统有m个物理块供调度，初始时全空，页面引用串长度为p，包含了n个不同的页号，无论用什么算法，缺页次数不会少于（）
考虑页面置换算法,系统有m个物理块供调度,初始时全空,页面引用串长度为p,包含了n个不同的页号,无论用什么算法,缺页次数不会少于( C ) A. m B. p C. n D. min(m, n) 无论 ...
AHRS系统的基本构成
AHRS系统的基本组成 AHRS 原本起源于飞行器相关技术,但是近几年随着成本的器件成本的不断降低也被广泛的应用于机动车辆与无人机,工业设备,摄像与天线云台,地面及水下设备,虚拟现实,生命运动科学分 ...
【分布式计算：原理、算法和系统】第二章分布式计算模型
[分布式计算:原理.算法和系统]第二章分布式计算模型 2.1 分布式程序一个分布式程序由一组nnn个异步进程(asynchronous processes)P1,P2,-,Pi,-,PnP_1,P ...
文本指纹算法和系统简述
文本指纹算法和系统简述文章转载于: https://zhuanlan.zhihu.com/p/21733784 作者:陈运文 [导读] 互联网网页存在大量的重复内容网页,无论对于搜索引擎的网页 ...
CORDIC算法——圆周系统之旋转模式
CORDIC(Coordinate Rotation Digital Computer) 算法,这个算法只利用移位和加减运算,就能计算常用三角函数值,如Sin,Cos,Sinh,Cosh等函数. 历史 ...
ofdm解调算法_OFDM系统中固定频偏算法
加入固定频偏: function berMatrix = freqOffsetOFDM(BPS,NS,M,SNR,ifftsize,carriers,N,ep) %标准OFDM系统 input_bit ...
《分布式计算--原理、算法与系统》读书笔记
Distributed Computing -- Principles, Algorithms, and Systems 分布式计算是指由计算机网络连接的多处理平台的各种形式的计算.信息访问与信息交换 ...
用银行家算法实现系统资源分配
银行家算法是 Dijkstra在1965年提出的一种避免死锁的算法.银行家算法陈述如下: 1) 当一个进程提出一个资源的请求时,假定分配给它,并调用检查系统状态安全性的算法.如果系统是安全的,则对 ...

Mahony算法 AHRS系统

Mahony算法 AHRS系统

简介

1、陀螺仪

1.1 零点漂移

1.2 白噪声

1.3 温度/加速度计影响

1.4 积分误差

2、加速度计

3、坐标系定义

4、算法流程

4.1 初始状态确定

4.2 加速度计修正

4.3 磁力计修正

4.4 对陀螺仪进行修正

参考资料

Mahony算法 AHRS系统相关推荐

最新文章

热门文章