基于視覺伺服的移動機器人編隊控制研究與設(shè)計*

趙 偉 陸 群 劉甜田 夏菽蘭 陳 健

（鹽城工學(xué)院電氣工程學(xué)院 鹽城 224000）

1 引言

隨著機器人技術(shù)的不斷發(fā)展，多移動機器人編隊控制問題受到了廣泛地關(guān)注，成為了研究的熱點之一［1～3］。多移動機器人編隊控制的研究在各個領(lǐng)域都有實際的應(yīng)用價值，如：服務(wù)型機器人、惡劣環(huán)境探、軍事監(jiān)視偵察、災(zāi)難現(xiàn)場搜救、航空器編隊飛行等［4～6］。

目前依據(jù)國內(nèi)外的研究，多移動機器人的編隊控制主要有人工勢場法、虛擬結(jié)構(gòu)法、圖論法以及主從跟隨法等［7］。文獻(xiàn)［8］中研究了基于人工勢場理論的多移動機器人編隊控制，利用基于相對位置的新勢場理論解決了全局最小問題。文獻(xiàn)［9］中研究了虛擬結(jié)構(gòu)法的分布式無人機編隊控制，利用分布式通信減輕了中心節(jié)點的通信壓力。文獻(xiàn)［10］中基于主從跟隨法，將編隊問題轉(zhuǎn)化為了經(jīng)典控制理論中的誤差跟蹤問題，并結(jié)合虛擬斥力旋轉(zhuǎn)勢場實現(xiàn)編隊避障控制。

以上編隊控制方法中，都需要利用通信設(shè)備來估計領(lǐng)導(dǎo)機器人速度的設(shè)計不僅增加了整個系統(tǒng)的成本，而且還需要跟隨者機器人的硬件支持，同時數(shù)據(jù)丟包和通信的不方便也限制了操作性能。因此基于視覺伺服的編隊控制方法被提出，即利用攝像機獲取視覺圖像信號進(jìn)行實時反饋，從而控制移動機器人到達(dá)指定的目標(biāo)位置，且在編隊控制中還可以減少領(lǐng)導(dǎo)者與跟隨者之間的通信。文獻(xiàn)中

［11］中通過相機選取特征點，實現(xiàn)了移動機器人的視覺跟蹤，但是其特征點與相機高度位置是固定不變的具有局限性。

輪式移動機器人是一個欠驅(qū)動系統(tǒng)，因此對控制器的設(shè)計有更高的要求。相比傳統(tǒng)的線性控制方法，非線性控制在處理不確定性和擾動更有優(yōu)勢。文獻(xiàn)［12］研究了非完整約束移動機器人的跟蹤問題，設(shè)計了滑?？刂破魈岣吡烁櫺阅埽珒H在仿真中進(jìn)行了驗證。

針對以上問題，本文基于領(lǐng)航-跟隨模型，通過檢測AprilTag標(biāo)簽［13～14］獲取移動機器人之間的位姿關(guān)系，并設(shè)計滑模雙冪次趨近律控制器提高移動機器人的編隊控制效果以及執(zhí)行效率。最后在麥克納姆輪全向移動機器人平臺上驗證了所提出方案的有效性和可行性。

2 移動機器人硬件系統(tǒng)與功能簡介

本文實驗設(shè)備是啟智CV移動機器人。移動機器人如圖1所示，使用的是麥克納姆輪底盤結(jié)構(gòu)的全向移動機器人平臺。

圖1 啟智CV移動機器人

四個輪子按照對角一致的原則安裝，可以實現(xiàn)前后，左右以及旋轉(zhuǎn)運動，具有三個自由度。該平臺相較于其他移動機器人平臺具有更高的靈活性。該移動機器人的主要硬件構(gòu)成如下。

1）機器人控制器模塊：具有接口豐富，功能強大，使用簡單便捷等特點?？刂破鞑捎靡夥ò雽?dǎo)體的Cortex-M3處理器作為主處理器，處理器頻率為72MHz，內(nèi)部自帶265K程序存儲器和48K數(shù)據(jù)儲存器，并通過外擴的Flash芯片，具有高達(dá)2MB的用戶程序儲存空間。

2）伺服電機模塊：采用直流電機，電機額定功率為20W。光電編碼器閉環(huán)控制，在有效負(fù)載內(nèi)均能保持勻速運動，可完成比較高精度的機器人運動控制。此外，伺服電機模塊具有完善的電流保護功能，避免過流過熱造成的硬件損壞。

3）姿態(tài)檢測模塊：啟智CV移動機器人搭載了一枚MPU6050慣性測量芯片，用來檢測機器人的姿態(tài)參數(shù)。MPU6050是一款6軸傳感器模塊，包括3軸加速度計，3軸陀螺儀，其體積小巧，性能出色。

4）視覺模塊：啟智CV移動機器人使用的是由奧比中光公司推出的型號為Astra的3D結(jié)構(gòu)光攝像頭，具備高精度、低功耗優(yōu)勢，可助力實現(xiàn)人機交互、三維建模等多種功能，適用于40cm～200cm識別距離的智能終端設(shè)備。

3 AprilTag視覺定位系統(tǒng)

在移動機器人編隊控制研究中，其位置信息和偏航信息是移動機器人編隊控制最為關(guān)鍵的信息，控制器的設(shè)計也是基于這些狀態(tài)信息。

美國密西根大學(xué)Ed Olson團隊提出的一種類似于QR碼的二維信息標(biāo)簽AprilTag，在增強現(xiàn)實，機器人，相機校準(zhǔn)等領(lǐng)域受到廣泛使用。降低二維碼自身的復(fù)雜程度，滿足了應(yīng)用領(lǐng)域中實時性的要求，可以快速地檢測標(biāo)志，并計算出相對的位置。

3.1 AprilTag檢測步驟

1）線段聚類

如圖2（a），根據(jù)梯度檢測出圖像中的各種邊緣。聚類算法是基于圖的算法，圖像上的每一個像素為圖的節(jié)點，圖像上相鄰的兩個像素皆存在一條邊，且邊的權(quán)重為梯度方向之差，根據(jù)增加的邊權(quán)值對這些邊進(jìn)行排序，并根據(jù)權(quán)重進(jìn)行邊的合并。

2）輪廓查找

接下來需要尋找標(biāo)簽的輪廓，連通域查找的方法計算出圖像中黑白的邊緣，且每個連通域都有唯一的ID，防止共用一條邊產(chǎn)生錯誤。

3）尋找四邊形

得到圖像的輪廓后，如圖2（b），需要對每一個輪廓進(jìn)行分割，找到四邊形輪廓，作為可能的標(biāo)簽。首先對無序的輪廓點按照對重心的角度進(jìn)行排序；有了排序的輪廓點，然后就是按照順序進(jìn)行直線擬合；然后取角點間的點擬合直線，求得四條直線的角點作為標(biāo)簽的頂點，如圖2（c）。

4）編碼與解碼

上一步檢測出的四邊形不一定是標(biāo)簽，所以需要進(jìn)行編碼，匹配和解碼。對標(biāo)簽最外層和次外層兩層色塊的灰度均值作為點陣中每個點判別黑白的閾值，大于改制編為1，否則為0，從而得到二進(jìn)制編碼。將得到的編碼與標(biāo)簽家族編碼庫進(jìn)行比對校驗，得到相應(yīng)的標(biāo)簽信息，如圖2（d）。

圖2 AprilTag檢測步驟

3.2 相機標(biāo)定

本文使用的標(biāo)定方法是一種基于平面棋盤格的標(biāo)定方法［15］，于1998年由張正友提出。該方法實用性強，精度高，相機和標(biāo)定板可以任意放置。下面對該方法做簡要介紹。

其中，P=(U,V,W)為世界坐標(biāo)系中的一個點，p=(u,v)為像素坐標(biāo)系下的像素坐標(biāo)，Z為尺度因子。等式左側(cè)從左到右的矩陣分別對應(yīng)：仿射變換、透視投影和剛體變換。

接著將矩陣化簡：

得到的矩陣為相機的內(nèi)參矩陣，其參數(shù)由相機自身所決定。其中?為像距，dX，dY分別表示X，Y方向上的一個像素在相機感光板上的物理長度，u0，v0分別表示相機感光板中心在像素坐標(biāo)系下的坐標(biāo)，θ表示感光板的橫邊和縱邊之間的角度。而矩陣為相機的外參矩陣，R表示旋轉(zhuǎn)矩陣，T表示平移矢量。最后的相機成像模型如下：

使用標(biāo)定板以各個角度出現(xiàn)在相機視野中，相機標(biāo)定結(jié)果如圖3所示。

圖3 相機標(biāo)定結(jié)果

最終計算出相機的內(nèi)參矩陣K以及畸變向量P：

3.3 坐標(biāo)系轉(zhuǎn)換和標(biāo)簽設(shè)置

在機器人編隊過程中，機器人坐標(biāo)系與世界坐標(biāo)系對齊，以X軸為移動機器人前進(jìn)方向。標(biāo)簽坐標(biāo)系，機器人坐標(biāo)系，相機坐標(biāo)系由圖4、圖5所示。

圖4 標(biāo)簽坐標(biāo)和機器人坐標(biāo)的關(guān)系

圖5 相機坐標(biāo)和機器人坐標(biāo)的關(guān)系

相機坐標(biāo)和機器人坐標(biāo)關(guān)系式可以由旋轉(zhuǎn)矩陣表示為

求得旋轉(zhuǎn)矩陣中的值為

則AprilTag標(biāo)簽的參數(shù)設(shè)置為

在原版AprilTag定位系統(tǒng)中最后輸出的是世界坐標(biāo)系到相機坐標(biāo)系的變換，而本文最后要得到的是移動機器人的位置信息是機器人坐標(biāo)系到世界坐標(biāo)系的變換，則根據(jù)圖4和圖5坐標(biāo)系的關(guān)系設(shè)計公式中的旋轉(zhuǎn)矩陣R和平移矢量T。

4 移動機器人編隊控制算法

4.1 傳統(tǒng)領(lǐng)航-跟隨法

領(lǐng)航-跟隨法如圖6，其主要有兩種編隊模型：l-φ模型和l-l模型。通過檢測AprilTag標(biāo)簽，可以獲取到X軸和Y軸的相對位置以及姿態(tài)關(guān)系，所以本文采用l-φ模式，其原理是控制跟隨者與領(lǐng)航者保持一定的距離和角度，進(jìn)而實現(xiàn)編隊控制。

圖6 傳統(tǒng)領(lǐng)航-跟隨法

4.2 虛擬結(jié)構(gòu)領(lǐng)-航跟隨法

本文在原有的基于l-φ編隊模式的基礎(chǔ)上，采用虛擬結(jié)構(gòu)的領(lǐng)航-跟隨法，即預(yù)先設(shè)計好這個編隊的剛性結(jié)構(gòu)，跟隨機器人跟蹤預(yù)先設(shè)計好的虛擬機器人，從而實現(xiàn)編隊控制。

圖7 虛擬結(jié)構(gòu)的領(lǐng)航-跟隨編隊模型

對該表達(dá)式求導(dǎo)可得到運動學(xué)誤差模型：

其中，v，w為跟蹤移動機器人的移動速度和旋轉(zhuǎn)速度，在運動學(xué)誤差模型中它們是控制量，vr和wr為虛擬移動機器人的移動速度和旋轉(zhuǎn)速度。跟隨機器人只需要到達(dá)虛擬跟隨機器人的位置，即可完成期望隊形的編隊任務(wù)。

4.3 編隊控制器的設(shè)計

對于輪式移動機器人這樣的欠驅(qū)動非線性系統(tǒng)，滑膜控制是一種非常有效的控制方法。該方法需要調(diào)節(jié)的參數(shù)少且對擾動有很強的抑制能力。其設(shè)計步驟可分為兩步：

1）設(shè)計滑模面?；Ｃ媸窍到y(tǒng)的線性或非線性函數(shù)，系統(tǒng)的狀態(tài)將沿著滑模面趨近于0，到達(dá)滑模面以后，便處于滑動模態(tài)，其滑模運動完全由切換面方程s(x)=0決定。

2）設(shè)計趨近律。為了保證系狀態(tài)最終趨于零，需要設(shè)計趨近律。本文采用的是雙冪次趨近律。

根據(jù)虛擬結(jié)構(gòu)的領(lǐng)航-跟隨模型開始設(shè)控制器，首先設(shè)計滑膜切換面：

對其求導(dǎo)可得：

然后趨近律為雙冪次趨近律：

其中，a＞0，b＞0，0＜α＜1，β＞1。當(dāng)系統(tǒng)狀態(tài)趨近滑膜面時，-a|s|αsign(s)項起主導(dǎo)作用，當(dāng)系統(tǒng)狀態(tài)遠(yuǎn)離滑膜面時，-b|s|βsign(s)項起主導(dǎo)作用。

將運動學(xué)誤差模型代入，可得

最后可得滑膜控制律為

在控制律v和ω的作用下，可以使得誤差系統(tǒng)漸近穩(wěn)定。證明過程參考文獻(xiàn)［16］。

5 實驗驗證與分析

設(shè)計實驗為領(lǐng)航機器人以vr=0.1m/s，ωr=0.04 rad/s的速度運行。跟蹤機器人和領(lǐng)航機器人保持如下隊形編隊移動。

移動機器人的初始位置如圖8所示。

圖8 領(lǐng)航跟隨機器人初始位置

在初始位置，攝像頭檢測AprilTag的結(jié)果如圖9所示。

圖9 初始位置AprilTag檢測結(jié)果

此時可以獲得位置關(guān)系和姿態(tài)關(guān)系，姿態(tài)關(guān)系以四元數(shù)的方式顯示，需要將其轉(zhuǎn)換為角度，其偏航角誤差為

則初始位置的誤差為

選取參數(shù)k1=1，k2=1，k11=0.2，k12=0.22，k21=0.1，k22=0.2，α1=0.5，β1=1，α2=0.5，β2=2。

同時設(shè)計線性控制作為對照實驗：

其中選取參數(shù)Kv=0.22，Kω=0.2。實驗結(jié)果如圖10～圖14所示。

圖10 xe誤差曲線

圖11 ye誤差曲線

圖12 θe誤差曲線

圖13 滑模雙冪次控制律控制量v

圖14 滑模雙冪次控制律控制量ω

從圖10～圖14可以發(fā)現(xiàn)采用滑模雙冪次控制律的xe，ye，θe的收斂速度明顯快于線性控制，且都能較快收斂到零，說明本文設(shè)計的控制律具有良好的性能。從圖11看出，θe和收斂到零但是存在一定程度的抖動，主要是因為攝像機獲取的數(shù)據(jù)存在跳動。

6 結(jié)語

本文研究了基于視覺伺服的多移動機器人編隊控制問題。通過移動機器人的攝像機檢測AprilTag標(biāo)簽獲取位姿信息進(jìn)行實時反饋，從而控制移動機器人到達(dá)指定的目標(biāo)位置，減少多移動機器人之間的相互通信。為了保證編隊控制的效率，和抗干擾能力，設(shè)計了滑模雙冪次趨近律控制器，實驗結(jié)果表明了控制器的有效性和整體方案的可行性。