刚性运动（rigid motion）
物体的连续运动，且在任意时刻
t
t
t，物体上两点距离不变，
∣
∣
p
(
t
)
−
q
(
t
)
∣
∣
=
∣
∣
p
(
0
)
−
q
(
0
)
∣
∣
||p(t) - q(t)||=||p(0)-q(0)||
∣∣p(t)−q(t)∣∣=∣∣p(0)−q(0)∣∣
刚性位移（rigid displacement）
物体在刚体运动前后的变化，包括平移（translation）和旋转（rotation）
用
g
:
R
3
→
R
3
g:R^3 \rightarrow R^3
g:R3→R3表示。
自由向量，
v
=
q
−
p
v = q-p
v=q−p，不和刚体某点绑定。
g
∗
(
v
)
=
g
(
q
)
−
g
(
p
)
g_*(v) = g(q)-g(p)
g∗(v)=g(q)−g(p)
刚体变换（rigid body transformation)
用作表示刚性运动中，物体坐标系的瞬时位置和方位。定义
刚性位移前后，点对的距离不变，
∣
∣
g
(
p
)
−
g
(
q
)
∣
∣
=
∣
∣
p
−
q
∣
∣
||g(p)-g(q)||=||p-q||
∣∣g(p)−g(q)∣∣=∣∣p−q∣∣刚性位移前后，向量角度关系不变，
g
∗
(
v
×
w
)
=
g
∗
(
v
)
×
g
∗
(
w
)
g_*(v\times w) = g_*(v) \times g_*(w)
g∗(v×w)=g∗(v)×g∗(w) 主要剔除镜像映射的情况，虽然距离不变，但是物理上不可达
∣
∣
v
1
+
v
2
∣
∣
=
∣
∣
g
∗
(
v
1
)
+
g
∗
(
v
2
)
∣
∣
||v_1 + v_2|| = ||g_*(v_1) + g_*(v_2)||
∣∣v1+v2∣∣=∣∣g∗(v1)+g∗(v2)∣∣
∣
∣
v
1
−
v
2
∣
∣
=
∣
∣
g
∗
(
v
1
)
−
g
∗
(
v
2
)
∣
∣
||v_1 - v_2|| = ||g_*(v_1) - g_*(v_2)||
∣∣v1−v2∣∣=∣∣g∗(v1)−g∗(v2)∣∣
v
1
T
v
2
=
g
∗
(
v
1
)
T
g
∗
(
v
2
)
v_1^Tv_2 = g_*(v_1)^Tg_*(v_2)
v1Tv2=g∗(v1)Tg∗(v2)
旋转
刚体运动下，物体点之间还是可以存在相对运动，就是旋转。 将坐标系绑定在物体某个点上，得到物体坐标系（body coordinate frame），并通过计算物体坐标系和某固定坐标系的运动关系来表示物体的布局（configuration）。
旋转矩阵
R
a
b
=
[
x
a
b
y
a
b
z
a
b
]
R_{ab} = \left[ \begin{array}{c} x_{ab} & y_{ab} & z_{ab} \end{array} \right]
Rab=[xabyabzab]，表示坐标系B的主轴在坐标系A中的坐标（B转A）。
R
a
b
∈
S
O
(
3
)
R_{ab}\in SO(3)
Rab∈SO(3)，特殊正交矩阵（special orthogonal），
R
T
=
R
−
1
R^T=R^{-1}
RT=R−1，
d
e
t
(
R
)
=
1
det(R)=1
det(R)=1。
x
^
=
[
0
−
x
3
x
2
x
3
0
−
x
1
−
x
2
x
1
0
]
\hat{x} = \left[ \begin{array}{c} 0 & -x_3 & x_2 \\ x_3 & 0 & -x_1 \\ -x_2 & x_1 & 0 \end{array} \right]
x^=⎣⎡0x3−x2−x30x1x2−x10⎦⎤，斜对称（skew-symmetric），
x
^
T
=
−
x
^
∈
s
o
(
3
)
\hat{x}^T = -\hat{x} \in so(3)
x^T=−x^∈so(3)
x
×
y
=
x
^
y
x \times y = \hat{x} y
x×y=x^y
(
v
+
w
)
^
=
v
^
+
w
^
\hat{(v+w)} = \hat{v} + \hat{w}
(v+w)^=v^+w^
R
w
^
R
T
=
R
w
^
R \hat{w} R^T = \hat{Rw}
Rw^RT=Rw^
旋转向量（指数坐标）
旋转轴
ω
∈
s
o
(
3
)
\omega \in so(3)
ω∈so(3)，转角
θ
\theta
θ 通过线速度和角速度的关系
q
˙
(
t
)
=
θ
˙
r
=
ω
×
q
(
t
)
\dot{q}(t) = \dot{\theta}r = \omega \times q(t)
q˙(t)=θ˙r=ω×q(t)，其中使用常数单位时间角速度
θ
˙
=
1
\dot{\theta}=1
θ˙=1 差分方程积分得到
q
(
t
)
=
e
ω
^
t
q
(
0
)
q(t) = e^{\hat{\omega}t}q(0)
q(t)=eω^tq(0)
旋转向量 to 旋转矩阵
泰勒展开
e
ω
^
t
=
I
+
ω
^
t
+
(
ω
^
t
)
2
2
!
+
.
.
.
e^{\hat{\omega}t}=I + \hat{\omega}t + \frac{(\hat{\omega}t)^2}{2!} + ...
eω^t=I+ω^t+2!(ω^t)2+...
a
^
2
=
a
a
T
−
∣
∣
a
∣
∣
2
I
\hat{a}^2 = aa^T-||a||^2I
a^2=aaT−∣∣a∣∣2I
a
^
3
=
−
∣
∣
a
∣
∣
2
a
^
\hat{a}^3 = -||a||^2\hat{a}
a^3=−∣∣a∣∣2a^
代入
t
=
θ
t=\theta
t=θ，
θ
\theta
θ个单位时间，有
e
ω
^
θ
=
I
+
ω
^
θ
+
(
ω
^
θ
)
2
2
!
+
.
.
.
e^{\hat{\omega}\theta}=I + \hat{\omega}\theta + \frac{(\hat{\omega}\theta)^2}{2!} + ...
eω^θ=I+ω^θ+2!(ω^θ)2+...
=
I
+
(
θ
−
θ
3
3
!
+
θ
5
5
!
−
.
.
.
)
ω
^
+
(
θ
2
2
!
−
θ
4
4
!
+
θ
6
6
!
−
.
.
.
)
ω
^
2
=I + (\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}-...)\hat{\omega} + (\frac{\theta^2}{2!}-\frac{\theta^4}{4!}+\frac{\theta^6}{6!}-...)\hat{\omega}^2
=I+(θ−3!θ3+5!θ5−...)ω^+(2!θ2−4!θ4+6!θ6−...)ω^2
=
I
+
ω
^
sin
θ
+
ω
^
2
(
1
−
cos
θ
)
=I + \hat{\omega}\sin\theta + \hat{\omega}^2(1-\cos\theta)
=I+ω^sinθ+ω^2(1−cosθ)
罗德里格公式（Rodrigues‘ formula）
旋转矩阵 to 旋转向量
θ
=
cos
−
1
(
t
r
a
c
e
(
R
)
−
1
2
)
,
t
r
a
c
e
(
R
)
=
r
11
+
r
22
+
r
33
\theta = \cos^{-1}(\frac{trace(R)-1}{2}), trace(R) = r_{11}+r_{22}+r_{33}
θ=cos−1(2trace(R)−1),trace(R)=r11+r22+r33
ω
=
1
2
sin
θ
[
r
32
−
r
23
r
13
−
r
31
r
21
−
r
12
]
=
1
2
sin
θ
[
R
−
R
T
]
ˇ
\omega = \frac{1}{2\sin\theta}\left[ \begin{array}{c} r_{32} -r_{23} \\ r_{13}-r_{31} \\ r_{21}-r_{12} \end{array}\right] = \frac{1}{2\sin\theta} [R - R^T]\check{\ }
ω=2sinθ1⎣⎡r32−r23r13−r31r21−r12⎦⎤=2sinθ1[R−RT] ˇ
θ
≠
0
\theta \neq 0
θ=0，双解
2
π
−
θ
,
θ
∈
[
0
,
2
π
)
2\pi - \theta, \theta \in [0, 2\pi)
2π−θ,θ∈[0,2π)
θ
=
0
\theta = 0
θ=0，奇异（singularity），
ω
\omega
ω任取
四元数
Q
=
(
x
,
y
,
z
,
w
)
Q = (x, y, z, w)
Q=(x,y,z,w)
共轭（conjugate）
Q
∗
=
(
−
x
,
−
y
,
−
z
,
w
)
=
(
q
,
w
q
)
Q^*=(-x, -y, -z, w)=(q, w_q)
Q∗=(−x,−y,−z,w)=(q,wq)大小
∣
∣
Q
∣
∣
2
=
x
2
+
y
2
+
z
2
+
w
2
||Q||^2 = x^2 + y^2 + z^2 + w^2
∣∣Q∣∣2=x2+y2+z2+w2逆
Q
−
1
=
Q
∗
∣
∣
Q
∣
∣
2
Q^{-1} = \frac{Q^*}{||Q||^2}
Q−1=∣∣Q∣∣2Q∗单位向量（identity）
(
0
,
0
,
0
,
1
)
(0, 0, 0, 1)
(0,0,0,1)乘法
Q
⋅
P
=
(
w
q
w
p
−
q
T
p
,
w
q
p
+
w
p
q
+
q
×
p
)
Q\cdot P = (w_qw_p-q^Tp, w_qp+w_pq+q\times p)
Q⋅P=(wqwp−qTp,wqp+wpq+q×p)
旋转向量 to 四元数
Q
=
(
ω
sin
(
θ
/
2
)
,
cos
(
θ
/
2
)
)
Q = (\omega\sin(\theta/2),\cos(\theta/2))
Q=(ωsin(θ/2),cos(θ/2))
四元数 to 旋转向量
θ
=
2
cos
−
1
w
q
\theta = 2 \cos^{-1} w_q
θ=2cos−1wq
ω
=
q
/
sin
(
θ
/
2
)
,
0
i
f
θ
=
0
\omega = q/\sin(\theta/2), 0 if\theta=0
ω=q/sin(θ/2),0ifθ=0
旋转 + 平移（仿射变换，affine transformation）
(
p
,
R
)
∈
S
E
(
3
)
(p, R) \in SE(3)
(p,R)∈SE(3)，特殊欧几里得群（special Euclidean group）
g
(
q
)
=
p
+
R
q
g(q) = p + Rq
g(q)=p+Rq
g
∗
(
v
)
=
g
(
s
)
−
g
(
r
)
=
R
(
s
−
r
)
=
R
v
g_*(v) = g(s) - g(r) = R(s-r) = Rv
g∗(v)=g(s)−g(r)=R(s−r)=Rv
齐次表示（Homogeneous representation）
point
q
ˉ
=
[
q
1
q
2
q
3
1
]
\bar{q} = \left[\begin{array}{c} q_1 \\ q_2 \\ q_3 \\ 1 \end{array}\right]
qˉ=⎣⎢⎢⎡q1q2q31⎦⎥⎥⎤，vector
v
ˉ
=
[
v
1
v
2
v
3
0
]
\bar{v} = \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \\ 0 \end{array}\right]
vˉ=⎣⎢⎢⎡v1v2v30⎦⎥⎥⎤，
g
ˉ
=
[
R
p
0
1
]
\bar{g} = \left[\begin{array}{c} R & p \\ 0 & 1 \end{array}\right]
gˉ=[R0p1]
g
ˉ
−
1
=
[
R
T
−
R
T
p
0
1
]
\bar{g}^{-1} = \left[\begin{array}{c} R^T & -R^Tp \\ 0 & 1 \end{array}\right]
gˉ−1=[RT0−RTp1]
指数坐标（twist）
旋转轴
ω
\omega
ω，转角
θ
\theta
θ，轴过点
q
q
q （twist）
ξ
=
(
v
,
ω
)
∈
s
e
(
3
)
\xi=(v, \omega) \in se(3)
ξ=(v,ω)∈se(3)
纯旋转
通过线速度和角速度的关系
p
˙
(
t
)
=
θ
˙
r
=
ω
×
(
p
(
t
)
−
q
)
\dot{p}(t) = \dot{\theta}r = \omega \times (p(t)-q)
p˙(t)=θ˙r=ω×(p(t)−q) 其中使用常数单位时间角速度
θ
˙
=
1
\dot{\theta}=1
θ˙=1 齐次形式
[
p
˙
0
]
=
[
ω
^
−
ω
^
q
0
0
]
[
p
1
]
\left[\begin{array}{c} \dot{p} \\ 0 \end{array}\right] = \left[\begin{array}{c} \hat{\omega} & -\hat{\omega}q \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} p \\ 1 \end{array}\right]
[p˙0]=[ω^0−ω^q0][p1] 令
ξ
^
=
[
ω
^
−
ω
^
q
0
0
]
\hat{\xi} = \left[\begin{array}{c} \hat{\omega} & -\hat{\omega}q \\ 0 & 0 \end{array}\right]
ξ^=[ω^0−ω^q0]，有
p
˙
ˉ
=
ξ
^
p
ˉ
\bar{\dot{p}}=\hat{\xi}\bar{p}
p˙ˉ=ξ^pˉ 差分方程积分得到
q
ˉ
(
t
)
=
e
ξ
^
t
q
ˉ
(
0
)
\bar{q}(t) = e^{\hat{\xi}t}\bar{q}(0)
qˉ(t)=eξ^tqˉ(0)
v
=
−
ω
^
q
v =-\hat{\omega}q
v=−ω^q
纯位移
ω
=
0
\omega=0
ω=0时，
v
v
v作为纯平移运动的速度，有
ξ
^
=
[
0
v
0
0
]
\hat{\xi} = \left[\begin{array}{c} 0 & v \\ 0 & 0 \end{array}\right]
ξ^=[00v0]
twist to 变换矩阵
代入
t
=
θ
t=\theta
t=θ，
θ
\theta
θ个单位时间，有
e
ξ
^
θ
=
I
+
ξ
^
θ
+
(
ξ
^
θ
)
2
2
!
+
.
.
.
e^{\hat{\xi}\theta} = I + \hat{\xi}\theta + \frac{(\hat{\xi}\theta)^2}{2!} + ...
eξ^θ=I+ξ^θ+2!(ξ^θ)2+...
ω
=
0
\omega=0
ω=0
e
ξ
^
θ
=
I
+
ξ
^
θ
e^{\hat{\xi}\theta} = I + \hat{\xi}\theta
eξ^θ=I+ξ^θ
else 构造辅助twist
ξ
′
^
=
g
′
−
1
ξ
^
g
′
=
[
ω
^
ω
ω
T
v
0
0
]
\hat{\xi'} = g'^{-1}\hat{\xi}g' = \left[\begin{array}{c} \hat{\omega} & \omega \omega^T v \\ 0 & 0 \end{array}\right]
ξ′^=g′−1ξ^g′=[ω^0ωωTv0]，其中
g
′
=
[
I
ω
×
v
0
1
]
g' = \left[\begin{array}{c}I & \omega \times v \\ 0 & 1 \end{array}\right]
g′=[I0ω×v1] 可证有
e
ξ
^
θ
=
e
g
′
(
ξ
′
^
θ
)
g
′
−
1
=
g
′
e
ξ
′
^
θ
g
′
−
1
e^{\hat{\xi}\theta} = e^{ g' ( \hat{\xi'} \theta ) g'^{-1}} = g' e^{ \hat{\xi'} \theta} g'^{-1}
eξ^θ=eg′(ξ′^θ)g′−1=g′eξ′^θg′−1 而计算
e
ξ
′
^
θ
e^{ \hat{\xi'} \theta}
eξ′^θ比较方便，因为有
(
ξ
′
^
)
2
=
[
ω
^
2
0
0
0
]
(\hat{\xi'})^2 = \left[\begin{array}{c} \hat{\omega}^2 & 0 \\ 0 & 0 \end{array}\right]
(ξ′^)2=[ω^2000]，
(
ξ
′
^
)
3
=
[
ω
^
3
0
0
0
]
(\hat{\xi'})^3 = \left[\begin{array}{c} \hat{\omega}^3 & 0 \\ 0 & 0 \end{array}\right]
(ξ′^)3=[ω^3000]
泰勒展开并计算，得到
e
ξ
′
^
θ
=
[
e
ω
^
θ
ω
ω
T
v
θ
0
1
]
e^{ \hat{\xi'} \theta} = \left[\begin{array}{c} e^{\hat{\omega} \theta} & \omega \omega^T v \theta \\ 0 & 1 \end{array}\right]
eξ′^θ=[eω^θ0ωωTvθ1]
e
ξ
^
θ
=
[
e
ω
^
θ
(
I
−
e
ω
^
θ
)
(
ω
×
v
)
+
ω
ω
T
v
θ
0
1
]
e^{ \hat{\xi} \theta} = \left[\begin{array}{c} e^{\hat{\omega} \theta} & (I-e^{\hat{\omega} \theta}) (\omega \times v) + \omega \omega^T v \theta \\ 0 & 1 \end{array}\right]
eξ^θ=[eω^θ0(I−eω^θ)(ω×v)+ωωTvθ1]
变换矩阵 to twist
R
=
I
R = I
R=I，平移
ξ
=
(
p
/
∣
∣
p
∣
∣
,
0
)
,
θ
=
∣
∣
p
∣
∣
\xi = (p/||p||, 0), \theta = ||p||
ξ=(p/∣∣p∣∣,0),θ=∣∣p∣∣else，旋转
ω
\omega
ω 和
θ
\theta
θ 通过 旋转矩阵 to 旋转向量 获得
v
=
A
−
1
p
v = A^{-1}p
v=A−1p，
A
=
(
I
−
e
ω
^
θ
)
ω
^
+
ω
ω
T
θ
A = (I-e^{\hat{\omega} \theta}) \hat{\omega} + \omega \omega^T \theta
A=(I−eω^θ)ω^+ωωTθ
Screws
旋转加平移（同一个轴）
表示
pitch
h
=
d
/
θ
=
t
r
a
n
s
l
a
t
i
o
n
/
r
o
t
a
t
i
o
n
h=d/\theta=translation/rotation
h=d/θ=translation/rotationaxis
l
=
{
q
+
λ
ω
}
,
q
=
0
i
f
θ
=
0
l = \{q+\lambda\omega\}, q=0\ if\ \theta =0
l={q+λω},q=0 if θ=0magnitude
M
=
θ
i
f
θ
≠
0
e
l
s
e
d
M=\theta\ if\ \theta\ne0\ else\ d
M=θ if θ=0 else d
g
p
=
q
+
e
ω
^
θ
(
p
−
q
)
+
h
θ
ω
gp = q + e^{ \hat{\omega} \theta}(p-q) + h\theta \omega
gp=q+eω^θ(p−q)+hθω
g
=
e
ξ
^
θ
=
[
e
ω
^
θ
(
I
−
e
ω
^
θ
)
q
+
h
θ
ω
0
1
]
g = e^{ \hat{\xi} \theta} = \left[\begin{array}{c} e^{\hat{\omega} \theta} & (I-e^{\hat{\omega} \theta}) q + h \theta \omega \\ 0 & 1 \end{array}\right]
g=eξ^θ=[eω^θ0(I−eω^θ)q+hθω1]
v
=
−
ω
×
q
+
h
w
v = -\omega\times q + hw
v=−ω×q+hw
twist to screw
θ
≠
0
\theta\ne0
θ=0
h
=
ω
T
v
∣
∣
ω
∣
∣
2
h=\frac{\omega^T v}{||\omega||^2}
h=∣∣ω∣∣2ωTv
l
=
{
ω
×
v
∣
∣
ω
∣
∣
2
+
λ
ω
}
l = \{\frac{\omega\times v}{||\omega||^2}+\lambda\omega\}
l={∣∣ω∣∣2ω×v+λω}
M
=
θ
M = \theta
M=θ
θ
=
0
\theta=0
θ=0
h
=
∞
h=\infin
h=∞
l
=
{
0
+
λ
v
}
l = \{0+\lambda v\}
l={0+λv}
M
=
d
M = d
M=d
screw to twist
h
=
∞
h=\infin
h=∞
ω
=
0
\omega = 0
ω=0
v
v
v from
l
l
l
θ
=
M
\theta = M
θ=M
else
ω
\omega
ω from
l
l
l
v
=
−
ω
×
q
+
h
w
v = -\omega\times q + hw
v=−ω×q+hw
θ
=
M
\theta = M
θ=M
速度
旋转（角速度）
空间坐标系A
q
a
(
t
)
=
R
a
b
(
t
)
q
b
q_a(t) = R_{ab}(t) q_b
qa(t)=Rab(t)qb
v
q
a
(
t
)
=
R
˙
a
b
(
t
)
q
b
=
R
˙
a
b
(
t
)
R
a
b
T
(
t
)
R
a
b
(
t
)
q
b
=
ω
^
a
b
a
(
t
)
q
a
(
t
)
v_{q_a}(t) = \dot{R}_{ab}(t) q_b = \dot{R}_{ab}(t) R_{ab}^T(t) R_{ab}(t) q_b = \hat{\omega}^a_{ab}(t) q_a(t)
vqa(t)=R˙ab(t)qb=R˙ab(t)RabT(t)Rab(t)qb=ω^aba(t)qa(t)
物体坐标系B
v
q
b
(
t
)
=
R
a
b
T
(
t
)
v
q
a
(
t
)
=
R
a
b
T
(
t
)
R
˙
a
b
(
t
)
q
b
=
ω
^
a
b
b
(
t
)
q
b
v_{q_b}(t) = R_{ab}^T(t) v_{q_a}(t) = R_{ab}^T(t) \dot{R}_{ab}(t) q_b = \hat{\omega}^b_{ab}(t) q_b
vqb(t)=RabT(t)vqa(t)=RabT(t)R˙ab(t)qb=ω^abb(t)qb
ω
a
b
a
(
t
)
=
R
a
b
ω
a
b
b
(
t
)
\omega^a_{ab}(t) = R_{ab} \omega^b_{ab}(t)
ωaba(t)=Rabωabb(t) 这里是指角速度，不是旋转向量
ω
a
b
a
\omega^a_{ab}
ωaba表示，（上标）a坐标系下，（下标）b到a的变换
物体坐标系下的表示不是指点q在相对于物体坐标系的速度（恒为0），而是指点q相对以其他坐标系的速度向量转换到物体坐标系下。
旋转 + 平移（刚体速度）
V
^
a
b
a
=
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
]
\hat{V}^a_{ab} = \dot{g}_{ab} g^{-1}_{ab} = \left[\begin{array}{c} \dot{R}_{ab} & \dot{p}_{ab} \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} R^T_{ab} & -R^T_{ab}p_{ab} \\ 0 & 1 \end{array}\right] = \left[\begin{array}{c} \dot{R}_{ab} R^T_{ab} & -\dot{R}_{ab} R^T_{ab}p_{ab} + \dot{p}_{ab} \\ 0 & 0 \end{array}\right]
V^aba=g˙abgab−1=[R˙ab0p˙ab0][RabT0−RabTpab1]=[R˙abRabT0−R˙abRabTpab+p˙ab0]
v
q
a
=
ω
^
a
b
a
q
a
−
ω
^
a
b
a
p
a
b
+
p
˙
a
b
=
ω
^
a
b
a
(
q
a
−
p
a
b
)
+
p
˙
a
b
v_{q_a} = \hat{\omega}^a_{ab} q_a - \hat{\omega}^a_{ab} p_{ab} + \dot{p}_{ab} = \hat{\omega}^a_{ab} (q_a - p_{ab}) + \dot{p}_{ab}
vqa=ω^abaqa−ω^abapab+p˙ab=ω^aba(qa−pab)+p˙ab （绕原点旋转的速度+原点速度）
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
]
\hat{V}^b_{ab} = g^{-1}_{ab} \dot{g}_{ab} = \left[\begin{array}{c} R^T_{ab} \dot{R}_{ab} & R^T_{ab} \dot{p}_{ab} \\ 0 & 0 \end{array}\right]
V^abb=gab−1g˙ab=[RabTR˙ab0RabTp˙ab0]
v
q
b
=
ω
^
a
b
b
q
b
+
R
a
b
T
p
˙
a
b
=
R
a
b
T
ω
a
b
a
^
R
a
b
T
(
q
a
−
p
a
b
)
+
R
a
b
T
p
˙
a
b
v_{q_b} = \hat{\omega}^b_{ab} q_{b} + R^T_{ab} \dot{p}_{ab} = \widehat{R^T_{ab}\omega^a_{ab}} R^T_{ab} (q_{a} - p_{ab}) + R^T_{ab} \dot{p}_{ab}
vqb=ω^abbqb+RabTp˙ab=RabTωaba
RabT(qa−pab)+RabTp˙ab
=
R
a
b
T
ω
^
a
b
a
(
q
a
−
p
a
b
)
+
R
a
b
T
p
˙
a
b
=
R
a
b
T
v
q
a
= R^T_{ab}\hat{\omega}^a_{ab} (q_{a} - p_{ab}) + R^T_{ab} \dot{p}_{ab} = R^T_{ab} v_{q_a}
=RabTω^aba(qa−pab)+RabTp˙ab=RabTvqa （坐标系B下：绕原点旋转的速度+原点速度） （相当于将所有东西搬到坐标系B计算）
V
^
a
b
a
=
g
a
b
V
^
a
b
b
g
a
b
−
1
\hat{V}^a_{ab} = g_{ab} \hat{V}^b_{ab} g^{-1}_{ab}
V^aba=gabV^abbgab−1
V
a
b
a
=
A
d
g
a
b
V
a
b
b
V^a_{ab} = Ad_{g_{ab}} V^b_{ab}
Vaba=AdgabVabb （伴随变换，adjoint transformation）
A
d
g
=
[
R
p
^
R
0
R
]
Ad_g = \left[\begin{array}{c} R & \hat{p} R \\ 0 & R \end{array}\right]
Adg=[R0p^RR]
A
d
g
−
1
=
[
R
T
−
R
T
p
^
0
R
T
]
=
A
d
g
−
1
Ad^{-1}_g = \left[\begin{array}{c} R^T & -R^T \hat{p} \\ 0 & R^T \end{array}\right] = Ad_{g^{-1}}
Adg−1=[RT0−RTp^RT]=Adg−1
因为
g
a
b
(
θ
)
=
e
ξ
^
θ
g
a
b
(
0
)
g_{ab}(\theta) = e^{\hat{\xi}\theta} g_{ab}(0)
gab(θ)=eξ^θgab(0)
g
˙
a
b
(
θ
)
=
ξ
^
θ
˙
e
ξ
^
θ
g
a
b
(
0
)
\dot{g}_{ab}(\theta) = \hat{\xi}\dot{\theta}e^{\hat{\xi}\theta} g_{ab}(0)
g˙ab(θ)=ξ^θ˙eξ^θgab(0) 所以
V
^
a
b
a
=
g
˙
g
−
1
=
(
ξ
^
θ
˙
e
ξ
^
θ
g
a
b
(
0
)
)
(
g
a
b
−
1
(
0
)
e
−
ξ
^
θ
)
=
ξ
^
θ
˙
\hat{V}^a_{ab} = \dot{g} g^{-1} = (\hat{\xi}\dot{\theta}e^{\hat{\xi}\theta} g_{ab}(0)) (g^{-1}_{ab}(0)e^{-\hat{\xi}\theta}) = \hat{\xi}\dot{\theta}
V^aba=g˙g−1=(ξ^θ˙eξ^θgab(0))(gab−1(0)e−ξ^θ)=ξ^θ˙
V
^
a
b
b
=
g
−
1
g
˙
=
(
g
a
b
−
1
(
0
)
e
−
ξ
^
θ
)
(
ξ
^
θ
˙
e
ξ
^
θ
g
a
b
(
0
)
)
=
A
d
g
−
1
ξ
^
θ
˙
\hat{V}^b_{ab} = g^{-1} \dot{g} = (g^{-1}_{ab}(0)e^{-\hat{\xi}\theta}) (\hat{\xi}\dot{\theta}e^{\hat{\xi}\theta} g_{ab}(0)) = \widehat{Ad_{g_{-1}}\xi}\dot{\theta}
V^abb=g−1g˙=(gab−1(0)e−ξ^θ)(ξ^θ˙eξ^θgab(0))=Adg−1ξ
θ˙
坐标系变换
V
a
c
a
=
V
a
b
a
+
A
d
g
a
b
V
b
c
b
V^a_{ac} = V^a_{ab} + Ad_{g_{ab}}V^b_{bc}
Vaca=Vaba+AdgabVbcb
V
a
c
c
=
A
d
g
c
b
V
a
b
b
+
V
b
c
c
V^c_{ac} = Ad_{g_{cb}}V^b_{ab} + V^c_{bc}
Vacc=AdgcbVabb+Vbcc
V
a
b
b
=
−
V
b
a
b
V^b_{ab} = -V^b_{ba}
Vabb=−Vbab（相对速度）
当两个坐标系相对固定，则他们的速度是一致的 例如B和C固定，则
V
a
c
a
=
V
a
b
a
V^a_{ac} = V^a_{ab}
Vaca=Vaba
Wrench
F
=
[
f
τ
]
F = \left[ \begin{array}{c} f \\ \tau \end{array} \right]
F=[fτ]，force + torch
δ
W
=
V
⋅
F
\delta W = V \cdot F
δW=V⋅F 瞬时做工
坐标转换
基于等价做工性质
B和C是相对固定的坐标系
V
a
c
c
⋅
F
c
=
V
a
b
b
⋅
F
b
V^c_{ac} \cdot F_c = V^b_{ab} \cdot F_b
Vacc⋅Fc=Vabb⋅Fb
=
V
a
c
b
⋅
F
b
= V^b_{ac} \cdot F_b
=Vacb⋅Fb
=
(
A
d
g
b
c
V
a
c
c
)
⋅
F
b
= (Ad_{g_{bc}} V^c_{ac}) \cdot F_b
=(AdgbcVacc)⋅Fb
=
V
a
c
c
⋅
A
d
g
b
c
T
F
b
= V^c_{ac} \cdot Ad_{g_{bc}}^T F_b
=Vacc⋅AdgbcTFb
则有
F
c
=
A
d
g
b
c
T
F
b
F_c = Ad_{g_{bc}}^T F_b
Fc=AdgbcTFb
力作用在转换后的坐标系原点
wrench to screw
f
≠
0
f \ne 0
f=0
h
=
∞
h = \infin
h=∞
M
=
∣
∣
τ
∣
∣
M = ||\tau||
M=∣∣τ∣∣
l
=
{
0
+
λ
τ
M
}
l = \{0+\lambda \frac{\tau}{M} \}
l={0+λMτ}
f
=
0
f=0
f=0
h
=
f
T
τ
∣
∣
f
∣
∣
2
h = \frac{f^T \tau}{||f||^2}
h=∣∣f∣∣2fTτ
M
=
∣
∣
f
∣
∣
M = ||f||
M=∣∣f∣∣
l
=
{
f
×
τ
∣
∣
f
∣
∣
2
+
λ
f
M
}
l = \{\frac{f \times \tau}{||f||^2}+\lambda \frac{f}{M} \}
l={∣∣f∣∣2f×τ+λMf}
screw to wrench
h
=
∞
h=\infin
h=∞
f
=
0
f = 0
f=0
τ
\tau
τ from
l
l
l 乘
M
M
M
else
f
f
f from
l
l
l
τ
=
−
f
×
q
+
h
f
\tau = -f\times q + hf
τ=−f×q+hf 乘
M
M
M
互逆螺旋（reciprocal screws）
V
⋅
F
=
0
V \cdot F = 0
V⋅F=0
对应的螺旋表示
S
V
S_V
SV和
S
F
S_F
SF就是互逆螺旋
互逆乘积
定义两个螺旋的互逆乘积为（就是从
F
⋅
V
F \cdot V
F⋅V展开得来）
S
1
⊙
S
2
=
M
1
M
2
(
(
h
1
+
h
2
)
cos
α
−
d
sin
α
)
S_1 \odot S_2 = M_1 M_2((h_1+h_2)\cos\alpha - d \sin\alpha)
S1⊙S2=M1M2((h1+h2)cosα−dsinα)
其中，
α
=
a
t
a
n
2
(
ω
1
×
ω
2
⋅
n
,
ω
1
⋅
ω
2
)
\alpha = atan2(\omega_1 \times \omega_2 \cdot n, \omega_1 \cdot \omega_2)
α=atan2(ω1×ω2⋅n,ω1⋅ω2)，
l
1
l_1
l1
l
2
l_2
l2之间的距离线段为
d
n
dn
dn，
d
d
d为长度，
n
n
n为方向
则有，互逆螺旋的互逆乘积为0
螺旋系统（system of screws）的纬度
r
r
r 和其对应的互逆螺旋系统（reciprocal screw system）的纬度
n
n
n 有关系
r
+
n
=
6
r+n=6
r+n=6