A*路径搜索超详细代码

---

前言

这里只记录代码以及代码详解，不讲解理论

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、主函数main.m

main.m文件

close all; clear all; clc;

% 初始化起点，目标点，地图大小
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);

% 测试障碍物是否加载成功
% visualize_map(map, 0);
% A*寻找路径
path = A_star_search(map, MAX_X,MAX_Y);

% 显示路径
visualize_map(map, path);

二、障碍物obstacle_map.m文件

obstacle_map.m文件

 function map = obstacle_map(xStart,yStart,xTarget,yTarget,MAX_X,MAX_Y)
% map = 起始点 +障碍物+终点
    rand_map = rand(MAX_X,MAX_Y); % 随机生成障碍物矩阵
    map(1,1) = xStart;
    map(1,2) = yStart;
    k=2;
    obstacle_ratio = 0.25; %  障碍物矩阵 < 0.25才表示真正的障碍物
    for i = 1:1:MAX_X
        for j = 1:1:MAX_Y
            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

三、可视化visualize_map.m文件

visualize_map.m文件

function visualize_map(map,path)
% 可视化，将障碍物，起始点，路径，目标点可视化
% -0.5表示将点偏至中间

    % 障碍物
    for obs_cnt = 2: size(map, 1) - 1
        scatter(map(obs_cnt, 1)-0.5,map(obs_cnt, 2)-0.5,250,155,'filled');
        hold on;
        grid on;
        axis equal;        
        axis ([0 10 0 10 ]);
        hold on;
    end
    % 起始点
    scatter(map(1, 1)-0.5, map(1, 2)-0.5,'b','*');
    hold on;
    % 目标点
	scatter(map(size(map, 1), 1)-0.5, map(size(map, 1), 2)-0.5, 'r','*');
    hold on;
    % 路径
    for path_cnt = 2:size(path,1)-1
        scatter(path(path_cnt,1) - 0.5,path(path_cnt,2) - 0.5,'b');
        hold on;
    end

end

四、A* 路径搜索A_star_search.m文件

A_star_search.m文件

function path = A_star_search(map,MAX_X,MAX_Y)
%%  

% map : 障碍物和起始点，终点信息
% MAX_X MAX_Y: 地图大小
% return : 路径

%%  初始化地图矩阵

    size_map = size(map,1);
    
    %Obstacle=-1, Target = 0, Start=1
    % 维护MAP地图
    MAP=3*(ones(MAX_X,MAX_Y));
    
    % 初始化目标点 0
    xval=floor(map(size_map, 1)) ;
    yval=floor(map(size_map, 2)) ;
    xTarget=xval;
    yTarget=yval;
    MAP(xval,yval)=0;
    
    %初始化障碍物 -1
    for i = 2: size_map-1
        xval=floor(map(i, 1)) ;
        yval=floor(map(i, 2)) ;
        MAP(xval,yval)=-1;
    end 
    
    % 初始化起始点
    xval=floor(map(1, 1)) ;
    yval=floor(map(1, 2)) ;
    xStart=xval;
    yStart=yval;
    MAP(xval,yval)=1;

    %% 
    % 维护open list
    OPEN=[];

    % 维护close list
    CLOSED=[];

    % 将障碍物添加到close list
    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);
    
    % 将起始点加入open list 
    xNode=xStart;
    yNode=yStart;
    OPEN_COUNT=1;
    goal_distance=distance(xNode,yNode,xTarget,yTarget); % fn = gn + fn, 此时gn = 0
    path_cost=0; % gn
    OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,goal_distance,path_cost,goal_distance);

    flag = 0; % 找到路径标志
    find_path = 1; % 路径回溯索引
%% 寻找路径
    while(1) % 无限循环
        if flag == 1 || OPEN_COUNT == 0
            break; % 直到找到目标点或在 open list 为空（找不到有效路径）， 退出循环
        end
        
        open_min_fn_idex= min_fn(OPEN, OPEN_COUNT); % 寻找open list,到目标点最小值fn的索引
        % 取出来的节点是否目标点
        if  (OPEN(open_min_fn_idex,1) == xTarget && OPEN(open_min_fn_idex,2) == yTarget)
            flag = 1; % 找到路径
        end
        node_x =  OPEN(open_min_fn_idex,1);
        node_y =  OPEN(open_min_fn_idex,2);

        gn = OPEN(open_min_fn_idex,4);        
        
        % 待扩展open节点  return x, y ,hn, gn, fn, parent_x, parent_y
        exp_open_array_new = expand_open_array(node_x, node_y,gn, xTarget,yTarget, CLOSED, MAX_X, MAX_Y);

        
        % 待扩展的点加入open list
        for i = 1 : size(exp_open_array_new, 1)
            in_open = 0; % 是否已经访问过标志
            % 是否已经在open节点里面
            for j = 1 : size(OPEN, 1)
                if exp_open_array_new(i, 1) == OPEN(j, 1) && exp_open_array_new(i, 2) == OPEN(j, 2)
                    in_open =1;
                    % 已存在open list，那么是否需要更新
                    if exp_open_array_new(i, 5) < OPEN(j, 5)
                         OPEN(j, :) = exp_open_array_new(i, :) ; % 更新
                    end
                    break;
                end
            end
            % 如何还没在open list， 添加新节点
            if in_open == 0
                OPEN_COUNT = OPEN_COUNT + 1;
                OPEN(OPEN_COUNT, :) = exp_open_array_new(i, :);
            end
        end
        
        % 将访问的节点和其父节点储存起来 [now_x, new_y, parent_x, parent_y]
        traj(find_path, :) = [OPEN(open_min_fn_idex, 1:2)  OPEN(open_min_fn_idex, 6:7)]; 
        find_path = find_path + 1; % 索引值增加1
        
        OPEN(open_min_fn_idex, :) = [ ]; % 移除已访问的节点
        OPEN_COUNT = OPEN_COUNT -1;  
        % 将其添加到close list
        CLOSED_COUNT = CLOSED_COUNT + 1;
        CLOSED(CLOSED_COUNT,1)=node_x;
        CLOSED(CLOSED_COUNT,2)=node_y;  
        
        
        
    end 
    %%  回溯路径
          
    path(1,:) = traj (end,1:2); % 从目标点开始回溯
    k =1;
     pin = traj(end,:); % 目标点x y，和其父节点 parent_x, parent_y
     condi = 0;
     while condi ==0
            for i = 1:length(traj)
                % 找出储存路径中父节点
                if traj(i,1) == pin(3) && traj(i,2) ==pin(4)
                    path(k+1,:) = [pin(3), pin(4)];
                     k = k+1;
                     pin = traj(i,:);
                end
            end
            % 回溯到起点
           if pin(1)== traj(1,1) &&pin(2) ==traj(1,2)
                condi =1; 
           end
     end
     path = path(1:end,:);
   
end

五、初始化open 表

insert_open.m文件

function new_node = insert_open(xNode,yNode,hn,gn,fn)
% xNode: 现在节点x
% yNode: 现在节点y
% hn: 
% gn: 
% fn: 

% return: 待插入的新节点

new_node(1,1) = xNode;
new_node(1,2) = yNode;
new_node(1,3) = hn;
new_node(1,4) = gn;
new_node(1,5) = fn;
new_node(1,6) = 0;
new_node(1,7) = 0;
end

六、取出open list 一个节点索引值

open_min_fn_idex= min_fn.m文件

function  open_min_fn_idex= min_fn(OPEN, OPEN_COUNT)
% OPEN: open表
% OPEN_COUNT: open表个数
% return: open_min_fn_idex索引
    temp_array = [];
    j = 1;
    for i = 1 : OPEN_COUNT
        temp_array(j,:) = [OPEN(i, : ) i];
        j = j + 1;
    end
    
    if (size(temp_array) ~=0)
        [~, temp_min] = min(temp_array(:,5));
        open_min_fn_idex = temp_min;
    else
        open_min_fn_idex = -1;
    end
    
end

七 扩展open list

expand_open_array.m文件

function exp_open_array = expand_open_array(node_x, node_y,gn, xTarget,yTarget, CLOSED,  MAX_X, MAX_Y)
% return x, y ,hn, gn, fn, parent_x, parent_y

    close_size = size(CLOSED, 1);
    exp_count = 1;
    exp_open_array =[];
    
    for i =1 : -1 : -1
        for j = 1 : -1 : -1
            
            if (i~=j || i~=0) 
                exp_x = node_x + i;
                exp_y = node_y + j;
                if exp_x > 0 && exp_y > 0 && exp_x < MAX_X && exp_y < MAX_Y
                    Flag = 1; % 扩展的节点不是已经在close list
                    for k = 1 : close_size
                        if exp_x == CLOSED(k, 1) && exp_y == CLOSED(k, 2)
                            Flag = 0; % 以访问
                        end
                    end
                    
                    if Flag == 1 % 未访问
                        exp_open_array(exp_count, 1)= exp_x;
                        exp_open_array(exp_count, 2) = exp_y;
                        exp_open_array(exp_count, 3) = distance(xTarget, yTarget, exp_x,exp_y);
                        exp_open_array(exp_count, 4) = gn + distance(node_x, node_y, exp_x,exp_y);
                        exp_open_array(exp_count, 5) = exp_open_array(exp_count, 3)  + exp_open_array(exp_count, 4); 
                        exp_open_array(exp_count, 6) = exp_x - i;
                        exp_open_array(exp_count, 7) = exp_y - j;
                        exp_count = exp_count + 1;
                    end
                end
            end
        end
    end                    
end

---

结果展示

增加障碍物