4. Dijkstra 全局规划器

归属 NavFn 势场 · §6.5.1 经典图搜索 · Autonomy ✅ dijkstra_planner

DijkstraPlanner 继承 NavfnPlanner，构造时强制 SetUseAstar(false)，排序键 \(f(n)=\phi(n)\)（\(h\equiv 0\)）。传播与路径提取走同一 calcNavFnDijkstra 代码路径；独立插件用于配置隔离与确定性回归基线。

1. 背景

单源最短路径. Dijkstra（1959）在非负权图上按标号 \(d\) 非递减永久标定节点，保证 SSSP 最优。栅格规划中，代价地图将自由空间离散为 4-连通图；边权有界且为整数时，Dial（1969）以单调桶队列将复杂度降至 \(O(E+WC)\)，\(W\) 为最大边权上界。

NavFn 系实现. Eriksson & Borenstein（1990）GURVEY 以平面波更新近似 Eikonal \(\|\nabla\phi\|=F\)（精确解见 Sethian FMM, 1996）。ROS / nav2 的 navfn 用 Dial 式桶队列传播 + 梯度跟踪；Autonomy dijkstra_planner 显式锁定该流水线的 Dijkstra 分支（§2），平面波与路径细节见 03_navfn.md。

2. 问题

任务. 给定 \(C\) 与 \(q_s,\, q_g \in \mathcal{C}_{\mathrm{free}}\)，求 \(\tau(0)=q_s\)，\(\tau(1)\approx q_g\)，最小化沿路径通行代价（与 NavFn §2 同型）。

输入 / 输出. DijkstraPlanner::CreatePlan 继承 NavfnPlanner::CreatePlan；输出 planning_msgs::Path 及 SUCCESS / NO_PATH / CANCELED。

在线形式. 全局规划通常 1–5 Hz 重规划；\(h\equiv 0\) 时扩展顺序仅由 \(\phi\) 决定，同输入同地图下输出确定性，适合作 NavFn 系回归基线。势场约定（与 NavFn 一致，架构 §1.3.1）：\(\phi(q_s)=0\)（setGoal），传播终止于 \(\phi(q_g)<\phi_{\mathrm{HIGH}}\)（setStart，atStart=true），路径自 \(q^*\) 梯度跟踪至 \(q_s\)。

插件继承与配置.

GlobalPlanner
    └── NavfnPlanner          ← use_astar 可选
            └── DijkstraPlanner  ← 强制 SetUseAstar(false)

项

dijkstra_planner

navfn_planner（Dijkstra 模式）

传播

calcNavFnDijkstra

use_astar=false 时相同

配置域

options.dijkstra()

options.navfn()

use_astar

不存在（构造时禁用）

可误改为 true

典型用途

确定性基线 / 默认插件 ID

可选切换 A*

 
DijkstraPlanner::DijkstraPlanner(...) : NavfnPlanner(...) {
    SetUseAstar(false);  // f = g = φ，h ≡ 0
}

3. 图松弛与 Dial 桶队列

章首路线：Dijkstra 松弛 → NavFn 势场对偶 → Dial 桶队列（propNavFnDijkstra）。\(F_{ij}\) 映射与 updateCell 见 NavFn §2.2–§3.3。

3.1 Dijkstra 松弛（1959）

\[ d(v) = \min_{(u,v)\in E} \big\{ d(u) + w(u,v) \big\}, \qquad d(q_s)=0. \]

  • \(d(v)\)：从 \(q_s\) 到 \(v\) 的最小累积代价。

  • 含义：非负权下按 \(d\) 非递减扩展；label-setting 保证每次弹出节点已最优。

3.2 栅格势场对偶

NavFn 在 4-连通栅格上求 \(\phi\)，连续极限满足 \(\|\nabla\phi\|=F\)，\(\phi(q_s)=0\)。\(\phi(q)\) 为从 \(q\) 到 \(q_s\) 的最小累积代价；在平面波离散更新下，\(\phi\) 与经典 \(d\) 近似共享同一最短路径结构（非严格图论等价；精确 Eikonal 见 FMM）。

方面

经典 Dijkstra

NavFn Dijkstra（本插件）

源点

\(d(q_s)=0\)

\(\phi(q_s)=0\)（initCost）

更新

边松弛

平面波 updateCell

优先级

二叉堆

Dial 式 curP/nextP/overP

终止

\(d(q_g)\) 确定

\(\phi(q_g)<\phi_{\mathrm{HIGH}}\)

路径

前驱回溯

calcPath 梯度跟踪

3.3 Dial 桶队列传播

Dial（1969）对整数权 SSSP 按距离分桶，均摊 \(O(1)\) 入队。NavFn 取 \(\Delta T=2C_n=100\)（priInc），三缓冲语义：

  • curP：当前 \(curT\) 下待更新格点；

  • nextP：\(\phi(n)<curT\) 的新 frontier；

  • overP：溢出桶，\(curT\leftarrow curT+\Delta T\) 后提升为 curP。

setupNavFn 在 \(q_s\) 播种后，主循环批量 updateCell → 交换 curP/nextP → 必要时提升 overP。永久标定（\(\phi\) 不再下降）的格点不再入队；atStart=true 时 \(\phi(q_g)<\phi_{\mathrm{HIGH}}\) 即停。因 \(F_{ij}\) 有界，工程上近 \(O(N)\)，\(N=n_x n_y\)；实际访问格数通常小于 \(N\)，但上界仍为 \(O(N)\)，且 frontier 可能略超出 \(q_g\) 最短测地邻域。cycles=\max(n_x n_y/20,\,n_x+n_y) 为安全上限。

路径提取与容差搜索同 NavFn，见 NavFn §4.3。相对 A*（Hart et al., 1968）：本插件 \(h\equiv 0\)；navfn_planner + use_astar=true 用 \(h=\|n-q_g\|_2\cdot C_n\) 加速（NavFn §4.2），可采纳 \(h\) 下与 Dijkstra 同解。对比见 06_survey.md §6.5。

4. 求解

§3 给出 Dijkstra 传播语义；以下 算法 1 为 DijkstraPlanner 特有流程（步骤 4 固定 Dijkstra）。算法 2–3（PropagateDijkstra / ExtractPath）与 NavFn §5.1 算法 2–3 相同，Alg. 2 无 A* 初阈分支。

4.1 算法（数学描述）

算法 1 DijkstraPlanner 全局规划

\(\mathrm{CreatePlan}(q_s,\, q_g,\, C;\, \Theta,\, \texttt{cancel}) \mapsto \mathrm{Path}\ \|\ \text{FAIL}\ \|\ \text{CANCELED}\)

方向

符号

说明

输入

\(q_s,\, q_g\)

起点 / 终点 PoseStamped

输入

\(C\)

全局 Costmap2D（加锁复制快照）

输入

\(\Theta\)

tolerance, allow_unknown, use_final_approach_orientation

输入

cancel

传播取消回调（NavFn Alg. 2）

输出

\(\mathrm{Path}\)

世界坐标航点（\(q_s \to q^*\)）

输出

状态码

SUCCESS / NO_PATH / CANCELED
\[\begin{split} \begin{aligned} \textbf{1.}\;&\text{worldToMap}(q_s);\; \text{加锁复制 } C \to C';\; c'(q_s)\leftarrow \text{FREE};\; \text{setCostmap}(C') \qquad\text{（NavFn §2.2）} \\[6pt] \textbf{2.}\;&\text{worldToMap}(q_g);\; \text{若任一步越界则 return FAIL} \\[6pt] \textbf{3.}\;&\texttt{setGoal}(q_s);\;\texttt{setStart}(q_g) \qquad\text{（}\phi(q_s)=0\text{；终止于 } q_g\text{）} \\[6pt] \textbf{4.}\;&\mathrm{PropagateDijkstra}(\texttt{cancel},\, \texttt{atStart=true}) \qquad\text{（NavFn Alg. 2；§3.3）} \\[6pt] \textbf{5.}\;&q^* \leftarrow \mathrm{ResolveGoal}(q_g,\, \varepsilon) \qquad\text{（NavFn §4.3）} \\[6pt] \textbf{6.}\;&\textbf{if not found}(q^*) \textbf{ then return FAIL} \\[6pt] \textbf{7.}\;&\mathrm{Path} \leftarrow \mathrm{ExtractPath}(q^*) \qquad\text{（NavFn Alg. 3）} \\[6pt] \textbf{8.}\;&\mathrm{smoothApproachToGoal}(q^*,\, \mathrm{Path}) \\[6pt] \textbf{9.}\;&\textbf{if } \texttt{use_final_approach_orientation} \textbf{ then } \text{修正末端航向} \\[6pt] \textbf{10.}\;&\textbf{return } \mathrm{Path} \end{aligned} \end{split}\]

子程序：势场传播与路径提取见 NavFn 算法 2–3。Complexity：\(O(N)\) 复制 + \(O(N)\) 或更小传播 + \(O(L)\) 梯度跟踪。等价性：与 navfn_planner（use_astar=false）共用 calcNavFnDijkstra，同输入同地图下同输出；配置域不同（§2 表）。

4.2 配置与调用

dijkstra_planner = {
    tolerance = 0.1,
    allow_unknown = false,
    use_final_approach_orientation = false,
},
default_planner_id = "dijkstra_planner",

auto path = server->GetPlan(start, goal, "dijkstra_planner", cancel_checker);

字段

说明

tolerance

目标容差 \(\varepsilon\)（米）；\(\phi(q_g)\) 不可达时在邻域内求 \(q^*\)

allow_unknown

UNKNOWN（255）是否视为可通行

use_final_approach_orientation

末端航向沿路径切向

现象

可能原因

处理

无路径

\(\phi(q^*)\ge\phi_{\mathrm{HIGH}}\)

增大 tolerance，检查 inflation / allow_unknown

锯齿路径

4-连通 + 梯度离散化

Theta* 或后处理平滑

与 NavFn Dijkstra 不一致

使用了 navfn_planner 且 use_astar=true

核对插件 ID 与 use_astar

规划慢

可达 frontier 大

navfn_planner + use_astar=true

5. 参考文献

  1. Dijkstra, E.W., “A Note on Two Problems in Connexion with Graphs”, Numerische Mathematik, 1(1):269–271, 1959. DOI:10.1007/BF01386390

  2. Dial, R.B., “Algorithm 360: Shortest-Path Forest with Topological Ordering”, Communications of the ACM, 12(11):632–633, 1969. DOI:10.1145/363269.363610

  3. Hart, P.E.; Nilsson, N.J.; Raphael, B., “A Formal Basis for the Heuristic Determination of Minimum Cost Paths”, IEEE Trans. SSC, 4(2):100–107, 1968. DOI:10.1109/TSSC.1968.300136

  4. Eriksson & Borenstein, “The GURVEY…”, IEEE AES Magazine, 1990. IEEE

  5. Sethian, J.A., “A Fast Marching Level Set Method…”, PNAS, 93(4):1591–1595, 1996. DOI:10.1073/pnas.93.4.1591

  6. Ferguson & Stentz, “The Field D* Algorithm…”, ICRA, 2005. — 栅格加权最短路径工程谱系

  7. LaValle, Planning Algorithms, 2006, Ch. 2. lavalle.pl/planning

  8. NavFn 规划器 · nav2_navfn_planner · §6 综述