四元数姿态更新详解

[imuncle]

刚接触惯性导航传感器的时候，被求解姿态角的一系列操作秀了一脸，虽然研究了一两天，但最后还是浅尝辄止，只了解了个大概，当时把MPU9250的算法逻辑捋了个大概思路，原文见：MPU9250姿态解析、MPU9250六轴算法。这其中最不理解的就是四元数的那一部分，为什么四元数可以这样求解出来？为什么四元数与欧拉角之间的转换关系式这样的？四元数到底有什么几何或者物理意义？

可惜的是普遍的讲解姿态解析的文章都是直接给出了最后的公式，并没有讲解为什么。最近又因为想做四足机器人，需要实现惯导的室内定位功能，正好趁此机会好好研究一下四元数。

以下内容绝大部分参考自《惯性导航》（作者 秦永元）一书。

什么是四元数

四元数的定义

顾名思义，四元数是由四个元构成的数，它是一个超复数，具有一个实部和三个虚部，也可以看成是一个四维空间中的一个向量。

四元数的运算法则

四元数的运算也有加减乘除四种，加减就很简单了，这里看看乘法和除法。

不过既然四元数是一个四维向量，那么它的运算法则就完全符合向量的运算法则，换句话说，符合矩阵的运算法则。

四元数的乘法

乘法分为一个四元数和一个常数相乘以及两个四元数相乘两种情况，第一种情况太简单了，就看第二种乘法运算：

四元数的除法

矩阵的除法可以转换成逆矩阵相乘，所以除法的关键是四元数的逆的求解。

四元数就了解到这里，接下来就研究四元数与欧拉角之间的关系。

四元数与姿态阵间的关系

首先看下图模型：

上图中，向量OA绕空间中某一旋转轴旋转至OA‘位置，这里需要注意的是，旋转过后二者的坐标轴原点不一定还重合在一起，这里是为了方便才画成一点的。

A点为该旋转刚体上的一点，假设旋转前刚体的坐标系为R（图中蓝色），旋转后的坐标系为b（图中绿色），MPU6050是放置在刚体上的，所以传感器所获得到的加速度和角速度是相对于刚体坐标系的，而我们想要获取的姿态角是相对于地面参考系的，为简便起见，这里假设R坐标系与地面坐标系重合。

根据欧拉定理，仅考虑刚体在0时刻和t时刻的角位置时，刚体从A位置到A'位置的转动可等效成绕瞬轴u（单位向量）转过θ角一次完成（如上图中的红线部分所示），将这一部分单独提取出来，如下图所示。这样，位置向量做圆锥运动，A和A'位于同一圆上，r和r'位于同一圆锥面上。





所以矩阵D就是由OA到OA'的旋转矩阵。

这里需要注意的是，对于原坐标系R来说，D是从始位置到末位置的旋转矩阵。而始位置在R中的坐标等效于末位置在b中的坐标，即该矩阵同时也是末位置在b中的坐标与在R中坐标的变换矩阵，所以矩阵D同时也是b系到R系的坐标变换矩阵

并以这四个数构造四元数：

将四元数回带入变换矩阵中，可以得到


上式的证明过程如下：

到了这一步，我们可以得出阶段性的结论：我们可以利用四元数Q求出不同坐标系之间的变换矩阵。

---

另一方面，如果我们知道了欧拉角，也可以求出相应的坐标变换矩阵，而且推导起来比四元数容易多了。

先看看简单的绕Z轴旋转的例子，考虑如下模型：




这就得到了绕Z轴的旋转矩阵。为便于叙述，称仅绕一根轴的旋转为基本旋转。两坐标系间任何复杂的角位置关系都可以看作有限次基本旋转的符合，变换矩阵等于基本旋转确定的变换矩阵的连乘，连乘的顺序依基本旋转的先后次序由右向左排列。

上图中飞机的旋转步骤可以分解如下：


需要注意的是，这里的姿态矩阵的具体形式与三轴的旋转先后顺序有关，不同的先后顺序得出来的矩阵也不同。

延用之前的字母，我们可以看出，这里得出的姿态矩阵是R坐标系到b坐标系的变换矩阵，与前面四元数的变换矩阵正好相反，这正好是一个逆过程，我们需要的目标时这个姿态矩阵的逆矩阵。

但我们这是旋转矩阵，而旋转矩阵都是正交矩阵，正交矩阵的逆矩阵和它的转置矩阵是一样的，所以有

基于上面的等式，我们可以很容易得出下面的解

推导到这一步，我们可以基本捋清楚四元数求解姿态角的原理了。

• 利用四元数我们可以求得一个变换矩阵
• 利用欧拉角我们也可以求得一个变换矩阵
• 这两个变换矩阵肯定相等，这就建立了四元数与欧拉角之间的等式关系，所以只要求得四元数就可以求得欧拉角

那么我们为什么不直接用三角函数求出欧拉角呢？这个稍微一想就知道，欧拉角全是三角函数，计算量很大，而四元数只是计算几个加减乘法，只有最后才解一个反三角函数。而且欧拉角表示方位有一个致命的弱点，那就是gimbal lock，也称万向节死锁。

那么我们的问题核心就转变为如何求四元数了。

四元数的求解

从以前的文章MPU9250姿态解析可以知道，普遍使用的是一阶龙格库塔（Runge-Kutta）方法计算。

一阶Runge-Kutta

这里参考《推導_四元數.pdf》里面的内容：

四元数微分

---

将上面两式综合可得四元数的计算方法

我们只需要给定一个四元数初始值，之后再利用测量到的角速度的值就可以计算出四元数了，进一步地计算出欧拉角，实现姿态解析。

四元数的意义

那么，推导了这么多，怎么直观地理解四元数这个东西呢？

其实，从前面的推导中，我们就可以看出，四元数其实就是个四维空间下的旋转矩阵，只不过我们将它用在了三维旋转的问题上。

以下内容截取自如何形象地理解四元数？

说白了，三维旋转就是四维旋转的一个特例，就像二维旋转是三维旋转的一个特例一样。说是特例其实不准确，准确的说是一个子集。如果四元数的实部，那么我们就认为它是一个纯四元数，纯四元数对应的就是四维空间里的一个“三维超平面”。

那么我们怎么保证每一次旋转都可以转回三维空间，而不是转到四维空间中呢，理论推导证明，左乘单位四元数，右乘其共轭的运算可以保证旋转限制在三维空间内，这一点在前面的推导中可以看出来

这种旋转可以用下面的图表示

[FENYUN323]

太强了

[FENYUN323]

慢慢学习中

[taoqiuwang]

gitblog 不错

[NINESTROBOTICS]

tql