干貨 | 相機(jī)標(biāo)定：機(jī)器人手眼標(biāo)定
來自于點擊下方卡片，關(guān)注“新機(jī)器視覺”公眾號
視覺/圖像重磅干貨，第一時間送達(dá)
相機(jī)標(biāo)定是機(jī)器人視覺中非常重要的一步，可以幫助機(jī)器人轉(zhuǎn)換識別到的視覺信息，從而完成后續(xù)的控制工作，例如視覺抓取等等。筆者做過一些機(jī)器人手眼標(biāo)定的工作，在此用盡量簡單的語言來描述下機(jī)器人手眼標(biāo)定的流程。本文的目的在于讓大家對于相機(jī)標(biāo)定有一個感性的認(rèn)知，能夠知道不同條件下機(jī)器人手眼標(biāo)定的流程，具體的實現(xiàn)方法及技術(shù)細(xì)節(jié)還需要大家自行g(shù)oogle。
▌1.?坐標(biāo)系標(biāo)定通用方法
機(jī)器人手眼標(biāo)定其實就是兩個坐標(biāo)系之間轉(zhuǎn)換關(guān)系的標(biāo)定。假設(shè)現(xiàn)在有兩個坐標(biāo)系robot和camera，并且我們已知對應(yīng)的幾個固定點?
?在這兩個坐標(biāo)系的坐標(biāo)?
?和?
?。那么根據(jù)坐標(biāo)系轉(zhuǎn)換公式，我們可以得到：
[1]?
上式中?
?就代表我們希望求出來的相機(jī)到機(jī)器人的轉(zhuǎn)換矩陣。??和??為補(bǔ)1后的“齊次坐標(biāo)”：?
?，這樣我們的齊次坐標(biāo)變換矩陣??便可以同時包含旋轉(zhuǎn)以及平移。
只要學(xué)過線性代數(shù)的同學(xué)應(yīng)該都清楚，式[1]相當(dāng)于就是一個N元一次方程，只要點??的個數(shù)大于我們求解的轉(zhuǎn)換矩陣維度同時這些點線性不相關(guān)，我們便可以通過偽逆矩陣計算出來?：
[2]?
計算出來的坐標(biāo)變換矩陣便可以直接應(yīng)用于后續(xù)的坐標(biāo)變換?，F(xiàn)在計算出來的矩陣允許坐標(biāo)進(jìn)行旋轉(zhuǎn)，平移，以及任意方向的縮放。如果想要把這個坐標(biāo)矩陣變成剛性的變換矩陣，只需要將其左上角的旋轉(zhuǎn)矩陣正交化即可，但是加了這個約束后有可能降低坐標(biāo)變換精度，即剛性變換矩陣約束太多，造成欠擬合。
當(dāng)坐標(biāo)變換存在非線性變換時，我們也可以設(shè)計并訓(xùn)練一個神經(jīng)網(wǎng)絡(luò)?
?直接擬合兩組坐標(biāo)之間的關(guān)系：
[3]?
當(dāng)然一般情況下，式[2]中的線性矩陣便足以描述機(jī)器人和相機(jī)之間的坐標(biāo)轉(zhuǎn)換關(guān)系。引入神經(jīng)網(wǎng)絡(luò)進(jìn)行非線性擬合，訓(xùn)練集的擬合精度會非常高，但是當(dāng)訓(xùn)練點個數(shù)不足的時候，會發(fā)生過擬合，使得實際測試時擬合精度反而降低。
通過以上的計算我們可以知道，只要我們能夠同時測量出來固定點??在兩個坐標(biāo)系的坐標(biāo)??和??并且測量出來多組數(shù)據(jù)，我們就可以很方便地計算出來坐標(biāo)變換矩陣。當(dāng)然在實際過程中我們有可能很難同時測量出來固定點??在兩個坐標(biāo)系的坐標(biāo)，在后面我會分相機(jī)和機(jī)器人的情況討論如何用一些小trick來盡量實現(xiàn)這個測量目標(biāo)，如何進(jìn)行標(biāo)定，以及如何進(jìn)行標(biāo)定精度的驗證。
▌2. 相機(jī)放置在固定位置，與機(jī)器人分開（eye-to-hand）
Fig 1. 機(jī)器人eye-to-hand標(biāo)定
我們應(yīng)該養(yǎng)成一個習(xí)慣，即遇到問題時先考慮我們的待求量和已知量。如Fig 1 所示。在eye-to-hand的問題中，待求量為相機(jī)到機(jī)器人基座坐標(biāo)系的固定轉(zhuǎn)換矩陣?
?。注意我們這里用的base而不是robot來表示機(jī)器人基座坐標(biāo)系，因為需要與機(jī)器人其他部分的坐標(biāo)系進(jìn)行區(qū)分。機(jī)器人基座坐標(biāo)系是固定不動的，而其他的關(guān)節(jié)，例如機(jī)器人末端坐標(biāo)系end，是不斷變化的?？紤]到相機(jī)是固定在一個位置，因此相機(jī)只有相對于基座坐標(biāo)系才是固定不動的，這樣我們的待求量??是一個固定參數(shù)，標(biāo)定才有意義。
根據(jù)我們在節(jié)1中討論的，我們接下來需要測量幾組固定點??在機(jī)器人基座坐標(biāo)系下的坐標(biāo)?
?和 在相機(jī)坐標(biāo)系下的坐標(biāo)??。那么怎么去測量這些點的坐標(biāo)呢？接下來我們需要用到一個標(biāo)定神器：棋盤格。
Fig 2. 棋盤格及角點識別
如Fig 2所示，我們可以通過相應(yīng)的視覺算法準(zhǔn)確地識別棋盤格的角點，具體算法在opencv，python，matlab，和ros等常用平臺中都有包裝好的函數(shù)，直接調(diào)用即可，在此不進(jìn)行贅述。
通過識別棋盤格，我們便可以得到棋盤格角點在相機(jī)坐標(biāo)系的坐標(biāo)?
?，但是這是二維坐標(biāo)，通過相機(jī)內(nèi)參和物體的深度信息我們可以計算出來??，這個我們之后會分為3D相機(jī)和2D相機(jī)進(jìn)行討論。我們可以假設(shè)已經(jīng)測量出來了，接下來我們只要測量出來對應(yīng)的棋盤格點在機(jī)器人中的坐標(biāo)??，便可以標(biāo)定出來機(jī)器人和相機(jī)之間的轉(zhuǎn)換矩陣??。棋盤格在標(biāo)定的時候，會固定在機(jī)器人的末端，而機(jī)器人的末端坐標(biāo)系（end）到基座坐標(biāo)系（base）可以通過機(jī)器人正向運動學(xué)計算得到。那么我們可以通過如下關(guān)系測量出來棋盤格點在機(jī)器人基座坐標(biāo)系的坐標(biāo)??：
[4]?
式[4] 中，?
?和?
?分別代表機(jī)器人末端坐標(biāo)系（end）到基座坐標(biāo)系（base）的轉(zhuǎn)換矩陣和棋盤格坐標(biāo)系（board）到機(jī)器人末端坐標(biāo)系（end）的轉(zhuǎn)換矩陣。其中??可以實時根據(jù)機(jī)器人正運動學(xué)得到，而??可以通過設(shè)計一個固定尺寸的棋盤格得到。當(dāng)棋盤格尺寸以及安裝固定后，我們可以設(shè)棋盤格的左上角點為原點，然后測量或者根據(jù)設(shè)計尺寸得到棋盤格原點到機(jī)器人末端坐標(biāo)原點的平移坐標(biāo)。另外棋盤格平面一般與機(jī)器人末端平面平行，這樣該原點的法向量也知道了，接下來便可以計算得到??。?
?代表棋盤格角點在棋盤格坐標(biāo)系下的坐標(biāo)，這個同樣根據(jù)設(shè)計棋盤格的尺寸得到。另外我們需要注意棋盤格的角點在機(jī)器人坐標(biāo)系和在圖像坐標(biāo)系的順序需要一一對應(yīng)，但是一般的方形棋盤格會有旋轉(zhuǎn)對稱的問題，即我們無法區(qū)分出來棋盤格的原點是左上角點還是右下角點，因此手眼標(biāo)定時我們也可以用下面的非對稱棋盤格。
Fig 3. 非對稱棋盤格及角點識別，旋轉(zhuǎn)180度可以區(qū)分左上角點跟右下角點
當(dāng)我們根據(jù)式[4]計算得到棋盤格角點在機(jī)器人基座坐標(biāo)系中的坐標(biāo)?，然后根據(jù)圖像識別加相機(jī)內(nèi)參和深度信息得到棋盤格在相機(jī)坐標(biāo)系中的坐標(biāo)??，我們便可以根據(jù)節(jié)1所述的方法計算得到相機(jī)到機(jī)器人的轉(zhuǎn)換矩陣：
[5]?
接下來我會再討論如何將棋盤格在圖像中的坐標(biāo)??，轉(zhuǎn)換為棋盤格在相機(jī)坐標(biāo)系中的坐標(biāo)??。
2.1 ?3D相機(jī)
相機(jī)的圖像坐標(biāo)為二維坐標(biāo)，代表了在圖像中的行和列數(shù)。將二維的圖像坐標(biāo)轉(zhuǎn)換為在相機(jī)坐標(biāo)系的三維坐標(biāo)，是基于需要用到相機(jī)的內(nèi)參公式和深度值:
[6]?
式[6]中?
?為目標(biāo)點的深度值，3D相機(jī)能夠測出來目標(biāo)點的深度，從而能夠計算出目標(biāo)點的3維坐標(biāo)。列數(shù)?
?和行數(shù)?
?為目標(biāo)點在圖像中的坐標(biāo)。?
?代表目標(biāo)點在相機(jī)坐標(biāo)系下的三維坐標(biāo)。?
?和?
?為焦距，用于描述像素單位和三維坐標(biāo)單位之間的比例關(guān)系。
?和?
?為相機(jī)光心在圖像中的投影位置，用于計算圖像原點和相機(jī)坐標(biāo)系原點之間的位移。
我們通過求逆矩陣，可以根據(jù)目標(biāo)點的圖像坐標(biāo)計算得到其三維坐標(biāo)。相機(jī)內(nèi)參一般相機(jī)提供方會給出，有的相機(jī)提供方還會直接提供計算好的三維點云信息。如果相機(jī)提供方只給出了一個深度圖，也沒有給相機(jī)內(nèi)參，則需要我們通過張正友標(biāo)定法，標(biāo)定出來相機(jī)的內(nèi)參。opencv，python，matlab等提供了完善的內(nèi)參標(biāo)定工具包，可以直接使用。
Fig 4. 相機(jī)內(nèi)參解釋
2.2 2D相機(jī)
2.2.1 基于平面深度z進(jìn)行目標(biāo)3D坐標(biāo)估計
在節(jié)2.1中我們說明了3D相機(jī)的情況下由2D圖像坐標(biāo)轉(zhuǎn)換到3D相機(jī)坐標(biāo)系的方法。如果我們使用的是2D相機(jī)，那么我們便缺失了深度信息，也就是式[6]中的??，因此2D相機(jī)一般用于一個平面的物體識別。這樣我們只需要估計出一個平面的??坐標(biāo)，然后便可以根據(jù)式[6] 計算得到目標(biāo)點在相機(jī)坐標(biāo)系下的三維坐標(biāo)??。
2.2.2 基于平面參考點集進(jìn)行目標(biāo)3D坐標(biāo)估計
另外當(dāng)2D相機(jī)只用于識別平面上物體的位移和旋轉(zhuǎn)時，我們也可以直接通過下面這個方法直接估計目標(biāo)點的位置和旋轉(zhuǎn)。
Fig 5. 非垂直坐標(biāo)系下目標(biāo)點的表示
如Fig 5所示，我們可以通過擺放三個標(biāo)志物于相機(jī)視野的角落，這樣識別出來三個位置點 在圖像坐標(biāo)系中的坐標(biāo)
,?
?,?
?。然后我們可以構(gòu)建平行四邊形，用兩個坐標(biāo)軸的向量去計算目標(biāo)點在圖像坐標(biāo)系的坐標(biāo)?
?。注意這里的坐標(biāo)軸不需要是直角坐標(biāo)軸，從而更加符合實際情況，因為實際貼標(biāo)志物時不可能做到兩條坐標(biāo)軸完全垂直。
[7]?
[8]?
[9]?
接下來，我們在機(jī)器人末端安裝探針，通過觸碰標(biāo)志物，得到這三個位置點在機(jī)器人基座坐標(biāo)系的坐標(biāo)
,?
?,?
?，這三個點應(yīng)該在一個平面上。因為式[9]計算出來的是一個比例關(guān)系，因此根據(jù)相似三角形的原理，我們可以計算出目標(biāo)點點在機(jī)器人基座坐標(biāo)系base的坐標(biāo)?
?:
[10]?
通過式[7]-[10]我們可以發(fā)現(xiàn)，通過建立一個平面中三個參考點的非直角坐標(biāo)系，然后就可以將目標(biāo)點的圖像坐標(biāo)??轉(zhuǎn)換為在機(jī)器人基座坐標(biāo)系的三維坐標(biāo)??。這個方法需要目標(biāo)平面與相機(jī)平面平行，但是不需要知道目標(biāo)點的深度值，也不需要參考點為直角坐標(biāo)系。
▌3. 相機(jī)固定在機(jī)器人末端（eye-in-hand)
3.1 3D相機(jī)
如Fig 6所示，當(dāng)相機(jī)固定于機(jī)械臂末端時，此時相機(jī)坐標(biāo)系與機(jī)械臂末端坐標(biāo)系的變換關(guān)系固定，而與基座坐標(biāo)系的變換關(guān)系會時刻變化，因此此時我們的待求量變?yōu)榱讼鄼C(jī)坐標(biāo)系camera到機(jī)械臂末端坐標(biāo)系end的變換矩陣?
?。而根據(jù)Fig 6圖中的各個坐標(biāo)系的關(guān)系，我們可以列出來如下的坐標(biāo)變換方程：
[11]?
式[11]中??代表固定在某個位置的棋盤格的點到機(jī)器人基座坐標(biāo)系的坐標(biāo)。這個坐標(biāo)的計算有兩種途徑：（i）在機(jī)械臂末端裝上探針，直接由末端去觸碰棋盤格點，根據(jù)機(jī)器人正向運動學(xué)得到的末端到基座坐標(biāo)系的轉(zhuǎn)換矩陣??加上棋盤格角點到末端坐標(biāo)系的坐標(biāo)?
?，我們可以得到固定的棋盤格角點在機(jī)器人基座坐標(biāo)系的坐標(biāo)??。（ii）接下來我們在末端裝上camera，因為3D相機(jī)可以直接測量出棋盤格角點以及對應(yīng)的3D坐標(biāo)??，所以我們加上機(jī)器人末端到基座坐標(biāo)系的轉(zhuǎn)換矩陣??和相機(jī)坐標(biāo)系到機(jī)器人末端坐標(biāo)系的轉(zhuǎn)換矩陣??便可以求出來棋盤格在機(jī)器人基座坐標(biāo)系的坐標(biāo)??。不過很不幸，相機(jī)坐標(biāo)系到機(jī)器人末端坐標(biāo)系的轉(zhuǎn)換矩陣??是我們的待求量，我們還不知道，因此我們需要對式[11]進(jìn)行下簡單的變換：
[12]?
觀察式[12]我們可以發(fā)現(xiàn)，左式中的三個變量都是已知量，因此我們便可以通過機(jī)器人觸碰棋盤格加相機(jī)識別棋盤格角點3D坐標(biāo)的方法，求出來相機(jī)到機(jī)器人末端坐標(biāo)系的轉(zhuǎn)換矩陣??。
3.2 2D相機(jī)
正如節(jié)2.2中討論的那樣，2D相機(jī)因為無法測量深度值，因此無法直接重建出來目標(biāo)物體的3D坐標(biāo)信息。如果2D相機(jī)裝在手臂上，那大多數(shù)是用來進(jìn)行視覺伺服控制的，可以不用進(jìn)行相機(jī)和機(jī)器人末端坐標(biāo)系的標(biāo)定。不過我們之前在節(jié)2.2.2中提出了一個基于平面參考點集的方法，在這里同樣適用。只要目標(biāo)物體所在工作平臺的參考點集可以實時被測量到，同時我們又事先標(biāo)定了這個參考點集在機(jī)器人基座坐標(biāo)系的坐標(biāo)，那么我們便可以通過同樣的比例關(guān)系（見式[7]-[10]），計算得到目標(biāo)點在機(jī)器人基座坐標(biāo)系的坐標(biāo)。
▌4. 標(biāo)定結(jié)果評價
當(dāng)手眼標(biāo)定結(jié)束后，我們還希望對我們的標(biāo)定結(jié)果進(jìn)行評價。根據(jù)式[1]我們可以發(fā)現(xiàn)，目標(biāo)點??在機(jī)器人坐標(biāo)系中的坐標(biāo)可以由兩種途徑獲得，一種是直接通過機(jī)器人末端觸碰的方式得到測量值?
?，另外一種是通過相機(jī)識別加標(biāo)定出來的轉(zhuǎn)換矩陣得到預(yù)測值?
?。通過測量幾組不同位置的目標(biāo)點，我們可以比較測量值??和預(yù)測值??的距離誤差。通過計算各個點的誤差的均值和標(biāo)準(zhǔn)差，最終我們可以計算得出標(biāo)定結(jié)果的系統(tǒng)誤差（均值）和隨機(jī)誤差（標(biāo)準(zhǔn)差）。
本文討論了在3D/2D eye-to-hand/eye-in-hand的不同情況下，機(jī)器人手眼關(guān)系的不同的標(biāo)定方法。正如我們在節(jié)1中所提到的，標(biāo)定兩個坐標(biāo)系最直接的方式就是盡量測量出來同一組點在不同坐標(biāo)系下的坐標(biāo)，從而直接可以通過矩陣偽逆的方法計算出來坐標(biāo)變換矩陣。當(dāng)然，還有更高級的方法在不滿足上述條件下的情況也可以標(biāo)定出來矩陣，也就是經(jīng)典的Ax=xB問題的求解，其中A，B已知，x為待求量。這個求解方法我之后有時間會再寫文章進(jìn)行討論。另外還有經(jīng)典的張正友相機(jī)標(biāo)定法，我也會抽空進(jìn)行解讀。