在ROS中实现A*路径规划
1. 方案设计目标
学习A*路径规划算法，优化启发式函数，并在ROS中进行测试。
明星算法教程
源码地址：
二、技术指标
安装Linux系统，推荐.04；
安装ROS环境并学习其基本操作；
查找A路径规划资料，学习并熟悉A路径规划算法；
优化或改进竞赛中提供的A*算法的启发式函数::()代码并编写代码。 本次比赛提供了三种基本的启发式函数代码：曼哈顿距离、对角线距离和欧氏距离。 您可以选择任何一种来改进，或者选择其他更好的启发式函数。
3.主要研究内容 3.1 A*算法的思想和原理
在我们过去学习的寻路中，我们可以想到广度优先搜索（BFS：First）和深度优先搜索（DFS：Depth-First-）进行寻路。 我们首先看一下广度优先搜索，如下所示。 BFS以起点A为圆心，首先搜索A周围的所有点，形成一个圆形的搜索区域，然后扩大搜索半径，进一步搜索其他未搜索的区域，直到终点B进入搜索区域，被发现。 如图3-1所示：
图3-1
让我们再看看深度优先搜索。 这里的深度优先搜索并不是搜索所有路径，而是沿着B点的方向搜索。DFS使搜索区域离A尽可能远，离B尽可能近。比如，如果你在一所陌生的大学现在校园里，你知道校门在你的北边，尽管你不知道你和校门之间的路况如何。 ，地形各异，但只要尽可能向北走，总能找到校门。 如图3-2所示：
图3-2
与BFS相比，DFS基于尽可能接近终点的原则。 它实际上是通过终点相对于当前点的方向来引导的，所以有一个大概的方向，所以不必盲目寻找。 这样就可以比BFS更快的找到它。 最短路径找到了，但是在这个快速搜索中，默认的起点A和终点B之间没有障碍物，地形的权重也相似。 如果起点和终点之间有障碍物，DFS会绕行。如图3-3
图3-3
图中的DFS算法让计算机一路探索到右下区域。 可见，当DFS遇到障碍时，实际上是没有办法找到最优路径的。 它只能保证DFS会提供其中一条路径（如果有的话）。 大致了解了BFS和DFS后，通过比较两者可以看出，BFS保证从起点到达路线上任意点的成本最小（但没有考虑这个过程是否需要搜索很多格子） ）； DFS保证通过不断修正行走方向和终点方向之间的关系，使得需要搜索更少的网格来找到终点（但它没有考虑这个过程是否走很长的路）。
算法A的设计结合了BFS和DFS的优点。 它不仅考虑从起点经过当前路线的成本（确保不会走弯路），而且不断计算当前路线方向是否更接近终点（确保不走弯路）車輛改道）。 它将搜索许多图块），是静态道路网络中最有效的直接搜索算法。 算法A使用了估值的思想。 搜索过程：在要遍历的列表中（只有开头的A点），搜索估计值最小的点（k）（当前点到终点的距离的估计，后面会讲到） ) 在列表中，对 k 点进行广度优先搜索。 ，即其移动到的下一个坐标（右、右上、上、左上、左、左下、下、右下）不包括已经遍历过的点和无法到达的点，并且这些点将能找到的点添加到队列中，并将K点从队列中删除。 重复步骤1和2，直到到达B点，或者队列为空，说明没有到达B点的路径。
使用思路：
先进行DFS搜索，再进行BFS搜索，循环这个过程，直到找到目标点B。
过程1：用DFS思维尝试找到距离B最近的点（即估值最小的点）。
过程2：利用BFS思想，以K点为圆心，搜索A周围所有未搜索的点。
3.2 A*算法的计算方法
公式：F=G+H； G=从起点A沿着到达该方格所生成的路径移动到指定方格的移动成本。 我们同意直线移动的成本为10，对角移动的成本为14。（实际对角移动距离是2的平方根，大约是横向或垂直移动成本的1.414倍）。 H=从指定方格移动到终点B的估计成本。计算从当前方格水平或垂直移动到目标所需的方格数，忽略对角线移动，然后将总数乘以10。
我们将当前点设置为K。H值很容易计算。 H=（横坐标两点距离+纵坐标两点距离）X 10
G值计算，计算从K到A的最小估值，只需计算K点周围8个点（可访问和已访问的点）的g值+到K点的移动成本，其中最小估价是点 K 的 g 值，该点称为点 K 的父节点。点 k 正在被访问，则其周围至少有一个点已被访问过。如图 3-4 所示
图3-4
箭头指向其父节点的坐标，稍后将使用该坐标来查找路线。
示例演示
坐标访问和父节点查找的约定顺序：右、右上、上、左上、左、左下、下、右下。 沿X轴增加的方向为右，沿Y轴增加的方向为上。 可能有多个父节点。 ，这里选择成本最低的父节点，最后进行搜索。 坐标A(2,2)，目标坐标B(6,3)，坐标A已经估计出来。如图3-5
图3-5
计算点 (2, 2) 八个方向的坐标。 它们的父节点都是(2, 2)。 最小估计坐标是紫色 (3, 3)。 紫色标记只是为了方便下一步查找。 我们商定了估价顺序（右、右上、上、左上、左、左下、下、右下），从现在起我们将遵循这个顺序。 如图3-6所示：
图3-6
计算点(3, 3)的八个方向的坐标（已经估计的不需要再次计算）。 我们将已经估计的点称为已访问过的点。 最小估计坐标是紫色 (4, 3)。 父节点搜索顺序约定（右、右上、上、左上、左、左下、下、右下）。 最后访问的具有最小 g 值的点是父节点。 如下所示。 在这张图中我们需要了解箭头是如何确定的。 例如，点(4, 3)的父节点可以是点(3, 3)或点(3, 2)。 接入顺序为先接入点(3, 3)，再接入点(3, 2)。 所以我们将点(3, 2)作为点(4, 3)的父节点。如图3-7
图3-7
继续搜索点(4, 3)，最小估计坐标为紫色(5, 3)。 如图3-8所示：
图3-8
继续寻找点(5, 3)。 当搜索到达终点时，停止搜索。 如图3-9所示：
图3-9
通过终点直到起点依次搜索它们的父节点，然后将坐标点反转，这就是我们想要的路线。 路线：(2,2)、(3,2)、(4,2)、(5,2)、(6,3)。 如图3-10所示：
图3-10
4. 代码实现与优化 4.1 启发式函数
启发式函数h\left(n\right)的确定是A*算法的重要组成部分。 启发式函数越准确，找到最优解的速度就越快。 常用的启发式函数包括欧几里得启发式函数、曼哈顿启发式函数和对角线启发式函数。
欧几里得启发函数
h(n)=(xn−xgoal)2+(yn−ygoal)2+(zn−zgoal)2h(n)=\sqrt{\left(x_n-x_{目标}\right)^2+\left( y_n-y_{目标}\right)^2+\left(z_n-z_{目标}\right)^2}h(n)=(xn−xgoal)2+(yn−ygoal)2+ (zn−zgoal)2
其中，x_i(i=n,goal)、y_i(i=n,goal)和z_i(i=n,goal)分别是节点n和末端节点在三维空间中的坐标。
曼哈顿启发函数
h(n)=∣xn−x目标∣+∣yn−y目标∣+∣zn−z目标∣h(n)=\left|x_n-x_{目标}\right|+\left|y_n-y_{目标}\右|+\左|z_n-z_{目标}\右|h(n)=∣xn−x目标∣+∣yn−y目标∣+∣zn−zgoal∣
对角启发函数
针对A算法路径不平滑、冗余转折点较多的问题，许多学者提出了A算法的改进，主要包括两类：对代价函数的改进和对扩展区域的改进。 这次，对成本函数中的重要因素：启发式函数h(n)h\left(n\right)h(n)进行了改进。 启发式函数可以控制A的扩展节点和运行时间： (1) 如果h(n)h\left(n\right)h(n)小于当前节点n到目标节点的实际成本， A算法保证找到一条最短路径。 h(n)h\left(n\right)h(n)越小，A扩展的节点越多，运行速度越慢。 (2) 如果h(n)h\left(n\right)h(n)大于当前节点n到目标节点的实际成本，算法A不能保证找到最短路径，但它有很少的路径扩展节点，运行流畅更快。 本文在初始参数设置阶段增加h(n)h\left(n\right)h(n)的权重系数W，通过调整动态改变代价函数h(n)h\left(n\right) W 的大小 )h(n) 的大小的目的是允许算法根据具体需要进行调整。
该程序的优化是欧氏启发函数的优化。 优化后的公式为：
h(n)=W*(xn−xgoal)2+(yn−ygoal)2+(zn−zgoal)2h\left(n\right)=W\ast\sqrt{\left(x_n-x_{目标} \right)^2+\left(y_n-y_{目标}\right)^2+\left(z_n-z_{目标}\right)^2}h(n)=W∗(xn−xgoal) 2+(yn−y目标)2+(zn−z目标)2
在网格法随机生成的地图中，通过改变权重W来动态调整A*算法的搜索路径、扩展节点和时间。如图4-1、4-2、4-3所示：
图4-1
图4-2
图4-3
由此可见，改变启发式函数的权重W后，对整体规划时间还是有影响的，特别是当权重为1且数字大于1时，但权重并不意味着越大更好的是，可以看出，实验过程中，权重越大，稳定所需的时间就越长。
4.2 代码介绍
功能包结构
.grid_path_searcher
├── CMakeLists.txt
├── include
│ ├── Astar_searcher.h
│ ├── backward.hpp
│ └── node.h
├── launch
│ ├── demo.launch
│ └── rviz_config
│ └── demo.rviz
├── package.xml
├── src
│ ├── Astar_searcher.cpp 改进A*算法的方法
│ ├── demo_node.cpp 节点文件
│ └── random_complex_generator.cpp 生成障碍物
.waypoint_generator
├── CMakeLists.txt
├── package.xml
├── src
│ ├── sample_waypoints.h
│ └── waypoint_generator.cpp 发布目标点信息
└── waypoint_generator.txt
主要修改代码
.cpp文件下，主要修改启发式函数::，代码如下：
double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2)
{
double h;
auto node1_coord = node1->coord;
auto node2_coord = node2->coord;
// Heuristics 1: Manhattan
//h = std::abs(node1_coord(0) - node2_coord(0) ) +
// std::abs(node1_coord(1) - node2_coord(1) ) +
// std::abs(node1_coord(2) - node2_coord(2) );
/*对欧几里得启发函数进行优化*/
// Heuristics 2: Euclidean
h = weight_ * ( std::sqrt(std::pow((node1_coord(0) - node2_coord(0)), 2 )+
std::pow((node1_coord(1) - node2_coord(1)), 2 ) +
std::pow((node1_coord(2) - node2_coord(2)), 2 )));
// Heuristics 3: Diagnol distance
// double dx = std::abs(node1_coord(0) - node2_coord(0) );
// double dy = std::abs(node1_coord(1) - node2_coord(1) );
//double dz = std::abs(node1_coord(2) - node2_coord(2) );
//double min_xyz = std::min({dx, dy, dz});
// h = dx + dy + dz + (std::sqrt(3.0) -3) * min_xyz;
return 0;
}
在 .h 文件下，定义启发式函数中使用的函数名称：
#ifndef _ASTART_SEARCHER_H
#define _ASTART_SEARCHER_H
#include cpp文件的代码已经优化。 通过ros::param::get函数，可以通过文件灵活修改启发式函数中的权重系数，这也为后续动态权重的设计做好了铺垫。
int main(int argc, char** argv)
{
ros::init(argc, argv, "demo_node");
ros::NodeHandle nh("~");
double weight;
double weight_;
_map_sub = nh.subscribe( "map", 1, rcvPointCloudCallBack );
_pts_sub = nh.subscribe( "waypoints", 1, rcvWaypointsCallback );
_grid_map_vis_pub = nh.advertise<sensor_msgs::PointCloud2>("grid_map_vis", 1);
_grid_path_vis_pub = nh.advertise<visualization_msgs::Marker>("grid_path_vis", 1);
_visited_nodes_vis_pub = nh.advertise<visualization_msgs::Marker>("visited_nodes_vis",1);
/*对权重系数进行传参*/
ros::param::get("~weight",weight_);
nh.param("map/cloud_margin", _cloud_margin, 0.0);
nh.param("map/resolution", _resolution, 0.2);
nh.param("map/x_size", _x_size, 50.0);
nh.param("map/y_size", _y_size, 50.0);
nh.param("map/z_size", _z_size, 5.0 );
nh.param("planning/start_x", _start_pt(0), 0.0);
nh.param("planning/start_y", _start_pt(1), 0.0);
nh.param("planning/start_z", _start_pt(2), 0.0);
_map_lower << - _x_size/2.0, - _y_size/2.0, 0.0;
_map_upper << + _x_size/2.0, + _y_size/2.0, _z_size;
_inv_resolution = 1.0 / _resolution;
_max_x_id = (int)(_x_size * _inv_resolution);
_max_y_id = (int)(_y_size * _inv_resolution);
_max_z_id = (int)(_z_size * _inv_resolution);
_astar_path_finder = new AstarPathFinder();
_astar_path_finder -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _max_z_id);
ros::Rate rate(100);
bool status = ros::ok();
while(status)
{
ros::spinOnce();
status = ros::ok();
rate.sleep();
}
delete _astar_path_finder;
return 0;
}
配置demo的参数文件。 文件：
<launch>
<arg name="map_size_x" default="10.0"/>
<arg name="map_size_y" default="10.0"/>
<arg name="map_size_z" default=" 2.0"/>
<arg name="start_x" default=" 0.0"/>
<arg name="start_y" default=" 0.0"/>
<arg name="start_z" default=" 1.0"/>
<node pkg="grid_path_searcher" type="demo_node" name="demo_node" output="screen" required = "true">
<remap from="~waypoints" to="/waypoint_generator/waypoints"/>
<remap from="~map" to="/random_complex/global_map"/>
<param name="weight_" value="10" />
<param name="map/margin" value="0.0" />
<param name="map/resolution" value="0.2" />
<param name="map/x_size" value="$(arg map_size_x)"/>
<param name="map/y_size" value="$(arg map_size_y)"/>
<param name="map/z_size" value="$(arg map_size_z)"/>
<param name="planning/start_x" value="$(arg start_x)"/>
<param name="planning/start_y" value="$(arg start_y)"/>
<param name="planning/start_z" value="$(arg start_z)"/>
node>
<node pkg ="grid_path_searcher" name ="random_complex" type ="random_complex" output = "screen">
<param name="init_state_x" value="$(arg start_x)"/>
<param name="init_state_y" value="$(arg start_y)"/>
<param name="map/x_size" value="$(arg map_size_x)" />
<param name="map/y_size" value="$(arg map_size_y)" />
<param name="map/z_size" value="$(arg map_size_z)" />
<param name="map/circle_num" value="40"/>
<param name="map/obs_num" value="300"/>
<param name="map/resolution" value="0.1"/>
<param name="ObstacleShape/lower_rad" value="0.1"/>
<param name="ObstacleShape/upper_rad" value="0.7"/>
<param name="ObstacleShape/lower_hei" value="1.0"/>
<param name="ObstacleShape/upper_hei" value="3.0"/>
<param name="CircleShape/lower_circle_rad" value="0.6"/>
<param name="CircleShape/upper_circle_rad" value="2.0"/>
<param name="sensing/rate" value="0.5"/>
node>
<node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
<remap from="~goal" to="/goal"/>
<param name="waypoint_type" value="manual-lonely-waypoint"/>
node>
launch>
运行步骤及结果对比
运行步骤
下载并解压文件夹； 创建工作区，Ctrl+alt+t，打开终端，一一运行以下命令；
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
将/src中的三个文件复制到//src路径下，并一一运行以下命令；
$ catkin_init_workspace
$ cd ~/catkin_ws/
$ catkin_make
$ source devel/setup.bash
$ roscore
重新打开终端并打开 Rviz：
$ source devel/setup.bash
$ rviz
点击鼠标左上角的文件按钮，添加配置文件。 配置文件路径如下。 打开后显示如下页面，如图4-4、4-5所示： /src//lau nch/onfig/demo.rviz
图4-4
图4-5
打开终端逐行运行以下命令，打开文件，加载地图，如图4-6所示；
$ source devel/setup.bash
$ roslaunch grid_path_searcher demo.launch
图4-6
选择 3D 新目标图标，单击地图上的任意位置，将出现一个绿色箭头。 移动鼠标来调整箭头的方向。 您可以看到 3D 地图中规划的从终点到地图中心的路径。
改进前后效果对比
如图4-7和4-8所示，当权重系数为1时（即优化前），路径规划所花费的时间如下：
图4-7
图4-8
可以看到整个规划时间在13ms到17ms之间，甚至可能出现路径无法规划的问题。
当权重系数为3时，如下图4-9所示：
图4-9
当权重系数为10时，如下图4-10所示：
图4-10
由此看来，权重系数的适度增加对于整个路径规划的成功率和效率都有相应的提升。
5. 后续改进与展望
后续过程中，考虑到权重系数的稳定性后，我们希望将扩展区域从4邻域扩展方法改进为8邻域和24邻域，并与成本函数的改进进行比较。 目前，我们已经取得了模拟8个邻域和24个邻域的时间和路径对比的进展，如下图5-1所示：
8邻域扩展 24邻域扩展
图5-1
从图中可以看出，邻域数量增加后，整体路径平滑度明显提高，但代价是牺牲了路径规划所花费的时间。 在未来的研究中，改进的A*算法将不断优化并应用到真实物体中。
