刚体运动几何（一）
参考：《A Mathematical Introduction to Robotic Manipulation》
疑问：
- 刚体的速度只有在选定坐标系时才有谈论意义，比如一台小车可以看作是一个刚体，其上有三个传感器，Camera有一个速度，IMU有一个速度，轮子有一个速度，首先它们的速度是不同的（比如把地球看作一个刚体，地心的速度与地球表面的速度是不同的），那么这些速度的关系是什么呢？
- VIO中为何不使用Camera系而是IMU作为body系，如果使用Camera作为body系，IMU的测量值（角速度和加速度）要如何转化到Camera坐标系下？
1. 转动速度
刚体上任意一点
q
q
q在空间坐标系中的运动轨迹为：
q
a
(
t
)
=
R
a
b
(
t
)
q
b
\mathbf{q}_{a}(t)=\mathbf{R}_{a b}(t) \mathbf{q}_{b}
qa(t)=Rab(t)qb
其中，A为空间坐标系，B为物体坐标系，坐标
q
b
\mathbf{q}_b
qb是在物体坐标系中固定不变的，该点在空间坐标系中的速度为：
v
q
a
(
t
)
=
d
d
t
q
a
(
t
)
=
R
˙
a
b
(
t
)
q
b
\mathbf{v}_{\mathbf{q}_{a}}(t)=\frac{d}{d t} \mathbf{q}_{a}(t)=\dot{\mathbf{R}}_{a b}(t) \mathbf{q}_{b}
vqa(t)=dtdqa(t)=R˙ab(t)qb
因此，
R
a
b
\mathbf{R}_{a b}
Rab将一点的刚体坐标变换为该点的空间速度，类似
R
a
b
\mathbf{R}_{a b}
Rab这种表示需要九个参数，不是很方便，转换成下面的简洁形式：
v
q
a
(
t
)
=
R
˙
a
b
(
t
)
R
a
b
−
1
(
t
)
R
a
b
(
t
)
q
b
v_{q_{a}}(t)=\dot{R}_{a b}(t) R_{a b}^{-1}(t) R_{a b}(t) q_{b}
vqa(t)=R˙ab(t)Rab−1(t)Rab(t)qb
我们已经知道
R
˙
a
b
(
t
)
R
a
b
−
1
(
t
)
∈
so
(
3
)
\dot{R}_{a b}(t) R_{a b}^{-1}(t) \in \operatorname{so}(3)
R˙ab(t)Rab−1(t)∈so(3)，是反对陈矩阵，记为：
ω
^
a
b
s
:
=
R
˙
a
b
R
a
b
−
1
\widehat{\omega}_{a b}^{s}:=\dot{R}_{a b} R_{a b}^{-1}
ω
abs:=R˙abRab−1
其中， ω ^ a b s \widehat{\omega}_{a b}^{s} ω abs表示 b b b系在 a a a系下的速度在空间坐标系下的表示，描述为物体相对空间坐标系A的瞬时角速度
1.1 刚体上一点的速度（A系与B系之间只有纯旋转运动）
现在就可以使用物体的瞬时角速度来表示一点的速度，如下：
v
q
a
(
t
)
=
ω
^
a
b
s
R
a
b
(
t
)
q
b
=
ω
a
b
s
(
t
)
×
q
a
(
t
)
v_{q_{a}}(t)=\widehat{\omega}_{a b}^{s} R_{a b}(t) q_{b}=\omega_{a b}^{s}(t) \times q_{a}(t)
vqa(t)=ω
absRab(t)qb=ωabs(t)×qa(t)
对应的物理含义为：刚体的转速与刚体上一点到坐标系原点的距离的叉乘即为这一点的瞬时速度， q b q_{b} qb这个向量的长度越长，速度就越大，反之亦然，当长度为0时（该点与坐标系原点重合时），速度为0
这样，只要知道物体的瞬时角速度和物体坐标上任意一点的位置，就可以得到该点在空间坐标下的速度
1.2 （瞬时）物体坐标系下的转动速度
同理也可得到
ω
^
a
b
b
\widehat{\omega}_{a b}^{b}
ω
abb，如下：
ω
^
a
b
b
:
=
R
a
b
−
1
R
˙
a
b
\widehat{\omega}_{a b}^{b}:=R_{a b}^{-1} \dot{R}_{a b}
ω
abb:=Rab−1R˙ab
它描述的是相对于瞬时物体坐标系B的角速度（ b b b系在 a a a系下的速度在物体坐标系下的表示），该角速度可以直接通过陀螺仪直接测量得到
由以上两式可得两种角速度之间的关系：
ω
^
a
b
b
=
R
a
b
−
1
ω
^
a
b
s
R
a
b
or
ω
a
b
b
=
R
a
b
−
1
ω
a
b
s
\widehat{\omega}_{a b}^{b}=R_{a b}^{-1} \widehat{\omega}_{a b}^{s} R_{a b} \quad \text { or } \quad \omega_{a b}^{b}=R_{a b}^{-1} \omega_{a b}^{s}
ω
abb=Rab−1ω
absRab or ωabb=Rab−1ωabs
因此，物体角速度为将空间角速度旋转到瞬时物体坐标系而得到的
2. 刚体速度
对于运动轨迹为为单参数曲线
g
a
b
(
t
)
∈
S
E
(
3
)
g_{a b}(t) \in S E(3)
gab(t)∈SE(3)的一般情况
g
a
b
(
t
)
=
[
R
a
b
(
t
)
p
a
b
(
t
)
0
1
]
g_{a b}(t)=\left[\begin{array}{cc} R_{a b}(t) & p_{a b}(t) \\ 0 & 1 \end{array}\right]
gab(t)=[Rab(t)0pab(t)1]
可得，
g
˙
a
b
g
a
b
−
1
=
[
R
˙
a
b
p
˙
a
b
0
0
]
[
R
a
b
T
−
R
a
b
T
p
a
b
0
1
]
=
[
R
˙
a
b
R
a
b
T
−
R
˙
a
b
R
a
b
T
p
a
b
+
p
˙
a
b
0
0
]
\dot{g}_{a b} g_{a b}^{-1}=\left[\begin{array}{cc} \dot{R}_{a b} & \dot{p}_{a b} \\ 0 & 0 \end{array}\right]\left[\begin{array}{cc} R_{a b}^{T} & -R_{a b}^{T} p_{a b} \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} \dot{R}_{a b} R_{a b}^{T} & -\dot{R}_{a b} R_{a b}^{T} p_{a b}+\dot{p}_{a b} \\ 0 & 0 \end{array}\right]
g˙abgab−1=[R˙ab0p˙ab0][RabT0−RabTpab1]=[R˙abRabT0−R˙abRabTpab+p˙ab0]
定义
V
a
b
s
∈
s
e
(
3
)
V_{ab}^s \in se(3)
Vabs∈se(3)表示
b
b
b系在
a
a
a系下的速度在空间坐标系下的表示
V
^
a
b
s
=
g
˙
a
b
g
a
b
−
1
,
V
a
b
s
=
[
v
a
b
s
ω
a
b
s
]
=
[
−
R
˙
a
b
R
a
b
T
p
a
b
+
p
˙
a
b
(
R
˙
a
b
R
a
b
T
)
∨
]
\widehat{V}_{a b}^{s}=\dot{g}_{a b} g_{a b}^{-1} ,\quad V_{a b}^{s}=\left[\begin{array}{c} v_{a b}^{s} \\ \omega_{a b}^{s} \end{array}\right]=\left[\begin{array}{c} -\dot{R}_{a b} R_{a b}^{T} p_{a b}+\dot{p}_{a b} \\ \left(\dot{R}_{a b} R_{a b}^{T}\right)^{\vee} \end{array}\right]
V
abs=g˙abgab−1,Vabs=[vabsωabs]=[−R˙abRabTpab+p˙ab(R˙abRabT)∨]
虽然 V a b s ∈ s e ( 3 ) V_{ab}^s \in se(3) Vabs∈se(3)，但这里的移动分量 v a b s v_{a b}^{s} vabs却是以
m/s为单位的平移速度，因为 p ˙ a b \dot{p}_{a b} p˙ab的单位是m/s
注意，移动分量 v a b s ( t ) v_{ab}^s(t) vabs(t) 并不是刚体坐标系原点的速度，而是刚体上一点在 t t t时刻经过空间坐标系原点时（可能是想象的）的速度。即，如果一个人站在空间坐标系的原点处去测量刚体上正好经过原点的一点的瞬时速度，那么该瞬时速度即为 v a b s ( t ) v_{ab}^s(t) vabs(t)
2.1 刚体上一点的速度
在空间坐标系中，固联在刚体上的一点
q
q
q的坐标为
q
a
(
t
)
=
g
a
b
(
t
)
q
b
q_{a}(t)=g_{a b}(t) q_{b}
qa(t)=gab(t)qb
两边微分得
v
q
a
=
q
˙
a
=
g
˙
a
b
q
b
=
g
˙
a
b
g
a
b
−
1
q
a
v_{q_{a}}=\dot{q}_{a}=\dot{g}_{a b} q_{b}=\dot{g}_{a b} g_{a b}^{-1} q_{a}
vqa=q˙a=g˙abqb=g˙abgab−1qa
进一步得到
v
q
a
=
V
^
a
b
s
q
a
=
ω
a
b
s
×
q
a
+
v
a
b
s
v_{q_{a}}=\widehat{V}_{a b}^{s} q_{a}=\omega_{a b}^{s} \times q_{a}+v_{a b}^{s}
vqa=V
absqa=ωabs×qa+vabs
其中， v q a v_{q_{a}} vqa由两部分构成， ω a b s × q a \omega_{a b}^{s} \times q_{a} ωabs×qa表示因旋转产生的速度， v a b s v_{a b}^{s} vabs表示平移产生的速度，两个向量相加即为最终的合成速度
2.2 （瞬时）物体坐标系下的刚体速度
向量可以直接通过坐标系之间旋转外参进行转换
由于
v
q
a
v_{q_{a}}
vqa是向量，使用旋转矩阵进行转换，可得物体坐标系中一点的速度为：
[
v
q
b
0
]
=
g
a
b
−
1
[
v
q
a
0
]
\begin{bmatrix} v_{q_{b}} \\ 0 \end{bmatrix}=g_{a b}^{-1} \begin{bmatrix} v_{q_{a}} \\ 0 \end{bmatrix}
[vqb0]=gab−1[vqa0]
忽略矩阵维度，简写成：
v
q
b
=
g
a
b
−
1
v
q
a
=
g
a
b
−
1
g
˙
a
b
q
b
=
V
^
a
b
b
(
t
)
q
b
v_{q_{b}}=g_{a b}^{-1} v_{q_{a}}=g_{a b}^{-1} \dot{g}_{a b} q_{b}=\widehat{V}_{a b}^{b}(t) q_{b}
vqb=gab−1vqa=gab−1g˙abqb=V
abb(t)qb
物体坐标系下的刚体速度定义为：
V
^
a
b
b
=
g
a
b
−
1
g
˙
a
b
=
[
R
a
b
T
R
˙
a
b
R
a
b
T
p
˙
a
b
0
0
]
,
V
a
b
b
=
[
v
a
b
b
ω
a
b
b
]
=
[
R
a
b
T
p
˙
a
b
(
R
a
b
T
R
˙
a
b
)
∨
]
\widehat{V}_{a b}^{b}=g_{a b}^{-1} \dot{g}_{a b}=\left[\begin{array}{cc} R_{a b}^{T} \dot{R}_{a b} & R_{a b}^{T} \dot{p}_{a b} \\ 0 & 0 \end{array}\right] ,\quad V_{a b}^{b}=\left[\begin{array}{c} v_{a b}^{b} \\ \omega_{a b}^{b} \end{array}\right]=\left[\begin{array}{c} R_{a b}^{T} \dot{p}_{a b} \\ \left(R_{a b}^{T} \dot{R}_{a b}\right)^{\vee} \end{array}\right]
V
abb=gab−1g˙ab=[RabTR˙ab0RabTp˙ab0],Vabb=[vabbωabb]=[RabTp˙ab(RabTR˙ab)∨]
需要指出的是，物体速度并不是物体相对于物体坐标系的速度（此速度总是零）
2.3 空间坐标下的刚体速度与（瞬时）物体坐标系下的刚体速度的关系
V ^ a b s = g ˙ a b g a b − 1 = g a b ( g a b − 1 g ˙ a b ) g a b − 1 = g a b V ^ a b b g a b − 1 \widehat{V}_{a b}^{s}=\dot{g}_{a b} g_{a b}^{-1}=g_{a b}\left(g_{a b}^{-1} \dot{g}_{a b}\right) g_{a b}^{-1}=g_{a b} \widehat{V}_{a b}^{b} g_{a b}^{-1} V abs=g˙abgab−1=gab(gab−1g˙ab)gab−1=gabV abbgab−1
可得，
V
a
b
s
=
[
v
a
b
s
ω
a
b
s
]
=
[
R
a
b
p
^
a
b
R
a
b
0
R
a
b
]
[
v
a
b
b
ω
a
b
b
]
=
A
d
(
g
a
b
)
V
a
b
b
V_{a b}^{s}=\left[\begin{array}{c} v_{a b}^{s} \\ \omega_{a b}^{s} \end{array}\right]=\left[\begin{array}{cc} R_{a b} & \widehat{p}_{a b} R_{a b} \\ 0 & R_{a b} \end{array}\right]\left[\begin{array}{c} v_{a b}^{b} \\ \omega_{a b}^{b} \end{array}\right] = \mathbf{Ad}(g_{ab}) V_{a b}^{b}
Vabs=[vabsωabs]=[Rab0p
abRabRab][vabbωabb]=Ad(gab)Vabb
3. 刚体速度的坐标转换
假如知道了IMU与底盘坐标系之间的外参以及IMU的速度，如何得到底盘坐标系的速度
3.1 空间坐标系下的转换
假设存在A、B、C三个坐标系，它们在空间坐标系下的刚体速度存在下述关系：
V
^
a
c
s
=
g
˙
a
c
g
a
c
−
1
=
(
g
˙
a
b
g
b
c
+
g
a
b
g
˙
b
c
)
(
g
b
c
−
1
g
a
b
−
1
)
=
g
˙
a
b
g
a
b
−
1
+
g
a
b
(
g
˙
b
c
g
b
c
−
1
)
g
a
b
−
1
=
V
^
a
b
s
+
g
a
b
V
^
b
c
b
g
a
b
−
1
\begin{aligned} \widehat{V}_{a c}^{s} &=\dot{g}_{a c} g_{a c}^{-1} \\ &=\left(\dot{g}_{a b} g_{b c}+g_{a b} \dot{g}_{b c}\right)\left(g_{b c}^{-1} g_{a b}^{-1}\right) \\ &=\dot{g}_{a b} g_{a b}^{-1}+g_{a b}\left(\dot{g}_{b c} g_{b c}^{-1}\right) g_{a b}^{-1} \\ &=\widehat{V}_{a b}^{s}+g_{a b} \widehat{V}_{b c}^{b} g_{a b}^{-1} \\ \end{aligned}
V
acs=g˙acgac−1=(g˙abgbc+gabg˙bc)(gbc−1gab−1)=g˙abgab−1+gab(g˙bcgbc−1)gab−1=V
abs+gabV
bcbgab−1
注意，这里使用的是 V ^ b c b \widehat{V}_{b c}^{b} V bcb，而书里面使用的是 V ^ b c s \widehat{V}_{b c}^{s} V bcs，好像是书里写的有问题
转换成向量的形式，
V
a
c
s
=
V
a
b
s
+
A
d
(
g
a
b
)
V
b
c
b
{V}_{a c}^{s} = {V}_{a b}^{s} + \mathbf{Ad}(g_{a b}) {V}_{b c}^{b}
Vacs=Vabs+Ad(gab)Vbcb
如果
g
b
c
g_{bc}
gbc是常量，相当于刚体上两个坐标系B和C之间的外参，此时二者的速度关系为
V
a
c
s
=
V
a
b
s
{V}_{a c}^{s} = {V}_{a b}^{s}
Vacs=Vabs
由于两个坐标系B和C同属于一个刚体，所以两个坐标系B和C在空间坐标系下的刚体速度是相同的
将上式转换到物体坐标系
A
d
(
g
a
c
)
V
a
c
c
=
A
d
(
g
a
b
)
V
a
b
s
V
a
c
c
=
A
d
(
g
a
c
)
−
1
A
d
(
g
a
b
)
V
a
b
s
V
a
c
c
=
A
d
(
g
c
b
)
V
a
b
s
\begin{aligned} \mathbf{Ad}(g_{ac}){V}_{a c}^{c} = \mathbf{Ad}(g_{ab}) {V}_{a b}^{s} \\ {V}_{a c}^{c} = \mathbf{Ad}(g_{ac})^{-1} \mathbf{Ad}(g_{ab}) {V}_{a b}^{s} \\ {V}_{a c}^{c} = \mathbf{Ad}(g_{cb}) {V}_{a b}^{s} \end{aligned}
Ad(gac)Vacc=Ad(gab)VabsVacc=Ad(gac)−1Ad(gab)VabsVacc=Ad(gcb)Vabs
即，刚体上两个坐标系B和C在各自的物体坐标系下的刚体速度关系
3.2 物体坐标系下的转换
同理可得，
V
^
a
c
c
=
g
a
c
−
1
g
˙
a
c
=
(
g
b
c
−
1
g
a
b
−
1
)
(
g
˙
a
b
g
b
c
+
g
a
b
g
˙
b
c
)
=
g
b
c
−
1
g
a
b
−
1
g
˙
a
b
g
b
c
+
g
b
c
−
1
g
˙
b
c
=
g
b
c
−
1
V
^
a
b
b
g
b
c
+
V
^
b
c
c
\begin{aligned} \widehat{V}_{a c}^{c} &=g_{a c}^{-1} \dot{g}_{a c} \\ &=\left(g_{b c}^{-1} g_{a b}^{-1}\right) \left(\dot{g}_{a b} g_{b c}+g_{a b} \dot{g}_{b c}\right) \\ &= g_{b c}^{-1} g_{a b}^{-1} \dot{g}_{a b} g_{b c} + g_{b c}^{-1} \dot{g}_{b c} \\ &= g_{b c}^{-1} \widehat{V}_{a b}^{b} g_{b c} + \widehat{V}_{b c}^{c} \end{aligned}
V
acc=gac−1g˙ac=(gbc−1gab−1)(g˙abgbc+gabg˙bc)=gbc−1gab−1g˙abgbc+gbc−1g˙bc=gbc−1V
abbgbc+V
bcc
转换成向量的形式，
V
a
c
c
=
A
d
(
g
b
c
−
1
)
V
a
b
b
+
V
b
c
c
{V}_{a c}^{c} = \mathbf{Ad}(g_{bc}^{-1}) {V}_{ab}^{b} + {V}_{bc}^{c}
Vacc=Ad(gbc−1)Vabb+Vbcc
如果
g
b
c
g_{bc}
gbc是常量，相当于刚体上两个坐标系B和C之间的外参，此时二者的速度关系为
V
a
c
c
=
A
d
(
g
b
c
−
1
)
V
a
b
b
{V}_{a c}^{c} = \mathbf{Ad}(g_{bc}^{-1}) {V}_{ab}^{b}
Vacc=Ad(gbc−1)Vabb