imuncle commented 5 years ago

刚接触惯性导航传感器的时候，被求解姿态角的一系列操作秀了一脸，虽然研究了一两天，但最后还是浅尝辄止，只了解了个大概，当时把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轴的旋转矩阵。为便于叙述，称仅绕一根轴的旋转为基本旋转。两坐标系间任何复杂的角位置关系都可以看作有限次基本旋转的符合，变换矩阵等于基本旋转确定的变换矩阵的连乘，连乘的顺序依基本旋转的先后次序由右向左排列。上图中飞机的旋转步骤可以分解如下：需要注意的是，这里的姿态矩阵的具体形式与三轴的旋转先后顺序有关，不同的先后顺序得出来的矩阵也不同。