04.1 从有体积的机器人到 A*：地图、代价与最短路证明

零基础入口：先把地图变成一场“找便宜路线”的游戏

图不是地图图片，而是一份允许连接的清单。节点是机器人参考点可停留的离散位置；边是两个节点之间允许走的运动段；代价是走这条边付出的数值。本课代价取路程（米），不是危险概率或电量。障碍格不作为可用节点，碰撞的连接不作为边。最短路只在这份清单里比较；即使搜索完美，粗网格也可能错过真实世界的窄通道。

开放表 open 是已发现但还有待展开的候选；展开是检查它的邻居；关闭表 closed 是已经展开的记录，不是障碍物。堆 heap 是快速取最小排序值的数据结构，不是把所有候选完全排序：最小值在顶端，插入时向父节点比较并上浮，取顶后用末尾填洞并向较小孩子下沉。数组中的父子关系只是存储关系，不是机器人运动路径；真正路线另用 parent 前驱记录。

把堆单独手演一次：三条候选的排序值依次为 5、3、4 m。插入 5 后数组为 [5]；插入 3 后先放尾部得到 [5,3]，因孩子更小而交换成 [3,5]；插入 4 后得到 [3,5,4]，它大于父节点 3，不交换。取出 3，用尾部 4 填顶得到 [4,5]，无需下沉。注意原来的 [3,5,4] 没有完全排序，只保证父节点不大于孩子；这已经足够每次取最小候选。实际条目还存节点编号与入堆时的代价，同一个节点后来改善时，旧条目必须识别并跳过。

为决定“下一次查谁”，定义 $f=g+h$ ： $g$ 是已经走到候选的最佳已知代价， $h$ 是候选到终点的乐观估计， $f$ 是沿当前这条已知前缀继续到终点的总代价下界，不一定是所有路线中的全局最优代价下界：已经走到这里的前缀也可能绕了远路。后文证明全局最优性时，需使用最优路径前沿节点上 $g=g^*$ 的条件，其中 $g^*$ 表示真正的最短前缀代价。例如已走 $2$ m、至少还要 $3$ m，则 $f=5$ m。这里“至少”很重要：可采纳表示估计不高于真实剩余最短路；隔着墙的两点直线距离可能很短，但绕墙只会更长，故直线距离适合作下界。它不保证精确预测剩余时间。

第一次手算：五个格子怎样进入和离开 open

先故意使用比后文简单的二维四邻域：每边 $1$ m，只能上下左右。坐标写为“列、行”，邻居顺序右、上、左、下；同分值按先入先出。图中 S 是起点、G 是终点、# 是障碍：

行 1     A —— B —— C
         |         |
行 0     S    #    G
         0    1    2   列

本小例用曼哈顿启发值：横向还差几格加纵向还差几格，再乘每格长度。只能沿轴走时，每个剩余轴向距离都必须走完，故它不会高估；后文允许对角移动时不能照搬这个启发值。

步骤	弹出及展开	open（节点： $g,h,f$ ，单位 m）	closed
初始化	尚未弹出	S：0,2,2	空
1	S 只能发现 A	A：1,3,4	S
2	A 发现 B；回 S 需 2 m，不优于 0	B：2,2,4	S,A
3	B 发现 C；障碍不入表	C：3,1,4	S,A,B
4	C 发现 G	G：4,0,4	S,A,B,C
5	弹出 G，终止	空	S,A,B,C,G

回溯 G 的前驱 C，再回 B、A、S，反转后得到 S→A→B→C→G，总代价 $4$ m。起点估计仅 $2$ m，不是算法出错，而是它没有把墙的绕行算进去。这个极小图每次恰好只有一个候选；复杂地图会有多个候选，堆才真正节省比较次数。下文代码用可重开的候选和过时代价检查，没有永久 closed 集；本表的 closed 仅帮助记录“展开过谁”。

先做再看代码： 三维 A* 在线练习采用 26 邻域，请先说明为何本表的曼哈顿启发不能直接移植，再实现整边判碰。

学习问题：一张标有障碍的地图，为什么不能直接把空格连起来？本课把“机器人能通过哪里”变成有限图，再从最小代价定义推导搜索顺序。输入是静态占据地图、机器人外形包络、起终点；输出是以米为单位的一串 NED 航点，不是速度，更不是推进器命令。先把几何问题说清，再讨论搜索最优性。

符号和建模范围

符号	含义	维数 / 单位	坐标或约定
$p^n,o$	位置、网格原点	3×1 / m	NED
$i,\delta$	整数体素索引、邻居偏移	3×1 / 无量纲	每轴整数
$d$	立方体素边长	标量 / m	各轴同分辨率
$\mathcal O,\mathcal K$	障碍集合、机器人占据形状	三维集合 / m	同一地图系
$r_{\rm inf}$	膨胀半径	标量 / m	球形保守包络
$c(i,j),g(i),h(i),f(i)$	边长、已知起点代价、剩余估计、排序键	标量 / m	本课只最小化长度
$g^,h^,C^*$	真实最短起点代价、剩余代价、全程代价	标量 / m	星号是最优值而非估计

假设地图在一次搜索期间不变、图有限、每条边代价严格为正、起点与终点可行。机器人姿态变化对占据空间的影响先用覆盖所有允许姿态的包络处理。若要利用细长外形穿窄缝，就必须把姿态加入规划状态，而不能同时使用点机器人图和姿态相关的真实外形。

先膨胀障碍，再离散位置

\begin{aligned}\mathcal O_{\rm cfg}&=\mathcal O\oplus(-\mathcal K),\\r_{\rm inf}&=r_{\rm body}+r_{\rm loc}+r_{\rm map}+r_{\rm track},\\i_k&=\left\lfloor\frac{p_k^n-o_k}{d}\right\rfloor,\quad p_i^n=o+d\left(i+\tfrac12\mathbf1\right).\end{aligned}

(4.1)

第一行是构型空间障碍：机器人参考点落入该集合，就存在身体上的一点与障碍相交。球形包络关于原点对称，负号不改变球，膨胀便退化为把障碍向外扩一个半径。第二行的四项分别覆盖身体、定位误差、地图误差与跟踪偏离，均为米；这是保守预算，不是把随机标准差直接相加就自动获得某个置信度。若以体素中心近似整格，还需覆盖半条体对角线，或直接将与膨胀障碍相交的整格标为占据。第三行解释索引为什么要取下整而非四舍五入：单元格对应半开区间，中心只是代表点。

26 邻域允许每个轴移动负一、零或正一格，排除全零，总共二十六种。它使三维路径不必只沿坐标轴折返，但也引入穿角问题：在二维切片中，起点格和对角目标格都空，两个侧邻格却被墙占据，直接连线仍会撞到墙角。三维空间对角线会接触更多面、棱和角；只检查终点的占据状态不够。

\begin{aligned}\delta&\in\{-1,0,1\}^3\setminus\{0\},\\c(i,i+\delta)&=d\|\delta\|_2,\\\ell(s)&=(1-s)p_i^n+s p_j^n,\quad 0\le s\le1,\\\operatorname{edgeFree}(i,j)&\iff\ell([0,1])\cap\mathcal O_{\rm cfg}=\varnothing.\end{aligned}

(4.2)

实现时用三维体素遍历枚举线段穿过或接触的全部体素，边界接触采用保守判碰。手写初版可以更保守：对一次邻域移动，检查由每个非零偏移轴“走或不走”组成的所有中间格；空间对角线最多检查七个新格。这样会拒绝一些本可通行的边，但不会把仅检查终点误当完整碰撞检测。做路径快捷连接时，边可能跨很多格，必须改用完整线段遍历。

膨胀公式为什么能把“有体积”换成“一个点”

令机器人参考点为 $p$ ，身体上某点相对参考点的偏移为 $k\in\mathcal K$ 。发生碰撞的充要条件，是存在障碍点 $o\in\mathcal O$ ，使 $p+k=o$ 。移项得到 $p=o-k$ ；把所有可能的 $o$ 与 $k$ 枚举出来，正好是 $\mathcal O\oplus(-\mathcal K)$ 。这说明膨胀不是为了画面好看，而是在固定姿态或保守包络假设下，精确改写碰撞判据。球包络半径 $0.25$ m、定位预算 $0.10$ m、地图预算 $0.05$ m、跟踪预算 $0.10$ m 时，障碍需外扩 $0.50$ m；原来宽 $0.90$ m 的通道，对这个预算下的参考点已经没有可用通路。

这里必须区分三个对象：占据体素是一个体积，格点坐标是其代表位置，搜索边是两个代表位置之间的一条线段。用边公式中的参数 $s$ 连续扫过 $[0,1]$ ，才覆盖整个运动段。参考程序的中间格组合检查是专为一格邻接边设计的保守替代；它不应拿去验证跨十格的平滑捷径。地图坐标到索引时，原点 $o$ 与分辨率 $d$ 共同定义世界尺度；若程序在索引上计算欧氏距离，却忘了乘 $d$ ，路线形状也许不变，输出代价的单位却会错。

从代价下界推导 A*

\begin{aligned}g(j)&\leftarrow\min\{g(j),g(i)+c(i,j)\},\\h(i)&=\|p_{\rm goal}^n-p_i^n\|_2,\qquad f(i)=g(i)+h(i),\\h(i)&\le c(i,j)+h(j),\qquad h({\rm goal})=0.\end{aligned}

(4.3)

第一行叫松弛：走到当前节点的已知最佳路线再走一条边，可能改进邻居。第二行把“已经付出的代价”和“至少还要付出的代价”相加，优先弹出最有希望的候选。第三行由欧氏距离的三角不等式直接得到，叫一致性。沿任何到终点的路径把第三行逐项相加，中间启发值抵消，就有启发值不超过该路径总长；再取最短路径，得到可采纳性。因此这里不是凭经验说欧氏启发式好，而是证明它为下界。

为什么目标应在弹出时结束，而不是第一次发现时结束？发现只是找到一条路线，堆中可能还有更便宜的路线尚未展开。作反证：假设弹出的目标代价大于最优代价；沿最优路径，从起点向后找尚未展开的第一个前沿节点。它的前驱已正确松弛，所以该节点的已知代价等于最优前缀代价，其启发值不超过剩余最优代价，于是排序值不大于全程最优代价。它应先于那个更贵目标弹出，矛盾。保持此论证需要允许更优路线重新打开节点，或采用一致启发式确保永久闭集正确。

\begin{aligned}h(i)&\le h^*(i),\\f(i_{\rm frontier})&=g^*(i_{\rm frontier})+h(i_{\rm frontier})\le C^*,\\f({\rm goal})&=g({\rm goal})>C^*.\end{aligned}

(4.4)

一致性还有另一个作用：沿一次松弛生成的路径，排序值不下降，因为下一点增加的边长至少补足启发值的下降。因此节点第一次以最小键弹出时，其起点代价已经最优。可采纳但不一致的启发式仍可用于允许重开的 A*；不能照搬“不再访问闭集”的优化。Hart、Nilsson、Raphael 的原论文和勘误是概念源头，TB04-S1；这里的证明专门针对本课有限、正权图，避免把“图最短”扩大成“真实水域任意运动最优”。

一致性证明的中间步骤与“关闭节点”的真正含义

把某条从节点 $i_0$ 到终点 $i_K$ 的路径记为 $i_0,i_1,\ldots,i_K$ 。对每一条边写一次一致性，不是直接背“欧氏距离可采纳”：

\begin{aligned} h(i_0)-h(i_1)&\le c(i_0,i_1),\\ h(i_1)-h(i_2)&\le c(i_1,i_2),\\ \sum_{j=0}^{K-1}[h(i_j)-h(i_{j+1})]&\le\sum_{j=0}^{K-1}c(i_j,i_{j+1}),\\ h(i_0)&\le\sum_{j=0}^{K-1}c(i_j,i_{j+1}). \end{aligned}

最后一个等价化简使用终点启发值为零；对右侧所有可行路径取最小值，才得到 $h(i_0)\le h^*(i_0)$ 。这是由局部边条件推导全局下界。反方向通常不成立：一个启发值可以总体不超过真实剩余代价，却在某一条边上下降过快，破坏一致性。

再看搜索不变式：g[u] 始终是一条已经发现的真实起点路径的长度，因此不可能比最优起点代价更小；parent[u] 记录实现该长度的前驱。弹出一次旧堆项不说明节点现在仍属于那条旧路线，故必须比较入堆快照与当前 g。参考实现每次严格改进才重新入堆，没有永久闭集，因此也能处理可采纳但不一致的启发式；在这里的一致欧氏启发式下，重开通常不会发生。

沿当前路径松弛到相邻点时，新的排序值满足

f(j)=g(i)+c(i,j)+h(j)\ge g(i)+h(i)=f(i).

它表示“沿这条候选路线向前走，下界不能变得更乐观”。这才是永久关闭节点优化的依据之一。目标第一次入堆只代表某条路线存在；目标以最新代价出堆才表示没有更小下界的前沿需要先处理。手算时把“发现”和“展开”分别画成两种颜色，证明与程序就能一一对应。

手算与从零实现

令分辨率为 0.5 m，索引从 (0,0,0) 到 (2,1,1)，无障碍。先走一个空间对角，再走一个轴向步，代价为 0.866025+0.5=1.366025 m；直线启发值为 1.224745 m，确实较小。若误用曼哈顿距离，起点启发值为 2 m，反而超过可行最短路。加入非负风险罚分时，必须先把风险换成与长度一致的代价单位；时间目标则可用距离除以最大可能地速作下界，不能把米与秒直接相加。

先手写，再展开参考实现

将本课公式写成一个 C++17 单文件程序。先列输入单位与退化分支，再写核心循环；不要先看答案再照抄。参考程序只依赖标准库，可用 g++ -std=c++17 -O2 lesson.cpp -o lesson && ./lesson 离线运行，不访问机器人。断言是对上文手算结果的检查，不是用程序输出代替推导。

参考解答（展开查看）

// 教学实现，不是 ArduSub 原文；g++ -std=c++17 -O2 lesson.cpp
#include <array>
#include <vector>
#include <cmath>
#include <limits>
#include <algorithm>
#include <iostream>
#include <cassert>
using Cell=std::array<int,3>;
struct Entry {double f,g; int id,serial;};
bool before(const Entry&a,const Entry&b){
    return a.f<b.f || (a.f==b.f && a.serial<b.serial);
}
struct Heap {
    std::vector<Entry> a;
    void push(Entry e){
        a.push_back(e); std::size_t k=a.size()-1;
        while(k>0){auto p=(k-1)/2;
            if(!before(a[k],a[p])) break;
            std::swap(a[k],a[p]); k=p;
        }
    }
    Entry pop(){
        assert(!a.empty()); Entry out=a[0],last=a.back(); a.pop_back();
        if(a.empty()) return out;
        a[0]=last; std::size_t k=0;
        while(2*k+1<a.size()){
            auto j=2*k+1;
            if(j+1<a.size() && before(a[j+1],a[j])) ++j;
            if(!before(a[j],a[k])) break;
            std::swap(a[j],a[k]); k=j;
        }
        return out;
    }
};
constexpr int N=4,SIZE=N*N*N;
int id(Cell p){return (p[0]*N+p[1])*N+p[2];}
Cell cell(int i){return {i/(N*N),(i/N)%N,i%N};}
bool inside(Cell p){return std::all_of(p.begin(),p.end(),[](int x){return x>=0&&x<N;});}
double distance(Cell a,Cell b){
    double s=0;for(int k=0;k<3;++k)s+=(a[k]-b[k])*(a[k]-b[k]);
    return .5*std::sqrt(s); // 0.5 m grid
}
bool edgeFree(Cell u,Cell d,const std::array<bool,SIZE>& wall){
    // 保守检查每个移动轴的全部中间组合，拒绝对角穿角。
    for(int mask=0;mask<8;++mask){Cell q=u;
        for(int k=0;k<3;++k)if(mask&(1<<k))q[k]+=d[k];
        if(!inside(q)||wall[id(q)])return false;
    }return true;
}
struct Result{double cost;std::vector<Cell> path;};
Result search(Cell start,Cell goal,const std::array<bool,SIZE>&wall,bool use_h){
    const double inf=std::numeric_limits<double>::infinity();
    if(!inside(start)||!inside(goal)||wall[id(start)]||wall[id(goal)])return {inf,{}};
    std::array<double,SIZE> g;g.fill(inf);
    std::array<int,SIZE> parent;parent.fill(-1);
    Heap open;int serial=0;g[id(start)]=0;
    open.push({use_h?distance(start,goal):0,0,id(start),serial++});
    while(!open.a.empty()){
        Entry top=open.pop(); if(top.g!=g[top.id])continue;
        Cell u=cell(top.id);
        if(u==goal){std::vector<Cell> path;
            for(int i=top.id;i!=-1;i=parent[i])path.push_back(cell(i));
            std::reverse(path.begin(),path.end());return {top.g,path};
        }
        for(int dx=-1;dx<=1;++dx)for(int dy=-1;dy<=1;++dy)for(int dz=-1;dz<=1;++dz){
            Cell d{dx,dy,dz};if(d==Cell{0,0,0}||!edgeFree(u,d,wall))continue;
            Cell v{u[0]+dx,u[1]+dy,u[2]+dz};double cand=top.g+distance(u,v);
            if(cand<g[id(v)]){
                g[id(v)]=cand;parent[id(v)]=top.id;
                open.push({cand+(use_h?distance(v,goal):0),cand,id(v),serial++});
            }
        }
    }return {inf,{}};
}
int main(){
    std::array<bool,SIZE> wall{};
    auto a=search({0,0,0},{2,1,1},wall,true);
    auto d=search({0,0,0},{2,1,1},wall,false);
    assert(std::abs(a.cost-(.5+.5*std::sqrt(3.)))<1e-12);
    assert(std::abs(a.cost-d.cost)<1e-12);
    wall[id({1,0,0})]=true;
    assert(!edgeFree({0,0,0},{1,1,0},wall));
    std::cout.precision(12);std::cout<<a.cost<<" m, "<<a.path.size()<<" nodes\n";
}

堆上浮和下沉每次至少跨一层，单次操作随堆大小呈对数增长。用递增序号处理相同代价，避免任意对象不可比较；保存入堆时的代价，便能跳过过时副本。上述版本没有永久闭集，因此更优路径可以重开节点。重建路径不断沿 parent 回溯到起点，再反转；无路时返回明确状态，不能返回空数组却当作“已到达”。

映射到项目与练习

项目阅读路线为 planner_astar.py 中的 load_occupancy_map → AStar3DPlanner，再交给 nav_los.py。前者读 YAML 边界、障碍和分辨率并膨胀，后者采用 26 邻域、欧氏边长和欧氏启发式；A* 可选且配置默认关闭，所以直接航点也可进入制导。阅读时分别找“节点合法”“边合法”“路径后处理”三个位置，不把其中任何一个代替另两个。

将启发式设为零，比较 A* 与 Dijkstra 的最优代价和展开顺序。
在二维切片封住两个侧邻格，比较“只看终点”和“检查中间格”的对角连接结果。
打印无障碍三维例每次出堆的 $g,h,f$ ，解释图最短路为什么不等于连续直线长度。

参考解答（展开查看）

练习一：手写堆后，把启发式置零得到 Dijkstra，对同一固定地图比较终点代价。答案要点：代价应相同，展开顺序和节点数可不同。练习二：在 2×2 二维切片封住两个侧邻格，尝试对角连接。答案：终点检查会误放行，保守中间格检查拒绝。练习三：为上面的无障碍三维例打印每次出堆的 g、h、f；解释 1.366025 m 与连续直线长度不同，是格点可选方向造成，而非 A* 没找到最优。

用一张纸复述完整算法链

地图坐标与边界先定义“哪里”；占据与膨胀定义“身体能否进入”；邻域及整边碰撞定义“哪些运动连接允许”；边代价定义“什么叫更好”； $g$ 记录已经找到的实际前缀， $h$ 给未走部分的下界，堆用 $f=g+h$ 决定下一次展开谁。搜索结束后沿 parent 回溯得到路径；路径后处理如果改变了边，就重新碰撞检查，最后交给制导而不是直接发电机。项目里这些阶段横跨 load_occupancy_map、AStar3DPlanner 与 LOSNavigator.update。A* 不在 ArduSub 接收函数里运行；飞控只接收后续已经转换好的运动参考，这个边界使规划复杂度不会阻塞低层姿态循环。

原始资料精读

TB04-S1 · Hart, Nilsson & Raphael：A Formal Basis for the Heuristic Determination of Minimum Cost Paths；1972 勘误

1968 原论文。先读图、代价和启发式定义，再区分找到最优解与搜索效率意义的 optimality；勘误见下一项。

TB04-S2 · Hart, Nilsson & Raphael：Correction to A Formal Basis…

1972 作者勘误全文；阅读它是为了避免把不同最优性命题混为一谈。