4.1 规划问题的形式化：构型空间

4.1 规划问题的形式化：构型空间

本节摘要：规划的第一步不是挑算法，而是把"找路"翻译成数学对象：状态空间、起点终点、约束、代价。构型空间（C-space）是标准答案——把有形状的机器人缩成一个点，把膨胀的障碍留给环境。本节讲清这套翻译与它附带的账单。

定义先行：规划问题的四要素

一个路径规划问题由四件东西定义。状态空间：机器人所有可能状态的集合（移动机器人取平面位姿，机械臂取各关节角的组合）；起终状态：出发与抵达；约束：哪些状态不可行（碰撞、超出关节限位）与运动学限制（转弯半径、速度上限）；代价函数：什么算"好路径"——最短、最快、最省电，或加权组合。算法章节光鲜，这四要素才是决定成败的地基：代价函数写错，算法越好跑得越偏。

构型空间：把机器人缩成一个点

在物理空间里做碰撞检查很痛苦——机器人是有形状的，同一个"位置"配不同朝向，占据的物理区域完全不同。构型空间（Configuration Space）的思路釜底抽薪：把机器人全部自由度打包成一个点 q（平面机器人取 3 维，六轴臂取 6 维），在 C-space 里机器人没有形状；代价是障碍不再是原样，而要在 C-space 里"膨胀"出来——机器人绕行障碍物一半车宽的轨迹，等价于点机器人在膨胀障碍外绕行。

这套翻译还顺带解释了两个著名现象。其一，窄门问题：车过两墙之间的窄通道，在物理空间看"车与墙的间隙"很直观，在 C-space 里窄门比物理通道窄得多——车长车宽全部折进了障碍膨胀，缝隙稍小就是彻底无解。其二，维度灾难：平面机器人 C-space 是 3 维，六轴臂是 6 维，双臂系统 12 维——栅格化后格子数随维度指数爆炸，这正是 4.2 节网格搜索与 4.4 节采样规划的分水岭。

图：物理空间里的机器人变成 C-space 里的一个点

障碍膨胀的账单：安全裕度从哪来

工程上很少做严格的 Minkowski 和，通用做法是把栅格图按安全半径 r_inf 膨胀：所有占据格的 r_inf 邻域都标记为不可通行。r_inf 的取值是一场三方谈判：

• 定位误差：第 3 章的位姿协方差，95% 置信半径，比如 3 厘米；
• 控制误差：轨迹跟踪残差，与速度相关，比如 5 厘米；
• 传感器残影：局部栅格更新延迟期间机器人已移动的距离，速度 0.5 m/s × 0.2 s = 10 厘米。

三者平方和开根约 12 厘米，再乘 1.5 倍安全系数，r_inf ≈ 18 厘米。看清楚这个账单的结构——膨胀半径不是保守拍出来的，是上游误差决定的。感知改善，r_inf 才有资格收缩，机器人才能在窄门里通过；反过来，一个感知粗糙的系统配一个激进的 r_inf，撞墙只是时间问题。规划与感知就这样被一个数字拴在了一起。

代价函数：同一张地图，不同的路

代价函数决定"好"的定义，最常见三种口径在同一仓库里会给出不同的路：

import numpy as np

def path_cost(grid_path, grid, w_len=1.0, w_near=0.0, w_head=0.0):
    """三种口径的代价：
    w_len  路径长度（米）
    w_near 贴近障碍惩罚：离膨胀边界越近代价越高
    w_head 转向惩罚：抑制抖动与频繁换向"""
    cost = 0.0
    for i in range(1, len(grid_path)):
        a, b = grid_path[i-1], grid_path[i]
        seg = np.hypot(b[0]-a[0], b[1]-a[1]) * grid.res
        cost += w_len * seg
        if w_near:
            d = grid.dist_to_obstacle(b)          # 距最近障碍的距离场
            if d < grid.r_inf * 2:
                cost += w_near * (grid.r_inf * 2 - d) * seg
        if w_head and i > 1:
            v1 = np.array(b) - np.array(a)
            v0 = np.array(a) - np.array(grid_path[i-2])
            cosang = v1 @ v0 / (np.linalg.norm(v1) * np.linalg.norm(v0) + 1e-9)
            cost += w_head * (1.0 - cosang)
    return cost
​

最短路径贴着障碍钻缝；加 w_near 后路径主动离障碍远一点，长 5% 却安全得多；差速底盘再加 w_head，路径从"锯齿折线"变成"大弧大直"。没有绝对正确的口径——货架拣选 AMR 求快用 w_len 主导，医院配送机器人求稳必须 w_near 主导。先答"什么算好"，再选算法。

本节要点

• 规划四要素：状态空间、起终点、约束、代价函数；代价函数错了，算法越好跑得越偏。
• C-space 把机器人缩成点、障碍按自由度膨胀，碰撞检查随之简化，窄门与维度灾难都可由此解释。
• 膨胀半径由定位误差、控制误差、传感器延迟三方合成，是感知与规划之间的合同数字。
• 代价口径（长度/贴近度/转向）按业务选择，同一张地图因此长出不同的路。

差速底盘的约束怎么进规划

圆形机器人的 C-space 是三维（x、y、朝向），但差速底盘不能横着走——瞬时速度方向必须沿车身。这条非完整约束让"C-space 里的几何路径"不等于"可行驶路径"：几何上 90 度急转是允许的，差速底盘却需要原地旋转再直行。工程处理有两条路：其一，规划时把朝向维弱化（先用二维 A* 找路，再事后加朝向分配），靠局部层（DWA 天然尊重运动学）消化急转；其二，直接在 SE(2) 上用考虑最小转弯半径的曲线族（圆弧加直线段，或 Reeds-Shepp 曲线——它还允许倒车）拼接，得到的路径原生可行驶。前者实现快，后者质量高，多数商用导航栈走第一条路加强力的局部层。

代价地图：分层维护的工程形态

4.1 节的膨胀思想在成熟导航栈里落地为"分层代价地图"：静态层存先验地图的膨胀结果；障碍层叠加传感器实时观测；膨胀层在两者之上按安全半径生成代价梯度；必要时再加禁区层（人工标注的禁行区，任何算法不得穿越）。分层的价值在于职责分离——静态地图更新、实时避障、安全裕度各自独立维护，任何一层出问题都能单独定位。代价数值的通行约定：致命障碍取 254（不可通行），膨胀区从 253 向外衰减到 1，自由区 0——规划器按"避开高代价但允许低代价通过"工作，比二值化的"能走或不能走"细腻得多。

问题：膨胀半径太大导致过不去门怎么办

门宽 0.8 米、机器人直径 0.4 米、膨胀半径 0.25 米——几何上必死。这不是调小膨胀的信号，而是三选一的决策题：核实感知精度后按 4.1 节账单重算膨胀（也许真实的定位误差根本不需要 0.25 米）；给门区做精修（局部建图更细、标定更准，裕度有依据地收缩）；或者承认物理极限，改道或改机器人尺寸。把这题当成"膨胀半径是合同数字"的活案例——想改合同，先拿出新的误差证据，而不是拍脑袋调小。