路徑規劃算法學習Day3

---

前言

算法原理：參考路徑規劃算法學習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.3、20*20地圖

1.4、50*50地圖

gif太大無法上傳，後面我會完善
主要就是想對比一下，可以讓大家看到迪傑斯特拉算法的缺點

2.函數解讀

後續會在這放關於此算法中所用大的函數用法鏈接