【光電智造】視覺機械臂自主抓取全流程

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

自主抓取是指，在沒有人為干預的情況下，視覺機械臂系統通過攝像頭獲取到目標物體的位置，并且通過驅動機械臂來完成對于目標物體的抓取任務。

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

相機標定——→相機和機械臂進行手眼標定

——→①讀取攝像頭信息

——→②識別出目標物體并得出位姿

——→③物體在相機中的二維坐標轉化成相對于相機光心的三維坐標

——→④再轉換成相對于機械臂底座的世界坐標

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

——→⑥通過已知的目標位置和末端執行器的位姿信息來計算機械臂上每個電機應轉動的角度

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

——→⑧驅動機械臂各個關節一步一步到達合適的位置

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

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

什么是相機標定？為什么要進行相機標定呢？

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

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

一般情況下，只要是對于成像精度較高的場景都需要進行標定，比如三維結構光成像、人臉識別、視覺測量等。

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

在OpenCv當中已經有了對應的庫，在使用前只需設置一下每個格子的大小以及格子的行列數（減去一）即可進行標定。相機標定的合格指標一共有四個，分別是x軸方向、y軸方向、遠近以及旋轉角度。

捕捉到多張有效圖片之后，進度條會變成綠色，點擊CALIBRATE可直接計算出關系矩陣。以下就是我采用奧比中光Astro Pro設備得到的標定結果。

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

OW-XWYWZW：世界坐標系，描述相機位置，單位m

Oc-XcYcZc：相機坐標系，光心為原點，單位m

xy：圖像坐標系，光心為圖像中點，單位mm

uv：像素坐標系，原點為圖像左上角，單位pixel

P：世界坐標系中的一點，現實空間中的點

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

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

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

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

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

圍繞著不同的坐標軸，旋轉不同的角度都是有對應矩陣的，如下圖所示：

從相機坐標系到圖像坐標系的變換，是從3D向2D的轉換，主要思想就是相似三角形原理，對應原理圖如下：

圖中P點就是現實世界的坐標，p點就是在相機中的成像坐標，f就是透鏡的焦距。

圖像坐標與像素坐標的轉換關系如下：

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

將以上這幾個公式進行整理，結果如下：

相機的內參和外參已在圖中標明，由于未知數有很多個，所以需要多組數據才能求解。

首先講一下什么是手眼標定？為什么要手眼標定？

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

這個位置轉換需要一個矩陣，這個矩陣就是由手眼標定得出的。

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

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

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

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

注意：

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

2、攝像頭與機械臂末端之間的坐標轉換關系不變，也就是說，TE→C始終不變；標定板與機械臂底座之間的坐標轉換關系不變，也就是說，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（攝像頭被安裝在一個固定不動的位置，而標定板被拿在機械臂手上）：

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

注意：

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

2、攝像頭與機械臂底座之間的坐標轉換關系不變，也就是說，TB→C始終不變；標定板與機械臂末端之間的坐標轉換關系不變，也就是說，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自由度的位置姿態關系，可以用3X3的旋轉矩陣和3X1的平移矩陣表示，也可以用其他方法表示如用四元數表示角度關系等。

求解（Eye-In-Hand）

坐標系之間的轉換關系說明：

baseHend：表示機械手坐標系到基礎坐標系的轉換關系，可以由機器人系統中得出。（已知）

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

objHcam：表示相機坐標系到標定板坐標系的轉換關系（相機外參），可以由相機標定求出；（相當于已知）

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

基礎坐標系(求解baseHend)

符合右手定則的XYZ三個坐標軸：

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

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

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

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

六個自由度

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

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

旋轉方式

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

旋轉矩陣的計算方法如下：R = R z ?R y ?R x

（opencv的旋轉方式是 z y x，所以旋轉矩陣R=Rx*Ry*Rz）

R=Rx?Ry?Rz=X1Y1Z3

其中c為cos，s為sin。

所以：

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

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

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

camHobj相機到標定板

思路大致如下：

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

對左右相機的兩張圖片調用OpenCV中的findChessboardCorners函數，找到內角點（如果結果不好，繼續提取亞像素點）；

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

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

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

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

這個部分可以用張正友相機標定Opencv實現

求解AX=XB

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

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

利用李群知識求解AX=XB

展開得到：

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

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

求解旋轉矩陣

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

變換可得到：A =XBX^T

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

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

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

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

?其中，

當只有兩組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)（×表示叉乘）

即可求得旋轉矩陣。

求解平移向量

θ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)

當有多組數據時：(c1^T?c1+c2^T?c2+...+cn^T?cn)?X=c1^T?d1+c2^T?d2+...+cn^T?dn

即可求得X，即平移向量

手眼標定步驟

1. 事先準備：機械臂可以按照正常的位姿移動，并且能夠正常的輸出機械臂末端姿態；機械臂能夠穩定搭載相機；相機能夠正確穩定地檢測到物體，并且能夠正確輸出位置參數。

   
   

2. 通過機械臂上面的相機，對待測物體進行位姿檢測，并且輸出參數。

   
   

3. 輸出機械臂末端的位置參數。

   
   

4. 重復2、3步驟，采集十組以上有效參數。

   
   

5. 將采集的有效參數放入代碼中進行計算。

   
   

6. 輸出手眼轉換矩陣。

   
   

代碼：

https://blog.csdn.net/hellohake/article/details/104808149

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

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

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

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

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

圖像分割

圖像分割是計算機視覺研究中的一個經典難題，已經成為圖像理解領域關注的一個熱點，圖像分割是圖像分析的第一步，是計算機視覺的基礎，是圖像理解的重要組成部分，同時也是圖像處理中最困難的問題之一。

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

簡單的說就是在一副圖像中，把目標從背景中分離出來。對于灰度圖像來說，區域內部的像素一般具有灰度相似性，而在區域的邊界上一般具有灰度不連續性。

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

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

三維對象實例分割

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

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

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

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

姿態估計

先簡單介紹三種方法：

A.通過深度神經網絡處理，來生成幾個候選的抓取位置。這些候選的抓握姿態被編碼到一個堆疊的多通道圖中。使用一個四層卷積神經網絡對每個候選抓握姿態進行評估，以獲得一個分數。

B.對整個三維空間進行均勻采樣生成候選抓取，利用三維CNN和候選抓取姿態的抓取可達性預測抓取穩定性，得到抓取成功概率。

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

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

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

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

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

UR5機械臂的Gazebo仿真圖

運動學正解

正運動學是已知關節六個角度求變換矩陣T

所以末端坐標系6到基座固定坐標系0的變換矩陣

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

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

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

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

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

運動學逆解

逆解相對復雜一些，根據機械臂末端的空間位置和姿態，來求解可能的關節角度。求逆解的方法有解析法，迭代法和幾何法等。下面我們推導一下ur的逆解。

首先計算變換矩陣T過程中的一些中間矩陣

?，其中c23=cos(θ2+θ3)，s23=sin(θ2+θ3)。

由

得到

計算

得到

等式兩邊矩陣的行列應該分別相等，由第三行第四列得到

可解得

有兩個解。這里注意寫程序的時候，求解這里的反正切是用atan2()這類的函數，返回之在(-π,+π]。

而反余弦的返回值在[0,π]，從而保證在2π范圍每個解是唯一的。

由第三行第三列得

可解得

兩個解。由第三行第二列得到

可解得

接著由

計算

得出等式左邊等于

視覺機械大致有11步，前文圖中九步加上相機標定、手眼標定。里面有很多知識也是借鑒別人的，畢竟一個的能力有限，不可能完成這么多理論知識。

自己在學習前也是找了很多資料，沒有發現一篇能把視覺機械臂全流程講詳細的帖子。所以我學習并理解了全流程之后，自己整理了一篇文章，僅供大家參考。

因為視覺機械臂本身涉及的內容比較多，所以我對于每一步的講解也不是非常的透徹。如果想更詳細的了解其中的某個或某些步驟，可以自行網上檢索。

原文鏈接：

https://blog.csdn.net/knightsinghua/article/details/125328920