無人工程機(jī)械路徑規(guī)劃算法應(yīng)用

楊夏寧　黃位偉　李志恒　玉海韓

關(guān)鍵詞：無人裝載機(jī)；路徑規(guī)劃；AGV；土方機(jī)械；路徑?jīng)_突

中圖分類號：TP18 文獻(xiàn)標(biāo)識碼：A

文章編號：1009-3044（2023）21-0024-04

0 引言

工程機(jī)械是體現(xiàn)國家高端制造水平的一個重要指標(biāo)，目前我國的工程機(jī)械發(fā)展水平面臨著前所未有的挑戰(zhàn)。其中，核心技術(shù)不足，數(shù)字化和智能化發(fā)展落后是主要的原因。融入現(xiàn)代化信息技術(shù)，使機(jī)械設(shè)備向著更高端的自動化、智能化系統(tǒng)發(fā)展，推進(jìn)“5G + 工程機(jī)械”模式[1]的發(fā)展是目前研究的重點(diǎn)之一。

近年來隨著物流技術(shù)的發(fā)展、5G通信技術(shù)的成熟和人工倉儲成本的提高。以AGV（AutomatedGuided Vehicle，AGV） 自動物料轉(zhuǎn)運(yùn)小車為代表的倉儲自動調(diào)度和路徑規(guī)劃算法發(fā)展迅速，無人物流倉儲通過裝備分揀系統(tǒng)，極大地提高了現(xiàn)代物流的效率，進(jìn)一步提高了自動化和智能化程度。AGV無人倉儲作業(yè)的成功帶動了各行業(yè)中無人作業(yè)的發(fā)展，相關(guān)技術(shù)的應(yīng)用也不再局限于物流行業(yè)[2-3]。AGV路徑規(guī)劃問題一般分為全局路徑規(guī)劃和局部路徑規(guī)劃兩步。全局路徑規(guī)劃從全局出發(fā)，不考慮動態(tài)變化規(guī)劃出符合全局的最優(yōu)路徑；后者則實(shí)時地更新自身運(yùn)行狀態(tài)，實(shí)現(xiàn)動態(tài)地避障功能。目前常用的全局規(guī)劃算法主要是A*算法[4]、粒子群算法[5]和蟻群算法[6]。局部規(guī)劃常用的算法有動態(tài)窗口法[7]和人工視場法[8]。路徑規(guī)劃一般以路徑距離代價作為主要的代價函數(shù)，計(jì)算得到的路徑雖然是最短的，但是并不一定是最優(yōu)的。

工程機(jī)械行業(yè)作業(yè)環(huán)境和自身運(yùn)行狀態(tài)復(fù)雜，對路徑規(guī)劃的影響都很大。單純的以路徑距離代價作為總代價函數(shù)無法滿足復(fù)雜的工程問題。為了改善規(guī)劃算法在工程機(jī)械及相關(guān)領(lǐng)域的應(yīng)用，本文結(jié)合全局規(guī)劃和局部規(guī)劃算法，對固定作業(yè)場景的裝載機(jī)倒料場景進(jìn)行柵格化和拓?fù)浠?，采取A*算法尋找最短路徑，并通過CBS（Conflict Based Search， CBS）算法[9]解決路徑?jīng)_突問題，從而找到全局最優(yōu)路徑，并基于路徑距離代價，考慮裝載機(jī)滿載和空載運(yùn)行代價，面向不同的工況進(jìn)行仿真實(shí)驗(yàn)，以提高算法的魯棒性。

1 問題描述

為具象化算法的應(yīng)用效果，本文將路徑規(guī)劃算法應(yīng)用到實(shí)際的裝載機(jī)裝卸料固定作業(yè)場景中。結(jié)合實(shí)際生產(chǎn)需要和裝載機(jī)運(yùn)行條件對場景進(jìn)行建模，分析算法的可行性。

1.1 場景描述

無人裝載機(jī)裝卸料任務(wù)在10個料倉和10個漏斗之間完成。由于場地空間的限制和對安全的考慮，裝載機(jī)在作業(yè)現(xiàn)場行駛的過程中不允許并行，所有的道路都是單行道，圖1是無人生產(chǎn)車間示意圖。

無人生產(chǎn)車間包括裝載機(jī)運(yùn)行通道、粗砂倉庫和粗砂漏斗各4個、中砂倉庫和中砂漏斗各2個、特細(xì)砂倉庫和特細(xì)砂漏斗各4個。為了滿足生產(chǎn)需要，裝載機(jī)需要不斷地從料倉中運(yùn)送砂料到漏斗，如何在規(guī)定的時間內(nèi)高效地完成任務(wù)是本文方法要解決的主要問題。

多車任務(wù)調(diào)度規(guī)劃問題是一個復(fù)雜的問題，通常對物料轉(zhuǎn)運(yùn)車的分配原則主要有最短路徑原則、最短工作時間原則、工作任務(wù)優(yōu)先級原則和綜合指標(biāo)的原則[10]?？紤]到裝載機(jī)造價昂貴，且作業(yè)現(xiàn)場空間受限，安全代價大。本文通過采用雙車模型對路徑規(guī)劃問題進(jìn)行求解，并對無人生產(chǎn)車間做出以下約束：

1） 通過嚴(yán)格限制作業(yè)場景動態(tài)信息，將作業(yè)場景固定，場景信息已知。裝載機(jī)可行走的道路固定不變，車輛路徑都為單行道。

2） 裝載機(jī)在漏斗和料倉的作業(yè)時間固定為t ∈ （t₁，t₂），時間參數(shù)由裝載機(jī)性能參數(shù)決定。

3） 兩臺裝載機(jī)的型號、速度等運(yùn)行參數(shù)完全一致。

由于大型機(jī)械滿載和空載期間的運(yùn)行狀態(tài)差異較大，場景描述除了對場景做出限制，車輛自身狀態(tài)的約束也是重要的舉措，盡可能地減少作業(yè)場景的動態(tài)信息是保證生產(chǎn)穩(wěn)定的一個前提。

1.2 場景建模

將實(shí)際作業(yè)空間轉(zhuǎn)換為模型地圖是算法研究的第一步。目前常見的地圖構(gòu)建方法有柵格圖法、可視圖法和拓?fù)鋱D法。拓?fù)浣Y(jié)構(gòu)簡潔直觀，對地圖進(jìn)行全局規(guī)劃的效率高。柵格地圖精細(xì)化程度高，適合車端進(jìn)行局部路徑規(guī)劃。為充分發(fā)揮二者的優(yōu)勢，本文在云端使用拓?fù)涞貓D進(jìn)行全局規(guī)劃，并結(jié)合車端柵格地圖進(jìn)行局部規(guī)劃，最大限度保證路徑規(guī)劃的準(zhǔn)確性和穩(wěn)定性。

柵格圖法將實(shí)際三維地圖二維化，使用整齊排列的柵格來代表地圖中的路徑和障礙物。其中1代表障礙物，不可通行由圖2上中黑色柵格表示；0代表路徑，可通行由圖2上中白色柵格表示[11]。柵格圖表示法在場景開闊或細(xì)小障礙物較多的場景時，由于自身精細(xì)化表達(dá)能力差，刻意縮小柵格大小又會極大地增加計(jì)算量。故而并不適用于繁雜的作業(yè)場景。考慮到無人裝載機(jī)裝卸料生產(chǎn)車間場景固定，作業(yè)范圍比較小，同時嚴(yán)格控制生產(chǎn)區(qū)間障礙物等動態(tài)事件。使用柵格地圖足夠達(dá)到任務(wù)的精度，也有利于管理控制。無人生產(chǎn)區(qū)間使用柵格地圖表示后的場景如圖2 所示。為了提高作業(yè)的安全性，在柵格地圖中設(shè)立了膨脹區(qū)域由圖2上中灰色柵格表示，膨脹區(qū)域內(nèi)的行進(jìn)代價會增加，以降低路徑規(guī)劃撞墻的概率。

2 路徑規(guī)劃算法

路徑規(guī)劃算法的任務(wù)是從當(dāng)下環(huán)境中尋找出一條無沖突，從起始點(diǎn)到終點(diǎn)路徑代價最小的路徑?；?.1所描述的場景可以將路徑的沖突分為頂點(diǎn)沖突和邊界沖突。

2.1 沖突概述

2.2 CBS 算法

CBS算法是由Guni等人于2015年提出的用于解決多目標(biāo)路徑規(guī)劃MAPF （Multi-Agent PathFinding，MAPF） 路徑?jīng)_突問題的方法。CBS算法的主要思想是通過尋找一系列連續(xù)的約束來處理所有目標(biāo)路徑的沖突，直到找到?jīng)]有沖突點(diǎn)的路徑集合。CBS是一個雙級（two-level） 搜索算法，高級搜索部分（highlevel）是一個二叉搜索樹，稱為沖突樹CT（ConflictTree） ，樹的每個節(jié)點(diǎn)包含了單條的路徑的基本信息和約束條件。算法低級部分是在考慮新增約束的情況下，使用A*算法重新計(jì)算得到的最短路徑。

CBS算法在目標(biāo)數(shù)量過多，或者路徑?jīng)_突密集的情況下，搜索到的路徑可能陷入局部最優(yōu)解，但是對于目標(biāo)數(shù)較少的模型（如雙目標(biāo)模型）中卻有著足夠高的效率和性能。在本文的雙裝載機(jī)作業(yè)場景中，使用CBS算法是高效的。

2.2.1 CBS 高級搜索

CBS高級搜索由沖突樹完成，樹的每一個節(jié)點(diǎn)N包含了三個部分：

1） 一系列的約束（N.constrains） 。每一個約束都作用于某一路徑，根節(jié)點(diǎn)的約束為空，子節(jié)點(diǎn)繼承父節(jié)點(diǎn)的約束。

2） 局部最優(yōu)路徑（N.solution） 。目前的節(jié)點(diǎn)路徑，路徑需要滿足所有約束。該路徑計(jì)算由A?算法完成。

3） 總代價（N.cost） 到該節(jié)點(diǎn)為止，得到的所有路徑總代價。

具體結(jié)構(gòu)及沖突處理如圖5所示。

首先，規(guī)定沖突樹的左子樹給節(jié)點(diǎn)1添加約束，Con 為節(jié)點(diǎn)約束，Cost 為路徑代價，sol 為目前的路徑集合，標(biāo)紅部分為沖突點(diǎn)。該二叉樹有如下特點(diǎn)：

1） 根結(jié)點(diǎn)約束為空；

2） 子結(jié)點(diǎn)繼承父節(jié)點(diǎn)的所有約束，并且所有路徑都是在以約束為前提的情況下得到，通過低級搜索得到；

3） 一旦搜索到目標(biāo)結(jié)點(diǎn)，該遍歷即可提前結(jié)束。

總體上，需要先使用A? 算法進(jìn)行低級搜索為所有的目標(biāo)ai 尋找最短路徑（N₁.solution），并且該低級搜索繼承了所有來自父節(jié)點(diǎn)的約束（N₁.constrains）。一旦找到新的連續(xù)路徑，需要重新計(jì)算這些路徑的總代價（N₁.cost），并且這些目標(biāo)點(diǎn)的新路徑會重新進(jìn)行沖突檢測。若沖突依然存在，則在該節(jié)點(diǎn)的基礎(chǔ)上增加新的約束。兩個新的約束分別作用于沖突的兩個目標(biāo)點(diǎn)，生成兩個新的子節(jié)點(diǎn)N₂，N₃繼承于N₁。依次遍歷直到為所有的目標(biāo)尋找到無沖突的路徑。

初始給定兩條沖突的最短路徑1：{ D2，C2，B2，A2，A3}2：{C1，B1，B2，B3，B4}。t = 2 時，兩條路徑在B2點(diǎn)發(fā)生頂點(diǎn)沖突。左子樹為路徑1添加約束Con：{1，B2，2}，路徑1在t = 2時不得通過B2點(diǎn)，新路徑為1：{ D2，D3，C3，B3，A3}2：{C1，B1，B2，B3，B4}。重新計(jì)算新路徑的沖突，并以相同的方式解決，最終得到兩條無沖突的局部最優(yōu)路徑：

類似地，沖突樹的右子樹為路徑2添加類似約束，重新計(jì)算得到新的路徑集合。一般地，最終沖突樹得到的多個局部最優(yōu)解需要通過路徑代價Cost 做進(jìn)一步篩選，但若僅以距離代價為代價總和，則沖突樹深度越深的節(jié)點(diǎn)代價會越高，故而一旦搜索到無沖突路徑即可解決CBS搜索。

對于多個目標(biāo)（> 2） 的路徑規(guī)劃，可以生成k 個子節(jié)點(diǎn)構(gòu)成約束森林，如圖6（a） 所示，節(jié)點(diǎn)1，2，3在t 時刻在v 處都發(fā)生了沖突。優(yōu)先考慮節(jié)點(diǎn)1，2的沖突，處理之后再考慮加入節(jié)點(diǎn)3的沖突，從而將森林拆分為二叉樹進(jìn)行新的迭代，如圖6（b） 所示。

綜合以上，CBS算法高級搜索的算法流程由表1 給出。

3 仿真實(shí)驗(yàn)分析

為了驗(yàn)證CBS路徑規(guī)劃算法在裝載機(jī)裝卸料模型中的有效性，本文通過OpenCV工具庫對算法進(jìn)行驗(yàn)證分析。柵格地圖用于裝載機(jī)車端的局部路徑規(guī)劃，拓?fù)鋱D用于云端全局路徑規(guī)劃，由此可以清晰地表示出CBS算法尋找出的路徑并實(shí)現(xiàn)精細(xì)化控制。

3.1 路徑規(guī)劃沖突仿真分析

針對兩種不同類型的沖突，設(shè)置一系列不同的任務(wù)以充分證明算法的有效性。任務(wù)序列如表2所示。

每一個裝料任務(wù)都包括了兩段距離，分別為裝載機(jī)起始位置St 到料倉C 和料倉C 到漏斗L。并且起始位置到料倉階段，裝載機(jī)空載，此時行駛的代價較小。

相反的，料倉到漏斗的距離階段，裝載機(jī)滿載，此時設(shè)定裝載機(jī)的行駛代價為空載時候的2倍。

假設(shè)兩輛裝載機(jī)的起始點(diǎn)分別為L1和L10，并且為方便計(jì)算，裝載機(jī)到達(dá)任務(wù)地點(diǎn)后的作業(yè)時間代價固定為11。首先由A? 算法根據(jù)任務(wù)列表尋得的最短路徑如表3所示，表中所有的路徑都使用圖1拓?fù)鋱D結(jié)構(gòu)來表示。為解決路徑?jīng)_突，同時使得路徑的代價最小，約定空載裝載機(jī)需要為滿載運(yùn)行的機(jī)械讓步，由此使用CBS算法得到新的無沖突路徑并計(jì)算其代價如表4所示。

綜上實(shí)驗(yàn)結(jié)果，雙車模型在固定的裝卸料作業(yè)場景中沖突次數(shù)較少，在本文選取4個任務(wù)單次CBS計(jì)算中僅有3次沖突。同時運(yùn)行過程中只要保證滿載車輛運(yùn)行的路徑最短，則相應(yīng)的任務(wù)總代價就會越小。由此證明了CBS算法在該應(yīng)用場景中不僅高效簡單，更有不錯的穩(wěn)定性。

4結(jié)論

本文針對無人裝載機(jī)裝卸料作業(yè)場景進(jìn)行路徑規(guī)劃分析，提出一種符合實(shí)際生產(chǎn)需求的解決方案，并通過實(shí)驗(yàn)分析證明了在固定的作業(yè)場景中，CBS算法在路徑規(guī)劃中的高效性。除此之外，采用柵格地圖進(jìn)行路徑規(guī)劃的局部規(guī)劃，便于精確地控制車輛運(yùn)行狀態(tài)；拓?fù)涞貓D進(jìn)行路徑規(guī)劃的全局規(guī)劃，方便操作人員及時地調(diào)整，增強(qiáng)了算法的實(shí)用性。

實(shí)驗(yàn)證明了在局限的場景中，CBS 算法簡單高效，且具有不錯的穩(wěn)定性。在保證裝載機(jī)滿載運(yùn)行路徑最短的情況下，可以得到規(guī)劃路徑的最優(yōu)解。如何更精細(xì)化地考慮機(jī)械運(yùn)行的其他參數(shù)，提高整個路徑規(guī)劃算法的魯棒性是后續(xù)工作需要研究的內(nèi)容。