最近找小鱼问手眼标定的非常多，但是小鱼时间有限，拉了个小群大家讨论，二维码失效可以关注公众号鱼香ROS下方加小鱼好友拉你进群。

手眼标定原理及常用算法Tsai-Lenz介绍

大家好，我是小鱼。

上几节主要介绍了手眼标定程序的使用，但没有对手眼标定原理进行介绍，所以本文主要介绍手眼标定的原理，并介绍手眼标定算法Tsai-Lenz(实际是作者的名字)的计算过程，以及AX=XB的推导过程。

• 下一篇文章将会使用Python实现该算法
• 该算法适用于将相机装在手抓上和将相机装在外部两种情况
• 论文已经传到git上，地址：https://gitee.com/ohhuo/handeye-tsai
• 本文中所说的夹爪=机器人末端执行器=机器人末端

如果你要进行手眼标定，可以参考我的其他文章：

• 手眼标定-基础使用
• 手眼标定-JAKA机械臂
• 手眼标定-AUBO机械臂
• 手眼标定-Aruco使用与相机标定
• 手眼标定-注意事项

如果上述程序使用过程中遇到问题，可以参考：

• 手眼标定-常见问题排查

如果你对手眼标定原理感兴趣，可以参考以下文章：

• 机器人手眼标定原理介绍（含详细推导过程）使用Tsai-Lenz算法
• 手眼标定算法TSAI_LENZ，眼在手外python代码实现
• 手眼标定算法Tsai-Lenz代码实现（Python、C++、Matlab）

为什么需要手眼标定？手眼标定标什么？

当我们要使用机械臂结合视觉进行抓取时，通过相机获取了物体在空间中的位姿信息，但此时的位姿信息是基于相机坐标系的，并不能直接使用。
我们想让机器人末端执行器到达目标位置，那就要知道目标位置在机器人坐标系下（一般指机器人底座）的位姿。

我们可以这样推导出来：

目标在机器人坐标系下的位姿=目标在相机坐标系下的位姿—>相机在夹爪（末端）坐标系下的位姿—>夹爪（末端）在机器人基坐标系下的位姿

等式右边的三个位姿我们怎么获取呢？

• 目标在相机中的位姿我们可以通过视觉识别程序获得（可以参考手眼标定-Aruco使用与相机标定）
• 夹爪（末端）在机器人基坐标系的位姿我们可以通过机械臂示教器或者配套的SDK获得（关于JAKA和AUBO机械臂的姿态获取程序可以参考手眼标定-JAKA机械臂， 手眼标定-AUBO机械臂）
• 相机在夹爪（末端）坐标系下的位姿关系就是我们要通过手眼标定程序计算的

所以为了让机械臂能够到达视觉所识别出来的空间位姿，就必须要知道相机和机械臂末端执行器之间的位姿关系，手眼标定就是标定出机械臂末端和相机之间的位姿关系。

AX=XB是什么?有什么用？

继续上图：

上图表示的是机械臂与相机坐标系的变换关系，i代表i时刻的机器人末端、相机等之间的位姿关系，j表示j时刻它们的位姿关系。

注意：i、j时刻标定板与机器人位置保持不变。

我们用H表示坐标变换（H指homogeneous matrices 齐次变换矩阵）。
例如表示i时刻下夹爪（gripper）的坐标变换： H g i {H^{}_{gi}} Hgi​

我们已知多组：

1. 夹爪在机器人坐标系中的坐标 H g {H^{}_{g}} Hg​
2. 标定板在相机坐标系中的坐标 H c {H^{}_{c}} Hc​

求

1. 夹爪和相机之间的位姿关系： H g c {H^{}_{gc}} Hgc​

并且我们知道标定过程中，机器人基坐标和标定板之间的相对位置关系保持不变，所以有：
H r c i = H g i H g c i H c i (1) {H^{i}_{rc}}=H^{i}_{g}H^{i}_{gc}H^{i}_{c} \\tag{1} Hrci​=Hgi​Hgci​Hci​(1) H r c j = H g j H g c j H c j (2) {H^{j}_{rc}}=H^{j}_{g}H^{j}_{gc}H^{j}_{c} \\tag{2} Hrcj​=Hgj​Hgcj​Hcj​(2) H r c = H r c j = H r c j H r c 机 器 人 基 座 到 标 定 板 之 间 的 变 换 关 系 (3) {H^{}_{rc}=H^{j}_{rc}}={H^{j}_{rc}} \\quad H^{}_{rc} 机器人基座到标定板之间的变换关系 \\tag{3} Hrc​=Hrcj​=Hrcj​Hrc​机器人基座到标定板之间的变换关系(3) H g c i = H g c j = H g c 两 次 变 换 中 夹 爪 与 相 机 位 姿 关 系 不 变 (4) {}H^{i}_{gc}=H^{j}_{gc}={H^{}_{gc}} \\quad两次变换中夹爪与相机位姿关系不变 \\tag{4} Hgci​=Hgcj​=Hgc​两次变换中夹爪与相机位姿关系不变(4)

可以得到：
H g i H g c H c i = H g j H g c H c j 将 i j 写 到 下 面 来 H^{}_{gi}H^{}_{gc}H^{}_{ci} =H^{}_{gj}H^{}_{gc}H^{}_{cj} \\quad 将ij写到下面来 Hgi​Hgc​Hci​=Hgj​Hgc​Hcj​将ij写到下面来
H g j − 1 H g i H g c = H g c H c j H c i − 1 先 右 乘 H c i − 1 再 左 乘 H g j − 1 (6) H^{-1}_{gj}H^{}_{gi}H^{}_{gc}=H^{}_{gc}H^{}_{cj}H^{-1}_{ci} \\quad \\tag{6}先右乘H^{-1}_{ci} 再左乘H^{-1}_{gj} Hgj−1​Hgi​Hgc​=Hgc​Hcj​Hci−1​先右乘Hci−1​再左乘Hgj−1​(6)

我们通过已知条件可以得到：

1. Ci到Cj之间的转换关系： H c i j = H c j H c i − 1 (6-1) {H^{}_{cij}}=H^{}_{cj}H^{-1}_{ci} \\tag{6-1} Hcij​=Hcj​Hci−1​(6-1)
2. Gi到Gj之间的转换关系： H g i j = H g j − 1 H g i (6-2) {H^{}_{gij}}=H^{-1}_{gj}H^{}_{gi} \\tag{6-2} Hgij​=Hgj−1​Hgi​(6-2)

Hgi这里使用的是相机中标定板的位姿

通过上面两个公式可以得到：
H g i j H g c = H g c H c i j (7) {H^{}_{gij}}H^{}_{gc}=H^{}_{gc}{H^{}_{cij}} \\tag{7} Hgij​Hgc​=Hgc​Hcij​(7)
设：
A = H g i j B = H c i j X = H g c A={H^{}_{gij}} \\quad B={H^{}_{cij}} \\quad X=H^{}_{gc} A=Hgij​B=Hcij​X=Hgc​
即可以得到： A X = X B (8) AX=XB \\tag{8} AX=XB(8)
我们的目标是求出相机和夹爪之间的位姿关系即求出AX=XB中的X
下面讲的Tsai-Lenz算法就是用来求解X的一种算法。

Tsai-Lenz算法求解步骤是什么？

问答

1. 要求解 H g i j H g c = H g c H c i j {H^{}_{gij}}H^{}_{gc}=H^{}_{gc}{H^{}_{cij}} Hgij​Hgc​=Hgc​Hcij​中的Hgc，首先要知道H是什么？

   H 为齐次变换矩阵，它由3x3的旋转矩阵和3x1的平移矩阵组成。
   H c g = [ R c g T c g 000 1 ] (9) H^{}_{cg}= \\left[ \\begin{matrix} R^{}_{cg} & T^{}_{cg} \\\\ 000 & 1 \\end{matrix} \\right] \\tag{9} Hcg​=[Rcg​000​Tcg​1​](9)

2. 什么是两步法？

   所谓两步法就是先求Hgc（相机和夹爪之间的齐次矩阵）的旋转部分，再使用旋转部分求出平移部分。
   我们将 H g i j H g c = H g c H c i j {H^{}_{gij}}H^{}_{gc}=H^{}_{gc}{H^{}_{cij}} Hgij​Hgc​=Hgc​Hcij​按{9)展开计算得
   { R g i j R c g = R c g R c i j ( R g i j − I ) T c g = R c g T c i j − T g i j (10) \\begin{cases}R^{}_{gij}R^{}_{cg}=R^{}_{cg}R^{}_{cij}& \\\\(R^{}_{gij}-I )T^{}_{cg}=R^{}_{cg}T^{}_{cij}-T^{}_{gij}& \\end{cases} \\tag{10} {Rgij​Rcg​=Rcg​Rcij​(Rgij​−I)Tcg​=Rcg​Tcij​−Tgij​​​(10)

步骤

求解旋转矩阵R:
在Tsai-Lenz论文中使用旋转轴+旋转角的方式来表示旋转。作者使用了修正的罗德里格斯参数表示旋转变换。
在齐次矩阵中的R表示一个旋转矩阵,R的特征向量和特征值一定是它的旋转轴和1。
我们可以定义R的旋转轴为Pr(旋转向量)，则有： R P r = P r RP^{}_{r}=P^{}_{r} RPr​=Pr​有了旋转轴和旋转角我们就可以确定一个旋转。
使用修正的罗德里格斯变换重新定义Pr: P r = 2 s i n θ 2 [ n 1 , n 2 , n 3 ] T 0 ⩽ θ ⩽ 180 (11) P^{}_{r}=2sin\\frac{\\theta}{2}[n1,n2,n3]^{T} \\quad \\small 0\\leqslant\\theta\\leqslant180 \\tag{11} Pr​=2sin2θ​[n1,n2,n3]T0⩽θ⩽180(11)
接下来上公式，然后证明：
S k e w ( P g i j + P c i j ) P c g ′ = P c i j − P g i j (12) Skew(P^{}_{gij}+P^{}_{cij})P^{\'}_{cg}=P^{}_{cij}-P^{}_{gij} \\tag{12} Skew(Pgij​+Pcij​)Pcg′​=Pcij​−Pgij​(12) P c g = 2 P c g ′ 1 + ∣ P c j ′ ∣ 2 (13) P^{}_{cg}=\\frac{2P^{\'}_{cg}}{\\sqrt{1+|P^{\'}_{cj}|^2}} \\tag{13} Pcg​=1+∣Pcj′​∣2 ​2Pcg′​​(13)
上面两个公式告诉了我们 P c g P^{}_{cg} Pcg​和 P c i j , P g i j P^{}_{cij},P^{}_{gij} Pcij​,Pgij​之间的关系。注意我们需要两组以上的数据才能求出 P c g P^{}_{cg} Pcg​
现在我们证明上面的公式：

这张图描述了各个向量之间的关系。从图中，我们根据向量之间的关系，可以得到：
P c g ⊥ ( P g i j − P c i j ) (14) P^{}_{cg}\\perp(P^{}_{gij}-P^{}_{cij}) \\tag{14} Pcg​⊥(Pgij​−Pcij​)(14)
也可以通过代数方式证明：
( P g i j − P c i j ) T P c g (P^{}_{gij}-P^{}_{cij})^TP^{}_{cg} (Pgij​−Pcij​)TPcg​
= ( P g i j − P c i j ) T R c g T R c g P c g =(P^{}_{gij}-P^{}_{cij})^TR^{T}_{cg}R^{}_{cg}P^{}_{cg} =(Pgij​−Pcij​)TRcgT​Rcg​Pcg​
= ( P g i j T R c g T − P c i j T R c g T ) R c g P c g =(P^{T}_{gij}R^{T}_{cg}-P^{T}_{cij}R^{T}_{cg})R^{}_{cg}P^{}_{cg} =(PgijT​RcgT​−PcijT​RcgT​)Rcg​Pcg​
= ( R c g P g i j − P g i j ) T P c g =(R^{}_{cg}P^{}_{gij}-P^{}_{gij})^TP^{}_{cg} =(Rcg​Pgij​−Pgij​)TPcg​
= [ ( R c g − I ) P g i j ] T P c g =[(R^{}_{cg}-I)P^{}_{gij}]^TP^{}_{cg} =[(Rcg​−I)Pgij​]TPcg​
= P g i j T ( R c g T − I ) T P c g =P^{T}_{gij}(R^{T}_{cg}-I)^TP^{}_{cg} =PgijT​(RcgT​−I)TPcg​
= 0 =0 =0

下次直接截图补充，用markdown写证明过程是真的累~
根据等式[14]可以得到：
(15)
同时我们可以得到：
P g i j − P c i j P^{}_{gij}-P^{}_{cij} Pgij​−Pcij​与 （ P g i j + P c i j ) × P c g （P^{}_{gij}+P^{}_{cij})\\times P^{}_{cg} （Pgij​+Pcij​)×Pcg​共线。
即：
P g i j − P c i j = s ( P g i j + P c i j ) × P c g (16) P^{}_{gij}-P^{}_{cij}=s(P^{}_{gij}+P^{}_{cij})\\times P^{}_{cg} \\tag{16} Pgij​−Pcij​=s(Pgij​+Pcij​)×Pcg​(16)
这个特性我们从图中就可以看出，也可以从等式[15]中得到。

接下来是非常核心的等式：
∣ P g i j − P c i j ∣ = ∣ （ P g i j + P c i j ) × P c g ′ ∣ (17) |P^{}_{gij}-P^{}_{cij}|=|（P^{}_{gij}+P^{}_{cij})\\times P^{\'}_{cg}| \\tag{17} ∣Pgij​−Pcij​∣=∣（Pgij​+Pcij​)×Pcg′​∣(17)
P c g ′ = P c g 4 − ∣ P c g ∣ 2 (18) P^{\'}_{cg}=\\frac{P^{}_{cg}}{\\sqrt{4-|P^{}_{cg}|^2}} \\tag{18} Pcg′​=4−∣Pcg​∣2 ​Pcg​​(18)

证明过程：
（19）
通过公式[13],[16],[19],我们可以得到：
（ P g i j + P c i j ) × P c g ′ = P g i j − P c i j (20) （P^{}_{gij}+P^{}_{cij})\\times P^{\'}_{cg} =P^{}_{gij}-P^{}_{cij}\\tag{20} （Pgij​+Pcij​)×Pcg′​=Pgij​−Pcij​(20)
使用skew来计算X乘，就可以得到等式[12]：
S k e w ( P g i j + P c i j ) P c g ′ = P c i j − P g i j (12) Skew(P^{}_{gij}+P^{}_{cij})P^{\'}_{cg}=P^{}_{cij}-P^{}_{gij} \\tag{12} Skew(Pgij​+Pcij​)Pcg′​=Pcij​−Pgij​(12)

通过等式[12]和等式10我们就能实现手眼矩阵的计算，下一讲我们将使用Python、C++、MATLAB来实现Tsai-Lenz手眼标定算法。

作者介绍：

我是小鱼，机器人领域资深玩家，现深圳某独脚兽机器人算法工程师一枚

初中学习编程，高中开始学习机器人，大学期间打机器人相关比赛实现月入2W+（比赛奖金）

目前在输出机器人学习指南、论文注解、工作经验，欢迎大家关注小智，一起交流技术，学习机器人