路径规划算法_路径规划算法学习Day3

路径规划算法学习Day3-Dijkstra算法实现

• 前言

• 1、Dijkstra算法

• • 1.1、地图创建

  • 1.2、matlab实现

  • 1.3、20*20地图

  • 1.4、50*50地图

• 2.函数解读

前言

算法原理：参考路径规划算法学习Day1
路径规划算法学习Day1
此方法会结合网络占用法-栅格法来进行实现

提示：本文会用matlab实现Dijkstra算法，并且会分享一些函数用法的链接，也是本人学习得来，供大家参考，批评指正

1、Dijkstra算法

1.1、地图创建

总所周知:栅格法生成地图常规是的自己一个一个打，这样既麻烦还浪费时间
这里介绍几种方法：
way1:在命令框中码：map=rand(k)>0.7 %k代表多少维地图
way2:在matlab中安装Robotics Toolbox工具箱 里有专门的函数makemap可以帮助我们生成一张地图

1.2、matlab实现

function path=DJS(Map,origin,destination)
cmap = [1 1 1; ...white
0 0 0; ...black
0 1 0; ...green
1 1 0; ...yellow
1 0 0; ...red
0 0 1; ...blue
0 1 1];
colormap(cmap);%map visualization 

[rows, cols]=size(Map);
logical_map = logical(Map);
map=zeros(rows, cols);
map(~logical_map)=1;%free
map(logical_map)=2;%obstacle
%定义一个变量node_cost_list来保存邻居以及它们到起始格的路程
%node_cost_list来保存这些信息，初始化为 Inf，表示从没有访问过。一旦有值，就说明是邻居，赋值的大小就表示该点跟起始点的路程。一旦变成红色，就把它的值再改回 Inf。
node_cost_list = Inf(rows, cols);
node_cost_list(origin(1),origin(2))=0;% set the node_cost of the origin node zero
%定义变量parent_list来保存路径
parent_list=zeros(rows, cols);% create parent_list
destination_index=sub2ind(size(Map),destination(1),destination(2));
origin_index=sub2ind(size(Map),origin(1),origin(2));

plan_success=false;
while true
map(origin(1),origin(2))=3;
map(destination(1),destination(2))=4;

image(0.5,0.5,map);
            grid on;
set(gca,'xtick',1:1:rows);
set(gca,'ytick',1:1:cols);
            axis image;
            drawnow;
%找出距离最小的节点          
%搜索中心与起始点的路程min_node,搜索中心的索引坐标：current_node,
[min_node,current_node]=min(node_cost_list(:));
if(min_node == inf || current_node == destination_index)
               plan_success=true;
break;
           end
node_cost_list(current_node) = inf;%当前搜索中心这个位置赋值为 Inf，表示它已经当过搜索中心了。min函数就不会再找这个位置
map(current_node) = 5;
[i,j]=ind2sub(size(Map),current_node);
for k = 0:3	% four direction
if(k == 0)
                    adjacent_node = [i-1,j];
                     elseif (k == 1)
                    adjacent_node = [i+1,j];
                    elseif (k == 2)
                        adjacent_node = [i,j-1];
elseif(k == 3)
                    adjacent_node = [i,j+1];
                end
if((adjacent_node(1)>0 && adjacent_node(1)<=rows) && (adjacent_node(2)>0 &&adjacent_node(2)<=cols))
if(map(adjacent_node(1),adjacent_node(2)) ~= 2 && map(adjacent_node(1),adjacent_node(2)) ~= 5)
if(node_cost_list(adjacent_node(1),adjacent_node(2)) > min_node + 1)
node_cost_list(adjacent_node(1),adjacent_node(2)) = min_node + 1;
if(map(adjacent_node(1),adjacent_node(2)) == 3)
parent_list(adjacent_node(1),adjacent_node(2)) = 0;%如果相邻节点是原点，则将父节点设置为0。
else
parent_list(adjacent_node(1),adjacent_node(2))=current_node;%否则设置当前节点为父节点
                          end
map(adjacent_node(1),adjacent_node(2)) = 6;
                       end
                    end
                end
           end
end

if(plan_success)
  path=[];
  node=destination_index;
while parent_list(node)~=0
    path=[parent_list(node),path];
    node=parent_list(node);
  end
for k = 2:size(path,2)
map(path(k)) = 7;
image(0.5,0.5,map);
    grid on;
set(gca,'xtick',1:1:rows);
set(gca,'ytick',1:1:cols);
    axis image;
    drawnow;
  end
else
   path=[];
end
end

• 1

• 2

• 3

• 4

• 5

• 6

• 7

• 8

• 9

• 10

• 11

• 12

• 13

• 14

• 15

• 16

• 17

• 18

• 19

• 20

• 21

• 22

• 23

• 24

• 25

• 26

• 27

• 28

• 29

• 30

• 31

• 32

• 33

• 34

• 35

• 36

• 37

• 38

• 39

• 40

• 41

• 42

• 43

• 44

• 45

• 46

• 47

• 48

• 49

• 50

• 51

• 52

• 53

• 54

• 55

• 56

• 57

• 58

• 59

• 60

• 61

• 62

• 63

• 64

• 65

• 66

• 67

• 68

• 69

• 70

• 71

• 72

• 73

• 74

• 75

• 76

• 77

• 78

• 79

• 80

• 81

• 82

• 83

• 84

• 85

• 86

• 87

• 88

• 89

• 90

• 91

1.3、20*20地图

1.4、50*50地图

gif太大无法上传，后面我会完善
主要就是想对比一下，可以让大家看到迪杰斯特拉算法的缺点

2.函数解读

后续会在这放关于此算法中所用大的函数用法链接