【更新】GD32F103+MPU9150 四轴飞行器第一步：9DOF姿态融合

gd32gd32

采用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，那么即便有强磁场干扰也不会造成全方位的失控。
按照惯例，上代码，代码里有详细注释：

GD32F103CB IMU-AHRSUpdate.rar (620.55 KB, 下载次数: 179)

2015-5-13 10:20 上传

点击文件名下载附件

关于PID控制中的2个参数Kp和Ki：
首先是理论：
PID算法中，需要整定的系数是Kp（比例系数）,Ki（积分系数）,Kd（微分系数）三个。这三个参数对系统性能的影响如下：
（1） 比例系数 Kp
① 对动态性能的影响 比例系数Kp加大，使系统的动作灵敏，速度加快，Kp偏大，振荡次数加多，调节时间加长。当Kp太大时，系统会趋于不稳定，若Kp太小，又会使系统的动作缓慢；
② 对稳态性能的影响 加大比例系数Kp，在系统稳定的情况下，可以减小静差，提高控制精度，但是加大Kp只是减少静差，不能完全消除。
（2） 积分系数 Ki
① 对动态性能的影响 积分系数Ki通常使系统的稳定性下降。Ki太大，系统将不稳定；Ki偏大，振荡次数较多；Ki太小，对系统性能的影响减少；而当Ki合适时，过渡特性比较理想；
② 对稳态性能的影响 积分系数能消除系统的静差，提高控制系统的控制精度。但是若Ki太小时，积分作用太弱，以致不能减小静差。
（3） 微分系数 Kd
微分控制可以改善动态特性，如超调量减少，调节时间缩短，允许加大比例控制，使静差减小，提高控制精度。但当Kd偏大或偏小时，超调量较大，调节时间较长，只有合适的时候，才可以得到比较满意的过渡过程。
对系数实行“先比例，后积分，再微分”的整定步骤。
（1） 首先只整定比例部分。即将比例系数由小到大，并观察相应的系统响应，直到得到反应快，超调小的响应。
（2） 加入积分环节。整定时首先置积分系数Ki一个较小的值，并将第（1）步中整定的比例系数略为缩小（例如缩小为原值的0.8倍），然后增大Ki，使在保持系统良好动态性能的情况下，静差得到消除。在此过程中，可根据响应的好坏反复改变比例系数与积分系数。
（3） 若使用比例积分调节器消除了静差，但动态过程经反复调整仍不能满意，则可加入微分环节。在整定时，可先置微分系数为0，在第一步的基础上，增大Kd，同时相应地改变比例系数。
以下是在没有起飞之前的一些个人经验：
一般kp我是在1-5之内来回调，太大了感觉角度会超调然后再回来，就是理论中的震荡，太小了感觉角度变化的慢，就是当四轴快速翻转到一定角度时，系统输出的角度还在慢慢的变化，没有很快到达实际的角度；
kp定下来之后再调ki，ki的话，我觉得0.001-0.5都可以，感觉上没什么影响，一般我是每隔0.1或0.3调1次，直到得出的角度比较理想为止。
在不起飞时，一般使用原代码默认的kp=2，ki=0.005即可。