一、原理及流程
机器人的手眼标定原理在本文中不再过多描述，基本流程都是先标定相机的内外参数，然后标定两台相机之间的位置关系，如果相机是可以转动的话，还要标定转台与机械臂之间的关系。
在手眼标定完成后，怎么确定标定结果是否准确呢？传统方法是利用指点验证的办法去进行测试，就是在手眼标定完成后，将棋盘格摆在相机视野范围内，然后双目相机计算出棋盘格角点坐标X、Y、Z，然后把计算的坐标结果，利用坐标系转换至机器人坐标系下X1、Y1、Z1，控制机械臂移动至刚才的X1、Y1、Z1坐标处，看看棋盘格角点与机械臂指点工具末端相差多少，如下图所示。这样本质上没有问题，只是速度会比较慢，而且经验不足的话，末端会撞到标定板。
本文是借助了aruco标定板，这个标定板本身就是可以用于机械臂手眼标定的，在Python里面是有现成的程序的，在Linux里面配置好ROS直接调用就可以
aruco制作的链接：Online ArUco markers generator (chev.me)
aruco手眼标定方法的链接：手眼标定——使用 easy_handeye 和 aruco_马天乐233的博客-CSDN博客_easyhandeye
基于ROS的机械臂手眼标定-Aruco使用与相机标定_鱼香ROS的博客-CSDN博客_aruco标定
Kinect v2 在ros上利用easy_handeye进行手眼标定 - 古月居 (guyuehome.com)
假如说每次相机工作都要转到不同的角度去识别不同的东西，可以在每个相机工作的角度都标定一次手眼关系，到达那个位置就调用哪个位置的标定文件。本文此次介绍的是其中一个位置，就拿初始位置，记作00位，来介绍精度验证原理及流程：
1、手眼标定的的本质就是在进行坐标系变换，存在两种情况（眼在手外、眼在手上），两者的区别是，眼在手外标定结果是相机与机器人基坐标系之间的关系，眼在手上标定结果是相机与机械臂末端之间的关系。
2、了解了基本情况后，如何实现坐标系转换呢，本文所建立的快速精度验证的方法，是建立在机械臂精度满足要求且没有故障的情况下。借助aruco板，通过相机识别aruco板，能够得出aruco板中心在相机坐标系下的坐标（这一步是ROS里面那个标定方法自带的，只要是相机能够看到板子，就会计算出中心坐标，并标记出坐标轴），如图所示，将坐标利用手眼标定关系转换到机器人基坐标系下，即为aruco坐标的测量值
3.怎么得出机器人基坐标系下，aruco中心点的真实坐标呢，这个需要借用机器人软件了，因为机器人软件一般都会有一个功能，就是实时显示机械臂末端点在基坐标系下的坐标，这个坐标也是可以自己写程序获取到的。借助标定工装，利用标定工装三维图里的数据，得到标定工装的实际长度，在机器人软件里面把末端工具的长度加进去就可以了，工装如图所示，根据实际情况自己设计加工。这样就得到了aruco标定板中心的坐标在机器人基座标系下的真实值。
4、需要注意的是，本次方法的前提是：①机器人精度满足要求且没有故障，这个一般都没有问题，应该都是各单位自己正在使用的产品；②双目标定的结果是准确的，目前还是根据重投影误差来初步判断，标定的时候注意光线，焦距等问题。标定板尽量买精度高一点的，这样的话还可以用来测试双目标定是否准确，比如说棋盘格角点间距是30cm，以此为真实值，利用双目识别两个角点，计算出来的间距为测量值，也可以判断双目结果是否准确。
5、怎么实现坐标系转化的过程呢，这一步需要理解下坐标系转换的过程
目前已知的是，相机坐标系下的aruco中心坐标为p_c_a，由标定结果可得机器人基坐标系到相机坐标系的旋转、平移矩阵为 Rjc,Tjc ，那么根据坐标系转换关系，可得aruco中心坐标在机器人基坐标系下的坐标为p_j_a = Rjc * p_c_a + Tjc
二、精度检测程序运行流程
1、打开节点，调用相机实时显示aruco坐标
rostopic echo /aruco_tracking/pose
2、想办法获取到坐标数据，我用的方法比较粗暴，直接录制一段数据，然后把rosbag数据再转成txt，程序去读取txt
rosbag record -O aruco --duration=5 /aruco_tracking/pose
rostopic echo -b aruco.bag /aruco_tracking/pose > aruco.txt
3、精度检测代码，眼在手外，代码如下：
#文件名称auto_verify.py
#功能：快速精度验证
#作者：laoli
#类型： 眼在手外
import pandas
import os
from scipy.spatial.transform import Rotation as R
import numpy as np
import time
import datetime
import socket
import struct
#读取TXT文件，
def read_tablemethod(filename):
data2 = pandas.read_table(filename, header=None, delim_whitespace=True)
return data2
#此处暂时没有用到
def readFile(filepath):
f1 = open(filepath, "r")
nowDir = os.path.split(filepath)[0]
fileName = os.path.split(filepath)[1]
newFileDir = os.path.join(nowDir, "python" + fileName)
# print("nowDir",nowDir)
# print("fileName",fileName)
print("newFileDir",newFileDir)
fnew = open(newFileDir, "w")
content = f1.read()
# s = [i for i in content if (str.isdigit(i) or i == '.' or i == '\n')] 等价于
s = [] # s是个字符list
for i in content:
# 保留数字，小数点、空格与换行符
if (str.isdigit(i) or i == '.' or i == '\n' or i == ''):
s.append(i)
# 将冒号换空格
elif i == ':':
s.append(' ')
s2 = ''.join(s)
fnew.write(s2)
f1.close()
fnew.close()
#此处暂时没有用到
def eachFile(filepath):
pathDir = os.listdir(filepath)
for s in pathDir:
newDir = os.path.join(filepath, s)
if os.path.isfile(newDir):
if os.path.splitext(newDir)[1] == ".txt":
readFile(newDir)
pass
else:
eachFile(newDir)
#读取R、T矩阵数据
def zhuanhuan_rt():
result = []
folder = '/opt/ros/calibration'#手眼标定结果的路径
for f in os.listdir(folder):
if f.endswith('00_rs_left.txt'):
with open(os.path.join(folder, f), 'r') as r:
data4 = r.read().split('\n')
# print("data4", data4)
for i in data4:
result.append(i.split(':')[-1])
# result = map(float, result)
# print("result", result)
# print("list",list(result))
with open('00_rs_left_result.txt','w') as r1:
r1.write('\n'.join([i for i in result]))
lines = open('00_rs_left_result.txt', 'r').readlines()
fp = open('00_rs_left_result.txt', 'w')
for s in lines:
fp.write(s.replace(' ', ''))
fp.close()
return result
#读取aruco中心坐标
def zhuanhuan_xyz():
result = []
folder = '/opt/ros/calibration'#aruco存储结果的路径
for f in os.listdir(folder):
if f.endswith('aruco.txt'):
with open(os.path.join(folder, f), 'r') as r:
data4 = r.read().split('\n')
for i in data4:
result.append(i.split(':')[-1])
# result = map(float, result)
# print("result", result)
# print("list", list(result))
with open('aruco_result.txt','w') as r1:
r1.write('\n'.join([i for i in result]))
lines = open('aruco_result.txt', 'r').readlines()
fp = open('aruco_result.txt', 'w')
for s in lines:
fp.write(s.replace(' ', ''))
fp.close()
return result
#读取机械臂数据，在这里我需要得到的是末端工具的坐标，也就是aruco在机器人基坐标系下的X,Y,Z
def jixiebi_xyz():
PORT = 30003 # The same port as used by the server
HOST = '192.168.x.xx'
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT))
dic = {'MessageSize': 'i', 'Time': 'd', 'q target': '6d', 'qd target': '6d', 'qdd target': '6d',
'I target': '6d',
'M target': '6d', 'q actual': '6d', 'qd actual': '6d', 'I actual': '6d', 'I control': '6d',
'Tool vector actual': '6d', 'TCP speed actual': '6d', 'TCP force': '6d', 'Tool vector target': '6d',
'TCP speed target': '6d', 'Digital input bits': 'd', 'Motor temperatures': '6d', 'Controller Timer': 'd',
'Test value': 'd', 'Robot Mode': 'd', 'Joint Modes': '6d', 'Safety Mode': 'd', 'empty1': '6d',
'Tool Accelerometer values': '3d',
'empty2': '6d', 'Speed scaling': 'd', 'Linear momentum norm': 'd', 'SoftwareOnly': 'd',
'softwareOnly2': 'd', 'V main': 'd',
'V robot': 'd', 'I robot': 'd', 'V actual': '6d', 'Digital outputs': 'd', 'Program state': 'd',
'Elbow position': '3d', 'Elbow velocity': '3d'}
data = s.recv(1108)
# print (data)
names = []
ii = range(len(dic))
for key, i in zip(dic, ii):
fmtsize = struct.calcsize(dic[key])
data1, data = data[0:fmtsize], data[fmtsize:]
fmt = "!" + dic[key]
names.append(struct.unpack(fmt, data1))
dic[key] = dic[key], struct.unpack(fmt, data1)
b = dic["Tool vector actual"]
# print(b)
# print("x: ", b[1][0])
# print("y: ", b[1][1])
# print("z: ", b[1][2])
return b[1][0]*1000, b[1][1]*1000, b[1][2]*1000
if __name__=="__main__":
#将录制转换后的aruco.txt，进一步剔除无用数据，得到aruco_result.txt
result_xyz = zhuanhuan_xyz()
#读取aruco_result.txt中aruco的xyz坐标
data = read_tablemethod('aruco_result.txt')
p_c_a = np.mat([[float(data[0][4])], [float(data[0][5])], [float(data[0][6])]])
print("p_c_a",p_c_a)
#读取标定结果中的旋转向量，注意此处是旋转向量，下面还需要转换成旋转矩阵
result_rt = zhuanhuan_rt()
Rq_a = [result_rt[5], result_rt[6], result_rt[7], result_rt[8]]
print("Rq_a", Rq_a)
#读取平移矩阵
data_t = read_tablemethod('00_rs_left_result.txt')
T = np.mat([[data_t[0][0]], [data_t[0][1]], [data_t[0][2]]])
print("T", T)
#旋转向量转化成旋转矩阵
Rm = R.from_quat(Rq_a)
# print("R", R)
rotation_matrix = Rm.as_matrix()
print('rotation:\n', rotation_matrix)
#查看逆矩阵
R = np.linalg.inv(rotation_matrix)
print('linalg_rotation:\n', R)
#计算机器人基坐标系下，aruco的坐标
pa = rotation_matrix * p_c_a + T
#精度检测，实际就是真实值与测量值求差
print("********************position verify******************************")
print("pa x,y,z ", -pa[0]*1000, -pa[1]*1000, pa[2]*1000)
# print("pa", type(pa[0]))
#在这里，xy坐标取负数的原因是在项目的实际应用时，把机器人的轴系正好旋转了180度
#需要自己手动给补回来，这块当时也是困扰了好久，最后才发现是机器人坐标系定义的问题
a = str(-pa[0]*1000)
b = str(-pa[1]*1000)
c = str(pa[2]*1000)
d = a+b+c
tim1 = datetime.datetime.now()
#转换成字符串
a1 = float(-pa[0] * 1000)
b1 = float(-pa[1] * 1000)
c1 = float(pa[2] * 1000)
#读取机械臂上末端的数据，也就aruco坐标真实值
jx, jy, jz = jixiebi_xyz()
print(" jx, jy ,jz", jx, jy , jz)
jx1 = str(jx)
jy1 = str(jy)
jz1 = str(jz)
dj = jx1 + ',' + jy1 + ',' + jz1
#计算误差
pa1 = (a1 - jx, b1 - jy, c1 - jz)
print("diff value", pa1)
a3 = str(pa1[0])
b3 = str(pa1[1])
c3 = str(pa1[2])
d3 = a3 + ',' + b3 + ',' + c3
e3 = a3 + ' ' + b3 + ' ' + c3
#保存结果
with open ( '/opt/ros/00_verify_result.txt','a+') as f:
[f.write(str(tim1) + d + '[' + dj + ']' + '[' + d3 + ']' + '\n') for item in pa[0]]
f.close()
#保存结果另外一个文件中，如果满足精度要求，数值置1，否则置零
#此处的目的是为了连续精度验证，在a位置精度验证完成后，检测是或合格，合格则转入b位置
with open ( '/opt/ros/00_rs_left_verify_result_single.txt','w') as f:
if ((a1 - jx) < 1000 and (b1 - jy)< 1000 and (c1 - jz)< 1000):
[f.write(e3 + ' ' + '1') for item in pa[0]]
f.close()
print("calib 00 position is ok ")
else:
print("calib position 00 again")
[f.write(e3 + ' ' + '0') for item in pa[0]]
f.close()
4 、运行脚本如下：
#!/bin/bash
#source /opt/ros/melodic/setup.bash
echo "work start !"
#录制节点信息
gnome-terminal -t "rostopic" -x bash -c "cd /opt/ros && rostopic echo /aruco_tracking/pose;exec bash"
gnome-terminal -t "rosbag" -x bash -c "cd /opt/ros && rosbag record -O aruco --duration=5 /aruco_tracking/pose ;exec bash"
sleep 8
gnome-terminal -t "txt" -x bash -c "rostopic echo -b aruco.bag /aruco_tracking/pose > aruco.txt ;exec bash"
sleep 3
gnome-terminal -t "verify_hand2eye_auto" -x bash -c "export PATH=/home/ros/anaconda3/bin:$PATH && source activate && conda activate net && python auto_verify.py;exec bash"
5、精度检测代码，眼在手上，代码如下：
#眼在手上，快速精度验证程序
#与眼在手外不同的是，眼在手上需要读取两个机械臂数据，一个是aruco坐标，一个将手眼标定结果，转换至基坐标系
import pandas
import os
from scipy.spatial.transform import Rotation as R
import numpy as np
import time
import datetime
import socket
import struct
import cv2
def read_tablemethod(filename):
data2 = pandas.read_table(filename, header=None, delim_whitespace=True)
return data2
def readFile(filepath):
f1 = open(filepath, "r")
nowDir = os.path.split(filepath)[0]
fileName = os.path.split(filepath)[1]
newFileDir = os.path.join(nowDir, "python" + fileName)
# print("nowDir",nowDir)
# print("fileName",fileName)
print("newFileDir",newFileDir)
fnew = open(newFileDir, "w")
content = f1.read()
# s = [i for i in content if (str.isdigit(i) or i == '.' or i == '\n')] 等价于
s = [] # s是个字符list
for i in content:
# 保留数字，小数点、空格与换行符
if (str.isdigit(i) or i == '.' or i == '\n' or i == ''):
s.append(i)
# 将冒号换空格
elif i == ':':
s.append(' ')
s2 = ''.join(s)
fnew.write(s2)
f1.close()
fnew.close()
def eachFile(filepath):
pathDir = os.listdir(filepath)
for s in pathDir:
newDir = os.path.join(filepath, s)
if os.path.isfile(newDir):
if os.path.splitext(newDir)[1] == ".txt":
readFile(newDir)
pass
else:
eachFile(newDir)
def zhuanhuan_rt():
result = []
folder = '/opt/ros/calibration'
for f in os.listdir(folder):
if f.endswith('eyeinhand_right.txt'):
with open(os.path.join(folder, f), 'r') as r:
data4 = r.read().split('\n')
for i in data4:
result.append(i.split(':')[-1])
with open('right_arm_result.txt','w') as r1:
r1.write('\n'.join([i for i in result]))
lines = open('right_arm_result.txt', 'r').readlines()
fp = open('right_arm_result.txt', 'w')
for s in lines:
fp.write(s.replace(' ', ''))
fp.close()
return result
def zhuanhuan_xyz():
result = []
folder = '/opt/ros/calibration'
for f in os.listdir(folder):
if f.endswith('aruco.txt'):
with open(os.path.join(folder, f), 'r') as r:
data4 = r.read().split('\n')
for i in data4:
result.append(i.split(':')[-1])
# result = map(float, result)
# print("result", result)
# print("list", list(result))
with open('aruco_result.txt','w') as r1:
r1.write('\n'.join([i for i in result]))
lines = open('aruco_result.txt', 'r').readlines()
fp = open('aruco_result.txt', 'w')
for s in lines:
fp.write(s.replace(' ', ''))
fp.close()
return result
def jixiebi_rightarm_xyz():
PORT = 30003 # The same port as used by the server
HOST = '192.168.x.xx'
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT))
dic = {'MessageSize': 'i', 'Time': 'd', 'q target': '6d', 'qd target': '6d', 'qdd target': '6d',
'I target': '6d',
'M target': '6d', 'q actual': '6d', 'qd actual': '6d', 'I actual': '6d', 'I control': '6d',
'Tool vector actual': '6d', 'TCP speed actual': '6d', 'TCP force': '6d', 'Tool vector target': '6d',
'TCP speed target': '6d', 'Digital input bits': 'd', 'Motor temperatures': '6d', 'Controller Timer': 'd',
'Test value': 'd', 'Robot Mode': 'd', 'Joint Modes': '6d', 'Safety Mode': 'd', 'empty1': '6d',
'Tool Accelerometer values': '3d',
'empty2': '6d', 'Speed scaling': 'd', 'Linear momentum norm': 'd', 'SoftwareOnly': 'd',
'softwareOnly2': 'd', 'V main': 'd',
'V robot': 'd', 'I robot': 'd', 'V actual': '6d', 'Digital outputs': 'd', 'Program state': 'd',
'Elbow position': '3d', 'Elbow velocity': '3d'}
data = s.recv(1108)
# print (data)
names = []
ii = range(len(dic))
for key, i in zip(dic, ii):
fmtsize = struct.calcsize(dic[key])
data1, data = data[0:fmtsize], data[fmtsize:]
fmt = "!" + dic[key]
names.append(struct.unpack(fmt, data1))
dic[key] = dic[key], struct.unpack(fmt, data1)
b = dic["Tool vector actual"]
print("b",b)
# print("x: ", b[1][0])
# print("y: ", b[1][1])
# print("z: ", b[1][2])
return b[1][0]*1000, b[1][1]*1000, b[1][2]*1000, b[1][3], b[1][4], b[1][5]
def jixiebi_leftarm_xyz():
PORT = 30003 # The same port as used by the server
HOST = '192.168.xx.xx'
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT))
dic = {'MessageSize': 'i', 'Time': 'd', 'q target': '6d', 'qd target': '6d', 'qdd target': '6d',
'I target': '6d',
'M target': '6d', 'q actual': '6d', 'qd actual': '6d', 'I actual': '6d', 'I control': '6d',
'Tool vector actual': '6d', 'TCP speed actual': '6d', 'TCP force': '6d', 'Tool vector target': '6d',
'TCP speed target': '6d', 'Digital input bits': 'd', 'Motor temperatures': '6d', 'Controller Timer': 'd',
'Test value': 'd', 'Robot Mode': 'd', 'Joint Modes': '6d', 'Safety Mode': 'd', 'empty1': '6d',
'Tool Accelerometer values': '3d',
'empty2': '6d', 'Speed scaling': 'd', 'Linear momentum norm': 'd', 'SoftwareOnly': 'd',
'softwareOnly2': 'd', 'V main': 'd',
'V robot': 'd', 'I robot': 'd', 'V actual': '6d', 'Digital outputs': 'd', 'Program state': 'd',
'Elbow position': '3d', 'Elbow velocity': '3d'}
data = s.recv(1108)
# print (data)
names = []
ii = range(len(dic))
for key, i in zip(dic, ii):
fmtsize = struct.calcsize(dic[key])
data1, data = data[0:fmtsize], data[fmtsize:]
fmt = "!" + dic[key]
names.append(struct.unpack(fmt, data1))
dic[key] = dic[key], struct.unpack(fmt, data1)
b = dic["Tool vector actual"]
print("b",b)
# print("x: ", b[1][0])
# print("y: ", b[1][1])
# print("z: ", b[1][2])
return b[1][0]*1000, b[1][1]*1000, b[1][2]*1000, b[1][3], b[1][4], b[1][5]
if __name__=="__main__":
result_xyz = zhuanhuan_xyz()
data = read_tablemethod('aruco_result.txt')
p_c_a = np.mat([[float(data[0][4])], [float(data[0][5])], [float(data[0][6])]])
print("p_c_a",p_c_a)
result_rt = zhuanhuan_rt()
Rq_a = [result_rt[5], result_rt[6], result_rt[7], result_rt[8]]
print("Rq_a", Rq_a)
data_t = read_tablemethod('right_arm_result.txt')
T = np.mat([[data_t[0][0]], [data_t[0][1]], [data_t[0][2]]])
print("T", T)
Rm = R.from_quat(Rq_a)
rotation_matrix = Rm.as_matrix()
print('rotation:\n', rotation_matrix)
R = np.linalg.inv(rotation_matrix)
print('linalg_rotation:\n', R)
pa = rotation_matrix * p_c_a + T
pa1 = (-pa[0]*1000, -pa[1]*1000, pa[2]*1000)
print("********************check arm camera ******************************")
print("pa x,y,z ", -pa[0]*1000, -pa[1]*1000, pa[2]*1000)
# print("pa", type(pa[0]))
a = str(-pa[0]*1000)
b = str(-pa[1]*1000)
c = str(pa[2]*1000)
d= a+b+c
tim1 = datetime.datetime.now()
a1 = float(-pa[0] * 1000)
b1 = float(-pa[1] * 1000)
c1 = float(pa[2] * 1000)
jrx, jry, jrz, RX, RY, RZ = jixiebi_rightarm_xyz()
print(" jrx, jry ,jrz", jrx, jry, jrz)
jx1 = str(jrx)
jy1 = str(jry)
jz1 = str(jrz)
dj = jx1 + ',' + jy1 + ',' + jz1
pa1 = (a1 - jrx, b1 - jry, c1 - jrz)
print("diff value", pa1)
a3 = str(pa1[0])
b3 = str(pa1[1])
c3 = str(pa1[2])
d3 = a3 + ',' + b3 + ',' + c3
e3 = a3 + ' ' + b3 + ' ' + c3
A = (RX, RY, RZ)
Rr, j = cv2.Rodrigues(A)
print("A", A)
print("Rr", Rr)
TtcpB = np.mat([[jrx/1000],[jry/1000],[jrz/1000]])
print("TtcpB", TtcpB)
#先转移到基坐标系下
parB = Rr * pa + TtcpB
#左右臂之间的关系
Rrf = np.mat([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
Trf = np.mat([[-0.522], [0], [0]])
#再转移到左臂
palB = Rrf * parB + Trf
print("pa2", palB)
palBx = str(palB[0]* 1000)
palBy = str(palB[1]* 1000)
palBz = str(palB[2]* 1000)
PalB = palBx + ',' + palBy + ',' + palBz
#读取左臂真实数据
jlx, jly, jlz, RlX, RlY, RlZ = jixiebi_leftarm_xyz()
print(" jlx, jly ,jlz", jlx, jly, jlz)
jlx1 = str(jlx)
jly1 = str(jly)
jlz1 = str(jlz)
dlj = jlx1 + ',' + jly1 + ',' + jlz1
al = float(palB[0] * 1000)
bl = float(palB[1] * 1000)
cl = float(palB[2] * 1000)
#计算误差
value = (al - jlx, bl - jly, cl - jlz)
print("diff value", value)
vx = str(value[0])
vy = str(value[1])
vz = str(value[2])
V = vx + ' ' + vy + ' ' + vz
#保存结果
with open(
'/opt/ros/arm_right_verify_result.txt',
'a+') as f:
[f.write(str(tim1) + '['+ PalB + ']'+ '[' + dlj + ']' + '[' + V + ']' + '\n') for item in pa[0]]
f.close()
with open(
'/opt/ros/arm_right_verify_result_single.txt',
'w') as f:
if ((al - jlx) < 1000 and (bl - jly) < 1000 and (cl - jlz) < 1000):
[f.write(V + ' ' + '1') for item in pa[0]]
f.close()
print("calib arm tool is ok ")
else:
print("calib arm tool again")
[f.write(e3 + ' ' + '0') for item in pa[0]]
f.close()