大家好，我是小鱼。
上几节主要介绍了手眼标定程序的使用，但没有对手眼标定原理进行介绍，所以本文主要介绍手眼标定的原理，并介绍手眼标定算法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
我们已知多组：
- 夹爪在机器人坐标系中的坐标 H g {H^{}_{g}} Hg
- 标定板在相机坐标系中的坐标 H c {H^{}_{c}} Hc
求
- 夹爪和相机之间的位姿关系： 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=HgiHgciHci(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=HgjHgcjHcj(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=HrcjHrc机器人基座到标定板之间的变换关系(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写到下面来 HgiHgcHci=HgjHgcHcj将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−1HgiHgc=HgcHcjHci−1先右乘Hci−1再左乘Hgj−1(6)
我们通过已知条件可以得到：
- 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=HcjHci−1(6-1)
- 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−1Hgi(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}
HgijHgc=HgcHcij(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=HgijB=HcijX=Hgc 即可以得到：
A
X
=
X
B
(8)
AX=XB \tag{8}
AX=XB(8) 我们的目标是求出相机和夹爪之间的位姿关系即求出AX=XB中的X 下面讲的Tsai-Lenz算法就是用来求解X的一种算法。
-
要求解 H g i j H g c = H g c H c i j {H^{}_{gij}}H^{}_{gc}=H^{}_{gc}{H^{}_{cij}} HgijHgc=HgcHcij中的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=[Rcg000Tcg1](9)
-
什么是两步法？
所谓两步法就是先求Hgc（相机和夹爪之间的齐次矩阵）的旋转部分，再使用旋转部分求出平移部分。 我们将 H g i j H g c = H g c H c i j {H^{}_{gij}}H^{}_{gc}=H^{}_{gc}{H^{}_{cij}} HgijHgc=HgcHcij按{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} {RgijRcg=RcgRcij(Rgij−I)Tcg=RcgTcij−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)TRcgTRcgPcg
=
(
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}
=(PgijTRcgT−PcijTRcgT)RcgPcg
=
(
R
c
g
P
g
i
j
−
P
g
i
j
)
T
P
c
g
=(R^{}_{cg}P^{}_{gij}-P^{}_{gij})^TP^{}_{cg}
=(RcgPgij−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+（比赛奖金）
目前在输出机器人学习指南、论文注解、工作经验，欢迎大家关注小智，一起交流技术，学习机器人 
