路径规划基于matlab A_star算法智能仓储机器人移动避碰路径规划含Matlab源码 1180期

Posted 2021-08-31 紫极神光

一、简介

A算法
A算法是一种典型的启发式搜索算法，建立在Dijkstra算法的基础之上，广泛应用于游戏地图、现实世界中，用来寻找两点之间的最短路径。A算法最主要的是维护了一个启发式估价函数，如式(1)所示。
f(n)=g(n)+h(n)(1)
其中，f(n)是算法在搜索到每个节点时，其对应的启发函数。它由两部分组成，第一部分g(n)是起始节点到当前节点实际的通行代价，第二部分h(n)是当前节点到终点的通行代价的估计值。算法每次在扩展时，都选取f(n)值最小的那个节点作为最优路径上的下一个节点。
在实际应用中，若以最短路程为优化目标，h(n)常取作当前点到终点的欧几里得距离(Euclidean Distance)或曼哈顿距离(Manhattan Distance)等。若令h(n)=0，表示没有利用任何当前节点与终点的信息，A算法就退化为非启发的Dijkstra算法，算法搜索空间随之变大，搜索时间变长。
A*算法步骤如下，算法维护两个集合：P表与Q表。P表存放那些已经搜索到、但还没加入最优路径树上的节点；Q表维护那些已加入最优路径树上的节点。
（1）P表、Q表置空，将起点S加入P表，其g值置0，父节点为空，路网中其他节点g值置为无穷大。
（2）若P表为空，则算法失败。否则选取P表中f值最小的那个节点，记为BT，将其加入Q表中。判断BT是否为终点T，若是，转到步骤（3）；否则根据路网拓扑属性和交通规则找到BT的每个邻接节点NT，进行下列步骤:

①计算NT的启发值
f(NT)=g(NT)+h(NT)(2)
g(NT)=g(BT)+cost(BT, NT)(3)
其中,cost(BT, NT)是BT到NT的通行代价。
②如果NT在P表中，且通过式(3)计算的g值比NT原先的g值小，则将NT的g值更新为式(3)结果，并将NT的父节点设为BT。
③如果NT在Q表中，且通过式(3)计算的g值比NT原先的g值小，则将NT的g值更新为式(3)结果，将NT的父节点设为BT，并将NT移出到P表中。
④若NT既不在P表，也不在Q表中，则将NT的父节点设为BT，并将NT移到P表中。
⑤转到步骤（2）继续执行。
（3）从终点T回溯，依次找到父节点，并加入优化路径中，直到起点S，即可得出优化路径。

二、部分源代码

%main函数,robot_number的数目目前支持2~6；
clear all;
close all;
clear variables;
clc;
robot_number=3;
[map,grid1,Nrow,Ncol,sorting_table,Obstacle]=initial_input();
% Obstacle_index=max(round(rand(1,30)*length(Obstacle)),1);
%    Obstacle_index=randperm(length(Obstacle),10);
   Obstacle_index=[48,24,70,43,34,15,59,12,58,35,46,61, 4,79,30,63,60,31,67,37, 6,20,50,73,72,56,47,14,78,62];
mission_list=[];start_node=[];
for i=1:length(Obstacle_index)
  mission_list=[mission_list,Obstacle(Obstacle_index(i))];
end
mission=mission_list(1:robot_number);
%从N个数中不重复的选取一些数
%   start_node=Obstacle(randperm(length(Obstacle),robot_number));
   start_node=[166,352,5,12,120,220,240,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40];
for k1=1:robot_number    
    robot_ID(k1)=robot();
    robot_ID(k1).location=start_node(k1);
end

[scedule_table,undistributed_mission]=Schedule1(robot_ID,mission,Nrow,Ncol);
%scedule_table结果的第一行为任务的索引，格式大多数情况为1:length(mission), 第二行为该任务分配给的小车。
for i=1:length(scedule_table(1,:))
    end_node(scedule_table(2,i))=mission(scedule_table(1,i));
end
for k2=1:robot_number
    map3=map;
    for map_i = 1:robot_number
        if map_i~=k2
            [ia,ib]=ind2sub([Nrow,Ncol],start_node(map_i));
            map3(Ncol-ib+1,ia)=2;
            [ia1,ib1]=ind2sub([Nrow,Ncol],end_node(map_i));
            map3(Ncol-ib1+1,ia1)=2;
        end
    end
    [sorting_table_index]= assign_sorting_table(end_node(k2),sorting_table);
    allpath_ID(k2)=allpath([Astar_method(start_node(k2),end_node(k2),0,map3),Astar_method(end_node(k2),sorting_table(sorting_table_index,1),1,map3),...
    sorting_table(sorting_table_index,2:4),Astar_method(sorting_table(sorting_table_index,5),end_node(k2),1,map3)]);
end

if length(robot_ID)>1
    % 所有的两两路径冲突检验
    all_conflict=nchoosek(1:length(robot_ID), 2);
    k1=1;iters=length(all_conflict(:,1))*2;
    while k1<=length(all_conflict(:,1))&iters>0;
        if k1==1
            [wait_path1,wait_path2,result1]=Enqueue1(allpath_ID(all_conflict(k1,1)).wait_path,...
                allpath_ID(all_conflict(k1,2)).wait_path,1,map);
            [wait_path4,wait_path3,result2]=Enqueue1(allpath_ID(all_conflict(k1,2)).wait_path,...
                allpath_ID(all_conflict(k1,1)).wait_path,1,map);
            if (result1==11)&(result2==11)
                disp('前两量车无法避让，需要重新规划路径')
            else
                if (result2==11)|((result1==10)&(length(wait_path1)+length(wait_path2)<=length(wait_path3)+length(wait_path4)))
                    allpath_ID(all_conflict(k1,1)).wait_path=wait_path1;
                    allpath_ID(all_conflict(k1,2)).wait_path=wait_path2;
                else
                    allpath_ID(all_conflict(k1,1)).wait_path=wait_path3;
                    allpath_ID(all_conflict(k1,2)).wait_path=wait_path4;
                end
            end
            k1=k1+1;
        else
            [wait_path5,wait_path6,result3]=Enqueue1(allpath_ID(all_conflict(k1,1)).wait_path,...
               
            if result3==10
                allpath_ID(all_conflict(k1,1)).wait_path=wait_path5;
                allpath_ID(all_conflict(k1,2)).wait_path=wait_path6;
                k1=k1+1;
            else 
                [wait_path6,wait_path5,result3]=Enqueue1(allpath_ID(all_conflict(k1,2)).wait_path,...
         
                if result3==10
                    allpath_ID(all_conflict(k1,1)).wait_path=wait_path5;
                    allpath_ID(all_conflict(k1,2)).wait_path=wait_path6;
                    k1=1;
                else
                    if result3==12
                       
                        
                        k1=1;
%                     disp('某两量车无法避让，需要重新规划路径')
                    end
                end
            end
        end
        iters=iters-1;
        if iters==0;
            disp('超出最大迭代次数，需要重新规划路径')
        end
    end
    % 参考程序Archive中astardemo.m
%存储可行的节点，朝向1，2，3，4
function [path]=Astar_method(start_node,end_node,loading,map,stop_car)
%     Nrow=20;start_node=115;shelf_node=225;end_node=143;loading=1;
    setOpen=[start_node];setOpenCosts=[0];setOpenHeuristics=[Inf];
    setClosed=[];setClosedCosts=[];
  %load /Users/jindi/Desktop/map.txt
    Nrow=length(map(1,:));
    Ncol=length(map(:,1));
%     if 
    %为该栅格右上角的坐标
    [xlabID_end,ylabID_end]=ind2sub([Nrow,Ncol],end_node);
    %将起点和终点改为没有障碍
    map(Ncol-ylabID_end+1,xlabID_end)=0;
    if loading==0
        for i=1:(Nrow*Ncol)
            connect_node_name=[0,0,0,0];
            %为该栅格右上角的坐标
            [xlabID,ylabID]=ind2sub([Nrow,Ncol],i);
            if (xlabID<Nrow)
                if (~ismember(map(Ncol-ylabID+1,xlabID+1),[2,99]))
                    connect_node_name(1)=i+1;
                end
            end
            if (ylabID<Ncol)
                %map的坐标关系和栅格中的对于y轴和列数是相反的
                if(~ismember(map(Ncol-ylabID,xlabID),[2,99]))
                    connect_node_name(2)=i+Nrow;
                end    
            end
            if (xlabID>1)
                if (~ismember(map(Ncol-ylabID+1,xlabID-1),[2,99]))
                    connect_node_name(3)=i-1;
                end
            end
            if (ylabID>1)
                if(~ismember(map(Ncol-ylabID+2,xlabID),[2,99]))
                    connect_node_name(4)=i-Nrow;
                end
            end
            connect_node_ID(i)=connect_node([connect_node_name,connect_node_name]);
        end
    else
        for i=1:(Nrow*Ncol)
            connect_node_name=[0,0,0,0];
            %为该栅格右上角的坐标
            [xlabID,ylabID]=ind2sub([Nrow,Ncol],i);
            if (xlabID<Nrow)
                if(~ismember(map(Ncol-ylabID+1,xlabID+1),[1,2,3,99]))
                    connect_node_name(1)=i+1;
                end 
            end
            if (ylabID<Ncol)
    %             [xlabID1,ylabID1]=ind2sub([Nrow,Nrow],i+Nrow)
                if(~ismember(map(Ncol-ylabID,xlabID),[1,2,3,99]))
                    connect_node_name(2)=i+Nrow;
                end
            end
            if (xlabID>1)
                if(~ismember(map(Ncol-ylabID+1,xlabID-1),[1,2,3,99]))
                    connect_node_name(3)=i-1;
                end
            end
            if (ylabID>1)
                if(~ismember(map(Ncol-ylabID+2,xlabID),[1,2,3,99]))
                    connect_node_name(4)=i-Nrow;
                end
            end
            %为了右侧优先，设定的重复路径
            connect_node_ID(i)=connect_node([connect_node_name connect_node_name]);
        end
    end

三、运行结果

四、备注

版本：2014a