姿态解算知识（三）-陀螺仪加速度计6轴数据融合

这么久的惯导总算是没白看，加上一篇博客的指点，这两天把Mahony的九轴数据融合算法看懂了。可惜第二版硬件还没到，磁力计用不了，没法验证效果~今天先总结下陀螺仪和加速度计的六轴数据融合。

版权声明
原创文章，转载请说明出处：sheng-blog.cn
原文出处

加计和陀螺仪都能计算出姿态，但为何要对它们融合呢，是因为加速度计对振动之类的扰动很敏感，但长期数据计算出的姿态可信，而陀螺仪虽然对振动这些不敏感，但长期使用陀螺仪会出现漂移，因此我们要进行互补，短期相信陀螺，长期相信加计。不过，其实加计无法对航向角进行修正，修正航向角需要磁力计，也就是下次要总结的9轴数据融合。

在融合之前先要对传感器原始数据进行一些处理。理想情况下，加速度计水平放置时，XY轴应该是0输出的，仅Z轴输出1个G，因此，我们需要对加速度计进行XY轴的零点校准（注意Z轴可不能一起校准去了~）；同样的，陀螺仪在水平静止放置时各轴输出应为0，因此需对陀螺仪进行三轴的校准。方法就是把机体标准水平静止放置时采集它个一两百次数据求个平均作为校准值保存起来喽，然后工作状态下各轴输出的数据就是采集来的数据减去校准值喽。仅此还不够，陀螺仪不进行滤波还可以接受，但加速度计噪声比较大，所以至少也得来个滑动窗口滤波，我用了20深度的滑动窗口，数据还是有很大波动，不过最后计算出的姿态角只有0.3度左右的抖动（我看大家一般都是建议8深度就够了，所以单滑动窗口滤波效果是没法做到更好了，可以考虑加个卡尔曼滤波？还没研究，不过这样处理器运算量就上去了）。（11.28补充：后面改对加计使用8深度滑动滤波，陀螺仪进行16阶凯撒窗滤波后只有0.1不到的波动，不过凯撒滤波参数的整定还不会，用的别人整定的一组参数，近期继续研究）。滤波效果如图：

接下来上六轴数据融合代码：（代码来源网络，我在前人的基础上再加些注释，稍作改动）

#defineKp 10.0f                        // 这里的KpKi是用于调整加速度计修正陀螺仪的速度
#defineKi 0.008f
#definehalfT 0.001f             // 采样周期的一半，用于求解四元数微分方程时计算角增量
floatq0 = 1, q1 = 0, q2 = 0, q3 = 0;    // 初始姿态四元数，由上篇博文提到的变换四元数公式得来
floatexInt = 0, eyInt = 0, ezInt = 0;    //当前加计测得的重力加速度在三轴上的分量//与用当前姿态计算得来的重力在三轴上的分量的误差的积分
voidIMUupdate(float gx, float gy, float gz, float ax, float ay, float az)//g表陀螺仪，a表加计
{float q0temp,q1temp,q2temp,q3temp;//四元数暂存变量，求解微分方程时要用float norm; //矢量的模或四元数的范数float vx, vy, vz;//当前姿态计算得来的重力在三轴上的分量float ex, ey, ez;//当前加计测得的重力加速度在三轴上的分量//与用当前姿态计算得来的重力在三轴上的分量的误差// 先把这些用得到的值算好float q0q0 = q0*q0;float q0q1 = q0*q1;float q0q2 = q0*q2;float q1q1 = q1*q1;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);//单位化加速度计，ax = ax /norm;// 这样变更了量程也不需要修改KP参数，因为这里归一化了ay = ay / norm;az = az / norm;//用当前姿态计算出重力在三个轴上的分量，//参考坐标n系转化到载体坐标b系的用四元数表示的方向余弦矩阵第三列即是（博文一中有提到）vx = 2*(q1q3 - q0q2);        vy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;//计算测得的重力与计算得重力间的误差，向量外积可以表示这一误差//原因我理解是因为两个向量是单位向量且sin0等于0//不过要是夹角是180度呢~这个还没理解ex = (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由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减//下面进行姿态的更新，也就是四元数微分方程的求解q0temp=q0;//暂存当前值用于计算q1temp=q1;//网上传的这份算法大多没有注意这个问题，在此更正q2temp=q2;q3temp=q3;//采用一阶毕卡解法，相关知识可参见《惯性器件与惯性导航系统》P212q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)*halfT;//单位化四元数在空间旋转时不会拉伸，仅有旋转角度，这类似线性代数里的正交变换norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;//四元数到欧拉角的转换，公式推导见博文一//其中YAW航向角由于加速度计对其没有修正作用，因此此处直接用陀螺仪积分代替Q_ANGLE.Z = GYRO_I.Z; // 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
}

上述代码图解：

新浪博客插代码好不方便，格式全乱了。最后忘了说了，千万别忘了传入的陀螺仪数据需乘一个系数将其转化为弧度制角速度！！！
补充，梯度下降法六轴融合图解：

姿态解算知识（三）-陀螺仪加速度计6轴数据融合相关推荐

AHRS姿态解算说明(加速度+陀螺仪+磁力计原理及原始数据分析)
转载链接:http://www.51hei.com/bbs/dpj-92911-1.html AHRS俗称航姿参考系统,AHRS由加速度计,磁场计,陀螺仪构成,AHRS的真正参考来自于地球的重力场和地 ...
基于STM32F407四旋翼无人机 --- 姿态解算讲解（四元数）（叉积法融合陀螺仪数据和加速度数据）（五）
基于STM32F407四旋翼无人机 --- 姿态解算讲解(五) 姿态解算姿态解算定义欧拉角四元数四元数性质方向余弦矩阵四元数方向余弦矩阵叉积法融合陀螺仪数据和加速度数据叉积运算一阶龙 ...
Pixhawk代码分析-姿态解算篇A
姿态解算篇A 基本知识 1.如何实现控制一个无人机系统的算法主要有两类:姿态检测算法.姿态控制算法.姿态控制.被控对象.姿态检测三个部分构成一个闭环控制系统.被控对象的模型是由其物理系统决定,设计无 ...
PX4姿态解算源码原理理解
PX4源码原理理解一．主要参考资料链接:1.1 取PX4源码一小部分姿态解算来进行讲解姿态解算源码中文注释:https://blog.csdn.net/zouxu634866/article/deta ...
四轴飞行器1.4 姿态解算和Matlab实时姿态显示
四轴飞行器1.4 姿态解算和Matlab实时姿态显示 MPU6050数据读取出来后,经过一个星期的努力,姿态解算和在matlab上的实时显示姿态终于完成了. 1:完成matlab的串口,并且实时通过波 ...
STM32入门笔记(02)：MPU6050、MPU9250、ICM20948及姿态解算（SPL库函数版)
目录 MPU6050 什么是MPU6050? MPU6050的特点 MPU6050框图 MPU6050初始化 MPU6050寄存器电源管理寄存器1(0X6B) 陀螺仪配置寄存器(0X1B) 加速度传 ...
LPMS-IMU姿态解算
参考文章:AHRS姿态解算说明(加速度+陀螺仪+磁力计原理及原始数据分析) AHRS俗称航姿参考系统,AHRS由加速度计,磁场计,陀螺仪构成,AHRS的真正参考来自于地球的重力场和地球的磁场--他的静 ...
android 陀螺仪滤波_Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验
Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验版权声明:本文为博主原创文章,未经博主允许不得转载. 2019年3月20日发布实例效果输出效果: 首先看看本例程XYZ轴的输出效果 ...
十三. 四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜中＞
十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(上) 十三.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(中) 十四.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算 ...

姿态解算知识（三）-陀螺仪加速度计6轴数据融合

姿态解算知识（三）-陀螺仪加速度计6轴数据融合相关推荐

最新文章

热门文章