2、路径规划基础：图论基础、搜索空间表示、代价函数与全额支付概念
路径规划算法的本质是在一个定义良好的搜索空间中，依据某种代价度量寻找从起点到目标点的最优或可行路径。无论是A*、Dijkstra还是RRT，它们都共享同一套底层语言：图论、搜索空间建模和代价函数设计。本章将系统梳理这些基础概念，并重点引入“全额支付”（Full Payment）这一在工程实践中常被忽视却至关重要的代价核算思想——它要求路径代价不仅包含路径本身的累积成本，还必须涵盖搜索过程中的计算开销、约束违反代价以及风险储备。
2.1 图论基础：路径规划的形式化语言
核心原理：图论为路径规划提供了数学抽象框架。一个图 G = (V, E) 由节点集合 V 和边集合 E 构成，每条边 e ∈ E 关联一个权重 w(e)。在机器人路径规划中，节点可以表示离散的位姿状态（如栅格坐标、关节角度组合），边表示状态之间的可行转移，权重则量化转移代价（距离、时间、能量等）。图的类型直接影响算法选择：无向图适用于对称代价场景（如地面移动机器人），有向图适用于非对称代价场景（如带风场影响的无人机、带单向约束的机械臂）。
判断方法：在实际工程中，需要根据任务特性判断图的构建方式：
- 稀疏性判断：若搜索空间大但可行连接少（如狭窄通道环境），适合用稀疏图 + 采样方法；若空间密集（如室内栅格），适合用稠密图 + 确定性搜索。
- 权重类型判断：若边权非负且满足三角不等式，Dijkstra和A*可保证最优；若存在负权边，则需Bellman-Ford或重新建模。
- 动态性判断：静态图可离线预处理（如构建距离场），动态图（如移动障碍物）需增量式更新策略。
2.2 搜索空间表示：从连续世界到离散模型
核心原理：搜索空间表示决定了算法的完备性和效率。常见的表示方法包括：
| 表示方法 | 适用场景 | 优点 | 缺点 |
|---|---|---|---|
| 栅格地图 | 室内机器人、2D导航 | 实现简单、便于障碍物建模 | 分辨率受限、内存开销大 |
| 路标图 | 公路导航、拓扑路径 | 节点少、搜索快 | 需要预处理、动态性差 |
| PRM | 高维空间、机械臂 | 概率完备、适合高维 | 需大量采样、路径非最优 |
| 配置空间(C-space) | 刚体运动规划 | 理论完备、可处理复杂约束 | 计算复杂度高 |
操作流程：构建搜索空间通常遵循以下步骤：①确定机器人自由度并定义配置空间；②根据传感器数据或CAD模型生成障碍物映射；③选择合适的离散化策略（栅格化、随机采样或路标提取）；④验证连通性并构建图结构；⑤为每条边计算转移代价。
2.3 代价函数设计：多目标权衡的艺术
核心原理：代价函数 f(n) = g(n) + h(n) 是A*算法的核心，其中 g(n) 是从起点到当前节点的实际累积代价，h(n) 是到目标点的启发式估计。Dijkstra算法可视为 h(n)=0 的特例。代价函数的设计直接影响路径的“最优性”定义：最短路径 ≠ 最快路径 ≠ 最安全路径。工程中常见的代价项包括：
- 距离代价：欧氏距离、曼哈顿距离或路径长度，单位m。
- 时间代价：考虑速度限制、加速度约束，单位s。
- 能量代价：电机功耗、燃料消耗，单位J或Wh。
- 风险代价：与障碍物距离的倒数、历史事故率，无量纲或概率值。
- 平滑性代价：曲率变化率、转向角度惩罚，用于生成可执行轨迹。
案例拆解：某手术机器人路径规划任务中，代价函数设计为：
J = w1 * L_path + w2 * T_time + w3 * R_risk + w4 * S_smooth
其中：
L_path = 路径总长度 (mm)
T_time = 预计执行时间 (s)
R_risk = 与关键器官的最小距离倒数 (1/mm)
S_smooth = 曲率变化积分 (rad/m)
权重：w1=0.3, w2=0.2, w3=0.4, w4=0.1
归一化：L_max=150mm, T_max=30s, R_max=0.5, S_max=2.0通过调整权重，医生可以在“最短路径”和“最安全路径”之间权衡。临床测试表明，当 w3 从0.2增至0.4时，路径长度增加12%，但最小安全距离从3.2mm提升至5.8mm，显著降低了手术风险。
2.4 全额支付概念：超越路径本身的代价核算
核心原理：“全额支付”（Full Payment）是本章引入的关键概念，它要求在进行算法比较或路径选择时，必须将以下四类代价全部纳入考量：
- 路径累积代价：沿路径逐边累加的代价，即传统意义上的 g(n)。
- 搜索计算代价：算法运行时间、内存占用、迭代次数。对于实时系统（如自动驾驶），即使路径代价稍高，若搜索时间减半，整体效益可能更优。
- 约束违反代价：路径是否违反动力学约束、时间窗约束或安全距离约束。若违反，需额外惩罚或重规划。
- 风险储备代价：为应对环境不确定性预留的代价余量，如传感器噪声、障碍物移动速度估计误差。
| 算法 | 路径代价 (g) | 搜索时间 (s) | 约束违反 | 风险储备 | 全额支付 |
|---|---|---|---|---|---|
| Dijkstra | 12.4m | 3.8 | 无 | 0.5m | 12.9m + 3.8s |
| A* (欧氏启发) | 12.6m | 1.2 | 无 | 0.5m | 13.1m + 1.2s |
| RRT (快速扩展) | 14.8m | 0.4 | 1次转弯过急 | 1.2m | 16.0m + 0.4s + 惩罚 |
操作流程：实施全额支付评估的步骤为：①定义所有代价项及其单位；②为每项设置归一化基准；③运行候选算法并记录各项指标；④将不同量纲代价转换为统一效用值（如货币成本或时间成本）；⑤计算全额支付总值并排序。
2.5 常见误区与总结复盘
常见误区：
- 误区一：认为“最短路径”就是“最优路径”。在存在能耗、风险、时间窗约束时，最短路径往往不是最优解。
- 误区二：忽略搜索计算代价。在嵌入式系统中，A*的0.5s搜索时间可能导致控制周期超时，必须改用更快的算法。
- 误区三：代价函数权重随意设置。权重比例应通过仿真或实验标定，而非经验猜测。
- 误区四：将全额支付与路径代价混为一谈。算法比较时，必须明确比较口径。