比较直升机来说，四旋翼乃至多旋翼飞翔器的机械结构简略，操控灵敏，飞翔安稳，体积也能做的更小，当然也能更大，它将直升机杂乱的机械结构规划难度转化到了电子电路和算法上面，因而四旋翼飞翔器的规划更简略上手，更民众化。
先介绍下姿势交融算法，姿势交融说白了便是将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-这么转一圈，这是个欠好的现象，试验了半响也没有处理，而AHRSupdate很好的处理了这个问题，不过因为AHRSupdate把磁力计的数据交融进了一切欧拉角，因而当传感器遭到外围强磁场搅扰时，就会形成全方位失控，导致坠机，而运用IMUupdate算法，顶多飞翔器会转圈罢了。
传感器我用的是invensense公司的MPU9150MPU9150芯片集成了加速度计、陀螺仪和磁力计，而且内置硬件DMP用于姿势交融，不过欠好用;MCU则用是Gigadevice公司的GD32F103系列，因为我也是刚触摸四旋翼飞翔器，第一个意图当然是能够平稳的飞起来，暂不考虑参加其他外围设备。后续可能会考虑运用GD32F107或许GD32F2xx系列，可扩展摄像头小玩一把航拍，当然更好的是GD32F4xx系列(期盼中)，自带浮点运算单元，因为我软件太菜，算法中呈现许多的浮点运算导致姿势更新频率和操控频率达不到很高。
做四轴飞翔器也是为了好玩，现在我只完成了第一步：姿势交融。接下来才是更重要的，挑选适宜的机架、电调、电机、螺旋桨，写PID操控代码，体系整合今后还要调试各种参数，抗搅扰，抗轰动，最终还要加各种运用器材。在此鼓舞一下自己，坚持便是胜利，渐渐磨洋工。
我现在软件完成的功用：算法用AHRSupdate、陀螺仪零偏校准、加速度计滑润滤波、磁力计平面校准，今后看情况可能会渐渐更新加速度计准确校准、磁力计椭球拟合校准、陀螺仪温度补偿等。
1.传感器：MPU9150(INVENSENSE公司的，单芯片内集成了加速度计、陀螺仪和磁力计，而且内置DMP用于姿势交融，不过只交融了加速度计和陀螺仪，没有交融磁力计进去，具有自校准功用，价格比MPU6050贵许多，可是省PCB面积，省劲，轴向重合度高。实际上便是把MPU6050和磁力计AK8975放在同一个芯片里，程序仍是运用MPU6050的驱动，缺陷是会偶然丢掉数据，自带的姿势交融算法的更新频率不高);
图1：体系初始化，次序从上到下依次是：初始化MPU、设置需求运用哪些传感器、设置陀螺仪丈量规模(我设的是正负500度/s)、设置加速度计丈量规模(我设的是正负4g)、装备fifo、设置采样率、装载DMP、设置陀螺仪轴向(比较重要)、使能DMP的一些玩意儿、设置DMP的FIFO、自校准陀螺仪和加速度计、敞开DMP、开端姿势交融，见下图：
围-180到+180;Pitch为俯仰角，表明机头正方向与水平线;Roll为翻滚角，表明机翼与水平线。下图为机身水平，且机头正北偏西37度左右的数据：
图5：看下欧拉角的奇特点，在奇特点处一个滚动状况对应无量多组自由度值，当物体转到这些奇特点邻近，便无法求解。图中当Pitch为+90度时，机体的姿势便无法操控，Roll的轴向发生了改动。如下图：
首要，轴向的界说跟初始化四元数和最终结算的欧拉角有关，跟四元数更新算法无关，换句话说，不论你的轴向怎么界说，姿势交融算法随意用，可是初始化四元数的公式和最终结算欧拉角的公式要做恰当的改动，这个后边算法中有说。加速度计也好，陀螺仪也好，磁力计也好，他们的轴向都要满意右手定理，如下图：
再附上一段注释用于解说怎么界说合理的轴向，以及怎么正确旋转传感器的轴向，解说这么多其实便是说界说好的轴向要满意右手定理，如下图：
下图是怎么确认旋转视点的正方向，用右手抓住坐标轴，拇指指向轴向的正方向，四个指头曲折的方向便是旋转视点的正方向，在初始化四元数时，计算出的欧拉角的正方向也要满意这个条件：
第一步是校准，加速度计和陀螺仪我用的是MPU9150内部自校准，磁力计的校准选用如下办法：见附件-磁力计校准
第二步是初始化四元数，常见的轴向界说是绕x轴旋转是Roll，绕y轴旋转是Pitch，绕z轴旋转是Yaw，我的程序中也有这样的界说，不过被我注释掉了，这儿举个别的一种轴向界说来对初始化四元数进行阐明，便利比较，也是我现在正在用的轴界说。
下面咱们来界说绕x轴旋转是Pitch，绕y轴旋转是Roll，绕z轴旋转是Yaw，轴向的正方向如上图相同，不变。
然后经过公式计算出初始化的Roll、Pitch、Yaw，留意加负号确保旋转视点的正方向，如下图：
然后由上面的欧拉角求出初始化四元数，这时要留意旋转次序的不同，公式也不同，大部分旋转次序是Z-Y-X，我的程序里也用的这个次序，在这儿咱们按Z-X-Y的次序来旋转，并得出求四元数的公式以做比较，其旋转矩阵:
第三步便是运用AHRSUpdate算法了，用完今后再依据公式计算出欧拉角，此公式跟旋转次序和旋转运用的轴向有关，咱们的旋转次序是Z-X-Y，且绕Z是Yaw，绕X是Pitch，绕Y是Roll，推到进程如下图：
将上图的方向余旋矩阵C与下图的四元数姿势矩阵做比照，即可求出欧拉角，留意上图的方向余旋矩阵C是跟着咱们对坐标轴的界说改动而改动的，而下图的四元数姿势矩阵是固定的：
能够看出正常的停止状况下，数据的动摇规模是不超越1度的，也不会有漂移。详细作用还需求上机架飞起来今后再做调整。
留意跑108Mhz时，需求修正推迟函数和串口函数，恰当添加一下推迟函数的数值防止I2C通讯失利，修正串口函数是为了防止108Mhz下串口乱码问题，详细修正方法参阅论坛顶置帖子。
快速链接
下载
公司