前言

最近由于组内的相关工作需求，需要进行机器人的高精度标定。原始的标定精度在6mm左右，虽然听起来是非常微小的偏差，但是由于研究方向手术机器人对精度要求极高。且在运动过程中，深度信息误差、畸变误差、机械误差、坐标系转换等一系列误差累积环节，将导致误差放大，因此远不能达到要求。

在经过了为其两周的研究和编程探索，最后成功将标定精度提升到0.5mm，达到了任务要求。

在研究和标定中，查阅了很多博客和相关资料，也踩了很多坑，作为也是小白的自己在研究和学习中发现当前的研究文献和博客最重要的问题是，没有从Tsai方法中AX=XB中，对A矩阵和B矩阵进行详细描述，也没有给出具体的计算代码和过程。默认读者已经搞清楚矩阵AB的物理意义和计算方式，对小白并不友好，笔者在进行手眼标定的过程中也经常由于计算错误A矩阵和B矩阵的左乘或右乘，得到错误的AB矩阵的输入数据，耽误了一些时间，因此在此记录

本系列的重点有三个：

原理中A，B矩阵的推导和代码计算
Tsai轴角方法和四元数等二步计算方法的实现
基于非线性优化的高精度手眼标定方法的原理与代码实现

原理推导

公式推导

在这里插入图片描述

如图所示，这是一个使用标定板进行机械臂手眼标定的原理示意图

在其中存在着四个重要的坐标系：机械臂基点坐标系，末端坐标系，相机坐标系，标定板坐标系

这四个坐标系可以确定四个变换关系（矩阵）（在此默认读者已经知道坐标系和变换矩阵的相关基础知识），Tsai算法的核心思想：由于四个变换关系（矩阵）存在闭环因此可得乘积为单位矩阵：

在这里插入图片描述

在整个标定过程中机械臂末端和相机会在世界坐标系中运动，这也就意味着与机械臂末端和相机所在的变换关系将进行动态变化，因此**四个变换关系（矩阵）**中只有机械臂基点和标定板在世界坐标系中始终固定不变，因此在世界坐标系中不同的两个机械臂位姿，位姿１和位姿２中存在如下等式：

在这里插入图片描述

为什么在数据采集中至少需要两个位姿信息

在很多的文章和博客中会提到一个增量的概念，用于去除cal_H_base在计算中的影响。

刚看的时候很容易迷糊，但是实际原理非常易于理解，由于标定板到机械臂基点的变换矩阵cal_H_base无法精密测量属于未知量，而等式中end_H_cam为所求值也属于未知数，使用增量的核心思想可以转化为：一个等式无法求得两个未知数，因此需要两个不同的位姿状态得到两个等式，将变换矩阵cal_H_base消除。

即使用两个不同位姿之间的增量变换矩阵用于求解手眼变换矩阵。

核心思想：通过两个不同的位姿表示cal_H_base进行消除，从而建立等式，从而通过唯一一个等式得到标定矩阵end_H_cam：

在这里插入图片描述
至此完成对TSAI标定方法中的A，B矩阵的推导。

MATLAB编程计算

A矩阵的计算和获取

如核心公式中的计算，在手眼标定的运算过程中矩阵A实际上是机械臂两次位姿之间的变换矩阵，由机械臂自身的参数和DH方法可以知道由机械臂在不同位姿状态时末端到基座标系的变化矩阵。

自研机械臂这是最重要的运动学搭建部分，在购买的机械臂中绝大部分可以直接通过API进行获取，在此不多做赘述，等有空可能会补上。

在机械臂运行矩阵的获取中一般可以直接获取关于机械臂基点到末端执行器的变换矩阵，本系列的数据计算中，将每个机械臂位姿的静态变换矩阵Ta储存于txt文档内，命名为pos，储存格式如下图：
在这里插入图片描述
但是需要注意的是，以上数据并不是实际带入手眼标定矩阵中的实际运算数据这一点需要额外注意

matlab计算代码

部分代码，可单独使用直接计算

img_num = 10;
x = load("E:\研究生学习\手眼标定\IMG_20230516_5\pos.txt");  
pos = zeros(4,4,img_num);
% 各个点机械臂位姿变化矩阵
for i=1:1:img_numpos(:,:,i) = x(((4*(i-1)+1):4*i),:);
end
% 相邻机械臂位姿变化增量 
for i=1:1:img_num-1A_add(:,:,i)= pos(:,:,i)\pos(:,:,i+1);
end
% 矩阵拼接
A = [];
for i=1:1:img_num-1A(:,:,i) =  A_add(:,:,i);
end

在以上代码中会对两次的位姿矩阵进行一次矩阵除法，实际上就是n-1点位姿矩阵的逆于n点位姿矩阵的进行乘法

B矩阵的计算和获取

矩阵B是公式计算中的相机标定矩阵B，在此需要重点注意的是使用的变换矩阵是矩阵的外参矩阵，不需要再进行内参和畸变参数的左乘（鱼眼镜头除外）
像素坐标系到世界坐标系的关系如下：

像素坐标点ｘ相机内参矩阵ｘ相机畸变矫正矩阵ｘ相机外参矩阵　＝　世界坐标系实际位姿坐标

相机坐标系到世界坐标系的变换关系如下：

相机坐标系坐标ｘ相机外参矩阵　＝　世界坐标系实际坐标点

matlab标定的过程在此默认读者都会不在赘述，后面可能会填坑（很有可能不会）

而在实际的采集和计算系统之中，将对检测目标即标定板进行识别，标定板中的识别角点于相机中计算得到的变换矩阵即是由相机到世界的变换矩阵。

而在世界坐标系中的唯一变化量为两次变换中的外参矩阵变化量，其余的矩阵将由于增量变化不被考虑在计算中，将标定完成后的标定参数命名为calibrationSession进行保存，通过matlab的load函数直接进行读取并获取相关参数，计算增量矩阵B：

matlab计算矩阵B

%% 相机标定数据导入
load('calibrationSession0516_5.mat')
for i=1:1:img_num-1Rr1 = calibrationSession.CameraParameters.RotationMatrices(:,:,i);Rr2 = calibrationSession.CameraParameters.RotationMatrices(:,:,i+1);Tr1 = calibrationSession.CameraParameters.TranslationVectors(i,:);Tr2 = calibrationSession.CameraParameters.TranslationVectors(i+1,:);B_add = [Rr1' (Tr1/1000)';0,0,0,1]/[Rr2' (Tr2/1000)';0,0,0,1];B(:,:,i) = B_add;
end