小鱼发现玩机械臂的小姐姐愈来愈多了。。。再说说手眼标定那些事~

你们好，我是小鱼。最近小鱼发现玩机械臂的小姐姐愈来愈多了，由于又有小姐姐找小鱼请教机械臂手眼标定的事情，因此小鱼今天为了小姐姐们优化一下本身以前写的代码。

以前小鱼以前手撸过tsai的手眼标定算法，但对代码没有作过多的解释，致使有些同窗用起来有些吃力，因此今天给你们讲讲小鱼写的手眼标定代码怎么使用。

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

接下来小鱼来一段段解析代码，手眼标定的输入和输出都很简单，昨天有位同窗在群里问，为何要手眼标定？

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

核心算法函数

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

对算法有兴趣的同窗，能够关注小鱼公众号，鱼香ROS，后台回复手眼标定便可得到论文原文及算法详解。

import transforms3d as tfsimport numpy as npimport 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]))    
复制代码

运行

直接调用刚刚的函数，便可完成标定。

标定结果

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

最后

若是有不懂的同窗能够加入咱们的鱼香ROS技术交流群(进群二维码在后台菜单栏)。

自我介绍

我是小鱼，机器人领域资深玩家，现深圳某独脚兽机器人算法工程师一枚

初中学习编程，高中开始学习机器人，大学期间打机器人相关比赛实现月入2W+（比赛奖金）

目前在作公众号，输出机器人学习指南、论文注解、工做经验，欢迎你们关注小鱼，一块儿交流技术，学习机器人