基于距離-角度多移動機(jī)器人編隊避障策略

咼生富，張鵬超，李海婷，徐鵬飛，劉亞恒

(1.陜西理工大學(xué)機(jī)械工程學(xué)院,陜西漢中 723000；2.陜西省工業(yè)自動化重點實驗室,陜西漢中 723000；3.陜西理工大學(xué)電氣工程學(xué)院,陜西漢中 723000)

0 前言

多移動機(jī)器人系統(tǒng)已廣泛應(yīng)用于軍事及日常生活等場景，如人員搜救、合作搬運、無人機(jī)表演及機(jī)器人足球等。編隊控制與避障作為多移動機(jī)器人系統(tǒng)的關(guān)鍵問題，眾多學(xué)者對其進(jìn)行了大量的研究。在多機(jī)器人合作完成任務(wù)過程中會遇到靜態(tài)或動態(tài)障礙物，所以有效避障是必要的，否則會造成任務(wù)失敗。

目前常用的避障方法分為兩種：一種是編隊拆分避障，如柵格法、可視圖法、模糊邏輯法、RTT算法、遺傳算法等；另一種為編隊整體避障，如人工勢場法、模型預(yù)測控制法、固定隊形變換法和基于行為法等。DAI等采用具有速度約束的多機(jī)器人規(guī)避和穿越障礙物的切換編隊策略，主要利用幾何避障控制方法來規(guī)劃安全路徑，但需要精確找到航路點才可以穩(wěn)定隊形進(jìn)行切換。陳剛等人公開一種基于模糊控制的以領(lǐng)航機(jī)器人廣播的形式告知所有跟隨機(jī)器人避障，跟隨者根據(jù)存儲的不同隊形信息以及領(lǐng)航機(jī)器人廣播的信息切換隊形，但模糊規(guī)則繁雜且屬人為規(guī)定，控制精度難以保證，且沒有考慮避障后隊形恢復(fù)和機(jī)器人之間在避障過程中碰撞問題。張佳龍等公開一種基于斥力場函數(shù)的編隊避障方法以及避障后路徑重規(guī)劃策略，保證在多機(jī)編隊執(zhí)行任務(wù)中，實現(xiàn)機(jī)內(nèi)實時避障，但斥力場越大，無人機(jī)離開觸發(fā)區(qū)的響應(yīng)速度和運動速度越快，隊形變換超調(diào)量較大，控制系統(tǒng)穩(wěn)定性降低。編隊整體避障需保持編隊內(nèi)部隊形，通過動態(tài)調(diào)節(jié)隊形進(jìn)行在線避障，相比于拆分避障，更具有靈活性、整體性和可靠性，可以有效避免拆分避障出現(xiàn)的跟隨者掉隊、機(jī)器人之間的碰撞問題。但多移動機(jī)機(jī)器人之間若沒有進(jìn)行有效的協(xié)調(diào)和溝通，將不能發(fā)揮出多機(jī)協(xié)同避障的優(yōu)勢，造成嚴(yán)重的經(jīng)濟(jì)損失。因此，設(shè)計一種合理有效的編隊避障控制算法對多移動機(jī)器人系統(tǒng)的應(yīng)用具有重要意義。

本文作者綜合考慮上述整體編隊避障過程中存在的系統(tǒng)響應(yīng)慢、隊形變換超調(diào)量大、控制精度低和在避障過程中機(jī)器人相互間會發(fā)生碰撞的問題，提出一種距離-角度優(yōu)先級的避障策略，通過建立隊形數(shù)據(jù)庫，根據(jù)障礙物類型的不同，選擇合理的避障隊形進(jìn)行避障，順利通過障礙物后，快速恢復(fù)原隊形繼續(xù)完成任務(wù)。

1 領(lǐng)航-跟隨隊形結(jié)構(gòu)模型及隊形數(shù)據(jù)庫的建立

1.1 領(lǐng)航-跟隨隊形結(jié)構(gòu)模型

為減小跟隨者對領(lǐng)航者的過度依賴，避免鏈?zhǔn)浇Y(jié)構(gòu)造成的跟蹤誤差累積問題，采用虛擬領(lǐng)航-跟隨運動結(jié)構(gòu)，如圖1所示。

圖1 跟隨者與領(lǐng)航者相對運動模型

設(shè)領(lǐng)航者的位姿為=[],=[]，跟隨機(jī)器人的位姿為=[],=[]，由圖1可得跟隨機(jī)器人與領(lǐng)航者的相對位姿方程：

(1)

其中:Δ=-,Δ=-，和為實際運動軌跡，和為理想運動軌跡。

由圖1得跟隨機(jī)器人的運動方程：

(2)

1.2 建立隊形數(shù)據(jù)庫

機(jī)器人的基本隊形主要有一字形、楔形、圓形和三角形等，如圖2所示。

圖2 隊形數(shù)據(jù)庫中的基本隊形形狀

在每個隊形中都至少有一個領(lǐng)航機(jī)器人，其余跟隨機(jī)器人接收領(lǐng)航者傳遞的消息，形成目標(biāo)隊形。多機(jī)器人在執(zhí)行任務(wù)過程中遇到障礙，通常是將隊形變化成一字形通過障礙物，不考慮機(jī)器人的運動特性、具體環(huán)境情況和障礙物的類型，這并非是最優(yōu)的避障方法。圖3給出了兩種不同的隊形進(jìn)行避障，其中圖3(a)中由原有隊形變成一字隊形，盡管該隊形容易通過當(dāng)前障礙物，但在一定程度上增加了機(jī)器人避障路徑長度，收斂隊形的時間長，且易發(fā)生內(nèi)部碰撞，隊形失真度比較大。而圖3(b)中以隊形內(nèi)部角度縮小形成的隊形通過障礙物，編隊沒有改變整體隊形結(jié)構(gòu)，只是在不會發(fā)生內(nèi)部碰撞的條件下，對機(jī)器人編隊隊形進(jìn)行收縮，收縮后的隊形順利通過障礙物，在當(dāng)前場景下，圖3(b)中的避障方法明顯要優(yōu)于圖3(a)。

圖3 不同隊形避障方式

以上是從具體的場景比較兩種不同隊形的優(yōu)劣程度，在實際避障過程中，移動機(jī)器人整體如何自主地選擇適應(yīng)當(dāng)前環(huán)境約束條件的拓?fù)浣Y(jié)構(gòu)需提出相對應(yīng)的避障策略。

目前缺少統(tǒng)一有效的方式來定義多移動機(jī)器人編隊的隊形形狀。為了方便描述編隊控制中具體領(lǐng)航者-跟隨者關(guān)系及隊形形狀信息。下面舉例說明：

設(shè)有3個機(jī)器人參與編隊，虛擬領(lǐng)航者表示為L，其位置坐標(biāo)表示為[],跟隨者表示為F(=1,2,3)，常用隊形有一字形、三角形，如圖4、圖5所示。根據(jù)機(jī)器人數(shù)量設(shè)定相應(yīng)的隊形數(shù)據(jù)庫。

圖4 一字形編隊位置示意

圖5 三角形編隊位置示意

領(lǐng)航者與跟隨者坐標(biāo)如表1和表2所示。

表1 一字形隊形各跟隨者位置坐標(biāo)

表2 三角形隊形各跟隨者位置坐標(biāo)

通過建立隊形數(shù)據(jù)庫，便于領(lǐng)航者迅速做出隊形選擇，降低計算復(fù)雜度，提高決策效率。

2 距離-角度優(yōu)先級避障策略

充分考慮在隊形變換過程中的環(huán)境約束，合理選擇隊形避障方式，分別針對單側(cè)障礙物和雙側(cè)障礙物提出基于距離-角度優(yōu)先級的隊形變化避障策略。

圖6所示為多移動機(jī)器人避障算法流程。機(jī)器人編隊在向目標(biāo)點運動的過程中，領(lǐng)航機(jī)器人通過自身攜帶的傳感器感知周圍環(huán)境信息，得到領(lǐng)航機(jī)器人與障礙物之間的相對位置，并考慮各個機(jī)器人的自身安全距離和通信范圍的關(guān)系判斷當(dāng)前以何種隊形避障最優(yōu)；跟隨者根據(jù)領(lǐng)航者的指令進(jìn)行避障。

圖6 多移動機(jī)器人避障流程

2.1 單雙側(cè)障礙物避障策略

設(shè)機(jī)器人自身安全距離為半徑為，則兩個機(jī)器人并排運動的最小安全距離為4，3個機(jī)器人跟隨領(lǐng)航者以三角形隊形沿水平方向運動。

(1)單側(cè)障礙物避障策略

單側(cè)障礙物避障示意如圖7—圖8所示。

圖7 躲避上側(cè)障礙物示意

圖8 躲避下側(cè)障礙物示意

編隊遇到單側(cè)障礙物時，如圖7所示，通過比較障礙物與編隊的距離和sin+的大小，判斷編隊是否需要避障；若>sin+，則不需要避障。若≤sin+，則需避障，同時機(jī)器人通過傳感器判斷障礙物，位于前進(jìn)方向的上側(cè)或下側(cè)，若為上側(cè)，則機(jī)器人系統(tǒng)保持初始隊形右拐進(jìn)行避障，若為下側(cè)，則機(jī)器人系統(tǒng)保持初始隊形左拐前進(jìn)進(jìn)行避障；當(dāng)編隊整體通過障礙物后隊形收斂到原軌跡水平線繼續(xù)前行。

(2)雙側(cè)障礙物避障策略

當(dāng)領(lǐng)航機(jī)器人檢測到雙側(cè)障礙物時，如圖9所示，如果>，編隊中的成員機(jī)器人保持現(xiàn)有隊形形狀即可通過障礙物。若≤，編隊無法直接通過障礙物區(qū)域，需要將隊形切換到另一種狀態(tài)，保障編隊順利通過。其中=2sin+2。

雙側(cè)障礙物屬于復(fù)雜區(qū)域，整體編隊需要具體判斷以哪種隊形避障，若<2，則按單側(cè)避障方案繞行避障；若2<<4，需變?yōu)橐蛔株犘瓮ㄟ^障礙物；若>4時，編隊通過減小角度減小隊形的寬度，依然按照原隊形即可通過障礙物。其中表示障礙物之間的最小間距。

圖9 樹杈狀三角形隊形避障示意

通過分析對一般環(huán)境中可能出現(xiàn)的單雙側(cè)障礙物，考慮機(jī)器人自身安全距離及隊形整體與障礙物之間距離，提出距離-角度優(yōu)先級避障策略，即先判斷是否需要避障，其次優(yōu)先考慮距離判斷是否需要改變隊形，最后決定是否需要改變角度選擇合適隊形通過障礙物。當(dāng)編隊中最后一個機(jī)器人成功通過障礙物后，由領(lǐng)航者機(jī)器人發(fā)布隊形恢復(fù)指令，隊形中的跟隨機(jī)器人以各自領(lǐng)航者為參考恢復(fù)隊形。

2.2 隊形快速恢復(fù)算法

為提高多機(jī)器人的工作效率，當(dāng)機(jī)器人編隊整體通過障礙物后需及時恢復(fù)隊形，沿原軌跡繼續(xù)前行，設(shè)避障后領(lǐng)航者位姿為()=[]，設(shè)隊形快速恢復(fù)函數(shù)為

()=(-)e-+

(3)

其中：>0；為初始值即避障后虛擬領(lǐng)航者位置狀態(tài)量；為()的穩(wěn)態(tài)值即恢復(fù)隊形后的位置狀態(tài)量。()按指數(shù)快速遞減到，可實現(xiàn)隊形以指數(shù)形式快速恢復(fù)。

3 仿真實驗及結(jié)果分析

為了充分驗證文中避障策略的可行性和優(yōu)越性，在兩種不同的障礙物環(huán)境中進(jìn)行仿真。

將3個機(jī)器人的初始編隊設(shè)置為三角形，初始位置分別為=[2 1 0 0]、=[0 1 1 0]、=[0 9 0],理想線速度、角速度分別為=1 m/s、=0.5 rad/s，機(jī)器人自身安全距離為0.3 m，最大通信距離為5 m，最大通信角度為120°，仿真結(jié)果如圖10—圖13所示。

圖10 躲避單側(cè)障礙物

圖11 躲避雙側(cè)障礙物

圖12 躲避雙側(cè)障礙物時實際與理想位置跟蹤誤差

圖13 躲避雙側(cè)障礙物時實際與理想速度跟蹤誤差

圖10、圖11分別為在單、雙側(cè)障礙物環(huán)境中機(jī)器人群的避障及隊形恢復(fù)示意，可得：機(jī)器人以三角形隊形運動遇到單側(cè)障礙物時，能夠準(zhǔn)確判別出障礙物的方向并控制編隊轉(zhuǎn)向安全通過障礙物；遇到雙側(cè)障礙物時，充分考慮障礙物與隊形的間距和角度，開始縮小隊形通過第一個障礙物，然后變?yōu)橐蛔中侮犘瓮ㄟ^第二個障礙物，并在完全通過障礙物時迅速在3 s內(nèi)恢復(fù)隊形繼續(xù)前進(jìn)。

圖12為機(jī)器人躲避雙側(cè)障礙物過程中的位置跟蹤誤差曲線，當(dāng)機(jī)器人保持隊形運動時，在軸方向上的平均絕對誤差為0.001 1 m，在軸方向上的平均絕對誤差為0.001 5 m，角度跟蹤平均絕對誤差為0.001 2 rad。當(dāng)機(jī)器人變換隊形時，在=30 s時隊形超調(diào)量最大，在軸上最大超調(diào)量為3 m，軸上的為2 m，即隊形最大超調(diào)距離約為3.606 m<5 m,角度最大超調(diào)量為1.2 rad<2π/3，機(jī)器人群始終處于通信范圍內(nèi)，沒有出現(xiàn)掉隊和碰撞現(xiàn)象。圖13為機(jī)器人躲避雙側(cè)障礙物過程中的速度跟蹤誤差：當(dāng)機(jī)器人保持隊形運動時，線速度跟蹤平均絕對誤差為0.001 2 m/s，角速度跟蹤平均絕對誤差為0.001 1 rad/s；當(dāng)機(jī)器人變換隊形時，考慮到實際情況，轉(zhuǎn)彎時需要減小速度，所以線速度與角速度跟蹤均出現(xiàn)了超調(diào)，但避障后，能夠在5 s內(nèi)恢復(fù)原有速度繼續(xù)前進(jìn)。

為了進(jìn)一步說明該避障策略的高效性，與文獻(xiàn)[12]中提出的幾何避障控制算法作對比，設(shè)置的障礙物和初始隊形與圖11中保持一致，仿真結(jié)果見圖14—圖16。

圖14是文獻(xiàn)[12]避障策略仿真軌跡，編隊在進(jìn)行隊形變換時，軌跡不平滑，導(dǎo)致隊形超調(diào)量較大。圖15為文獻(xiàn)[12]中避障策略下機(jī)器人躲避雙側(cè)障礙物過程中的位置跟蹤誤差曲線，當(dāng)機(jī)器人變換隊形時，在=30 s時隊形超調(diào)量最大，軸方向最大超調(diào)量為3.4 m，軸上為2.2 m，即隊形超調(diào)最大距離為4.04 m。圖16為文獻(xiàn)[12]中避障策略下機(jī)器人躲避雙側(cè)障礙物過程中的速度跟蹤誤差，當(dāng)機(jī)器人變換隊形時，完成避障后編隊需要7 s才能恢復(fù)初始隊形。

圖14 文獻(xiàn)[12]中避障策略雙側(cè)避障

圖15 文獻(xiàn)[12]中避障策略與理想位置跟蹤誤差

圖16 文獻(xiàn)[12]中避障策略與理想速度跟蹤誤差

為方便表述，將文中所提避障策略、文獻(xiàn)[12]中設(shè)計的避障方法分別表述為A算法和B算法。由圖11和圖14可得：與B算法相比，A算法下編隊軌跡相對比較平滑；由圖12、13和圖15、16可知：當(dāng)機(jī)器人保持隊形運動時，與B算法相比，A算法下位置跟蹤誤差在軸方向上的平均絕對誤差、在軸方向上的平均絕對誤差以及角度跟蹤平均絕對誤差分別提高了0.003 m和0.004 m；在線速度和角速度跟蹤方面，A算法比B算法分別提高了0.002 m/s和0.004 rad/s；此外，隊形恢復(fù)時間縮短了28.57%。

以上是對編隊避障過程中的跟蹤誤差進(jìn)行具體對比分析，證明了A算法的有效性。為了進(jìn)一步說明A算法完成避障的高效性，現(xiàn)對編隊整體避障過程進(jìn)行綜合評價，評價指標(biāo)函數(shù)設(shè)計如下：

(1)避障路徑函數(shù)

當(dāng)機(jī)器人速度一定時，編隊是否能夠高效完成任務(wù)取決于路徑的長短。將走過路徑的長短作為評價避障性能函數(shù)之一，來描述機(jī)器人群避障走過路徑的長度。避障路徑函數(shù)為

(4)

其中：表示編隊開始避障的初始時刻；表示編隊完全通過障礙物的時刻。函數(shù)值與避障效率成反比。

(2)隊形結(jié)構(gòu)失真函數(shù)

切換隊形時會引起編隊控制系統(tǒng)不穩(wěn)定，需要編隊隊形變形量盡可能地小。將隊形結(jié)構(gòu)失真作為評價避障性能的函數(shù)之一，來描述隊形變換與原始隊形結(jié)構(gòu)畸變程度。隊形結(jié)構(gòu)失真函數(shù)為

(5)

其中：表示編隊中機(jī)器人數(shù)量；()表示機(jī)器人在性切換隊形中位置；(0)表示機(jī)器人在初始隊形中的位置；，lF為各跟隨者與領(lǐng)航者的相對距離。函數(shù)值表明隊形失真度，值越大系統(tǒng)越不穩(wěn)定，隊形難以恢復(fù)。

(3)隊形恢復(fù)時間

當(dāng)機(jī)器人通過障礙物時，需要恢復(fù)隊形繼續(xù)前行完成任務(wù)，因此，隊形恢復(fù)效率也是影響編隊整體執(zhí)行任務(wù)效率的影響因素之一。將隊形恢復(fù)時間作為評價避障的附加技術(shù)指標(biāo)，一方面來描述避障時隊形的分散程度，另一方面，編隊過程中需要多次變換隊形時，就需要多次恢復(fù)隊形，隊形恢復(fù)時間從側(cè)面反映出避障的效率。隊形恢復(fù)時間函數(shù)為

(6)

其中：表示編隊切換隊形的次數(shù)；s表示避障后開始恢復(fù)隊形的時刻；d表示隊形恢復(fù)結(jié)束的時刻。函數(shù)值與避障效率成反比。

由表3可得：比B算法相比，路徑的長度方面，A算法下的避障路徑更短，減少了14.29%；在隊形結(jié)構(gòu)畸變程度方面，A算法減小了12.5%；在隊形恢復(fù)時間上，A算法縮短了28.57%，極大地提高了避障效率。綜上，文中編隊避障策略效果更佳。

表3 文中方法與文獻(xiàn)[12]中方法的指標(biāo)評價對比

4 結(jié)論

針對復(fù)雜環(huán)境下多移動機(jī)器人編隊整體避障效率低的問題，提出一種基于距離-角度優(yōu)先級避障策略。該策略通過領(lǐng)航者機(jī)器人攜帶的傳感器獲取未知環(huán)境中障礙物的具體信息，引導(dǎo)整體編隊通過障礙區(qū)。相對于傳統(tǒng)的隊形避障方法，此方法充分考慮編隊避障中環(huán)境、自身安全和通信范圍的約束，先通過整體編隊隊形的寬度與障礙物可通過區(qū)域的比較，選擇合適隊形，然后各跟隨者調(diào)整自己與領(lǐng)航者的角度，形成目標(biāo)隊形通過障礙物。該策略充分考慮了當(dāng)前環(huán)境對隊形的影響，具有使編隊自主切換到當(dāng)前避障合理隊形的良好性能，對于多移動機(jī)器人編隊避障研究具有一定的理論意義和參考價值。