机器人抓取平台搭建记录（六）：手眼标定--眼在手上--Kinova Gen2 JACO2 七自由度机械臂--Realsense D435

吐槽一，手眼标定两个多月前就做好了，这篇文章也是两个多月前就写了，但当时不知出了什么差错，在我还没有写完想暂时保存一下草稿的时候，写的内容竟然没了小半。于是就拖到现在，拖到忘记了当时写作的思路，操作细节有些也记不得了。可能内容要缩水一下了。

吐槽二，笔者实际操作过程遇到好多坑。

本文很长，并不是简单表述怎么操作。而且不太适合纯新手。

先来个概述：

笔者先用easy_hand_eye ros包，针对kinova 7自由度这款机器人做了修改，自认为操作没问题，但是结果一直不对。

后来换了aruco_hand_eye ros包才成功。

笔者也都是参照网络上的博客进行的标定，在实际操作中，需要修改一些东西，至于改哪里、怎么改，相信下文的内容有所裨益。

---

手眼标定原理

读者需要知道手眼标定的原理，本文不做介绍。

机器人手眼标定Ax=xB（eye to hand和eye in hand）及平面九点法标定_yaked19的博客-CSDN博客_手眼标定

机器人手眼标定 - 知乎

---

知道原理之后，我们应该知道以下几点：

1. 手眼标定结果是一个位姿。【笔者很确定这是准确的描述，很多博客或文章都不够明晰或准确】

眼在手上，就是相机（坐标系）在机器人末端（坐标系）下的位姿；

眼在手外，就是相机（坐标系）在机器人基座（坐标系）下的位姿。

重要的题外话，不看吃大亏：

位姿是位置加姿态，位置就是空间坐标（x，y，z），姿态的表达方式很多：欧拉角，四元数，旋转矩阵。

常用的表示位姿的方法是齐次变换矩阵，通常用字母T表示。

齐次变换矩阵有三种意义（懂了这三种，就超越99%的同龄人了）：

1. 位姿表示。表示两个坐标系之间的位姿关系。

2. 坐标系运动（刚体运动）。指明一个坐标系（刚体）如何运动。

3. 坐标变换。指明如何  把一个空间点     在A坐标系下的坐标   转换成    B坐标系下的坐标。

举例来说，眼在手上的情况：

我们标定的手眼关系，就是相机坐标系在末端坐标系下的位姿，即意义1。这个位姿我们用T来表示。

同时，这个T又指明了，末端坐标系通过T这个旋转加平移（或平移加旋转）运动，能够和相机坐标系重合，即意义2。所以你会看到很多文章说，标定出来的手眼关系，是机器人末端到相机的变换。

还有，这个T还能把相机坐标系下的点的坐标，变换到末端坐标系下，即意义3。这个坐标变换是我们标定手眼关系的根本目的。所以，在一些文章中你会看到，标定出的手眼关系，能够将相机坐标系下的坐标转换成末端坐标系下的坐标。

懂的扣个666

【这里我没表述错误，标定出来的手眼关系T，

就是相机坐标系在末端坐标系下的位姿，

就是末端坐标系到相机坐标系的变换矩阵。】

【做个延伸，加深理解】

1. 机器人末端的位姿，就是一般可以直接读出的那个值，是末端坐标系在基座坐标系下的位姿，是基座坐标系到末端坐标系的变换矩阵，能将点在末端坐标系下的坐标转换到基座坐标系下。

2.标定板的位姿，一般是指标定板坐标系在相机坐标系下的位姿，也即相机坐标系到标定板坐标系的变换矩阵。

2. 不管眼在手上还是眼在手外，本质都是求解AX=XB中的X。

【具体怎么求，原理我们甚至都可以不去了解，只需要提供A和B，送到功能包（opencv，matlab，visp等）里自会返回结果。】

A和B只跟以下两个位姿相关 ：标定板（坐标系）在相机（坐标系）下的位姿，机器人末端（坐标系）在机器人基座（坐标系）下的位姿。

所以，不管是通过何种途径标定手眼关系，一定是需要准备多组上面讲的两种位姿的。

如何获取这两种位姿呢？前面也简单提供了些信息，末端位姿一般可以直接读出，本质就是机器人正运动学的一个结果；标定板在相机坐标系的位姿，需要搞个标定板，然后搞点代码检测出来，具体怎么搞，暂先不表。

重要的题外话2，机械臂末端：

这是一个困扰我多年的问题，机械臂末端到底在哪里？

我装一个不是原装配套的抓手，抓手抓取中心点几乎不会和末端重合的，那它们之间的位姿关系又是啥样的？不知道抓手和末端的位姿关系，还是不能用来抓取。

在笔者翻阅小量资料后，做出这样的判断：

一般来说，可以把机械臂最后一个link（连杆）看成是一个圆柱，那么末端坐标系的原点一般在这个圆柱的中心线或其延长线上，坐标系z轴一般沿着这条线指向外侧，x轴和y轴的方向也是确定的：正常来讲，把最后一个关节调到0点，然后将抓手以一个合理的姿态安装上去，则x轴和二指抓手的两根手指末端的连接线平行。整个坐标系满足右手定则，据此可判断y轴的方向。

末端坐标系的坐标原点具体在延长线的什么位置，不同的机械臂可能不同。不过Kinova机械臂的末端是在原装抓手的抓取中心，其他机械臂如果有原装抓手，大概也是这样。如果没有抓手，那也不要紧。为什么呢？

如果机械臂没有抓手，或者原装抓手被拆了，我们装上新的抓手，在新的抓手的最佳闭合抓取点定义一个坐标系，定义的该坐标系和末端坐标系只有z方向的一个平移差距。那么其实我们可以试探性地设置几个z值，然后控制机械臂相应运动，大致就能确定一个值z_diff。之后比如我们想让机器人抓手中心运动到（x,y,z)时，实际末端应该到的位置是（x,y,z-z_diff)，我们将这个坐标传送给机械臂控制代码即可。

---

实际标定过程

如前第2点所述，不管是眼在手上还是手外，手眼标定都需要：多组 “标定板（坐标系）在相机（坐标系）下的位姿，机器人末端（坐标系）在机器人基座（坐标系）下的位姿”。【看了前面讲的，应该可以理解这两个位姿的含义。】

因为Kinova支持ROS，realsense D435也支持ROS，我们可以直接利用用于手眼标定ROS库，有两个：easy_hand_eye 和 aruco_hand_eye。

这两个的原理都是一样的：需要提前打印一张aruco码出来；然后写好launch文件，运行，一步步操作即可。需要下载aruco-ros库用于aruco标定板的位姿求解，下载visp-ros库计算手眼关系。

网上教程比较多的是easy_hand_eye，可以搜一下相关博客，看一下具体操作流程和launch文件内容。不过大多是针对kinova 6轴机械臂的，需要对launch文件内容做相应调整(这是难点)。核心是坐标系的设置，你只需牢记，需要的两个位姿是“标定板（坐标系）在相机（坐标系）下的位姿，机器人末端（坐标系）在机器人基座（坐标系）下的位姿”。

提供几个链接：

kinova jaco2 和realsense d435i手眼标定-爱代码爱编程

Kinova JACO2机械臂与Azure Kinect手眼标定及坐标点的变换_WCaiL的博客-CSDN博客

Ubuntu18.04 + kinova joca2机械臂 + RealSense D435i深度相机进行eye to hand手眼标定_initative的博客-CSDN博客

但是笔者用easy_hand_aye标定了许多次，自认为操作没有问题，但是结果始终不对。

于是又尝试了aruco_hand_eye，成功。提供一个链接。

kinova_realsense 标定（eye-in-hand）-爱代码爱编程

具体的细节不再陈述，大家可以参照链接，有问题可以留言交流。