导航及无人机姿态检测融合算法很难？用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放在同一个芯片里