A星(A*、A Star)路径规划的代码 从matalb移植到北太天元
% 北太天元上实现A星算法,找到两点之间的最短路径，两点之间可能有障碍物，
% 两间之间的道路只能是水平或者竖直，每一步的距离一定要是dL(这里是整数1)
% 脚步不能落在障碍物上
clear all;
clf
close all
load_plugin("time")
%hold on;
%设置探索区域的边界
Estart_x = 0; Estart_y = 0;
Weight = 40; High = 40;
dL = 1; % 每一步走的最小距离
%% 生成所有的网格点
xL = 0:dL:Weight;
yL = 0:dL:High;
[XL , YL] = meshgrid(xL,yL); %
C = zeros(size(XL)); %标记点是否可以到达
%--------------------%
%边界标记为100
C(1,:) = 100; C(:,1) = 100;
C(end,:) = 100; C(:,end) = 100;
hold on
%%障碍物标记为80
%--------------------%
barrier_1 = 5:30; %障碍物的横坐标是从20到30
a1(1:length(barrier_1)) = 20; % 障碍物的纵坐标
C(barrier_1,20)=80; %
plot(barrier_1,a1,'k-*');
axis([Estart_x-5 Weight 0 High]);
axis equal;
%--------------------%
global startx
global starty
global endx;
global endy;
startx = 1 ;
starty = 2;
endx = 30;
endy = 35;
tic;
%% A星算法计算最短距离
[dis,road] = Axing_fun(startx,starty,endx,endy,Estart_x,Estart_y,Weight,High,C);
disp("A星算法计算的时间是");
toc
fprintf('得到的最短距离是%d\n',dis);
line(road(:,1),road(:,2))
text(road(1,1),road(1,2),'起点')
text(road(end,1),road(end,2),'终点')
hold off
%x 是一个cell，每一个元素是一个1x1的double
function A = cell2mat(x)
[m,n] = size(x);
A = zeros(m,n);
for k=1:m
for l=1:n
A(k,l) = x{k,l};
end
end
end
function A = sortrows(B, k)
[m,n] = size(B);
if( m <= 1 || k<0 || k >n)
A = B;
return
end
col_k = cell2mat(B(:,k));
[~, ind] = sort(col_k);
A = B(ind,:);
end
