Part II · 第 7 章结构与运动学 · 7.2Part II · Ch. 7 Structure & Kinematics · 7.2

构型空间Configuration Space

一台 $n$ 自由度的机器人，此刻的整个样子只是一个 $n$ 维空间里的一个点。规划、逆运动学、控制和学习都在这个空间里发生，但它和手爪所在的三维空间完全是两回事：角度会绕回来，旋转的空间是一个对径点被粘起来的球，一个圆形的障碍物在这里会变成扭曲的红色区域。这一节把这个空间画出来、摸一摸，并弄清楚它的形状为什么决定了"两个构型有多近"、"怎样做插值和平均"，以及 $\texttt{nq}\ne\texttt{nv}$ 是怎么回事。 A robot with $n$ degrees of freedom is, at every instant, one point in an $n$-dimensional space. Planning, inverse kinematics, control and learning all happen there, yet it is a completely different thing from the 3D space the gripper moves in: angles wrap around, the space of rotations is a ball whose antipodal boundary points are glued together, and a round obstacle becomes a twisted red region. This lesson draws that space, lets you touch it, and explains why its shape decides what "close" means, how to interpolate and average, and where $\texttt{nq}\ne\texttt{nv}$ comes from.

约 35 分钟~35 min 前置：7.1 连杆、关节与自由度 · 5.2 旋转的表示 · 5.4 为什么需要李群 · 3.4 机器人雅可比Prereqs: 7.1 Links, Joints & DoF · 5.2 Representing Rotations · 5.4 Why Lie Groups? · 3.4 The Robot Jacobian 7 个交互7 interactives

1为什么机器人需要它Why robots need this

要说清一台机械臂"现在是什么姿势"，只需要给出每个关节转了多少：一组关节角 $\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 节教你数自由度，本节则讲这个数字背后的空间。

To say which posture an arm is in, it is enough to give how far each joint has turned: a list of joint angles $\boldsymbol\theta=(\theta_1,\dots,\theta_n)$. Treat that list as a point and the whole robot is compressed into one point. The space of all such points is the configuration space (C-space), written $\mathcal C$. A robot with $n$ degrees of freedom has an $n$-dimensional $\mathcal C$ (Lesson 7.1 explains how to count $n$). Nearly every robotics algorithm runs in this space:

Motion planning: moving from posture A to posture B is a curve in $\mathcal C$; "do not hit anything" is a set that curve must avoid. (RRT and PRM in Part IV sample inside $\mathcal C$.)
Inverse kinematics: given a gripper pose, find the points of $\mathcal C$ with $\mathrm{FK}(\mathbf q)=T$: there may be two, infinitely many, or none (Lesson 7.5).
Control and learning: the robot's state is a point of $\mathcal C$ plus its velocity. A policy's observations and actions are mostly quantities in $\mathcal C$, so "how close are two configurations" decides how nearest neighbours, averages, interpolation and loss functions must be written.

Keep two spaces apart: the workspace is the set of positions the gripper can reach (in $\R^3$), whereas the configuration space describes the whole robot. Elbow-up and elbow-down put the gripper at the same spot yet are two far-apart points of $\mathcal C$; a table top is a flat slab in the workspace but a strangely shaped region in $\mathcal C$.

This lesson builds on Lesson 5.1 (frames), 5.2 (representing rotations), 5.4 (Lie groups) and 6.10 (optimization on manifolds): there we saw that "a rotation is not a vector in $\R^3$"; here we see what that means for a whole robot. Lesson 7.1 taught you to count degrees of freedom; this one is about the space behind that number.

2直觉：两个数，一个点；方框的边是粘起来的Intuition: two numbers, one point, and a box with glued edges

从最简单的例子开始：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$，所以"手爪离基座同样远"的所有构型恰好落在方框里的两条水平线上（紫色虚线）。

Start with the simplest example: the planar arm of Lesson 3.4, keeping just two links ($l_1=1$, $l_2=0.8$). It has two joint angles $\theta_1,\theta_2$, so $\mathcal C$ is two-dimensional and can be drawn in full: $\theta_1$ horizontally, $\theta_2$ vertically, every point in the box one posture of the arm. The two panels below are two spellings of the same thing: drag a joint on the left and the point on the right follows; drag the point on the right and the arm on the left follows.

Notice two things. First, $\theta=\pi$ and $\theta=-\pi$ are the same direction, so the left and right edges of the box are glued together, and so are the top and bottom: drag the point off the right edge and it comes back from the left while the arm turns continuously. Glue the left and right edges and you get a cylinder; glue its two ends and you get the surface of a doughnut, the torus $T^2=S^1\times S^1$. Second, one gripper position corresponds to two points of $\mathcal C$ (elbow up and elbow down): the hollow purple dot on the right and the purple ghost arm on the left. The distance from the base depends only on $\theta_2$: $\|\mathbf p\|^2=l_1^2+l_2^2+2l_1l_2\cos\theta_2$, so all configurations with the gripper "equally far from the base" lie on two horizontal lines of the box (the dashed purple lines).

FIG 1手臂与它的环面The arm and its torus

工作空间（手爪在哪里）WORKSPACE · where the hand is

构型空间 T²（机器人在做什么）C-SPACE T² · what the robot is doing

左：拖动红色圆圈改变 $\theta_1$，绿色圆圈改变 $\theta_2$。右：拖动琥珀色点；方框外的淡色部分是周期延拓（同一个点的拷贝），点被拖出边界后会从对面回来。紫色空心点与虚影臂是手爪保持不动时的另一种肘部构型，紫色水平虚线是"离基座同样远"的两条等距线。 Left: drag the red circle to change $\theta_1$, the green one to change $\theta_2$. Right: drag the amber dot; the faded area outside the box is the periodic continuation (copies of the same point), and a dot dragged out of the box returns from the opposite side. The hollow purple dot and the ghost arm are the other elbow configuration with the gripper held still; the dashed purple horizontal lines are the two "equally far from the base" curves.

试一试Try this

让 $\theta_1$ 越过 $\pm\pi$：把右图的点拖出右边框，或者让第一根连杆转过 $-x$ 轴。点从另一边出现，而手臂的运动是连续的。Push $\theta_1$ across $\pm\pi$: drag the right-hand dot through the right edge, or swing the first link past the $-x$ axis. The dot reappears on the other side while the arm moves continuously.
按"翻转肘部"：手爪纹丝不动，右图的点却跳到了很远的另一个位置。它跳到了哪里（提示：$\theta_2\to-\theta_2$）？Press "Flip elbow": the gripper does not move, yet the dot jumps to a distant place. Where does it land (hint: $\theta_2\to-\theta_2$)?

核心洞见Key insight

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

Configuration $\ne$ position. Configuration space is a space with glued edges, not $\R^n$: each revolute joint contributes a circle $S^1$, not a line. Locally it looks like $\R^n$ ($n$ numbers describe nearby points), globally it is curved and wraps around. A space that is "locally Euclidean but globally not" is called a manifold.

3动手玩：障碍物在构型空间里长什么样Play: what an obstacle looks like in configuration space

现在放进一个障碍物。对 $\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$ 里是一条曲线。拖动滑块或按"播放"，手臂会沿着当前路径运动，碰到障碍物的连杆会变红。

Now add an obstacle. For every point $\mathbf q$ of $\mathcal C$ ask a yes/no question: "in this posture, does either link (including the one attached to the base) touch the obstacle?" Paint every "yes" red: that is the C-obstacle $\mathcal C_{\text{obs}}$; the rest is the free space $\mathcal C_{\text{free}}$. The figure below does this for $180\times180$ configurations (geometrically, a segment-to-disc or segment-to-capsule distance). Drag the obstacle on the left and watch the red region on the right.

Then do what a planner most wants to do: draw a straight line in C-space from the start S to the goal G, $\mathbf q(t)=\mathbf q_s+t\,\Delta$, and test whether it crosses the red region. Because angles wrap, there are two such lines: each angle takes the short arc (wrapped, shortest), or you simply subtract (unwrapped). A third option is a straight line in the workspace: the gripper moves along a line and inverse kinematics supplies the joint angles at each point (keeping the start's elbow branch); in $\mathcal C$ that is a curve. Drag the slider or press Play and the arm follows the current path, with any link that hits the obstacle turning red.

FIG 2C-障碍物与路径C-obstacles and paths

工作空间WORKSPACE

构型空间：红 = C-障碍物C-SPACE · red = C-obstacle

右图中青色圆点 S 是起点，紫色圆点 G 是终点，琥珀色圆点是"探针"：它的构型就是左图中手臂的姿势（也可以直接拖动它）。绿色实线是自由的路径，红色虚线是碰撞之后的部分，叉号标出第一个碰撞点。斜线条纹区域是被关节限位切掉的部分；自由空间按连通分量着色（与起点相连的是淡绿色，其余是淡紫色）。连杆有 0.045 的半径；不考虑自碰撞。 On the right the cyan dot S is the start, the purple dot G the goal, and the amber dot is a "probe": its configuration is the arm's posture on the left (you can also drag it). The solid green line is the free part of the path, the red dashed line what follows the first collision, with a cross at the first hit. The diagonal stripes are regions cut away by the joint limits; free space is tinted by connected component (the one containing S pale green, others pale purple). Links have radius 0.045; self-collision is ignored.

试一试Try this

选"不包裹"路径。拖动障碍物，让最短的直线撞上它，而不包裹的那条自由（提示：把障碍物放到 $\theta_1$ 经过 $\pm\pi$ 的那一侧，即手臂向左摆的地方）。Select the "unwrapped" path. Drag the obstacle so that the shortest line hits it while the unwrapped one is free (hint: put the obstacle where $\theta_1$ passes through $\pm\pi$, i.e. to the left of the base).
选"工作空间直线"。找到一个障碍物位置，使得最短的 C-空间直线撞上它，而手爪的直线路径是自由的：同一对起点终点，两种"直线"不是一回事。Select the "workspace line". Find an obstacle position where the shortest C-space line hits it but the gripper's straight path is free: for the same endpoints, the two kinds of "straight" are different things.
让自由空间分裂成 $\ge2$ 块：试试"两个圆盘"，或者收紧 $\theta_1$ 的限位。为什么一个靠近基座的圆盘会切出一整条竖直的红带？Split the free space into $\ge2$ pieces: try "two discs", or tighten the $\theta_1$ limit. Why does a disc near the base cut out a whole vertical red stripe?

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

A few things deserve a pause. (1) A disc is not a disc in $\mathcal C$, because the coordinates are angles, not lengths; it usually appears twice, for elbow-up and elbow-down ways of touching it. (2) If the disc is near the base, the first link hits it for every $\theta_2$, so the red region becomes a full vertical stripe; two stripes cut the torus in two, with no way across. (3) Free start and goal with a blocked straight line does not mean there is no path: as long as both lie in the same connected component, a curve around the red region exists, and finding it is the planner's job.

4第二、第三视角：圆、环面与旋转球Second and third views: circles, tori and the rotation ball

角度住在圆上，而不是数轴上Angles live on a circle, not on a line

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

Why must the edges of the box be glued? Because $\theta$ and $\theta+2\pi$ give the robot exactly the same look, so a joint angle is a point on the circle $S^1$. Cut the circle open at $\pm\pi$ and straighten it and you get the familiar interval $[-\pi,\pi]$, at a price: $179°$ and $-179°$, neighbours across the cut, become two points $358°$ apart on the number line. Every operation built on subtraction fails at the cut: differences, averages, interpolation, nearest neighbours, error feedback. Drag a and b in the figure below (on the circle or on the unrolled number line) and compare "just subtract" with "do it on the circle".

FIG 3圆 vs. 数轴：差与平均Circle vs. number line: difference and mean

上：单位圆，红色刻度是剪口 $\pm\pi$。下：把圆在剪口处展开成数轴，两端 $-\pi\equiv\pi$ 是同一个点。红色虚线是"直接相减"（$b-a$）走的弧，绿色实线是包裹后的最短弧；红色菱形是算术平均，绿色菱形是圆上的平均（单位向量求和再取角度）。当 a、b 恰好相对（相差 $\pi$）时圆上的平均没有定义。 Top: the unit circle; the red tick is the cut at $\pm\pi$. Bottom: the circle unrolled at the cut into a number line whose ends $-\pi\equiv\pi$ are the same point. The red dashed arc is what "just subtract" ($b-a$) travels, the solid green arc the wrapped shortest arc; the red diamond is the arithmetic mean, the green diamond the mean on the circle (sum unit vectors, take the angle). When a and b are exactly opposite (differ by $\pi$) the circular mean is undefined.

让两种平均落在圆的几乎相对的两侧（相差超过 2.5 rad）。例如 $a=3$，$b=-3$：算术平均是 0，两个角其实都在 $\pi$ 附近。Make the two means land on nearly opposite sides of the circle (more than 2.5 rad apart). For example $a=3$, $b=-3$: the arithmetic mean is 0, yet both angles are really near $\pi$.

圆上的四个基本操作The four basic operations on a circle

差：$\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 节"在圆上取平均"的一般形式：在流形上，"相减"换成"沿最短路径走的位移"（对数映射），"加上位移"换成"沿路径走一步"（指数映射）。

Difference: $\operatorname{wrap}(b-a)$ with $\operatorname{wrap}(x)=x-2\pi\lfloor (x+\pi)/2\pi\rfloor\in[-\pi,\pi)$. Distance: $|\operatorname{wrap}(b-a)|$. Interpolation: $a+t\operatorname{wrap}(b-a)$. Mean: $\operatorname{atan2}(\sum\sin\theta_i,\sum\cos\theta_i)$. These are the circle case of Lesson 6.10's "averaging on a circle": on a manifold "subtract" becomes "the displacement along the shortest path" (the log map) and "add a displacement" becomes "take a step along a path" (the exp map).

环面上的两种走法：绕远路与回路Two ways to go on a torus: the long way round, and loops

把方框向四面八方重复铺开，就得到整个平面 $\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$）。拖动下面的终点穿过方块边界，右边的三维环面上能同时看到这条路径。

Tile the box in every direction and you get the whole plane $\R^2$ (the covering space); every tile holds a copy of the start and of the goal, and $(\theta_1,\theta_2)$ and $(\theta_1+2\pi m,\ \theta_2+2\pi n)$ ($m,n$ integers) are the same configuration of $\mathcal C$. To go from S, you may pick any copy G of the goal and walk the straight line in the plane. Back on the torus all these paths share their endpoints but travel differently: a goal in tile $(m,n)$ means joint 1 turns $m$ more times than on the shortest route and joint 2 $n$ more. That is "why the arm sometimes unwinds a full turn": the planner picked another copy, perhaps because the shortest one was blocked by an obstacle, a joint limit or a cable.

More interesting is when the goal is another copy of the start itself: the path is a closed curve returning to the original posture after a full revolution. Such a loop cannot be continuously shrunk to a point (a loop around the hole of a doughnut will not slide off); paths in $\mathcal C$ fall into classes of "can be deformed into each other", one class per integer vector $(m,n)$ (mathematically $\pi_1(T^2)=\mathbb Z^2$). Drag the goal across a tile boundary below and watch the same path on the 3D torus on the right.

FIG 4覆盖平面与环面The covering plane and the torus

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

环面 T²（可拖动旋转视角）TORUS T² (drag to orbit)

青色是起点 S（只能在中央方块里拖动），紫色是终点：实心点是当前选中的拷贝 G（可以拖到任意方块），空心点是其他拷贝。琥珀色是路径；绿色虚线是最短的那份拷贝对应的路径。右图的红圈是 $\theta_1$ 方向的基本回路，绿圈是 $\theta_2$ 方向的基本回路（都过 S）。 Cyan is the start S (draggable only inside the central tile); purple is the goal: the solid dot is the currently selected copy G (drag it into any tile), hollow dots are the other copies. Amber is the path; the dashed green line is the path to the copy that gives the shortest route. On the torus the red circle is the basic loop in the $\theta_1$ direction and the green circle the basic loop in the $\theta_2$ direction (both through S).

把终点拖进相邻的方块，使路径的 $|\Delta\theta_1|>\pi$：环面上的终点没变，路径却多绕了一圈。Drag the goal into a neighbouring tile so that the path has $|\Delta\theta_1|>\pi$: the goal on the torus has not changed but the path winds once more.
把终点拖到起点的另一份拷贝上（青色空心点），得到一条绕了整整一圈的回路。读数里的"最短"标志变成了什么？Drag the goal onto another copy of the start (a hollow cyan dot) to get a loop that winds a full turn. What does the "shortest" flag in the readout say?

旋转：一个对径点被粘起来的球Rotations: a ball whose antipodal boundary points are glued

环面是"方框把对边粘起来"；三维旋转 $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）球里的"坐标距离"不是真实的旋转差：靠近边界时，球里相距很远的两点可能只差一点点旋转。

The torus is "a box with opposite edges glued"; the space of 3D rotations $SO(3)$ is stranger. Any rotation is a turn by $\theta\in[0,\pi]$ about a unit axis $\hat{\mathbf n}$; put it at $\mathbf r=\theta\hat{\mathbf n}$ and all rotations fill a solid ball of radius $\pi$. But at $\theta=\pi$, turning about $\hat{\mathbf n}$ and about $-\hat{\mathbf n}$ give the same rotation, so every pair of antipodal points on the surface is glued. That is the shape of $SO(3)$ (topologically, real projective 3-space $\mathbb{RP}^3$). Push a point out of one side of the ball and it re-enters from the opposite side while the object in your hand keeps turning continuously. In the figure below, choose an axis and angle with three sliders, then push the angle past $\pi$.

Two consequences are worth remembering. (1) Any global description with just three numbers (Euler angles, axis-angle) has either a singularity or a discontinuity somewhere, because a "ball with glued boundary" cannot be laid continuously and one-to-one into $\R^3$ (the gimbal lock of Lesson 5.2 is how it shows up). (2) "Coordinate distance" in the ball is not the real rotation difference: near the boundary two points far apart in the ball may differ by only a small rotation.

FIG 5SO(3)：半径 π 的球，对径点相同SO(3): a ball of radius π with antipodal points identified

旋转向量球 ‖r‖ ≤ πROTATION-VECTOR BALL ‖r‖ ≤ π

同一个旋转：物体与转轴THE SAME ROTATION: body and axis

左：琥珀色点是 $\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$。右：旋转后的物体（实线）与原位置（淡色）、转轴（琥珀色）和转过的角度弧。拖动任一画面可以转动视角。 Left: the amber dot is $\mathbf r=\theta'\hat{\mathbf n}$ with $\theta'=\theta$ ($\theta\le\pi$) or $\theta-2\pi$ ($\theta>\pi$); the dashed diameter is the rotation axis, its two hollow end rings are the glued points $\pm\pi\hat{\mathbf n}$; the amber polyline traces $\theta$ growing from 0, with a thin dashed line through the centre joining the exit and the re-entry. The purple dot is a fixed reference rotation $R_a$. Right: the rotated body (solid) against the original (faint), the axis (amber) and the swept angle arc. Drag either panel to orbit.

把 $\theta$ 从 0 拖过 $\pi$（比如用"转一整圈"）：点从球的一侧消失、从另一侧出现，但右边的物体始终在平滑转动，读数里"两种写法的差"始终是 0。Drag $\theta$ from 0 past $\pi$ (or press "Spin a full turn"): the dot vanishes on one side and reappears on the other, yet the body on the right turns smoothly and the "difference between the two spellings" stays 0.
先按"对准 R_a 的轴"，再把 $\theta$ 调到 3.3 附近：球里的坐标距离超过 5，而真实的旋转角度差不到 1 rad。Press "Align axis with R_a", then set $\theta$ near 3.3: the coordinate distance in the ball exceeds 5 while the true rotation angle between them is below 1 rad.

三种视角看同一件事：双覆盖Three views of one fact: the double cover

单位四元数 $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$，"腰带戏法"）。

Unit quaternions $q\in S^3$ (the unit sphere in four dimensions) map two-to-one onto rotations: $q$ and $-q$ are the same rotation (Lesson 5.2). Cut $S^3$ in half (take a "hemisphere") and you get the solid ball in the figure; the glued antipodal points on its surface are the pairs $q,-q$ on the equator of $S^3$. On $S^3$ nothing is glued and a path needs $4\pi$ to return; in $SO(3)$ a $2\pi$ turn already brings the posture back, but that loop cannot be shrunk to a point while a double loop ($4\pi$) can ($\pi_1(SO(3))=\mathbb Z_2$, the "belt trick").

5再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，FIG 2 与 FIG 3 里对应的元素会被高亮。Tip: hover the highlighted symbols in the formulas; the matching elements in FIG 2 and FIG 3 light up.

定义：构型、构型空间、流形Definitions: configuration, C-space, manifold

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

A configuration $\mathbf q$ is a complete description of where every point of the robot is; the set of all configurations is the configuration space $\mathcal C$, whose dimension is the number of degrees of freedom $n$ (Lesson 7.1). $\mathcal C$ is an $n$-dimensional manifold: near each point, $n$ real numbers serve as local coordinates (a chart), but in general no single set of coordinates works globally. The circle $S^1$ needs at least two charts (the angle is cut at $\pm\pi$, but the cut can be moved, or a second chart can cover it); each chart of $S^1\times S^1$ is a box whose edges are to be glued. Workspace and configuration space are related by forward kinematics:

$$ \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 节）解集的结构会突然变化。

FK is generally many-to-one: all $\mathbf q$ over one gripper pose form the inverse-kinematics solution set (the two points in FIG 1, Lesson 7.5); where the differential of FK loses rank (singular postures, Lesson 3.4) the structure of that set changes abruptly.

常见机器人的构型空间Configuration spaces of common robots

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

The configuration space of a composite system is the product of its parts. An unlimited revolute joint contributes $S^1$, a limited one an interval $I$; a prismatic joint contributes an interval; a rigid body contributes $SE(2)$ (planar, $\cong\R^2\times S^1$) or $SE(3)$ (spatial, $\cong\R^3\times SO(3)$). ($\cong$ means homeomorphic as manifolds; as a group $SE(n)$ is not a direct product, but topologically it is.)

系统System	$\mathcal C$	维数dim	nq / nv
平面两连杆（无限位）Planar 2R (no limits)	$T^2=S^1\times S^1$	2	2 / 2 （角度）或 4 / 2（Pinocchio：$\cos,\sin$）(angles) or 4 / 2 (Pinocchio: $\cos,\sin$)
平面刚体（移动机器人）Planar rigid body (mobile robot)	$SE(2)\cong\R^2\times S^1$	3	3 / 3 或or 4 / 3 （$x,y,\cos\phi,\sin\phi$）($x,y,\cos\phi,\sin\phi$)
六自由度机械臂（有限位）6-DoF arm (with limits)	$I^6$	6	6 / 6
7 自由度臂装在平面底盘上（无限位）7-DoF arm on a planar base (no limits)	$SE(2)\times T^7$	10	10 / 10 （角度）或 18 / 10（Pinocchio）(angles) or 18 / 10 (Pinocchio)
四旋翼 / 自由飞行刚体Quadrotor / free-flying rigid body	$SE(3)\cong\R^3\times SO(3)$	6	7 / 6 （四元数）(quaternion)
人形机器人：浮动基座 + 30 个关节Humanoid: floating base + 30 joints	$SE(3)\times I^{30}$	36	37 / 36

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

The last column is a software convention worth noticing: when orientation is stored as a quaternion, the number of position variables nq (numbers needed to store a configuration) exceeds the number of velocity variables nv (components of the generalized velocity) by one, because a unit quaternion has 4 components while $SO(3)$ is 3-dimensional. $\dim\mathcal C$ always equals nv. In FIG 7 you can assemble a robot and watch these numbers change.

度量：什么叫"两个构型很近"Metrics: what it means for two configurations to be close

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

The topology of $\mathcal C$ (what is next to what) is fixed, but a distance has to be chosen. Some standard, correct choices:

$$ 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$ 是一个设计选择（比如机器人的特征尺寸）。关节混有转动与移动关节时同理：弧度和米不能直接相加。

The last equality writes the distance with the quaternion inner product, where the absolute value $|\cdot|$ takes care of the double cover $q$ versus $-q$. The distance on $SO(3)$ is the angle of the relative rotation between the two (the log map of Lessons 5.5–5.6). On $SE(3)$ there is no single correct distance: position differences carry a length unit, angle differences do not, so one writes $d^2=\|\mathbf p_1-\mathbf p_2\|^2+\ell^2\,d_{SO(3)}^2$, with the length scale $\ell$ a design choice (say, the robot's characteristic size). The same holds for mixed revolute and prismatic joints: radians and metres cannot simply be added.

C-障碍物与自由空间C-obstacles and free space

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

Let $\mathcal A(\mathbf q)$ be the workspace region the robot occupies at configuration $\mathbf q$, and $\mathcal O$ the obstacle region. Then

$$ \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}}$ 可能有多个连通分量，起点和终点要在同一个分量里才有路径。

For the arm of FIG 2 the links are the segments $s_1(\theta_1)$ (base to elbow) and $s_2(\theta_1,\theta_2)$ (elbow to hand) with thickness $\rho$, and the obstacle is a capsule (a segment with radius $r$), so $\mathbf q\in\mathcal C_{\text{obs}}\iff\min_k\operatorname{dist}\big(s_k,\ \text{obstacle axis}\big)\le r+\rho$. Note that $s_1$ depends on $\theta_1$ only, so once it touches the obstacle the entire line $\theta_1=\text{const}$ lies in $\mathcal C_{\text{obs}}$: that is the red vertical stripe. $\mathcal C_{\text{obs}}$ is typically closed and $\mathcal C_{\text{free}}$ open; $\mathcal C_{\text{free}}$ may have several connected components, and a path exists only if start and goal lie in the same one.

为什么旋转没有"好"的三参数全局坐标Why rotations have no good global three-parameter coordinates

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

$SO(3)$ is a compact three-dimensional manifold (finite, no boundary). If a continuous one-to-one three-parameter global coordinate existed, $SO(3)$ would embed homeomorphically in $\R^3$; by invariance of domain the image would be an open subset of $\R^3$, but an open set cannot be compact, a contradiction. So Euler angles must have singularities (gimbal lock, Lesson 5.2), and axis-angle must be discontinuous at $\theta=\pi$ (the glued boundary of FIG 5). Representations that are continuous and singularity-free need more numbers: unit quaternions (4 numbers, two-to-one) or rotation matrices (9 numbers); Zhou et al. (CVPR 2019) went further and showed that, when regressing rotations with a neural network, any representation of dimension 4 or lower is discontinuous while a 6D one (the first two columns of the matrix, followed by Gram–Schmidt) is continuous and trains better.

约束把构型空间切成子流形Constraints cut C-space down to a submanifold

闭链机构（四连杆、并联机器人、双臂抓同一物体）里关节角不独立，必须满足闭环方程 $\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 节强调"约束数"的原因。

In closed-chain mechanisms (four-bar linkages, parallel robots, two arms holding one object) the joint angles are not independent but obey loop-closure equations $\boldsymbol\phi(\mathbf q)=\mathbf 0$. The feasible configurations form a submanifold $\{\boldsymbol\phi=\mathbf 0\}$ of $\R^N$ or of a torus, of dimension $N-\operatorname{rank}(\partial\boldsymbol\phi/\partial\mathbf q)$. A planar four-bar has 3 moving link angles and 2 loop equations, so $\mathcal C$ is a curve (1-dimensional, matching Lesson 7.1's Grübler count) inside the torus $T^3$, often with more than one branch (the "open" and "crossed" assembly modes), and one branch can only be reached from the other by disassembling the linkage. Planning in such a $\mathcal C$ needs special methods (projection, tangent-space sampling), which is why Lesson 7.1 stresses the number of constraints.

速度住在切空间里Velocities live in the tangent space

构型的变化率 $\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 节"用切空间里的增量更新流形上的点"的理由：

The rate of change $\dot{\mathbf q}$ does not lie in $\mathcal C$ but in the tangent space $T_{\mathbf q}\mathcal C$ of $\mathcal C$ at $\mathbf q$, an $n$-dimensional vector space. For tori and intervals $T_{\mathbf q}\mathcal C=\R^n$ and $\dot{\mathbf q}=\dot{\boldsymbol\theta}$; for $SO(3)$, $\dot R=R\,[\boldsymbol\omega]_\times$, and the tangent space is represented by the angular velocity $\boldsymbol\omega\in\R^3$, not by the four derivatives of a quaternion. That is where nq $\neq$ nv comes from, and why Lesson 6.10 updates a point on a manifold with an increment in the tangent space:

$$ \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 就是这一对操作。

Here $\oplus$ is the exp map (take a step along a tangent vector) and $\ominus$ the log map (the displacement between two points). The integrate and difference functions in robotics software are exactly this pair.

6机器人中的它：窄通道与"数一数你的 nq 和 nv"In robotics: narrow passages, and "count your nq and 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}$ 个格点，已经不可能了，所以实际系统都是采样并只对采到的点做碰撞检测。

Narrow passages. What a planner fears is not many obstacles but a narrow door in free space. Here is a three-DoF example: a rod (a planar rigid body, $\mathcal C=SE(2)$) must pass through a doorway in a wall. The doorway is about 0.48 wide and the rod 1.2 long, 2.5 times the width: the rod fits only when roughly aligned with the door normal, and the allowed orientations form a narrow band. In $\mathcal C$ that is a thin free corridor. The right panel is a slice of $\mathcal C$ at the rod centre's current $x$ (horizontal $y$, vertical orientation $\phi$): red marks the $(y,\phi)$ that hit the wall, and the small free region in the middle is the "door". Drag the rod centre toward the wall and the free region in the slice narrows; once the rod stands upright ($\phi$ near $\pm90°$), the door is shut.

This is the difficulty of sampling-based planners (PRM and RRT in Part IV): a random sample lands in a narrow door with probability proportional to its volume, which is often tiny, and the higher the dimension, the worse it gets. In FIG 2 a $180^2$ grid draws all of $\mathcal C$; a six-DoF arm at the same resolution would need $180^6\approx3.4\times10^{13}$ cells, which is out of the question, so real systems sample and run collision checks only at the sampled points.

FIG 6门洞里的杆：SE(2) 的一个切片A rod in a doorway: a slice of SE(2)

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

C-空间切片 (y, φ)，固定当前 xC-SPACE SLICE (y, φ) at the current x

左：拖动琥珀色圆圈移动杆的中心，拖动杆端箭头旋转它（箭头指向 $\phi$）。右：在当前 $x$ 处，所有碰撞的 $(y,\phi)$ 涂红；横轴 $y$，纵轴 $\phi$（上下边粘在一起，杆有箭头所以周期是 $2\pi$，自由区在 $\phi=0$ 与 $\phi=\pm\pi$ 各有一块）。琥珀色圆圈是当前杆的位置，也可以在这里拖动。灰色虚线是理论值：杆心在门轴线上（$x=0$）、$y=0$ 时可倾斜的上限。 Left: drag the amber circle to move the rod's centre and the arrow end to rotate it (the arrow points along $\phi$). Right: at the current $x$, every colliding $(y,\phi)$ is painted red; horizontal $y$, vertical $\phi$ (top and bottom are glued; the rod has an arrow so the period is $2\pi$, giving one free lens at $\phi=0$ and one at $\phi=\pm\pi$). The amber circle is the rod's current pose and can also be dragged here. The grey dashed lines are the theoretical limit on tilt when the rod's centre is on the door line ($x=0$) at $y=0$.

把杆的中心放到门轴线上（$|x|<0.04$），让杆无碰撞地倾斜超过 $57°$。杆长是门宽的 2.5 倍，为什么还能这样倾斜？切片里这个点在哪？Put the rod's centre on the door line ($|x|<0.04$) and tilt the rod beyond $57°$ without collision. The rod is 2.5 times the door width; why can it still tilt this far? Where is that point in the slice?

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

Count your nq and nv. Assemble a robot below and see the topology and dimension of its configuration space, and the nq and nv the software reports.

FIG 7搭一个机器人：构型空间的账本Assemble a robot: the C-space ledger

dim 𝒞

条形图把每一项分成基座部分（琥珀色）与关节部分（青色/紫色）。nq 按 Pinocchio 的约定：自由飞行基座 7（位置 + 四元数），平面基座 4（$x,y,\cos\phi,\sin\phi$），有限位转动关节 1，无限位（连续）转动关节 2（$\cos\theta,\sin\theta$），移动关节 1；nv 就是自由度数。最后一行是用每个轴 100 个格点做网格需要的格点数，只用来感受维数的代价。 The bars split each count into the base part (amber) and the joint part (cyan/purple). nq follows Pinocchio's convention: a free-flyer base is 7 (position + quaternion), a planar base 4 ($x,y,\cos\phi,\sin\phi$), a limited revolute joint 1, an unlimited (continuous) revolute joint 2 ($\cos\theta,\sin\theta$), a prismatic joint 1; nv is the number of degrees of freedom. The last line is the number of cells a grid with 100 points per axis would need, to give a feel for the cost of dimension.

选"浮动基座"并把转动关节加到 20 个以上（或点"人形"预设）：nq 比 nv 多几个？无限位关节选项又会怎样？Choose the "floating" base and raise the revolute joints above 20 (or click the "humanoid" preset): by how much does nq exceed nv? What does the unlimited-joints option do?

真实系统里In real systems

规划：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$ 剪口处学习一个不连续的函数。

Planning: OMPL models configuration space as "state spaces" (RealVectorStateSpace, SO2StateSpace, SO3StateSpace, SE2StateSpace, SE3StateSpace and their CompoundStateSpace combinations), each with a distance, interpolation and sampler that respect its topology; MoveIt builds on it, and continuous (unlimited) revolute joints take the shortest arc automatically. Collision checking (FCL, Bullet, Coal) is usually the main cost of planning, since every sample and every edge asks "is this configuration free?". Dynamics libraries distinguish nq from nv: Pinocchio's model.nq/model.nv with integrate, difference, distance, randomConfiguration; Drake's num_positions()/num_velocities() with MapQDotToVelocity; MuJoCo's nq/nv with mj_integratePos: a floating base (free joint) has 7 position variables and 6 velocity variables. Learning: joint angles in policy observations and actions are best written as $(\sin\theta,\cos\theta)$ or as wrapped differences to a target, and orientation as a 6D representation or a quaternion (handling $q$ versus $-q$); otherwise the network must learn a discontinuous function at the $\pm\pi$ cut.

7常见误区Common pitfalls

误区 1：把角度当成实数Pitfall 1: treating angles as real numbers

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

Subtracting or adding directly in nearest-neighbour search, linear interpolation, averaging or the joint error $\theta_{\text{des}}-\theta$ makes the robot "take the long way" at the $\pm\pi$ cut (even spinning a joint a full turn). The numbers of FIG 3: for $a=3$, $b=-3$, plain subtraction gives $-6$ rad while the true difference is only $+0.28$ rad. The opposite trap: if the joint has a mechanical stop (say $[-170°,170°]$) it can never reach the other side of the cut, its $\mathcal C$ is an interval and not a circle, and wrapping is wrong. First ask: is this joint continuous or limited?

误区 2：四元数 $q$ 和 $-q$ 是不同的点Pitfall 2: treating quaternions $q$ and $-q$ as different points

它们是同一个姿态，却是 $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$ 不敏感）。

They are the same orientation yet the two farthest-apart points of $S^3$. Treat quaternions as 4D vectors for Euclidean distance, averaging or a regression loss and you can get the "farthest" answer. The usual remedies: flip $q$ into the same hemisphere as a reference ($\langle q,q_{\text{ref}}\rangle\ge0$) first; use $2\arccos|\langle q_1,q_2\rangle|$ as the distance; average quaternions with the top eigenvector of $\sum q_iq_i\T$ (insensitive to the sign).

误区 3：以为工作空间里的小障碍在 $\mathcal C$ 里也是一小块Pitfall 3: assuming a small workspace obstacle is a small blob in C-space

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

In FIG 2 a small disc becomes a band in $\mathcal C$, possibly in two copies, and near the base a stripe spanning the whole $\theta_2$ axis. The C-space shape of an obstacle is decided by the robot's kinematics and is not a simple enlargement of the obstacle's own shape. It also means "far from the obstacle in the workspace" does not imply "far from $\mathcal C_{\text{obs}}$ in $\mathcal C$", and vice versa: judging safety by Euclidean distance in joint space is meaningless, and one must go back to the geometry and compute the minimum distance.

误区 4：忽略关节限位和连通性Pitfall 4: ignoring joint limits and connectivity

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

Joint limits cut the torus into a box, and collisions cut it further into pieces. A task with free start and goal may have no solution because they lie in different connected components (tighten the $\theta_1$ limit and add an obstacle in FIG 2); or it may be solvable "only the long way" (the other copy in FIG 4). When planning fails, first tell apart a weak planner from a disconnected problem. The industrial-arm classic, "IK has a solution but path planning fails", is often an elbow branch cut off by obstacles and limits.

误区 5：混淆构型维数与速度维数，或把不同量纲相加Pitfall 5: confusing configuration and velocity dimensions, or adding different units

浮动基座机器人 $\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$ 显式地表达它。

A floating-base robot's $\mathbf q$ has $n+7$ numbers while $\dot{\mathbf q}$ (or the generalized velocity $\mathbf v$) has only $n+6$; $\mathbf q_{k+1}=\mathbf q_k+\mathbf v\Delta t$ is wrong already by dimension, and the quaternion part no longer keeps unit length. Use integrate (the exp map). A related trap: putting positions (metres) and angles (radians) in one vector and taking a 2-norm silently decides that "1 m equals 1 rad"; make that choice explicit with a length scale $\ell$.

8小练习Check yourself

1. 平面两连杆臂（$l_1\ne l_2$，无关节限位），手爪位于可达范围内部的某一点（既不在最外圈也不在最内圈）。在构型空间（环面）里有几个构型让手爪恰好在这一点？1. A planar two-link arm ($l_1\ne l_2$, no joint limits) has its gripper at an interior point of its reachable region (neither on the outer nor the inner boundary). How many configurations in the torus put the gripper exactly there?

肘部朝上与朝下各一个（$\theta_2=\pm\arccos c_2$，$\theta_1$ 随之确定）。每个转动关节的 $\theta$ 与 $\theta+2\pi$ 是同一个构型，所以不会因为"多转一圈"再多出解。只有在边界上（手臂完全伸直或折叠，$\theta_2=0$ 或 $\pi$）两个解合并成一个。One elbow-up and one elbow-down ($\theta_2=\pm\arccos c_2$, with $\theta_1$ then determined). $\theta$ and $\theta+2\pi$ are the same configuration for each revolute joint, so extra turns add no solutions. Only on the boundary (arm fully stretched or folded, $\theta_2=0$ or $\pi$) do the two solutions merge into one.

2. 一个关节从 $\theta_a=3.0$ rad 转到 $\theta_b=-3.0$ rad（该关节无限位）。沿最短的方向，它需要转多少？2. A joint (no limits) must move from $\theta_a=3.0$ rad to $\theta_b=-3.0$ rad. By the shortest direction, how far must it turn?

$\operatorname{wrap}(b-a)=\operatorname{wrap}(-6)=-6+2\pi\approx+0.283$。两个角都靠近 $\pi$，只隔着剪口，所以沿正方向转 0.28 rad 即可到达。选项 A 是直接相减。$\operatorname{wrap}(b-a)=\operatorname{wrap}(-6)=-6+2\pi\approx+0.283$. Both angles are near $\pi$, separated only by the cut, so a +0.28 rad turn gets there. Option A is plain subtraction.

3. 一个 7 自由度机械臂（7 个无限位转动关节）装在平面移动底盘上（可前后左右移动并旋转）。它的构型空间是多少维？3. A 7-DoF arm (seven unlimited revolute joints) is mounted on a planar mobile base (it can translate in the plane and rotate). What is the dimension of its configuration space?

底盘是 $SE(2)$（3 维：$x,y,\phi$），臂是 $T^7$（7 维）。$\mathcal C=SE(2)\times T^7$，$3+7=10$ 维。13 是把底盘当成 $SE(3)$（6 维）的结果，那是浮动基座，而不是平面底盘。The base is $SE(2)$ (3-dimensional: $x,y,\phi$), the arm $T^7$ (7-dimensional). $\mathcal C=SE(2)\times T^7$, $3+7=10$ dimensions. 13 would be treating the base as $SE(3)$ (6-dimensional), a floating base and not a planar one.

4. 一个人形机器人有浮动基座和 30 个有限位转动关节。按 Pinocchio / Drake / MuJoCo 的约定（姿态用四元数），它的 nq 与 nv 是？4. A humanoid has a floating base and 30 limited revolute joints. With the Pinocchio / Drake / MuJoCo convention (orientation as a quaternion), what are its nq and nv?

基座贡献 7 个位置变量（3 位置 + 4 四元数）与 6 个速度变量（3 线速度 + 3 角速度），每个有限位转动关节各贡献 1 个。$\texttt{nq}=7+30=37$，$\texttt{nv}=6+30=36=\dim\mathcal C$。The base contributes 7 position variables (3 position + 4 quaternion) and 6 velocity variables (3 linear + 3 angular), and each limited revolute joint contributes 1 of each. $\texttt{nq}=7+30=37$, $\texttt{nv}=6+30=36=\dim\mathcal C$.

5. 为什么不存在一种只用三个数、既连续又没有奇异点、能描述所有三维旋转的全局参数化？5. Why is there no global parametrization of all 3D rotations that uses three numbers and is continuous and singularity-free?

旋转正好有 3 个自由度（四元数的 4 个分量带一个单位长度约束），问题不是数目，而是形状：无边界的紧致三维流形不可能嵌入 $\R^3$（区域不变性）。这是拓扑定理，不是工程上的缺陷，欧拉角的万向节死锁与轴角在 $\pi$ 处的跳变是它的两种表现。A rotation has exactly 3 degrees of freedom (the quaternion's 4 components carry one unit-length constraint); the issue is shape, not count: a compact 3-manifold without boundary cannot be embedded in $\R^3$ (invariance of domain). This is a topological theorem, not an engineering deficiency; gimbal lock in Euler angles and the jump at $\pi$ in axis-angle are two of its faces.

9延伸阅读Further reading

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

系统System	\(\mathcal C\)	维数dim	nq / nv
平面两连杆（无限位）Planar 2R (no limits)	\(T^2=S^1\times S^1\)	2	2 / 2 （角度）或 4 / 2（Pinocchio：\(\cos,\sin\)）(angles) or 4 / 2 (Pinocchio: \(\cos,\sin\))
平面刚体（移动机器人）Planar rigid body (mobile robot)	\(SE(2)\cong\R^2\times S^1\)	3	3 / 3 或or 4 / 3 （\(x,y,\cos\phi,\sin\phi\)）(\(x,y,\cos\phi,\sin\phi\))
六自由度机械臂（有限位）6-DoF arm (with limits)	\(I^6\)	6	6 / 6
7 自由度臂装在平面底盘上（无限位）7-DoF arm on a planar base (no limits)	\(SE(2)\times T^7\)	10	10 / 10 （角度）或 18 / 10（Pinocchio）(angles) or 18 / 10 (Pinocchio)
四旋翼 / 自由飞行刚体Quadrotor / free-flying rigid body	\(SE(3)\cong\R^3\times SO(3)\)	6	7 / 6 （四元数）(quaternion)
人形机器人：浮动基座 + 30 个关节Humanoid: floating base + 30 joints	\(SE(3)\times I^{30}\)	36	37 / 36