python 机械臂 路径规划 python机械手标定

1.参数说明

calibrateHandeye()
参数描述如下：R_gripper2base，t_gripper2base是机械臂抓手相对于机器人基坐标系的旋转矩阵与平移向量，需要通过机器人运动控制器或示教器读取相关参数转换计算得到；R_target2cam
， t_target2cam 是标定板相对于双目相机的齐次矩阵，在进行相机标定时可以求取得到（calibrateCamera()得到）,
也可以通过solvePnP()单独求取相机外参数获得 ； R_cam2gripper ，
t_cam2gripper是求解的手眼矩阵分解得到的旋转矩阵与平移矩阵；OpenCV实现了5种方法求取手眼矩阵 ， Tsai两步法速度最快。

2.引用代码

import cv2
import numpy as np
import glob
from math import *
import pandas as pd
import os

K=np.array([[4283.95148301687,-0.687179973528103,2070.79900757240],
            [0,4283.71915784510,1514.87274457919],
            [0,0,1]],dtype=np.float64)#大恒相机内参
chess_board_x_num=11#棋盘格x方向格子数
chess_board_y_num=8#棋盘格y方向格子数
chess_board_len=10#单位棋盘格长度,mm


#用于根据欧拉角计算旋转矩阵
def myRPY2R_robot(x, y, z):
    Rx = np.array([[1, 0, 0], [0, cos(x), -sin(x)], [0, sin(x), cos(x)]])
    Ry = np.array([[cos(y), 0, sin(y)], [0, 1, 0], [-sin(y), 0, cos(y)]])
    Rz = np.array([[cos(z), -sin(z), 0], [sin(z), cos(z), 0], [0, 0, 1]])
    R = Rz@Ry@Rx
    return R

#用于根据位姿计算变换矩阵
def pose_robot(x, y, z, Tx, Ty, Tz):
    thetaX = x / 180 * pi
    thetaY = y / 180 * pi
    thetaZ = z / 180 * pi
    R = myRPY2R_robot(thetaX, thetaY, thetaZ)
    t = np.array([[Tx], [Ty], [Tz]])
    RT1 = np.column_stack([R, t])  # 列合并
    RT1 = np.row_stack((RT1, np.array([0,0,0,1])))
    # RT1=np.linalg.inv(RT1)
    return RT1

#用来从棋盘格图片得到相机外参
def get_RT_from_chessboard(img_path,chess_board_x_num,chess_board_y_num,K,chess_board_len):
    '''
    :param img_path: 读取图片路径
    :param chess_board_x_num: 棋盘格x方向格子数
    :param chess_board_y_num: 棋盘格y方向格子数
    :param K: 相机内参
    :param chess_board_len: 单位棋盘格长度,mm
    :return: 相机外参
    '''
    img=cv2.imread(img_path)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    size = gray.shape[::-1]
    ret, corners = cv2.findChessboardCorners(gray, (chess_board_x_num, chess_board_y_num), None)
    # print(corners)
    corner_points=np.zeros((2,corners.shape[0]),dtype=np.float64)
    for i in range(corners.shape[0]):
        corner_points[:,i]=corners[i,0,:]
    # print(corner_points)
    object_points=np.zeros((3,chess_board_x_num*chess_board_y_num),dtype=np.float64)
    flag=0
    for i in range(chess_board_y_num):
        for j in range(chess_board_x_num):
            object_points[:2,flag]=np.array([(11-j-1)*chess_board_len,(8-i-1)*chess_board_len])
            flag+=1
    # print(object_points)

    retval,rvec,tvec  = cv2.solvePnP(object_points.T,corner_points.T, K, distCoeffs=None)
    # print(rvec.reshape((1,3)))
    # RT=np.column_stack((rvec,tvec))
    RT=np.column_stack(((cv2.Rodrigues(rvec))[0],tvec))
    RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
    # RT=pose(rvec[0,0],rvec[1,0],rvec[2,0],tvec[0,0],tvec[1,0],tvec[2,0])
    # print(RT)

    # print(retval, rvec, tvec)
    # print(RT)
    # print('')
    return RT

folder = r"calib"#棋盘格图片存放文件夹
# files = os.listdir(folder)
# file_num=len(files)
# RT_all=np.zeros((4,4,file_num))

# print(get_RT_from_chessboard('calib/2.bmp', chess_board_x_num, chess_board_y_num, K, chess_board_len))
'''
这个地方很奇怪的特点，有些棋盘格点检测得出来，有些检测不了，可以通过函数get_RT_from_chessboard的运行时间来判断
'''
good_picture=[1,3,5,6,7,8,10,11,12,13,14,15]#存放可以检测出棋盘格角点的图片
# good_picture=[1,3,10,11,12]
file_num=len(good_picture)

#计算board to cam 变换矩阵
R_all_chess_to_cam_1=[]
T_all_chess_to_cam_1=[]
for i in good_picture:
    # print(i)
    image_path=folder+'/'+str(i)+'.bmp'
    RT=get_RT_from_chessboard(image_path, chess_board_x_num, chess_board_y_num, K, chess_board_len)

    # RT=np.linalg.inv(RT)

    R_all_chess_to_cam_1.append(RT[:3,:3])
    T_all_chess_to_cam_1.append(RT[:3, 3].reshape((3,1)))
# print(T_all_chess_to_cam.shape)

#计算end to base变换矩阵
file_address='calib/机器人基坐标位姿.xlsx'#从记录文件读取机器人六个位姿
sheet_1 = pd.read_excel(file_address)
R_all_end_to_base_1=[]
T_all_end_to_base_1=[]
# print(sheet_1.iloc[0]['ax'])
for i in good_picture:
    # print(sheet_1.iloc[i-1]['ax'],sheet_1.iloc[i-1]['ay'],sheet_1.iloc[i-1]['az'],sheet_1.iloc[i-1]['dx'],
    #                                   sheet_1.iloc[i-1]['dy'],sheet_1.iloc[i-1]['dz'])
    RT=pose_robot(sheet_1.iloc[i-1]['ax'],sheet_1.iloc[i-1]['ay'],sheet_1.iloc[i-1]['az'],sheet_1.iloc[i-1]['dx'],
                                      sheet_1.iloc[i-1]['dy'],sheet_1.iloc[i-1]['dz'])
    # RT=np.column_stack(((cv2.Rodrigues(np.array([[sheet_1.iloc[i-1]['ax']],[sheet_1.iloc[i-1]['ay']],[sheet_1.iloc[i-1]['az']]])))[0],
    #                    np.array([[sheet_1.iloc[i-1]['dx']],
    #                                   [sheet_1.iloc[i-1]['dy']],[sheet_1.iloc[i-1]['dz']]])))
    # RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
    # RT = np.linalg.inv(RT)

    R_all_end_to_base_1.append(RT[:3, :3])
    T_all_end_to_base_1.append(RT[:3, 3].reshape((3, 1)))

# print(R_all_end_to_base_1)
R,T=cv2.calibrateHandEye(R_all_end_to_base_1,T_all_end_to_base_1,R_all_chess_to_cam_1,T_all_chess_to_cam_1)#手眼标定
RT=np.column_stack((R,T))
RT = np.row_stack((RT, np.array([0, 0, 0, 1])))#即为cam to end变换矩阵
print('相机相对于末端的变换矩阵为：')
print(RT)

#结果验证，原则上来说，每次结果相差较小
for i in range(len(good_picture)):

    RT_end_to_base=np.column_stack((R_all_end_to_base_1[i],T_all_end_to_base_1[i]))
    RT_end_to_base=np.row_stack((RT_end_to_base,np.array([0,0,0,1])))
    # print(RT_end_to_base)

    RT_chess_to_cam=np.column_stack((R_all_chess_to_cam_1[i],T_all_chess_to_cam_1[i]))
    RT_chess_to_cam=np.row_stack((RT_chess_to_cam,np.array([0,0,0,1])))
    # print(RT_chess_to_cam)

    RT_cam_to_end=np.column_stack((R,T))
    RT_cam_to_end=np.row_stack((RT_cam_to_end,np.array([0,0,0,1])))
    # print(RT_cam_to_end)

    RT_chess_to_base=RT_end_to_base@RT_cam_to_end@RT_chess_to_cam#即为固定的棋盘格相对于机器人基坐标系位姿
    RT_chess_to_base=np.linalg.inv(RT_chess_to_base)
    print('第',i,'次')
    print(RT_chess_to_base[:3,:])
    print('')

以上代码调用时的实际例子

R,T=cv2.calibrateHandEye(R_all_end_to_base_1,T_all_end_to_base_1,R_all_chess_to_cam_1,T_all_chess_to_cam_1)#手眼标定

1.根据欧拉角计算旋转矩阵

#用于根据欧拉角计算旋转矩阵
def myRPY2R_robot(x, y, z):
    Rx = np.array([[1, 0, 0], [0, cos(x), -sin(x)], [0, sin(x), cos(x)]])
    Ry = np.array([[cos(y), 0, sin(y)], [0, 1, 0], [-sin(y), 0, cos(y)]])
    Rz = np.array([[cos(z), -sin(z), 0], [sin(z), cos(z), 0], [0, 0, 1]])
    R = Rz@Ry@Rx
    return R

2.用于根据位姿计算变换矩阵推理在10分左右的地方 传送门

#用于根据位姿计算变换矩阵
def pose_robot(x, y, z, Tx, Ty, Tz):
    thetaX = x / 180 * pi
    thetaY = y / 180 * pi
    thetaZ = z / 180 * pi
    R = myRPY2R_robot(thetaX, thetaY, thetaZ)
    t = np.array([[Tx], [Ty], [Tz]])
    RT1 = np.column_stack([R, t])  # 列合并
    RT1 = np.row_stack((RT1, np.array([0,0,0,1])))
    # RT1=np.linalg.inv(RT1)
    return RT1

3.通过以上两部分，传入通过 good_picture 传入多张标定图片，即棋盘格图片，用之前需要先通过 calibrateCamera（）函数测试看下能否提取出角点哈，从而得到 calibrateHandeye的前两个输入参数，具体调用流程结合代码看参数博客说明参考 ：

R_gripper2base，t_gripper2base是机械臂抓手相对于机器人基坐标系的旋转矩阵与平移向量，需要通过机器人运动控制器或示教器读取相关参数转换计算得到

补如果完成后精度不行的话可以多试试该函数下不同的method 解法 如下

R_all_end_to_base_1=[]
T_all_end_to_base_1=[]
# print(sheet_1.iloc[0]['ax'])
for i in good_picture:
    # print(sheet_1.iloc[i-1]['ax'],sheet_1.iloc[i-1]['ay'],sheet_1.iloc[i-1]['az'],sheet_1.iloc[i-1]['dx'],
    #                                   sheet_1.iloc[i-1]['dy'],sheet_1.iloc[i-1]['dz'])
    RT=pose_robot(sheet_1.iloc[i-1]['ax'],sheet_1.iloc[i-1]['ay'],sheet_1.iloc[i-1]['az'],sheet_1.iloc[i-1]['dx'],
                                      sheet_1.iloc[i-1]['dy'],sheet_1.iloc[i-1]['dz'])
    # RT=np.column_stack(((cv2.Rodrigues(np.array([[sheet_1.iloc[i-1]['ax']],[sheet_1.iloc[i-1]['ay']],[sheet_1.iloc[i-1]['az']]])))[0],
    #                    np.array([[sheet_1.iloc[i-1]['dx']],
    #                                   [sheet_1.iloc[i-1]['dy']],[sheet_1.iloc[i-1]['dz']]])))
    # RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
    # RT = np.linalg.inv(RT)

    R_all_end_to_base_1.append(RT[:3, :3])
    T_all_end_to_base_1.append(RT[:3, 3].reshape((3, 1)))

这里的代码作者是直接输入的相机内参，而得到rvec：标定板坐标系到相机坐标系的旋转向量，可用cv:Rodrigues（）变为旋转矩阵tvec：标定板坐标系到相机坐标系的旋转矩阵，这些是通过 opencv的

calibrateCamera()得到函数的mtx变量输出得到内参也就是代码里面的 K，以下为我通过使用该函数得到的数据节选，这里可以直接引入图片通过该函数得到下面的，也可以把该部分数据提取出来，后自行添加到代码里面，提取成json文件的代码参考我的这个博客传送门

4.solvePnP原理说明

solvePnP函数参数解释

Parameters:objectPoints - 世界坐标系下的控制点的坐标，vector的数据类型在这里可以使用

imagePoints - 在图像坐标系下对应的控制点的坐标。vector在这里可以使用

cameraMatrix - 相机的内参矩阵

distCoeffs - 相机的畸变系数

以上两个参数通过相机标定可以得到。相机的内参数的标定参见：

rvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系

tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系

flags - 默认使用CV_ITERATIV迭代法

易错点提示

1.第一个的意思（来自b站木鱼木夕评论）

关于23:52 易错点一的笔记:
标定相机时, 使用calibrateCamera()函数的主要目的在于得到相机内参和畸变系数,
 此外得到的cam_board_R和cam_board_T是几组相机外参; calibrateCamera()的功能:Finds the
 camera intrinsic and extrinsic parameters from several views of a
 calibration pattern.（从标定模式的几个视图中查找摄像机的内外参数。）
 ——————————————————————————————————————————————————————————————
 手眼标定时, 使用solvePnP()函数的主要目的在于得到cam_board_R和cam_board_T,
 使用solvePnP()函数时需要输入相机内参和畸变系数; solvePnP()的功能:Finds an object pose from
 3D-2D point correspondences. This function returns the rotation and
 the translation vectors that transform a 3D point expressed in the
 object coordinate frame to the camera coordinate frame, using
 different methods:
 （:找到一个对象的姿势3 d-2d通讯。这个函数返回旋转和
式中表示的三维点变换的平移向量
对象坐标帧到摄像机坐标帧，使用
不同的方法:）
 —————————————————————————————————————————————————————————————
 也就是说, 标定相机和手眼标定是两个有点区别的问题场景;

2.第二个反正就是手眼标定的时候棋盘格右下角在画面上要尽量偏向右方就是了。

补：旋转矩阵 旋转向量 欧拉角 互相转换