nav2 A*导航算法代码解释

首先明白地图坐标系和cartographer一样

/**
 * note: 地图坐标系可视化展示
 * ros的地图坐标系    cartographer的地图坐标系     cartographer地图的像素坐标系 
 * 
 * ^ y                            ^ x              0------> x
 * |                              |                |
 * |                              |                |
 * 0 ------> x           y <------0                y       
 * 
 * ros的地图坐标系: 左下角为原点, 向右为x正方向, 向上为y正方向, 角度以x轴正向为0度, 逆时针为正
 * cartographer的地图坐标系: 坐标系右下角为原点, 向上为x正方向, 向左为y正方向
 *             角度正方向以x轴正向为0度, 逆时针为正
 * cartographer地图的像素坐标系: 左上角为原点, 向右为x正方向, 向下为y正方向
 */

这里是算法入口，在navfn_planner.cpp文件中238行

void
NavFn::setupNavFn(bool keepit)
{
  // reset values in propagation arrays
  for (int i = 0; i < ns; i++) {
    potarr[i] = POT_HIGH;
    if (!keepit) {
      costarr[i] = COST_NEUTRAL;
    }
    gradx[i] = grady[i] = 0.0;
  }

  // outer bounds of cost array   ba si zhou de bian tian cheng obstacle
  COSTTYPE * pc;  //下面更新四条边界的cost值为COST_OBS，不可以碰到
  pc = costarr;
  for (int i = 0; i < nx; i++) { 
    *pc++ = COST_OBS;
  }
  pc = costarr + (ny - 1) * nx;
  for (int i = 0; i < nx; i++) {
    *pc++ = COST_OBS;
  }
  pc = costarr;
  for (int i = 0; i < ny; i++, pc += nx) {
    *pc = COST_OBS;
  }
  pc = costarr + nx - 1;
  for (int i = 0; i < ny; i++, pc += nx) {
    *pc = COST_OBS;
  }

  // priority buffers
  curT = COST_OBS; 
  curP = pb1;  //初始化一维数组int[1000]，当前点的指针
  curPe = 0;
  nextP = pb2;  //初始化一维数组int[1000]，下一个点的指针
  nextPe = 0;
  overP = pb3; //初始化一维数组int[1000]，某条件下下一个点的指针
  overPe = 0;
  memset(pending, 0, ns * sizeof(bool));

  // set goal
  int k = goal[0] + goal[1] * nx;
  initCost(k, 0);   //初始化目标点potarr =0，curp有四个数据 {curP[curPe++] = n; pending[n] = true;}} 目标点四邻域的点及其检索号n

  // find # of obstacle cells
  pc = costarr;
  int ntot = 0;
  for (int i = 0; i < ns; i++, pc++) {
    if (*pc >= COST_OBS) {
      ntot++;  // number of cells that are obstacles  计算有多少个障碍物点
    }
  }
  nobs = ntot;
}

bool
NavFn::propNavFnAstar(int cycles)
{
  int nwv = 0;  // max priority block size
  int nc = 0;  // number of cells put into priority blocks
  int cycle = 0;  // which cycle we're on

  // set initial threshold, based on distance
  float dist = hypot(goal[0] - start[0], goal[1] - start[1]) * static_cast<float>(COST_NEUTRAL);
  curT = dist + curT;

  // set up start cell
  int startCell = start[1] * nx + start[0];

  // do main cycle
  for (; cycle < cycles; cycle++) {  // go for this many cycles, unless interrupted
    if (curPe == 0 && nextPe == 0) {  // priority blocks empty  //相当于 overPe ==0 && nextPe == 0  找不到路了
      break;
    }

    // stats
    nc += curPe;    //经过上面运算curPe =4，
    if (curPe > nwv) {
      nwv = curPe;
    }

    // reset pending flags on current priority buffer
    int * pb = curP;   curp有四个数据 {curP[curPe++] = n  curP[1] = k+1,k是目标点检索号
    int i = curPe;   //经过上面运算curPe =4
    while (i-- > 0) {
      pending[*(pb++)] = false;
    }

    // process current priority buffer
    pb = curP;  //curp有四个数据 目标点四邻域的检索号
    i = curPe;
    while (i-- > 0) {
      updateCellAstar(*pb++);  //*pb++指向下一个数据,开始时刻指向目标点四邻域的一个数据 ,四邻域的点potential都被更新 ,nextP也被更新，即找到新的四邻域点
    }

    // if (displayInt > 0 && (cycle % displayInt) == 0) {
    //   displayFn(this);
    // }

    // swap priority blocks curP <=> nextP
    curPe = nextPe;    //Nextpe在 {nextP[nextPe++] = n; pending[n] = true;}} 被更新,即上面的updateCellAstar
    nextPe = 0;     //每次循环都把nextPe清0
    pb = curP;  // swap buffers  curP<==>nextP
    curP = nextP; //nextP在上面updateCellAstar被更新，即找到新的四邻域点
    nextP = pb;

    // see if we're done with this priority level
    if (curPe == 0) {   //如果某个点的四邻域没有nexp
      curT += priInc;  // increment priority threshold priInc = 100
      curPe = overPe;  // set current to overflow block
      overPe = 0;
      pb = curP;  // swap buffers  curP<==>overP
      curP = overP;
      overP = pb;
    }

    // check if we've hit the Start cell
    if (potarr[startCell] < POT_HIGH) {   ///目标点的potarr是最小的0，从目标点开始寻找，找到起始点，每个找过的点都会更新potarr,但是都小于POT_HIGH，直到找到起始点，并设置起始点potarr为止
      break;
    }
  }

  last_path_cost_ = potarr[startCell];

  RCLCPP_DEBUG(
    rclcpp::get_logger("rclcpp"),
    "[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n",
    cycle, nc, (int)((nc * 100.0) / (ns - nobs)), nwv);

  if (potarr[startCell] < POT_HIGH) { //从目标点开始更新potarr,直到更新到起始点，也就是找到起始点了，返回
    return true;  // finished up here}
  } else {
    return false;
  }
}

inline void
NavFn::updateCellAstar(int n)
{
  // get neighbors
  float u, d, l, r;
  l = potarr[n - 1];  //potarr越大，离障碍物越近
  r = potarr[n + 1];
  u = potarr[n - nx];
  d = potarr[n + nx];
  // ROS_INFO("[Update] c: %0.1f  l: %0.1f  r: %0.1f  u: %0.1f  d: %0.1f\n",
  // potarr[n], l, r, u, d);
  // ROS_INFO("[Update] cost of %d: %d\n", n, costarr[n]);

  // find lowest, and its lowest neighbor 最小的potarr是上一个n点，刚开始当然是goal点
  float ta, tc;
  if (l < r) {tc = l;} else {tc = r;}
  if (u < d) {ta = u;} else {ta = d;}

  // do planar wave update
  if (costarr[n] < COST_OBS) {  // don't propagate into obstacles
    float hf = static_cast<float>(costarr[n]);  // traversability factor
    float dc = tc - ta;  // relative cost between ta,tc
    if (dc < 0) {  // ta is lowest ta是四邻域种potential值最小的一个
      dc = -dc;
      ta = tc;             //dc是“左右邻点最小pot值与上下邻点最小pot值之差的绝对值”
    }

    // calculate new potential
    float pot;
    if (dc >= hf) {  // if too large, use ta-only update
      pot = ta + hf;    //四邻域中的潜在值+当前点的带价值
    } else {  // two-neighbor interpolation update
      // use quadratic approximation
      // might speed this up through table lookup, but still have to
      //   do the divide
      float d = dc / hf;
      float v = -0.2301 * d * d + 0.5307 * d + 0.7040;  //最小二乘法求potential，圆形向外传播找路径点
      pot = ta + hf * v;
    }

    // ROS_INFO("[Update] new pot: %d\n", costarr[n]);

    // now add affected neighbors to priority blocks
    //四邻域中的潜在值+当前点的带价值 < 当前点的潜在值   第一次搜四邻域时搜到了起始点，起始点potarr = 0 小于 e10, 所以将当前点（即起始点周围的四邻域点）设置为0+cost值，也即起始点周围的四邻域点被设置为cost值
    //第二次搜索的时候n为起始点四邻域的四邻域点，所以potentil[n] = e10,搜到的最小值为起始点的四邻域,即上一个n值,为0+cost,满足pot < potarr[n],此处为广度有先搜索
    //potarr中记录了节点的权重和，此处计算到起始点为后已经生成一条路径
    if (pot < potarr[n]) {   
      float le = INVSQRT2 * static_cast<float>(costarr[n - 1]);
      float re = INVSQRT2 * static_cast<float>(costarr[n + 1]);
      float ue = INVSQRT2 * static_cast<float>(costarr[n - nx]);
      float de = INVSQRT2 * static_cast<float>(costarr[n + nx]);

      // calculate distance
      int x = n % nx;
      int y = n / nx;
      float dist = hypot(x - start[0], y - start[1]) * static_cast<float>(COST_NEUTRAL);
        
      potarr[n] = pot;  //将当前计算的四邻域potential+当前点cost值赋给当前点的潜在值
      pot += dist;       //pot = 将当前计算的四邻域potential+当前点cost值 + 到目标点start的欧氏距离     setStart(map_goal);这个start就是目标点
      if (pot < curT) {  // low-cost buffer block  curt = 50(代价地图的空旷值) * 目标点与起始点的欧式距离 + 254(代价地图的空旷值 代价地图的障碍物值),这里是启发式搜索的关键，离目标点越近越优先
        if (l > pot + le) {push_next(n - 1);}   //把某个点n的四邻域点作为nexp//l潜在值 > pot + 0.707*代价  
        if (r > pot + re) {push_next(n + 1);}       //r > pot + re,l > pot + le 防止一下加入的点为上一个n点，因为上一个n点的potarr小于当前potarr
        if (u > pot + ue) {push_next(n - nx);}   // nextP[nextPe++] = n-nx; pending[n-nx] = true
        if (d > pot + de) {push_next(n + nx);}
      } else {
        if (l > pot + le) {push_over(n - 1);}  //这里是pot超过了欧式距离的值*50(代价地图的空旷值)的情况
        if (r > pot + re) {push_over(n + 1);}
        if (u > pot + ue) {push_over(n - nx);}
        if (d > pot + de) {push_over(n + nx);}
      }
    }
  }
}

navfn_planner.cpp350行获取路径点

int
NavFn::calcPath(int n, int * st)
{
  // test write
  // savemap("test");

  // check path arrays
  if (npathbuf < n) {
    if (pathx) {delete[] pathx;}
    if (pathy) {delete[] pathy;}
    pathx = new float[n]; //初始化路径容器
    pathy = new float[n];
    npathbuf = n;
  }

  // set up start position at cell
  // st is always upper left corner for 4-point bilinear interpolation4点双线性插值
  if (st == NULL) {st = start;}
  int stc = st[1] * nx + st[0];  //起始点的检索号,navfn.cpp 中的start 就是navfn_planner.cpp中的map_goal

  // set up offset
  float dx = 0;  
  float dy = 0;
  npath = 0;

  // go for <n> cycles at most
  for (int i = 0; i < n; i++) {
    // check if near goal
    int nearest_point = std::max(
      0,
      std::min(
        nx * ny - 1, stc + static_cast<int>(round(dx)) +     //dx+nx*dy 求最小路径下的x轴平移和y轴平移的距离和   +stc就是目标点的检索号
        static_cast<int>(nx * round(dy))));
    if (potarr[nearest_point] < COST_NEUTRAL) {                //到达目标点了
      pathx[npath] = static_cast<float>(goal[0]);
      pathy[npath] = static_cast<float>(goal[1]);
      return ++npath;  // done!
    }

    if (stc < nx || stc > ns - nx) {  // would be out of bounds//在第一行或最后一行，即超出边界
      RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "[PathCalc] Out of bounds");
      return 0;
    }

    // add to path
    pathx[npath] = stc % nx + dx;
    pathy[npath] = stc / nx + dy;
    npath++;

    震荡检测，某一步和上上一步的位置是否一样
    bool oscillation_detected = false;
    if (npath > 2 &&
      pathx[npath - 1] == pathx[npath - 3] &&  
      pathy[npath - 1] == pathy[npath - 3])
    {
      RCLCPP_DEBUG(
        rclcpp::get_logger("rclcpp"),
        "[PathCalc] oscillation detected, attempting fix.");
      oscillation_detected = true;
    }

    int stcnx = stc + nx;    //坐标点的下面一个坐标
    int stcpx = stc - nx;  //坐标点的上面一个坐标

    // check for potentials at eight positions near cell//检查当前到达节点的周边的8个节点是否有障碍物代价值，如果有的话，则直接将stc指向这8个节点中potential值最低的节点
    if (potarr[stc] >= POT_HIGH ||
      potarr[stc + 1] >= POT_HIGH ||
      potarr[stc - 1] >= POT_HIGH ||
      potarr[stcnx] >= POT_HIGH ||
      potarr[stcnx + 1] >= POT_HIGH ||
      potarr[stcnx - 1] >= POT_HIGH ||
      potarr[stcpx] >= POT_HIGH ||
      potarr[stcpx + 1] >= POT_HIGH ||
      potarr[stcpx - 1] >= POT_HIGH ||
      oscillation_detected)
    {
      RCLCPP_DEBUG(
        rclcpp::get_logger("rclcpp"),
        "[Path] Pot fn boundary, following grid (%0.1f/%d)", potarr[stc], npath);

      // check eight neighbors to find the lowest 检查八邻域找最小的potential值
      int minc = stc;
      int minp = potarr[stc];
      int st = stcpx - 1;
      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}  //左下角的值最小，就把左下角的检索赋给minc ,minc赋给stc
      st++;
      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}  
      st++;
      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
      st = stc - 1;
      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
      st = stc + 1;
      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
      st = stcnx - 1;
      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
      st++;
      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
      st++;
      if (potarr[st] < minp) {minp = potarr[st]; minc = st;}
      stc = minc;
      dx = 0;
      dy = 0;

      RCLCPP_DEBUG(
        rclcpp::get_logger("rclcpp"), "[Path] Pot: %0.1f  pos: %0.1f,%0.1f",
        potarr[stc], pathx[npath - 1], pathy[npath - 1]);

      if (potarr[stc] >= POT_HIGH) {       //八邻域里面最小的都大于POT_HIGH，没路可走了
        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "[PathCalc] No path found, high potential");
        // savemap("navfn_highpot");
        return 0;
      }
    } else {  // have a good gradient here
      // get grad at four positions near cell
      gradCell(stc);  //传入的是当前坐标和右边坐标，下方坐标和右下坐标,//potarr中记录了节点的权重和，计算到起始点为后已经生成一条路径,按权重值判断即可得到一条路线
      gradCell(stc + 1);  
      gradCell(stcnx);
      gradCell(stcnx + 1);


      // get interpolated gradient//这一块的算法连同下面的 check for overflow我也没看懂，希望有大神给我指点指点
      float x1 = (1.0 - dx) * gradx[stc] + dx * gradx[stc + 1];   //gradx[n] = norm * dx;
      float x2 = (1.0 - dx) * gradx[stcnx] + dx * gradx[stcnx + 1];
      float x = (1.0 - dy) * x1 + dy * x2;  // interpolated x
      float y1 = (1.0 - dx) * grady[stc] + dx * grady[stc + 1];
      float y2 = (1.0 - dx) * grady[stcnx] + dx * grady[stcnx + 1];
      float y = (1.0 - dy) * y1 + dy * y2;  // interpolated y

#if 0
      // show gradients
      RCLCPP_DEBUG(
        rclcpp::get_logger("rclcpp"),
        "[Path] %0.2f,%0.2f  %0.2f,%0.2f  %0.2f,%0.2f  %0.2f,%0.2f; final x=%.3f, y=%.3f\n",
        gradx[stc], grady[stc], gradx[stc + 1], grady[stc + 1],
        gradx[stcnx], grady[stcnx], gradx[stcnx + 1], grady[stcnx + 1],
        x, y);
#endif

      // check for zero gradient, failed
      if (x == 0.0 && y == 0.0) {
        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "[PathCalc] Zero gradient");
        return 0;
      }

      // move in the right direction
      float ss = pathStep / hypot(x, y);
      dx += x * ss;
      dy += y * ss;

      // check for overflow
      if (dx > 1.0) {stc++; dx -= 1.0;}  //这里算出来dx，dy 决定梯度的方向，和路径点的位置 ，以及路径点的小数位
      if (dx < -1.0) {stc--; dx += 1.0;}
      if (dy > 1.0) {stc += nx; dy -= 1.0;}
      if (dy < -1.0) {stc -= nx; dy += 1.0;}
    }

    //      ROS_INFO("[Path] Pot: %0.1f  grad: %0.1f,%0.1f  pos: %0.1f,%0.1f\n",
    //      potarr[stc], x, y, pathx[npath-1], pathy[npath-1]);
  }

  //  return npath;  // out of cycles, return failure
  RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "[PathCalc] No path found, path too long");
  // savemap("navfn_pathlong");
  return 0;  // out of cycles, return failure
}

参考文献：(128条消息) 【ROS-Navigation】NavFn全局规划源码解读-2_BRAND-NEO的博客-CSDN博客_pot_high

(130条消息) ROS学习笔记之——路径规划及avoid obstacles_gwpscut的博客-CSDN博客_ros学习笔记之 路径规划

(130条消息) navigation路径规划器navfn原理详解_chennuo0125-HIT的博客-CSDN博客_navfn