CN110497373B - 一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法 - Google Patents

Abstract

本发明公开了一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，本发明属于移动作业机器人自主目标识别、抓取及人机协作技术领域。其使用普通方形纸板作为机器人标定过程中的参照物，通过机器人所搭载的三维激光雷达进行三维点云采集，对获取的点云进行预处理，识别机械臂末端所夹持的方形纸板，得到机械臂在三维激光雷达中的坐标值，通过机械臂控制器获取其在控制器中的坐标值；重复调整机械臂位置N次(N≥8)，并获取其相应的坐标值，最后使用最小二乘法求解机械臂在三维激光雷达中的坐标与其控制器中的坐标转换关系。

Description

一种移动作业机器人的三维激光雷达与机械臂间的联合标定 方法

技术领域

本发明属于移动作业机器人自主目标识别、抓取及人机协作技术领域，涉及到三维激光雷达与机械臂之间的联合工作，特别涉及到三维激光雷达与机械臂之间的联合标定方法。

背景技术

移动作业机器人是由移动机器人底盘、安装在底盘上面的三维激光雷达和多自由度机械臂三个主要部件构成，其中底盘可实现机器人在工作场景中的大范围移动，三维激光雷达负责完成三维环境感知，机械臂用于目标物体抓取及人机协作，其构成示意如图1所示。移动作业机器人是集机械、控制、模式识别等多学科先进技术于一体的新型工业机器人，相对于传统工业机器人具有自主灵活作业、高安全性(无需安全隔离带)、与人共享工作空间等特性，具有广阔的应用前景。要实现具有自主作业、高安全性的新一代机器人系统，如何解决安装在底盘上的三维激光雷达与机械臂之间的坐标变换问题，即手眼配合问题是最根本的问题。

在移动作业机器人应用过程中，需要对目标物体实施精确定位与抓取，机器人的手眼配合精准度直接决定了其工作的可靠性，因此提高机器人所搭载的三维激光雷达传感器坐标系与机械臂坐标系之间的坐标变换精度，可以提高其工作可靠性。国内外学者对此已经展开了大量的研究工作，并取得了一定的研究成果，但是仍然具有一定的局限性与不确定性。文献(Malm H,Heyden A.Extensions of plane-based calibration to the caseof translational motion in a robot vision setting[J].IEEE Transactions onRobotics,2006,22(2):322-333.)介绍了一种传统的手眼标定方法，该方法使用已知的标定对象(黑白标定板)作为参照物，标定视觉与机械臂之间的关系；该方法将相机固定在机械臂末端，通过调整机械臂间接调整相机的位姿，以不同的姿态去拍摄标定对象，求解相机与机械臂之间的坐标转换关系。文献(HaixiaXu,YaonaWang,WeiChen,etc.A Self-Calibration Approach to Hand-Eye Relation Using a Single Point[C].Proceedingsof the 2008 IEEE International Conference on Information and Automation,Zhangjiajie China,2008:413-418)介绍了一种使用场景中已知单点参照物的手眼标定方法，该方法将相机固定于机械臂末端，控制相机以7种不同的位姿去观测场景中的已知参照物，得到参照物在相机中7组不同的坐标值，线性求解坐标转换矩阵即为相机到机械臂之间的转换矩阵。上述两种方法均将相机固定于机械臂末端，无法满足移动作业机器人视觉与机械臂分离的需求；同时相机双目测距精度与双目间基线有光，且误差与距离相关，距离越远误差越大，其测距性能不如激光稳定(激光测距不受距离限制)，无法满足移动作业机器人对大范围物体精确定位的需求。

文献(王英男,戴曙光.线激光器的手眼标定方法[J].电子科技,2015,28(7):183-184.)描述了一种使线激光器以不同位姿扫描空间已知的标定对象(标准球)的方法，获取不同的位姿坐标，从而获取机械臂与线激光器的位置关系。但是此方所得的转换精度受标定对象限制，特别是当线激光器距离标定对象较远时，其标定结果受到显著影响；同时，使用二维激光器无法完成三维环境感知，不能满足移动作业机器人对三维环境感知的需求。

发明内容

为了克服已有标定方法存在的缺陷与不足，本发明提供了一种基于三维激光雷达与机械臂间的标定方法，即手眼标定的方法。该方法可以实现移动作业机器人所搭载的三维激光雷达与机械臂间的自动标定，解决了传统标定方法对参照物的依赖，同时提高了标定精度。

本发明专利通过三维激光雷达对周围环境进行扫描，得到一个全局坐标系下的点云数据，通过点云匹配与场景识别技术，识别夹持在机械臂末端的方形纸板(纸板边长介于5cm至15cm之间)，精确获取机械臂末端执行器在三维激光雷达中的坐标P_c，接着通过机械臂控制器读取机械臂末端坐标P_w；然后控制机械臂末端执行器至另一点获取另外一组坐标P_c与P_w；重复控制机械手臂N(N≥8)次至不同位置，获取N组不同的坐标值；最后使用最小二乘法对所获取的N组坐标进行求解，得到三维激光雷达至机械臂末端的最优变换阵，即最优旋转矩阵R与平移矩阵T。

本发明技术方案如下：

一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，步骤如下：

一、控制机械臂末端运动位姿

通过机械臂控制器去改变机械臂末端位姿，使其末端出现在三维激光雷达可视区域，要求机械臂位姿与此前位姿均不相同。

二、激光点云数据预处理

尽管三维激光测距系统工作环境不受可见光限制，但是由于受到传感器本身的固有特性、激光的多次反射等影响，导致采集到的三维点云数据包含有孤立点噪声、随机噪声等缺陷，而且这些三维点云数据所包含的缺陷无法消除。受三维激光测距系统本身的原理与精度限制，会导致所测得是点云偏离实际位置或者误差完全不相同，因此在使用这些三维点云数据前，需要对其进行预处理与优化，即去除噪声点。在三维点云数据中，若某个指定数据点一定半径r范围内，其邻点个数少于一定的阀值时，则该点为孤立噪声点。如附图2所示，如果设定阀值个数为1(一定半径r范围的个数为1)，则a点为孤立噪声点，将会被剔除；如果设定阀值个数为2，则a点与b点都是孤立噪声点，都将被剔除；显然孤立噪点个数与所设定的阈值有关。孤立噪点的剔除处理与本发明所研究的对象没有任何关系，但是却会影响实验数据，因此对采集的三维点云数据进行滤波，就变得十分重要。

由于孤立噪声点一般具有一定半径r内邻近点较少或者不存在的特点，因此我们只需要构造点云K-D树索引，通过K-D树索引搜索该点一定半径r内的邻近点个数，依据邻近点个数是否小于所设定的阀值来判断此点是否为孤立噪点，孤立点噪声剔除流程如图3所式，本文中实验所选取的阀值个数为1。

三、机械臂末端执行器位姿的计算

由于机械臂末端所夹持的方形板具有明显的平面特性，我们利用平面属性进行局部平面拟合，然后将局部平面合并得到整个完整的平面，依据平面的面积确认是否为机械臂末端所夹持的方形板。

获取机械臂末端位姿步骤如下：

(1)利用三维激光点云空间邻域的关系，提取出完整平面；

具体如下：利用三维激光点云空间邻域的关系，定义一个k×k的邻域，每个领域包含P₁，P₂，……，P_n共n个激光点，则通过公式(5)可以计算该邻域的重心w，

设平面拟合后的单位法向量为n，如果该邻域为一个平面，则通过公式(6)所计算得到的误差e最小。当相邻的领域平面法向量足够小时，则认为该邻域的平面为同一平面，将此相邻的平面合并为同一个平面；通过迭代计算整个三维点云数据，可以提取出完整平面。

(2)依据提取的完整平面，确定平面的四个顶点在三维激光雷达中的坐标P_i＝(x_i,y_i,z_i)，其中i＝1，2，3，4，利用三维激光栅格计算其平面面积S，若等式|S-S_default|＜S_th成立，则认为此平面为机械手臂末端所夹持的方形板，其中S_default为方形板的实际面积，S_th为所测得的平面的面积与实际平面面积允许的最大误差。

(3)依据步骤(2)所计算得到的顶点坐标P_i，计算得到机械臂末端在三维激光雷达中的坐标

同时读取机械臂在控制器中的坐标P_w

四、获取机械手臂不同位姿下的坐标

重复上述步骤一至步骤三N次，其中N≥8，获取N组不同的坐标P_c、P_w。

五、最小二乘法求解最优旋转矩阵与平移矩阵

本专利将三维激光雷达坐标系标定到机械臂坐标系中，则有

P_w＝RP_c+T (7)

其中P_c为物体在三维激光雷达中的坐标，P_w为物体在机械臂中的坐标，R为旋转矩阵，T为平移矩阵。三维激光雷达坐标系与机械臂坐标系的映射关系如图4所示。

考虑到通过三维点云匹配所获取的坐标存在误差，本专利采集了N(N≥8)组机械臂末端不同的位姿坐标，通过最小二乘法对N组坐标计算求解旋转矩阵R与平移矩阵T，以获取最优的旋转矩阵与平移矩阵。通过最小二乘法求解的R与T，其误差函数如下所示：

其中本专利中N≥8，为了使公式(6)所示的误差公式所计算的值最小，必须分别求解R与T，其求解过程如下：

(1)计算两组坐标集合的质心

以及坐标集合与各自质心的偏移距离，构成两个新的坐标集合

与

其计算公式如下所示：

(2)依据最小二乘法原理，其旋转矩阵可由如下公式求出：

为了完成R的计算过程，需要依据公式(11)计算系数矩阵H，然后对H进行奇异值分解：H＝U∑V^T，最后计算X＝VU^T，U与V均为正交矩阵，如果矩阵X满足|X|＝1，则最优旋转矩阵R＝X，否则求解失败，需要重新获取新的坐标集合，进行重新求解。

(3)计算平移矩阵T，平移矩阵计算公式如下：

如果为了提高标定精度，可以采集更多的坐标集合，但是数据采集与计算时间会明显增加。

本发明的有益效果：该方法实现了移动作业机器人所搭载的三维激光雷达与机械臂之间的坐标转换，同时使用普通的方形板作为标定参照物，大大降低了对标定参照物的依赖，标定过程简单，提高了标定精度，有助于提升移动作业机器人高效准确的对大范围内目标物体的定位、抓取以及人机协作。本发明可以用在移动作业机器的场景理解、大范围内目标物体定位与抓取以及人机协作等人工智能领域。

附图说明

图1是移动作业机器人组构成意图。

图2是孤立噪点示意图。

图3是孤立噪点剔除流程图。

图4是三维激光雷达坐标系与机械臂坐标系转换示意图。

具体实施方法

本发明具体实施过程使用的三维激光雷达为Velodyne公司的16线激光VLP-16，机械臂为6自由度机器臂SRQ05A-6D以及其相应的控制器，机械臂末端所夹持的方形纸板为正方形纸板，其边长为10cm。本发明所提的方法同样适合于32、64线或者其他规格的三维激光雷达，以及其他自由度的机械臂。

以文本发明三维激光雷达与机械臂为例，尤其是16线三维激光与6轴机械臂。三维激光的水平扫描角度为360度，垂直扫描角度为±15度，扫描频率为10Hz，每秒约采集30万个点云数据；6轴机械臂具有6个自由度，有效负载5kg，关节运行速度可达180°/S。

第一步，通过机械臂控制器控制6轴机械臂，使其末端所夹持的纸板出现在三维激光雷达可扫描区域；利用三维激光雷达进行点云数据采集，通过孤立点滤波对所采集的数据进行预处理，然后对连续的激光数据进行ICP配准，得到全局的三维点云数据。

第二步，利用方形纸板的平面特性，将三维点云数据进行局部平面拟合，然后将局部平面扩展至整个平面。获取纸板顶点所在的四个坐标，然后计算得到纸板所在面积S，若等式|S-S_default|＜S_th成立，则认为此平面为机械手臂末端所夹持的正方形纸板，其中S_default为正方形纸片的实际面积，S_th为所测得的平面的面积与实际平面面积允许的最大误差，本实施方法中允许的最大误差S_th为4cm2。确认为机械臂末端所夹持的纸板后，依据其所在的坐标计算得到机械臂末端在三维激光雷达中的坐标P_c，同时读取机械臂末端在控制器中的坐标值P_w。如果三维点云中没有所要识别的参照物(方形纸板)，则重复第一步，调整机械臂所在的位姿。

第三步，重复第一步与第二步N次，其中N≥8，获取N组机械臂末端不同位姿下的坐标。完成对坐标数据的采集。

第四步，依据采集的N组P_c与P_w(P_c为机械臂末端在三维激光雷达中的坐标值，P_w为机械臂末端在控制器中的坐标值)，使用最小二乘法求解旋最优转矩阵R与平移矩阵T，使等式P_w＝RP_c+T成立。旋转矩阵R与平移矩阵T即为机器人所搭载的三维激光雷达坐标与机械臂坐标的转换矩阵。

Claims (9)

1.一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，其特征在于，步骤如下：

一、控制机械臂末端运动位姿；

二、激光点云数据预处理；

三、机械臂末端位姿的计算

(1)利用三维激光点云空间邻域的关系，提取出完整平面；

(2)依据提取的完整平面，确定平面的四个顶点在三维激光雷达中的坐标P_i＝(x_i,y_i,z_i)，其中i＝1，2，3，4，利用三维激光栅格计算其平面面积S，当等式|S-S_default|＜S_th成立时，则认为此平面为机械臂末端所夹持的方形板，其中S_default为方形板的实际面积，S_th为所测得的平面的面积与实际平面面积允许的最大误差；

(3)依据步骤(2)所计算得到的坐标P_i，计算得到机械臂末端在三维激光雷达中的坐标

同时读取机械臂末端在控制器中的坐标P_w

四、获取机械臂末端不同位姿下的坐标

重复上述步骤一至步骤三N次，其中N≥8，获取N组不同的坐标P_c、P_w；

五、最小二乘法求解最优旋转矩阵与平移矩阵

将三维激光雷达坐标系标定到机械臂末端坐标系中，则有

P_w＝RP_c+T (7)

其中P_c为机械臂末端在三维激光雷达中的坐标，P_w为机械臂末端在控制器中的坐标，R为旋转矩阵，T为平移矩阵；

通过最小二乘法对N组坐标计算求解旋转矩阵R与平移矩阵T，以获取最优的旋转矩阵与平移矩阵；通过最小二乘法求解的R与T，其误差函数如下所示：

2.根据权利要求1所述的一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，其特征在于，所述步骤二中，预处理方法为基于高斯分布的滤波算法。

3.根据权利要求2所述的一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，其特征在于，基于高斯分布的滤波算法中对于孤立噪声点的筛选方法为：由于孤立噪声点具有一定半径r内邻近点较少或者不存在的特点，因此只需要构造点云K-D树索引，通过K-D树索引搜索该点一定半径r内的邻近点个数，依据邻近点个数是否小于所设定的阀值来判断此点是否为孤立噪声点。

4.根据权利要求1、2或3所述的一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，其特征在于，提取出完整平面的具体过程为：定义一个k×k的邻域，每个邻域包含P₁，P₂，…，P_i，…，P_n共n个激光点，则通过公式(5)计算该邻域的重心w，

设平面拟合后的单位法向量为n，当该邻域为一个平面时，则通过公式(6)所计算得到的误差e最小；当相邻的邻域平面法向量夹角小于0.2度时，则认为该邻域的平面为同一平面，将此相邻的平面合并为同一个平面；通过迭代计算整个三维点云数据，便能提取出完整平面；

5.根据权利要求1、2或3所述的一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，其特征在于，方形板的长度与宽度均介于5cm与15cm之间；且S_th≤4cm²。

6.根据权利要求4所述的一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，其特征在于，方形板的长度与宽度均介于5cm与15cm之间；且S_th≤4cm²。

7.根据权利要求1、2、3或6所述的一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，其特征在于，步骤五中，R与T求解过程如下：

(1)计算两组坐标集合的质心

以及坐标集合与各自质心的偏移距离，构成两个新的坐标集合

与

其计算公式如下所示：

(2)依据最小二乘法原理，其旋转矩阵由如下公式求出：

依据公式(12)计算系数矩阵H，然后对H进行奇异值分解：H＝U∑V^T，其中，U与V均为正交矩阵，∑为对角矩阵，最后计算旋转矩阵X＝VU^T，当旋转矩阵X满足|X|＝1时，则最优旋转矩阵R＝X，否则求解失败，需要重新获取新的坐标集合，进行重新求解；

(3)计算平移矩阵T，平移矩阵计算公式如下：

8.根据权利要求4所述的一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，其特征在于，步骤五中，R与T求解过程如下：

(1)计算两组坐标集合的质心

以及坐标集合与各自质心的偏移距离，构成两个新的坐标集合

与

其计算公式如下所示：

(2)依据最小二乘法原理，其旋转矩阵由如下公式求出：

依据公式(12)计算系数矩阵H，然后对H进行奇异值分解：H＝U∑V^T，其中，U与V均为正交矩阵，∑为对角矩阵，最后计算旋转矩阵X＝VU^T，当旋转矩阵X满足|X|＝1时，则最优旋转矩阵R＝X，否则求解失败，需要重新获取新的坐标集合，进行重新求解；

(3)计算平移矩阵T，平移矩阵计算公式如下：

9.根据权利要求5所述的一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法，其特征在于，步骤五中，R与T求解过程如下：

(1)计算两组坐标集合的质心

以及坐标集合与各自质心的偏移距离，构成两个新的坐标集合

与

其计算公式如下所示：

(2)依据最小二乘法原理，其旋转矩阵由如下公式求出：

依据公式(12)计算系数矩阵H，然后对H进行奇异值分解：H＝U∑V^T，其中，U与V均为正交矩阵，∑为对角矩阵，最后计算旋转矩阵X＝VU^T，当旋转矩阵X满足|X|＝1时，则最优旋转矩阵R＝X，否则求解失败，需要重新获取新的坐标集合，进行重新求解；

(3)计算平移矩阵T，平移矩阵计算公式如下：

Images