< >
Home » Pixhawk代码分析 » Pixhawk代码分析-姿态解算篇F

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

姿态解算篇F

前言

在多旋翼进行姿态估计的过程中，最简单的就是直接使用gyro测量角速度进行积分求取欧拉角（RPY），但是由于gyro自身存在的bias和drift，导致直接测量过程随着时间的推荐变得越来越不精确。

所以，许多研究者想到了使用加速度计和磁力计测量重力加速度和地球磁场强度对gyro的bias和drift进行补偿修正（不是eliminate gyro’s bias and drift，该叙述是错误的，加速度计和磁力计不可能排除gyro的bias和drift）。常用的方法就是CF、GD、EKF。

但是由于磁力计测量地球磁场时极易被干扰（distortion），而且在补偿修正时磁场时无法修正RP的。所以，又有很多研究者想到了分开修正补偿求取欧拉角的方法，即通过加速度计和gyro一起求取RP，用磁力计和gyro一起修正补偿求取Yaw。该类解决方法中常见的就是two_stage EKF、two_stage GD、GD_EKF。

代码分析

该篇博文主要结合源代码介绍一下GD的实现过程，不再赘述理论的推导过程。说了那么多主要还是引出GD（gradient descent），关于该算法的介绍大家自行阅读madgwick的论文吧，该部分就不叙述算法推导过程的。 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。还有一个写的比较详细的关于GD介绍的blog，其中大部分都是论文翻译，英语不好的可以结合着看。链接：http://blog.csdn.net/nemol1990/article/details/23102643

IMUfilter::IMUfilter(double rate, double gyroscopeMeasurementError){  
    firstUpdate = 0;  
    //Estimated orientation quaternion elements with initial conditions.  
    SEq_1 = 1;  
    SEq_2 = 0;  
    SEq_3 = 0;  
    SEq_4 = 0;  
    //Sampling period (typical value is ~0.1s).  
    deltat = rate;  
    //Gyroscope measurement error (in degrees per second).  
    gyroMeasError = gyroscopeMeasurementError;  
    //Compute beta.  
    beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));  
}  
   
void IMUfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) {  
   
    //Local system variables.  
    //Vector norm.  
    double norm;  
    //Quaternion rate from gyroscope elements.  
    double SEqDot_omega_1;  
    double SEqDot_omega_2;  
    double SEqDot_omega_3;  
    double SEqDot_omega_4;  
    //Objective function elements.  
    double f_1;  
    double f_2;  
    double f_3;  
    //Objective function Jacobian elements.  
    double J_11or24;  
    double J_12or23;  
    double J_13or22;  
    double J_14or21;  
    double J_32;  
    double J_33;  
    //Objective function gradient elements.  
    double nablaf_1;  
    double nablaf_2;  
    double nablaf_3;  
    double nablaf_4;  
   
    //Auxiliary variables to avoid reapeated calcualtions.  
    double halfSEq_1 = 0.5 * SEq_1;  
    double halfSEq_2 = 0.5 * SEq_2;  
    double halfSEq_3 = 0.5 * SEq_3;  
    double halfSEq_4 = 0.5 * SEq_4;  
    double twoSEq_1 = 2.0 * SEq_1;  
    double twoSEq_2 = 2.0 * SEq_2;  
    double twoSEq_3 = 2.0 * SEq_3;  
   
    //Compute the quaternion rate measured by gyroscopes.  
    SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;  
    SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;  
    SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;  
    SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;  
   
    //Normalise the accelerometer measurement.  
    norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);  
    a_x /= norm;  
    a_y /= norm;  
    a_z /= norm;  
   
    //Compute the objective function and Jacobian.  
    f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;  
    f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;  
    f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;  
    //J_11 negated in matrix multiplication.  
    J_11or24 = twoSEq_3;  
    J_12or23 = 2 * SEq_4;  
    //J_12 negated in matrix multiplication  
    J_13or22 = twoSEq_1;  
    J_14or21 = twoSEq_2;  
    //Negated in matrix multiplication.  
    J_32 = 2 * J_14or21;  
    //Negated in matrix multiplication.  
    J_33 = 2 * J_11or24;  
   
    //Compute the gradient (matrix multiplication).  
    nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;  
    nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;  
    nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;  
    nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;  
   
    //Normalise the gradient.  
    norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);  
    nablaf_1 /= norm;  
    nablaf_2 /= norm;  
    nablaf_3 /= norm;  
    nablaf_4 /= norm;  
   
    //Compute then integrate the estimated quaternion rate.  
    SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;  
    SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;  
    SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;  
    SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;  
   
    //Normalise quaternion  
    norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);  
    SEq_1 /= norm;  
    SEq_2 /= norm;  
    SEq_3 /= norm;  
    SEq_4 /= norm;  
    
}  
void IMUfilter::reset(void) {  
   
    firstUpdate = 0;  
    //Estimated orientation quaternion elements with initial conditions.  
    SEq_1 = 1;  
    SEq_2 = 0;  
    SEq_3 = 0;  
    SEq_4 = 0;  
}

重点来了

1、首先就是初始化函数，包含四元数、采样周期、beta。源代码如下：

IMUfilter::IMUfilter(double rate, double gyroscopeMeasurementError){  
    firstUpdate = 0;  
    //Estimated orientation quaternion elements with initial conditions.  
    SEq_1 = 1;  
    SEq_2 = 0;  
    SEq_3 = 0;  
    SEq_4 = 0;  
    //Sampling period (typical value is ~0.1s).  
    deltat = rate;  
    //Gyroscope measurement error (in degrees per second).  
    gyroMeasError = gyroscopeMeasurementError;  
    //Compute beta.  
    beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));  
}

通过上述代码可以了解到，采样周期就是整体代码中该算法的执行频率的倒数。该部分比较重要的一点就是beta的计算，由上述可知beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0))，主要就是通过gyro的芯片手册查找到gyroscopeMeasurementError，然后转换成弧度表示并乘一个权重。Beta的由来可以参考madgwick的论文的3.6节filter gain部分。论文中给出的介绍如下：Thefilter gain β represents all mean zerogyroscope measurement errors, expressed as the magnitude of a quaternionderivative. The sources of error include: sensor noise, signal aliasing, quantisation errors, calibration errors, sensor miss-alignment,sensor axis nonorthogonality and frequency response characteristics.建议大家还是好好阅读madgwick的论文，吃透它。

2、然后捏，一系列的参数变量，一个个自己去对吧，前提是需要了解整个过程是什么样的，梯度下降法使用的就是梯度下降的理论，所以了解梯度下降就理解了该算法的精髓（梯度的作用和求取过程）。

//Local system variables.  
    //Vector norm.  
    double norm;  
    //Quaternion rate from gyroscope elements.  
    double SEqDot_omega_1;  
    double SEqDot_omega_2;  
    double SEqDot_omega_3;  
    double SEqDot_omega_4;  
    //Objective function elements.  
    double f_1;  
    double f_2;  
    double f_3;  
    //Objective function Jacobian elements.  
    double J_11or24;  
    double J_12or23;  
    double J_13or22;  
    double J_14or21;  
    double J_32;  
    double J_33;  
    //Objective function gradient elements.  
    double nablaf_1;  
    double nablaf_2;  
    double nablaf_3;  
    double nablaf_4;  
    //Auxiliary variables to avoid reapeated calcualtions.  
    double halfSEq_1 = 0.5 * SEq_1;  
    double halfSEq_2 = 0.5 * SEq_2;  
    double halfSEq_3 = 0.5 * SEq_3;  
    double halfSEq_4 = 0.5 * SEq_4;  
    double twoSEq_1 = 2.0 * SEq_1;  
    double twoSEq_2 = 2.0 * SEq_2;  
    double twoSEq_3 = 2.0 * SEq_3;

3、然后捏，由w更新四元数，这个大家都很熟悉吧，一阶毕卡，不在赘述。

//Compute the quaternion rate measured by gyroscopes.  
    SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;  
    SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;  
    SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;  
    SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;

4、然后捏，构造目标函数（objectivefunction）和雅克比矩阵，并用它们求取梯度。

//Compute the objective function and Jacobian.  
    f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;  
    f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;  
    f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;  
    //J_11 negated in matrix multiplication.  
    J_11or24 = twoSEq_3;  
    J_12or23 = 2 * SEq_4;  
    //J_12 negated in matrix multiplication  
    J_13or22 = twoSEq_1;  
    J_14or21 = twoSEq_2;  
    //Negated in matrix multiplication.  
    J_32 = 2 * J_14or21;  
    //Negated in matrix multiplication.  
    J_33 = 2 * J_11or24;

看着上面的是不是比较晕呢？不用怕，结合下面的整体部分看着就不晕了。

请输入图片描述

5、然后捏，求梯度：梯度公式为▽f=transpose(J)*f

//Compute the gradient (matrix multiplication).  
nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;  
nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;  
nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;  
nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;

6、然后捏，归一化，不管用什么算法进行姿态估计，都别忘记归一化（DCM还需要正交化）。

//Normalise the gradient.  
    norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);  
    nablaf_1 /= norm;  
    nablaf_2 /= norm;  
    nablaf_3 /= norm;  
    nablaf_4 /= norm;

7、再然后捏，算法融合，即把一阶毕卡得到的姿态四元数和GD得到的姿态四元数融合，为什么这么做？为什么beta用作权重，移步madgwick的论文吧，里面有详细介绍，方法很巧妙。

//Compute then integrate the estimated quaternion rate.  
    SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;  
    SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;  
    SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;  
    SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;  
    最后，还是归一化，不写了~~~

总结

GD没什么东西，看懂理解了就OK了，主要是学习这样思想。就像开篇里面介绍的，现在大部分研究者都是集中精力实现各种姿态估计算法的解算算法（即杂交）。论文看的多了，感觉都一样，百分之50~60的内容都是在阐述一样古老的东西，在最后的最后才引出作者改变的一点东西，比如自适应算法，就是想一种方式实现对步长的取优变化（动态即自适应）。

一部大部分时间还是在学习EKF，说简单它也简单就是五步法；说难也难，PQR的确定问题。还没有看五步法的由来，都是直接看应用案列，感觉这样理解的更快。I think the easiest way to understand the EKF is to start off with an example~~~~

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: pixhawk代码分析, pixhawk姿态解算