手眼标定的一些经验（基于眼在手上和眼在手外）

手眼标定是机器视觉进行应用的开端。狭义上将，进行机械臂抓取之前，必定要进行手眼标定，以完成视觉传感器到执行机械臂的互联。广义上讲，任何需要机器视觉以及执行机构的设备，比如自动驾驶系统，或者三维重构系统，手眼标定都是不可或缺的。

手眼标定分为两大类别，眼在手上（Eye-in-Hand）和眼在手外(Eye-To- Hand)两大类。
 

顾名思义，两种标定方式区别于相机移动与否。在标定原理上讲，两种原理都是求解一个AX=XB求解矩阵X的过程，至于为什么在每个分章节都会讲到。

眼在手外（Eye-To-Hand）

眼在手外的标定法，将
定义：
p（Pix）：像素坐标，标定格坐标
c（Camera）：相机坐标
h（Hand）：机械臂末端坐标
b（Base）：世界坐标
Tbc：从相机坐标系到世界坐标系的转移矩阵，这里是待求的矩阵
Tbh：从机械臂末端到世界坐标系的转移矩阵
Tcp：从标定板坐标系到相机坐标系的转移矩阵，这里和扩展的外参数指的是同一个矩阵
Thp：从标定板到机械手末端的转移矩阵，这里充当不变量，勾连起各个参数
 

我们可以得到如下等式：
Tbc*Tcp=Tbh*Thp
此等式自然成立，变换后得到
Tbh-1* Tbc*Tcp=Thp

所以我们可以通过多组数据得到：
T_bh^(①-1) T_bc T_cp^①=T_bh^(②-1) T_bc T_cp^②  
移项得到
〖T_bh^② T〗_bh^(①-1) T_bc=T_bc T_cp^② T_cp^(①-1)

这时候我们发现AX=XB出现了，
 

-0.0244673453075173    0.0129474934887250    0.999616782285073    -23.2031360705242
-0.999630883532739    0.0114943948954908    -0.0246165711166530    -1434.12212027217
-0.0118087129339921    -0.999850109415579    0.0126614770266597    886.281289665969
0    0    0    1

AX=XB解法：
       本文主要是讲解经典手眼标定问题中的TSAI-LENZ 文献方法，参考文献为“A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration”，并且实现了基于OpenCV的C++代码程序，code可去CSDN资源下载，MATLAB版本作者为苏黎世理工的Christian Wengert，也可在此处下载。

手眼标定问题描述
       在机器人校准测量、机器人手眼协调以及机器人辅助测量等领域，都要求知道机器人执行器末端（抓取臂）坐标系和传感器（比如用来测量三维空间中目标位置和方向并固定在机器人手上的摄像机）坐标系之间的相互关系，确定这种转换关系在机器人领域就是通常所说的手眼标定。
       将手眼标定系统如下图所示，其中HgijHgij为机器人执行器末端坐标系之间相对位置姿态的齐次变换矩阵；HcijHcij为摄像机坐标系之间相对位置姿态的齐次变换矩阵；HcgHcg为像机与机器人执行器末端之间的相对位置姿态齐次矩阵。
  
       经过坐标系变换，HgijHgij、HcijHcij和HcgHcg满足如下关系： 
HgijHcg=HcgHcij⇔(Rgij0Tgij1)(Rcg0Tcg1)=(Rcg0Tcg1)(Rcij0Tcij1)HgijHcg=HcgHcij⇔(RgijTgij01)(RcgTcg01)=(RcgTcg01)(RcijTcij01)

这时候可以看到我们的AX=XB出现了，Hcg就是我们这里要求的X

       将上式展开，可以得到手眼标定的基本方程： 
{RgijRcg=RcgRcij(Rgij−I)Tcg=RcgTcij−Tgij{RgijRcg=RcgRcij(Rgij−I)Tcg=RcgTcij−Tgij

       因此，手眼标定问题也就转化为从上述方程组中求解出RcgRcg和TcgTcg，下面就按照TSAI文献所述求解该方程组。
“两步法”手眼标定
       一般用“两步法”求解基本方程，即先从基本方程上式求解出RcgRcg，再代入下式求解出TcgTcg。在TSAI文献中引入旋转轴-旋转角系统来描述旋转运动来进行求解该方程组，具体的公式推导可以查看原始文献，这里只归纳计算步骤，不明白的地方可阅读文献，计算步骤如下：
Step1：利用罗德里格斯变换将旋转矩阵转换为旋转向量
{rgij=rodrigues(Rgij)rcij=rodrigues(Rcij){rgij=rodrigues(Rgij)rcij=rodrigues(Rcij)
Step2：向量归一化
⎧⎩⎨θgij=∥rgij∥2Nrgij=rgijθgijθcij=∥rcij∥2Nrcij=rcijθcij{θgij=‖rgij‖2Nrgij=rgijθgijθcij=‖rcij‖2Nrcij=rcijθcij
Step3：修正的罗德里格斯参数表示姿态变化
⎧⎩⎨Pgij=2sinθgij2NrgijPcij=2sinθcij2Nrcij{Pgij=2sin⁡θgij2NrgijPcij=2sin⁡θcij2Nrcij
Step4：计算初始旋转向量P′cgPcg′
skew(Pgij+Pcij)Pcg′=Pcij−Pgijskew(Pgij+Pcij)Pcg′=Pcij−Pgij
       其中，skew为反对称运算，假设一个三维向量V=[vx;vy;vz]V=[vx;vy;vz]，其反对称矩阵为： 
skew(V)=⎡⎣⎢0vz−vy−vz0vxvy−vx0⎤⎦⎥skew(V)=[0vz−vy−vz0vxvy−vx0]
Step5：计算旋转向量PcgPcg
Pcg=2Pcg′1+|Pcg′|2−−−−−−−−√Pcg=2Pcg′1+|Pcg′|2
Step6：计算旋转矩阵RcgRcg
Rcg=(1−|Pcg|22)I+12(PcgPcgT+4−|Pcg|2−−−−−−−−√skew(Pcg))Rcg=(1−|Pcg|22)I+12(PcgPcgT+4−|Pcg|2skew(Pcg))
Step7：计算平移向量
(Rgij−I)Tcg=RcgTcij−Tgij