文章同步更新于 github page.
简介
机械臂在运动时,往往需要配合视觉信息进行目标的定位或识别,这就涉及到如何将相机坐标系(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下的位姿为 E B T {}^B_ET EBT,即末端位姿,这可以从机械臂输出数据中读出,一般视为精确值。标定时,将一个已知精确大小的标定板(比如棋盘格)固定放置,然后让机械臂运动,以使相机可以从多个不同角度拍摄标定板图像,拍摄图像的同时,要记录当时的机械臂末端位姿。假设相机内参已知,每一张标定板图像都可以算出相机(C)在标定板(P)下的位姿 C P T {}^P_CT CPT. 这样每一个 E B T {}^B_ET EBT都对应一个 C P T {}^P_CT CPT。现在设相机在end-effector下的位姿为 C E T {}^E_CT CET,标定板在base frame下的位姿为 P B T {}^B_PT PBT,他们之间存在如下关系
E B T i C E T = P B T C P T i {}^B_ET_i{}^E_CT={}^B_PT{}^P_CT_i EBTiCET=PBTCPTi
其中 C E T {}^E_CT CET和 P B T {}^B_PT PBT是未知量。
初看,这是 A X = Z B AX=ZB AX=ZB形式的方程,虽然存在一些解这类方程的方法,但根据个人经验,条件都比较苛刻,数值稳定性不佳。实际上,在上式中,我们并不关心 P B T {}^B_PT PBT,可以想办法将其消去。显然有
E B T i C E T P C T i = P B T = E B T j C E T P C T j {}^B_ET_i{}^E_CT{}^C_PT_i={}^B_PT={}^B_ET_j{}^E_CT{}^C_PT_j EBTiCETPCTi=PBT=EBTjCETPCTj
既
E B T i C E T P C T i = E B T j C E T P C T j {}^B_ET_i{}^E_CT{}^C_PT_i={}^B_ET_j{}^E_CT{}^C_PT_j EBTiCETPCTi<