4、C空间与构型空间：机器人规划的“隐形战场”
各位工程师朋友，欢迎来到第四章。今天聊的话题，是轨迹规划里最基础、也最容易被忽视的概念——构型空间，也就是我们常说的 C 空间。
说实话，我刚开始做机器人规划时，根本没把 C 空间当回事。觉得不就是把机器人关节角度画个坐标系嘛，有什么好学的？直到有一次，我在做七轴机械臂的避障规划，算法怎么跑都撞墙，调试了整整三天。最后才发现，我一直在笛卡尔空间里做规划，完全忽略了 C 空间的特性。嗯，从那以后，我再也不敢跳过 C 空间了。
4.1 什么是构型空间？
先问大家一个问题：机器人的“位置”到底怎么描述？
如果你问一个移动机器人，它可能会说“我在 (x, y, θ)”。但如果你问一个六轴机械臂，它得告诉你六个关节的角度值 (θ₁, θ₂, …, θ₆)。
这六个角度值，就构成了一个构型。所有可能的构型组成的集合，就是构型空间，简称 C 空间。
核心定义：构型空间 = 机器人所有可能状态的集合。每个状态对应 C 空间中的一个点。
举个例子。一个在平面上移动的小车，它的构型是 (x, y, θ)。x 和 y 是位置，θ 是朝向。那么它的 C 空间就是 R² × S¹——两个直线维度加一个旋转维度。说白了，就是所有可能的位置和朝向的组合。
我个人习惯把 C 空间想象成一个“地图”。地图上的每个点，都对应机器人真实世界中的一个姿态。规划问题，就变成了在这个地图上找一条从起点到终点的路径。
4.2 障碍物在 C 空间中的表示
好，现在问题来了：真实世界里有障碍物，它们在 C 空间里长什么样？
这其实是整个 C 空间理论最精妙的地方。真实世界中的障碍物，经过机器人的运动学映射，会变成 C 空间中的禁区——也就是机器人不能进入的区域。
我给大家画个图，帮助理解这个映射过程。
你看，真实空间里一个方方正正的障碍物，到了 C 空间就变成了一个扭曲的形状。为什么会这样？因为机器人的关节运动是非线性的。一个关节转动，可能让末端执行器走出一条弧线，这个弧线扫过的区域，在 C 空间里就是一块禁区。
我记得在做一个协作机器人项目时，遇到过一个问题：桌面上放了一个杯子，机器人要绕过它抓取后面的零件。在笛卡尔空间里看，杯子只是一个小圆柱。但在 C 空间里，这个杯子对应的禁区却非常大——因为机器人的大臂和小臂都可能撞到它。这就是 C 空间障碍物“膨胀”的现象。
实战技巧：在做碰撞检测时，不要只看末端是否碰到障碍物。要把整个机器人的连杆都考虑进去。我习惯用“扫掠体”的方法，把每个连杆在运动过程中扫过的体积都算进 C 空间障碍物里。
4.3 C 空间维度的概念
聊完了障碍物，咱们说说维度。C 空间的维度，说白了就是机器人自由度的数量。
来看个表格，一目了然：
| 机器人类型 | 自由度 | C 空间维度 | 典型构型表示 |
|---|---|---|---|
| 平面移动小车 | 3 | 3 维 | (x, y, θ) |
| 差分驱动小车 | 3 | 3 维 | (x, y, θ) |
| 四轴机械臂 | 4 | 4 维 | (θ₁, θ₂, θ₃, θ₄) |
| 六轴工业机械臂 | 6 | 6 维 | (θ₁, θ₂, …, θ₆) |
| 七轴协作机器人 | 7 | 7 维 | (θ₁, θ₂, …, θ₇) |
| 四旋翼无人机 | 6 | 6 维 | (x, y, z, roll, pitch, yaw) |
这里有个关键点：维度越高，规划越难。你想想看，2 维空间里找路径，就是在一张纸上画线。3 维空间里找路径，是在一个立方体里穿行。到了 6 维、7 维，人脑根本没法想象——这就是所谓的“维度诅咒”。
我曾经做过一个项目，用 RRT 算法给七轴机械臂做规划。在 7 维 C 空间里，随机采样的点大部分都落在障碍物区域里。算法的效率低得令人发指。后来我换了一种采样策略，才勉强跑通。
避坑指南：我曾经犯过一个错误——在 C 空间维度很高时，还使用均匀随机采样。结果算法几乎找不到可行路径。后来我改用“目标偏置采样”，让采样点更倾向于朝着终点方向生成，效率提升了 10 倍以上。
4.4 C 空间的拓扑结构
说到维度，就不得不提 C 空间的拓扑结构。不是所有维度都是直线型的。比如旋转关节，它的角度是循环的——0° 和 360° 其实是同一个位置。
这就引出了 C 空间的几种基本拓扑：
- Rⁿ（欧几里得空间）：平移关节、移动机器人的位置部分。直线维度，没有边界循环。
- S¹（圆环）：单个旋转关节。0° 和 360° 是同一个点。
- Tⁿ（n 维环面）：多个旋转关节的组合。比如六轴机械臂的 C 空间就是 T⁶。
- SE(2) / SE(3)：移动机器人的位姿空间。SE(2) = R² × S¹，SE(3) = R³ × SO(3)。
你可能会问：知道拓扑有什么用？用处大了。比如在 S¹ 上做规划时，你不能用普通的欧几里得距离——因为 350° 到 10° 的直线距离是 340°，但实际最短路径只有 20°（绕一圈）。
我习惯在代码里专门处理这种循环维度。用一个 wrap_angle 函数，把角度差值归一化到 [-π, π] 之间。
def wrap_angle(angle):
"""将角度归一化到 [-π, π]"""
while angle > math.pi:
angle -= 2 * math.pi
while angle < -math.pi:
angle += 2 * math.pi
return angle
def angular_distance(a1, a2):
"""计算两个角度之间的最短距离"""
diff = wrap_angle(a1 - a2)
return abs(diff)核心要点：C 空间的拓扑结构决定了距离度量的方式。用错了距离，规划出来的路径可能绕远路，甚至根本找不到路。
4.5 从 C 空间到实际规划
好了，讲了这么多理论，咱们回到实际。C 空间到底怎么用在轨迹规划里？
流程其实很清晰：
- 建立 C 空间模型：确定机器人的自由度，定义每个维度的范围。
- 构建障碍物映射：把真实世界的障碍物，通过运动学映射到 C 空间中，标记出 C_obs 区域。
- 定义起点和终点：把真实世界的起始姿态和目标姿态，转换成 C 空间中的两个点。
- 在 C 空间中规划：在 C_free（自由空间）中找一条连接起点和终点的连续路径。
- 映射回真实空间：把 C 空间中的路径点，通过正运动学转换回关节角度或末端位姿。
这个流程看起来简单，但每一步都有坑。比如第二步，构建障碍物映射时，如果机器人有多个连杆，你需要对每个连杆都做碰撞检测。我见过有人只检测末端，结果大臂撞墙了都不知道。
再比如第四步，在 C 空间中规划时，路径的平滑性很重要。C 空间中的直线，映射回真实空间可能是一条很扭曲的轨迹。所以我在规划完成后，通常会加一个平滑后处理步骤。
我的习惯：在 C 空间规划完成后，我会用 B 样条曲线对路径进行拟合。这样既能保证路径在 C 空间中的连续性，又能让真实空间的运动更平滑。具体做法是：把 C 空间路径点作为控制点，生成一条 3 阶 B 样条，然后重新采样。
最后，我想强调一点：C 空间不是万能的。对于高自由度机器人，C 空间的维度太高，直接规划可能很慢。这时候可以考虑混合规划——在 C 空间中做全局规划，在笛卡尔空间中做局部优化。这个我们后面章节会详细讲。