ROS中Navigation功能包里路径规划A*算法详解

一、下载编译功能包

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

二、功能包里涵盖的文件

在这里插入图片描述

功能包功能
acml定位算法
move_basenavigation中最主要的框架
base_local_planner局部路径规划器
dwa_local_plannerdwa算法局部路径规划实现
global_planner全局路径规划
navfn全局路径规划,旧版本的,有bug
carrot_planner一个简单的全局路径规划器
clear_costmap_recovery清除代价地图的恢复行为
costmap_2d实现2d代价地图
fake_localizationacml的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 <global_planner/planner_core.h>
#include <global_planner/expander.h>
#include <vector>
#include <algorithm>

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<Index> queue_;
};

} 

2、astar.cpp

#include<global_planner/astar.h>
#include<costmap_2d/cost_values.h>

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_为启发式搜索到的向量队列:<i , cost>
     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 <algorithm>

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_;
};
} 
评论 9
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值