机械臂手眼标定ZED相机——眼在手外python、matlab

目录

1.眼在手外原理

2.附上眼在手外求得手眼矩阵的python代码

3.眼在手外标定步骤

1)打印棋盘格

2)得到hand数据

3)得到camera数据

4.运行python得到手眼矩阵



1.眼在手外原理

眼在手外所求的手眼矩阵是基坐标到相机的转换矩阵

2.附上眼在手外求得手眼矩阵的python代码

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为基坐标系下抓夹的位姿,一般从示教器上获取,我用的是ur5机械臂,注意单位mm和rad,三行为三组数据,hand=(x,y,z,rx,ry,rz),同时应将弧度制rad转为角度制°

camera为相机中标定板的位姿,为相机的外参(平移矩阵、旋转矩阵),三行为三组数据,camera=(x,y,z,rx,ry,rz),同时应将弧度制转为角度制

3.眼在手外标定步骤

1)打印棋盘格

为了使标定精度更加准确,棋盘格精度越高越好;角点数越多越好;尽量减少相机中心到标定板的距离(适当小的标定板)等

参考资料:

手眼标定,我的结果显示手和眼相距上千米!手眼标定结果准确率如何提高?_提高手眼标定精度_鱼香ROS的博客-CSDN博客

这里我选择4*4的棋盘格

2)得到hand数据

示教器右侧为hand数据,记录五组,并转化为角度制

3)得到camera数据

将相机固定在距离机械臂基坐标的一定距离的地方,固定相机,记住距离

将棋盘格粘贴在机械臂末端执行器上,用相机拍摄五组棋盘格图片,使用matlab计算相机外参TR,将得到的TR转换为欧拉角

(matlab工具箱使用单目标定,其中得到的T为translationvectors,R为rotationvectors)

(不知道这个棋盘格不正会不会有影响)

matlab标定得到的是旋转向量/旋转矩阵,得到的数据转化为欧拉角,参考下面的链接手眼标定必备------旋转向量转换为旋转矩阵python------罗德里格斯公式Rodrigues_python rodrigues_邸笠佘司的博客-CSDN博客

手眼标定中旋转矩阵转欧拉角------matlab_邸笠佘司的博客-CSDN博客

4.运行python得到手眼矩阵

相关推荐
code bean7 小时前
【工控】线扫相机小结 第四篇
数码相机·工控·工业相机
那就举个栗子!1 天前
多传感器融合slam过程解析【大白话版】
数码相机
xy189901 天前
相机触发模式
数码相机
yunfanleo2 天前
代替Spinnaker 的 POINTGREY工业级相机 FLIR相机 Python编程案例
c++·python·数码相机
摆烂仙君3 天前
《Camera-free Image to PanoramaGeneration with Diffusion Model》论文解析——CamFreeDiff
人工智能·数码相机·计算机视觉
weixin_Todd_Wong20103 天前
【海思Hi3519DV500】双目网络相机套板硬件规划方案
数码相机
isyoungboy3 天前
普通单片机为什么无法直接驱动dcmi接口相机
单片机·嵌入式硬件·数码相机
爱煲汤的夏二3 天前
摄影:相机控色
数码相机
OAK中国_官方4 天前
‘视’不可挡:OAK相机助力无人机智控飞行!
数码相机·无人机
环境感知4 天前
基于MATLAB的激光雷达与相机联合标定原理及实现方法——以标定板为例
开发语言·人工智能·数码相机·算法·matlab