学习笔记：A*算法matlab实现

石磊

A*算法是一种很常用很经典的路径搜索算法。与Dijkstra算法对比，Dijkstra算法计算一个点到其他点的最短路径，而A*算法存眷点到点的最短路径。Dijkstra算法的广度优先算法，A*算法是启发式算法，类似于深度优先算法。关于A*算法其他介绍本文不再赘述，重点存眷A*算法的伪代码和matlab实现。
A*算法伪代码：

1. * 初始化open_set和close_set；
2. * 将起点插手open_set中，并设置cost为0（优先级最高）；
3. * 如果open_set不为空，则从open_set中拔取cost最小的节点n：
4.     * 如果节点n为终点，则：
5.         * 从终点开始逐步追踪parent节点，一直达到起点；
6.         * 返回找到的成果路径，算法结束；
7.     * 如果节点n不是终点，则：
8.         * 将节点n从open_set中删除，并插手close_set中；
9.         * 遍历节点n所有的邻近节点：
10.             * 如果邻近节点m在close_set中，则：
11.                 * 跳过，拔取下一个邻近节点
12.             * 如果邻近节点m也不在open_set中，则：
13.                 * 设置节点m的parent为节点n
14.                 * 计算节点m的cost
15.                 * 将节点m插手open_set中
16.             * 如果邻近节点m在open_set中，则：
17.                 * 若节点m的cost小于本来的cost：
18.                     * 更新节点m的cost
19.                     * 设置节点m的parent为节点n

复制代码

A*算法matlab实现：

1. function [path] = AStar(map, p_start, p_goal)
2.     [~, mapWidth] = size(map);
3.     p_start_index = getIndex(p_start, mapWidth);
4.     p_goal_index = getIndex(p_goal, mapWidth);
5.     openList = [p_start_index; 0]; % 为了拔取cost最小的节点，open_set中也存放了节点的代价
6.     closeList = [];
7.     % costTable存放了节点id、到起点的欧式距离、到终点的曼哈顿距离、节点的估计代价
8.     costTable = [p_start_index; 0; Mdistance(p_start, p_goal); Mdistance(p_start, p_goal)];
9.     edges = int32.empty(0, 2);
10.     while ~isempty(openList)
11.         [~, min_cost_index] = min(openList(2, :));
12.         p_current_index = openList(1, min_cost_index); % 拔取cost最小的节点作为当前节点
13.         openList(:, min_cost_index) = [];
14.         closeList = [closeList, p_current_index];
15.         p_current = index2node(p_current_index, mapWidth);
16.         if (isequal(p_current, p_goal)) % 如果当前节点就是终点，回溯得到起点到终点之间的路径
17.             path = findPath(edges, p_start_index, p_goal_index, mapWidth);
18.             return;
19.         end
20.         % 遍历当前节点的邻居节点
21.         for i = -1 : 1
22.             for j = -1 : 1
23.                 p_neig = p_current + int32([i, j]);
24.                 p_neig_index = getIndex(p_neig, mapWidth);
25.                 % 如果邻居节点不成行或者已经在close_set中，跳过
26.                 if ~isNodeFree(map, p_neig) || ~isempty(find(closeList == p_neig_index, 1))
27.                     continue;
28.                 end
29.                 % 计算邻居节点的估计代价
30.                 [EdOfPNeig, MdOfPNeig, costOfPNeig] = cost(p_neig, p_current, p_current_index, costTable, p_goal);
31.                 if isempty(find(openList(1, :) == p_neig_index, 1))
32.                     openList = [openList, [p_neig_index; costOfPNeig]];
33.                     costTable = [costTable, [p_neig_index; EdOfPNeig; MdOfPNeig; costOfPNeig]];
34.                     edges = [edges; [p_neig_index, p_current_index]];
35.                 else
36.                     [~, column] = find(costTable(1, :) == p_neig_index);
37.                     if costOfPNeig < costTable(4, column)
38.                         % 若邻居节点已经在open_set中且新cost小于原cost，则更新邻居节点的估计代价和父节点
39.                         [row, ~] = find(edges(:, 1) == p_neig_index);
40.                         edges(row, :) = [];
41.                         edges = [edges; [p_neig_index, p_current_index]];
42.                         costTable(:, column) = [];
43.                         costTable = [costTable, [p_neig_index; EdOfPNeig; MdOfPNeig; costOfPNeig]];
44.                         [~, col] = find(openList(1, :) == p_neig_index);
45.                         openList(:, col) = [];
46.                         openList = [openList, [p_neig_index; costOfPNeig]];
47.                     end
48.                 end
49.             end
50.         end
51.     end
52. end

复制代码

以上就是A*算法的matlab实现，下面对此中的一些自定义函数进行补充：

1. % 获取节点在地图中的id
2. function [index] = getIndex(p, mapWidth)
3.     index = p(2) + mapWidth * (p(1) - 1);
4.     index = int32(index);
5. end
7. % 将节点id转换成节点坐标
8. function coordinate = index2node(index, mapWidth)
9.     y = int32(mod(index, mapWidth));
10.     if y == 0
11.         y = mapWidth;
12.     end
13.     x = int32((index-y)/mapWidth + 1);
14.     coordinate = int32([x, y]);
15. end
17. % 计算两个节点之间的欧式距离
18. function [Ed] = Edistance(p1, p2)
19.     p1_p2 = int32(p1) - int32(p2);
20.     if p1_p2(1) == 0 && p1_p2(2) == 0
21.         Ed = 0;
22.     elseif p1_p2(1) == 0 || p1_p2(2) == 0
23.         Ed = 10;
24.     else
25.         Ed = 14;
26.     end
27.     Ed = int32(Ed);
28. end
30. % 计算两个节点之间的曼哈顿距离
31. function [Md] = Mdistance(p1,p2)
32.     p1_p2 = int32(p1) - int32(p2);
33.     Md = abs(p1_p2(1)) + abs(p1_p2(2));
34.     Md = Md * 10;
35.     Md = int32(Md);
36. end
38. % 判断节点是否为可行节点
39. function [isFree] = isNodeFree(map, p)
40.     [mapLength, mapWidth] = size(map);
41.     if p(1) < 1 || p(2) < 1 || p(1) > mapLength || p(2) > mapWidth
42.         isFree = 0;
43.     else
44.         isFree = ~map(p(1), p(2));
45.     end
46. end
48. % 计算邻居节点到起点的欧式距离、到终点的曼哈顿距离、节点的估计代价
49. function [di2start, Md2goal, co] = cost(p_neig, p_current, p_current_index, costTable, p_goal)
50.     [~, column] = find(costTable(1, :) == p_current_index);
51.     di2start = int32(costTable(2, column)) + Edistance(p_neig, p_current);
52.     Md2goal = Mdistance(p_neig, p_goal);
53.     co = int32(di2start + Md2goal);
54. end
56. % 通过回溯得到该算法实现找到的最优路径
57. function [path] = findPath(edges, p_start_index, p_goal_index, mapWidth)
58.     [row, ~] = find(edges(:, 1) == p_goal_index);
59.     index_son = edges(row, 1);
60.     index_father = edges(row, 2);
61.     path = index2node(index_son, mapWidth);
62.     while index_father ~= p_start_index
63.         [row, ~] = find(edges(:, 1) == index_father);
64.         index_son = index_father;
65.         path = [path; index2node(index_son, mapWidth)];
66.         index_father = edges(row, 2);
67.     end
68.     path = [path; index2node(p_start_index, mapWidth)];
69.     path = flipud(path);
70. end

复制代码

至此，我们完成了A*算法的matlab实现。通过读取png图像作为二维地图，并对障碍物进行膨胀操作，执行A*算法得到如下的效果：


通过上图可以看出，A*算法能够搜索出一条可行路径，但是存在一些问题，例如：

• 存在多余的拐点；
  
• 搜索出来的路径不够平滑；
  
• 没有考虑实际的运动学约束。
  

针对这些问题，可以通过去除多余拐点，曲线插值/拟合，考虑机器人实际的运动学模型等方式来解决。


上图为针对上述问题进行优化后的最终效果，可以看到优化后的路径对比于原始路径有不少的提升，但仍存在可以改良的处所，后续将运用更多方式针对路径优化进行探究。