ＩＭＵ惯性测量单元之陀螺仪综述_陀螺仪四元数是什么

ＩＭＵ惯性测量单元之陀螺仪综述

---

前言

ＩＭＵ惯性测量单元如今使用越来越频繁，其中，以陀螺仪、加速度计、磁力计为主要使用模块，常见的模块又ＭＰＵ6050，还有ＪＹ901等。下面我来阐述一下陀螺仪部分的主要原理。

---

一、坐标系

要介绍这些模块，必须要清楚的就是坐标系，一个是会跟着模块动的固连坐标系（载体坐标系、机体坐标系）、一个是不动的大地坐标系。实际上右手定则或者左手定则都可以，但是我们规定了之后就不能动，常用的是右手定则：固连坐标系为右ｘ前ｙ上ｚ，大地坐标系为东ｘ北ｙ天ｚ。如下图：

右前上

东北天

可以看到，这两个坐标系一样，我们在实际使用时，也是把他们的初始状态设置为一致的。当模块跟着载体发生转动时，载体坐标系与大地坐标系就会产生角度偏差，这个偏差可以理解为载体坐标系根据某个变换得到大地坐标系（当然，反着推也是可以的，只不过是要把下述矩阵求逆）：
说明：

说明：Ｃｂｎ表示从载体坐标系到大地坐标系的变换，里面就是一个3*3的矩阵。

附：这个变换矩阵除了在姿态解算中使用以外，其他地方也常用，如图片的变换等。

二、姿态角

要理解陀螺仪，还需要对更好可视化的“姿态角”进行一个了解。姿态角可以认为就三个角，一个是俯仰角ｐｉｔｃｈ，一个是翻滚角ｒｏｗ，一个是偏航角ｙａｗ，注意，这里我没说明是绕哪个轴旋转，那是因为这个都看个人。

我个人偏向于使用绕Ｘ轴为俯仰角、绕Ｙ轴为翻滚角、绕Ｚ轴为偏航角，如模块所示：

三、变换矩阵

通过上面也可以大致知道，角度的变化实际上可以理解为坐标系的变换，那么，这个变换就是旋转矩阵。
１.　绕某一个轴旋转：

假设绕Ｙ轴旋转某一角度，可以得到上述的矩阵，这就是绕某一个轴的旋转矩阵，如果是绕多个轴的话，则只需要多次累乘即可。
附：可以使用累乘的原理：
一个物体从一个姿态变化到另一个姿态，那么我们可以看为，这个物体其实是绕了某一个轴通过无中间过程的一次旋转即旋转到了新的姿态。在实际中物体可能需要多次的中间旋转过程才能变化到目标姿态，所以我们现在尝试关心一下这个中间过程，并把这个中间过程简化为一种变换关系。因为变换关系即代表中间过程，所以我们只需找到这种变换关系，通过它即可求出物体姿态变化后的新姿态。
参考知乎中的大神的一幅图：

可以看到，旋转矩阵里面就已经包含了角度信息！

四、陀螺仪

目的：

使用陀螺仪的目的是为了得到当前模块距离初始状态转动了多少角度。

所得数据：

陀螺仪测量的是绕自身三个轴（载体坐标系）转动的角速度。当设备旋转式，绕自身ｘ轴、ｙ轴、ｚ轴旋转的角速度为（ｗｘ，ｗｙ，ｗｚ）．

四元数：

通过角速度，我们其实是可以求得绕自身坐标系旋转了多少角度，再根据上一次的初始量，则能得知当前距初始大地坐标系（或者初始载体坐标系）的角度变化。

然而，我们使用的时候一般都是使用角速度来求“四元数”，继而解算姿态角。原因有几个（注：这里暂时笼统说明一下，在后面的“求解姿态角”再进行详细阐述）：①计算复杂，需要计算三角函数；②存在万向锁问题，缺失了维度；③存在奇异点，当绕第二个轴旋转的角度达到９０度附近时，另外的角度会发生突变。

计算四元数：ｑ０，ｑ１，ｑ２，ｑ３，分别初始化为（１，０，０，０），根据角速度与上一次的四元数，可得：

姿态解算后续详细介绍，此处只做概述：利用该四元数，可以求得旋转矩阵，继而可以求得对应的角度了。

五、姿态解算

终于来到最重要的一个步骤了，前面所讲的其实都是为了这一步。“姿态解算”可以理解为求解从初始姿态到某一姿态的变换矩阵，进而求得具体绕轴的变化角度。

用欧拉角表示旋转矩阵：

“变换矩阵”这个概念就是前面第三点所讲到的。一个姿态到另外一个姿态，可以按照某一顺序（如ZYX轴的顺序）将其分解为绕某一轴旋转的旋转矩阵的累乘。直观一点的话可以使用欧拉角的旋转矩阵表示（按照右前上坐标系），如（注：该矩阵为从载体坐标系到大地坐标系的变换，如果想大地从坐标系到载体坐标系的变换的话，只需求逆，又因为这是正定矩阵，则可转化为求转置即可）：

令矩阵的第一行第一列的元素为r11，第二行第一列为r21，则：
偏航角：Ψ = arctan(r21/r22)
俯仰角：φ = arctan(r32/r33)
翻滚角：θ = -arcsin(r31)

万向锁、奇异点：

乍一看好像没问题，但假设 φ = 90°，此时r33就等于0了，那么γ的值实际上就求不出来了！
另外，当φ=90°时，上述矩阵可化为：
[ 0 sin(φ - Ψ) cos(φ - Ψ)
0 cos(φ - Ψ) -sin(φ - Ψ)
-1 0 0 ]

这就是问题的关键了：原本三个维度，有一个确定了，照理来说还有两个，但是，从这里可以看出，另φ - Ψ = c，则整个变换只有c这么一个维度，降维了！

四元数表示旋转矩阵：

根据上面欧拉角表示旋转矩阵的缺陷，就引出了四元数。
四元数是什么这里就暂时不展开，里面的知识有一本书这么厚，感兴趣的可以找相关的资料，此处直接使用。

对于同一个三维转换过程，用欧拉角旋转矩阵和用四元数旋转矩阵来表示都是等价的，即Rzyx = Rq。
那么（针对右前上坐标系）：
偏航角：yaw = arctan(r21/r22)
俯仰角：roll = arctan(r32/r33)
翻滚角：pitch = -arcsin(r31)

代码实现例子如下（代码也是很简单的，直接利用模块所获得的四元数，然后代入上述公式即可，此处就不贴代码了）：

六、总结

陀螺仪部分的姿态解算大致就这些，对于刚开始接触的人，比较头大的应该就是旋转矩阵这一部分，本文讲得比较概括性一点，其实里面还可以再深入一些，例如关于顺序这部分，我再上文只说了右前上，还有zyx这些，实际上它还可以有其他顺序，不过，他们的核心也大致差不多，所以我也没有细说。
另外，我们通常都是使用四元数来求姿态角的，所以，使用角速度求姿态角这一部分我没有展开来讲，一切按实际使用出发。
还有，我们在使用过陀螺仪模块的时候，经常听说“漂移”，那是因为姿态角的求解里面包含了“积分”，对于离散数据的积分其实就是用矩形框近似，这么一来就会累积误差，到后面越来越大。解决方案是引入加速度计来更新俯仰角和翻滚角，引入磁力计修正偏航角，这些内容在后面章节再展。
本文就到此为止，有问题的欢迎各位提问，也欢迎各位提出质疑，大家一起进步！