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

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

0 前言

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

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

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

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

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

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

圖1 跟隨者與領(lǐng)航者相對(duì)運(yùn)動(dòng)模型

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

(1)

其中:Δ=-,Δ=-，和為實(shí)際運(yùn)動(dòng)軌跡，和為理想運(yùn)動(dòng)軌跡。

由圖1得跟隨機(jī)器人的運(yùn)動(dòng)方程：

(2)

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

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

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

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

圖3 不同隊(duì)形避障方式

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

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

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

圖4 一字形編隊(duì)位置示意

圖5 三角形編隊(duì)位置示意

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖9 樹杈狀三角形隊(duì)形避障示意

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

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

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

()=(-)e-+

(3)

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

3 仿真實(shí)驗(yàn)及結(jié)果分析

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

將3個(gè)機(jī)器人的初始編隊(duì)設(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è)障礙物時(shí)實(shí)際與理想位置跟蹤誤差

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

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

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

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

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

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

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

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

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

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

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

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

(4)

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

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

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

(5)

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

(3)隊(duì)形恢復(fù)時(shí)間

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

(6)

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

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

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

4 結(jié)論

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