A* 路径规划（Matlab）

A* 路径规划的算法步骤可以参见文章

“钱程,许映秋,谈英姿. A Star算法在RoboCup救援仿真中路径规划的应用[J]. 指挥与控制学报,2017,3(3):260-264. DOI:10.3969/j.issn.2096-0204.2017.03.0260.”

 

 

 

是由起始点经由节点到目标点的估计函数，表示从起点移动到方格 的实际移动代价，表示从方格移动到目标点的估算移动代价。

由曼哈顿距离，对角线距离，欧几里得距离三种常见的形式，本文的程序用的是欧几里得距离，

下面是程序部分

clc
clear
%%===========================================================================
pic_num = 1;% 输出为gif文件用到的参数,与本文涉及的算法无关
%% =========================================================================
%创建具有障碍物的栅格地图
%矩阵中0代表黑色栅格
map = ones(20);
map(3,3:7)=0;
map(3:10,7)=0;
map(10,3:7)=0;
map(17,13:17)=0;
map(10:17,13)=0;
map(10,13:17)=0;
map(14,15)=0;
map1 = map;
map1(end + 1,end + 1) = 0;
colormap([0,0,0;1,1,1]);  % 创建颜色
%pcolor(map1');
pcolor(0.5:size(map,1) + 0.5, 0.5:size(map,2) + 0.5, map1'); % 赋予栅格颜色
set(gca,'XTick',1:size(map,1),'YTick',1:size(map,2));  % 设置坐标
axis image xy;  % 沿每个坐标轴使用相同的数据单位，保持一致
hold on
grid
axis([0,20,0,20]);
%% 
%================================================================
%A star 算法
start_point = [2,2];%起始点坐标
end_point = [15,15];%目标点坐标
openlen = 0;%open列表长度
closelen = 0;%close列表长度
open = struct([]);%虽然算法中称为列表，但实际用的是结构体，因为要包含每个点的坐标和函数值
close = struct([]);%道理同上
move = [-1,1;0,1;1,1;1,0;1,-1;0,-1;-1,-1;-1,0];%因为涉及到可以对角移动，用一个列表的形式是一种很巧妙的方法
isGoal = 0;%是否到达目标点的标志位
%起点初始化
open(1).axis = start_point;%open集的第一个元素是起始点
open(1).g = 0;
open(1).h = abs(end_point(1)-start_point(1))^2 + abs(end_point(2)-start_point(1))^2;%用到的是欧几里得距离
open(1).f = open(1).g + open(1).h;
openlen = openlen + 1;
father(1,:) = start_point;%父节点的轨迹，这是为了保存算法走过的每个点的坐标
get_son = 0;%得到一个新的子节点
current = open(1);%当前所在节点的初始化
%%
%%=========================================================================
%在地图上标出起点（绿*）和终点（红*）
plot(end_point(1),end_point(2),'r*'); hold on;
plot(start_point(1),start_point(2),'g*'); hold on;
text(start_point(1),start_point(2),'起点');
text(end_point(1),end_point(2),'终点');
%%
%%=========================================================================
%起点初始化结束，进入循环部分
for i = 1:25 %本例用25步循环足以，如果地图范围更大，可以用while循环
    if isequal(current.axis,end_point) %判断是否到达终点
        isGoal = 1; %如是，标志位置1；
        disp('Find the Goal!');
        break; %跳出循环
    else
        if openlen ~= 0 %openlen ~= 0表示还有可以继续走下去的机会，openlen == 0就见else部分
            plot(current.axis(1),current.axis(2),'bo'); hold on; %画出当前所在的节点
            x = current.axis(1); %当前节点的x坐标 
            y = current.axis(2); %当前节点的y坐标 
            temp = struct([]); %temp是当前节点所有可通行的区域,临时的结构体变量
            f1 = [];f2 = []; %定义f1和f2的目的是为了存储close集和open集所包含的节点的坐标，用结构体的形式也可以，但是不便于后续的比较和查找
            if closelen~=0
                for j=1:length(close)
                    tpp = [close(j).axis(1),close(j).axis(2)];
                    f1 = [f1;tpp]; %将close集中的每个节点的坐标以一个n*2的矩阵的形式保存下来，以f1命名
                end
            end   
            k1 = 0; %每个节点8个方向的运动可能不会都有效，这是区别于下文的j变量的临时变量
            for j = 1:8
                x1 = x + move(j,1);
                y1 = y + move(j,2); %以查询列表的形式做8个方向的运动
                if closelen~=0
                    [tmp,ind] = ismember([x1,y1],f1,'rows'); %看当前的[x1,y1]是否在close集内，'rows'表示按行比较
                    if tmp == 1  %如果新的点在close列表内，tmp == 1，则后续不执行，跳过该节点
                        continue;
                    end
                end
                if x1>0 && x1<=20 && y1>0 && y1<=20 && map(x1,y1)~=0 %防止周围的八个点越界和保证障碍点不能作为可行点
                    k1 = k1 + 1; %有一个可行点，则 k1增加1  
                    temp(k1).axis = [x1,y1]; %对于当前的节点而言，其temp包含了其所有的下一步可行点
                    temp(k1).g = get_son + 1;
                    temp(k1).h = abs(end_point(1) - x1)^2 + abs(end_point(2) - y1)^2;
                    temp(k1).f = temp(k1).g + temp(k1).h;
                else
                    disp('Cross the border or Meet Obstacle!');
                end
            end
            tpp1 = []; %tpp的作用与f1和f2的作用类似，保存open集中的所有节点的坐标，n*2的矩阵的形式
            for k = 1:length(open)
                tpp1(end+1,:) = [open(k).axis(1),open(k).axis(2)];
            end
            for j = 1:length(temp)%将当前节点所有可通行的下一步节点，如果不再open集内，就添加到open内
                tpp2 = [temp(j).axis(1),temp(j).axis(2)];
                if ismember(tpp2,tpp1,'rows')
                    continue;
                else
                    open(end+1).axis = temp(j).axis;
                    open(end).g = temp(j).g;
                    open(end).h = temp(j).h;
                    open(end).f = temp(j).f;
                    openlen = openlen + 1; % 每向open集内增加一个，openlen加1  
                end     
            end    
            for j = 1:length(temp)
                f2(j) = temp(j).f; %f2保存当前节点的temp中所有节点的f函数值，以具最小f值的节点作为下一步将要走的节点
            end
            [f3,index] = sort(f2); %找到f值最小的那个节点，sort函数默认升序排列
            index = index(1); %sort函数得到的index是一组索引，取第一个即可
            get_son = get_son + 1; %得到一个新的节点，子节点数加1
            father(get_son,:) = current.axis; %保存当前的父节点
            f4 = [];
            for j = 1:length(open)
                f4 = [f4;open(j).axis];
            end
            [~,current_index] = ismember(current.axis,f4,'rows') ;
            close(end+1).axis = open(current_index).axis; %close集中添加当前节点
            close(end).h = open(current_index).h;
            close(end).g = open(current_index).g;
            close(end).f = open(current_index).f;
            closelen = closelen + 1; %closelen加1
            open(current_index) = []; %将当前节点从open集中删掉
            openlen = openlen - 1; %openlen减1
            current = temp(index); %找到新的当前节点(子节点) 
            plot(current.axis(1),current.axis(2),'bo'); hold on;
            temp4 = ['Now:(',num2str(current.axis(1)),',',num2str(current.axis(2)),')'];
            disp(temp4); %显示部分
        else
            disp('No way to the goal!'); %如果没有可以走的下一步，就显示没有通向终点的路
            break;
        end
    end
    pause(0.5); %用于控制GIF的变化速度
    %======================================================================
    %将结果显示为GIF的部分
    F = getframe(gcf);
    I = frame2im(F);
    [I,map2] = rgb2ind(I,256);
    if pic_num == 1
        imwrite(I,map2,'global.gif','gif', 'Loopcount',inf,'DelayTime',0.2);
    else
        imwrite(I,map2,'global.gif','gif','WriteMode','append','DelayTime',0.2);
    end
    pic_num = pic_num + 1;

end
hold off;

 最后的输出结果为