文章目录

前言
1. 导航的相关启动和配置文件
- 1.1 demo01_gazebo.launch
- 1.2 nav06_path.launch
- 1.3 nav04_amcl.launch
- 1.4 nav05_path.launch
- 1.5 move_base_params.yaml
- 1.6 global_planner_params.yaml
2. Astar路径规划算法解析
- 2.1 astar.h
- 2.2 astar.cpp
参考文献

前言

最近在学习ROS的navigation部分，写些东西作为笔记，方便理解与日后查看。本文从Astar算法入手，对navigation源码进行解析。
PS:ros navigation源码版本https://gitcode.net/mirrors/ros-planning/navigation/-/tree/noetic-devel

对于Astar算法，想必大家都非常熟悉了。具体算法原理就不在本文详细介绍了，不太熟悉的话，可以参考下面这篇博客：
自动驾驶路径规划——A*（Astar）算法

1. 导航的相关启动和配置文件

1.1 demo01_gazebo.launch

<launch><!-- 将 Urdf 文件的内容加载到参数服务器 --><param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf/xacro/car.urdf.xacro" /><!-- 启动 gazebo --><include file="$(find gazebo_ros)/launch/empty_world.launch" ><arg name="world_name" value="$(find urdf02_gazebo)/worlds/test.world"/></include><!-- 在 gazebo 中显示机器人模型 --><node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description  -x -3.182779  -y 0.866966 -Y -1.57"  /><!-- <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description "  /> -->
</launch>

首先该launch文件启动,将机器人模型的xacro文件加载到参数服务器;再启动gazebo,加载出预设的地图以及加载机器人的初始位置.

1.2 nav06_path.launch

<launch><!-- 设置地图的配置文件 --><arg name="map" default="test01.yaml" /><!-- 运行地图服务器，并且加载设置的地图--><node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/$(arg map)"/><!-- 启动AMCL节点 --><include file="$(find nav_demo)/launch/nav04_amcl.launch" /><!-- 启动move_base节点 --><include file="$(find nav_demo)/launch/nav05_path.launch" /><!-- 运行rviz --><node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_demo)/config/test02.rviz" /><!-- 添加关节状态发布节点 --><node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" /><!-- 添加机器人状态发布节点 --><node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
</launch>

启动这个launch文件后,将会运行地图服务器，加载设置的地图;启动AMCL节点,实现机器人的定位功能;启动move_base节点,加载move_base相关的参数;运行rviz,显示机器人在rviz中的图像.

1.3 nav04_amcl.launch

这部分就不细讲了,具体可以参考【ROS】—— 机器人导航(仿真)—导航实现(十八)中的amcl部分.

1.4 nav05_path.launch

<launch><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true"><rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find nav_demo)/param/local_costmap_params.yaml" command="load" /><rosparam file="$(find nav_demo)/param/global_costmap_params.yaml" command="load" /><!-- <rosparam file="$(find nav_demo)/param/base_local_planner_params.yaml" command="load" /> --><rosparam file="$(find nav_demo)/param/move_base_params.yaml" command="load" /><rosparam file="$(find nav_demo)/param/global_planner_params.yaml" command="load" /><rosparam file="$(find nav_demo)/param/dwa_local_planner_params.yaml" command="load" /></node>
</launch>

这部分是move_base的核心,主要目的是加载代价图的相关参数,包括全局代价地图和局部代价地图的参数;加载move_base的参数以及全局规划器和局部规划器(这里选用的是dwa_local_planner)的相关参数.部分参数文件在【ROS】—— 机器人导航(仿真)—导航实现(十八)中已有讲述,这里就不重复了.本文主要对move_base_params.yaml global_planner_params.yaml进行介绍.

1.5 move_base_params.yaml

参数配置的说明可以参考注释

shutdown_costmaps: false  #当move_base在不活动状态时,是否关掉costmap.controller_frequency: 15.0  #向底盘控制移动话题cmd_vel发送命令的频率.
controller_patience: 3.0 #在空间清理操作执行前,控制器花多长时间等有效控制下发planner_frequency: 5.0  #全局规划操作的执行频率.如果设置为0.0,则全局规划器仅#在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作.
planner_patience: 5.0  #在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.oscillation_timeout: 10.0  #执行修复机制前,允许振荡的时长.
oscillation_distance: 0.02  #来回运动在多大距离以上不会被认为是振荡.#全局路径规划器
# base_global_planner: "global_planner/GlobalPlanner" #指定用于move_base的全局规划器插件名称.
# base_global_planner: "navfn/NavfnROS" #指定用于move_base的局部规划器插件名称.
base_global_planner: "global_planner/GlobalPlanner"
# base_global_planner: "carrot_planner/CarrotPlanner"#局部路径规划器的调用在文件【teb_local_planner.launch】、【dwa_local_planner.launch】对应文件内自动调用，该部分可以忽略
# base_local_planner: "teb_local_planner/TebLocalPlannerROS" #指定用于move_base的局部规划器插件名称.
base_local_planner: "dwa_local_planner/DWAPlannerROS" #指定用于move_base的全局规划器插件名称.max_planning_retries: 1  #最大规划路径的重复次数，-1表示无限次recovery_behavior_enabled: true  #是否启动恢复机制
clearing_rotation_allowed: true  #是否启动旋转的恢复，必须是recovery_behavior_enabled在使能的基础上才能生效recovery_behaviors:  # 自恢复行为- name: 'conservative_reset'  type: 'clear_costmap_recovery/ClearCostmapRecovery'  #- name: 'aggressive_reset'#  type: 'clear_costmap_recovery/ClearCostmapRecovery'#- name: 'super_reset'#  type: 'clear_costmap_recovery/ClearCostmapRecovery'- name: 'clearing_rotation'type: 'rotate_recovery/RotateRecovery'  # - name: 'move_slow_and_clear'#   type: 'move_slow_and_clear/MoveSlowAndClear'#保守清除，用户指定区域之外的障碍物将从机器人地图中清除
conservative_reset:  reset_distance: 1.0  #layer_names: [static_layer, obstacle_layer, inflation_layer]layer_names: [obstacle_layer]
#保守清除后，如果周围障碍物允许，机器人将进行原地旋转以清理空间#保守清除失败，积极清除，清除指定区域之外的所有障碍物，然后进行旋转
aggressive_reset:  reset_distance: 3.0  #layer_names: [static_layer, obstacle_layer, inflation_layer]layer_names: [obstacle_layer]
#积极清除也失败后，放弃规划路径#可能是更进一步的清除，wiki未找到相关资料
super_reset:  reset_distance: 5.0  #layer_names: [static_layer, obstacle_layer, inflation_layer]layer_names: [obstacle_layer]#另一种恢复行为，需要注意该行为只能与具有动态设置速度限制功能的局部路径规划器适配，例如dwa
move_slow_and_clear:  clearing_distance: 0.5  #与小车距离0.5内的障碍物会被清除limited_trans_speed: 0.1  limited_rot_speed: 0.4  limited_distance: 0.3

 base_global_planner: "navfn/NavfnROS"base_global_planner: "global_planner/GlobalPlanner"base_global_planner: "carrot_planner/CarrotPlanner"

此部分指定用于move_base的全局规划器插件名称.这里选择global_planner/GlobalPlanner,当然也可以选择navfn/NavfnROS,只不过后者版本较老,而carrot_planner/CarrotPlanner适合目标点距离障碍物比较近的规划场景,对于大部分导航场景来说,可能并非那么实用.

recovery_behavior_enabled: true
clearing_rotation_allowed: true

此部分来决定是否启动恢复机制与旋转机制(可以减少小车规划路径失败导致的卡死现象)

1.6 global_planner_params.yaml

GlobalPlanner:allow_unknown: false  #默认true，是否允许路径穿过未知区域default_tolerance: 0.2  #默认0.0，目标容差visualize_potential: false #默认false，是否显示从PointCloud2计算得到的势区域use_dijkstra: false #默认true，true表示使用dijkstra's否则使用A*use_quadratic: true #默认true，true表示使用二次函数近似函数use_grid_path: false #默认false，true表示使路径沿栅格边界生成，否则使用梯度下降算法old_navfn_behavior:  true #默认false，是否复制navfn规划器的结果lethal_cost: 253 #默认253，致命代价值neutral_cost: 50 #默认50，中等代价值cost_factor: 3.0 #默认3.0，代价因子publish_potential: true #默认true，是否发布costmap的势函数orientation_mode: 0 #默认0，设置点的方向orientation_window_size: 1 #默认1，根据orientation_mode指定的位置积分确定窗口方向

在修改这部分参数的时候遇到了一些问题:
若要启动Astar算法,需将use_dijkstra:置为false,此时运行遇到以下问题

[ERROR] [1674480358.446397317, 514.551000000]: NO PATH!
[ERROR] [1674480358.446429697, 514.551000000]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.

参考Navigation中全局路径规划效果对比和Some errors when using GlobalPlanner,得出以下可能的原因:
在ROS navigation包中A*算法是基于梯度势场实现的,而并非我们所熟知的栅格地图,这种方式可能是非最优的,但其规划出的路径平滑度更高.但这种方法在"precise start(精确启动?)"机制上可能存在着一些BUG,所以经常会启动失败.

解决方法:

将use_grid_path置为true.即将其转化为基于栅格地图的Astar算法.实现效果如下图所示(规划出的路线不光滑,折线明显).
将old_navfn_behavior置为true.使用旧版本的规划器.实现效果如下图所示

也有人通过减小inflation radius 并增大 cost scaling factor,从而使得全局代价地图在障碍物周围膨胀得并不多,从而不至于规划失败.这种方法可以参考,但我没有细调过参数所以没有实现.

2. Astar路径规划算法解析

附上Astar的源码

2.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;
};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_;
};} //end namespace global_planner
#endif

2.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) {}/* function：计算规划代价的函数potential: 代价数组， 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 将起点放入队列queue_.push_back(Index(start_i, 0));//2 将所有点的potential都设为一个极大值1e10//potential就是估计值g, f=g+h//std::fill函数的作用是：将一个区间的元素都赋予指定的值，即在[first, last)范围内填充指定值std::fill(potential, potential + ns_, POT_HIGH);//3 起点的potential设为0；potential[start_i] = 0;//终点对应的序号int goal_i = toIndex(end_x, end_y); int cycle = 0;//4 进入循环，继续循环的判断条件为只要队列大小大于0且循环次数小于cycles //代码中cycles = 2 *nx * ny, 即为所有格子数的2倍///目的：得到最小cost的索引，并删除它，如果索引指向goal(目的地)则退出算法，返回truewhile (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;//6. 对前后左右四个点执行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)
*/
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_)return;//未搜索的点cost为POT_HIGH，如小于该值，则为已搜索点,跳过；if (potential[next_i] < POT_HIGH)return;//障碍物点 忽略if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))return;/*potential 存储所有点的g(n),即从初始点到节点n的实际代价costs[next_i] + neutral_cost_potential[next_i]   是起点到当前点的cost即g(n)neutral_cost_ 设定的一个默认值，为50prev_potentia   当前点的fcalculatePotential()计算根据use_quadratic的值有下面两个选择若为TRUE， 则使用二次曲线计算若为False， 则采用简单方法计算， return prev_potential + cost即：costs[next_i] + neutral_cost_+ prev_potential地图代价+单格距离代价(初始化为50)+之前路径代价 为G*/potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);int x = next_i % nx_, y = next_i / nx_;//启发式函数：即h(n) 从节点n到目标点最佳路径的估计代价，这里选用了曼哈顿距离float distance = abs(end_x - x) + abs(end_y - y);//两个cost后，加起来即为f(n),代价 f = g +h;，将其存入队列中// f = potential[next_i] + distance * neutral_cost_ 为总queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));//对加入的再进行堆排序， 把最小代价点放到front队头queue_[0]std::push_heap(queue_.begin(), queue_.end(), greater1());
}}

首先来看一下AStarExpansion::calculatePotentials这个函数,这个函数的传参为地图指针costs,起点和终点的位置信息,循环次数cycles以及代价数组potential.

程序开始,先对启发式搜索得到的向量队列进行初始化

queue_.clear();

再将起始点的位置信息转化为序列号

int start_i = toIndex(start_x, start_y);

toIndex()具体定义如下:

        inline int toIndex(int x, int y) {return x + nx_ * y;}//nx_为栅格地图中x方向的像素数目

将起点放入队列之中

index()定义如下

// 第一项为序列号值,第二项为代价值
class Index {public:Index(int a, float b) {i = a;cost = b;}int i;float cost;
};

将所有点的potential都设为一个极大值 1 e 10 1e10 1e10
potential就是实际代价值 g , f = g + h g, f=g+h g,f=g+h,在planner_core.h中,POT_HIGH定义如下

#define POT_HIGH 1.0e10

std::fill函数的作用是：将一个区间的元素都赋予指定的值，即在[first, last)范围内填充指定值

起点的potential设为0；

potential[start_i] = 0;

再将终点的位置信息转化为序列号

int goal_i = toIndex(end_x, end_y);

进入循环，继续循环的判断条件为:队列大小大于0且循环次数小于cycles
代码中cycles = 2 *nx * ny, 即为所有格子数的2倍
目的：得到最小cost的索引，并删除它，如果索引指向goal(目的地)则退出算法，返回true

top为最小代价点

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周围点加入搜索队里并更新代价值(和grid算法不同,这里的Astar是四邻域的)

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

再来看看AStarExpansion::add函数
add函数的作用:添加点并更新代价

首先判断新的点是否在地图内;是否被访问过;是否是障碍物.是的话,跳过这个点.

    if (next_i < 0 || next_i >= ns_)return;if (potential[next_i] < POT_HIGH)return;if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))return;

计算start_i到邻点(i+1,i-1,i+nx,i-nx) 的最小代价g(n)，

potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);

potential 存储所有点的g(n),即从初始点到节点n的实际代价costs[next_i] + neutral_cost_
potential[next_i] 是起点到当前点的cost即g(n)
neutral_cost_ 设定的一个默认值，为50
prev_potentia 当前点的f

        virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1){if(prev_potential < 0){// get min of neighborsfloat 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;}

启发式函数：即h(n) 从节点n到目标点最佳路径的估计代价，这里选用了曼哈顿距离

float distance = abs(end_x - x) + abs(end_y - y);

两个cost后，加起来即为f(n),代价 f = g +h;，将其存入队列中

    queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));

对加入的再进行堆排序，把最小代价点放到front队头queue_[0]

std::push_heap(queue_.begin(), queue_.end(), greater1());

参考文献

[1] ros navigation 解析之A*路径规划
[2] Navigation中全局路径规划效果对比
[3] ROS - move_base全局路径规划之A*程序分析
[4] 移动机器人gazebo仿真(5)—规划算法Ａ*

【ROS-Navigation】—— Astar路径规划算法解析相关推荐

ROS常用局部路径规划算法比较
本博文主要讨论ROS导航包中集成的局部路径规划算法,DWA.TEB.MPC等算法在使用过程中的各自的优缺点.以下均为自己在使用过程中总结的经验及查阅资料得来,如有理解不到位的地方,还希望在评论区多多讨 ...
【硬核】 ROS Navigation 局部路径规划常见算法
简介最近,作者参加了关于RMUS 高校 SimReal挑战赛,首次接触到了机器人导航领域,这里记录一下这段时间的收货.sim2real的全称是simulation to reality,是强化学 ...
AStar路径规划算法
概要 AStar是一种路径规划算法,其思想包含Dijkstra算法和启发式算法.该算法也是通过起点开始,逐步记录能够搜索到的节点,并记录它们离起点的最短距离:同时还会估算搜索到的点离终点的距离. 公式 ...
【ROS-Navigation】—— DWA路径规划算法解析
文章目录前言 1. 涉及的核心配置文件与启动文件 1.1 demo01_gazebo.launch 1.2 nav06_path.launch 1.3 nav04_amcl.launch 1.4 n ...
(一)路径规划算法---Astar实现自定义的全局路径规划插件
Astar实现自定义的全局路径规划插件文章目录 Astar实现自定义的全局路径规划插件 1.插件功能包的建立 2. 相关步骤 2.1 建立工作空间和环境变量的配置 2.2 建立功能包 2.3 添加源 ...
ros自己写避障算法_基于ROS系统自主路径规划与避障小车的研究
龙源期刊网 http://www.qikan.com.cn 基于 ROS 系统自主路径规划与避障小车的研究作者:李阳卢健何耀帧来源:<科技风> 2018 年第 04 期摘要: ...
D* Lite路径规划算法
D* Lite路径规划算法 1.D* Lite算法简述 2.D* Lite算法伪代码 3.D*Lite算法一个简单的例子 3.1 地图无变化时 3.2地图变化时 4.算法总结参考资料搜索算法其他文 ...
无人车路径规划算法—（3）基于搜索的路径规划算法（BFS/DFS/Dijkstra）
1.BFS(广度优先搜索) && DFS(深度优先搜索) 广度优先遍历图的方式为,一次性访问当前顶点的所有未访问状态相邻顶点,并依次对每个相邻顶点执行同样处理.因为要依次对每个相邻顶点 ...
Field Ｄ*路径规划算法
Field D*路径规划算法 1.栅格法路径规划存在的问题 2.Filed D*算法主要思想解析 3.Filed D*算法伪代码 4.算法优化 5.算法总结参考文献搜索算法其他文章紧接着上一篇D ...

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