【路徑規(guī)劃】基于A星算法之求解最短路徑matlab GUI
一、簡介
A算法
A算法是一種典型的啟發(fā)式搜索算法，建立在Dijkstra算法的基礎(chǔ)之上，廣泛應(yīng)用于游戲地圖、現(xiàn)實世界中，用來尋找兩點之間的最短路徑。A算法最主要的是維護了一個啟發(fā)式估價函數(shù)，如式(1)所示。
f(n)=g(n)+h(n)(1)
其中，f(n)是算法在搜索到每個節(jié)點時，其對應(yīng)的啟發(fā)函數(shù)。它由兩部分組成，第一部分g(n)是起始節(jié)點到當(dāng)前節(jié)點實際的通行代價，第二部分h(n)是當(dāng)前節(jié)點到終點的通行代價的估計值。算法每次在擴展時，都選取f(n)值最小的那個節(jié)點作為最優(yōu)路徑上的下一個節(jié)點。
在實際應(yīng)用中，若以最短路程為優(yōu)化目標(biāo)，h(n)常取作當(dāng)前點到終點的歐幾里得距離(Euclidean Distance)或曼哈頓距離(Manhattan Distance)等。若令h(n)=0，表示沒有利用任何當(dāng)前節(jié)點與終點的信息，A算法就退化為非啟發(fā)的Dijkstra算法，算法搜索空間隨之變大，搜索時間變長。
A*算法步驟如下，算法維護兩個集合：P表與Q表。P表存放那些已經(jīng)搜索到、但還沒加入最優(yōu)路徑樹上的節(jié)點；Q表維護那些已加入最優(yōu)路徑樹上的節(jié)點。
（1）P表、Q表置空，將起點S加入P表，其g值置0，父節(jié)點為空，路網(wǎng)中其他節(jié)點g值置為無窮大。
（2）若P表為空，則算法失敗。否則選取P表中f值最小的那個節(jié)點，記為BT，將其加入Q表中。判斷BT是否為終點T，若是，轉(zhuǎn)到步驟（3）；否則根據(jù)路網(wǎng)拓撲屬性和交通規(guī)則找到BT的每個鄰接節(jié)點NT，進行下列步驟:
①計算NT的啟發(fā)值
f(NT)=g(NT)+h(NT)(2)
g(NT)=g(BT)+cost(BT, NT)(3)
其中,cost(BT, NT)是BT到NT的通行代價。
②如果NT在P表中，且通過式(3)計算的g值比NT原先的g值小，則將NT的g值更新為式(3)結(jié)果，并將NT的父節(jié)點設(shè)為BT。
③如果NT在Q表中，且通過式(3)計算的g值比NT原先的g值小，則將NT的g值更新為式(3)結(jié)果，將NT的父節(jié)點設(shè)為BT，并將NT移出到P表中。
④若NT既不在P表，也不在Q表中，則將NT的父節(jié)點設(shè)為BT，并將NT移到P表中。
⑤轉(zhuǎn)到步驟（2）繼續(xù)執(zhí)行。
（3）從終點T回溯，依次找到父節(jié)點，并加入優(yōu)化路徑中，直到起點S，即可得出優(yōu)化路徑。
二、源代碼
function varargout = A_GUI(varargin)
% A_GUI MATLAB code for A_GUI.fig
% ? ? ?A_GUI, by itself, creates a new A_GUI or raises the existing
% ? ? ?singleton*.
%
% ? ? ?H = A_GUI returns the handle to a new A_GUI or the handle to
% ? ? ?the existing singleton*.
%
% ? ? ?A_GUI('CALLBACK',hObject,eventData,handles,...) calls the local
% ? ? ?function named CALLBACK in A_GUI.M with the given input arguments.
%
% ? ? ?A_GUI('Property','Value',...) creates a new A_GUI or raises the
% ? ? ?existing singleton*. ?Starting from the left, property value pairs are
% ? ? ?applied to the GUI before A_GUI_OpeningFcn gets called. ?An
% ? ? ?unrecognized property name or invalid value makes property application
% ? ? ?stop. ?All inputs are passed to A_GUI_OpeningFcn via varargin.
%
% ? ? ?*See GUI Options on GUIDE's Tools menu. ?Choose "GUI allows only one
% ? ? ?instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help A_GUI
% Last Modified by GUIDE v2.5 21-Oct-2018 17:10:48
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name', ? ? ? mfilename, ...
? ? ? ? ? ? ? ? ? 'gui_Singleton', ?gui_Singleton, ...
? ? ? ? ? ? ? ? ? 'gui_OpeningFcn', @A_GUI_OpeningFcn, ...
? ? ? ? ? ? ? ? ? 'gui_OutputFcn', ?@A_GUI_OutputFcn, ...
? ? ? ? ? ? ? ? ? 'gui_LayoutFcn', ?[] , ...
? ? ? ? ? ? ? ? ? 'gui_Callback', ? []);
if nargin && ischar(varargin{1})
? ?gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
? ?[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
? ?gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT
% --- Executes just before A_GUI is made visible.
function A_GUI_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject ? ?handle to figure
% eventdata ?reserved - to be defined in a future version of MATLAB
% handles ? ?structure with handles and user data (see GUIDATA)
% varargin ? command line arguments to A_GUI (see VARARGIN)
% Choose default command line output for A_GUI
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
% UIWAIT makes A_GUI wait for user response (see UIRESUME)
% uiwait(handles.figure1);
% --- Outputs from this function are returned to the command line.
function varargout = A_GUI_OutputFcn(hObject, eventdata, handles)
% varargout ?cell array for returning output args (see VARARGOUT);
% hObject ? ?handle to figure
% eventdata ?reserved - to be defined in a future version of MATLAB
% handles ? ?structure with handles and user data (see GUIDATA)
% Get default command line output from handles structure
varargout{1} = handles.output;
% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject ? ?handle to pushbutton1 (see GCBO)
% eventdata ?reserved - to be defined in a future version of MATLAB
% handles ? ?structure with handles and user data (see GUIDATA)
% set up color map for display 生成彩色地圖
global cmap;
global map;
global n_r;
global n_c;
global state;
cmap = [1 1 1; ...% 1 -白色-無障礙
? ? ? ?0 0 0; ...% 2 -黑色-有障礙
? ? ? ?0 0.8 0; ...% 3 -綠色-已搜索
? ? ? ?0 0.4 0; ...% 4 -粉色-正在搜索
? ? ? ?0 1 1; ...% 5 -淺藍色-起始點
? ? ? ?1 1 0; ...% 6 -黃色-目標(biāo)點
? ? ? ?0 0 1]; ? % 7 -藍色-最終路徑
colormap(cmap);
%生成隨機地圖
map = zeros(n_r,n_c);
randmap = rand(n_r,n_c);
for i = 2:(sub2ind(size(randmap),n_r,n_c)-1)
? ?if (randmap(i) >= 0.75)
? ? ? ?map(i) = 2;
? ?end
end
map(1, 1) = 5; % start_coords 起點坐標(biāo)
map(n_r, n_c) = 6; % dest_coords 終點坐標(biāo)
image(1.5,1.5,map);
grid on;
axis image;
set(handles.text5,'string','隨機地圖生成完畢');
% --- Executes on button press in pushbutton2.
function pushbutton2_Callback(hObject, eventdata, handles)
% hObject ? ?handle to pushbutton2 (see GCBO)
% eventdata ?reserved - to be defined in a future version of MATLAB
% handles ? ?structure with handles and user data (see GUIDATA)
%搜索最佳路徑
global n_r;
global n_c;
global cmap;
global map;
global state;
nrows = n_r;
ncols = n_c;
start_node = sub2ind(size(map), 1, 1);
%sub2ind()函數(shù)將矩陣中的某個元素的線性序號計算出來
%線性索引號例子：2*2矩陣[1 3;中，1是第一個，5是第二個
% ? ? ? ? ? ? ? ? ? ? ? 5 7] ?，3是第三個，7是第四個
%（matlab是列優(yōu)先，不是我們通常習(xí)慣的行優(yōu)先）
dest_node = sub2ind(size(map), n_r, n_c);
% Initialize distance array 初始化距離數(shù)組
distanceFromStart = Inf(nrows,ncols);
distanceFromStart(start_node) = 0 ;
[X, Y] = meshgrid (1:ncols, 1:nrows);
H = abs(Y - n_r) + abs(X - n_c);
f = Inf(nrows,ncols);
f(start_node) = H(start_node);
% For each grid cell this array holds the index of its parent 對于每個網(wǎng)格單元，該數(shù)組都保存其父單元的索引
parent = zeros(nrows,ncols);
% Main Loop
while true
?% Draw current map
?map(start_node) = 5;
?map(dest_node) = 6;
?image(1.5, 1.5, map);
?grid on; %網(wǎng)格
?axis image; %顯示坐標(biāo)
?drawnow; %刷新屏幕
?% Find the node with the minimum distance 找到距離最短的節(jié)點
% ? [min_dist, current] = min(distanceFromStart(:));
[~, current] = min(f(:)); [min_dist, ~] = min(distanceFromStart(:));
?if ((current == dest_node) || isinf(min_dist)) %TF = isinf(A) 返回一個和A尺寸一樣的數(shù)組， 如果A中某個元素是inf ?(無窮)， 則對應(yīng)TF中元素是1， 否則TF中對應(yīng)元素是0。
? ? ? break;
?end;
?%搜索中心的索引坐標(biāo)：current,
?%搜索中心與起始點的路程：min_dist
?% 這兩個值后面會用。
?map(current) = 3;
% ? distanceFromStart(current) = Inf;
f(current) = Inf;
?[i, j] = ind2sub(size(distanceFromStart), current); %索引號變?yōu)樽鴺?biāo)
?neighbor = [i-1,j;
? ? ? ? ? ? ?i+1,j;
? ? ? ? ? ? ?i,j+1;
? ? ? ? ? ? ?i,j-1];
? ?outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows)+(neighbor(:,2)<1) + (neighbor(:,2)>ncols);
? ?locate = find(outRangetest>0); ?%返回outRangetest中大于0的元素的相對應(yīng)的線性索引值。
? ?neighbor(locate,:)=[];
? ?neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2));
for i=1:length(neighborIndex)
if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5)
? ? map(neighborIndex(i)) = 4;
? if (distanceFromStart(neighborIndex(i))>= min_dist + 1 ) ? ?
? ? ? distanceFromStart(neighborIndex(i)) = min_dist+1;
? ? ? ? parent(neighborIndex(i)) = current; ?
? ? ? ? f(neighborIndex(i)) = H(neighborIndex(i));
? ? ? ?% pause(0.02);
? end
?end
end
end
% %%
if (isinf(distanceFromStart(dest_node)))
? ? %route = [];
? ? disp('路徑搜索失敗');
? ? set(handles.text5,'string','路徑搜索失敗');
else
? ? %提取路線坐標(biāo)
? ? set(handles.text5,'string','路徑搜索成功');
? ? route = [dest_node];
? ? ? while (parent(route(1)) ~= 0)
? ? ? ? ? ? ? route(1);
? ? ? ? ? ? ? parent(route(1))
? ? ? ? ? ? ? route = [parent(route(1)), route] ;
? ? ? ?end
三、運行結(jié)果
