从C-Space到A*：多边形机器人避障路径规划的完整实现链路

做机器人避障路径规划的人，十有八九是从A算法入的门。但很多人最初在栅格地图上把A跑得飞起，用的都是点模型——把机器人当成一个没有体积的像素，地图上能走就是能走。等真要往多边形机器人上迁移，问题就全冒出来了：地图上看起来有条缝能钻过去，实际机器人宽度根本过不去；规划出来的路径贴着障碍物拐角走，真机一开就蹭上。我自己刚接触多边形机器人避障路径规划那阵子，第一个版本就是在二维栅格上直接跑A*，仿真里撞了五六次才咂摸出问题不在A本身，而是地图从一开始就建错了。这篇文章就把"从C-Space到A"这条完整的多边形机器人避障路径规划链路拆开讲清楚：为什么必须引入C-Space、C-Space怎么构建、A在栅格图上实现有哪些细节，最后给出一套可以直接抄的Matlab实现框架。适合刚入门路径规划、以及已经在点模型上跑通A但想扩展到真实机器人尺寸的人，看完至少能少走两三条弯路。

1. 从点模型到多边形：为什么必须先换成C-Space

1.1 点模型A*直接套用的典型翻车场景

先说一个非常典型的翻车过程。假设地图上有两个障碍物，中间留了一条0.5米宽的通道。你在栅格地图上用点模型跑A*，这条通道里的栅格是自由的，算法理所当然会从通道中间穿过去。但如果你的机器人是一个长0.8米、宽0.6米的矩形，物理上它就是过不去。你拿Matlab画出来的路径明明非常干净，仿真里机器人一到通道口就撞墙，原因只有一个：你规划的路径根本没有考虑机器人的体积占位。

这种问题不是靠把A*的代码改一改就能解决的。路径规划的输出是一系列机器人的构型点，而构型点本身有明确的物理含义——对平面移动机器人来说，一个构型就是机器人参考点的位置加上它的朝向。只要这个构型下机器人的多边形与障碍物有交集，这个构型就是非法的。关键在于：点模型把"机器人占据多少空间"这件事完全丢掉了，所以在点模型的地图上，任何碰壁都是地图表达方式的问题，而不是搜索算法的问题。

这个认知很重要。我见过不少人拿着点模型的A*代码，到处调步长、调启发函数，折腾很久都没想明白"路径为什么撞墙"，其实就是没搞懂机器人的几何体积需要纳入地图建模。一旦把体积纳入，接下来的问题就变成：怎么把有体积的机器人，变回地图上一个点？答案就是C-Space。

1.2 C-Space的本质：把机器人体积"转嫁"给障碍物

C-Space，构型空间，说的是一件事：把每一个可能的机器人构型映射成空间中的一个点，然后根据这个构型下机器人是否与障碍物碰撞，把空间划分为自由区域和障碍区域。机器人在某个构型下能安全待着，这个构型就是自由空间里的一个点；会碰撞，就是障碍空间的点。

对只能平移、不能旋转的平面多边形机器人来说，构型就是(x, y)两个自由度，C-Space仍然是一个二维平面。这时有一个非常漂亮的性质：某个位置(x, y)下机器人会撞上障碍物，等价于参考点(x, y)落在"障碍物多边形"与"反向机器人多边形"的闵可夫斯基和之内。换句话说，C-Space障碍物 = 原障碍物 ⊕ 反向机器人多边形。你可以这么理解：把障碍物想象成一块石头，把机器人想象成一块橡皮，沿着石头边界滚动橡皮，橡皮参考点扫过的区域就是C-Space障碍物。

为什么说这个转换是路径规划的"第一道坎"？因为转换之后，多边形机器人又变成了一个点。你在点模型上写的所有A代码、Dijkstra代码、可视化工具，一行都不用改，只需要把输入地图从工作空间地图换成C-Space地图。我当初把C-Space构建函数写完，第一反应就是：原来A代码可以原封不动，跑在C-Space上的A和跑在普通栅格上的A根本没区别。搜索算法负责"在正确的空间里找路"，而C-Space负责"把空间的语义搞对"。

1.3 参考点怎么选，直接影响C-Space形态

构建C-Space的第一步，是确定机器人的参考点。参考点就是C-Space里那个"点"到底对应机器人的哪个部位，一般取机器人的几何中心或者质心。MATLAB里处理起来很直接：把机器人多边形顶点定义成以参考点为原点的局部坐标，比如一个长0.8米、宽0.6米的矩形机器人：

robotPoly = [-0.4, -0.3; 0.4, -0.3; 0.4, 0.3; -0.4, 0.3];

要把机器人放到工作空间某个位置(wx, wy)，就是把局部坐标整体平移：movedRobot = robotPoly + [wx, wy]。参考点的选择不同，C-Space障碍物的形状会整体平移，但自由空间的拓扑结构不变——路径能不能走通、要走多长，本质上不受参考点位置影响，只要保持一致性就行。不过实际使用中还是建议把参考点放在机器人几何中心，因为这样C-Space障碍物相对原障碍物是对称扩展的，视觉上最直观，调试的时候不容易把自己绕晕。

有一点必须强调：上面说的C-Space只在"机器人不能旋转"的假设下成立。如果机器人可以旋转，构型就变成(x, y, θ)三个自由度，C-Space是三维的，每个θ切片下都有一个不一样的障碍物膨胀形状。工程上偷懒的做法是直接用机器人的外接圆半径做膨胀，够保守但浪费很多可通行空间；严谨做法是建多角度切片。本篇先聚焦平移机器人的二维情形，这也是大多数入门项目采用的设定。

2. C-Space构建的两种路线：逐栅格采样与Minkowski和展开

2.1 基于栅格采样的碰撞检测：直观但要注意分辨率

C-Space最常见的构建方式，就是逐栅格采样碰撞检测。思路非常朴素：把机器人参考点放到每个栅格中心，用机器人多边形和平移后的障碍物多边形做相交检测，碰撞了就把这个栅格标为障碍。

核心碰撞检测函数用分离轴定理（SAT）实现。两个凸多边形如果不相交，必存在一条分离轴，使得两个多边形在这条轴上的投影区间不重叠；而分离轴一定垂直于某一条边。对矩形、三角形这类凸多边形，检测起来很快。MATLAB代码如下：

function flag = polygonsIntersect(p1, p2) % 凸多边形相交检测，基于分离轴定理 for i = 1:size(p1, 1) next = mod(i, size(p1, 1)) + 1; edge = p1(i, :) - p1(next, :); if hasSeparatingAxis(edge, p1, p2) flag = false; return; end end for i = 1:size(p2, 1) next = mod(i, size(p2, 1)) + 1; edge = p2(i, :) - p2(next, :); if hasSeparatingAxis(edge, p1, p2) flag = false; return; end end flag = true; end function sep = hasSeparatingAxis(edge, p1, p2) axis = [-edge(2), edge(1)]; axis = axis / norm(axis); dots1 = p1 * axis'; dots2 = p2 * axis'; sep = max(dots1) < min(dots2) || max(dots2) < min(dots1); end

有了相交检测，逐栅格构建C-Space就很容易：

function grid = buildCSpaceBySampling(rows, cols, cellSize, robotPoly, obstacles) grid = false(rows, cols); for r = 1:rows for c = 1:cols wx = (c - 0.5) * cellSize; wy = (r - 0.5) * cellSize; movedRobot = robotPoly + [wx, wy]; if isColliding(movedRobot, obstacles) grid(r, c) = true; end end end end function flag = isColliding(movedRobot, obstacles) flag = false; for k = 1:numel(obstacles) if polygonsIntersect(movedRobot, obstacles{k}) flag = true; return; end end end

这套方法的优点是通用：障碍物是非凸的也能处理，因为相交检测本身不要求障碍物是凸多边形（非凸可以拆成凸多边形的并集，或者直接用更通用的多边形相交算法）。缺点是慢：地图分辨率越高，循环次数越大。一个100×100的栅格就是一万次碰撞检测，每帧仿真跑一遍会拖速度。优化手段是先用轴对齐包围盒（AABB）做快速剔除，机器人AABB和障碍物AABB不相交就直接跳过，能砍掉一大半计算量。

2.2 凸多边形障碍的Minkowski和快速展开

如果障碍物是凸多边形，还有一条更优雅的路线：直接算C-Space障碍物的多边形边界，也就是闵可夫斯基和。两个凸多边形的闵可夫斯基和有一个简洁结论：结果多边形的顶点，一定来自"障碍物顶点 + 反向机器人顶点"的所有配对之和的凸包。MATLAB里convhull函数可以一行搞定凸包计算。

function cObs = minkowskiSumExpand(obsPoly, robotPoly) negRobot = -robotPoly; pts = []; for i = 1:size(obsPoly, 1) for j = 1:size(negRobot, 1) pts(end+1, :) = obsPoly(i, :) + negRobot(j, :); %#ok<AGROW> end end K = convhull(pts(:,1), pts(:,2)); cObs = pts(K, :); end

注意这里用的是-robotPoly，因为C-Space障碍物是"障碍物 ⊕ 反向机器人"，反向机器人就是把机器人多边形关于参考点取反。对于矩形机器人、矩形障碍物，这种展开的几何意义非常直观：矩形的四个角会被"磨圆"，每个顶点向外扩展出一个小圆角，C-Space障碍物变成一个圆角多边形。

拿到C-Space障碍物的多边形之后，需要把它栅格化到地图上才能喂给A*。栅格化有很多种做法，最常见的是遍历每个栅格，用inpolygon检查栅格中心是否在多边形内部。这里有个需要拿捏的点：如果只把"中心落在多边形内"的栅格标为障碍，边界上的半覆盖栅格会被漏掉，路径可能从特别贴边的位置穿过；反过来把所有"和C-Space障碍物有交集"的栅格都标为障碍，则会让路径略微偏离理论最短路径。实际项目里我习惯取前者，并额外预留一个安全余量，后面会说到。

2.3 两种路线怎么选

两种C-Space构建方式各有适用场景，我整理了对比，做项目时可以直接参考：

如果地图里全是凸多边形障碍物，推荐Minkowski和展开，因为C-Space边界精确，A*跑出来的路径更接近理论解，而且计算量小得多，适合需要频繁重规划的场景。如果地图是从视觉或者激光雷达生成的栅格地图，障碍物根本没有多边形表示，那就只能走逐栅格采样的路子了。两种方法我不觉得是替代关系，更像是一个工具箱里的两把扳手：建模阶段多想想手头地图长什么样，再决定用哪把。

3. A*算法在C-Space栅格图上的关键实现细节

3.1 为什么选A*而不是别的搜索算法

C-Space构建好了，搜索算法有很多选择：Dijkstra、贪心最佳优先、A*，还有RRT、RRT这一族采样算法。在栅格图上，A是性价比最高的默认选择。Dijkstra无差别地向四周均匀扩散，完全没用上目标点的方向信息，很多无用区域都会被探索；贪心最佳优先只盯着启发函数走，速度快但容易被局部障碍骗进死胡同，路径不保证最优。A*把已花费代价g和预估代价h加在一起，既保证找到最优路径（在启发函数可采纳的前提下），又比Dijkstra省掉大量无用搜索。

换句话说，A是"全局最优"和"搜索效率"之间最舒服的折中。对栅格离散空间这种低维结构化搜索问题，A就是主力。采样类的RRT*更适合高维连续空间，那是另一个方向，等到了机器人带旋转自由度、或者环境维度更高的时候再考虑。

3.2 八邻域扩展、对角线代价与穿角约束

A*在栅格图上扩展邻域，有4邻域和8邻域两种主流选择。4邻域只允许上下左右移动，路径会显得僵硬，常见锯齿状折线；8邻域增加了对角方向，路径更自然，也更接近欧氏最短路径。但8邻域有一个核心问题：对角移动的代价要对。四方向移动每步代价是1，对角移动距离是√2，所以步长必须是√2，否则算法会偏爱斜向移动，路径失真。我在代码里直接用sqrt(dr^2 + dc^2)算步长，干净利落。

8邻域还有个隐蔽的坑：穿角。机器人从格子A移动到对角格子B，即使A和B都是自由栅格，这条路径可能沿着公共角"蹭"过两个相邻障碍栅格的交点。对于点模型，这个角可能勉强算可通行；对于多边形机器人，C-Space已经膨胀过一轮，路径贴近C-Space障碍物边界穿角，风险更高。所以正规的8邻域A*必须加穿角检查：对角线移动时，两个正交相邻的边侧栅格必须至少有一个是自由的。代码如下：

if abs(dr) == 1 && abs(dc) == 1 % 穿角禁止：从当前格走对角线时，两个边侧格不能都是障碍 if grid(cur(1)+dr, cur(2)) && grid(cur(1), cur(2)+dc) continue; end end

这个坑我在第一个版本里就踩过。不加穿角检查时，路径偶尔会沿着障碍物45度角的位置硬切过去，看着很怪，仿真一放大就是一次碰撞。加完这个检查，路径立刻变得符合直觉。

3.3 启发函数：选错会直接失去最优性

启发函数是A效率与最优性的命根子。栅格图上有三个常见选择：曼哈顿距离、欧氏距离、八格距离。很多人随手就用曼哈顿距离，在8邻域下这是最大的坑——曼哈顿距离在8邻域栅格上会高估真实代价。举个极端例子：从(0,0)到(1,1)，曼哈顿距离是2，实际对角一步只要√2≈1.414。启发函数一旦高估，A就不再保证能找到最优路径。这不是效率问题，是正确性问题。

欧氏距离永远是安全的，因为在任意度量下，两点间的欧氏距离都不会超过任何实际可行路径的长度。但它对8邻域栅格不是最精确的——实地可走的是斜向网格，欧氏距离会低估一些。最贴合的启发函数是八格距离（Octile Distance），它把"先走对角、再走直线"的真实最短代价直接算出来：

function d = octileDist(a, b) dx = abs(a(2) - b(2)); dy = abs(a(1) - b(1)); s = min(dx, dy); l = max(dx, dy); d = (l - s) + s * sqrt(2); end

我做过一组对比实验，同样一张100×100的C-Space栅格图，起点终点相同，三种启发函数的搜索结果如下：

扩展节点数直接对应搜索耗时，八格距离让A在最贴合真实代价的前提下保持高效率。我的建议很直接：8邻域栅格A，启发函数无脑用八格距离，不要犹豫。

下面是完整的A*核心实现，包含open list、closed set、父节点回溯：

function path = astarGrid(grid, startIdx, goalIdx) [rows, cols] = size(grid); neighbors8 = [-1,-1; -1,0; -1,1; 0,-1; 0,1; 1,-1; 1,0; 1,1]; gScore = inf(rows, cols); fScore = inf(rows, cols); parent = cell(rows, cols); gScore(startIdx(1), startIdx(2)) = 0; fScore(startIdx(1), startIdx(2)) = octileDist(startIdx, goalIdx); openList = [startIdx, fScore(startIdx(1), startIdx(2))]; closed = false(rows, cols); while ~isempty(openList) [~, k] = min(openList(:,3)); cur = openList(k, 1:2); openList(k, :) = []; if isequal(cur, goalIdx) path = backtrackPath(parent, startIdx, goalIdx); return; end closed(cur(1), cur(2)) = true; for i = 1:size(neighbors8, 1) dr = neighbors8(i,1); dc = neighbors8(i,2); nr = cur(1) + dr; nc = cur(2) + dc; if nr < 1 || nr > rows || nc < 1 || nc > cols continue; end if grid(nr, nc) || closed(nr, nc) continue; end if abs(dr) == 1 && abs(dc) == 1 if grid(cur(1)+dr, cur(2)) && grid(cur(1), cur(2)+dc) continue; end end stepCost = sqrt(dr^2 + dc^2); newG = gScore(cur(1), cur(2)) + stepCost; if newG < gScore(nr, nc) gScore(nr, nc) = newG; f = newG + octileDist([nr, nc], goalIdx); fScore(nr, nc) = f; parent{nr, nc} = cur; openList = [openList; nr, nc, f]; %#ok<AGROW> end end end path = []; end function path = backtrackPath(parent, startIdx, goalIdx) path = goalIdx; while ~isequal(path(1,:), startIdx) p = parent{path(1,1), path(1,2)}; if isempty(p) break; end path = [p; path]; %#ok<AGROW> end end

这个实现里我用了简单的open list数组加min扫描。数据量小的时候完全够用，地图大了之后建议换成二叉堆或者MATLAB自带的PriorityQueue结构，否则open list变长之后min(openList(:,3))的开销会拖慢整体。另外，当某个节点找到更小的g值时，我把新节点直接追加到open list，旧的劣质条目留在原地等被跳过——这是工程上很常见的简化处理，配合closed set不会影响正确性，只多消耗一点内存。

4. Matlab实现：环境建模、C-Space生成与仿真验证

4.1 环境与机器人建模

仿真环境我用的是10米×10米的矩形场地，栅格分辨率0.1米，这样网格是100×100。障碍物用多边形顶点数组表示，两个凸多边形障碍各占一块区域：

cellSize = 0.1; worldWidth = 10; worldHeight = 10; rows = round(worldHeight / cellSize); cols = round(worldWidth / cellSize); obstacles = { [3, 2; 5, 2; 5, 4; 3, 4], % 矩形障碍 [7, 5; 9, 5; 9, 8; 7, 6] % 不规则四边形 }; robotPoly = [-0.4, -0.3; 0.4, -0.3; 0.4, 0.3; -0.4, 0.3];

这里有个坐标约定的细节：栅格矩阵的行对应世界坐标的y轴，列对应x轴，所以世界坐标(wx, wy)到栅格索引的转换是：

startIdx = [ceil(0.5 / cellSize), ceil(0.5 / cellSize)]; goalIdx = [ceil(8.0 / cellSize), ceil(9.5 / cellSize)];

startIdx(1)是row也就是y方向，startIdx(2)是col也就是x方向。这个约定在绘制和可视化时要保持一致，否则你明明画的是第20行，世界坐标里却跑到y=2米去了，调试起来很痛苦。我最初就是在这里栽了跟头，路径画出来后起点和地图对不上，找了半天才发现是行列和xy的映射弄反了。

4.2 生成C-Space并使用A*搜索

构建C-Space时，我采用Minkowski和展开的方式，因为这两个障碍物都是凸的，很快：

cspaceGrid = false(rows, cols); % 对所有障碍物求C-Space多边形 cObsList = cell(1, numel(obstacles)); for k = 1:numel(obstacles) cObs = minkowskiSumExpand(obstacles{k}, robotPoly); cObsList{k} = cObs; % 栅格化：栅格中心落在C-obstacle内则标记为障碍 for r = 1:rows for c = 1:cols wx = (c - 0.5) * cellSize; wy = (r - 0.5) * cellSize; if inpolygon(wx, wy, cObs(:,1), cObs(:,2)) cspaceGrid(r, c) = true; end end end end % 调用A* path = astarGrid(cspaceGrid, startIdx, goalIdx);

inpolygon是MATLAB自带函数，判断点是否在多边形内部，省事。如果不想用这个函数，也可以自己写射线法，但没必要重复造轮子。注意inpolygon在点落在边界上时默认返回true，对于C-Space边界栅格，这恰好是我们想要的结果——边界栅格应当视为不可通行，宁可保守一档。

4.3 可视化与结果判读

路径跑完之后，一定要把图完整画出来看，不要只看路径数组。我的习惯是一张图上叠三层信息：原始工作空间障碍物、C-Space障碍物边界、规划出的路径。这样C-Space膨胀了多少、路径距离障碍物多远，一眼就能看出来。

figure('Color', 'w'); hold on; axis equal; % 原始障碍物 for k = 1:numel(obstacles) patch(obstacles{k}(:,1), obstacles{k}(:,2), 'k', 'FaceAlpha', 0.6); end % C-Space边界 for k = 1:numel(cObsList) plot(cObsList{k}(:,1), cObsList{k}(:,2), 'r--', 'LineWidth', 1.2); end % 路径 pathWorld = [(path(:,2) - 0.5) * cellSize, (path(:,1) - 0.5) * cellSize]; plot(pathWorld(:,1), pathWorld(:,2), 'b-', 'LineWidth', 2.5); plot(startIdx(2)*cellSize - 0.5*cellSize, startIdx(1)*cellSize - 0.5*cellSize, ... 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); plot(goalIdx(2)*cellSize - 0.5*cellSize, goalIdx(1)*cellSize - 0.5*cellSize, ... 'rx', 'MarkerSize', 12, 'LineWidth', 2); xlabel('X (m)'); ylabel('Y (m)'); grid on;

如果也想看C-Space栅格本身长什么样，用imagesc加上两色colormap就可以：

figure('Color', 'w'); imagesc(cspaceGrid); colormap([1 1 1; 0 0 0]); % 0=自由→白色, 1=障碍→黑色 axis xy; axis equal; title('C-Space栅格图（黑色为膨胀后的障碍区域）');

路线顺利的话，会看到蓝色路径从起点出发，绕过黑色C-Space障碍区域，抵达终点，而且和红色虚线保持一小段肉眼可见的距离。如果路径紧贴红色虚线甚至贴到黑色障碍区，说明分辨率不够或者缺少安全余量，回第5节看解决方案。

5. 实测踩坑记录：贴墙路径、穿角穿缝与分辨率敏感

5.1 路径为什么总是贴着障碍物走，以及安全余量的必要性

第一次跑通整套流程后，我盯着仿真画面看了好久：路径倒是畅通无阻，但总是紧紧贴着C-Space障碍物的边界走。这意味着实际机器人运动时，一旦有里程计误差、惯性漂移、控制器跟踪偏差，基本不可避免地蹭到障碍物。原因在于:栅格化的C-Space把"恰好不碰撞"和"留有余量"当成了同一件事。栅格中心落在C-Space多边形外但在边界附近时，这个栅格被标记为自由，A*就把这当成可通行区域，当然会钻。

解决思路有两种。第一种是给障碍物多边形手动做一次外扩，也就是在构建C-Space之前，先把每个障碍物多边形向外偏移一个安全距离（比如0.2米），再用膨胀后的多边形去求C-Space。这个偏移可以用polybuffer函数实现，简单粗暴但有效，机器人最后的路径天然距离真实障碍物有一段距离。第二种是在C-Space栅格图上做个形态学膨胀，把临近C-Space障碍物边界的自由栅格也标成障碍。两者的本质一样，都是人为添加安全余量。我的建议是优先做多边形偏移，因为安全距离直接对应物理空间的米数，语义清晰，方便后续给不同场景配置不同数值。

5.2 对角线穿角和穿过缝隙的隐蔽问题

前面在代码里加了穿角检查，为什么实际问题还是会出现？因为栅格空间里"相邻两个格中心之间连线无碰撞"和"真实几何下机器人沿该线段扫过无碰撞"不是严格等价。C-Space栅格化时用了中心点判定，两个对角自由栅格之间的路径虽然避开了栅格标记的障碍，但扫地机器人真实的扫掠面积可能擦到C-Space边界。

这个问题的根治方法，是对规划出的路径做一次连续的扫掠碰撞检查：把路径上相邻两个航点连成线段，把机器人多边形沿这条线段做连续扫掠，看扫掠体是否与所有原始障碍物相交。扫掠碰撞的完整实现比较重，工程上常用折中方案——在线段上密集采样几个中间位姿，逐个做离散碰撞检测。采样间距取栅格尺寸的1/2到1/3，基本能覆盖绝大部分漏检情况。我把这个步骤放在A*之后作为"路径后处理"执行，实测能把路径的可靠性从"仿真看着行"提升到"真机敢开着走"。

5.3 路径简化平滑：少走拐点，真机才好跟踪

A*吐出的路径是逐格连接的折线，一个明显的缺陷是拐点太多。哪怕是一条直线段，格点连接也可能呈现锯齿状，机器人跟踪起来需要频繁转向，十分难受。最常用的后处理是视线法简化：从当前航点出发，沿着原路径向后看，找到最远一个"与原路径之间无障碍"的格点，直接把中间节点全部删掉，拉成一条长线段。判断两点之间有没有障碍，用Bresenham画线算法遍历路径上的栅格：

function lineCells = bresenhamLine(a, b) % a, b 为 [row, col]，返回连线经过的所有栅格 x0 = a(2); y0 = a(1); x1 = b(2); y1 = b(1); dx = abs(x1 - x0); dy = -abs(y1 - y0); sx = sign(x1 - x0); sy = sign(y1 - y0); err = dx + dy; lineCells = []; while true lineCells(end+1, :) = [y0, x0]; %#ok<AGROW> if x0 == x1 && y0 == y1, break; end e2 = 2 * err; if e2 >= dy, err = err + dy; x0 = x0 + sx; end if e2 <= dx, err = err + dx; y0 = y0 + sy; end end end function valid = hasLineOfSight(a, b, grid) cells = bresenhamLine(a, b); valid = all(~grid(sub2ind(size(grid), cells(:,1), cells(:,2)))); end

简化之后路径的折线数量大幅减少，直线段占比明显提升。要注意简化只对C-Space栅格做了视线检查，真实机器人的安全性仍然依赖5.2节的扫掠碰撞验证。我自己用这两步组合：先视线简化，再对简化后的每条线段做离散采样碰撞检测，两步都通过才认为路径可以交付给底层控制器。

最后的个人体会：这套"从C-Space到A*"的框架，真正的难点从来不在A*本身——那些教科书上的伪代码看一遍就能写出来。难点全在C-Space的理解和构建是否扎实。我后来再看任何栅格路径规划项目，都会先问一句"你的地图是工作空间还是C-Space？安全余量是多少？"这两个问题回答不清楚，后面算法调得再花哨也白搭。如果这篇分享能帮你把C-Space这条"第一道坎"迈过去，后面的路就顺多了。