机器人手眼标定快速精度验证方法
一、原理及流程
机器人的手眼标定原理在本文中不再过多描述，基本流程都是先标定相机的内外参数，然后标定两台相机之间的位置关系，如果相机是可以转动的话，还要标定转台与机械臂之间的关系。
在手眼标定完成后，怎么确定标定结果是否准确呢？传统方法是利用指点验证的办法去进行测试，就是在手眼标定完成后，将棋盘格摆在相机视野范围内，然后双目相机计算出棋盘格角点坐标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、怎么实现坐标系转化的过程呢，这一步需要理解下坐标系转换的过程
姿态矩阵表达：
- Rij：表示从i坐标系到j坐标系的旋转矩阵，注意是从第一个小写字母表达的坐标系到第二个小写字母表达的坐标系；
- Rik=Rjk*Rij：表示坐标系i到坐标系k的姿态矩阵传递链，中间通过坐标系j过度，注意矩阵相乘顺序；
目前已知的是，相机坐标系下的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()