【路径规划】基于matlab A_star算法多机器人路径规划【含Matlab源码 1251期】

原创

佛怒火莲 2021-11-08 13:44:21 ©著作权

文章标签 算法 matlab 自动驾驶 sed 搜索 文章分类 数据结构与算法人工智能

©著作权归作者所有：来自51CTO博客作者佛怒火莲的原创作品，请联系作者获取转载授权，否则将追究法律责任

二、A_star算法简介

1 A Star算法及其应用现状

进行搜索任务时提取的有助于简化搜索过程的信息被称为启发信息.启发信息经过文字提炼和公式化后转变为启发函数.启发函数可以表示自起始顶点至目标顶点间的估算距离, 也可以表示自起始顶点至目标顶点间的估算时间等.描述不同的情境、解决不同的问题所采用的启发函数各不相同.我们默认将启发函数命名为H (n) .以启发函数为策略支持的搜索方式我们称之为启发型搜索算法.在救援机器人的路径规划中, A Star算法能结合搜索任务中的环境情况, 缩小搜索范围, 提高搜索效率, 使搜索过程更具方向性、智能性, 所以A Star算法能较好地应用于机器人路径规划相关领域.

2 A Star算法流程

承接2.1节, A Star算法的启发函数是用来估算起始点到目标点的距离, 从而缩小搜索范围, 提高搜索效率.A Star算法的数学公式为:F (n) =G (n) +H (n) , 其中F (n) 是从起始点经由节点n到目标点的估计函数, G (n) 表示从起点移动到方格n的实际移动代价, H (n) 表示从方格n移动到目标点的估算移动代价.

如图2所示, 将要搜寻的区域划分成了正方形的格子, 每个格子的状态分为可通过(walkable) 和不可通过 (unwalkable) .取每个可通过方块的代价值为1, 且可以沿对角移动 (估值不考虑对角移动) .其搜索路径流程如下:

【路径规划】基于matlab A_star算法多机器人路径规划【含Matlab源码 1251期】_算法

图2 A Star算法路径规划

Step1:定义名为open和closed的两个列表;open列表用于存放所有被考虑来寻找路径的方块, closed列表用于存放不会再考虑的方块;

Step2:A为起点, B为目标点, 从起点A开始, 并将起点A放入open列表中, closed列表初始化为空;

Step3:查看与A相邻的方格n (n称为A的子点, A称为n的父点) , 可通过的方格加入到open列表中, 计算它们的F, G和H值.将A从open移除加入到closed列表中;

Step4:判断open列表是否为空, 如果是, 表示搜索失败, 如果不是, 执行下一步骤;

Step5:将n从open列表移除加入到closed列表中, 判断n是否为目标顶点B, 如果是, 表示搜索成功, 算法运行结束;

Step6:如果不是, 则扩展搜索n的子顶点:

a.如果子顶点是不可通过或在close列表中, 忽略它.

b.子顶点如果不在open列表中, 则加入open列表, 并且把当前方格设置为它的父亲, 记录该方格的F, G和H值.

Step7:跳转到步骤Step4;

Step8:循环结束, 保存路径.从终点开始, 每个方格沿着父节点移动直至起点, 即是最优路径.A Star算法流程图如图3所示.

【路径规划】基于matlab A_star算法多机器人路径规划【含Matlab源码 1251期】_自动驾驶_02

图3 A Star算法流程

三、部分源代码

%% 使用A *搜索演示

%% 参数的设定
% 定义二维地图框架
MAX_X = 10;
MAX_Y = 10;
MAP = zeros(MAX_X,MAX_Y) % 此数组存储地图的坐标和每个坐标中的对象
% 获得障碍，目标和机器人位置
% 使用输入值初始化MAP
% 障碍= -1，目标= 1，起点= 1，空格= 0
j=0;
x_val = 1;
y_val = 1;
axis([1 MAX_X+1 1 MAX_Y+1])  % 框图的大小
grid on;                     % 加入网格
hold on;                     % 与新图共存
n=0;                         % 障碍物的数量

 for BB = 1:3
%% 确定目标点
pause(1);                           % 一般是为了动态观察变化过程 pause(a)暂停a秒后执行下一条指令
h=msgbox('请使用鼠标左键选择目标');  % 显示提示窗口
uiwait(h,5);
if ishandle(h) == 1
    delete(h);
end
xlabel('请使用鼠标左键选择目标','Color','black');
but=0;                               % 这个地方可以设置成1,这样的话，统一目标点
while (but ~= 1)                     % 重复，直到没有单击“向左”按钮
    [xval,yval,but]=ginput(1);
end
xval=floor(xval);
yval=floor(yval);
xTarget=xval;                        % X目标的坐标
yTarget=yval;                        % Y目标的坐标
MAP(xval,yval)=1;                    % 初始化地图中的目标位置
plot(xval+.5,yval+.5,'gd');
text(xval+1,yval+.5,'目标点')

%% 确定障碍物
pause(2);
h=msgbox('使用鼠标左键选择障碍物，使用右键选择最后一个障碍物');
  xlabel('使用鼠标左键选择障碍物，使用右键选择最后一个障碍物','Color','blue');
uiwait(h,10);
if ishandle(h) == 1
    delete(h);
end
while but == 1
    [xval,yval,but] = ginput(1);
    xval=floor(xval);
    yval=floor(yval);
    MAP(xval,yval)=-1;             %Put on the closed list as well
    plot(xval+.5,yval+.5,'ro');
end

%% 确定起点
pause(1);
h=msgbox('请使用鼠标左键选择初始位置');
uiwait(h,5);
if ishandle(h) == 1
    delete(h);
end
xlabel('请选择初始位置','Color','black');
but=0;
while (but ~= 1)                    % 重复，直到没有单击“向左”按钮
    [xval,yval,but]=ginput(1);
    xval=floor(xval);
    yval=floor(yval);
end
xStart=xval;                       % 起始位置
yStart=yval;                    
MAP(xval,yval)=1;
 plot(xval+.5,yval+.5,'bo');
% End of obstacle-Target pickup

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LISTS USED FOR ALGORITHM  用于算法的列表
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%OPEN LIST STRUCTURE       开放列表结构
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%--------------------------------------------------------------------------
OPEN=[];
%CLOSED LIST STRUCTURE 封闭的列表结构
%--------------
%X val | Y val |
%--------------
% CLOSED=zeros(MAX_VAL,2);
CLOSED=[];

%Put all obstacles on the Closed list 将所有障碍物放在关闭列表中
k=1;   % Dummy counter
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);
%set the starting node as the first node  将起始节点设置为第一个节点
xNode=xval;
yNode=yval;
OPEN_COUNT=1;
path_cost=0;
goal_distance=distance(xNode,yNode,xTarget,yTarget);
OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distance);
OPEN(OPEN_COUNT,1)=0;
CLOSED_COUNT=CLOSED_COUNT+1;
CLOSED(CLOSED_COUNT,1)=xNode;
CLOSED(CLOSED_COUNT,2)=yNode;
NoPath=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% START ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while((xNode ~= xTarget || yNode ~= yTarget) && NoPath == 1)
%  plot(xNode+.5,yNode+.5,'go');
 exp_array=expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,MAX_X,MAX_Y);
 exp_count=size(exp_array,1);
 %UPDATE LIST OPEN WITH THE SUCCESSOR NODES  更新列表打开SUCCESSOR NODES
 %OPEN LIST FORMAT
 %--------------------------------------------------------------------------
 %IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
 %--------------------------------------------------------------------------
 %EXPANDED ARRAY FORMAT
 %--------------------------------
 %|X val |Y val ||h(n) |g(n)|f(n)|
 %--------------------------------
 for i=1:exp_count
    flag=0;
    for j=1:OPEN_COUNT
        if(exp_array(i,1) == OPEN(j,2) && exp_array(i,2) == OPEN(j,3) )
            OPEN(j,8)=min(OPEN(j,8),exp_array(i,5)); %#ok<*SAGROW>
            if OPEN(j,8)== exp_array(i,5)
                %UPDATE PARENTS,gn,hn
                OPEN(j,4)=xNode;
                OPEN(j,5)=yNode;
                OPEN(j,6)=exp_array(i,3);
                OPEN(j,7)=exp_array(i,4);
            end;%End of minimum fn check
            flag=1;
        end;%End of node check
%         if flag == 1
%             break;
    end;%End of j for
    if flag == 0
        OPEN_COUNT = OPEN_COUNT+1;
        OPEN(OPEN_COUNT,:)=insert_open(exp_array(i,1),exp_array(i,2),xNode,yNode,exp_array(i,3),exp_array(i,4),exp_array(i,5));
     end;%End of insert new element into the OPEN list
 end;%End of i for
 %END OF WHILE LOOP

 % Find out the node with the smallest fn  找出fn最小的节点
  index_min_node = min_fn(OPEN,OPEN_COUNT,xTarget,yTarget);
  if (index_min_node ~= -1)    
   %Set xNode and yNode to the node with minimum fn
   xNode=OPEN(index_min_node,2);
   yNode=OPEN(index_min_node,3);
   path_cost=OPEN(index_min_node,6);  % Update the cost of reaching the parent node 更新到达父节点的成本
  %Move the Node to list CLOSED     将节点移动到列表CLOSED
  CLOSED_COUNT=CLOSED_COUNT+1;
  CLOSED(CLOSED_COUNT,1)=xNode;
  CLOSED(CLOSED_COUNT,2)=yNode;
  OPEN(index_min_node,1)=0;
  else
                  % No path exists to the Target!!
      NoPath=0;   % Exits the loop!
  end;            % End of index_min_node check
end;
% 算法运行后，通过从最后一个节点（如果它是目标节点）开始生成最佳路径，然后识别其父节点，直到它到达起始节点。这是最佳路径

i=size(CLOSED,1);
Optimal_path=[];
xval=CLOSED(i,1);
yval=CLOSED(i,2);
i=1;
Optimal_path(i,1)=xval;
Optimal_path(i,2)=yval;

i=i+1;
if ( (xval == xTarget) && (yval == yTarget))
    inode=0;
   % Traverse OPEN and determine the parent nodes 遍历OPEN并确定父节点
   parent_x=OPEN(node_index(OPEN,xval,yval),4); % node_index returns the index of the node     node_index返回节点的索引
   parent_y=OPEN(node_index(OPEN,xval,yval),5);
   
   while( parent_x ~= xStart || parent_y ~= yStart)
           Optimal_path(i,1) = parent_x;
           Optimal_path(i,2) = parent_y;
           %Get the grandparents:-)
           inode=node_index(OPEN,parent_x,parent_y);
           parent_x=OPEN(inode,4);    % node_index returns the index of the node  node_index返回节点的索引
           parent_y=OPEN(inode,5);
           i=i+1;
    end;
 j=size(Optimal_path,1);
 % Plot the Optimal Path!  % 绘制最佳路径！
 p=plot(Optimal_path(j,1)+.5,Optimal_path(j,2)+.5,'bo');
 j=j-1;
 for i=j:-1:1
 
 h=msgbox('不好意思，没有找到目标的路径!','warn');
 uiwait(h,5);
end
Optimal_path
[m,n]=size(Optimal_path);
for ii=1:m;
%     for jj=1:1
       a=Optimal_path(ii,1)
       b=Optimal_path(ii,2)
        MAP(a,b)=1
       c=a+0.5;
       d=b+0.5;
%         Optimal_path(i,j)=1;
%     end
plot(c,d,'*k');hold on;
end



% MAP(Optimal_path(2,1),Optimal_path(2,2))=1
MAP1=rot90(MAP)
number = size(Optimal_path,1)

if (BB==1)
Optimal_path1 = Optimal_path
        hold on;
else if (BB==2)
        Optimal_path2 = Optimal_path
        hold on;
    else
        Optimal_path3 = Optimal_path
    end
end

 end