MRMP A*

A*

理论

参考链接如下：
[运动规划入门 | 2. 白话A*，从原理到Matlab实现]:https://www.guyuehome.com/6560
在学习完Dijkstra算法后可知，Dijkstra在运行的时候会花费很多精力去访问一些对规划结果没有贡献的节点，如果Dijkstra在搜索的时候能够带有目的性地去搜索，想必能够大大提高搜索的效率，Dijkstra和贪婪算法结合的A*算法就应运而生。两种算法的核心思想区别主要如下：

Algorithm	Cost Function
Dijkstra	g(n)
A*	f(n) = g(n) + h(n)

其中g(n)表示从起点到节点n的累计代价估计，h(n)表示从节点n到终点的最小代价估计。更形象地可表示为下图，其中g(n)和h(n)具体的值会因选择的计算函数而异，例如：g(n)可选择为距离起点的欧几里德距离，而h(n)可选择为距离终点的欧几里德距离。

算法流程

• Maintain a priority queue to store all the nodes to be expanded
• The heuristic function h(n) for all nodes are pre-defined
• The priority queue is initialized with the start state $X_S$
• Assign $g(X_S)=0$, and $g(n)=\inf$ for all other nodes in the graph
• Loop
  • If the queue is empty, return FALSE; break;
  • Remove the node “n” with the lowest $f(n)=g(n)+h(n)$ from the priority queue
  • Mark node “n” as expanded
  • If the node “n” is the goal state, return TRUE; break;
  • For all unexpanded neighbors “m” of node “n”
    • If $g(m)=\inf$
      • $g(m)=g(n)+C_{nm}$
      • Push node “m” into the queue
    • If $g(m)>g(n)+C_{nm}$
      • $g(m)=g(n)+C_{nm}$
• End loop

A*的最优性

A*算法的启发函数可纳性限制如下：

$$h(n)\le h^{\ast }(n)$$

其中h*(n)是当前节点到达终点的最小代价。上式表明，启发函数的值不能大于当前节点到终点的实际距离。
A*算法的启发函数一致性限制如下：

$$h(n)\le c_{nm}+h(m)$$

其中$c_{nm}$为节点n到节点m的代价。上是表明对每个节点n的任意后继节点m，节点n的h(n)不大于从n到m的总代价与m的h(m)之和。在A*中，只要h(n)满足一致性，那么h(n)必然满足可纳性。

• 如果h(n)满足可纳性，那么h(n)是 admisssible(optimistic)；
• 如果h(n)是 admisssible 的，那么A*搜索到的路径是最优的。

启发函数的选择

h(n)	可行性
欧几里德距离（L2范数）	总是可行的
曼哈顿距离（L1范数）	四连接运动可行，八连接运动不可行
L$\infty$范数	总是可行的
0	总是可行的（退化为Dijkstra）

由于栅格地图的结构化，使得能够找出一个h(n)=h*(n)，使得启发函数更加tight，即Diagonal Heuristic。从而使得搜索时间大大降低，在3D地图中甚至能有20-50倍的差距。

dx = abx(node.x - goal.x);
dy = abx(node.y - goal.y);
h = (dx + dy) + (sqrt(2)-2)*min(dx, dy);

Tie Breaker(\neq)

由于一些节点他们的f(n)相同（h(n)不同），而导致搜索的范围加大，降低了搜索效率，因此考虑打破这些节点的平衡。

1. $h=h\ast (1.0+p),p<\frac{\min costof1wtep}{expected\max pathcost}$，轻微打破了h(n)的 admissibility ；
2. f(n)相同的时候，比较h(n)，采用比较小的h(n)；
3. 为路径加上倾向性，例如对角线；

   dx1 = abx(node.x - goal.x);
   dy1 = abx(node.y - goal.y);
   dx2 = abx(start.x - goal.x);
   dy2 = abx(start.y - goal.y);
   cross = abs(dx1*dy2 - dx2*dy1);
   h = h + cross*0.001;
   

实践

在学习深蓝学院《移动机器人运动规划》课程中，对于作业代码参考了以下两个链接：
[Amos-Chen98]:https://github.com/Amos-Chen98/mobile_robots_motion_planning
[Kailin Tong]:https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots
主要参考Amos-Chen98的作业源码和CPP代码，Kailin Tong的PPT和MATLAB代码。
其中在 Kailin Tong 的matlab代码中发现，在最后的路径点建立中，多出了以下三行代码，使得最后的路径 path 的起始点重复了两次，注释就好。

    assert(index==1, 'Something wrong!')
%     xval = OPEN(index,4);
%     yval = OPEN(index,5);
%     path(n+1,:) = [xval,yval];
    path = flip(path);

在CPP作业实现中，参考了以下链接：
[hw1]:https://amos98.notion.site/hw1-Quick-start-e7ecd81b4590445b895d40469aa9c796
[hw2]:https://amos98.notion.site/hw2-A-in-ROS-953f72b189d84da19b931a663198bb7f
[深蓝学院motion planning作业的一些问题]:https://blog.csdn.net/jiduqiulianga/article/details/124436343
遇到的问题补充如下：

1. 如果能打开RVIZ，但是没有点云，可能是订阅的话题错了，将/demo_node/grid_map_vis改为/random_complex/global_map，并保存设置，避免再次运行的重复操作。
2. 如果遇到roslaunch，运行选择目标点后就rviz报错闪退，这是因为程序没有写完，或者程序中编写得有问题。

   ================================================================================
   REQUIRED process [demo_node-2] has died!
   process has died [pid 147988, exit code -11, cmd /home/s7fu/ros_workspace/MRMP/hw_2/devel/lib/grid_path_searcher/demo_node ~waypoints:=/waypoint_generator/waypoints ~map:=/random_complex/global_map __name:=demo_node __log:=/home/s7fu/.ros/log/beb660bc-0de8-11ee-ad4c-4d5b65f91b18/demo_node-2.log].
   log file: /home/s7fu/.ros/log/beb660bc-0de8-11ee-ad4c-4d5b65f91b18/demo_node-2*.log
   Initiating shutdown!
   ================================================================================
   

3. Kailin Tong的CPP代码中，Astar_searcher.cpp的第B2行多了个g，编译中就会遇到，应该是碰到键盘敲上去的，删除后便能成功运行。
4. Amos-Chen98的CPP代码中，Astar_searcher.cpp的第3ZI和3ZZ行之间缺少以下代码：

   neighborPtr = neighborPtrSets[i];
   

   以此对neighborPtrSets[]中的每个neighbor进行遍历处理。