一个基于Dijkstra算法的路径规划大作业

最新推荐文章于 2024-08-03 21:32:05 发布

daylight714

最新推荐文章于 2024-08-03 21:32:05 发布

阅读量1.4k

点赞数

文章标签： matlab dijkstra

本文链接：https://blog.csdn.net/daylight714/article/details/109766755

版权

基于Dijkstra算法的路径规划大作业

对医院轨道传输系统进行环境建模，通过抽象表达，集合各个路径、节点的属性，选择Dijkstra算法作为关键路径规划算法，设计一种简单的规则来解决多TV间的冲突。

背景

医院轨道物流系统
医院的轨道物流系统Track Vehicle System (TVS)是一个鲜为人知但很有潜力的研究方向，这样一个系统将住院部、急诊室、护士室、手术室、检验科、血库、中心药房等站点连接起来，利用物联网技术完成对小车的智能控制以及系统中各个实体之间的通信交互，而无需人为干涉。

问题

这里将问题抽象，并进行简化，先构造无向图，表示轨道均为双向单轨：
自定义医院站点无向图连接
为简化模型，我们设计无向图，站点1表示空车存储区，站点8表示手术室，有最高优先级，点之间的权重仅表示站点的相对轨道距离，节点10到19表示系统中的转轨器，不被选为目标点。
进一步简化模型，我们进行以下的约束：

每次产生任务，仅允许有两个，这样既不失一般性，又让任务不会过于复杂，任务起点及终点通过随机数生成，如果任务终点是手术室V8，赋予第一优先级；其他按任务产生的先后顺序。
约定每次产生任务时，随机生成3个处于空闲状态TV且正在回到TV站点的过程中，TV站点中的TV数量充足。
未获取任务的TV可通过改变路径或者在等待区等候，让任务中的TV优先通过回到总站，不会对任务中的TV产生干扰。
优先级高的行驶路径不受干扰，优先级低的通过调整路径来避免冲突。
主要冲突仅考虑两点：
(1) 当某TV到达当前转轨器时刻，它到下一个节点的路径上出现相向而来的TV或即将相向而来的TV；
(2) 出现与其同时到达某站点的TV。

冲突描述

在这里插入图片描述

算法

考虑本文的模型规模较小，故采用Dijkstra算法作为主要的搜索算法。下面设计在一次任务中，整个系统的执行流程：

定义初始节点，设有 $m$ 个可执行站点， $n$ 个转轨器，生成 $m + n$ 维的邻接矩阵。
任务出现，将新产生的3个TV作为3个新节点，取这3个节点及TV站所在节点（设在V1）这三个节点到任务出发点的最短路径节点，作为获取任务的TV，最短路径通过Dijkstra算法得到；优先级低的任务执行相同操作。
判定两路径首次出现的冲突以及冲突的种类， $k = 0$ 表示无冲突，跳到第5步； $k = 1$ 表示会同时出现在同一站点； $k = 2$ 表示两TV将相向而行。
若出现在同一站点则通过在等待区等待一个单位时长解决冲突；若两路径会在某两个节点之间产生冲突，在此处重新规划路径，重新规划的路径有两种选择方式：将冲突节点与下一节点的权重增加，增加的大小是理论上低优先级任务TV在冲突点处需要等待高优先级TV通过的单位时间，由此形成新的邻接矩阵，再搜索从冲突点到下个目标点的最短路径。执行完此步后回到第3步。等待时间 $t$ 的计算：
若高优先级TV到达节点时检测到冲突：
$t = 2*(x_2(TV_2+1)-x_2(TV_2))-TN_2+1,.$
若低优先级TV到达节点时检测到冲突：
$t=TN_1+1$
$TV_1$ 、 $TV_2$ 表示当前到站或未到下一站的前站点在原路径中的序号；
$TV_1$ 、 $TV_2$ 表示与下一站的距离。
整理两条路径向量，以累计距离为横坐标，节点序号为纵坐标，由于TV的运动是缓慢的匀速运动，累计距离即相当于时间。

结果

这里取一个典型例子，运行时的MATLAB运行结果
高优先级任务：V2—>V8；低优先级任务：V8—>V3；
三个随机TV产生在[7,16]、[13,19]、[11,13]节点之间

在这里插入图片描述

在这里插入图片描述
从时序图可以看到，第一次冲突会在V12处产生，可通过在等待区等待1个单位时长解决冲突；但是下一次冲突会在V10和V12之间产生，于是重新规划路径，在V10处更换路径来解决冲突。

在这里插入图片描述

代码

主函数TVS2，主要功能是解决冲突，输出结果

clc;
clear;
%***********************************************************************************************
%通过输入子节点下标与权值产生邻接矩阵W
V{1,1}=[19;2];V{1,2}=[15;10];V{1,3}=[4 18;2 5];V{1,4}=[3 18;2 3];V{1,5}=[14 17;5 7];
V{1,6}=[7 17;5 8];V{1,7}=[6 16;5 2];V{1,8}=[10;3];V{1,9}=[10 12;8 10];
V{1,10}=[8 9 11 12 16;3 8 20 12 18];V{1,11}=[10 12 13 14;20 23 5 11];V{1,12}=[9 10 11 19;10 12 23 5];
V{1,13}=[11 18 19;5 12 10];V{1,14}=[5 11 16 17;5 11 6 15];V{1,15}=[2 18 19;10 2 5];
V{1,16}=[7 10 14;2 18 6];V{1,17}=[5 6 14;7 8 15];V{1,18}=[3 4 13 15;5 3 12 2];
V{1,19}=[1 12 13 15;2 5 10 5];
%输入可执行站点m与转轨器n的数量,第一节点V1表示空车区
m=9;n=10;
%邻接矩阵维数
DW=m+n;
W=zeros(DW);
for i=1:DW     %生成对角线元素为零，其他元素为inf的初始W矩阵
    for j=1:DW
        if (i==j)
            continue;
        end
        W(i,j)=inf;
    end
end
for i=1:DW     %通过输入的子节点下标与权值产生最终W矩阵
    if (size(V{1,i})==0)
        continue;
    end
    for j=1:size(V{1,i},2)
        W(i,V{1,i}(1,j))=V{1,i}(2,j);
    end
end
flag=0;
for i=1:DW     %检查W矩阵的正确性
    for j=i:DW
        if (W(i,j)~=W(j,i))
            flag=1;
        end
    end
end
if i==1
    fprintf('此图为有向图!\n');
else
    fprintf('此图为无向图!\n');
end
%********************************************************************************************** 
%随机产生2个任务，如果任务终点是手术室V8，赋予第一优先级；其他按任务产生的先后顺序
%优先级低的通过调整路径来避免冲突
%主要冲突仅考虑当某TV到达当前转轨器时刻，它到下一个节点的路径上出现相向而来的TV
%或者出现与其同时到达某站点的TV时判断此路不通
%随机产生任务起点与终点,起点不在TV站
a1=randperm(m-1)+1;a2=randperm(m-1)+1;
task_st1=a1(1);task_end1=a1(2);
task_st2=a2(1);task_end2=a2(2);
%当第一任务终点不是手术室且第二任务终点是手术室时，交换任务顺序
if task_end1~=8 && task_end2==8
    temp1=task_st1;task_st1=task_st2;task_st2=temp1;
    temp2=task_end1;task_end1=task_end2;task_end2=temp2;
end    
%产生3个空闲TV
TV_num=3;
%初始化空闲TV与原邻接矩阵构成的新矩阵new_W，以及所插入的位置X Y
new_W=cell(1,TV_num);X=zeros(1,TV_num);Y=zeros(1,TV_num);
for i=1:TV_num
    [new_W{i},X(i),Y(i)]=createTV(W);
end
%求最短路径，路径的累计长度以及获取任务的TV的序号，第二优先级要排除非TV站获取任务的TV的序号
[path1,x1,k1]=selectTV(W,new_W,task_st1,task_end1);
if k1~=TV_num+1
    new_W(k1)=[];
    fprintf('对于第一任务，最近的空闲小车在V%d和V%d之间产生，此小车所在节点设为V%d\n',X(k1),Y(k1),DW+1);
    fprintf('从V%d到V%d',DW+1,task_st1);
else
    fprintf('对于第一任务，最近的空闲小车在总车站V%d处产生\n',1);
    fprintf('从V%d到V%d',1,task_st1);
end
fprintf('再到V%d的最短路径为:\n',task_end1);
i=1;
while(1)
    fprintf('V%d',path1(i));
        i=i+1;
    if(i>length(path1))  %循环终止条件
        fprintf('\n');
        break;
    end
    fprintf('->');
end
fprintf('最短路径值为:%d\n',x1(length(path1)));
[path2,x2,k2]=selectTV(W,new_W,task_st2,task_end2);
%绘制最初时序图
plot(x1,path1,'-*r',x2,path2,'-*b');
%判定是否有冲突以及冲突的种类,conflict_num表示出现冲突的站点序号
%k=0表示无冲突；k=1表示会同时出现在同一站点；k=2表示两TV将相向而行
while(1)
    [k,conflict_num,t]=testconflict(path1,x1,path2,x2);
    if k==1   %若出现在同一节点则通过在等待区等待一个单位时长解决冲突
        plot(x1,path1,'-*r',x2,path2,'-*b');
        for i=conflict_num:length(x2)
            x2(i)=x2(i)+1;
        end
        fprintf('任务TV将同时出现在同一站点V%d,通过在等待区等待一个单位时长解决冲突\n',path2(conflict_num));
        %解决一次冲突绘制一次时序图
        figure;
        plot(x1,path1,'-*r',x2,path2,'-*b');
    elseif k==2  %若两路径会在某两个节点产生冲突，在此处重新规划路径
        %分类讨论，若目标起点在冲突点之前，则直接以目标终点为重新路径规划的目标终点
        %若目标起点在冲突点之后，则要直接先以目标起点为重新路径规划的目标终点
        [~,st_temp]=find(path2==task_st2); %获取目标起点序号
        plot(x1,path1,'-*r',x2,path2,'-*b');
        W_temp=W;
        %增加权重，若再次选择此路则相当于在此节点处等待
        W_temp(path2(conflict_num),path2(conflict_num+1))=t;
        W_temp(path2(conflict_num+1),path2(conflict_num))=t;
        fprintf('由于两路径会在V%d和V%d之间产生冲突，在V%d处重新规划路径\n',path2(conflict_num),path2(conflict_num+1),path2(conflict_num));
        if st_temp<conflict_num    
            [path_temp,~]=FuncDijkstra(W_temp,path2(conflict_num),task_end2);
            if path2(conflict_num+1)==path_temp(2)
                for i=conflict_num:length(x2)
                    x2(i)=x2(i)+t;
                end
                fprintf('选择在V%d处等待%d单位时间来解决冲突\n',path2(conflict_num),t);
            else
                %合成总路径
                path2=[path2(:,1:conflict_num),path_temp(:,2:length(path_temp))];
                x_temp=zeros(1,length(path_temp));
                x_temp(1)=x2(conflict_num);
                for i=2:length(path_temp)
                    x_temp(i)=x_temp(i-1)+W_temp(path_temp(i-1),path_temp(i));
                end
                x2=[x2(:,1:conflict_num),x_temp(:,2:length(x_temp))];
            end
        else
            [path_temp,~]=FuncDijkstra(W_temp,path2(conflict_num),task_st2);
            if path2(conflict_num+1)==path_temp(2)
                for i=conflict_num:length(x2)
                    x2(i)=x2(i)+t;
                end
                fprintf('选择在V%d处等待%d单位时间来解决冲突\n',path2(conflict_num),t);
            else
                %合成总路径
                path2=[path2(:,1:conflict_num),path_temp(:,2:length(path_temp)),path2(:,st_temp+1:length(path2))];
                x_temp=zeros(1,length(path_temp));
                x_temp(1)=x2(conflict_num);
                for i=2:length(path_temp)
                    x_temp(i)=x_temp(i-1)+W_temp(path_temp(i-1),path_temp(i));
                end
                x2=[x2(:,1:conflict_num),x_temp(:,2:length(x_temp)),x2(:,st_temp+1:length(x2))-x2(st_temp+1)+x_temp(length(x_temp))+W(path2(st_temp+1),path_temp(length(x_temp)))];
            end
        end
        %解决一次冲突绘制一次时序图
        figure;
        plot(x1,path1,'-*r',x2,path2,'-*b');
    else
        break;
    end
end
if k2~=TV_num+1 && k1<=k2
    k2=k2+1;
end
if k2==TV_num+1
    fprintf('第二任务最近的空闲小车在总车站V%d处产生\n',1);
    fprintf('从V%d到V%d',1,task_st2);
else
    fprintf('第二任务最近的空闲小车在V%d和V%d之间产生，此小车所在节点设为V%d\n',X(k2),Y(k2),DW+1);
    fprintf('从V%d到V%d',DW+1,task_st2);
end
fprintf('再到V%d的最短路径为:\n',task_end2);
i=1;
while(1)
    fprintf('V%d',path2(i));
        i=i+1;
    if(i>length(path2))  %循环终止条件
        fprintf('\n');
        break;
    end
    fprintf('->');
end
fprintf('最短路径值为:%d\n',x2(length(path2)));

下面是各个执行函数，包括FuncDijkstra.m——用来搜索图中起始节点到终节点的最短路径；createTV.m——在图中随机产生一个新节点，返回新的无向图；selectTV.m——选择包括TV站点在内的离任务点最近的TV所在节点；testconflict.m——检测两条路径是否存在冲突，仅返回首次检测到的冲突类型。

function [shortpath,Wmin]=FuncDijkstra(W,start,task)
    %W为输入的邻接矩阵，start为输入目标起点的下标，task为输入目标终点的下标
    %到目标终点的反向路径
    n=max(size(W));
    path=zeros(1,n);
    path(1)=task;
    %当起始点不是V1时交换行列
    if(start~=1)
       W([start,1],:)=W([1,start],:);
       W(:,[start,1])=W(:,[1,start]);
    end
    %当终点是V1时的变换
    flag=1;
    if(task==1)
       task=start;
       flag=2;
    end
    %定义已是P标号节点的集合S；P向量与T向量存v1到各点的权值；La向量存各节点的父节点下标
    S=zeros(1,n);P=S;
    for i=2:n
        P(i)=inf;
    end
    S(1)=1;T=P;La=P;
    k=1;  %最新P节点下标
    while(1)  %最外层循环，不断循环Dijkstra算法的二三步
        for i=1:n   %第二步
            if (W(k,i)~=0 && W(k,i)~=inf && S(i)~=1)  %满足此条件允许更新权值
                if(P(k)+W(k,i)<T(i))
                    T(i)=P(k)+W(k,i);   %更新第i节点的T值
                    La(i)=k;            %更新第i节点的父节点下标
                end
            end
        end
        for i=1:n   %将T中已经存在P中的对应权值变成无穷，取最小T时不参与运算
            if (S(i)~=0)
                T(i)=inf;
            end
        end
        [TW,k]=min(T);  %找到当前T标号中最小权值与下标,进行第三步
        if(TW==inf)      %循环终止条件
            break;
        end
        P(k)=TW;        %更新P节点下标及权重
        S(k)=1;         %更新P标号节点的集合
    end
    %输出第一点到目标终点的最短路径
    i=1;
    Wmin=0;
    while(1)
        i=i+1;
        path(i)=La(task);
        task=La(task);
        if task==0 
            break;
        end
        Wmin=Wmin+W(path(i),path(i-1));
    end
    for j=flag:i
        if path(j)==1
           path(j)=start;
           continue;
        elseif path(j)==start
           path(j)=1;
           continue;
        end
    end
    path=[path(:,1:i-1)];
    shortpath=flip(path);
end

function [new_W,x,y]=createTV(W)
    n=max(size(W));
    while(1)
        a=randperm(n);
        x=a(1);
        y=a(2);
        if(W(x,y)~=inf)
            break;
        end
    end
    b1=unidrnd(W(x,y)-1);
    b2=W(x,y)-b1;
    B=zeros(1,n);
    W(x,y)=inf;W(y,x)=inf;
    for i=1:n
        B(i)=inf;
    end
    B(x)=b1;B(y)=b2;
    temp_W=[W(:,1:n),B'];
    B=[B 0];
    new_W=[temp_W(1:n,:);B];
end

function [path,x,k]=selectTV(W,new_W,task_st,task_end)
    %输入参数分别表示原邻接矩阵、空闲TV与原邻接矩阵构成的新矩阵、任务的出发点与终点
    %输出参数path表示最短路径，x表示路径的累计长度，k获取任务的TV的序号
    %初始化
    TV_num=max(size(new_W));
    n=max(size(new_W{1}));
    path_st{TV_num}=[];Wmin_st=zeros(1,TV_num);
    for i=1:TV_num  %寻找其中最短的路径
        [path_st{i},Wmin_st(i)]=FuncDijkstra(new_W{i},n,task_st);
    end
    [path_st{TV_num+1},Wmin_st(TV_num+1)]=FuncDijkstra(W,1,task_st);
    [~,k]=min(Wmin_st);
    [path_end,~]=FuncDijkstra(W,task_st,task_end);
    %合成总路径
    path=[path_st{k}(:,1:length(path_st{k})),path_end(:,2:length(path_end))];
    x=zeros(1,length(path));
    for i=2:length(path)
        if k~=TV_num+1 && i==2
            x(i)=x(i-1)+new_W{k}(path(i-1),path(i));
        else
            x(i)=x(i-1)+W(path(i-1),path(i));
        end
    end
end

function [k,TV2,t]=testconflict(path1,x1,path2,x2)
%用来检测首次冲突发生的节点（第二条路径的节点）与冲突的种类
%定义flag表示是否到站；TV表示此刻或未到下一站的站点在path中的序号；TN表示与下一站的距离
%t表示路径2在冲突点理论上需要等待的时间单位
    flag=[1 1];TV1=1;TV2=1;TN1=x1(2);TN2=x2(2);t=0;
%k=0表示无冲突；k=1表示会同时出现在同一站点；k=2表示两TV将相向而行
    while(1)
        if x1(TV1)==x2(TV2) && path1(TV1)==path2(TV2) && path1(TV1)*path2(TV2)~=400
            k=1;
            break;
        elseif path1(TV1)==path2(TV2+1) && path2(TV2)==path1(TV1+1)
            k=2;
            %如果要等待，计算等待时间t
            if flag(1)==1
                t=2*(x2(TV2+1)-x2(TV2))-TN2+1;
            elseif flag(2)==1
                t=TN1+1;
            end
            break;
        elseif TV1+1==length(path1) || TV2+1==length(path2)
            k=0;
            break;
        end
        %取此时离下个站点更近的序号及距离
        if TN1==TN2
            TV1=TV1+1;TV2=TV2+1;
            TN1=x1(TV1+1)-x1(TV1);TN2=x2(TV2+1)-x2(TV2);
            flag=[1 1];
        elseif TN1<TN2
            TV1=TV1+1;
            TN2=TN2-TN1;TN1=x1(TV1+1)-x1(TV1);
            flag=[1 0];
        else
            TV2=TV2+1;
            TN1=TN1-TN2;TN2=x2(TV2+1)-x2(TV2);
            flag=[0 1];
        end
    end

daylight714

关注

0
点赞
踩
11

收藏

觉得还不错? 一键收藏
1
评论
一个基于Dijkstra算法的路径规划大作业

基于Dijkstra算法的路径规划大作业对医院轨道传输系统进行环境建模，通过抽象表达，集合各个路径、节点的属性，选择Dijkstra算法作为关键路径规划算法，设计一种简单的规则来解决多TV间的冲突。背景医院的轨道物流系统Track Vehicle System (TVS)是一个鲜为人知但很有潜力的研究方向，这样一个系统将住院部、急诊室、护士室、手术室、检验科、血库、中心药房等站点连接起来，利用物联网技术完成对小车的智能控制以及系统中各个实体之间的通信交互，而无需人为干涉。问题这里将问题抽象，并进
复制链接

扫一扫