% 用二維數組代替地圖和場地信息?
% 可用場地：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)
?
