navigation路径规划器navfn原理详解

navfn综述

navfn算法首先根据代价地图生成一个加权无向图，代价地图中的每个栅格代表加权无向图的每个节点，每个栅格的代价值构成加权无向图边的权重．然后根据加权无向图，利用dijkstra算法从起点开始传播到目标点，获取传播过程中到各个节点的最优行走代价值．最后从目标点开始，沿着最优行走代价值梯度下降的方向寻找到起点的最优轨迹．其主要过程如下：
　　（１）NavFn::setCostmap(const COSTTYPE *cmap, bool isROS, bool allow_unknown)
　　　　（设置代价地图：将代价地图转换成加权无向图）
　　（２）NavFn::setupNavFn(bool keepit)
　　　　（设置potential数组：对于每个新的规划，需要重新设置potential数组为默认值）
　　　　注：potential数组的值即为起点到各个节点的最优行走代价值
　　（３）NavFn::propNavFnDijkstra(int cycles, bool atStart)
　　　　（dijkstra传播函数：通过dijkstra算法以广度优先的方式更新potential数组）
　　（４）NavFn::calcPath(int n, int *st)
　　　　（计算轨迹：从目标点开始沿着最优行走代价值梯度下降的方向寻找到起点的最优轨迹）

（１）设置代价地图

　   //根据代价值生成无向权重图
    NavFn::setCostmap(const COSTTYPE *cmap, bool isROS, bool allow_unknown)
    {
      COSTTYPE *cm = costarr;
      for (int i=0; i<ny; i++)
      {
        int k=i*nx;
        for (int j=0; j<nx; j++, k++, cmap++, cm++)
        {
          *cm = COST_OBS;
          int v = *cmap;//cmap数组为各个节点的权重值
          if (v < COST_OBS_ROS)
          {
          //默认最小权重值为COST_NEUTRAL＝50，最大权重值为COST_OBS＝254
          //注：最小权重值即行走单个free(无障碍物影响)栅格所付出的权重代价  
          　　最大权重值即行走单个障碍物栅格所付出的权重代价
            v = COST_NEUTRAL+COST_FACTOR*v;
            if (v >= COST_OBS)
              v = COST_OBS-1;
            *cm = v;
          }
          else if(v == COST_UNKNOWN_ROS && allow_unknown)
          {
            v = COST_OBS-1;
            *cm = v;
          }
        }
      }
    }

（２）设置potential数组

　 //初始化最优行走代价值表
   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;//顺带着初始化x,y方向的梯度表，在第四步的时候需要
     }
     // outer bounds of cost array
     //将无向权重图最外围节点的权重值置为COST_OBS＝254，即障碍物代价值，目的是将地图周围封闭起来， 
     //防止轨迹规划出边界
     COSTTYPE *pc;
     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; //当前用于传播的节点索引数组
     curPe = 0;//当前用于传播的节点的数量
     nextP = pb2;//用于下个传播过程的节点索引数组
     nextPe = 0;//用于下个传播过程的节点的数量
     overP = pb3;//传播界限外的节点索引数组
     overPe = 0;//传播界限外的节点的数量
     memset(pending, 0, ns*sizeof(bool));//设置所有的节点状态都为非"等待状态"
     // set goal//设置目标点
     int k = goal[0] + goal[1]*nx;
     //初始化代价：将起始节点所在的potential值（行走代价值）置为0，并且将上下左右四个栅格节点放入curP，
     //将状态置为"等待状态"
     initCost(k,0);
     // 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;
   }

（３）dijkstra传播

    //dijkstra算法广度优先传播更新potential数组，即可获得起点到传播过程中任意点的最优行走代价值．
    NavFn::propNavFnDijkstra(int cycles, bool atStart)	
   {
     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 up start cell
     int startCell = start[1]*nx + start[0];
     //此处cycles大小不用太纠结，设置足够大就行，能保证传播到目标点，到了目标点之后，会自动break掉
     for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted
     {
       // 如果当前用于传播的节点的数量和接下来用于传播的节点的数量都为０，则直接退出传播过程，表示传播不下去了
       if (curPe == 0 && nextPe == 0) // priority blocks empty
         break;
       // stats
       nc += curPe;
       if (curPe > nwv)
         nwv = curPe;
       // reset pending flags on current priority buffer
       // 传播当前节点，并将当前节点上下左右四个节点保存到nextP数组当中，当当前节点传播完之后，将nextP数组中的
       // 节点数组传递给当前节点数组curP，又继续开始传播当前节点，循环往复．
       int *pb = curP;
       int i = curPe;			
       while (i-- > 0)		
         pending[*(pb++)] = false;
       // process current priority buffer
       pb = curP; 
       i = curPe;
       while (i-- > 0)		
         updateCell(*pb++);
       if (displayInt > 0 &&  (cycle % displayInt) == 0)
         displayFn(this);
       // swap priority blocks curP <=> nextP
       curPe = nextPe;
       nextPe = 0;
       pb = curP;		// swap buffers
       curP = nextP;
       nextP = pb;
       // see if we're done with this priority level
       // 当当前节点数组中节点个数为0时，增加传播界限阈值，并将overP传递给curP，然后再重复上面的过程
       if (curPe == 0)
       {
         curT += priInc;	// increment priority threshold
         curPe = overPe;	// set current to overflow block
         overPe = 0;
         pb = curP;		// swap buffers
         curP = overP;
         overP = pb;
       }
       // check if we've hit the Start cell
       //　当目标点的行走代价值小于最大默认值时，即说明已经传播到了目标点，此时就可以结束传播过程．
       if (atStart)
         if (potarr[startCell] < POT_HIGH)
           break;
     }
     if (cycle < cycles) return true; // finished up here
     else return false;
   }

上述过程有几个疑点：怎样生成nextP overP？ 传播界限是什么？　如何计算栅格的最优行走代价值？
这些疑惑的解释都在下面的函数中：

// 更新节点
NavFn::updateCell(int n)
   {
     // get neighbors
     // 获取上下左右四个栅格节点的行走代价值
     float u,d,l,r;
     l = potarr[n-1];
     r = potarr[n+1];		
     u = potarr[n-nx];
     d = potarr[n+nx];
     //	 potarr[n], l, r, u, d);
     // find lowest, and its lowest neighbor
     // 寻找上下和左右两组行走代价值的最低值
     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
     {
       // 此处为计算当前节点potential值的过程，我试着猜测 float v = -0.2301*d*d + 0.5307*d + 0.7040 这个公式的含义，但
       是都没成功，后来寻求到了作者的解释：https://github.com/ros-planning/navigation/issues/890
       float hf = (float)costarr[n]; // traversability factor
       float dc = tc-ta;		// relative cost between ta,tc
       if (dc < 0) 		// ta is lowest
       {
         dc = -dc;
         ta = tc;
       }
       // 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;
         pot = ta + hf*v;
       }
       // now add affected neighbors to priority blocks
       //判断当前节点计算的potential值是否小于本身的potential值，只有当小于时，才更新当前栅格节点的potential值
       if (pot < potarr[n])
       {
        // 计算当前节点上下左右四个节点的权重值
         float le = INVSQRT2*(float)costarr[n-1];
         float re = INVSQRT2*(float)costarr[n+1];
         float ue = INVSQRT2*(float)costarr[n-nx];
         float de = INVSQRT2*(float)costarr[n+nx];
         potarr[n] = pot;
         // 当当前节点的potential值小于传播界限时，将上下左右四个节点传递给nextP
         if (pot < curT)	// low-cost buffer block 
         {
           // 如果上下左右四个节点的权重值加上当前节点的potential值小于这四个节点的potential值时，才加入nexP数组进
           行这四个节点potential值的更新，否则没有必要更新
           if (l > pot+le) push_next(n-1);
           if (r > pot+re) push_next(n+1);
           if (u > pot+ue) push_next(n-nx);
           if (d > pot+de) push_next(n+nx);
         }
         // 否则传递给overP
         else			// overflow block
         {
           if (l > pot+le) push_over(n-1);
           if (r > pot+re) push_over(n+1);
           if (u > pot+ue) push_over(n-nx);
           if (d > pot+de) push_over(n+nx);
         }
       }
     }
   }

下面通过实验来说说 v = -0.2301dd + 0.5307d + 0.7040 pot = ta + hfv 这种计算方法的作用
当直接利用 pot = ta+hf 计算，获得的potential分布图如下:
当利用 v = -0.2301dd + 0.5307d + 0.7040 , pot = ta + hfv 计算，获得的potential分布图如下：

从对比结果可以看出来，利用 v = -0.2301dd + 0.5307d + 0.7040 , pot = ta + hfv公式计算获得的potential分布是以圆的形式向外传播，而直接利用pot = ta+hf这个公式计算获得的potential分布是以方形的形式向外传播，明显以圆的形式向外传播的这种分布方式更结合实际．

再通过实验说明一下传播界限的作用：
当未设置传播界限时：

当设置传播界限时：

从对比结果可以看出，当设置传播界限时，传播方式是以potential值为边界一步步向外传播，而不设置传播界限时，就是以固定的方形传播方式往外传播，会增加很多额外的不必要传播．所以设置边界能提高传播速度．

（４）计算轨迹

// 通过potential的分布，从目标点开始沿着梯度下降的方向搜索到起点
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 interpolation 
      if (st == NULL) st = start;
      int stc = st[1]*nx + st[0];//stc指向当前到达的栅格节点
      // 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+(int)round(dx)+(int)(nx*round(dy))));
        if (potarr[nearest_point] < COST_NEUTRAL)
        {
          pathx[npath] = (float)goal[0];
          pathy[npath] = (float)goal[1];
          return ++npath;	// done!
        }
        if (stc < nx || stc > ns-nx) // would be out of bounds
        {
          ROS_DEBUG("[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] )
        {
          ROS_DEBUG("[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)
        {
          ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potarr[stc], npath);
          // check eight neighbors to find the lowest
          int minc = stc;
          int minp = potarr[stc];
          int st = stcpx - 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; }
          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;
          ROS_DEBUG("[Path] Pot: %0.1f  pos: %0.1f,%0.1f",
              potarr[stc], pathx[npath-1], pathy[npath-1]);
          if (potarr[stc] >= POT_HIGH)
          {
            ROS_DEBUG("[PathCalc] No path found, high potential");
            //savemap("navfn_highpot");
            return 0;
          }
        }
        // have a good gradient here
        // 如果有好的梯度，则直接计算梯度，并沿着梯度方向查找下一个节点
        else			
        {
          // get grad at four positions near cell
          gradCell(stc);// 计算当前节点梯度
          gradCell(stc+1);// 计算右边相邻节点梯度
          gradCell(stcnx);//计算上边相邻节点梯度
          gradCell(stcnx+1);//计算右上边节点梯度
          // get interpolated gradient
          // 插值当前节点的梯度
          float x1 = (1.0-dx)*gradx[stc] + dx*gradx[stc+1];
          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
          // check for zero gradient, failed
          if (x == 0.0 && y == 0.0)
          {
            ROS_DEBUG("[PathCalc] Zero gradient");	  
            return 0;
          }
          //向梯度方向移动
          float ss = pathStep/hypot(x, y);
          dx += x*ss;
          dy += y*ss;
          // check for overflow
          if (dx > 1.0) { stc++; dx -= 1.0; }
          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; }
        }
      }
      return 0;			// out of cycles, return failure
    }