slam全局路径规划算法详解（Dijkstra、A*)

1. 原理

Dijkstra算法
核心思想 ：贪心策略，始终选择当前状态下距离起点最近的顶点作为扩展方向，并更新其相邻顶点的最短路径。通过迭代逐步构建从起点到所有顶点的最短路径树。
特点 ：
仅适用于无负权重边的图。
确保找到全局最优解（最短路径）。
时间复杂度为 O(n ^2 ) （邻接矩阵存储）。
A*算法
核心思想 ：结合Dijkstra的全局代价g(n) 和启发式函数h(n) ，综合评估节点优先级。公式为：
f(n)=g(n)+h(n)
其中：
g(n) ：起点到当前节点的实际代价；
h(n) ：当前节点到目标的启发式估计代价（如欧几里得距离）。
特点 ：
启发式函数引导搜索方向，显著减少探索范围；
若 h(n) 不高估实际代价，则能保证找到最优解。

2. 优劣势对比

Dijkstra 是经典的单源最短路径算法，适合小规模图且需严格最优解的场景。
A* 通过启发式优化大幅提高效率，但依赖 h(n) 的设计，适用于大规模地图的高效寻路。

3. 示例对比（二维网格地图寻路）

• 地图描述 ：10x10网格，起点为左下角(0,0)，终点为右上角(9,9)，障碍物随机分布。

• Dijkstra行为 ：

  • 从起点向外层扩展，均匀探索所有方向；
  • 即使终点在右侧，也会探索左侧区域，导致计算量大；
  • 最终路径长度最短，但计算耗时较高。

• A*行为 ：

  • 使用启发式函数 h(n)=曼哈顿距离 ；
  • 优先探索靠近终点的方向，跳过大部分无关区域；
  • 路径长度与Dijkstra相同，但计算速度显著提升。

4. 伪代码

Dijkstra算法

• 使用两个列表 mark（已访问节点）和 unmarked（未访问节点）。
• 每次迭代选择当前距离最小的顶点，更新其邻居的最短路径

function Dijkstra(Graph, source):create vertex set Q        // 未知最短路径的顶点集合for each vertex v in Graph:dist[v] ← INFINITY     // 初始化所有顶点距离为无穷大prev[v] ← UNDEFINED    // 前驱节点初始化add v to Q             // 将顶点加入集合Qdist[source] ← 0           // 起点到自身的距离为0while Q is not empty:u ← vertex in Q with minimum dist[u]  // 选择当前距离最小的顶点remove u from Qfor each neighbor v of u:alt ← dist[u] + length(u, v)      // 计算替代路径长度if alt < dist[v]:                 // 若更短路径存在dist[v] ← altprev[v] ← ureturn dist[], prev[]

A*算法

• 使用 openList（待探索）和 closeList（已探索）。
• 启发式函数 h(n) 可采用曼哈顿距离或欧几里得距离。
• 每次扩展节点时，优先选择 f(n) 最小的节点，加速目标搜索。

function A_Star(start, goal):openList ← {start}        // 可探索的节点集合closeList ← empty         // 已探索的节点集合g_score[start] ← 0        // 起点到当前节点的实际代价f_score[start] ← g_score[start] + h(start, goal)  // f(n) = g(n) + h(n)while openList is not empty:current ← node in openList with minimum f_score[]  // 选择f值最小的节点if current = goal:return reconstruct_path()  // 找到目标，重建路径remove current from openListadd current to closeListfor each neighbor v of current:if v in closeList:continue                // 跳过已探索的邻居tentative_g ← g_score[current] + d(current, v)  // 计算新g值if v not in openList or tentative_g < g_score[v]:g_score[v] ← tentative_gf_score[v] ← g_score[v] + h(v, goal)if v not in openList:add v to openList