三轴陀螺仪的四元数解算方法——详细分析

三轴陀螺仪结算

在结算三轴陀螺仪数据之前需要先了解坐标系的概念，关于齐次坐标变换和变换矩阵详情可以参考笔者学习的书本，熊有伦教授主编的《机器人学》。

1、建系

首先建立两个坐标系，导航坐标系也被称为地理坐标系，载体坐标系也被称为物体坐标系。

导航坐标系：东-北-天（地理坐标系X、Y、Z轴的正方向）

载体坐标系：右-前-上（物体坐标系X、Y、Z轴的正方向）

注意：全文按这两个坐标系进行行文，不同坐标系的建立也会有不同的公式，读者切忌照搬。

2、姿态描述方法的选取

在很多博客中都存在一个问题：对姿态描述方法的混用与模糊概念。在描述刚体方位方面，多数博客中用到了RPY角方法和欧拉角方法。但未解释两者之间的区别。欧拉角的定义在不同书中也有不同之处，本文主要介绍两种不同绕系旋转方式的区别。

RPY角方法 

RPY角方法：刚体绕固定坐标系三轴依次旋转一定的角度。旋转矩阵的求取采用的是矩阵“左乘”的方法若。RPY角最早开始是描述船舶（后来逐渐用于手爪上）在海中航行时姿态的一种方法。将船行驶方向取为Z轴方向，则绕Z轴旋转（）称为回转（roll），使用右手螺旋定则，用Z轴确定另外两轴，绕Y轴旋转（）称为俯仰（pitch），绕X轴旋转（）称为偏转（yaw），各书中叫法不同，读者能理解三个角的实际意义即可。同时，机械臂手爪姿态的规定方法与之类似。

刚体绕固定坐标系三轴依次旋转一定的角度。采用的是矩阵“左乘”的方法。例如，坐标系{B}与参考系{A}，初始位置重合。{B}先绕旋转 角，绕旋转角，绕 旋转角，得到相应的旋转矩阵

 

 欧拉角方法 

欧拉角方法：这种描述法中的各次转动都是相对运动坐标系的某轴进行的而不是相对固定坐标系{A}进行的。 旋转矩阵的求取采用矩阵“右乘”的方法计算旋转矩阵。

例如，{B}与参考系{A}的初始方位相同，首先使{B}绕轴旋转角，然后绕轴旋转角，最后绕轴旋转。

 

 将上述两式乘开，即可得到很多博客中提到的余弦矩阵。但是，当绕着不同顺序的旋转轴旋转时，余弦矩阵也不相同，所以掌握余弦矩阵的推导更为重要。

使用四元数解算，最后也是要将四元数转换为欧拉角的，所以此部分是四元数结算的基础。 

四元数解算姿态

四元数相比较于欧拉角，其优点是没有欧拉角解算时的“万向锁” ，但是计算起来难以理解，复杂度高。笔者也是一知半解，就简单地按照自己的理解来写，若有不当之处，请各位读者斧正。
 

 四元数基本概念

此性质是以右手系为前提，对右手系下的四元数的性质进行描述和分析。其中，四元数的乘法需要注意。这里我们为了工程上计算和编程方便，通常使用矩阵式乘法。

了解这些在工程应用上就足够了。

四元数左右手系的选取

四元数在网上一般有左手系（JPL）和右手系(Hamilton)两种表述。他们之间的区别与差异，可参考右手系转左手系、旋转矩阵转四元数、四元数的两种表达（Hamilton/JPL)_左手系旋转矩阵-CSDN博客

但一般情况下都是用右手系（Hamilton）四元数，左手系则多用于航空航天领域，不过各位读者以后在读代码时还是需要注意一下，如果“四元数转旋转矩阵”和“旋转矩阵转四元数”分别采用了在不同坐标系下的转换公式，将会出错。

四元数及旋转矩阵

我们了解四元数，其实想要的就是它在陀螺仪结算中应用。根据上文大家都知道，用欧拉角解算姿态核心就是当绕着不同顺序轴所得到的旋转矩阵，其实四元数也拥有他自己的旋转矩阵。具体推导过程，可参考博客四元数解算姿态角解析_四元数姿态解算-CSDN博客

 在四元数解算姿态时，认为物体系是由地理系经过无中间过程的一次性等效旋转形成的，四元数包含了这种等效关系，假设，绕空间任意轴n旋转角，设四元数Q 

根据博客中的推导可得四元数的旋转矩阵C

取C中第三行的三个元素为g1,g2,g3.第一行第二列为g4,第一行第一列为g5. 使这些元素与欧拉法中的旋转矩阵元素一一对应。以（Z-Y-X）顺序为例，可求得

显然，根据上述关系，想要根据四元数求欧拉角，要先求出q0,q1,q2,q3. 回忆上文提到的，陀螺仪传入的数据是角速度，上文欧拉法应用欧拉角的微分方程求取角度，同理四元数也有自己的微分方程。

 四元数的微分方程

 对Q进行微分

 

因为， 

代入公式可得，其中wx，wy，wz为陀螺仪测得的数据

 

 应用一阶差分的方法，则在△t时间内，满足以下公式

 将dQ/dt代入公式可得，

根据上述公式和四元数与欧拉角之间的关系可知，如果已知角速度wx，wy，wz和初始位置的四元数，就可以求得任意位置的欧拉角了。不过还是要注意，不同的旋转顺序，四元数转换为欧拉角的公式不一样。

此部分参考文章：四元数解算姿态角解析_四元数姿态解算-CSDN博客