经典手眼标定算法之Tsai-Lenz的OpenCV实现

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

---

手眼标定问题描述

       在机器人校准测量、机器人手眼协调以及机器人辅助测量等领域，都要求知道机器人执行器末端（抓取臂）坐标系和传感器（比如用来测量三维空间中目标位置和方向并固定在机器人手上的摄像机）坐标系之间的相互关系，确定这种转换关系在机器人领域就是通常所说的手眼标定。

       将手眼标定系统如下图所示，其中$H_{gij}$为机器人执行器末端坐标系之间相对位置姿态的齐次变换矩阵；$H_{cij}$为摄像机坐标系之间相对位置姿态的齐次变换矩阵；$H_{cg}$为像机与机器人执行器末端之间的相对位置姿态齐次矩阵。

       经过坐标系变换，$H_{gij}$、$H_{cij}$和$H_{cg}$满足如下关系：
KaTeX parse error: Unknown column alignment: * at position 77: …{\begin{array}{*̲{20}{c}} { {R_…
       将上式展开，可以得到手眼标定的基本方程：
KaTeX parse error: Unknown column alignment: * at position 24: …{\begin{array}{*̲{20}{c}} { {R_…
       因此，手眼标定问题也就转化为从上述方程组中求解出$R_{cg}$和$T_{cg}$，下面就按照TSAI文献所述求解该方程组。

---

“两步法”手眼标定

       一般用“两步法”求解基本方程，即先从基本方程上式求解出$R_{cg}$，再代入下式求解出$T_{cg}$。在TSAI文献中引入旋转轴-旋转角系统来描述旋转运动来进行求解该方程组，具体的公式推导可以查看原始文献，这里只归纳计算步骤，不明白的地方可阅读文献，计算步骤如下：

Step1：利用罗德里格斯变换将旋转矩阵转换为旋转向量

KaTeX parse error: Unknown column alignment: * at position 24: …{\begin{array}{*̲{20}{c}} { {r_…

Step2：向量归一化

KaTeX parse error: Unknown column alignment: * at position 24: …{\begin{array}{*̲{20}{c}} {\be…

Step3：修正的罗德里格斯参数表示姿态变化

KaTeX parse error: Unknown column alignment: * at position 24: …{\begin{array}{*̲{20}{c}} { {P_…

Step4：计算初始旋转向量$P_{cg}^{\prime }$

$$skew(P$$