微型四轴飞行器（5）九轴姿态融合算法A

1 概述

所谓的九轴姿态融合就是将通过传感器获得的3轴加速度、3轴角速度、3轴磁场数据，在相应的算法处理后能够得到飞行器的姿态信息（欧拉角）。输入输出如下图

在惯性导航领域的欧拉角分别表示的是航向角（yaw）、横滚角（roll）、俯仰角（pitch）。我们拟建一空间直角坐标系，在该坐标系中，物体做出的任何姿态同样也可以用一个空间直角坐标系表示，而这个新的坐标系都可以通过原坐标系围绕三个轴向进行一共三次转动得到。也是就说，物体的姿态变化可以通过与三个轴向的转动情况来表示。如若将原坐标系定位地球坐标系，也就是我们的参考坐标系，将物体姿态坐标系作为机体坐标系。则飞行器的任何姿态变化便能通过欧拉角来表示（这里暂不考虑万向节死锁的情况）。

2 传感器

2.1 加速度计

加速度计可以被用来测量三个轴向的加速度。设计方式就像在一个立方体中放入一个球体。当我们将这个立方盒放在一个没有地球引力的地方时，球体不会对任意一个面有作用力，此时三个轴向输出的压力都是0，也就没有加速度。当我们放在正常环境下时，由于只受到重力作用，小球会压盒子下表面。我们将得到下表面的压力。用牛顿第一定律力等于质量乘加速度可求得相应轴向的加速度，这里的加速度正好等于重力加速度g（9.8m/s^2）。当我们将盒子选择45°时，小球将会同时接触X和Z面。也就是会对两个面有作用力。当盒子只守重力时，那两个面的垂直分力之和也等于重力g。通常情况下，立方盒的三个面都会受到小球的作用力，当物体保持静止或者匀速直线运动时，三个轴的合力等于重力加速度g，并且方向永远垂直地面向下的。

我们能够通过加速度计求得一个用于参考对比的重力向量，比对求得的重力向量与各个轴向量的大小，能够求出对应的姿态角。但加速度也有缺陷，首先是求得标准重力加速度的条件是垂直方向上的静止或者匀速直线运动，其次就是当物体围绕重力方向旋转的时候，加速度计时无法感知的。加速度计通常适应静态或者低速的情况，所以单单加速度计是无法满足姿态解算的需求的。

2.2 陀螺仪

陀螺仪又名角速度传感器，是被用来检测角速度的（单位时间内旋转角度的变化）。陀螺仪的主要理论基础是角动量守恒定理，它的轴心有一个可以旋转的转子。当转子高速转动的时候，能够保持其旋转轴在惯性空间内的方向稳定性不变。角速度数据的推导过程比较复杂，文章重点在于数据处理，暂且将他作为一个能够输出三轴角速度的传感器来使用。

陀螺仪能够弥补加速度计动态特性不足的问题，通过他能够得到三个轴向的角速度。想要得到角度其实我们只需要对角速度积分即可，计算出来的就是姿态角，但陀螺仪也有一些问题导致我们没法完全相信得出的姿态角数据。误差与噪声的影响使其每次得出的数据并不准确，尤其又通过积分使其误差累计，这样数据就更加不可信了。为了能减小误差，通常会在使用陀螺仪的时候进行零偏矫正，也就是当陀螺仪在处于静止状态的时候，仍然会有一定的数据输出，我们通过累计一定的数据，算出偏差值，来修正使其处于静止状态的时候，三轴数据趋于零。

2.3 磁力计

磁力计是通过内部电路的电阻变化来检测三轴磁场强度的传感器。这里磁力计主要是用来纠正垂直于地面这个轴向（下面简称Z轴）数据总是漂移的问题。由于加速度计无法测量重力方向同时也是Z轴方向的旋转，于是就只能依靠陀螺仪，但由于陀螺仪会累计误差，又得不到加速度计的修正，误差只会越来越大，这个时候就需要能准确知道东南西北方向的磁力计来修正。地球磁场总是由磁南指向磁北的，在空中划出一道弧线，以垂直地面为Z轴，我们同样可以建立一个空间直角坐标系，而磁力计得到的三轴磁场强度，也就是该坐标系下的三个轴向。我们可以通过垂直于Z轴的两个轴向量来求得一个和向量，这个和向量就是指向磁北的。于是我们就能得到一个参考向量，从而修正绕Z轴旋转的角度了。由于地球磁场通常是很微弱的，会受到外界大量的干扰，就必须对它进行校准。理想情况下，在坐标系中，我们将物体绕一个轴旋转360°，每个磁场强度的点连接起来是一个圆形，如果有干扰的话，绘制出来的变是一个椭圆。我们就需要对数据处理修正。这里使用的是一元一次函数。首先使用Xmax，Xmin，Ymax，Ymin分别表示垂直与指向地面轴线的另外两个轴向的磁场最大最小值，先拟定最大最小值差距更大的一组轴的比例系数Xk为1，另外一个轴的比例系数可由下公式可得出：

Y_k=〖(X〗_max-X_min)/Y_max-Y_min

之后求出两轴的常数Xb，Yb：

X_b=X_k [1/2(X_max-X_min ) -X_max]
Y_b=Y_k [1/2(Y_max-Y_min ) -Y_max]

最后使用比例和偏置常数对数据进行修正：

X_output=X_input∙X_k+X_b
Y_output=Y_input∙Y_k+Y_b