目錄
1.眼在手外原理
2.附上眼在手外求得手眼矩陣的python代碼
3.眼在手外標(biāo)定步驟
1）打印棋盤格
2）得到hand數(shù)據(jù)
3）得到camera數(shù)據(jù)
4.運(yùn)行python得到手眼矩陣
1.眼在手外原理
?
眼在手外所求的手眼矩陣是基坐標(biāo)到相機(jī)的轉(zhuǎn)換矩陣
2.附上眼在手外求得手眼矩陣的python代碼
#!/usr/bin/env python
# coding: utf-8
import transforms3d as tfs
import numpy as np
import math
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
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]])
# 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]
hand = [
# -0.05448,-0.15018,0.06552,89.61059916,-2.119943842,-1.031324031,
# -0.10149,-0.23025,0.04023,96.7725716,6.187944187,5.328507495,
# -0.10114,-0.2207,0.04853,97.00175472,5.729577951,1.375098708 毫米單位
# -54.48, -150.18, 65.52, 89.61059916, -2.119943842, -1.031324031,
# -101.49,-230.25, 40.23, 96.7725716, 6.187944187, 5.328507495,
# -101.14,-220.7 , 48.53, 97.00175472, 5.729577951, 1.375098708
# -122.12, -323.09, -206.86, 82.62051406, -7.161972439 ,1.948056503,
# -96.54, -324.53, -215.59, 83.70913387, -7.734930234 ,7.276563998,
# -116.41, -325.50, -202.05, 81.18811957, -7.505747116 ,6.187944187
# zed
-54.29, -95.24, 199.74, 80.45, -5.88, 8.8,
-54.28, -95.25, 199.73, 80.66, -6.62, 18.13,
-54.3, -95.27, 199.75, 81.57, -0.76, 13.05
]
# 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]
camera = [
# -0.0794887,-0.0812433,0.0246,0.0008,0.0033,0.0182,
# -0.078034,-0.0879632,0.4881494,-0.1085,0.0925,-0.1569,
# -0.1086702,-0.0881681,0.4240367,-0.1052,0.1251,-0.1124,
# -79.4887, -81.2433, 24.6, 0.0008, 0.0033, 0.0182,
# -78.034, -87.9632, 488.1494, -0.1085, 0.0925, -0.1569,
# -108.6702, -88.1681, 424.0367, -0.1052, 0.1251, -0.1124,
# 23.22020448 ,-69.45610195 ,370.5620915 ,0.2530 ,-0.0707, -1.5724,
# 46.8704669 ,-60.19912413 ,263.9729574 ,0.1473 ,-0.1117 ,-1.6090,
# 53.92881454 ,-39.52795178 ,453.4331562 ,0.2702 ,-0.1256, -1.6270,
-174.01022, 1797.687613, 2425.313638, 1.2710, 0.1238, 0.0033,
-175.3384083, 260.4775862, 1349.136325, 1.5191, 0.0664, 0.0058,
123.6753385, -109.6917624, 695.5448714 , 0.4651, -0.1328, -0.2488
]
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],))
Hcs.append(
get_matrix_eular_radu(camera[i], camera[i + 1], camera[i + 2], camera[i + 3], camera[i + 4], camera[i + 5]))
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, )
print(tfs.affines.compose(Tcg, np.squeeze(Rcg), [1, 1, 1]))
其中：
????????hand為基坐標(biāo)系下抓夾的位姿，一般從示教器上獲取，我用的是ur5機(jī)械臂，注意單位mm和rad，三行為三組數(shù)據(jù)，hand=(x,y,z,rx,ry,rz)，同時應(yīng)將弧度制rad轉(zhuǎn)為角度制°
????????camera為相機(jī)中標(biāo)定板的位姿，為相機(jī)的外參（平移矩陣、旋轉(zhuǎn)矩陣），三行為三組數(shù)據(jù)，camera=(x,y,z,rx,ry,rz)，同時應(yīng)將弧度制轉(zhuǎn)為角度制
3.眼在手外標(biāo)定步驟
1）打印棋盤格
為了使標(biāo)定精度更加準(zhǔn)確，棋盤格精度越高越好；角點數(shù)越多越好；盡量減少相機(jī)中心到標(biāo)定板的距離（適當(dāng)小的標(biāo)定板）等
參考資料：
手眼標(biāo)定，我的結(jié)果顯示手和眼相距上千米！手眼標(biāo)定結(jié)果準(zhǔn)確率如何提高？_提高手眼標(biāo)定精度_魚香ROS的博客-CSDN博客
這里我選擇4*4的棋盤格
2）得到hand數(shù)據(jù)
示教器右側(cè)為hand數(shù)據(jù)，記錄五組，并轉(zhuǎn)化為角度制
3）得到camera數(shù)據(jù)
將相機(jī)固定在距離機(jī)械臂基坐標(biāo)的一定距離的地方，固定相機(jī)，記住距離
將棋盤格粘貼在機(jī)械臂末端執(zhí)行器上，用相機(jī)拍攝五組棋盤格圖片，使用matlab計算相機(jī)外參TR，將得到的TR轉(zhuǎn)換為歐拉角
（matlab工具箱使用單目標(biāo)定，其中得到的T為translationvectors，R為rotationvectors）
?（不知道這個棋盤格不正會不會有影響）
matlab標(biāo)定得到的是旋轉(zhuǎn)向量/旋轉(zhuǎn)矩陣，得到的數(shù)據(jù)轉(zhuǎn)化為歐拉角，參考下面的鏈接手眼標(biāo)定必備——旋轉(zhuǎn)向量轉(zhuǎn)換為旋轉(zhuǎn)矩陣python——羅德里格斯公式Rodrigues_python rodrigues_邸笠佘司的博客-CSDN博客
手眼標(biāo)定中旋轉(zhuǎn)矩陣轉(zhuǎn)歐拉角——matlab_邸笠佘司的博客-CSDN博客
?
文章來源：http://www.zghlxwxcb.cn/news/detail-673179.html
4.運(yùn)行python得到手眼矩陣
?文章來源地址http://www.zghlxwxcb.cn/news/detail-673179.html
到了這里，關(guān)于機(jī)械臂手眼標(biāo)定ZED相機(jī)——眼在手外python、matlab的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
