机器人规划算法——A*算法

最近在研读ROS中的A*算法,参见了文章

【ROS-Navigation】------ Astar路径规划算法解析

这里作为笔记:

问题1:如何理解如下代码?

cpp 复制代码
float QuadraticCalculator::calculatePotential(float* potential, unsigned char cost, int n, float prev_potential) {
    // get neighbors
    float u, d, l, r;
    l = potential[n - 1];
    r = potential[n + 1];
    u = potential[n - nx_];
    d = potential[n + nx_];
    //  ROS_INFO("[Update] c: %f  l: %f  r: %f  u: %f  d: %f\n",
    //     potential[n], l, r, u, d);
    //  ROS_INFO("[Update] cost: %d\n", costs[n]);

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

    float hf = cost; // traversability factor
    float dc = tc - ta;        // relative cost between ta,tc
    if (dc < 0)         // tc is lowest
            {
        dc = -dc;
        ta = tc;
    }

    // calculate new potential
    if (dc >= hf)        // if too large, use ta-only update
        return 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;
        return ta + hf * v;
    }
}

我注意到,在计算相邻点的代价值时,使用 QuadraticCalculator::calculatePotential,生成的路径比较直,而使用PotentialCalculatorr::calculatePotential::calculatePotential计算的路径比较曲折。

这个函数使用了一个基于两个邻居之间相对代价的二维插值方法来计算新的潜在值。具体的算法原理如下:

  1. 首先计算左右邻居和上下邻居的值,分别存储在l、r、u和d中。

  2. 然后比较左右邻居的值,选择较小的作为tc;比较上下邻居的值,选择较小的作为ta。

  3. 接着计算每个邻居之间的相对代价dc,即tc和ta之间的差值。如果tc更小,则将dc取反,并将ta更新为tc。

  4. 根据代价因子hf(在此算法中为cost值)与dc的比较,决定使用单一邻居更新还是两个邻居插值更新。

  5. 如果dc大于等于hf,则直接返回ta加上hf作为新的潜在值。

  6. 如果dc小于hf,则进行两个邻居的插值更新。通过二次近似来计算v值,然后返回ta加上hf乘以v作为新的潜在值。

整个算法利用了两个邻居之间的相对代价来进行潜在值的更新,根据代价因子和相对代价的大小来决定更新策略,从而有效地平衡插值和单一邻居更新的权衡。

相关推荐
一只乔哇噻3 小时前
java后端工程师进修ing(研一版‖day44)
java·开发语言·学习·算法
·云扬·5 小时前
【Leetcode hot 100】101.对称二叉树
算法·leetcode·职场和发展
Greedy Alg10 小时前
LeetCode 142. 环形链表 II
算法
睡不醒的kun10 小时前
leetcode算法刷题的第三十二天
数据结构·c++·算法·leetcode·职场和发展·贪心算法·动态规划
先做个垃圾出来………11 小时前
残差连接的概念与作用
人工智能·算法·机器学习·语言模型·自然语言处理
SuperCandyXu12 小时前
P3205 [HNOI2010] 合唱队-普及+/提高
c++·算法·洛谷
_OP_CHEN12 小时前
数据结构(C语言篇):(十二)实现顺序结构二叉树——堆
c语言·数据结构·算法·二叉树·学习笔记··顺序结构二叉树
Yingjun Mo13 小时前
1. 统计推断-基于神经网络与Langevin扩散的自适应潜变量建模与优化
人工智能·神经网络·算法·机器学习·概率论
zskj_zhyl13 小时前
七彩喜智慧养老:用科技温暖晚年,让关爱永不掉线
人工智能·科技·机器人
地平线开发者13 小时前
征程 6 | 灰度图部署链路介绍
算法·自动驾驶