Part II · 第 7 章结构与运动学 · 7.2

构型空间

一台 $n$ 自由度的机器人，此刻的整个样子只是一个 $n$ 维空间里的一个点。规划、逆运动学、控制和学习都在这个空间里发生，但它和手爪所在的三维空间完全是两回事：角度会绕回来，旋转的空间是一个对径点被粘起来的球，一个圆形的障碍物在这里会变成扭曲的红色区域。这一节把这个空间画出来、摸一摸，并弄清楚它的形状为什么决定了"两个构型有多近"、"怎样做插值和平均"，以及 $\texttt{nq}\ne\texttt{nv}$ 是怎么回事。

约 35 分钟前置：7.1 连杆、关节与自由度 · 5.2 旋转的表示 · 5.4 为什么需要李群 · 3.4 机器人雅可比 7 个交互

1为什么机器人需要它

要说清一台机械臂"现在是什么姿势"，只需要给出每个关节转了多少：一组关节角 $\boldsymbol\theta=(\theta_1,\dots,\theta_n)$。把这组数当成一个点，整台机器人的样子就被压缩成了一个点。所有这样的点构成的空间叫构型空间（configuration space，简称 C-space），记作 $\mathcal C$。$n$ 自由度的机器人对应 $n$ 维的 $\mathcal C$（怎样数这个 $n$，见 7.1 节）。机器人学里几乎所有算法都是在这个空间里做的：

运动规划：从姿势 A 摆到姿势 B，就是 $\mathcal C$ 里的一条曲线；"不要撞到东西"则是这条曲线要避开的一个集合。（Part IV 的 RRT、PRM 都在 $\mathcal C$ 里采样。）
逆运动学：给定手爪位姿，求 $\mathcal C$ 里满足 $\mathrm{FK}(\mathbf q)=T$ 的点：可能有两个、无穷多个，也可能一个都没有（7.5 节）。
控制与学习：机器人的状态是 $\mathcal C$ 中的一个点加上它的速度。策略网络的观测与动作大多是 $\mathcal C$ 里的量，因此"两个构型有多近"决定了最近邻、平均、插值、损失函数该怎么写。

要特别分清两个空间：工作空间是手爪能到达的位置所组成的集合（在 $\R^3$ 里），而构型空间描述整台机器人。肘部朝上和肘部朝下的两种姿势，手爪在同一个位置，却是 $\mathcal C$ 里相距很远的两个点；一块桌面在工作空间里是一块平板，在 $\mathcal C$ 里却变成一个形状古怪的区域。

这一节建立在 5.1 节（坐标系）、5.2 节（旋转的表示）、5.4 节（李群）和 6.10 节（流形上的优化）之上：那里讲过"旋转不是 $\R^3$ 里的向量"，这里要说清这件事对整个机器人意味着什么。7.1 节教你数自由度，本节则讲这个数字背后的空间。

2直觉：两个数，一个点；方框的边是粘起来的

从最简单的例子开始：3.4 节里的平面臂，只留两根连杆（$l_1=1$，$l_2=0.8$）。它有两个关节角 $\theta_1,\theta_2$，所以 $\mathcal C$ 是二维的，可以整个画出来：横轴 $\theta_1$，纵轴 $\theta_2$，方框里的每一个点就是手臂的一个姿势。下面两张图是同一件事的两种写法：拖动左图的关节，右图的点跟着动；拖动右图的点，左图的手臂跟着动。

留意两件事。第一，$\theta=\pi$ 和 $\theta=-\pi$ 是同一个朝向，所以方框的左右两边是粘在一起的，上下两边也是：把点拖出右边，它会从左边回来，手臂却是在连续地转动。把左右边粘起来得到一个圆筒，再把上下口粘起来，就得到一个甜甜圈的表面，数学上叫环面（torus）$T^2=S^1\times S^1$。第二，手爪停在同一个位置，却对应 $\mathcal C$ 里的两个点（肘部朝上与朝下）。右图的紫色空心点和左图的紫色虚影就是它们。到基座的距离只取决于 $\theta_2$：$\|\mathbf p\|^2=l_1^2+l_2^2+2l_1l_2\cos\theta_2$，所以"手爪离基座同样远"的所有构型恰好落在方框里的两条水平线上（紫色虚线）。

FIG 1手臂与它的环面

工作空间（手爪在哪里）

构型空间 T²（机器人在做什么）

左：拖动红色圆圈改变 $\theta_1$，绿色圆圈改变 $\theta_2$。右：拖动琥珀色点；方框外的淡色部分是周期延拓（同一个点的拷贝），点被拖出边界后会从对面回来。紫色空心点与虚影臂是手爪保持不动时的另一种肘部构型，紫色水平虚线是"离基座同样远"的两条等距线。

试一试

让 $\theta_1$ 越过 $\pm\pi$：把右图的点拖出右边框，或者让第一根连杆转过 $-x$ 轴。点从另一边出现，而手臂的运动是连续的。
按"翻转肘部"：手爪纹丝不动，右图的点却跳到了很远的另一个位置。它跳到了哪里（提示：$\theta_2\to-\theta_2$）？

核心洞见

构型 $\ne$ 位置。构型空间是一个带"粘合边"的空间，不是 $\R^n$：每个转动关节贡献一个圆 $S^1$，而不是一条直线。局部看它像 $\R^n$（可以用 $n$ 个数描述附近的点），整体看却是弯的、绕回来的。这类"局部像欧氏空间、整体不是"的空间叫流形。

3动手玩：障碍物在构型空间里长什么样

现在放进一个障碍物。对 $\mathcal C$ 里的每一个点 $\mathbf q$，问一个是/否问题："手臂摆成这个姿势，两根连杆（包括连在基座上的第一根）碰到障碍物了吗？"把所有回答"是"的点涂成红色，就得到 C-障碍物（C-obstacle）$\mathcal C_{\text{obs}}$，其余的是自由空间 $\mathcal C_{\text{free}}$。下图对 $180\times180$ 个构型逐个做这件事（几何上是线段到圆或胶囊的距离）。拖动左图中的障碍物，看右图的红色区域怎么变。

然后做一件规划器最想做的事：在起点 S 与终点 G 之间画一条C-空间里的直线 $\mathbf q(t)=\mathbf q_s+t\,\Delta$，并检查它是否穿过红色区域。因为角度会绕回来，从 S 到 G 有两种直线：每个角都走短的那条弧（包裹，最短），或者直接相减（不包裹）。第三种是工作空间里的直线：让手爪沿直线走，用逆运动学逐点求关节角（保持起点的肘部分支），它在 $\mathcal C$ 里是一条曲线。拖动滑块或按"播放"，手臂会沿着当前路径运动，碰到障碍物的连杆会变红。

FIG 2C-障碍物与路径

工作空间

构型空间：红 = C-障碍物

右图中青色圆点 S 是起点，紫色圆点 G 是终点，琥珀色圆点是"探针"：它的构型就是左图中手臂的姿势（也可以直接拖动它）。绿色实线是自由的路径，红色虚线是碰撞之后的部分，叉号标出第一个碰撞点。斜线条纹区域是被关节限位切掉的部分；自由空间按连通分量着色（与起点相连的是淡绿色，其余是淡紫色）。连杆有 0.045 的半径；不考虑自碰撞。

试一试

选"不包裹"路径。拖动障碍物，让最短的直线撞上它，而不包裹的那条自由（提示：把障碍物放到 $\theta_1$ 经过 $\pm\pi$ 的那一侧，即手臂向左摆的地方）。
选"工作空间直线"。找到一个障碍物位置，使得最短的 C-空间直线撞上它，而手爪的直线路径是自由的：同一对起点终点，两种"直线"不是一回事。
让自由空间分裂成 $\ge2$ 块：试试"两个圆盘"，或者收紧 $\theta_1$ 的限位。为什么一个靠近基座的圆盘会切出一整条竖直的红带？

几点值得停下来想一想。（1）一个圆盘在 $\mathcal C$ 里不是圆，因为坐标是角度而不是长度；而且往往有两份，对应肘部朝上与朝下两种方式去碰它。（2）如果圆盘靠近基座，第一根连杆无论 $\theta_2$ 怎么取都会撞到它，红色就成了一整条竖带；两条竖带把环面切成两半，两个半边之间无路可走。（3）起点和终点都自由、直线被挡住，并不说明没有路：只要在同一个连通分量里，绕开红色区域的曲线就存在，规划器的任务就是找到它。

4第二、第三视角：圆、环面与旋转球

角度住在圆上，而不是数轴上

为什么方框的边要粘起来？因为 $\theta$ 与 $\theta+2\pi$ 对应机器人完全相同的样子，所以关节角是圆 $S^1$ 上的点。把圆在 $\pm\pi$ 处剪开拉直，就得到熟悉的区间 $[-\pi,\pi]$，代价是：剪口两侧紧挨着的 $179°$ 和 $-179°$ 在数轴上变成了相距 $358°$ 的两个点。所有依赖"相减"的操作都会在剪口处出错：差、平均、插值、最近邻、误差反馈。拖动下图中的 a 和 b（既可以在圆上拖，也可以在下方展开的数轴上拖），对比"直接相减"和"圆上的做法"。

FIG 3圆 vs. 数轴：差与平均

上：单位圆，红色刻度是剪口 $\pm\pi$。下：把圆在剪口处展开成数轴，两端 $-\pi\equiv\pi$ 是同一个点。红色虚线是"直接相减"（$b-a$）走的弧，绿色实线是包裹后的最短弧；红色菱形是算术平均，绿色菱形是圆上的平均（单位向量求和再取角度）。当 a、b 恰好相对（相差 $\pi$）时圆上的平均没有定义。

让两种平均落在圆的几乎相对的两侧（相差超过 2.5 rad）。例如 $a=3$，$b=-3$：算术平均是 0，两个角其实都在 $\pi$ 附近。

圆上的四个基本操作

差：$\operatorname{wrap}(b-a)$，其中 $\operatorname{wrap}(x)=x-2\pi\lfloor (x+\pi)/2\pi\rfloor\in[-\pi,\pi)$。距离：$|\operatorname{wrap}(b-a)|$。插值：$a+t\operatorname{wrap}(b-a)$。平均：$\operatorname{atan2}(\sum\sin\theta_i,\sum\cos\theta_i)$。它们是 6.10 节"在圆上取平均"的一般形式：在流形上，"相减"换成"沿最短路径走的位移"（对数映射），"加上位移"换成"沿路径走一步"（指数映射）。

环面上的两种走法：绕远路与回路

把方框向四面八方重复铺开，就得到整个平面 $\R^2$（覆盖空间）；每个方框里都有起点和终点的一份拷贝，$(\theta_1,\theta_2)$ 与 $(\theta_1+2\pi m,\ \theta_2+2\pi n)$（$m,n$ 为整数）在 $\mathcal C$ 里是同一个构型。从 S 走到终点，可以挑任意一份终点的拷贝 G，在平面里走直线。落回环面上，这些路径的起点终点都一样，却走法不同：终点在方块 $(m,n)$ 里，意味着关节 1 比最短走法多转了 $m$ 圈，关节 2 多转了 $n$ 圈。这就是"为什么手臂有时会反向转一整圈"：规划器选了另一份拷贝，也许因为最短的那条被障碍、限位或者线缆挡住了。

更有意思的是终点正好是起点自己的另一份拷贝：路径是一条回到原姿势的闭合曲线，手臂转了整整一圈。这样的回路不能连续地缩成一个点（环面上绕过洞的圈收不回去）；要把 $\mathcal C$ 里的路径按"能否连续变形成彼此"分类，每一类对应一个整数向量 $(m,n)$（数学上 $\pi_1(T^2)=\mathbb Z^2$）。拖动下面的终点穿过方块边界，右边的三维环面上能同时看到这条路径。

FIG 4覆盖平面与环面

覆盖平面 ℝ²（方块 (m,n)）

环面 T²（可拖动旋转视角）

青色是起点 S（只能在中央方块里拖动），紫色是终点：实心点是当前选中的拷贝 G（可以拖到任意方块），空心点是其他拷贝。琥珀色是路径；绿色虚线是最短的那份拷贝对应的路径。右图的红圈是 $\theta_1$ 方向的基本回路，绿圈是 $\theta_2$ 方向的基本回路（都过 S）。

把终点拖进相邻的方块，使路径的 $|\Delta\theta_1|>\pi$：环面上的终点没变，路径却多绕了一圈。
把终点拖到起点的另一份拷贝上（青色空心点），得到一条绕了整整一圈的回路。读数里的"最短"标志变成了什么？

旋转：一个对径点被粘起来的球

环面是"方框把对边粘起来"；三维旋转 $SO(3)$ 的形状更怪。任何旋转都可以写成绕单位轴 $\hat{\mathbf n}$ 转 $\theta\in[0,\pi]$；把它放在 $\mathbf r=\theta\hat{\mathbf n}$ 处，所有旋转就填满一个半径为 $\pi$ 的实心球。但转 $\pi$ 的时候，绕 $\hat{\mathbf n}$ 与绕 $-\hat{\mathbf n}$ 给出同一个旋转，所以球面上的每一对对径点要粘在一起。这就是 $SO(3)$ 的形状（拓扑上叫三维实射影空间 $\mathbb{RP}^3$）。把一个点从球的一侧推出去，它会从正对面回来，而手里的物体始终在连续地转动。下图用三个滑块选轴和角度，让角度超过 $\pi$ 试试。

两个后果值得记住。（1）任何只用三个数的全局写法（欧拉角、轴角）要么有奇异点，要么在某处不连续，因为一个"边界粘起来的球"无法连续、一一对应地铺进 $\R^3$（5.2 节的万向节死锁就是它的表现）。（2）球里的"坐标距离"不是真实的旋转差：靠近边界时，球里相距很远的两点可能只差一点点旋转。

FIG 5SO(3)：半径 π 的球，对径点相同

旋转向量球 ‖r‖ ≤ π

同一个旋转：物体与转轴

左：琥珀色点是 $\mathbf r=\theta'\hat{\mathbf n}$，其中 $\theta'=\theta$（$\theta\le\pi$）或 $\theta-2\pi$（$\theta>\pi$）；虚线是转轴所在直径，两端的空心圈是被粘在一起的 $\pm\pi\hat{\mathbf n}$；琥珀色折线记录 $\theta$ 从 0 增大的轨迹，穿过球心的细虚线连接出口与入口。紫色点是固定的参考旋转 $R_a$。右：旋转后的物体（实线）与原位置（淡色）、转轴（琥珀色）和转过的角度弧。拖动任一画面可以转动视角。

把 $\theta$ 从 0 拖过 $\pi$（比如用"转一整圈"）：点从球的一侧消失、从另一侧出现，但右边的物体始终在平滑转动，读数里"两种写法的差"始终是 0。
先按"对准 R_a 的轴"，再把 $\theta$ 调到 3.3 附近：球里的坐标距离超过 5，而真实的旋转角度差不到 1 rad。

三种视角看同一件事：双覆盖

单位四元数 $q\in S^3$（四维空间里的单位球面）与旋转是二对一：$q$ 和 $-q$ 给出同一个旋转（5.2 节）。$S^3$ 切掉一半（一个"半球"）就是图里的实心球，球面上被粘起来的对径点，就是 $S^3$ 的赤道上 $q$ 与 $-q$ 的配对。在 $S^3$ 上没有粘合，路径转 $4\pi$ 才回到原点；在 $SO(3)$ 里转 $2\pi$ 就回到了原姿势，但这条回路缩不成一点，转两圈（$4\pi$）的回路才可以（$\pi_1(SO(3))=\mathbb Z_2$，"腰带戏法"）。

5再看数学

提示：把鼠标悬停在公式中带底色的符号上，FIG 2 与 FIG 3 里对应的元素会被高亮。

定义：构型、构型空间、流形

构型 $\mathbf q$ 是对机器人上每一个点的位置的完整描述；所有构型的集合是构型空间 $\mathcal C$，其维数就是自由度数 $n$（7.1 节）。$\mathcal C$ 是一个 $n$ 维流形：每个点附近都能用 $n$ 个实数做局部坐标（图表），但一般没有一组坐标管全局。圆 $S^1$ 需要至少两个图表（角度在 $\pm\pi$ 处剪开，剪口的位置可以随意挪，也可以用第二个图表盖住剪口）；$S^1\times S^1$ 的每个图表是方框，边要粘起来。工作空间与构型空间通过正运动学联系：

$$ \mathrm{FK}:\ \mathcal C\ \longrightarrow\ SE(3),\qquad \mathbf q\ \longmapsto\ {}^{0}T_{n}(\mathbf q). $$

FK 一般是多对一的：一个手爪位姿对应的所有 $\mathbf q$ 就是逆运动学的解集（FIG 1 的两个点，7.5 节）；在 FK 的微分降秩的地方（奇异位形，3.4 节）解集的结构会突然变化。

常见机器人的构型空间

复合系统的构型空间是各部分的乘积。转动关节无限位时贡献 $S^1$，有限位时贡献一个区间 $I$；移动关节贡献一个区间；刚体贡献 $SE(2)$（平面，$\cong\R^2\times S^1$）或 $SE(3)$（空间，$\cong\R^3\times SO(3)$）。（$\cong$ 表示作为流形同胚；作为群 $SE(n)$ 并不是直积，但拓扑上是。）

系统	$\mathcal C$	维数	nq / nv
平面两连杆（无限位）	$T^2=S^1\times S^1$	2	2 / 2 （角度）或 4 / 2（Pinocchio：$\cos,\sin$）
平面刚体（移动机器人）	$SE(2)\cong\R^2\times S^1$	3	3 / 3 或 4 / 3 （$x,y,\cos\phi,\sin\phi$）
六自由度机械臂（有限位）	$I^6$	6	6 / 6
7 自由度臂装在平面底盘上（无限位）	$SE(2)\times T^7$	10	10 / 10 （角度）或 18 / 10（Pinocchio）
四旋翼 / 自由飞行刚体	$SE(3)\cong\R^3\times SO(3)$	6	7 / 6 （四元数）
人形机器人：浮动基座 + 30 个关节	$SE(3)\times I^{30}$	36	37 / 36

最后一列是软件里的约定，值得注意：用四元数记姿态时，位置变量数 nq（描述构型要存多少个数）比速度变量数 nv（$\dot{\mathbf q}$ 或广义速度有多少个分量）多一个，因为单位四元数有 4 个分量，而 $SO(3)$ 只有 3 维。$\dim\mathcal C$ 永远等于 nv。FIG 7 里可以自己搭一个机器人，看这几个数怎么变。

度量：什么叫"两个构型很近"

$\mathcal C$ 的拓扑（谁和谁相邻）是固定的，但距离需要自己选。几个常用的、各自正确的选择：

$$ d_{S^1}(a,b)=\hl{wrapd}{\big|\operatorname{wrap}(b-a)\big|},\qquad d_{T^n}(\boldsymbol\theta,\boldsymbol\theta')=\sqrt{\sum_{i=1}^{n}\operatorname{wrap}(\theta_i'-\theta_i)^2}, $$ $$ d_{SO(3)}(R_1,R_2)=\big\|\Log(R_1\T R_2)\big\|=\arccos\frac{\operatorname{tr}(R_1\T R_2)-1}{2}=2\arccos\big|\langle q_1,q_2\rangle\big|. $$

最后一个等号把距离写成四元数的内积，绝对值 $|\cdot|$ 恰好处理了 $q$ 与 $-q$ 的双覆盖。$SO(3)$ 上的距离就是两个旋转之间那一个相对旋转的角度（5.5–5.6 节的对数映射）。对 $SE(3)$ 没有唯一正确的距离：位置差有长度单位，转角差没有，只能写成 $d^2=\|\mathbf p_1-\mathbf p_2\|^2+\ell^2\,d_{SO(3)}^2$，其中长度尺度 $\ell$ 是一个设计选择（比如机器人的特征尺寸）。关节混有转动与移动关节时同理：弧度和米不能直接相加。

C-障碍物与自由空间

设机器人在构型 $\mathbf q$ 时占据的工作空间区域为 $\mathcal A(\mathbf q)$，障碍物区域为 $\mathcal O$。则

$$ \hl{cobs}{\mathcal C_{\text{obs}}}=\big\{\mathbf q\in\mathcal C:\ \mathcal A(\mathbf q)\cap\mathcal O\neq\varnothing\big\},\qquad \hl{cfree}{\mathcal C_{\text{free}}}=\mathcal C\setminus\mathcal C_{\text{obs}}. $$

对 FIG 2 的臂：两根连杆是线段 $s_1(\theta_1)$（基座到肘）与 $s_2(\theta_1,\theta_2)$（肘到手爪），厚度 $\rho$，障碍物是胶囊（线段加半径 $r$），于是 $\mathbf q\in\mathcal C_{\text{obs}}\iff\min_k\operatorname{dist}\big(s_k,\ \text{obstacle axis}\big)\le r+\rho$。注意 $s_1$ 只依赖 $\theta_1$，所以只要它碰到障碍物，整条 $\theta_1=\text{const}$ 的竖线都在 $\mathcal C_{\text{obs}}$ 里：这就是红色竖带。$\mathcal C_{\text{obs}}$ 通常是闭集，$\mathcal C_{\text{free}}$ 是开集；$\mathcal C_{\text{free}}$ 可能有多个连通分量，起点和终点要在同一个分量里才有路径。

为什么旋转没有"好"的三参数全局坐标

$SO(3)$ 是一个紧致的三维流形（有限大小、没有边界）。如果存在连续且一一对应的三参数全局坐标，$SO(3)$ 就会同胚地嵌进 $\R^3$；由区域不变性，像会是 $\R^3$ 的开集，但开集不可能是紧致的，矛盾。所以欧拉角必须有奇异点（万向节死锁，5.2 节），轴角必须在 $\theta=\pi$ 处出现不连续（FIG 5 的粘合边界）。可以连续、无奇异地表示旋转的方式至少需要更多的数：单位四元数（4 个数，二对一）、旋转矩阵（9 个数）；Zhou 等人（CVPR 2019）进一步证明，用神经网络回归旋转时，维数不超过 4 的表示都不连续，而 6 维表示（旋转矩阵的前两列，再做 Gram–Schmidt）连续，效果更好。

约束把构型空间切成子流形

闭链机构（四连杆、并联机器人、双臂抓同一物体）里关节角不独立，必须满足闭环方程 $\boldsymbol\phi(\mathbf q)=\mathbf 0$。可行构型是 $\R^N$ 或环面里的子流形 $\{\boldsymbol\phi=\mathbf 0\}$，维数是 $N-\operatorname{rank}(\partial\boldsymbol\phi/\partial\mathbf q)$。平面四连杆有 3 个活动连杆角、2 个闭环方程，于是 $\mathcal C$ 是环面 $T^3$ 里的一条曲线（1 维，和 7.1 节 Grübler 公式算出的自由度一致），并且往往不止一个分支（比如"开口"与"交叉"两种装配方式），不同分支之间只能拆开重装才能到达。在这样的 $\mathcal C$ 里做规划需要特殊的方法（投影、切空间采样），也是 7.1 节强调"约束数"的原因。

速度住在切空间里

构型的变化率 $\dot{\mathbf q}$ 不在 $\mathcal C$ 里，而在 $\mathcal C$ 在 $\mathbf q$ 处的切空间 $T_{\mathbf q}\mathcal C$（$n$ 维线性空间）里。对环面和区间，$T_{\mathbf q}\mathcal C=\R^n$，$\dot{\mathbf q}=\dot{\boldsymbol\theta}$；对 $SO(3)$，$\dot R=R\,[\boldsymbol\omega]_\times$，切空间由角速度 $\boldsymbol\omega\in\R^3$ 表示，而不是四元数的 4 个导数。这就是 nq $\neq$ nv 的来源，也是 6.10 节"用切空间里的增量更新流形上的点"的理由：

$$ \mathbf q_{k+1}=\mathbf q_k\oplus\mathbf v\,\Delta t\quad(\text{for a rotation: } R_{k+1}=R_k\Exp(\boldsymbol\omega\,\Delta t)),\qquad \mathbf v=\mathbf q_{k+1}\ominus\mathbf q_k. $$

其中 $\oplus$ 是指数映射（沿切向量走一步），$\ominus$ 是对数映射（两点之间的位移）。机器人软件里的 integrate 和 difference 就是这一对操作。

6机器人中的它：窄通道与"数一数你的 nq 和 nv"

窄通道。规划器最怕的不是障碍物多，而是自由空间里的"窄门"。下面是一个只有三个自由度的例子：一根杆（平面刚体，$\mathcal C=SE(2)$）要穿过墙上的一个门洞。门洞净宽约 0.48，杆长 1.2，是门宽的 2.5 倍：杆只有在大致对准门的法线方向时才过得去，允许的朝向只是很窄的一段。在 $\mathcal C$ 里，这就是一条细长的自由通道。右图是 $\mathcal C$ 沿杆中心 $x$ 坐标取的一个切片（横轴 $y$，纵轴朝向 $\phi$）：红色是会撞墙的 $(y,\phi)$，中间那一小块自由区域就是"门"。拖动杆的中心靠近墙，切片里的自由区域会收窄；杆一旦竖起来（$\phi$ 接近 $\pm90°$），门就关上了。

这就是采样式规划器（Part IV 的 PRM、RRT）的难处：在 $\mathcal C$ 里随机撒点，落进窄门的概率与它占的体积成比例，而这个体积往往很小；维数越高，情况越糟。FIG 2 里我们用 $180^2$ 个格点就能画出整个 $\mathcal C$，六自由度臂用同样的分辨率要 $180^6\approx3.4\times10^{13}$ 个格点，已经不可能了，所以实际系统都是采样并只对采到的点做碰撞检测。

FIG 6门洞里的杆：SE(2) 的一个切片

工作空间：杆 (x, y, φ)

C-空间切片 (y, φ)，固定当前 x

左：拖动琥珀色圆圈移动杆的中心，拖动杆端箭头旋转它（箭头指向 $\phi$）。右：在当前 $x$ 处，所有碰撞的 $(y,\phi)$ 涂红；横轴 $y$，纵轴 $\phi$（上下边粘在一起，杆有箭头所以周期是 $2\pi$，自由区在 $\phi=0$ 与 $\phi=\pm\pi$ 各有一块）。琥珀色圆圈是当前杆的位置，也可以在这里拖动。灰色虚线是理论值：杆心在门轴线上（$x=0$）、$y=0$ 时可倾斜的上限。

把杆的中心放到门轴线上（$|x|<0.04$），让杆无碰撞地倾斜超过 $57°$。杆长是门宽的 2.5 倍，为什么还能这样倾斜？切片里这个点在哪？

数一数你的 nq 与 nv。下面搭一个机器人，看它的构型空间的拓扑、维数，以及软件里的 nq、nv。

FIG 7搭一个机器人：构型空间的账本

dim 𝒞

条形图把每一项分成基座部分（琥珀色）与关节部分（青色/紫色）。nq 按 Pinocchio 的约定：自由飞行基座 7（位置 + 四元数），平面基座 4（$x,y,\cos\phi,\sin\phi$），有限位转动关节 1，无限位（连续）转动关节 2（$\cos\theta,\sin\theta$），移动关节 1；nv 就是自由度数。最后一行是用每个轴 100 个格点做网格需要的格点数，只用来感受维数的代价。

选"浮动基座"并把转动关节加到 20 个以上（或点"人形"预设）：nq 比 nv 多几个？无限位关节选项又会怎样？

真实系统里

规划：OMPL 把构型空间建成"状态空间"（RealVectorStateSpace、SO2StateSpace、SO3StateSpace、SE2StateSpace、SE3StateSpace 以及它们的 CompoundStateSpace 组合），每种都自带符合其拓扑的距离、插值与采样；MoveIt 在其上调用，连续（无限位）转动关节自动取最短弧。碰撞检测（FCL、Bullet、Coal）通常是规划时间的主要部分，因为每个采样点和每条边都要问一次"这个构型自由吗"。动力学库区分 nq 与 nv：Pinocchio 的 model.nq/model.nv 与 integrate、difference、distance、randomConfiguration，Drake 的 num_positions()/num_velocities() 与 MapQDotToVelocity，MuJoCo 的 nq/nv 与 mj_integratePos：浮动基座的 free joint 是 7 个位置变量、6 个速度变量。学习：策略的动作与观测里出现的关节角要先写成 $(\sin\theta,\cos\theta)$ 或相对于目标的包裹差，姿态用 6D 或四元数（并处理 $q$ 与 $-q$），否则网络要在 $\pm\pi$ 剪口处学习一个不连续的函数。

7常见误区

误区 1：把角度当成实数

在最近邻搜索、线性插值、平均、关节误差 $\theta_{\text{des}}-\theta$ 里直接做减法或加法，在 $\pm\pi$ 剪口处会让机器人"绕远路"（甚至让关节猛转一整圈）。FIG 3 的数字：$a=3$，$b=-3$ 时，直接相减是 $-6$ rad，而真实的差只有 $+0.28$ rad。反过来的坑：如果关节有机械限位（比如 $[-170°,170°]$），它根本到不了剪口另一侧，这时的 $\mathcal C$ 是区间而不是圆，包裹反而是错的。先问清楚：这个关节是连续的还是有限位的？

误区 2：四元数 $q$ 和 $-q$ 是不同的点

它们是同一个姿态，却是 $S^3$ 上相距最远的两点。把四元数当作 4 维向量做欧氏距离、平均或回归损失，可能得到"最远"的结果。常见的做法：先把 $q$ 翻转到与参考同一半球（$\langle q,q_{\text{ref}}\rangle\ge0$），再计算；距离用 $2\arccos|\langle q_1,q_2\rangle|$；平均四元数用 $\sum q_iq_i\T$ 的最大特征向量（对 $\pm$ 不敏感）。

误区 3：以为工作空间里的小障碍在 $\mathcal C$ 里也是一小块

FIG 2 里一个小圆盘在 $\mathcal C$ 里是带形、可能分成两份、靠近基座时变成贯穿整个 $\theta_2$ 轴的竖带。障碍物的 C-空间形状取决于机器人的运动学，不是障碍物本身形状的简单放大。这也意味着"工作空间里离障碍物很远"不能推出"在 $\mathcal C$ 里离 $\mathcal C_{\text{obs}}$ 很远"，反之亦然：用关节空间的欧氏距离判断是否安全是没有意义的，必须回到几何上算最小距离。

误区 4：忽略关节限位和连通性

关节限位把环面切成盒子，碰撞又把它切成若干块。一个起点终点都自由的任务可能因为在不同的连通分量里而无解（FIG 2 中收紧 $\theta_1$ 限位并放一个障碍物）；也可能"有解但要绕远"（FIG 4 的另一份拷贝）。规划失败时，先区分：是规划器太弱，还是问题本身不连通？对工业臂常见的"逆运动学有解但路径规划失败"，很多时候就是肘部分支被障碍物与限位隔开了。

误区 5：混淆构型维数与速度维数，或把不同量纲相加

浮动基座机器人 $\mathbf q$ 有 $n+7$ 个数，而 $\dot{\mathbf q}$（或广义速度 $\mathbf v$）只有 $n+6$ 个；直接做 $\mathbf q_{k+1}=\mathbf q_k+\mathbf v\Delta t$ 在维数上就是错的，在含四元数的分量上更是不再保持单位长度。必须用 integrate（指数映射）。另一个相关的坑：把位置（米）和角度（弧度）放进同一个向量做二范数距离，"1 米等于 1 弧度"是一个悄悄做出的设计选择，用 $\ell$ 显式地表达它。

8小练习

1. 平面两连杆臂（$l_1\ne l_2$，无关节限位），手爪位于可达范围内部的某一点（既不在最外圈也不在最内圈）。在构型空间（环面）里有几个构型让手爪恰好在这一点？

肘部朝上与朝下各一个（$\theta_2=\pm\arccos c_2$，$\theta_1$ 随之确定）。每个转动关节的 $\theta$ 与 $\theta+2\pi$ 是同一个构型，所以不会因为"多转一圈"再多出解。只有在边界上（手臂完全伸直或折叠，$\theta_2=0$ 或 $\pi$）两个解合并成一个。

2. 一个关节从 $\theta_a=3.0$ rad 转到 $\theta_b=-3.0$ rad（该关节无限位）。沿最短的方向，它需要转多少？

$\operatorname{wrap}(b-a)=\operatorname{wrap}(-6)=-6+2\pi\approx+0.283$。两个角都靠近 $\pi$，只隔着剪口，所以沿正方向转 0.28 rad 即可到达。选项 A 是直接相减。

3. 一个 7 自由度机械臂（7 个无限位转动关节）装在平面移动底盘上（可前后左右移动并旋转）。它的构型空间是多少维？

底盘是 $SE(2)$（3 维：$x,y,\phi$），臂是 $T^7$（7 维）。$\mathcal C=SE(2)\times T^7$，$3+7=10$ 维。13 是把底盘当成 $SE(3)$（6 维）的结果，那是浮动基座，而不是平面底盘。

4. 一个人形机器人有浮动基座和 30 个有限位转动关节。按 Pinocchio / Drake / MuJoCo 的约定（姿态用四元数），它的 nq 与 nv 是？

基座贡献 7 个位置变量（3 位置 + 4 四元数）与 6 个速度变量（3 线速度 + 3 角速度），每个有限位转动关节各贡献 1 个。$\texttt{nq}=7+30=37$，$\texttt{nv}=6+30=36=\dim\mathcal C$。

5. 为什么不存在一种只用三个数、既连续又没有奇异点、能描述所有三维旋转的全局参数化？

旋转正好有 3 个自由度（四元数的 4 个分量带一个单位长度约束），问题不是数目，而是形状：无边界的紧致三维流形不可能嵌入 $\R^3$（区域不变性）。这是拓扑定理，不是工程上的缺陷，欧拉角的万向节死锁与轴角在 $\pi$ 处的跳变是它的两种表现。

9延伸阅读

S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006, Ch. 4 "The Configuration Space" — 流形、拓扑、刚体与连杆机构的构型空间、C-障碍物，免费在线
T. Lozano-Pérez, "Spatial Planning: A Configuration Space Approach", IEEE Transactions on Computers C-32(2), 108–120, 1983 — 构型空间用于运动规划的经典论文
K. Lynch & F. Park, Modern Robotics, Ch. 2 (Configuration Space) and Ch. 10 (Motion Planning) — 自由度、拓扑与表示、Grübler 公式，以及规划里的 C-障碍物
H. Choset et al., Principles of Robot Motion: Theory, Algorithms, and Implementations, MIT Press, 2005, Ch. 3 — 构型空间的几何与障碍物的构造
Y. Zhou, C. Barnes, J. Lu, J. Yang, H. Li, "On the Continuity of Rotation Representations in Neural Networks", CVPR 2019 — 为什么用神经网络回归旋转时 4 维以下的表示都不连续，以及 6D 表示
I. A. Şucan, M. Moll, L. E. Kavraki, "The Open Motion Planning Library", IEEE Robotics & Automation Magazine 19(4), 72–82, 2012 — OMPL 的状态空间设计

系统	\(\mathcal C\)	维数	nq / nv
平面两连杆（无限位）	\(T^2=S^1\times S^1\)	2	2 / 2 （角度）或 4 / 2（Pinocchio：\(\cos,\sin\)）
平面刚体（移动机器人）	\(SE(2)\cong\R^2\times S^1\)	3	3 / 3 或 4 / 3 （\(x,y,\cos\phi,\sin\phi\)）
六自由度机械臂（有限位）	\(I^6\)	6	6 / 6
7 自由度臂装在平面底盘上（无限位）	\(SE(2)\times T^7\)	10	10 / 10 （角度）或 18 / 10（Pinocchio）
四旋翼 / 自由飞行刚体	\(SE(3)\cong\R^3\times SO(3)\)	6	7 / 6 （四元数）
人形机器人：浮动基座 + 30 个关节	\(SE(3)\times I^{30}\)	36	37 / 36