AHRS姿态解算说明(加速度+陀螺仪+磁力计原理及原始数据分析)

转载链接：http://www.51hei.com/bbs/dpj-92911-1.html
AHRS俗称航姿参考系统，AHRS由加速度计，磁场计，陀螺仪构成，AHRS的真正参考来自于地球的重力场和地球的磁场～～他的静态终精度取决于对磁场的测量精度和对重力的测量精度 ,而则陀螺决定了他的动态性能。
　　这就是AHRS～在这种前提下。说明AHRS离开了地球这种有重力和磁场环境的时候是没法正常工作的～～
本章旨在讲解以下内容
1.加速度
2.陀螺仪
3.磁力计
一直想写篇文章关于姿态解算原理的，使用尽量通俗的语句说明如何从加速度计和陀螺 仪的数据，融合得到载体的姿态角。无奈自己的水平有限，一直搁置。 淡泊以明志,宁静以致远.人总是要逼自己做些事，才过得心安理得。那就拿点时间把这 方面的资料整合一下吧。这篇文章的大部分内容都不是本人原创的,感谢网络上无私奉献的 人.
在此介绍一下实验的姿态板 ,新一代的mini AHRS,采用STM32F103单片机进行姿态解算，板子上集成有
1.MPU6050，三轴的加速度和陀螺仪
2.HMC5883 三轴的磁力计
3.BMP180 高精度气压高度计
这些传感器都通过I2C接口连接到主控制器STM32.不需要额外的ADC电路,直接通过数字接口就可以读取传感器的当前输出.

Mini AHRS硬件框图

1 加速度计
加速度计顾名思义,就是测量加速度的.那么,我们如何认识这个加速度呢?在此用一个盒子形 状的立方体来做模型，认识加速度,如下,盒子内的图像。

如果我们把盒子形状的立方体 放在一个没引力场的地方,球会保持在盒子的中间.你可以想 象，这个盒子是在外太空，远离任何天体，很难找到这样的地方,就想象飞船轨道围绕地球飞，一切都是在失重状态下。那么六个壁面感受到的压力都是0.
如果我们突然将立方体向左侧移动（我们加快加速，1G =9.8米/ S ^ 2），皮球打在了墙上X-。然后，我们测量球适用于在X轴上的壁和输出-1g值的压力。如下图

请注意，加速度计反应的加速向量与当前的受力方向是相反的.如上图所示,受力方向向 左,但是加速度的向量方向为右.
如果我们把这个小盒子拿来放在地球上,那么小球会落在Z-壁面上，并会为1G的底壁施加一个力，在下面的图片所示：

在这种情况下，框不动，但我们仍然可以得到Z轴的读数-1G。球在墙壁上的压力造成的引力场。
到目前为止，我们已经分析了单个轴加速度计的输出，这是你会得到一个单轴加速度计。 三轴加速度计的真正价值，即是他们可以同时检测到所有三个轴的惯性力。让我们回到我 们的盒模型，并让旋转45度在右边的框中。球会触及两面墙：Z和X-在下面的图片所示：

x 和 z 轴受到值的0.71是不是任意的? 它们实际上是一个近似SQRT（1/2）.要知道当盒子只受重力场时，$x^2+Y^2+z^2=1g$ 这将变得更加清晰，为大家介绍一下我们的下一个样子的加速度计。
在之前的盒子模型中，我们有固定的引力场及旋转。在刚刚说明的两个例子中，我们分析 了2个不同的输出框位置，而力矢量保持不变。这更有利于了解加速度计如何与外部交互， 并显示当前读数。

请看一下上面的模型，这是一个新的模型代替刚刚的盒子立方体。试想一下，在新模型中 的每个轴是垂直于盒子的壁面。向量R是加速度计测量（从上面的例子或这两者的组合，它 可以是引力场或惯性力）的力矢量。接收时，Ry，R Z与上的X，Y，Z轴的R矢量投影。请注 意下面的关系式：
R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2 （公式1）
请记住，早一点我告诉你，SQRT（1/2）的值不是随机的〜0.71。如果您将上述公式中，回 顾我们的引力场为1 g后，我们可以验证：
1 ^ 2 =（-SQRT（1/2））^ 2 + 0 ^ 2 +（-SQRT（1/2））^ 2
简单地通过代以R = 1时，接收= SQRT（1/2）时，Ry = 0，RZ = SQRT（1/2）在方程
经过以上的理论分析我们越来越接近现实生活中的加速度计。 值RX，RY，RZ实际上是呈线性关系对应到真正的现实加速度计的x轴 y轴 z轴.
在此提出一个问题， 加速度计如何将这些信息告诉我们？ 目前市面上的加速度计从输出上区分为两种，一种是数字的，另一种是模拟
的.miniAHRS 使用的是MPU6050三轴加速度计，是I2C接口的数字传感器.通过特定的命令可以配置加速度的量程,并将内部ADC的转换结果读出来.
现在，我们有我们的加速度计的读数，以LSB为单位的，它仍然不是g（9.8米/秒^ 2），需要最后的转换，我们要知道加速度计灵敏度，通常表示为LSB /g。比方说当我们选择2g的量程时，对应的灵敏度= 16384 LSB/ G 。为了得到最终的力值，单位为g，我们用下面的公式：
RX = ADCRx /灵敏度
也就是说 当x轴的计数为ADCRx 时，那么对应的加速度值就是 (ADCRx/16384)g.回到加速度向量模型，将相关角度符号补上，如下图

方向余弦
我们感兴趣的是向量R 和 X、Y、Z轴之间的角度，将它们定义为 Axr Ayr Azr. 可以看到
由R 和Rx 组成的直角三角形：
COS（Axr）= RX / R 依此类推：
COS（Ayr）= RY / R
COS（Azr）= RZ / R
可以得到 R = SQRT（RX ^ 2 + RY ^ 2 + RZ ^ 2） 我们发现 当使用 arccos() 反余弦 :
Axr = arccos(RX / R)
Ayr = arccos(RY / R)
Azr = arccos(RZ / R)
已以通过很多公式解释加速度计模型。我们也会很快解释陀螺仪 以及如何用加速度计和陀 螺仪的数据进行整合，以得到更精确的角度估计。
在这之前 我们先来看看更有用的公式： cosX = cos(Axr) = Rx / R
cosY = cos(Ayr) = Ry / R
cosZ = cos(Azr) = Rz / R
这三个公式通常被称为方向余弦。你可以轻松地验证： SQRT（cosX ^ 2 + COSY ^ 2 + cosZ ^ 2）= 1
这个属性可以避免监视R矢量的模（长度）。很多时候，我们只对惯性矢量方向感兴趣，对 矢量进行规范化对简化程序运算很有意义。

重力向量
我们暂且从理论分析回到现实的传感器输出中,当水平放置MPU6050,只有Z轴感受到重力向量，它将输出1g。对应的ADC值就是16384 (2g的量程)。此时,R就是重力向量,Rx=0. Ry=0. Rz = R =1g.满足 R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2 得到重力向量与各个轴的夹角
Axr = arccos(RX / R) = 90度
Ayr = arccos(RY / R) = 90度
Azr = arccos(RZ / R) = 0 度

加速度计的标定
当MPU6050水平放置时，理论上Z轴感受到重力 将读出16384。同时X 轴和Y轴的读数将是0.可实际并不是这样的。这是由于每个芯片在制作时都不一样，数据手册上的都是理论的值，真正的芯片在水平时Z轴可能并不是16384.我们需要找到当各个轴在0g重力时的计数，
1g时的读数，以及-1g时的读数，得到一个补偿值，在每次读取ADC结果后都进行补偿。这个过程我们称之为标定。用数学公式表示为:
ADCx = K*Gx + Offset
ADCx 传感器输出
Gx 真实的加速值
Offset 加速度为0g时传感器的输出
K 标度因数

陀螺仪
陀螺仪是测试角速度的传感器，也有人把角速度说成角速率，说的是一样的物理量。拿电机做例子，当我们说一个电机10转每秒。一转是360度,那么它的主轴在一秒内转过3600度。也就是说这个电机在转动时的角速度是3600dps.dps 就是dergee per second 度每秒(或者写成 deg/s).
MPU6050 集成了三轴的陀螺仪.角速度全格感测范围为±250、±500、±1000与±2000°/sec (dps).当选择量程为±250dps的时候，将会得到分辩率为131LSB/(º/s).也就是当载体在X+轴转动1dps时，ADC将输出131.

回到加速度向量模型，将相关角度符号补上，如下图

陀螺仪测量什么？
MPU6050带有三个陀螺仪，每个陀螺仪各自负责检测相应轴的转动速度，也就是检测围绕各个轴转动的速度。像三轴的陀螺仪将同时检测 X Y Z轴的旋转。 由上面这个模型图，首先我们定义： Rxz - 是R向量在XZ平面上的投影 Ryz - 是R向量在XY平面上的投影 Rxz和Rz所形成的直角三角形，利用勾股定理，我们得到： Rxz ^ 2 = RX ^ 2 + RZ ^ 2，和同样： Ryz ^ 2 = RY ^ 2 + RZ ^ 2
还要注意的是：
R ^ 2 = RXZ ^ 2 + Ry^ 2，这可以来自从公式1和上面的等式，或它可以是来自于由R和Ryz
R ^ 2 = Ryz ^ 2 + Rx^ 2 形成的直角三角我们不会在本文中使用这些公式，只是让读者认识到所有值之间的关系。 同时我们将定义Z轴和Rxz 、RyZ之间的夹角。 Axz - Rxz和Z轴间的夹角 Ayz - Ryz和Z轴间的夹角 现在看看,从这个模型中,陀螺仪测量什么?
上述的说明,已经知道陀螺仪测量角度的变化率.为了解释这一点，让我们假设，我们已经 测量围绕Y轴的旋转角（这将是Axz角）在时刻t0，我们将其定义为Axz0，接下来，我们测 量这个角度是在稍后的时间t1是Axz1。变化率将被计算如下：
RateAxz =（Axz1 - Axz0）/（t1 - t0）
如果Axz单位是度，并以秒为时间单位，那么RateAxz将以deg / s表示。
MPU6050并不会以 deg / s 单位输出，我们需要在读完后进行转换。先来看看各个量程对 应的灵敏度。

从ADC值到 dps
通过I2C接口读出来的转换结果ADC值，并不是以度每秒为单位。一般按以下公式进行转换：
Anglerate = ADCrate /灵敏度
以量程为±1000º/s为例，说明如何转换。假设读取x轴的ADC值为200,从上表中得知在±1000º/s下的灵敏度为32.8LSB/(º/s) 。根据上面的公式：
Anglerate = 200/32.8 = 6.09756º/s
这就是说,MPU6050检测到模块正在以约6度每秒的速度绕X轴(或者叫在YZ平面上)旋转.
ADC值并不都是正的,请注意,当出现负数时,意味着该设备从现有的正方向相反的方向旋转.

磁力计
通过上面的介绍，读者对加速度和陀螺仪已经有一个认识了。
1.陀螺仪的强项在于测量设备自身的旋转运动。对设备自身运动更擅长。但不能确定设备的方位。
2.加速计的强项在于测量设备的受力情况。对设备相对外部参考物(比如，地面)的运动更擅长。
那么 为什么所有的AHRS模块都带有一个三轴的磁力计呢？磁力计是做什么用的？
首先，AHRS 全称为姿态航向参考系统，加速度和陀螺只能提供姿态参考，并不能解算出正确的航向。而磁力计是用于感受地磁向量以解算出模块与北的夹角。磁力计的这个功能类似 于指南针，所以也叫电子指南针,或者称为电子磁罗盘。 为了认识这个传感器，重新认识一下指南针。

从指南针开始
指南针是用以测定方向基准的仪器。其主要组成部分是一根装在轴上可以自由转动的磁 针。指南针之所以能够指示方向，是因为地球本身存在磁场，磁针在地磁场作用下能保持在 磁子午线的切线方向上。磁针的N极指向地磁的南极，利用这一性能可以辨别方向。常用于 航海、大地测量、旅行及军事等方面。 指南针是用以测定方向基准的仪器。其主要组成部分是一根装在轴上可以自由转动的磁 针。指南针之所以能够指示方向，是因为地球本身存在磁场，磁针在地磁场作用下能保持在 磁子午线的切线方向上。磁针的N极指向地磁的南极，利用这一性能可以辨别方向。常用于 航海、大地测量、旅行及军事等方面。

霍尔传感器
霍尔传感器是根据霍尔效应制作的一种磁场传感器。霍尔效应是磁电效应的一种，这一现 象是霍尔（A.H.Hall，1855—1938）于1879年在研究金属的导电机构时发现的.
在半导体薄片两端通以控制电流I，并在薄片的垂直方向施加磁感应强度为B的匀强磁场，则在垂直于电流和磁场的方向上，将产生电势差为VH的霍尔电压.
霍尔效应的典型应用就是霍尔开关,它由稳压器、霍尔元件、差分放大器，斯密特触发器和输出级组成，它输出数字量。常常用于检测发动机转速和产生点火信号,等等 在这里说霍尔效应 是证明磁场是存在的,也是可以被检测到的.
磁力计与霍尔传感器是有差别的,前者是用于检测微量的磁场,大地磁场约在0.3-0.6高斯,如此微弱的磁力,我们需要借助惠斯通电桥来检测,

磁力计的惠斯通电桥
如上图所示。R1/R2/R3/R4是初始状态相同的AMR电阻，但是R1/R2和R3/R4具有相反的磁化特性。当检测到外界正交偏置磁场的时候，R1/R2阻值增加∆R而R3/R4减少∆R。这样在没有外界正交偏置磁场的情况下，电桥的输出为零；而在有外界磁场时电桥的输出为一个微小的电压∆V。磁力计就是利用惠斯通电桥检测AMR阻值的变化,来感觉外部的磁力.当然这里的∆V很小，需要进入放大电路处理。
在些解释一下电桥的关键部件AMR电阻,也叫各向异性磁电阻效应,简称磁控电阻.当外部的 磁力线垂直于电阻时与外部磁力线平行于电阻时呈现不一样的电阻率.
HMC5883是三轴的磁力计,当然它内部有三个电桥.将三维磁阻传感器按照载体三维坐标系安装,通过测量载体空间磁场的三维磁感应强度，按照一定的算法就可以计算出载体在空间 的姿态信息。 这就是电子指南针.

磁干扰
这个世界上不是只有地球才能产生磁场,我们身边很多的物体都是可以产生磁场的,比如磁 铁,电机,钢筋构建的楼房,通电流的直导线也会产生磁场.
我们可以做这样的实验,找个指南针,当用磁铁靠近指南针时,它指示的方向会发生变化,此 时它再不能用做指南针给我们导航.因为它受到外界磁力干扰,指示的方向已经不能保证正确 了.这样的现象,我们称之为磁干扰.
电子指南针主要是通过感知地球磁场的存在来计算磁北极的方向。然而由于地球磁场在一 般情况下只有微弱的0.5高斯左右，而一个普通的手机喇叭当相距2厘米时仍会有大约4高斯 的磁场，一个手机马达在相距2厘米时会有大约6高斯的磁场，这一特点使得针对电子设备表 面地球磁场的测量很容易受到电子设备本身的干扰。 磁场干扰是指由于具有磁性物质或者可以影响局部磁场强度的物质存在，使得磁传感器所 放置位置上的地球磁场发生了偏差。 磁干扰又分成两种,一种是硬磁干扰,另一个是软磁干扰.
硬铁磁场由磁力计平台(可认为是载体)上的永久性磁铁和被磁化的钢铁物质组成，其特点 是当载体位于某一固定位置时，其强度为一定值，不随航向的变化而变化.软铁磁场可认为 由地球磁场与磁力计周围的磁化物质相互作用而产生。与硬铁磁场不同的是，软铁磁场强度 的大小与方向与磁力计的方位有关.

磁校准
受环境因素和电子磁罗盘自身因素的影响，电子磁罗盘常存在较大的航向角误差，因此经 常需要在使用前校准．磁罗盘校准的一般方法是使安装有磁罗盘的载体做特定的运动，或者 将载体转动到某些特定的角度，得到磁罗盘在不同姿态下的磁场强度测量值．通过对测量值 分析，进行磁罗盘的校准
平面校准方法 针对XY轴的校准，将配备有磁传感器的设备分别在XY平面内自转，相当于将地球磁场矢量 绕着过点O(γx,γy)垂直于XY平面的法线旋转， 而红色的圆为磁场矢量在旋转过程中在XY平 面内投影的轨迹。这可以找到圆心的位置为((Xmax + Xmin)/2, (Ymax + Ymin)/2).