看到群里有人在问关于这个GD32和四轴的话题，竟然被我找到了这样的帖子，本着好东西大家一起share的原则，特整理出来与诸君共享。算是新人福利吧。
废话不多说，开始：
算法参考自Madgwick写的AHRSUpdate和IMUUpdate，简单有效，其中AHRSUpdate是融合了陀螺仪、加速度计和磁力计，而IMUUpdate只融合了陀螺仪和加速度计，但对于做四轴飞行器来说，IMUUpdate也够用了，不过我没发现有用AHRSUpdate的，并且我的一大半的时间都是花在AHRSUpdate这个算法上，直到现在也没用这个算法解算出来正确的四元数和欧拉角，什么方法都试过了，导致我现在都开始怀疑这个算法的正确性，经过变通，我现在用的算法：先用IMUUpdate算出Pitch和Roll，然后再结合互补滤波求出Yaw，其中求Yaw的公式参考自john800422。
第一部分：硬件 1.传感器：MPU9150（INVENSENSE公司的，单芯片内集成了加速度计、陀螺仪和磁力计，并且内置DMP用于姿态融合，不过只融合了加速度计和陀螺仪，具有自校准功能，价格比MPU6050贵很多，但是省PCB面积，省事，轴向重合度高。实际上就是把MPU6050和磁力计AK8975放在同一个芯片里，程序还是使用MPU6050的驱动）； 2.MCU：GD32F103CB（Gigadevice公司的，支持国产，与ST同型号的MCU 直接兼容，性价比更高，外接8M晶振，晶振远离传感器，避免干扰磁力计） 3.电源芯片：TLV70233DBVR（TI的LDO，输入2-6V，输出3.3V，只需要外接2个X7R无极性陶瓷电容） 4.串口：MAX3232（方便调试） 5.USB供电 上图： 图1：PCB的3D效果图，实物图就不上了，太丑，测试版，先追求调通得出姿态角：
第二部分：软件 1.使用keil，uvision4.1.0，工具链：RealView MDK-ARM Version4.12； 2.库文件：ST官方库STM32F10x_StdPeriph_Driver version V3.3.0； 3.驱动：官方的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的轴向发生了变化。如下图：
第三部分：如何确定自己的轴向 首先，轴向的定义跟初始化四元数和最后结算的欧拉角有关，跟IMUUpdate四元数更新算法无关，换句话说，不管你的轴向如何定义，IMUUpdate随便用，但是初始化四元数的公式和最后结算欧拉角的公式要做适当的改变，这个后面算法中有说。加速度计也好，陀螺仪也好，磁力计也好，他们的轴向都要满足右手定理，如下图：
再附上一段注释用于解释如何定义合理的轴向，以及如何正确旋转传感器的轴向，解释这么多其实就是说定义好的轴向要满足右手定理，如下图：
下图，旋转前是[x y z]，旋转后就是[-y x z]：
下图是如何确定旋转角度的正方向，用右手握住坐标轴，拇指指向轴向的正方向，四个指头弯曲的方向就是旋转角度的正方向，在初始化四元数时，计算出的欧拉角的正方向也要满足这个条件：
我的程序使用的轴向如下图所示，未作任何改变：
第四部分：算法 第一步是校准，加速度计和陀螺仪我用的是MPU9150内部自校准，磁力计的校准采用如下方法：
磁力计校准.pdf
(268.69 KB, 下载次数: 145)
| 
发表于 2015-5-13 10:16:19
/4 
