机械臂与相机的标定

2019-09-05 12:39:02 阅读：754 来源： 互联网

文章目录

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

初看，这是 $AX=ZB$ AX=ZB形式的方程，虽然存在一些解这类方程的方法，但根据个人经验，条件都比较苛刻，数值稳定性不佳。实际上，在上式中，我们并不关心 ${}^B_PT$ PBT，可以想办法将其消去。显然有
${}^B_ET_i{}^E_CT{}^C_PT_i={}^B_PT={}^B_ET_j{}^E_CT{}^C_PT_j$ EBTiCETPCTi=PBT=EBTjCETPCTj
既
${}^B_ET_i{}^E_CT{}^C_PT_i={}^B_ET_j{}^E_CT{}^C_PT_j$ EBTiCETPCTi=EBTjCETPCTj
整理后得
${}^E_BT_j{}^B_ET_i{}^E_CT={}^E_CT{}^C_PT_j{}^P_CT_i$ BETjEBTiCET=CETPCTjCPTi
${}^E_BT_j{}^B_ET_i$ BETjEBTi实际上就是两个位姿之间的相对变换， ${}^C_PT_j{}^P_CT_i$ PCTjCPTi同理，因此上式的未知数只有我们关心的 ${}^E_CT$ CET一个，方程变为了典型的 $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}$ RARXRAtX+tA=RXRB=RXtB+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ΣKVKT，则线性方程的解即为 $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=RXtB−tA
堆叠后求得 $t_X$ tX的最小二乘解。最终
$X=\begin{pmatrix} R_X & t_X \\ 0 & 1 \end{pmatrix}$ X=(RX0tX1)

Fixed Camera

如下图所示，对Fixed Camera进行手眼标定时，主要关注相机（C）到Base frame（B）之间的变换。一般会将标定板固定在机械臂的end-effector上，然后运动机械臂，使相机能从不同视角拍摄到标定板，每拍摄图像时，同时记录当时机械臂末端的位姿。
在这里插入图片描述
使用In-hand camera中的符号体系，可以发现如下关系
${}^B_ET_i{}^E_PT={}^B_CT{}^C_PT_i$ EBTiPET=CBTPCTi
同理，我们不关心 ${}^E_PT$ PET，用上一节中同样的方式将其消去，有
${}^E_BT_i{}^B_CT{}^C_PT_i={}^E_PT={}^E_BT_j{}^B_CT{}^C_PT_j$ BETiCBTPCTi=PET=BETjCBTPCTj
即
${}^B_ET_j{}^E_BT_i{}^B_CT={}^B_CT{}^C_PT_j{}^P_CT_i$ EBTjBETiCBT=CBTPCTjCPTi
其中 ${}^B_ET_j{}^E_BT_i$ EBTjBETi和 ${}^C_PT_j{}^P_CT_i$ PCTjCPTi都是相对变换，上式的未知数只有 ${}^B_CT$ CBT，因此方程又变为了 $AX=XB$ AX=XB的形式，可使用与In-hand camera相同的方法求解。

非线性优化

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

References

标签：PT,RX,标定,相机,Tj,vec,Ti,机械,CT
来源： https://blog.csdn.net/AIchipmunk/article/details/100554796

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9