阅读 209

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

一、简介

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
%存储可行的节点,朝向1234
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

文章分类
人工智能
文章标签