前言

算法原理：參考路徑規(guī)劃算法學(xué)習(xí)Day1

此方法會結(jié)合網(wǎng)絡(luò)占用法-柵格法來進(jìn)行實(shí)現(xiàn)

提示：

本文會用matlab實(shí)現(xiàn)Dijkstra算法，并且會分享一些函數(shù)用法的鏈接，也是本人學(xué)習(xí)得來，供大家參考，批評指正。

1、Dijkstra算法

1.1、地圖創(chuàng)建

總所周知:柵格法生成地圖常規(guī)是的自己一個(gè)一個(gè)打，這樣既麻煩還浪費(fèi)時(shí)間。

這里介紹幾種方法：

way1:在命令框中碼：map=rand(k)>0.7 %k代表多少維地圖

way2:在matlab中安裝Robotics Toolbox工具箱 里有專門的函數(shù)makemap可以幫助我們生成一張地圖

1.2、matlab實(shí)現(xiàn)

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
%定義一個(gè)變量node_cost_list來保存鄰居以及它們到起始格的路程
%node_cost_list來保存這些信息，初始化為 Inf，表示從沒有訪問過。一旦有值，就說明是鄰居，賦值的大小就表示該點(diǎn)跟起始點(diǎn)的路程。一旦變成紅色，就把它的值再改回 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;
      %找出距離最小的節(jié)點(diǎn)     
      %搜索中心與起始點(diǎn)的路程min_node,搜索中心的索引坐標(biāo)：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;%當(dāng)前搜索中心這個(gè)位置賦值為 Inf，表示它已經(jīng)當(dāng)過搜索中心了。min函數(shù)就不會再找這個(gè)位置
      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;%如果相鄰節(jié)點(diǎn)是原點(diǎn)，則將父節(jié)點(diǎn)設(shè)置為0。
             else
               parent_list(adjacent_node(1),adjacent_node(2))=current_node;%否則設(shè)置當(dāng)前節(jié)點(diǎn)為父節(jié)點(diǎn)
             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太大無法上傳，后面我會完善。

主要就是想對比一下，可以讓大家看到迪杰斯特拉算法的缺點(diǎn)

2、A*（Astar）算法

2.1、原理

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

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

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

其中：

f(n) ：是從初始狀態(tài)經(jīng)由狀態(tài)n到目標(biāo)狀態(tài)的代價(jià)估計(jì)，

g(n)：是在狀態(tài)空間中從初始狀態(tài)到狀態(tài)n的實(shí)際代價(jià)，

h(n)：是從狀態(tài)n到目標(biāo)狀態(tài)的最佳路徑的估計(jì)代價(jià)。

對于路徑搜索問題，狀態(tài)就是圖中的節(jié)點(diǎn)，代價(jià)就是距離

h(n)的選取保證找到最短路徑（最優(yōu)解的）條件，關(guān)鍵在于估價(jià)函數(shù)f(n)的選取（或者說h(n)的選取）。

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

如果h(n)< d(n)到目標(biāo)狀態(tài)的實(shí)際距離，這種情況下，搜索的點(diǎn)數(shù)多，搜索范圍大，效率低。但能得到最優(yōu)解。

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

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

A* 算法是在迪杰斯特拉算法的基礎(chǔ)上進(jìn)行改進(jìn)的一種算法。

與之不同的是，A算法是一種啟發(fā)式搜索，不會像dijkstra算法一樣對整個(gè)地圖都進(jìn)行遍歷，A算法是有方向的遍歷。

2.2、啟發(fā)式搜索

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

它是利用問題擁有的啟發(fā)信息來引導(dǎo)搜索，達(dá)到減少搜索范圍、降低問題復(fù)雜度的目的。

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

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

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

3、總結(jié)

Dijkstra與A* 對比

相同點(diǎn)：

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

不同點(diǎn)：

Dijkstra算法遍歷的時(shí)候是對4周平等對待，沒有區(qū)分的盲目進(jìn)行遍歷。

A* 算法是在迪杰斯特拉算法的基礎(chǔ)上進(jìn)行改進(jìn)的一種算法。

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

它會對周圍各點(diǎn)進(jìn)行評估，然后再進(jìn)行搜索。

后續(xù)程序依舊是基于柵格進(jìn)行，用matlab實(shí)現(xiàn)

審核編輯：湯梓紅