多传感器融合定位七-惯性导航解算及误差分析其一
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 相关。