前面铺垫了那么久，今天终于要讲到正题了。
这关于刚体运动的这五篇文章实际上就是在讲两件事：
1.刚体旋转如何表示？2.刚体运动（包括刚体的旋转+平移）如何表示？对于1
刚体旋转我们介绍了两种表示法，一种是旋转矩阵，另一种是轴角表示法（指数表示法）旋转矩阵： R ∈ S O ( 3 ) : 3 × 3 R \in S O(3) : 3 \times 3 R∈SO(3):3×3 matrices轴角表示（指数表示）： [ ω ^ ] θ ∈ s o ( 3 ) [\hat{\omega}] \theta \in s o(3) [ω^]θ∈so(3)，其中 ω ^ \hat{\omega} ω^代表旋转轴然后我们研究了，由旋转矩阵到轴角的映射，以及轴角到旋转矩阵的映射函数（李代数） exp : [ ω ^ ] θ ∈ s o ( 3 ) → R ∈ S O ( 3 ) \exp :[\hat{\omega}] \theta \in s o(3) \rightarrow R \in S O(3) exp:[ω^]θ∈so(3)→R∈SO(3)，即 R = Rot ( ω ^ , θ ) = e [ ω ^ ] θ R=\operatorname{Rot}(\hat{\omega}, \theta)=e^{[\hat{\omega}] \theta} R=Rot(ω^,θ)=e[ω^]θ log : R ∈ S O ( 3 ) → [ ω ^ ] θ ∈ s o ( 3 ) \log : R \in S O(3) \rightarrow[\hat{\omega}] \theta \in s o(3) log:R∈SO(3)→[ω^]θ∈so(3)疑问：旋转矩阵是我们非常熟悉的一种旋转运动的表示方法，为什么我们还要研究轴角（指数）的这种表示方法呢？
答：因为旋转矩阵虽然通用，但是实际上并不直观，看到一个旋转矩阵，我们往往都想象不出这是一个什么样的旋转运动。而轴角表示方法则非常直观，指定一个旋转轴，指定一个角度，我们能直观的想象这个运动。并且对于机器人而言，很多关节都是转动关键，旋转轴就是关节的轴线，用轴角的表示方法是非常自然的。另外，我们引入了由轴角到旋转矩阵的映射函数，这就让我们使用轴角更加方便了。
对于2（这就是我们这篇文章将要介绍的内容了。）
前文中，我们介绍了一种刚体运动的表示方法，就是变换矩阵。这篇中，我们将介绍另一种方法：旋量。变换矩阵： T ∈ S E ( 3 ) : 4 × 4 T \in S E(3) : 4 \times 4 T∈SE(3):4×4 matrices， T = [ R p 0 1 ] T=\left[ \begin{array}{ll}{R} & {p} \\ {0} & {1}\end{array}\right] T=[R0p1]旋量： [ S ] θ ∈ s e ( 3 ) [\mathcal{S}] \theta \in {se}(3) [S]θ∈se(3)，其中 S \mathcal{S} S代表旋量轴（screw axis）和旋转类似，由变换矩阵到旋量，旋量到变换矩阵的映射函数（李代数） exp : [ S ] θ ∈ se ( 3 ) → T ∈ S E ( 3 ) \exp :[\mathcal{S}] \theta \in \operatorname{se}(3) \rightarrow T \in S E(3) exp:[S]θ∈se(3)→T∈SE(3)，即 T = e [ S ] θ T=e^{[\mathcal{S}] \theta} T=e[S]θ log : T ∈ S E ( 3 ) → [ S ] θ ∈ se ( 3 ) \log : T \in S E(3) \rightarrow[\mathcal{S}] \theta \in \operatorname{se}(3) log:T∈SE(3)→[S]θ∈se(3)疑问：已经有了变换矩阵这种方法，为什么我们还要研究旋量的这种表示方法呢？
答：这个问题在下文中，我们会在介绍旋量理论的过程中逐渐解答。
现在让我们进入正题吧。
首先，让我们从上面这一个例子开始。 为了绘图的直观，我们在一个二维空间内介绍旋量理论的基本框架。三维空间是完全类似的。 假设有三个坐标系，其中{s}系为固定系。{b}系是体坐标系，{c}系是{b}系经过旋转后到达的新坐标系。我们给出{s}与{b}，{s}系与{c}系的变换矩阵如下： T s b = [ cos 3 0 ∘ − sin 3 0 ∘ 1 sin 3 0 ∘ cos 3 0 ∘ 2 0 0 1 ] T_{s b}=\left[ \begin{array}{ccc}{\cos 30^{\circ}} & {-\sin 30^{\circ}} & {1} \\ {\sin 30^{\circ}} & {\cos 30^{\circ}} & {2} \\ {0} & {0} & {1}\end{array}\right] Tsb=⎣⎡cos30∘sin30∘0−sin30∘cos30∘0121⎦⎤ T s c = [ cos 6 0 ∘ − sin 6 0 ∘ 2 sin 6 0 ∘ cos 6 0 ∘ 1 0 0 1 ] T_{s c}=\left[ \begin{array}{ccc}{\cos 60^{\circ}} & {-\sin 60^{\circ}} & {2} \\ {\sin 60^{\circ}} & {\cos 60^{\circ}} & {1} \\ {0} & {0} & {1}\end{array}\right] Tsc=⎣⎡cos60∘sin60∘0−sin60∘cos60∘0211⎦⎤ 在基于变换矩阵的刚体表示方法中，{b}系到{c}系的旋转可以表示为： T c b = T s c T s b − 1 = [ 0.866 − 0.5 2.134 0.5 0.866 − 1.2321 0 0 1 ] T_{\mathrm{cb}}=T_{\mathrm{sc}} T_{\mathrm{sb}}^{-1}= \left[ \begin{array}{ccc}0.866&-0.5&2.134\\ 0.5 &0.866&-1.2321\\0&0&1\end{array}\right] Tcb=TscTsb−1=⎣⎡0.8660.50−0.50.86602.134−1.23211⎦⎤
但细心观察我们可以发现，这个刚体运动，可以被看做一个单纯的旋转运动。即绕着 q = ( 3.37 , 3.37 ) q=(3.37,3.37) q=(3.37,3.37)这个点，顺时针旋转 θ \theta θ度。我们可以大胆的假设，那么是不是所有的刚体运动，都可以看做是一个旋转运动呢？答案是肯定的。二维空间，这是大家非常容易想象的。而即便在三维空间，任意刚体运动也都可以视作为绕某一轴的旋转运动。这就是旋量理论的由来。旋量理论就是告诉我们如何找到这个旋量轴，利用旋量轴来定义刚体运动。
有些同学可能会想到，如果是纯平移呢？这也能作为旋转运动吗？ 实际上这可以视作为半径无穷大的旋转运动。当然在旋量理论中，对于纯平移这种特殊情况也是有分开考虑的。
旋量轴 S \mathcal{S} S（screw axis）的定义与前文中提到过的twist的定义密不可分。 先来回顾一下twist的定义： V = [ ω v ] ∈ R 6 ， [ V ] = [ [ ω ] v 0 0 ] ∈ s e ( 3 ) \mathcal{V}=\left[ \begin{array}{c}{\omega} \\ {v}\end{array}\right] \in \mathbb{R}^{6} ，\left[\mathcal{V}\right]=\left[ \begin{array}{cc}{\left[\omega\right]} & {v} \\ {0} & {0}\end{array}\right] \in {se}(3) V=[ωv]∈R6，[V]=[[ω]0v0]∈se(3)， 旋量轴的定义分了两种情况进行考虑：
一般的刚体运动： i f ω ≠ 0 : S = V / ∥ ω ∥ = ( ω / ∥ ω ∥ , v / ∥ ω ∥ ) ， θ ˙ = ∥ ω ∥ if \ \omega \neq 0 : \mathcal{S}=\mathcal{V} /\|\omega\|=(\omega /\|\omega\|, v /\|\omega\|)，\dot{\theta}=\|\omega\| if ω̸=0:S=V/∥ω∥=(ω/∥ω∥,v/∥ω∥)，θ˙=∥ω∥纯平移运动： i f ω = 0 : S = V / ∥ v ∥ = ( 0 , v / ∥ v ∥ ) ， θ ˙ = ∥ v ∥ if \ \omega=0 : \mathcal{S}=\mathcal{V} /\|v\|=(0, v /\|v\|)，\dot{\theta}=\|v\| if ω=0:S=V/∥v∥=(0,v/∥v∥)，θ˙=∥v∥所以，旋量轴与twist的关系为： S θ ˙ = V \mathcal{S} \dot{\theta}=\mathcal{V} Sθ˙=V
值得注意的是，这里不能想象成刚体围绕这个旋量轴进行旋转。毕竟旋量轴是一个六维的矢量，而刚体在三维空间中的旋转只能围绕一个三维矢量进行。 实际上，旋量轴定义了旋转轴的位置。 对于一般刚体运动而言，旋量轴的前三个量定义了旋转轴矢量的方向，而后三个量和前三个量则共同定义了旋转轴的位置。
为了解释这个问题，来个穿越，我们可以再回到先前的例子中去。 根据图中的{b}系和{c}系的位置，我们可以通过做图测量的方法，找到两个坐标系的旋转中心 q = ( 3.37 , 3.37 ) q=(3.37,3.37) q=(3.37,3.37)。我们假设{b}系到{c}系的旋转角速度为 ω 3 \omega_3 ω3，那么我们知道，在此旋转运动下，{s}系原点处对应的速度为 v = ( 3.37 ω 3 , − 3.37 ω 3 ) v=(3.37\omega_3,-3.37\omega_3) v=(3.37ω3,−3.37ω3)（角速度乘距离）。
注意：这里的量都是表示在{s}系下的。
根据上一篇文章中，关于 V = ( ω , v ) \mathcal{V}=(\omega,v) V=(ω,v)的定义可知： V = [ 0 , 0 , ω 3 , 3.37 ω 3 , − 3.37 ω 3 , 0 ] T \mathcal{V}=[0,0,\omega_3,3.37\omega_3,-3.37\omega_3,0]^T V=[0,0,ω3,3.37ω3,−3.37ω3,0]T
所以，根据旋量轴的定义，旋量轴为： S = [ 0 , 0 , 1 , 3.37 , − 3.37 , 0 ] T \mathcal{S}=[0,0,1,3.37,-3.37,0]^T S=[0,0,1,3.37,−3.37,0]T
可知， S \mathcal{S} S的前三个量代表了旋转轴的方向，在本例中就是z轴指向的方向。 S \mathcal{S} S的后三个量与前三个量一起，确定了旋转轴的位置，就是 ( 3.37 , 3.37 ) (3.37,3.37) (3.37,3.37)。
这一部分就不予以推导了，直接给出结论。但我们可以通过例子来验证，这种变换关系的有效性。
旋量——>变换矩阵： exp : [ S ] θ ∈ se ( 3 ) → T ∈ S E ( 3 ) \exp :[\mathcal{S}] \theta \in \operatorname{se}(3) \rightarrow T \in S E(3) exp:[S]θ∈se(3)→T∈SE(3)，即 T = e [ S ] θ T=e^{[\mathcal{S}] \theta} T=e[S]θ 变换矩阵——>旋量： log : T ∈ S E ( 3 ) → [ S ] θ ∈ se ( 3 ) \log : T \in S E(3) \rightarrow[\mathcal{S}] \theta \in \operatorname{se}(3) log:T∈SE(3)→[S]θ∈se(3)
下面通过上文中的例子来验证所给关系的正确性。 首先，关于旋量轴，还需要补充下列表示方法： S = [ ω v ] ∈ R 6 \mathcal{S}=\left[ \begin{array}{c}{\omega} \\ {v}\end{array}\right] \in \mathbb{R}^{6} S=[ωv]∈R6 [ S ] = [ [ ω ] v 0 0 ] ∈ s e ( 3 ) , [ ω ] = [ 0 − ω 3 ω 2 ω 3 0 − ω 1 − ω 2 ω 1 0 ] ∈ s o ( 3 ) [\mathcal{S}]=\left[ \begin{array}{cc}{[\omega]} & {v} \\ {0} & {0}\end{array}\right] \in {se}(3), \quad[\omega]=\left[ \begin{array}{ccc}{0} & {-\omega_{3}} & {\omega_{2}} \\ {\omega_{3}} & {0} & {-\omega_{1}} \\ {-\omega_{2}} & {\omega_{1}} & {0}\end{array}\right] \in {so}(3) [S]=[[ω]0v0]∈se(3),[ω]=⎣⎡0ω3−ω2−ω30ω1ω2−ω10⎦⎤∈so(3) 所以，据此，上文中我们得到了 S = [ 0 , 0 , 1 , 3.37 , − 3.37 , 0 ] T \mathcal{S}=[0,0,1,3.37,-3.37,0]^T S=[0,0,1,3.37,−3.37,0]T，实际上换成二维平面的旋量轴就是： S = [ ω 3 v 1 v 2 ] = [ 1 3.37 − 3.37 ] \mathcal{S}=\left[ \begin{array}{c}{\omega_{3}} \\ {v_{1}} \\ {v_{2}}\end{array}\right]=\left[ \begin{array}{c}{1} \\ {3.37} \\ {-3.37}\end{array}\right] S=⎣⎡ω3v1v2⎦⎤=⎣⎡13.37−3.37⎦⎤ 那么有： [ S ] = [ 0 − 1 3.37 1 0 − 3.37 0 0 0 ] ， θ = π / 6 rad ( or 3 0 ∘ ) [\mathcal{S}]=\left[ \begin{array}{rrr}{0} & {-1} & {3.37} \\ {1} & {0} & {-3.37} \\ {0} & {0} & {0}\end{array}\right]，\theta=\pi / 6 \operatorname{rad}\left(\text { or } 30^{\circ}\right) [S]=⎣⎡010−1003.37−3.370⎦⎤，θ=π/6rad( or 30∘) 所以我们计算可得（根据上文对 S \mathcal{S} S的分析，可知这是把{b}系变到{c}系的变换矩阵）： T c b = e [ S ] θ = [ 0.866 − 0.5 2.1365 0.5 0.866 − 1.2335 0 0 1 ] T_{\mathrm{cb}}=e^{[\mathcal{S}] \theta}=\left[ \begin{array}{ccc}0.866&-0.5&2.1365\\ 0.5 &0.866&-1.2335\\0&0&1\end{array}\right] Tcb=e[S]θ=⎣⎡0.8660.50−0.50.86602.1365−1.23351⎦⎤
上文中我们通过变换矩阵，计算得到的是： T c b = T s c T s b − 1 = [ 0.866 − 0.5 2.134 0.5 0.866 − 1.2321 0 0 1 ] T_{\mathrm{cb}}=T_{\mathrm{sc}} T_{\mathrm{sb}}^{-1}= \left[ \begin{array}{ccc}0.866&-0.5&2.134\\ 0.5 &0.866&-1.2321\\0&0&1\end{array}\right] Tcb=TscTsb−1=⎣⎡0.8660.50−0.50.86602.134−1.23211⎦⎤
可以看出，经过上面两种不同的计算方式，得到的变换矩阵是一样的，也就证明了我们所提变换关系的有效性。
至此，旋量理论的基本内容就差不多结束了。
最后我们来探讨一下为什么要用旋量理论。
就我个人看法而言，对于机器人，大多都是旋转关节，而对于旋转关节，旋转轴是很容易确定的，这就给刚体运动的表达带来了很大的便利性。
在这篇文章的最后，我们介绍一个与作用力和力矩有关的概念——wrenches。 这个单词如何翻译成中文我还不太确定，只好姑且先这么用着。 这个概念将来学习到动力学部分时，是非常重要的！
第一个重要的问题：这个概念如何定义的呢？
考虑有一个线性作用力 f f f，作用在刚体的 r r r点上。定义一个坐标系{a}，那么 r a r_a ra代表 r r r点在{a}系中的坐标，作用力 f f f在{a}系中被表示为 f a f_a fa。所以，这个力在{a}系中会产生一个作用力矩： m a = r a × f a m_{a}=r_{a} \times f_{a} ma=ra×fa 我们模仿之前对于twists的定义，定义wrench（或者叫做spatial force）为（参考系{a}下的）： F a = [ m a f a ] ∈ R 6 \mathcal{F}_{a}=\left[ \begin{array}{c}{m_{a}} \\ {f_{a}}\end{array}\right] \in \mathbb{R}^{6} Fa=[mafa]∈R6
值得注意的是，当线性作用力部分为0时，这就是个纯力矩。
另一个重要的问题：不同坐标系下的wrench如何相互转换？ 我们知道，能量是不随坐标系的不同而不同的，与坐标系定义无关。所以，下面等式成立： V b T F b = V a T F a \mathcal{V}_{b}^{T} \mathcal{F}_{b}=\mathcal{V}_{a}^{T} \mathcal{F}_{a} VbTFb=VaTFa V b T F b = ( [ Ad T a b ] V b ) T F a = V b T [ A d T a b ] T F a \begin{aligned} \mathcal{V}_{b}^{T} \mathcal{F}_{b} &=\left(\left[\operatorname{Ad}_{T_{a b}}\right] \mathcal{V}_{b}\right)^{T} \mathcal{F}_{a} \\ &=\mathcal{V}_{b}^{T}\left[\mathrm{Ad}_{T_{a b}}\right]^{T} \mathcal{F}_{a} \end{aligned} VbTFb=([AdTab]Vb)TFa=VbT[AdTab]TFa F b = [ A d T a b ] T F a \mathcal{F}_{b}=\left[\mathrm{Ad}_{T_{a b}}\right]^{T} \mathcal{F}_{a} Fb=[AdTab]TFa 类似的： F a = [ A d T b a ] T F b \mathcal{F}_{a}=\left[\mathrm{Ad}_{T_{\mathrm{ba}}}\right]^{T} \mathcal{F}_{b} Fa=[AdTba]TFb
值得注意的是，当刚体上作用了多个wrench时，只要把他们表示在同一坐标系下以后，简单相加即可！！！
