AStar算法通用实现+可视化（Matlab）
生活随笔
收集整理的這篇文章主要介紹了
AStar算法通用实现+可视化（Matlab）
小編覺(jué)得挺不錯(cuò)的,現(xiàn)在分享給大家,幫大家做個(gè)參考.
%% AStar算法通用實(shí)現(xiàn)+可視化clc, clear, close all;%% 定義結(jié)構(gòu)體% 全局變量 起點(diǎn) 終點(diǎn) 當(dāng)前點(diǎn)
global start_point end_point current_point ValueSet% 加載地圖
load("mapData01.mat")
% 地圖大小
[row, col] = size(map);% 新節(jié)點(diǎn)
NewPoint = @(a, b) struct('x', a, 'y', b);SetKeyStr = @(x, y) string(num2str([x, y], '%03d'));InvalidKeyStr = "000000";
% 新待搜索的節(jié)點(diǎn)
NewSearchPoint = @(a, b) struct('key', SetKeyStr(a, b), 'x', a, 'y', b, 'G', 0, 'H', 0, 'F', 0, 'parent_key', InvalidKeyStr);
%啟發(fā)式搜索代價(jià)函數(shù)類(lèi)型
HType = 2;
% 各類(lèi)值設(shè)定（同樣用于染色）
ValueSet = struct('passable', 255, 'impassable', 0, 'openlist', 180, 'closelist', 120, 'start', 60, 'target', 60, 'current', 30, 'path', 60);% 定義起點(diǎn) 與 終點(diǎn)
[start_point, end_point] = deal(NewPoint(2, 2), NewPoint(20, 20)); %起始/結(jié)束點(diǎn)坐標(biāo)
[map(start_point.x, start_point.y), map(end_point.x, end_point.y)] = deal(ValueSet.start, ValueSet.target);% 搜索點(diǎn)（下左右4點(diǎn) 分別為dx dy 以及移動(dòng)代價(jià)（實(shí)際為sqrt(dx*dx+dy*dy)）
SearchDxy = [0, -1, 1; -1, 0, 1; 1, 0, 1; 0, 1, 1];%% 初始化 1.起始點(diǎn)加入open_list% 初始狀態(tài)設(shè)定
open_list(1) = NewSearchPoint(start_point.x, start_point.y);
% 計(jì)算代價(jià) 根據(jù)函數(shù)： F = G + H
open_list(1).H = CalcH(start_point.x, start_point.y, end_point.x, end_point.y, HType);
open_list(1).G = 0;
open_list(1).F = open_list.H;
%待確定代價(jià)的點(diǎn)
open_list = struct2table(open_list);
%已確定代價(jià)的點(diǎn)
close_list = [];
%當(dāng)前點(diǎn)（設(shè)定為初始點(diǎn)）
current_point = open_list;
figure
% 是否發(fā)現(xiàn)路徑
b_find_path = 0; %% 2.遍歷open_list，找到最小合代價(jià)F點(diǎn)
while ~isempty(open_list)index_min_open_list_F = SearchOptimalPoint(open_list, close_list, current_point, end_point);% 最小代價(jià)F的點(diǎn)選中為當(dāng)前點(diǎn)，進(jìn)行后續(xù)open_list選取current_point = open_list(index_min_open_list_F, :); % 在open_list中將其刪除open_list(index_min_open_list_F, :) = [];% 將其加入close_listclose_list = [close_list; current_point]; % 將新加入的close_list點(diǎn)標(biāo)記（染色）map(current_point.x, current_point.y) = ValueSet.closelist; DrawMap(map); %繪圖% 3.檢查是否符合退出條件if current_point.x == end_point.x && current_point.y == end_point.yb_find_path = true;break;end% 4. 檢查當(dāng)前點(diǎn)周?chē)梢苿?dòng)點(diǎn)，并加入open_list中for search_dxy = SearchDxy'search_point = NewSearchPoint(current_point.x + search_dxy(1), current_point.y + search_dxy(2));key = SetKeyStr(search_point.x, search_point.y);% 4.1如果它是不可抵達(dá)的或者它在 close list 中，忽略它if search_point.x <= 0 || search_point.y <= 0 || map(search_point.x, search_point.y) == ValueSet.impassable || map(search_point.x, search_point.y) == ValueSet.closelistcontinue;endsearch_point = struct2table(search_point);% 移動(dòng)代價(jià)search_point.G = current_point.G + search_dxy(3); % 估算成本search_point.H = CalcH(search_point.x, search_point.y, end_point.x, end_point.y, HType); search_point.F = search_point.G + search_point.H;search_point.parent_key = current_point.key;% 判定當(dāng)前open_list中是否存在該搜索點(diǎn)index_existed_in_openlist = find(open_list.key == key, 1); % 4.2如果它不在 open list 中 把它加入 open list% 并且把當(dāng)前方格設(shè)置為它的父親 記錄該方格的 F G 和 H 值if map(search_point.x, search_point.y) ~= ValueSet.openlistopen_list = [open_list; search_point];% 將新加入的open_list點(diǎn)標(biāo)記（染色）map(search_point.x, search_point.y) = ValueSet.openlist; % 4.3如果它已經(jīng)在 open list 中 檢查這條路徑 ( 即經(jīng)由當(dāng)前方格到達(dá)它那里 ) 是否更好，用 G 值作參考% 更小的 G 值表示這是更好的路徑。如果是這樣 把它的父親設(shè)置為當(dāng)前方格 并重新計(jì)算它的 G 和 F 值else if search_point.G < open_list.G(index_existed_in_openlist)%若open_list中存在值的G值更大，表示由當(dāng)前點(diǎn)到達(dá)該值更優(yōu)，將原本儲(chǔ)存點(diǎn)的信息替換為當(dāng)前搜索點(diǎn)信息open_list(index_existed_in_openlist, :) = search_point; % 進(jìn)行替換endendend
end%% 從候選點(diǎn)中選取與目標(biāo)方向最接近的點(diǎn)
function index_min_dyaw = FindMinDyaw(base_point_start, base_point_end, candidate_point_start, candidate_point_end, b_output_single)if nargin < 5%是否只輸出唯一值b_output_single = false;endend_yaw = atan2(base_point_end.y - base_point_start.y, base_point_end.x - base_point_start.x);open_list_yaw = atan2(candidate_point_end.y - candidate_point_start.y, candidate_point_end.x - candidate_point_start.x);dyaw = abs(LimitInPi(open_list_yaw - end_yaw));if ~b_output_singleindex_min_dyaw = find(dyaw == min(dyaw));elseindex_min_dyaw = find(dyaw == min(dyaw), 1, 'last');end
end%% 估算代價(jià)計(jì)算
% 參考文章《A*算法中啟發(fā)函數(shù)H的選取》
% h_diagonal = 沿著對(duì)角線移動(dòng)的步數(shù), h_straight = 曼哈頓距離,
% 通過(guò)考慮所有的對(duì)角線移動(dòng)的步數(shù)(每步耗散D2)以及剩下的直線移動(dòng)的步數(shù)(每步耗散D)將這兩者結(jié)合在一起.
function H = CalcH(x1, y1, x2, y2, type)if nargin < 5type = 3;enddx = x2 - x1;dy = y2 - y1;switch typecase 1% 歐式距離 乘以系數(shù)可以加快搜索距離，但可能造成無(wú)解情況H = sqrt(dx * dx + dy * dy); case 2% 曼哈頓距離H = abs(dx) + abs(dy); case 3h_diagonal = min(abs(dx), abs(dy));h_straight = abs(dx) + abs(dy);H = sqrt(2) * h_diagonal + (h_straight - 2 * h_diagonal); % 可沿對(duì)角移動(dòng)時(shí)的代價(jià)函數(shù)case 4% Dijkstra算法H = 0; end
end%% 繪制map圖
function DrawMap(map)global start_point end_point current_point ValueSet% 注意這里對(duì)map的操作只是為了顯示效果，不會(huì)影響到主函數(shù)內(nèi)的map，[map(start_point.x, start_point.y), map(end_point.x, end_point.y), map(current_point.x, current_point.y)] = deal(ValueSet.start, ValueSet.target, ValueSet.current);imagesc(map')% 注意用plot和imagesc的區(qū)別，這里加了一個(gè)轉(zhuǎn)置set(gca, 'XDir', 'normal', 'YDir', 'normal'); %如果不加normal是直接顯示A的結(jié)構(gòu)pause(0.0001);
end%% 搜索最小代價(jià)點(diǎn)
function index_min_open_list_F = SearchOptimalPoint(open_list, close_list, current_point, end_point)%找到最小代價(jià)indexindex_min_open_list_F = find(open_list.F == min(open_list.F)); %如果有多個(gè)最小代價(jià)值，按一定規(guī)則優(yōu)先選取最優(yōu)解if length(index_min_open_list_F) > 1% 起點(diǎn)時(shí)出現(xiàn)，則優(yōu)先選取同起始/結(jié)束連線夾角最接近者if height(close_list) == 1index_min_dyaw_end = FindMinDyaw(current_point, end_point, current_point, open_list(index_min_open_list_F, :), 1);index_min_open_list_F = index_min_open_list_F(index_min_dyaw_end);% 否則找到與上一刻方向最接近的點(diǎn)else index_last = find(close_list.key == current_point.parent_key, 1);last_point = close_list(index_last, :);index_min_dyaw_last = FindMinDyaw(last_point, current_point, current_point, open_list(index_min_open_list_F, :));index_min_open_list_F = index_min_open_list_F(index_min_dyaw_last);% 如果還有多個(gè)結(jié)果，優(yōu)先選取同當(dāng)前/結(jié)束連線夾角最接近者if length(index_min_open_list_F) > 1index_min_dyaw_end = FindMinDyaw(current_point, end_point, current_point, open_list(index_min_open_list_F, :), 1);index_min_open_list_F = index_min_open_list_F(index_min_dyaw_end);endendend
end%% 功能：將輸入角度范圍限制在+-pi以內(nèi)
function angle = LimitInPi(angle)% 輸入：弧度% 輸出：限制在+-pi以內(nèi)的弧度angle = mod(angle, 2 * pi); % 對(duì)2pi取余kk = find(abs(angle) > pi);if ~isempty(kk)angle(kk) = angle(kk) - sign(angle(kk)) * 2 * pi;end
end
說(shuō)明
- matlab實(shí)現(xiàn)并不實(shí)用，僅作演示，會(huì)考慮用c#的實(shí)現(xiàn)
- load("mapData01.mat")這里，地圖數(shù)據(jù)如下表
- csv數(shù)據(jù)格式，直接改擴(kuò)展名為.xlsx即可
- 參考&感謝
https://www.cnblogs.com/technology/archive/2011/05/26/2058842.html
總結(jié)
以上是生活随笔為你收集整理的AStar算法通用实现+可视化（Matlab）的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。
- 上一篇： GB/T 7714-2005《文后参考文
- 下一篇： Unity 脚本入门教程