作业框架
使用math.m作为主函数入口
A_star: useful functions for A*
distance.m:
用于计算笛卡尔坐标系下任意两点的距离（此函数可以用作修改启发函数，例如将距离计算换做曼哈顿距离）
insert_open.m:
用于将node加入OPEN_LIST中，OPEN的规定格式为一个8 * 1数组：|1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
expand_array.m：
用于返回node n 周围的m个扩展node的列表，规定格式为一个m * 5 * 1数组：|X val |Y val ||h(n) |g(n)|f(n)|
min_fn.m:
用于返回OPEN列表中cost最小的node的索引
node_index.m:
用于返回OPEN列表中坐标为（xval,yval）的node的索引
A_star_search.m:
作业部分完成A_star_search.m，输出路径点列表，规定格式为n * 2 * 1数组：|x_val | y_val，可以选择使用A_start里写好的函数，也可以独立完整完成所有工作。
--------------------------------------------------
obstacle_map.m:
用于返回一个含有随意分布障碍物的地图（障碍物出现概率可以通过改变obstacle_ratio的数值大小来实现）
visualize_map.m:
用于可视化二维的栅格地图，包含障碍物、起点、终点和最优路径点
程序解析
main.m
% 清除 MATLAB 工作空间中的所有变量和图形，并将 A_star 目录添加到 MATLAB 的搜索路径中。
close all; clear all; clc;
addpath('A_star')
% 栅格地图数据
xStart = 1.0;
yStart = 1.0;
xTarget = 9.0;
yTarget = 9.0;
MAX_X = 10;
MAX_Y = 10;
% 创建随机地图
map = obstacle_map(xStart, yStart, xTarget, yTarget, MAX_X, MAX_Y);
% A*算法获取最短路径
path = A_star_search(map, MAX_X,MAX_Y);
% 画图显示结果
visualize_map(map, path, []);
% 保存地图
% save('Data/map.mat', 'map', 'MAX_X', 'MAX_Y');
obstacle_map.m
function map = obstacle_map(xStart,yStart,xTarget,yTarget,MAX_X,MAX_Y)
% This function returns a map contains random distribution obstacles.
% 随机生成MAX_X * MAX_Y大小的数组
rand_map = rand(MAX_X,MAX_Y);
% 初始化地图
map = [];
map(1,1) = xStart;
map(1,2) = yStart;
k=2;
obstacle_ratio = 0.25;
for i = 1:1:MAX_X
for j = 1:1:MAX_Y
% 如果不是起点和终点，小于obstacle_ratio的栅格设定为障碍
if( (rand_map(i,j) < obstacle_ratio) && (i~= xStart || j~=yStart) && (i~= xTarget || j~=yTarget))
map(k,1) = i;
map(k,2) = j;
k=k+1;
end
end
end
map(k,1) = xTarget;
map(k,2) = yTarget;
end
最后生成的map样式
xStart yStart
障碍1_x 障碍1_y
障碍2_x 障碍2_y
…
xTarget yTarget
distance.m
欧氏距离 x 2 + y 2 \sqrt {x^2 + y^2} x2+y2
function dist = distance(x1,y1,x2,y2)
dist=sqrt((x1-x2)^2 + (y1-y2)^2);
insert_open.m
function new_row = insert_open(xval,yval,parent_xval,parent_yval,hn,gn,fn)
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%-------------------------------------------------------------------------
new_row=[1,8];
new_row(1,1)=1;
new_row(1,2)=xval;
new_row(1,3)=yval;
new_row(1,4)=parent_xval;
new_row(1,5)=parent_yval;
new_row(1,6)=hn;
new_row(1,7)=gn;
new_row(1,8)=fn;
end
expand_array.m
function exp_array=expand_array(node_x,node_y,gn,xTarget,yTarget,CLOSED,MAX_X,MAX_Y)
%EXPANDED ARRAY FORMAT
%--------------------------------
%|X val |Y val ||h(n) |g(n)|f(n)|
%--------------------------------
% 初始化 exp_array 和 exp_count 变量，分别用于存储扩展后的节点信息和节点信息数量。
exp_array=[];
exp_count=1;
% 获取 CLOSED 列表中的元素数量 c2
c2=size(CLOSED,1);
% 使用两个嵌套循环遍历周围八个方向的节点，不包括当前节点（k != j || k != 0）
for k= 1:-1:-1 % 1, 0, -1
for j= 1:-1:-1
if (k~=j || k~=0)
% 计算相邻节点的坐标 s_x 和 s_y。
s_x = node_x+k;
s_y = node_y+j;
% 检查相邻节点是否在地图边界内，如果不在边界内，则跳过。
if( (s_x >0 && s_x <=MAX_X) && (s_y >0 && s_y <=MAX_Y))
% 使用flag变量标记该相邻节点是否在CLOSED列表中,如果在CLOSED列表中，则将flag设置为0
flag=1;
for c1=1:c2
if(s_x == CLOSED(c1,1) && s_y == CLOSED(c1,2))
flag=0;
end
end
if (flag == 1)
% |X val |Y val ||h(n) |g(n)|f(n)|
exp_array(exp_count,1) = s_x;
exp_array(exp_count,2) = s_y;
% 更新启发式函数h(n)
exp_array(exp_count,3) = distance(xTarget,yTarget,s_x,s_y);
% g(m)= g(n) + Cnm
exp_array(exp_count,4) = gn+distance(node_x,node_y,s_x,s_y);
% f(n) = h(n) + g(n)
exp_array(exp_count,5) = exp_array(exp_count,3)+exp_array(exp_count,4);
exp_count=exp_count+1;
end
end
end
end
end
min_fn.m
function i_min = min_fn(OPEN,OPEN_COUNT,xTarget,yTarget)
% 获取队列中最小cost节点
temp_array=[];
k=1;
% 指示是否找到目标节点
flag=0;
% 用于存储目标节点索引
goal_index=0;
% 使用 for 循环遍历 OPEN 列表中的每个节点
for j=1:OPEN_COUNT
% 检查当前节点是否被弹出，1为未被弹出
if (OPEN(j,1)==1)
% 将当前节点的信息（x、y、g、h、fn 和其在 OPEN 中的索引）添加到 temp_array 中
temp_array(k,:)=[OPEN(j,:) j];
% 检查当前节点是否是目标节点，如果是目标节点，将 flag 设置为 1，将目标节点的索引存储在 goal_index 中
if (OPEN(j,2)==xTarget && OPEN(j,3)==yTarget)
flag=1;
goal_index=j;
end;
k=k+1;
end;
end;
%if flag == 1 % one of the successors is the goal node so send this node
% i_min=goal_index;
%Send the index of the smallest node
%end;
% 检查 temp_array 是否为空：
if size(temp_array ~= 0)
% 如果 temp_array 不为空，找到 temp_array 中的最小 fn 值及其对应的索引
[min_fn,temp_min]=min(temp_array(:,8));
% 将 i_min 设置为 OPEN 列表中 fn 值最小的节点的索引。
i_min=temp_array(temp_min,9);
else
% 如果 temp_array 为空，将 i_min 设置为 -1，表示没有可用的路径。
i_min=-1;
end;
A_star_search.m
function path = A_star_search(map, MAX_X, MAX_Y)
%%
% 地图/障碍物/其他设置
% 预处理网格地图，添加偏移
size_map = size(map, 1);
Y_offset = 0;
X_offset = 0;
% 初始化二维网格地图数组
% 障碍物=-1，目标=0，起点=1
% 以3 * 3地图为例：
% 2 2 2
% 2 2 2
% 2 2 2
MAP = 2 * ones(MAX_X, MAX_Y);
% 使用目标位置初始化 MAP
% floor取数值下界如1.5 -> 1, -1.5 -> -2
xval = floor(map(size_map, 1)) + X_offset;
yval = floor(map(size_map, 2)) + Y_offset;
xTarget = xval;
yTarget = yval;
MAP(xval, yval) = 0;% 目标=0
% 使用障碍物位置初始化 MAP
% 遍历寻找障碍物
for i = 2: size_map - 1
xval = floor(map(i, 1)) + X_offset;
yval = floor(map(i, 2)) + Y_offset;
MAP(xval, yval) = -1;% 障碍物=-1
end
% 使用起点位置初始化 MAP
xval = floor(map(1, 1)) + X_offset;
yval = floor(map(1, 2)) + Y_offset;
xStart = xval;
yStart = yval;
MAP(xval, yval) = 1;% 起点=1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 算法使用的列表
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% OPEN 列表结构
% --------------------------------------------------------------------------
% 是否在列表中 1/0 | X 坐标 | Y 坐标 | 父节点 X 坐标 | 父节点 Y 坐标 | h(n) | g(n) | f(n) |
% --------------------------------------------------------------------------
OPEN = [];
% CLOSED 列表结构
% --------------
% X 坐标 | Y 坐标 |
% --------------
CLOSED = [];
% 将所有障碍物放入 Closed 列表
k = 1; % 虚拟计数器
for i = 1:MAX_X
for j = 1:MAX_Y
if MAP(i, j) == -1
CLOSED(k, 1) = i;
CLOSED(k, 2) = j;
k = k + 1;
end
end
end
CLOSED_COUNT = size(CLOSED, 1);
% 将初始节点设置为第一个节点
xNode = xval;% 初始节点x
yNode = yval;% 初始节点y
OPEN_COUNT = 1;
goal_distance = distance(xNode, yNode, xTarget, yTarget);% f(n)
path_cost = 0;% g(n)
OPEN(OPEN_COUNT, :) = insert_open(xNode, yNode, xNode, yNode, goal_distance, path_cost, goal_distance);
OPEN(OPEN_COUNT, 1) = 0;
CLOSED_COUNT = CLOSED_COUNT + 1;
CLOSED(CLOSED_COUNT, 1) = xNode;
CLOSED(CLOSED_COUNT, 2) = yNode;
NoPath = 1;
%%
% 这是你的作业
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 算法开始
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while (0) % 你需要决定退出循环的条件
% 完成循环体
end % 循环结束
% 一旦算法运行，最佳路径可以通过从最后一个节点开始（如果它是目标节点）
% 然后识别它的父节点，直到到达起点。这是最短路径
% 如何在 A* 搜索后获得最佳路径？
% 请完成它
path = [];
end
作业部分
% 将初始节点设置为第一个节点
xNode = xval;
yNode = yval;
OPEN_COUNT = 1;
goal_distance = distance(xNode, yNode, xTarget, yTarget);
path_cost = 0;
OPEN(OPEN_COUNT, :) = insert_open(xNode, yNode, xNode, yNode, goal_distance, path_cost, goal_distance);
% 因为第一步要识别OPEN列表是否为空，为了不在第一步就被识别失败，我取消了外部的初始节点弹出
% OPEN(OPEN_COUNT, 1) = 0;
% CLOSED_COUNT = CLOSED_COUNT + 1;
% CLOSED(CLOSED_COUNT, 1) = xNode;
% CLOSED(CLOSED_COUNT, 2) = yNode;
% 如果NoPath = 1说明没有找到路径，初始化为0
NoPath = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 算法开始
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 将初始节点设置为第一个节点
xNode = xval;
yNode = yval;
OPEN_COUNT = 1;
goal_distance = distance(xNode, yNode, xTarget, yTarget);
path_cost = 0;
OPEN(OPEN_COUNT, :) = insert_open(xNode, yNode, xNode, yNode, goal_distance, path_cost, goal_distance);
% 因为第一步要识别OPEN列表是否为空，为了不在第一步就被识别失败，我取消了外部的初始节点弹出
% OPEN(OPEN_COUNT, 1) = 0;
% CLOSED_COUNT = CLOSED_COUNT + 1;
% CLOSED(CLOSED_COUNT, 1) = xNode;
% CLOSED(CLOSED_COUNT, 2) = yNode;
% 如果NoPath = 1说明没有找到路径，初始化为0
NoPath = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 算法开始
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while (1) % 你需要决定退出循环的条件
% 完成循环体
i_min = min_fn(OPEN,OPEN_COUNT,xTarget,yTarget);
% 检测队列为空
if i_min ~= -1
% 标记节点n为已扩展
OPEN(i_min, 1) = 0;
CLOSED_COUNT = CLOSED_COUNT + 1;
CLOSED(CLOSED_COUNT, 1) = OPEN(i_min, 2);
CLOSED(CLOSED_COUNT, 2) = OPEN(i_min, 3);
xNode = OPEN(i_min, 2);
yNode = OPEN(i_min, 3);
path_cost = OPEN(i_min, 7);
% 检测节点n是不是目标点
if (xNode == xTarget && yNode == yTarget)
break;
end
else
% 队列为空
NoPath = 1;
break;
end
% 寻找节点n的所有未拓展节点m
exp_array = expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,MAX_X,MAX_Y);
for l = 1:size(exp_array,1)
for j = 1:OPEN_COUNT
if(OPEN(j, 1) == 1 && OPEN(j, 2) == exp_array(l, 1) && OPEN(j, 3) == exp_array(l, 2))
% 保证节点从启动状态起具有最小的成本
if exp_array(l, 4) <= OPEN(j, 7)
OPEN(j, :) = [1, exp_array(l, 1), exp_array(l, 2), xNode, yNode, exp_array(l, 3), exp_array(l, 4), exp_array(l, 5)];
break;
else
continue;
end
end
end
% 更新节点累积成本
OPEN_COUNT = OPEN_COUNT + 1;
OPEN(OPEN_COUNT, :) = insert_open(exp_array(l, 1), exp_array(l, 2), xNode, yNode, exp_array(l, 3), exp_array(l, 4), exp_array(l, 5));
end
end % 循环结束
% 从最后目标节点开始，然后识别它的父节点，直到到达起点。
path = [];
if NoPath ~= 1
path(1,1) = xTarget;
path(1,2) = yTarget;
n_index = node_index(OPEN,xTarget,yTarget);
k = 2;
while (1)
xval = OPEN(n_index, 4);
yval = OPEN(n_index, 5);
path(k,1) = xval;
path(k,2) = yval;
if (xval == xStart && yval == yStart)
break;
end
n_index = node_index(OPEN,xval,yval);
k = k+1;
end
end
end
% 从最后目标节点开始，然后识别它的父节点，直到到达起点。
path = [];
if NoPath ~= 1
path(1,1) = xTarget;
path(1,2) = yTarget;
n_index = node_index(OPEN,xTarget,yTarget);
k = 2;
while (1)
xval = OPEN(n_index, 4);
yval = OPEN(n_index, 5);
path(k,1) = xval;
path(k,2) = yval;
if (xval == xStart && yval == yStart)
break;
end
n_index = node_index(OPEN,xval,yval);
k = k+1;
end
end
end
运行结果：
2757
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
