A

判断三维坐标系旋转正方向的简单方法

引言

做iOS开发，不免要接触到一些特效，其中不乏3D特效，这时候就要对iOS所使用的坐标系了解才行。若不限于iOS开发，还有MacOS开发，若不知道它们所使用坐标系的不同，初学者会很容易陷于混乱，

三维坐标系

做3D特效，就要用到三维坐标系，这是后人在笛卡尔的平面坐标系的基础上发明的。三维坐标系分两种，左手坐标系和右手坐标系，为什么用左手和右手来区分呢？这是因为当确定了x轴，y轴方向之后,z轴的方向的两种，它可以通过左手或右手来确定。下面就是这两个坐标系的规则示意图（图中固定了x轴的正方向向右，y轴的正方向向上）：

左手坐标系与右手坐标系

相信大多数人对图中的右手坐标系很眼熟，没错，这就是初高中数学教材用到的三维坐标系，只是我们不一定知道它叫右手坐标系。

左手坐标系我们之前很少接触，但是在计算机图形学中这种坐标系非常重要，比如iOS的UIView使用的坐标系就是左手坐标系。有人可能会说，不对吧，UIView的坐标系是原点在左上角，y轴正方向向下，图中的不是这样啊，其实没错啦，把图中的左手坐标系沿x轴旋转180度就是原点在左上角的左手坐标系，区别就是旋转的角度不同而已。这是因为左手坐标系或者右手坐标系整体旋转后性质是不变的。

对坐标系使用左手与右手的命名，有一个作用就是用来方便判断旋转的正方向，这就是左手法则和右手法则。例如对左手坐标系，确定一个旋转轴后，左手握住拳头，拇指指向旋转轴的正方向，四指弯曲的方向为旋转的正方向。相应地，右手坐标系就用右手来判定。确定了旋转的正方向后，在公式计算中就很容易知道是该使用正角度还是负角度了。下图就是右手的例子：

右手法则

但是，这个判断旋转正方向的方法还是不够快。给定任意一个旋转角度的三维坐标系，如果按上面的方法判断旋转正方向，首先，你得确定这个坐标系是左手坐标系还是右手坐标系，这时你会先拿出一只手来，像上图一样摆好三根手指的姿势来比对给定坐标系的x、y、z轴正方向看是否一致。然后根据旋转轴的正方向，用相应的手来判断旋转正方向。

其实，完全没有必要这么麻烦。怎么更方便地判断，且看我慢慢道来。

先看第一个图的两个坐标系，左边的为左手坐标系，右边的为右手坐标系，两坐标系的x轴和y轴正方向保持一致，z轴正方向相反。分别用左手法则与右手法则去判断它们各自绕z轴旋转的正方向，那么从我们眼睛看屏幕的角度来看，它们绕z轴旋转的正方向都是逆时针，这当然不会是巧合。观察这两个坐标系，就会发现这个逆时针方向与x轴正方向箭头顶点指向y轴正方向箭头顶点的方向一致，这说明绕z轴旋转的正方向与x轴正方向箭头顶端指向y轴正方向箭头顶端的方向有关联吗？我想是的。

然后再尝试判断两坐标系绕x轴旋转的正方向，它与y轴正方向顶端指向z轴正方向顶端的方向一致；而绕y轴旋转的正方向，与z轴正方向顶端指向x轴正方向顶端的方向一致。

结论一

据此，我觉得可以得出一个结论：对于任意旋转角度的三维坐标系，绕某一坐标轴旋转的正方向，与另外两个坐标轴的正方向顶端按X—>Y—>Z—>X的顺序进行指向的方向一致。

这就意味着，判断三维坐标系绕某一坐标轴旋转的正方向，不用事先知晓这个坐标系是左手坐标系还是右手坐标系，完全不需要你用手去比划.

反过来，既然判断旋转正方向这么容易，我们也可以利用它来快速判断一个坐标系是左手坐标系和右手坐标系：使用上述结论确定坐标系绕某一某旋转的正方向，然后逆用左手法则与右手法则，大拇指指向该轴的正方向，如果左手四指弯曲的方向与旋转的正方向一致，该坐标系就是左手坐标系，反之就是右手坐标系。

不过这还是复杂，还是需要用手比划。我突然想到了一个更好的方法：
想象y轴是一面墙，你面朝前方斜靠在墙上，可以假设你的头部为y轴正方向顶点，脚为x轴正方向顶点，那么z轴在你的左侧时就是左手坐标系，在右侧时就是右手坐标系。这个时候，人体的生长方向也刚好是绕z轴旋转的正方向。

结论二

再扩展一下就是：对于任意旋转角度的三维坐标系，想象你的脚踩在一个坐标轴（如x轴）正方向的顶点，头倚靠在其邻高坐标轴（如y轴）的正方向顶点，面朝背离原点的方向，那么，第三轴正方向顶点在你的左手边时，这个坐标系就是左手坐标系，在右手边时就是右手坐标系，而人体此时的生长方向就是绕第三轴(如z轴)旋转的正方向。
（注：这里的邻高坐标轴是我自己定义的一个概念，X轴的邻高坐标轴为Y轴，Y轴的邻高坐标轴为Z轴，Z轴的邻高坐标轴为X轴.）

在这个方法里，坐标系属性与绕坐标轴旋转正方向的判断达到了统一，从此可以抛弃左手法则与右手法则，也可以抛弃手指比划的方式来判断左右手坐标系，是不是会觉得很简单？

B

多传感器融合定位理论基础（五）：惯性导航解算 - 知乎 (zhihu.com)

多传感器融合定位理论基础（五）：惯性导航解算

任乾

保持谦虚的态度，直面每一个问题

一、概述

惯性导航的解算是一个实现起来非常简单，但是理解起来要费一番功夫的东西，所谓“实现”就是把公式变成代码，所谓“理解”，就是要弄明白几个公式是怎么推导出来的。

鉴于这种情况，我们先把本篇文章的核心思路捋清楚，然后再去搞细节，不然很容易稀里糊涂地陷入繁琐的公式推导当中去，相信各位以前应该都有过这种体会，每一个公式都看得懂，但是连在一起就不知道它为什么要这么干，对于“理解”，“为什么”可比“是什么”要重要的多了。

在这里，“理解”就包含以下三点：

1）做什么：惯性导航解算的目的是什么？

2）怎么做：惯性导航解算的方法是什么？

3）为什么：这个方法是怎么来的？

二、惯性导航解算的目的

目的当然很简单，从IMU原始的角速度（陀螺仪采集）和加速度（加速度计采集）数据得到它的导航结果（位置、速度、姿态）呗。但如果我们深入下去，好像就不是这一句话能回答的了的了，比如导航信息是怎么描述的，位置和速度好说，姿态就显得略微复杂一些，这是一个long story，喝口水慢慢讲。

1.姿态描述

在导航定位领域，描述一个物体的姿态，常用的描述有欧拉角、旋转矩阵、四元数，这三个概念的理解难度逐渐递增。另外，还有一种表示方式，是等效旋转矢量，它主要作为运算过程中的一个工具，而一般不用来对外作为姿态的描述输出。

1.1 欧拉角

1）欧拉角定义

欧拉角是一个非常直观的概念，一般使用中，它包括俯仰角、横滚角、航向角。

a. 俯仰角

俯仰角描述载体“抬头”的角度大小，一般以抬头(向上)为正，低头(向下)为负

b. 横滚角

横滚角描述载体“侧身”的角度大小，一般以绕着正前方逆时针旋转为正，顺时针旋转为负。

c. 航向角

航向描述载体的朝向，一般以逆时针旋转为正，顺时针旋转为负。（传统组合导航领域与此相反，在此暂时不过多提及，只是让大家看资料时别觉得奇怪就行）

2）欧拉角与坐标轴的对应关系

我们常常把一个xyz直角坐标系放在载体上，这样才方便进行数学描述嘛，如果用前后左右上下来对应载体的方向的话，常见的载体坐标系的对应关系有“右(x)-前(y)-上(z)”、“前(x)-右(y)-下(z)”、“前(x)-左(y)-上(z)”，其中前两种更多的在传统组合导航领域使用，而第三种在机器人、自动驾驶领域使用比较多，由于我们的文章是以后者为主要目标的，所以后面所有的推导就直接使用“前(x)-左(y)-上(z)”坐标系。

在这种坐标系下，我们可以看到，俯仰角是绕y轴的顺时针为正，逆时针为负，横滚角绕x轴的逆时针为正，顺时针为负，航向角绕z轴的逆时针为正，顺时针为负。需要注意的是，一般我们习惯的坐标系表示中，都是逆时针为正，顺时针为负，而俯仰角在这里与y轴的对应关系是反的，这是因为欧拉角有明确的物理意义(比如俯仰角抬头为正、低头为负)，违反这种物理意义会对使用带来一些障碍，所以一般宁愿去改变与坐标轴的对应关系。当然，也有很多不愿意转变的，也是很常见，所以大家在使用时要擦亮眼睛，接触一个新的系统之前，要记得首先去核实对应关系。

1.2 旋转矩阵

1）旋转矩阵定义

旋转矩阵是一个非常常见的姿态描述和计算手段了，因为它可以很方便地对坐标系中的矢量进行计算。

假设旋转前，载体系(b系)的单位正交基为 $(e_{1}, e_{2}, e_{3})$ ，旋转后对应的单位正交基为 $(e_{1}^{'}, e_{2}^{'}, e_{3}^{'})$ ，在世界坐标系(w系，不随载体的旋转而旋转)下有向量 $a$ ，它在旋转前后两个坐标系中的坐标分别为 ${[a_{1}, a_{2}, a_{3}]}^{T}$ 和 ${[a_{1}^{'}, a_{2}^{'}, a_{3}^{'}]}^{T}$ ，那么有

$[e_{1}, e_{2}, e_{3}] [\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}] = [e_{1}^{'}, e_{2}^{'}, e_{3}^{'}] [\begin{matrix} a_{1}^{'} \\ a_{2}^{'} \\ a_{3}^{'} \end{matrix}]$

由此可以得到

$a = [\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}] = [\begin{array}{lll} e_{1}^{T} e_{1}^{'} & e_{1}^{T} e_{2}^{'} & e_{1}^{T} e_{3}^{'} \\ e_{2}^{T} e_{1}^{'} & e_{2}^{T} e_{2}^{'} & e_{2}^{T} e_{3}^{'} \\ e_{3}^{T} e_{1}^{'} & e_{3}^{T} e_{2}^{'} & e_{3}^{T} e_{3}^{'} \end{array}] [\begin{array}{l} a_{1}^{'} \\ a_{2}^{'} \\ a_{3}^{'} \end{array}] \overset{def}{=} {R a}^{'}$

其中 $R$ 就是旋转矩阵，带上坐标系下标，可以写为 $R_{w b}$

2）旋转矩阵性质

旋转矩阵的主要特性是单位正交矩阵，即

$R^{- 1} = R^{T}$

1.3 四元数

1）四元数定义

对很多接触过导航定位算法的同志们来讲，可能四元数是一个让人头疼的问题了，想把它彻底讲明白，要费很大力气，更何况我自己也不敢说完全把它搞透彻了，曾经尝试过，我记得有一本专门写四元数的书，有好几百页，直接把我劝退，后来退而求其次，觉得从物理意义上大致理解它就行。

先从复数开始讲起，可能更容易理解些。我们知道一个实数，可以对应一个坐标轴上的一个点，姑且写作 $A = x$ 吧，后来又有了复数 $A = a + b i$ ，发现它不仅可以描述坐标，还可以描述方向了 $θ = a r c t a n (\frac{b}{a})$ ，所以这种东西在信号处理里面经常用，这个角度就对应信号的相位。这里可能会自然而然产生一个疑问，我直接用向量描述不也是一样的吗，比如写作 $A = (a, b)$ ，不照样也能得到角度吗？这个问题的核心在于向量不可向复数那样运算，即复数有 $i^{2} = - 1$ 的性质，因此两个复数相乘，就有了

$A \cdot B = (a + b i) (c + d i) = a c - b d + (a c + b d) i$

这种性质让它具有了向量不具有的优势，复数域的直接运算带来的便利性是无可比拟的。

后来，三维空间的运动描述越来越迫切的时候，复数已经不能满足需求，因为它只有一个角度嘛，不是三维的，汉密尔顿想(可自行百度下汉密尔顿与四元数)，能不能再加一维来表示呢，比如 $A = a + b i + c j$ ，后来发现不行，那就再加一维 $A = a + b i + c j + d k$ ，发现行了，于是就有了四元数。哈哈，他发明四元数的过程当然没有那么简单，因为这个“再加一维”牵扯到的东西太多太多，也是理解四元数的关键，不是拍一下脑袋这么简单。

为了弄明白这个“再加一维”，我们可以换一个角度来看它，若另 $A = a + b i$ , $B = c + d i$ ，会发现

$\begin{aligned} Q & = A + B j \\ = (a + b i) + (c + d i) j \\ = a + b i + c j + d i j \end{aligned}$

在这里，另 $k = i j$ ，那上式不就是四元数了嘛

$\begin{aligned} Q = a + b i + c j + d k \end{aligned}$

因此，从 $Q = A + B j$ 可以看出，四元数可以理解为“复数的复数”。

明白了这一点，再回到复数的定义，就可以严格点讲，复数不是在实数上加一维，而是“实数的复数”，看起来好像一句废话，但这样描述是有必要的。更抽象一点，可以总结为，把一类东西(假设类别为X)在复数维度升维，应该写成 $X_{a} + X_{b} j$ ，而不应该是 $X_{a} + b j$ (这里的b是一个实数)。当X表示实数时，这两种理解方式并无差异，而当X已经是复数时，两种方式造成的结果就产生了差异。这就是刚才所提到的，为什么汉密尔顿一开始在描述三维空间的角度时，只在复数上加一维( $A = a + b i + c j$ )会不行的原因。

由此扩展下去，四元数的复数是八元数，八元数的复数是十六元数，这不是我胡说八道，在数学领域确实是存在的，它们统称为超复数。由于每一次升维，都会损失一个性质（比如四元数没有了乘法交换性，而复数有），且在三维空间领域，四元数已经足够用来描述，因此其他超复数就没有用武之地了，大家也就见不到。

2）四元数性质

四元数的性质很多，我们挑一些后面能用到的说。首先把四元数按照常见的形式写成

$q = q_{w} + q_{v} = q_{w} + q_{x} i + q_{y} j + q_{z} k$

其中 $q_{w}$ 是实部， $q_{v}$ 是虚部。

a. 单位四元数

在姿态表示中，我们所用的四元数都是单位四元数，即

$| | q | | = \sqrt{q_{w}^{2} + q_{x}^{2} + q_{y}^{2} + q_{z}^{2}} = 1$

b. 共轭四元数与四元数的逆

共轭四元数指的是实部相同，虚部相反的四元数

$q^{*} = q_{w} - q_{v} = q_{w} - q_{x} i - q_{y} j - q_{z} k$

对于单位四元数来讲，它的逆与它的共轭四元数相等

$q^{- 1} = q^{*}$

c. 四元数乘法

四元数乘法定义为

$p \otimes q = [\begin{matrix} p_{w} q_{w} - p_{v}^{T} q_{v} \\ p_{w} q_{v} + q_{w} p_{v} + p_{v} \times q_{v} \end{matrix}]$

如果你把上面的式子展开，会发现四元数乘法可以表示成矩阵与向量相乘的形式

$p \otimes q = [p]_{L} q$

其中

$\begin{aligned} _{L} & = [\begin{array}{cccc} p_{w} & - p_{x} & - p_{y} & - p_{z} \\ p_{x} & p_{w} & - p_{z} & p_{y} \\ p_{y} & p_{z} & p_{w} & - p_{x} \\ p_{z} & - p_{y} & p_{x} & p_{w} \end{array}] \\ = p_{w} I + [\begin{array}{cc} 0 & - p_{v}^{T} \\ p_{v} & {[p_{v}]}_{\times} \end{array}] \end{aligned}$

其中 $[\cdot]_{\times}$ 表示向量的反对称矩阵(不了解这个概念的麻烦自行去查一下)

四元数乘法也可以展开为

$p \otimes q = [q]_{R} p$

其中

$\begin{aligned} _{R} & = [\begin{array}{cccc} q_{w} & - q_{x} & - q_{y} & - q_{z} \\ q_{x} & q_{w} & q_{z} & - q_{y} \\ q_{y} & - q_{z} & q_{w} & q_{x} \\ q_{z} & q_{y} & - q_{x} & q_{w} \end{array}] \\ = q_{w} I + [\begin{array}{cc} 0 & - q_{v}^{T} \\ q_{v} & - {[q_{v}]}_{\times} \end{array}] \end{aligned}$

因此可以得到

$\begin{array}{l} (q \otimes x) \otimes p = [p]_{R} [q]_{L} x \\ q \otimes (x \otimes p) = [q]_{L} [p]_{R} x \end{array}$

这是两个很重要的性质，后面的推导中会多次用到

1.4 等效旋转矢量

等效旋转矢量，从物理意义上比较好理解，借用欧拉角的定义，它表示物体绕坐标轴分别按三个欧拉角旋转三次，就可以得到它现在所处的姿态。而等效旋转矢量的定义是，物体绕空间某一个轴旋转一次，就可以达到现在的姿态。二者的区别在于，欧拉角的旋转是绕xyz这样的直角坐标轴的，而等效旋转矢量的所绕的轴可能朝向空间任意一个方向，这个轴取决于载体的姿态。

等效旋转矢量可以用向量 $ϕ$ ，并用单位向量 $u$ 表示它的朝向， $ϕ$ 表示它的大小，因此有

$ϕ = ϕ u$

等效旋转矢量的指数形式，是推导中重要的推导过程，因此先在这里给出

$\exp (ϕ^{\land}) = \exp (ϕ u^{\land}) = \sum_{n = 0}^{\infty} \frac{1}{n!} {(ϕ u^{\land})}^{n}$

由于带着无穷项，因此需要化简才能用。由于反对称矩阵具有如下性质

${(ϕ^{\land})}^{i} = {\begin{cases} (- 1)^{(i - 1) / 2} ϕ^{i - 1} (ϕ^{\land}) & i = 1, 3, 5, \dots \\ (- 1)^{(i - 2) / 2} ϕ^{i - 2} {(ϕ^{\land})}^{2} & i = 2, 4, 6, \dots \end{cases}$

因此，指数形式可以化简为

$\begin{aligned} \exp (ϕ^{\land}) & = I + ϕ (u^{\land}) + \frac{1}{2!} ϕ^{2} {(u^{\land})}^{2} + \frac{1}{3!} ϕ^{3} {(u^{\land})}^{3} + \frac{1}{4!} ϕ^{4} {(u^{\land})}^{4} + \dots \\ = I + ϕ (u^{\land}) + \frac{1}{2!} ϕ^{2} {(u^{\land})}^{2} - \frac{1}{3!} ϕ^{3} (u^{\land}) - \frac{1}{4!} ϕ^{4} {(u^{\land})}^{2} + \dots \\ = I + {(u^{\land})}^{2} + \underset{\sin ϕ}{\underset{⏟}{(ϕ - \frac{1}{3!} ϕ^{3} + \frac{1}{5!} ϕ^{5} - \dots)}} (u^{\land}) - \underset{\cos ϕ}{\underset{⏟}{(1 - \frac{1}{2!} ϕ^{2} + \frac{1}{4!} ϕ^{4} - \dots)}} {(u^{\land})}^{2} \\ = I + \sin ϕ (u^{\land}) + (1 - \cos ϕ) {(u^{\land})}^{2} \\ = I + \frac{\sin ϕ}{ϕ} (ϕ^{\land}) + \frac{(1 - \cos ϕ)}{ϕ^{2}} {(ϕ^{\land})}^{2} \end{aligned}$

1.5 各种表示方式之间的转换

既然各种方式之间是等价的，且实际使用中，各种形式都有可能出现，那么理清它们之间的转换关系就是必要的。不过由于很多推到过程过于复杂且无聊，这里就只能给出结论，对过程感兴趣的自己去查资料吧。

1）欧拉角与旋转矩阵

按照机器人前(x)-左(y)-上(z)的坐标系定义，并令横滚角为 $α$ 、俯仰角为 $β$ 、航向角为 $γ$ ，假设旋转矩阵是按照z-y-x的顺序旋转得来，那么旋转矩阵可以表示为

$R_{w b} = {(R_{x} (α) R_{y} (- β) R_{z} (γ))}^{T}$

其中

$R_{x} (α) = [\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos (α) & \sin (α) \\ 0 & - \sin (α) & \cos (α) \end{array}]$

$R_{y} (- β) = [\begin{array}{ccc} \cos (β) & 0 & \sin (β) \\ 0 & 1 & 0 \\ - \sin (β) & 0 & \cos (β) \end{array}]$

$R_{z} (γ) = [\begin{array}{ccc} \cos (γ) & \sin (γ) & 0 \\ - \sin (γ) & \cos (γ) & 0 \\ 0 & 0 & 1 \end{array}]$

也可以直接写出结果形式

$R_{w b} = [\begin{array}{ccc} c β c γ & - s α s β c γ - c α s γ & s α s γ - c α s β c γ \\ c β s γ & c α c γ - s α s β s γ & - c α s β s γ - s α c γ \\ s β & s α c β & coc β \end{array}]$

其中 $s ∙ = \sin (∙) c ∙ = \cos (∙)$

因此，观察该矩阵，可以得出由旋转矩阵得到欧拉角的方式

$\begin{array}{l} α = \arctan 2 (R_{w b} (3, 2), R_{w b} (3, 3)) \\ β = \arcsin (R_{w b} (3, 1)) \\ γ = \arctan 2 (R_{w b} (2, 1), R_{w b} (1, 1)) \end{array}$

2) 四元数与旋转矩阵

由四元数转旋转矩阵的公式为

$R_{u b} = [\begin{array}{ccc} q_{u}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2} & 2 (q_{x} q_{y} - q_{w} q_{z}) & 2 (q_{x} q_{z} + q_{w} q_{y}) \\ 2 (q_{x} q_{y} + q_{w} q_{z}) & q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2} & 2 (q_{y} q_{z} - q_{w} q_{x}) \\ 2 (q_{x} q_{z} - q_{w} q_{y}) & 2 (q_{y} q_{z} + q_{w} q_{x}) & q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2} \end{array}]$

由旋转矩阵转四元数的公式为

$\begin{array}{l} q_{u} = \frac{\sqrt{1 + R_{w b} (1, 1) + R_{w b} (2.2) + R_{w b} (3.3)}}{2} \\ q_{x} = \frac{R_{w b} (3, 2) - R_{w b} (2, 3)}{4 q_{w}} \\ q_{y} = \frac{R_{u b} (1, 3) - R_{w b} (3, 1)}{4 q_{w}} \\ q_{z} = \frac{R_{w b} (2, 1) - R_{w b} (1, 2)}{4 q_{w}} \end{array}$

不过需要注意的是，此处需要满足

$q_{w} \neq 0, 1 + R_{w b} (1, 1) + R_{w b} (2, 2) + R_{w b} (3, 3) > 0$

不满足的时候也能转，不过太复杂，写不动了。。。。。。

3）旋转矩阵与等效旋转矢量

由旋转矢量得到旋转矩阵的公式为

$R_{w b} = I + \frac{\sin ϕ}{ϕ} (ϕ^{\land}) + \frac{1 - \cos ϕ}{ϕ^{2}} {(ϕ^{\land})}^{2}$

可以发现，这跟等效旋转矢量的指数形式是一样的，这就是传说中的罗德里格斯公式。

由旋转矩阵得到旋转矢量的公式为

$\begin{array}{l} ϕ = \arccos \frac{tr (R_{w b}) - 1}{2} \\ u = \frac{{(R_{w b} - {(R_{w b})}^{T})}^{\lor}}{2 \sin ϕ} \end{array}$

4) 四元数与等效旋转矢量

由旋转矢量计算四元数的公式为

$q = \cos \frac{ϕ}{2} + \frac{\sin \frac{ϕ}{2}}{ϕ} ϕ$

由旋转矢量计算四元数的公式为

$\begin{matrix} ϕ = 2 \arctan (‖ q_{v} ‖, q_{w}) \\ u = q_{v} / ‖ q_{v} ‖ \end{matrix}$

这两个公式里，有一个很有意思的问题，就是第一个公式里面有两处除以2，第二个公式里有一处乘以2，这个2怎么来的呢？

我们知道，在旋转矩阵表示法中，一个向量在两个坐标系之间转换的公式为

$a^{'} = R a$

而四元数则不这么简单，它为

$a^{'} = q \otimes a \otimes q^{*}$

这个也可以从一个比较奇特的角度来解释，大家看一下，一个四元数和一个矢量相乘(其实乘的是矢量对应的纯虚四元数，即实部为0，虚步三个值对应这个三维矢量)，得到的是什么？已经忘记的可以网上翻去看一下四元数乘法。结论是乘出来的一个东西，实部不为0，这就尴尬了，一个三维矢量，乘出来变成了一个四维的东西(一个实部和三个虚部)，为了解决这个问题，就写成了上面的形式，左乘一个，右乘一个，实际就是对应两次旋转，自然每次就只旋转一半，即每一个四元数对应的角度只是想要的旋转角度的一半，这样改变之后，惊奇地发现，实部永远为0，等于三维矢量先是被第一个四元数给旋转到四维空间，又被第二个四元数给拉回到三维空间了，完美！

三、惯性导航解算的方法

经过上面啰嗦一大堆，我们解决了“做什么”、“怎么做”、“为什么”三个问题中的第一个，接下来解决第二个。

“怎么做”在这里就是怎么去解算的问题了，而解算就是“角速度->姿态”、“加速度->速度”、“速度->位置”(当然由于有坐标系问题，这种描述并不十分严谨，但更容易理解)。看到这些，我们很容易想到，就是个积分问题嘛，把这个积分过程用数学公式表示出来，就等于知道了“怎么做”的问题。

积分公式的形式其实很简单，但它的推导过程比较繁琐，因此这里先给出结论，过程是“为什么”这个环节要解决的问题。另外，积分与微分本是一家，因此本小节会从微分方程引入，讲“为什么”的时候就只讲微分方程怎么来的就好了，这样这两节就可以无缝衔接在一起。

1.基于旋转矩阵的姿态更新

1) 积分形式介绍

旋转矩阵的微分方程为

${\dot{R}}_{w b} = R_{w b} [ω]_{\times}$

其中 $ω$ 为IMU三个轴测量得到的角速度。

根据微分方程可以很容易写出它的积分形式为（此处用到的是高数中讲的求一阶微分方程通解的方法，基础知识不展开介绍了）

$R_{w b_{k}} = R_{w b_{k - 1}} e^{\int_{t_{k - 1}}^{t_{k}} [ω]_{\times} d τ}$

这里给出的是由 k-1 时刻的姿态积分得到 k 时刻姿态的积分形式。仔细观察指数部分，它表示的是角速度矢量的积分，而角速度矢量的积分其实不就是 k-1 时刻到 k 时刻对应的等效旋转矢量嘛，因此，我们可以直接写成

$R_{w b_{k}} = R_{w b_{k - 1}} e^{ϕ \times}$

由于反对称矩阵的指数函数前面已经推导完毕(罗德里格斯公式)，因此上式又可以写为

$R_{w b_{k}} = R_{w b_{k - 1}} R_{b_{k - 1} b_{k}}$

其中

$R_{b_{k - 1} b_{k}} = I + \frac{\sin ϕ}{ϕ} (ϕ \times) + \frac{1 - \cos ϕ}{ϕ^{2}} (ϕ \times)^{2}$

可见，求旋转矩阵的问题，最终转化成了求等效旋转矢量的问题。咋求？接着用微分方程求呗。等效旋转矢量的微分方程为（方程怎么来的，同样放在“为什么”中去讲）

$\dot{ϕ} \approx ω + \frac{1}{2} ϕ \times ω$

这个方程看起来还是比较复杂的，可以也应当化简。在方程右边第二项，可以看到是两个矢量的叉乘，根据叉乘的定义，当两个矢量方向重合时，叉乘的结果为0。一般 imu 的采样频率不会低于 100hz，即 k-1 时刻到 k 时刻的时间间隔不会大于10ms，而载体一般自动驾驶车或者机器人也不会发生过于高频的运动，因此可以近似认为满足叉乘为0的条件，因此可以直接把上式简化为

$\dot{ϕ} \approx ω$

它对应的积分形式就很简单了

$ϕ \approx \int_{t_{k - 1}}^{t_{k}} ω (τ) d τ$

有了旋转矢量的积分形式，那么该旋转矩阵的形式就了然了，直接带进去就行。

但是，到这里会遇到一个问题，我们一直讨论的都是连续时间的，即 k-1 时刻到 k 时刻之间的所有值都是连续可测的，而实际数据都是离散的，只能得到 k-1 和 k 这两个时刻的值，那么必须把上面的连续时间形式高程离散形式，才能继续走下去了。

2）离散时间处理

这就牵扯到常见的三种方法：欧拉法、中值法、龙哥库塔法，三者按复杂程度逐渐递增，按精度也是逐渐递增。我们先从简单的开始说。

a.欧拉法

欧拉法很好理解，虽然我不知道 k-1 到 k 时刻的所有值，但是如果我假设 imu 在这个时间段内是匀速旋转的，那不就解决了吗，此时，对应的旋转矢量为

$ϕ = ω_{k - 1} (t_{k} - t_{k - 1})$

b.中值法

欧拉法固然简单，但是匀速的假设还是有些太强了，实际使用中会发现精度不够，于是就有了中值法。中值法假设 imu 是匀加速运动的，这就比匀速更高级了一些，此时离散时间形式为

$ϕ = \frac{ω_{k - 1} + ω_{k}}{2} (t_{k} - t_{k - 1})$

c.龙哥库塔法

中值法假设的匀加速运动，在imu运动更加剧烈的情况下仍然不够，那就继续把模型复杂化呗，龙哥库塔法就是这种，把运动模型搞成一个高阶曲线。由于这部分略显复杂，而且实际使用中，中值法多数情况下已经够用，因此在此处就不对这种方法做过多介绍了。

2.基于四元数的姿态更新

1）积分形式介绍

四元数的微分方程为

${\dot{q}}_{w b} = q_{w b} \otimes \frac{1}{2} [\begin{array}{l} 0 \\ ω \end{array}]$

其矩阵形式为(可回顾四元数乘法那一小节的推导)

${\dot{q}}_{w b} = \frac{1}{2} {[\begin{array}{l} 0 \\ ω \end{array}]}_{R} q_{w b}$

同样按照微分方程的解法，得到其积分形式为

${\dot{q}}_{w b} = e^{\frac{1}{2} Θ} q_{w b}$

其中

$Θ = [\begin{array}{cccc} 0 & - ϕ_{x} & - ϕ_{y} & - ϕ_{z} \\ ϕ_{x} & 0 & ϕ_{z} & - ϕ_{y} \\ ϕ_{y} & - ϕ_{z} & 0 & ϕ_{x} \\ ϕ_{z} & ϕ_{y} & - ϕ_{x} & 0 \end{array}]$

为了求解该方程，要对指数函数进行泰勒展开，同样包含高次幂，展开方法与前面旋转矢量指数形式展开的方法类似，可以自行推导，此处直接给出结论

$e^{\frac{1}{2} Θ (T)} = I \cos \frac{ϕ}{2} + \frac{Θ}{ϕ} \sin \frac{ϕ}{2}$

因此有

$q_{w b_{k}} = [I \cos \frac{ϕ}{2} + \frac{Θ}{ϕ} \sin \frac{ϕ}{2}] q_{w b_{k - 1}}$

由于

${[\begin{matrix} \cos \frac{ϕ}{2} \\ \frac{ϕ}{ϕ} \sin \frac{ϕ}{2} \end{matrix}]}_{R} = [I \cos \frac{ϕ}{2} + \frac{Θ}{ϕ} \sin \frac{ϕ}{2}]$

另

$q_{b_{k - 1} b_{k}} = [\begin{matrix} \cos \frac{ϕ}{2} \\ \frac{ϕ}{ϕ} \sin \frac{ϕ}{2} \end{matrix}]$

则

$q_{w b_{k}} = q_{w b_{k - 1}} \otimes q_{b_{k - 1} b_{k}}$

2) 离散时间处理

此处也是同样用中值法算出旋转矢量即可得到 $q_{b_{k - 1} b_{k}}$ ，从而得到更新后的四元数 $q_{w b_{k}}$ ，与前述方法相同，不再重复。

3.速度更新

相比来讲，速度更新就显得很简单，速度的微分方程为

$\dot{v} = R_{w b} a - g$

其中 $a = [\begin{array}{lll} a_{x} & a_{y} & a_{z} \end{array}]$ 为测量加速度， $g = [\begin{array}{lll} 0 & 0 & g_{0} \end{array}]$ 为重力加速度。

该微分方程的通解形式为

$Δ v = (R_{w b} a - g) Δ t$

对应的基于中值法的速度更新形式为

$v_{k} = v_{k - 1} + (\frac{R_{w b_{k}} a_{k} + R_{w b_{k}} a_{k - 1}}{2} - g) (t_{k} - t_{k - 1})$

4.位置更新

位置更新就更加简单，微分形式为

$\dot{p} = v$

其通解形式为

$Δ p = v Δ t$

基于中值法的离散形式为

$p_{k} = p_{k - 1} + \frac{v_{k} + v_{k - 1}}{2} (t_{k} - t_{k - 1})$

另外，如果把速度的通解代入，则位置的通解形式也可以写为

$Δ p = v Δ t + \frac{1}{2} a Δ t^{2}$

相应的基于中值法的离散形式为

$\begin{aligned} p_{k} = p_{k - 1} + v_{k - 1} (t_{k} - t_{k - 1}) + \frac{1}{2} (\frac{R_{w b_{k}} a_{k} + R_{w b_{k - 1}} a_{k - 1}}{2} - g) {(t_{k} - t_{k - 1})}^{2} \end{aligned}$

三、解算方法的来源

这一部分就是要解决“为什么”的问题，其实就是推导刚才用到的那些微分方程怎么来的。由于速度、位置的微分方程过于简单，此处就不介绍了，只介绍旋转矩阵、四元数、旋转矢量的微分方程。

1.旋转矩阵微分方程

假设世界坐标系(w系)中有一个固定不动的矢量 $r^{w}$ ，它在载体坐标系(b系)下的表示为 $r^{b}$ ，则有

$r^{w} = R_{w b} r^{b}$

两边同时微分，得到

${\dot{r}}^{w} = R_{w b} {\dot{r}}^{b} + {\dot{R}}_{w b} r^{b}$

由于 ${\dot{r}}^{w} = 0$ ， ${\dot{r}}^{b} = - ω_{w b}^{b} \times r^{b}$ （其中 $ω_{w b}^{b}$ 代表载体旋转角速度在b系下的表示，实际使用时，指的就是陀螺仪的角速度输出，且暂不考虑误差），因此有

$0 = R_{w b} (- ω_{w b}^{b} \times r^{b}) + {\dot{R}}_{w b} r^{b}$

移项可得

$R_{w b} (ω_{w b}^{b} \times r^{b}) = {\dot{R}}_{w b} r^{b}$

即

$R_{w b} (ω_{w b}^{b} \times r^{b}) = {\dot{R}}_{w b} r^{b}$

因此有

${\dot{R}}_{w b} = R_{w b} {[ω_{w b}^{b}]}_{\times}$

2.四元数微分方程

$r^{w}$ 和 $r^{b}$ 两个矢量之间可以用四元数转换如下

$r^{w} = q_{w b} \otimes r^{b} \otimes q_{w b}^{*}$

等式两边同时右乘 $q_{w b}$ ，可得

$r^{w} \otimes q_{w b} = q_{w b} \otimes r^{b}$

上式两边同时求微分，可得

$\begin{aligned} {\dot{r}}^{w} \otimes q_{w b} + r^{w} & \otimes {\dot{q}}_{w b} = {\dot{q}}_{w b} \otimes r^{b} + q_{w b} \otimes {\dot{r}}^{b} \end{aligned}$

由于

$\begin{array}{l} {\dot{r}}^{b} = - ω_{w b}^{b} \times r^{b} = - ω_{w b}^{b} \otimes r^{b} \\ {\dot{r}}^{w} = 0 \end{array}$

则有

$\begin{aligned} r^{w} \otimes {\dot{q}}_{w b} \\ = & (q_{w b} \otimes r^{b} \otimes q_{w b}^{*}) \otimes {\dot{q}}_{w b} \\ = & {\dot{q}}_{w b} \otimes r^{b} - q_{w b} \otimes ω_{w b}^{b} \otimes r^{b} \end{aligned}$

等式两边同时左乘 $q_{w b}^{*}$ ，可得

$\begin{aligned} r^{b} \otimes q_{w b}^{*} \otimes {\dot{q}}_{w b} = q_{w b}^{*} \otimes {\dot{q}}_{w b} \otimes r^{b} - ω_{w b}^{b} \otimes r^{b} \end{aligned}$

移项可得

$ω_{w b}^{b} \otimes r^{b} = (q_{w b}^{*} \otimes {\dot{q}}_{w b}) \otimes r^{b} - r^{b} \otimes (q_{w b}^{*} \otimes {\dot{q}}_{w b})$

利用前面四元数相乘展开成矩阵与向量相乘的公式，将上式展开，可得

$[\begin{array}{cc} 0 & 0_{1 \times 3} \\ 0_{3 \times 1} & {[ω_{w b}^{b}]}_{\times} \end{array}] [\begin{array}{l} 0 \\ r^{b} \end{array}] = [\begin{array}{cc} 0 & 0_{1 \times 3} \\ 0_{3 \times 1} & 2 {[{(q_{w b}^{*} \otimes {\dot{q}}_{w b})}_{v}]}_{\times} \end{array}] [\begin{matrix} 0 \\ r^{b} \end{matrix}]$

因此有

${(q_{w b}^{*} \otimes {\dot{q}}_{w b})}_{v} = \frac{1}{2} ω_{w b}^{b}$

以上只是把虚部求解出来了，实部还得搞一搞，如下

根据四元数与旋转矢量的关系，有

$q_{w b}^{*} = [\begin{matrix} \cos \frac{ϕ}{2} \\ - u \sin \frac{ϕ}{2} \end{matrix}]$

${\dot{q}}_{w b} = [\begin{matrix} - \frac{\dot{ϕ}}{2} \sin \frac{ϕ}{2} \\ \dot{u} \sin \frac{ϕ}{2} + u \frac{ϕ}{2} \cos \frac{ϕ}{2} \end{matrix}]$

因此有

$\begin{aligned} q_{w b}^{*} \otimes {\dot{q}}_{w b} & = [\begin{matrix} \cos \frac{ϕ}{2} \\ - u \sin \frac{ϕ}{2} \end{matrix}] \otimes [\begin{matrix} - \frac{\dot{ϕ}}{2} \sin \frac{ϕ}{2} \\ \dot{u} \sin \frac{ϕ}{2} + u \frac{ϕ}{2} \cos \frac{ϕ}{2} \end{matrix}] \\ = [\begin{matrix} - \frac{\dot{ϕ}}{2} \sin \frac{ϕ}{2} \cos \frac{ϕ}{2} + {(u \sin \frac{ϕ}{2})}^{T} (\dot{u} \sin \frac{ϕ}{2} + u \frac{\dot{Φ}}{2} \cos \frac{ϕ}{2}) \\ \cos \frac{\dot{ϕ}}{2} (\dot{u} \sin \frac{ϕ}{2} + u \frac{\dot{ϕ}}{2} \cos \frac{ϕ}{2}) + u \sin \frac{ϕ}{2} \cdot \frac{\dot{ϕ}}{2} \sin \frac{ϕ}{2} - (u \sin \frac{ϕ}{2}) \times (\dot{u} \sin \frac{ϕ}{2} + u \frac{\dot{ϕ}}{2} \cos \frac{ϕ}{2}) \end{matrix}] \\ = [\begin{matrix} 0 \\ u \cos \frac{ϕ}{2} \sin \frac{ϕ}{2} + u \frac{\dot{ϕ}}{2} - u \sin \frac{ϕ}{2} \times \dot{u} \sin \frac{ϕ}{2} \end{matrix}] \end{aligned}$

可以看出实部为0。

那么最终就可以得到

$q_{w b}^{*} \otimes {\dot{q}}_{w b} = \frac{1}{2} [\begin{matrix} 0 \\ ω_{w b}^{b} \end{matrix}]$

即

${\dot{q}}_{w b} = q_{w b} \otimes \frac{1}{2} [\begin{matrix} 0 \\ ω_{w b}^{b} \end{matrix}]$