3D视觉机器人中手眼标定的精度提升方法记录——ICP算法

目录

• 1.ICP算法
• • [1.1 point-to-point ICP算法](#1.1 point-to-point ICP算法)
• 2.3D视觉机器人中手眼标定的精度提升方法记录------ICP算法
• • [2.1 理论说明](#2.1 理论说明)
  • [2.2 求解思路](#2.2 求解思路)
  • [2.3 TODO（待验证）](#2.3 TODO（待验证）)

1.ICP算法

推荐：https://mp.weixin.qq.com/s/s99eC_JSTk0_vZHAQOwyFg

精配准算法主要有两种形式：一是ICP(Iterative Closest Point)类算法，基于环境的几何结构特征匹配；二是NDT(Normal Distributions Transform)类算法，基于对环境特征的概率描述，使得source和target的点云概率分布尽可能接近。

传统的ICP算法主要考虑的是point-to-point 配准，但是该算法鲁棒性较差，对初值的要求非常高，算法迭代非常容易陷入局部最优。因此，后续在point-to-point 的基础上，演变出了point-to-plane 以及point-to-line的ICP算法。

1.1 point-to-point ICP算法

• 算法原理：
  目标：使得两个点云配准后，距离平方和最小
  
• 算法步骤：
  

感谢视频：https://www.bilibili.com/video/BV1VX4y1c7jh/?spm_id_from=333.880.my_history.page.click&vd_source=858585879400a2acad4b4d9a0283f25d

ICP算法：

给定两个点云之间的对应点匹配关系以后，如何计算一个旋转矩阵R和一个平移矩阵t，通过R和t实现点云配准。

首先应该对两个点云进行一个去质心的操作，然后将去质心以后的对应点相乘，对前N项进行一个求和，得到一个3x3的矩阵，再对这个矩阵进行奇异值分解，将奇异值分解以后得到的矩阵V乘以矩阵U的转置，得到我们想要的旋转矩阵R，再把这个R代入到一个公式中，用CQ减去R乘以CP得到了平移矩阵t。

https://www.bilibili.com/video/BV1z9vWemEiH/?spm_id_from=333.880.my_history.page.click

2.3D视觉机器人中手眼标定的精度提升方法记录------ICP算法

推荐：https://www.bilibili.com/video/BV1VX4y1c7jh/?spm_id_from=333.880.my_history.page.click&vd_source=858585879400a2acad4b4d9a0283f25d

2.1 理论说明

• 背景说明
  以眼在手外为例，目的是求解相机到机械臂基座的空间变化关系，即T_cam_to_base

按照正常的步骤是两步 ：

1.将标定板固定在机械臂末端，获取多个机械臂姿态下标定板相对于相机的位姿即T_board_to_cam

2.根据AX=XB公式，即opencv提供的解算方法，得到相机到机械臂基座位姿关系，即T_cam_to_base （手眼标定实际是求解AX=XB方程，这是一个线性方程组，这个方程的未知数要求了最低的拍照数量，数量少就构造了一个欠定方程组，解为无限个。）

• 存在的问题
  正常完成上面两步以后就能求解到结果，T_cam_to_base的精度受到输入数据的影响，如果采集的数据不好，可能得到的结果精度就会很差（和拍摄的位置和拍照数量都有关系，一般要求13个指定拍摄位姿，得到的精度相对高一点）。换种说法，只通过上面这两步得到的结果其精度是有上限的，如果需要更高的精度，需要另想办法。

在（四）3D视觉机器人的手眼标定（眼在手外）中计算得到的眼在手外矩阵，精度误差较高，需要进一步提高精度。

• 解决方法
  解决方法就是ICP（Iterative Closest Point）。

原理： 在机械臂的基坐标系中，3D相机的坐标系是一个刚体变换的关系，它们之间没有缩放存在，也就是不是仿射变换，而是刚体变换，我们所做的任何标定其实就是来求解这个空间变换矩阵（3D相机相对于机械臂基座）。

换个思路想，只需要知道在相机坐标系下若干个点的坐标，然后再知道在机械臂基坐标系下若干个点的坐标，通过求解空间变换的方法，来求得3D相机相对于机械臂基座的空间变换矩阵。

常用的求解方法就包括ICP，但是ICP算法的前提是有两套点，场景点和模板点，它们之间的空间变换关系一定是刚体变换。但是为什么在一开始就没有使用ICP方法进行解算呢？因为ICP有一个限制，是对初值的要求，即那两套点一定要变换关系比较小，换句话说就是ICP对迭代初值很敏感，如果那两套点的空间变化关系比较大，那ICP可能会收敛到一个局部最优解而非增减，最后导致求解失败。

所以经过最初的两个步骤之后，已经得到了一个空间变换矩阵（相机到机械臂基座位姿关系，即T_cam_to_base），这个空间变换矩阵虽然它还有误差，但是它已经接近真值了，这时候就可以作为ICP的一个初值来使用。

2.2 求解思路

在前两步骤的基础上，增加第三步。

• 1.首先准备一个标定板（这里以棋盘格标定板为例），将其放置于相机视野内的任何一个地方，放置之后用相机来拍摄这个标定板，获取标定板上面所有角点的坐标（x,y,z），因为是3D相机，所以知道每个角点在相机坐标系下的坐标，将其设为 ^camP_i ，表示在相机坐标系下点的位置信息，所有的角点可以表示为：^camP_i = [ ^camx_i ,^camy_i ,^camz_i ]^T

• 2.同样的，在机械臂末端装一个尖端，设置好TCP后，移动机械臂去戳标定板中的每个角点，通过机器人示教器上面的读数，可以得到每个角点在机械臂基座标系下的位置信息，将其设为 ^baseP_i ，表示在机械臂基座标系下角点的位置信息，所有的角点可以表示为：^baseP_i = [ ^basex_i ,^basey_i ,^basez_i ]^T

• 3.至此我们获取了同一个角点在相机坐标系和在机械臂基坐标系下的坐标，将上面所有的角点坐标整理为一个集合，这个集合包含了所有的角点在两组坐标系下的位置坐标，将上面这些点代入ICP中进行计算（直接调用PCL的ICP），其中ICP的初值可以用之前两步计算得到的结果 T_cam_to_base

ICP是为了得到T：^baseP_i = T · ^camP_i

• 4.最终求解相机到机械臂基座的空间变化矩阵是：
  T^'_cam_to_base = T · T_cam_to_base

2.3 TODO（待验证）

1.PCL的ICP计算还没验证过

2.按照视频的ICP求解过程，需要在机械臂末端装一个尖端，然后在一张棋盘格标定板上面戳所有的角点并获取所有角点的T_point_to_base，这一步很耗时（但实际实际中只需要戳三个点（左上角、右上角、左下角）就可以做ICP计算得到那个变换矩阵T。猜测：ICP计算可能有最少点的限制，3个也能做计算，需要实际用代码验证。）