最近有个做机器人课程设计的同学来问我，说老师给定了一个作业：一个不规则多边形机器人，在二维地图里绕开障碍物，自己走到目标点。但前提是不能用现成工具箱，必须自己实现。他纠结的点是——机器人明明带着形状，为什么路径规划却要先讲什么C-Space，还要扯上A*算法？这两者到底怎么串起来？
这个课题我在类似项目里也折腾过一轮。直接把工作空间里的多边形碰撞问题丢给路径搜索算法，会遇到大量不必要的计算，而先构造C-Space（Configuration Space，构型空间），把带形状的机器人简化成单个质点，再用A*在栅格化的C-Space上搜索，整套流程会清晰很多。这篇文章就把我从问题拆解到Matlab实现的全过程整理出来，适合正在做机器人路径规划课设、或者想从底层理解避障算法原理的读者。
1. 问题拆解：为什么非要从C-Space开始谈起
1.1 工作空间里做路径规划，到底难在哪
先想一个最基本的场景：地图里有一堆多边形障碍物，机器人也是一个多边形，你要给这个机器人规划一条从起点到终点的路径，要求整条路径上机器人不能和任何障碍物发生碰撞。
如果直接在所谓的工作空间（机器人实际运动的空间）里思考，问题立刻就变得很麻烦。因为机器人不是一个点，它有面积、有朝向。你判断“机器人中心点不在障碍物里面”是远远不够的，还要考虑机器人的边缘会不会擦到障碍物、直角转弯的时候机身会不会扫过障碍区。每一步运动都要做一次完整的多边形相交检测，计算量很大，而且路径搜索算法本身也很难直接融合这种连续空间的碰撞约束。
我踩过一个很典型的坑：一开始想用采样法直接在工作空间撒点，再用碰撞检测过滤非法点。结果问题来了——即便两个采样点之间画出的直线段不碰到障碍物，机器人以某个朝向沿着这条直线走的时候，机身仍然可能蹭到障碍物。路径是“点”层面的无碰撞，但不是“形体”层面的无碰撞。这个错位让人非常抓狂。
1.2 C-Space到底在做什么：把机器人缩成一个点
C-Space的核心思想，就是换一个视角看问题。机器人不是有形状吗？那好，我们把机器人身上选一个参考点（比如几何中心），然后问这样一个问题：当这个参考点在地图上的某个位置时，机器人会不会和障碍物碰撞？把所有会导致碰撞的参考点位置全部标记出来，这些位置就构成了C-Space中的“障碍物区域”。剩下的空白区域，就是机器人参考点可以安全到达的位置。
这样一来，原本“带形状的机器人绕障碍物”的问题，就转化成了“一个点在这张C-Space地图里绕开障碍区域”的问题。路径规划算法只需要处理一个点的移动，很多经典算法（A*、Dijkstra、RRT）都能直接用上。这也是为什么几乎所有的路径规划教材都要先讲C-Space——它把问题的维度降下来了，把碰撞约束从运动过程中剥离出来，变成了一张静态地图。我当时第一次在Matlab里画出C-Space障碍物之后，有一种“原来如此”的感觉：机器人被缩成一个点，地图上的障碍物全都膨胀了一圈，整个问题一下子就变简单了。
1.3 为什么选A*而不是Dijkstra或RRT
确定了在C-Space上做搜索之后，下一个问题是选择路径搜索算法。我最终选的是A*，原因有三个。
第一，C-Space栅格化之后，地图是离散的、已知的，A正好适用于这种静态离散网格上的最短路径搜索。第二，A引入了启发式函数（一般用欧几里得距离或曼哈顿距离），搜索时优先朝靠近目标点的方向扩展，比Dijkstra这种无方向的搜索快得多。第三，A*实现起来足够简单，不依赖采样，结果也稳定。
那为什么不选RRT（快速随机探索随机树）？RRT适合高维空间或者障碍物非常复杂、难以建模的场景，但它是概率完备的——也就是说它不保证每次都能找到路径，也不保证找到的是最优路径。对于二维C-Space栅格地图来说，A*既能保证最优性（在满足启发函数一致性的前提下），又能保证完备性（只要有路径就一定能找到），对学生党做课设来说非常合适。
2. C-Space构建的完整逻辑：从几何碰撞到栅格地图
2.1 多边形机器人的位姿描述
要构建C-Space，先得搞清楚怎么描述一个多边形机器人的“状态”。在二维平面上，一个刚体多边形其实有三个自由度：水平位置x、垂直位置y、朝向角θ。也就是说，一个完整的位姿可以写成(x, y, θ)。
但这里有一个工程上的取舍。如果机器人的朝向θ可以任意变化，那么C-Space就是一个三维空间（x, y, θ），栅格化之后会产生巨大的网格数量。举例来说，一张100×100的地图，如果只按位置划分栅格，是10000个格子；如果再把θ分成36个离散角度，那就是36万个栅格。构建C-Space的碰撞检测计算量直接翻了36倍。
所以我在这个项目里做了一个简化处理：固定机器人的朝向（θ=0），只考虑平移，不考虑旋转。这样做的好处是C-Space退化成二维，构建速度快，A*搜索也只需要查二维栅格；坏处是，如果实际机器人需要旋转着穿过狭窄通道，这种方法就会漏掉可行的路径。如果确实要考虑旋转，常见的做法是取几个离散角度（比如0°、45°、90°…）分别构建二维C-Space，然后搜索时把层与层之间的旋转也当成一种动作来处理。
2.2 碰撞检测：两个多边形是否相交
构建C-Space的底层是碰撞检测。说白了就是：给定机器人的参考点位置，把机器人多边形平移到那个位置，然后判断它和每个障碍物多边形是否相交。
判断两个凸多边形是否相交，最常用的方法是分离轴定理（Separating Axis Theorem，SAT）。简单说就是：把两个多边形分别投影到所有边的法线方向上，如果在某一条法线方向上，两个多边形的投影区间没有重叠，那这两个多边形就一定不相交；如果所有方向上投影都重叠，那它们就相交了。这个算法对凸多边形非常高效，对凹多边形要先拆分成多个凸多边形。
在我实际写的Matlab代码里，为了更通用，我用了另一种做法：遍历机器人多边形的每一条边，判断这条边是否与障碍物多边形的任意一条边相交；再反向遍历障碍物的边，判断是否与机器人多边形相交；最后再检查一个多边形是否完全包含另一个多边形。四步检查做完，只要任何一步有“碰撞”或“包含”的迹象，就判定为发生碰撞。这种方法简洁直观，代码也容易写。
2.3 栅格化C-Space的工程实现
有了碰撞检测函数之后，构建C-Space就是个体力活了：遍历地图上的每一个栅格点，把机器人参考点移到这个位置，调用碰撞检测，如果碰撞了就标记为障碍物栅格，否则标记为自由栅格。
这里要注意栅格分辨率的设置。我第一次做的时候把分辨率设得很高，比如每1个像素取一个点，结果一张地图几万个点，每个点都要做多边形相交检测，Matlab跑了将近两分钟，慢到怀疑人生。后来把分辨率调低，让每个栅格代表5个像素，C-Space构建一下就快了很多。分辨率的选择直接影响计算速度和地图精度，实际使用中需要在两者之间找平衡点。
补充一句：构建C-Space耗时高不要紧，因为它是一次性的“预处理”。路径规划是实时在线做的，C-Space只要建好一次，后续跑A*就非常快了。这也是这个方案在工程上可行的重要原因——把最耗时的碰撞检测从在线搜索阶段剥离到了离线预处理阶段。
2.4 如何手工验证C-Space的膨胀效果
画出来的C-Space对不对，最直观的验证方法就是把工作空间障碍物和C-Space障碍物叠在一起看。你会发现C-Space障碍物明显比原障碍物大一圈，而且形状不完全一样，这和直接“把障碍物膨胀一个圆形半径”不同，膨胀形状取决于机器人形状本身。
我做过一个有趣的验证：用一个长方形机器人去绕一个正方形障碍物，得到的C-Space障碍物是圆角方形的，角上明显是圆弧过渡。这就是机器人参考点在绕着障碍物边界走的时候，机身四个角分别扫出来的轨迹叠加效果。这个现象很直观地解释了为什么C-Space不能简单地用“膨胀圆”代替——机器人的形状会影响膨胀的形状。
3. A*算法落地：从C-Space到最优路径
3.1 A*的代价函数与启发式选择
C-Space构建好之后，路径规划就变成了一个二维栅格上的搜索问题。A算法的本质是维护一个优先队列，每次从队列中取出综合代价f(n)最小的节点进行扩展。f(n)由两部分组成：g(n)是从起点到当前节点n的实际代价，h(n)是从当前节点n到目标点的启发式估计代价。A能保证找到最短路径的前提是：h(n)不能高估实际代价，也就是所谓的一致性条件。
栅格地图上常用的启发函数有两个：曼哈顿距离和欧几里得距离。如果允许机器人朝八个方向移动（上下左右加上四个对角线方向），曼哈顿距离会严重低估实际的移动代价，导致搜索效率下降；而欧几里得距离对八邻域移动来说是比较贴合的估计，而且实现也简单。所以我选择用欧几里得距离作为启发函数。实测下来，在100×100量级的地图上，A*的扩展节点数量明显少于Dijkstra，搜索时间可以缩短到原来的三分之一左右。
3.2 邻域扩展规则与移动代价
机器人在地图上移动，我采用的是八邻域扩展，也就是从当前栅格可以走到周围的8个相邻栅格。移动代价这样设定：水平或垂直移动一步，代价为1；对角线移动一步，代价为√2，约等于1.414。这个设置比较符合几何直觉——斜着走比横着走实际距离更长，代价自然更高。
在扩展节点的时候，有两个栅格会被排除：一个是C-Space中被标记为障碍物的栅格，另一个是已经在关闭列表中（已经处理完毕的节点）的栅格。如果新扩展出来的栅格不在开放列表中，就把它加进去；如果它已经在开放列表中，就检查一下通过当前节点到达它的g值是否比之前记录的更小，如果更小就更新其父节点和代价。
A*算法说到底是贪心加回溯的结合——贪心是指每次优先扩展“看起来”最接近目标的节点，回溯是当找到目标点后，通过每个节点保存的父节点指针一步步回溯到起点，就能得到一条完整路径。这条路径是第一篇文章里就强调过的C-Space降维的成果：我们搜索到的是参考点的轨迹，机器人按这个轨迹走，机身不会撞到任何障碍物。
3.3 Matlab实现A*核心代码
我把A*的核心部分写在这里，这段代码可以直接跑通一个栅格地图的搜索。
function path = astar_grid(map, start, goal) % map: 二维矩阵，0表示自由栅格，1表示障碍物 % start, goal: [行, 列] 坐标 [nr, nc] = size(map); % 八个方向的移动向量 dirs = [-1 -1; -1 0; -1 1; 0 -1; 0 1; 1 -1; 1 0; 1 1]; costs = [sqrt(2) 1 sqrt(2) 1 1 sqrt(2) 1 sqrt(2)]; % g(n) 和父节点 g = inf(nr, nc); g(start(1), start(2)) = 0; parent = zeros(nr, nc, 2); % 开放列表用简单的优先队列结构 open = containers.Map('KeyType', 'char', 'ValueType', 'any'); % 将节点编码成 'row,col' 字符串作为key key = sprintf('%d,%d', start(1), start(2)); open(key) = cost_estimate(start, goal); closed = false(nr, nc); while ~isempty(open) % 找到f值最小的节点 keys = open.keys; fvals = cellfun(@(k) open(k), keys); [~, idx] = min(fvals); curKey = keys{idx}; open.remove(curKey); % 解析坐标 parts = sscanf(curKey, '%d,%d'); r = parts(1); c = parts(2); % 到达目标 if r == goal(1) && c == goal(2) path = reconstruct_path(parent, start, goal); return; end closed(r, c) = true; for d = 1:8 nr2 = r + dirs(d, 1); nc2 = c + dirs(d, 2); if nr2 < 1 || nr2 > nr || nc2 < 1 || nc2 > nc continue; end if map(nr2, nc2) == 1 || closed(nr2, nc2) continue; end new_g = g(r, c) + costs(d); newKey = sprintf('%d,%d', nr2, nc2); if new_g < g(nr2, nc2) g(nr2, nc2) = new_g; parent(nr2, nc2, 1) = r; parent(nr2, nc2, 2) = c; open(newKey) = new_g + cost_estimate([nr2 nc2], goal); end end end path = []; end function h = cost_estimate(node, goal) h = sqrt((node(1) - goal(1))^2 + (node(2) - goal(2))^2); end function path = reconstruct_path(parent, start, goal) path = goal; cur = goal; while ~(cur(1) == start(1) && cur(2) == start(2)) pr = parent(cur(1), cur(2), 1); pc = parent(cur(1), cur(2), 2); cur = [pr pc]; path = [cur; path]; %#ok<AGROW> end end上面这段代码有几个注释要说明一下：open列表我用了Matlab的containers.Map来做，方便按key快速查找和删除，优先级比较时遍历了所有key，在节点数量大的时候性能一般，但胜在代码直观，课设完全够用。如果你想让A*跑得更快，可以换成最小堆实现优先队列，但工程复杂度会上去一些。
3.4 路径平滑与后处理
A*搜索出来的路径有一个典型问题：走的是栅格，路径会有很多锯齿。尤其在对角线移动被栅格离散化之后，路径看起来像是折线拼出来的，不够自然。
我的做法是做一个简单的后处理平滑：把路径上连续的几个点拿出来，检查从第i个点直接到第i+2个点的直线段是否不经过障碍物，如果不经过，就删掉中间那个转折点。重复这个动作直到路径不再变短为止。这个操作能让路径上的转折点数量明显减少，同时也让路径更短更平滑。
如果想让机器人走得更顺滑，可以在平滑后的路径点之间做插值，生成圆弧过渡或者贝塞尔曲线插值。但对于课设来说，三点共线删除法就已经能达到很好的展示效果了，代码也就十几行。
function smooth_path = simplify_path(path, map) smooth_path = path; changed = true; while changed changed = false; if size(smooth_path, 1) <= 2 break; end i = 1; while i <= size(smooth_path, 1) - 2 p1 = smooth_path(i, :); p2 = smooth_path(i+2, :); if is_line_clear(p1, p2, map) smooth_path(i+1, :) = []; changed = true; else i = i + 1; end end end end简单提一句is_line_clear这个函数：它需要从p1到p2沿直线采样若干点，检查这些点对应的栅格都不是障碍物。采样间隔越小，检查越保守，但也越耗时。一般取两点间距的1/2作为采样步长就够了。
4. 完整实操流程：一个可复现的Matlab示例
4.1 环境搭建与地图定义
我用一个具体场景来演示整个流程。地图大小取120×120的栅格网络，障碍物设置为三个多边形：一个矩形、一个L形、一个不规则四边形。机器人为一个五边形，参考点取它的几何中心。
在Matlab里定义这些多边形的方式很简单，就是存下每个顶点的x和y坐标。
% 地图尺寸 map_width = 120; map_height = 120; % 障碍物多边形集合 obstacles = { [30 30 50 50; 30 50 50 30] % 矩形 [70 70 95 95 95; 20 35 35 20 10] % L形 [20 45 60 35; 80 95 85 70] % 不规则四边形 }; % 机器人多边形 robot = [0 5 5 2 -2; -2 -2 2 5 2]; % 五边形顶点 robot_center = mean(robot, 2); % 几何中心作为参考点4.2 从工作空间到C-Space：碰撞检测与地图构建
构建C-Space的核心逻辑就是遍历地图上每个可能的参考点位置，检查机器人平移到该位置后是否与任何障碍物碰撞。这里把碰撞检测封装成一个独立的函数，返回值为布尔值，true表示碰撞。
function flag = is_collision(robot_poly, obstacles) flag = false; for k = 1:length(obstacles) obs = obstacles{k}; % 判断机器人是否与当前障碍物碰撞 if is_polygon_intersect(robot_poly, obs) flag = true; return; end end end function result = is_polygon_intersect(poly1, poly2) % 用多边形顶点边界相交检测 % 简化起见，这里以线段相交检测为主，再补充包含判定 v1 = poly1; v2 = poly2; % 依次检查poly1的每条边与poly2的每条边 for i = 1:size(v1, 2) p1 = v1(:, i); q1 = v1(:, mod(i, size(v1, 2)) + 1); for j = 1:size(v2, 2) p2 = v2(:, j); q2 = v2(:, mod(j, size(v2, 2)) + 1); if segments_intersect(p1, q1, p2, q2) result = true; return; end end end % 检查是否包含（poly1的一个点是否在poly2内部） if inpolygon(v1(1, 1), v1(2, 1), v2(1, :), v2(2, :)) result = true; return; end result = false; end段与段的相交检测segments_intersect，用叉积方向判断就好，这段代码虽然朴素但足够可靠。
构建C-Space时就两层循环遍历整个地图栅格：
grid_res = 1; % 栅格分辨率 cspace_map = zeros(map_height, map_width); half_width = max(robot(1,:)) - min(robot(1,:)); half_height = max(robot(2,:)) - min(robot(2,:)); for ix = 1:map_width for iy = 1:map_height % 将机器人中心平移到 (ix, iy) robot_offset = robot - robot_center; robot_trans = robot_offset + [ix; iy]; if is_collision(robot_trans, obstacles) cspace_map(iy, ix) = 1; end end end运行完这段代码后，用imagesc(cspace_map)可视化，你会看到原始障碍物周围有一圈或多或少的膨胀区域，膨胀程度取决于机器人形状在对应方向上的最大延伸。
4.3 起点终点设置与A*搜索
在C-Space地图上设置起点和终点时，注意要选择自由栅格，不能被cspace_map标记为1。比如我取起点在(10, 10)，终点在(105, 100)，然后调用之前的astar_grid函数。
start = [10 10]; goal = [105 100]; path = astar_grid(cspace_map, start, goal);如果A*返回的path不为空，就可以把它折算回工作空间的真实坐标路径。由于C-Space的栅格坐标就是工作空间的坐标（参考点位置），所以不需要额外换算。路径点存的是参考点在每个栅格上的位置。
4.4 结果验证：输出路径质量指标
把路径画在地图上之后，我通常会输出几个指标来判断算法效果：路径总长度（把所有相邻路径点的欧几里得距离累加）、路径转折次数（路径方向变化超过45度的位置数量）、A*搜索耗时（用tic/toc测量）。
在某一次实际运行中，120×120地图、三个障碍物的场景下，A*在C-Space构建完成后，搜索耗时只有不到0.5秒，路径长度大约是130多个像素单位，转折点经过平滑后从28个减少到了7个。这个表现对于课设展示来说已经相当不错了。
4.5 参数调优建议与效果对比
从我的实验来看，有几个参数对结果影响比较明显。
- 栅格分辨率：分辨率越高，C-Space越精确，但构建时间越长。我建议先用分辨率=2快速验证整个流程，确认没问题后再调到1做精细结果。
- A*启发函数：欧几里得距离比曼哈顿距离表现更好，尤其在允许八邻域移动时。
- 路径平滑迭代次数：平滑虽然提高美观度，但迭代太多有可能把路径拉得太靠近障碍物边界，留下安全隐患。所以平滑之后最好加一个碰撞校验。
另外，如果你想让机器人走得更自然，可以在平滑后的路径上做插值细分，生成轨迹曲线。这里就不展开讲了，感兴趣可以自己尝试。
5. 常见问题与排查技巧实录
5.1 A*明明在地图上，却找不到路径
这种情况大部分是因为C-Space的膨胀把可行通道完全“堵死”了。可能是机器人形状太大，也可能是栅格分辨率太低，把一个还能勉强通过的细缝离散化成了连续障碍物。排查方法很直接：用imagesc把C-Space可视化，肉眼判断起点到终点之间是否还有连通的自由区域。如果显示确实被堵死了，要么换一条更宽的通道走，要么降低分辨率重试，要么缩小机器人的物理尺寸。
还有一种隐蔽情况：起点或者终点本身就不小心落在了C-Space障碍物栅格里。这个用cspace_map(start(1), start(2)) == 0检查一下就能确定。
5.2 路径贴着障碍物边缘，看着就很危险
这是路径平滑过于“贪婪”的常见后果。平滑算法会尝试删除中间节点，让路径更短，但如果不加校验，就可能让连线无限靠近障碍物。解决方法是：在平滑的is_line_clear检查时，额外加一个安全距离约束，也就是要求采样点不仅要不在障碍物栅格里，到障碍物栅格的距离还要大于某个阈值（比如2个栅格单位）。代价是路径会稍微长一点，但安全性大幅提升，看起来也更可信。
5.3 C-Space构建太慢，跑一次要等好久
这个问题的本质是碰撞检测函数太慢，或者地图太大。可以从两个方向优化。一是向量化：把机器人多边形的顶点坐标预先算好，不要在每个栅格里重复计算robot_offset；二是剪枝：先判断参考点距离障碍物包围盒中心是否超过某个半径，如果明显太远，直接跳过精细碰撞检测。
我试过在实际代码里把这两步优化做完之后，120×120地图的C-Space构建时间从90多秒降到了20秒以内。如果还不够快，就降低分辨率。
5.4 路径锯齿太严重，转折点太多
A*在栅格上本来就会走出阶梯状路径。我的经验是两层处理：第一层是八邻域搜索本身比四邻域搜索的锯齿少；第二层就是前面说的三点平滑法。如果平滑完之后依然很多折线，那大概率是地图里有大量零散的小障碍物，可以考虑把地图做一次闭运算处理，把细碎的噪声点抹掉再搜索。
5.5 多边形碰撞检测时灵时不灵
排除代码bug后，最可疑的是“包含”判定没有做完整。如果两个多边形完全没有边相交，但一个完全包含在另一个里面，也会发生碰撞。我在实际测试中就遇到过机器人完全落在障碍物内部的极端情况——比如起点附近机器人初始位置放在一个凹形障碍物的凹陷里，这时边相交检测全部返回false，但机器人实际上已经侵入了障碍物范围。所以碰撞检测里inpolygon这一项不能省。
6. 这个课题还能怎么玩：扩展方向与个人体会
把C-Space构建和A搜索这一套跑通之后，其实已经掌握了一个很通用的路径规划框架。后面想扩展可以做这几件事：一是加入机器人旋转自由度，把C-Space从二维变成三维，每一层固定一个离散角度，搜索时增加一个“原地旋转”的动作；二是把静态地图换成动态障碍物场景，每次检测到障碍物移动后局部更新C-Space再重规划；三是在同样框架下把A替换成JPS（Jump Point Search）或者D* Lite，对比不同算法的搜索效率。
个人最大的体会是：C-Space的美妙在于它把“机器人有体积”这个看起来很麻烦的约束，变成了一个离线预处理步骤。一旦你学会了这个问题分解方式，很多看似复杂的机器人运动规划问题，都能拆成“构型空间建模 + 图上搜索”两个独立模块来处理。而A*作为图上搜索的基石，理解它的启发式设计和代价更新逻辑之后，再去看其他搜索算法都会觉得轻松很多。
回到最开始那位同学的问题——C-Space和A到底怎么串起来？其实答案已经浮出水面：C-Space把带形状的机器人变成一个点，A负责让这个点从起点走到终点，两者结合，就是一个完整、漂亮、可复现的多边形机器人避障路径规划方案。你把这套实现跑通，后续加再多花样也都是在这两个模块上做文章。
