[Modern Robotics] 1 -刚体在平面上的运动

刚体平面运动

二维运动

   考虑下图所示的平面刚体， { s } \{s\} {s}为固定坐标系，${\hat{x}}_s$与${\hat{y}}_s$是其坐标轴，^符号代表其是单位向量。坐标系 { b } \{b\} {b}是平面一个刚体上的坐标系，${\hat{x}}_b$与${\hat{y}}_b$代表其坐标轴。

为描述固定坐标系 { s } \{s\} {s}中的刚体，仅需要确定其在 { s } \{s\} {s}中的位置与方向。刚体坐标系的原点 $p$ 在 { s } \{s\} {s}中可表达为：
$$p=p_x{\hat{x}}_s+p_y{\hat{y}}_s$$
描述坐标系 { b } \{b\} {b}相对于 { s } \{s\} {s}另一种表示方法为：
$$\begin{gathered} \widehat{x_b}=cos\theta \widehat{x_s}+sin\theta \widehat{y_s} \\ \widehat{y_b}=-sin\theta \widehat{x_s}+cos\theta \widehat{y_s} \end{gathered}$$
同样，$p$ 点可以表示成一个列向量，$p\in {\mathbb{R}}^2$：
$$p=\left[\begin{array}{c}{p}_x\\ p_y\end{array}\right],\text{\hspace{1em}(3.4)}$$

$\widehat{x_b}$与$\widehat{y_b}$向量可以打包成一个$2\ast 2$的矩阵$P$：
$$P=\left[\begin{array}{c}\widehat{x_b}\widehat{y_b}\end{array}\right]=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right].\text{\hspace{1em}(3.5)}$$

$P$ 矩阵就是一个旋转矩阵。实际上，$(P,p)$ 这一对参数就描述了坐标系 { b } \{b\} {b}相对于 { s } \{s\} {s}的方向与位置。$P$ 的每列都是单位向量，且两列的向量正交。

倘若 { c } \{c\} {c}坐标系相对于 { s } \{s\} {s}的方位为$(R,r)$，可以写成如下形式：
$$r=\left[\begin{array}{c}{r}_x\\ r_y\end{array}\right],R=\left[\begin{array}{cc}cos\varphi & -sin\varphi \\ sin\varphi & cos\varphi \end{array}\right].\text{\hspace{1em}(3.6)}$$

我们同样可以定义 { c } \{c\} {c}相对于 { b } \{b\} {b}的方位。使向量 $q$ 从 { b } \{b\} {b}的原点指向 { c } \{c\} {c} 的原点，并且假设 $Q$ 为 { c } \{c\} {c} 相对于 { b } \{b\} {b} 的方向。将 { c } \{c\} {c}相对于 { b } \{b\} {b}的关系写成参数对$(Q,q)$，其中：
$$q=\left[\begin{array}{c}{q}_x\\ q_y\end{array}\right],Q=\left[\begin{array}{cc}cos\psi & -sin\psi \\ sin\psi & cos\psi \end{array}\right].\text{\hspace{1em}(3.7)}$$
如果我们知道了$(Q,q)$，$(P,p)$ ，就可以计算出 { c } \{c\} {c}相对于 { s } \{s\} {s}的方位：
$$\begin{gathered} R=PQ \\ r=Pq+p \end{gathered}$$
值得注意的是，这里的 $p,q,r$ 均是坐标，而不是长度，初看易误解。

从这里可以看出，$(P,p)$ 不仅仅代表了 { b } \{b\} {b}在 { s } \{s\} {s}中的方位，也可以把 { b } \{b\} {b}中的一点或者相对于 { b } \{b\} {b}的坐标系转化到 { s } \{s\} {s}坐标系下。

现在考虑一个刚体上固定了两个坐标系 { d } \{d\} {d} 和 { c } \{c\} {c}, { d } \{d\} {d}初始时与 { s } \{s\} {s}重合，而 { c } \{c\} {c}用$(R,r)$这对参数来描述其在 { s } \{s\} {s}中的方位。从上图可以看出，此刚体的移动是从 { d } \{d\} {d}移动到 { d ′ } \{d'\} {d′}，与此时的 { b } \{b\} {b}一致，与 { s } \{s\} {s}的相对关系用$(P,p)$来描述。移动之后， { c } \{c\} {c}坐标系到了 { c ′ } \{c'\} {c′}处。如图所示，此时可以验证：
$$\begin{gathered} R^{\prime }=PR \\ {r}^{\prime }=Pr+p \end{gathered}$$
从图(a)，我们可以认为，先对 { c } \{c\} {c}做了一个$P$ 旋转，然后按照 { s } \{s\} {s}中的$p$向量平移。

通过上述过程，一般来说，$(P,p)$ 这对矩阵-向量参数，可用于三个目的：
(1) 描述一个刚体在固定坐标系 { s } \{s\} {s}中的方位。
(2) 改变向量或者刚体坐标系的参考坐标系，如 { s } \{s\} {s}。
(3) 用来变换一个向量或刚体（平移及旋转）。

再看到图(b)，相比于图(a)所示的运动，我们可以将刚体直接绕着一个固定点 $s$ 旋转$\beta$ 角度得到最终状态。这是一个简单的平面内的screw motion。整个位移过程可以三个参数来表示：$(\beta ,s_x,s_y)$，其中$(s_x,s_y)$ = (0, 2)，即 { s } \{s\} {s}坐标系中$s$点的位置坐标。

另一种来表示screw motion的方式是将运动认为是角速度与线速度同时进行得到的位移。图(b)中，以单位角速度（$w=1rad/s$）绕着 $s$ 点旋转，意味着 { s } \{s\} {s}坐标系的原点以每秒两个单位的速度沿着 { s } \{s\} {s}坐标系的$+x$方向移动，如$v=(v_x,v_y)=(2,0)$。我们把上述两个量写成一个三维向量 $S=(w,v_x,v_y)=（1,2,0）$， 也即旋量轴（screw axis）。绕着这跟轴旋转 $\theta =\pi /2$角度，即达到最终状态了。这样我们用三个坐标来表示这个位移$S\theta =(\pi /2,\pi ,0)$。这样的坐标表示具有一定的优势，称之为平面刚体运动的指数坐标。

将角速度与线性速度结合起来，我们称之为 $twist$，取旋量轴$S=(w,v_x,v_y)$，其中$w=1$，再将他与某个旋转速度 $\dot{\theta }$相乘，得到$twist$，$V=S\dot{\theta }$。可以这样认为，绕着旋量轴 $S$ 旋转 $\theta$ 角度与绕着旋量轴 $S$ ，以速度 $\dot{\theta }=\theta$ 旋转单位时间是等价的。所以，$V=S\dot{\theta }$ 也可以认为是指数坐标。

三维运动入门

   考虑如图3.6的刚体三维运动，假设 { s } \{s\} {s}为固定坐标系， { b } \{b\} {b}坐标系为刚体坐标系，右手坐标系下有 $\hat{x}\times \hat{y}=\hat{z}$。$p$向量从固定坐标系的原点指向刚体坐标系的原点，在固定坐标系下，可表达为:
$$p=p_1\widehat{x_s}+p_2\widehat{y_s}+p_3\widehat{z_s}\text{\hspace{1em}(3.12)}$$

刚体坐标系的坐标轴可表示为:
$$\begin{gathered} \widehat{x_b}=r_{11}\widehat{x_s}+r_{21}\widehat{y_s}+r_{31}\widehat{z_s} \\ \widehat{y_b}=r_{12}\widehat{x_s}+r_{22}\widehat{y_s}+r_{32}\widehat{z_s} \\ \widehat{z_b}=r_{13}\widehat{x_s}+r_{23}\widehat{y_s}+r_{33}\widehat{z_s} \end{gathered}$$
定义 $p\in {\mathbb{R}}^3$，$R\in {\mathbb{R}}^{3\times 3}$，则有：
$$p=\left[\begin{array}{c}{p}_1\\ p_2\\ p_3\end{array}\right],R=\left[\begin{array}{ccc}\widehat{x_b}& \widehat{y_b}& \widehat{z_b}\end{array}\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]\text{\hspace{1em}(3.16)}$$
上述的 $(R,p)$ 12个值，描述了刚体坐标系相对于固定坐标系的位置与方向。考虑到一个刚体的方向有三个自由度，所以 $R$ 矩阵9个元素中只有3个是独立的。而指数坐标表示的旋转正好由三个参数组成，它定义了一个旋转轴以及绕着此轴旋转一定的角度。同样也有别的表示方法，如欧拉角，$RPY$角，四元数等。

$Chasles-Mozzi$ 定理表明了每个刚体的运动都可以通过绕着一个旋量轴进行有限的旋转和平移得到。

下一节将介绍旋转矩阵。