[Halcon&3D] 3D手眼标定理论与示例解析

📢博客主页：https://loewen.blog.csdn.net
📢欢迎点赞 👍 收藏 ⭐留言 📝 如有错误敬请指正！
📢本文由丶布布原创，首发于 CSDN，转载注明出处🙉
📢现在的付出，都会是一种沉淀，只为让你成为更好的人✨

文章预览：

一. 3D手眼标定理论基础

因为3D相机知道的是点云坐标，机械手是空间坐标系，分为基础（底座）坐标系和工具（末端）坐标系，手眼标定目的就是将相机的图像坐标系的坐标和机械手的基础坐标系的坐标进行相互转化（即求空间变换矩阵）。

在实际控制中，3D相机对场景中的物体扫描，并进行「三维匹配」，获取到物体的「姿态Pose」后，以3D点云的形式交给机械臂，通过标定好的坐标转换矩阵将相机获取到的3D点云坐标变换到机械手的基础坐标系中，然后根据机械手坐标系计算出各个电机该如何运动，从而控制机械手到达指定位置，可以用于物体抓取、无序分拣、装配、打磨等工作。

注：

1、如果是直接购买市面上的3D相机使用，在其生成对象的点云数据时，其内部已经完成了图像坐标系到世界坐标系的标定转换，所以需要我们做的就变成了求对象所在的世界坐标系（传感器坐标系）和机械手所在的基础坐标系之间的转换矩阵，本文默认相机内部已经完成图像坐标系到世界坐标系的标定，即相机坐标系（传感器坐标系）等价于世界坐标系。
2、若机械手中心不在法兰中心，需要做TCP（Tool Center Point）标定，得到机械手中心和法兰中心之间的相对位置关系，有四点法、六点法等。

场景示意图：

在这里插入图片描述

和2D标定一样，根据相机的装载位置分为两种情况：

eye-to-hand（眼在手外）：相机固定在一个地方，机械手的运动不会带着相机一起移动；
eye-in-hand（眼在手上）：相机安装在机械手上，随着机械手一起移动；

本质都是为了在机器人基础坐标系下，获取物体的姿态Pose情况（注意区分：2D手眼标定得到的是XY坐标位置和角度），便于机械手完成抓取等任务。本文主要介绍的是eye-to-hand。

二. 3D手眼标定流程（eye-to-hand）

参考示例：`分类 — 方法 — 三维匹配（基于表面）` —— `calibrate hand eye stationary 3d sensor.hdev`

1、创建标准件的点云模型

* 1.读入点云以及创建模型
read_object_model_3d ('handeye/robot_gripper_3d_model.om3', 1, [], [], OM3DModel, Status)
create_surface_model (OM3DModel, 0.03, [], [], SurfaceModelID) //0.03代表采样距离在点云最小外界球体直径所占的比例
* 简化点云（针对散点），提高匹配速度
sample_object_model_3d (OM3DModel, 'fast', 0.0009, [], [], SampledObjectModel3D) //0.0009代表采样距离

PS：

simplify_object model_3d：也是简化点云，和上面sample_object_model_3d算子区别是它针对三角化之后的点云进行简化。
create_surface_model：创建点云模板，算子各个参数的含义：传送门。

读取的点云模型如图所示：

在这里插入图片描述

2、创建手眼标定模型、多角度匹配标准件的实例点云数据

流程梳理：

获取tool_in_base_pose，即工具坐标系相对于机器人基础坐标系下的姿态Pose —可从机器人示教器上读取到；
PS：可通过write_pose将机器人工具坐标系的默认姿态通过机器人示教器获取到，然后保存至本地，最后通过read_pose读取出来即可。
匹配标准件的实例点云数据，获取标准件的姿态Pose；
将步骤1和步骤2获取到的姿态Pose，设置进标定模型中；

代码展示：

* 2.创建手眼标定模型
create_calib_data ('hand_eye_stationary_cam', 0, 0, HECCalibDataID)
for I := 1 to 15 by 1
    * 读取工具坐标系相对于机器人基础坐标系下的姿态
    read_pose ('tool_in_base_pose_' + I$'02d' + '.dat', ToolInBasePose)
    * 重新读取物体三维点云模型呢，通过对物体进行三维匹配获得其相对于世界坐标系（或传感器坐标系）下的姿态
    filename := 'handeye/robot_gripper_3d_scene_' + I$'02d'
    read_object_model_3d (filename, 1, [], [], OM3DScene, Status1)
    * 0.05也是一个比例，因为前面我们创建模板的时候对采样点进行了缩减，所以这里在重新读取一个点云模型时，也适当忽略一些点进行匹配
    * 输出获取匹配对象的姿态ObjInCamPose
    find_surface_model (SurfaceModelID, OM3DScene, 0.05, 1, 0, 'false', [], [], ObjInCamPose, Score, SurfaceMatchingResultID)
    * 判断模型是否被找到
    if (|Score|)
        * 将上面的工具坐标系相对于基础坐标系的姿态以及匹配对象的姿态设置到标定模型中
        set_calib_data (HECCalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)
        set_calib_data_observ_pose (HECCalibDataID, 0, 0, I, ObjInCamPose)
    endif
    clear_object_model_3d (OM3DScene)
endfor

3、开始手眼标定

 * 3.开始手眼标定
calibrate_hand_eye (HECCalibDataID, HECPoseError)

标定完之后：

可以获得世界坐标系（传感器坐标系）相对于机器人基础坐标系下的姿态；
又已知工具坐标系相对于机器人基础坐标系下的姿态；

所以可知工具坐标系相对于世界坐标系（传感器坐标系）下的姿态，从而完成机器手的抓取。

4、获得各坐标系之间的姿态关系

* ⑴机器人基础坐标系相对于相机（传感器）坐标系的姿态 
get_calib_data (HECCalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInSensorPose)
* ⑵物体所在的世界坐标系相对于工具坐标系的姿态
get_calib_data (HECCalibDataID, 'calib_obj', 0, 'obj_in_tool_pose', ObjInToolPose)

5、物体在机器人基础坐标系下的姿态求解

步骤：

对物体进行三维点云匹配获取物体相对于传感器的姿态ObjInCamPose；
对标定得到的基础坐标系相对于传感器坐标系的姿态BaseInSensorPose进行翻转，获取传感器坐标系相对于基础坐标系的姿态SensorInBasePose；
的SensorInBasePose联合的三维匹配得到的物体相对于传感器的姿态ObjInCamPose，最终求出物体在机器人基础坐标系下的姿态ObjInBasePose；
最终将物体相对于机器人基础坐标系下的姿态信息通过通讯传给机器人，从而命令其完成相关指令；

代码：

* 1.相机3D扫描，获取物体点云数据
read_object_model_3d (filename, 1, [], [], OM3DScene, Status1)
*   对物体进行3D点云匹配，获取物体在传感器坐标系下的姿态ObjInCamPose
find_surface_model (SurfaceModelID, OM3DScene, 0.05, 1, 0, 'false', [], [], ObjInCamPose, Score, SurfaceMatchingResultID)
* 2.物体在机器人基础坐标系下的姿态求解
*   翻转标定得到的基础坐标系相对于传感器坐标系下的姿态BaseInSensorPose，获得SensorInBasePose
pose_invert (BaseInSensorPose,SensorInBasePose)  
*   SensorInBasePose联合ObjInCamPose，求解物体在机器人基础坐标系下的姿态
pose_compose (SensorInBasePose, ObjInCamPose, ObjInBasePose)

下雨天，最惬意的事莫过于躺在床上静静听雨，雨中入眠，连梦里也长出青苔。