譚榮凱,楊浩秦,王堯堯

(南京航空航天大學(xué) a. 機(jī)電學(xué)院; b. 材料科學(xué)與技術(shù)學(xué)院,江蘇 南京 210016)

0 引言

目前無人機(jī)和多自由度機(jī)械臂所組成的空中操作系統(tǒng)由于其特有的優(yōu)勢(shì),受到越來越廣泛的關(guān)注。它們可以主動(dòng)地和周圍環(huán)境進(jìn)行交互,在一些特殊的場(chǎng)景代替人類進(jìn)行工作,有著非常廣闊的應(yīng)用前景。為了進(jìn)一步提高飛行機(jī)械臂對(duì)周圍環(huán)境的感知能力,對(duì)空中機(jī)械臂系統(tǒng)增加了視覺反饋系統(tǒng),輔助機(jī)械臂進(jìn)行作業(yè)。

空中機(jī)械臂的視覺系統(tǒng)采用的相機(jī)主要有3類:單目相機(jī)、雙目相機(jī)和深度相機(jī)。單目相機(jī)體積小、價(jià)格低,但是其無法獲取目標(biāo)的深度信息;深度相機(jī)通過紅外可直接獲取目標(biāo)的深度信息,但是其體積大,價(jià)格高,不適合用于無人機(jī)的實(shí)時(shí)監(jiān)測(cè);雙目相機(jī)通過左右圖像的視差來計(jì)算目標(biāo)的深度信息,其體積和價(jià)格介于單目相機(jī)和深度相機(jī)之間,可以滿足無人機(jī)實(shí)時(shí)監(jiān)測(cè)的要求。在機(jī)械臂工作過程中,視覺系統(tǒng)承擔(dān)的主要工作為目標(biāo)追蹤與姿態(tài)估計(jì)。目標(biāo)追蹤算法經(jīng)過多年的研究,已經(jīng)有了長(zhǎng)足的發(fā)展,傳統(tǒng)的算法比如均值漂移算法、基于Kalman濾波的目標(biāo)追蹤等以及目前時(shí)興的深度學(xué)習(xí)算法。深度學(xué)習(xí)可以識(shí)別許多種類的目標(biāo),精度較高,但是深度學(xué)習(xí)依靠大量的計(jì)算資源,且需要大量的樣本進(jìn)行訓(xùn)練,而傳統(tǒng)的目標(biāo)追蹤算法所需資源較少,適用于無人機(jī)的實(shí)時(shí)追蹤任務(wù)。

近些年來,有很多學(xué)者在無人機(jī)的視覺領(lǐng)域作了很多工作。韓國(guó)首爾大學(xué)的 SEO等[1]基于單目相機(jī)開發(fā)了一種基于圖像的圓柱體檢測(cè)算法,該算法利用3D空間中透視投影圓的幾何特征,實(shí)現(xiàn)了圓柱體的抓取。賓夕法尼亞大學(xué)的THOMAS等[2]從快速移動(dòng)的猛禽獲得靈感,基于單目相機(jī)在圖像空間中研究了無人機(jī)的動(dòng)力學(xué)模型,在視覺反饋的基礎(chǔ)上生成運(yùn)動(dòng)軌跡,為無人機(jī)的抓取作了鋪墊。日本立命館大學(xué)的LEEWIWATWONG等[3]研究了一種新的圖像區(qū)域分離方法,通過霍夫變換來實(shí)現(xiàn)對(duì)棒狀物體的識(shí)別,從而控制空中機(jī)械臂系統(tǒng)自動(dòng)與棒狀物體對(duì)齊。塞爾維亞大學(xué)的RAMON-SORIA等[4]使用深度相機(jī),利用人工神經(jīng)網(wǎng)絡(luò)(ANN)來檢測(cè)目標(biāo),然后通過對(duì)齊算法來獲取目標(biāo)的姿態(tài)。東南大學(xué)王營(yíng)華等[5]在目標(biāo)上貼ArUco標(biāo)簽,在標(biāo)簽大小已知的情況下,利用n點(diǎn)透視算法求解攝像頭位姿,進(jìn)而利用攝像頭位姿對(duì)空中機(jī)械臂系統(tǒng)進(jìn)行分級(jí)控制,實(shí)現(xiàn)對(duì)目標(biāo)的抓取。哈爾濱工業(yè)大學(xué)LUO等[6]利用ORB特征獲取目標(biāo)的特征點(diǎn),在已知目標(biāo)物真實(shí)大小的情況下,求解目標(biāo)的姿態(tài)信息。

目前許多姿態(tài)估計(jì)算法采用深度學(xué)習(xí)的方式或者采用特征點(diǎn)的方式,計(jì)算量較大,對(duì)無人機(jī)機(jī)載計(jì)算機(jī)性能要求較高,且實(shí)時(shí)性難以保證。本文通過顏色特征結(jié)合目標(biāo)的幾何特征對(duì)目標(biāo)進(jìn)行追蹤和姿態(tài)估計(jì),采用的目標(biāo)特征較少且計(jì)算方便,適合無人機(jī)等實(shí)時(shí)性要求較高的平臺(tái)。

本文做的工作主要如下:

1)提出了一種輕量化的目標(biāo)姿態(tài)估計(jì)算法,通過改進(jìn)Camshift算法,結(jié)合目標(biāo)幾何特征對(duì)目標(biāo)進(jìn)行姿態(tài)估計(jì),可用于實(shí)時(shí)性要求較高的飛行作業(yè)平臺(tái);

2)對(duì)機(jī)械臂和相機(jī)進(jìn)行了系統(tǒng)建模,求解各坐標(biāo)系之間的轉(zhuǎn)換關(guān)系,通過逆運(yùn)動(dòng)學(xué)運(yùn)算求解已知目標(biāo)姿態(tài)時(shí)各關(guān)節(jié)的抓取角度。

1 系統(tǒng)整體建模

1.1 繩驅(qū)動(dòng)機(jī)械臂建模

機(jī)械臂采用繩索驅(qū)動(dòng)的形式。繩驅(qū)動(dòng)機(jī)械臂將電機(jī)和減速器安裝在機(jī)械臂的底座處,通過繩索傳遞運(yùn)動(dòng)與力,而傳統(tǒng)機(jī)械臂將電機(jī)和減速器安裝在機(jī)械臂的關(guān)節(jié)處,在臂的運(yùn)動(dòng)過程中,機(jī)械臂的質(zhì)量會(huì)導(dǎo)致飛行器本身的質(zhì)心發(fā)生遷移,降低飛行器的控制精度和作業(yè)質(zhì)量,而繩驅(qū)動(dòng)機(jī)械臂將電機(jī)安裝在底座位置可以降低機(jī)械臂慣性對(duì)飛行器控制的不利影響。

使用D-H參數(shù)法對(duì)繩驅(qū)動(dòng)機(jī)械臂進(jìn)行建模,建立繩驅(qū)動(dòng)連桿坐標(biāo)系如圖1所示。設(shè)關(guān)節(jié)i繞zi軸正方向的轉(zhuǎn)動(dòng)角度為θi,繞xi軸正方向的轉(zhuǎn)動(dòng)角度為αi-1,連桿沿zi軸正方向的移動(dòng)距離為di,連桿長(zhǎng)度為Ai。

圖1 繩驅(qū)動(dòng)機(jī)械臂坐標(biāo)系

由于機(jī)械臂只在平面內(nèi)進(jìn)行運(yùn)動(dòng),所以3個(gè)關(guān)節(jié)繞xi軸正方向的轉(zhuǎn)動(dòng)角度αi-1以及連桿沿zi軸正方向的移動(dòng)距離di均為0,各連桿長(zhǎng)度為:A0=10cm,A1=35cm,A2=35cm,AT=25cm。

(1)

式中:c1=cosθ1;c12=cos(θ1+θ2);c123=cos(θ1+θ2+θ3);s1=sinθ1;s12=sin(θ1+θ2);s123=sin(θ1+θ2+θ3)。

通過立體視覺可以獲得目標(biāo)在空間的位姿,將位姿作為機(jī)械臂末端執(zhí)行器的期望位姿,通過運(yùn)動(dòng)學(xué)逆解,求解機(jī)械臂各關(guān)節(jié)的角度,再通過控制機(jī)械臂關(guān)節(jié)運(yùn)動(dòng),即可實(shí)現(xiàn)抓取任務(wù)?？捎媚┒藞?zhí)行器在底部坐標(biāo)系x,y方向上的坐標(biāo)以及相當(dāng)于z軸的旋轉(zhuǎn)來描述其在世界坐標(biāo)系下的位姿,其變換矩陣如下式:

(2)

式中:c為cos;s為sin。

聯(lián)立式(1)和式(2),通過代數(shù)法進(jìn)行逆運(yùn)動(dòng)學(xué)求解,求解過程如下所示。

(3)

通過對(duì)機(jī)械臂關(guān)節(jié)角進(jìn)行分析以及對(duì)上式的聯(lián)立可得下式,其中X、Y為中間變量。

(4)

最終可得:

(5)

由于機(jī)械臂采用繩驅(qū)動(dòng)的方式,使得機(jī)械臂關(guān)節(jié)在運(yùn)動(dòng)過程中會(huì)對(duì)其他關(guān)節(jié)產(chǎn)生影響,即關(guān)節(jié)之間存在耦合效應(yīng),所以關(guān)節(jié)期望的轉(zhuǎn)角θa與實(shí)際電機(jī)的轉(zhuǎn)角θm并不一致,兩者之間存在一個(gè)系數(shù)I,即θm=Iθa,限于篇幅,在此直接給出I如下所示。

(6)

1.2 雙目相機(jī)建模

雙目相機(jī)相對(duì)于單目相機(jī)的優(yōu)勢(shì)是可以獲取物體的深度信息,雙目相機(jī)的左右兩個(gè)攝像頭可以同時(shí)采集信息,通過比較物體在兩幅圖像中的位置差異,可以獲取物體的深度信息。本文所用的雙目相機(jī)基線長(zhǎng)度可變,通過調(diào)節(jié)基線長(zhǎng)度可以實(shí)現(xiàn)探測(cè)深度的改變,實(shí)驗(yàn)中相機(jī)采用的基線長(zhǎng)度為3cm,可探測(cè)深度范圍為15cm～80cm之間,滿足機(jī)械臂抓取要求。

圖2所示為成像過程中涉及的4個(gè)坐標(biāo)系,分別為世界坐標(biāo)系{OW}、相機(jī)坐標(biāo)系{OC}、左右成像坐標(biāo)系{OL}、{OR}和位于成像坐標(biāo)系左上角的像素坐標(biāo)系,通過各個(gè)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系,可以獲取二維平面上的物體在三維空間中的位置關(guān)系。

圖2 坐標(biāo)系之間的關(guān)系

像素坐標(biāo)系與相機(jī)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系如下:

(7)

式中K稱為相機(jī)的內(nèi)參數(shù)矩陣,通過標(biāo)定獲得。

相機(jī)坐標(biāo)系與世界坐標(biāo)系之間的轉(zhuǎn)換關(guān)系如下:

(8)

(9)

2 目標(biāo)追蹤與姿態(tài)識(shí)別

Camshift算法的主要流程如下:給定d維空間R的n個(gè)樣本點(diǎn)x= [xi] ,i= 1,…,n,選定其中某一點(diǎn)xi,做一個(gè)半徑為r的圓,其中有k個(gè)樣本點(diǎn)落入圓中,記這k個(gè)點(diǎn)的集合為{Sk},落在圓內(nèi)的所有點(diǎn)都與xi構(gòu)成一個(gè)向量,將這些向量相加,所得的結(jié)果稱為meanshift向量。如圖3(a)所示,P1即為選定的點(diǎn),P1指向P2的向量即這個(gè)時(shí)刻meanshift向量,然后P2作為新的選定點(diǎn)繼續(xù)做圓,如圖3(b)所示。最終meanshift向量可以收斂到概率密度最大的地方,如圖3(c)所示。

圖3 Camshift算法

meanshift向量通過搜索框搜索概率密度最大區(qū)域來確定目標(biāo)對(duì)象,但其固有的缺陷,算法的收斂速度與選取的窗口有關(guān),而且窗口一旦選定,窗口的大小不能改變。當(dāng)目標(biāo)在視野中發(fā)生變化,比如變大或變小時(shí),窗口不能自適應(yīng)的去調(diào)整大小。

為了能讓搜索窗口的大小隨著目標(biāo)的變化自適應(yīng)調(diào)整大小,本文利用目標(biāo)的顏色直方圖模型將圖像轉(zhuǎn)變?yōu)轭伾怕史植紙D,同meanshift向量一樣,初始化一個(gè)搜索窗口的位置和大小,根據(jù)搜索窗口得到的結(jié)果自適應(yīng)調(diào)整下一幀搜索窗口的位置和大小,最終得到圖片中顏色概率密度分布最大的區(qū)域。

想要將圖像轉(zhuǎn)變?yōu)轭伾怕史植紙D,需要對(duì)圖像進(jìn)行反向投影。首先將圖像從RGB顏色空間轉(zhuǎn)換到HSV空間,可以降低光照的影響,對(duì)其中的H分量做直方圖,在直方圖中包含了不同的H分量值出現(xiàn)的概率,然后將圖像中每個(gè)像素的值用其顏色出現(xiàn)的概率替換,就得到了顏色概率分布圖。

RGB到HSV空間的轉(zhuǎn)換關(guān)系如下[7]:

(10)

(11)

(12)

(13)

假設(shè)以綠色作為目標(biāo)顏色,Camshift算法利用自適應(yīng)的搜索框?qū)Ω信d趣目標(biāo)顏色區(qū)域(ROI)進(jìn)行實(shí)時(shí)檢測(cè)并返回ROI的相關(guān)信息,如ROI的外接矩形的4個(gè)頂點(diǎn)位置以及其中心位置。如此可以利用Camshift完成目標(biāo)追蹤的功能,并利用檢測(cè)目標(biāo)區(qū)域的相關(guān)信息進(jìn)行姿態(tài)估計(jì)。算法返回的ROI外接矩形框的相關(guān)信息如圖4所示。

圖4 目標(biāo)追蹤與姿態(tài)估計(jì)

故而可以得到P0—P44個(gè)點(diǎn)在二維圖像中的位置信息。設(shè)pi(i=0～4)代表P0—P4在像素坐標(biāo)系中的坐標(biāo),PWi(i=0～4)為世界坐標(biāo)系中的坐標(biāo)。通過式(7)和式(8)可得:

(14)

由此可求得PWi,[XWiYWiZWi] 為PWi的坐標(biāo)。故圓柱體的半徑r等價(jià)于中心點(diǎn)到任意點(diǎn)間的距離:

r=|PW0PW3=

(15)

由于圓柱體的幾何特性,在求解圓柱體姿態(tài)時(shí)只考慮圓柱體的法向量。從P1—P4中任取兩點(diǎn),例如P1、P2,設(shè)P0指向P1的向量為n1,P0指向P2的向量為n2,則圓柱體的法向量n可表示為

n=n1×n2

(16)

3 實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)所用的機(jī)械臂為本課題組自行設(shè)計(jì)的繩驅(qū)動(dòng)機(jī)械臂,用STM32F103單片機(jī)進(jìn)行控制,相機(jī)采用免驅(qū)動(dòng)可變基線雙目相機(jī),機(jī)載計(jì)算機(jī)為NVIDIA JESTON NX,目標(biāo)為圓柱形物體。主要控制流程為相機(jī)檢測(cè)到目標(biāo)物體后,通過機(jī)載計(jì)算機(jī)實(shí)時(shí)計(jì)算目標(biāo)的大小與姿態(tài)信息,通過逆運(yùn)動(dòng)學(xué)解算后得到機(jī)械臂各關(guān)節(jié)的抓取角度,再將此抓取角度信息傳遞給單片機(jī),單片機(jī)控制機(jī)械臂完成抓取過程,實(shí)驗(yàn)平臺(tái)如圖5所示。

圖5 實(shí)驗(yàn)平臺(tái)

圖6所示為機(jī)載計(jì)算機(jī)通過算法估計(jì)的圓柱體目標(biāo)的半徑值,直線為圓柱體半徑真實(shí)值為27.6mm,曲線為視覺算法估計(jì)的半徑值,誤差為[-0.21,+0.25]mm 。

圖6 圓柱體半徑估計(jì)

圖7所示為繩驅(qū)動(dòng)飛行機(jī)械臂的整個(gè)抓取過程。理想狀態(tài)下,無人機(jī)從預(yù)定位置起飛,在預(yù)定的軌跡上行進(jìn),當(dāng)雙目相機(jī)檢測(cè)到目標(biāo)后通過Camshift算法對(duì)目標(biāo)進(jìn)行實(shí)時(shí)追蹤并引導(dǎo)無人機(jī)靠近目標(biāo)。當(dāng)目標(biāo)處于機(jī)械臂的工作空間后,通過機(jī)載計(jì)算機(jī)實(shí)時(shí)計(jì)算目標(biāo)在三維空間中的姿態(tài)信息,然后通過逆運(yùn)動(dòng)學(xué)解算出機(jī)械臂各關(guān)節(jié)期望運(yùn)動(dòng)角度,機(jī)載計(jì)算機(jī)將數(shù)據(jù)發(fā)送到單片機(jī)中,然后單片機(jī)控制機(jī)械臂抓取,抓取完成后無人機(jī)退回到預(yù)定位置。

圖7 繩驅(qū)動(dòng)飛行機(jī)械臂抓取過程

4 結(jié)語

1)針對(duì)飛行機(jī)械臂實(shí)時(shí)性要求較高的情況,提出了一種輕量化的目標(biāo)姿態(tài)估計(jì)算法。

2)建立了雙目相機(jī)模型和繩驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)學(xué)模型,得到了目標(biāo)在相機(jī)坐標(biāo)系與世界坐標(biāo)系的轉(zhuǎn)換關(guān)系,通過逆運(yùn)動(dòng)學(xué)求解了在已知目標(biāo)位姿情況下繩驅(qū)動(dòng)機(jī)械臂的抓取角度。

3)進(jìn)行了目標(biāo)抓取實(shí)驗(yàn),驗(yàn)證了姿態(tài)估計(jì)算法的可行性。