机器人操作数学重学 01：坐标、旋转与螺旋运动

相机报告“物体在我前方 1 米”，机械臂控制器却需要基座坐标下的位置。把这句话变成一次坐标变换，再让相机开始转动，就能连起旋转矩阵、刚体变换、角速度和运动旋量。这篇用可手算的例子重学《机器人操作的数学导论》的几何基础。

重学路线 · 下一篇：指数积、雅可比与奇异性

1. 先读懂下标：这个坐标是谁看到的

约定 $g_{ab}$ 把 b 系表示的坐标变为 a 系表示的坐标。同一个物理点在两系里的数字分别是 $x_b$、$x_a$：

$$x_a=R_{ab}x_b+p_{ab},\qquad g_{ab}=\begin{bmatrix}R_{ab}&p_{ab}\\0&1\end{bmatrix}.$$

$p_{ab}$ 是 b 系原点在 a 系中的位置；$R_{ab}$ 的三列，是 b 系三个单位轴在 a 系中的坐标。它不是“先移动再旋转”的口令，而是一个写清输入、输出坐标系的映射。

取 $R_{ab}=R_z(90^\circ)$、$p_{ab}=(1,2,0)^T$，相机看到 $x_b=(1,0,0)^T$：

$$x_a= \begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix} \begin{bmatrix}1\\0\\0\end{bmatrix} +\begin{bmatrix}1\\2\\0\end{bmatrix} =\begin{bmatrix}1\\3\\0\end{bmatrix}.$$

调整图中的角度，先看 b 系的 x 轴指向哪里，再看点的位置。点在 b 系里的数字不变，但 b 系在 a 系里的位姿改变，点在 a 系里的数字就会改变。

主动与被动要分开读。 坐标系固定、把向量真正转过一个角度，是主动旋转；物理向量固定、换一组基底描述，是被动换坐标。例如已知 $x_a=R_{ab}x_b$，反向换坐标应使用 $x_b=R_{ab}^Tx_a$。公式看起来相似，输入输出的含义决定使用哪个矩阵。

2. 组合顺序与逆变换可以靠下标检查

若已有 $g_{ab}$ 和 $g_{bc}$，从 c 到 a 应当先 c→b、再 b→a：

$$g_{ac}=g_{ab}g_{bc},\qquad R_{ac}=R_{ab}R_{bc},\qquad p_{ac}=p_{ab}+R_{ab}p_{bc}.$$

两个平移向量不能不看坐标系就直接相加：$p_{bc}$ 必须先用 $R_{ab}$ 变到 a 系。

逆变换直接从 $x_b=R^T(x_a-p)$ 得到：

$$g^{-1}=\begin{bmatrix}R^T&-R^Tp\\0&1\end{bmatrix}.$$

例中的逆平移是 $(-2,1,0)^T$，不是简单的 $(-1,-2,0)^T$。把 $x_a=(1,3,0)^T$ 代回，才会恢复 $x_b=(1,0,0)^T$。

矩阵一般不交换。以原点为例，先沿固定 x 轴平移 1 再绕固定 z 轴转 90°，得到 $(0,1,0)$；顺序反过来得到 $(1,0,0)$。在当前位姿上左乘小变换，是用空间系描述该增量；右乘则是用当前物体系描述增量。

3. 为什么角速度藏在反对称矩阵里

旋转保持长度和右手性，因此

$$SO(3)=\{R\mid RR^T=I,\ \det R=1\}.$$

$R$ 有九个数字，却不能任意独立改变。对约束求导：

$$\dot R R^T+R\dot R^T=0 \quad\Longrightarrow\quad (\dot R R^T)^T=-\dot R R^T.$$

所以 $\dot R R^T$ 是反对称矩阵，只需三个独立数。定义叉乘矩阵

$$\widehat\omega= \begin{bmatrix} 0&-\omega_z&\omega_y\\ \omega_z&0&-\omega_x\\ -\omega_y&\omega_x&0 \end{bmatrix},\qquad \widehat\omega x=\omega\times x.$$

于是 $\widehat\omega_s=\dot RR^T$，$\widehat\omega_b=R^T\dot R$。两者描述同一角速度，分别用空间系和物体系表示，满足 $\omega_s=R\omega_b$。这也说明“旋转矩阵逐项求导”并不是直接得到一个三维角速度向量。

4. 旋量的前 3 项为什么不总是末端速度

本专题采用 Murray–Li–Sastry（MLS）的排列：

$$V=\begin{bmatrix}v\\\omega\end{bmatrix},\qquad \widehat V=\begin{bmatrix}\widehat\omega&v\\0&0\end{bmatrix}.$$

令 $g(t)=[R(t),p(t);0,1]$ 表示物体系相对于空间系的位姿。直接展开乘法：

$$\widehat V_s=\dot g g^{-1} =\begin{bmatrix}\dot RR^T&\dot p-\dot RR^Tp\\0&0\end{bmatrix}, \qquad V_s=\begin{bmatrix}\dot p-\omega_s\times p\\\omega_s\end{bmatrix}.$$

因此 空间旋量的 $v_s$ 通常不等于物体原点速度 $\dot p$。需要原点速度时应恢复

$$\dot p=v_s+\omega_s\times p.$$

物体旋量则为

$$\widehat V_b=g^{-1}\dot g, \qquad V_b=\begin{bmatrix}R^T\dot p\\\omega_b\end{bmatrix}.$$

一个最容易检验的情形：刚体绕空间原点的 z 轴转动，$\omega_s=(0,0,1)^T$。此时 $v_s=0$，但位于 $p=(1,0,0)^T$ 的物体原点仍有 $\dot p=(0,1,0)^T$。零的是旋量中的平移分量，并不是所有点都没有线速度。这些定义对应原书第 2 章的刚体速度推导。作者公开版

伴随变换：改变旋量的表达坐标

由 $\widehat V_s=g\widehat V_bg^{-1}$ 得到

$$V_s=\operatorname{Ad}_gV_b,\qquad \operatorname{Ad}_g= \begin{bmatrix}R&\widehat pR\\0&R\end{bmatrix}.$$

右上角的项说明，角速度在更换参考原点时还会影响线性部分。只把两个三维向量分别乘以 $R$，遗漏了这个原点变化。

与旧 Modern Robotics 笔记如何对接

原有刚体运动笔记采用 Modern Robotics 常用的 $(\omega,v)$ 次序。该书官方旋量讲解也按角分量在前叙述。两种次序都可以，不能直接混用矩阵块：

$$P=\begin{bmatrix}0&I_3\\I_3&0\end{bmatrix},\qquad V_{\mathrm{MR}}=PV_{\mathrm{MLS}},\qquad \operatorname{Ad}^{\mathrm{MR}}_g=P\operatorname{Ad}^{\mathrm{MLS}}_gP^T.$$

雅可比也要按 $J_{\mathrm{MR}}=PJ_{\mathrm{MLS}}$ 换行。MLS 力旋量配套使用 $F=(f,\tau)$，使功率为 $F^TV=f^Tv+\tau^T\omega$；改成另一种排列时，力旋量也要一起置换。

5. 从瞬时速度积累成有限运动

若角速度方向固定，单位轴为 $\omega$，累计角度为 $\theta$，旋转矩阵为 $R(\theta)=e^{\widehat\omega\theta}$。因为单位轴的叉乘矩阵满足 $\widehat\omega^3=-\widehat\omega$，把指数级数中的奇偶幂归组，得到 Rodrigues 公式：

$$R(\theta)=I+\sin\theta\,\widehat\omega +(1-\cos\theta)\widehat\omega^2.$$

有限刚体运动同样写成 $e^{\widehat\xi\theta}$。这里 $\xi=(v,\omega)$ 是归一化的运动轴，不是已经包含速度大小的瞬时旋量 $V$；沿固定轴运动时才有 $V=\xi\dot\theta$。

对转动关节，$\|\omega\|=1$，$\theta$ 是转角；对移动关节，$\omega=0$、$\|v\|=1$，$\theta$ 是移动距离。速度 $V$ 中的线性部分单位为米/秒，角速度为弧度/秒，不能把六项无权重地当成同一种物理量。

绕不经过原点的轴旋转

设转轴经过 $q=(1,0,0)^T$，方向 $\omega=(0,0,1)^T$，没有沿轴平移。归一化轴为

$$v=-\omega\times q=(0,-1,0)^T,\qquad \xi=\begin{bmatrix}0&-1&0&0&0&1\end{bmatrix}^T.$$

旋转后的点为 $x'=R(x-q)+q$，因此刚体变换的平移项为 $p=q-Rq$。转 90° 后，原点移动到 $(1,-1,0)^T$。先在图里找圆心，再画圆弧，能避免误以为 $v$ 只是沿转轴的速度。

更一般的螺旋运动可以沿轴同时平移，螺距为 $h$ 时 $v=-\omega\times q+h\omega$。对 $\|\omega\|=1$：

$$e^{\widehat\xi\theta}= \begin{bmatrix}R(\theta)&G(\theta)v\\0&1\end{bmatrix}, \qquad G(\theta)=I\theta+(1-\cos\theta)\widehat\omega +(\theta-\sin\theta)\widehat\omega^2.$$

纯平移要单独用 $R=I,p=v\theta$。不要把 $\omega=0$ 塞进要求单位转轴的推导，却仍沿用轴归一化步骤。

6. 数值实现时先检查这三件事

1. 角度接近零： 1-cos(theta) 和 theta-sin(theta) 会损失有效数字。用泰勒展开、稳定的专用函数，并测试 $e^0=I$。
2. 从矩阵取对数： 轴角表示不是全局唯一的。角度接近 $\pi$ 时轴提取敏感；恰好 $\pi$ 时相反的轴给出同一旋转。轨迹中的对数分支跳变不能当作突然出现的真实角速度。
3. 矩阵是否仍是旋转： 积分和测量误差可能使 $R^TR$ 偏离 $I$。先检查正交性与行列式，再决定是否投影回 $SO(3)$；逐项修正并不能自动维持旋转约束。

7. 手算练习与答案

练习 A： 第一个例子中的点改成 $x_b=(0,1,0)^T$，求 $x_a$。答案：先旋转得到 $(-1,0,0)^T$，再加平移得到 $(0,2,0)^T$。

练习 B： $p=(1,0,0)^T$、$\omega_s=(0,0,2)^T$、$\dot p=(0,2,0)^T$，求 $v_s$。答案：$v_s=\dot p-\omega_s\times p=0$；物体原点的线速度仍非零。

练习 C： 绕经过 $(1,0,0)$ 的 z 轴转 180°，原点到哪里？答案：$q-R_z(\pi)q=(2,0,0)^T$。固定轴不动，原点绕轴走半个圆。

完成后应能把“点坐标”“位姿”“原点速度”“旋量”四件事分开解释。下一篇把这些几何对象接到两关节机械臂，推导 PoE 正运动学与雅可比。