AGV调整Matlab实现

% 用二维数组代替地图和场地信息 
% 可用场地：0
% 小车本身：1
% 货物点及入库点：2
% 地图边界: 100
% AGV出发区：11
% 监测区：12
% 充电区：13
% 生产区A1、A2：14
% 生产区B3、B4、B5：15
% 仓库1、4: 16
% 仓库2、3：17

% 注：以下坐标均在matlab的map矩阵中表示，下标是从[1,1]开始
% 需要注意一个问题 平常的x-y轴坐标表示方法与matlab的矩阵表示方法相反
% 四个入库点  入库A [4,16] [27,16] 入库B [16,4] [16,27] 

%  基本假设：
% （1）AGV 的行驶速度为匀速；
% （2）AGV 在转弯时的用时固定；
% （3）AGV 从货架上叉取货物的用时固定；
% （4）AGV 从自身储位上放货物的用时固定；
% （5）不考虑 AGV 自身启动和制动对速度的影响；
% 定义：小车移动一个格子的时间为1s，转向花费的时间为1s
%      小车到达取货点时取货时间1s 卸货时间1s
global UnitTime TurningTime
global PickUpTime
global DrapTime
UnitTime = 1;  TurningTime = 1;
PickUpTime = 1;  DrapTime = 1;

%% 地图初始化
[AGVMapSize, AGVMap] = mapInit();

%% 向地图中添加场地信息 -- 出发区、充电区、生产地、仓库、监测区
AGVMap = addMapVenue(AGVMap);

%% 任务分配采用随机任务分配策略
x = rand(1,12) * (4-1) + 1; % 12个任务随机分配给3辆小车
%% 向地图中添加小车及货物信息 -- 3辆小车、12个货物、4个入库点

% 注：更改小车点和货物点后需要在函数addMapGoods、函数judgeCargoShelves、函数mapPlot里面进行对应修改
[AGVInitCoorPoint, AGVStartPoint, storageA, storageB, AGVMission, AGVMap] = addMapGoods(AGVMap, x);

AGVTime = [0;0;0]; % 记录三辆小车的总任务时间
numAGV = 3;
AGVPath = cell(1,numAGV);  % AGV的初始化路径矩阵
maxRow = max([size(AGVMission{1},1), size(AGVMission{2},1), size(AGVMission{3},1)]); % 获取小车中的最大任务数
% AGVPath的第一列和第二列表示规划路径的横纵坐标
% 第三列表示是否发生转弯 0代表未发生转弯 1代表在此节点上发生了转弯
% 第四列表示花费总时间
% 第五列表示是空载还是负载 0代表空载 1代表负载低优先级任务 2代表负载高优先级任务
% 第六列表示是发生冲突的类型 1代表相交节点冲突 2代表相向节点冲突 3代表相向冲突
% 另注：如果小车不去对应的货物点取货，那么那个取货点对他来说一直是一个障碍物，卸货点也是一样的设置方式

for i = 1:maxRow
    for j = 1:numAGV
        % 执行顺序：小车1的任务1，小车2的任务1，小车3的任务1，小车1的任务2，小车2的任务2...
        if i > size(AGVMission{j},1)
            continue
        else
            fprintf('开始第%d个小车的第%d个货物的轨迹规划\n', j, i)
            % 对应的小车去对应的货物取货
            isTake = 1; % 是否取货
            isSave = 0; % 是否卸货
            start = AGVStartPoint(j,:);
            goal = AGVMission{j}(i,:);
            travelTime = AGVTime(j);
            % 取货最优路径
            AGVMap(goal(1), goal(2)) = 0; % 将目标货物点设置为可用场地
            [AGVpickUpPath, ~, travelTime] = Astar(start, goal, AGVMapSize, AGVMap, 0, isTake, isSave, travelTime);
            AGVPath{j} = [AGVPath{j};AGVpickUpPath]; % 将路径存储到路径矩阵中
            AGVMap(goal(1), goal(2)) = 1; % 将目标货物点设置为不可用场地

            % 卸货路径
            start = goal;
            travelTime = travelTime + PickUpTime;  % 取货时间1s 
            % 判断货物对应的货架
            [goal1, goal2] = judgeCargoShelves(start, storageA, storageB);
            AGVMap(goal1(1), goal1(2)) = 0; % 将目标卸货点设置为可用场地
            AGVMap(goal2(1), goal2(2)) = 0; % 将目标卸货点设置为可用场地

            % 判断此时任务点离哪一个入库点更近 进行卸货
            isTake = 0;
            isSave = 1;
            tmpTime = travelTime;
            [AGVUnLoadPath1, ~, travelTime1] = Astar(start, goal1, AGVMapSize, AGVMap, 0, isTake, isSave, tmpTime);
            [AGVUnLoadPath2, ~, travelTime2] = Astar(start, goal2, AGVMapSize, AGVMap, 0, isTake, isSave, tmpTime);
            if travelTime1 < travelTime2
                AGVPath{j} = [AGVPath{j};AGVUnLoadPath1]; % 将路径存储到路径矩阵中
                travelTime = travelTime1;
                start = goal1;
            else
                AGVPath{j} = [AGVPath{j};AGVUnLoadPath2]; % 将路径存储到路径矩阵中
                travelTime = travelTime2;
                start = goal2;
            end  
            travelTime = travelTime + DrapTime;  % 卸货时间1s
            AGVMap(goal1(1), goal1(2)) = 1; % 将目标卸货点设置为不可用场地
            AGVMap(goal2(1), goal2(2)) = 1; % 将目标卸货点设置为不可用场地

            AGVTime(j) = travelTime;
            % 如果已经执行完最后一个任务，则回到原始出发点
            if i == size(AGVMission{j},1)
                isTake = 1; 
                isSave = 0; 
                travelTime = AGVTime(j);
                goal = AGVInitCoorPoint(j,:);
                [AGVpickUpPath, ~, ~] = Astar(start, goal, AGVMapSize, AGVMap, 0, isTake, isSave, travelTime);
                AGVPath{j} = [AGVPath{j};AGVpickUpPath]; % 将路径存储到路径矩阵中
                AGVMap(goal(1), goal(2)) = 1; % 将静止的小车视为一个障碍物
            else
                % 此时代表一次任务完成，开始执行下一个小车的任务
                AGVStartPoint(j,:) = start; % 更新出发点
            end
        end
    end
end

    % 对路径进行预处理 -- 对回到初始位置的小车置为高优先级,避免发生碰撞
    AGVPath = preProcessing(AGVPath);
    
    % 时间窗算法 规划出多AGV无冲突的路径
    [AGVPath, sumTime] = timeWindow(AGVPath, AGVMap);
    
    % 对时间窗算法规划后的路径进行处理 -- 细分路径和补齐长度
    AGVPath = finalProcessing(AGVPath);
    
    %  动态展示绘图效果
    % 开始绘图
    mapPlot(AGVPath, 1);
    fprintf('总花费时间为%d秒\n', sumTime)