NavFn（Navigation Function）是 ROS 中最经典、最常用的 全局路径规划算法，核心是在栅格地图上用 Dijkstra / A* 找一条 “代价最小” 的安全路径。下面用最通俗的方式讲清楚：它在干嘛、输入什么、输出什么、怎么算的。

一、一句话理解 NavFn

NavFn = 栅格地图 + 代价地图 + Dijkstra（或 A）+ 梯度回溯 = 全局最优路径*

• 作用：给机器人从 起点 A 到 目标点 B 规划一条 不撞墙、尽量短、尽量好走 的全局路线。
• 定位：全局规划器（Global Planner），只算大方向、粗路径，不管实时避障（那是局部规划器 DWA 的事）。

二、通俗原理：像 “水波纹扩散” 一样找路

你可以把 NavFn 的过程想象成：在地图上从终点往起点 “放水”，波纹（代价）扩散，最后从起点沿着 “水往低处流” 的方向走回终点。

1. 地图是什么样的？（栅格 + 代价）

• 地图被切成一个个小方格（栅格 /cell）。
• 每个格子有 代价 cost：
  • 0 = 完全自由（好走）
  • 254/255 = 障碍物 / lethal（不能走）
  • 中间值 = 代价高 / 难走 / 靠近墙
• 如图所示：
• 

2. 核心四步

1. 读入地图与代价：知道哪里能走、哪里是墙、哪里难走。
2. 设置起点 & 终点。
3. 波前传播（Dijkstra）：
   • 从 终点 开始，往外一层层扩散（像水波纹）。
   • 给每个格子算一个 potential（代价 / 势能）：
     • 终点代价 = 0
     • 离终点越远、路越难走，代价 越大
4. 梯度下降找路径：
   • 从 起点 出发，每一步都往代价更小的方向走
   • 一直走到终点 → 这一串点就是 最优路径.                  

5. 栅格邻居示意图，势能在格子间扩散时，主要考虑上下左右四个邻居（也可扩展到 8 邻域）：

      上(u)
       |
左 --- 当前格(n) --- 右
       |
      下(d)

        势能从目标点向外扩散，像水波一样；格子势能 = 邻居最小势能 + 通行代价。

三、输入与输出

1. 输入

必备输入

• 代价地图（Costmap）：二维栅格 + 每个栅格的代价值（最核心输入）。
• 起点坐标（x, y）：机器人当前位置。
• 目标点坐标（x, y）：要去的地方。
• 地图参数：宽、高、分辨率（每个栅格对应现实多少米）。

常用参数

• allow_unknown：是否允许穿过 “未知区域”。
• use_astar：用 A*（更快）还是默认 Dijkstra（更稳、保证最优）。
• 机器人半径：算法默认假设是 圆形机器人，自动膨胀障碍物。

2. 输出

主输出：全局路径

• 一串连续的 世界坐标点 (x, y) 序列。
• 从 起点 → ... → 目标点，机器人按顺序走这些点。
• 格式：ROS 中是 nav_msgs/Path 消息（含 PoseStamped 数组）。

内部输出（调试 / 可视化）

• potential 数组：每个栅格的代价值（波纹高度图）。
• 梯度数组：每个点往哪走代价下降（方向场）。

四、核心流程与代码解析

NavFn 的实现可分为 初始化 → 势能扩散 → 梯度计算 → 路径回溯 四步，下面结合 代码逐段解析。

流程 1：初始化与设置（setupNavFn）

这一步负责初始化势能数组、设置边界、将目标点加入待处理队列。

核心代码

Dijkstra 算法入口
bool NavFn::calcNavFnDijkstra(bool atStart)
{
  setupNavFn(true);  // 初始化势能数组
  return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), atStart);
}

void NavFn::setupNavFn(bool keepit)
{
  // 1. 初始化势能数组为高值（表示未访问）
  for (int i = 0; i < ns; i++) {
    potarr[i] = POT_HIGH;  // POT_HIGH 近似无穷大
  }

  // 2. 设置边界为障碍物（防止机器人走出地图）
  // ... 省略边界设置代码，核心是将四周边界设为 COST_OBS

  // 3. 初始化优先级缓冲区（用于 Dijkstra 波前传播）
  curP = pb1;   curPe = 0;   // 当前优先级队列
  nextP = pb2;  nextPe = 0;  // 下一优先级队列
  overP = pb3;  overPe = 0;  // 溢出缓冲区

  // 4. 设置目标点势能为 0，并加入待处理队列
  int k = goal[0] + goal[1] * nx;  // 目标点在数组中的索引
  initCost(k, 0);  // 目标点势能 = 0
}

对应公式（势能初始化）：

所有格子初始势能为无穷大，仅目标点势能为 0。

流程 2：Dijkstra 势能扩散（propNavFnDijkstra + updateCell）

这是 NavFn 的核心：从目标点开始，像水波纹一样向外扩散，计算每个格子的最小势能。

核心代码 1：扩散循环

Dijkstra 波前传播主循环
bool NavFn::propNavFnDijkstra(int cycles, bool atStart)
{
  int startCell = start[1] * nx + start[0];  // 起点格子索引

  for (; cycle < cycles; cycle++) {
    // 1. 检查缓冲区是否为空（无格子可扩展）
    if (curPe == 0 && nextPe == 0) break;

    // 2. 处理当前优先级队列中的所有格子
    pb = curP;
    i = curPe;
    while (i-- > 0) {
      updateCell(*pb++);  // 核心：更新单个格子的势能
    }

    // 3. 交换缓冲区：当前队列处理完，切换到下一队列
    curPe = nextPe;
    nextPe = 0;
    std::swap(curP, nextP);

    // 4. 如果当前队列为空，从溢出缓冲区补充（提高优先级阈值）
    if (curPe == 0) {
      curT += priInc;  // 提高优先级阈值
      curPe = overPe;
      overPe = 0;
      std::swap(curP, overP);
    }

    // 5. 检查是否到达起点（可选优化：到达起点即停止）
    if (atStart && potarr[startCell] < POT_HIGH) {
      break;  // 找到起点，停止传播
    }
  }
}

核心代码 2：单个格子势能更新

更新单个格子的势能（核心计算逻辑）
inline void NavFn::updateCell(int n)
{
  // 1. 获取上下左右四个邻居的势能值
  float l = potarr[n - 1];     // 左邻居
  float r = potarr[n + 1];     // 右邻居
  float u = potarr[n - nx];    // 上邻居
  float d = potarr[n + nx];    // 下邻居

  // 2. 找出水平和垂直方向的最小势能
  float ta, tc;
  if (l < r) tc = l; else tc = r;  // 水平方向最小
  if (u < d) ta = u; else ta = d;  // 垂直方向最小

  // 3. 计算新势能值（平面波更新，考虑二次插值）
  float hf = costarr[n];  // 当前格子的通行代价
  float dc = fabs(tc - ta);  // 两个最小邻居的势能差

  float pot;
  if (dc >= hf) {
    // 简单情况：直接取最小邻居 + 当前代价
    pot = min(ta, tc) + hf;
  } else {
    // 复杂情况：二次插值，更精确地估计势能
    float d = dc / hf;
    float v = -0.2301 * d * d + 0.5307 * d + 0.7040;  // 经验系数
    pot = min(ta, tc) + hf * v;
  }

  // 4. 如果势能值降低了，更新并传播给邻居
  if (pot < potarr[n]) {
    potarr[n] = pot;  // 更新当前格子势能
    // 将邻居加入待处理队列（根据势能大小分队列）
    if (pot < curT) {
      push_next(n - 1);   // 左邻居
      push_next(n + 1);   // 右邻居
      push_next(n - nx);  // 上邻居
      push_next(n + nx);  // 下邻居
    } else {
      push_over(n - 1);   // 势能较高，加入溢出队列
      // ... 省略其他邻居的 push_over
    }
  }
}

对应公式（势能扩散更新）

下一个格子 q 的势能 = min (原值，当前格子 p 的势能 + 移动代价 c(p,q))。

流程 3：梯度计算（gradCell）

势能扩散完成后，需要计算每个格子的梯度（即 “水往低处流” 的方向）。

核心代码

计算单个格子的梯度
float NavFn::gradCell(int n)
{
  float cv = potarr[n];
  float dx = 0.0, dy = 0.0;

  // 梯度 = 左右/上下邻居的势能差
  if (potarr[n - 1] < POT_HIGH) dx += potarr[n - 1] - cv;  // 左
  if (potarr[n + 1] < POT_HIGH) dx += cv - potarr[n + 1];  // 右
  if (potarr[n - nx] < POT_HIGH) dy += potarr[n - nx] - cv; // 上
  if (potarr[n + nx] < POT_HIGH) dy += cv - potarr[n + nx]; // 下

  // 归一化梯度（得到单位方向向量）
  float norm = hypot(dx, dy);
  gradx[n] = dx / norm;  // x 方向梯度
  grady[n] = dy / norm;  // y 方向梯度
}

流程 4：路径回溯（calcPath）

最后从起点出发，沿梯度方向 “下坡”，一步步走到终点，生成路径。

核心代码

int NavFn::calcPath(int n, int * st)
{
  int stc = start[1] * nx + start[0];  // 起点格子索引
  float dx = 0.0, dy = 0.0;            // 起点在格子内的偏移

  for (int i = 0; i < n; i++) {
    // 检查是否接近目标（势能足够小）
    if (potarr[stc] < COST_NEUTRAL) {
      return npath;  // 到达目标
    }

    // 计算当前格子及相邻格子的梯度（用于双线性插值）
    gradCell(stc);
    gradCell(stc + 1);
    gradCell(stc + nx);
    gradCell(stc + nx + 1);

    // 双线性插值：计算精确的连续梯度方向
    float x = (1.0 - dx) * gradx[stc] + dx * gradx[stc + 1];
    float y = (1.0 - dy) * grady[stc] + dy * grady[stc + 1];

    // 沿梯度方向移动一步（步长为 pathStep）
    float ss = pathStep / hypot(x, y);
    dx += x * ss;
    dy += y * ss;

    // 处理格子边界：如果偏移超出当前格子，切换到相邻格子
    while (dx >= 1.0) { dx -= 1.0; stc += 1; }
    while (dx < 0.0)  { dx += 1.0; stc -= 1; }
    while (dy >= 1.0) { dy -= 1.0; stc += nx; }
    while (dy < 0.0)  { dy += 1.0; stc -= nx; }

    // 记录路径点（转换为世界坐标）
    pathx[npath] = stc % nx + dx;
    pathy[npath] = stc / nx + dy;
    npath++;
  }
  return npath;
}