机器人路径规划其二 A-Star Algorithm【附动态图源码】

首先要说明的是，机器人路径规划与轨迹规划属于两个不同的概念，一般而言，轨迹规划针对的对象为机器人末端坐标系或者某个关节的位置速度加速度在时域的规划，常用的方法为多项式样条插值，梯形轨迹等等，而路径规划针对的是机器人的一个整体如移动机器人或者无人机在已知或者未知的环境中规划其运动的路线，在slam机器人应用较多。然而两者的界限有时也有交叉，如机械臂末端工具运动到操作对象时需要避障以及规划时间时，也可以看作是路径规划问题。

常用的路径规划算法有Dijkstra, A*，D*, RRT, PRM以及在这些算法上演变的各种算法，这两天把Dijkstra Algorithm和A-star学习了下并用QT代码复现，Dijkstra算法已经在上节说明，本节介绍A-star算法。A*算法是在一个移动机器人项目the Shakey project开发中被Hart, P. E.; Nilsson, N. J.; Raphael, B设计出来，开始Nils Nilsson建议使用图遍历算法的模式，在迭代过程中只考虑启发式距离heuristic function->h(n)，即从节点n到目标点goal的预估距离，而不去考虑g(n)->从n点到起始点间的距离，但是Bertram Raphael建议使用两者的和：f(n)=g(n)+h(n)作为选择新的迭代点的评判标准，于是大名鼎鼎的A*算法由此诞生。如果读者之前有学习过Dijkstr Algorithm就会发现，两者十分相似，因为Dijkstra算法除了评价指标只使用g(n)外其它和A*算法是一致的，A*算法可以看作是Dijkstra算法的扩展或者优化。

---

Ⅰ. 算法步骤

说明：把最短路径问题想象成一张图上某个起始点initial node到终点destination node的路径最短求解，图上除了起始点和终点还存在一些其它的可到达点以及无法达到点(障碍物)，每两个点之间都有一条权重不同的路径Edge，计每个点到起始点的路径长度为该点的距离的cost

1. 标记所有节点node为未访问状态unvisited（未访问节点集合），程序里一般给这些节点的cost设置为一个足够大的数。并建立一个{未访问节点集合}和{已接触节点集合}，以及{已访问节点集合}。

2. 设置起始点为迭代的第一个点，命名为current node，该点的cost显然为应该设置为0，将其纳入{已接触节点集合}。

3. 在每一次迭代中，对于当前的节点current node，计算所有与该节点能直接相连（相邻）的节点到该节点的距离costUV，如果 newCost=[costUV+(current node).cost] 比某一个相邻的节点的原先cost要小，则将该相邻的节点的cost更新为newCost，同时更新总的COST为COST=newCost+hCost，这里的hCost可以设计为该相邻的点到目标点的直线距离或者曼哈顿距离(如像素点情况下)，否则不做改变。将所有更新完cost的相邻节点纳入到{已接触节点集合}。

4. 当遍历完current node所有相邻的节点后，将current node节点从{已接触节点集合}中移除，并纳入到{已访问节点集合}。处于{已访问节点集合}的节点将不会在进入迭代计算过程。

5. 进入下一步迭代，新的current node从{已接触节点集合}选出，选择COST(而不是cost)最小的那一个。

6. 如果目标点destination node被纳入{已访问节点集合}（代表最短路径已经找到）或者{已接触节点集合}中的所有点的cost都无穷大（代表找不到路径，有障碍），则迭代终止。

为了更好的理解上述步骤，可以参考以上的配图，上图是我从*上截取的，感觉比我自己做的动画(后文贴出)要好，上图中，红色渐变点为visited nodes，即属于{已访问节点集合}，蓝色点为{已接触节点集合}，这些蓝色点原本为白色背景(未访问节点集合)，由于current node的每一次迭代更新，更新了相应的cost和COST后被归类为已接触点，之后在下一次迭代中作为新的current node的预备种子集合，如果其中某个点COST最小，将会被选中称为current node，在迭代中最终会变成visited node，而只有visited node才能有机会称为最短路径中的一点，可谓是过五关斩六将。当迭代终止后，我们就需要从{已访问节点集合}中选中一组构成最短路径的节点集合，这里需要提前将节点node的数据结构定义好，显然的是，如果迭代找到了最终节点并停止了下来，那么最终节点会被存储进{已访问节点集合}，我们从最终节点开始回溯，最终节点的上一个节点应该是哪一个？这就需要在节点node的数据机构中提前定义好一个parent id的数据成员变量，在每个迭代步骤中的第3步，如果相邻的节点满足更新cost的条件，我们就将该相邻的节点成员中的parent id赋值为current node的id，没错，node数据结构除了parent id，还应该有一个唯一标识自身的id，这中结构类似于单向链表，按照这个步骤回溯到起始点，最短路径也就找到了。

Ⅱ. 程序实现

为了便于可视化实时的迭代过程，利用Qt的Qpainter在QWidger空间上通过定时器每隔一段时间暂停重启迭代过程，随机生成一个二维的图，二维矩阵中的每个元素的值对应于不同的节点，如0代表未访问，2代表已接触，6代表已访问等等，程序中因为设计到[cos+hCost]进行排序，需要用到priority queue加快速度，A-star与Dijkstra算法大致相同，唯一的不同在于选取current node的标准不同，A-star算法选取的代价函数为cost+hCost的和，代码如下：

github源码：https://github.com/ShieldQiQi/Path-Planning-Showed-By-Animation

 1     const std::vector<Node> motion = GetMotion();
 2
 3     // Main loop
 4     if(!new_a_star.doneFlag)
 5     {
 6         Node current = new_a_star.open_list_.top();
 7         new_a_star.open_list_.pop();
 8         current.id_ = current.x_ * new_a_star.n + current.y_;
 9
10         // find if current is the goal node
11         if (CompareCoordinates(current, new_a_star.goal_))
12         {
13             new_a_star.closed_list_.push_back(current);
14             grid[current.x_][current.y_] = 5;
15             //return new_a_star.closed_list_;
16             new_a_star.doneFlag = true;
17             KillTimer(NULL, 1);
18         }
19         grid[current.x_][current.y_] = 5;
20         w->update();
21
22         // find all the neighbours and update their f(n)=g(n)+h(n)
23         for (const auto& m : motion)
24         {
25             Node new_point;
26             new_point = current + m;
27
28             // update id and parent id
29             new_point.id_ = new_a_star.n * new_point.x_ + new_point.y_;
30             new_point.pid_ = current.id_;
31
32             // the difference between Dijkstar and A-star Algorithm
33             // f(n)=g(n)+h(n) where the g(n)=cost while h(n)=Manhattan distance of current node and goal node
34             new_point.h_cost_ = std::abs(new_point.x_ - new_a_star.goal_.x_) + std::abs(new_point.y_ - new_a_star.goal_.y_);
35
36             if (CompareCoordinates(new_point, new_a_star.goal_)){
37                 new_a_star.open_list_.push(new_point);
38                 break;
39             }
40
41             // Check boundaries
42             if (checkOutsideBoundary(new_point, new_a_star.n)) {
43                 continue;}
44             // obstacle or visited
45             if (grid[new_point.x_][new_point.y_] == 1 || grid[new_point.x_][new_point.y_] == 6) {
46               continue;}
47
48             if(costGrid[new_point.x_][new_point.y_]>new_point.cost_)
49             {
50                 // check the new_point if is already in open_list_
51                 // if new_point in open_list then
52                 new_a_star.open_list_.push(new_point);
53                 grid[new_point.x_][new_point.y_] = 2;
54                 costGrid[new_point.x_][new_point.y_] = new_point.cost_;
55                 // otherwise, update the cost of the previus one
56                 // Todo, not a serious problem, but would be better if is done
57             }
58         }
59
60         new_a_star.closed_list_.push_back(current);
61         grid[current.x_][current.y_] = 6;
62     }else{
63         new_a_star.closed_list_.clear();
64         Node no_path_node(-1, -1, -1, -1, -1, -1);
65         new_a_star.closed_list_.push_back(no_path_node);
66         new_a_star.doneFlag = true;
67         KillTimer(NULL, 1);
68     }

Ⅲ. 迭代过程动画

因为设置的运动方向只有上下左右，所以最终的路径是不能走斜线的，如果设置成斜线，最短路径可以视觉上更短，下图中，灰色方块表示visited nodes，蓝色代表已接触，红色代表障碍物，最终的规划路径用绿色方块表示，下图中第一个图为A-star算法，第二个图为前一节所讲的Dijkstra算法，可以看到，有A-star有针对性的筛选使得它比Dijkstra算法拥有更快的速度。

  

Reference:

[1] https://en.wikipedia.org/wiki/A*_search_algorithm

[2] https://github.com/vss2sn/path_planning