Pixhawk代码分析-姿态解算篇E

姿态解算篇E

前言

最近研究了现有的几种关于姿态解算的实现算法，理论大都相通，请各位耐心看完。 希望阅读本部分的读者对姿态解算算法有一定基础的认识，该部分涉及了常用的三种姿态解算算法：Explicit Complementary Filter(ECF)、Extended Kalman Filter(EKF)、Gradient Descent Filter(GD)。本篇只做大致介绍，不结合代码分析，后续会结合代码分析算法单独出文章。

现有的开源的姿态解算算法的代码实现在如下地址都可以找到，里面涉及了很多的姿态解算算法，慢慢的一个个的扣吧。

链接：https://developer.mbed.org/users/gke/code/UAVXArm-GKE/file/90292f8bd179/attitude.c

姿态解算

经过近一段时间的研究，主要是关于多旋翼无人机姿态解算算法的研究，开源PX4源代码里面默认使用的是基于mahony的互补滤波算法（explicit complement filter）进行姿态解算的；还有一套备用姿态解算算法是基于kalman的EKF（extended kalman filter）；然后呢，其实还有一套PX4源代码中尚未涉及的基于madgwick的梯度下降算法（gradient descent）。

1.1、三套不同的算法各有优势可有优缺点。

三套算法都可以使用，很多公司偏向于EKF，所以对EKF的要求还是比较高的，慢慢撸EKF吧，不然工作就愁了。小四轴用的比较多的还是互补滤波算法，尤其是大学生参加比赛也好，个人爱好也好，基于STM32自己制作小四轴的大多基于这套（我见过的很多都是用的CF，可能也有很多用的别的算法吧，不用太纠结这个）。梯度下降用的属于最少的了吧。最后出场的就是大头了：EKF；啥都别说了，好好学习它没错。

1.1.1、mahony算法

Mahony算法即所谓的互补滤波算法，通过PID反馈控制器把误差量反馈补偿修正陀螺仪的误差。Mahony的互补滤波算法是基于IMU系统的，即陀螺仪和加速度计，过程中不涉及磁力计的修正。而我们使用过程中一般都是AHRS系统（即MARG：Magnetic、Angular Rate、Gravity），这个就需要寻求另外的算法或者基于mahony算法进行改进。PX4源代码中就是使用的改进的mahony算法，即加入了磁力计数据和GPS数据进行姿态解算。该算法是最简单和最容易实现的算法，PX4姿态解算默认的就是使用的这个。但是滤波器截止频率不好确定且与采样频率有关，Kp大截止频率就大。

关于这一块，现在研究的比较多就是如何实现自适应调参。固定的参数不能获得所有情况下的最优运动姿态角，可以设计参数可调的自适应算法在不同运动状态下进行调节参数的大小。其参数调节规则为：正常运动状态情况下，Kp和Ki值取为系统初始化值；当运动体具有较大运动加速度或姿态变化剧烈时，应选择较小的Kp值（可取其初始化值的0.1倍），而Ki值应在同一数量级内适当取大一点。具体取值需根据实际应用系统选取。

Mahony论文：《AComplementary Filter for Attitude Estimation of a Fixed-Wing UAV》和《NonlinearComplementary Filters on the Special Orthogonal Group》

Mahony团队发表了太多的论文了，有兴趣的可以到researchgate查看：

链接：https://www.researchgate.net/profile/Robert_Mahony/publications/3

1.1.2 EKF算法

EKF俗称扩展卡尔曼滤波算法，优点不言而喻。补充一句：Kalman还活着，还活着呢~~~

EKF是出了名的计算量大，因为算法实现过程在不范是各种矩阵运算，对处理器的运算速度和精度要求很高对MCU的主频要求比较大，再加上系统的非线性，难以建立稳定可靠的状态方程以及合适的过程噪声协方差阵Q以及测量噪声协方差阵R。一言以蔽之，就是CPU计算负荷大。但是理解了KF以后，对整合滤波过程还是比较清晰的，很多公司都要求必须会EKF算法，优势就是在于处理过程中的预测和修正，其中一点就是可以估计陀螺仪的bias；在算法的计算处理过程中每一次都在不停的修正K和bias，相当于动态自适应算法了，主要就是调好Q、R。所以得把EKF搞懂，虽然比较难，但是必须知难而上。后续会根据直接KF求欧拉角和EKF求取四元数来更新blog，kalman filter相当有研究价值。

相关论文：《A Double-Stage Kalman Filter for Orientation TrackingWith an Integrated Processor in 9-D IMU》

1.1.3、梯度下降法

The Gradient Descent是madgwick在2010搞出来的一套算法，其核心思想是将用加速度计和磁力计通过梯度下降法得到的姿态四元数与由陀螺仪积分得到的姿态，进行线性融合，得到最优的姿态。在算法实现过程中需要了解目标函数的建立，梯度下降法是根据目标函数求取误差向量的最小值的，这一点在madgwick的论文中尚未提及，应该是觉得默认大家都懂这个吧。

该算法首先是只用陀螺仪数据根据一阶毕卡更新四元数，然后才是梯度下降法对加速度数据和磁力计数据进行处理以便求出最小的误差四元数，最后根据权重值（线性）把两者融合到一起得到最终的姿态四元数。使用梯度下降法就是对加速度数据和磁力计数据和相应参考向量求取误差量，这一部分和mahony_base算法非常类似，mahony_base是利用向量的叉乘求取量测值和参考向量的误差的，而梯度下降法是根据反梯度方向迭代求取最小误差的（理论基础就是构造目标函数使用梯度下降法求取误差向量的最小值）。过程中最重要就是参数β需要通过实验进行确定gyro的测量噪声（measurement error）。

该算法适用于IMU和AHRS系统，其明显特点就是计算量小（computationally inexpensive），低频有效性（比如10hz）。Madgwick说该算法最后的精度比kalman_base算法要好，我也不知道，没实际测试过；基于梯度下降的数据融合算法能够显著降低对处理器速度的要求。

Madgwick论文：《An efficient orientation filter for inertial and inertial magneticsensor arrays》和《Estimation of IMU and MARG orientation using agradient descent algorithm》。梯度wiki：https://en.wikipedia.org/wiki/Gradient_descent

算法代码试下：http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/

结论

上述三种算法都是以陀螺仪采集的角速度数据位主要控制量，通过不同的算法处理加速度数据和磁力计数据对由陀螺仪估计的姿态进行修正补偿。比如mahony_base算法是通过构造PID反馈控制器实现对由陀螺仪估计的的姿态修正的，主要过程是基于重力或者重力和磁力参考向量修正陀螺仪数据，最后根据一阶毕卡（或多阶）更新姿态四元数；EKF是通过量测方程实现对由陀螺仪估计的姿态角修正的；Gradient descent是通过两种算法的融合来进行姿态解算的，首先是只用陀螺仪数据根据一阶毕卡更新四元数，然后才是梯度下降法对加速度数据和磁力计数据进行处理以便求出最小的误差四元数，最后根据权重值把两者融合到一起得到最终的姿态四元数。如果kalman算法是自适应的，那解算精度就比梯度下降算法强了。

综上所述，姿态解算不过就是利用陀螺仪的高动态性能估计实时的姿态，但是由于陀螺仪固有的缺陷问题，导致随着时间的漂移实时的姿态估计的越来越不准确，所以呢，找一个也能估计姿态的sensor来修正一下，但是呢，总不能用两个陀螺仪吧，可以要找一个可以和陀螺仪固有缺陷相互补的sensor，那就是加速计了，无漂移，低频特性好。

那么为何不直接只用加速度计进行姿态估计呢？原因就在于它的高频特性太烂，响应时间久，比如想实现一个3°角度变化量，多旋翼飞行速度很快，加速计响应时间来个上百ms的话，飞机早掉地上上升天了。这个也是为什么PX4里面对姿态控制部分分为两级PID控制的原因所在吧，内环控制角速度响应速度极快，外环控制角度响应速度相对较慢，但是有内环在接近实时的PID反馈修正，所以就不怕飞机升天了。