姿态角(Euler角)pitch yaw roll
飞行器的姿态角并不是指哪个角度,是三个角度的统称。
它们是:俯仰、滚转、偏航。你可以想象是飞机围绕XYZ三个轴分别转动形成的夹角。

地面坐标系(earth-surface inertial reference frame)Sg--------OXgYgZg
<ignore_js_op> 
①在地面上选一点Og
②使Xg轴在水平面内并指向某一方向
③Zg轴垂直于地面并指向地心(重力方向)
④Yg轴在水平面内垂直于Xg轴,其指向按右手定则确定

机体坐标系(Aircraft-body coordinate frame)Sb-------OXYZ
<ignore_js_op>

①原点O取在飞机质心处,坐标系与飞机固连
②x轴在飞机对称平面内并平行于飞机的设计轴线指向机头
③y轴垂直于飞机对称平面指向机身右方
④z轴在飞机对称平面内,与x轴垂直并指向机身下方

欧拉角/姿态角(Euler Angle)
<ignore_js_op> 
<ignore_js_op>

机体坐标系与地面坐标系的关系是三个Euler角,反应了飞机相对地面的姿态。
俯仰角θ(pitch):机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上(抬头)时,俯仰角为正,否则为负。
<ignore_js_op>

偏航角ψ(yaw):
机体坐标系xb轴在水平面上投影与地面坐标系xg轴(在水平面上,指向目标为正)之间的夹角,由xg轴逆时针转至机体xb的投影线时,偏航角为正,即机头右偏航为正,反之为负。
<ignore_js_op>

滚转角Φ(roll):机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角,机体向右滚为正,反之为负。
<ignore_js_op>

首先要明确,MPU6050 是一款姿态传感器,使用它就是为了得到待测物体(如四轴、平衡小车) x、y、z 轴的倾角(俯仰角 Pitch、滚转角 Roll、偏航角 Yaw) 。我们通过 I2C 读取到 MPU6050 的六个数据(三轴加速度 AD 值、三轴角速度 AD 值)经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。

本帖主要介绍三种姿态融合算法:四元数法 、一阶互补算法和卡尔曼滤波算法。

一、四元数法

关于四元数的一些概念和计算就不写上来了,我也不懂。我能告诉你的是:通过下面的算法,可以把六个数据转化成四元数(q0、q1、q2、q3),然后四元数转化成欧拉角(P、R、Y 角)。

虽然 MPU6050 自带的 DMP库可以直接输出四元数,减轻 STM32 的运算负担, 这里在此没有使用,因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的(http://bbs.elecfans.com/forum.ph ... 4&page=1#pid3625735),DMP库需要对 I2C 函数进行修改,如 DMP 库中的 I2C 写:i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]));有4个输入变量,而 STM32 硬件 I2C 的 I2C 写为:void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr),只有 3 个输入量(这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430 单片机写的),所以必须进行修改,但是改固件库是一件很痛苦的事,你们应该都懂。当然,如果你用模拟 I2C 的话,是容易实现的,网上的 DMP 移植几乎都是基于模拟 I2C 的。

 

复制代码

#include<math.h>

#include "stm32f10x.h"

//---------------------------------------------------------------------------------------------------

// 变量定义

#define Kp 100.0f                        // 比例增益支配率收敛到加速度计/磁强计

#define Ki 0.002f                // 积分增益支配率的陀螺仪偏见的衔接

#define halfT 0.001f                // 采样周期的一半

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          // 四元数的元素,代表估计方向

float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差

float Yaw,Pitch,Roll;  //偏航角,俯仰角,翻滚角

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

{

float norm;

float vx, vy, vz;

float ex, ey, ez;

// 测量正常化

norm = sqrt(ax*ax + ay*ay + az*az);

ax = ax / norm;                   //单位化

ay = ay / norm;

az = az / norm;

// 估计方向的重力

vx = 2*(q1*q3 - q0*q2);

vy = 2*(q0*q1 + q2*q3);

vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和

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;

// 调整后的陀螺仪测量

gx = gx + Kp*ex + exInt;

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

// 整合四元数率和正常化

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;

// 正常化四元

norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

q0 = q0 / norm;

q1 = q1 / norm;

q2 = q2 / norm;

q3 = q3 / norm;

Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数

Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv

//Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //此处没有价值,注掉

}

要注意的的是,四元数算法输出的是三个量 Pitch、Roll 和 Yaw,运算量很大。而像平衡小车这样的例子只需要一个角(Pitch 或 Roll )就可以满足工作要求,个人觉得做平衡小车最好不用四元数法。

二、一阶互补算法

MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角(加速度不能得到 Yaw 角),就是说有两组 Pitch、Roll 角,到底应该选哪组呢?别急,先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点,三轴的加速度值没有累积误差,且通过算 tan()  可以得到倾角,但是它包含的噪声太多(因为待测物运动时会产生加速度,电机运行时振动会产生加速度等),不能直接使用;陀螺仪对外界振动影响小,精度高,通过对角速度积分可以得到倾角,但是会产生累积误差。所以,不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角,需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值,把它们结合到一起,进行修正。得到 Pitch 角的程序如下:

 

复制代码

//一阶互补滤波

float K1 =0.1; // 对加速度计取值的权重

float dt=0.001;//注意:dt的取值为滤波器采样时间

float angle;

angle_ax=atan(ax/az)*57.3;     //加速度得到的角度

gy=(float)gyo[1]/7510.0;       //陀螺仪得到的角速度

Pitch = yijiehubu(angle_ax,gy);

float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度

{

angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);

return angle;

}

互补算法只能得到一个倾角,这在平衡车项目中够用了,而在四轴飞行器设计中还需要 Roll 和 Yaw,就需要两个 互补算法,我是这样写的,注意变量不要搞混:

 

复制代码

//一阶互补滤波

float K1 =0.1; // 对加速度计取值的权重

float dt=0.001;//注意:dt的取值为滤波器采样时间

float angle_P,angle_R;

float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度

{

angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);

return angle_P;

}

float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度

{

angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);

return angle_R;

}

单靠 MPU6050 无法准确得到 Yaw 角,需要和地磁传感器结合使用。

三、卡尔曼滤波

其实卡尔曼滤波和一阶互补有些相似,输入也是一样的。卡尔曼原理以及什么5个公式等等的,我也不太懂,就不写了,感兴趣的话可以上网查。在此给出具体程序,和一阶互补算法一样,每次卡尔曼滤波只能得到一个方向的角度。

 

复制代码

#include<math.h>

#include "stm32f10x.h"

#include "Kalman_Filter.h"

//卡尔曼滤波参数与函数

float dt=0.001;//注意:dt的取值为kalman滤波器采样时间

float angle, angle_dot;//角度和角速度

float P[2][2] = {{ 1, 0 },

{ 0, 1 }};

float Pdot[4] ={ 0,0,0,0};

float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度

float R_angle=0.5 ,C_0 = 1;

float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

//卡尔曼滤波

float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy

{

angle+=(gyro_m-q_bias) * dt;

angle_err = angle_m - angle;

Pdot[0]=Q_angle - P[0][1] - P[1][0];

Pdot[1]=- P[1][1];

Pdot[2]=- P[1][1];

Pdot[3]=Q_gyro;

P[0][0] += Pdot[0] * dt;

P[0][1] += Pdot[1] * dt;

P[1][0] += Pdot[2] * dt;

P[1][1] += Pdot[3] * dt;

PCt_0 = C_0 * P[0][0];

PCt_1 = C_0 * P[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = C_0 * P[0][1];

P[0][0] -= K_0 * t_0;

P[0][1] -= K_0 * t_1;

P[1][0] -= K_1 * t_0;

P[1][1] -= K_1 * t_1;

angle += K_0 * angle_err; //最优角度

q_bias += K_1 * angle_err;

angle_dot = gyro_m-q_bias;//最优角速度

return angle;

}

作个总结:三种融合算法都能够输出姿态角(Pitch 和 Roll ),一次四元数法可以输出 P、R、Y 三个倾角,计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。

谈一谈 MPU6050 姿态融合(转)的更多相关文章

  1. MPU6050滤波、姿态融合(一阶互补、卡尔曼)

    前几天做了6050原始数据的串口输出和上位机波形的查看.这篇博客我们来看一下对原始数据的处理. 任务:利用STC89C52RC对MPU6050原始数据进行滤波与姿态融合. 首先我摘抄了一段别人在昨晚这 ...

  2. 从一张图开始,谈一谈.NET Core和前后端技术的演进之路

    从一张图开始,谈一谈.NET Core和前后端技术的演进之路 邹溪源,李文强,来自长沙.NET技术社区 一张图 2019年3月10日,在长沙.NET 技术社区组织的技术沙龙<.NET Core和 ...

  3. 谈一谈Java8的函数式编程(二) --Java8中的流

    流与集合    众所周知,日常开发与操作中涉及到集合的操作相当频繁,而java中对于集合的操作又是相当麻烦.这里你可能就有疑问了,我感觉平常开发的时候操作集合时不麻烦呀?那下面我们从一个例子说起. 计 ...

  4. 谈一谈泛型(Generic)

    谈一谈泛型 首先,泛型是C#2出现的.这也是C#2一个重要的新特性.泛型的好处之一就是在编译时执行更多的检查. 泛型类型和类型参数 ​ 泛型的两种形式:泛型类型( 包括类.接口.委托和结构 没有泛型枚 ...

  5. 谈一谈Elasticsearch的集群部署

      Elasticsearch天生就支持分布式部署,通过集群部署可以提高系统的可用性.本文重点谈一谈Elasticsearch的集群节点相关问题,搞清楚这些是进行Elasticsearch集群部署和拓 ...

  6. 谈一谈iOS事件的产生和传递

    谈一谈iOS事件的产生和传递 1.事件的产生 发生触摸事件后,系统会将该事件加入到一个由UIApplication管理的事件队列中. UIApplication会从事件队列中取出最前面的事件,并将事件 ...

  7. 谈一谈对MySQL InnoDB的认识及数据库事物处理的隔离级别

    介绍: InnoDB引擎是MySQL数据库的一个重要的存储引擎,和其他存储引擎相比,InnoDB引擎的优点是支持兼容ACID的事务(类似于PostgreSQL),以及参数完整性(有外键)等.现在Inn ...

  8. 谈一谈APP版本号问题

    如题:谈一谈APP版本号问题 为什么要谈这个问题,周五晚上11~12点,被微信点名,说APP有错,无效的版本号,商城无法下单.我正在准备收拾东西,周末回老家,结果看到这样问题,菊花一紧.我擦,我刚加的 ...

  9. 谈一谈深度学习之semantic Segmentation

    上一次发博客已经是9月份的事了....这段时间公司的事实在是多,有写博客的时间都拿去看paper了..正好春节回来写点东西,也正好对这段时间做一个总结. 首先当然还是好好说点这段时间的主要工作:语义分 ...

随机推荐

  1. [原创]Burp Suite web应用程序渗透测试神器

    [原创]Burp Suite web应用程序渗透测试神器 一 Burp Suite介绍 Burp Suite是Web应用程序测试的最佳工具之一,其多种功能可以帮我们执行各种任务.请求的拦截和修改,扫描 ...

  2. c++中函数的参数传递,内联函数和默认实参的理解

    1.参数传递 1)函数调用时,c++中有三种传递方法:值传递.指针传递.引用传递. 给函数传递参数,遵循变量初始化规则.非引用类型的形参一相应的实参的副本初始化.对(非引用)形参的任何修改仅作用域局部 ...

  3. 跟着柴毛毛学Spring(3)——简化Bean的配置

    通过前面的学习.我们会感觉到对于一个有较多Bean的大项目,Spring的配置会比較复杂. 那么接下来我们就介绍怎样简化Spring的配置. 简化Spring的配置主要分为两类: 1. 自己主动装配 ...

  4. 类中添加log4j日志

    在编写代码的时候需要随时查看工作日志,查看工作日志的好处就是随时能检查出错误.所以我一般就需要在编写代码的前期添加工作日志,以便更好的查看相关错误输出. 以一个springmvc小demo为例子  主 ...

  5. lua 源码分析之线程对象lua_State

    lua_State 中放的是 lua 虚拟机中的环境表.注册表.运行堆栈.虚拟机的上下文等数据. 从一个主线程(特指 lua 虚拟机中的线程,即 coroutine)中创建出来的新的 lua_Stat ...

  6. server后台TCP连接存活问题

    公司的server后台部署在某一个地方,接入的是用户的APP,而该地方的网络信号较差,导致了server后台在执行一段时间后用户无法接入,那边的同事反馈使用netstat查看系统.存在较多的TCP连接 ...

  7. SNF软件开发机器人-子系统-功能-功能类型(普通表改为树型表)

    功能类型(普通表改为树型表) 功能类型分为普通功能和树形功能.因为资源表中不存在ParentId字段,故只能将多表的主表改为树型表. 1.效果展示: 2.使用说明: (1)当需要用一张普通的资源表显示 ...

  8. atitit 如何才能保持基业长青,建立万年企业v2 q66.docx

    1. 价值观 1 2. 分权 民主化 扁平化管理 1 3. 剥离经济部门,,降低经济的重要性 .超越利润的追求 1 4. 构建愿景 1 5. 强大的团队文化 企业文化 宗教文化 .教派般的文化 1 1 ...

  9. 【Nginx】Nginx在Linux下的入门介绍

    Nginx的安装 下载.解压 从Nginx下载安装包,我下的是nginx-1.8.0.tar.gz.解压后的目录为: [root@blog third_package]# tar -zxf nginx ...

  10. curl命令例解

    curl -i --url "https://open.abc.com/ddn/purge/ItemIdReceiver" \-X "POST" \-u &qu ...