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

最新推荐文章于 2024-08-26 16:31:59 发布

原创

最新推荐文章于 2024-08-26 16:31:59 发布 · 1.9k 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #算法

本文演示了A星算法（A*）在MATLAB中的通用实现，包括地图数据加载、节点结构定义、启发式搜索代价函数、搜索过程及路径染色的可视化。通过不断迭代寻找起点到终点的最短路径，最终找到路径后停止搜索。虽然MATLAB实现效率不高，但展示了A*算法的基本原理。

%% 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_list
    close_list = [close_list; current_point]; 
    % 将新加入的clos