小魚發現玩機械臂的小姐姐愈來愈多了。。。再說說手眼標定那些事~

你們好，我是小魚。最近小魚發現玩機械臂的小姐姐愈來愈多了，由於又有小姐姐找小魚請教機械臂手眼標定的事情，因此小魚今天爲了小姐姐們優化一下本身以前寫的代碼。

以前小魚以前手擼過tsai的手眼標定算法，但對代碼沒有作過多的解釋，致使有些同窗用起來有些吃力，因此今天給你們講講小魚寫的手眼標定代碼怎麼使用。

若是不知道四元數、歐拉角、旋轉矩陣是什麼的同窗建議先看一下臺大林沛羣老師的機器人學公開課：www.bilibili.com/video/av540… 或者燕山大學公開課，機器人技術。

接下來小魚來一段段解析代碼，手眼標定的輸入和輸出都很簡單，昨天有位同窗在羣裏問，爲何要手眼標定？

其實答案就是不標，不知道相機和機械臂到底啥關係，相機識別到了物品，機械臂不知道到哪裏抓。

核心算法函數

小魚將代碼進行了二次的封裝，將手眼標定變成了一個只有三四十行的函數。

對算法有興趣的同窗，能夠關注小魚公衆號，魚香ROS，後臺回覆手眼標定便可得到論文原文及算法詳解。

import transforms3d as tfsimport numpy as npimport math
def handeyecalib(Hgs,Hcs):    def skew(v):        return np.array([[0,-v[2],v[1]],                         [v[2],0,-v[0]],                         [-v[1],v[0],0]])
    def rot2quat_minimal(m):        quat =  tfs.quaternions.mat2quat(m[0:3,0:3])        return quat[1:]
    def quatMinimal2rot(q):        p = np.dot(q.T,q)        w = np.sqrt(np.subtract(1,p[0][0]))        return tfs.quaternions.quat2mat([w,q[0],q[1],q[2]])
    Hgijs,Hcijs = [], []    A,B  = [],[]    size = 0    for i in range(len(Hgs)):        for j in range(i+1,len(Hgs)):            size += 1            Hgij = np.dot(np.linalg.inv(Hgs[j]),Hgs[i])            Hgijs.append(Hgij)            Pgij = np.dot(2,rot2quat_minimal(Hgij))
            Hcij = np.dot(Hcs[j],np.linalg.inv(Hcs[i]))            Hcijs.append(Hcij)            Pcij = np.dot(2,rot2quat_minimal(Hcij))
            A.append(skew(np.add(Pgij,Pcij)))            B.append(np.subtract(Pcij,Pgij))    MA = np.asarray(A).reshape(size*3,3)    MB = np.asarray(B).reshape(size*3,1)    Pcg_  =  np.dot(np.linalg.pinv(MA),MB)    pcg_norm = np.dot(np.conjugate(Pcg_).T,Pcg_)    Pcg = np.sqrt(np.add(1,np.dot(Pcg_.T,Pcg_)))    Pcg = np.dot(np.dot(2,Pcg_),np.linalg.inv(Pcg))    Rcg = quatMinimal2rot(np.divide(Pcg,2)).reshape(3,3)

    A ,B = [], []    id = 0    for i in range(len(Hgs)):        for j in range(i+1,len(Hgs)):            Hgij = Hgijs[id]            Hcij = Hcijs[id]            A.append(np.subtract(Hgij[0:3,0:3],np.eye(3,3)))            B.append(np.subtract(np.dot(Rcg,Hcij[0:3,3:4]),Hgij[0:3,3:4]))            id += 1
    MA = np.asarray(A).reshape(size*3,3)    MB = np.asarray(B).reshape(size*3,1)    Tcg = np.dot(np.linalg.pinv(MA),MB).reshape(3,)    return tfs.affines.compose(Tcg,np.squeeze(Rcg),[1,1,1])
複製代碼

能夠看到，這個函數有兩個輸入參數，一個是Hgs,一個是Hcs。

• Hgs是機械臂末端的位姿的齊次矩陣數組
• Hcs相機中標定板的位姿的齊次矩陣數組（也可使用相機標定時的外參）

不知道什麼是齊次矩陣的能夠瞅瞅小魚這篇文章：手眼標定搞定了，手眼矩陣不知道怎麼用？快戳！！齊次矩陣就是將旋轉矩陣和平移變換合併到了一塊兒。

你們平時用的時候，獲得的四元數比較多，因此都會遇到四元數轉旋轉矩陣再合併平移變換成齊次矩陣的問題，不知道代碼怎麼寫，小魚這裏再推薦一個開源庫給你：開源推薦：寫機器人算法，你必須掌握的python開源庫

數據生產

知道了算法輸入是什麼，咱們就產生數據來投喂吧！由於小魚使用的機械臂給出的是歐拉角，因此小魚就使用歐拉角來產生兩個矩陣數據。

看看小魚下面的代碼是否是很清晰了，數據經過計算轉換成齊次矩陣數組。而後把數組餵給剛剛的上面的函數就完成了手眼標定了

def get_matrix_eular_radu(x,y,z,rx,ry,rz):    rmat = tfs.euler.euler2mat(math.radians(rx),math.radians(ry),math.radians(rz))    rmat = tfs.affines.compose(np.squeeze(np.asarray((x,y,z))), rmat, [1, 1, 1])    return rmat
hand = [1.1988093940033604, -0.42405585264804424, 0.18828251788562061, 151.3390418721659, -18.612399542280507, 153.05074895025035,        1.1684831621733476, -0.183273375514656, 0.12744868246620855, -161.57083804238462, 9.07159838346732, 89.1641128844487,        1.1508343174145468, -0.22694301453461405, 0.26625166858469146, 177.8815855486261, 0.8991159570568988, 77.67286224959672]camera = [-0.16249272227287292, -0.047310635447502136, 0.4077761471271515, -56.98037030812389, -6.16739631361851, -115.84333735802369,          0.03955405578017235, -0.013497642241418362, 0.33975949883461, -100.87129330834215, -17.192685528625265, -173.07354634882094,          -0.08517949283123016, 0.00957852229475975, 0.46546608209609985, -90.85270962096058, 0.9315977976503153, 175.2059707654342]
Hgs,Hcs = [],[]for i in range(0,len(hand),6):    Hgs.append(get_matrix_eular_radu(hand[i],hand[i+1],hand[i+2],hand[i+3],hand[i+4],hand[i+5]))    
複製代碼

運行

直接調用剛剛的函數，便可完成標定。

標定結果

手眼標定對數據的要求比較高，必定要標定好相機內參，去掉畸變後的數據來獲取外參。

最後

若是有不懂的同窗能夠加入咱們的魚香ROS技術交流羣(進羣二維碼在後臺菜單欄)。

自我介紹

我是小魚，機器人領域資深玩家，現深圳某獨腳獸機器人算法工程師一枚

初中學習編程，高中開始學習機器人，大學期間打機器人相關比賽實現月入2W+（比賽獎金）

目前在作公衆號，輸出機器人學習指南、論文註解、工做經驗，歡迎你們關注小魚，一塊兒交流技術，學習機器人