Planning Algorithms 学习站 首页 › Part II › 第 3 章 几何表示与变换

Planning Algorithms · Steven M. LaValle · Chapter 3

几何表示与变换

运动规划要回答的问题是「机器人能不能从这里走到那里」。在回答之前,必须先用数学把两件事说清楚:世界里有什么(障碍和机器人长什么样),以及机器人怎么动(一个姿态参数 $q$ 如何决定机器人占据空间的哪一块)。第 3 章就是这两件事的全部基础。

对应原书 pp. 81–126 · 3.1 建模 → 3.2 单刚体 → 3.3 链 → 3.4 树与环 → 3.5 非刚体 · 12 个可交互图(拖动画布、拉动滑杆)

导读这一章在干什么

全书 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 几何建模 O 和 A 长什么样 3.2 单刚体 T = 旋转 + 平移 3.3 运动链 T₁T₂…Tₘ · DH 参数 3.4 运动树 分叉约束 · 闭环 描述「有什么」 描述「怎么动」(越来越复杂的物体) 3.5 非刚体:放松假设 → 第 6 章 组合规划(半代数模型) → 第 4 章 构型空间 C
本章路线图。3.1 主要为第 6 章的组合方法服务;3.2、3.3 是整个 Part II 的必备基础;若先学第 5 章(采样规划),半代数模型可以略读。

怎么读这份教程

  • 每一节先给直觉,再给书上的公式和编号(如 (3.35)),方便对照原文。
  • 带 LAB 标记的卡片是可交互图:拖动画布上的点、拉动滑杆、切换按钮。每个 Lab 下方「试一试」给出了几个值得观察的现象。
  • 三种颜色贯穿全文:绿色是障碍 $\mathcal O$,红色是机器人 $\mathcal A$,蓝色是坐标系与结构。3D 图里 x/y/z 轴按惯例是红/绿/蓝。
  • 文末有速查表与常见陷阱和习题导引。

3.1几何建模 Geometric Modeling

先定义世界 $\mathcal W$:要么 $\mathcal W=\mathbb R^2$(2D),要么 $\mathcal W=\mathbb R^3$(3D)。世界里有两类东西:

  1. 障碍(obstacles):永久占据的部分,比如建筑物的墙。所有障碍点的集合记为障碍区域 $\mathcal O\subseteq\mathcal W$。
  2. 机器人(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$)。由于顶点逆时针排列,多边形内部始终在每条边的左侧。

书里略过的一步:由两点求 a, b, c

设边从 $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}$$
LAB 1

半平面的交 = 凸多边形(对应 Fig 3.1、3.2)

拖动圆点测试任意位置;把鼠标悬停在某条边上,会显示这条边对应的半平面 $H_i$($f_i\le 0$ 的一侧)。右侧实时给出每个 $f_i$ 的值和谓词 $\alpha=e_1\wedge\cdots\wedge e_5$ 的结果。

试一试:① 把点拖到多边形外,看哪些 $e_i$ 变成 ✗,是否恰好对应「点在那条边右侧」。② 勾选全部半平面,观察 5 个半平面的交集正好是五边形,而每个单独的半平面都是无限大的。③ 把点放到边界上,$f_i\approx 0$,仍算在 $\mathcal O$ 内(闭集)。

非凸多边形 = 凸多边形的并

凸的假设太受限。非凸的多边形区域写成

$$\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):

face F₁ face F₂ h twin(h) prev(h) next(h) origin(h) 每条半边记录 5 个指针: origin · twin · face next · prev 每个面:沿外法向看 逆时针的半边环
DCEL 的核心是「半边」(half-edge):每条真实的边存成两条方向相反、互为 twin 的半边,分别属于相邻的两个面。有了 origin/twin/face/next/prev 五个指针,就能沿多面体表面任意「行走」。

实体表示:每个面确定一个平面 $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$ 表示「外部」,于是「与外部求交」就等于「挖掉」。

LAB 2

姜饼人脸:四个代数原语的交(对应 Fig 3.4b、例 3.1)

着色区域是当前的 $\mathcal O$。每个原语可以从交集中「移除」,也可以把它的符号翻转。拖动探针点查看四个 $f_i$ 的取值。

参与交集 / 翻转符号:
试一试:① 翻转 $f_2$:左眼从「挖掉的洞」变成「只剩眼睛那个圆盘」(因为此时要求点同时在外圆内、左眼内)。② 取消 H₁:脸没有外边界了,$\mathcal O$ 变成整个平面挖掉三个洞,说明交集里的每个原语都在「收紧」区域。③ 把 r₂ 调大到眼睛探出外圆,边界上照样处理得干净,这是隐式表示的好处。

哪些关系真的增加了表达力?

除了 $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)$,相邻点之间(以及首尾之间)有边。顶点应选得使多边形简单(边不相交),这样内部是良定义的:每条边的左侧。

外边界:逆时针 孔边界:顺时针 沿箭头走,阴影 永远在左手边
Fig 3.5 的要点:带孔多边形用方向区分外边界与孔。外边界逆时针,孔顺时针,于是「内部在左侧」这一规则对所有边都成立。3D 多面体同理,用相反的定向表示孔。

3D 三角形网格

每个三角形由三个点给出,是图形学中最常用的模型(显卡原生支持)。规定三角形内部属于模型,两个三角形「碰撞」指一个戳进了另一个。优点是毫无约束,缺点也是毫无约束:没有一致性可以利用,很难判断一个点在 3D 障碍「里面」还是「外面」。为了去掉共享顶点的冗余,有三角形带(triangle strip,相邻三角形共享一条边)和三角形扇(triangle fan,所有三角形共享一个顶点)。

1234567triangle strip:7 个点 5 个三角形 triangle fan:共享中心顶点
Fig 3.6:带和扇都是在「有一些连贯性」时减少重复顶点的编码方式。

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 里是

$$\big(|x/a|^{n_1}+|y/b|^{n_2}\big)^{n_1/n_2}+|z/c|^{n_1}-1\le0,\qquad n_1,n_2\ge2. \tag{3.20}$$

$n_1=n_2=2$ 时是椭球;指数越大越像圆角盒子。

LAB 3

超椭圆:一个参数从椭圆连续变到方盒

2D 截面 $|x/a|^{n}+|y/b|^{n}\le1$。拉动 $n$,看一个原语就能表达的形状家族。

$n=2$ 椭圆;$n\to\infty$ 趋向矩形;$n<2$ 时「凹进去」变成星形(书中要求 $n\ge2$ 以保证凸性,这里放开让你看看为什么)。

广义圆柱

普通圆柱的中轴是直线、半径是常数;广义圆柱把中轴换成一条脊线 $(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$ 中,且满足两条:

  1. 任意两点间的距离保持不变;
  2. 定向保持不变(不允许「镜像」)。

$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}$$
陷阱 1:原语里出现的是逆变换

要判断世界点 $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。

每个变换有两种解释(Fig 3.7)

① 世界系不动,机器人被变换;② 机器人不动,世界系被(反向)变换。前者是从固定世界看变换的效果,后者是从机器人视角看。运动规划默认用第一种,因为它建模的是「机器人在物理世界里移动」。很多坐标变换的书用第二种,导致公式看起来「反过来」。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 个自由度。

陷阱 2:平移和旋转不可交换

先转再平移得到 $Rp+t$;先平移再转得到 $R(p+t)=Rp+Rt$。两者仅在 $Rt=t$ 时相同。书中约定 $T$ 永远是「先转再平移」。

LAB 4

2D 刚体变换与齐次矩阵(对应 (3.35)、Fig 3.7)

红色 L 形机器人在体坐标系中定义,原点在它的角上。拉动 $x_t, y_t, \theta$,右侧矩阵实时更新;淡色轮廓是未变换的 $\mathcal A$。小圆点是体坐标 $a=(1.5,0.3)$ 的点,用来验证 $w=Ta$。

运算顺序:
试一试:① 固定 $\theta=90°$,切换运算顺序,看平移向量怎么被旋转「带偏」。② 勾选视角 ②:蓝色世界系按 $T^{-1}$ 移动,机器人回到原位,但两者的相对关系和视角 ① 完全一样。③ 勾选圆盘原语:世界点 $w$ 落在圆盘里当且仅当 $|R^{\mathsf T}(w-t)|\le1$,即 $f(h^{-1}(w))\le0$。

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$。)

陷阱 3:顺序是 roll → pitch → yaw,而且每一步都绕「世界轴」

$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 做的事:

$$\alpha=\operatorname{atan2}(r_{21},r_{11}),\qquad \beta=\operatorname{atan2}\!\Big(-r_{31},\sqrt{r_{32}^2+r_{33}^2}\Big),\qquad \gamma=\operatorname{atan2}(r_{32},r_{33}). \tag{3.47–3.49}$$

此法假定 $r_{11}\ne0,\ r_{33}\ne0$($\beta=\pm\pi/2$ 时失效,即万向节死锁,见习题 2)。

LAB 5

Yaw-Pitch-Roll:顺序与世界轴(对应 Fig 3.8、(3.42))

灰色是世界系,彩色三轴是画在飞机上的体坐标系(机头 +x)。拖动画布旋转视角。右侧给出 $R(\alpha,\beta,\gamma)$ 的数值,以及用 atan2 从矩阵反解出的三个角,用来验证 (3.47–3.49)。

试一试:① 先设 $\beta=60°,\gamma=0$,再拉 $\alpha$:飞机绕世界 $z$(竖直虚线)打转,而不是绕自己歪掉的机体 $z$ 轴。② 把 $\beta$ 推到 $\pm89°$,反解出的 $\alpha,\gamma$ 开始剧烈抖动,这是万向节死锁的前兆。③ 点「演示」,看三步依次作用的效果。

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)$。

延伸:为什么第 4 章要换成四元数

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连杆机构(linkage);连成一条单链的叫运动链(kinematic chain)。

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$ 是常数。两种关节可以混在同一条链里。

LAB 6

三连杆:一步一步把 $\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)$。

阶段:
杆长 a₁ a₂ a₃:
试一试:① 在阶段「T₃」里只拉 $\theta_3$:$\mathcal A_3$ 绕 $\mathcal A_2$ 坐标系中 $(a_2,0)$ 这一点转,因为 $T_3$ 是「先转再沿 $x$ 平移 $a_2$」。② 在阶段「T₁T₂T₃」里拉 $\theta_1$,整条链一起转:$\theta_1$ 出现在最左边的矩阵里,影响后面所有连杆。③ 习题 8:把杆长设为 1, 2, 1,角度 45°, 90°, 45°,末端点应为 $(-1-\tfrac{\sqrt2}{2},\ \tfrac{3\sqrt2}{2})\approx(-1.707,\ 2.121)$。

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)
陷阱 4:下标不对齐是故意的

$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 章需要保持拓扑性质时可能是必要的。

LAB 7

DH 四参数:从坐标系 $i-1$ 到坐标系 $i$(对应 Fig 3.15、(3.56))

实线坐标系是 $i-1$(固定在原点),虚线坐标系是 $i$。拉动四个参数,右侧显示 (3.56) 的数值矩阵。「分步」可以只看 $Q_{i-1}$ 的效果。拖动画布旋转视角。

显示:
试一试:① 只动 $a_{i-1}$:坐标系 $i$ 沿红色 $x_{i-1}$ 轴滑动,这就是两条 $z$ 轴之间的公垂线长度。② 只动 $\alpha_{i-1}$:蓝色 $z_i$ 轴绕 $x_{i-1}$ 倾斜。③ 只动 $d_i$ 和 $\theta_i$:坐标系 $i$ 沿自己的 $z_i$ 轴滑动和自转,这是关节的「螺旋」自由度。④ 让 $a=0,\alpha=0$:两条 $z$ 轴重合,$T_i=R_i$,这正是 $T_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)$00$\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)。

LAB 8

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$)。

试一试:① 全部归零看「零构型」,再只动 $\theta_1$:整臂绕世界 $z$ 转,因为 $T_1$ 在最左边。② 只动 $\theta_4$ 或 $\theta_6$:末端位置几乎不变、只有朝向变,这就是「后三轴管朝向」。③ 对比 $\theta_2$ 与 $\theta_3$:肘关节 $\theta_3$ 只带动前臂,肩关节带动整条臂,矩阵越靠左影响越大。

例 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}$$
LAB 9

辛烷骨架:$a_i=0$ 的运动链(对应例 3.5、Fig 3.19/3.20)

8 个碳原子就是 8 个坐标系原点,7 根键沿各 $z_i$ 轴。键长 $d=1.54$ Å,键角 109.5°(对应 DH 的 $\alpha=70.5°$)。拉动扭转角看构象变化。

试一试:① 全反式是平面锯齿(最稳定的构象);全顺式把链卷成一个近似的环。② 只动 $\theta_4$:中间那根键扭转,两端整体相对旋转,键长和键角始终不变,这正是 DH 中只有 $\theta$ 可变的意思。③ 勾选坐标系:每根键就是一根蓝色 $z$ 轴。

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$ 是一个刚体。

LAB 10

带分叉的树:一个连杆、两个坐标系、一条约束(对应例 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)。

试一试:① 拉 $\theta_2$:两条链一起转,三角形保持刚性。② 勾选「忘记约束」再拉 $\theta_{2u}$:三角形被撕开成两块,这就是 Fig 3.24 里多出一个假自由度的错误。③ 数一数自由度:6 个 $\theta$,正好 6 个连杆,分叉点并没有多给自由度。

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 节。

LAB 11

把环断成两条链:闭合约束 (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$。两条链末端之间的红色虚线是「闭合误差」。初始为正十边形(闭合)。

上链(蓝):
下链(绿):
试一试:① 随便动一个角,环立刻裂开(红色虚线)。② 点「闭合」:固定其余 8 个角,只调 $\theta_4,\theta_5$ 让两端重合,这就是 (3.81) 的一个两未知数特例,有两个解(肘上/肘下)或无解。③ 自由度核算:10 个角,2 个被方程吃掉,剩 8 个可以自由改。

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 节的框架就能顺带包含缩放和剪切。

LAB 12

一般 2×2 线性变换:哪些是刚体变换?

直接拉四个矩阵元。右侧判断:行列式(面积缩放比、负号即镜像)和 $M^{\mathsf T}M=I$ 是否成立(成立且 $\det=+1$ 才是旋转)。

试一试:① 「镜像」:$\det=-1$,字母 F 翻成了反的,距离都没变却不是刚体变换,这正是定义里「保定向」那一条的作用。② 「剪切」:面积不变($\det=1$)但角度变了。③ 手动把矩阵调成 $\begin{pmatrix}0&-1\\1&0\end{pmatrix}$,看判定变成「旋转 90°」。

柔性材料

把床垫搬过门:可以把床垫近似成二维的连杆阵列,但复杂度和自由度都太高。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。

常见陷阱一览

  1. $h$ 还是 $h^{-1}$:边界表示(顶点)正着变换;实体表示(原语里的 $f$)要把世界点拉回体坐标系,用逆变换。
  2. 顺序:$T$ 是「先转再平移」;$R=R_zR_yR_x$ 是「先 roll 再 pitch 最后 yaw」;链是 $T_1T_2\cdots T_m p$ 从右往左作用。矩阵乘法不交换,但关于同一根轴的转动与平移(螺旋)交换。
  3. 绕哪个轴:从左边乘进来的矩阵都绕世界轴转,不绕当前机体轴。
  4. 定向:逆时针(外边界)vs 顺时针(孔);叉积 $v_{12}\times v_{23}$ 给外法向。顺序弄反,整个符号就反了。
  5. DH 下标:$a_{i-1},\alpha_{i-1}$ 与 $d_i,\theta_i$ 下标不同是有意的;参数可正可负(PUMA 的 $a_3,d_3<0$)。
  6. 分叉点不是新自由度:同一刚体在两条链里出现时,角度差是常数 $\phi$。
  7. 欧拉角不是旋转空间的好坐标:多值、$\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) parametersDH 参数$d_i,\theta_i,a_{i-1},\alpha_{i-1}$
screw螺旋关于同一根轴的转动 + 平移
end-effector末端执行器机械臂的手
conformation构象分子的「构型」
junction分叉点挂着三个以上关节的连杆
closed kinematic chain闭合运动链含环的连杆机构
inverse kinematics逆运动学给末端位姿反求关节参数

习题习题导引

这里只给思路和可核对的答案,完整推导留给你自己。

  1. 挖掉三角形鼻子(半代数模型)。三角形鼻子 $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)$,用分配律展开成三个交集的并。
  2. 不同 $(\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°$ 可以感受到。
  3. 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)$(请自己算一遍)。
  4. 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。
  5. 12 种欧拉角。三个轴位置各选一轴,但相邻两次不能用同一轴:$3\times2\times2=12$。其中 6 种三轴互异(Tait–Bryan,如 ZYX),6 种首尾同轴(真欧拉角,如 ZYZ)。同轴连用等于一次旋转,自由度不够三。
  6. 单位圆盘旋转不变。原语要用 $h^{-1}=R(-\theta)$:$f(R(-\theta)w)=\|R(-\theta)w\|^2-1=\|w\|^2-1$,因为旋转保长度。所以 $H$ 不变。
  7. 三根 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$ 任意。
  8. 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 可以直接验证。
  9. 球关节 ≈ 三个零长转动关节。令三根短杆的转轴互相垂直,写出三个 $T_i$($a\to0$),乘积的旋转部分趋向 $R_zR_yR_x$ 形式;对比 (3.42) 直接放一个 $3\times3$ 旋转矩阵。前者用 DH 统一处理,后者参数化更直接但要自己选旋转的参数化(又回到欧拉角 vs 四元数的问题)。
  10. 线接触、点接触的关节。线接触:沿线滑(1)、绕线转(1)、绕法线转(1)等组合;点接触:三个转动 + 两个滑动 + 滚动与滑动的区分。关键是把「允许的相对运动」数出来。
  11. 螺旋关节。沿 $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$ 决定螺纹上点的体坐标,不进矩阵。
  12. 让例 3.6 的零构型长成 Fig 3.25 的样子。在每个 $T_i$ 的 $\theta_i$ 上加一个常数偏置 $\theta_i+\theta_i^{0}$,$\theta_i^0$ 由图中连杆方向算出;分叉点的 $\phi$ 也体现为这样的常数偏置。
  13. 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 文件里,可以当起点改。