lol赛事下注>嵌入式技术> > 正文

ROS中导航功能包里路径规划A*算法中步骤和代码详解

2021年09月13日 16:49 ? 次阅读

一、下载编译功能包

?

cd ~/catkin_ws/src
sudo apt-get install https://github.com/ros-planning/navigaTIon
cd ..
catkin_make

二、功能包里涵盖的文件


功能包 功能
acml 定位算法
move_base navigaTIon中最主要的框架
base_local_planner 局部路径规划器
dwa_local_planner dwa算法局部路径规划实现
global_planner 全局路径规划
navfn 全局路径规划,旧版本的,有bug
carrot_planner 一个简单的全局路径规划器
clear_costmap_recovery 清除代价地图的恢复行为
costmap_2d 实现2d代价地图
fake_localizaTIon acml的API接口
map_server 提供地图数据,yaml或者是image
move_slow_and_clear 缓慢移动修复机制,会限制机器人速度
nav_core 路径规划接口类
rotate_recovery 旋转恢复
voxel_grid 三维代价地图
全局规划器有 3 个:
(1)carrot_planner

carrot_planner 检查需要到达的目标是不是一个障碍物,如果是一个障碍物,它就将目标点替换成一个附近可接近的点。因此,这个模块其实并没有做任何全局规划的工作。在复杂的室内环境中,这个模块并不实用。

(2)navfn

navfn使用 Dijkstra 算法找到最短路径。

(3)global planner

global planner是navfn的升级版。
它相对于navfn增加了更多的选项:支持 A* 算法;可以切换二次近似;切换网格路径;

三、Global Planner 全局路径规划
该文件下的内容:10个头文件,8个源文件

看其中A*算法的文件

先做一个算例,结合算例理解

以下文件会包含其他文件,需要整体看,这里先整理三个文件,其他的慢慢来
1、astar.h

#ifndef _ASTAR_H
#define _ASTAR_H

#include 
#include 
#include 
#include 

namespace global_planner {
class Index {
    public:
        Index(int a, float b) {
            i = a;
            cost = b;
        }
        int i;
        float cost;
};
//Index(i,cost)对应的点及代价

struct greater1 {
        bool operator()(const Index& a, const Index& b) const {
            return a.cost > b.cost;
        }
};

class AStarExpansion : public Expander {
    public:
        AStarExpansion(PotenTIalCalculator* p_calc, int nx, int ny);
        bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, float* potential);
    private:
        void add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x, int end_y);
        std::vector queue_;
};

} 

?

2、astar.cpp

?

#include
#include

namespace global_planner {

AStarExpansion::AStarExpansion(PotentialCalculator* p_calc, int xs, int ys) :
        Expander(p_calc, xs, ys) {
}  //命名空间名::标识符名?


/*	
	costs: 地图指针
	cycles:循环次数
*/
bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,int cycles, float* potential)      //传参
 {
    
    //queue_为启发式搜索到的向量队列:
     queue_.clear();  //清空函数:将队列清空
    
    //将起点放入队列
    int start_i = toIndex(start_x, start_y);
    //(1,2)
  
    //push_back:向数据结构中添加元素
    queue_.push_back(Index(start_i, 0));  
    //首先将start点以及其代价加入,即(13,0) 
	
	//std::fill函数的作用是:将一个区间的元素都赋予指定的值,即在(first, last)范围内填充指定值
	//将所有点的potential都设为一个极大值,potential就是估计值g,f=g+h
    std::fill(potential, potential + ns_, POT_HIGH);   //?
     
    //将起点的potential设为0
    potential[start_i] = 0;

    //终点对应的序号
    int goal_i = toIndex(end_x, end_y);//(4,4)
    int cycle = 0;
    
    //进入循环,继续循环的判断条件为只要队列大小大于0且循环次数小于cycles 
	//代码中cycles = 2 *nx * ny, 即为所有格子数的2倍  //?
	//目的:得到最小cost的索引,并删除它,如果索引指向goal(目的地)则退出算法,返回true
    while (queue_.size() > 0 && cycle < cycles) 
    {
        Index top = queue_[0];
        
        ///将首元素放到最后,其他元素按照Cost值从小到大排列
		//pop_heap() 是将堆顶元素与最后一个元素交换位置,之后用pop_back将最后一个元素删除
		//greater1()是按小顶堆
        std::pop_heap(queue_.begin(), queue_.end(), greater1());
        queue_.pop_back();

        //如果到了目标点,就结束了
        int i = top.i;
        if (i == goal_i) 
            return true;

       // 对前后左右四个点执行add函数,将代价最小点i周围点加入搜索队里并更新代价值
        add(costs, potential, potential[i], i + 1, end_x, end_y);
        add(costs, potential, potential[i], i - 1, end_x, end_y);
        add(costs, potential, potential[i], i + nx_, end_x, end_y);
        add(costs, potential, potential[i], i - nx_, end_x, end_y);

        cycle++;
    }

    return false;
}

/*add函数:添加点并更新代价函数;
如果是已经添加的点则忽略,根据costmap的值如果是障碍物的点也忽略。
potential[next_i]是起点到当前点的cost即g(n), 
distance * neutral_cost_是当前点到目的点的cost即h(n)。
f(n) = g(n) + h(n)
*/

// potential 存储所有点的g(n),即从初始点到节点n的实际代价
// prev_potentia 当前点的f
void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,int end_y) 
{

    //next_i 点不在网格内,忽略
    if (next_i < 0 || next_i >= ns_)  //ns_?
        return;

    //未搜索的点cost为POT_HIGH,如小于该值,则为已搜索点,跳过;
    //potential[next_i]是起点到当前点的cost即g(n)
    if (potential[next_i] < POT_HIGH)   //POT_HIGH?
        return;
        
    //障碍物点,忽略?
    if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION)) 
        return;
        
    // potential[next_i]是起点到当前点的cost即g(n)
    // potential 存储所有点的g(n),即从初始点到节点n的实际代价
    // prev_potentia   当前点的f
    // =====见下方potential_calculate.cpp文件=====
    potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);

    // 得到在栅格地图中的(x,y)
    int x = next_i % nx_, y = next_i / nx_;  
    //x=14%5=4  y=14/5=2 ?

    //启发式函数:即h(n) 从节点n到目标点最佳路径的估计代价,这里选用了曼哈顿距离
    float distance = abs(end_x - x) + abs(end_y - y);
    // A的i值是13,则add中的next_i分别是12,14,7,19。
    // 以14为例,则x=2, y=2。而B为(4,4)。因此distance = 2+2 =4
    // 由于这只是格子的个数,还有乘上每个格子的真实距离或者是分辨率,所以最后的H = distance *neutral_cost_
    // 因此最后的F = potential[next_i] + distance *neutral_cost_  (F=g+h)
    // 将当前点next_i以及对应的cost加入
    queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
    //potential[next_i]:是起点到当前点的cost即g(n)
    //distance * neutral_cost_:是当前点到目的点的cost即h(n)。
    //f(n)=g(n)+h(n):计算完这两个cost后,加起来即为f(n),将其存入队列中。

    //对加入的再进行堆排序, 把最小代价点放到front队头queue_[0]
    //数据结构?
    std::push_heap(queue_.begin(), queue_.end(), greater1());
}

} 

?

3、potential_calculate.h

neutral_cost_ 设定的一个默认值,为50

calculatePotential()计算根据use_quadratic的值有下面两个选择:
若为TRUE, 则使用二次曲线计算
若为False, 则采用简单方法计算, return prev_potential + cost。即:costs[next_i] + neutral_cost_+ prev_potential
地图代价+单格距离代价(初始化为50)+之前路径代价为G
?

#ifndef _POTENTIAL_CALCULATOR_H
#define _POTENTIAL_CALCULATOR_H

#include 

namespace global_planner {

class PotentialCalculator 
{
    public:
        PotentialCalculator(int nx, int ny) 
        {
            setSize(nx, ny);
        }

        virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1)
        {
            if(prev_potential < 0)
            {
                float min_h = std::min( potential[n - 1], potential[n + 1] ),
                      min_v = std::min( potential[n - nx_], potential[n + nx_]);
                prev_potential = std::min(min_h, min_v);
            }

            return prev_potential + cost;
        }
//在prev_potentia当前点的f不小于0的时候,返回的是prev_potential + cost
//以start_cost为例,最开始给其赋值是0,因此返回就是cost,可以理解为到下一个栅格的距离,或者是分辨率。
//在计算完potential=g值后开始计算h值,即distance,利用的就是移动多少格子,只能上下左右移动.

        virtual void setSize(int nx, int ny) 
        {
            nx_ = nx;
            ny_ = ny;
            ns_ = nx * ny;
        }

    protected:
        inline int toIndex(int x, int y) 
        {
            return x + nx_ * y;
        }

        int nx_, ny_, ns_;
};
} 

?

下载发烧友APP

打造属于您的人脉电子圈

关注电子发烧友微信

有趣有料的资讯及技术干货

关注发烧友课堂

锁定最新课程活动及技术直播

电子发烧友观察

一线报道 · 深度观察 · 最新资讯
收藏 人收藏
分享:

评论

相关推荐

如何对电机控制算法FOC进行调试

什么是FOC? FOC算法的原理是什么? FOC算法的模块是由哪些部分组成的? 如何对FOC算法进行调试? ...
发表于 2021-09-18 09:16? 0次阅读
如何对电机控制算法FOC进行调试

GNSS+IMU+MM车载组合导航系统解决方案

近年来,随着定位业务的迅速发展,用户对于车载端定位精度提出了越来越高的要求,由原来的导航级逐渐更替到....
发表于 2021-09-17 14:40? 114次阅读
GNSS+IMU+MM车载组合导航系统解决方案

算法对比误差公式

算法对比误差公式 D
发表于 2021-09-17 08:38? 0次阅读
算法对比误差公式

Hadoop MapperReduce编程

一、前言 以微博为例,每个用户会发很多微博,其中包含了很多关键词信息。而这些关键词就是用户可能感兴趣的事物。我们...
发表于 2021-09-17 06:51? 0次阅读
Hadoop MapperReduce编程

对CK3M控制卡进行自定义伺服算法二次开发

本文的章节安排如下1 注意事项功能快捷键合理的创建标题,有助于目录的生成如何改变文本的样式插入链接与图片如何插入...
发表于 2021-09-17 06:45? 0次阅读
对CK3M控制卡进行自定义伺服算法二次开发

使用C语言进行PID算法实现

前文对PID算法离散化和增量式PID算法原理进行来探索,之后又使用Matlab进行了仿真实验,对PID三个参数又有了...
发表于 2021-09-16 09:11? 0次阅读
使用C语言进行PID算法实现

使用C语言进行PID算法实现

前文对PID算法离散化和增量式PID算法原理进行来探索,之后又使用Matlab进行了仿真实验,对PID三个参数又有了...
发表于 2021-09-15 09:20? 0次阅读
使用C语言进行PID算法实现

低压直流伺服电机在AGV机器人中的应用

  现在国内知名AGV机器人生产厂家,AGV机器人车辆采用单/双差速驱动磁导航方案。针对此方案,我公....
发表于 2021-09-15 09:16? 23次阅读
低压直流伺服电机在AGV机器人中的应用

ROS小车的制作记录

在寒假期间完成了ROS小车的制作,记录一下,不过也有很多不足,之后再进行修正,本系列文章将分为硬件购买篇、下位...
发表于 2021-09-15 09:12? 0次阅读
ROS小车的制作记录

算法裁员,释放出了哪些“恶之花”?

算法招聘不稀奇,现在利用AI算法来自动化处理海量简历筛选,已经是司空见惯的操作。但算法裁员,对于许多....
发表于 2021-09-15 08:49? 547次阅读
算法裁员,释放出了哪些“恶之花”?

光伏逆变器MPPT算法步长设定

发表于 2021-09-15 07:08? 0次阅读
光伏逆变器MPPT算法步长设定

分解式虚拟模型步态算法VMC介绍

轨迹控制机器人足端曲线使用贝赛尔曲线等轨迹设定,控制方法简单,但是很难适应多变的地形。分解式虚拟模型步态算法V...
发表于 2021-09-15 06:23? 0次阅读
分解式虚拟模型步态算法VMC介绍

DWA算法简介

一、简介DWA算法全称为dynamic window approach,其原理主要是在速度空间(v,w)中采样多组速度,并模拟这些速度在...
发表于 2021-09-15 06:19? 0次阅读
DWA算法简介

增量式PID算法的STM32实现 分析比例、积分...

虽然PID不是什么牛逼的东西,但是真心希望以后刚刚接触这块的人能尽快进入状态。特地分享一些自己如何实....
发表于 2021-09-13 16:37? 687次阅读
增量式PID算法的STM32实现 分析比例、积分...

北鲲云超算平台助推生物制药行业发展

随着科技进步,生物制药正在被视为改写人类命运的关键。北鲲云超算平台蛋白设计助推生物制药行业发展,提供....
发表于 2021-09-10 14:15? 30次阅读
北鲲云超算平台助推生物制药行业发展

卫星通信的常用产品及其特点

Teledyne SP Devices卫星通信促使了全世界导航、气候、移动手机、广播和电视机的发展。
发表于 2021-09-04 17:12? 95次阅读
卫星通信的常用产品及其特点

算法题类型以及准备策略

今天就和大家聊聊大公司的面试环节经常涉及的算法题类型以及准备策略。 问题难度首先大家比较关心的就是面....
发表于 2021-09-02 10:50? 206次阅读
算法题类型以及准备策略

粗放生长时代结束 无人驾驶的铺垫高精地图开始收紧...

二十年前,人们出行基本上靠活地图大脑和纸质版地图;十年前,第一代电子导航地图开始在车辆中控大屏上线,....
发表于 2021-09-02 09:25? 932次阅读
粗放生长时代结束 无人驾驶的铺垫高精地图开始收紧...

AI落地盘古开天 跨越AI天堑时:行动代号“盘古...

我采访过一个案例,某工厂的IT负责人想要应用AI,咨询之后却发现开发成本过于高昂,人才、算力、算法等....
发表于 2021-09-01 16:07? 862次阅读
AI落地盘古开天 跨越AI天堑时:行动代号“盘古...

Python 代码加速运行的的小技巧

Python 是一种脚本语言,相比 C/C++ 这样的编译语言,在效率和性能方面存在一些不足。但是,....
发表于 2021-09-01 11:28? 243次阅读
Python 代码加速运行的的小技巧

如何使用Anbox在Jetson Nano 2G...

**Jetson Nano 2GB是NVIDIA的一款高性价比嵌入式平台。麻雀虽小五脏俱全,战力强悍....
发表于 2021-08-26 17:46? 580次阅读
如何使用Anbox在Jetson Nano 2G...

一文带你快速读懂支持向量机 SVM 算法

简介 支持向量机基本上是最好的有监督学习算法了。最开始接触SVM是去年暑假的时候,老师要求交《统计学....
发表于 2021-08-26 15:27? 198次阅读
一文带你快速读懂支持向量机 SVM 算法

Microchip携手生态系统合作伙伴打造“电子...

工信部的最新数据显示,截止2020年底,中国新增5G基站58万个,推动共建共享5G基站33万个,年初....
发表于 2021-08-24 09:40? 1727次阅读
Microchip携手生态系统合作伙伴打造“电子...

基于FPGA的自适应阈值分割算法实现

在图像预处理中经常会碰到图像分割问题,把感兴趣的目标从背景图像中提取出来,而经常使用的是简单的全局阈....
发表于 2021-08-23 16:27? 1528次阅读
基于FPGA的自适应阈值分割算法实现

基于Verilog的“自适应”形态学滤波算法实现

一、背景介绍 基于二值图像的滤波算法即形态学滤波,在图像目标采集的预处理中经常被使用到,针对不同的使....
发表于 2021-08-23 16:17? 1322次阅读
基于Verilog的“自适应”形态学滤波算法实现

中海达获广州市首批高水平企业研究院资格认定

近日,根据《广州市推进高水平企业研究院建设行动方案(2020年-2022年)》(穗科规字〔2020〕....
发表于 2021-08-18 15:33? 1956次阅读
中海达获广州市首批高水平企业研究院资格认定

四维图新王鹏:高精度地图在自动驾驶中的作用与发展...

7月31日,在2021第十二届高工智能汽车开发者大会上,四维图新地图云解决方案架构师 王鹏发表演讲,....
发表于 2021-08-18 10:56? 1036次阅读
四维图新王鹏:高精度地图在自动驾驶中的作用与发展...

HALCON项目应使用哪种编程语言

来源:机器视觉算法与应用 大多数HALCON解决方案都必须嵌入到PC端的应用程序中(例如,提供图形用....
发表于 2021-08-18 10:36? 356次阅读
HALCON项目应使用哪种编程语言

什么是ROS rosserial和micro_r...

ROS 简介 最初 2007 年左右,斯坦福机器人实验室的两个博士生,Eric Berger 和 K....
发表于 2021-08-13 16:42? 539次阅读
什么是ROS rosserial和micro_r...

在Vitis中把Settings信息传递到底层的...

本篇文章来自赛灵思高级工具产品应用工程师 Hong Han. 本篇博文将继续介绍在Vitis中把Se....
发表于 2021-08-13 14:35? 1676次阅读
在Vitis中把Settings信息传递到底层的...

如何利用MDE的pydbg工具进行函数的直接调用

内容简介 本文介绍如何利用MDE的pydbg工具进行函数的直接调用,以方便程序猿的测试工作。 测试工....
发表于 2021-08-10 09:34? 313次阅读
如何利用MDE的pydbg工具进行函数的直接调用

道术之间:人文社科教师为什么要学习AI 这是道与...

我们的读者,应该对在人文社科领域中的AI技术并不陌生。 几年来,我们报道过AI应用于古文字识别、考古....
发表于 2021-08-09 19:06? 2284次阅读
道术之间:人文社科教师为什么要学习AI 这是道与...

常见的AGV导航导引方式介绍

随着智能制造的发展,工厂智能化已成为必然趋势,无人搬运车(Automated Guided Vehi....
发表于 2021-08-06 18:06? 208次阅读
常见的AGV导航导引方式介绍

物联网加持后煤炭时代 数字货舱一个会说话的箱子

作家巴巴拉·弗里兹在《coal:A human History》一书中曾写道,煤炭被视作人类进步的缩....
发表于 2021-08-06 15:13? 1463次阅读
物联网加持后煤炭时代  数字货舱一个会说话的箱子

激光导航无人叉车有什么优势

随着创新技术的不断更新迭代,智慧仓储不再只是停留在理论中,而是频繁出现在实际应用中,那些依赖运输体系....
发表于 2021-08-04 09:23? 65次阅读
激光导航无人叉车有什么优势

目前的AI就是“暴力计算”,那未来呢?

lol赛事网址(AI)技术在这几年发展非常迅速,但真正的落地项目就目前来说还不多,应用得最多的可能就是图像....
发表于 2021-07-30 14:17? 2990次阅读
目前的AI就是“暴力计算”,那未来呢?

投资8亿!广汽和华为建立合资子公司进军智能汽车S...

7月9日,广汽集团发布公告,同意其全资子公司广汽埃安新能源汽车有限公司与华为AH8车型)项目的实施。....
发表于 2021-07-13 09:58? 4562次阅读
投资8亿!广汽和华为建立合资子公司进军智能汽车S...

SAR图像导航定位检测方案的简介

为什么要提升飞行器的定位精度 对于飞行器来说,导航定位的精度与其任务息息相关。对于载人飞行器来说,失....
发表于 2021-07-13 09:58? 72次阅读
SAR图像导航定位检测方案的简介

三维C型臂导航系统在骨科手术中的应用

随着人口老龄化越来越严重,患有骨科疾病的老年患者增多,为了更好的满足其看诊需求,基层医院的整体医疗设....
发表于 2021-07-09 09:31? 130次阅读
三维C型臂导航系统在骨科手术中的应用

教你们C语言打印如何输出红色字体

除了Linux,在VS下也可以实现变色这一效果,先看下面的一段代码: #include 《stdio....
发表于 2021-07-08 12:48? 506次阅读
教你们C语言打印如何输出红色字体

eBPF是什么以及eBPF能干什么

一、eBPF是什么 eBPF是extended BPF的缩写,而BPF是Berkeley Packe....
发表于 2021-07-05 15:17? 613次阅读
eBPF是什么以及eBPF能干什么

Zephyr如何在第三方工具链下开启TLS

在Zephyr TLS线程本地存储的实现一文中说明了如何在Zephyr上使用TLS,在这种默认的情况....
发表于 2021-07-05 08:58? 335次阅读
Zephyr如何在第三方工具链下开启TLS

大学应该更偏向技术还是算法和数据结构这类?

很多时候我们需要的算法都被封装到编程语言的基础库里了,以至于很多同学会觉得算法离我们太远,其实不是的....
发表于 2021-07-04 15:11? 430次阅读
大学应该更偏向技术还是算法和数据结构这类?

队列实现栈原理是什么?队列实现栈方案有哪几种?

栈是一种后进先出的数据结构,而队列是一种先进先出的数据结构,两者原理不难理解,使用也简单。
发表于 2021-07-04 13:28? 379次阅读
队列实现栈原理是什么?队列实现栈方案有哪几种?

AGV小车的导航形式及其特点

agv小车已经成为现代物流仓储中的一项利器,不仅能高精度地搬运重物,还能多机调度,在行驶过程中也不会....
发表于 2021-07-02 10:57? 122次阅读
AGV小车的导航形式及其特点