第2章 配置空间与工作空间：C-Space概念、位形空间构建、障碍物映射、自由空间与障碍空间
好，咱们正式开始聊轨迹规划里最基础、也最容易让人绕晕的一个概念——配置空间。我当年刚入行时，看了三天论文才搞明白它到底在说什么。说白了，这玩意儿就是给机器人“换个坐标系”来思考问题。
2.1 工作空间 vs 配置空间：你到底在哪儿？
先问个问题：你平时看到的机器人，是在什么空间里运动的？
答案是——工作空间（Workspace）。也就是我们肉眼看到的、机器人末端执行器在三维物理世界里的位置和姿态。比如一个机械臂的末端在 (x, y, z) 坐标，或者一个移动机器人在房间里的 (x, y, θ) 位置。
但问题来了：轨迹规划的核心是“让机器人从A点走到B点，同时避开障碍物”。如果你只在工作空间里规划，你会发现——
- 机械臂的每个关节角度组合，对应着末端的一个位姿；但反过来，一个末端位姿可能对应多个关节角度组合（逆运动学多解）。
- 障碍物在工作空间里是三维物体，但机器人本身也是三维物体，碰撞检测变得极其复杂。
- 你没法直接告诉电机“末端走到 (0.5, 0.3, 0.2)”，你得告诉每个关节“转到多少度”。
所以，聪明的前辈们想了个办法：把问题从工作空间映射到配置空间（Configuration Space，简称C-Space）。
核心定义：配置空间是机器人所有可能位形（Configuration）的集合。一个位形，就是一组能完全描述机器人每个自由度位置的参数。对于n自由度的机器人，C-Space就是一个n维空间。
举个例子：一个6轴工业机械臂，每个关节角度 θ₁, θ₂, ..., θ₆ 就是一个位形。所有可能的 (θ₁, θ₂, ..., θ₆) 组合，构成了一个6维的C-Space。你想想看，在这个空间里，机器人退化成了一个“点”——每个点代表一个完整的机器人姿态。
| 概念 | 维度 | 代表什么 | 规划难度 |
|---|---|---|---|
| 工作空间 | 3D (x,y,z) 或 6D (位姿) | 机器人末端在物理世界的位置 | 高（机器人形状复杂） |
| 配置空间 | nD（n=自由度） | 机器人每个关节/自由度的状态 | 低（机器人退化为点） |
2.2 位形空间构建：从物理世界到抽象空间
怎么构建C-Space？我习惯分三步走：
- 确定自由度：数清楚机器人有几个独立运动的关节。移动机器人一般是3个（x, y, θ），平面机械臂2个，空间机械臂6个。
- 定义每个自由度的范围：关节限位、移动范围等。比如关节1只能转 -180° 到 +180°。
- 建立映射关系：正运动学把C-Space的点映射到工作空间；逆运动学反过来。
这里有个关键点：C-Space的拓扑结构。不是所有C-Space都是简单的欧几里得空间。比如旋转关节的角度范围是 [0, 2π)，但0和2π其实是同一个位置——这就形成了环形拓扑（S¹）。对于6轴机械臂，它的C-Space其实是 (S¹)⁶，一个6维环面。嗯，这里要注意，如果你用欧几里得空间的距离去度量，可能会出问题。
我的经验：在代码实现时，我通常用浮点数数组表示位形，但处理角度关节时，一定要做角度归一化（wrap to [-π, π]）。我曾经在一个项目中没做归一化，结果规划器以为关节要转360°才能到目标，白白浪费了计算资源。
2.3 障碍物映射：把“墙”变成“禁区”
这是C-Space里最巧妙的一步。在工作空间里，障碍物是三维物体；但在C-Space里，我们要把障碍物映射成“机器人不能进入的位形区域”。
具体怎么做？
想象一下：你有一个2自由度的平面机械臂（两个旋转关节），工作空间里有一面墙。对于每一个可能的关节角度组合 (θ₁, θ₂)，你计算一下此时机械臂的每个连杆是否与墙碰撞。如果碰撞，就把这个 (θ₁, θ₂) 标记为“障碍物空间”（C-obstacle）。
说白了，C-obstacle就是所有会导致机器人碰撞的位形的集合。
避坑指南：我曾经天真地以为，工作空间里的障碍物映射到C-Space后还是简单的形状。结果发现——完全不是！一个工作空间里的矩形障碍物，映射到C-Space后可能变成扭曲的、非凸的、甚至分片的区域。这是因为机器人的运动学是非线性的。所以，别想着用解析法求C-obstacle边界，老老实实用采样或离散化方法。
2.4 自由空间与障碍空间：规划的核心战场
有了C-Space和C-obstacle，剩下的就是自由空间（Free Space）了：
C_free = C_Space - C_obstacle
轨迹规划的任务，就是在C_free中找一条从起始位形到目标位形的连续路径。注意，是连续路径——这意味着路径上的每一个点都不能落在C_obstacle里。
这里有个重要的性质：C_free的连通性。如果起始点和目标点在C_free的同一个连通分量里，那么存在可行路径；否则，无解。我遇到过不少新手，规划失败后疯狂调参数，结果发现是起始位形本身就在C_obstacle里——机器人一开始就“卡住了”。
| 空间类型 | 定义 | 规划中的角色 |
|---|---|---|
| C_Space | 所有可能位形的集合 | 搜索空间的总范围 |
| C_obstacle | 导致碰撞的位形集合 | 禁止进入的区域 |
| C_free | C_Space - C_obstacle | 路径搜索的可行域 |
2.5 核心知识体系：一张图看懂
下面这张图是我自己总结的C-Space知识框架，帮你把整个逻辑串起来：
2.6 实际项目中的C-Space处理
聊点实际的。在真实项目中，C-Space的维度可能很高（比如人形机器人有30+自由度），你不可能显式地构建整个C-Space。那怎么办？
我常用的策略是：
- 采样法：不构建完整的C-Space，而是随机采样位形，只对采样到的点做碰撞检测。PRM、RRT这些算法就是这么干的。
- 降维处理：如果机器人有冗余自由度，可以固定一部分自由度，在低维子空间里规划。
- 增量式构建：边规划边构建，只探索路径附近的C-Space区域。
小技巧：在做碰撞检测时，别每次都做精确的网格碰撞。先用包围盒（AABB或OBB）做粗略检测，快速排除大部分无碰撞位形。我习惯用“先粗后精”的两级检测策略，速度能提升5-10倍。
最后说一句：C-Space的概念虽然抽象，但它是所有现代轨迹规划算法的基石。你理解了它，后面学PRM、RRT、A*在C-Space上的变种，就会觉得顺理成章。嗯，今天就到这儿，把这张图存下来，后面章节我们会反复用到它。