基于FANUC工业机器人的坐标系转换、多视角拼接与三维重建

0.简介

• 总体任务：机械臂末端安装三维相机，绕着工件进行拍摄，并在计算机中将每次拍摄的点云合并在同一个坐标系下，从而获得更加完整全面的点云。
• 机械臂：FANAUC
• 相机：梅卡曼德
• 技术方案：使用相机外参、机械臂位姿进行坐标系转换，将不同视角点云坐标系都转换到机器人基座（基于ICP或深度学习的点云匹配也可以实现类似效果，但实际应用鲁棒性不够，对视角间点云的重叠度要求较高或是难以获取相似度高的大量训练数据）

1.多位姿点云拍摄与读取

• 机械臂移动拍摄工件不同视角，记录下每个视角的点云、机械臂位姿
• 相关数据
• 
• 点云读取及简单处理可以参考这里
• 注意：梅卡曼德拍摄后点云的xyz坐标单位是m，而FANUC等转换矩阵的单位是mm，所以要注意单位统一

pcd = o3d.io.read_point_cloud('datasFANUC/' + 'point_cloud_00000' + '.ply')
pcd_np = np.array(pcd.points) * 1000
pcd.points = o3d.utility.Vector3dVector(pcd_np)
pcd.paint_uniform_color([0, 1.0, 0])
o3d.visualization.draw_geometries([pcd])

2.手眼标定的外参转换矩阵

• 通过梅卡曼德官方软件与FANUC配合完成相机内外参的标定，外参标定结果如下（使用和FANUC相同的表示方式）：

x,y,z,w,p,r = 70.5, 269.927, -33.947, -0.85, 0.78, 147.95

3.FANUC机械臂转换矩阵

• FANUC工业机器人采用的是定角旋转方式，法兰（机械臂末端坐标系）到机械臂基座坐的相对位置由X- Y-Z,W-P-R表示，W表示绕X轴旋转的角度；P表示绕Y轴旋转的角度；R表示绕Z轴旋转的角度；它们的旋转方向由右手定则判断。（参考资料）
• 定角旋转：定角旋转与欧拉角旋转类似，只不过定角旋转的转轴一直是最原始的坐标系转轴（该轴不随着旋转而变化）
• 三次绕固定轴旋转（定角旋转）的最终姿态和以相反顺序三次绕运动坐标轴旋转（欧拉角旋转）的最终姿态相同（机器人学——姿态描述方法（欧拉角，固定角，D-H法，绕定轴旋转））
• 机器人默认显示旋转顺序是WPR，即依次绕XYZ轴进行旋转，对应的旋转矩阵为RZRYRX
• 绕定轴旋转的相关程序如下：

'''
旋转矩阵相关
'''
def Rx(theta):
    '''
    绕X轴旋转矩阵
    '''
    RX = np.array([[1, 0, 0],
                   [0, math.cos(theta), -1*math.sin(theta)],
                   [0, math.sin(theta), math.cos(theta)]])
    return RX
def Ry(theta):
    '''
    绕Y轴旋转矩阵
    '''
    RY = np.array([[math.cos(theta), 0, math.sin(theta)],
                   [0, 1, 0],
                   [-1*math.sin(theta), 0, math.cos(theta)]])
    return RY
def Rz(theta):
    '''
    绕Z轴旋转矩阵
    '''
    RZ = np.array([[math.cos(theta), -1*math.sin(theta), 0],
                   [math.sin(theta), math.cos(theta), 0],
                   [0, 0, 1]])
    return RZ

• FANUC机械臂转换矩阵相关程序：

'''
机械臂转换矩阵相关
'''
def WPR2R(w,p,r):
    '''
    定角旋转WPR转为旋转矩阵(角度)
    '''
    R = Rz(r/180*math.pi)@Ry(p/180*math.pi)@Rx(w/180*math.pi)
    return R
def FANUC2trans(x,y,z,w,p,r):
    '''
    FANUC转换矩阵(mm,角度，wpr)
    '''
    t = np.zeros((4, 4))
    t[0:3, 0:3] = WPR2R(w,p,r)
    t[:,3] = x,y,z,1
    t[3,0:3] = 0,0,0
    return t

4.多视角拼接

• 初始化一个点云pcd_all用来保存完成坐标变换后的点云
• 为了与转换矩阵相匹配，为点云添加一列：

pcd_np = np.concatenate((pcd_np, np.expand_dims(np.ones(pcd_np.shape[0]), axis=1)), axis=1)

• 点云左乘相机外参转换矩阵，再左乘机械臂转换矩阵，即得到点云在机械臂基座的坐标
• 最终效果：
  

5.完整代码

import numpy as np
import math
import open3d as o3d
from Transformations import xyzrpw_to_H

'''
旋转矩阵相关
'''
def Rx(theta):
    '''
    绕X轴旋转矩阵
    '''
    RX = np.array([[1, 0, 0],
                   [0, math.cos(theta), -1*math.sin(theta)],
                   [0, math.sin(theta), math.cos(theta)]])
    return RX
def Ry(theta):
    '''
    绕Y轴旋转矩阵
    '''
    RY = np.array([[math.cos(theta), 0, math.sin(theta)],
                   [0, 1, 0],
                   [-1*math.sin(theta), 0, math.cos(theta)]])
    return RY
def Rz(theta):
    '''
    绕Z轴旋转矩阵
    '''
    RZ = np.array([[math.cos(theta), -1*math.sin(theta), 0],
                   [math.sin(theta), math.cos(theta), 0],
                   [0, 0, 1]])
    return RZ

'''
机械臂转换矩阵相关
'''
def WPR2R(w,p,r):
    '''
    定角旋转WPR转为旋转矩阵(角度)
    '''
    R = Rz(r/180*math.pi)@Ry(p/180*math.pi)@Rx(w/180*math.pi)
    return R
def FANUC2trans(x,y,z,w,p,r):
    '''
    FANUC转换矩阵(mm,角度，wpr)
    '''
    t = np.zeros((4, 4))
    t[0:3, 0:3] = WPR2R(w,p,r)
    t[:,3] = x,y,z,1
    t[3,0:3] = 0,0,0
    return t

'''
相机外参
'''
def get_waican(x,y,z,rx,ry,rz):
    '''
    获取外参转换矩阵，mm，角度，欧拉角xyz
    '''
    R = Rx(rx/180*math.pi)@Ry(ry/180*math.pi)@Rz(rz/180*math.pi)
    t = np.zeros((4, 4))
    t[0:3, 0:3] = R
    t[:, 3] = x, y, z, 1
    t[3, 0:3] = 0, 0, 0
    return t

if __name__ == '__main__':

    root = 'datasFANUC'

    #外参转换矩阵
    # t_waican = get_waican(70.609, 264.977, -33.942, -0.80, 0.73, 147.95+10)
    t_waican = FANUC2trans(70.5, 269.927, -33.947, -0.85, 0.78, 147.95)
    print(t_waican)
    #t_waican = xyzrpw_to_H(np.array([70.5, 269.927, -33.947, -0.85, 0.78, 147.95]))
    print('外参转换矩阵：')
    print(t_waican)

    '''
    文本文件读取机械臂姿态
    索引,文件名（无后缀）,X,Y,Z(mm),W,P,R(角度)
    '''
    flag = 1
    robot_data = root + '/datas.txt'
    f = open(robot_data)
    for line in f.readlines():
        line = line.strip('\n').split(',')
        print('索引：', line[0])
        file_data = line[1]
        print('处理文件：', file_data)
        # 转换矩阵
        line = line[2:]
        line = [float(i) for i in line]
        x,y,z,w,p,r = line
        t_robot = FANUC2trans(x,y,z,w,p,r)
        #print('机器人转换矩阵：', t_robot)
        #t_robot = xyzrpw_to_H(np.array([x,y,z,w,p,r]))
        t = t_robot@t_waican
        print(t)

        # 读取点云
        pcd = o3d.io.read_point_cloud(root + '/' + file_data + '.ply')
        pcd.paint_uniform_color([1.0, 0, 0])
        # o3d.visualization.draw_geometries([pcd])
        pcd_np = np.array(pcd.points) * 1000
        pcd_np = np.concatenate((pcd_np, np.expand_dims(np.ones(pcd_np.shape[0]), axis=1)), axis=1)
        #print(pcd_np.shape)
        pcd_np = pcd_np.T
        pcd_np = t@pcd_np
        pcd_np = pcd_np.T



        # 使用第一个点云初始化最终点云
        if flag:
            pcd_all = pcd_np
            flag = 0
        else:
            pcd_all = np.concatenate((pcd_all, pcd_np))
            #print(pcd_all.shape)
            #print(pcd_all)
            #break

    pcd_show = o3d.geometry.PointCloud()
    pcd_show.points = o3d.utility.Vector3dVector(pcd_all[:, :3])
    pcd_show.paint_uniform_color([1.0, 0, 0])

    # zero = o3d.geometry.PointCloud()
    # zero.points = o3d.utility.Vector3dVector(np.zeros((1, 3)))
    # zero.paint_uniform_color([0, 1.0, 0])

    #pcd = o3d.io.read_point_cloud('datasFANUC/' + 'point_cloud_00000' + '.ply')
    #pcd_np = np.array(pcd.points) * 1000
    #pcd.points = o3d.utility.Vector3dVector(pcd_np)
    #pcd.paint_uniform_color([0, 1.0, 0])
    o3d.visualization.draw_geometries([pcd_show])
    # zero,

6.调包获得转换矩阵

• Transformations下载地址

from Transformations import xyzrpw_to_H
t_waican = xyzrpw_to_H(np.array([70.5, 269.927, -33.947, -0.85, 0.78, 147.95]))

• 注意：包里使用的是欧拉角旋转，计算顺序和FANUC的定角旋转是相反的，所以使用的是xyzrpw_to_H函数而不是xyzwpr_to_H