基于雙目視覺的繩驅動飛行機械臂目標識別與抓取

譚榮凱,楊浩秦,王堯堯

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

0 引言

目前無人機和多自由度機械臂所組成的空中操作系統由于其特有的優勢,受到越來越廣泛的關注。它們可以主動地和周圍環境進行交互,在一些特殊的場景代替人類進行工作,有著非常廣闊的應用前景。為了進一步提高飛行機械臂對周圍環境的感知能力,對空中機械臂系統增加了視覺反饋系統,輔助機械臂進行作業。

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

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

目前許多姿態估計算法采用深度學習的方式或者采用特征點的方式,計算量較大,對無人機機載計算機性能要求較高,且實時性難以保證。本文通過顏色特征結合目標的幾何特征對目標進行追蹤和姿態估計,采用的目標特征較少且計算方便,適合無人機等實時性要求較高的平臺。

本文做的工作主要如下:

1)提出了一種輕量化的目標姿態估計算法,通過改進Camshift算法,結合目標幾何特征對目標進行姿態估計,可用于實時性要求較高的飛行作業平臺;

2)對機械臂和相機進行了系統建模,求解各坐標系之間的轉換關系,通過逆運動學運算求解已知目標姿態時各關節的抓取角度。

1 系統整體建模

1.1 繩驅動機械臂建模

機械臂采用繩索驅動的形式。繩驅動機械臂將電機和減速器安裝在機械臂的底座處,通過繩索傳遞運動與力,而傳統機械臂將電機和減速器安裝在機械臂的關節處,在臂的運動過程中,機械臂的質量會導致飛行器本身的質心發生遷移,降低飛行器的控制精度和作業質量,而繩驅動機械臂將電機安裝在底座位置可以降低機械臂慣性對飛行器控制的不利影響。

使用D-H參數法對繩驅動機械臂進行建模,建立繩驅動連桿坐標系如圖1所示。設關節i繞zi軸正方向的轉動角度為θi,繞xi軸正方向的轉動角度為αi-1,連桿沿zi軸正方向的移動距離為di,連桿長度為Ai。

圖1 繩驅動機械臂坐標系

由于機械臂只在平面內進行運動,所以3個關節繞xi軸正方向的轉動角度αi-1以及連桿沿zi軸正方向的移動距離di均為0,各連桿長度為: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)。

通過立體視覺可以獲得目標在空間的位姿,將位姿作為機械臂末端執行器的期望位姿,通過運動學逆解,求解機械臂各關節的角度,再通過控制機械臂關節運動,即可實現抓取任務。可用末端執行器在底部坐標系x,y方向上的坐標以及相當于z軸的旋轉來描述其在世界坐標系下的位姿,其變換矩陣如下式:

(2)

式中:c為cos;s為sin。

聯立式(1)和式(2),通過代數法進行逆運動學求解,求解過程如下所示。

(3)

通過對機械臂關節角進行分析以及對上式的聯立可得下式,其中X、Y為中間變量。

(4)

最終可得:

(5)

由于機械臂采用繩驅動的方式,使得機械臂關節在運動過程中會對其他關節產生影響,即關節之間存在耦合效應,所以關節期望的轉角θa與實際電機的轉角θm并不一致,兩者之間存在一個系數I,即θm=Iθa,限于篇幅,在此直接給出I如下所示。

(6)

1.2 雙目相機建模

雙目相機相對于單目相機的優勢是可以獲取物體的深度信息,雙目相機的左右兩個攝像頭可以同時采集信息,通過比較物體在兩幅圖像中的位置差異,可以獲取物體的深度信息。本文所用的雙目相機基線長度可變,通過調節基線長度可以實現探測深度的改變,實驗中相機采用的基線長度為3cm,可探測深度范圍為15cm～80cm之間,滿足機械臂抓取要求。

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

圖2 坐標系之間的關系

像素坐標系與相機坐標系之間的轉換關系如下:

(7)

式中K稱為相機的內參數矩陣,通過標定獲得。

相機坐標系與世界坐標系之間的轉換關系如下:

(8)

(9)

2 目標追蹤與姿態識別

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

圖3 Camshift算法

meanshift向量通過搜索框搜索概率密度最大區域來確定目標對象,但其固有的缺陷,算法的收斂速度與選取的窗口有關,而且窗口一旦選定,窗口的大小不能改變。當目標在視野中發生變化,比如變大或變小時,窗口不能自適應的去調整大小。

為了能讓搜索窗口的大小隨著目標的變化自適應調整大小,本文利用目標的顏色直方圖模型將圖像轉變為顏色概率分布圖,同meanshift向量一樣,初始化一個搜索窗口的位置和大小,根據搜索窗口得到的結果自適應調整下一幀搜索窗口的位置和大小,最終得到圖片中顏色概率密度分布最大的區域。

想要將圖像轉變為顏色概率分布圖,需要對圖像進行反向投影。首先將圖像從RGB顏色空間轉換到HSV空間,可以降低光照的影響,對其中的H分量做直方圖,在直方圖中包含了不同的H分量值出現的概率,然后將圖像中每個像素的值用其顏色出現的概率替換,就得到了顏色概率分布圖。

RGB到HSV空間的轉換關系如下[7]:

(10)

(11)

(12)

(13)

假設以綠色作為目標顏色,Camshift算法利用自適應的搜索框對感興趣目標顏色區域(ROI)進行實時檢測并返回ROI的相關信息,如ROI的外接矩形的4個頂點位置以及其中心位置。如此可以利用Camshift完成目標追蹤的功能,并利用檢測目標區域的相關信息進行姿態估計。算法返回的ROI外接矩形框的相關信息如圖4所示。

圖4 目標追蹤與姿態估計

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

(14)

由此可求得PWi,[XWiYWiZWi] 為PWi的坐標。故圓柱體的半徑r等價于中心點到任意點間的距離:

r=|PW0PW3=

(15)

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

n=n1×n2

(16)

3 實驗結果

實驗所用的機械臂為本課題組自行設計的繩驅動機械臂,用STM32F103單片機進行控制,相機采用免驅動可變基線雙目相機,機載計算機為NVIDIA JESTON NX,目標為圓柱形物體。主要控制流程為相機檢測到目標物體后,通過機載計算機實時計算目標的大小與姿態信息,通過逆運動學解算后得到機械臂各關節的抓取角度,再將此抓取角度信息傳遞給單片機,單片機控制機械臂完成抓取過程,實驗平臺如圖5所示。

圖5 實驗平臺

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

圖6 圓柱體半徑估計

圖7所示為繩驅動飛行機械臂的整個抓取過程。理想狀態下,無人機從預定位置起飛,在預定的軌跡上行進,當雙目相機檢測到目標后通過Camshift算法對目標進行實時追蹤并引導無人機靠近目標。當目標處于機械臂的工作空間后,通過機載計算機實時計算目標在三維空間中的姿態信息,然后通過逆運動學解算出機械臂各關節期望運動角度,機載計算機將數據發送到單片機中,然后單片機控制機械臂抓取,抓取完成后無人機退回到預定位置。

圖7 繩驅動飛行機械臂抓取過程

4 結語

1)針對飛行機械臂實時性要求較高的情況,提出了一種輕量化的目標姿態估計算法。

2)建立了雙目相機模型和繩驅動機械臂運動學模型,得到了目標在相機坐標系與世界坐標系的轉換關系,通過逆運動學求解了在已知目標位姿情況下繩驅動機械臂的抓取角度。

3)進行了目標抓取實驗,驗證了姿態估計算法的可行性。