眼在手外手眼标定 机械臂位姿参数获取

### 机械臂手眼标定概述

机械臂手眼标定的主要目的是建立相机坐标系与机械臂基座坐标系之间的变换关系，以便将由相机捕捉到的目标物体位置信息转换成机械臂可理解并操作的空间坐标[^1]。具体来说，在手眼标定过程中，需要确定的是摄像机相对于末端执行器的姿态（即旋转和平移），这可以通过一系列已知姿态下拍摄图像来获得。

对于不同类型的配置——眼在手上(eye-on-hand) 和眼在外(eye-to-hand)，虽然具体的数学模型有所不同，但基本思路保持一致：

- **眼在手上 (Eye-in-Hand)**: 此情况下，摄像头固定于机器人手臂末端执行器上移动；因此每次当手臂改变其姿势时，相机会随之一起变动。

- **眼在外面 (Eye-To-Hand)**: 这里指得是摄像头位于远离机器人的某个固定点处观察整个工作区域内的对象变化情况而不随同运动部件一同动作。

### 实现过程

为了实现上述目的，一般采用如下几个步骤来进行实际操作:

#### 数据采集阶段
收集多组不同的机械臂关节角度及其对应条件下所拍摄影像中特征点的位置信息作为训练样本集。这些数据用于后续算法处理以估计两者间的相对位姿矩阵[^2]。

#### 计算内外参
利用OpenCV或其他计算机视觉库函数计算内参数（焦距、主点偏移量等）和外参数（R|t形式表示的旋转向量和平移向量）。这里涉及到的关键技术包括但不限于张正友棋盘格法校准相机内部结构特性，并通过SVD分解等方式求解最优解路径[^3]。

#### 坐标转换方程构建
基于前面提到的数据集合以及内外参结果，建立起从像素平面映射至三维世界坐标的线性或非线性的映射关系式子。该表达式的准确性直接影响到最后一步定位精度的好坏程度[^4]。

import numpy as np
from scipy.spatial.transform import Rotation as R

def compute_hand_eye_transform(T_c_w_list, T_e_b_list):
    """
    使用Tsai-Lenz方法计算手眼关系
    
    参数:
        T_c_w_list : list of ndarray(shape=(4,4))
            相机到世界坐标系的一系列变换矩阵列表
        
        T_e_b_list : list of ndarray(shape=(4,4)) 
            末端执行器到基座坐标系的一系列变换矩阵列表
            
    返回值:
        tuple(ndarray(shape=(4,4)), str)
            手眼变换矩阵T_cam_endeffector及说明文字
    """

    A = []
    B = []

    for i in range(len(T_c_w_list)):
        Rcwi = T_c_w_list[i][:3,:3]
        rebi = R.from_matrix(T_e_b_list[i][:3,:3]).as_rotvec()
        
        Ai = np.hstack((np.eye(3), -Rcwi.T @ skew_symmetric(rebi)))
        Bi = Rcwi.T @ T_c_w_list[i][:3,3]

        A.append(Ai)
        B.append(Bi)

    A = np.vstack(A)
    B = np.concatenate(B)

    X = np.linalg.lstsq(A,B,rcond=None)[0]

    r_vec = X[:3].reshape(-1,)
    t_vec = X[3:].reshape(-1,)

    rotation = R.from_rotvec(r_vec).as_matrix()

    transform = np.identity(4)
    transform[:3,:3] = rotation
    transform[:3,3] = t_vec.flatten()

    return transform,"这是通过最小二乘法估算出来的手眼变换矩阵"
    
def skew_symmetric(v):
    """创建一个反对称矩阵"""
    return np.array([[0,-v[2], v[1]],
                     [v[2], 0,-v[0]], 
                     [-v[1], v[0], 0]])