AStar算法通用实现+可视化（Matlab）

%% AStar算法通用实现+可视化clc, clear, close all;%% 定义结构体% 全局变量 起点 终点 当前点
global start_point end_point current_point ValueSet% 加载地图
load("mapData01.mat")
% 地图大小
[row, col] = size(map);% 新节点
NewPoint = @(a, b) struct('x', a, 'y', b);SetKeyStr = @(x, y) string(num2str([x, y], '%03d'));InvalidKeyStr = "000000";
% 新待搜索的节点
NewSearchPoint = @(a, b) struct('key', SetKeyStr(a, b), 'x', a, 'y', b, 'G', 0, 'H', 0, 'F', 0, 'parent_key', InvalidKeyStr);
%启发式搜索代价函数类型
HType = 2;
% 各类值设定（同样用于染色）
ValueSet = struct('passable', 255, 'impassable', 0, 'openlist', 180, 'closelist', 120, 'start', 60, 'target', 60, 'current', 30, 'path', 60);% 定义起点 与 终点
[start_point, end_point] = deal(NewPoint(2, 2), NewPoint(20, 20)); %起始/结束点坐标
[map(start_point.x, start_point.y), map(end_point.x, end_point.y)] = deal(ValueSet.start, ValueSet.target);% 搜索点（下左右4点 分别为dx dy 以及移动代价（实际为sqrt(dx*dx+dy*dy)）
SearchDxy = [0, -1, 1; -1, 0, 1; 1, 0, 1; 0, 1, 1];%% 初始化 1.起始点加入open_list% 初始状态设定
open_list(1) = NewSearchPoint(start_point.x, start_point.y);
% 计算代价 根据函数： 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;
%待确定代价的点
open_list = struct2table(open_list);
%已确定代价的点
close_list = [];
%当前点（设定为初始点）
current_point = open_list;
figure
% 是否发现路径
b_find_path = 0; %% 2.遍历open_list，找到最小合代价F点
while ~isempty(open_list)index_min_open_list_F = SearchOptimalPoint(open_list, close_list, current_point, end_point);% 最小代价F的点选中为当前点，进行后续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点标记（染色）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. 检查当前点周围可移动点，并加入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如果它是不可抵达的或者它在 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);% 移动代价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;% 判定当前open_list中是否存在该搜索点index_existed_in_openlist = find(open_list.key == key, 1); % 4.2如果它不在 open list 中 把它加入 open list% 并且把当前方格设置为它的父亲 记录该方格的 F  G 和 H 值if map(search_point.x, search_point.y) ~= ValueSet.openlistopen_list = [open_list; search_point];% 将新加入的open_list点标记（染色）map(search_point.x, search_point.y) = ValueSet.openlist; % 4.3如果它已经在 open list 中 检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考% 更小的 G 值表示这是更好的路径。如果是这样 把它的父亲设置为当前方格 并重新计算它的 G 和 F 值else if search_point.G < open_list.G(index_existed_in_openlist)%若open_list中存在值的G值更大，表示由当前点到达该值更优，将原本储存点的信息替换为当前搜索点信息open_list(index_existed_in_openlist, :) = search_point; % 进行替换endendend
end%% 从候选点中选取与目标方向最接近的点
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%% 估算代价计算
% 参考文章《A*算法中启发函数H的选取》
% h_diagonal = 沿着对角线移动的步数, h_straight = 曼哈顿距离,
% 通过考虑所有的对角线移动的步数(每步耗散D2)以及剩下的直线移动的步数(每步耗散D)将这两者结合在一起.
function H = CalcH(x1, y1, x2, y2, type)if nargin < 5type = 3;enddx = x2 - x1;dy = y2 - y1;switch typecase 1% 欧式距离 乘以系数可以加快搜索距离，但可能造成无解情况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); % 可沿对角移动时的代价函数case 4% Dijkstra算法H = 0; end
end%% 绘制map图
function DrawMap(map)global start_point end_point current_point ValueSet% 注意这里对map的操作只是为了显示效果，不会影响到主函数内的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的区别，这里加了一个转置set(gca, 'XDir', 'normal', 'YDir', 'normal'); %如果不加normal是直接显示A的结构pause(0.0001);
end%% 搜索最小代价点
function index_min_open_list_F = SearchOptimalPoint(open_list, close_list, current_point, end_point)%找到最小代价indexindex_min_open_list_F = find(open_list.F == min(open_list.F)); %如果有多个最小代价值，按一定规则优先选取最优解if length(index_min_open_list_F) > 1% 起点时出现，则优先选取同起始/结束连线夹角最接近者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);% 否则找到与上一刻方向最接近的点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);% 如果还有多个结果，优先选取同当前/结束连线夹角最接近者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以内
function angle = LimitInPi(angle)% 输入：弧度% 输出：限制在+-pi以内的弧度angle = mod(angle, 2 * pi); % 对2pi取余kk = find(abs(angle) > pi);if ~isempty(kk)angle(kk) = angle(kk) - sign(angle(kk)) * 2 * pi;end
end

说明

matlab实现并不实用，仅作演示，会考虑用c#的实现
load("mapData01.mat")这里，地图数据如下表
csv数据格式，直接改扩展名为.xlsx即可

0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0,0,0,255,0
0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0

参考&感谢

https://www.cnblogs.com/technology/archive/2011/05/26/2058842.html

AStar算法通用实现+可视化（Matlab）相关推荐

matlab最小费用最大流函数,Matlab最小费用最大流算法通用程序
下面的最小费用最大流算法采用的是"基于Floyd最短路算法的Ford和Fulkerson迭加算法",其基本思路为:把各条弧上单位流量的费用看成某种长度,用Floyd求最短路的方法确 ...
利用Astar算法实现飞行轨迹的三维规划（基于Matlab代码实现）
目录 1 概述 1.1研究背景 2 运行结果 3 Matlab代码实现 4 参考文献 1 概述随着自动化技术的发展,现代航空技术水平有了前所未有的提高,促进了无人机在军事.民用领域的 ...
利用Astar算法实现飞行轨迹的三维路径规划（基于Matlab代码实现）
目录 1 概述 1.1研究背景 2 运行结果 3 Matlab代码实现 4 参考文献 1 概述随着自动化技术的发展,现代航空技术水平有了前所未有的提高,促进了无人机在军事.民用领域的 ...
基于Astar算法的栅格地图最优路径搜索matlab仿真,可以修改任意数量栅格
目录 1.算法描述 2.仿真效果预览 3.MATLAB核心程序 4.完整MATLAB 1.算法描述 Astar算法是一种图形搜索算法,常用于寻路.它是个以广度优先搜索为基础,集Dijkstra算法与最 ...
matlab最小费用最大流函数,最小费用最大流算法通用Matlab程序
下面的最小费用最大流算法采用的是"基于Floyd最短路算法的Ford和Fulkerson迭加算法",其基本思路为:把各条弧上单位流量的费用看成某种长度,用Floyd求最短路的方法确 ...
基于Astar算法的栅格地图目标最短路径搜索算法MATLAB仿真,带GUI界面
目录 1.算法描述 2.仿真效果预览 3.MATLAB核心程序 4.完整MATLAB 1.算法描述 Astar算法是一种图形搜索算法,常用于寻路.它是个以广度优先搜索为基础,集Dijkstra算法与最 ...
基于Astar算法的二维栅格地图路径规划和避障MATLAB仿真
目录 1.算法仿真效果 2.MATLAB源码 3.算法概述 4.部分参考文献 1.算法仿真效果 matlab2022a仿真结果如下: 2.MATLAB源码 %********************
免疫优化算法 matlab,基于人工免疫克隆选择算法的调度优化MATLAB源码
人工免疫克隆选择算法是一种比较新型的智能算法,其基本算法结构与遗传算法是类似的,以下源码是为网络节点分组调度问题而设计的算法. function [BestX,BestY,AllABfarm,LC1, ...
A-Star算法探索和实现（一）
什么是A-Star算法? A*搜寻算法俗称A星算法,又叫A-Star算法.A*算法是比较流行的启发式搜索算法之一,被广泛应用于路径优化领域(引用自百度百科).在游戏开发中,我们可以将A*算法作为一种敌 ...
25-混合A星算法Hybrid_Astar路径规划MATLAB代码
资源: Hybrid-Astar(混合A星算法)路径规划MATLAB代码-电子商务文档类资源-CSDN文库主要内容: 以车辆的运动学模型为节点,以当前点到终点的Astar距离和RS距离两者最大的距离 ...

AStar算法通用实现+可视化（Matlab）

说明

AStar算法通用实现+可视化（Matlab）相关推荐

最新文章

热门文章