Planning Algorithms · Steven M. LaValle · Chapter 3
几何表示与变换
运动规划要回答的问题是「机器人能不能从这里走到那里」。在回答之前,必须先用数学把两件事说清楚:世界里有什么(障碍和机器人长什么样),以及机器人怎么动(一个姿态参数 $q$ 如何决定机器人占据空间的哪一块)。第 3 章就是这两件事的全部基础。
导读这一章在干什么
全书 Part II 讨论运动规划,而规划算法要操作的对象,是「几何体 + 变换」。本章用一句话概括:
障碍区域 $\mathcal O \subseteq \mathcal W$ 用若干原语(primitive)的布尔组合表示;机器人 $\mathcal A$ 先在自己的体坐标系(body frame)里定义,再由参数 $q$ 决定的刚体变换 $h(q,\cdot)$ 放进世界 $\mathcal W$;多个刚体靠齐次变换矩阵连乘 $T_1T_2\cdots T_m$ 串起来;有分叉就走树,有环就加闭合约束。
怎么读这份教程
3.1几何建模 Geometric Modeling
先定义世界 $\mathcal W$:要么 $\mathcal W=\mathbb R^2$(2D),要么 $\mathcal W=\mathbb R^3$(3D)。世界里有两类东西:
- 障碍(obstacles):永久占据的部分,比如建筑物的墙。所有障碍点的集合记为障碍区域 $\mathcal O\subseteq\mathcal W$。
- 机器人(robots):有几何模型、可以被运动规划控制的物体,记为 $\mathcal A$。
「机器人」是个泛称:可以是真机器人,也可以是柔性分子(障碍是折叠的蛋白质),或者游戏里的虚拟人。两者都当作 $\mathcal W$ 的闭子集(包含边界)。
描述一个几何体有两条路:
| 方式 | 思想 | 行星的例子 | 多边形的例子 |
|---|---|---|---|
| 边界表示 boundary representation | 写出表面方程/边界元素 | 一个和地表重合的球面方程 | 按逆时针列出顶点序列 |
| 实体表示 solid representation | 描述「所有被包含的点」构成的集合 | 球面内所有点的集合 | 若干半平面的交集 |
本节的主线是实体表示:用容易计算的原语 $H_i$ 做有限次并、交、差,拼出复杂的 $\mathcal O$。
3.1.1多边形与多面体模型
凸集
$X\subset\mathbb R^n$ 是凸的,当且仅当任意两点间的线段都在 $X$ 内:对任意 $x_1,x_2\in X$ 和 $\lambda\in[0,1]$,
$$\lambda x_1+(1-\lambda)x_2\in X. \tag{3.1}$$直觉:没有「坑」和「缩进」。不凸的集合叫 nonconvex(书里特意不叫 concave,那个词更适合描述透镜)。
凸多边形 = 半平面的交
一个 $m$ 边凸多边形的边界表示是逆时针顺序的 $m$ 个顶点 $(x_1,y_1),\dots,(x_m,y_m)$。它的实体表示则是 $m$ 个半平面的交。关键工具是一条直线对应的函数
$$f(x,y)=ax+by+c,$$它在直线一侧为负、另一侧为正、线上为零,并且可以理解为到直线的带符号距离(差一个比例)。约定:沿边 $(x_i,y_i)\to(x_{i+1},y_{i+1})$ 的方向,左侧 $f<0$(如果不是,就把 $f$ 乘上 $-1$)。由于顶点逆时针排列,多边形内部始终在每条边的左侧。
设边从 $p_1=(x_1,y_1)$ 到 $p_2=(x_2,y_2)$。取 $f(x,y)=(y_2-y_1)(x-x_1)-(x_2-x_1)(y-y_1)$, 即 $a=y_2-y_1$,$b=-(x_2-x_1)$,$c=-(ax_1+by_1)$。它就是叉积 $(p_2-p_1)\times(p-p_1)$ 的相反数,所以 $p$ 在左侧时 $f<0$。
每条边定义一个原语
$$H_i=\{(x,y)\in\mathcal W\mid f_i(x,y)\le 0\}, \tag{3.2}$$凸多边形障碍就是它们的交
$$\mathcal O=H_1\cap H_2\cap\cdots\cap H_m. \tag{3.3}$$半平面的交 = 凸多边形(对应 Fig 3.1、3.2)
拖动圆点测试任意位置;把鼠标悬停在某条边上,会显示这条边对应的半平面 $H_i$($f_i\le 0$ 的一侧)。右侧实时给出每个 $f_i$ 的值和谓词 $\alpha=e_1\wedge\cdots\wedge e_5$ 的结果。
非凸多边形 = 凸多边形的并
凸的假设太受限。非凸的多边形区域写成
$$\mathcal O=\mathcal O_1\cup\mathcal O_2\cup\cdots\cup\mathcal O_n, \tag{3.4}$$每个 $\mathcal O_i$ 是按 (3.3) 表示的凸多边形,彼此允许重叠。几点说明:
- 分解不唯一。用最少的原语做分解看起来很美,但自动化地求最优分解可能是 NP-hard 的(6.5.1 节)。实用的做法是第 6.2.2 节的垂直单元分解。
- 集合差可以消掉:要「挖掉」原语 $H_i=\{f_i<0\}$,等价于保留 $f_i\ge0$,即 $-f_i\le0$,这又是一个新原语 $H'_i$。所以任意由并、交、差组成的表达式,用布尔代数都能化成「若干交的并」。
从集合到逻辑谓词:一个 O(n) 的碰撞检测器
这套表示的价值在哪?它直接给出一个谓词(predicate)$\phi:\mathcal W\to\{\text{true},\text{false}\}$,判断点是否在障碍里。令 $e_i(x,y)$ 在 $f_i(x,y)\le0$ 时为真。凸多边形对应合取(AND):
$$\alpha(x,y)=e_1(x,y)\wedge e_2(x,y)\wedge\cdots\wedge e_m(x,y), \tag{3.5}$$$n$ 个凸块的并对应析取(OR):
$$\phi(x,y)=\alpha_1(x,y)\vee\alpha_2(x,y)\vee\cdots\vee\alpha_n(x,y). \tag{3.6}$$每个原语常数时间可算,所以点碰撞检测是 $O(n)$,$n$ 是原语总数。集合论的 $\cap,\cup$ 与逻辑的 $\wedge,\vee$ 一一对应;「任何逻辑式都能化为析取范式(sum of products)」正是「$\mathcal O$ 总能写成交的并」的另一种说法。
多面体模型(3D)
把多边形换成多面体,半平面换成半空间,一切类比成立。边界表示有三种特征:顶点、边、面。一个常用数据结构是双向连接边表(doubly connected edge list,DCEL):
实体表示:每个面确定一个平面 $ax+by+cz+d=0$,令 $f(x,y,z)=ax+by+cz+d$,半空间 $H_i=\{f_i\le0\}$,凸多面体 $\mathcal O=\bigcap H_i$,非凸多面体是凸多面体的并。怎样保证 $f_i$ 在多面体内部为负?2D 时靠逆时针顺序;3D 时用半边结构取出每个面逆时针(相对外法向)的顶点环,取连续三个不共线的顶点 $p_1,p_2,p_3$,令 $v_{12}=p_2-p_1,\ v_{23}=p_3-p_2$,则叉积
$$v=v_{12}\times v_{23}$$必定指向多面体外侧且垂直于面。取 $a=v[1],b=v[2],c=v[3]$,内部就满足 $f\le0$。
3.1.2半代数模型 Semi-Algebraic Models
到目前为止 $f$ 都是一次函数。把 $f$ 放宽为任意实系数多项式,就得到半代数模型:
$$H=\{(x,y)\in\mathcal W\mid f(x,y)\le0\}. \tag{3.10}$$- 单个多项式原语确定的点集叫代数集(algebraic set);代数集经有限次并、交得到的叫半代数集(semi-algebraic set)。多边形/多面体模型是一次多项式的特例。
- 例:$f=x^2+y^2-4$ 给出以原点为心、半径 2 的圆盘。
- 与多边形不同,半代数原语的交不一定凸。
- 谓词 $\phi$ 照样可以建,碰撞检测仍然是原语个数的线性时间:评估一个多项式就行。
例 3.1(姜饼人脸):外圆半径 $r_1$ 在原点;两只眼睛半径 $r_2,r_3$ 在 $(x_2,y_2),(x_3,y_3)$;嘴是中心 $(0,y_4)$、半轴 $a,b$ 的椭圆。
$$\begin{aligned} f_1&=x^2+y^2-r_1^2,\\ f_2&=-\big((x-x_2)^2+(y-y_2)^2-r_2^2\big),\\ f_3&=-\big((x-x_3)^2+(y-y_3)^2-r_3^2\big),\\ f_4&=-\big(x^2/a^2+(y-y_4)^2/b^2-1\big), \end{aligned}\tag{3.11}$$ $$\mathcal O=H_1\cap H_2\cap H_3\cap H_4. \tag{3.12}$$$f_2,f_3,f_4$ 前的负号是精髓:圆/椭圆方程本来 $\le0$ 表示「内部」,乘 $-1$ 后 $\le0$ 表示「外部」,于是「与外部求交」就等于「挖掉」。
姜饼人脸:四个代数原语的交(对应 Fig 3.4b、例 3.1)
着色区域是当前的 $\mathcal O$。每个原语可以从交集中「移除」,也可以把它的符号翻转。拖动探针点查看四个 $f_i$ 的取值。
哪些关系真的增加了表达力?
除了 $f\le0$,还可以定义 $f\ge0,\ f=0,\ f<0,\ f\ne0$ 等原语,但大多数并不扩大能表示的模型类,只是用着方便:
- $\{f\ge0\}$ 等价于 $\{-f\le0\}$,换个多项式即可。
- $\{f=0\}=\{f\le0\}\cap\{-f\le0\}$,两个原语的交。
- $\{f<0\}$ 确实增加了表达力:它能表示不含边界的开集(比如球面内部但不含球面),这在第 4 章会用到。等价地,保留 $\le$ 但允许集合补运算也行。
3.1.3其他模型
第 6 章的组合规划方法直接在表示上做几何运算,表示方式是算法的一部分。第 5 章的采样规划方法只通过「碰撞检测」这个黑箱接触几何,表示方式对规划器几乎不可见。所以下面这些模型更多出现在采样方法里,但别彻底忘了表示的存在。
非凸多边形/多面体的直接边界表示
不必拆成凸块,直接按逆时针列出顶点 $(x_1,y_1),\dots,(x_m,y_m)$,相邻点之间(以及首尾之间)有边。顶点应选得使多边形简单(边不相交),这样内部是良定义的:每条边的左侧。
3D 三角形网格
每个三角形由三个点给出,是图形学中最常用的模型(显卡原生支持)。规定三角形内部属于模型,两个三角形「碰撞」指一个戳进了另一个。优点是毫无约束,缺点也是毫无约束:没有一致性可以利用,很难判断一个点在 3D 障碍「里面」还是「外面」。为了去掉共享顶点的冗余,有三角形带(triangle strip,相邻三角形共享一条边)和三角形扇(triangle fan,所有三角形共享一个顶点)。
NURBS(非均匀有理 B 样条)
工程设计系统(飞机、汽车车身)常用。它和半代数模型的本质区别是:半代数是隐式方程 $f(x,y)\le0$,样条是参数方程 $C(u)$。参数形式渲染容易、碰撞检测难。2D 曲线写成
$$C(u)=\frac{\sum_{i=0}^{n}w_iP_iN_{i,k}(u)}{\sum_{i=0}^{n}w_iN_{i,k}(u)}, \tag{3.18}$$$w_i$ 是权重,$P_i$ 是控制点,$N_{i,k}$ 是 $k$ 次基函数,由节点向量 $\{t_0,\dots,t_m\}$(非减序列)递归定义:
$$N_{i,k}(u)=\Big(\frac{u-t_i}{t_{i+k}-t_i}\Big)N_{i,k-1}(u)+\Big(\frac{t_{i+k+1}-u}{t_{i+k+1}-t_{i+1}}\Big)N_{i+1,k-1}(u), \tag{3.19}$$递归基:$N_{i,0}(u)=1$ 若 $t_i\le u 把有界区域离散成矩形格子,每格 1 表示含有 $\mathcal O$ 的点、0 表示不含。分辩率决定格子数和近似质量。它不优雅,却在应用中到处出现:移动机器人用传感器建的地图就是位图。推广为占据栅格(occupancy grid):每格存一个数,比如「有障碍的概率」或「穿越代价」(行星车地形图)。 $f_i$ 不一定要是多项式。超椭球(superellipsoid)在 3D 里是 $n_1=n_2=2$ 时是椭球;指数越大越像圆角盒子。 2D 截面 $|x/a|^{n}+|y/b|^{n}\le1$。拉动 $n$,看一个原语就能表达的形状家族。 普通圆柱的中轴是直线、半径是常数;广义圆柱把中轴换成一条脊线 $(x(s),y(s),z(s))$,$s\in[0,1]$,半径换成函数 $r(s)$。在 $s$ 处的截面是垂直于脊线切向、半径 $r(s)$ 的圆。位图与占据栅格
超二次曲面
超椭圆:一个参数从椭圆连续变到方盒
广义圆柱
3.2刚体变换 Rigid-Body Transformations
3.1 节的任何方法都可以同时用来定义障碍 $\mathcal O$ 和机器人 $\mathcal A$。区别在于:$\mathcal O$ 在 $\mathcal W$ 里固定不动,而规划问题要让 $\mathcal A$「动起来」。
3.2.1一般概念
机器人 $\mathcal A$ 定义为 $\mathbb R^2$ 或 $\mathbb R^3$ 的子集(维度与 $\mathcal W$ 一致)。一个刚体变换是函数 $h:\mathcal A\to\mathcal W$,把 $\mathcal A$ 的每个点映射到 $\mathcal W$ 中,且满足两条:
- 任意两点间的距离保持不变;
- 定向保持不变(不允许「镜像」)。
$h(a)$ 是点 $a\in\mathcal A$ 在 $\mathcal W$ 中「占据」的位置,整个机器人占据的区域是像集
$$h(\mathcal A)=\{h(a)\in\mathcal W\mid a\in\mathcal A\}. \tag{3.21}$$变换模型时,到底用 h 还是 h⁻¹?
这是本节最容易出错、也最值得搞清楚的一点。如果 $\mathcal A$ 是边界表示(一串顶点),直接对每个顶点算 $h(a)$ 就完了。但若 $\mathcal A$ 用原语 $H_i=\{a\in\mathbb R^2\mid f_i(a)\le0\}$ 定义,变换后的集合是
$$h(H_i)=\{h(a)\in\mathcal W\mid f_i(a)\le0\}. \tag{3.23}$$这个写法以 $a$ 为变量,不方便。我们想直接用 $\mathcal W$ 中的点 $w$ 来描述。因为 $a=h^{-1}(w)$,
$$h(H_i)=\{w\in\mathcal W\mid f_i(h^{-1}(w))\le0\}. \tag{3.24}$$要判断世界点 $w$ 是否在变换后的机器人里,需要把 $w$「拉回」体坐标系再代入 $f_i$,所以是 $f_i(h^{-1}(w))$。顶点用 $h$ 正着推,原语用 $h^{-1}$ 倒着拉。例 3.2 会具体看到。
参数化的变换族
规划关心的不是一个变换,而是一族由参数 $q\in\mathbb R^n$ 选出的变换。于是 $h$ 接受两个变量:$h(q,a)$,整个机器人变成 $h(q,\mathcal A)\subset\mathcal W$。书中用简写
$$a(q):=h(q,a),\qquad \mathcal A(q):=h(q,\mathcal A).$$这记法略有滥用,但很形象:调节 $q$,机器人 $\mathcal A(q)$ 在 $\mathcal W$ 里到处跑。$\mathcal A(q)$ 可以看作一个集合值函数。遇到困惑时(尤其分不清该用 $h$ 还是 $h^{-1}$),回到这里的定义。
坐标系
- 世界坐标系(world frame):$\mathcal W$ 的原点和基向量,所有 $w\in\mathcal W$ 都用它表达。作用类比牛顿力学的惯性系。
- 体坐标系(body frame):定义 $\mathcal A$ 时用的 $\mathbb R^2$/$\mathbb R^3$ 的原点和基,可以想象成画在机器人身上。
变换机器人 = 把它的模型从体坐标系换到世界坐标系,效果是把 $\mathcal A$ 以某个位置和朝向「放进」$\mathcal W$。3.3 节中每个刚体都有自己的体坐标系,变换就是把所有物体都表达到世界坐标系里。
3.2.22D 变换
平移
两个参数 $q=(x_t,y_t)$:
$$h(x,y)=(x+x_t,\;y+y_t). \tag{3.25}$$边界表示:每个顶点 $(x_i,y_i)\mapsto(x_i+x_t,y_i+y_t)$。实体表示:原语 $H_i=\{f(x,y)\le0\}$ 变成
$$h(H_i)=\{(x,y)\in\mathcal W\mid f(x-x_t,\;y-y_t)\le0\}. \tag{3.27}$$例 3.2(平移圆盘):单位圆盘 $H=\{x^2+y^2-1\le0\}$ 平移后是 $\{(x-x_t)^2+(y-y_t)^2-1\le0\}$,正是圆心在 $(x_t,y_t)$ 的圆盘方程。注意减号:这就是 $h^{-1}$。
平移后的机器人记作 $\mathcal A(x_t,y_t)$;$(0,0)$ 是恒等变换。自由度(degrees of freedom,DOF)指完全刻画变换所需的独立参数的最大个数:若 $(x_t,y_t)$ 的取值集合是 $\mathbb R^2$ 的二维子集,自由度为 2。
① 世界系不动,机器人被变换;② 机器人不动,世界系被(反向)变换。前者是从固定世界看变换的效果,后者是从机器人视角看。运动规划默认用第一种,因为它建模的是「机器人在物理世界里移动」。很多坐标变换的书用第二种,导致公式看起来「反过来」。Lab 4 里有一个开关可以切换这两种视角。
旋转
绕原点逆时针转 $\theta\in[0,2\pi)$:
$$(x,y)\mapsto(x\cos\theta-y\sin\theta,\;x\sin\theta+y\cos\theta), \tag{3.30}$$写成旋转矩阵
$$R(\theta)=\begin{pmatrix}\cos\theta&-\sin\theta\\ \sin\theta&\cos\theta\end{pmatrix}. \tag{3.31}$$三个值得记住的观察:
- 列向量 = 新基向量。$R(\theta)$ 的第一列是体坐标系 $x$ 轴转过 $\theta$ 后的方向,第二列是 $y$ 轴转后的方向。两列是单位向量且互相正交。这个解释对任意维度的旋转矩阵都成立。
- 旋转绕的是原点。因此定义 $\mathcal A$ 时,应把原点放在你希望它绕着转的轴上(3.3 节放在关节处)。
- 变换半代数原语要用逆旋转 $R(-\theta)=R(\theta)^{\mathsf T}$。
旋转 + 平移:齐次变换矩阵
先转 $\theta$、再平移 $(x_t,y_t)$,可以把机器人摆到任意位姿。每点变为
$$\begin{pmatrix}x\cos\theta-y\sin\theta+x_t\\ x\sin\theta+y\cos\theta+y_t\end{pmatrix}. \tag{3.33}$$把坐标补一个 1,用一次矩阵乘法得到同样结果:
$$\begin{pmatrix}\cos\theta&-\sin\theta&x_t\\ \sin\theta&\cos\theta&y_t\\ 0&0&1\end{pmatrix}\begin{pmatrix}x\\y\\1\end{pmatrix}=\begin{pmatrix}x\cos\theta-y\sin\theta+x_t\\ x\sin\theta+y\cos\theta+y_t\\1\end{pmatrix}. \tag{3.34}$$这个 $3\times3$ 矩阵
$$T=\begin{pmatrix}\cos\theta&-\sin\theta&x_t\\ \sin\theta&\cos\theta&y_t\\ 0&0&1\end{pmatrix} \tag{3.35}$$叫齐次变换矩阵(homogeneous transformation matrix)。它表示先旋转再平移,不是反过来。称「齐次」是因为在 $\mathbb R^3$ 里它只是一个没有平移的线性变换:升一维把平移「吸收」进线性变换,是射影几何的常见技巧。变换后的机器人记作 $\mathcal A(x_t,y_t,\theta)$,共 3 个自由度。
先转再平移得到 $Rp+t$;先平移再转得到 $R(p+t)=Rp+Rt$。两者仅在 $Rt=t$ 时相同。书中约定 $T$ 永远是「先转再平移」。
2D 刚体变换与齐次矩阵(对应 (3.35)、Fig 3.7)
红色 L 形机器人在体坐标系中定义,原点在它的角上。拉动 $x_t, y_t, \theta$,右侧矩阵实时更新;淡色轮廓是未变换的 $\mathcal A$。小圆点是体坐标 $a=(1.5,0.3)$ 的点,用来验证 $w=Ta$。
3.2.33D 变换
3D 的平移和 2D 一样简单:$(x,y,z)\mapsto(x+x_t,y+y_t,z+z_t)$,原语变为 $\{f_i(x-x_t,y-y_t,z-z_t)\le0\}$。难点全在旋转。
Yaw、Pitch、Roll
借用航空术语,绕三个正交轴的旋转分别叫:
| 名称 | 绕哪个轴 | 角 | 矩阵 | 直觉(飞机机头朝 +x) |
|---|---|---|---|---|
| Yaw 偏航 | $z$ 轴 | $\alpha$ | $R_z(\alpha)=\begin{pmatrix}\cos\alpha&-\sin\alpha&0\\ \sin\alpha&\cos\alpha&0\\0&0&1\end{pmatrix}$ (3.39) | 像开车左转 |
| Pitch 俯仰 | $y$ 轴 | $\beta$ | $R_y(\beta)=\begin{pmatrix}\cos\beta&0&\sin\beta\\0&1&0\\-\sin\beta&0&\cos\beta\end{pmatrix}$ (3.40) | 机头抬起/低下 |
| Roll 翻滚 | $x$ 轴 | $\gamma$ | $R_x(\gamma)=\begin{pmatrix}1&0&0\\0&\cos\gamma&-\sin\gamma\\0&\sin\gamma&\cos\gamma\end{pmatrix}$ (3.41) | 机翼左右倾斜 |
每个矩阵都是 2D 旋转矩阵的简单扩展:$R_z(\alpha)$ 的左上 $2\times2$ 块就是 $R(\alpha)$,第三行第三列像单位阵,表示 $z$ 坐标不变。三者相乘得到一个能表示任意朝向的矩阵:
$$R(\alpha,\beta,\gamma)=R_z(\alpha)R_y(\beta)R_x(\gamma)=\begin{pmatrix} c\alpha\,c\beta & c\alpha\,s\beta\,s\gamma-s\alpha\,c\gamma & c\alpha\,s\beta\,c\gamma+s\alpha\,s\gamma\\ s\alpha\,c\beta & s\alpha\,s\beta\,s\gamma+c\alpha\,c\gamma & s\alpha\,s\beta\,c\gamma-c\alpha\,s\gamma\\ -s\beta & c\beta\,s\gamma & c\beta\,c\gamma \end{pmatrix} \tag{3.42}$$($c=\cos,\ s=\sin$。)
$R(\alpha,\beta,\gamma)\,p=R_z(\alpha)\big(R_y(\beta)\big(R_x(\gamma)p\big)\big)$:最靠近 $p$ 的 $R_x$ 最先作用。换顺序会得到不同的矩阵。更微妙的是:最后施加的 yaw 是绕世界系的 $z$ 轴,不是飞机此刻的机体 $z$ 轴。若 $\beta=\gamma=0$,两者重合,yaw 感觉像转车;但 $\beta,\gamma$ 任意时,飞机已经歪了,yaw 轴和机身不再对齐。每个从左边乘进来的新矩阵都不关心机体系,它只是把 $\mathbb R^3$ 中所有点按世界系转一下。
3D 旋转有三个参数 $\alpha,\beta,\gamma$,2D 只有一个 $\theta$。变换后的机器人记作 $\mathcal A(\alpha,\beta,\gamma)$。
从旋转矩阵反解 yaw、pitch、roll
给定任意旋转矩阵 $(r_{ij})$,与 (3.42) 逐项比较:$r_{21}/r_{11}=\tan\alpha$,$r_{32}/r_{33}=\tan\gamma$,$r_{31}=-\sin\beta$,$\sqrt{r_{32}^2+r_{33}^2}=\cos\beta$。反正切有四个象限可选,按分子分母的符号确定,这正是 C 语言 atan2 做的事:
此法假定 $r_{11}\ne0,\ r_{33}\ne0$($\beta=\pm\pi/2$ 时失效,即万向节死锁,见习题 2)。
Yaw-Pitch-Roll:顺序与世界轴(对应 Fig 3.8、(3.42))
灰色是世界系,彩色三轴是画在飞机上的体坐标系(机头 +x)。拖动画布旋转视角。右侧给出 $R(\alpha,\beta,\gamma)$ 的数值,以及用 atan2 从矩阵反解出的三个角,用来验证 (3.47–3.49)。
3D 齐次变换矩阵
把 $R(\alpha,\beta,\gamma)$ 放进 $4\times4$ 矩阵的左上角,平移放最后一列:
$$T=\begin{pmatrix} R(\alpha,\beta,\gamma) & \begin{matrix}x_t\\y_t\\z_t\end{matrix}\\ \begin{matrix}0&0&0\end{matrix} & 1\end{pmatrix} \tag{3.50}$$它表示的操作顺序是:① roll $\gamma$,② pitch $\beta$,③ yaw $\alpha$,④ 平移 $(x_t,y_t,z_t)$。能平移又能旋转的 3D 刚体有 6 个自由度,记作 $\mathcal A(x_t,y_t,z_t,\alpha,\beta,\gamma)$。
yaw-pitch-roll(欧拉角的一种,共有 12 种轴序变体)最容易理解,适合做快速的 3D 动画,但它没有正确刻画旋转空间的结构:同一个旋转对应多组角,$\beta=\pm\pi/2$ 时还退化。规划算法在意的是状态空间的拓扑,所以第 4.2 节会引入四元数。两种都要会:欧拉角用来构造例子,四元数用来做规划。
3.3运动链的变换 Transforming Kinematic Chains
多个刚体连在一起,变换就复杂了。每个刚体叫一个连杆(link),记 $\mathcal A_1,\dots,\mathcal A_m$。对 $1\le i
3.3.12D 运动链
连接会吃掉自由度
两个互不相连的 2D 刚体各有 3 个自由度,合计 6 个:$x_1,y_1,\theta_1,x_2,y_2,\theta_2$。连接处叫关节(joint)。2D 有两种:
- 转动关节(revolute joint):一个连杆只能相对另一个转动。
- 移动关节(prismatic joint):一个连杆沿另一个滑动。
每种关节都从这对刚体上拿掉 2 个自由度。以转动关节为例:$\mathcal A_2$ 体坐标系的原点 $(0,0)$ 永久固定在 $\mathcal A_1$ 体坐标系中的某点 $(x_a,y_a)$ 上,于是 $\mathcal A_2$ 的平移完全由 $x_a,y_a$ 决定(它们又取决于 $x_1,y_1,\theta_1$)。两杆合计 4 个自由度:$x_1,y_1,\theta_1,\theta_2$。
体坐标系怎么放
单刚体时,体坐标系原点决定旋转轴。对链中的连杆,为了少吃苦头,约定(Fig 3.10):
- $\mathcal A_i$($i>1$)的原点放在它与 $\mathcal A_{i-1}$ 的关节处;
- $x_i$ 轴沿 $\mathcal A_i$ 上两个关节的连线方向;
- 最后一杆 $\mathcal A_m$ 的 $x_m$ 轴可任意,原点仍在与 $\mathcal A_{m-1}$ 的关节处;
- 第一杆 $\mathcal A_1$ 按单刚体的考虑放置。
令 $a_{i-1}$ 为 $\mathcal A_{i-1}$ 上两个关节之间的距离,$\theta_i$ 为 $\mathcal A_i$ 相对 $\mathcal A_{i-1}$ 的转角。
2D 链的齐次变换矩阵
第一杆用普通的 (3.35):
$$T_1=\begin{pmatrix}\cos\theta_1&-\sin\theta_1&x_t\\ \sin\theta_1&\cos\theta_1&y_t\\0&0&1\end{pmatrix}. \tag{3.51}$$其后每一杆($1 $$T_i=\begin{pmatrix}\cos\theta_i&-\sin\theta_i&a_{i-1}\\ \sin\theta_i&\cos\theta_i&0\\0&0&1\end{pmatrix}, \tag{3.52}$$
含义是:① 逆时针转 $\theta_i$;② 沿 $x$ 轴平移 $a_{i-1}$。$T_i$ 表达的是 $\mathcal A_i$ 的体坐标系相对 $\mathcal A_{i-1}$ 的体坐标系的差别:对 $\mathcal A_i$ 的点施加 $T_i$,就把它从自己的体坐标系搬到了 $\mathcal A_{i-1}$ 的体坐标系;再施加 $T_{i-1}$,又搬到 $\mathcal A_{i-2}$ 的体坐标系……一路搬到世界:
$$T_1T_2\cdots T_m\begin{pmatrix}x\\y\\1\end{pmatrix}. \tag{3.53}$$$T_1T_2T_3\,p$ 先算 $T_3p$($\mathcal A_3$ 的点 → $\mathcal A_2$ 的坐标系),再 $T_2(\cdot)$(→ $\mathcal A_1$ 的坐标系),最后 $T_1(\cdot)$(→ 世界)。每一步都只处理「相邻两杆的相对位姿」,这就是为什么把 $T_i$ 定义成「相对父杆」的差别后,乘积自然就是绝对位姿。例 3.3 和 Lab 6 把这个过程拆成四步看。
转动关节:$a_i$ 是常数,$\theta_i$ 是变量,第 $m$ 杆写成 $\mathcal A_m(x_t,y_t,\theta_1,\dots,\theta_m)$;若第一杆固定在世界中,则只剩 $\mathcal A_m(\theta_1,\dots,\theta_m)$。移动关节则反过来:$a_i$ 是变量、$\theta_i$ 是常数。两种关节可以混在同一条链里。
三连杆:一步一步把 $\mathcal A_3$ 搬进世界(对应例 3.3、Fig 3.11)
选择「阶段」观察:$\mathcal A_3$ 在自己的体坐标系里 → 施加 $T_3$ 进入 $\mathcal A_2$ 的坐标系 → 施加 $T_2$ 进入 $\mathcal A_1$ 的坐标系 → 施加 $T_1$ 进入世界。每个阶段里,画布中央的坐标系就是「当前所在的坐标系」。末端点是 $\mathcal A_3$ 体坐标 $(a_3,0)$。
3.3.23D 运动链与 DH 参数
3D 的难点仍然是旋转,而且关节类型更多。由两个物体之间 2D 面接触产生的关节有六种(Fig 3.12):
| 关节 | 自由度 | 允许的运动 |
|---|---|---|
| Revolute 转动 | 1 | 绕一根轴转 |
| Prismatic 移动 | 1 | 沿一根轴滑 |
| Screw 螺旋 | 1 | 绕轴转的同时按固定比例沿轴进(螺丝) |
| Cylindrical 圆柱 | 2 | 绕轴转 + 沿轴滑,彼此独立 |
| Spherical 球 | 3 | 绕一点任意转(肩关节,受限版) |
| Planar 平面 | 3 | 在平面内平移 + 转动 |
主思路与 2D 完全一致,四步走:① 为每个 $\mathcal A_i$ 小心放置体坐标系;② 根据关节关系量出几个参数;③ 参数定义齐次矩阵 $T_i$;④ $\mathcal A_m$ 上任意点在世界中的位置是 $T_1T_2\cdots T_m\,p$。
坐标系的放法:z 轴是转轴,x 轴是公垂线
设每对相邻杆由转动关节相连。2D 时坐标系依据「连接点」,3D 时依据「转动轴」(它是连接点在 3D 的对应物)。这些轴一般是 $\mathbb R^3$ 中的异面直线(skew lines,Fig 3.14)。
- $z_i$ 轴:把 $\mathcal A_i$ 连到 $\mathcal A_{i-1}$ 的转动关节的转轴。
- $x_i$ 轴:连接 $z_i$ 与 $z_{i+1}$ 两条轴之间最近的一对点,原点在 $z_i$ 上,方向指向 $z_{i+1}$ 上的最近点。两轴不平行时它唯一确定。
- $y_i$ 轴:右手系补齐。
这个坐标系的原点可能根本不在 $\mathcal A_i$ 的实体上,看起来别扭,但换来的是形式极简的变换矩阵。
四个 Denavit–Hartenberg 参数
2D 的 $T_i$ 只需 $a_{i-1},\theta_i$ 两个参数,3D 需要四个(Fig 3.15):
| 参数 | 量的是 | 沿/绕 | Fig 3.15 |
|---|---|---|---|
| $d_i$ | $x_{i-1}$ 轴与 $x_i$ 轴分别和 $z_i$ 轴相交的两点之间的带符号距离($x_i$ 在 $x_{i-1}$「上方」为正) | 沿 $z_i$ | (a) |
| $\theta_i$ | $x_{i-1}$ 转到与 $x_i$ 重合所需的角 | 绕 $z_i$ | (b) |
| $a_{i-1}$ | $z_{i-1}$ 轴与 $z_i$ 轴之间的距离(沿公垂线) | 沿 $x_{i-1}$ | (c) |
| $\alpha_{i-1}$ | $z_{i-1}$ 转到与 $z_i$ 平行所需的角 | 绕 $x_{i-1}$ | (d) |
$d_i,\theta_i$ 带下标 $i$,描述的是绕/沿 $z_i$ 的量;$a_{i-1},\alpha_{i-1}$ 带下标 $i-1$,描述的是绕/沿 $x_{i-1}$ 的量。记住「$z$ 的量下标 $i$,$x$ 的量下标 $i-1$」就不会乱。任何参数都可以为正、零、负。
两个「螺旋」拼出 Ti
第一个矩阵绕 $z_i$ 转 $\theta_i$ 并沿 $z_i$ 平移 $d_i$:
$$R_i=\begin{pmatrix}\cos\theta_i&-\sin\theta_i&0&0\\ \sin\theta_i&\cos\theta_i&0&0\\0&0&1&d_i\\0&0&0&1\end{pmatrix}. \tag{3.54}$$因为转和移都是关于同一根轴,两者可交换。关于同一根轴的转动加平移叫一个螺旋(screw),像螺丝拧进螺母。第二个矩阵是关于 $x_{i-1}$ 的螺旋:转 $\alpha_{i-1}$、移 $a_{i-1}$:
$$Q_{i-1}=\begin{pmatrix}1&0&0&a_{i-1}\\0&\cos\alpha_{i-1}&-\sin\alpha_{i-1}&0\\0&\sin\alpha_{i-1}&\cos\alpha_{i-1}&0\\0&0&0&1\end{pmatrix}. \tag{3.55}$$相乘得到 $1 $$T_i=Q_{i-1}R_i=\begin{pmatrix} \cos\theta_i&-\sin\theta_i&0&a_{i-1}\\ \sin\theta_i\cos\alpha_{i-1}&\cos\theta_i\cos\alpha_{i-1}&-\sin\alpha_{i-1}&-\sin\alpha_{i-1}\,d_i\\ \sin\theta_i\sin\alpha_{i-1}&\cos\theta_i\sin\alpha_{i-1}&\cos\alpha_{i-1}&\cos\alpha_{i-1}\,d_i\\ 0&0&0&1\end{pmatrix}. \tag{3.56}$$
它是 (3.52) 的 3D 对应物,依次执行四个操作:① 沿 $z_i$ 平移 $d_i$;② 绕 $z_i$ 逆时针转 $\theta_i$;③ 沿 $x_{i-1}$ 平移 $a_{i-1}$;④ 绕 $x_{i-1}$ 逆时针转 $\alpha_{i-1}$。
$T_1$ 的约定:若第一杆可任意放置,用通用的 (3.50)。若它只能绕一个转动关节转,则令 $a_0=\alpha_0=0$(没有 $z_0$ 轴),$Q_0=I$,于是 $T_1=R_1$。
$\mathcal A_m$ 上点 $(x,y,z)$ 的世界位置:
$$T_1T_2\cdots T_m\begin{pmatrix}x\\y\\z\\1\end{pmatrix}. \tag{3.57}$$转动关节:只有 $\theta_i$ 是变量。移动关节:让 $a_i$ 变。更复杂的关节可拆成一串退化关节(球关节 = 三个零长度转动关节,依次 roll、pitch、yaw),或干脆放弃 DH、直接写齐次矩阵。后者在第 4 章需要保持拓扑性质时可能是必要的。
DH 四参数:从坐标系 $i-1$ 到坐标系 $i$(对应 Fig 3.15、(3.56))
实线坐标系是 $i-1$(固定在原点),虚线坐标系是 $i$。拉动四个参数,右侧显示 (3.56) 的数值矩阵。「分步」可以只看 $Q_{i-1}$ 的效果。拖动画布旋转视角。
例 3.4:PUMA 560
经典六轴机械臂。前三个关节让手(末端执行器,end-effector)在 $\mathcal W$ 中大范围移动,后三个决定手的朝向。六个自由度全部来自转动关节,所以每个 $T_i$ 只是 $\theta_i$ 的函数,记 $T_i(\theta_i)$。DH 参数表(Fig 3.17):
| 矩阵 | $\alpha_{i-1}$ | $a_{i-1}$ | $\theta_i$ | $d_i$ |
|---|---|---|---|---|
| $T_1(\theta_1)$ | 0 | 0 | $\theta_1$ | 0 |
| $T_2(\theta_2)$ | $-\pi/2$ | 0 | $\theta_2$ | $d_2$ |
| $T_3(\theta_3)$ | 0 | $a_2$ | $\theta_3$ | $d_3$ |
| $T_4(\theta_4)$ | $\pi/2$ | $a_3$ | $\theta_4$ | $d_4$ |
| $T_5(\theta_5)$ | $-\pi/2$ | 0 | $\theta_5$ | 0 |
| $T_6(\theta_6)$ | $\pi/2$ | 0 | $\theta_6$ | 0 |
注意 $a_3,d_3$ 在这个例子里是负数:它们是带符号的位移,不是距离。把表代入 (3.56),例如
$$T_2(\theta_2)=\begin{pmatrix}\cos\theta_2&-\sin\theta_2&0&0\\0&0&1&d_2\\-\sin\theta_2&-\cos\theta_2&0&0\\0&0&0&1\end{pmatrix},\qquad T_4(\theta_4)=\begin{pmatrix}\cos\theta_4&-\sin\theta_4&0&a_3\\0&0&-1&-d_4\\ \sin\theta_4&\cos\theta_4&0&0\\0&0&0&1\end{pmatrix}.$$($\alpha=-\pi/2$ 使 $\cos\alpha=0,\sin\alpha=-1$,于是第二、三行交换并变号,自己代一遍是很好的练习。)末端连杆 $\mathcal A_6$ 上的点在世界中的位置为 $T_1(\theta_1)\cdots T_6(\theta_6)\,(x,y,z,1)^{\mathsf T}$(3.64)。
PUMA 560:六个 $\theta$,一次矩阵连乘(对应例 3.4、Fig 3.16)
直接用上面的 DH 表和 (3.56) 算出 $T_1\cdots T_6$,把每个坐标系的原点连成线就是「火柴棍」机械臂。小坐标系是各 $\mathcal A_i$ 的体坐标系,大的是末端坐标系。杆长数值为示意($a_2=0.43,\ a_3=-0.02,\ d_2=0.15,\ d_3=-0.13,\ d_4=0.43$)。
例 3.5:辛烷分子
辛烷 C8H18 有 8 个碳原子排成一条链(忽略氢)。每根 C–C 键都可以扭转,研究分子的构型(化学上叫构象,conformation)是计算生物学的重要课题。假定有 7 个自由度,每个来自一根键的扭转。把它当成运动链,DH 参数的对应关系非常漂亮:
- 键恰好是转轴,所以 $z_i$ 轴取在键上;
- 相邻的键相交于原子,两条 $z$ 轴之间距离为零:$a_i=0$;
- $d_i$ = 键长(相邻碳原子的距离);
- $\alpha_i$ = 相邻两轴的夹角,必须保持不变,化学上叫键角(bond angle);
- $\theta_i$ 是变量,即扭转角(dihedral/torsion angle)。
因为 $a_i=0$,$x_i$ 轴的方向本来是不确定的(退化),但不碍事。每根键看作一个连杆 $\mathcal A_i$,原点取在 $z_i$ 与 $z_{i+1}$ 的交点(即一个原子),所以 $\mathcal A_i$ 的大部分点落在 $-z_i$ 方向(Fig 3.20)。世界系贴在第一根键上,第二个原子在原点,键沿 $-z$ 方向。$T_1=R_1$,取 $d_1=0$;其余 $i\in\{2,\dots,7\}$ 用 (3.56) 并代 $a_{i-1}=0$:
$$T_i(\theta_i)=\begin{pmatrix}\cos\theta_i&-\sin\theta_i&0&0\\ \sin\theta_i\cos\alpha_{i-1}&\cos\theta_i\cos\alpha_{i-1}&-\sin\alpha_{i-1}&-\sin\alpha_{i-1}d_i\\ \sin\theta_i\sin\alpha_{i-1}&\cos\theta_i\sin\alpha_{i-1}&\cos\alpha_{i-1}&\cos\alpha_{i-1}d_i\\0&0&0&1\end{pmatrix}. \tag{3.66}$$辛烷骨架:$a_i=0$ 的运动链(对应例 3.5、Fig 3.19/3.20)
8 个碳原子就是 8 个坐标系原点,7 根键沿各 $z_i$ 轴。键长 $d=1.54$ Å,键角 109.5°(对应 DH 的 $\alpha=70.5°$)。拉动扭转角看构象变化。
3.4运动树的变换 Transforming Kinematic Trees
很多有趣的系统不是一条链,而是一棵树(Fig 3.21a):人体的四肢和头都挂在躯干上,膝、肘是转动关节,肩近似球关节;大多数有机分子也是树状的。本节假定没有环(Fig 3.21b 的情形推到第 4.4 节,本节末尾略谈)。
药物设计要判断一个候选分子能否扭成合适的形状,低能量地「停靠」进蛋白质的空腔;蛋白质折叠问题的分子又比药物大几个数量级。两者都要在构象空间里搜索,而构象空间正是用运动树参数化的(7.5 节)。
最简单的情况
2D 树,每个连杆最多两个关节:指定一个连杆为根 $\mathcal A_1$。要变换任意 $\mathcal A_i$,沿着从 $\mathcal A_i$ 到根的那条链,直接用 3.3.1 节的方法,忽略其他连杆。每个连杆贡献一个 $\theta_i$。
分叉点:一个连杆挂着多个关节
麻烦出在有三个或更多关节的连杆,叫分叉点(junction,Fig 3.22 的 $\mathcal A_7$)。
- 只有一个分叉点:把它选作根就基本没事了。人形模型里躯干就是分叉点,让它当根(而不是右脚),四肢和头各是一条独立的链。唯一要处理的是各链第一杆不挂在躯干同一点上:插入一个固定的、虚拟的连杆,从躯干原点连到肢体的挂点即可。
- 多个分叉点:以 Fig 3.22 为例,根 $\mathcal A_1$ 经一条链连到 $\mathcal A_5$。变换 $\mathcal A_9$ 用 $T_1\cdots T_5T_6T_7T_8T_9$ (3.68),变换 $\mathcal A_{13}$ 用 $T_1\cdots T_5T_6T_7T_{12}T_{13}$ (3.69)。看似没问题,但 $\mathcal A_7$ 的体坐标系被定义了两次(Fig 3.23):走上链时 $x_7$ 指向 $\mathcal A_8$ 的关节,走下链时指向 $\mathcal A_{12}$ 的关节。要强迫两者用同一个坐标系,至少一条链得放弃 3.3.1 节漂亮的坐标系约定;3D 时意味着放弃 DH。
解法:允许两个坐标系,但加一个约束
用两个坐标系没问题,只是需要一条额外信息。想象变换整棵树,$\theta_7$ 会出现两次:上链里的 $\theta_{7u}$ 和下链里的 $\theta_{7l}$。它们能独立取值吗?不能,否则相当于 $\mathcal A_7$ 被劈成两个独立转动的连杆 $\mathcal A_{7u},\mathcal A_{7l}$(Fig 3.24),忘记了它是刚体。所以把 $\theta_{7l}$ 当独立变量,并强制
$$\theta_{7u}=\theta_{7l}+\phi,$$$\phi$ 是两个坐标系 $x$ 轴之间的固定夹角(Fig 3.23),由连杆形状决定。
例 3.6(2D 树):六个连杆,$\mathcal A_2$ 是分叉点,$\phi=\pi/4$。变换 $\mathcal A_6$ 走下链:$T_1T_{2l}T_5T_6$ (3.70);变换 $\mathcal A_4$ 走上链:$T_1T_{2u}T_3T_4$ (3.75),其中有趣的是
$$T_{2u}=\begin{pmatrix}\cos\theta_{2u}&-\sin\theta_{2u}&a_1\\ \sin\theta_{2u}&\cos\theta_{2u}&0\\0&0&1\end{pmatrix}=\begin{pmatrix}\cos(\theta_{2l}+\pi/4)&-\sin(\theta_{2l}+\pi/4)&a_1\\ \sin(\theta_{2l}+\pi/4)&\cos(\theta_{2l}+\pi/4)&0\\0&0&1\end{pmatrix}, \tag{3.78}$$约束 $\theta_{2u}=\theta_{2l}+\pi/4$ 体现了 $\mathcal A_2$ 是一个刚体。
带分叉的树:一个连杆、两个坐标系、一条约束(对应例 3.6、Fig 3.23/3.25)
三角形的 $\mathcal A_2$ 是分叉点。实线坐标系是走下链(→$\mathcal A_5$→$\mathcal A_6$)时的 $x_{2l}$,虚线是走上链(→$\mathcal A_3$→$\mathcal A_4$)时的 $x_{2u}$,两者夹角固定为 $\phi=45°$。你只能控制 $\theta_2=\theta_{2l}$,$\theta_{2u}$ 由约束自动算出。为了看得清楚,这里取 $a_3=1$(书中为 0)。
3D 树遵循同样的原则。有些情况完全不需要特殊处理,例如柔性分子:相邻转轴都相交,分叉点正好落在交点上。一般情况下,对每条链用 DH,再确定并施加适当的约束来表达树的真实自由度。Khalil–Kleinfinger 参数化是 DH 的优雅扩展,系统地解决了树(和环)的坐标系分配问题。
如果有环呢?
最一般的情形是连杆连成闭合运动链(closed kinematic chain,Fig 3.26)。例子很多:人形机器人双脚着地;两台 PUMA 一起抬一个物体;移动机器人们合力搬钢琴(Fig 1.5);大量有机分子含柔性环。
难点不在变换本身,而在哪些参数值能保证环是闭的。给定一组合法的参数,变换和树一样做:假装忘掉某个关节(比如 $\mathcal A_5$–$\mathcal A_6$ 之间的),环就断成两条链(Fig 3.27):上链 $\mathcal A_1\to\mathcal A_5$,下链 $\mathcal A_{10}\to\mathcal A_6$,从同一个最左关节出发。那个被忘掉的关节,用两条链各算一次位置:
$$T_1(\theta_1)T_2(\theta_2)T_3(\theta_3)T_4(\theta_4)T_5(\theta_5)\begin{pmatrix}a_5\\0\\1\end{pmatrix} =T_{10}(\theta_{10})T_9(\theta_9)T_8(\theta_8)T_7(\theta_7)T_6(\theta_6)\begin{pmatrix}a_6\\0\\1\end{pmatrix}. \tag{3.81}$$这是一团非线性三角方程,解集可能非常复杂。不要求闭合时有 10 个自由度;要求闭合则减 2(公共关节是转动关节,只去掉 2 个),剩 8;若 $\mathcal A_5,\mathcal A_6$ 必须刚性连接则只剩 7。对大多数带环问题,解集没有漂亮的参数化。这是经典的逆运动学(inverse kinematics)问题的一种形式。
多个环就迭代地「忘掉」关节,每忘一个少一个环,直到变成树;每个被忘掉的关节贡献一条形如 (3.81) 的方程,必须同时满足。实践中的困难是:用动作捕捉得到某一时刻的参数很容易,但模型一动,就很难保证新参数仍满足闭合约束。数字演员的脚会穿过地板。更多讨论见 4.4 节。
把环断成两条链:闭合约束 (3.81)(对应 Fig 3.26/3.27)
十根等长连杆,在 $\mathcal A_5$–$\mathcal A_6$ 处断开。蓝色是上链 $\mathcal A_1\ldots\mathcal A_5$,绿色是下链 $\mathcal A_{10}\ldots\mathcal A_6$。两条链末端之间的红色虚线是「闭合误差」。初始为正十边形(闭合)。
3.5非刚体变换 Nonrigid Transformations
很容易想象对非刚体做运动规划,这超出了前面几节的变换族。简要列举几类。
线性变换
旋转是线性变换的特例。一般的线性变换由 $n\times n$ 矩阵 $M$ 给出:
$$\begin{pmatrix}m_{11}&m_{12}\\m_{21}&m_{22}\end{pmatrix}\begin{pmatrix}x\\y\end{pmatrix}. \tag{3.82}$$$M$ 是旋转矩阵时大小形状不变。若允许变形:
- 缩放:$\operatorname{diag}(m_{11},m_{22})$,沿 $x$ 放大 $m_{11}$ 倍、沿 $y$ 放大 $m_{22}$ 倍(均为正);其中一个取负就得到镜像。(3.83)
- 剪切:$\begin{pmatrix}1&m_{12}\\0&1\end{pmatrix}$,$m_{12}\ne0$;Fig 3.28 画的是 $m_{12}=1$。(3.84)
缩放、剪切、旋转矩阵可以相乘,得到显式参数化各效果的一般矩阵;也可以升维到 $(n+1)\times(n+1)$ 加入平移,并直接推广到 $\mathbb R^3$。这样 3.2–3.4 节的框架就能顺带包含缩放和剪切。
一般 2×2 线性变换:哪些是刚体变换?
直接拉四个矩阵元。右侧判断:行列式(面积缩放比、负号即镜像)和 $M^{\mathsf T}M=I$ 是否成立(成立且 $\det=+1$ 才是旋转)。
柔性材料
把床垫搬过门:可以把床垫近似成二维的连杆阵列,但复杂度和自由度都太高。100 个转动关节的蛇形机器人用 3.3 节的方法要 100 个 $\theta$,规划起来过于笨重。替代方案是用参数化的曲线/曲面族来近似,样条最常用,因为它天生就是「少数参数控制形状」;3.1.3 节的广义圆柱、超二次曲面也可以。
代价是约束变得复杂:蛇形机器人每个关节的转角范围用运动链很好表达,但「哪些样条满足这个约束」很难刻画;柔性材料还有基于弹性的约束,甚至求它在受力下的形状都要对弹性能量做积分。
总结速查表与常见陷阱
原语与谓词
$$H_i=\{f_i\le0\},\quad \mathcal O=\bigcup_j\bigcap_i H_i$$ $$\phi=\bigvee_j\bigwedge_i e_i,\qquad \text{点碰撞检测 }O(n)$$多边形:$f$ 一次、逆时针、内部在左。半代数:$f$ 任意多项式。挖洞 = 把 $f$ 乘 $-1$ 后求交。
2D 刚体
$$T=\begin{pmatrix}\cos\theta&-\sin\theta&x_t\\ \sin\theta&\cos\theta&y_t\\0&0&1\end{pmatrix}$$先转再平移。3 DOF。顶点用 $T$,原语用 $T^{-1}$。
3D 旋转
$$R=R_z(\alpha)R_y(\beta)R_x(\gamma)$$roll→pitch→yaw,都绕世界轴。反解:$\alpha=\operatorname{atan2}(r_{21},r_{11})$,$\beta=\operatorname{atan2}(-r_{31},\sqrt{r_{32}^2+r_{33}^2})$,$\gamma=\operatorname{atan2}(r_{32},r_{33})$。6 DOF。
2D 链
$$T_i=\begin{pmatrix}\cos\theta_i&-\sin\theta_i&a_{i-1}\\ \sin\theta_i&\cos\theta_i&0\\0&0&1\end{pmatrix},\quad T_1T_2\cdots T_m\,p$$原点在与父杆的关节,$x_i$ 过本杆两关节。每个关节吃掉 2 DOF。
3D 链(DH)
$$T_i=Q_{i-1}(\alpha_{i-1},a_{i-1})\,R_i(\theta_i,d_i)$$$z_i$ = 转轴,$x_i$ = $z_i,z_{i+1}$ 的公垂线。$z$ 的量下标 $i$($d_i,\theta_i$),$x$ 的量下标 $i-1$($a_{i-1},\alpha_{i-1}$)。$T_1=R_1$。
树与环
树:沿到根的链连乘;分叉点两套坐标系 + 约束 $\theta_u=\theta_l+\phi$。环:断开成链 + 闭合方程 (3.81),每断一个转动关节 DOF 减 2。
常见陷阱一览
- $h$ 还是 $h^{-1}$:边界表示(顶点)正着变换;实体表示(原语里的 $f$)要把世界点拉回体坐标系,用逆变换。
- 顺序:$T$ 是「先转再平移」;$R=R_zR_yR_x$ 是「先 roll 再 pitch 最后 yaw」;链是 $T_1T_2\cdots T_m p$ 从右往左作用。矩阵乘法不交换,但关于同一根轴的转动与平移(螺旋)交换。
- 绕哪个轴:从左边乘进来的矩阵都绕世界轴转,不绕当前机体轴。
- 定向:逆时针(外边界)vs 顺时针(孔);叉积 $v_{12}\times v_{23}$ 给外法向。顺序弄反,整个符号就反了。
- DH 下标:$a_{i-1},\alpha_{i-1}$ 与 $d_i,\theta_i$ 下标不同是有意的;参数可正可负(PUMA 的 $a_3,d_3<0$)。
- 分叉点不是新自由度:同一刚体在两条链里出现时,角度差是常数 $\phi$。
- 欧拉角不是旋转空间的好坐标:多值、$\beta=\pm\pi/2$ 退化。构造例子可以用,做规划请看第 4 章的四元数。
术语对照
| English | 中文 | 一句话 |
|---|---|---|
| boundary / solid representation | 边界表示 / 实体表示 | 描述表面 vs 描述点集 |
| primitive | 原语 | $H=\{f\le0\}$ 这种容易计算的基本集合 |
| semi-algebraic set | 半代数集 | 多项式原语的有限并/交 |
| predicate | 谓词 | 返回真/假的函数,这里当碰撞检测器 |
| half-edge, DCEL | 半边,双向连接边表 | 多面体表面的可遍历数据结构 |
| rigid-body transformation | 刚体变换 | 保距 + 保定向的映射 |
| world frame / body frame | 世界坐标系 / 体坐标系 | 固定参考系 vs 画在机器人上的坐标系 |
| homogeneous transformation matrix | 齐次变换矩阵 | 升一维,把旋转和平移装进一个矩阵 |
| degrees of freedom (DOF) | 自由度 | 完全刻画变换所需的独立参数个数 |
| yaw / pitch / roll | 偏航 / 俯仰 / 翻滚 | 绕 $z$ / $y$ / $x$ 轴的旋转 |
| link, joint, linkage, kinematic chain | 连杆,关节,连杆机构,运动链 | 刚体,连接处,连在一起的刚体,单链 |
| revolute / prismatic joint | 转动关节 / 移动关节 | 只能转 / 只能滑 |
| Denavit–Hartenberg (DH) parameters | DH 参数 | $d_i,\theta_i,a_{i-1},\alpha_{i-1}$ |
| screw | 螺旋 | 关于同一根轴的转动 + 平移 |
| end-effector | 末端执行器 | 机械臂的手 |
| conformation | 构象 | 分子的「构型」 |
| junction | 分叉点 | 挂着三个以上关节的连杆 |
| closed kinematic chain | 闭合运动链 | 含环的连杆机构 |
| inverse kinematics | 逆运动学 | 给末端位姿反求关节参数 |
习题习题导引
这里只给思路和可核对的答案,完整推导留给你自己。
- 挖掉三角形鼻子(半代数模型)。三角形鼻子 $N=H_a\cap H_b\cap H_c$(三条边的半平面,内部为 $\le0$)。要移除它,取补集:$\mathcal W\setminus N=\{-f_a\le0\}\cup\{-f_b\le0\}\cup\{-f_c\le0\}$(严格说用 $<$ 才去掉边界,见 3.1.1 的脚注)。于是 $\mathcal O'=\mathcal O\cap(H'_a\cup H'_b\cup H'_c)$,用分配律展开成三个交集的并。
- 不同 $(\alpha,\beta,\gamma)$ 给出同一旋转。两类情形:① 周期性:各角加 $2\pi$ 的整数倍。② 真正的双重覆盖:$R(\alpha,\beta,\gamma)=R(\alpha+\pi,\ \pi-\beta,\ \gamma+\pi)$,可以直接代入 (3.42) 验证。③ 退化:$\beta=\pm\pi/2$ 时 $\cos\beta=0$,矩阵只依赖 $\alpha\mp\gamma$,一整条一维的角族给出同一旋转(万向节死锁)。Lab 5 里把 $\beta$ 推到 $\pm89°$ 可以感受到。
- 2D 旋转可交换,3D 不可。2D:用和角公式证明 $R(\theta_1)R(\theta_2)=R(\theta_1+\theta_2)=R(\theta_2)R(\theta_1)$。3D:找一个反例即可,例如 $R_x(\pi/2)R_y(\pi/2)$ 和 $R_y(\pi/2)R_x(\pi/2)$ 作用在 $(1,0,0)$ 上分别得到 $(0,0,-1)$ 和 $(0,1,0)$(请自己算一遍)。
- ZYZ 欧拉角。(a) 直接相乘 $R_z(\alpha)R_y(\beta)R_z(\gamma)$。(b) 利用 $R_y(-\beta)=R_z(\pi)R_y(\beta)R_z(\pi)$ 和 $R_z(\pi)^2=I$。(c) 从 (a) 的矩阵读出 $r_{13}=c\alpha\,s\beta$,$r_{23}=s\alpha\,s\beta$,$r_{33}=c\beta$,$r_{31}=-s\beta\,c\gamma$,$r_{32}=s\beta\,s\gamma$,再套 atan2。
- 12 种欧拉角。三个轴位置各选一轴,但相邻两次不能用同一轴:$3\times2\times2=12$。其中 6 种三轴互异(Tait–Bryan,如 ZYX),6 种首尾同轴(真欧拉角,如 ZYZ)。同轴连用等于一次旋转,自由度不够三。
- 单位圆盘旋转不变。原语要用 $h^{-1}=R(-\theta)$:$f(R(-\theta)w)=\|R(-\theta)w\|^2-1=\|w\|^2-1$,因为旋转保长度。所以 $H$ 不变。
- 三根 12×2 的杆。用 (3.51)(3.52):$a_1=a_2=10$(关节间距),$T_1=R(\theta_1)$,$T_2=\begin{pmatrix}R(\theta_2)&(10,0)^{\mathsf T}\\0&1\end{pmatrix}$,$T_3$ 同理。杆上任意点先写成体坐标(杆沿 $x$ 轴,宽 2,所以 $y\in[-1,1]$,$x\in[-1,11]$),再乘 $T_1T_2T_3$。(b) 末端 $(10,0)\in\mathcal A_3$ 回到原点,等价于三根长 10 的连杆首尾相接构成等边三角形:$\theta_2=\theta_3=\pm2\pi/3$(两种绕向),$\theta_1$ 任意。
- Fig 3.30 的链。$a_1=1,\ a_2=2$,点 $(4,0)$ 在 $\mathcal A_3$ 体坐标中是 $(1,0)$。$T_1T_2T_3(1,0,1)^{\mathsf T}$ 在 $(\pi/4,\pi/2,\pi/4)$ 处等于 $\big(-1-\tfrac{\sqrt2}{2},\ \tfrac{3\sqrt2}{2}\big)\approx(-1.707,\ 2.121)$。Lab 6 可以直接验证。
- 球关节 ≈ 三个零长转动关节。令三根短杆的转轴互相垂直,写出三个 $T_i$($a\to0$),乘积的旋转部分趋向 $R_zR_yR_x$ 形式;对比 (3.42) 直接放一个 $3\times3$ 旋转矩阵。前者用 DH 统一处理,后者参数化更直接但要自己选旋转的参数化(又回到欧拉角 vs 四元数的问题)。
- 线接触、点接触的关节。线接触:沿线滑(1)、绕线转(1)、绕法线转(1)等组合;点接触:三个转动 + 两个滑动 + 滚动与滑动的区分。关键是把「允许的相对运动」数出来。
- 螺旋关节。沿 $x$ 轴转 $\theta$ 的同时前进 $d\theta/(2\pi)$:$T=\begin{pmatrix}1&0&0&d\theta/2\pi\\0&\cos\theta&-\sin\theta&0\\0&\sin\theta&\cos\theta&0\\0&0&0&1\end{pmatrix}$,只有 1 个自由度。半径 $r$ 决定螺纹上点的体坐标,不进矩阵。
- 让例 3.6 的零构型长成 Fig 3.25 的样子。在每个 $T_i$ 的 $\theta_i$ 上加一个常数偏置 $\theta_i+\theta_i^{0}$,$\theta_i^0$ 由图中连杆方向算出;分叉点的 $\phi$ 也体现为这样的常数偏置。
- 3D 剪切。$\begin{pmatrix}1&m_{12}&m_{13}\\0&1&m_{23}\\0&0&1\end{pmatrix}$,上三角、对角为 1,$\det=1$,体积不变。
实现题 14–18(2D 连杆、分子球棍模型、Tinkertoy 编辑器、人体树、PUMA 模拟器):本页的 Lab 6、9、10、8 分别是它们的最小可用版本,源码就在这个 HTML 文件里,可以当起点改。