[Modern Robotics] 3 -刚体运动与旋量

刚体运动与旋量

实际上，齐次变换矩阵 $T$ 与旋转矩阵 $R$ 类似，旋量轴 $S$ 与旋转轴 $\hat{w}$ 类似， 旋量 $V$ 可以用 $S\dot{\theta }$ 来表示，与角速度 $w=\hat{w}\dot{\theta }$ 类似。刚体运动指数坐标 $S\theta \in {\mathbb{R}}^6$ 与旋转指数坐标 $\hat{w}\theta$ 类似。

齐次变换矩阵

我们现在考虑将刚体的方向与位置结合在一起的表达。一个自然的想法是用一个旋转矩阵 $R\in SO(3)$ 来表示刚体坐标系 { b } \{b\} {b} 在固定坐标系 { s } \{s\} {s} 中的方向，用一个向量 $p\in {\mathbb{R}}^3$ 来表示刚体坐标系 { b } \{b\} {b} 的原点在固定坐标系 { s } \{s\} {s} 中的位置。也可以将他们放到一个矩阵中。

定义
特殊欧氏群：即齐此变换矩阵，是 $4\times 4$ 的实矩阵的集合，并有如下形式：
$$T=\left[\begin{array}{c}Rp\\ 01\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_1\\ r_{21}& r_{22}& r_{23}& p_2\\ r_{31}& r_{32}& r_{33}& p_3\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(3.62)}$$
许多的机器人运动都在平面内，我们有如下定义：

定义
特殊欧氏群 $SE(2)$ 是 $3\times 3$ 实矩阵：
$$T=\left[\begin{array}{c}Rp\\ 01\end{array}\right]\text{\hspace{1em}(3.63)}$$
其中， $R\in SO(2)，p\in {\mathbb{R}}^2$。
矩阵 $T\in SE(2)，通常有如下形式：$
$$T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& p_1\\ r_{21}& r_{22}& p_2\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}cos\theta & -sin\theta & p_1\\ sin\theta & cos\theta & p_2\\ 0& 0& 1\end{array}\right]$$
其中，$\theta \in [0,2\pi )$。

齐次变换矩阵的性质

定义：齐次变换矩阵 $T\in SE(3)$ 的逆也是一个变换矩阵，具有如下形式：
$$T^{-1}={\left[\begin{array}{c}Rp\\ 01\end{array}\right]}^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]\text{\hspace{1em}(3.64)}$$
定义：两个齐次变换矩阵的积也是一个变换矩阵。

定义：变换矩阵相乘满足结合律，所以 $(T_1{T}_2)T_3=T_1(T_2{T}_3)$，但是通常不满足交换律。

定义：给定 $T=(R,p)\in SE(3),x,y\in {\mathbb{R}}^3$，下列等式成立：

1. $||Tx-Ty||=||x-y||$，其中$||.||$是标准欧氏范数，即$||x||=\sqrt{x^{T}x}$。
2. $<Tx-Tz,Ty-Tz>=<x-z,y-z>$，对于 $z\in {\mathbb{R}}^3$，其中$<.,.>$是${\mathbb{R}}^3$标准欧氏内积，即$<x,y>=x^{T}y$。

在上述定义中，$T$ 是被当做一个对一个点的变换。$T$ 使一个点从 $x$ 变到 $Tx$。性质1断言 $T$ 保持距离，性质2断言 $T$ 保持角度。特别地，如果 $x,y,z\in {\mathbb{R}}^3$ 代表一个三角形的三个顶点，那么由 { T x , T y , T z } \{Tx, Ty, Tz\} {Tx,Ty,Tz} 组成的三角形与之有相同的长度与角度。

变换矩阵的使用

与旋转矩阵类似，变换矩阵 $T$ 也有三个主要通途：

1. 表征一个刚体的位置与方向。
2. 改变向量或坐标系的参考坐标系。
3. 移动一个向量或坐标系。

如图3.14所示，有三个参考坐标系 { a } , { b } , { c } \{a\}, \{b\},\{c\} {a},{b},{c}，和一个点 $v$。

表征方向及位置
固定坐标系 { s } \{s\} {s} 与 { a } \{a\} {a}重合，变换矩阵分别为$T_{sa}=(R_{sa},p_{sa}),T_{sb}=(R_{sb},p_{sb}),T_{sc}=(R_{sc},p_{sc})$。用如下矩阵表示：
$$R_{sa}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],R_{sb}=\left[\begin{array}{ccc}0& 0& 1\\ 0& -1& 0\\ 1& 0& 0\end{array}\right],R_{sc}=\left[\begin{array}{ccc}-1& 0& 0\\ 0& 0& 1\\ 0& 1& 0\end{array}\right].$$
原点位置相对于 { s } \{s\} {s} 写作：
$$p_{sa}=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right],p_{sb}=\left[\begin{array}{c}0\\ -2\\ 0\end{array}\right],p_{sa}=\left[\begin{array}{c}-1\\ 1\\ 0\end{array}\right].$$
由于 { a } \{a\} {a} 与 { s } \{s\} {s} 一致，所以由$(R_{sa},p_{sa})$ 构成的$T_{sa}$ 是单位阵。
任何一个坐标系可以相对于任意一个坐标系来表示，并不只是 { s } \{s\} {s} ；比如$T_{bc}=(R_{bc},p_{bc})$ 表示 { b } \{b\} {b}相对于 { c } \{c\} {c}的位置及方向：
$$R_{bc}=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& -1\\ -1& 0& 0\end{array}\right],p_{bc}=\left[\begin{array}{c}0\\ -3\\ -1\end{array}\right].$$
同样，对于任意坐标系 { d } \{d\} {d}和 { e } \{e\} {e}，有：
$$T_{de}=T_{ed}^{-1}$$
改变向量或坐标系的参考坐标系
同样可以用下标去除原则来进行切换，如对任意的坐标系 { a } \{a\} {a}， { b } \{b\} {b}， { c } \{c\} {c}，或任意向量 $v$ 在 { b } \{b\} {b}中表示为 $v_b$，有：
$$\begin{gathered} T_{ab}{T}_{bc}=T_{ab/}{T}_{b/c}=T_{ac} \\ {T}_{ab}{v}_b=T_{ab/}{v}_{b/}=v_a \end{gathered}$$
其中，$v_a$ 是向量 $v$ 在 { a } \{a\} {a}中的表示。

移动（旋转和平移）一个向量或坐标系
变换矩阵 $T$ 可以认为是 $(R,p)=(Rot(\hat{w},\theta ),p)$ 组成的对，能够作用于一个坐标系使其绕轴 $\hat{w}$ 旋转 $\theta$ 角度，再进行 $p$ 平移，我们把一个只包含旋转的 $4\times 4$ 矩阵记为：
$$Rot(\hat{w},\theta )=\left[\begin{array}{cc}R& 0\\ 0& 1\end{array}\right],$$
同样定义一个只包含平移的矩阵为：
$$Trans(p)=\left[\begin{array}{cccc}1& 0& 0& p_x\\ 0& 1& 0& p_y\\ 0& 0& 1& p_z\\ 0& 0& 0& 1\end{array}\right].$$
沿着单位向量 $\hat{p}$ 的方向移动 $||p||$ 距离，也记为 $Trans(\hat{p},||p||)$，$p=\hat{p}||p||$。
左乘还是右乘 $T=(R,p)$ 决定了 $\hat{w}，p$ 是在固定坐标系中，还是在刚体坐标系中。
$$\begin{array}{rl}{T}_{s{b}^{\prime }}& =T{T}_{sb}=Trans(p)Rot(\hat{w},\theta )T_{sb}(fixedframe)\\ & =\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_{sb}& p_{sb}\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}R{R}_{sb}& R{p}_{sb}+p\\ 0& 1\end{array}\right]\end{array}\text{\hspace{1em}(3.66)}$$
$$\begin{array}{rl}{T}_{s{b}^{\prime \prime }}& =T_{sb}T=T_{sb}Trans(p)Rot(\hat{w},\theta )(bodyframe)\\ & =\left[\begin{array}{cc}{R}_{sb}& p_{sb}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_{sb}R& R_{sb}p+p_{sb}\\ 0& 1\end{array}\right]\end{array}\text{\hspace{1em}(3.67)}$$
在固定坐标系下进行的变换（左乘 $T$），可以这样来解释：首先将 { b } \{b\} {b}坐标系绕着 { s } \{s\} {s}中的轴 $\hat{w}$ 进行 $\theta$ 角度的旋转，再进行 { s } \{s\} {s}中的 $p$ 平移，得到最终坐标系 { b ′ } \{b&#x27;\} {b′}。

而在刚体坐标系中的变换（右乘 $T$），解释为：先进行 { b } \{b\} {b}中的 $p$ 平移，然后绕着新坐标系中的 $\hat{w}$ 旋转，得到 { b ′ ′ } \{b&#x27;&#x27;\} {b′′}。

固定坐标系及刚体坐标系中的变换如下图3.15，其中 $\hat{w}=(0,0,1),\theta =90°,p=(0,2,0)$，于是：

$$T=(Rot(\hat{w},\theta ),p)=\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 2\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$$
而 { b } \{b\} {b}相对于 { s } \{s\} {s}为：
$$T_{sb}=\left[\begin{array}{cccc}0& 0& 1& 0\\ 0& -1& 0& -2\\ 1& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right],$$
针对固定坐标系，刚体坐标系的两个变换分别如下：
$$T{T}_{sb}=T_{s{b}^{\prime }}=\left[\begin{array}{cccc}0& 1& 0& 2\\ 0& 0& 1& 2\\ 1& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right],T_{sb}T=T_{s{b}^{\prime \prime }}=\left[\begin{array}{cccc}0& 0& 1& 0\\ -1& 0& 0& -4\\ 0& -1& 0& 0\\ 0& 0& 0& 1\end{array}\right].$$

旋量

我们现在考虑一个移动坐标系的线速度与角速度。如：
$$T_{sb}(t)=T(t)=\left[\begin{array}{cc}R(t)& p(t)\\ 0& 1\end{array}\right]\text{\hspace{1em}(3.68)}$$
上式表达的是 { b } \{b\} {b}相对于 { s } \{s\} {s}的方向及位置。我们用 $T$ 来替代 $T_{sb}$。
在前述章节，我们通过 $\dot{R}{R}^{-1}$ 或 $R^{-1}\dot{R}$ 分别得到了在固定坐标系与刚体坐标系中的角速度向量的反对称矩阵表示。那对于变换矩阵 $T$呢？

让我们先看 $T^{-1}\dot{T}$：
$$\begin{array}{rl}{T}^{-1}\dot{T}& =\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]\left[\begin{array}{cc}\dot{R}& \dot{p}\\ 0& 0\end{array}\right]\\ & =\left[\begin{array}{cc}{R}^T\dot{R}& R^T\dot{p}\\ 0& 0\end{array}\right]\\ & =\left[\begin{array}{cc}[w_b]& v_b\\ 0& 0\end{array}\right].\end{array}\text{\hspace{1em}(3.69)}$$
回忆下，$R^T\dot{R}=[w_b]$ 正是相对于 { b } \{b\} {b}中的角速度的反对称阵表达。同样，$\dot{p}$ 是 { b } \{b\} {b}的原点在 { s } \{s\} {s}中的线速度，而 $R^T\dot{p}=v_b$ 是这个线速度相对于 { b } \{b\} {b}坐标系的表示。把这两项放在一起，我们可以认为 $T^{-1}\dot{T}$ 表示的是相对于刚体坐标系的线速度与角速度。
自然的，我们会想到把结果的 $w_b,v_b$ 结合成一个六维速度向量。我们定义刚体坐标系下的空间速度（spatial velocity）或者称（body twist）为：
$${\mathcal{V}}_b=\left[\begin{array}{c}{w}_b\\ v_b\end{array}\right]\in {\mathbb{R}}^6.\text{\hspace{1em}(3.70)}$$
正如我们用一个反对称阵表示角速度向量一样，我们也用一个矩阵表示旋量：
$$T^{-1}\dot{T}=[{\mathcal{V}}_b]=\left[\begin{array}{cc}[w_b]& v_b\\ 0& 0\end{array}\right]\in se(3)，\text{\hspace{1em}(3.71)}$$
其中，$[w_b]\in so(3),v_b\in {\mathbb{R}}^3$。

让我们看看 $\dot{T}{T}^{-1}$：
$$\begin{array}{rl}\dot{T}{T}^{-1}& =\left[\begin{array}{cc}\dot{R}& \dot{p}\\ 0& 0\end{array}\right]\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}\dot{R}{R}^T& \dot{p}-\dot{R}{R}^T\dot{p}\\ 0& 0\end{array}\right]\\ & =\left[\begin{array}{cc}[w_s]& v_s\\ 0& 0\end{array}\right].\end{array}\text{\hspace{1em}(3.72)}$$
注意到，反对称阵 $[w_s]=\dot{R}{R}^T$ 是在固定坐标系下的角速度，但是 $v_s=\dot{p}-\dot{R}{R}^T\dot{p}$ 却不是刚体坐标系的原点在固定坐标系下的速度。如果我们写成：
$$v_s=\dot{p}-w_s\times p=\dot{p}+w_s\times (-p),\text{\hspace{1em}(3.73)}$$
$v_s$ 的物理意义可以推断出：假设移动的刚体无限大，$v_s$ 是指固定坐标系的原点作为刚体上的一点在此时的瞬时速度。如图3.17。

我们也将$w_s,v_s$ 写成一个六维旋量：
$${\mathcal{V}}_s=\left[\begin{array}{c}{w}_s\\ v_s\end{array}\right]\in {\mathbb{R}}^6,[{\mathcal{V}}_s]=\left[\begin{array}{cc}[w_s]& v_s\\ 0& 0\end{array}\right]=\dot{T}{T}^{-1}\in se(3),\text{\hspace{1em}(3.74)}$$
其中 $[{\mathcal{V}}_s]$ 是 ${\mathcal{V}}_s$ 的矩阵表示。我们称 ${\mathcal{V}}_s$ 为空间坐标系下的空间速度或者空间旋量。

如果我们假设移动的刚体是无限大的，那么在 ${\mathcal{V}}_s=(w_s,v_s)$ 与 ${\mathcal{V}}_b=(w_b,v_b)$ 有一个自然有趣的对称性。

1. [ w b ] 是 在 { b } 坐 标 系 下 的 角 速 度 [w_b] 是在\{b\}坐标系下的角速度 [wb​]是在{b}坐标系下的角速度， [ w s ] 是 在 { s } 坐 标 系 下 的 角 速 度 [w_s] 是在\{s\}坐标系下的角速度 [ws​]是在{s}坐标系下的角速度。
2. [ v b ] 是 { b } 坐 标 系 的 原 点 在 { b } 坐 标 系 下 的 线 速 度 ， [ v s ] 是 { s } 的 原 点 在 { s } 坐 标 系 下 的 线 速 度 [v_b] 是\{b\}坐标系的原点在\{b\}坐标系下的线速度，[v_s] 是\{s\}的原点在\{s\}坐标系下的线速度 [vb​]是{b}坐标系的原点在{b}坐标系下的线速度，[vs​]是{s}的原点在{s}坐标系下的线速度。

我们可以从 ${\mathcal{V}}_s$ 推出 ${\mathcal{V}}_b$：
$$[{\mathcal{V}}_b]=T^{-1}\dot{T}=T^{-1}[{\mathcal{V}}_s]T.\text{\hspace{1em}(3.75)}$$
反过来有：
$$[{\mathcal{V}}_s]=T[{\mathcal{V}}_b]T^{-1}.\text{\hspace{1em}(3.76)}$$
对式（3.76）展开有：
$$[{\mathcal{V}}_s]=\left[\begin{array}{cc}R[w_b]R^T& -R[w_b]R^{T}p+R{v}_b\\ 0& 0\end{array}\right]$$
其中，$R[w]R^T=[Rw]，[w]p=-[p]w$，可以整理成：
$$\left[\begin{array}{c}{w}_s\\ v_s\end{array}\right]=\left[\begin{array}{cc}R& 0\\ [p]R& R\end{array}\right]\left[\begin{array}{c}{w}_b\\ v_b\end{array}\right].$$
定义
给定一个矩阵 $T=(R,p)\in SE(3)$，其伴随表示为：
$$[A{d}_T]=\left[\begin{array}{cc}R& 0\\ [p]R& R\end{array}\right]\in {\mathbb{R}}^{6\times 6}.$$
对于任意的 $\mathcal{V}\in {\mathbb{R}}^6$，与 $T$ 关联的伴随表示为：
$${\mathcal{V}}^{\prime }=[A{d}_T]\mathcal{V},$$
也可以写作：
$${\mathcal{V}}^{\prime }=A{d}_T(\mathcal{V}).$$
对于$[\mathcal{V}]\in se(3),\mathcal{V}\in {\mathbb{R}}^6$，有：
$$[{\mathcal{V}}^{\prime }]=T[\mathcal{V}]T^{-1}.$$
并且有如下性质：

1. $A{d}_{T_1}(A{d}_{T_2}(\mathcal{V}))=A{d}_{T_1{T}_2}(\mathcal{V})$
2. $[A{d}_{T_1}][A{d}_{T_2}](\mathcal{V})=[A{d}_{T_1{T}_2}]\mathcal{V}$
3. 对于任意 $T\in SE(3)$，$[A{d}_T{]}^{-1}=[A{d}_{T^{-1}}]$

旋量小结

总结一下上面的旋量知识点。

定理
给定一个固定空间坐标系 {s}，一个刚体坐标系 {b}，和一个变换 $T_{sb}(t)\in SE(3)$， 其中：
$$T_{sb}(t)=\left[\begin{array}{cc}R(t)& p(t)\\ 0& 1\end{array}\right],\text{\hspace{1em}(3.80)}$$
那么有：
$$T_{sb}^{-1}\dot{T_{sb}}=[{\mathcal{V}}_b]=\left[\begin{array}{cc}[w_b]& v_b\\ 0& 0\end{array}\right]\in se(3)\text{\hspace{1em}(3.81)}$$
同样有：
$$\dot{T_{sb}}{T}_{sb}^{-1}=[{\mathcal{V}}_s]=\left[\begin{array}{cc}[w_s]& v_s\\ 0& 0\end{array}\right]\in se(3)\text{\hspace{1em}(3.82)}$$
而${\mathcal{V}}_s$ 与 ${\mathcal{V}}_b$ 的关系如下：
$${\mathcal{V}}_s=\left[\begin{array}{c}{w}_s\\ v_s\end{array}\right]=\left[\begin{array}{cc}R& 0\\ [p]R& R\end{array}\right]\left[\begin{array}{c}{w}_b\\ v_b\end{array}\right]=[A{d}_{T_{sb}}]{\mathcal{V}}_b\text{\hspace{1em}(3.83)}$$
$${\mathcal{V}}_b=\left[\begin{array}{c}{w}_b\\ v_b\end{array}\right]=\left[\begin{array}{cc}{R}^T& 0\\ -R^T[p]& R^T\end{array}\right]\left[\begin{array}{c}{w}_s\\ v_s\end{array}\right]=[A{d}_{T_{bs}}]{\mathcal{V}}_s\text{\hspace{1em}(3.84)}$$
更一般的，对于任意两个坐标系 { c } , { d } \{c\}, \{d\} {c},{d}，旋量的表达为：
$${\mathcal{V}}_c=[A{d}_{T_{cd}}]{\mathcal{V}}_d,{\mathcal{V}}_d=[A{d}_{T_{dc}}]{\mathcal{V}}_c.$$

旋量的螺旋解释

正如角速度 $w$ 可以看做是 $\hat{w}\dot{\theta }$，旋量 $\mathcal{V}$ 可以看做是旋量轴 $\mathcal{S}$ 的旋转，且速度为 $\dot{\theta }$。
旋量轴的运动与螺旋类似，绕着这跟轴转且伴随沿着轴的平移运动。旋量轴的一种表示为：$\mathcal{S}$ 由 { q , s ^ , h } \{q, \hat{s}, h\} {q,s^,h} 组成，其中 $q\in {\mathbb{R}}^3$，是轴上的任意一点，$\hat{s}$ 是轴的单位向量， $h$ 是螺距，表示的是线速度与角速度的比值。如图3.19。

则 $\mathcal{V}=(w,v)$ 与之关联的绕轴 $\mathcal{S}$ 的角速度为 $\dot{\theta }$，可以写作：
$$\mathcal{V}=\left[\begin{array}{c}w\\ v\end{array}\right]=\left[\begin{array}{c}\hat{s}\dot{\theta }\\ -\hat{s}\dot{\theta }\times q+h\hat{s}\dot{\theta }\end{array}\right].$$
注意到，线速度是由两部分组成的：

1. 沿着旋量轴的移动，$h\hat{s}\dot{\theta }$,
2. 由于绕轴旋转产生的原点线性移动，$-\hat{s}\dot{\theta }\times q$.

有如下定义：

1. 如果 $w≠0$, 则$\mathcal{S}=\mathcal{V}/||w||=(w/||w||,v/||w||)$。旋量轴 $\mathcal{S}$ 就是用$\mathcal{V}$ 除以角速度向量的长度。角速度 $\dot{\theta }=||w||$， 且$\mathcal{S}\dot{\theta }=\mathcal{V}$。
2. 如果 $w=0$，则$\mathcal{S}=\mathcal{V}/||v||=(0,v/||v||)$。沿着旋量轴的线速度 $\dot{\theta }=||v||$，$\mathcal{S}\dot{\theta }=\mathcal{V}$。

定义
对于给定的一个参考坐标系，旋量轴 $\mathcal{S}$ 写作：
$$\mathcal{S}=\left[\begin{array}{c}w\\ v\end{array}\right]\in {\mathbb{R}}^6,$$
要么，（i）$||w||=1$，要么 (ii) $w=0,||v||=1$。
如果(i)成立，那么 $v=-w\times q+hw$，其中 $q$ 是旋量轴上的一点，$h$ 是螺距（如果h=0,意味着这个运动只有纯粹的绕轴旋转）。
如果(ii)成立，那么螺距是无限大，运动变成沿着轴的平移，且速度为 $v$。

由于旋量轴只是单位化的旋量，$\mathcal{S}=(w,v)$ 的矩阵表示$[\mathcal{S}]$ 为：
$$[\mathcal{S}]=\left[\begin{array}{cc}[w]& v\\ 0& 0\end{array}\right]\in se(3),[w]=\left[\begin{array}{ccc}0& -w_3& w_2\\ w_3& 0& -w_1\\ -w_2& w_1& 0\end{array}\right]\in so(3)\text{\hspace{1em}(3.85)}$$
当然，对于任意两个坐标系， { a } , { b } \{a\}, \{b\} {a},{b} 有：
$${\mathcal{S}}_a=[A{d}_{T_{ab}}]{\mathcal{S}}_b,{\mathcal{S}}_b=[A{d}_{T_{ba}}]{\mathcal{S}}_a$$

刚体移动的指数坐标表示

刚体移动的指数坐标

在前述章节，我们可以看到，任何平面刚体的移动总是可以绕着某个平面内的某个固定点选择得到（对于纯平移，这个点在无限远处）。对于空间的刚体移动，也有同样的结论，不过是绕着一根固定的旋量轴 $\mathcal{S}$。
类似于旋转的指数坐标 $\hat{w}\theta$，我们定义一个六维的齐次变换指数坐标：$\mathcal{S}\theta \in {\mathbb{R}}^6$。$\mathcal{S}$ 是旋量轴，$\theta$ 是将坐标系从原点移动到 $T$ 的值。
同样的有：
$$\begin{gathered} exp:[\mathcal{S}]\theta \in se(3)\to T\in SE(3) \\ log:T\in SE(3)\to [\mathcal{S}]\theta \in se(3) \end{gathered}$$

定义
设 $\mathcal{S}=(w,v)$ 是旋量轴，如果$||w||=1$，对于任意 $\theta \in \mathbb{R}$， 有：
$$e^{[\mathcal{S}]\theta }=\left[\begin{array}{cc}{e}^{[w]\theta }& (I\theta +(1-cos\theta )[w]+(\theta -sin\theta )[w{]}^2)v\\ 0& 1\end{array}\right].\text{\hspace{1em}(3.88)}$$
如果$||w||=0,||v||=1$，那么：
$$e^{[\mathcal{S}]\theta }=\left[\begin{array}{cc}I& v\theta \\ 0& 1\end{array}\right].\text{\hspace{1em}(3.89)}$$

刚体移动的矩阵对数

如果给定一对$(R,p)\in SE(3)$，那么总能找到一个旋量轴 $\mathcal{S}=(w,v)$ 和标量 $\theta$：
$$e^{[\mathcal{S}]\theta }=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right],\text{\hspace{1em}(3.90)}$$
比如，矩阵：
$$[\mathcal{S}]\theta =\left[\begin{array}{cc}[w]\theta & v\theta \\ 0& 0\end{array}\right]\in se(3)$$
是 $T=(R,p)$ 的矩阵对数。

算法
给定一对$(R,p)$，对应的矩阵 $T\in SE(3)$，找到一个 $\theta \in [0,\pi ]$和一个旋量轴 $\mathcal{S}=(w,v)\in {\mathbb{R}}^6$，如此 $e^{[\mathcal{S}]\theta }=T$。向量 $\mathcal{S}\theta \in {\mathbb{R}}^6$ 组成了矩阵 $T$ 的指数坐标，而矩阵 $[\mathcal{S}]\theta \in se(3)$ 是矩阵 $T$ 的对数。

1. 如果 $R=I$，那么使 $w=0,v=p/||p||,\theta =||p||$。
2. 其余情况，使用 $SO(3)$ 上的矩阵对数来决定 $R$ 的 $w(SO(3)$算法里写作$\hat{w}),\theta$。那么：

$$v=G^{-1}(\theta )p\text{\hspace{1em}(3.91)}$$
其中，
$$G^{-1}(\theta )=\frac{1}{\theta }I-\frac{1}{2}[w]+(\frac{1}{\theta }-\frac{1}{2}cot\frac{\theta }{2})[w{]}^2\text{\hspace{1em}(3.92)}$$

示例

一个刚体在平面$\widehat{x_s}-\widehat{y_s}$内运动。初始坐标系 { b } \{b\} {b}与最终坐标系 { c } \{c\} {c}如图3.20所以，可表达为：

$$T_{sb}=\left[\begin{array}{cccc}cos30°& -sin30°& 0& 1\\ sin30°& cos30°& 0& 2\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$$
$$T_{sc}=\left[\begin{array}{cccc}cos60°& -sin60°& 0& 2\\ sin60°& cos60°& 0& 1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$$

由于运动发生在平面内，所以旋量轴与${\hat{z}}_s$共方向，且螺距为0。则旋量轴在固定坐标系内表示为：$\mathcal{S}=(w,v)$，并且有如下形式：
$$\begin{gathered} w=(0,0,w_3), \\ v=(v_1,v_2,0). \end{gathered}$$
我们寻找一个旋量运动使得 $T_{sb}$ 到达 $T_{sc}$，如 $T_{sc}=e^{[\mathcal{S}]\theta }{T}_{sb}$或：
$$T_{sc}{T}_{sb}^{-1}=e^{[\mathcal{S}]\theta }，$$

其中：
$$[\mathcal{S}]=\left[\begin{array}{cccc}0°& -w_3& 0& v_1\\ w_3& 0& 0& v_2\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$$
我们可以使用矩阵对数算法$T_{sc}{T}_{sb}^{-1}$ 来求 $[\mathcal{S}]，\theta$：
$$[\mathcal{S}]=\left[\begin{array}{cccc}0& -1& 0& 3.37\\ 1& 0& 0& -3.37\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right],\mathcal{S}=\left[\begin{array}{c}{w}_1\\ w_2\\ w_3\\ v_1\\ v_2\\ v_3\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 1\\ 3.37\\ -3.37\\ 0\end{array}\right],\theta =\frac{\pi }{6}rador30°.$$