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

姿态解算篇D

基础知识

研究多旋翼无人机前期主要需要了解其气动布局和复杂的动力学模型，然后就是姿态解算和控制器的设计。为了实现精确四旋翼飞行器的姿态估计，首先就是需要了解各传感器采集的数据和误差存在的原因，然后就是各种数学运算（都是基于一定的数学平台），比如DCM随时间的变化、四元数微分方程、四元数随时间的变化、重正交化等等。

在理解构建四旋翼动力学模型时，需要了解它是属于四输入六输出的欠驱动系统，欠驱动主要就是输入只值靠四个电机的拉力实现控制角速度、角度、上升速度、下降速度、上升距离和下降距离。动力学模型还在研究中。

实现四旋翼飞行器的自主飞行主要就是靠姿态解算和姿态控制。姿态控制是四旋翼飞行器实现各项工作的核心，位置控制建立在完善的姿态控制的基础上。前面已经介绍过，仅仅依靠陀螺仪积分获取角度值是不可靠的，由于陀螺仪由于其本身的固有的drift，积分带来的误差随时间的积累越来越大；而加速度计又容易受到噪声和震动的干扰。

因此在此基础上使用各种算法对两种数据进行融合，以获取较为理想的姿态数据。比较常见的融合算法有EKF（Extended Kalman Filter）、UKF（Unscented Kalman Filter）、梯度下降法、CKF（Cubature Kalman Filter）、PF（Particle Filter）和CF（Complementary Filter）。

其中mahony姿态解算算法主要就是基于CF设计的数据融合滤波器，该算法需要重点掌握其原理思想其分为AHRS（陀螺仪、加速度计、磁力计）和IMU（陀螺仪、加速度计）两种。

目前研究多集中在非线性控制领域，由于非线性控制对模型准确性有较强的依赖，在模型误差存在的条件下，PID控制更加实用，所以现有的基于mahony互补滤波器设计的算法中大多数都是基于PID反馈控制器的。

接下来就是使用Simulink进行控制器建模仿真，也在研究中。

代码算法分析

1、传感器的数值模型（mahony互补滤波器）

首先还是贴图吧，word编辑的公式还是搞不进来。

2、互补滤波器的设计（基于四元数）

这里讲的互补滤波就是在短时间内采用陀螺仪得到的角度做为最优值，定时对加速度采样来的加速度值进行取平均值来校正陀螺仪的得到的角度。短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，这就是互补了，不过加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号。

例如，加速度计测加速度值，其动态响应较慢，在高频时信号不可用，所以可通过低通滤波器抑制高频干扰；陀螺仪响应快，积分后可测倾角，不过由于零点漂移等，在低频段信号不好，通过高通滤波器可抑制低频干扰。将两者结合，就将陀螺仪和加速度计的优点融合起来，得到在高频和低频都较好的信号，互补滤波需要选择切换的频率点，即高通和低通的频率。

在深入一点

1、对mahony互补滤波算法和PX4Firmware源代码中的姿态解算算法的一点见解

首先是最原始mahony滤波算法中的加速度计数据修正陀螺仪数据，源码如下。

1.  // measured by Accelerate sensor
2.    ax = ax / norm;  
3.    ay = ay / norm;  
4.    az = az / norm;  
5.  
6.  // estimated direction of gravity  (v)   
7.    vx = 2*(q1q3 - q0q2);  
8.    vy = 2*(q0q1 + q2q3);  
9.    vz = q0q0 - q1q1 - q2q2 + q3q3;  
10.  
11. // error is sum ofcross product between reference direction of fields and direction measured by Accelerate sensor   
12.   ex = ay*vz - az*vy ;  
13.   ey = az*vx - ax*vz ;  
14.   ez = ax*vy - ay*vx ;  
15.  

对上述代码的理解：[ax,ay,az] 表示由机体上的加速度计实际测量到的加速度数据，根据数据融合修正原理，加速度计在低频的情况下效果比较好，陀螺仪在高频情况下效果好；在低频下，忽略机体的运动加速度，即加速度计测量到的数据就是近似的纯重力加速度向量。[vx,vy,vz]表示由陀螺仪积分以后的姿态矩阵中获取的重力加速度向量（如何得到的不再赘述）。并且这两个向量都是在机体坐标系中，两个重力加速度向量做外积得到的向量就是所谓的误差向量（12、13、14行代码），且叉积的大小和陀螺仪的积分误差成正比，用该误差向量修正下一次的陀螺仪数据即可达到用加速度计修正陀螺仪数据误差的效果。

然后就是PX4Firmware源码中的加速度计数据修正陀螺仪数据，源码如下。

1.  // Accelerometer correction    
2.  // Project 'k' unit vector of earth frame to body frame    
3.  // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); n系到   
4.  // b系  
5.  // Optimized version with dropped zeros    
6.      Vector<3> k(    
7.          2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),    
8.          2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),    
9.          (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))    
10.     );    
11.  corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;  
12.    

该算法中，Vector<3> k就是由陀螺仪积分以后的姿态矩阵中获取的重力加速度向量（等同于mahony算法中的[vx,vy,vz]）；这部分都类似，主要的不同就是下面的加速度计测量数据的处理，首先通过加速度计获取总的加速度向量，然后同过GPS或者空速管获取运动加速度向量，然后通过这两个向量之差求取纯重力加速度，即：总的加速度 =运动加速度 + 重力加速度（此处考虑了低频情况下的运动加速度，修正效果更好）。_w_accel代表权重，即类似于Kp。

上述代码中的“%”运算符定义为叉积运算：

1.  Vector<3> operator %(const Vector<3> &v) const {    
2.          return Vector<3>(    
3.                     data[1] * v.data[2] - data[2] * v.data[1],    
4.                     data[2] * v.data[0] - data[0] * v.data[2],    
5.                     data[0] * v.data[1] - data[1] * v.data[0]    
6.                 );    
7.      }    
8.  };