MPU6050 获取角度理论推导(一)

当你搜到这篇文章的时候说明你已经在做陀螺仪的项目了。

那么陀螺仪具体的东西应该不用多说,他其实就是一个获取原始数据然后通过DMP或者MCU的计算处理,

然后得到角度的一个传感器。

MPU6050内部集成了一个陀螺仪一个加速传感器还有DMP计算单元。

陀螺仪是提供实时的三轴角速度,其中三轴分辨是Pitch, Roll,Yaw。简单来说我们视之为XYZ三轴。

还有三轴加速度,加速计时用来获取三轴方向的实时加速度值。

所以三轴陀螺仪+三轴加速计  就是6轴数据的融合了。

如果再配合一个三轴的磁力计,那么就变成9轴数据融合了。

这里先和大家说一下自己的亲身体验。

9轴的陀螺仪,带有磁力计。可以维持Yaw方向不漂移。但是对磁场的要求相对较高。放到实际产品中的时候要考虑如何避免

产品中磁场的影响。这样才能保证稳定性。

6轴的陀螺仪虽然Z轴会漂,但是使用软件滤波的方法还是可以维持一段时间稳定性的。且对磁场没有要求。

 

接下来我们来剖析一下陀螺仪6轴和9轴的融合原理。

原文我忘记是哪里的了,地址就不贴了(找到了再补上),也是参考别人的。

 

  • 姿态角解算---理论推导

 

姿态航向参考系统(AHRS)

首先我们需要了解两个坐标系:

导航坐标系(n): 导航坐标系 n 指的是以地球为参考的坐标系,定义为东北天右手直角坐标系。

载体坐标系(b): 载体坐标系 b 则是以四轴飞行器自身为参考的坐标系, 也定义为右手直角坐标系。

例如:飞机向前的方向为 Y 轴正方向,取飞机向右的方向为X轴正方向,取飞机向上的方向为Z轴正方向。 

 

四元数、欧拉角、方向余弦: 

在百度百科中,欧拉角是这样被描述的:用来确定定点转动刚体位置的3个一组独立角参量,由章动角θ、旋进角(即进动角)ψ和自转角j组成,为欧拉首先提出而得名。简单点来说,就是:绕Z轴旋转为偏航角(YAW)ψ,绕Y轴旋转为横滚角(ROLL)θ,绕X轴旋转为俯仰角(PITCH)φ。

 

绕Z轴旋转ψ角(YAW):

 

定义导航坐标系 n 中某一点的坐标为(x,y,z),使用矩阵表示为: (注意这里是x y z,不是图中的x’ y’ z’)。

设该点在载体坐标系中坐标为(x’,y’,z’),使用矩阵表示为:

 

对于该任意点,易得到两个坐标系下坐标之间的关系:

 

表示成矩阵的形式如下:

 

同理可得: 
绕Y轴旋转θ角(ROLL): 

 

两个坐标系下的转换关系:

 

绕X轴旋转φ角(PITCH):

两个坐标系下的转换关系:

 

由前面的结论可以得到进过三个欧拉角的旋转,得到导航坐标系下的向量 与旋转后的载体坐标系下的向量 之间的关系: 

 

给出由 的坐标变换矩阵:

 

所以可以得到用欧拉角表示的坐标变换矩阵: 

这样我们就得到了使用欧拉角表示的坐标变换矩阵,这个公式先放在这里,等会再用。

 

接下来我们来看看四元数: 

四元数是简单的超复数。

复数是由实数加上虚数单位 i 组成,其中i^2 = -1。

相似地,四元数都是由实数加上三个虚数单位 i、j、k 组成,而且它们有如下的关系: i^2 = j^2 = k^2 = -1, i^0 = j^0 = k^0 = 1 , 每个四元数都是 1、i、j 和 k 的线性组合,即是四元数一般可表示为a + bk+ cj + di,其中a、b、c 、d是实数。

对于i、j、k本身的几何意义可以理解为一种旋转,其中i旋转代表X轴与Y轴相交平面中X轴正向向Y轴正向的旋转,j旋转代表Z轴与X轴相交平面中Z轴正向向X轴正向的旋转,k旋转代表Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转,-i、-j、-k分别代表i、j、k旋转的反向旋转。

这里已经讲得比较清楚了,我们可以把四元数看成一个常数加上一个三维矢量,即

 

四元数的乘法运算:

对于任意一个四元数 来说,q0、q1、q2、q3都是实数,i、j、k为互相正交的单位向量,也是虚单位 。 

 

 

 

 

满足乘法关系如下:

 

举例:假设有两个四元数,

则这两个四元数相乘结果为:

将上面的运算表示成矩阵形式:

设两个四元数Q和P的乘积为四元数

 

或者

 

从M(Q)中,第一列为四元数Q本身,第一行为四元数Q的共轭的转置,不管第一行和第一列,我们可以提取出一个3*3的矩阵VQ,称其为M(Q)的核。 

同理可得,M(P)的核VP: 

四元数的相关知识的准备差不多完成了,下面开始推导四元数的公式: 
我们定义一个四元数 ,用来表示从导航坐标系n和载体坐标系b之间的旋转变换:

 

可以得到旋转矩阵 的数学关系:

 

到这里我们就推出了使用四元数表示的旋转矩阵

 

联立两者对应项相等,求解方程组即可。解方程的步骤就省略了,直接写出结果。令:

 

推出结果:

 

前面我们用欧拉角推导出来的旋转矩阵 也可以叫做方向余弦矩阵(DCM),使用的是Z-Y-X顺规,不做赘述,有兴趣可以再去查找相关资料。 

这里我们代入方向余弦矩阵对应项的值求出欧拉角与四元数的关系,并做一些三角函数的变换整理得到下面的形式:

上式是欧拉角用表示四元数的公式。 
还是由方向余弦矩阵(DCM)可以得到: 

 

 

 

这四个公式的意义是,给出了四元数与欧拉角之间的关系,我们可以很方便地使用这几个公式将欧拉角与四元数相互转换。还需要注意一点,因为方向余弦矩阵的定义不同,对应的欧拉角旋转方式不同,公式也会不同。

  • 23
    点赞
  • 174
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
MPU6050是一种常见的六轴陀螺仪加速度计模块,可以通过读取它的传感器数据来获取角度。下面是获取角度的一些步骤: 1.初始化MPU6050,设置其采样率、滤波器、量程等参数; 2.读取MPU6050的加速度计和陀螺仪数据; 3.将加速度计和陀螺仪数据进行滤波和校准; 4.使用互补滤波算法,计算出角度值。 互补滤波算法是一种将加速度计和陀螺仪数据进行融合的算法,可以用来计算出角度值。其基本思想是将加速度计和陀螺仪的数据加权平均,从而得到更加稳定和准确的角度值。具体实现方式如下: 1.使用加速度计计算出当前姿态的俯仰角和横滚角; 2.使用陀螺仪计算当前姿态的变化率; 3.将加速度计和陀螺仪的数据进行加权平均,得到最终的角度值。 这里提供一个使用Arduino和MPU6050模块实现角度测量的示例代码: ``` #include <Wire.h> #include <MPU6050.h> MPU6050 mpu; void setup() { Serial.begin(9600); Wire.begin(); mpu.initialize(); mpu.dmpInitialize(); mpu.setDMPEnabled(true); } void loop() { Quaternion q; VectorFloat euler; mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetEuler(euler.val, &q); float pitch = euler.x * 180 / M_PI; float roll = euler.y * 180 / M_PI; Serial.print("Pitch: "); Serial.print(pitch); Serial.print(" Roll: "); Serial.println(roll); delay(100); } ``` 在上述示例代码中,我们使用MPU6050库来初始化MPU6050模块,并使用dmpGetEuler函数获取当前的俯仰角和横滚角。最后,将角度值通过串口输出。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值