MR chapter 3 Rigid-Body Motions

本文链接：https://blog.csdn.net/qq_33673253/article/details/132562024

在本章中，我们开发了一种系统的方法来描述刚体的位置和方向，该方法依赖于将参考坐标系a reference frame 连接到刚体。当前坐标系相对于固定参考系的configuration表示为4×4矩阵。该矩阵是C空间隐式表示的一个例子，如前一章所述：通过对4×4实矩阵的16维空间应用10个约束，获得了刚体构型的实际6维空间。

这样的矩阵不仅表示坐标系的配置the configuration of a frame，而且还可以用于

平移和旋转矢量或坐标系
将矢量或坐标系的表示从一坐标系中的坐标更改为另一坐标系的坐标

这些运算可以通过简单的线性代数来执行，这是我们选择将配置表示为4×4矩阵的主要原因。

位置和方向的C空间的非欧几里得（即非“平坦”）non-Euclidean (i.e., non-"flat") 性质导致我们使用矩阵表示（隐式表示）。

然而，刚体的速度可以简单地表示为R6中的一个点，由三个角速度和三个线速度定义，我们一起称之为空间速度spatial velocity 或 twist。

更一般地说，即使机器人的C空间可能不是向量空间 vector space ，但C空间中任何点的可行速度集总是形成向量空间。

例如，考虑一个C空间为球体的机器人：尽管C空间不是平面的，但在球体上的任何点上，速度空间都可以被认为是与球体上该点相切的平面（向量空间）the plane (a vector space)。

任何刚体配置（位姿？）都可以通过从固定（原始）参考系开始并在指定时间内积分恒定twist来实现。这种运动类似于螺杆screw的运动，围绕同一固定轴旋转并沿着同一固定轴线平移。观察到所有配置都可以通过螺杆运动实现，这激发了称为指数坐标 exponential coordinates 的配置的六参数表示。这六个参数可以分为描述螺杆轴线方向的参数和指示螺杆运动必须进行多远才能实现所需配置的标量

机器人工程师进阶之路（七）旋量法（下） - 知乎 (zhihu.com)

本章最后对力进行了讨论。正如角速度和线速度在R6中被打包成一个向量一样，力矩（力矩）moments (torques) 和力也被打包成六个向量，称为空间力 spatial force 或 wrench。

为了说明这些概念并提供本章的概要，我们从一个激励性的平面示例开始a motivating planar example.。在这样做之前，我们对向量表示法做一些说明 remarks。

关于向量和参考系的一句话

自由向量free vector 是一个有长度和方向的几何量。把它想象成Rn中的一支箭。它之所以被称为“自由”，是因为它不一定植根于任何地方；只有它的长度和方向很重要。线速度可以被视为自由向量：箭头的长度是速度，箭头的方向是速度的方向。自由矢量由直立文本符号upright text symbol 表示，例如v。

如果已经为v所在的底层空间选择了参考系和长度比例，那么可以将该自由向量移动到一个位置，使得箭头的底部位于原点，而不改变方向。自由向量v然后可以由其在参考系中的坐标来表示。我们用斜体字写矢量， $v\in\mathbb{R}^n,$ 其中v位于帧坐标中箭头的“头部”。如果选择不同的参考系和长度标度，则表示v将改变，但基础自由向量v不变。

换句话说，我们说v是无坐标的 coordinate free；它指的是底层空间中的一个物理量，它不在乎我们如何表示它。然而，v是v的一种表示，它取决于坐标系的选择。

物理空间中的点p也可以表示为向量。给定物理空间的参考系和长度尺度的选择，点p可以表示为从参考系原点到p的向量；其矢量用斜体表示为 $p\in\mathbb{R}^n.$ 这里，和以前一样，物理空间的参考系和长度尺度的不同选择导致物理空间中相同点p的不同表示 $p\in\mathbb{R}^n.$ 。见图3.1。

在本书的其余部分中，将始终假设长度尺度的选择，但我们将处理不同位置和方向的参考系。参考系可以放置在空间中的任何地方，任何参考系都会导致底层空间和其中对象的同等有效的表示。

我们总是假设定义了一个静止的固定框架fixed frame 或空间框架 space frame，表示为{s}。例如，它可能附着在房间的某个角落。

类似地，我们经常假设至少有一个框架连接到一些移动的刚体上，例如在房间里飞行的四旋翼机的机身。这个body frame，表示为{b}，是在任何时刻与车身连接坐标系重合的静止坐标系。

虽然通常将{b}坐标系的原点连接到物体上的某个重要点，例如质心，但这不是必要的。

{b}坐标系的原点甚至不需要在物理体本身上，只要从相对于物体静止的观察者来看，其相对于物体的配置是恒定的即可。

的这本书中的所有坐标系都是静止的，惯性的。当我们提到身体坐标系body frame {b}时，我们指的是一个静止的坐标系，它与固定在（可能移动的）物体上的坐标系瞬间重合。

为了简单起见，我们通常将body frame 称为连接到移动刚体上的坐标系frame。尽管如此，在任何时刻，我们所说的“body frame ”实际上是指与随身体移动的frame瞬间重合的静止frame

It is worth repeating one more time: all frames are stationary

所有参考系都是右旋的，如图3.2所示。绕轴的正旋转被定义为当拇指指向轴时右手手指卷曲的方向（图3.2）。

3.1 Rigid-Body Motions in the Plane

考虑图3.3中的平面体（灰色形状）；它的运动仅限于平面。假设长度标度和固定参考系{s}已被选择，如图所示，单位轴为 $\hat{x}_{s}$ 和 $\hat{y}_{s}$ 。（在整本书中，hat表示单位向量。）类似地，我们将具有单位轴的参考系连接到平面体上。因为这个框架随身体移动，所以它被称为body frame，并表示为{b}。

为了描述平面主体的配置，只需要指定body frame 相对于fixed frame的位置和方向。body-frame o原点p可以用{s}的坐标轴表示

$p=p_x\hat{\mathrm{x}}_\mathrm{s}+p_y\hat{\mathrm{y}}_\mathrm{s}$

描述主体框架{b}相对于固定框架{s}的方向最简单的方法是指定角度θ，如图3.3所示。另一种(诚然不太简单)的方法是指定{b}相对于{s}的单位轴的方向，以形式表示

乍一看，这似乎是一个相当低效的方式来表示body frame的方向。然而，想象一下，如果身体在三维空间中任意移动;一个角θ不足以描述位移参考系的方向。

然而，想象一下，如果身体在三维空间中任意移动;一个角θ不足以描述位移参考系的方向。

我们实际上需要三个角，但目前还不清楚如何定义一个合适的三个角的集合。然而，用{s}坐标轴的系数来表示{b}坐标轴的方向，就像我们上面在平面情况下所做的那样，是直截了当的straightforward。

假设我们同意用{s}来表示所有东西，那么，就像点p可以表示为列向量p∈R2的形式

$p=\left[\begin{array}{c}p_x\\p_y\end{array}\right]$

两个向量 $\hat{x}_{b}$ 和 $\hat{y}_{b}$ 也可以写成列向量，并打包成如下的2 × 2矩阵P:

$P=\begin{bmatrix}\hat{\mathrm{x}}_{\mathrm{b}}&\hat{\mathrm{y}}_{\mathrm{b}}\end{bmatrix}=\left[\begin{array}{cc}\cos\theta&-\sin\theta\\\sin\theta&\cos\theta\end{array}\right]$

矩阵P是旋转矩阵的一个例子。虽然P由四个数字组成，但它们受到三个约束(P的每一列必须是一个单位向量，并且两列必须彼此正交)，剩下的一个自由度由θ参数化。这对(P, p)一起提供了{b}相对于{s}的方向和位置的描述。

现在参考图3.4中的三个框架。重复上面的方法，并将{s}中的{c}表示为(R,r )对：

$r=\left[\begin{array}{c}r_x\\r_y\end{array}\right],\quad R=\left[\begin{array}{cc}\cos\phi&-\sin\phi\\\sin\phi&\cos\phi\end{array}\right]$

我们也可以描述相对于{b}的框架{c}。设q表示从{b}原点到{c}原点的向量，用{b}坐标表示，设Q表示{c}相对于{b}的方向，我们可以将{c}相对于{b}写成(Q, q)对:

$q=\left[\begin{array}{c}q_x\\q_y\end{array}\right],\quad Q=\left[\begin{array}{cc}\cos\psi&-\sin\psi\\\sin\psi&\cos\psi\end{array}\right]$

如果我们知道(Q, q) ({c}相对于{b}的构型)和(P, p) ({b}相对于{s}的构型)，我们可以计算{c}相对于{s}的构型如下:

因此(P, p)不仅表示{s}中{b}的构型;它还可以用于将点或坐标系的表示从{b}坐标转换为{s}坐标。

现在考虑一个刚体，上面有两个坐标系{d}和{c}。框架{d}最初与{s}重合，{c}最初由{s}中的(R, r)描述(图3.5(a))。然后，物体以这样一种方式移动，即{d}移动到{d '}，与{s}中由(P, p)描述的坐标系{b}重合。{c}在这个运动之后结束在哪里? 表示新框架{c '}的配置为(R '， r')，您可以验证它

类似式(3.8)和式(3.9)。

不同之处在于(P, p)与(R, r)在同一坐标系中表示，因此方程不被视为坐标变化，而是作为刚体位移rigid-body displacement (也称为刚体运动 rigid-body motion):

在图3.5(a)中，变换1根据P旋转{c}，变换2在{s}中平移p。

因此，我们看到(P, p)这样的旋转矩阵-向量对可以用于三个目的:

表示{s}中刚体的构型(图3.3);
改变表示矢量或坐标系的参考系(图3.4);
移动矢量或坐标系(图3.5(a))

参考图3.5(b)，注意到图3.5(a)中所示的刚体运动，表示为旋转之后的平移；可以通过简单地将物体围绕固定点s旋转一个角度β来获得。这是螺旋运动 a screw motion 的平面例子。因此，位移可以用三个螺旋坐标screw coordinates (β，s_x, s_y)来参数化，其中(s_x, s_y) =(0,2)表示固定坐标系{s}中点s(即螺杆轴在页面外)的坐标。

另一种表示螺旋运动的方法是将其视为在给定距离内同时跟随角速度和线速度而获得的位移。检查图3.5（b），我们可以看到，以单位角速度（ω=1 rad/s）绕s点旋转意味着{s}坐标系原点处的点最初以每秒两个单位的速度在{s}帧的x正方向上移动，即 $v=(v_{x},v_{y})=(2,0).$ 我们可以把这些包装成一个三维向量、

$\mathcal{S}=(\omega,v_x,v_y)=(1,2,0)$ a representation of the screw axis.

Following this screw axis走角度θ=π/2产生最终的位移

因此，我们可以使用三个坐标

$\mathcal{S}\theta=(\pi/2,\pi,0)$

来表示位移。

这些坐标有一些优点，我们称之为平面刚体位移的指数坐标 exponential coordinates。

为了表示称为twist的角速度和线速度的组合，我们取一个螺旋轴 $\mathcal{S}=(\omega,v_x,v_y)$ ，其中ω=1，并通过乘以某个旋转速度 $\dot{\theta}$ 来缩放它。The twist is $\mathcal{V}=\mathcal{S}\dot{\theta}.$

通过绕螺杆轴线S旋转角度θ获得的净位移等于以速 $\dot{\theta} = \theta$ 绕S旋转单位时间所获得的位移。

所以 $\mathcal{V}=\mathcal{S}\dot{\theta}$ 也可以被认为是一组指数坐标

本章剩余部分的预览：

在本章的其余部分，我们将上述概念推广到三维刚体运动。为此，考虑一个占据三维物理空间的刚体，如图3.6所示。假设已经选择了物理空间的长度尺度，并且已经选择了固定框架{s}和主体框架{b}，如图所示。

在整本书中，所有参考系都是右旋的。设p表示从固定框架原点到身体框架原点的向量。根据固定的帧坐标，p可以表示为

$p=p_1\hat{\mathrm{x}}_s+p_2\hat{\mathrm{y}}_s+p_3\hat{\mathrm{z}}_s.$

body frame 的轴也可以表示为

定义 $p\in\mathbb{R}^{3}{\mathrm{~and~}}R\in\mathbb{R}^{3\times3}{\mathrm{~as}}$

$p=\left[\begin{array}{c}p_1\\p_2\\p_3\end{array}\right],\quad R=\left[\begin{matrix}\hat{\mathrm{x}}_\mathrm{b}&\hat{\mathrm{y}}_\mathrm{b}&\hat{\mathrm{z}}_\mathrm{b}\end{matrix}\right]=\left[\begin{array}{ccc}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{array}\right],\quad(3.16)$

由（R，p）给出的12个参数提供了刚体相对于固定框架的位置和方向的描述

由于刚体的方向有三个自由度，因此R中的九项中只有三个可以独立选择。

只有三参数的旋转的表示由指数坐标提供，其限定了旋转轴线和围绕该轴线旋转的角度。

我们将其他常用的方位表示（三参数欧拉角Euler angles 和滚转-俯仰-偏航角roll–pitch–yaw angles、受一个约束的四个变量：Cayley-Rodrigues参数？？Other representations of a rotation | Rotations (berkeley.edu)

和单位四元数，）保留在附录B中

然后，我们研究了刚体构型的六参数指数坐标，该坐标由刚体的角速度和线速度组成的six-dimensional twist积分而来。

该表示源自Chasles–Mozzi定理，该定理指出，每个刚体位移都可以通过绕固定螺旋轴的有限旋转和平移来获得。

最后，我们讨论了力和力矩。我们没有将这些视为单独的三维量，而是将力矩和力向量合并到一个六维 wrench 中。twist and wrench，以及操纵它们的规则，构成了后续章节运动学和动力学分析的基础。

3.2 Rotations and Angular Velocities

3.2.1 Rotation Matrices

我们之前认为，在旋转矩阵R中的九项中，只有三个可以独立选择。我们首先对R的项表示一组六个显式约束。回想一下，R的三列对应于the body-frame单位轴 $\{\hat{x}_{\mathrm{b}},\hat{y}_{\mathrm{b}},\hat{z}_{\mathrm{b}}\}$ 。因此，必须满足以下条件：

单位范数条件： $\{\hat{x}_{\mathrm{b}},\hat{y}_{\mathrm{b}},\hat{z}_{\mathrm{b}}\}$ 都是单位向量，即
正交性条件：（此处·表示内积） $\hat{\mathrm{x}_b}\cdot\hat{\mathrm{y}_b}=\hat{\mathrm{x}_b}\cdot\hat{\mathrm{z}_b}=\hat{\mathrm{y}_b}\cdot\hat{\mathrm{z}_b}=0$

这六个约束可以更紧凑地表示为矩阵R上的单个约束集：

$R^\mathrm{T}R=I,\quad(3.19)$

其中RT表示R的转置，I表示单位矩阵。

仍然存在这样一个问题，即坐标系是右手的（即， $\hat{\mathrm{x}}_{\mathrm{b}}\times\hat{\mathrm{y}}_{\mathrm{b}}=\hat{\mathrm{z}}_{\mathrm{b}}$ ，其中×表示叉积），而不是左手的（即 $\hat{\mathrm{x}}_{\mathrm{b}}\times\hat{\mathrm{y}}_{\mathrm{b}}=-\hat{\mathrm{z}}_{\mathrm{b}}$ ）；我们上面的六个等式约束没有区分左右坐标系。

我们回顾以下公式来评估3×3矩阵M的行列式：

用a、b和c分别表示M的三列，其行列式由下式给出

$\det M=a^{\mathrm{T}}(b\times c)=c^{\mathrm{T}}(a\times b)=b^{\mathrm{T}}(c\times a).\quad(3.20)$

将R的列替换到此公式中，然后产生约束 $\operatorname{det}R=1.$

请注意，如果坐标系是左旋的，我们将得到det R=−1。

总之，由方程（3.19）表示的六个等式约束意味着det R=±1；施加附加约束det R＝1意味着只允许右手坐标系。约束det R=1不改变参数化R所需的独立连续变量的数量。

3×3旋转矩阵的集合形成了特殊的正交群 $SO(3)$ ，我们现在正式定义它。

Definition 3.1.：

特殊正交群SO（3），也称为旋转矩阵群，是满足

$R^TR=I$
det R=1

2x2旋转矩阵的集合是SO（3）的子群，称为SO（2）

Definition 3.2.

特殊正交群SO（2）是满足

$R^TR=I$
det R=1

由定义可知，每个R∈SO（2）都可以写成

$R=\left[\begin{array}{rr}r_{11}&r_{12}\\r_{21}&r_{22}\end{array}\right]=\left[\begin{array}{rr}\cos\theta&-\sin\theta\\\sin\theta&\cos\theta\end{array}\right]$

其中θ∈[0，2π）。SO（2）的元素表示平面取向，SO（3）的元素代表空间取向。

3.2.1.1 Properties of Rotation Matrices 旋转矩阵的性质

旋转矩阵 SO(2) 和 SO(3) 的集合称为群 groups，因为它们满足mathematical group 所需的属性。（更具体地说，SO(n) 组也称为矩阵李群 matrix Lie groups（其中“Lie”发音为“Lee”），因为组的元素形成一个可微流形 differentiable manifold。）

具体来说，一个群由一组元素和对两个元素（SO(n) 的矩阵乘法）的操作组成 operation

对于群中的所有 A、B，满足以下属性：

closure: AB is also in the group
associativity: (AB)C = A(BC).
identity element existence: There exists an element I in the group (the identity matrix for SO(n)) such that AI = IA = A.
inverse element existence: There exists an element A−1 in the group such that AA−1 = A−1A = I.

以下使用单位矩阵I是旋转矩阵的平凡例子这一事实给出了这些性质的证明。

Proposition 3.3.

旋转矩阵R∈SO（3）的逆也是旋转矩阵，它等于R的转置，即 $R^{-1}=R^T$

证明：The condition $R^TR=I$ implies that $R^{-1}=R^T$ and $RR^T=I$ Since $det R^T = det R = 1$ , $R^T$ is also a rotation matrix.

Proposition 3.4.

两个旋转矩阵的乘积是一个旋转矩阵

Proposition 3.5.

旋转矩阵的乘法是结合的 $(R_1R_2)R_3=R_1(R_2R_3)$ ，但通常不是交换的， $R_1R_2\neq R_2R_1$ 。对于SO（2）中旋转矩阵的特殊情况，旋转交换。平面旋转的交换性来自于直接计算。

Proposition 3.6.

另一个重要性质是旋转矩阵对向量的作用（例如，旋转一个向量）不会改变向量的长度

对于任何向量x∈R3和R∈SO（3），向量y=Rx与x的长度相同。

3.2.1.2 Uses of Rotation Matrices

类似于第3.1节方程式3.10和（3.11）之后的讨论，旋转矩阵R有三个主要用途：

to represent an orientation;、
to change the reference frame in which a vector or a frame is represented;
to rotate a vector or a frame.

在第一种使用中，R被认为表示一个坐标系；在第二种和第三种用法中，R被认为是作用于向量或坐标系（改变其参考坐标系或旋转）的算子operator。

为了说明这些用途，请参阅图3.7，图中显示了三个不同的坐标系——{a}、{b}和{c}——表示相同的空间。这些zbx被选择为具有相同的原点，因为我们只是表示方向，但是，为了使轴清晰，图中显示了绘制三次的相同空间。

还显示了空间中的点p。未显示的是与｛a｝对齐的固定空间zbx｛s｝。三个zbx相对于｛s｝的方向可以写成

$R_{a}=\left[\begin{array}{ccc}1&0&0\\0&1&0\\0&0&1\end{array}\right],R_{b}=\left[\begin{array}{ccc}0&-1&0\\1&0&0\\0&0&1\end{array}\right],R_{c}=\left[\begin{array}{ccc}0&-1&0\\0&0&-1\\1&0&0\end{array}\right]$

并且点p在这些帧中的位置为：

$p_a=\left[\begin{array}{c}1\\1\\0\end{array}\right],\quad p_b=\left[\begin{array}{c}1\\-1\\0\end{array}\right],\quad p_c=\left[\begin{array}{c}0\\-1\\-1\end{array}\right]$

｛b｝是通过将｛a｝绕着za轴旋转90而获得的◦, 并且{c}是通过将{b}绕着yb轴旋转−90而获得的。

表示方向

当我们写Rc时，我们隐含地指帧{c}相对于固定帧{s}的方向。我们可以通过将其写成Rsc来更明确地说明这一点：我们表示第二下标的帧{c}相对于第一下标的帧｛s}。这种表示法允许我们相对于另一个不是{s}的帧来表示一个帧；例如，Rbc是{c}相对于{b}的取向。如果所涉及的帧没有混淆的可能性，我们可以简单地写R。

检查图3.7，我们看到

$R_{ac}=\left[\begin{array}{ccc}0&-1&0\\0&0&-1\\1&0&0\end{array}\right],\quad R_{ca}=\left[\begin{array}{ccc}0&0&1\\-1&0&0\\0&-1&0\end{array}\right]$

实际上对于任意两个帧{d}和{e}有

$R_{de}=R_{ed}^{-1}=R_{ed}^{\mathrm{T}}.$

更改参照系 Changing the reference frame

旋转矩阵Rab表示{a}中{b}的方向，Rbc表示{b}中{c}的方向。一个简单的计算表明，{a}中{c}的方向可以计算为 $R_{ac}=R_{ab}R_{bc}.$

在前面的方程中，Rbc可以被视为{c}方向的表示，而Rab可以看作是将参照系从{b}变为{a}的数学算子。

下标消去规则帮助我们记住这个性质。当两个旋转矩阵相乘时，如果第一个矩阵的第二个下标与第二个矩阵的第一个下标匹配，则两个下标“取消”，实现参照系的变化:

旋转矩阵只是三个单位向量的集合，所以矢量的参考系也可以通过旋转矩阵来改变

$R_{ab}p_b = p_a$

旋转矢量或坐标系 Rotating a vector or a frame

图3.8显示了一个帧{c}最初与{s}以轴{ {x，y，z}对齐。

如果我们将帧{c}绕单位轴ω旋转量θ，则新帧{c′}（浅灰色）具有坐标轴{ x′，y′，z′}。旋转矩阵R=Rsc′表示{c′}相对于{s}的方向，但我们可以将其视为表示从{s}到{c′｝的旋转运算。

强调我们把R看作一个旋转算子，而不是一个方向，我们可以写\

$R=\mathrm{Rot}(\hat{\omega},\theta),$

意味着将单位矩阵表示的方向旋转到R表示的方向的操作。关于坐标系轴的旋转操作示例如下

任意R∈SO(3)都可以由单位矩阵绕某个θ ω旋转得到。还应注意， ${\mathrm{Rot}}({\hat{\omega}},\theta) = {\mathrm{Rot}}({-\hat{\omega}},-\theta)$

现在，假设Rsb表示{b}相对于{s}的某个{b}，我们想将{b}绕单位轴 $\hat{\omega}$ 旋转θ，即 ${\mathrm{Rot}}({\hat{\omega}},\theta)$ 。为了明确我们的意思，我们必须指定旋转的轴 $\hat{\omega}$ 是用{s}坐标表示还是用{b}坐标表示。根据我们的选择，除非{s}和{b}帧对齐，否则相同的数值 $\hat{\omega}$ (因此相同的数值R)在底层空间中对应不同的旋转轴。

设{b '}为绕着ωs = ω旋转θ 后的新坐标系(旋转轴ω被认为在固定坐标系 fixed frame {s}中)，{b "}是绕着ωb = ω旋转θ 新的坐标系(旋转轴ω被认为是在body frame{b}中)

这些新帧的表示可以计算为

$\begin{aligned}R_{sb\prime}&=\text{rotate.by.}R\_\text{in.}\{\text{s}\}\_\text{frame }(R_{sb})=RR_{sb}\quad&(3.23)\\R_{sb''}&=\text{rotate.by.}R\_\text{in.}\{\text{b}\}\_\text{frame }(R_{sb})=R_{sb}R.\quad&(3.24)\end{aligned}$

换句话说，左乘 premultiplying 矩阵R = Rot(ω， θ)得到一个绕轴ω的旋转，被认为是在fixed frame中

后乘(右乘) postmultiplying R得到一个绕ω的旋转，被认为是在body frame中。在{s}帧和{b}帧中R的旋转如图3.9所示。

要旋转向量v，注意只涉及到一个坐标系，即表达v向量的坐标系。因此，ω必须被解释为在这个坐标系中。在同一坐标系中，旋转后的向量v '是 $v^{\prime}=Rv.$

3.2.2 Angular Velocities

参照图3.10(a)，假设一个单位轴为 $\{\hat{\mathrm{x}},\hat{\mathrm{y}},\hat{\mathrm{z}}\}$ 的坐标系附着在一个旋转体上。

我们来确定这些单位轴的时间导数。从 $\dot{\hat{x}}$ 开始，首先注意到 ${\hat{x}}$ 是单位长度;只有x的方向可以随时间变化(y和z也是如此)。

如果我们在时间t和t+∆t检查body frame，则frame方向的变化可以描述为角度∆θ围绕穿过原点的某个单位轴 $\hat{\omega}$ 的旋转。该轴 $\hat{\omega}$ 是无坐标的coordinate-free；它还没有在任何特定的参考系中被表示。

在极限情况下，∆t趋近于零时，∆θ/∆t的比值即为旋转速率the rate of rotation $\dot{\theta}$ ,同样地，可将 $\hat{\omega}$ 视为瞬时旋转轴。实际上，可以将 $\hat{\omega}$ 和 $\dot{\theta}$ 结合起来定义角速度angular velocity w如下:

$\mathrm{w}=\hat{\mathrm{w}}\dot{\theta}.(3.25)$

参考图3.10(b)，很明显

$\begin{array}{rcl}\dot{\hat{x}}&=&\mathrm{w\times\hat{x},}(3.26)\\\dot{\hat{y}}&=&\mathrm{w\times\hat{y},}(3.27)\\\dot{\hat{z}}&=&\mathrm{w\times\hat{z}.}(3.28)\end{array}$

为用坐标表示这些方程，我们必须选择一个参考系来表示w。我们可以选择任何参考系，但两种自然的选择是固定参考系fixed frame{s}和主体参考系body frame{b}。

让我们从固定坐标系{s}开始。设R(t)为描述物体坐标系在时刻t相对于固定坐标系的方向的旋转矩阵;̇ $\dot{R(t)}$ 是它的时间变化率。R(t)的第一列，记为 $r_1(t)$ ，在固定坐标系中描述了 $\hat{x}$ 轴;同样， $r_2(t)$ 和 $r_3(t)$ 分别描述固定坐标系下的 $\hat{y}$ 和 $\hat{z}$ 轴。在特定时刻t，设 ${{\omega}}_{s}\in{\mathbb{R}}^{3}$ 为固定坐标系下的角速度w。则式(3.26)-式(3.28)可在定坐标系下表示为：

$\dot{r}_i=\omega_s\times r_i,\quad i=1,2,3.$

这三个方程可以重新排列成以下单个3 × 3矩阵方程:

$\dot R=[\omega_s\times r_1\ \omega_s\times r_2\ \omega_s\times r_3]=\omega_s\times R.\quad\quad(3.29)$

为了消除式(3.29)右边的叉乘，我们引入一些新的符号，将ωs × R改写为 $[\omega_s]R$

式中[ωs]是ωs∈R3的3 × 3偏对称矩阵skew-symmetric matrix表示:

Definition 3.7.

给定向量 $x=[x_1\mathrm{~}x_2\mathrm{~}x_3]^{\mathrm{T}}\in\mathbb{R}^3$ ，定义

$[x]=\left[\begin{array}{ccc}0&-x_3&x_2\\x_3&0&-x_1\\-x_2&x_1&0\end{array}\right].\quad(3.30)$

矩阵[x]是x的3 × 3偏对称矩阵表示;

$[x]=-[x]^{\mathrm{T}}.$

所有3 × 3实偏对称矩阵的集合称为 $so(3)$ （斜对称矩阵的集合 $so(3)$ 称为李群 $SO(3)$ 的李代数 Lie algebra。）

下面是关于旋转和偏对称矩阵的一个有用的性质。

Proposition 3.8.

给定任意ω∈R3且R∈SO(3)，下列条件总是成立:

$R[\omega]R^\mathrm{T}=[R\omega].\quad\quad\quad\quad\quad(3.31)$

证明：设 $r_i^T$ 是R的第i行(把之前公式里的r_i变成转置)，我们有

其中第二行用的是3 × 3矩阵的行列式，即，如果M是一个列为{a, b, c}的3 × 3矩阵，则

$\operatorname*{det}M=a^{\mathrm{T}}(b\times c)=c^{\mathrm{T}}(a\times b)=b^{\mathrm{T}}(c\times a)$

使用斜对称表示法，我们可以将式(3.29)重写为

$[\omega_s]R=\dot{R}.\quad(3.33)$

我们可以将方程(3.33)两边同乘以 $R^{-1}$ 得到

$[\omega_s]=\dot{R}R^{-1}.$

现在让 $\omega_b$ 用body-frame坐标表示为w，为了了解如何从ωs得到ωb，反之亦然，我们将R显式地写成 $R_{sb}$ 。那么ωs和ωb是相同角速度w的两个不同的矢量表示，根据我们的下标消去规则有

$\omega_s=R_{sb} \omega_b$

因此

$\omega_b=R_{sb}^{-1}\omega_s=R^{-1}\omega_s=R^{\text{T}}\omega_s.\quad(3.35)$

现在让我们用斜对称矩阵的形式来表达这个关系：

总之，有两个方程将R, $\dot{R}$ 与角速度w联系起来：

Proposition 3.9.

设R（t）表示从fixed frame看rotating frame的方向。用w表示rotating frame的角速度:

其中，ωs∈R3是w的fixed-frame向量表示,其中 $[\omega_s]\in so(3)$ 是w的3 × 3矩阵表示

其中，ωb∈R3是w的body-frame向量表示， $[\omega_b]\in so(3)$ 是它的3 × 3矩阵表示

重要的是要注意ωb不是相对于运动坐标系的角速度。相反，ωb是相对于静止stationary坐标系{b}的角速度，它与附着在运动物体上的坐标系瞬间重合。

同样重要的是要注意，fixed-frame角速度ωs并不取决于body frame的选择。同样，body-frame角速度ωb也不依赖于fixed frame的选择。

虽然式(3.37)和式(3.38)似乎依赖于两个坐标系(因为R和R分别依赖于{s}和{b})，但乘积 $\dot{R}R^{-1}$ 与{b}无关，而乘积 $R^{-1}\dot{R}$ 与{s}无关。

最后，在任意坐标系{d}中表示的角速度可以在另一个坐标系{c}中表示，如果我们知道从{c}到{d}的旋转，使用我们现在熟悉的下标消去规则:

$\omega_{c}=R_{cd}\omega_{d}.$

3.2.3 Exponential Coordinate Representation of Rotation 旋转的指数坐标表示

我们现在介绍旋转的三参数表示，旋转的指数坐标 exponential coordinates for rotation。

指数坐标根据旋转轴(由单位向量 $\hat{\omega}$ 表示)和绕该轴旋转θ的角度参数化旋转矩阵;然后向量 $\hat{\omega}\theta\in\mathbb{R}^{3}$ 作为旋转的三参数指数坐标表示。分开写 $\hat{\omega}$ 和 θ 是旋转的轴角表示 axis-angle representation。

旋转矩阵R的指数坐标表示 $\hat{\omega}\theta\in\mathbb{R}^{3}$ 可以等价地解释为:

轴 $\hat{\omega}$ ，旋转角度为θ，这样，如果最初与{s}重合的坐标系绕着轴 $\hat{\omega}$ 旋转θ，则其相对于{s}的最终方向将由R表示；
以{s}表示的角速度 $\hat{\omega}\theta$ ，这样，如果最初与{s}重合的坐标系在一个时间单位内跟随着 $\hat{\omega}\theta$ （即，在该时间间隔上积分 $\hat{\omega}\theta$ ），则其最终方向将由R表示
以{s}表示的角速度 $\hat{\omega}$ ，这样，如果一个帧最初与{s}重合，在θ个时间单位内跟随着 $\hat{\omega}$ （即，在这个时间间隔上对 $\hat{\omega}$ 进行积分），则其最终方向将由R表示。

后两种观点建议我们在设置线性微分方程in the setting of linear differential equations 时考虑指数坐标。下面我们简要回顾一下线性微分方程理论的一些关键结果。

3.2.3.1 Essential Results from Linear Differential Equations Theory 线性微分方程理论的基本结果

让我们从简单的标量线性微分方程开始

$\dot{x}(t)=ax(t),\quad(3.39)$

其中 $x(t)\in\mathbb{R},a\in\mathbb{R}$ ，a是常量，并且给出了初始条件 $x(0)=x_0$ 。方程（3.39）有解:

$x(t)=e^{at}x_0.$

记住指数函数的级数展开也是有用的：

$e^{at}=1+at+\frac{(at)^{2}}{2!}+\frac{(at)^{3}}{3!}+\cdots.$

现在考虑向量线性微分方程

$\dot{x}(t)=Ax(t),\quad(3.40)$

其中 $x(t)\in\mathbb{R}^n,A\in\mathbb{R}^{n\times n}$ 是常数,并且给出了初始条件 $x(0)=x_0$ 。从上面的标量结果可以推测出以下形式的解：

$x(t)=e^{At}x_0\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad(3.41)$

再次模拟标量情况，我们将矩阵指数matrix exponential $e^{At}$ 定义为

$e^{At}=I+At+\frac{(At)^2}{2!}+\frac{(At)^3}{3!}+\cdots\quad(3.42)$

要解决的第一个问题是，这个级数在什么条件下收敛，使得矩阵指数得到了很好的定义。可以证明，如果A是常数和有限的，那么这个级数series总是保证收敛converge到有限极限finite limit；

第二个问题是，使用方程（3.42）的方程（3.41）是否确实是方程（3.40）的解。取（3.41）的时间导数:

这证明了（3.41）确实是一个解。这是一个唯一的解，这源于线性常微分方程的基本存在性和唯一性结果，我们在这里引用它而没有证明。

对于任意方阵 square matrices A和B， $AB\neq BA$ ，总有

$Ae^{At}=e^{At}A\quad(3.44)$

因此，在方程（3.43）的第四行中，A也可以被向右分解，即

$\dot{x}(t)=e^{At}Ax_{0}.$ 、

虽然矩阵指数 $e^{At}$ 被定义为一个无限级数，但闭式表达式closed form expressions 通常是可用的。例如，对于一些D∈Rn×n和可逆的P∈Rn×n，如果A可以表示为 $A=PDP^{-1}$ ，那么

此外，如果D是对角的，即D＝diag{d1，d2，…，dn}，则其矩阵指数特别容易评估：

$e^{Dt}=\left[\begin{array}{ccccc}e^{d_1t}&0&\cdots&0\\0&e^{d_2t}&\cdots&0\\\vdots&\vdots&\ddots&\vdots\\0&0&\cdots&e^{d_nt}\end{array}\right].\quad(3.46)$

我们在下面的命题中总结了上述结果。

Proposition 3.10.

矩阵指数 $e^{At}$ 进一步满足以下性质：

3.2.3.2 Exponential Coordinates of Rotations 旋转的指数坐标

旋转的指数坐标可以等价地看作:

一个旋转的单位轴 $\hat{\omega}$ ( $\hat{\omega}$ ∈R3， $\left \| \hat{\omega} \right \|=1$ )和一个绕轴θ∈R的旋转角度
二者相乘得到的3维向量，ωθ∈R3、

当我们在下一章中表示机器人关节的运动时，第一种的优点是将关节轴的描述与关于轴的运动θ分开。

参照图3.11，设三维矢量p(0)绕 $\hat{\omega}$ 旋转θ至p(θ);这里我们假设所有的量都是以固定坐标系坐标表示的

这种旋转可以通过想象p(0)从t = 0到t = θ以1 rad/s的恒定速率旋转(因为ω具有单位大小)来实现。设p(t)表示由向量尖端跟踪的路径。p(t)的速度，记为 $\dot{p}$ :

$\dot{p}=\hat{\omega}\times p.\quad(3.49)$

微分方程(3.49)可表示为(见式(3.30))

$\dot{p}=[\hat{\omega}]p\quad(3.50)$

初始条件为p(0)。这是一个线性微分方程，形式为 $\dot{x}=Ax$ ,我们之前学过;它的解是 $p(t)=e^{[\hat{\omega}]t}p(0).$

因为t和θ可以互换，所以上面的方程也可以写成

$p(\theta)=e^{[\hat{\omega}]\theta}p(0).$

现在我们把矩阵指数 $e^{[\hat{\omega}]\theta}$ 展开成级数形式。通过简单的计算，我们可以得到 $[\hat{\omega}]^3=-[\hat{\omega}],$ 因此我们可以将 $[\hat{\omega}]^3$ 替换为 $-[\hat{\omega}]$ ，将 $[\hat{\omega}]^4$ 替换为 $-[\hat{\omega}]^2$ ，将 $[\hat{\omega}]^5$ 替换为 $[\hat{\omega}]$ ，以此类推，得到

现在回想一下sin θ和cos θ的级数展开:

$\begin{aligned}\sin\theta\quad&=\quad\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}-\cdots\\\cos\theta\quad&=\quad1-\frac{\theta^2}{2!}+\frac{\theta^4}{4!}-\cdots\end{aligned}$

指数 $e^{[\hat{\omega}]\theta}$ 因此简化为:

Proposition 3.11.

给定一个向量 $\hat{\omega}\theta\in\mathbb{R}^{3},$ 使得θ是任意标量，且 $\hat{\omega} \in\mathbb{R}^{3},$ 是单位向量,则 $[\hat{\omega}]\theta=[\hat{\omega}\theta]\in so(3)$ 的矩阵指数为

$\operatorname{Rot}(\hat{\omega},\theta)=e^{[\hat{\omega}]\theta}=I+\sin\theta\left[\hat{\omega}\right]+(1-\cos\theta)[\hat{\omega}]^2\in SO(3).\quad(3.51)$

式(3.51)也称为Rodrigues旋转公式 Rodrigues' formula。

我们已经展示了如何使从旋转轴 $\hat{\omega}$ 和角度θ中用矩阵指数构造旋转矩阵.

此外，量 $e^{[\hat{\omega}]\theta}$ 表示将p∈R3绕固定框架fixed-frame轴 $\hat{\omega}$ 旋转角度θ的效果。

类似地，考虑到旋转矩阵R由三个列向量组成，旋转矩阵 $R^{\prime}=e^{[\hat{\omega}]\theta}R=\operatorname{Rot}(\hat{\omega},\theta)R$ 是在固定坐标系中将R绕轴 $\hat{\omega}$ 旋转θ所获得的方向。

颠倒矩阵乘法的顺序， $R^{\prime\prime}=Re^{[\hat{\omega}]\theta}=R\operatorname{Rot}(\hat{\omega},\theta)$ 是通过在body frame中将R绕 $\hat{\omega}$ 旋转θ而获得的方向。

Example 3.12.

图3.12中的帧{b}是由与固定帧{s}对齐的初始方向绕单位轴 $\hat{\omega}_1 = (0, 0.866, 0.5)$ 旋转θ1 = 30◦= 0.524 rad. 得到的。{b}的旋转矩阵表示可以计算为

坐标系{b}的方向可以用R表示，也可以用单位轴 $\hat{\omega}_1 = (0, 0.866, 0.5)$ 和角θ1 = 0.524 rad即指数坐标 $\hat{\omega}_1\theta_1=(0,0.453,0.262).$

然后如果{b}绕固定框架轴 $\hat{\omega}_{2}\neq\hat{\omega}_{1}$ 旋转θ2 ，即:

$R^{\prime}=e^{[\hat{\omega}_2]\theta_2}R$

然后坐标系最终到达的位置与{b}绕body frame中轴 $\hat{\omega}_2$ 旋转θ2的位置不同，即:

$R^{\prime\prime}=Re^{[\hat{\omega}_2]\theta_2}\neq R^{\prime}=e^{[\hat{\omega}_2]\theta_2}R.$

我们的下一个任务是证明对于任何旋转矩阵R∈SO(3)，我们总能找到一个单位向量 $\hat{\omega}$ 和标量θ使得 $e^{[\hat{\omega}]\theta}$ 。

3.2.3.3 Matrix Logarithm of Rotations 旋转的矩阵对数

若 $\hat{\omega}\theta\in\mathbb{R}^{3},$ 表示旋转矩阵R的指数坐标，则偏对称矩阵 $[\hat{\omega}\theta]=[\hat{\omega}]\theta$ 是旋转矩阵R的矩阵对数matrix logarithm（我们使用“矩阵对数"the matrix logarithm”这个术语来指代一个特定的矩阵，即R的对数，以及计算这个特定矩阵的算法。同样，一个矩阵R可以有多个矩阵对数(正如sin−1(0)有解0、π、2π等)，我们通常称之为“矩阵对数”的即矩阵对数算法返回的唯一解。）

矩阵对数是矩阵指数的颠倒 inverse。

就像矩阵指数“积分”角速度的矩阵表示 $[\hat{\omega}]\theta\in so(3)$ 一秒以给出方向 $R \in SO(3)$ 一样，矩阵对数“微分” $R \in SO(3)$ 以找到恒定角速度的矩阵表示 $[\hat{\omega}]\theta\in so(3)$ 即如果积分一秒钟，则将帧从I旋转到R。

换句话说： $\begin{array}{rccc}\exp:&[\hat{\omega}]\theta\in so(3)&\to&R\in SO(3),\\\log:&R\in SO(3)&\to&[\hat{\omega}]\theta\in so(3).\end{array}$

为了推导矩阵对数，让我们展开方程（3.51）中 $e^{[\hat{\omega}]\theta}$ 的每个条目 $\operatorname{Rot}(\hat{\omega},\theta)=e^{[\hat{\omega}]\theta}=I+\sin\theta\left[\hat{\omega}\right]+(1-\cos\theta)[\hat{\omega}]^2\in SO(3).\quad(3.51)$

$\left[\begin{array}{ccc}c_\theta+\hat{\omega}_1^2(1-c_\theta)&\hat{\omega}_1\hat{\omega}_2(1-c_\theta)-\hat{\omega}_3s_\theta&\hat{\omega}_1\hat{\omega}_3(1-c_\theta)+\hat{\omega}_2\hat{\omega}_\theta\\\hat{\omega}_1\hat{\omega}_2(1-c_\theta)+\hat{\omega}_3s_\theta&c_\theta+\hat{\omega}_2^2(1-c_\theta)&\hat{\omega}_2\hat{\omega}_3(1-c_\theta)-\hat{\omega}_1s_\theta\\\hat{\omega}_1\hat{\omega}_3(1-c_\theta)-\hat{\omega}_2\mathrm{s}_\theta&\hat{\omega}_2\hat{\omega}_3(1-c_\theta)+\hat{\omega}_1\mathrm{s}_\theta&c_\theta+\hat{\omega}_3^2(1-c_\theta)\end{array}\right],\\(3.52)$

其中 $\hat{\omega}=(\hat{\omega}_{1},\hat{\omega}_{2},\hat{\omega}_{3})$ 令上述矩阵等于给定的R∈SO(3)，两边同时减去转置，可得:

$r_{32}-r_{23}=2\hat{\omega}_1sin\theta$

$r_{13}-r_{31}=2\hat{\omega}_2sin\theta$

$r_{21}-r_{12}=2\hat{\omega}_3sin\theta$

因此，只要 $sin\theta\neq0$ (或者，等价地，θ不是π的整数倍)，我们可以写

$\begin{gathered} \hat{\omega}_{1} =\quad\frac1{2\sin\theta}(r_{32}-r_{23}), \\ \hat{\omega}_{2} =\quad\frac1{2\sin\theta}(r_{13}-r_{31}), \\ \hat{\omega}_{3} =\quad\frac1{2\sin\theta}(r_{21}-r_{12}). \end{gathered}$

上述方程也可以用偏对称矩阵形式表示为

$[\hat{\omega}]=\left[\begin{array}{ccc}0&-\hat{\omega}_3&\hat{\omega}_2\\\hat{\omega}_3&0&-\hat{\omega}_1\\-\hat{\omega}_2&\hat{\omega}_1&0\end{array}\right]=\frac{1}{2\sin\theta}\left(R-R^\mathrm{T}\right).\quad(3.53)$

回想一下， $\hat{\omega}$ 表示给定R的旋转轴。由于分母中有sinθ项，如果θ是π的整数倍，则 $[\hat{\omega}]$ 不能很好地定义。（这样的奇异性对于任何三参数旋转表示都是不可避免的。欧拉角和滚转-俯仰-偏航角具有相似的奇异性。）现在让我们假设 $sin\theta\neq0$ 并求出θ的表达式。设R =(3.52)，取等式两边的迹(回想一下，矩阵的迹是其对角线元素的和)

$\operatorname{tr}R=r_{11}+r_{22}+r_{33}=1+2\cos\theta.\quad(3.54)$

由于 $\hat{\omega}_1^2+\hat{\omega}_2^2+\hat{\omega}_3^2=1.$ ，故上式成立。对于任意满足1 + 2 cos θ = trR且θ不是π的整数倍的θ， R可表示为具有式(3.53)所示 $[\hat{\omega}]$ 的指数 $e^{[\hat{\omega}]\theta}$ 。

现在让我们回到θ = kπ的情况，其中k是一个整数。当k是一个偶数时，不管 $\hat{\omega}$ 如何，我们都旋转到R = I原位置，所以向量 $\hat{\omega}$ 是没有定义的。

当k是一个奇数(对应于θ =±π，±3π，…，这反过来意味着tr R = - 1)，指数公式(3.51)化简为

$R=e^{[\hat{\omega}]\pi}=I+2[\hat{\omega}]^2.\quad(3.55)$

方程（3.55）的三个对角线项可以给出

$\hat{\omega}_i=\pm\sqrt{\frac{r_{ii}+1}{2}},\quad i=1,2,3.\quad(3.56)$

非对角线项导致以下三个等式：

$\begin{aligned} &2\hat{\omega}_{1}\hat{\omega}_{2} \begin{array}{cc}=&r_{12},\end{array} \\ &2\hat{\omega}_{2}\hat{\omega}_{3} =\quad r_{23}, \\ &2\hat{\omega}_{1}\hat{\omega}_{3} \begin{array}{cc}=&r_{13},\end{array} \end{aligned} \quad(3.57)$

从方程（3.55）中，我们还知道R必须是对称的，方程（3.56）和（3.57）可能都是获得 $\hat{\omega}$ 的解所必需的。一旦找到了这样的解，则R= $e^{[\hat{\omega}]\theta}$ ，其中θ=±π，±3π，...。

从上面可以看出，θ的解以2π的间隔存在。如果我们把θ限制在区间[0，π]，那么下面的算法可以用来计算旋转矩阵R∈SO（3）的矩阵对数

Algorithm:

给定R∈SO(3)，求一个θ∈[0，π]和一个单位旋转轴 $\hat{\omega}$ ∈R3， $\left \| \hat{\omega} \right \|=1$ ，使得 $e^{[\hat{\omega}]\theta}$ = R。向量 $\hat{\omega}\theta\in\mathbb{R}^{3},$ 是R的指数坐标，偏对称矩阵 $[\hat{\omega}]\theta\in so(3)$ 是R的矩阵对数。

若 $R=I$ ，则θ = 0， $\hat{\omega}$ 无定义。
如果tr R =−1，则θ = π。将 $\hat{\omega}$ 设置为以下三个向量中的任何一个，这是可行的解决方案：
否则 $\theta=\cos^{-1}\left(\frac12(\operatorname{tr}R-1)\right)\in[0,\pi)$ 且 $[\hat{\omega}]=\frac{1}{2\sin\theta}(R-R^{\mathrm{T}}).\quad (3.61)$

由于每个R∈SO（3）满足算法中三种情况中的一种，因此对于每个R，存在一个矩阵对数 $[\hat{\omega}]\theta\in so(3)$ ，因此存在一组指数坐标 $\hat{\omega}\theta\in\mathbb{R}^{3},$ 。

因为矩阵对数计算的指数坐标为 $\hat{\omega}\theta\in\mathbb{R}^{3},$ 满足 $\left \| \hat{\omega}\theta \right \|\leq \pi$ ，所以我们可以把旋转群SO(3)想象成半径为π的实心球(见图3.13):

在这个实心球中给定一个点r∈R3，设 $\hat{\omega}=r/\|r\|$ 为原点到点r方向上的单位轴，设θ =‖r‖为原点到r的距离，则 $r=\hat{\omega}\theta$ 。对应于r的旋转矩阵可以看作是绕着轴 $\hat{\omega}$ 转一个角度θ的旋转。

对于任R∈SO(3)使得 $trR\neq-1$ ，则实心球内部存在一个唯一的r使得 $e^{[r]}=R$ 。当 $trR=-1$ 时， $logR$ 由实心球表面上的两个对映点antipodal points(相反的点)给出。也就是说，如果存在r，使得 $e^{[r]}=R$ 且 $\left \| r \right \|=\pi$ ，则 $e^{[-r]}=R$ 也成立;r和- r都对应相同的旋转R。

3.3 Rigid-Body Motions and Twists 刚体运动和Twists

在本节中，我们推导了刚体配置和速度的表示，与第3.2节中旋转和角速度的表示类似。特别地，齐次变换矩阵T类似于旋转矩阵R；螺杆轴S类似于旋转轴 $\hat{\omega}$ ；twist V可以表示为 ${\mathcal{S}}{\dot{\theta}}$ ，并且类似于角速度 $\omega=\hat{\omega}\dot{\theta};$ ；刚体运动的指数坐标 ${\mathcal{S}}\theta\in\mathbb{R}^{6}$ 类似于旋转的指数坐标 $\hat{\omega}\theta\in\mathbb{R}^{3}$ 。

3.3.1 Homogeneous Transformation Matrices 齐次变换矩阵

我们现在考虑刚体的组合方向和位置的表示。一个自然的选择是使用旋转矩阵R∈SO（3）来表示固定框架{s}中body frame {b}的方向，并使用向量p∈R3来表示{s}中{b}的原点。我们将它们打包成一个单独的矩阵，如下所示。

Definition 3.13.

特殊欧几里得群 special Euclidean group SE(3)，也称为刚体运动群或 $\mathbb{R}^3$ 中的齐次变换矩阵 homogeneous transformation matrices，是所有4 × 4实矩阵T的集合，其形式为:

$T=\left[\begin{array}{ccc}R&p\\0&1\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],\quad(3.62)$

R∈SO（3），p∈R3是列向量。

元素 $T\in SE(3)$ 有时表示为（R，p）。在本节中，我们将建立SE（3）的一些基本性质，并解释为什么我们将R和p封装成这种矩阵形式。

到目前为止，我们遇到的许多机器人机构都是平面的。考虑到平面刚体运动，我们做出以下定义：

Definition 3.14.

特殊欧几里得群SE（2）是形式如下的所有3×3实矩阵T的集合：

$T=\left[\begin{array}{cc}R&p\\0&1\end{array}\right],\quad(3.63)$

其中 R ∈ SO(2), p ∈ R2 和 0 表示两个零的行向量。

矩阵 T ∈ 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]$

θ ∈ [0, 2π)

3.3.1.1 Properties of Transformation Matrices 变换矩阵的性质

我们现在列出变换矩阵的一些基本属性，可以通过计算证明。首先，单位阵I是变换矩阵的一个简单例子。前三个性质证实了SE（3）是一个群。

Proposition 3.15.

变换矩阵T∈SE（3）的逆也是变换矩阵，它具有以下形式：

$T^{-1}=\left[\begin{array}{cc}R&p\\0&1\end{array}\right]^{-1}=\left[\begin{array}{cc}R^\mathrm{T}&-R^\mathrm{T}p\\0&1\end{array}\right].\quad(3.64)$

Proposition 3.16.

两个变换矩阵的乘积也是一个变换矩阵。

Proposition 3.17.

变换矩阵的乘法是结合的，因此 $(T_1T_2)T_3=T_1(T_2T_3)$

但通常不可交换： $T_1T_2\neq T_2T_1$

在陈述下一个命题之前，我们注意到，就像在第3.1节中一样，计算Rx+p通常是有用的，其中x∈R3，（R，p）表示T。如果我们在x上附加一个“1”，使其成为四维向量，则此计算可以作为单个矩阵乘法执行：

$T\left[\begin{array}{c}x\\1\end{array}\right]=\left[\begin{array}{cc}R&p\\0&1\end{array}\right]\left[\begin{array}{c}x\\1\end{array}\right]=\left[\begin{array}{c}Rx+p\\1\end{array}\right].\quad(3.65)$

向量 $[x^T 1]^T$ 是x在齐次坐标 homogeneous coordinates中的表示，因此，T∈SE(3)称为齐次变换 homogenous transformation.。由于符号的滥用，我们把Tx写成Rx + p。

Proposition 3.18.

给定 $T=(R,p)\in SE(3)$ 和 $x,y\in\mathbb{R}^3$ ，有：

$\|Tx-Ty\|=\|x-y\|,$ 其中‖·‖为 $\mathbb{R}^3$ 中的标准欧氏范数，即 $\|x\|={\sqrt{x^{\mathrm{T}}x}}.$
$\langle Tx-Tz,Ty-Tz\rangle=\langle x-z,y-z\rangle$ ，对于所有z∈R3，其中<·，·>表示R3中的标准欧几里德内积， $\langle x,y\rangle=x^{\mathrm{T}}y.$

在命题3.18中，T被看作是一个关于R3中点的变换;T将点x变换为T x，性质(a)表示T保持距离，而性质(b)表示T保持角度。

具体来说，如果x, y, z∈R3表示三角形的三个顶点，那么由变换后的顶点{T x, T y, T z}组成的三角形与三角形{x, y, z}具有相同的长度和角度集合(这两个三角形称为等距三角形 isometric )。可以很容易地想象取{x, y, z}作为刚体上的点，在这种情况下{T x, T y, T z}表示刚体的位移版本。从这个意义上说，SE(3)可以与刚体运动相一致。

3.3.1.2 Uses of Transformation Matrices 变换矩阵的应用

与旋转矩阵的情况一样，变换矩阵T有三种主要用途:

以表示刚体的配置（位置和方向）；
以改变其中表示向量或坐标系的参考系；
移动矢量或坐标系

在第一种使用中，T被认为表示坐标系的配置；在第二种和第三种用法中，T被认为是一个用来改变参考坐标系或移动矢量或坐标系的运算符。

为了说明这些用途，我们参考了图3.14中的三个参考系{a}、{b}和{c}以及点v。坐标系的选择方式使其轴线对齐清晰，从而可以对计算进行视觉确认。

Representing a configuration.

固定帧{s}与{a}重合，帧{a}、{b}和{c}由 $T_{sa}=(R_{sa},p_{sa}),T_{sb}=(R_{sb},p_{sb}),T_{sc}=(R_{sc},p_{sc})$ 分别表示，可以通过旋转表示相对于{s}为：

$R_{sa}=\left[\begin{array}{ccc}1&0&0\\0&1&0\\0&0&1\end{array}\right],\quad R_{sb}=\left[\begin{array}{ccc}0&0&1\\0&-1&0\\1&0&0\end{array}\right],\quad R_{sc}=\left[\begin{array}{ccc}-1&0&0\\0&0&1\\0&1&0\end{array}\right].$

每个帧的原点相对于{s}的位置可以写成

$p_{sa}=\left[\begin{array}{c}0\\0\\0\end{array}\right],\quad p_{sb}=\left[\begin{array}{c}0\\-2\\0\end{array}\right],\quad p_{sc}=\left[\begin{array}{c}-1\\1\\0\end{array}\right].$

由于{a}与{s}并置，由 $(R_{sa},p_{sa})$ 构造的变换矩阵Tsa为单位矩阵。

根据命题3.15，也可以证明对于任意两个帧{d}和{e}有 $T_{de}=T_{ed}^{-1}$

Changing the reference frame of a vector or a frame.

通过类似于旋转的下标消去规则，对于任意三个参考系{a}， {b}和{c}，任意向量v在{b}中表示为vb，有

其中va是用{a}表示的向量v。、

Displacing (rotating and translating) a vector or a frame.

一个变换矩阵T，被看作(R, p) = (Rot(ω， θ)， p)，可以作用于一个框架sb，通过绕一个轴ω旋转θ，并通过p平移它。我们可以将3 × 3旋转算子R = Rot(ω， θ)扩展为一个4 × 4的变换矩阵，它旋转而不平移。

$\operatorname{Rot}(\hat{\omega},\theta)=\left[\begin{array}{cc}R&0\\0&1\end{array}\right],$

我们可以类似地定义一个平移算子，它不旋转地平移，

$\text{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].$

对Tsb前乘（左乘）还是后乘（右乘）T = (R, p)决定了ω轴和p是在固定坐标系fixed frame {s}中解释，还是在主体坐标系body frame {b}中解释:

$\begin{aligned} &T_{sb^{\prime}} =TT_{sb}=\text{Trans}(p)\operatorname{Rot}(\hat{\omega},\theta)T_{sb}\quad\mathrm{(fixed~frame)} \\ &=\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}RR_{sb}&Rp_{sb}+p\\0&1\end{array}\right]&& (3.66) \\ &T_{sb^{\prime\prime}} =T_{sb}T=T_{sb}\text{ Trans}(p)\operatorname{Rot}(\hat{\omega},\theta)\quad\mathrm{(body~frame)} \\ &=\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].&& (3.67) \end{aligned}$

固定坐标系变换(对应于左乘T)可以解释为首先将{b}坐标系绕{s}系中的轴 $\hat{\omega}$ 旋转θ。(这种旋转将导致{b}的原点移动，如果它与{s}的原点不一致)，然后在{s}坐标系中平移p得到一个坐标系{b '}。

body-frame变换(对应于右乘T)可以解释为首先将{b}系平移在{b}系中的p，然后在这个新的body-frame中旋转(这不会移动帧的原点)以得到{b ''}。

Fixed-frame 和 body-frame 变换如图3.15所示，其中变换T为: $\hat{\omega}=(0,0,1),\theta=90^{\circ},\mathrm{~and~}p=(0,2,0)$ 得到

$T=(\operatorname{Rot}(\hat{\omega},\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}开始

$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],$

通过Fixed-frame变换实现的新{b′}通过body-frame 变换现的新{b′′}为

$TT_{sb}=T_{sb}=\left[\begin{array}{cccc}0&1&0&2\\0&0&1&2\\1&0&0&0\\0&0&0&1\end{array}\right],\quad T_{sb}T=T_{sb^{\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].$

Example 3.19.

图3.16显示了安装在轮式移动平台上的机械臂在房间内移动，以及固定在天花板上的摄像头。

坐标系{b}和{c}分别连接到轮式平台和机械臂的末端执行器，坐标系{d}连接到相机。已经建立了固定坐标系｛a｝，机器人必须拾取具有body frame｛e｝的物体。假设变换Tdb和Tde可以根据用相机获得的测量值来计算。可以使用手臂的关节角度测量值来计算变换Tbc。假定转换Tad是预先已知的。假设这些计算的和已知的变换如下所示： $\begin{array}{ccccc}T_{db}&=&\left[\begin{array}{cccc}0&0&-1&250\\0&-1&0&-150\\-1&0&0&200\\0&0&0&1\end{array}\right],\end{array}$

$T_{de}=\left[\begin{array}{ccccc}0&0&-1&300\\0&-1&0&100\\-1&0&0&120\\0&0&0&1\end{array}\right],$ $T_{ad}\quad=\quad\left[\begin{array}{ccccc}0&0&-1&400\\0&-1&0&50\\-1&0&0&300\\0&0&0&1\end{array}\right],$ $T_{bc}\quad=\quad\left[\begin{array}{cccc}0&-1/\sqrt{2}&-1/\sqrt{2}&30\\0&1/\sqrt{2}&-1/\sqrt{2}&-40\\1&0&0&25\\0&0&0&1\end{array}\right].$

为了计算如何移动机械臂以拾取物体，必须确定物体相对于机器人手Tce的配置。我们知道

$T_{ab}T_{bc}T_{ce}=T_{ad}T_{de},$

我们没有给出 Tce 之外的唯一量是 Tab。但是，由于 Tab = TadTdb，我们可以确定 Tce 如下：

$T_{ce}=\left(T_{ad}T_{db}T_{bc}\right)^{-1}T_{ad}T_{de}.$

从给定的转换中我们得到

由此评估Tce为 $T_{ce}\quad=\quad\left[\begin{array}{ccccc}0&0&1&-75\\-1/\sqrt{2}&1/\sqrt{2}&0&-260/\sqrt{2}\\-1/\sqrt{2}&-1/\sqrt{2}&0&130/\sqrt{2}\\0&0&0&1\end{array}\right].$

3.3.2 Twists

我们现在考虑移动坐标系的线速度和角速度。如前所述，{s} 和 {b} 分别表示固定（空间）和移动（身体）帧。

$T_{sb}(t)=T(t)=\left[\begin{array}{cc}R(t)&p(t)\\0&1\end{array}\right]\quad(3.68)$

表示从{s}看到的{b}的configuration。为了保持符号的整洁，我们暂时用T代替通常的Tsb。

在第3.2.2节中，我们发现，对 $\dot{R}$ 左乘或右乘以 $R^{-1}$ 会得到角速度矢量的偏对称表示，无论是在固定坐标系中还是在body-frame坐标系中。人们可能会合理地问，类似的性质是否也适用于T，即， $T^{-1}\dot{T}$ 和 $\dot{T}T^{-1}$ 是否具有类似的物理解释。

让我们先看看当我们将 $\dot{T}$ 左乘 $T^{-1}$ 时会发生什么:

[ωb]是{b}坐标系中角速度的斜对称矩阵表示。同样， $\dot{p}$ 是{b}原点在固定坐标系{s}中表示的线速度， $R^T \dot{p}=v_b$ 是在坐标系{b}中表示的线速度。

将这两个观测结果结合起来，我们可以得出结论， $T^{-1}\dot{T}$ 表示运动系相对于当前与运动系对齐的静止系{b}的线速度和角速度。以上对 $T^{-1}\dot{T}$ 的计算表明，ωb和 $v_b$ 合并为一个六维速度矢量是合理的。我们定义在body frame中的空间速度 spatial velocity in the body frame 或是 body twist

（“twist”一词在力学和螺旋理论文献中有不同的用法。然而，在机器人技术中，通常使用这个术语来指代空间速度spatial velocity。我们通常使用术语“twist”而不是“空间速度”来减少冗词，例如，“body twist”与“spatial velocity in the body frame”。）

$\mathcal{V}_b=\left[\begin{array}{c}\omega_b\\v_b\end{array}\right]\in\mathbb{R}^6.\quad(3.70)$

正如角速度矢量的斜对称矩阵表示很方便一样，twist的矩阵表示法也很方便，如方程（3.69）所示

$T^{-1}\dot{T}=[\mathcal{V}_b]=\left[\begin{array}{cc}[\omega_b]&v_b\\0&0\end{array}\right]\in se(3),\quad(3.71)$

其中 $[\omega_{b}]\in so(3)\mathrm{~and~}v_{b}\in\mathbb{R}^{3}.$ 这种形式的所有4×4矩阵的集合称为 $se(3)$ ，包括与刚体构型 $SE(3)$ 相关的twist的矩阵表示。（se(3)称为李群SE(3)的李代数。当T = I时，它由所有可能的 $\dot{T}$ 组成。

现在我们有了 $T^{-1}\dot{T}$ 的物理解释，让我们计算 $\dot{T}T^{-1}$ :

观察到斜对称矩阵 $[\omega_s]=\dot{R}R^\mathrm{T}$ 是在fixed-frame坐标中表示的角速度，但 ${v}_s=\dot{p}-\dot{R}R^\mathrm{T}p$ 不是在固定坐标系中表示的body-frame原点的线速度(这个量可以简单地表示为 $\dot{p}$ )。把vs写成:

$v_s=\dot{p}-\omega_s\times p=\dot{p}+\omega_s\times(-p),\quad\quad\quad\quad(3.73)$

现在可以推断出vs的物理意义:

想象运动物体是无限大的，vs是物体上当前在固定坐标系原点的点的瞬时速度，用固定坐标系表示（见图3.17）。

正如我们对ωb和vb所做的那样，我们将ωs和vs组合成一个六维twist，

$\mathcal{V}_s=\left[\begin{array}{c}\omega_s\\v_s\end{array}\right]\in\mathbb{R}^6,\quad[\mathcal{V}_s]=\left[\begin{array}{cc}[\omega_s]&v_s\\0&0\end{array}\right]=\dot{T}T^{-1}\in se(3),\quad(3.74)$

$[\mathcal{V}_{s}]$ 是 $\mathcal{V}_{s}$ 的4×4矩阵表示。我们称Vs为空间帧中的空间速度 spatial velocity in the space frame，或简称为空间扭曲 spatial twist。

如果我们认为运动物体是无限大的，那么在Vs=（ωs，Vs）和Vb=（ωb，Vb）之间存在一个吸引人的自然对称性：

ωb是用{b}表示的角速度，ωs是用{s}表示的角度速度。
vb是用{b}表示的{b}原点处的点的线速度，vs是用{s}表示的{s}原点处点的线速率。

写出方程（3.76）中的乘积，我们得到 $\mathcal{V}_s=\left[\begin{array}{cc}R[\omega_b]R^\mathrm{T}&-R[\omega_b]R^\mathrm{T}p+Rv_b\\0&0\end{array}\right]$

对于p，ω∈R3，使用 $R[\omega]R^{\mathrm{T}}=[R\omega]$ (Proposition 3.8)和 $[\omega]p=-[p]\omega$ 可以转化为Vb和Vs之间的如下关系:

6 × 6矩阵左乘Vb对于改变twists和wrenches的参照系很有用

Definition 3.20.

给定T = (R, p)∈SE(3)，其伴随表示 adjoint representation $[Ad_T]$ 为

对于任意 $\mathcal{V}\in\mathbb{R}^6,$ ，与T相关的伴随映射 adjoint map 为

$\mathcal{V}^{\prime}=[\mathrm{Ad}_T]\mathcal{V},$ 有时也写成 $\mathcal{V}^{\prime}=\mathrm{Ad}_{T}(\mathcal{V}).$

对于 $\mathcal{V}\in\mathbb{R}^6,$ 的矩阵形式 $[\mathcal{V}]\in se(3)$ ，有 $[\mathcal{V}']=T[\mathcal{V}]T^{-1}.$

伴随映射 adjoint map 满足下列性质，可直接计算验证:

Proposition 3.21.

$T_{1},T_{2}\in SE(3)and\mathcal{V}=(\omega,v).$ 有

$\mathrm{Ad}_{T_1}\left(\mathrm{Ad}_{T_2}(\mathcal{V})\right)=\mathrm{Ad}_{T_1T_2}(\mathcal{V})\quad or\quad[\mathrm{Ad}_{T_1}][\mathrm{Ad}_{T_2}]\mathcal{V}=[\mathrm{Ad}_{T_1T_2}]\mathcal{V}.\quad(3.77)$

同样，对于任意T∈SE(3)，成立:

$[\mathrm{Ad}_T]^{-1}=[\mathrm{Ad}_{T^{-1}}],\quad\quad\quad\quad(3.78)$

选择 $T_1=T^{-1}$ 和 $T_2=T$ ，因此有

$\mathrm{Ad}_{T^{-1}}\left(\mathrm{Ad}_T(\mathcal{V})\right)=\mathrm{Ad}_{T^{-1}T}(\mathcal{V})=\mathrm{Ad}_I(\mathcal{V})=\mathcal{V}.\quad\quad(3.79)$

3.3.2.1 Summary of Results on Twists

到目前为止，关于twists的主要结果总结在以下命题中：

Proposition 3.22.

给定一个固定（空间）坐标系 fixed (space) frame{s}，一个body frame{b}，和一个可微 $T_{sb}(t)\in SE(3)$

$T_{sb}(t)=\left[\begin{array}{cc}R(t)&p(t)\\0&1\end{array}\right],\quad(3.80)$

那么 $T_{sb}^{-1}\dot{T}_{sb}=[\mathcal{V}_b]=\left[\begin{array}{cc}[\omega_b]&v_b\\0&0\end{array}\right]\in se(3)\quad(3.81)$ 是body twist的矩阵表示，以及 $\dot{T}_{sb}T_{sb}^{-1}=[\mathcal{V}_s]=\left[\begin{array}{cc}[\omega_s]&v_s\\0&0\end{array}\right]\in se(3)\quad(3.82)$ 是spatial twist的矩阵表示。

Vs和Vb联系为

更一般地说，对于任意两个坐标系{c}和{d}，在{c}中表示为Vc的twist与其在{d}中的表示Vd的关系为

$\mathcal{V}_c=[\operatorname{Ad}_{T_{cd}}]\mathcal{V}_d,\quad\mathcal{V}_d=[\operatorname{Ad}_{T_{dc}}]\mathcal{V}_c.$

再次类似于角速度的情况，重要的是要认识到，对于给定的twist，其固定坐标系表示v不依赖于体坐标系body frame{b}的选择，其body-frame坐标系表示Vb也不依赖于固定坐标系{s}的选择。

Example 3.23.

图3.18显示了一辆在飞机上行驶的汽车的俯视图，该汽车只有一个可操纵的前轮。

主体框架{b}的ξzb轴进入页面，而固定框架{s}的Γzs轴离开页面。汽车前轮的角度使汽车的运动成为纯角速度w＝2 rad/s，在平面中的点r处围绕页面外的轴。检查该图，我们可以将r写为 $r_s=(2,-1,0)$ 或 $r_b=(2,-1.4,0)$ ,w为 $\omega_s=(0,0,2)$ 或 $\omega_b=(0,0,-2)$

$T_{sb}=\left[\begin{array}{ccc}R_{sb}&p_{sb}\\0&1\end{array}\right]=\left[\begin{array}{cccc}-1&0&0&4\\0&1&0&0.4\\0&0&-1&0\\0&0&0&1\end{array}\right].$

从图形和简单的几何，我们得到

$\begin{aligned}v_s&=\omega_s\times(-r_s)=r_s\times\omega_s=(-2,-4,0),\\v_b&=\omega_b\times(-r_b)=r_b\times\omega_b=(2.8,4,0),\end{aligned}$

从而获得twists Vs和Vb：

$\mathcal{V}_s=\left[\begin{array}{c}\omega_s\\v_s\end{array}\right]=\left[\begin{array}{c}0\\0\\2\\-2\\-4\\0\end{array}\right],\quad\mathcal{V}_b=\left[\begin{array}{c}\omega_b\\v_b\end{array}\right]=\left[\begin{array}{c}0\\0\\-2\\2.8\\4\\0\end{array}\right].$

要确认这些结果，请尝试计算 $\mathcal{V}_s=[\mathrm{Ad}_{T_{sb}}]\mathcal{V}_b.$

3.3.2.2 The Screw Interpretation of a Twist

就像角速度ω可以看作是 $\hat{\omega}\dot{\theta}$ ，其中 $\hat{\omega}$ 是单位旋转轴，而θ是绕该轴旋转的速率，twist V可以用螺旋轴S和螺旋轴上的速度 $\dot{\theta}$ 来解释。

螺杆轴表示螺杆常见的运动：绕轴旋转，同时也沿轴平移。螺旋轴s的一种表示是集合 $\{q,\hat{s},h\}$ .其中q∈R3是轴上的任何点， $\hat{s}$ 是轴方向上的单位向量，h是螺距 screw pitch ，它定义了沿螺杆轴线的线速度与绕螺杆轴线的角速度的比值（图3.19）。

使用图3.19和几何结构，我们可以将对应于关于轴S的角速度 $\dot{\theta}$ 的twist $\mathcal{V}=(\omega,v)$ （由 $\{q,\hat{s},h\}$ 表示）写成

$\mathcal{V}=\left[\begin{array}{c}\omega\\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].$

请注意，线速度v是两项的总和：

一项是由于沿螺杆轴的平移， $h \hat{s} \dot{\theta}$

另一项是由绕轴旋转引起的原点处的线性运动， $-\hat{s}\dot{\theta}\times q$

第一个项在方向 $\hat{s}$ 上，而第二个项在与方向 $\hat{s}$ 垂直的平面上。

不难证明，对于任何 $\mathcal{V}=(\omega,v)$ ，其中 $\omega \neq 0$ ，存在一个等效的螺杆轴 $\{q,\hat{s},h\}$ 和速度 $\dot{\theta}$ ，其中，选择 $\hat{s}=\omega/\|\omega\|,\dot{\theta}=\|\omega\|,h=\hat{\omega}^\mathrm{T}v/\dot{\theta},$ 并取q，使得项 $-\hat{s}\dot{\theta}\times q$ 提供v与螺杆轴正交的部分。

如果ω = 0，则螺杆的螺距h是无限的。在这种情况下，选择 $\hat{s}=v/\left \| v \right \|$ ，并将 $\dot{\theta}$ 解释为沿 $\hat{s}$ 的线速度‖v‖。

代替用麻烦的集合 $\{q,\hat{s},h\}$ 来表示螺旋轴S，考虑到h可能是无限的，并且考虑到q的非唯一性(沿螺旋轴的任何q都可能被使用)，我们用任意扭转 $\mathcal{V}=(\omega,v)$ 对应于沿螺旋轴运动的归一化形式normalized version 来定义螺旋轴S:

如果 $\omega \neq 0$ ，则 $\mathcal{S}=\mathcal{V}/\|\omega\|=(\omega/\|\omega\|,v/\|\omega\|)$ 。螺杆轴S通过角速度矢量的长度简单地被V归一化。绕螺杆轴的角速度 $\dot{\theta}=\left \| \omega \right \|$ ，因此 $\mathcal{S}\dot{\theta}=\mathcal{V}.$
如果 $\omega = 0$ ，则 $\mathcal{S}=\mathcal{V}/\|v\|=(0,v/\|v\|)$ 。螺杆轴线S简单地通过线性速度矢量的长度进行V归一化。沿螺杆轴线的线速度 $\dot{\theta}=\left \| v \right \|$ ，因此 $\mathcal{S}\dot{\theta}=\mathcal{V}.$

这就引出了以下“单位”(归一化)螺旋轴的定义:

Definition 3.24.

对于给定的参考系，螺旋轴S写成

$\mathcal{S}=\left[\begin{array}{c}\omega\\v\end{array}\right]\in\mathbb{R}^6,$

其中

(i)‖ω‖= 1或

(ii) ω = 0且‖v‖= 1。

如果(i)成立，则 $v=-\omega \times q+h\omega$ ，其中q是螺杆轴上的一个点，h是螺杆的螺距(对于绕螺杆轴的纯旋转，h = 0)。

如果(ii)成立，则螺杆的螺距是无限大的，twist是沿v定义的轴的平移。

重要提示:虽然我们对归一化螺轴 $\mathcal{S}$ (其中‖ω‖或‖v‖中的一个必须是统一的)和一般twist $\mathcal{V}$ (其中ω和v没有约束)都使用(ω， v)对，但从上下文中应该清楚其含义。

由于螺旋轴S只是一个归一化的扭转，因此S = (ω， v)的4×4矩阵表示[S]为

$[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),\quad(3.85)$

其中[S]的最后一行全是0。

同样，在坐标系{a}中表示为Sa的螺旋轴与坐标系{b}中的表示Sb关系为

$\mathcal{S}_a=[\mathrm{Ad}_{T_{ab}}]\mathcal{S}_b,\quad\mathcal{S}_b=[\mathrm{Ad}_{T_{ba}}]\mathcal{S}_a.$

3.3.3 Exponential Coordinate Representation of Rigid-Body Motions 刚体运动的指数坐标表示

3.3.3.1 Exponential Coordinates of Rigid-Body Motions 刚体运动的指数坐标

在3.1节的平面例子中，我们看到任何平面刚体位移都可以通过使刚体绕某平面内固定点旋转来实现(对于纯平移，这一点位于无穷远处)。空间刚体位移也存在类似的结果:

Chasles-Mozzi定理表明，每个刚体位移都可以表示为空间中沿固定螺旋轴S的位移。

【现代机器人学】基于指数积的机械臂正运动学 - 古月居 (guyuehome.com)

类比于旋转的指数坐标 $\hat{\omega}\theta$ ，我们定义一个齐次变换T的六维指数坐标 exponential coordinates of a homogeneous transformation 为 ${\cal S}\theta\in\mathbb{R}^{6}$ ，其中S是螺旋轴，θ是沿螺旋轴移动的距离从原点I到T。

如果螺杆轴S的节距是有限的，则 $\left \| \omega \right \|=1$ ，θ对应于绕螺杆轴的旋转角。如果螺杆的螺距是无穷大的，那么ω=0，并且‖v‖=1，θ对应于沿螺杆轴线行进的线性距离。

同样，与旋转情况类似，我们定义了矩阵指数（exp）和矩阵对数(log):

我们首先导出矩阵指数 $e^{[\mathcal{S}]\theta}$ 的闭式表达式。以级数形式扩展矩阵指数有：

使用恒等式 $[\omega]^3=-[\omega]$ ，可以简化为

把所有东西放在一起会产生以下命题：

Proposition 3.25.

设 $\mathcal{S}=(\omega,v)$ 为螺旋轴。如果 $\left \| \omega \right \|=1$ ，则对于沿轴行进的任何距离θ∈R，有

$e^{(\mathcal{S}]\theta}=\left[\begin{array}{cc}e^{[\omega]\theta}&\left(I\theta+(1-\cos\theta)[\omega]+(\theta-\sin\theta)[\omega]^2\right)v\\0&1\end{array}\right].\quad(3.88)$

如果ω=0且 $\left \| v \right \|=1$ ，则 $e^{|\mathcal{S}|\theta}=\left[\begin{array}{cc}I&v\theta\\0&1\end{array}\right].\quad\quad\quad\quad\quad(3.89)$

3.3.3.2 Matrix Logarithm of Rigid-Body Motions 刚体运动的矩阵对数

上述推导基本上提供了Chasles-Mozzi定理的构造性证明。也就是说，给定任意（R，p）∈SE（3），总是可以找到螺旋轴 $\mathcal{S}=(\omega,v)$ 和标量θ，使得

$e^{|\mathcal{S}|\theta}=\left[\begin{array}{cc}R&p\\0&1\end{array}\right],\quad(3.90)$

即矩阵 $[\mathcal{S}]\theta=\left[\begin{array}{cc}[\omega]\theta&v\theta\\0&0\end{array}\right]\in se(3)$ 是T=（R，p）的矩阵对数。

Algorithm:

给定（R，p）写成T∈SE（3），求θ∈[0，π]和螺旋轴 $\mathcal{S}=\left.(\omega,v)\right.\in\mathbb{R}^6$ （其中， $\left \| \omega \right \|$ 和 $\left \| v\right \|$ 中的至少一个是单位1），使得 $e^{[\mathcal{S}]\theta}=T$ 。

如果R=I，则设 $\omega=0,v=p/\|p\|,\mathrm{~and~}\theta=\|p\|.$
否则，用SO(3)上的矩阵对数确定R的ω(在SO(3)算法中写为 $\hat{\omega}$ )和θ，则v计算为 $v=G^{-1}(\theta)p\quad(3.91)$ 其中 $G^{-1}(\theta)=\frac{1}{\theta}I-\frac{1}{2}[\omega]+\left(\frac{1}{\theta}-\frac{1}{2}\cot\frac{\theta}{2}\right)[\omega]^{2}.\quad(3.92)$

(3.92)式的验证留作练习。

Example 3.26.

在这个例子中，刚体运动被限制在 $\hat{x}_s-\hat{y}_s$ 平面上。

图3.20中的初始帧{b}和最终帧{c}可以SE(3)表示为

$\begin{aligned}T_{sb}&=&\left[\begin{array}{ccccc}\cos30^\mathrm{o}&-\sin30^\mathrm{o}&0&1\\\sin30^\mathrm{o}&\cos30^\mathrm{o}&0&2\\0&0&1&0\\0&0&0&1\end{array}\right],\\T_{sc}&=&\left[\begin{array}{ccccc}\cos60^\mathrm{o}&-\sin60^\mathrm{o}&0&2\\\sin60^\mathrm{o}&\cos60^\mathrm{o}&0&1\\0&0&1&0\\0&0&0&1\end{array}\right].\end{aligned}$

由于运动发生在 $\hat{x}_s-\hat{y}_s$ 平面上，因此相应的螺杆的轴与 $\hat{z}_s$ 轴方向一致，螺距为零。螺旋轴S = (ω， v)，用{S}表示，因此有这样的形式

$\begin{array}{rcl}\omega&=&(0,0,\omega_3),\\v&=&(v_1,v_2,0).\end{array}$

我们寻求从Tsb到Tsc位移坐标系的螺旋运动;即， $T_{sc}=e^{[\mathcal{S}]\theta}T_{sb}$ 或 $T_{sc}T_{sb}^{-1}=e^{[\mathcal{S}]\theta},$ 其中

$[\mathcal{S}]=\left[\begin{array}{cccc}0&-\omega_3&0&v_1\\\omega_3&0&0&v_2\\0&0&0&0\\0&0&0&0\end{array}\right].$

我们可以将矩阵对数算法直接应用于 $T_{sc}T_{sb}^{-1}$ ，得到[S](因此得到S)和θ如下:

$[\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}\omega_1\\\omega_2\\\omega_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}\mathrm{~rad~}(\mathrm{or~}30^\circ).$

S的值表示在固定坐标系{S}中表示的恒定螺旋轴，在{S}坐标系中表示为绕 $\hat{z}_s$ 轴 1 rad/s左右的角速度和(当前在{S}原点的点的)(3.37，−3.37,0)的线速度。

或者，我们可以观察到位移不是纯粹的平移:Tsb和Tsc的旋转分量相差30°。我们很快就确定θ=30◦ ω3=1。我们还可以图形化地确定螺钉轴穿过的平面中的点q=（qx，qy）；对于我们的例子，这一点由q=（3.37,3.37）给出。

对于像这样的平面刚体运动，我们可以导出一个平面矩阵对数算法，该算法将SE(2)的元素映射到se(2)，其形式为

$\left[\begin{array}{ccc}0&-\omega&v_1\\\omega&0&v_2\\0&0&0\end{array}\right].$

3.4 Wrenches

考虑作用在点r的刚体上的线性力f。定义参考帧{a}，点r可以表示为 $r_{a}\in\mathbb{R}^{3}$ ，力f可以表示为 $f_{a}\in\mathbb{R}^{3}$ 。该力在{a}帧中产生一个力矩 torque or moment $m_{a}\in\mathbb{R}^{3}$

$m_a=r_a\times f_a.$

注意，力沿其作用线的施加点是无关紧要的。

就像twists一样，我们可以将力矩和力合并成一个单一的六维空间力 spatial force，或扳手 wrench，用{a}框架表示，Fa:

$\mathcal{F}_a=\left[\begin{array}{c}m_a\\f_a\end{array}\right]\in\mathbb{R}^6.\quad(3.93)$

如果一个以上的 wrench 作用在刚体上，则该刚体上的总wrench就是单个wrench的矢量和 vector sum ，前提是这些wrench在同一坐标系中表示。具有零线性分量的 wrench 称为纯力矩 pure moment。

如果已知Tba，则{a}框架中的扳手可以在另一框架{b}中表示(图3.21)。推导 $\mathcal{F}_a$ 和 $\mathcal{F}_b$ 之间关系的一种方法是在我们已经使用的技术的基础上推导单个力和力矩矢量之间的适当变换。

然而，导出Fa和Fb之间关系的一种更简单、更深入的方法是

使用我们已经导出的关于相同twist的表示Va和Vb的结果,以及
使用这样一个事实，即由（F，V）对产生（或耗散）的功率必须是相同的，而不管它在哪个帧中表示。

回想一下力和速度的点积是功率 power，功率是一个与坐标无关的量。正因为如此，我们知道

$\mathcal{V}_{b}^{\mathrm{T}}\mathcal{F}_{b}=\mathcal{V}_{a}^{\mathrm{T}}\mathcal{F}_{a}. \quad (3.94)$

由命题3.22可知， $\mathcal{V}_a~=~[\mathrm{Ad}_{T_{ab}}]\mathcal{V}_b$ ，因此式(3.94)可改写为

$\begin{aligned} \mathcal{V}_{b}^{\mathrm{T}}\mathcal{F}_{b}& =([\mathrm{Ad}_{T_{ab}}]\mathcal{V}_{b})^{\mathrm{T}}\mathcal{F}_{a} \\ &=\mathcal{V}_{b}^{\mathrm{T}}[\mathrm{Ad}_{T_{ab}}]^{\mathrm{T}}\mathcal{F}_{a}. \end{aligned}$

因为这个式子对所有Vb都成立，所以化简为

$\mathcal{F}_b=[\operatorname{Ad}_{T_{ab}}]^{\mathrm{T}}\mathcal{F}_a.\quad(3.95)$ 同样的 $\mathcal{F}_a=[\mathrm{Ad}_{T_{ba}}]^\mathrm{T}\mathcal{F}_b.(3.96)$

Proposition 3.27.

给定一个wrench F，在{a}中表示为Fa，在{b}中表示为Fb，这两个表示联系为：

$\begin{matrix}\mathcal{F}_b&=&\mathrm{Ad}_{T_{ab}}^\mathrm{T}(\mathcal{F}_a)=[\mathrm{Ad}_{T_{ab}}]^\mathrm{T}\mathcal{F}_a,&(3.97)\\\mathcal{F}_a&=&\mathrm{Ad}_{T_{ba}}^\mathrm{T}(\mathcal{F}_b)=[\mathrm{Ad}_{T_{ba}}]^\mathrm{T}\mathcal{F}_b.&(3.98)\end{matrix}$

由于我们通常有一个固定的空间框架 fixed space frame {s}和一个本体框架 body frame {b}，我们可以定义一个空间扳手 spatial wrench Fs和一个body wrench Fb。

Example 3.28.

图3.22中的机械手在重力场g=10m/s2（四舍五入以保持数字简单）中握住一个质量为0.1 kg的苹果，重力场向下作用在页面上。手的质量为0.5公斤。手和机械臂之间的六轴力-扭矩传感器测量的力和扭矩是多少？

我们在力-扭矩传感器处定义了框架{f}，在手的质心处定义了{h}，在苹果的质心处则定义了{a}。根据图3.22中的坐标轴，{h}中手上的重力扳手 wrench (重力分量)由列向量给出

$\mathcal{F}_h=(0,0,0,0,-5\text{N},0)$

{a}中苹果上的重力扳手是

$\mathcal{F}_a=(0,0,0,0,0,1\text{N})$

给定L1=10cm和L2=15cm，变换矩阵 $T_{hf} and T_{af}$

$T_{hf}=\left[\begin{array}{cccc}1&0&0&-0.1\text{m}\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{array}\right],\quad T_{af}=\left[\begin{array}{cccc}1&0&0&-0.25\text{m}\\0&0&1&0\\0&-1&0&0\\0&0&0&1\end{array}\right].$

六轴力-扭矩传感器测量的扳手wrench为

$\begin{aligned} \mathcal{F}_{f}& =[\mathrm{Ad}_{T_{hf}}]^{\mathrm{T}}\mathcal{F}_{h}+[\mathrm{Ad}_{T_{af}}]^{\mathrm{T}}\mathcal{F}_{a} \\ &=[0\text{ 0 }-0.5\text{ Nm 0 }-5\text{ N }0]^\mathrm{T}+[0\text{ 0 }-0.25\text{ Nm 0 }-1\text{ N }0]^\mathrm{T} \\ &=[0\text{ 0 }-0.75\text{ Nm 0 }-6\text{ N }0]^\mathrm{T}. \end{aligned}$

3.5 Summary

下表简洁地总结了本章的一些关键概念，以及旋转和刚体运动之间的平行关系。要了解更多细节，请参阅本章的相应部分。

3.6 Software

以下功能包含在随书发行的软件中。下面的代码是MATLAB格式的，但也有其他语言版本。有关该软件的更多详细信息，请参阅代码及其文档。

3.7 Notes and References

本章中介绍的旋转的指数坐标在运动学文献中也被称为欧拉-罗德里格斯参数 the Euler–Rodrigues parameters 。旋转的其他表示，如欧拉角、凯利-罗德里格斯参数 Cayley–Rodrigues parameters 和单位四元数 unit quaternions 在附录B中描述;关于这些以及旋转群SO（3）的相关参数化的进一步细节可以在例如[1691113186122135]中找到。

经典的螺旋理论起源于Mozzi和Chasles的著作，他们独立发现刚体的运动可以通过绕某个轴旋转，然后绕同一轴平移来获得[25]。Ball的论文[6]通常被认为是螺旋理论的经典参考文献，而更现代的治疗方法可以在Bottema和Roth[18]、Angeles[2]和McCarthy[113]中找到。

Brockett在[20]中首次用刚体运动SE（3）的李群结构识别了经典螺旋理论的元素，他更进一步，证明了开链的正向运动学可以表示为矩阵指数的乘积（这是下一章的主题）。矩阵指数、对数、其导数和其他相关公式的公式的推导可以在[921123122]中找到。