前言

算法原理：參考路徑規劃算法學習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、A*（Astar）算法

2.1、原理

A*（A-Star)算法是一種靜態路網中求解最短路徑最有效的直接搜索方法，也是解決許多搜索問題的有效算法。

算法中的距離估算值與實際值越接近，最終搜索速度越快。

公式表示為：f(n)=g(n)+h(n)。

其中：

f(n) ：是從初始狀態經由狀態n到目標狀態的代價估計，

g(n)：是在狀態空間中從初始狀態到狀態n的實際代價，

h(n)：是從狀態n到目標狀態的最佳路徑的估計代價。

對于路徑搜索問題，狀態就是圖中的節點，代價就是距離

h(n)的選取保證找到最短路徑（最優解的）條件，關鍵在于估價函數f(n)的選?。ɑ蛘哒fh(n)的選取）。

我們以d(n)表達狀態n到目標狀態的距離，那么h(n)的選取大致有如下三種情況：

如果h(n)< d(n)到目標狀態的實際距離，這種情況下，搜索的點數多，搜索范圍大，效率低。但能得到最優解。

如果h(n)=d(n)，即距離估計h(n)等于最短距離，那么搜索將嚴格沿著最短路徑進行， 此時的搜索效率是最高的。

如果 h(n)>d(n)，搜索的點數少，搜索范圍小，效率高，但不能保證得到最優解。

A* 算法是在迪杰斯特拉算法的基礎上進行改進的一種算法。

與之不同的是，A算法是一種啟發式搜索，不會像dijkstra算法一樣對整個地圖都進行遍歷，A算法是有方向的遍歷。

2.2、啟發式搜索

啟發式搜索(Heuristically Search)又稱為有信息搜索(Informed Search)。

它是利用問題擁有的啟發信息來引導搜索，達到減少搜索范圍、降低問題復雜度的目的。

這種利用啟發信息的搜索過程稱為啟發式搜索。

這種搜索方式優點是搜索快，提高了效率，缺點就是得到的解有可能是次優解也有可能什么都得不到。

一句話就是犧牲了精度得到了效率。

3、總結

Dijkstra與A* 對比

相同點：

兩者都是以尋找最短路徑為目的的算法。

不同點：

Dijkstra算法遍歷的時候是對4周平等對待，沒有區分的盲目進行遍歷。

A* 算法是在迪杰斯特拉算法的基礎上進行改進的一種算法。

與之不同的是，A* 算法是一種啟發式搜索，不會像dijkstra算法一樣對整個地圖都進行遍歷，A* 算法是有方向的遍歷。

它會對周圍各點進行評估，然后再進行搜索。

后續程序依舊是基于柵格進行，用matlab實現

審核編輯：湯梓紅