机器人手眼标定原理介绍（含详细推导过程）使用Tsai-Lenz算法

最近找小鱼问手眼标定的非常多，但是小鱼时间有限，拉了个小群大家讨论，二维码失效可以关注公众号鱼香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时刻它们的位姿关系。

扫描二维码关注公众号，回复： 14227876 查看本文章

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

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

我们已知多组：

1. 夹爪在机器人坐标系中的坐标 $$H_g^{}$$
2. 标定板在相机坐标系中的坐标 $$H_c^{}$$

求

1. 夹爪和相机之间的位姿关系：$$H_{gc}^{}$$

并且我们知道标定过程中，机器人基坐标和标定板之间的相对位置关系保持不变，所以有：
$$H_{rc}^i=H_g^i{H}_{gc}^i{H}_c^i\text{\hspace{1em}(1)}$$$$H_{rc}^j=H_g^j{H}_{gc}^j{H}_c^j\text{\hspace{1em}(2)}$$$$H_{rc}^{}=H_{rc}^j=H_{rc}^j{H}_{rc}^{}机器人基座到标定板之间的变换关系\text{\hspace{1em}(3)}$$$$H_{gc}^i=H_{gc}^j=H_{gc}^{}两次变换中夹爪与相机位姿关系不变\text{\hspace{1em}(4)}$$

可以得到：
$$H_{gi}^{}{H}_{gc}^{}{H}_{ci}^{}=H_{gj}^{}{H}_{gc}^{}{H}_{cj}^{}将ij写到下面来$$
$$H_{gj}^{-1}{H}_{gi}^{}{H}_{gc}^{}=H_{gc}^{}{H}_{cj}^{}{H}_{ci}^{-1}先右乘H_{ci}^{-1}再左乘H_{gj}^{-1}\text{\hspace{1em}(6)}$$

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

1. Ci到Cj之间的转换关系：$$H_{cij}^{}=H_{cj}^{}{H}_{ci}^{-1}\text{\hspace{1em}(6-1)}$$
2. Gi到Gj之间的转换关系：$$H_{gij}^{}=H_{gj}^{-1}{H}_{gi}^{}\text{\hspace{1em}(6-2)}$$

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

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

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

问答

1. 要求解$$H_{gij}^{}{H}_{gc}^{}=H_{gc}^{}{H}_{cij}^{}$$中的Hgc，首先要知道H是什么？

   H 为齐次变换矩阵，它由3x3的旋转矩阵和3x1的平移矩阵组成。
   $$H_{cg}^{}=\left[\begin{array}{cc}{R}_{cg}^{}& T_{cg}^{}\\ 000& 1\end{array}\right]\text{\hspace{1em}(9)}$$

2. 什么是两步法？

   所谓两步法就是先求Hgc（相机和夹爪之间的齐次矩阵）的旋转部分，再使用旋转部分求出平移部分。
   我们将$$H_{gij}^{}{H}_{gc}^{}=H_{gc}^{}{H}_{cij}^{}$$按{9)展开计算得
   $$\{\begin{array}{ll}{R}_{gij}^{}{R}_{cg}^{}=R_{cg}^{}{R}_{cij}^{}& \\ (R_{gij}^{}-I)T_{cg}^{}=R_{cg}^{}{T}_{cij}^{}-T_{gij}^{}& \end{array}\text{\hspace{1em}(10)}$$

步骤

求解旋转矩阵R:
在Tsai-Lenz论文中使用旋转轴+旋转角的方式来表示旋转。作者使用了修正的罗德里格斯参数表示旋转变换。
在齐次矩阵中的R表示一个旋转矩阵,R的特征向量和特征值一定是它的旋转轴和1。
我们可以定义R的旋转轴为Pr(旋转向量)，则有：$$R{P}_r^{}=P_r^{}$$有了旋转轴和旋转角我们就可以确定一个旋转。
使用修正的罗德里格斯变换重新定义Pr:$$P_r^{}=2sin\frac{\theta }{2}[n1,n2,n3{]}^{T}0⩽\theta ⩽180\text{\hspace{1em}(11)}$$
接下来上公式，然后证明：
$$Skew(P_{gij}^{}+P_{cij}^{})P_{cg}^{{}^{\prime }}=P_{cij}^{}-P_{gij}^{}\text{\hspace{1em}(12)}$$$$P_{cg}^{}=\frac{2P_{cg}^{{}^{\prime }}}{\sqrt{1+|P_{cj}^{{}^{\prime }}{|}^2}}\text{\hspace{1em}(13)}$$
上面两个公式告诉了我们$P_{cg}^{}$和$P_{cij}^{},P_{gij}^{}$之间的关系。注意我们需要两组以上的数据才能求出$P_{cg}^{}$
现在我们证明上面的公式：

这张图描述了各个向量之间的关系。从图中，我们根据向量之间的关系，可以得到：
$$P_{cg}^{}\perp (P_{gij}^{}-P_{cij}^{})\text{\hspace{1em}(14)}$$
也可以通过代数方式证明：
$(P_{gij}^{}-P_{cij}^{}{)}^T{P}_{cg}^{}$
$=(P_{gij}^{}-P_{cij}^{}{)}^T{R}_{cg}^T{R}_{cg}^{}{P}_{cg}^{}$
$=(P_{gij}^T{R}_{cg}^T-P_{cij}^T{R}_{cg}^T)R_{cg}^{}{P}_{cg}^{}$
$=(R_{cg}^{}{P}_{gij}^{}-P_{gij}^{}{)}^T{P}_{cg}^{}$
$=[(R_{cg}^{}-I)P_{gij}^{}{]}^T{P}_{cg}^{}$
$=P_{gij}^T(R_{cg}^T-I{)}^T{P}_{cg}^{}$
$=0$

下次直接截图补充，用markdown写证明过程是真的累~
根据等式[14]可以得到：
(15)
同时我们可以得到：
$P_{gij}^{}-P_{cij}^{}$与$（P_{gij}^{}+P_{cij}^{})\times P_{cg}^{}$共线。
即：
$$P_{gij}^{}-P_{cij}^{}=s(P_{gij}^{}+P_{cij}^{})\times P_{cg}^{}\text{\hspace{1em}(16)}$$
这个特性我们从图中就可以看出，也可以从等式[15]中得到。

接下来是非常核心的等式：
$$|P_{gij}^{}-P_{cij}^{}|=|（P_{gij}^{}+P_{cij}^{})\times P_{cg}^{{}^{\prime }}|\text{\hspace{1em}(17)}$$
$$P_{cg}^{{}^{\prime }}=\frac{P_{cg}^{}}{\sqrt{4-|P_{cg}^{}{|}^2}}\text{\hspace{1em}(18)}$$

证明过程：
（19）
通过公式[13],[16],[19],我们可以得到：
$$（P_{gij}^{}+P_{cij}^{})\times P_{cg}^{{}^{\prime }}=P_{gij}^{}-P_{cij}^{}\text{\hspace{1em}(20)}$$
使用skew来计算X乘，就可以得到等式[12]：
$$Skew(P_{gij}^{}+P_{cij}^{})P_{cg}^{{}^{\prime }}=P_{cij}^{}-P_{gij}^{}\text{\hspace{1em}(12)}$$

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

作者介绍：

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

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

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