ROS的导航功能包navfn中计算单元格的势值的原理介绍

一、前言

   ROS的导航功能包中，自带的全局规划功能包主要有global_planner和navfn，由于普遍认为global_planner是对navfn重构和扩展，且global_planner中的A * 算法的实现又较为简单，只用了两个很简单且很容易理解的函数，经过多种不同环境的实验测试，规划性能，稳定性都较为出色，是一个很好的基础算法，再加上从原理上，当A*算法的h值权重取为0时，便退化成了Dijkstra算法，所以对包中带的Dijkstra算法之前没有怎么关注过，最近看navfn中Dijkstra算法的实现思路时发现与A * 的思路差别还是很大的，所以就仔细看了一下源码

   源码中其他的函数相对来说比较容易理解，但是NavFn::updateCell(int n)函数中计算节点的势力值的相关部分，第一次看确实很难理解，尤其是多项式v = -0.2301 * d * d + 0.5307 * d + 0.7040的含义，看的一头雾水，所以就仔细研究了一下这部分内容，同时也找到了原作者对该多项式含义的英文解释，链接如下：

   https://github.com/locusrobotics/robot_navigation/blob/master/dlux_global_planner/README.md

   根据我个人对该英文文档中相关内容的理解，结合源码中的NavFn::updateCell(int n)函数的相关部分，接下来，本文对将该部分内容的原理展开较为详细的介绍

二、单元格势值的核心计算原理介绍

   该部分的核心是基于其相邻单元格的势值来计算当前单元格的势值。最简单的方法是将相邻单元格的势值加上移动到该单元格的代价（即中立代价加上代价地图中存储的当前栅格的代价值）来确定势值。然而，这种方法并没有利用网格的二维结构。如果两个相邻单元格的势值已经被计算出来，我们可能希望将它们结合起来以获得更好的势值。

   navfn中该部分内容的原理源自 Roland Philippsen 的论文 A Light Formulation of the E Interpolated Path Replanner。接下来对多项式v = - 0.2301 * d * d + 0.5307 * d + 0.7040的含义和由来进行简要数学推导：

   上述过程对应于NavFn::updateCell(int n)函数的部分如下：

   （1）获取上下左右四个邻居的当前势值，并确定每个维度上的较小值（如上面假设中的P(A)和P( C ) ）存放在ta、tc中,然后确定ta和tc中较小的那个（如上面假设中的ta，即假设P(A)和 < P( C )）存放在ta中，并计算他俩非差值存放在dc中

     // 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
     {
       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;
       }

   （2）如果差值较大，则采用上述的简单的更新公式： $P(X)=P(A)+h$，否则，则采用上面推导出近似公式 $P(X)\approx P(A)+h(c_2{\delta }^2+c_1\delta +c_0)$来计算P(X)，本文开头处说的v = -0.2301 * d * d + 0.5307 * d + 0.7040也就是该近似公式的二次多项式部分

       // 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;
       }

三、如何理解 $(P(X)-P(A){)}^2+(P(X)-P(C){)}^2=h^2$ ？

   1. 背景介绍

  在路径规划中，势值（Potential）表示从某个点到达目标的代价或距离。为了估计网格中某个单元格 ( X ) 的势值 ( P(X) )，我们可以利用相邻单元格（例如 ( A ) 和 ( C )）的势值来进行插值。这个方程反映的是在二维网格中，根据相邻单元格 ( A ) 和 ( C ) 的势值来计算单元格 ( X ) 的势值。

  我们希望通过相邻单元格 ( A ) 和 ( C ) 的已知势值，计算出目标单元格 ( X ) 的势值。方程的目的是寻找 ( P(X) )，即单元格 ( X ) 的潜在值，它结合了来自相邻单元格的信息，同时考虑了“移动代价” ( h )。

   2. 方程的物理意义

  方程：
$$(P(X)-P(A){)}^2+(P(X)-P(C){)}^2=h^2$$
  类似于二维空间中的距离公式，它表示的是从单元格 ( A ) 和 ( C ) 到 ( X ) 的距离。具体来说：

• ( P(X) ) 是我们要计算的单元格 ( X ) 的势值。
• ( P(A) ) 和 ( P( C ) ) 分别是相邻单元格 ( A ) 和 ( C ) 的势值。
• ( h ) 表示移动到单元格 ( X ) 的代价（来自代价地图的值，也可以理解为步长或网格距离）。

   3. 几何解释

  这个方程的几何含义可以理解为，在一个二维平面上，单元格 ( A ) 和 ( C ) 形成的势值梯度会影响单元格 ( X ) 的势值。方程类似于圆的方程，它说明了从 ( P(A) ) 和 ( P( C ) ) 到 ( P(X) ) 的“距离”(势值差)是一个固定值 ( h )。

  具体来看，方程的结构类似于圆的方程：
$$(x-x_0{)}^2+(y-y_0{)}^2=r^2$$
  这里 ( r ) 是圆的半径，( x_0 ) 和 ( y_0 ) 是圆心的坐标。在势值计算中，我们的“圆”并不是真实空间中的几何圆，而是在势值空间中，表示两个已知势值 ( P(A) ) 和 ( P( C ) ) 对单元格 ( X ) 的影响。

  方程的核心思想是：我们想找到一个 ( P(X) )，使得它与 ( P(A) ) 和 ( P( C ) ) 的“距离”（通过势值差异表示）符合特定的代价 ( h )。也就是说，单元格 ( X ) 的势值 ( P(X) ) 同时受到 ( A ) 和 ( C ) 的影响，而不是单独依赖其中一个。

   我们可以把 ( P(A) ) 和 ( P( C ) ) 看作是两个已知点，它们的值代表已经计算好的势值。我们希望找到一个合适的 ( P(X) )，让它与 ( P(A) ) 和 ( P© ) 之间的距离（基于势值）符合移动的代价 ( h ) 的关系。

   4. 求解

  根据这个方程，( P(X) ) 的解会让单元格 ( X ) 的势值既受 ( A ) 单元格的影响，也受 ( C ) 单元格的影响。这种方法比仅仅使用一个相邻单元格的势值（例如 ( P(A) ) 或 ( P( C ) ) 之一）更加精确，因为它结合了两个方向上的信息。因此，这个方程的目的是通过结合两个相邻单元格的势值来更准确地计算某个单元格的势值。

–

四、添加详细注释的完整NavFn::updateCell(int n)函数

–

// 函数作用：更新指定单元格的势值，并根据该势值影响其相邻单元格的路径规划。
// 参数：
//   n - 当前栅格节点的索引。
// 该函数的主要工作是根据相邻节点的势值计算并更新当前节点的势值，同时将影响到的相邻节点加入到下一步的优先级队列中进行更新。

NavFn::updateCell(int n)
{
    // 获取邻居节点的势值
    // 分别获取左右和上下四个相邻节点的潜在代价值（势值）
    float u, d, l, r;
    l = potarr[n-1];       // 左侧相邻节点的势值
    r = potarr[n+1];       // 右侧相邻节点的势值
    u = potarr[n-nx];      // 上方相邻节点的势值
    d = potarr[n+nx];      // 下方相邻节点的势值
    
    // 查找左右和上下两组邻居节点中较小的势值
    float ta, tc;
    if (l < r) tc = l; else tc = r;   // 左右中取较小的势值
    if (u < d) ta = u; else ta = d;   // 上下中取较小的势值
    
    // 如果当前节点不是障碍物，则更新其势值
    if (costarr[n] < COST_OBS) // 如果当前节点是障碍物，则不更新其势值，跳过传播
    {
        float hf = (float)costarr[n]; // 获取当前节点的通行代价因子
        float dc = tc - ta;           // 计算两个较小邻居势值的相对代价差异
        
        // 如果 tc 小于 ta，则交换它们，使 ta 为较小的势值
        if (dc < 0) 
        {
            dc = -dc;  // 将 dc 取正
            ta = tc;   // 更新 ta 为较小的势值
        }
        
        // 计算当前节点的新的潜在代价（势值）
        float pot;
        if (dc >= hf)   // 如果势值差异过大，使用单邻居更新
            pot = ta + hf;   // 使用 ta 进行直接更新
        else              // 如果差异较小，使用双邻居插值更新
        {
            // 使用二次插值近似公式计算新的潜在代价
            float d = dc / hf;   // 归一化的代价差异
            // 使用已知的插值公式计算近似值
            float v = -0.2301 * d * d + 0.5307 * d + 0.7040;
            pot = ta + hf * v;   // 计算新的势值
        }
        
        // 判断新计算出的势值是否比当前势值更优（更小）
        if (pot < potarr[n]) // 只有当新势值小于当前节点的势值时，才更新该节点
        {
            // 计算相邻节点的通行代价，使用对角线方向的代价因子 INVSQRT2
            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;  // 更新当前节点的势值
            
            // 如果新势值小于低成本缓冲区的阈值 curT，则将相邻节点加入 nextP 列表
            if (pot < curT) // 低成本缓冲区块
            {
                // 判断相邻节点是否需要更新，如果需要则将它们加入 nextP 队列
                if (l > pot + le) push_next(n-1);  // 如果左侧节点势值需要更新，加入 nextP
                if (r > pot + re) push_next(n+1);  // 如果右侧节点势值需要更新，加入 nextP
                if (u > pot + ue) push_next(n-nx); // 如果上方节点势值需要更新，加入 nextP
                if (d > pot + de) push_next(n+nx); // 如果下方节点势值需要更新，加入 nextP
            }
            // 否则，将相邻节点加入到 overflow 块的 overP 列表
            else   // 溢出块
            {
                if (l > pot + le) push_over(n-1);  // 如果左侧节点需要更新，加入 overP
                if (r > pot + re) push_over(n+1);  // 如果右侧节点需要更新，加入 overP
                if (u > pot + ue) push_over(n-nx); // 如果上方节点需要更新，加入 overP
                if (d > pot + de) push_over(n+nx); // 如果下方节点需要更新，加入 overP
            }
        }
    }
}