角速度怎么算-MPU6050 姿态解算系列二：陀螺仪姿态解算

背景介绍：

通过上一篇已经知道如何使用惯性传感器里的加速度数据解算姿态了。

由于加速度计对振动敏感，易产生高频噪声使解算出的姿态产生抖动，所以需要探索其他的解算方法来解决这个问题。

本篇 Sugar 就来说一说如何使用惯性传感器里的陀螺仪数据解算姿态。

陀螺仪输出的是什么

我们回头再看一下 上对陀螺仪输出数据的图形表述：

上图中白色空心的箭头表示陀螺仪的输出：是分别绕 X、Y 和 Z 轴的旋转角速度。

其正方向的定义依然符合笛卡尔右手系规则：右手大姆指指向对应座标轴正向，其余四指环绕方向是绕该轴转动的正方向。

陀螺仪的输出和姿态角速度有什么关系

在中说到在 Sugar 习惯的 飞控定义下，横滚角 roll（用 Ф 表示）指重力加速度与“X-Z 平面”的夹角，俯仰角 pitch（用 θ 表示）是重力加速度与“Y-Z 平面”的夹角。这里我们把 Ф 和 θ 分别对时间 t 求导，得到 Ф’ 和 θ’。如果读者看不出 Ф’ 和 θ’ 分别表示横滚角速度和俯仰角速度，那么请回想一下“路程、速度和时间的关系”。

到此，我们看到了“姿态角度速”，并且知道了陀螺仪的输出是“绕各个轴转动的角速度”，很明显的共同点是：都是角速度。惯性传感器姿态的晃动会让陀螺仪检测到绕各个轴的转动，所以只要明确姿态角速度与各个轴角速度的对应关系就可以通过陀螺仪得到姿态角速度了。为了使这个对应关系更容易在推文里表述，我们先来做几个约定：

(1) 用 phi 表示 Ф;

(2) 用 theta 表示 θ;

(3) 用 trans 表示由陀螺仪输出到姿态角速度的转换矩阵;

(4) 用 gx 表示陀螺仪 X 轴输出;

(5) 用 gy 表示陀螺仪 y 轴输出;

(6) 用 gz 表示陀螺仪 z 轴输出。

在这样的约定下 trans 在 里可以写为：

同样用 的表示方法，我们记陀螺仪各轴的输出为：

姿态角速度的算法如下：

这里 Sugar 为了读着容易理解直接用汉字写出了各个姿态角速度。

姿态角速度与姿态角有什么关系

估计看到这里的人都知道：姿态角速度对 dt 积分就是姿态角。

这里 Sugar 就想说一个点：怎样用程序表示姿态角速度对时间 dt 的积分。

我们用 表示通过陀螺仪数据算出的姿态角，则在 里可以这样写：

我们假设 初始值为 [0 0 0]’，不断在循环里用 累积 dt 时间产生的角度变化，这就是程序里表示姿态角速度对 dt 积分的方法。这里 dt 一般是 毫秒(ms) 级。

姿态解算的中点 or 终点

通过视频不难看出，本篇仍然是姿态解算系列中的一个小里程碑，并不是终点。虽然陀螺仪对振动不敏感，不产生高频噪声，但陀螺仪有零偏、积分出的姿态会飘移。姿态解算尚未结束，下面仍要继续努力。

欢迎扫码关注 Sugar 微信视频号看“加速度姿态解算”和“陀螺仪姿态解算”的效果对比视频。

PS

Sugar 多说一句：当我们研究算法的时候，重点是要理解怎么算而不是程序怎么写。对算法有理解之后，可以任选语言来实现算法，比如在嵌入式平台上就可以选：C/C++ 或 。到具体平台上选定开发语言之后，才是重点想该怎么写程序的时候。

新人学习不要同时有两个甚至更多重点，阶段性前进要把注意力集中在唯一的一个重点上。如果有很多点要攻克，那就多划分几个阶段，优雅地让每个阶段对应唯一的一个重点。

玩儿算法推荐用 或 ，原因是语法极其简单有助于淡化编程、集中精力学算法。

往期回顾

1、

2、

admin永久会员

打赏 收藏 海报 链接