文章目录

• 简介
• In-Hand Camera
• Fixed Camera
• 非线性优化
• References

简介

​ 机械臂在运动时，往往需要配合视觉信息进行目标的定位或识别，这就涉及到如何将相机坐标系（Camera Frame）下的物体转换到机械臂自身的坐标系下（Base Frame）。这一问题一般通过手眼标定（Hand-Eye Calibration）解决，其中的“手”即为机械臂，“眼”即为相机。

​ 具体的标定方式还与手眼之间的安装方式有关，这里大体分为两类。其一，相机安装在机械臂的末端上，跟随机械臂一起运动，这种一般称为In-Hand Camera。其二，相机固定安装在某个位置，不随机械臂运动，这种一般称为Fixed Camera或Global Camera。下面分别对两张手眼安装形式的标定进行介绍。

In-Hand Camera

​ 如下图所示，In-Hand Camera的标定主要是为了求出相机与机械臂末端夹具（End-effector）之间的变换关系，一般是一个大小为4x4的刚体变换矩阵。

​ 令end-effector在base frame下的位姿为${}^B_ET$EB​T，即末端位姿，这可以从机械臂输出数据中读出，一般视为精确值。标定时，将一个已知精确大小的标定板（比如棋盘格）固定放置，然后让机械臂运动，以使相机可以从多个不同角度拍摄标定板图像，拍摄图像的同时，要记录当时的机械臂末端位姿。假设相机内参已知，每一张标定板图像都可以算出相机（C）在标定板（P）下的位姿${}^P_CT$CP​T. 这样每一个${}^B_ET$EB​T都对应一个${}^P_CT$CP​T。现在设相机在end-effector下的位姿为${}^E_CT$CE​T，标定板在base frame下的位姿为${}^B_PT$PB​T，他们之间存在如下关系
${}^B_ET_i{}^E_CT={}^B_PT{}^P_CT_i$EB​Ti​CE​T=PB​TCP​Ti​
其中${}^E_CT$CE​T和${}^B_PT$PB​T是未知量。

​ 初看，这是$AX=ZB$AX=ZB形式的方程，虽然存在一些解这类方程的方法，但根据个人经验，条件都比较苛刻，数值稳定性不佳。实际上，在上式中，我们并不关心${}^B_PT$PB​T，可以想办法将其消去。显然有
${}^B_ET_i{}^E_CT{}^C_PT_i={}^B_PT={}^B_ET_j{}^E_CT{}^C_PT_j$EB​Ti​CE​TPC​Ti​=PB​T=EB​Tj​CE​TPC​Tj​
既
${}^B_ET_i{}^E_CT{}^C_PT_i={}^B_ET_j{}^E_CT{}^C_PT_j$EB​Ti​CE​TPC​Ti​=EB​Tj​CE​TPC​Tj​
整理后得
${}^E_BT_j{}^B_ET_i{}^E_CT={}^E_CT{}^C_PT_j{}^P_CT_i$BE​Tj​EB​Ti​CE​T=CE​TPC​Tj​CP​Ti​
${}^E_BT_j{}^B_ET_i$BE​Tj​EB​Ti​实际上就是两个位姿之间的相对变换，${}^C_PT_j{}^P_CT_i$PC​Tj​CP​Ti​同理，因此上式的未知数只有我们关心的${}^E_CT$CE​T一个，方程变为了典型的$AX=XB$AX=XB的形式。这类方程有很多解法，比如Tsai方法，dual-quaternion方法以及Kronecker Product方法等，实测下来都比较稳定。其中Kronecker product方法实现简单明了，在一些比较中也优于其他方法，在此着重介绍。

​ 设方程的形式为$AX=XB$AX=XB，其中$A,B,X\in SE(3)$A,B,X∈SE(3)，则方程可以分成两个部分
$\begin{aligned} R_AR_X&=R_XR_B \\ R_At_X+t_A&=R_Xt_B+t_X \end{aligned}$RA​RX​RA​tX​+tA​​=RX​RB​=RX​tB​+tX​​
首先解只含有旋转矩阵的部分。用向量化算子$\text{vec}(\cdot)$vec(⋅)作用于等式两边，可得
$(I^T\otimes R_A)\text{vec}(R_X)=(R_B^T\otimes I)\text{vec}(R_X)$(IT⊗RA​)vec(RX​)=(RBT​⊗I)vec(RX​)
即
$(I\otimes R_A-R_B^T\otimes I)\text{vec}(R_X)=0$(I⊗RA​−RBT​⊗I)vec(RX​)=0
多组$R_A$RA​和$R_B$RB​直接按照$(I\otimes R_A-R_B^T\otimes I)$(I⊗RA​−RBT​⊗I)计算后堆叠起来即可，设堆叠后的矩阵为$K\in \mathbb{R}^{9n\times 9}$K∈R9n×9, $\text{vec}(R_X)$vec(RX​)是9x1的矢量，上式就变为一个简单的齐次线性方程$K\text{vec}(R_X)=0$Kvec(RX​)=0. 对$K$K做奇异值分解有$K=U_K\Sigma_K V_K^T$K=UK​ΣK​VKT​，则线性方程的解即为$V_K$VK​的最后一列（非零最小二乘解）。将解出的$\text{vec}(R_X)$vec(RX​)还原回3x3矩阵（设为$R_X'$RX′​)后，并不一定是正交旋转阵，需要再次对其进行奇异值分解，得$R_X'=U\Sigma V^T$RX′​=UΣVT，最终取$R_X=UV^T$RX​=UVT作为原方程的解。其中$\Sigma$Σ在一定程度上可以反映标定结果的好坏，良好的标定其$\Sigma$Σ的对角元素都应该非常接近甚至完全相等。

求出$R_X$RX​后，$t_X$tX​的求解也变成了一个解线性方程的问题，即
$(R_A-I)t_X=R_Xt_B-t_A$(RA​−I)tX​=RX​tB​−tA​
堆叠后求得$t_X$tX​的最小二乘解。最终
$X=\begin{pmatrix} R_X & t_X \\ 0 & 1 \end{pmatrix}$X=(RX​0​tX​1​)

Fixed Camera

​ 如下图所示，对Fixed Camera进行手眼标定时，主要关注相机（C）到Base frame（B）之间的变换。一般会将标定板固定在机械臂的end-effector上，然后运动机械臂，使相机能从不同视角拍摄到标定板，每拍摄图像时，同时记录当时机械臂末端的位姿。

​ 使用In-hand camera中的符号体系，可以发现如下关系
${}^B_ET_i{}^E_PT={}^B_CT{}^C_PT_i$EB​Ti​PE​T=CB​TPC​Ti​
同理，我们不关心${}^E_PT$PE​T，用上一节中同样的方式将其消去，有
${}^E_BT_i{}^B_CT{}^C_PT_i={}^E_PT={}^E_BT_j{}^B_CT{}^C_PT_j$BE​Ti​CB​TPC​Ti​=PE​T=BE​Tj​CB​TPC​Tj​
即
${}^B_ET_j{}^E_BT_i{}^B_CT={}^B_CT{}^C_PT_j{}^P_CT_i$EB​Tj​BE​Ti​CB​T=CB​TPC​Tj​CP​Ti​
其中${}^B_ET_j{}^E_BT_i$EB​Tj​BE​Ti​和${}^C_PT_j{}^P_CT_i$PC​Tj​CP​Ti​都是相对变换，上式的未知数只有${}^B_CT$CB​T，因此方程又变为了$AX=XB$AX=XB的形式，可使用与In-hand camera相同的方法求解。

非线性优化

​ 一般情况下，上述代数方法的标定结果已经足够好了，不过仍然可以使用非线性优化的方式进一步改善。以In-hand camera为例，可以使用以下代价函数（cost function)，同时优化${}^E_CT$CE​T和${}^B_PT{}$PB​T使代价函数最小
$\sum_i\Vert{}^B_ET_i{}^E_CT-{}^B_PT{}^P_CT_i\Vert^2$i∑​∥EB​Ti​CE​T−PB​TCP​Ti​∥2
也可以消去不关心的${}^B_PT$PB​T后，仅优化${}^E_CT$CE​T使下式最小
$\sum_{i,j}\Vert{}^E_BT_j{}^B_ET_i{}^E_CT-{}^E_CT{}^C_PT_j{}^P_CT_i\Vert^2$i,j∑​∥BE​Tj​EB​Ti​CE​T−CE​TPC​Tj​CP​Ti​∥2
优化后的结果能改善多少完全取决于数据。根据个人经验，在数据质量可靠的情况下，使用代数方法的结果作为优化初值，迭代10次左右就会收敛。

References

1. A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration

2. Simultaneous robot-world and hand-eye calibration using dual-quaternion and Kronecker product

3. Optimal Hand-Eye Calibration