c++ A*和JPS算法框架navigation_astar

代码在我的码云里面：
2DA*: https://gitee.com/spiderman-spiderman/navigation_astar.git
3DA*:

我将从下面几点介绍3DA*代码框架的理解：

第一、代码框架介绍；

第二、四个重要函数介绍；

第三、3D栅格图实现的路径规划转变为2D栅格图实现；

第一、代码框架介绍；

代码框架中我认为最难的几个部分如下：
1、初始化地图中的三级指针

GridNodePtr这个三级指针，使用3维数组来初始化自己并填充数据，并且每个位置添充的数据是封装好的数据结构struct GridNode的地址。其中GLX_SIZE、GLY_SIZE、GLZ_SIZE，分别是栅格图坐标中最大的索引值，例如地图分辨率是 0.2,而 地图真实尺度是 10 * 10 * 5，单位m。所以对应的栅格图最大索引值分别是10 / 0.2 = 50、50、25，虽然真实地图上x,y坐标上有正负之分，但栅格坐标系上一定从0开始到最大索引值。

2、初始化地图中的一维指针数组

uint8_t是一个字节， GLXYZ_SIZE = GLX_SIZEGLY_SIZEGLZ_SIZE=62500，这里用C 库函数 void *memset(void *str, int c, size_t n) ；来初始化数组，使其数组中的元素都为0，表示空闲状态。接下来在函数setObs中进行初始化。

该函数传进来的coord_x, coord_y, coord_z，是尺度地图中障碍物的坐标，首先判断该障碍物坐标是否超出上下边界超出则停止此次操作，其次将其转化为栅格坐标。
然后下面这个data[]中的公式较难理解，该GLYZ_SIZE是GLY_SIZE * GLZ_SIZE = 1250表示y轴和z轴上的栅格数，如下图所示(红色是x轴)，所以与障碍物x方向的栅格坐标idx_x相乘，表示“底面积乘高”,当前障碍物的栅格坐标点在第几层。而障碍物y方向的栅格坐标idx_y和GLZ_SIZE相乘则表示这层栅格中的第几行。最后加上idx_z则表示，障碍物坐标点在这行中的第几个列。通过这样找层高得到二维平面，在二维平面中找行找列数就可以得到正确的栅格坐标，并赋值为1，表示该栅格是占有的状态。

这个公式让我产生一个问题，为什么不能使用下面的两个公式来初始化障碍物栅格坐标？

答案：这就要用上面的GridNodePtr这个三维数组解释了，因为这个三维数组在初始化的时候，GLZ_SIZE用来表示地图的深度也就是层高，GLX_SIZE表示行，GLY_SIZE表示列，初始化时首先填充0行0列0层～0行0列25层，然后0行1列0层～0行1列25层，，，，，，，，，
这说明数据从左到右，从下到上的顺序排列。所以你若是找障碍物坐标点的索引值，那就需要把它前面的点的索引值确定下来。所以这就需要从yz这个底平面开始找，然后是行和列。

3、栅格坐标转实际坐标公式中为什么需要加上 0.5 ?

这个还要从初始化地图GridNodePtr这个三维数组解释，因为初始化过程中的栅格坐标索引值是从0开始的，如果这里不加上0.5，会使每块小栅格转化后的真实坐标对应它们底面的位置。
这样显然不能够精确的反映它们的位置，为了改变这种情况，也为了使栅格真实坐标能够真正对应栅格坐标，所以这里加上0.5代表栅格中心位置，用栅格的真实坐标来对应它的中心位置而不是栅格的地面。

第二、四个重要函数介绍；

src/grid_path_searcheer/src/Astar_searcher.cpp 下的四个函数：

void AstarPathFinder::AstarGetSucc(…); //拓展节点函数
double AstarPathFinder::getHeu(…); //启发式函数h(n)
void AstarPathFinder::AstarGraphSearch(…); //Astar寻路函数
vector AstarPathFinder::getPath(…); //回溯寻找的路径函数

四个函数中拓展节点函数是核心，有必要说一下算法的几个核心概念和算法伪代码：
1、g（n）：从开始状态到节点“n”的累积成本
2、g（m）：更新节点“n”的所有未扩展邻居“m”的累积成本
3、h（n）：从当前节点n到目标状态的估计最小成本（即目标成本）
4、Dijkstra算法用最小的g(n)来扩展结点找到最佳路径。A*用最小的f（n）=g（n）+h（n）来扩展节点。

这里只看A*伪代码：（和Dijkstra算法唯一的区别:f（n）=g（n）+h（n））

•维护优先级队列，以存储所有要扩展的节点
•所有节点的启发式函数h（n）都是预定义的
•优先级队列以启动状态XS初始化
•为地图中的所有节点指定 id=-1和id = 1       (-1表示节点已被扩展，1表示节点没有扩展）
•循环 Loop
	•如果队列为空，则返回FALSE；break;
	•从优先级队列中选出f（n）=g（n）+h（n）最低的节点“n”
	•将节点“n”标记为扩展，令 id = -1
	•如果节点“n”就是我们找的目标点，则返回TRUE；break;
	•对于节点“n”的所有未展开邻居“m”
	   	 •如果m邻居的 id == -1，表明没有被扩展，需要放到OpenSet中
	    		•g（m）=g（n）+Cnm
			•将节点“m”推入优先级队列中
	    	•如果g（m）>g（n）+C nm
			•g（m）=g（n）+Cnm
	•结束
•结束循环 Break Loop

好了，开始分析代码：

一、拓展节点函数：

1、引入 inline 关键字的原因：
在 c/c++ 中，为了解决一些频繁调用的小函数大量消耗栈空间（栈内存）的问题，引入了inline修饰符，表示为内联函数。
定义在类中的成员函数默认都是内联的，如果在类定义时就在类内给出函数定义，那当然最好。如果在类中未给出成员函数定义，而又想内联该函数的话，那在类外要加上inline，否则就认为不是内联的。这也侧面说明了inline是一种"用于实现的关键字"，而不是一种"用于声明的关键字"。

2、函说中传入的三个参数
一、GridNodePtr currentPtr ：它代表当前节点的指针，如下图所示。
图中GridNode这个数据结构体包含了当前节点的id、实际坐标、栅格坐标、g(n)、f(n)、父节点、带参数的构造函数等。

二、vector & neighborPtrSets ：
它是一个容器，用来存放当前节点currentPtr扩展到的周围邻居节点的指针。

三、vector & edgeCostSets：
它是一个容器，用来存放当前节点currentPtr到周围邻居节点的代价值，这里的代价值必须是实际坐标点之间的欧式距离。

3、如何遍历当前节点周围的邻居节点？

1、二维情况下：当前节点是在一个9宫格的中心位置，需要遍历8个栅格邻居。
2、三维情况下：当前节点是在一个由27个栅格组成的正方体的正中心，需要遍历26个栅格邻居具体流程：

首先，需要取出当前节点的栅格坐标 idx、idy、idz。
然后进行三次for循环，for循环的起点是 -1，终点是1，每次循环也就有了三个索引 -1，0，1。栅格坐标和for循环的三个索引相加就会上下左右偏移位置，也就可以遍历周围的节点。

在循环内部需要加一些限制条件，才能保证节点是我们需要的。具体可以加两个：1、如果拓展的节点不满足地图坐标并超出了边界就跳过，2、如果拓展的节点是占据状态就跳过。
如果满足这两个条件就可以把这个节点push_back 进我们的容器 neighborPtrSets 中了。还有一个存放距离的容器edgeCostSets，需要把遍历到的邻居节点的栅格坐标转化为真实坐标后与当前节点进行欧式距离求解 push_back 进去就可以了。

二、启发式函数h(n)

启发式函数h ( n ) h(n)h(n)告诉A*从任意结点n到目标点的最小代价评估值。因此，选择一个好的启发式函数是重要的。可以得到下面的图表：

关于启发式函数我这里仅介绍三个:
1、曼哈顿距离
标准的启发式函数是曼哈顿距离（Manhattan distance），曼哈顿距离是两点在南北方向上的距离加上在东西方向上的距离即 D(I，J)= |XI - XJ| + |YI – YJ|。

2、对角线距离
如果图形中允许斜着朝邻近的节点移动，则启发函数可以使用对角距离。它的2维和3维的计算方法和图示如下：
h(n) = (abs(node.x – goal.x) + abs(node.y – goal.y) + (sqrt(2) – 2)*min(dx, dy)

h(n) = (abs(node.x – goal.x) + abs(node.y – goal.y) + abs(node.z – goal.z) + (sqrt(3) – 3)*min(dx, dy, dz)

3、欧几里得距离
欧几里得距离是指两个节点之间的直线距离，因此其计算方法也是我们比较熟悉的：
dx = abs(node.x – goal.x)
dy = abs(node.y – goal.y)
dz = abs(node.z – goalzy)
h(n) = sqrt(dx * dx + dy * dy + dz*dz)

三、A*路径搜索函数 AstarPathFinder::AstarGraphSearch(;😉

拓展节点函数和启发式函数理解后，加上伪代码就会很容易梳理下去。这个函数会传入起点实际坐标和终点实际坐标，首先需要将两个坐标变为栅格坐标start_idx 和 end_idx，然后在转变为实际坐标点，防止超出边界。然后转变为我们定义的节点数据结构，如下所示。这两个指针，它表示开始节点和目标节点。

最后需要将这个开始节点放到OpenSet这个优先级队列中，并令 id = 1。后面的操作具体步骤大致如下：
1、进入主循环
2、将成本函数f(n)最小的节点从openSet移到closedSet, 并判断是否id == -1
3、id = -1 说明这个结点在closedset中，已被扩展，跳出本次循环
4、如果当前节点是目标点，结束当前循环
5、未被扩展的结点需要扩展，获取邻居结点和距离，就需要进入AstarGetSucc 函数中
6、遍历节点"n"的所有未扩展的邻居"m"
6.1、根据 id 值判断邻居是否可以扩展
6.2、id == 0, 说明它不在closeSet和openSet中，需要放到openset中
6.3、id == 1, 说明该节点在开放集openSet，需要判断是否需要更新
7、结束循环

四、获得A*搜索的完整路径函数 std::vectorEigen::Vector3d getPath();

这个函数并不难，所以我这里直接贴出代码：

主要思想是：起始点父节点 cameFrom的指针是NULL。需要从目标节点一直回溯父节点 cameFrom的指针，一直回溯到起始点的指针就能结束。
好了Astar代码介绍到此结束。

第三、3D栅格图实现的路径规划转变为2D栅格图实现