基于視覺伺服的移動(dòng)機(jī)器人編隊(duì)控制研究與設(shè)計(jì)*

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

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

1 引言

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

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

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

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

輪式移動(dòng)機(jī)器人是一個(gè)欠驅(qū)動(dòng)系統(tǒng)，因此對(duì)控制器的設(shè)計(jì)有更高的要求。相比傳統(tǒng)的線性控制方法，非線性控制在處理不確定性和擾動(dòng)更有優(yōu)勢(shì)。文獻(xiàn)［12］研究了非完整約束移動(dòng)機(jī)器人的跟蹤問題，設(shè)計(jì)了滑模控制器提高了跟蹤性能，但僅在仿真中進(jìn)行了驗(yàn)證。

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

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

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

圖1 啟智CV移動(dòng)機(jī)器人

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

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

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

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

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

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

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

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

3.1 AprilTag檢測(cè)步驟

1）線段聚類

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

2）輪廓查找

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

3）尋找四邊形

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

4）編碼與解碼

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

圖2 AprilTag檢測(cè)步驟

3.2 相機(jī)標(biāo)定

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

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

接著將矩陣化簡：

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

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

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

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

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

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

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

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

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

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

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

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

4 移動(dòng)機(jī)器人編隊(duì)控制算法

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

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

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

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

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

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

對(duì)該表達(dá)式求導(dǎo)可得到運(yùn)動(dòng)學(xué)誤差模型：

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

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

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

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

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

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

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

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

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

將運(yùn)動(dòng)學(xué)誤差模型代入，可得

最后可得滑膜控制律為

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

5 實(shí)驗(yàn)驗(yàn)證與分析

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

移動(dòng)機(jī)器人的初始位置如圖8所示。

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

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

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

此時(shí)可以獲得位置關(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è)計(jì)線性控制作為對(duì)照實(shí)驗(yàn)：

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

圖10 xe誤差曲線

圖11 ye誤差曲線

圖12 θe誤差曲線

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

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

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

6 結(jié)語

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