什么是相機標(biāo)定？視覺機械臂自主抓取全流程

第第頁什么是相機標(biāo)定？視覺機械臂自主抓取全流程簡介

視覺（機械）臂是（智能）（機器人）的一個重要分支，它主要包括（控制芯片）、（驅(qū)動電路）、機械臂、相機等部分。

自主抓取是指，在沒有人為干預(yù)的情況下，視覺機械臂系統(tǒng)通過攝像頭獲取到目標(biāo)物體的位置，并且通過驅(qū)動機械臂來完成對于目標(biāo)物體的抓取任務(wù)。

整個抓取過程大致分為以下幾步（含圖中九步）：

相機標(biāo)定——→相機和機械臂進(jìn)行手眼標(biāo)定

——→①讀取攝像頭信息

——→②識別出目標(biāo)物體并得出位姿

——→③物體在相機中的二維坐標(biāo)轉(zhuǎn)化成相對于相機光心的三維坐標(biāo)

——→④再轉(zhuǎn)換成相對于機械臂底座的世界坐標(biāo)

——→⑤計算機械臂末端執(zhí)行器抓取物體時的位姿

——→⑥通過已知的目標(biāo)位置和末端執(zhí)行器的位姿信息來計算機械臂上每個（電機）應(yīng)轉(zhuǎn)動的角度

——→⑦根據(jù)有無障礙物、路徑距離要求、時間要求規(guī)劃出合適的路徑

——→⑧驅(qū)動機械臂各個關(guān)節(jié)一步一步到達(dá)合適的位置

——→⑨末端執(zhí)行器抓取物體——→投放到指定位置

下面我將根據(jù)以上這些步驟，對視覺機械臂自主抓取全流程做出更加詳細(xì)的介紹。

相機標(biāo)定

什么是相機標(biāo)定？為什么要進(jìn)行相機標(biāo)定呢？

由于相機采用小孔成像的原理來采集圖像，為了使物體成像更加清晰，就采用了透鏡的方式。但是，因為透鏡本身在成像的過程中會不可避免的引入徑向畸變和切向畸變（比如一條直線成像后會顯示成一條弧線）。

為了使得現(xiàn)實中的圖像能夠真實的呈現(xiàn)在圖片當(dāng)中，有利于之后機械臂的抓取，則需要對相機進(jìn)行畸變校正。

一般情況下，只要是對于成像精度較高的場景都需要進(jìn)行標(biāo)定，比如三維結(jié)構(gòu)光成像、人臉識別、視覺測量等。

現(xiàn)在對于相機標(biāo)定最常用的方法是張正友相機標(biāo)定法，這種方法是利用棋盤格標(biāo)定板每個角點像素坐標(biāo)的相對關(guān)系以及在已知真實坐標(biāo)相對關(guān)系的條件下計算出來的。

在OpenCv當(dāng)中已經(jīng)有了對應(yīng)的庫，在使用前只需設(shè)置一下每個格子的大小以及格子的行列數(shù)（減去一）即可進(jìn)行標(biāo)定。相機標(biāo)定的合格指標(biāo)一共有四個，分別是x軸方向、y軸方向、遠(yuǎn)近以及旋轉(zhuǎn)角度。

捕捉到多張有效圖片之后，進(jìn)度條會變成綠色，點擊CALIBRATE可直接計算出關(guān)系矩陣。以下就是我采用奧比中光AstroPro設(shè)備得到的標(biāo)定結(jié)果。

我們采用逆向計算的方法通過物體的像素坐標(biāo)獲取到圖像的真實空間位置（世界坐標(biāo)），在下圖的推到過程中會涉及到以下四個坐標(biāo)系：

OW-XWYWZW：世界坐標(biāo)系，描述相機位置，單位m

Oc-XcYcZc：相機坐標(biāo)系，光心為原點，單位m

xy：圖像坐標(biāo)系，光心為圖像中點，單位mm

uv：像素坐標(biāo)系，原點為圖像左上角，單位（pi）xel

P：世界坐標(biāo)系中的一點，現(xiàn)實空間中的點

p：點P在圖像中的成像點，在圖像坐標(biāo)系中的坐標(biāo)為(x,y)，在像素坐標(biāo)系中的坐標(biāo)為(u,v)

f：相機焦距，等于o與Oc的距離，f=||o-Oc||

世界坐標(biāo)系是為了更好的描述相機的空間位置，原點（OW）的選擇在雙目深度相機當(dāng)中一般會是其中一個相機的中心或者兩個相機的中點。

在視覺機械臂的標(biāo)定中，原點（OW）一般被確定為機械臂底座的中心。

世界坐標(biāo)和相機坐標(biāo)的變換過程屬于剛體變換：即物體不會發(fā)生形變，只是進(jìn)行了平移和旋轉(zhuǎn)的變換。其中R表示旋轉(zhuǎn)矩陣，T表示偏移向量（R、T被稱為外參）。

圍繞著不同的坐標(biāo)軸，旋轉(zhuǎn)不同的角度都是有對應(yīng)矩陣的，如下圖所示：

從相機坐標(biāo)系到圖像坐標(biāo)系的變換，是從3D向2D的轉(zhuǎn)換，主要思想就是相似三角形原理，對應(yīng)原理圖如下：

圖中P點就是現(xiàn)實世界的坐標(biāo)，p點就是在相機中的成像坐標(biāo)，f就是透鏡的焦距。

圖像坐標(biāo)與像素坐標(biāo)的轉(zhuǎn)換關(guān)系如下：

圖像坐標(biāo)系的原點是相機光軸和成像平面的交點，單位是mm，屬于物理單位。像素坐標(biāo)系的原點是成像平面的左上角，單位是pixel。

將以上這幾個公式進(jìn)行整理，結(jié)果如下：

相機的內(nèi)參和外參已在圖中標(biāo)明，由于未知數(shù)有很多個，所以需要多組數(shù)據(jù)才能求解。

手眼標(biāo)定

首先講一下什么是手眼標(biāo)定？為什么要手眼標(biāo)定？

因為一個物體與相機的相對位置關(guān)系和這個物體與機械臂之間的關(guān)系是不一樣的，所以在相機確定了物體的位置之后，還要把此時的位置轉(zhuǎn)換成相對于機械臂的位置，這樣機械臂才能進(jìn)行抓取。

這個位置轉(zhuǎn)換需要一個矩陣，這個矩陣就是由手眼標(biāo)定得出的。

手眼標(biāo)定的核心公式只有一個，AX=XB，這里的X就是指手（機械臂末端）與眼（攝像頭）之間的坐標(biāo)轉(zhuǎn)換關(guān)系。

視覺機械臂的相機和機械臂有兩種結(jié)合方式，一種是眼在手上（Eye-In-Hand），一種是眼在手外（Eye-To-Hand），所以手眼標(biāo)定也分兩種。

Eye-In-Hand（攝像頭被安裝在機械臂上）：

這種方式的手眼標(biāo)定是通過兩次運動解得相機坐標(biāo)系（Camera）與機械臂末端坐標(biāo)系（End）之間的坐標(biāo)關(guān)系。

注意：

1、機械臂末端（End）在機器人（Base）坐標(biāo)系下的坐標(biāo)是可以通過機械臂各個關(guān)節(jié)的角度值直接解算出來的。

2、攝像頭與機械臂末端之間的坐標(biāo)轉(zhuǎn)換關(guān)系不變，也就是說，（TE）→C始終不變；標(biāo)定板與機械臂底座之間的坐標(biāo)轉(zhuǎn)換關(guān)系不變，也就是說，TB→O也是不變的。

把TB→O按照前后兩次運動展開，有：

TB→E1TE1→C1TC1→O=TB→E2TE2→C2TC2→O

(TB→E2)-1TB→E1TE1→C1TC1→O=TE2→C2TC2→O

(TB→E2)-1TB→E1TE1→C1=TE2→C2TC2→O(TC1→O)-1

TE2→E1TE1→C1=TE2→C2TC2→C1

記

A=TE2→E1=

(TB→E2)-1TB→E1

B=TC2→C1=TC2→O(TC1→O)-1

X=TE→C=TE1→C1=TE2→C2

就得到了

AX=XB

Eye-To-Hand（攝像頭被安裝在一個固定不動的位置，而標(biāo)定板被拿在機械臂手上）：

這種方法的手眼標(biāo)定是通過兩次運動解得相機坐標(biāo)系（Camera）與機器人坐標(biāo)系（Base）之間的坐標(biāo)關(guān)系。

注意：

1、機械臂末端（End）在機器人（Base）坐標(biāo)系下的坐標(biāo)是可以通過機械臂各個關(guān)節(jié)的角度值直接解算出來的。

2、攝像頭與機械臂底座之間的坐標(biāo)轉(zhuǎn)換關(guān)系不變，也就是說，TB→C始終不變；標(biāo)定板與機械臂末端之間的坐標(biāo)轉(zhuǎn)換關(guān)系不變，也就是說，TE→O始終不變。

把TB→O按照前后兩次運動展開，有：

TE1→BTB→CTC→O1=TE2→BTB→CTC→O2

(TE2→B)-1TE1→BTB→CTC→O1=TB→CTC→O2

(TE2→B)-1TE1→BTB→C=TB→CTC→O2(TC→O1)-1

記

A=(TE2→B)-1TE1→B

B=TC→O2(TC→O1)-1

X=TB→C

就得到了

AX=XB

實際上，求解的只是一個固定的6自由度的位置姿態(tài)關(guān)系，可以用3X3的旋轉(zhuǎn)矩陣和3X1的平移矩陣表示，也可以用其他方法表示如用四元數(shù)表示角度關(guān)系等。

求解（Eye-In-Hand）

坐標(biāo)系之間的轉(zhuǎn)換關(guān)系說明：

baseHend：表示機械手坐標(biāo)系到基礎(chǔ)坐標(biāo)系的轉(zhuǎn)換關(guān)系，可以由機器人系統(tǒng)中得出。（已知）

endHcam：表示相機坐標(biāo)系到機械手坐標(biāo)系的轉(zhuǎn)換關(guān)系；這個轉(zhuǎn)化關(guān)系在機械手移動過程中是不變的；（未知，待求）

objHcam：表示相機坐標(biāo)系到標(biāo)定板坐標(biāo)系的轉(zhuǎn)換關(guān)系（相機外參），可以由相機標(biāo)定求出；（相當(dāng)于已知）

baseHobj：表示標(biāo)定板坐標(biāo)系到基礎(chǔ)坐標(biāo)系的變換，這個是最終想要得到的結(jié)果；只要機械手和標(biāo)定板的相對位置不變，這個變換矩陣不發(fā)生變化。

基礎(chǔ)坐標(biāo)系(求解baseHend)

符合右手定則的XYZ三個坐標(biāo)軸：

原點：機器人底座的中心點

X軸正向：指向機器人的正前方

Z軸正向：指向機器人的正上方

Y軸正向：由右手定則確定

六個自由度

三個位置：x、y、z（第六軸法蘭盤圓心相對于原點的偏移量）

三個角：Rx、Ry、Rz（第六軸法蘭盤的軸線角度，由初始姿態(tài)即豎直向上繞x軸旋轉(zhuǎn)Rx度，再繞Y軸旋轉(zhuǎn)Ry度，再繞Z軸旋轉(zhuǎn)Rz度得到）

旋轉(zhuǎn)方式

繞定軸X-Y-Z旋轉(zhuǎn)(判斷機械臂輸出四元數(shù)與代碼得到的四元數(shù)是否相等得到)

旋轉(zhuǎn)矩陣的計算方法如下：R=Rz?Ry?Rx

（opencv的旋轉(zhuǎn)方式是zyx，所以旋轉(zhuǎn)矩陣R=Rx*Ry*Rz）

R=Rx?Ry?Rz=X1Y1Z3

其中c為cos，s為sin。

所以：

其中t=[x,y,z]^T,從示教器讀取Rx、Ry、Rz分別是繞x、y、z軸的旋轉(zhuǎn)角度。

歐拉角：分別繞x、y、z軸旋轉(zhuǎn)的角度，不一樣的旋轉(zhuǎn)次序，得到的R不一樣；

李代數(shù)：維度是3，是繞一個軸轉(zhuǎn)動一定的角度。歐拉角可以理解成李代數(shù)在x、y、z軸上的分解旋轉(zhuǎn)。

camHobj相機到標(biāo)定板

思路大致如下：

已知雙目相機的內(nèi)參、畸變系數(shù)、外參（Pr=R?Pl+t),

對左右相機的兩張圖片調(diào)用OpenCV中的fin（dC）hessboardCorne（rs）函數(shù)，找到內(nèi)角點（如果結(jié)果不好，繼續(xù)提取亞像素點）；

將左右相機的像素點對應(yīng)起來，得到匹配的2d點；

使用空間異面直線的方法，用對應(yīng)的2d點計算出以右相機為世界坐標(biāo)系的3維坐標(biāo)Pcam；

計算出每個角點以棋盤格為世界坐標(biāo)的3維坐標(biāo)Pcal；

通過解方程Pcam=camHcal*Pcal求解出外參；

這個部分可以用張正友相機標(biāo)定Opencv實現(xiàn)

求解AX=XB

李代數(shù)到李群的轉(zhuǎn)換滿足指數(shù)映射關(guān)系，假設(shè)[w]∈so(3)，而exp[w]∈SO(3)，則其指數(shù)映射滿足羅德里格斯公式：

其中∣∣ω∣∣^2=ω12+ω22+ω32

利用李群知識求解AX=XB

展開得到：

θA?θX=θX?θB，θA?bX+bA=θX?bB+bX

采用“兩步法”求解上述方程，先解算旋轉(zhuǎn)矩陣，再求得平移向量。

求解旋轉(zhuǎn)矩陣

假設(shè)AX=XB，這里的都是旋轉(zhuǎn)矩陣(SO(3))，并非變換矩陣(SE(3))。

變換可得到：A=XBX^T

兩邊取對數(shù)：log(A)=log(XBX^T)(對數(shù)映射)

令logA=[α],logB=[β]，則上式可以化為[α]=X[β]X^T=[Xβ]從而α=Xβ

存在多組觀測值時，求解該方程可以轉(zhuǎn)化為下面最小二乘擬合問題：

很顯然，上述問題是典型的絕對定向問題，因而求解上式與絕對定向相同，其解為

其中，

當(dāng)只有兩組A、B時，即有A1,A2,B1,B2

α1=logA1,α2=logA2,β1=logB1,β2=logB2，θX=MN^?1

其中，M=(α1α2α1×α2)，N=(β1β2β1×β2)（×表示叉乘）

即可求得旋轉(zhuǎn)矩陣。

求解平移向量

θA?bX+bA=θX?bB+bX移項化簡得到：(I?θA)?bX=bA?(θX?bB)計作：c*X=d

又c不一定是可逆，所以兩邊同時乘以c^T。即為：c^T?c?X=c^T?d

所以：X=（c^T?c)^?1?(c^T?d)

當(dāng)有多組數(shù)據(jù)時：(c1^T?c1+c2^T?c2+...+cn^T?cn)?X=c1^T?d1+c2^T?d2+...+cn^T?dn

即可求得X，即平移向量

手眼標(biāo)定步驟

事先準(zhǔn)備：機械臂可以按照正常的位姿移動，并且能夠正常的輸出機械臂末端姿態(tài)；機械臂能夠穩(wěn)定搭載相機；相機能夠正確穩(wěn)定地（檢測）到物體，并且能夠正確輸出位置參數(shù)。

通過機械臂上面的相機，對待測物體進(jìn)行位姿檢測，并且輸出參數(shù)。

輸出機械臂末端的位置參數(shù)。

重復(fù)2、3步驟，采集十組以上有效參數(shù)。

將采集的有效參數(shù)放入代碼中進(jìn)行計算。

輸出手眼轉(zhuǎn)換矩陣。

代碼：

/hellohake/ar（ti）cle/det（ai）ls/104808149

讀取出攝像頭信息并確定目標(biāo)物體的位姿

前兩步完成之后就要通過攝像頭讀取信息了，這個步驟不難，比較難的是確定目標(biāo)物體的姿態(tài)。

它首先要進(jìn)行圖像分割（實例分割、語義分割、部件分割），把圖像中的每個個體單獨分出來。然后進(jìn)行物體識別，來確定哪個物體才是目標(biāo)物體。

之后是確定物體的姿態(tài)，比如一個瓶子，它是立著的，還是倒著的，是橫著的，還是豎著的。

最后確定抓取的位置，是抓取瓶子的蓋子下面，還是抓取瓶子的中間部分，還是底部。

此處的圖像還分為二維的灰度圖、RGB圖，三維的點云圖、三角面片、Volumetric、Multi-View等。

圖像分割

圖像分割是（計算機視覺）研究中的一個經(jīng)典難題，已經(jīng)成為圖像理解領(lǐng)域關(guān)注的一個熱點，圖像分割是圖像分析的第一步，是計算機視覺的基礎(chǔ)，是圖像理解的重要組成部分，同時也是圖像處理中最困難的問題之一。

所謂圖像分割是指根據(jù)灰度、彩色、空間紋理、幾何形狀等特征把圖像劃分成若干個互不相交的區(qū)域，使得這些特征在同一區(qū)域內(nèi)表現(xiàn)出一致性或相似性，而在不同區(qū)域間表現(xiàn)出明顯的不同。

簡單的說就是在一副圖像中，把目標(biāo)從背景中分離出來。對于灰度圖像來說，區(qū)域內(nèi)部的像素一般具有灰度相似性，而在區(qū)域的邊界上一般具有灰度不連續(xù)性。

圖像分割的方法主要分為以下幾類：基于閾值的分割方法、基于區(qū)域的分割的方法、基于邊緣的分割方法以及基于特定理論的分割方法。

下圖中就是基于邊緣的分割方法，正方體和香蕉的分割效果很好，易拉罐分割效果次之。

三維對象實例分割

三維對象實例分割是指從輸入的三維點云中檢測某一類的點集實例對象。與二維對象實例分割類似，雙階段方法需要區(qū)域建議，而單階段方法則需要自由建議。

三維對象實例分割的典型功能曲線圖如圖所示。

三維對象實例分割在機器人抓取任務(wù)中非常重要。但目前的方法主要是利用二維實例分割方法來獲得目標(biāo)對象的三維點云，這利用了RGB-D圖像的優(yōu)勢。

目前，三維對象實例分割仍是一個快速發(fā)展的領(lǐng)域，如果其性能和速度大大提高，在未來將得到廣泛的應(yīng)用。

姿態(tài)估計

先簡單介紹三種方法：

A.通過深度（神經(jīng)網(wǎng)絡(luò)）處理，來生成幾個候選的抓取位置。這些候選的抓握姿態(tài)被編碼到一個堆疊的多通道圖中。使用一個四層卷積神經(jīng)網(wǎng)絡(luò)對每個候選抓握姿態(tài)進(jìn)行評估，以獲得一個分?jǐn)?shù)。

B.對整個三維空間進(jìn)行均勻采樣生成候選抓取，利用三維CNN和候選抓取姿態(tài)的抓取可達(dá)性預(yù)測抓取穩(wěn)定性，得到抓取成功概率。

C.還有一種名為6-自由度GraspNet的（算法），該算法使用變分自動編碼器對抓取建議進(jìn)行采樣，并使用抓取評估器模型對采樣的抓取進(jìn)行參考。

第一幅圖可以看到，算法生成了很多的抓取位置，然后以評分的方式選擇其中一種。第二幅圖中，通過算法進(jìn)行模型匹配，來生成最佳的抓取位置。

機械臂正逆運動學(xué)求解

首先，大致說一下什么是正逆學(xué)求解。機械臂的運動主要是靠電機的旋轉(zhuǎn)來實現(xiàn)末端的移動，一個旋轉(zhuǎn)，一個空間的位置移動。

輸入每個電機旋轉(zhuǎn)的角度j1,j2…,jn，輸出是機械臂末端執(zhí)行器的位置(x,y,z)與姿態(tài)(α,β,γ)，這就是機械臂運動學(xué)正解。反之，為逆解。

機械臂還有一個常用的參數(shù)叫自由度。機械臂機架上有n個電機，就是n自由度機械臂，也叫n軸機械臂，記為nDOF。（末端執(zhí)行器上的不算，因為它不影響機械臂的運動學(xué)結(jié)果。）

UR5機械臂的Gazebo（仿真）圖

運動學(xué)正解

正運動學(xué)是已知關(guān)節(jié)六個角度求變換矩陣T

所以末端坐標(biāo)系6到基座固定坐標(biāo)系0的變換矩陣

那么求正解就很簡單了，只要輸入六個關(guān)節(jié)角度θi，就得到末端坐標(biāo)在基坐標(biāo)系的變換矩陣T。

ur機械臂的視教板上末端點的坐標(biāo)是用六個值[x,y,z,rx,ry,rz]表示的。

前三個值[x,y,z]是三維笛卡爾坐標(biāo)，表示空間位置，后三個值[rx,ry,rz]是坐標(biāo)旋轉(zhuǎn)向量，表示空間姿態(tài)。我們得到的變換矩陣T怎么變成六值坐標(biāo)[x,y,z,rx,ry,rz]呢？設(shè)

T的左上角的3x3矩陣是旋轉(zhuǎn)矩陣，旋轉(zhuǎn)矩陣和旋轉(zhuǎn)向量之間可以通過羅德里格斯（Rodrigues）變換進(jìn)行轉(zhuǎn)換。

opencv里有相應(yīng)的函數(shù)調(diào)用。算法也比較簡單，不用opencv的函數(shù)自己寫代碼也不難。T的右上角3x1就是空間位置[x,y,z]。這樣有變換矩陣T得到六值坐標(biāo)，完成了正解。

運動學(xué)逆解