07｜三维 A*：离散路径怎样接近真实空间

搜索解决的是图上的问题

A* 用 f(n)=g(n)+h(n) 选择扩展节点：g 是从起点走来的代价，h 是到终点剩余代价的下界。在三维 26 邻域网格中，轴向、面对角和体对角移动长度分别为 l、√2l、√3l。欧氏距离是不高估剩余几何长度的启发函数；允许斜走却用 Manhattan 距离，可能高估并破坏最优性条件。

若代价含非负风险项，可以保留合适权重的几何长度下界；若目标是时间，可用距离除以最大可能地速构造下界。海流使顺流与逆流的边代价不同，因此图的代价也可能有方向性。

为什么要膨胀障碍

搜索节点代表机器人参考点，但真实机器人有体积。将障碍按机器人包络和必要余量膨胀后，才能用点搜索近似扫掠体碰撞检查。对角边还要检查经过的空间，不能从两个障碍体素之间“斜穿角落”。余量包含定位误差和跟踪误差，不只是外壳半径。

路径还不是轨迹

A* 产生折线，拐角处方向可突然改变；真实机器人速度与加速度不能瞬间转向。后续需要连接、平滑和时间参数化：p(t)、v(t)、a(t) 才能成为控制器合理参考。平滑后的曲线可能切入原本避开的障碍，因此对平滑曲线和前视连线仍需碰撞检查。

算例：机器人以 0.4 m/s 前进，可用减速度 0.2 m/s²，理想刹停距离 v²/(2a)=0.4 m；若感知与执行总延迟 0.25 s，还要加约 0.1 m，合计已是 0.5 m，尚未计入误差余量。地图留 0.1 m 空隙不可能靠“控制更准”弥补这种动态限制。

实现与实验

从空地图验证最短长度，再增加墙体、狭窄通道与竖直路径；接着测试对角穿角和障碍膨胀。最后对同一路径分别使用折线直跟踪、平滑轨迹和限速转弯，观察控制饱和。进阶可学 D* Lite 等增量搜索、运动原语和 kinodynamic planning，但首先要区分“图上可走”与“当前速度下可执行”。

阅读 A* 原论文，重点理解启发函数的条件：https://ai.stanford.edu/~nilsson/OnlinePubs-Nils/PublishedPapers/astar.pdf 。