基于時間窗模型的雙向機器人路徑規劃方法

王 琛，茅 健

上海工程技術大學 機械與汽車工程學院，上海 201620

提起搬運設備，一般人首先想到的無非是叉車、卡車等傳統的搬運設備。隨著智能化與自動化的發展，制造業對自動化智能化的需求漸增，對提高制造系統的物流效率，進而提高整個生產系統的生產效率需求更為強烈，機器人搬運車應運而生[1]。雙驅雙向機器人不僅速度快、精度高、可靠性高、操作維護方便，并且沿預定軌跡行駛，因而被廣泛使用[2]。物流配送系統優化主要是進行配送車輛的合理調度[3]。優化后的配送路徑不僅大大提高了企業倉儲的運行效率，而且將人們從繁重的體力勞動中解放出來，同時也避免了安全事故的發生[4]。

宋宇、王志明[5]將RRT算法用于柵格環境下產生初始路徑，其次提出一種新的插入算子，最后進行路徑優化。黨宏社、孫心妍[6]采用遺傳算法進行機器人路徑規劃，并加入物料類型選擇的循環套，通過多次實驗確定最合理的控制參數，從而產生機器人運輸多種類型物料的最優路徑結果。李天童、寧平凡等[7]在傳統的障礙物柵格地圖中疊加了環境安全信息，構建了融合信息柵格地圖，提出了一種改進的遺傳路徑規劃算法。以上算法都是針對單機器人路徑規劃。

通常在實際生產時，需要多臺機器人同時進行貨物運輸[8]。因為受到空間和場地的影響，多機器人不能自由的穿越障礙物，所以多機器人路徑規劃就成為眾多挑戰中的一個[9]。通常所遇到的算法主要有A*算法[10-11]、遺傳算法[12-13]、模擬退火算法[14]、Dijkstra算法[15]等。孟祥忠、劉健等[16]將時間窗算法和Dijkstra算法相結合，依次規劃各機器人的路徑，并采用實時更新位置信息和時間窗排布的方法對沖突路段進行路徑動態規劃。雖然能得出最短路徑的最優解，但由于它遍歷計算的節點很多，所以效率低。袁洋、葉峰等[17]摒棄了傳統A*算法只考慮單一運行路程的評價函數，引入了運行路程結合區域負載作為新評價函數的方式。在幾乎不增大運行路程的前提下，實現了機器人運行路網的區域負載均衡。齊權、張新敏[18]在分析影響柔性生產車間內機器人運行效率因素的基礎上，建立了多機器人路徑優化數學模型，并提出一種兩階段的路徑規劃方法。孫煒等[19]以路徑分割成節點的方法對A*算法進行改進，雖然可以獲得比遺傳算法更快的解決方案，但很容易陷入一些局部極小值，而遺傳算法總是可以達到全局最優或近似全局最優。

對于多臺機器人同時行駛，不僅要使每臺機器人行駛路徑最短，用時最少，還要能夠保證機器人之間的協作問題[20]。針對以上問題，提出了一種改進的遺傳算法。在初始化總群時，盡可能使隨機生成的路徑較短，通過選擇中點的相鄰點擴大選擇范圍確保線路連續。種群適應度函數保證機器人路徑最短和改進路徑平滑度防止轉向次數過多。為了防止遺傳算法陷入局部最優解，通過輪盤賭的方法確保一部分非最優個體。在改進后的遺傳算法基礎上，對車輛沖突類型進行分類處理，明確辨別方法。根據優先級順序，并結合提出的時間窗模型，制定對應的協調方案，以實現多機器人物流系統總時間最少為目標的多機器人路徑規劃系統。

1 時間窗模型說明

為防止單車道雙向機器人系統中的沖突，有必要確定每個正在運行的機器人占據路段的時間段，即機器人系統路徑規劃的時間窗口。它的真正含義是從進入某個路徑到離開該路徑的時間段，在此期間，機器人有權延特定方向使用該路徑。在機器人路徑規劃中，時間窗算法由于其簡單性和實用性而被廣泛使用。

建立模型的前提條件：

（1）機器人可以在每個路線上沿兩個方向行駛；

（2）在路徑的特定狀態下，機器人以恒定速度行駛；

（3）為避免碰撞，沿同一方向行駛的機器人通過一定的安全措施確定機器人之間的間隔距離；

（4）忽略通過交叉點后的減速時間和加速時間；

（5）忽略機器人從等候站返回路徑的時間；

（6）任務期間忽略機器人的裝卸時間；

（7）任務的優先級可以預先設置，通常情況下優先級設置順序為任務調度順序。

基于以上假設，多機器人路徑規劃系統的時間窗口公式：

時間窗口代表一段時間段及機器人從進入路段到離開路段這段時間。根據市面上一些常見的設備如圖1，確定下列相關參數：r為彎曲路段上的轉彎半徑，取2 m；vz1為空載機器人在直線路段的速度，通常取3 m/s；vz2為空載機器人在彎曲路段的速度，通常取1.5 m/s；vl1為運貨機器人在直線路段的速度，通常取2 m/s；vl2為運貨機器人在彎曲路段的速度，通常取1 m/s；ljk表示jk路段的直線距離；表示jk路段的第m個時間窗口；表示jk路段的第m個時間窗口的開始時間；表示jk路段的第m個時間窗口的結束時間。

圖1 機器人Fig.1 Robot

2 基于時間窗的算法流程

2.1 建立模型

根據上述的條件，定義機器人物流規劃系統不能只依據最短路徑。機器人物流規劃系統的最終目標是系統所有機器人完成規定任務的時間最少。它的目標函數是：

它的約束函數是：

參數說明：n表示倉儲系統中存在的機器人數；tti表示第i臺機器人執行系統任務所需的時間；tci表示第i臺向系統輸入任務到開始執行規定任務所花費的最少時間；表示在時間窗內在路段jk上的機器人數；表示在時間窗內在路段kj上的機器人數。

公式的含義：式（3）表示執行任務的機器人不大于物流系統中的機器人數；式（4）和（5）表示在時間窗內在路段jk上的機器人數；式（6）表示路徑總數大于物流系統中機器人數；式（7）表示在時間窗內在路段jk上不存在沿相反方向行駛的機器人。

2.2 優化后的遺傳算法

遺傳算法是一種智能優化算法，主要用來解決優化問題，其主要步驟為種群初始化、適應度函數計算、選擇、交叉和變異。應用于移動機器人路徑規劃時其主要步驟，流程圖如圖2所示。下面將對每步的方法做詳細介紹。

圖2 遺傳算法流程圖Fig.2 Flow chart of genetic algorithm

2.2.1 建立柵格地圖

在對機器人進行路徑規劃時，需要先建立地圖。本文通過柵格法建立空間地圖模型，柵格越密集，空間中各種環境信息表示越精確。

2.2.2 初始化種群

初始化種群要求隨機產生多條可行路徑，可行路徑表示不與障礙物柵格相碰撞的路徑。可行路徑的產生分為兩個主要步驟。

首先產生一條間斷路徑。機器人每次行走一個柵格，因此每一行或者每一列至少有一個柵格在可行路徑中。所以初始化時先按順序在每一行或每一列隨機取出一個無障礙柵格，形成一條間斷的路徑。

第二步是將間斷的路徑連接為連續路徑。在這一步中首先從第一個柵格開始判斷相鄰的兩個柵格是否為連續柵格，柵格是否連續的判斷方法為：S=max{abs(xi+1-xi),abs(yi+1-yi)}。若S等于1則說明兩個相鄰柵格連續，反之不連續。

對于不連續的柵格取兩個柵格的中點柵格，中點柵格的坐標計算為：

若中點柵格為障礙物柵格，則取相鄰柵格。如果相鄰柵格是障礙物柵格，則刪除這條路徑。如果中點柵格或者相鄰柵格為無障礙柵格且不在路徑中則插入路徑中，繼續判斷新插入的柵格和新插入的柵格的前一個柵格是否連續，若不連續則循環以上步驟，直到兩個柵格連續。當兩個柵格連續后取下一個柵格，循環以上步驟，直到整條路徑連續。初始化一條路徑的流程圖如圖3所示。

圖3 初始化種群Fig.3 Initialized population

2.2.3 種群適應度

適應度函數分成兩部分，分別用來判斷路徑的長短和平滑程度。路徑長度的計算相對簡單，公式如下：

要求的是機器人的路徑最短，而選擇方式采用的是輪盤賭的方式，所以適應度函數的第一部分為：

機器人由于運動學和動力學的約束，行進時拐彎不宜過大，并且相對平滑的路徑有利于機器人的行駛，因此產生的路徑有平滑度的要求。路徑之間的夾角越大，用來表示路徑夾角的三個點之間的距離就越大，路徑也就越平滑，計算公式如下：

d2的值可對應路徑中連續三點的夾角的情況分別為180°和直角，其中180°的三點成一直線，平滑度最好，直角次之。對除直線外情況給予懲罰30，可以根據實際情況改動懲罰的大小。懲罰之和的倒數即為適應度函數的第二部分fit2。

適應度函數的兩部分需要取一個權重，權重公式如下：

根據路徑長度和平滑度之間的權重選擇參數a和b。

2.2.4 選擇方法

選擇方法采用簡單的基于概率的輪盤賭方法。首先計算出所有個體的適應度函數的和，再計算每一個個體所占的比率，計算公式如下：

然后根據每個個體的概率，以輪盤賭的方式選擇出下一代個體。為了防止遺傳算法陷入局部最優解，通過輪盤賭的方法確保一部分非最優個體。

2.2.5 交叉

首先需要確定一個交叉概率Pc，產生一個0到1的隨機數，并和交叉概率Pc比較，若小于Pc則進行交叉操作。交叉方式采用的是單點交叉的方式，具體的交叉操作是找出兩條路徑中所有相同的點，然后隨機選擇其中的一個點，將之后的路徑進行交叉操作。具體的流程圖如圖4所示。

2.2.6 變異

首先需要確定一個變異概率Pm，產生一個0到1的隨機數，并和變異概率Pm比較，若小于Pm則進行變異操作。變異方法是隨機選取路徑中除起點和終點以外的兩個柵格，將這兩個點進行連續操作。此時有可能無法產生連續的路徑，則需要重新選擇兩個柵格執行以上操作，直到完成變異操作。具體的流程圖如圖5所示。

圖4 交叉操作流程圖Fig.4 Crossover operation flow chart

圖5 變異操作流程圖Fig.5 Mutation operation flow chart

2.2.7 算法說明

在Matlab 2018a，i7-9700KF的CPU環境下進行仿真。改進遺傳算法的平均計算時間與路徑長度之間的關系，如表1所示。

2.3 基于時間窗的算法流程

當系統中輸入任務時，機器人路徑規劃系統都會根據任務設定的優先級對整體任務進行規劃。該算法流程是：

表1 時間與路徑長度之間的關系Table 1 Relationship between time and path length

（1）向系統中導入任務列表。

（2）找出優先級順序與時間順序相符的任務和優先級順序與時間順序不相符的任務。

（3）優先級較高的任務在執行完當前任務是否能參與次一級任務從當前停靠點到達任務出發點的運行。時間順序較早的任務在執行完當前任務是否能參與優先級較高任務從當前停靠點到達任務出發點的運行。

（4）明確各個機器人的任務分配，根據優化后的遺傳算法確定運輸線路。

（5）在步驟（4）的基礎上，判斷各個任務從開始到結束的運輸線路是否存在重合的節點。如果存在，根據重合節點數判斷沖突的類型，如果不存在，跳轉到步驟（8）。

（6）如果機器人在同一條線路上重合的節點數等于1表明可能存在交叉沖突。交叉沖突如圖6所示。通過機器人通過交叉節點的時間判斷是否存在交叉沖突。如果存在交叉沖突，次優先級任務選擇等待和重新規劃路徑，通過比較選擇較好的方案。

圖6 交叉沖突Fig.6 Cross conflict

（7）如果機器人在同一條線路上重合的節點數大于等于2且順序相同表明可能存在同向沖突，同向沖突如圖7所示。判斷所容納的數量是否小于道路承受能力，來確定是否存在同向沖突。任務，時間成本太高，對優先級高的任務進行線路規劃，路徑增加則放棄當前新路徑。以上情況都不滿足，再次對優先級低的任務選擇等待和重新進行路徑規劃，選擇出最優路徑。

圖7 同向沖突Fig.7 Co-direction conflict

（8）如果機器人在同一條線路上重合的節點數大于等于2且順序相反表明可能存在相向沖突，相向沖突如圖8所示。通過機器人通過相向沖突節點的時間判斷是否存在交叉沖突。如果存在交叉沖突，依照任務的優先級從低到高的順序，選擇等待和重新規劃優先級低的

圖8 相向沖突Fig.8 Opposing conflict

（9）得出最優路徑。

3 對基于時間窗的算法進行驗證計算

3.1 情景說明

隨著貨物數量的爆炸性增長和用工成本的不斷增加，更高效的倉儲和物流系統就成為越來越多企業的首選。現在簡化了一個大型倉庫，如圖9所示。

圖9 倉庫作業的路網結構和作業站點Fig.9 Road network structure and operation sites

任務1、任務2、任務3和任務4是4個即將輸入系統任務，具體任務為見表2，優先級數字越小優先級越高。物流系統中有4臺機器人分別是機器人1、機器人2、機器人3和機器人4。當前處于空載空閑狀態，停靠點分別是在9、4、15和55號站點等待系統調用。

表2 機器人系統作業任務表Table 2 Task list of robot system

3.2 最優路徑計算

3.2.1 最優任務分配和確定運輸線路

根據遺傳算法，可以計算出機器人到起點的相應最短路徑以及最短的行駛時間。通過比較確定任務分配的機器人，在此基礎上計算完成任務所需的時間。

分別計算任務1、任務2、任務3和任務4調用機器人到達各自任務起點的路徑，如表3～6所示。

表3 機器人1、2、3、4到達起始點11的優化路徑Table 3 Optimized path of robots 1，2，3，4 to point 11

表4 機器人1、2、3、4到達起始點13的優化路徑Table 4 Optimized path of robots 1，2，3，4 to point 13

表5 機器人1、2、3、4到達起始點19的優化路徑Table 5 Optimized path of robots 1，2，3，4 to point 19

表6 機器人1、2、3、4到達起始點54的優化路徑Table 6 Optimized path of robots 1，2，3，4 to point 54

執行任務1時，分別計算機器人1、機器人2、機器人3、機器人4到達起點11的時間。因為機器人1執行任務1任務所花費的時間最少，所以選擇機器人1執行任務1。

使用機器人1計算任務2時，機器人1已經完成任務1，因此需要從308.04 s開始計算。通過對比發現機器人2到達起始點13的時間所花費的時間最少，因此選擇機器人2執行Task2任務。

機器人1和機器人2必須先完成任務1和任務2，才能夠執行任務3。任務3的優先級高于任務4，任務4的開始執行時間早于任務3。分別計算機器人1、機器人2、機器人3、機器人4到達任務3起始點19的時間，機器人3所花費的時間最少。在上述基礎上，執行任務4。當執行完任務4，機器人4到達起始點19的時間大于機器人3到達任務3起始點19所花費的時間。選擇機器人3執行任務3。

最優任務分配：任務1選擇機器人1；任務2選擇機器人2；任務3選擇機器人3；任務4選擇機器人4。它們到達起始點的路徑分別是9-10-11、4-14-13、15-14-19和55-54。運輸線路：任務1的運輸線路：9-10-11-12-13-14-19-24-25-26-27-28；任務2的運輸線路：4-14-13-12-18-22-33-38；任務3的運輸線路：15-14-19-24-25-35-34；任務4的運輸線路：55-54-53-46-40-36-27-26。

3.2.2 當前線路時間窗

系統當前線路時間窗結果如表7～10所示。

表7 機器人1執行任務1的優化路徑Table 7 Optimized path of robot 1 to perform task 1

表8 機器人2執行任務2的優化路徑Table 8 Optimized path of robot 2 to perform task 2

表9 機器人3執行任務3的優化路徑Table 9 Optimized path of robot 3 to perform task 3

表10 機器人4執行任務4的優化路徑Table 10 Optimized path of robot 4 to perform task 4

3.2.3 優化當前線路時間窗沖突

分析任務發生沖突的時間窗詳細情況如下：

（1）任務1

機器人1與機器人2：

機器人1與機器人3：

機器人1與機器人4：

（2）任務2

機器人2與機器人1：

（3）任務3

機器人3與機器人1：

（4）任務4：

機器人4與機器人1：

在路徑12-13和13-14上，機器人1和機器人2某一時間段內沿相反方向行駛。機器人1和機器人3在路徑14-19、19-24和24-25上沿相同方向行駛。機器人1和機器人4沿相反方向行駛。

解決線路沖突的方法主要有以下兩種：避讓和嘗試新的線路，通過嘗試兩種可能方案，選擇花費時間較短的作為最優方案。

（1）避讓

當機器人1按照任務1規劃的線路行駛和機器人2按照任務2規劃的線路行駛時，在節點12-13-14處產生碰撞。為避免該碰撞，將如任務2的時間節點后移動，至少需要推遲的時間是100 s。

（2）嘗試新的線路

路徑的發生沖突時，依照任務的優先級從低到高的順序，重新規劃優先級低的任務，路徑增加則放棄當前新路徑，對優先級高的任務進行線路規劃，路徑增加則放棄當前新路徑。以上情況都不滿足，再次對優先級低的任務重新進行路徑規劃。對比新路徑和在當前路徑基礎上進行避讓，選擇出最優路徑。

任務2的優先級低于任務1，因此重新對任務2進行線路規劃。新路徑大于原路徑，轉而尋找任務1的新路徑。結果如下：任務1的路徑是9-10-11-12-18-22-23-24-25-26-27-28；任務2的路徑是4-14-13-12-18-22-33-38。機器人1和機器人2在執行任務1和任務2時，避免了在線路12-13-14沖突。

對于26-27路徑的沖突，因任務4優先級更低，對任務4重新進行路徑規劃，見表11。運輸路徑是55-54-53-52-45-39-35-25-26，所花費時間310.61 s。在原路徑基礎上進行等待，生成的路徑是55-54-53-46-40-36（等待）-27-26，所花費時間260.61 s。在解決了沖突后，任務1和4的時間窗口如表12和13所示。

表11 機器人4執行任務4重新進行路徑規劃Table 11 Robot 4 performing task 4 and re-planning path

表12 任務1最優路徑（優化后）Table 12 Optimal path of task 1（Optimized）

表13 任務4最優路徑（優化后）Table 13 Optimal path of task 4（Optimized）

機器人4的等待時間：280.32-246.61+2=35.71 s。

新路徑的可能沖突的時間窗詳細分析：

（1）任務1

機器人1與機器人2：

機器人1與機器人3：

機器人1與機器人4：

（2）任務2

機器人2與機器人1：

（3）任務3

機器人3與機器人1：

（4）任務4

機器人4與機器人1：

任務1和任務4的新路徑所花費的時間增多，分別是310.32-308.04=2.28 s，320.61-306.32=14.29 s。

任務1重新規劃路徑增加的2.28 s在可接受范圍內，如果任務1不重新規劃，任務2將會推遲100 s。任務4重新規劃路徑增加的15.9 s大于推遲時間窗的11.5 s，故對任務4使用等待的方法來避免沖突。

4 結束語

通過對單車道雙向多機器人路徑規劃問題進行研究，提出了一種基于時間窗的算法流程，同時通過一個模型進行驗證證明了算法流程的有效性。為多機器人協同作業，提供了新的選擇。未來將進一步對多機器人協同作業進行研究，為了進一步提高效率可以讓各機器人預先出發，從而為執行任務進一步節約時間。以及不同性能的雙車道雙向機器人以及機器人在交叉路口相遇，在保證安全的前提下如何避障。