|
本帖最后由 zkcaptain 于 2014-6-19 18:11 编辑 —————————————————————————————————————————————————————————————— 请各位不要私信密我了，没空照顾那么多地方，就在帖子里回吧，我尽量回复，说的对其他人看了也受益，说的不对，其他人 也会指正，对大家都有好处。 —————————————————————————————————————————————————————————————— 2014年6月16日更新： PID+乱七八糟心得部分，内有完整代码和试飞视频，见该帖：http://bbs.21ic.com/forum.php?mo ... 1&extra=#pid4167670 --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- 自己摸索着做的一个小应用，希望有朝一日能够飞起来，欢迎在电机、电调、算法、动力学等方面有研究的朋友一起讨论。 做四轴飞行器也是为了好玩，目前我只完成了第一步：基于加速度计、陀螺仪和磁力计的姿态融合，能够实时更新四元数，并由四元数解算出姿态角。接下来才是更重要的，比如上各种什么机架、电调、电机、螺旋桨之类的，然后写各种PID控制代码，系统整合以后还要调试各种参数，抗干扰，使系统稳定，最后还可能要加各种应用器件，目前对这方面了解的还不多。 算法参考自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内部自校准，磁力计的校准采用如下方法： 第二步是初始化四元数，常见的轴向定义是绕x轴旋转是Roll，绕y轴旋转是Pitch，绕z轴旋转是Yaw，我的程序也是这样定义的，详细说明见程序，这里举个另外一种轴向定义来对初始化四元数进行说明，方便比较。 下面我们来定义绕x轴旋转是Pitch，绕y轴旋转是Roll，绕z轴旋转是Yaw，轴向的正方向如上图一样，不变。 先对加速度计和磁力计的数据进行处理，得到init_xx来供我们使用如下图： 然后通过公式计算出初始化的Roll、Pitch、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之间相乘的顺序不能随意变，在前的先计算，在后的后计算相乘的公式如下图： 至此初始化四元数完成。 第三步就是使用IMUUpdate算法了，用完以后再根据公式计算出欧拉角，此公式跟旋转顺序和旋转使用的轴向有关，我们的旋转顺序是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是随着我们对坐标轴的定义变化而变化的，而下图的四元数姿态矩阵是固定的： 最后一步就是求出欧拉角，公式如下图： 至此，算法完成，参考如下： //本程序的读写mpu9150部分是在作者_spetrel的基础上修改而来； //本程序Yaw的互补滤波出自作者john800422； //本程序采用的gyro accel融合算法出自作者Madgwick； 在此感谢以上作者的无私分享和帮助！ 附上自己的代码和AHRS IMU原始代码： ---------------------------------------------------------------------------------------------------------------------------- 2013年11月28日更新-采用AHRSUpdate算法进行姿态融合： 这段时间一直没有太多时间搞这玩意，不过苦恼多日的AHRSUpdate算法问题终于解决了，该算法直接融合9轴数据，包括加速度计、陀螺仪和磁力计，其中0°<yaw<360°, -90°<pitch<+90°, -180°<roll<180°，并且我定义：Yaw北偏西为正，pitch往上为正，roll“右翼”下沉为正。传感器坐标轴：绕y旋转是roll，绕x旋转为pitch，绕z旋转为yaw。代码里只有姿态更新，更新频率接近400Hz，这还只是跑72Mhz频率下的数据，想要跑108M可以自己修改。 说下改进： 1.由于IMUupdate算法只融合了加速度计和陀螺仪的数据，因此我用了互补滤波来融合磁力计以修正偏航角，但是这种互补滤波有个小问题，就是假如定义偏航角的范围是0-360度，那么当机头大概从北偏西1度转到北偏东364度时，机头会经过0度(360度)这个点，那么这时，yaw不会直接1-0-364这么变化，而是会被逆向积分从1-20-180-270-364这么转一圈，这是个不好的现象，实验了半天也没有解决，而AHRSupdate很好的解决了这个问题。 2.放弃了DMP输出，DMP输出会有部分数据丢失现象，输出频率只有200Hz，虽然也够用，但还是太低了，采用I2C直接读取raw数据，做了简单校准，下一步打算边做精确校准边搞PID控制，然后再加上加速度计的平滑滤波和磁力计的椭圆拟合校准，最好还有陀螺仪的自动零漂校准。 3.考虑到电机的转动频率是100Hz以上，因此我采用加速度计的低通滤波(DLPF)为98Hz，可以有效滤掉电机振动带来的影响，结合平滑滤波效果应该不错。 说下AHRSUpdate算法的缺点： 由于把磁力计的数据融合到了pitch和roll里面，那么当磁力计突然收到强磁场干扰时，会导致四轴失控。亲测过拿手机等物品靠近飞控板时，pitch、roll和yaw的数据有明显变化，变化程度跟物品具有的磁场强度成正比，不过应该是有算法可以滤掉这些固定的外部干扰的。 而IMUupdate只把磁力计融合进了yaw，那么即便有强磁场干扰也不会造成全方位的失控。 按照惯例，上代码，代码里有详细注释： |
|
|
