多传感器融合定位七-惯性导航解算及误差分析其一

Reference:

1. 深蓝学院-多传感器融合
2. 多传感器融合定位理论基础

文章跳转：

1. 多传感器融合定位一-3D激光里程计其一：ICP
2. 多传感器融合定位二-3D激光里程计其二：NDT
3. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
4. 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
5. 多传感器融合定位五-点云地图构建及定位
6. 多传感器融合定位六-惯性导航原理及误差分析
7. 多传感器融合定位七-惯性导航解算及误差分析其一
8. 多传感器融合定位八-惯性导航解算及误差分析其二
9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
12. 多传感器融合定位十二-基于图优化的建图方法其一
13. 多传感器融合定位十三-基于图优化的建图方法其二
14. 多传感器融合定位十四-基于图优化的定位方法
15. 多传感器融合定位十五-多传感器时空标定(综述)

1. 三维运动描述基础知识

1.1 概述

多传感器融合中三维运动的导航信息包含姿态、速度、位置，其中姿态的处理最为复杂，也最为核心。
姿态有三种表示形式：欧拉角、旋转矩阵、四元数，此外还有等效旋转矢量，但它一般在中间计算过程中使用。(等效旋转矢量推导过程中的一些计算，推导会更加简单方便。)
注：这里只介绍基于四元数和旋转矩阵的姿态更新，不介绍基于欧拉角的更新。(我们做姿态更新的时候，不会给出欧拉角的姿态更新和误差形式，欧拉角在实际过程中不会参与这种性质的运算，更多的是给人看的，很直观，但是在公式中会非常复杂。)

1.2 姿态描述方法

1.2.1 欧拉角

欧拉角等同于把姿态绕三次不同轴旋转。
不同的旋转顺序会得到不同的欧拉角，常见的有：
a. 机器人坐标系：xyz分别对应前左上，旋转顺序为z-y-x；
b. 惯性导航坐标系：xyz分别对应右前上，旋转顺序为z-x-y；
c. 另一种惯性导航坐标系：xyz分别对应前右下，旋转顺序为z-y-x。

万向锁：当载体处在某个姿态时，会产生奇异性问题，导致丢失一个自由度。
不同的旋转顺序下，产生万向锁时所处的姿态不同。下图展示了z-y-x的旋转顺序下的万向锁问题。(最后一个图中的旋转和第一次旋转是一样的，导致丢失了一个自由度)

• 为什么不同的坐标系定义下，会选择不同的旋转顺序？
  不同坐标系定义下，选择不同旋转顺序的原因：其本质都是按照“航向(yaw)->俯仰(pitch)->横滚(roll)”的顺序旋转，因为此时万向锁出现在俯仰为90°时的情况，而多数载体出现该姿态的几率最小。

需要注意的是，欧拉角有明确的物理意义，不随坐标系定义的不同而改变：
a. 俯仰角：载体抬头为正，低头为负；
b. 横滚角：向右滚为正，向左滚为负；
c. 航向角：机器人中，一般以逆时针旋转为正，顺时针旋转为负。(在与地理系相关的惯性导航中，常以北偏东为正，北偏西为负，遇到时需要注意)

由于欧拉角必然产生奇异性，因此一般只用它做人机交互的显示，而不用来做姿态解算。

1.2.2 旋转矩阵

旋转矩阵是描述旋转的一个三维矩阵，一个真实姿态对应一个唯一的旋转矩阵。
假设旋转前，载体系(b系)的单位正交基为 $\left({\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right)$，旋转后对应的单位正交基为 $\left({\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }\right)$
假设在世界坐标系($\mathbf{w}$ 系，不随载体的旋转而旋转)下有向量 $\mathbf{a}$ ，它在旋转前后两个坐标系中的坐标分别为 ${\left[a_1,a_2,a_3\right]}^{\mathrm{T}}$ 和 ${\left[a_1^{\prime },a_2^{\prime },a_3^{\prime }\right]}^{\mathrm{T}}$，那么有

$$\begin{array}{rl}& \left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\end{array}$$由此可以得到
$$\begin{array}{r}\mathbf{a}=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{\mathbf{e}}_1^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\stackrel{\text{ def }}{=}\mathbf{R}{\mathbf{a}}^{\prime }\end{array}$$其中 $\mathbf{R}$ 便是旋转矩阵，在结合机器人模型推导时，记为 ${\mathbf{R}}_{wb}$

从矩阵可以看出，三个基向量都是单位正交基，所以旋转矩阵也就是单位正交矩阵，即行列式为1，且满足 $\mathbf{a}={\mathbf{R}}^{-1}{\mathbf{a}}^{\prime }={\mathbf{R}}^{\mathrm{T}}{\mathbf{a}}^{\prime }$。

优点：
a. 没有奇异性，适合用于解算；

缺点：
a. 用9个元素表示3个自由度，会增加计算复杂度；
b. 为了保持正交性，一般更新完毕后，要重新做正交化。不然连续解算几次，旋转矩阵的性质变了，会遇到奇异性问题。

1.2.3 四元数

四元数是超复数，即 “复数的复数” 。(这个说法很有意思，但超复数不单单指四元数，比如还包含复数的复数的复数。四元数用来表示三维，但为什么不是三元数表示三维？)
若有复数
$$\begin{array}{rl}& A=a+b\mathbf{i}\\ & B=c+d\mathbf{i}\end{array}$$则复数的复数为(将$A$当作复数的实部，$B$当作复数的虚部)：
$$\begin{array}{rl}\mathbf{q}& =A+B\mathbf{j}\\ & =a+b\mathbf{i}+c\mathbf{j}+d\mathbf{i}\mathbf{j}\end{array}$$若令 $\mathbf{k}=\mathbf{i}\mathbf{j}$, 则有
$$\mathbf{q}=a+b\mathbf{i}+c\mathbf{j}+d\mathbf{k}$$此即为四元数。
一般四元数的常见表示符号为：
$$\mathbf{q}=q_w+{\mathbf{q}}_v=q_w+q_x\mathbf{i}+q_y\mathbf{j}+q_z\mathbf{k}$$共轭四元数(实部相同，虚部相反)：
$${\mathbf{q}}^{\ast }=q_w-{\mathbf{q}}_v=q_w-q_x\mathbf{i}-q_y\mathbf{j}-q_z\mathbf{k}$$四元数的逆：
$${\mathbf{q}}^{-1}=\frac{{\mathbf{q}}^{\ast }}{\Vert \mathbf{q}\Vert }$$姿态运算时，四元数为单位四元数(单位四元数即模为一，即$\Vert \mathbf{q}\Vert$)，此时有：
$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }=q_w-{\mathbf{q}}_v$$四元数乘法：(注意单位 $\otimes$)
$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-{\mathbf{p}}_v^{\mathrm{T}}{\mathbf{q}}_v\\ p_w{\mathbf{q}}_v+q_w{\mathbf{p}}_v+{\mathbf{p}}_v\times {\mathbf{q}}_v\end{array}\right]$$若在四元数乘法中出现三维向量，指的是和三维向量构成的纯虚四元数相乘，比如
$$\mathbf{p}\otimes \mathbf{u}=\mathbf{p}\otimes \left[\begin{array}{c}0\\ u_1\mathbf{i}\\ u_2\mathbf{j}\\ u_3\mathbf{k}\end{array}\right]$$乘法结合律
$$\begin{array}{rl}& (\mathbf{p}\otimes \mathbf{q})\otimes \mathbf{r}=\mathbf{p}\otimes (\mathbf{q}\otimes \mathbf{r})\\ & \mathbf{p}\otimes (\mathbf{q}+\mathbf{r})=\mathbf{p}\otimes \mathbf{q}+\mathbf{p}\otimes \mathbf{r}\\ & (\mathbf{p}+\mathbf{q})\otimes \mathbf{r}=\mathbf{p}\otimes \mathbf{r}+\mathbf{q}\otimes \mathbf{r}\end{array}$$四元数相乘，可以展开为矩阵与向量相乘的形式
$$\mathbf{p}\otimes \mathbf{q}=[\mathbf{p}{]}_L\mathbf{q}$$其中
$$\begin{array}{rl}[\mathbf{p}{]}_L& =\left[\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}\right]\\ & =p_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{p}}_v^{\mathrm{T}}\\ {\mathbf{p}}_v& {\left[{\mathbf{p}}_v\right]}_{\times }\end{array}\right]\end{array}$$其中，符号 $[\bullet {]}_{\times }=[\bullet {]}^{\wedge }$， 同样表示反对称矩阵。(就一个东西)

也可以展开为
$$\mathbf{p}\otimes \mathbf{q}=[\mathbf{q}{]}_R\mathbf{p}$$其中
$$\begin{array}{rl}[\mathbf{q}{]}_R& =\left[\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}\right]\\ & =q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\mathrm{T}}\\ {\mathbf{q}}_v& -{\left[{\mathbf{q}}_v\right]}_{\times }\end{array}\right]\end{array}$$由此，可以得出重要性质(后续推导时常用)(可以看出来$[\mathbf{p}{]}_R[\mathbf{q}{]}_L=[\mathbf{q}{]}_L[\mathbf{p}{]}_R$)：
$$\begin{array}{rl}& (\mathbf{q}\otimes \mathbf{x})\otimes \mathbf{p}=[\mathbf{p}{]}_R[\mathbf{q}{]}_L\mathbf{x}\\ & \mathbf{q}\otimes (\mathbf{x}\otimes \mathbf{p})=[\mathbf{q}{]}_L[\mathbf{p}{]}_R\mathbf{x}\end{array}$$

1.2.4 等效旋转矢量

理解：把旋转当做绕空间一个固定轴转过一个角度。在不引起歧义的情况下可简称为旋转矢量或旋转向量。(将欧拉角的三次旋转等效成了一次旋转)
直接用向量 $\mathbf{\varphi }$ 表示，其方向即为转轴方向，对应的单位向量记为 $\mathbf{u}$ 它的长度 $\varphi =|\mathbf{\varphi }|$ 即为转角。
等效旋转矢量的指数形式，可以表示为
$$\exp \left({\mathbf{\varphi }}^{\wedge }\right)=\exp \left(\varphi {\mathbf{u}}^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left(\varphi {\mathbf{u}}^{\wedge }\right)}^n$$上式包含高次幂，为了便于后续计算，需要对高次幂进行化简。
由于反对称矩阵具有以下性质(可自行推导)，
$${\left({\mathbf{\varphi }}^{\wedge }\right)}^i=\{\begin{array}{ll}(-1{)}^{(i-1)/2}{\varphi }^{i-1}\left({\mathbf{\varphi }}^{\wedge }\right)& i=1,3,5,\cdots \\ (-1{)}^{(i-2)/2}{\varphi }^{i-2}{\left({\mathbf{\varphi }}^{\wedge }\right)}^2& i=2,4,6,\cdots \end{array}$$因此，等效旋转矢量的指数函数可以表示如下
(罗德里格斯公式，也写成 $R=\exp (\theta {\mathbf{a}}^{\wedge })=I+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^T+\sin \theta {\mathbf{a}}^{\wedge }$)：
$$\begin{array}{rl}\exp \left({\mathbf{\varphi }}^{\wedge }\right)& =I+\varphi \left({\mathbf{u}}^{\wedge }\right)+\frac{1}{2!}{\varphi }^2{\left({\mathbf{u}}^{\wedge }\right)}^2+\frac{1}{3!}{\varphi }^3{\left({\mathbf{u}}^{\wedge }\right)}^3+\frac{1}{4!}{\varphi }^4{\left({\mathbf{u}}^{\wedge }\right)}^4+\cdots \\ & =I+\varphi \left({\mathbf{u}}^{\wedge }\right)+\frac{1}{2!}{\varphi }^2{\left({\mathbf{u}}^{\wedge }\right)}^2-\frac{1}{3!}{\varphi }^3\left({\mathbf{u}}^{\wedge }\right)-\frac{1}{4!}{\varphi }^4{\left({\mathbf{u}}^{\wedge }\right)}^2+\cdots \\ & =I+{\left({\mathbf{u}}^{\wedge }\right)}^2+\underset{\sin \varphi }{\underbrace{\left(\varphi -\frac{1}{3!}{\varphi }^3+\frac{1}{5!}{\varphi }^5-\cdots \right)}}\left({\mathbf{u}}^{\wedge }\right)-\underset{\cos \varphi }{\underbrace{\left(1-\frac{1}{2!}{\varphi }^2+\frac{1}{4!}{\varphi }^4-\cdots \right)}}{\left({\mathbf{u}}^{\wedge }\right)}^2\\ & =I+\sin \varphi \left({\mathbf{u}}^{\wedge }\right)+(1-\cos \varphi ){\left({\mathbf{u}}^{\wedge }\right)}^2\\ & =I+\frac{\sin \varphi }{\varphi }\left({\mathbf{\varphi }}^{\wedge }\right)+\frac{(1-\cos \varphi )}{{\varphi }^2}{\left({\mathbf{\varphi }}^{\wedge }\right)}^2\end{array}$$

1.3 各描述方法之间的关系

1.3.1 欧拉角与旋转矩阵

按照机器人前$(\mathrm{x})$-左$(\mathrm{y})$-上$(\mathrm{z})$的坐标系定义，并令横滚角为 $\alpha$、俯仰角为 $\beta$、航向角为 $\gamma$ 。
a. 欧拉角转旋转矩阵
由于旋转矩阵是按照z-y-x(本质都是按照yaw-pitch-roll旋转)的顺序旋转得来，因此可以表示为
$${\mathbf{R}}_{wb}={\left({\mathbf{R}}_x(\alpha ){\mathbf{R}}_y(-\beta ){\mathbf{R}}_z(\gamma )\right)}^T$$其中，
$${\mathbf{R}}_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (\alpha )& \sin (\alpha )\\ 0& -\sin (\alpha )& \cos (\alpha )\end{array}\right]{\mathbf{R}}_y(-\beta )=\left[\begin{array}{ccc}\cos (\beta )& 0& \sin (\beta )\\ 0& 1& 0\\ -\sin (\beta )& 0& \cos (\beta )\end{array}\right]{\mathbf{R}}_z(\gamma )=\left[\begin{array}{ccc}\cos (\gamma )& \sin (\gamma )& 0\\ -\sin (\gamma )& \cos (\gamma )& 0\\ 0& 0& 1\end{array}\right]$$

b. 旋转矩阵转欧拉角

由欧拉角得到的旋转矩阵，其完整形式为：
$${\mathbf{R}}_{wb}=\left[\begin{array}{ccc}{c}_{\beta }{c}_{\gamma }& -s_{\alpha }{s}_{\beta }{c}_{\gamma }-c_{\alpha }{s}_{\gamma }& s_{\alpha }{s}_{\gamma }-c_{\alpha }{s}_{\beta }{c}_{\gamma }\\ c_{\beta }{s}_{\gamma }& c_{\alpha }{c}_{\gamma }-s_{\alpha }{s}_{\beta }{s}_{\gamma }& -c_{\alpha }{s}_{\beta }{s}_{\gamma }-s_{\alpha }{c}_{\gamma }\\ s_{\beta }& s_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\beta }\end{array}\right]$$其中符号：$s\bullet =\sin (\bullet )c\bullet =\cos (\bullet )$
观察矩阵，可以看出：
$$\begin{array}{rl}& \alpha =\arctan 2\left({\mathbf{R}}_{wb}(3,2),{\mathbf{R}}_{wb}(3,3)\right)\\ & \beta =\arcsin \left({\mathbf{R}}_{wb}(3,1)\right)\\ & \gamma =\arctan 2\left({\mathbf{R}}_{wb}(2,1),{\mathbf{R}}_{wb}(1,1)\right)\end{array}$$

1.3.2 旋转矩阵与四元数

它们转换的推导过程较为复杂，此处直接给出结论。
四元数转旋转矩阵(常用)：
$${\mathbf{R}}_{wb}=\left[\begin{array}{ccc}{q}_w^2+q_x^2-q_y^2-q_z^2& 2\left(q_x{q}_y-q_w{q}_z\right)& 2\left(q_x{q}_z+q_w{q}_y\right)\\ 2\left(q_x{q}_y+q_w{q}_z\right)& q_w^2-q_x^2+q_y^2-q_z^2& 2\left(q_y{q}_z-q_w{q}_x\right)\\ 2\left(q_x{q}_z-q_w{q}_y\right)& 2\left(q_y{q}_z+q_w{q}_x\right)& q_w^2-q_x^2-q_y^2+q_z^2\end{array}\right]$$旋转矩阵转四元数(不那么常用)：
$$\begin{array}{rl}& q_w=\frac{\sqrt{1+R_{wb}(1,1)+R_{wb}(2,2)+R_{wb}(3,3)}}{2}\\ & q_x=\frac{R_{wb}(3,2)-R_{wb}(2,3)}{4q_w}\\ & q_y=\frac{R_{wb}(1,3)-R_{wb}(3,1)}{4q_w}\\ & q_z=\frac{R_{wb}(2,1)-R_{wb}(1,2)}{4q_w}\end{array}$$需要注意的是，这需要满足：
$$q_w\ne 0,1+R_{wb}(1,1)+R_{wb}(2,2)+R_{wb}(3,3)>0$$当不满足该条件时，转换步骤比较复杂，此处不讲述。

1.3.3 旋转矩阵与旋转矢量

(和十四讲写的一样，但这里总看着怪怪的)
a. 由旋转矢量计算旋转矩阵
$${\mathbf{R}}_{wb}=I+\frac{\sin \varphi }{\varphi }\left({\mathbf{\varphi }}^{\wedge }\right)+\frac{1-\cos \varphi }{{\varphi }^2}{\left({\varphi }^{\wedge }\right)}^2$$此公式也被称为罗德里格斯公式，并且与旋转矢量的指数运算结果相同。
b. 由旋转矩阵计算旋转矢量
$$\begin{array}{c}\varphi =\arccos \frac{\mathrm{tr}\left({\mathbf{R}}_{wb}\right)-1}{2}\\ \mathbf{u}=\frac{{\left({\mathbf{R}}_{wb}-{\left({\mathbf{R}}_{wb}\right)}^T\right)}^{\vee }}{2\sin \varphi }\end{array}$$其中，符号 ${\bullet }^{\vee }$ 表示由反对称矩阵得到对应的矢量。

1.3.4 四元数与旋转矢量

a. 由旋转矢量计算四元数
$$\mathbf{q}=\cos \frac{\varphi }{2}+\frac{\sin \frac{\varphi }{2}}{\varphi }\mathbf{\varphi }$$思考：为什么角度是旋转矢量转角的一半?
三维空间中的一个矢量，使用四元数对它进行旋转，得到新的矢量时，其计算过程为 ${\mathbf{a}}^{\prime }=\mathbf{q}\otimes \mathbf{a}\otimes {\mathbf{q}}^{\ast }$
原因是矢量是三维的，四元数与矢量(对应的纯虚四元数)直接相乘是四维(即实部不为零)，而用上式计算出的结果则能保证永远是三维。
可以理解为两次旋转，第一次转到四维，第二次再转回三维，每次转总角度的一半。

• 以下为《从零开始手写 VIO》内容：
  假设某个旋转运动的旋转轴为单位向量 $\mathbf{u}$，绕该轴旋转的角度为 $\theta$，那么它对应的单位四元数为 $\mathbf{q}=\left[\begin{array}{c}\cos \frac{\theta }{2}\\ \mathbf{u}\sin \frac{\theta }{2}\end{array}\right]$。

  当旋转一段微小时间，即角度趋于 $0$ 时，容易有：
  $$\Delta \mathbf{q}=\left[\begin{array}{c}\cos \frac{\delta \theta }{2}\\ \mathbf{u}\sin \frac{\delta \theta }{2}\end{array}\right]\approx \left[\begin{array}{c}1\\ \mathbf{u}\frac{\delta \theta }{2}\end{array}\right]=\left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\delta }\theta \end{array}\right]$$其中 $\delta \mathbf{\theta }$ 的方向表示旋转轴，模长表示旋转角度。

b. 由四元数计算旋转矢量
$$\begin{array}{c}\varphi =2\arctan \left(\Vert {\mathbf{q}}_v\Vert ,q_w\right)\\ \mathbf{u}={\mathbf{q}}_v/\Vert {\mathbf{q}}_v\Vert \end{array}$$

2. 三维运动的微分性质

欧拉角不是解算时要用的东西，在此不参与讨论。

微分本质上表达的就是四元数和角速度的关系，以及旋转矩阵和角速度的关系。有了这个关系，才能进行后面的导航解算。关联起来之后，有了角速度就可以反向的解出来旋转矩阵是多少。

2.1 旋转矩阵微分方程

假设世界坐标系(w系)中有一个固定不动的矢量 ${\mathbf{r}}^w$，它在载体坐标系(b系)下的表示为 ${\mathbf{r}}^b$，则有
$${\mathbf{r}}^w={\mathbf{R}}_{wb}{\mathbf{r}}^b$$两边同时微分，可得
$${\dot{\mathbf{r}}}^w={\mathbf{R}}_{wb}{\dot{\mathbf{r}}}^b+{\dot{\mathbf{R}}}_{wb}{\mathbf{r}}^b$$由于(这里可看前面文章：SLAM 几何基础)
$$\begin{array}{rl}{\dot{\mathbf{r}}}^w& =0\text{坐标系固定不动的}\\ {\dot{\mathbf{r}}}^b& =-{\mathbf{\omega }}_{wb}^b\times {\mathbf{r}}^b\text{坐标系在变化}\end{array}$$其中 ${\mathbf{\omega }}_{wb}^b$ 代表载体旋转角速度在 $\mathrm{b}$ 系下的表示，实际使用时，指的就是陀螺仪的角速度输出(暂不考虑误差)。

因此有：
$$0={\mathbf{R}}_{wb}\left(-{\mathbf{\omega }}_{wb}^b\times {\mathbf{r}}^b\right)+{\dot{\mathbf{R}}}_{wb}{\mathbf{r}}^b$$移项可得：
$${\mathbf{R}}_{wb}\left({\mathbf{\omega }}_{wb}^b\times {\mathbf{r}}^b\right)={\dot{\mathbf{R}}}_{wb}{\mathbf{r}}^b$$变换可得：
$${\mathbf{R}}_{wb}\left({\left[{\mathbf{\omega }}_{wb}^b\right]}_{\times }{\mathbf{r}}^b\right)={\dot{\mathbf{R}}}_{wb}{\mathbf{r}}^b$$因此有：
$${\dot{\mathbf{R}}}_{wb}={\mathbf{R}}_{wb}{\left[{\mathbf{\omega }}_{wb}^b\right]}_{\times }$$(该式被称为泊松公式，很重要，跟四元数都是求导，没什么区别)

2.2 四元数微分方程

${\mathbf{r}}^w$ 和 ${\mathbf{r}}^b$ 两个矢量之间可以用四元数转换如下：
$${\mathbf{r}}^w={\mathbf{q}}_{wb}\otimes {\mathbf{r}}^b\otimes {\mathbf{q}}_{wb}^{\ast }$$等式两边同时右乘 ${\mathbf{q}}_{wb}$，可得：
$${\mathbf{r}}^w\otimes {\mathbf{q}}_{wb}={\mathbf{q}}_{wb}\otimes {\mathbf{r}}^b$$上式两边同时求微分，可得：
$$\begin{array}{rl}& {\dot{\mathbf{r}}}^w\otimes {\mathbf{q}}_{wb}+{\mathbf{r}}^w\otimes {\dot{\mathbf{q}}}_{wb}\\ =& {\dot{\mathbf{q}}}_{wb}\otimes {\mathbf{r}}^b+{\mathbf{q}}_{wb}\otimes {\dot{\mathbf{r}}}^b\end{array}$$由于：
$$\begin{array}{rl}& {\dot{\mathbf{r}}}^b=-{\mathbf{\omega }}_{wb}^b\times {\mathbf{r}}^b=-{\mathbf{\omega }}_{wb}^b\otimes {\mathbf{r}}^b\\ & {\dot{\mathbf{r}}}^w=0\end{array}$$则有：
$$\begin{array}{rl}& {\mathbf{r}}^w\otimes {\dot{\mathbf{q}}}_{wb}\\ =& \left({\mathbf{q}}_{wb}\otimes {\mathbf{r}}^b\otimes {\mathbf{q}}_{wb}^{\ast }\right)\otimes {\dot{\mathbf{q}}}_{wb}\\ =& {\dot{\mathbf{q}}}_{wb}\otimes {\mathbf{r}}^b-{\mathbf{q}}_{wb}\otimes {\mathbf{\omega }}_{wb}^b\otimes {\mathbf{r}}^b\end{array}$$等式两边同时左乘 ${\mathbf{q}}_{wb}^{\ast }$ ，可得：
$$\begin{array}{rl}& {\mathbf{r}}^b\otimes {\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\\ =& {\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\otimes {\mathbf{r}}^b-{\mathbf{\omega }}_{wb}^b\otimes {\mathbf{r}}^b\end{array}$$移项可得：
$${\mathbf{\omega }}_{wb}^b\otimes {\mathbf{r}}^b=\left({\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\right)\otimes {\mathbf{r}}^b-{\mathbf{r}}^b\otimes \left({\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\right)$$

利用前面四元数相乘展开成矩阵与向量相乘的公式，
$$\mathbf{p}\otimes \mathbf{q}=[\mathbf{p}{]}_L\mathbf{q}$$其中，$[\mathbf{p}{]}_L=p_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{p}}_v^{\mathrm{T}}\\ {\mathbf{p}}_v& {\left[{\mathbf{p}}_v\right]}_{\times }\end{array}\right]$
$$\mathbf{p}\otimes \mathbf{q}=[\mathbf{q}{]}_R\mathbf{p}$$其中，$[\mathbf{q}{]}_R=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\mathrm{T}}\\ {\mathbf{q}}_v& -{\left[{\mathbf{q}}_v\right]}_{\times }\end{array}\right]$
将 ${\mathbf{\omega }}_{wb}^b\otimes {\mathbf{r}}^b=\left({\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\right)\otimes {\mathbf{r}}^b-{\mathbf{r}}^b\otimes \left({\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\right)$ 展开得，
$$\left[\begin{array}{cc}0& 0_{1\times 3}\\ 0_{3\times 1}& {\left[{\mathbf{\omega }}_{wb}^b\right]}_{\times }\end{array}\right]\left[\begin{array}{c}0\\ {\mathbf{r}}^b\end{array}\right]=\left[\begin{array}{cc}0& 0_{1\times 3}\\ 0_{3\times 1}& 2{\left[{\left({\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\right)}_v\right]}_{\times }\end{array}\right]\left[\begin{array}{c}0\\ {\mathbf{r}}^b\end{array}\right]$$因此有 ${\left({\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\right)}_v=\frac{1}{2}{\mathbf{\omega }}_{wb}^b$，即虚部求解完毕。

根据四元数与旋转矢量的关系，有
$${\mathbf{q}}_{wb}^{\ast }=\left[\begin{array}{c}\cos \frac{\varphi }{2}\\ -\mathbf{u}\sin \frac{\varphi }{2}\end{array}\right]{\dot{\mathbf{q}}}_{wb}=\left[\begin{array}{c}-\frac{\dot{\varphi }}{2}\sin \frac{\varphi }{2}\\ \dot{\mathbf{u}}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\dot{\varphi }}{2}\cos \frac{\varphi }{2}\end{array}\right]$$因此有
$$\begin{array}{l}{\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}=\left[\begin{array}{c}\cos \frac{\varphi }{2}\\ -\mathbf{u}\sin \frac{\varphi }{2}\end{array}\right]\otimes \left[\begin{array}{c}-\frac{\dot{\varphi }}{2}\sin \frac{\varphi }{2}\\ \dot{\mathbf{u}}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\varphi }{2}\cos \frac{\varphi }{2}\end{array}\right]\\ =\left[\begin{array}{c}-\frac{\dot{\varphi }}{2}\sin \frac{\varphi }{2}\cos \frac{\varphi }{2}+{\left(\mathbf{u}\sin \frac{\varphi }{2}\right)}^{\mathrm{T}}\left(\dot{\mathbf{u}}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\dot{\varphi }}{2}\cos \frac{\varphi }{2}\right)\\ \cos \frac{\varphi }{2}\left(\dot{\mathbf{u}}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\dot{\varphi }}{2}\cos \frac{\varphi }{2}\right)+\mathbf{u}\sin \frac{\varphi }{2}\cdot \frac{\dot{\varphi }}{2}\sin \frac{\varphi }{2}-\left(\mathbf{u}\sin \frac{\varphi }{2}\right)\times \left(\dot{\mathbf{u}}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\dot{\varphi }}{2}\cos \frac{\varphi }{2}\right)\end{array}\right]\\ =\left[\begin{array}{c}0\\ \dot{\mathbf{u}}\cos \frac{\varphi }{2}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\dot{\varphi }}{2}-\mathbf{u}\sin \frac{\varphi }{2}\times \dot{\mathbf{u}}\sin \frac{\varphi }{2}\end{array}\right]\end{array}$$可以看出实部为 0
因此有 ${\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}=\frac{1}{2}\left[\begin{array}{c}0\\ {\mathbf{\omega }}_{wb}^b\end{array}\right]⟹{\dot{\mathbf{q}}}_{wb}={\mathbf{q}}_{wb}\otimes \frac{1}{2}\left[\begin{array}{c}0\\ {\mathbf{\omega }}_{wb}^b\end{array}\right]$
(这里的公式很重要，四元数对于时间求导公式。)

2.3 等效旋转矢量微分方程

在旋转矩阵微分方程中，把旋转矩阵用等效旋转矢量表示，则可以求出等效旋转矢量的微分方程。同样地，在四元数微分方程中也可以按此方式得到。(很复杂，这里不证了)

此处直接给出结论
$$\dot{\varphi }={\mathbf{\omega }}_{wb}^b+\frac{1}{2}\mathbf{\varphi }\times {\mathbf{\omega }}_{wb}^b+\frac{1}{{\varphi }^2}\left(1-\frac{\varphi }{2}\cot \frac{\varphi }{2}\right)(\varphi \times {)}^2{\mathbf{\omega }}_{wb}^b$$形式较为复杂，为了化简，对三角函数泰勒展开，并去除高阶项，可得
$$\dot{\mathbf{\varphi }}\approx {\mathbf{\omega }}_{wb}^b+\frac{1}{2}\mathbf{\varphi }\times {\mathbf{\omega }}_{wb}^b$$

反正这三个微分方程都只和 ${\mathbf{\omega }}_{wb}^b$ 相关。