导航及无人机姿态检测融合算法很难？用sensor fusion来搞定！

1.什么是姿态融合算法

简单来说，姿态融合算法就是融合多种运动传感器数据（一般需要3轴加速度， 3轴陀螺仪或者3轴地磁感应传感器），通过数字滤波算法容错补偿，实现当前姿态检测。

2.数字滤波算法的选择

根据运动传感器噪声模型，一般以下滤波算法可供融合算法选择：

a)互补滤波算法

b)扩展卡尔曼滤波算法

c)无迹卡尔曼滤波算法

d)粒子滤波算法

e)Mahony互补滤波算法

3.ST姿态检测融合算法

ST能提供完整的，高性能姿态检测融合算法--MotionFX/iNemoEngine。其能最大限度规避因加速度和地磁感应传感器数据异常，或者陀螺仪零飘带来的误差。多种数据输出选择，包括四元素，欧拉角，线性加速度以及重力矢量。

还有9轴和6轴两种选择：9轴的话需要3轴加速度，3轴陀螺仪和3轴地磁感应，能够提供当前三维空间的绝对方向信息，可以用作导航及无人机姿态检测等应用；6轴需要3轴加速度和3轴陀螺仪数据，可用于游戏，AR/VR等应用。

应用案例——GD32F103+MPU9150四旋翼飞行器第一步：姿态融合算法

前言：

相比直升机来说，四旋翼乃至多旋翼飞行器的机械结构简单，操控灵活，飞行稳定，体积也能做的更小，当然也能更大，它将直升机复杂的机械结构设计难度转化到了电子电路和算法上面，因此四旋翼飞行器的设计更容易上手，更民众化。

四旋翼飞行器的软件核心包括两大部分：姿态融合算法和控制算法；硬件核心便是MCU和传感器。

先介绍下姿态融合算法，姿态融合说白了就是将3轴加速度、3轴角速度和3轴磁场强度融合成四元数，再将四元数转化为欧拉角，最后将欧拉角最为控制量输送到所有电机以达控制飞行器姿态的目的。欧拉角包括偏航角Yaw、俯仰角Pitch和滚动角Roll。我用的算法是Madgwick写的AHRSUpdate和IMUUpdate，简单有效，其中AHRSUpdate是融合了陀螺仪、加速度计和磁力计，而IMUUpdate只融合了陀螺仪和加速度计，就优缺点来说，IMUupdate算法只融合了加速度计和陀螺仪的数据，还需要使用互补滤波算法来融合磁力计以修正偏航角Yaw，不然飞行器会找不到北，但是这种互补滤波有个小问题，就是假如定义偏航角的范围是0-360度，那么当机头大概从北偏西1度转到北偏东364度时，机头会经过0度(360度)这个点，那么这时，yaw不会直接1-0-364这么变化，而是会被逆向积分从1-20-180-270-364这么转一圈，这是个不好的现象，实验了半天也没有解决，而AHRSupdate很好的解决了这个问题，不过由于AHRSupdate把磁力计的数据融合进了所有欧拉角，因此当传感器受到外围强磁场干扰时，就会造成全方位失控，导致坠机，而使用IMUupdate算法，顶多飞行器会转圈而已。

传感器我用的是invensense公司的MPU9150，MPU9150芯片集成了加速度计、陀螺仪和磁力计，并且内置硬件DMP用于姿态融合，不过不好用；MCU则用是Gigadevice公司的GD32F103系列，由于我也是刚接触四旋翼飞行器，第一个目的当然是能够平稳的飞起来，暂不考虑加入其他外围设备。后续可能会考虑使用GD32F107或者GD32F2xx系列，可扩展摄像头小玩一把航拍，当然更好的是GD32F4xx系列（期盼中），自带浮点运算单元，由于我软件太菜，算法中出现大量的浮点运算导致姿态更新频率和控制频率达不到很高。

做四轴飞行器也是为了好玩，目前我只完成了第一步：姿态融合。接下来才是更重要的，选择合适的机架、电调、电机、螺旋桨，写PID控制代码，系统整合以后还要调试各种参数，抗干扰，抗震动，最后还要加各种应用器件。在此鼓励一下自己，坚持就是胜利，慢慢磨洋工。

我现在软件实现的功能：算法用AHRSupdate、陀螺仪零偏校准、加速度计平滑滤波、磁力计平面校准，以后看情况可能会慢慢更新加速度计精确校准、磁力计椭球拟合校准、陀螺仪温度补偿等。

第一部分：硬件

1.传感器：MPU9150（INVENSENSE公司的，单芯片内集成了加速度计、陀螺仪和磁力计，并且内置DMP用于姿态融合，不过只融合了加速度计和陀螺仪，没有融合磁力计进去，具有自校准功能，价格比MPU6050贵很多，但是省PCB面积，省事，轴向重合度高。实际上就是把MPU6050和磁力计AK8975放在同一个芯片里，程序还是使用MPU6050的驱动，缺点是会偶尔丢失数据，自带的姿态融合算法的更新频率不高）；

2.MCU：GD32F103CB（Gigadevice公司的，ARMcortex-M3内核，32位MCU，主频最高108Mhz，48Pin，与ST同型号的32位MCU 直接兼容，性价比更高，外接8M晶振，也可使用内部8M晶振，晶振远离传感器，避免干扰磁力计，不过当时考虑不周，这个芯片的timer太少了，以后会先采用GD32F103VCT6或者GD32F107VCT6）；

3.电源芯片：TLV70233DBVR（TI的LDO，输入2-6V，输出3.3V，只需要外接2个X7R无极性陶瓷电容，价格太高）

4.串口：MAX3232（方便调试）

5.USB供电，输出电压5V

6.目前机架、电调、电池、电机和螺旋桨已买好，来张图，比较大众化：

上图：

图1：PCB的3D效果图，测试版，先追求调通得出姿态角，后续改版会做大的调整：

第二部分：软件

1.使用keil，uvision4.1.0，工具链：RealViewMDK-ARM Version4.12；

2.驱动：官方的MPU6050驱动inv_mpu.c和inv_mpu_dmp_motion_driver.c；

先看几个图，然后再说坐标轴的设定和算法部分。

上图：

图1：系统初始化，顺序从上到下依次是：初始化MPU、设置需要使用哪些传感器、设置陀螺仪测量范围（我设的是正负500度/s）、设置加速度计测量范围（我设的是正负4g）、配置fifo、设置采样率、装载DMP、设置陀螺仪轴向(比较重要)、使能DMP的一些玩意儿、设置DMP的FIFO、自校准陀螺仪和加速度计、开启DMP、开始姿态融合，见下图：

图2：由四元数求出的最终姿态角，其中Yaw为航向角，表示机头偏离正北方多少度，范

围-180到+180；Pitch为俯仰角，表示机头正方向与水平线的夹角，范围-90到+90；Roll为翻滚角，表示机翼与水平线的夹角，范围：-180到+180。下图为机身水平，且机头正北偏西37度左右的数据：

图3：下图为机翼水平，机头指向正北，且机头向下25度的数据

图4：下图为机头指向正北，保持水平，且机翼的右翼向下倾斜23度的数据

图5：看下欧拉角的奇异点，在奇异点处一个转动状态对应无穷多组自由度值，当物体转到这些奇异点附近，便没法求解。图中当Pitch为+90度时，机体的姿态便没法控制，Roll的轴向发生了变化。如下图：

第三部分：如何确定自己的轴向

首先，轴向的定义跟初始化四元数和最后结算的欧拉角有关，跟四元数更新算法无关，换句话说，不管你的轴向如何定义，姿态融合算法随便用，但是初始化四元数的公式和最后结算欧拉角的公式要做适当的改变，这个后面算法中有说。加速度计也好，陀螺仪也好，磁力计也好，他们的轴向都要满足右手定理，如下图：

再附上一段注释用于解释如何定义合理的轴向，以及如何正确旋转传感器的轴向，解释这么多其实就是说定义好的轴向要满足右手定理，如下图：

下图，旋转前是[x y z]，旋转后就是[-y x z]：

下图是如何确定旋转角度的正方向，用右手握住坐标轴，拇指指向轴向的正方向，四个指头弯曲的方向就是旋转角度的正方向，在初始化四元数时，计算出的欧拉角的正方向也要满足这个条件：

我的程序使用的轴向如下图所示，未作任何改变：

第四部分：算法

第一步是校准，加速度计和陀螺仪我用的是MPU9150内部自校准，磁力计的校准采用如下方法：见附件-磁力计校准

第二步是初始化四元数，常见的轴向定义是绕x轴旋转是Roll，绕y轴旋转是Pitch，绕z轴旋转是Yaw，我的程序中也有这样的定义，不过被我注释掉了，这里举个另外一种轴向定义来对初始化四元数进行说明，方便比较，也是我目前正在用的轴定义。

下面我们来定义绕x轴旋转是Pitch，绕y轴旋转是Roll，绕z轴旋转是Yaw，轴向的正方向如上图一样，不变。

先对加速度计和磁力计的数据进行处理，得到init_xx来供我们使用如下图：

然后通过公式计算出初始化的Roll、Pitch、Yaw，注意加负号保证旋转角度的正方向，如下图：

其中Yaw的正方向未必对，可以自己去验证下，具体参考公式见附件-ST电子罗盘计算Yaw

然后由上面的欧拉角求出初始化四元数，这时要注意旋转顺序的不同，公式也不同，大部分旋转顺序是Z-Y-X，我的程序里也用的这个顺序，在这里我们按Z-X-Y的顺序来旋转，并得出求四元数的公式以做比较，其旋转矩阵:

q=qyaw*qpitch*qroll=

(cos(0.5*Yaw)+ksin(0.5*Yaw)) *(cos(0.5*Pitch)+isin(0.5* Pitch)) * (cos(0.5*Roll)+jsin(0.5* Roll))

得出初始化四元数计算公式如下图所示：

其中i，j，k之间相乘的顺序不能随意变，在前的先计算，在后的后计算相乘的公式如下图：

至此初始化四元数完成。

第三步就是使用AHRSUpdate算法了，用完以后再根据公式计算出欧拉角，此公式跟旋转顺序和旋转使用的轴向有关，我们的旋转顺序是Z-X-Y，且绕Z是Yaw，绕X是Pitch，绕Y是Roll，推到过程如下图：

首先得出3个方向余旋矩阵：

下图绕Z轴Yaw：

下图绕X轴pitch：

下图绕Y轴Roll：

然后按照我们的Z-X-Y顺序求得C=Croll * Cpitch * Cyaw，如下图：

将上图的方向余旋矩阵C与下图的四元数姿态矩阵做对比，即可求出欧拉角，注意上图的方向余旋矩阵C是随着我们对坐标轴的定义变化而变化的，而下图的四元数姿态矩阵是固定的：

最后一步就是求出欧拉角，公式如下图：

以下是静止时，刚上电时的数据：

以下是上电半个小时以后的数据：

可以看出正常的静止状态下，数据的波动范围是不超过1度的，也不会有漂移。具体效果还需要上机架飞起来以后再做调整。

最后总结一下：

我的代码中，0°

注意跑108Mhz时，需要修改延迟函数和串口函数，适当增加一下延迟函数的数值避免I2C通信失败，修改串口函数是为了避免108Mhz下串口乱码问题