机械臂 视觉

手眼标定全过程梳理

对前段时间学习的总结，以及对手眼标定的原理推导再梳理

Posted by R on 2023-09-07

之前写过关于手眼标定的推导性内容，后面写到一半因为时间紧张就没有写下去，之后因为汇报需要写成了 PPT 的形式。经过自己的一番讲解后感觉理解上更加顺畅了，但是无论是 PPT 还是之前写的文章都没有很清晰的进行表达，包括两种情况下的推导对比都比较混乱，下标也没有统一。

这样如果是站在学习的人的角度是很容易搞乱的，包括我自己在学习的过程中也是反复调查进行对比才慢慢理解，我觉得这个过程本不必这么复杂，于是再把前面的成果重新汇总汇总，写一个相对流畅的过程讲解。

从六月写完 c++ 多线程的记录之后将近三个月的时间一直没有写进展记录及梳理，过程中重学了 ros，也对 c++ 有一段时间学习，接下来的一段时间努力把之前学的一些东西补上，慢慢的需要形成体系。

（公式一旦居中就无法显示了，所以下文一些公式没有居中显示，可能是 hexo 渲染的问题，不影响阅读。）

手眼标定简介

首先声明，手眼标定是建立在此前机械臂运动学的推导基本成立的基础上，机械臂正逆运动学使得机械臂能够按你设定好的点位运动到指定位置，而手眼标定则引入了新的器件——相机。经过手眼标定，最终需要实现在相机画面内选取一点，机械臂就移动到对应点，即相机和机械臂的联动。

手眼标定旨在求解出工业机器人的末端坐标系与相机坐标系之间的坐标变换关系（眼在手上），或者工业机器人的基底坐标系与相机坐标系之间的坐标变换关系（眼在手外）。

眼在手上是指相机固定在机械臂末端，跟随机械臂移动，此时相机和机械臂末端的位置关系是固定的；眼在手外是指相机相对于机械臂是固定的，机械臂的运动和相机没有对应关系，此时相机和机械臂基座的位置关系固定。我们在此处对两种关系都进行推导，确保足够深入的掌握这方面的知识。

标定的推导目标是精确的实现相机视野下物体的空间坐标转换为机械臂基坐标系下的坐标。（及机械臂夹取时的x、y、z、rx、ry、rz）

1

2

以上分别为眼在手上和眼在手外的对应示意图。

手眼标定原理过程

不论是眼在手上还是眼在手外，都涉及到了几个坐标系：基础坐标系（base）、末端坐标系（tool）、相机坐标系（cam）以及标定板坐标系（cal）。

几个坐标系从字面意思上理解即可，基础坐标系建立在机械臂底座上，在世界坐标系下是固定的；末端坐标系是建立在机械臂末端的，随着机械臂末端移动而移动；相机坐标系则是相机自身的建立的坐标系，和其外参、内参等参数有关系；标定板坐标系则是建立在标定板上的坐标系，一般以标定板左上角角点为坐标系原点。

为了方便角标标注和讲解，后文统一用英文缩写代替表述几个坐标系，即 base、tool、cam、cal。

眼在手上

如果两个坐标系的关系一直相对不定，那我们求解起来也就没有任何意义，所以手眼标定需要找到固定的转换关系进行求解。

在手在眼上的方案中，相机固定在机械臂末端，随着它的移动而移动，此时它在 base 下肯定是不断移动的，但它与 tool 的关系确实相对静止的，即两者有着固定的转换关系，这正是我们求解的目标。

从下图可以看出，几个坐标系的关系其实是可以构成一个环的（即等式），我们在这里需要求解 （cam 和 base 的转换关系矩阵）。

1

从图上看，cam 出发到 base 是有两条路径，一种通过 tool，另一种通过 cal，因此可以达成一个等式：

在这个等式中，待求量是 ， 是通过运动学进行计算的， 可以通过相机标定获得。

这里额外说明一下相机标定，相机拍摄视野范围内的标定板，再结合自身内参可以获得标定板与相机坐标系的转换矩阵，具体过程可以搜索相机标定进行了解。

还有一个量是 ，这是标定板和基坐标系之间的转换矩阵，这一矩阵可以测量但是难度较高容易有误差，我们不对其进行测量处理，先作为未知量保存下来。

目前等式中有两个未知量，但是 这一转换矩阵是始终固定的，不会因为机械臂末端的移动而改变，所以我们可以多记录几组等式，构成方程来消去未知量。


i 是指标定次数，我们以任意两个等式组成的方程组讲述消去未知的过程：


前面提到， 这一转换矩阵是始终固定的，因此：

则上面两组等式可以合并为一个等式：

此时，等式中便只有一个未知量，即我们要求解的 ，可以在此设为 （注意这个矩阵也是始终固定的，所以两边的 矩阵是相等的）：

对以上方程做变换：

这边得出了最终方程，方程的形式为 。

之所以不直接求解，而是化为以上形式是因为过程中都是矩阵运算，求解起来十分复杂，没有具体的方法。而 ROGER Y. TSAI 和 REIMAR K. LENZ 两位学者于1989年发表的论文《A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration》中给出了手眼标定的解析算法，只要把方程化解为 的形式，就可以使用这种方法进行计算，且该方法是所有方法中求解速度最快的。

当然剩下的求解过程就不在我们的推导中了，感兴趣可以去读原论文推导，opencv 已经将该算法写成函数，我们在程序里直接引用即可，这些函数的使用后文会再做介绍。

眼在手外

如果理解了眼在手上的推导流程，那么眼在手外的推导就很容易理解了。在此方案中，相机固定在机械臂外部某一处，和机械臂运动完全无关，此时 cam 和 base 的关系便成为了固定关系，和 tool 的关系则时不断变化的。

同样从下图出发寻找坐标系之间的关系，记住我们在这里需要求解 （cam 和 base 的转换关系矩阵）。

2

从图上看，cam 到 cal 有两条路径，一种就是转换矩阵 ，另一种是通过 base、tool 再到 cam，同样通过关系等价可以得到等式（其实等价关系很多，在这个环内随便找两个起点终点都相同的路径都行，无需拘泥）：

和前面的推断一样， 是通过运动学进行计算的， 可以通过相机标定获得。

还存在一个量是 ，手在眼外的情况中，标定板是夹持在机械臂末端进行标定的，所以这个关系矩阵也是固定不变的，但同样无法进行测量求解，需要作为未知量看待。

后续流程就和前面眼在手上的推导一样，由于 在标定过程中始终保持不变，可以多记录几组进行消去：


i 是指标定次数，以任意两个等式组成的方程组讲述消去未知的过程：


始终保持不变：

则上面两组等式可以合并为一个等式：

转化可得：

这边得出了最终方程，方程的形式为 ，同样剩余求解步骤已经成熟完备，可直接执行 opencv 函数进行求解。

程序主要相关函数说明

1. 手眼标定函数

calibrateHandEye() 函数就是用于求解我们上文提到的 方程，其函数说明如下：

3

R_gripper2base，t_gripper2base 是机械臂抓手相对于机器人基坐标系的旋转矩阵与平移向量，需要通过机器人运动控制器或示教器读取相关参数转换计算得到，这一个在之前的正运动学代码中可以得到。

R_target2cam ， t_target2cam是标定板相对于双目相机的齐次矩阵，在进行相机标定时获取得到。（calibrateCamera()得到或solvePnP()得到）

R_cam2gripper，t_cam2gripper是求解的手眼矩阵分解得到的旋转矩阵与平移矩阵，opencv提供了五种方法对手眼矩阵进行求解，其中Tsai两步法求解速度最快。

2. 相机标定函数

以上推导中的所有矩阵格式均为 4 X 4 的旋转位移矩阵，其形式如下：

4

左侧三列分别为三个方向上的向量，右侧 、、 表示空间位移，旋转矩阵和位移矩阵共同构成转换矩阵。

在相机标定中，相机拍摄标定板在视野范围内的图片，调用 Opencv 对标定板的角点进行识别，以第一个点为坐标系原点取长方向为 x 轴，宽方向为 y 轴，这样即可算出对应的 和 ，并且根据叉乘运算给出 ，这样得到了三个构成旋转矩阵的基石。

识别角点的函数如下：

5

输入图像、横向格子数、纵向格子数即可返回角点坐标信息。之后返回的信息传递给 solvePnP 函数结合内参进行处理，即可得到转换矩阵：

6

  • 函数输入：

    objectPoints - 世界坐标系下的控制点的坐标
    imagePoints - 在图像坐标系下对应的控制点的坐标
    cameraMatrix - 相机的内参矩阵
    distCoeffs - 相机的畸变系数

  • 函数输出：

    rvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系
    tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系

结语

本篇侧重于理论推导，代码实现目前还有误差等因素没有调好，所以暂时不作讲解。主要的核心函数就是上文介绍的三个函数，在掌握原理后其实相关程序都是可以自行看懂的，可以找一些相关手眼标定函数进行研究。

另外，手眼标定在 ROS 下比较成熟，有完备的功能包可以直接处理，但无法直接移植到 Windows 下进行研究（因为使用了 ROS 特有的坐标转换功能包），在参考代码时注意甄别。

FEATURED TAGS
机械臂 视觉