羅京，劉成林，劉飛

(江南大學(xué) 輕工過程先進(jìn)控制教育部重點(diǎn)實(shí)驗(yàn)室，江蘇 無錫 214122)

多移動(dòng)機(jī)器人的領(lǐng)航-跟隨編隊(duì)避障控制

羅京，劉成林，劉飛

(江南大學(xué) 輕工過程先進(jìn)控制教育部重點(diǎn)實(shí)驗(yàn)室，江蘇 無錫 214122)

針對(duì)多移動(dòng)機(jī)器人的編隊(duì)控制問題，提出了一種結(jié)合Polar Histogram避障法的領(lǐng)航-跟隨協(xié)調(diào)編隊(duì)控制算法。該算法在領(lǐng)航-跟隨l-φ編隊(duì)控制結(jié)構(gòu)的基礎(chǔ)上引入虛擬跟隨機(jī)器人，將編隊(duì)控制轉(zhuǎn)化為跟隨機(jī)器人對(duì)虛擬跟隨機(jī)器人的軌跡跟蹤控制。結(jié)合移動(dòng)機(jī)器人自身傳感器技術(shù)，在簡單甚至復(fù)雜的環(huán)境下為機(jī)器人提供相應(yīng)的路徑運(yùn)動(dòng)策略，實(shí)現(xiàn)實(shí)時(shí)導(dǎo)航的目的。以兩輪差動(dòng)Qbot移動(dòng)機(jī)器人為研究對(duì)象，搭建半實(shí)物仿真平臺(tái)，進(jìn)行仿真實(shí)驗(yàn)。仿真結(jié)果表明：該方法可以有效地實(shí)現(xiàn)多移動(dòng)機(jī)器人協(xié)調(diào)編隊(duì)和避障控制。

多移動(dòng)機(jī)器人；領(lǐng)航-跟隨；編隊(duì)控制；避障

近年來，隨著機(jī)器人技術(shù)、網(wǎng)絡(luò)通信技術(shù)和自動(dòng)控制技術(shù)的不斷發(fā)展，多移動(dòng)機(jī)器人系統(tǒng)協(xié)調(diào)控制研究引起了眾多領(lǐng)域研究者的關(guān)注, 并在軍事、空間探索、交通控制、醫(yī)療與服務(wù)行業(yè)等領(lǐng)域展現(xiàn)了廣闊的應(yīng)用前景[1]。作為多機(jī)器人系統(tǒng)協(xié)調(diào)控制的最基礎(chǔ)和最重要的研究問題之一，編隊(duì)控制是指空間分布的多個(gè)機(jī)器人，達(dá)到控制目標(biāo)的同時(shí)，保持期望的空間隊(duì)形，同時(shí)要適應(yīng)環(huán)境約束(例如存在障礙物或者空間的物理限制)[2]。

編隊(duì)控制所考察的機(jī)器人主要包括：地面移動(dòng)機(jī)器人[3-4]、無人飛行器[5-7]、衛(wèi)星[8]和自主式潛水器[9-10]等。對(duì)于期望隊(duì)形的實(shí)現(xiàn)問題，控制算法主要有：基于行為法[3-4]、虛擬結(jié)構(gòu)法[11-13]和領(lǐng)航跟隨法[14-16]。此外，對(duì)于多移動(dòng)機(jī)器人系統(tǒng)來說，避障問題也是編隊(duì)中需要考慮的一個(gè)重要問題。在有障礙物的約束環(huán)境下，多移動(dòng)機(jī)器人的編隊(duì)避障控制會(huì)變得更加復(fù)雜，因?yàn)橐苿?dòng)機(jī)器人既要保持整體隊(duì)形，又要合理躲避障礙物。對(duì)于未知環(huán)境下的機(jī)器人避障問題，已經(jīng)有很多有效的解決方法。文獻(xiàn)[17]提出人工勢場法，基本原理是構(gòu)造機(jī)器人和障礙物、目標(biāo)點(diǎn)間的力場，機(jī)器人將在目標(biāo)點(diǎn)的引力和障礙物的斥力的合力作用下運(yùn)動(dòng)。勢場法存在若干缺陷，機(jī)器人在相近障礙物間不能找到路徑，易在障礙物前震蕩，在狹窄通道中擺動(dòng)[18]。為解決這些問題，文獻(xiàn)[19]提出了向量場直方圖法(vector field histogram：VFH)，該方法將障礙物對(duì)機(jī)器人的影響量化為機(jī)器人各個(gè)角度上的障礙強(qiáng)度值，在障礙強(qiáng)度值低于閾值的角度范圍內(nèi)選擇移動(dòng)方向。VFH法存在閾值敏感問題，閾值過小時(shí)，一些可行通道被忽略，閾值過大時(shí)，不一定發(fā)現(xiàn)前方障礙物[20]。

本文采用領(lǐng)航-跟隨編隊(duì)控制算法來解決多移動(dòng)機(jī)器人系統(tǒng)的編隊(duì)控制問題。為使得多移動(dòng)機(jī)器人系統(tǒng)能夠成功地進(jìn)行編隊(duì)和避障，還采用了一種極坐標(biāo)系下基于障礙物密度的Ploar Histogram避障算法，該避障策略可有效避免VFH法中存在閾值敏感問題。最后，通過3個(gè)Qbot移動(dòng)機(jī)器人的實(shí)驗(yàn)驗(yàn)證了所提算法的有效性。

1 兩輪差分驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)

本文考察的是Quanser公司提供的Qbot移動(dòng)機(jī)器人，屬于差分驅(qū)動(dòng)輪式移動(dòng)機(jī)器人，假設(shè)其質(zhì)心與兩驅(qū)動(dòng)輪軸中心重合。在輪式移動(dòng)機(jī)器人的理論研究中，一般都是假設(shè)車輪與地面之間點(diǎn)接觸，且接觸點(diǎn)和地面之間只有純滾動(dòng)沒有相對(duì)的滑動(dòng)(包括縱向與側(cè)向滑動(dòng))，這種理想條件使得機(jī)器人受到非完整約束?？筛鶕?jù)剛體力學(xué)法求得兩輪差動(dòng)移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型。

差分驅(qū)動(dòng)機(jī)器人向前或向后運(yùn)動(dòng)時(shí)，左右驅(qū)動(dòng)輪的速度相等；當(dāng)左右驅(qū)動(dòng)輪之間存在速度差時(shí)，機(jī)器人將會(huì)繞左(或右)驅(qū)動(dòng)輪軸線上某一點(diǎn)旋轉(zhuǎn)，該點(diǎn)稱為瞬時(shí)曲率中心ICC(Instantaneous Center of Curvature)，如圖1所示。機(jī)器人的運(yùn)動(dòng)軌跡可以通過改變兩驅(qū)動(dòng)輪的速度進(jìn)行控制。

機(jī)器人狀態(tài)可由左右驅(qū)動(dòng)輪軸中點(diǎn)坐標(biāo)(x,y)和航向角θ表示。

圖1 兩輪差分機(jī)器人運(yùn)動(dòng)學(xué)Fig.1 Two differential robot kinematics

由圖1可得：

(1)

(2)

(3)

式中：Vl和Vr分別為左右驅(qū)動(dòng)輪的速度，V為機(jī)器人線速度(假設(shè)機(jī)器人速度V大于等于零)，ω為機(jī)器人繞ICC的旋轉(zhuǎn)角速度，R為左右驅(qū)動(dòng)輪軸心到曲率中心的距離，d為左右驅(qū)動(dòng)輪中心之間的距離。由式(1)，(2)變形可得：

(4)

(5)

機(jī)器人的正向運(yùn)動(dòng)學(xué)描述了機(jī)器人速度與位置狀態(tài)之間的關(guān)系，在給定左右驅(qū)動(dòng)輪速度和機(jī)器人初始位姿(x,y,θ)t=0的情況下，可以求得任意時(shí)刻t時(shí)的機(jī)器人的位姿(x,y,θ)t=t。

下面介紹機(jī)器人相對(duì)初始狀態(tài)的位姿[21]。

已知機(jī)器人速度V(t)和航向角θ(t)，機(jī)器人在t=0時(shí)刻的初始位姿為(0,0,0)，受非完整約束條件下的輪式機(jī)器人存在如下關(guān)系：

(6)

由式(6)可得機(jī)器人在t+δt的位姿：

(7)

式中：(x,y,θ)和(x*,y*,θ*)分別為t時(shí)刻和t+δt時(shí)刻機(jī)器人的位姿。由式(7)可知，通過改變線速度V和角速度ω可以控制移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡。

對(duì)于兩輪差動(dòng)機(jī)器人將式(3)和(5)帶入方程(6)可得：

(8)

同樣由式(8)化簡可得：

(9)

由式(8)和式(9)可知，對(duì)于兩輪差動(dòng)機(jī)器人通過改變左右驅(qū)動(dòng)輪的速度Vl和Vr就能控制移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡。本文通過控制Vr和Vl來驅(qū)動(dòng)機(jī)器人，實(shí)現(xiàn)編隊(duì)控制。

2 領(lǐng)航-跟隨編隊(duì)控制結(jié)構(gòu)

領(lǐng)航-跟隨編隊(duì)控制法是指定編隊(duì)中的某一機(jī)器人作為領(lǐng)航者，其他機(jī)器人作為跟隨者跟隨領(lǐng)航機(jī)器人運(yùn)動(dòng)。在多移動(dòng)機(jī)器人系統(tǒng)中，一般設(shè)定一個(gè)主領(lǐng)航機(jī)器人，負(fù)責(zé)提供編隊(duì)的導(dǎo)航，決定編隊(duì)的主軌跡。如果系統(tǒng)中的兩個(gè)機(jī)器人存在局部的領(lǐng)航與跟隨關(guān)系，則分別稱這兩個(gè)機(jī)器人為領(lǐng)航機(jī)器人和跟隨機(jī)器人。

圖2 領(lǐng)航-跟隨隊(duì)形結(jié)構(gòu)模型Fig.2 Leader-follower formation structure model

本文在l-φ控制法的基礎(chǔ)上引入虛擬跟隨機(jī)器人，跟隨機(jī)器人與領(lǐng)航機(jī)器人保持期望的隊(duì)形結(jié)構(gòu)，只要跟隨機(jī)器人運(yùn)動(dòng)到虛擬跟隨機(jī)器人的位置，就可以實(shí)現(xiàn)期望的隊(duì)形編隊(duì)控制[23]。如圖3所示，(xl,yl,θl)為領(lǐng)航機(jī)器人位姿，(xf,yf,θf)為跟隨機(jī)器人位姿，跟隨距離和角度分別為l和φ，可得虛擬跟隨機(jī)器人位姿(xv,yv,θv)為

(10)

圖3 領(lǐng)航-虛擬跟隨隊(duì)形結(jié)構(gòu)模型Fig.3 Leader-virtual follower formation structure model

將(xv,yv,θv)作為跟隨機(jī)器人的目標(biāo)點(diǎn)帶入式(9)可得：

(11)

式中(x,y,θ)為跟隨機(jī)器人當(dāng)前位姿，由式(11)得：

(12)

(13)

由式(13)可得：

(14)

式(14)即為編隊(duì)中的跟隨機(jī)器人的左右驅(qū)動(dòng)輪輸入。

虛擬跟隨機(jī)器人的引入，將隊(duì)形保持轉(zhuǎn)換為跟隨機(jī)器人對(duì)于虛擬跟隨機(jī)器人的跟蹤控制，這種隊(duì)形保持模型，可描述任意隊(duì)形結(jié)構(gòu)。

3 避障算法

VFH避障算法通過構(gòu)建向量場直方圖來確定可行方向(如圖4)[19]。VFH算法采用柵格模型，機(jī)器人的工作空間劃分為若干連續(xù)的二維柵格，將機(jī)器人感知的360°范圍劃分為n個(gè)扇區(qū)，每個(gè)扇區(qū)的夾角為360°/n，通過VFH法中定義的極線障礙強(qiáng)度值計(jì)算方法，計(jì)算對(duì)應(yīng)扇區(qū)的極線障礙強(qiáng)度值Hk(k=0,1,…,n-1)。圖4為計(jì)算后得到的直方圖示例，直方圖上的每一個(gè)Hk可視為k扇區(qū)內(nèi)障礙物的密度，Hk越大說明扇區(qū)內(nèi)障礙物越密集。在直方圖上，設(shè)定閾值δ，若扇區(qū)k的Hk值低于δ則視為無障礙區(qū)，若干個(gè)無障礙扇區(qū)構(gòu)成候選區(qū)(Valley)，圖4(以0°～180°為例)中根據(jù)直方圖和閾值δ將得到4個(gè)候選區(qū)，機(jī)器人根據(jù)候選區(qū)的寬度判斷其是否可以通過，在所有可以通過的候選區(qū)中，選擇最接近目標(biāo)的一個(gè)通過。該方法的不足之處是對(duì)閾值敏感，閾值δ過大時(shí)可能會(huì)碰到障礙物，過小則可能一些可行通道將被忽略。

圖4 向量場直方圖Fig.4 Vector field histogram

針對(duì)以上不足之處，本文利用紅外線測距傳感器獲取局部環(huán)境信息，采用一種基于極坐標(biāo)系下障礙物密度的避障方法PloarHistogram避障法。該算法根據(jù)障礙密度搜索一條最安全的導(dǎo)航方向，利用紅外測距傳感器所測數(shù)據(jù)(即距障礙物距離)來設(shè)計(jì)障礙密度值，選擇障礙密度最小的方向作為機(jī)器人的安全導(dǎo)航方向。如圖5所示，其中圖5(a)表示位于0°、45°、90°、135°和180°方向的5個(gè)傳感器獲得的障礙物距離數(shù)據(jù)(m)，其中90°方向?yàn)闄C(jī)器人的正前方。

圖5(b)表示相應(yīng)方向上的極坐標(biāo)障礙物密度值(POD)，其計(jì)算方法如下：

(15)

(16)

圖5 極坐標(biāo)下的障礙物距離及障礙物密度Fig.5 Polar plots of range data and obstacle density

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

為驗(yàn)證算法的實(shí)際有效性，本文以基于iRobot的Create平臺(tái)開發(fā)的自主移動(dòng)機(jī)器人Qbot為研究對(duì)象，其帶有5個(gè)紅外傳感器，分別安裝在機(jī)器人的0°、45°、90°、135°和180°方向上，如圖6所示，搭建Quanser無人工具實(shí)驗(yàn)系統(tǒng)仿真平臺(tái)，進(jìn)行半實(shí)物仿真實(shí)驗(yàn)。

4.1 無人工具實(shí)驗(yàn)系統(tǒng)

實(shí)驗(yàn)系統(tǒng)大致包括以下幾部分，如圖7所示：

1)QuaRc實(shí)時(shí)控制軟件與多智能體任務(wù)開發(fā)系統(tǒng);

2)地面控制站;

3)無人地面機(jī)器人;

4)照相機(jī)定位系統(tǒng)。

圖6 Qbot實(shí)物圖Fig.6 Qbot physical figure

圖7 Quanser無人工具實(shí)驗(yàn)系統(tǒng)Fig.7 Quanser unmanned tool experiment system

QuaRC實(shí)時(shí)控制軟件與Matlab/Simulink兼容，可以方便地調(diào)用Matlab/Simulink中的函數(shù)。將設(shè)計(jì)的控制器與相應(yīng)的系統(tǒng)硬件端口Simulink模塊相連，編譯并下傳到QuaRC中，實(shí)現(xiàn)實(shí)時(shí)控制。地面控制站是通過一臺(tái)地面控制計(jì)算機(jī)來實(shí)現(xiàn)，包括定位、多智能體控制等，主控制機(jī)通過無線局域網(wǎng)和各個(gè)智能體進(jìn)行通信，地面控制站主要進(jìn)行任務(wù)規(guī)劃和定位等目的，一旦控制系統(tǒng)的控制算法設(shè)計(jì)完成，控制站就只起到定位作用。無人地面機(jī)器人是基于iRobot的Create平臺(tái)開發(fā)的自主移動(dòng)機(jī)器人Qbot，它帶有5個(gè)紅外線傳感器，可以用于探測障礙物，探測距離最大可達(dá) 。照相機(jī)定位系統(tǒng)通過8個(gè)紅外照相機(jī)實(shí)現(xiàn)空間的三維定位。

4.2 實(shí)驗(yàn)原理

系統(tǒng)采用Host-Target工作模式進(jìn)行實(shí)時(shí)控制，控制器開發(fā)在裝有MATLAB/Simulink的主機(jī)上實(shí)現(xiàn)，控制器搭建完成后，在Simulink下進(jìn)行編譯并且下載到移動(dòng)機(jī)器人的控制器(target)中實(shí)現(xiàn)實(shí)時(shí)控制。主機(jī)Host可同時(shí)控制多個(gè)控制器(Target)，從而可以在MATLAB環(huán)境下進(jìn)行多智能體控制算法的研究。每個(gè)移動(dòng)機(jī)器人上都裝有3個(gè)紅外線反射球，使裝在墻壁上的8臺(tái)紅外照相機(jī)可以實(shí)時(shí)捕捉每個(gè)移動(dòng)機(jī)器人的位姿坐標(biāo)(x,y,θ)，通過主機(jī)Host和無線網(wǎng)絡(luò)傳輸給每個(gè)移動(dòng)機(jī)器人，跟隨機(jī)器人按預(yù)先設(shè)計(jì)好的算法計(jì)算出與領(lǐng)航機(jī)器人的位置差，調(diào)整方向跟蹤領(lǐng)航機(jī)器人以實(shí)現(xiàn)編隊(duì)控制。編隊(duì)過程中，領(lǐng)航和跟隨機(jī)器人通過自身攜帶的紅外線傳感器檢測環(huán)境中的障礙物，機(jī)器人根據(jù)檢測到的障礙物情況利用PolarHistogram避障控制策略實(shí)時(shí)避開障礙物，實(shí)現(xiàn)編隊(duì)避障控制。

4.3 算法實(shí)現(xiàn)步驟

1)在主機(jī)Host上設(shè)計(jì)領(lǐng)航機(jī)器人的跟蹤路徑。建立無線局域網(wǎng)。

2)主機(jī)Host實(shí)時(shí)地將照相機(jī)定位系統(tǒng)捕捉到的領(lǐng)航和跟隨機(jī)器人的位姿信息發(fā)送給對(duì)應(yīng)的領(lǐng)航和跟隨機(jī)器人，同時(shí)將設(shè)計(jì)好的路徑命令發(fā)送給領(lǐng)航機(jī)器人。

3)領(lǐng)航機(jī)器人實(shí)時(shí)接收主機(jī)傳來的路徑命令和自身位姿信息向目標(biāo)點(diǎn)運(yùn)行，當(dāng)自身攜帶的紅外線傳感器檢測到障礙物時(shí)，利用設(shè)計(jì)好的避障算法進(jìn)行避障，繞過障礙后繼續(xù)駛向目標(biāo)點(diǎn)。同時(shí)，領(lǐng)航機(jī)器人根據(jù)l-φ控制法生成虛擬機(jī)器人的軌跡命令，并發(fā)送給跟隨機(jī)器人。

4)跟隨機(jī)器人實(shí)時(shí)接收主機(jī)Host發(fā)送的自身位姿信息和領(lǐng)航機(jī)器人發(fā)送的虛擬機(jī)器人軌跡命令，跟隨機(jī)器人沿著虛擬跟隨機(jī)器人的軌跡跟隨領(lǐng)航機(jī)器人，同時(shí)能夠有效避開環(huán)境中的障礙物，實(shí)現(xiàn)編隊(duì)避障控制。

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

設(shè)計(jì)一個(gè)3 m×3 m的場地，構(gòu)造機(jī)器人編隊(duì)避開障礙物到達(dá)目標(biāo)點(diǎn)的仿真環(huán)境，利用領(lǐng)航跟隨編隊(duì)法和PolarHistogram避障法，分別進(jìn)行了3個(gè)機(jī)器人柱形編隊(duì)避障控制實(shí)驗(yàn)和3個(gè)機(jī)器人三角形編隊(duì)避障控制實(shí)驗(yàn)，柱形和三角形編隊(duì)隊(duì)形如圖8所示。

圖8 多移動(dòng)機(jī)器人的編隊(duì)隊(duì)形Fig.8 Formation of multiple mobile robots

實(shí)驗(yàn)中設(shè)置機(jī)器人的紅外傳感器探測距離dth=0.4 m，控制周期為0.1 s，Qbot兩驅(qū)動(dòng)輪之間的距離d=252.5 mm。

實(shí)驗(yàn)1 3個(gè)Qbot移動(dòng)機(jī)器人柱形編隊(duì)避障實(shí)驗(yàn)

在主機(jī)Host上設(shè)計(jì)領(lǐng)航機(jī)器人從任意初始位置，經(jīng)中間點(diǎn)(0.9，0.9)、(-0.9，0.9)、(-0.9，-0.9)和(0.9，-0.9)運(yùn)動(dòng)到目標(biāo)點(diǎn)(0.9，0.9)。障礙物為長方體和圓柱體障礙物，位置任意擺放在路徑當(dāng)中。編隊(duì)隊(duì)形為柱形，跟隨機(jī)器人RF1和RF2與領(lǐng)航機(jī)器人RL的期望相對(duì)距離分別為0.4m和0.8m，相對(duì)方位角都為φ=180°。圖9是3個(gè)Qbot柱形編隊(duì)實(shí)驗(yàn)過程的部分截圖，可以看出兩跟隨機(jī)器人可以跟隨領(lǐng)航機(jī)器人從初始位置，經(jīng)中間點(diǎn)(0.9，0.9)、(-0.9，0.9)、(-0.9，-0.9)和(0.9，-0.9)運(yùn)動(dòng)到目標(biāo)點(diǎn)(0.9，0.9)，并且可以順利避開障礙物。

圖9 柱形編隊(duì)避障過程Fig.9 Cylindrical formation process of obstacle avoidance

編隊(duì)軌跡如圖10(a)所示，兩跟隨機(jī)器人RF1和RF2跟隨領(lǐng)航機(jī)器人RL保持柱形編隊(duì)順利避開障礙物達(dá)到目標(biāo)點(diǎn)，領(lǐng)航機(jī)器人運(yùn)動(dòng)的軌跡即為跟隨機(jī)器人期望的跟蹤軌跡。領(lǐng)航機(jī)器人RL和跟隨機(jī)器人RF1、RF2的左右輪速度控制輸入如圖10(b)、(d)和(f)所示，可以看出各機(jī)器人的左右輪速度控制輸入是時(shí)變的，當(dāng)機(jī)器人轉(zhuǎn)彎或遇障礙物變向時(shí)，各機(jī)器人左右輪速度變化較大。領(lǐng)航機(jī)器人RL的位姿如圖8(c)所示，領(lǐng)航機(jī)器人跟蹤點(diǎn)(0.9，0.9)、(-0.9，0.9)、(-0.9，-0.9)以及(0.9，-0.9)最終到達(dá)目標(biāo)點(diǎn)(0.9，0.9)，其中在遇到障礙物時(shí)機(jī)器人偏離期望軌跡存在誤差，但隨著編隊(duì)的進(jìn)行，機(jī)器人回歸期望軌跡，誤差趨近于0。跟隨機(jī)器人RF1和RF2的位姿如圖10(e)和(g)所示，跟隨機(jī)器人都以較小的誤差跟隨虛擬機(jī)器人(即領(lǐng)航機(jī)器人的軌跡)，完成軌跡跟蹤控制并且可以實(shí)時(shí)避開障礙物。

(a)編隊(duì)軌跡

(b)領(lǐng)航機(jī)器人左輪和右輪速度控制輸入

(c)領(lǐng)航機(jī)器人的位姿

(e)跟隨機(jī)器人1的位姿

(f)跟隨機(jī)器人2左輪和右輪速度控制輸入

(g)跟隨機(jī)器人2的位姿 圖10 3個(gè)機(jī)器人柱形編隊(duì)控制實(shí)驗(yàn)Fig.10 Three robots cylindrical formation control experiment

實(shí)驗(yàn)2 3個(gè)Qbot移動(dòng)機(jī)器人三角形編隊(duì)避障實(shí)驗(yàn)。

設(shè)計(jì)領(lǐng)航機(jī)器人RL從初始位置點(diǎn)(0，0)，經(jīng)中間點(diǎn)(0，0.6)和(0，1.6)最終到達(dá)目標(biāo)點(diǎn)(0，2.2)。引入虛擬跟隨機(jī)器人Rv1和Rv2分別與領(lǐng)航機(jī)器人RL保持相對(duì)距離為0.5 m與0.5 m，相對(duì)期望角為135°與-135°。跟隨機(jī)器人RF1和RF2分別跟隨虛擬跟隨機(jī)器人Rv1與Rv2。

圖11是3個(gè)Qbot三角形編隊(duì)避障實(shí)驗(yàn)過程的部分截圖，可以看出機(jī)器人小組在保持三角形編隊(duì)的同時(shí)也可以順利避開障礙物。

編隊(duì)軌跡如圖12(a)所示，機(jī)器人隊(duì)形為三角形，前進(jìn)一段時(shí)間后，編隊(duì)遇到障礙物，領(lǐng)航和跟隨機(jī)器人各自避開障礙物后，繼續(xù)以三角形編隊(duì)到達(dá)目標(biāo)點(diǎn)。領(lǐng)航機(jī)器人RL和跟隨機(jī)器人RF1、RF2的左右輪速度控制輸入如圖12(b) 、(d)和(f)所示，可以看出，各機(jī)器人的左右輪速度控制輸入是時(shí)變的，當(dāng)機(jī)器人轉(zhuǎn)彎或遇障礙物變向時(shí)，各機(jī)器人左右輪速度變化較大。領(lǐng)航機(jī)器人的位姿如圖12(c)所示，領(lǐng)航機(jī)器人在初始位置(0，0)跟蹤中間點(diǎn)(0，0.6)和(0，1.6)最終到達(dá)目標(biāo)點(diǎn)(0，2.2)，在避障時(shí)，機(jī)器人偏離期望跟蹤點(diǎn)存在誤差，但隨著編隊(duì)的進(jìn)行，機(jī)器人回歸期望跟蹤軌跡，誤差趨近于0。跟隨機(jī)器人RF1和RF2的位姿如圖12(e)和(g)所示，跟隨機(jī)器人RF1和RF2都以較小的誤差跟蹤虛擬跟隨機(jī)器人Rv2與Rv1的軌跡，同時(shí)可以順利避開障礙物，完成編隊(duì)控制。

圖11 三角形編隊(duì)避障過程Fig.11 Triangle formation process of obstacle avoidance

(a) 編隊(duì)軌跡

(b) 領(lǐng)航機(jī)器人左輪和右輪速度控制輸入

(c) 領(lǐng)航機(jī)器人的位姿

(d) 跟隨機(jī)器人1左輪和右輪速度控制輸入

(e)跟隨機(jī)器人1的位姿

(f)跟隨機(jī)器人2左輪和右輪速度控制輸入

(g)跟隨機(jī)器人2的位姿圖12 3個(gè)機(jī)器人三角形編隊(duì)控制實(shí)驗(yàn)Fig.12 Three robots triangle formation control experiment

實(shí)驗(yàn)3 3個(gè)Qbot移動(dòng)機(jī)器人隊(duì)形變換避障實(shí)驗(yàn)

圖13為3個(gè)Qbot隊(duì)形變換的編隊(duì)避障實(shí)驗(yàn)過程的部分截圖。

圖13 隊(duì)形變換編隊(duì)避障過程Fig.13 Formation transformation process of obstacle avoidance

設(shè)計(jì)領(lǐng)航機(jī)器人RL從初始位置點(diǎn)為(0，0)，經(jīng)中間點(diǎn)(0，0.4)和(0，1.7)最終到達(dá)目標(biāo)點(diǎn)(0，2.2)。無障礙環(huán)境下，虛擬跟隨機(jī)器人Rv1和Rv2分別與領(lǐng)航機(jī)器人RL保持相對(duì)距離為0.5 m與0.5 m，對(duì)應(yīng)的相對(duì)期望角為135°與-135°。存在障礙的環(huán)境下，虛擬跟隨機(jī)器人Rv1和Rv2分別與領(lǐng)航機(jī)器人RL保持相對(duì)距離為0.3 m與0.6 m，對(duì)應(yīng)的相對(duì)期望角均為0°。跟隨機(jī)器人RF1和RF2分別跟隨虛擬跟隨機(jī)器人Rv1與Rv2。在未發(fā)現(xiàn)障礙物時(shí)，機(jī)器人小組保持三角形編隊(duì)隊(duì)形行駛；當(dāng)檢測到障礙物時(shí)，機(jī)器人小組變換為柱形編隊(duì)，在跟隨機(jī)器人緊跟領(lǐng)航機(jī)器人穿過障礙物后，機(jī)器人小組恢復(fù)三角形編隊(duì)到達(dá)目標(biāo)點(diǎn)。

3個(gè)機(jī)器人隊(duì)形變換編隊(duì)控制實(shí)驗(yàn)如圖14所示。

編隊(duì)軌跡如圖14(a)所示：機(jī)器人初始以三角形隊(duì)形前進(jìn)；在檢測到障礙物后，隊(duì)形變換為柱形通過障礙區(qū)；安全避障后，恢復(fù)三角形編隊(duì)到達(dá)目標(biāo)點(diǎn)。

(a) 編隊(duì)軌跡

(b) 領(lǐng)航機(jī)器人左輪和右輪速度控制輸入

(c) 領(lǐng)航機(jī)器人的位姿

(d) 跟隨機(jī)器人1左輪和右輪速度控制輸入

(e)跟隨機(jī)器人1的位姿

(g)跟隨機(jī)器人2的位姿圖14 3個(gè)機(jī)器人隊(duì)形變換編隊(duì)控制實(shí)驗(yàn)Fig.14 Three robots formation transformation control experiment

領(lǐng)航機(jī)器人RL和跟隨機(jī)器人RF1，RF2的左右輪速度控制輸入如圖14(b) ，(d)和(f)所示：各機(jī)器人的左右輪速度控制輸入是時(shí)變的，當(dāng)機(jī)器人轉(zhuǎn)彎或遇障礙物變向時(shí)，各機(jī)器人左右輪速度變化較大。領(lǐng)航機(jī)器人的位姿如圖14(c)所示：領(lǐng)航機(jī)器人在初始位置(0，0)跟蹤中間點(diǎn)(0，0.4)和(0，1.7)最終到達(dá)目標(biāo)點(diǎn)(0，2.2)；避障時(shí)，機(jī)器人偏離期望跟蹤點(diǎn)存在偏差，但隨著編隊(duì)的進(jìn)行，機(jī)器人回歸期望跟蹤軌跡，誤差趨近于0。跟隨機(jī)器人RF1和RF2的位姿如圖14(e)和(g)所示：跟隨機(jī)器人RF1和RF2都以較小的誤差跟蹤虛擬跟隨機(jī)器人Rv2與Rv1的軌跡，完成編隊(duì)控制。

從上述實(shí)驗(yàn)過程能夠看出，機(jī)器人小組可以保持某單一隊(duì)形完成編隊(duì)避障控制，也可以根據(jù)環(huán)境的約束自主變換隊(duì)形。機(jī)器人小組在選擇更安全的隊(duì)形通過障礙區(qū)后，自主恢復(fù)到初始隊(duì)形，最終順利到達(dá)目標(biāo)點(diǎn)，完成編隊(duì)避障控制。

6 結(jié)束語

本文以兩輪差動(dòng)機(jī)器人為研究對(duì)象，提出了在障礙環(huán)境中的多移動(dòng)機(jī)器人的編隊(duì)控制方法，通過引入虛擬跟隨機(jī)器人，將編隊(duì)控制轉(zhuǎn)化為跟蹤控制，此外，通過編隊(duì)中的領(lǐng)航和跟隨機(jī)器人攜帶的紅外線傳感器自主感知獲取外部環(huán)境信息，采用PolarHistogram避障法，引導(dǎo)多機(jī)器人編隊(duì)智能地避開障礙物，實(shí)現(xiàn)了多移動(dòng)機(jī)器人的編隊(duì)避障控制，該避障策略不存在閾值敏感性問題，因而對(duì)復(fù)雜的環(huán)境也具有很好的適應(yīng)性。利用Quanser無人工具實(shí)驗(yàn)系統(tǒng)仿真平臺(tái)，分別對(duì)柱形編隊(duì)、三角形編隊(duì)以及隊(duì)形變換編隊(duì)進(jìn)行實(shí)物實(shí)驗(yàn)，由仿真實(shí)驗(yàn)結(jié)果可看出本文所提出的控制方法的有效性，多移動(dòng)機(jī)器人既可以保持編隊(duì)隊(duì)形，同時(shí)各機(jī)器人也可以自主避障，實(shí)現(xiàn)多移動(dòng)機(jī)器人協(xié)調(diào)編隊(duì)和避障控制。

[1]FARINELLIA,LOCCHIL,NARDID.Multirobotsystems:aclassificationfocusedoncoordination[J].IEEEtransactionsonsystems,man,andcybernetics,partB(cybernetics), 2004, 34(5): 2015-2028.

[2]王祥科, 李迅, 鄭志強(qiáng). 多智能體系統(tǒng)編隊(duì)控制相關(guān)問題研究綜述[J]. 控制與決策, 2013, 28(11): 1601-1613.WANGXiangke,LIXun,ZHENGZhiqiang.Surveyofdevelopmentsonmulti-agentformationcontrolrelatedproblems[J].Controlanddecision, 2013, 28(11): 1601-1613.

[3]楊帆, 劉士榮, 董德國. 編隊(duì)控制中的機(jī)器人行為與基于服務(wù)的運(yùn)動(dòng)行為結(jié)構(gòu)設(shè)計(jì)[J]. 機(jī)器人, 2012, 34(1): 120-128.YANGFan,LIUShirong,DONGDeguo.Robotbehaviorandservice-basedmotionbehaviorstructuredesigninformationcontrol[J].Robot, 2012, 34(1): 120-128.

[4]ARRICHIELLOF,CHIAVERINIS,INDIVERIG,etal.Thenull-space-basedbehavioralcontrolformobilerobotswithvelocityactuatorsaturations[J].Theinternationaljournalofroboticsresearch, 2010, 29(10): 1317-1337.

[5]宋敏, 魏瑞軒, 沈東, 等. 基于非線性動(dòng)態(tài)逆的無人機(jī)編隊(duì)協(xié)同控制[J]. 控制與決策, 2011, 26(3): 448-452.SONGMin,WEIRuixuan,SHENDong,etal.UAVformationcontrolbasedonnonlineardynamicinversion[J].Controlanddecision, 2011, 26(3): 448-452.

[6]李少斌, 陳炎財(cái), 楊忠, 等. 具有通信延遲的多無人機(jī)編隊(duì)飛行控制[J]. 信息與控制, 2012, 41(2): 142-146.LIShaobin,CHENYancai,YANGZhong,etal.Formationfightcontrolofmulti-UAVswithcommunicationdelay[J].Informationandcontrol, 2012, 41(2): 142-146.

[7]魏瑞軒, 茹常劍, 祁曉明. 通信延遲條件下無人機(jī)編隊(duì)重構(gòu)的自主安全控制[J]. 控制理論與應(yīng)用, 2013, 30(9): 1099-1108.WEIRuixuan,RUChangjian,QIXioaming.Autonomoussafetycontrolofunmannedaerialvehicleformationreconfigurationundercommunicationdelay[J].Controltheory&applications, 2013, 30(9): 1099-1108.

[8]吳錦杰, 劉昆, 韓大鵬, 等. 基于對(duì)偶四元數(shù)的衛(wèi)星主從式編隊(duì)姿軌跟蹤的優(yōu)化控制[J]. 控制理論與應(yīng)用, 2013, 30(9): 1069-1078.WUJinjie,LIUKun,HANDapeng,etal.Optimizedtracking-controlforattitudeandorbitofsatelliteformationflyingusingdualquaternion[J].Controltheory&applications, 2013, 30(9): 1069-1078.

[9]WANGY,YANW,LIJ.Passivity-basedformationcontrolofautonomousunderwatervehicles[J].IETcontroltheory&applications, 2012, 6(4): 518-525.

[10]王銀濤, 嚴(yán)衛(wèi)生. 多自主水下航行器系統(tǒng)一致性編隊(duì)跟蹤控制[J]. 控制理論與應(yīng)用, 2013, 30(3): 379-384.WANGYintao,YANWeisheng.Consensusformationtrackingcontrolofmultipleautonomousunderwatervehiclesystems[J].Controltheory&applications, 2013, 30(3): 379-384.

[11]RENWei,SORENSENN.Distributedcoordinationarchitectureformulti-robotformationcontrol[J].Roboticsandautonomoussystems, 2008, 56(4): 324-333.

[12]GHOMMAMJ,MEHRJERDIH,SAADM,etal.Formationpathfollowingcontrolofunicycle-typemobilerobots[J].Roboticsandautonomoussystems, 2010, 58(5): 727-736.

[13]MEHRJERDIH,GHOMMAMJ,SAADM.Nonlinearcoordinationcontrolforagroupofmobilerobotsusingavirtualstructure[J].Mechatronics, 2011, 21(7): 1147-1155.

[14]CONSOLINIL,MORBIDIF,PRATTICHIZZOD,etal.Leader-followerformationcontrolofnonholonomicmobilerobotswithinputconstraints[J].Automatica, 2008, 44(5): 1343-1349.

[15]SHAOJ,XIEG,WANGL.Leader-followingformationcontrolofmultiplemobilevehicles[J].IETcontroltheory&applications, 2007, 1(2): 545-552.

[16]楊麗, 曹志強(qiáng), 譚民. 不確定環(huán)境下多機(jī)器人的動(dòng)態(tài)編隊(duì)控制[J]. 機(jī)器人, 2010, 32(2): 283-288.YANGLi,CAOZhiqiang,TANMin.Dynamicformationcontrolformultiplerobotsinuncertainenvironments[J].Robot, 2010, 32(2): 283-288.

[17]KHATIBO.Real-timeobstacleavoidanceformanipulatorsandmobilerobots[C]//Proceedingsof1985IEEEInternationalConferenceonRoboticsandAutomation.St.Louis,MO,USA:IEEE, 1985: 500-505.

[18]章蘇書, 吳敏, 曹衛(wèi)華. 一種局部動(dòng)態(tài)環(huán)境下的避障算法[J]. 計(jì)算技術(shù)與自動(dòng)化, 2003, 22(1): 12-16.ZHANGSushu,WUMin,CAOWeihua.Anobstacleavoidancealgorithminlocaldynamicenvironments[J].Computingtechnologyandautomation, 2003, 22(1): 12-16.

[19]BORENSTEINJ,KORENY.Thevectorfieldhistogram-fastobstacleavoidanceformobilerobots[J].IEEEtransactionsonroboticsandautomation, 1991, 7(3): 278-288.

[20]李云翀, 何克忠. 基于激光雷達(dá)的室外移動(dòng)機(jī)器人避障與導(dǎo)航新方法[J]. 機(jī)器人, 2006, 28(3): 275-278.LIYunchong,HEKezhong.Anovelobstacleavoidanceandnavigationmethodforoutdoormobilerobotbasedonlaserradar[J].Robot, 2006, 28(3): 275-278.

[21]DUDEKG,JENKINM.Computationalprinciplesofmobilerobotics[M]. 2nded.Cambridge:CambridgeUniversityPress, 2010.

[22]DESAIJP,OSTROWSKIJ,KUMARV.Controllingformationsofmultiplemobilerobots[C]//Proceedingsof1998IEEEInternationalConferenceonRoboticsandAutomation.Leuven,Belgium:IEEE, 1998, 4: 2864-2869.

[23]任立敏, 王偉東, 杜志江, 等. 障礙環(huán)境下多移動(dòng)機(jī)器人動(dòng)態(tài)優(yōu)化隊(duì)形變換[J]. 機(jī)器人, 2013, 35(5): 535-543.RENLimin,WANGWeidong,DUZhijiang,etal.Dynamicandoptimizedformationswitchingformultiplemobilerobotsinobstacleenvironments[J].Robot, 2013, 35(5): 535-543.

羅京，男，1989年生，碩士研究生，主要研究方向?yàn)槎嘧灾黧w系統(tǒng)的編隊(duì)控制研究。

劉成林，男，1981年，副教授，碩士生導(dǎo)師，主要研究方向?yàn)槎嘧灾黧w系統(tǒng)協(xié)調(diào)控制，主持國家自然科學(xué)基金面上項(xiàng)目和江蘇省自然科學(xué)基金面上項(xiàng)目各1項(xiàng)，發(fā)表學(xué)術(shù)論文多篇，其中被SCI檢索20余篇。

劉飛，男，1965年，教授，博士生導(dǎo)師，主要研究領(lǐng)域?yàn)橄冗M(jìn)控制理論與應(yīng)用，過程監(jiān)控，主持國家“863”、國家自然科學(xué)基金等省部以上課題12項(xiàng)，企業(yè)技術(shù)開發(fā)20余項(xiàng)，完成省部鑒定科技成果8項(xiàng)，獲省部及行業(yè)協(xié)會(huì)科技獎(jiǎng)5項(xiàng)，申請/獲得專利及軟件著作權(quán)8項(xiàng)，被SCI/EI檢索論文100余篇。

Piloting-following formation and obstacleavoidance control of multiple mobile robots

LUO Jing， LIU Chenglin， LIU Fei

( Key Laboratory of Advanced Process Control for Light Industry(Ministry of Education), Jiangnan University, Wuxi 214122， China)

In this paper, we propose a piloting-follower coordination formation control algorithm accompanied with a polar histogram obstacle avoidance method; we suggest here that this combined approach addresses the formation control defects that occur when multiple mobile robots must work in unison. Based on the piloting-following formation control structure, we introduce a virtual following robot, thus converting formation control into trajectory tracking control conducted by the robots that follow the virtual following robot. Using the sensing technology of the mobile robot itself, we achieved real-time navigation of corresponding paths in both simple and complicated environments. By using the two-wheel differential Qbot mobile robot as our research object, we constructed a semi-physical simulation platform for our simulation experiments. Simulation results showed that our method was able to effectively achieve coordination formation and obstacle avoidance control of multiple robots.

multiple mobile robots; piloting-following coordination control; formation control; obstacle avoidance

2015-07-22.

日期：2017-02-20.

國家自然科學(xué)基金項(xiàng)目(61473138,61104092,61134007)；江蘇省自然科學(xué)基金項(xiàng)目(BK20151130).

劉成林. E-mail：liucl@jiangnan.edu.cn.

10.11992/tis.201507029

http://www.cnki.net/kcms/detail/23.1538.tp.20170220.1008.004.html

TP

A

1673-4785(2017)02-0202-11

羅京，劉成林，劉飛. 多移動(dòng)機(jī)器人的領(lǐng)航-跟隨編隊(duì)避障控制[J]. 智能系統(tǒng)學(xué)報(bào)， 2017, 12(2): 202-212.

英文引用格式：LUO Jing， LIU Chenglin， LIU Fei. Piloting-following formation and obstacle avoidance control of multiple mobile robots[J]. CAAI transactions on intelligent systems, 2017, 12(2): 202-212.