汪暢，王桐，高山

哈爾濱工程大學(xué) 信息與通信工程學(xué)院，黑龍江 哈爾濱 150001

多機(jī)器人系統(tǒng)是近年來機(jī)器人研究的熱點(diǎn)問題，在軍事、醫(yī)療、運(yùn)輸、安全巡邏、救援救災(zāi)等領(lǐng)域有著重要的應(yīng)用前景。多機(jī)器人協(xié)同的一個(gè)經(jīng)典例子就是多機(jī)器人編隊(duì)控制，其目標(biāo)是實(shí)現(xiàn)和保持一個(gè)預(yù)定義的編隊(duì)模式，使得多機(jī)器人可以協(xié)同完成給定的任務(wù)。在未知環(huán)境下不僅需要有效的編隊(duì)控制算法和協(xié)同避障算法，保證完成團(tuán)隊(duì)任務(wù)，還需要機(jī)器人感知周圍的環(huán)境信息。

目前，多移動(dòng)機(jī)器人編隊(duì)控制方法主要包括領(lǐng)航-跟隨法、虛擬結(jié)構(gòu)法、基于行為法等。其中領(lǐng)航-跟隨法由于其簡單性和可擴(kuò)展性，被廣泛應(yīng)用。Desai 等[1]將多機(jī)編隊(duì)控制問題分為軌跡跟蹤、機(jī)器人控制和編隊(duì)控制3 個(gè)子問題；多機(jī)編隊(duì)控制是通過控制相對(duì)角度、相對(duì)距離來實(shí)現(xiàn)的；孫玉嬌等[2]建立了在非完整約束條件下輪式機(jī)器人的數(shù)學(xué)模型，然后通過全局坐標(biāo)轉(zhuǎn)換將領(lǐng)航-跟隨模型轉(zhuǎn)換為局部坐標(biāo)系統(tǒng)誤差模型；吳軍成等[3]針對(duì)多機(jī)器人在不確定環(huán)境下難以保持某種隊(duì)形到達(dá)預(yù)定目標(biāo)等問題，提出了一種無需全局坐標(biāo)的多機(jī)器人編隊(duì)控制算法；有研究者提出基于Lyapunov 方法[4-5]推導(dǎo)系統(tǒng)的全局穩(wěn)定性，但是領(lǐng)導(dǎo)者和跟隨者之間的定位誤差是有界的，不能收斂到零。

盡管多機(jī)器人的領(lǐng)航-跟隨編隊(duì)控制取得重大進(jìn)展，但是在編隊(duì)控制設(shè)計(jì)上，要加強(qiáng)避障算法的研究。由于人工勢場法具有結(jié)構(gòu)簡單、實(shí)時(shí)性好等特點(diǎn)，被廣泛應(yīng)用在多機(jī)器人編隊(duì)中。Kowdiki 等[6]基于領(lǐng)航跟隨法提出一種混合編隊(duì)控制技術(shù)，并使用人工勢場法為領(lǐng)航智能體進(jìn)行路徑規(guī)劃，保持整個(gè)編隊(duì)的穩(wěn)定控制；同時(shí)人工勢場法也存在著缺陷，桑雷等[7]指出人工勢場法容易陷入局部極小值等問題；李金芝等[8]通過添加虛擬斥力旋轉(zhuǎn)勢場，解決了局部極小值問題，實(shí)現(xiàn)了多機(jī)器人編隊(duì)在靜態(tài)障礙環(huán)境中無碰撞路徑規(guī)劃。

現(xiàn)有研究方法，針對(duì)未知環(huán)境下的多機(jī)器人編隊(duì)研究較少，特別是如何保持編隊(duì)隊(duì)形穩(wěn)定、實(shí)時(shí)避障、編隊(duì)反饋等方面。本文基于領(lǐng)航-跟隨法進(jìn)行編隊(duì)隊(duì)形控制，引入輸出反饋控制律和吸附行為，保證編隊(duì)的穩(wěn)定性；通過機(jī)器人攜帶的傳感器自主感知并獲取環(huán)境信息，采用結(jié)合人工勢場法和基于“虛擬引導(dǎo)車”的沿墻行為在未知環(huán)境下進(jìn)行避障，避免陷入局部最小值。最后，通過軟件仿真驗(yàn)證可行性，并在自主搭建的多移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)上進(jìn)行驗(yàn)證。

1 領(lǐng)航-跟隨控制算法

1.1 非完整移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

由于輪式移動(dòng)機(jī)器人的結(jié)構(gòu)簡單、控制容易和運(yùn)動(dòng)穩(wěn)定等特點(diǎn)，具有較高使用價(jià)值和應(yīng)用前景。本文研究采用輪式移動(dòng)機(jī)器人，具有非完整運(yùn)動(dòng)約束的運(yùn)動(dòng)模型[9]，運(yùn)動(dòng)模型如圖1 所示。

圖1 輪式移動(dòng)機(jī)器人運(yùn)動(dòng)模型

定義機(jī)器人在環(huán)境中的位姿為q=[x,y,θ,γ]T，機(jī)器人將繞瞬心ICR 以半徑RB做圓弧運(yùn)動(dòng)，可以得到輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型為

1.2 領(lǐng)航-跟隨編隊(duì)控制算法的改進(jìn)

1.2.1 基于輸出反饋的編隊(duì)控制律

基于l-φ的編隊(duì)控制方式只需設(shè)置一個(gè)領(lǐng)航者，跟隨者根據(jù)相對(duì)距離和角度進(jìn)行運(yùn)動(dòng)，實(shí)現(xiàn)期望隊(duì)形的編隊(duì)控制，在編隊(duì)方式上更為靈活。但是編隊(duì)沒有明確的反饋，由于領(lǐng)導(dǎo)者的存在缺乏魯棒性，因此，在此基礎(chǔ)上，利用輸出反饋控制律設(shè)計(jì)編隊(duì)控制方案。

基于輸出反饋控制律如圖2 所示，跟隨者通過計(jì)算與領(lǐng)航者之間的位置，得出實(shí)際相對(duì)距離和角度，再根據(jù)系統(tǒng)設(shè)定的相對(duì)距離和角度，由控制器輸出速度控制的調(diào)整參數(shù)，使跟隨者調(diào)整自身位姿，跟隨領(lǐng)航者運(yùn)動(dòng)，并保持相對(duì)距離和角度的收斂，即l→ld，φ →φd。

圖2 基于輸出反饋控制律

領(lǐng)航-跟隨算法編隊(duì)控制運(yùn)動(dòng)模型如圖3 所示。以旋轉(zhuǎn)中心為基準(zhǔn)定義機(jī)器人i的位姿為(xi,yi,θi)，分別為第i個(gè)機(jī)器人的x坐標(biāo)、y坐標(biāo)和迎角；vi、wi為旋轉(zhuǎn)中心處的速度，l為領(lǐng)航者與跟隨者之間的歐氏距離， γ為跟隨者在其參考點(diǎn)處的相對(duì)方位角， φ為領(lǐng)航者與跟隨者之間的相對(duì)角度，d為旋轉(zhuǎn)中心與參考點(diǎn)之間的位移，且其正方向表示機(jī)器人前進(jìn)方向。

圖3 領(lǐng)航-跟隨算法編隊(duì)控制運(yùn)動(dòng)模型

跟隨者的運(yùn)動(dòng)學(xué)方程為

1.2.2 基于吸附行為的領(lǐng)航-跟隨編隊(duì)控制算法

基于輸出反饋控制律的表達(dá)式為

式中 α1、 α2為比例控制系數(shù)。

跟隨者的控制輸出為

根據(jù)跟隨者的控制輸出，通過領(lǐng)航者位姿相關(guān)信息與跟隨者位姿相關(guān)信息，得出被控對(duì)象(跟隨者)在下一周期t+T時(shí)刻的控制輸入為(v2(t+T),w2(t+T))，通過改變跟隨者當(dāng)前的運(yùn)動(dòng)狀態(tài)，完成編隊(duì)任務(wù)，使l和 φ收斂并保持系統(tǒng)設(shè)定值ld和 φd。

根據(jù)文獻(xiàn)[10]提出的相關(guān)理論，對(duì)編隊(duì)系統(tǒng)的穩(wěn)定性進(jìn)行分析可知，基于輸出反饋控制律的編隊(duì)控制中，若領(lǐng)航者作勻速直線運(yùn)動(dòng)或勻速圓周運(yùn)動(dòng)，編隊(duì)隊(duì)形將收斂為固定隊(duì)形(l=ld,φ=φd)，且隊(duì)形與跟隨者的初始狀態(tài)并不影響該固定隊(duì)形。

通過基于輸出反饋控制律的編隊(duì)控制分析可知，多機(jī)器人編隊(duì)運(yùn)動(dòng)對(duì)于l和 φ是漸近穩(wěn)定的，若領(lǐng)航者運(yùn)動(dòng)速度過快將導(dǎo)致跟隨者很難跟上，跟隨者的運(yùn)動(dòng)軌跡相對(duì)領(lǐng)航者呈現(xiàn)發(fā)散狀態(tài)，即編隊(duì)隊(duì)形遭到破壞。

為了使編隊(duì)系統(tǒng)能夠穩(wěn)定地編隊(duì)運(yùn)動(dòng)，本節(jié)將一種吸附行為[11]和基于輸出反饋控制律編隊(duì)相結(jié)合，提出一種基于吸附行為的領(lǐng)航跟隨算法。通過在領(lǐng)航者附近設(shè)置若干吸附區(qū)，跟隨者被吸引而直接移動(dòng)至領(lǐng)航者附近，同時(shí)自身也形成吸附區(qū)，從而可以應(yīng)用于更大規(guī)模的編隊(duì)。步驟描述如下：

1)以領(lǐng)航者的坐標(biāo)(x0,y0)為中心建立對(duì)跟隨者的吸附區(qū)，定義為

2)第i個(gè)跟隨者以較短的時(shí)間朝著領(lǐng)航者的吸附區(qū)移動(dòng)，并到達(dá)勢點(diǎn)附近；

2 傳統(tǒng)人工勢場法

2.1 傳統(tǒng)人工勢場法理論

Khatib[12]首次提出人工勢場的概念，移動(dòng)機(jī)器人在虛擬勢場中所受到的力由勢場的負(fù)梯度產(chǎn)生，環(huán)境中的障礙物對(duì)機(jī)器人產(chǎn)生斥力，目標(biāo)對(duì)機(jī)器人產(chǎn)生引力，引力和斥力共同作用下的合力控制機(jī)器人向總勢場下降最快的方向運(yùn)動(dòng)。圖4為機(jī)器人受力分析。

圖4 機(jī)器人受力分析

1）引力勢場函數(shù)。

由目標(biāo)點(diǎn)產(chǎn)生引力場，將機(jī)器人抽象為二維空間內(nèi)的質(zhì)點(diǎn)，則人工勢場法中引力勢場函數(shù)為

式中：Uia為引力勢場函數(shù)；Fia為引力，由引力勢場函數(shù)的負(fù)梯度所得；ka為引力勢能增益系數(shù)；d(q,qa)為機(jī)器人q與目標(biāo)點(diǎn)qa之間的距離。

2）斥力勢場函數(shù)。

由障礙物產(chǎn)生斥力場，則人工勢場法中斥力勢場函數(shù)為

式中：Uir為斥力勢場函數(shù)；Fir為斥力，由斥力勢場函數(shù)的負(fù)梯度所得；kr為斥力勢能增益系數(shù)；d(q,qr)為機(jī)器人q與障礙物qr之間的距離；dmax為障礙物的斥力勢場形成的作用力最大距離。

3）局限性。

人工勢場法存在局限性，最重要的有2 個(gè)：1)目標(biāo)不可達(dá)問題；2)局部最小值問題。目標(biāo)不可達(dá)問題如圖5 所示，當(dāng)目標(biāo)位置位于障礙物斥力勢場范圍內(nèi)，機(jī)器人向目標(biāo)移動(dòng)時(shí)，斥力場迅速增大，引力場迅速減小，因此機(jī)器人達(dá)不到目標(biāo)。局部最小值問題如圖6 所示，機(jī)器人運(yùn)動(dòng)過程中的某一點(diǎn)達(dá)到引力和斥力的平衡，機(jī)器人誤以為達(dá)到目標(biāo)點(diǎn)而停止不動(dòng)或在該點(diǎn)徘徊。

圖5 目標(biāo)不可達(dá)問題

圖6 局部最小值問題

2.2 傳統(tǒng)人工勢場法的改進(jìn)

多機(jī)器人根據(jù)搭載的傳感器獲取環(huán)境信息建立人工勢場，同時(shí)將當(dāng)前生成的路徑與環(huán)境信息形成閉環(huán)控制，實(shí)現(xiàn)在未知環(huán)境下的避障。為解決人工勢場法的局限性采用基于行為法中的一種沿墻運(yùn)動(dòng)行為[13]和人工勢場法相結(jié)合，提出融合沿墻行為的避障策略；利用虛擬引導(dǎo)車的生成/消失條件不斷切換運(yùn)動(dòng)狀態(tài)，保證機(jī)器人在未知環(huán)境下有效地避障，同時(shí)機(jī)器人進(jìn)行避障不再受初始設(shè)置勢場力的影響。

2.2.1 基于虛擬引導(dǎo)車的沿墻行為

沿墻運(yùn)動(dòng)行為是指在運(yùn)動(dòng)過程中，遇到墻壁或者障礙物時(shí)，沿墻壁或障礙物表面進(jìn)行運(yùn)動(dòng)，從而實(shí)現(xiàn)無碰撞運(yùn)動(dòng)。

由于沿墻行為缺少全局信息時(shí)會(huì)導(dǎo)致無法擺脫局部最小值問題，因此在基于沿墻行為的避障運(yùn)動(dòng)中，引入“虛擬引導(dǎo)車”的概念，如圖7 所示。虛擬引導(dǎo)車是真實(shí)機(jī)器人在障礙物或墻體表面的投影，位于真實(shí)機(jī)器人通過正交投影與障礙物表面相交的點(diǎn)，虛擬引導(dǎo)車運(yùn)動(dòng)的迎角是機(jī)器人所在位置的切線與墻面或障礙物表面形成的角度。在沿墻行為中，為了保持虛擬引導(dǎo)車與真實(shí)機(jī)器人之間的距離和角度，需要添加虛擬的斥力作用。因此，機(jī)器人避障是虛擬引導(dǎo)車的“斥力”與目標(biāo)點(diǎn)的引力共同作用的結(jié)果。

采用沿墻行為需要對(duì)墻面進(jìn)行建模，主要包括定義墻面方向和墻面類型確定2 個(gè)方面的工作。

1）墻面方向。

在機(jī)器人上使用模擬的激光測距儀，可以比較精確地辨識(shí)墻面輪廓，因此可通過均值濾波的方法大致判定墻面走向。如圖7 所示，激光點(diǎn)i(i=1,2,···)的坐標(biāo)為(oxi,oyi)，激光點(diǎn)i和i+1 之間的矢量連線角度為

墻面的走向?yàn)?/p>

式中N為反射的激光點(diǎn)個(gè)數(shù)，N≥2。當(dāng)N=1 時(shí)，ηi無法計(jì)算，機(jī)器人將按原方向進(jìn)行運(yùn)動(dòng)。當(dāng)ηwall的方向與機(jī)器人的迎角 θveh的夾角大于90°，導(dǎo)致機(jī)器人陷入沿墻運(yùn)動(dòng)，需要對(duì)墻面走向進(jìn)行修正，得到新的墻面方向?yàn)?/p>

2）墻面類型確定。

機(jī)器人在移動(dòng)過程中會(huì)遇到幾種墻面類型，主要根據(jù)機(jī)器人能夠掃描到的墻面數(shù)量進(jìn)行區(qū)分。一個(gè)機(jī)器人一次性掃描得到的障礙物信息點(diǎn)序列為若?δi∈(0,2(rveh+D))，則稱機(jī)器人面對(duì)的障礙物是單面墻體；若?δi∈(2(rveh+D),+∞)，則稱機(jī)器人面對(duì)障礙物是多面墻體。其中，rveh為當(dāng)前機(jī)器人的最大半徑，D為機(jī)器人的安全碰撞距離。

2.2.2 融合沿墻行為的避障算法

基于上述原理，通過將人工勢場法和沿墻行為進(jìn)行整合設(shè)計(jì)避障算法。如圖8 所示為融合沿墻行為的避障算法的基本原理。該算法主要由以下4 個(gè)步驟組成。

圖8 沿墻運(yùn)動(dòng)法與人工勢場法的切換

1)虛擬引導(dǎo)車的生成條件判定。

機(jī)器人在避障過程中，如果所受的合力處于平衡狀態(tài)，會(huì)陷入局部最小值問題，機(jī)器人停止運(yùn)動(dòng)，此時(shí)生成虛擬引導(dǎo)車，將機(jī)器人與引導(dǎo)車之間的初始距離記為ds0。

2)沿墻運(yùn)動(dòng)狀態(tài)的描述。

機(jī)器人以沿墻行為方式運(yùn)動(dòng)時(shí)，首先需要確定墻面的類型和相應(yīng)的墻面方向η′wall，機(jī)器人運(yùn)動(dòng)過程中速度保持在v0。沿墻避障運(yùn)動(dòng)時(shí)，要求機(jī)器人實(shí)時(shí)判斷與虛擬引導(dǎo)車的間距ds，若ds

3)虛擬引導(dǎo)車消失條件的判定。

機(jī)器人在避障過程中需要在沿墻運(yùn)動(dòng)和人工勢場法之間切換，因此需要對(duì)虛擬引導(dǎo)車的消失條件進(jìn)行判斷，判定條件為：1）當(dāng)障礙物的作用范圍不對(duì)機(jī)器人產(chǎn)生影響時(shí)，虛擬引導(dǎo)車將會(huì)消失；2）當(dāng)機(jī)器人所受虛擬引導(dǎo)車的斥力與引力方向構(gòu)成銳角時(shí)，其已經(jīng)離開障礙物表面，向目標(biāo)點(diǎn)移動(dòng)。

4)以人工勢場法進(jìn)行運(yùn)動(dòng)。

在人工勢場法的作用下，機(jī)器人朝目標(biāo)運(yùn)動(dòng)，當(dāng)遇到障礙物時(shí)，切換至沿墻運(yùn)動(dòng)行為，重復(fù)步驟1)~3)。

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

3.1 多移動(dòng)機(jī)器人編隊(duì)控制仿真

為了證明基于l-φ輸出反饋律改進(jìn)后的多機(jī)器人編隊(duì)運(yùn)動(dòng)算法的穩(wěn)定性，在MATLAB 中進(jìn)行多機(jī)器人編隊(duì)仿真實(shí)驗(yàn)。將參與編隊(duì)運(yùn)動(dòng)的5 個(gè)機(jī)器人分布在仿真環(huán)境為15 m×15 m的矩陣區(qū)域中，5個(gè)機(jī)器人的初始位姿分別為V1為領(lǐng)航者，V2～V5為跟隨者，以領(lǐng)航者為參考基準(zhǔn)，跟隨者和領(lǐng)航者之間的距離絕對(duì)值為‖l12‖ =‖l13‖ =0.7和‖l14‖ = ‖l15‖ =1.4，相對(duì)角度的絕對(duì)值為‖φ12‖ = ‖φ14‖ =‖φ13‖ = ‖φ15‖ =π/2。

領(lǐng)航-跟隨編隊(duì)運(yùn)動(dòng)效果如圖9（a）所示，基于吸附行為的領(lǐng)航跟隨編隊(duì)運(yùn)動(dòng)效果如圖9（b）所示。可以看出領(lǐng)航-跟隨編隊(duì)運(yùn)動(dòng)中，當(dāng)領(lǐng)航者大于0.8 m/s 速度時(shí)運(yùn)動(dòng)，導(dǎo)致整個(gè)編隊(duì)隊(duì)形擴(kuò)散，即編隊(duì)被破壞；而融合吸附行為后，編隊(duì)成員的運(yùn)動(dòng)軌跡都是比較平滑無振蕩的，而且跟隨者的運(yùn)動(dòng)軌跡都是收斂于領(lǐng)航者的運(yùn)動(dòng)軌跡，表明基于吸附行為的領(lǐng)航-跟隨算法的編隊(duì)系統(tǒng)能保持穩(wěn)定狀態(tài)。如圖9 所示，多機(jī)器人編隊(duì)系統(tǒng)由5 個(gè)編號(hào)為1~5 的機(jī)器人組成，5 個(gè)機(jī)器人從起點(diǎn)位置出發(fā)前往目標(biāo)節(jié)點(diǎn)。

圖9 改進(jìn)前后編隊(duì)運(yùn)動(dòng)軌跡

為進(jìn)一步驗(yàn)證改進(jìn)算法的有效性和準(zhǔn)確性，將基于吸附行為的領(lǐng)航-跟隨（attachment behavior based leader-follower, ABLF）控制算法與經(jīng)典的領(lǐng)航-跟隨（leader-follower, LF）控制算法以及基于虛擬領(lǐng)航者的領(lǐng)航-跟隨（virtual leader leader-follower,VLLF）控制算法進(jìn)行對(duì)比。當(dāng)領(lǐng)航者以1 m/s 速度運(yùn)動(dòng)時(shí)，得到如圖10 所示的領(lǐng)航者和跟隨者2在運(yùn)行過程中相對(duì)距離l和相對(duì)角度φ的對(duì)比結(jié)果。

圖10 3 種算法對(duì)比仿真

從圖10（a）可以很明顯看出在領(lǐng)航者行駛速度較快時(shí)，LF 的領(lǐng)航者與跟隨者的相對(duì)距離逐漸拉開，VLLF 由于虛擬領(lǐng)航者的存在，在一定程度上也能保持穩(wěn)定狀態(tài)，但速度過快同樣會(huì)導(dǎo)致領(lǐng)航者與跟隨者相對(duì)距離變大。而ABLF 算法下領(lǐng)航者與跟隨者的相對(duì)距離保持在穩(wěn)定的0.7 m 左右。如圖10（b） 所示為領(lǐng)航者與跟隨者2 的相對(duì)角度的變化，可以看出ABLF 的相對(duì)角度穩(wěn)定在90°左右，而LF 和VLLF 的相對(duì)角度則出現(xiàn)跳變，無法保持穩(wěn)定。

從對(duì)比結(jié)果可以分析得出ABLF 算法下各成員之間的相對(duì)距離和角度穩(wěn)定在設(shè)定值（l=ld，φ=φd），從而形成穩(wěn)定的編隊(duì)運(yùn)動(dòng)，證明了基于吸附行為的領(lǐng)航跟隨編隊(duì)控制算法具有更高的穩(wěn)定性和準(zhǔn)確性。

3.2 多移動(dòng)機(jī)器人避障仿真

針對(duì)傳統(tǒng)的人工勢場函數(shù)存在的局限性問題，本文提出的基于沿墻行為的避障策略能夠有效地避免該問題。本文對(duì)改進(jìn)前后的算法進(jìn)行仿真，驗(yàn)證其有效性。機(jī)器人初始位置隨機(jī)分布，仿真環(huán)境中隨機(jī)設(shè)置一些靜態(tài)障礙物（黑色塊部分），5 個(gè)機(jī)器人從起點(diǎn)位置出發(fā)前往目標(biāo)節(jié)點(diǎn)。

1) 目標(biāo)不可達(dá)問題。

通過實(shí)驗(yàn)對(duì)比發(fā)現(xiàn)，如圖11 所示，當(dāng)障礙物在目標(biāo)附近，傳統(tǒng)人工勢場法會(huì)導(dǎo)致機(jī)器人無法到達(dá)。而改進(jìn)后的融合沿墻行為的避障算法可以有效地到達(dá)目標(biāo)點(diǎn)。

圖11 目標(biāo)不可達(dá)問題

2) 局部最小值問題。

如圖12（a）所示移動(dòng)機(jī)器人在傳統(tǒng)的人工勢場函數(shù)下進(jìn)行避障運(yùn)動(dòng)，可以看到機(jī)器人在障礙物處停止了，即陷入了局部最小值的“陷阱”。如圖12（b）所示是融合沿墻行為的避障運(yùn)動(dòng)，經(jīng)過障礙物附近的運(yùn)動(dòng)軌跡與障礙物表面的形狀基本吻合，即進(jìn)入沿墻行為，如4 號(hào)機(jī)器人和5 號(hào)機(jī)器人所示的軌跡；未檢測到障礙物的機(jī)器人在目標(biāo)點(diǎn)的吸引下朝著目標(biāo)方向移動(dòng)，如1~3 號(hào)機(jī)器人的運(yùn)動(dòng)軌跡，機(jī)器人都成功地避免開障礙物，并未出現(xiàn)停止運(yùn)動(dòng)的狀態(tài)。仿真結(jié)果表明機(jī)器人融合沿墻行為的避障策略成功地避免了陷入局部最小值的情況。

圖12 局部最小值問題

3.3 多移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)仿真

本文以自主研制的多移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)來驗(yàn)證算法的有效性。該系統(tǒng)由移動(dòng)機(jī)器人和工作站組成，如圖13 所示，所有機(jī)器人和工作站均安裝ubuntu16.04 操作系統(tǒng)，并運(yùn)行ROS Indigo。移動(dòng)機(jī)器人采用上位機(jī)與下位機(jī)通信模式的軟件架構(gòu)，其中上位機(jī)采用樹莓派4B 作為主控板，下位機(jī)采用STM32 作為驅(qū)動(dòng)板，同時(shí)配置激光雷達(dá)、電機(jī)等。

圖13 多移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)

機(jī)器人以分散的方式控制，工作站不負(fù)責(zé)控制任何跟隨機(jī)器人。每個(gè)機(jī)器人和工作站運(yùn)行一個(gè)自組織通信節(jié)點(diǎn)，從而組成一個(gè)自組織網(wǎng)絡(luò)。Adhoc 通信節(jié)點(diǎn)創(chuàng)建一個(gè)服務(wù)調(diào)用，其中包括發(fā)送方和接收方ID、傳輸數(shù)據(jù)、目的地發(fā)布數(shù)據(jù)主題等。在工作站執(zhí)行一個(gè)Adhoc 通信節(jié)點(diǎn)，與機(jī)器人通信，并為機(jī)器人定義編隊(duì)中的角色，領(lǐng)導(dǎo)者或者跟隨者。所需編隊(duì)的相對(duì)距離和角度從工作站發(fā)送給每個(gè)跟隨機(jī)器人。通信模式如圖14 所示。

圖14 通信模式

實(shí)驗(yàn)1多移動(dòng)機(jī)器人編隊(duì)控制實(shí)驗(yàn)驗(yàn)證。

利用搭建的多移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)驗(yàn)證本文提出算法在實(shí)際環(huán)境中的有效性。本文設(shè)計(jì)一個(gè)5 m×5 m 的場地，構(gòu)造移動(dòng)機(jī)器人編隊(duì)避障仿真環(huán)境，選取3 臺(tái)機(jī)器人完成三角形隊(duì)形實(shí)驗(yàn)。以各移動(dòng)機(jī)器人的中心點(diǎn)為參考，設(shè)置領(lǐng)航者和跟隨者之間的期望距離為0.3 m；以領(lǐng)航者為參考點(diǎn)，設(shè)置跟隨者與領(lǐng)航者的期望相對(duì)角度絕對(duì)值為90°。

本文提出的編隊(duì)方案測試結(jié)果如圖15 所示。領(lǐng)航者速度為1 m/s，根據(jù)觀察從圖15（a）~圖15（d）編隊(duì)運(yùn)動(dòng)過程中的運(yùn)行軌跡（黑色虛線）可知，移動(dòng)機(jī)器人在行進(jìn)過程中編隊(duì)隊(duì)形基本上保持不變。

圖15 基于改進(jìn)領(lǐng)航跟隨算法的編隊(duì)運(yùn)動(dòng)

統(tǒng)計(jì)領(lǐng)航者與2 個(gè)跟隨者編隊(duì)運(yùn)動(dòng)過程的相對(duì)距離和相對(duì)角度的結(jié)果如圖16 所示。領(lǐng)航者與跟隨者的相對(duì)距離和角度與期望的設(shè)定值基本吻合，圖16 中出現(xiàn)波動(dòng)的點(diǎn)表示編隊(duì)在進(jìn)行轉(zhuǎn)彎動(dòng)作時(shí)出現(xiàn)的正常結(jié)果。由實(shí)驗(yàn)結(jié)果可以驗(yàn)證基于改進(jìn)領(lǐng)航跟隨編隊(duì)控制算法的可行性、有效性。

圖16 領(lǐng)航者與跟隨者相對(duì)距離和相對(duì)角度

實(shí)驗(yàn)2多移動(dòng)機(jī)器人避障算法實(shí)驗(yàn)驗(yàn)證。

本文考慮多移動(dòng)機(jī)器人在未知障礙環(huán)境下的避障仿真實(shí)驗(yàn)，由3 臺(tái)移動(dòng)機(jī)器人構(gòu)成三角編隊(duì)，由2 個(gè)尺寸為500 mm×450 mm×50 mm 的箱子作為障礙物放置在起點(diǎn)和目標(biāo)點(diǎn)之間，機(jī)器人沿障礙物移動(dòng)的安全距離為20 mm。

多移動(dòng)機(jī)器人避障視頻截圖如圖17 所示。移動(dòng)機(jī)器人在移動(dòng)的過程中，不斷探測周圍環(huán)境信息，更新領(lǐng)航者位置，通過運(yùn)動(dòng)軌跡可以看出，移動(dòng)機(jī)器人在運(yùn)動(dòng)過程中會(huì)處于局部最小值狀態(tài)，通過改進(jìn)的人工勢場法，移動(dòng)機(jī)器人進(jìn)行沿墻運(yùn)動(dòng)可以擺脫最小值狀態(tài)，成功通過障礙區(qū)域到達(dá)目的地。由實(shí)驗(yàn)結(jié)果可以驗(yàn)證算法的可行性。

圖17 基于改進(jìn)人工勢場的避障運(yùn)動(dòng)

4 結(jié)論

1) 本文設(shè)計(jì)一種基于吸附行為的領(lǐng)航跟隨算法，通過設(shè)置若干吸附區(qū)有效解決了編隊(duì)隊(duì)形發(fā)散問題，提高了隊(duì)形的穩(wěn)固性。

2）提出在未知環(huán)境下移動(dòng)機(jī)器人融合沿墻行為的避障策略，在傳統(tǒng)人工勢場法的基礎(chǔ)上融合基于“虛擬引導(dǎo)車”的沿墻行為，解決了傳統(tǒng)人工勢場局部極小值和目標(biāo)不可達(dá)的局限性。

3）利用軟件仿真驗(yàn)證算法的有效性，最后通過自主搭建的多移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)，完成未知環(huán)境下編隊(duì)避障實(shí)驗(yàn)，仿真和實(shí)驗(yàn)結(jié)果表明本文所提算法可以保持隊(duì)形的穩(wěn)定性。