突然发现玩机械臂的小伙伴越来越多了。。。再说说手眼标定那些事~

突然发现玩机械臂的小伙伴越来越多了。。。再说说手眼标定那些事~ 虽万人吾往矣 2023-04-14 11:33:23 1102

如果不知道四元数、欧拉角、旋转矩阵是什么的同学建议先看一下台大林沛群老师的机器人学公开课:https://www.bilibili.com/video/av54047883 或者燕山大学公开课,机器人技术。

接下来小鱼来一段段解析代码,手眼标定的输入和输出都很简单,昨天有位同学在群里问,为什么要手眼标定?

其实答案就是不标,不知道相机和机械臂到底啥关系,相机识别到了物品,机械臂不知道到哪里抓。

核心算法函数

小鱼将代码进行了二次的封装,将手眼标定变成了一个只有三四十行的函数。

对算法有兴趣的同学,可以关注小鱼公众号,鱼香ROS,后台回复手眼标定即可获得论文原文及算法详解。

import transforms3d as tfs
import numpy as np
import 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]))
运行
直接调用刚刚的函数,即可完成标定。

手眼标定对数据的要求比较高,一定要标定好相机内参,去掉畸变后的数据来获取外参。

声明:本文内容由易百纳平台入驻作者撰写,文章观点仅代表作者本人,不代表易百纳立场。如有内容侵权或者其他问题,请联系本站进行删除。
红包 点赞 收藏 评论 打赏
评论
0个
内容存在敏感词
手气红包
    易百纳技术社区暂无数据
相关专栏
置顶时间设置
结束时间
删除原因
  • 广告/SPAM
  • 恶意灌水
  • 违规内容
  • 文不对题
  • 重复发帖
打赏作者
易百纳技术社区
虽万人吾往矣
您的支持将鼓励我继续创作!
打赏金额:
¥1易百纳技术社区
¥5易百纳技术社区
¥10易百纳技术社区
¥50易百纳技术社区
¥100易百纳技术社区
支付方式:
微信支付
支付宝支付
易百纳技术社区微信支付
易百纳技术社区
打赏成功!

感谢您的打赏,如若您也想被打赏,可前往 发表专栏 哦~

举报反馈

举报类型

  • 内容涉黄/赌/毒
  • 内容侵权/抄袭
  • 政治相关
  • 涉嫌广告
  • 侮辱谩骂
  • 其他

详细说明

审核成功

发布时间设置
发布时间:
是否关联周任务-专栏模块

审核失败

失败原因
备注
拼手气红包 红包规则
祝福语
恭喜发财,大吉大利!
红包金额
红包最小金额不能低于5元
红包数量
红包数量范围10~50个
余额支付
当前余额:
可前往问答、专栏板块获取收益 去获取
取 消 确 定

小包子的红包

恭喜发财,大吉大利

已领取20/40,共1.6元 红包规则

    易百纳技术社区