路径优化算法--Dijkstra和A*算法及其Matlab实现

chasing

写在前面的话：只是对两种路径优化算法进行简单的理解和尝试，为后续使用做准备。如果用到，请再次好好理解原理和Matlab源码。本文摘自作者博客 https://blog.csdn.net/qq_15390133 欢迎关注，留言。
参考博客：
1. [用Matlab实现A*算法和Dijkstra算法](https://blog.csdn.net/weixin_43795921/article/details/87945260)
2. [运动规划入门 | 1. 白话Dijkstra，从原理到Matlab实现](https://www.guyuehome.com/5652)
3. [运动规划入门 | 2. 白话A*，从原理到Matlab实现](https://www.guyuehome.com/6560)

直接上干货（参考上述博客可得）首先给出Matlab下的三个脚本文件：
TestScript.m

1. %
   
2. % TestScript for Assignment 1
   
3. %
   
4. 
   
5. %% Define a small map
   
6. % map = false(10);
   
7. map = ans;
   
8. % Add an obstacle
   
9. % map (1:5, 6) = true;
   
10. map = logical(map);
    
11. start_coords = [1, 1];
    
12. dest_coords  = [40, 20];
    
13. 
    
14. %%
    
15. close all;
    
16. %   [route, numExpanded] = DijkstraGrid (map, start_coords, dest_coords);
    
17. % Uncomment following line to run Astar
    
18.   [route, numExpanded] = AStarGrid (map, start_coords, dest_coords);
    
19. 
    
20. %HINT: With default start and destination coordinates defined above, numExpanded for Dijkstras should be 76, numExpanded for Astar should be 23.

复制代码

AStarGrid.m

1. 
   
2. function [route,numExpanded] = AStarGrid (input_map, start_coords, dest_coords)
   
3. % Run A* algorithm on a grid.
   
4. % Inputs :
   
5. %   input_map : a logical array where the freespace cells are false or 0 and
   
6. %   the obstacles are true or 1
   
7. %   start_coords and dest_coords : Coordinates of the start and end cell
   
8. %   respectively, the first entry is the row and the second the column.
   
9. % Output :
   
10. %    route : An array containing the linear indices of the cells along the
    
11. %    shortest route from start to dest or an empty array if there is no
    
12. %    route. This is a single dimensional vector
    
13. %    numExpanded: Remember to also return the total number of nodes
    
14. %    expanded during your search. Do not count the goal node as an expanded node.
    
15. 
    
16. % set up color map for display用一个map矩阵来表示每个点的状态
    
17. % 1 - white - clear cell
    
18. % 2 - black - obstacle
    
19. % 3 - red = visited 相当于CLOSED列表的作用
    
20. % 4 - blue  - on list 相当于OPEN列表的作用
    
21. % 5 - green - start
    
22. % 6 - yellow - destination
    
23. 
    
24. cmap = [1 1 1; ...
    
25.     0 0 0; ...
    
26.     1 0 0; ...
    
27.     0 0 1; ...
    
28.     0 1 0; ...
    
29.     1 1 0; ...
    
30.     0.5 0.5 0.5];
    
31. 
    
32. colormap(cmap);
    
33. 
    
34. % variable to control if the map is being visualized on every
    
35. % iteration
    
36. drawMapEveryTime = true;
    
37. 
    
38. [nrows, ncols] = size(input_map);
    
39. 
    
40. % map - a table that keeps track of the state of each grid cell用来上色的
    
41. map = zeros(nrows,ncols);
    
42. 
    
43. map(~input_map) = 1;   % Mark free cells
    
44. map(input_map)  = 2;   % Mark obstacle cells
    
45. 
    
46. % Generate linear indices of start and dest nodes将下标转换为线性的索引值
    
47. start_node = sub2ind(size(map), start_coords(1), start_coords(2));
    
48. dest_node  = sub2ind(size(map), dest_coords(1),  dest_coords(2));
    
49. 
    
50. map(start_node) = 5;
    
51. map(dest_node)  = 6;
    
52. 
    
53. % meshgrid will `replicate grid vectors' nrows and ncols to produce
    
54. % a full grid
    
55. % type `help meshgrid' in the Matlab command prompt for more information
    
56. parent = zeros(nrows,ncols);%用来记录每个节点的父节点
    
57. 
    
58. %
    
59. [X, Y] = meshgrid (1:ncols, 1:nrows);
    
60. 
    
61. xd = dest_coords(1);
    
62. yd = dest_coords(2);
    
63. 
    
64. % Evaluate Heuristic function, H, for each grid cell
    
65. % Manhattan distance用曼哈顿距离作为启发式函数
    
66. H = abs(X - xd) + abs(Y - yd);
    
67. H = H';
    
68. % Initialize cost arrays
    
69. f = Inf(nrows,ncols);
    
70. g = Inf(nrows,ncols);
    
71. 
    
72. g(start_node) = 0;
    
73. f(start_node) = H(start_node);
    
74. 
    
75. % keep track of the number of nodes that are expanded
    
76. numExpanded = 0;
    
77. 
    
78. % Main Loop
    
79. 
    
80. while true
    
81. 
    
82.     % Draw current map
    
83.     map(start_node) = 5;
    
84.     map(dest_node) = 6;
    
85. 
    
86.     % make drawMapEveryTime = true if you want to see how the
    
87.     % nodes are expanded on the grid.
    
88.     if (drawMapEveryTime)
    
89.         image(1.5, 1.5, map);
    
90.         grid on;
    
91.         axis image;
    
92.         drawnow;
    
93.     end
    
94. 
    
95.     % Find the node with the minimum f value,其中的current是index值，需要转换
    
96.     [min_f, current] = min(f(:));
    
97. 
    
98.     if ((current == dest_node) || isinf(min_f))
    
99.         break;
    
100.     end;
     
101. 
     
102.     % Update input_map
     
103.     map(current) = 3;
     
104.     f(current) = Inf; % remove this node from further consideration
     
105.     numExpanded=numExpanded+1;
     
106.     % Compute row, column coordinates of current node
     
107.     [i, j] = ind2sub(size(f), current);
     
108. 
     
109.     % *********************************************************************
     
110.     % ALL YOUR CODE BETWEEN THESE LINES OF STARS
     
111.     % Visit all of the neighbors around the current node and update the
     
112.     % entries in the map, f, g and parent arrays
     
113.     %
     
114.     action=[-1 0; 1 0; 0 -1; 0 1];%上，下，左，右
     
115.     for a=1:4
     
116.         expand=[i,j]+action(a,:);
     
117.         expand1=expand(1,1);
     
118.         expand2=expand(1,2);
     
119.         %不超出边界，不穿越障碍，不在CLOSED列表里，也不是起点，则进行扩展
     
120.         if ( expand1>=1 && expand1<=nrows && expand2>=1 && expand2<=nrows && map(expand1,expand2)~=2 && map(expand1,expand2)~=3 && map(expand1,expand2)~=5)
     
121.             if ( g(expand1,expand2)> g(i,j)+1 )
     
122.                 g(expand1,expand2)= g(i,j)+1;
     
123.                 f(expand1,expand2)= g(expand1,expand2)+H(expand1,expand2);
     
124.                 parent(expand1,expand2)=current;
     
125.                 map(expand1,expand2)=4;
     
126.             end
     
127.         end
     
128.     end
     
129.     %*********************************************************************
     
130. 
     
131. 
     
132. end
     
133. 
     
134. %% Construct route from start to dest by following the parent links
     
135. if (isinf(f(dest_node)))
     
136.     route = [];
     
137. else
     
138.     route = [dest_node];
     
139. 
     
140.     while (parent(route(1)) ~= 0)
     
141.         route = [parent(route(1)), route];
     
142.     end
     
143. 
     
144.     % Snippet of code used to visualize the map and the path
     
145.     for k = 2:length(route) - 1        
     
146.         map(route(k)) = 7;
     
147.         pause(0.1);
     
148.         image(1.5, 1.5, map);
     
149.         grid on;
     
150.         axis image;
     
151.     end
     
152. end
     
153. 
     
154. end

复制代码

DijkstraGrid.m

1. 
   
2. function [route,numExpanded] = DijkstraGrid (input_map, start_coords, dest_coords)
   
3. % Run Dijkstra's algorithm on a grid.
   
4. % Inputs :
   
5. %   input_map : a logical array where the freespace cells are false or 0 and
   
6. %   the obstacles are true or 1
   
7. %   start_coords and dest_coords : Coordinates of the start and end cell
   
8. %   respectively, the first entry is the row and the second the column.
   
9. % Output :
   
10. %    route : An array containing the linear indices of the cells along the
    
11. %    shortest route from start to dest or an empty array if there is no
    
12. %    route. This is a single dimensional vector
    
13. %    numExpanded: Remember to also return the total number of nodes
    
14. %    expanded during your search. Do not count the goal node as an expanded node.
    
15. 
    
16. 
    
17. % set up color map for display
    
18. % 1 - white - clear cell
    
19. % 2 - black - obstacle
    
20. % 3 - red = visited
    
21. % 4 - blue  - on list
    
22. % 5 - green - start
    
23. % 6 - yellow - destination
    
24. 
    
25. cmap = [1 1 1; ...
    
26.         0 0 0; ...
    
27.         1 0 0; ...
    
28.         0 0 1; ...
    
29.         0 1 0; ...
    
30.         1 1 0; ...
    
31.         0.5 0.5 0.5];
    
32. 
    
33. colormap(cmap);
    
34. 
    
35. % variable to control if the map is being visualized on every
    
36. % iteration
    
37. drawMapEveryTime = true;
    
38. 
    
39. [nrows, ncols] = size(input_map);
    
40. 
    
41. % map - a table that keeps track of the state of each grid cell
    
42. map = zeros(nrows,ncols);
    
43. 
    
44. map(~input_map) = 1;   % Mark free cells
    
45. map(input_map)  = 2;   % Mark obstacle cells
    
46. 
    
47. % Generate linear indices of start and dest nodes
    
48. start_node = sub2ind(size(map), start_coords(1), start_coords(2));
    
49. dest_node  = sub2ind(size(map), dest_coords(1),  dest_coords(2));
    
50. 
    
51. map(start_node) = 5;
    
52. map(dest_node)  = 6;
    
53. 
    
54. % Initialize distance array
    
55. distanceFromStart = Inf(nrows,ncols);
    
56. 
    
57. % For each grid cell this array holds the index of its parent
    
58. parent = zeros(nrows,ncols);
    
59. 
    
60. distanceFromStart(start_node) = 0;
    
61. 
    
62. % keep track of number of nodes expanded
    
63. numExpanded = 0;
    
64. 
    
65. % Main Loop
    
66. while true
    
67. 
    
68.     % Draw current map
    
69.     map(start_node) = 5;
    
70.     map(dest_node) = 6;
    
71. 
    
72.     % make drawMapEveryTime = true if you want to see how the
    
73.     % nodes are expanded on the grid.
    
74.     if (drawMapEveryTime)
    
75.         image(1.5, 1.5, map);
    
76.         grid on;
    
77.         axis image;
    
78.         drawnow;
    
79.     end
    
80. 
    
81.     % Find the node with the minimum distance
    
82.     [min_dist, current] = min(distanceFromStart(:));
    
83. 
    
84.     if ((current == dest_node) || isinf(min_dist))
    
85.         break;
    
86.     end;
    
87. 
    
88.     % Update map
    
89.     map(current) = 3;         % mark current node as visited
    
90.     numExpanded=numExpanded+1;
    
91.     % Compute row, column coordinates of current node
    
92.     [i, j] = ind2sub(size(distanceFromStart), current);
    
93. 
    
94.    % *********************************************************************
    
95.     % YOUR CODE BETWEEN THESE LINES OF STARS
    
96. 
    
97.     % Visit each neighbor of the current node and update the map, distances
    
98.     % and parent tables appropriately.
    
99.     action=[-1 0; 1 0; 0 -1; 0 1];%上，下，左，右
    
100.     for a=1:4
     
101.         expand=[i,j]+action(a,:);
     
102.         expand1=expand(1,1);
     
103.         expand2=expand(1,2);
     
104.         %不超出边界，不穿越障碍，不在CLOSED列表里，则进行扩展
     
105.         if ( expand1>=1 && expand1<=nrows && expand2>=1 && expand2<=ncols && map(expand1,expand2)~=2 && map(expand1,expand2)~=3 && map(expand1,expand2)~=5 )
     
106. %           if ( expand1>=1 && expand1<=nrows && expand2>=1 && expand2<=ncols && map(expand1,expand2)~=2 && map(expand1,expand2)~=3 && map(expand1,expand2)~=5)
     
107.             if ( distanceFromStart(expand1,expand2)> distanceFromStart(i,j)+1 )
     
108.                 distanceFromStart(expand1,expand2)= distanceFromStart(i,j)+1;
     
109.                 parent(expand1,expand2)=current;
     
110.                 map(expand1,expand2)=4;
     
111.             end
     
112.         end
     
113.     end
     
114.     distanceFromStart(current) = Inf; % remove this node from further consideration
     
115.     %*********************************************************************
     
116. 
     
117. end
     
118. 
     
119. %% Construct route from start to dest by following the parent links
     
120. if (isinf(distanceFromStart(dest_node)))
     
121.     route = [];
     
122. else
     
123.     route = [dest_node];
     
124. 
     
125.     while (parent(route(1)) ~= 0)
     
126.         route = [parent(route(1)), route];
     
127.     end
     
128. 
     
129.         % Snippet of code used to visualize the map and the path
     
130.     for k = 2:length(route) - 1        
     
131.         map(route(k)) = 7;
     
132.         pause(0.1);
     
133.         image(1.5, 1.5, map);
     
134.         grid on;
     
135.         axis image;
     
136.     end
     
137. end
     
138. 
     
139. end

复制代码

注： 运行环境，Matlab 2019a 版本，安装 `RTB（Robotic Tool Box）` 工具包，链接地址为，[RTB安装链接](https://petercorke.com/toolboxes/robotics-toolbox/) 。该工具包中可以运行作者大佬写到的matlab/simulink四轴历程，只需要使用指令 `sl_quadrotor` 即可。

                               
登录/注册后可看大图

                               
登录/注册后可看大图

使用方法：
在Matlab 中，使用 makemap(30) 来生成地图，通过鼠标来设置障碍形状。该例子生成了一个`30*30`的方阵，然后直接运行TestScript.m即可。其中要在TestScript.m中选择是采用A*算法，还是Dijkstra算法。同时设置起始点和终点在哪。下图显示得到的A*算法路径优化结果。其中绿色点为起点，黄色点为终点，黑色表示障碍，白色表示空闲，红色表示搜寻过，灰色表示最后规划的路径。

                               
登录/注册后可看大图

下图显示Dijkstra算法的路径优化结果：

                               
登录/注册后可看大图

对应的动态效果已经录屏，下面给出传送门（录屏水印广告请忽略）：
1. [A*路径优化算法Matlab实现](

)
2. [Dijkstra路径优化算法matlab实现](

)

通过对比可以看出：A* 算法搜索速度较快，毕竟里面有贪心算法。这在地图较大的场景应用较好。但是A*算法只能得到局部最优解，并不能保证全局最优解。相比之下，Dijkstra算法尽管搜索速度慢，但是是全局最优解。不知道两种方法结合gmapping，hector或者cartographer生成的栅格地图会是什么样的效果。后面期待尝试一下。

maxiou

这博客文章越来越6了啊，视频走起来~~~

chasing

maxiou 发表于 2020-6-10 14:04
这博客文章越来越6了啊，视频走起来~~~

amov_msq

chasing 发表于 2020-6-10 20:20

文章很溜啊 到时候做课程老师啊

chasing

amov_msq 发表于 2020-6-13 11:06
文章很溜啊 到时候做课程老师啊

还是萧齐老师讲的好，我这半吊子水平不行