四旋翼姿态解算——互补滤波算法及理论推导

2017年02月24日 17:37:11 阅读数：8763 标签：算法数学传感器数据更多

个人分类： STM32四旋翼无人机

转载请注明出处：http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/
文章链接：http://blog.csdn.net/hongbin_xu/article/details/56846490 或 http://hongbin96.com/111

上次我们讨论了姿态解算基础理论以及几个比较重要的公式的一些推导，如果有不熟悉的请点击这里打开链接。这次来介绍一些实际的姿态解算算法吧！
一般在程序中，姿态解算的方式有两种：一种是欧拉角法，一种是四元数法。这里不介绍欧拉角法，只介绍四元数法，如有兴趣可以去查找相关资料。

互补滤波算法：
顾名思义，是多组数据结合互补，并进行滤波处理稳定输出，得到姿态的算法。而我们使用的传感器就是加速度计和陀螺仪。加速度计用于测量加速度，陀螺仪用于测量角速度。加速度计的静态稳定性更好，而在运动时其数据相对不可靠；陀螺仪的动态稳定性更好，但是静止时数据相对不可靠。所以，我们可以通过加速度计的输出来修正陀螺仪的漂移误差，换句话说，通过加速度计来修正陀螺仪。

这个是我在网上找到的说明互补滤波法的框图：（原图下载：点击这里打开)

首先，我们取定导航坐标系n中标准重力加速度g，定义为，那么将导航坐标系n下的转换为载体坐标系b下的：。
这里用到了，前面在推导基础公式时导出了使用四元数表示的旋转矩阵。
公式如下：

但是我们要用的是，所以还要对做一个矩阵逆变换。由于它是正交矩阵，对于正交矩阵有这个性质：正交矩阵的逆矩阵等于其的转置。所以我们很容易得到：

将上式代入，而且我们已知，所以得到：

接着再定义载体坐标系b中加速度计输出为a，由于前面计算导航坐标系时我们采用的重力加速度是标准重力加速度，所以还需要对其进行归一化，才能继续运算。
设加速度计三个轴的值分别是ax，ay，az。
首先求模：。
归一化：

根据框图的说明再整理一下前面得到的结果：
标准重力加速度从n系转到b系中的矩阵表示：

b系下加速度计测量得到的加速度的矩阵表示：
（备注：这是归一化之后的值）

对和做向量叉乘，即可得到给陀螺仪的校正补偿值e。

表示成矩阵形式更为直观：

然后再使用PI控制器进行滤波，准确地说事消除漂移误差，只要存在误差控制器便会持续作用，直至误差为0。控制的效果取决于P和I参数，分别对应比例控制和积分控制的参数。

这里给出PI控制的公式：

是我们要负反馈给陀螺仪进行校正补偿的值，是比例控制项，是积分控制项，在程序中采用离散累加计算。关于PID控制理论的东西，这里不做赘述。

如框图中所写，接下来将前面得到的补偿值加在陀螺仪输出的数据上进行校正。

跟着框图往下走：

将前面的陀螺仪数据通过四元数微分方程转换为四元数输出。
因为有几个地方我也没搞懂，所以就简单介绍一下四元数微分方程，详细步骤请查阅秦永元的惯性导航一书（第三篇 9.2.3节）：
由于载体的运动，四元数Q是变量，其参数可以表示成关于时间的函数。
使用四元数的三角形式：
为刚体瞬时绕轴转过的角度，为归一化后的位置矢量。
设角速度为：
对四元数的三角表示形式求导：

因为，且
所以：

将上式表示成矩阵形式：

或者
上面的两个公式被称为四元数微分方程。利用陀螺仪的数据进行离散累积便可得到四元数的值，最后再转换成欧拉角形式输出即可。

再附上代码：


#define Kp 10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.008f                          // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f                   // half the sample period采样周期的一半float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{float norm;
//  float hx, hy, hz, bx, bz;float vx, vy, vz;// wx, wy, wz;float ex, ey, ez;// 先把这些用得到的值算好float q0q0 = q0*q0;float q0q1 = q0*q1;float q0q2 = q0*q2;
//  float q0q3 = q0*q3;float q1q1 = q1*q1;
//  float q1q2 = q1*q2;float q1q3 = q1*q3;float q2q2 = q2*q2;float q2q3 = q2*q3;float q3q3 = q3*q3;if(ax*ay*az==0)return;norm = sqrt(ax*ax + ay*ay + az*az);       //acc数据归一化ax = ax /norm;ay = ay / norm;az = az / norm;// estimated direction of gravity and flux (v and w)              估计重力方向和流量/变迁vx = 2*(q1q3 - q0q2);                                             //四元素中xyz的表示vy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;// error is sum of cross product between reference direction of fields and direction measured by sensorsex = (ay*vz - az*vy) ;                                             //向量外积在相减得到差分就是误差ey = (az*vx - ax*vz) ;ez = (ax*vy - ay*vx) ;exInt = exInt + ex * Ki;                                //对误差进行积分eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;// adjusted gyroscope measurementsgx = gx + Kp*ex + exInt;                                              //将误差PI后补偿到陀螺仪，即补偿零点漂移gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt;                                          //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减// integrate quaternion rate and normalise                           //四元素的微分方程q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;// normalise quaternionnorm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;//Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yawQ_ANGLE.Y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitchQ_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}

这段代码网上很多地方都可以看见，匿名四轴（老版本的）的程序也是用的这段。
上面也添加了一些注释，把互补滤波算法的理论部分过了一遍后再来对着代码读一遍，应该不会觉得有多难了吧。程序中就只是把前面推导的公式一个个用上去了而已，如果不懂原理直接使用也不会有太大问题。
程序中的积分运算是采用离散累积的方法运算的。PID控制器只是起到了一个稳定数据消除漂移误差的作用。

希望这些笔记能给更多的人提供帮助，欢迎交流指正。

我的前一篇博文讲的是姿态解算的基础知识和一些相关公式的推导，有兴趣的可以去查看：http://blog.csdn.net/hongbin_xu/article/details/55667899

最后再分享一些我在网上找到的资料，个人觉得对学习很有帮助。
链接：
http://blog.csdn.net/wkdwl/article/details/52119163
http://blog.csdn.net/nemol1990/article/details/16924745
http://blog.csdn.net/super_mice/article/details/45619945
http://www.openedv.com/thread-42657-1-1.html

互补滤波算法及理论推导相关推荐

四旋翼姿态解算——互补滤波算法及理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
AdaBoost算法部分理论推导
一.引言本人在看周志华老师编写的<机器学习>一书的AdaBoost推导部分的内容时,总感觉内容有点跳跃,思路不太流畅,并且书中的一些操作也不是很能理解(例如8.13的泰勒展开),故在参考 ...
彻底剖析激光-视觉-IMU-GPS融合SLAM算法：理论推导、代码讲解和实战
应用背景介绍自主导航是机器人与自动驾驶的核心功能,而SLAM技术是实现自主导航的前提与关键.现有的机器人与自动驾驶车辆往往会安装激光雷达,相机,IMU,GPS等多种模态的传感器,而且已有许多优秀的激 ...
恋爱算法 - 麦穗理论-推导过程
麦穗理论有一天,柏拉图问老师苏个拉底什么是爱情?老师就让他先到麦田里去,摘一颗全麦田里最大最金黄的麦穗来.期间只能摘一次,并且期间只能向前走,不能回头. 柏拉图于是按照老师说的去做了,结果他两手空空 ...
四旋翼姿态解算——梯度下降法理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
贝叶斯滤波（三）贝叶斯滤波算法推导
一. 状态估计(state estimation) 状态估计根据可获取的量测数据估算动态系统内部状态的方法. 贝叶斯滤波也就是在已知观测和可选的控制输入的概率分布下,估计出的状态概率分布. 状态随着时 ...
四轴之互补滤波与四元数算法简单分析
有人问我关于四元数姿态解算算法的分析,每次都解释好久,今日空闲,特发一帖,供大家参考.本分析将结合程序,分析姿态解算思路,由于能力有限,难免有错误之处请谅解,同时希望能够抛砖引玉,得到大神指点.感谢圆 ...
姿态解算进阶：互补滤波（陀螺仪、加速度计、地磁计数据融合）
互补滤波原理: 在四轴入门理论知识那节我们说,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系.陀 ...
Kalman滤波算法详细推导及简单匀速直线运动程序仿真（matlab）
Kalman滤波算法详细推导及简单匀速直线运动程序仿真(matlab) 起初只是知道Kalman滤波的核心公式和会用,没有仔细研究,最近老师让讲Kalman算法,所以系统的学习了该算法,并结合匀速直线 ...

互补滤波算法及理论推导

四旋翼姿态解算——互补滤波算法及理论推导

互补滤波算法及理论推导相关推荐

最新文章

热门文章