《Modern Robotics》阅读笔记5——刚体的运动（五）

《Modern Robotics》阅读笔记5——刚体的运动（五）

前面铺垫了那么久，今天终于要讲到正题了。

1. 前文小结

这关于刚体运动的这五篇文章实际上就是在讲两件事：

• 1.刚体旋转如何表示？
• 2.刚体运动（包括刚体的旋转+平移）如何表示？

对于1

• 刚体旋转我们介绍了两种表示法，一种是旋转矩阵，另一种是轴角表示法（指数表示法）
• 旋转矩阵：$R\in SO(3):3\times 3$ matrices
• 轴角表示（指数表示）：$[\hat{\omega }]\theta \in so(3)$，其中$\hat{\omega }$代表旋转轴
• 然后我们研究了，由旋转矩阵到轴角的映射，以及轴角到旋转矩阵的映射函数（李代数）
• $\exp :[\hat{\omega }]\theta \in so(3)\to R\in SO(3)$，即$R=\mathrm{Rot}(\hat{\omega },\theta )=e^{[\hat{\omega }]\theta }$
• $\log :R\in SO(3)\to [\hat{\omega }]\theta \in so(3)$

疑问：旋转矩阵是我们非常熟悉的一种旋转运动的表示方法，为什么我们还要研究轴角（指数）的这种表示方法呢？

答：因为旋转矩阵虽然通用，但是实际上并不直观，看到一个旋转矩阵，我们往往都想象不出这是一个什么样的旋转运动。而轴角表示方法则非常直观，指定一个旋转轴，指定一个角度，我们能直观的想象这个运动。并且对于机器人而言，很多关节都是转动关键，旋转轴就是关节的轴线，用轴角的表示方法是非常自然的。另外，我们引入了由轴角到旋转矩阵的映射函数，这就让我们使用轴角更加方便了。

对于2（这就是我们这篇文章将要介绍的内容了。）

• 前文中，我们介绍了一种刚体运动的表示方法，就是变换矩阵。这篇中，我们将介绍另一种方法：旋量。
• 变换矩阵：$T\in SE(3):4\times 4$ matrices，$T=\left[\begin{array}{ll}R& p\\ 0& 1\end{array}\right]$
• 旋量：$[\mathcal{S}]\theta \in se(3)$，其中$\mathcal{S}$代表旋量轴（screw axis）
• 和旋转类似，由变换矩阵到旋量，旋量到变换矩阵的映射函数（李代数）
• $\exp :[\mathcal{S}]\theta \in \mathrm{se}(3)\to T\in SE(3)$，即$T=e^{[\mathcal{S}]\theta }$
• $\log :T\in SE(3)\to [\mathcal{S}]\theta \in \mathrm{se}(3)$

疑问：已经有了变换矩阵这种方法，为什么我们还要研究旋量的这种表示方法呢？

答：这个问题在下文中，我们会在介绍旋量理论的过程中逐渐解答。

现在让我们进入正题吧。

2. 旋量理论（Screw）

2.1 一个小例子

首先，让我们从上面这一个例子开始。
为了绘图的直观，我们在一个二维空间内介绍旋量理论的基本框架。三维空间是完全类似的。
假设有三个坐标系，其中{s}系为固定系。{b}系是体坐标系，{c}系是{b}系经过旋转后到达的新坐标系。我们给出{s}与{b}，{s}系与{c}系的变换矩阵如下：
$$T_{sb}=\left[\begin{array}{ccc}\cos 30^{\circ }& -\sin 30^{\circ }& 1\\ \sin 30^{\circ }& \cos 30^{\circ }& 2\\ 0& 0& 1\end{array}\right]$$
$$T_{sc}=\left[\begin{array}{ccc}\cos 60^{\circ }& -\sin 60^{\circ }& 2\\ \sin 60^{\circ }& \cos 60^{\circ }& 1\\ 0& 0& 1\end{array}\right]$$
在基于变换矩阵的刚体表示方法中，{b}系到{c}系的旋转可以表示为：
$$T_{\mathrm{c}\mathrm{b}}=T_{\mathrm{s}\mathrm{c}}{T}_{\mathrm{s}\mathrm{b}}^{-1}=\left[\begin{array}{ccc}0.866& -0.5& 2.134\\ 0.5& 0.866& -1.2321\\ 0& 0& 1\end{array}\right]$$

但细心观察我们可以发现，这个刚体运动，可以被看做一个单纯的旋转运动。即绕着$q=(3.37,3.37)$这个点，顺时针旋转$\theta$度。我们可以大胆的假设，那么是不是所有的刚体运动，都可以看做是一个旋转运动呢？答案是肯定的。二维空间，这是大家非常容易想象的。而即便在三维空间，任意刚体运动也都可以视作为绕某一轴的旋转运动。这就是旋量理论的由来。旋量理论就是告诉我们如何找到这个旋量轴，利用旋量轴来定义刚体运动。

有些同学可能会想到，如果是纯平移呢？这也能作为旋转运动吗？
实际上这可以视作为半径无穷大的旋转运动。当然在旋量理论中，对于纯平移这种特殊情况也是有分开考虑的。

2.2 旋量轴的定义

旋量轴$\mathcal{S}$（screw axis）的定义与前文中提到过的twist的定义密不可分。
先来回顾一下twist的定义：
$$\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)$$，
旋量轴的定义分了两种情况进行考虑：

• 一般的刚体运动：$if\omega ̸=0:\mathcal{S}=\mathcal{V}/\Vert \omega \Vert =(\omega /\Vert \omega \Vert ,v/\Vert \omega \Vert )，\dot{\theta }=\Vert \omega \Vert$
• 纯平移运动：$if\omega =0:\mathcal{S}=\mathcal{V}/\Vert v\Vert =(0,v/\Vert v\Vert )，\dot{\theta }=\Vert v\Vert$

所以，旋量轴与twist的关系为：
$$\mathcal{S}\dot{\theta }=\mathcal{V}$$

值得注意的是，这里不能想象成刚体围绕这个旋量轴进行旋转。毕竟旋量轴是一个六维的矢量，而刚体在三维空间中的旋转只能围绕一个三维矢量进行。
实际上，旋量轴定义了旋转轴的位置。 对于一般刚体运动而言，旋量轴的前三个量定义了旋转轴矢量的方向，而后三个量和前三个量则共同定义了旋转轴的位置。

为了解释这个问题，来个穿越，我们可以再回到先前的例子中去。
根据图中的{b}系和{c}系的位置，我们可以通过做图测量的方法，找到两个坐标系的旋转中心$q=(3.37,3.37)$。我们假设{b}系到{c}系的旋转角速度为${\omega }_3$，那么我们知道，在此旋转运动下，{s}系原点处对应的速度为$v=(3.37{\omega }_3,-3.37{\omega }_3)$（角速度乘距离）。

注意：这里的量都是表示在{s}系下的。

根据上一篇文章中，关于$\mathcal{V}=(\omega ,v)$的定义可知：
$$\mathcal{V}=[0,0,{\omega }_3,3.37{\omega }_3,-3.37{\omega }_3,0{]}^T$$

所以，根据旋量轴的定义，旋量轴为：
$$\mathcal{S}=[0,0,1,3.37,-3.37,0{]}^T$$

可知，$\mathcal{S}$的前三个量代表了旋转轴的方向，在本例中就是z轴指向的方向。$\mathcal{S}$的后三个量与前三个量一起，确定了旋转轴的位置，就是$(3.37,3.37)$。

2.3 旋量与变换矩阵的相互转换

这一部分就不予以推导了，直接给出结论。但我们可以通过例子来验证，这种变换关系的有效性。

旋量——>变换矩阵：
$\exp :[\mathcal{S}]\theta \in \mathrm{se}(3)\to T\in SE(3)$，即$T=e^{[\mathcal{S}]\theta }$
变换矩阵——>旋量：
$\log :T\in SE(3)\to [\mathcal{S}]\theta \in \mathrm{se}(3)$

下面通过上文中的例子来验证所给关系的正确性。
首先，关于旋量轴，还需要补充下列表示方法：
$$\mathcal{S}=\left[\begin{array}{c}\omega \\ v\end{array}\right]\in {\mathbb{R}}^6$$
$$[\mathcal{S}]=\left[\begin{array}{cc}[\omega ]& v\\ 0& 0\end{array}\right]\in se(3),[\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)$$
所以，据此，上文中我们得到了$\mathcal{S}=[0,0,1,3.37,-3.37,0{]}^T$，实际上换成二维平面的旋量轴就是：
$$\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]$$
那么有：
$$[\mathcal{S}]=\left[\begin{array}{rrr}0& -1& 3.37\\ 1& 0& -3.37\\ 0& 0& 0\end{array}\right]，\theta =\pi /6\mathrm{rad}\left(\text{ or }30^{\circ }\right)$$
所以我们计算可得（根据上文对$\mathcal{S}$的分析，可知这是把{b}系变到{c}系的变换矩阵）：
$$T_{\mathrm{c}\mathrm{b}}=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]$$

上文中我们通过变换矩阵，计算得到的是：
$$T_{\mathrm{c}\mathrm{b}}=T_{\mathrm{s}\mathrm{c}}{T}_{\mathrm{s}\mathrm{b}}^{-1}=\left[\begin{array}{ccc}0.866& -0.5& 2.134\\ 0.5& 0.866& -1.2321\\ 0& 0& 1\end{array}\right]$$

可以看出，经过上面两种不同的计算方式，得到的变换矩阵是一样的，也就证明了我们所提变换关系的有效性。

2.4 为什么要用旋量？

至此，旋量理论的基本内容就差不多结束了。

最后我们来探讨一下为什么要用旋量理论。

就我个人看法而言，对于机器人，大多都是旋转关节，而对于旋转关节，旋转轴是很容易确定的，这就给刚体运动的表达带来了很大的便利性。

3. Wrenches

在这篇文章的最后，我们介绍一个与作用力和力矩有关的概念——wrenches。
这个单词如何翻译成中文我还不太确定，只好姑且先这么用着。
这个概念将来学习到动力学部分时，是非常重要的！

第一个重要的问题：这个概念如何定义的呢？

考虑有一个线性作用力$f$，作用在刚体的$r$点上。定义一个坐标系{a}，那么$r_a$代表$r$点在{a}系中的坐标，作用力$f$在{a}系中被表示为$f_a$。所以，这个力在{a}系中会产生一个作用力矩：
$$m_a=r_a\times f_a$$
我们模仿之前对于twists的定义，定义wrench（或者叫做spatial force）为（参考系{a}下的）：
$${\mathcal{F}}_a=\left[\begin{array}{c}{m}_a\\ f_a\end{array}\right]\in {\mathbb{R}}^6$$

值得注意的是，当线性作用力部分为0时，这就是个纯力矩。

另一个重要的问题：不同坐标系下的wrench如何相互转换？
我们知道，能量是不随坐标系的不同而不同的，与坐标系定义无关。所以，下面等式成立：
$${\mathcal{V}}_b^T{\mathcal{F}}_b={\mathcal{V}}_a^T{\mathcal{F}}_a$$$$\begin{array}{rl}{\mathcal{V}}_b^T{\mathcal{F}}_b& ={\left(\left[{\mathrm{Ad}}_{T_{ab}}\right]{\mathcal{V}}_b\right)}^T{\mathcal{F}}_a\\ & ={\mathcal{V}}_b^T{\left[{\mathrm{A}\mathrm{d}}_{T_{ab}}\right]}^T{\mathcal{F}}_a\end{array}$$
$${\mathcal{F}}_b={\left[{\mathrm{A}\mathrm{d}}_{T_{ab}}\right]}^T{\mathcal{F}}_a$$
类似的：
$${\mathcal{F}}_a={\left[{\mathrm{A}\mathrm{d}}_{T_{\mathrm{b}\mathrm{a}}}\right]}^T{\mathcal{F}}_b$$

值得注意的是，当刚体上作用了多个wrench时，只要把他们表示在同一坐标系下以后，简单相加即可！！！

本篇文章结束。

注：为了更好的阅读体验，避免文章过长，原本一章的内容被拆分成了多篇文章。