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

1. 三维运动描述基础知识
- 1.1 概述
- 1.2 姿态描述方法
- - 1.2.1 欧拉角
  - 1.2.2 旋转矩阵
  - 1.2.3 四元数
  - 1.2.4 等效旋转矢量
- 1.3 各描述方法之间的关系
- - 1.3.1 欧拉角与旋转矩阵
  - 1.3.2 旋转矩阵与四元数
  - 1.3.3 旋转矩阵与旋转矢量
  - 1.3.4 四元数与旋转矢量
2. 三维运动的微分性质
- 2.1 旋转矩阵微分方程
- 2.2 四元数微分方程
- 2.3 等效旋转矢量微分方程

Reference:

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

文章跳转：

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

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系)的单位正交基为 ( e 1 , e 2 , e 3 ) \left(\mathbf{e}_1, \mathbf{e}_2, \mathbf{e}_3\right) (e1,e2,e3)，旋转后对应的单位正交基为 ( e 1 ′ , e 2 ′ , e 3 ′ ) \left(\mathbf{e}_1^{\prime}, \mathbf{e}_2^{\prime}, \mathbf{e}_3^{\prime}\right) (e1′,e2′,e3′)
假设在世界坐标系( w \boldsymbol{w} w 系，不随载体的旋转而旋转)下有向量 a \boldsymbol{a} a ，它在旋转前后两个坐标系中的坐标分别为 [ a 1 , a 2 , a 3 ] T \left[a_1, a_2, a_3\right]^{\mathrm{T}} [a1,a2,a3]T 和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T \left[a_1^{\prime}, a_2^{\prime}, a_3^{\prime}\right]^{\mathrm{T}} [a1′,a2′,a3′]T，那么有

[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] \begin{aligned} & {\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{aligned} [e1,e2,e3] a1a2a3 =[e1′,e2′,e3′] a1′a2′a3′ 由此可以得到
a = [ a 1 a 2 a 3 ] = [ 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 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] = def R a ′ \begin{aligned}\boldsymbol{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 }}{=} \boldsymbol{R} \boldsymbol{a}^{\prime} \end{aligned} a= a1a2a3 = e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′ a1′a2′a3′ = def Ra′其中 R \boldsymbol{R} R 便是旋转矩阵，在结合机器人模型推导时，记为 R w b \boldsymbol{R}_{w b} Rwb

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

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

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

1.2.3 四元数

四元数是超复数，即 “复数的复数” 。(这个说法很有意思，但超复数不单单指四元数，比如还包含复数的复数的复数。四元数用来表示三维，但为什么不是三元数表示三维？)
若有复数
A = a + b i B = c + d i \begin{aligned} & A=a+b \boldsymbol{i} \\ & B=c+d \boldsymbol{i} \end{aligned} A=a+biB=c+di则复数的复数为(将 A A A当作复数的实部， B B B当作复数的虚部)：
q = A + B j = a + b i + c j + d i j \begin{aligned} \boldsymbol{q} & =A+B \boldsymbol{j} \\ & =a+b \boldsymbol{i}+c \boldsymbol{j}+d \boldsymbol{i} \boldsymbol{j} \end{aligned} q=A+Bj=a+bi+cj+dij若令 k = i j \boldsymbol{k}=\boldsymbol{i} \boldsymbol{j} k=ij, 则有
q = a + b i + c j + d k \boldsymbol{q}=a+b \boldsymbol{i}+c \boldsymbol{j}+d \boldsymbol{k} q=a+bi+cj+dk此即为四元数。
一般四元数的常见表示符号为：
q = q w + q v = q w + q x i + q y j + q z k \boldsymbol{q}=q_w+\boldsymbol{q}_v=q_w+q_x \boldsymbol{i}+q_y \boldsymbol{j}+q_z \boldsymbol{k} q=qw+qv=qw+qxi+qyj+qzk共轭四元数(实部相同，虚部相反)：
q ∗ = q w − q v = q w − q x i − q y j − q z k \boldsymbol{q}^*=q_w-\boldsymbol{q}_v=q_w-q_x \boldsymbol{i}-q_y \boldsymbol{j}-q_z \boldsymbol{k} q∗=qw−qv=qw−qxi−qyj−qzk四元数的逆：
q − 1 = q ∗ ∥ q ∥ \boldsymbol{q}^{-1}=\frac{\boldsymbol{q}^*}{\|\boldsymbol{q}\|} q−1=∥q∥q∗姿态运算时，四元数为单位四元数(单位四元数即模为一，即 ∥ q ∥ \|\boldsymbol{q}\| ∥q∥)，此时有：
q − 1 = q ∗ = q w − q v \boldsymbol{q}^{-1}=\boldsymbol{q}^*=q_w-\boldsymbol{q}_v q−1=q∗=qw−qv四元数乘法：(注意单位 ⊗ \otimes ⊗)
p ⊗ q = [ p w q w − p v T q v p w q v + q w p v + p v × q v ] \boldsymbol{p} \otimes \boldsymbol{q}=\left[\begin{array}{c} p_w q_w-\boldsymbol{p}_v^{\mathrm{T}} \boldsymbol{q}_v \\ p_w \boldsymbol{q}_v+q_w \boldsymbol{p}_v+\boldsymbol{p}_v \times \boldsymbol{q}_v \end{array}\right] p⊗q=[pwqw−pvTqvpwqv+qwpv+pv×qv]若在四元数乘法中出现三维向量，指的是和三维向量构成的纯虚四元数相乘，比如
p ⊗ u = p ⊗ [ 0 u 1 i u 2 j u 3 k ] \boldsymbol{p} \otimes \boldsymbol{u}=\boldsymbol{p} \otimes\left[\begin{array}{c} 0 \\ u_1 \boldsymbol{i} \\ u_2 \boldsymbol{j} \\ u_3 \boldsymbol{k} \end{array}\right] p⊗u=p⊗ 0u1iu2ju3k 乘法结合律
( p ⊗ q ) ⊗ r = p ⊗ ( q ⊗ r ) p ⊗ ( q + r ) = p ⊗ q + p ⊗ r ( p + q ) ⊗ r = p ⊗ r + q ⊗ r \begin{aligned} & (\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{aligned} (p⊗q)⊗r=p⊗(q⊗r)p⊗(q+r)=p⊗q+p⊗r(p+q)⊗r=p⊗r+q⊗r四元数相乘，可以展开为矩阵与向量相乘的形式
p ⊗ q = [ p ] L q \boldsymbol{p} \otimes \boldsymbol{q}=[\boldsymbol{p}]_L \boldsymbol{q} p⊗q=[p]Lq其中
[ p ] L = [ 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 ] = p w I + [ 0 − p v T p v [ p v ] × ] \begin{aligned} {[\boldsymbol{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 \boldsymbol{I}+\left[\begin{array}{ccc} 0 & -\boldsymbol{p}_v^{\mathrm{T}} \\ \boldsymbol{p}_v & {\left[\boldsymbol{p}_v\right]_{\times}} \end{array}\right] \end{aligned} [p]L= pwpxpypz−pxpwpz−py−py−pzpwpx−pzpy−pxpw =pwI+[0pv−pvT[pv]×]其中，符号 [ ∙ ] × = [ ∙ ] ∧ [\bullet]_{\times}=[\bullet]^{\wedge} [∙]×=[∙]∧，同样表示反对称矩阵。(就一个东西)

也可以展开为
p ⊗ q = [ q ] R p \boldsymbol{p} \otimes \boldsymbol{q}=[\boldsymbol{q}]_R \boldsymbol{p} p⊗q=[q]Rp其中
[ q ] R = [ 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 ] = q w I + [ 0 − q v T q v − [ q v ] × ] \begin{aligned} {[\boldsymbol{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 \boldsymbol{I}+\left[\begin{array}{ccc} 0 & -\boldsymbol{q}_v^{\mathrm{T}} \\ \boldsymbol{q}_v & -\left[\boldsymbol{q}_v\right]_{\times} \end{array}\right] \end{aligned} [q]R= qwqxqyqz−qxqw−qzqy−qyqzqw−qx−qz−qyqxqw =qwI+[0qv−qvT−[qv]×]由此，可以得出重要性质(后续推导时常用)(可以看出来 [ p ] R [ q ] L = [ q ] L [ p ] R [\mathbf{p}]_R[\mathbf{q}]_L=[\mathbf{q}]_L[\mathbf{p}]_R [p]R[q]L=[q]L[p]R)：
( q ⊗ x ) ⊗ p = [ p ] R [ q ] L x q ⊗ ( x ⊗ p ) = [ q ] L [ p ] R x \begin{aligned} & (\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{aligned} (q⊗x)⊗p=[p]R[q]Lxq⊗(x⊗p)=[q]L[p]Rx

1.2.4 等效旋转矢量

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

1.3 各描述方法之间的关系

1.3.1 欧拉角与旋转矩阵

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

b. 旋转矩阵转欧拉角

由欧拉角得到的旋转矩阵，其完整形式为：
R w b = [ 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 β c α c β ] \boldsymbol{R}_{w b}=\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] Rwb= cβcγcβsγsβ−sαsβcγ−cαsγcαcγ−sαsβsγsαcβsαsγ−cαsβcγ−cαsβsγ−sαcγcαcβ 其中符号： s ∙ = sin ⁡ ( ∙ ) c ∙ = cos ⁡ ( ∙ ) s \bullet=\sin (\bullet) \quad c \bullet=\cos (\bullet) s∙=sin(∙)c∙=cos(∙)
观察矩阵，可以看出：
α = 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 ) ) \begin{aligned} & \alpha=\arctan 2\left(\boldsymbol{R}_{w b}(3,2), \boldsymbol{R}_{w b}(3,3)\right) \\ & \beta=\arcsin \left(\boldsymbol{R}_{w b}(3,1)\right) \\ & \gamma=\arctan 2\left(\boldsymbol{R}_{w b}(2,1), \boldsymbol{R}_{w b}(1,1)\right) \end{aligned} α=arctan2(Rwb(3,2),Rwb(3,3))β=arcsin(Rwb(3,1))γ=arctan2(Rwb(2,1),Rwb(1,1))

1.3.2 旋转矩阵与四元数

它们转换的推导过程较为复杂，此处直接给出结论。
四元数转旋转矩阵(常用)：
R w b = [ q w 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 ] \boldsymbol{R}_{w b}=\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] Rwb= qw2+qx2−qy2−qz22(qxqy+qwqz)2(qxqz−qwqy)2(qxqy−qwqz)qw2−qx2+qy2−qz22(qyqz+qwqx)2(qxqz+qwqy)2(qyqz−qwqx)qw2−qx2−qy2+qz2 旋转矩阵转四元数(不那么常用)：
q w = 1 + R w b ( 1 , 1 ) + R w b ( 2 , 2 ) + R w b ( 3 , 3 ) 2 q x = R w b ( 3 , 2 ) − R w b ( 2 , 3 ) 4 q w q y = R w b ( 1 , 3 ) − R w b ( 3 , 1 ) 4 q w q z = R w b ( 2 , 1 ) − R w b ( 1 , 2 ) 4 q w \begin{aligned} & q_w=\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_{w 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{aligned} qw=21+Rwb(1,1)+Rwb(2,2)+Rwb(3,3) qx=4qwRwb(3,2)−Rwb(2,3)qy=4qwRwb(1,3)−Rwb(3,1)qz=4qwRwb(2,1)−Rwb(1,2)需要注意的是，这需要满足：
q w ≠ 0 , 1 + R w b ( 1 , 1 ) + R w b ( 2 , 2 ) + R w b ( 3 , 3 ) > 0 q_w \neq 0,1+R_{w b}(1,1)+R_{w b}(2,2)+R_{w b}(3,3)>0 qw=0,1+Rwb(1,1)+Rwb(2,2)+Rwb(3,3)>0当不满足该条件时，转换步骤比较复杂，此处不讲述。

1.3.3 旋转矩阵与旋转矢量

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

1.3.4 四元数与旋转矢量

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

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

当旋转一段微小时间，即角度趋于 0 0 0 时，容易有：
Δ q = [ cos ⁡ δ θ 2 u sin ⁡ δ θ 2 ] ≈ [ 1 u δ θ 2 ] = [ 1 1 2 δ θ ] \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} \boldsymbol{\delta} \theta \end{array}\right] Δq=[cos2δθusin2δθ]≈[1u2δθ]=[121δθ]其中 δ θ \delta \boldsymbol{\theta} δθ 的方向表示旋转轴，模长表示旋转角度。

b. 由四元数计算旋转矢量
ϕ = 2 arctan ⁡ ( ∥ q v ∥ , q w ) u = q v / ∥ q v ∥ \begin{gathered} \phi=2 \arctan \left(\left\|\boldsymbol{q}_v\right\|, q_w\right) \\ \boldsymbol{u}=\boldsymbol{q}_v /\left\|\boldsymbol{q}_v\right\| \end{gathered} ϕ=2arctan(∥qv∥,qw)u=qv/∥qv∥

2. 三维运动的微分性质

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

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

2.1 旋转矩阵微分方程

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

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

2.2 四元数微分方程

r w \boldsymbol{r}^w rw 和 r b \boldsymbol{r}^b rb 两个矢量之间可以用四元数转换如下：
r w = q w b ⊗ r b ⊗ q w b ∗ \boldsymbol{r}^w=\boldsymbol{q}_{w b} \otimes \boldsymbol{r}^b \otimes \boldsymbol{q}_{w b}^* rw=qwb⊗rb⊗qwb∗等式两边同时右乘 q w b \boldsymbol{q}_{w b} qwb，可得：
r w ⊗ q w b = q w b ⊗ r b \boldsymbol{r}^w \otimes \boldsymbol{q}_{w b}=\boldsymbol{q}_{w b} \otimes \boldsymbol{r}^b rw⊗qwb=qwb⊗rb上式两边同时求微分，可得：
r ˙ w ⊗ q w b + r w ⊗ q ˙ w b = q ˙ w b ⊗ r b + q w b ⊗ r ˙ b \begin{aligned} & \dot{\boldsymbol{r}}^w \otimes \boldsymbol{q}_{w b}+\boldsymbol{r}^w \otimes \dot{\boldsymbol{q}}_{w b} \\ = & \dot{\boldsymbol{q}}_{w b} \otimes \boldsymbol{r}^b+\boldsymbol{q}_{w b} \otimes \dot{\boldsymbol{r}}^b \end{aligned} =r˙w⊗qwb+rw⊗q˙wbq˙wb⊗rb+qwb⊗r˙b由于：
r ˙ b = − ω w b b × r b = − ω w b b ⊗ r b r ˙ w = 0 \begin{aligned} & \dot{\boldsymbol{r}}^b=-\boldsymbol{\omega}_{w b}^b \times \boldsymbol{r}^b=-\boldsymbol{\omega}_{w b}^b \otimes \boldsymbol{r}^b \\ & \dot{\boldsymbol{r}}^w=\mathbf{0} \end{aligned} r˙b=−ωwbb×rb=−ωwbb⊗rbr˙w=0则有：
r w ⊗ q ˙ w b = ( q w b ⊗ r b ⊗ q w b ∗ ) ⊗ q ˙ w b = q ˙ w b ⊗ r b − q w b ⊗ ω w b b ⊗ r b \begin{aligned} & \boldsymbol{r}^w \otimes \dot{\boldsymbol{q}}_{w b} \\ = & \left(\boldsymbol{q}_{w b} \otimes \boldsymbol{r}^b \otimes \boldsymbol{q}_{w b}^*\right) \otimes \dot{\boldsymbol{q}}_{w b} \\ = & \dot{\boldsymbol{q}}_{w b} \otimes \boldsymbol{r}^b-\boldsymbol{q}_{w b} \otimes \boldsymbol{\omega}_{w b}^b \otimes \boldsymbol{r}^b \end{aligned} ==rw⊗q˙wb(qwb⊗rb⊗qwb∗)⊗q˙wbq˙wb⊗rb−qwb⊗ωwbb⊗rb等式两边同时左乘 q w b ∗ \boldsymbol{q}_{w b}^* qwb∗ ，可得：
r b ⊗ q w b ∗ ⊗ q ˙ w b = q w b ∗ ⊗ q ˙ w b ⊗ r b − ω w b b ⊗ r b \begin{aligned} & \boldsymbol{r}^b \otimes \boldsymbol{q}_{w b}^* \otimes \dot{\boldsymbol{q}}_{w b} \\ = & \boldsymbol{q}_{w b}^* \otimes \dot{\boldsymbol{q}}_{w b} \otimes \boldsymbol{r}^b-\boldsymbol{\omega}_{w b}^b \otimes \boldsymbol{r}^b \end{aligned} =rb⊗qwb∗⊗q˙wbqwb∗⊗q˙wb⊗rb−ωwbb⊗rb移项可得：
ω w b b ⊗ r b = ( q w b ∗ ⊗ q ˙ w b ) ⊗ r b − r b ⊗ ( q w b ∗ ⊗ q ˙ w b ) \boldsymbol{\omega}_{w b}^b \otimes \boldsymbol{r}^b=\left(\boldsymbol{q}_{w b}^* \otimes \dot{\boldsymbol{q}}_{w b}\right) \otimes \boldsymbol{r}^b-\boldsymbol{r}^b \otimes\left(\boldsymbol{q}_{w b}^* \otimes \dot{\boldsymbol{q}}_{w b}\right) ωwbb⊗rb=(qwb∗⊗q˙wb)⊗rb−rb⊗(qwb∗⊗q˙wb)

利用前面四元数相乘展开成矩阵与向量相乘的公式，
p ⊗ q = [ p ] L q \boldsymbol{p} \otimes \boldsymbol{q}=[\boldsymbol{p}]_L \boldsymbol{q} p⊗q=[p]Lq其中， [ p ] L = p w I + [ 0 − p v T p v [ p v ] × ] [\boldsymbol{p}]_L=p_w \boldsymbol{I}+\left[\begin{array}{cc}0 & -\boldsymbol{p}_v^{\mathrm{T}} \\ \boldsymbol{p}_v & {\left[\boldsymbol{p}_v\right]_{\times}}\end{array}\right] \quad \quad [p]L=pwI+[0pv−pvT[pv]×]
p ⊗ q = [ q ] R p \boldsymbol{p} \otimes \boldsymbol{q}=[\boldsymbol{q}]_R \boldsymbol{p} p⊗q=[q]Rp其中， [ q ] R = q w I + [ 0 − q v T q v − [ q v ] × ] [\boldsymbol{q}]_R=q_w \boldsymbol{I}+\left[\begin{array}{cc}0 & -\boldsymbol{q}_v^{\mathrm{T}} \\ \boldsymbol{q}_v & -\left[\boldsymbol{q}_v\right]_{\times}\end{array}\right] [q]R=qwI+[0qv−qvT−[qv]×]
将 ω w b b ⊗ r b = ( q w b ∗ ⊗ q ˙ w b ) ⊗ r b − r b ⊗ ( q w b ∗ ⊗ q ˙ w b ) \boldsymbol{\omega}_{w b}^b \otimes \boldsymbol{r}^b=\left(\boldsymbol{q}_{w b}^* \otimes \dot{\boldsymbol{q}}_{w b}\right) \otimes \boldsymbol{r}^b-\boldsymbol{r}^b \otimes\left(\boldsymbol{q}_{w b}^* \otimes \dot{\boldsymbol{q}}_{w b}\right) ωwbb⊗rb=(qwb∗⊗q˙wb)⊗rb−rb⊗(qwb∗⊗q˙wb) 展开得，
[ 0 0 1 × 3 0 3 × 1 [ ω w b b ] × ] [ 0 r b ] = [ 0 0 1 × 3 0 3 × 1 2 [ ( q w b ∗ ⊗ q ˙ w b ) v ] × ] [ 0 r b ] \left[\begin{array}{cc} 0 & \mathbf{0}_{1 \times 3} \\ \mathbf{0}_{3 \times 1} & {\left[\boldsymbol{\omega}_{w b}^b\right]_{\times}} \end{array}\right]\left[\begin{array}{c} 0 \\ \boldsymbol{r}^b \end{array}\right]=\left[\begin{array}{cc} 0 & \mathbf{0}_{1 \times 3} \\ \mathbf{0}_{3 \times 1} & 2\left[\left(\boldsymbol{q}_{w b}^* \otimes \dot{\boldsymbol{q}}_{w b}\right)_v\right]_{\times} \end{array}\right]\left[\begin{array}{c} 0 \\ \boldsymbol{r}^b \end{array}\right] [003×101×3[ωwbb]×][0rb]=[003×101×32[(qwb∗⊗q˙wb)v]×][0rb]因此有 ( q w b ∗ ⊗ q ˙ w b ) v = 1 2 ω w b b \left(\boldsymbol{q}_{w b}^* \otimes \dot{\boldsymbol{q}}_{w b}\right)_v=\frac{1}{2} \boldsymbol{\omega}_{w b}^b (qwb∗⊗q˙wb)v=21ωwbb，即虚部求解完毕。

根据四元数与旋转矢量的关系，有
q w b ∗ = [ cos ⁡ ϕ 2 − u sin ⁡ ϕ 2 ] q ˙ w b = [ − ϕ ˙ 2 sin ⁡ ϕ 2 u ˙ sin ⁡ ϕ 2 + u ϕ ˙ 2 cos ⁡ ϕ 2 ] \boldsymbol{q}_{w b}^*=\left[\begin{array}{c} \cos \frac{\phi}{2} \\ -\boldsymbol{u} \sin \frac{\phi}{2} \end{array}\right] \quad \dot{\boldsymbol{q}}_{w b}=\left[\begin{array}{c} -\frac{\dot{\phi}}{2} \sin \frac{\phi}{2} \\ \dot{\boldsymbol{u}} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\dot{\phi}}{2} \cos \frac{\phi}{2} \end{array}\right] qwb∗=[cos2ϕ−usin2ϕ]q˙wb=[−2ϕ˙sin2ϕu˙sin2ϕ+u2ϕ˙cos2ϕ]因此有
q w b ∗ ⊗ q ˙ w b = [ cos ⁡ ϕ 2 − u sin ⁡ ϕ 2 ] ⊗ [ − ϕ ˙ 2 sin ⁡ ϕ 2 u ˙ sin ⁡ ϕ 2 + u ϕ 2 cos ⁡ ϕ 2 ] = [ − ϕ ˙ 2 sin ⁡ ϕ 2 cos ⁡ ϕ 2 + ( u sin ⁡ ϕ 2 ) T ( u ˙ sin ⁡ ϕ 2 + u ϕ ˙ 2 cos ⁡ ϕ 2 ) cos ⁡ ϕ 2 ( u ˙ sin ⁡ ϕ 2 + u ϕ ˙ 2 cos ⁡ ϕ 2 ) + u sin ⁡ ϕ 2 ⋅ ϕ ˙ 2 sin ⁡ ϕ 2 − ( u sin ⁡ ϕ 2 ) × ( u ˙ sin ⁡ ϕ 2 + u ϕ ˙ 2 cos ⁡ ϕ 2 ) ] = [ 0 u ˙ cos ⁡ ϕ 2 sin ⁡ ϕ 2 + u ϕ ˙ 2 − u sin ⁡ ϕ 2 × u ˙ sin ⁡ ϕ 2 ] \begin{array}{l} \boldsymbol{q}_{w b}^* \otimes \dot{\boldsymbol{q}}_{w b}=\left[\begin{array}{c} \cos \frac{\phi}{2} \\ -\boldsymbol{u} \sin \frac{\phi}{2} \end{array}\right] \otimes\left[\begin{array}{c} -\frac{\dot{\phi}}{2} \sin \frac{\phi}{2} \\ \dot{\boldsymbol{u}} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\phi}{2} \cos \frac{\phi}{2} \end{array}\right] \\ =\left[\begin{array}{c} -\frac{\dot{\phi}}{2} \sin \frac{\phi}{2} \cos \frac{\phi}{2}+\left(\boldsymbol{u} \sin \frac{\phi}{2}\right)^{\mathrm{T}}\left(\dot{\boldsymbol{u}} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\dot{\phi}}{2} \cos \frac{\phi}{2}\right) \\ \cos \frac{\phi}{2}\left(\dot{\boldsymbol{u}} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\dot{\phi}}{2} \cos \frac{\phi}{2}\right)+\boldsymbol{u} \sin \frac{\phi}{2} \cdot \frac{\dot{\phi}}{2} \sin \frac{\phi}{2}-\left(\boldsymbol{u} \sin \frac{\phi}{2}\right) \times\left(\dot{\boldsymbol{u}} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\dot{\phi}}{2} \cos \frac{\phi}{2}\right) \end{array}\right] \\ =\left[\begin{array}{c} 0 \\ \dot{\boldsymbol{u}} \cos \frac{\phi}{2} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\dot{\phi}}{2}-\boldsymbol{u} \sin \frac{\phi}{2} \times \dot{\boldsymbol{u}} \sin \frac{\phi}{2} \end{array}\right] \\ \end{array} qwb∗⊗q˙wb=[cos2ϕ−usin2ϕ]⊗[−2ϕ˙sin2ϕu˙sin2ϕ+u2ϕcos2ϕ]= −2ϕ˙sin2ϕcos2ϕ+(usin2ϕ)T(u˙sin2ϕ+u2ϕ˙cos2ϕ)cos2ϕ(u˙sin2ϕ+u2ϕ˙cos2ϕ)+usin2ϕ⋅2ϕ˙sin2ϕ−(usin2ϕ)×(u˙sin2ϕ+u2ϕ˙cos2ϕ) =[0u˙cos2ϕsin2ϕ+u2ϕ˙−usin2ϕ×u˙sin2ϕ]可以看出实部为 0
因此有 q w b ∗ ⊗ q ˙ w b = 1 2 [ 0 ω w b b ] ⟹ q ˙ w b = q w b ⊗ 1 2 [ 0 ω w b b ] \quad \boldsymbol{q}_{w b}^* \otimes \dot{\boldsymbol{q}}_{w b}=\frac{1}{2}\left[\begin{array}{c}0 \\ \boldsymbol{\omega}_{w b}^b\end{array}\right] \Longrightarrow \dot{\boldsymbol{q}}_{w b}=\boldsymbol{q}_{w b} \otimes \frac{1}{2}\left[\begin{array}{c}0 \\ \boldsymbol{\omega}_{w b}^b\end{array}\right] qwb∗⊗q˙wb=21[0ωwbb]⟹q˙wb=qwb⊗21[0ωwbb]
(这里的公式很重要，四元数对于时间求导公式。)

2.3 等效旋转矢量微分方程

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

此处直接给出结论
ϕ ˙ = ω w b b + 1 2 ϕ × ω w b b + 1 ϕ 2 ( 1 − ϕ 2 cot ⁡ ϕ 2 ) ( ϕ × ) 2 ω w b b \dot{\phi}=\boldsymbol{\omega}_{w b}^b+\frac{1}{2} \boldsymbol{\phi} \times \boldsymbol{\omega}_{w b}^b+\frac{1}{\phi^2}\left(1-\frac{\phi}{2} \cot \frac{\phi}{2}\right)(\phi \times)^2 \boldsymbol{\omega}_{w b}^b ϕ˙=ωwbb+21ϕ×ωwbb+ϕ21(1−2ϕcot2ϕ)(ϕ×)2ωwbb形式较为复杂，为了化简，对三角函数泰勒展开，并去除高阶项，可得
ϕ ˙ ≈ ω w b b + 1 2 ϕ × ω w b b \dot{\boldsymbol{\phi}}\approx\boldsymbol{\omega}_{w b}^b+\frac{1}{2} \boldsymbol{\phi} \times \boldsymbol{\omega}_{w b}^b ϕ˙≈ωwbb+21ϕ×ωwbb

反正这三个微分方程都只和 ω w b b \boldsymbol{\omega}_{w b}^b ωwbb 相关。

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

多传感器融合定位六-惯性导航原理及误差分析
多传感器融合定位六-惯性导航原理及误差分析 1. 惯性技术简介 1.1 惯性技术发展历史 1.2 惯性器件 1.2.1 机械陀螺(几乎没人用了) 1.2.2 激光陀螺 1.2.3 光纤陀螺 1.2.4 ...
《多传感器融合定位》惯性导航基础（二）
续: <多传感器融合定位>惯性导航基础(一). 惯性导航基础习题二四.一组对自定义数据进行惯性导航解算验证 1.使用gnss-ins-sim仿真imu运动数据 2.对自定义运动惯性导航解 ...
多传感器融合定位三-3D激光里程计其三：点云畸变补偿
多传感器融合定位三-3D激光里程计其三:点云畸变补偿 1. 产生原因 2. 补偿方法 Reference: 深蓝学院-多传感器融合多传感器融合定位理论基础文章跳转: 多传感器融合定位一-3D激光里 ...
多传感器融合定位五-点云地图构建及定位
多传感器融合定位五-点云地图构建及定位 1. 回环检测 1.1 基于Scan Context 1.2 基于直方图 2. 后端优化 2.1 后端优化基本原理 2.2 李群.李代数基本知识 2.3 李群. ...
多传感器融合定位十四-基于图优化的定位方法
多传感器融合定位十四-基于图优化的定位方法 1. 基于图优化的定位简介 1.1 核心思路 1.2 定位流程 2. 边缘化原理及应用 2.1 边缘化原理 2.2 从滤波角度理解边缘化 3. 基于kitt ...
多传感器融合定位十五-多传感器时空标定(综述)
多传感器融合定位十五-多传感器时空标定 1. 多传感器标定简介 1.1 标定内容及方法 1.2 讲解思路 2. 内参标定 2.1 雷达内参标定 2.2 IMU内参标定 2.3 编码器内参标定 2.4 ...
多传感器融合定位四-3D激光里程计其四：点云线面特征提取
多传感器融合定位四-3D激光里程计其四:点云线面特征提取 1. 点云线面特征提取 1.1 按线数分割 1.2 计算曲率(重要!) 1.3 按曲率大小筛选特征点 2. 基于线面特征的位姿变化 2.1 帧 ...
多传感器融合定位二-3D激光里程计其二：NDT
多传感器融合定位二-3D激光里程计其二:NDT 1. 经典NDT 2. 计算方式 2.1 2D场景求解: 2.2 3D场景求解: 3. 其他 NDT Reference: 深蓝学院-多传感器融合多传 ...
多传感器融合定位第六章惯性导航结算及误差模型
多传感器融合定位第六章惯性导航结算及误差模型参考博客:深蓝学院-多传感器融合定位-第6章作业代码下载:https://github.com/kahowang/sensor-fusion-for ...

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

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

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

1.1 概述

1.2 姿态描述方法

1.2.1 欧拉角

1.2.2 旋转矩阵

1.2.3 四元数

1.2.4 等效旋转矢量

1.3 各描述方法之间的关系

1.3.1 欧拉角与旋转矩阵

1.3.2 旋转矩阵与四元数

1.3.3 旋转矩阵与旋转矢量

1.3.4 四元数与旋转矢量

2. 三维运动的微分性质

2.1 旋转矩阵微分方程

2.2 四元数微分方程

2.3 等效旋转矢量微分方程

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

最新文章

热门文章