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

簡(jiǎn)介

視覺(jué)機(jī)械臂是智能機(jī)器人的一個(gè)重要分支，它主要包括控制芯片、驅(qū)動(dòng)電路、機(jī)械臂、相機(jī)等部分。

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

整個(gè)抓取過(guò)程大致分為以下幾步（含圖中九步）：

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

——→①讀取攝像頭信息

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

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

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

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

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

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

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

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

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

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

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

由于相機(jī)采用小孔成像的原理來(lái)采集圖像，為了使物體成像更加清晰，就采用了透鏡的方式。但是，因?yàn)橥哥R本身在成像的過(guò)程中會(huì)不可避免的引入徑向畸變和切向畸變（比如一條直線成像后會(huì)顯示成一條弧線）。

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

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

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

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

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

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

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

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

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

uv：像素坐標(biāo)系，原點(diǎn)為圖像左上角，單位pixel

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

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

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

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

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

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

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

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

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

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

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

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

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

手眼標(biāo)定

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

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

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

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

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

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

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

注意：

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

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

把TB→O按照前后兩次運(yùn)動(dòng)展開，有：

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（攝像頭被安裝在一個(gè)固定不動(dòng)的位置，而標(biāo)定板被拿在機(jī)械臂手上）：

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

注意：

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

把TB→O按照前后兩次運(yùn)動(dòng)展開，有：

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

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

求解（Eye-In-Hand）

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

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

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

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

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

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

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

原點(diǎn)：機(jī)器人底座的中心點(diǎn)

X軸正向：指向機(jī)器人的正前方

Z軸正向：指向機(jī)器人的正上方

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

六個(gè)自由度

三個(gè)位置：x、y、z（第六軸法蘭盤圓心相對(duì)于原點(diǎn)的偏移量）

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

旋轉(zhuǎn)方式

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

旋轉(zhuǎn)矩陣的計(jì)算方法如下：R = R z ?R y ?R x

（opencv的旋轉(zhuǎn)方式是 z y x，所以旋轉(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，是繞一個(gè)軸轉(zhuǎn)動(dòng)一定的角度。歐拉角可以理解成李代數(shù)在x、y、z軸上的分解旋轉(zhuǎn)。

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

思路大致如下：

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

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

將左右相機(jī)的像素點(diǎn)對(duì)應(yīng)起來(lái)，得到匹配的2d點(diǎn)；

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

計(jì)算出每個(gè)角點(diǎn)以棋盤格為世界坐標(biāo)的3維坐標(biāo)Pcal；

通過(guò)解方程Pcam=camHcal*Pcal求解出外參；

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

求解AX=XB

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

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

利用李群知識(shí)求解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

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

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

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

很顯然，上述問(wèn)題是典型的絕對(duì)定向問(wèn)題，因而求解上式與絕對(duì)定向相同，其解為

其中，

當(dāng)只有兩組A、B時(shí)，即有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 移項(xiàng)化簡(jiǎn)得到：(I?θA)?bX=bA?(θX?bB)計(jì)作：c*X=d

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

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

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

即可求得X，即平移向量

手眼標(biāo)定步驟

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

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

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

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

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

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

代碼：

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

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

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

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

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

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

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

圖像分割

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

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

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

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

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

三維對(duì)象實(shí)例分割

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

三維對(duì)象實(shí)例分割的典型功能曲線圖如圖所示。

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

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

姿態(tài)估計(jì)

先簡(jiǎn)單介紹三種方法：

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

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

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

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

機(jī)械臂正逆運(yùn)動(dòng)學(xué)求解

首先，大致說(shuō)一下什么是正逆學(xué)求解。機(jī)械臂的運(yùn)動(dòng)主要是靠電機(jī)的旋轉(zhuǎn)來(lái)實(shí)現(xiàn)末端的移動(dòng)，一個(gè)旋轉(zhuǎn)，一個(gè)空間的位置移動(dòng)。

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

機(jī)械臂還有一個(gè)常用的參數(shù)叫自由度。機(jī)械臂機(jī)架上有n個(gè)電機(jī)，就是n自由度機(jī)械臂，也叫n軸機(jī)械臂，記為n DOF。（末端執(zhí)行器上的不算，因?yàn)樗挥绊憴C(jī)械臂的運(yùn)動(dòng)學(xué)結(jié)果。）