CN109202912A - 一种基于单目深度传感器和机械臂配准目标轮廓点云的方法 - Google Patents

Abstract

本发明公开了一种基于单目深度传感器和机械臂配准目标轮廓点云的方法，机械臂(1)末端搭载单目深度传感器(2)，上位机(3)控制机械臂精准移动采样位置，控制单目深度相机拍摄目标轮廓点云并预处理，将当前点云标记为源点云S；通过控制柜(4)获取当前机械臂(1)各轴旋转角度值，根据运动学理论对机械臂(1)进行建模，并计算当前采样点单目深度传感器基于基坐标系的位姿；使用改进后的迭代最近点算法完成S与D两视角下点云的精确配准，将配准后的点云标记为源点云S；重复执行步骤S4、S5，再次配准下一视角下的点云，直到目标轮廓的点云模型完整，配准结束。

Description

一种基于单目深度传感器和机械臂配准目标轮廓点云的方法

技术领域

本发明涉及机器人技术及计算机视觉领域，尤其涉及一种基于单目深度传感器和机械臂配准目标轮廓点云的方法。

背景技术

三维点云模型是计算机视觉中的重要组成部分，是智能机器人与未知环境交互要研究解决的热点问题。单目深度视觉***一般利用红外线发射至目标，通过接受返回的光波来计算深度信息，有飞行时间法、结构光法等。在机械臂识别目标物体或者对目标物体的轮廓进行定位时，需要获取到目标不同视角下的全部轮廓信息，所以需要对不同视角下采集到的点云进行配准，现有方案无法在视角相差较大的情况下快速高效完成点云配准，并同时进行目标轮廓的准确定位。而深度传感器连续采集、逐步配准则计算复杂、硬件平台要求较高。

发明内容

本发明提供了一种基于单目深度传感器使用机械臂配准目标轮廓点云的方法，能够提高在拍摄视角相差较大情况下的配准效率，降低配准误差，并且获取目标轮廓的点云信息。

一种基于单目深度传感器和机械臂配准目标轮廓点云的方法，包含如下步骤：

步骤S1，根据目标对象(5)的类型及大小，确定最佳采样位置和最少采样方案；

步骤S2，机械臂(1)末端搭载单目深度传感器(2)，上位机(3)控制机械臂精准移动采样位置，控制单目深度相机拍摄目标轮廓点云并预处理，将当前点云标记为源点云S；

步骤S3，通过控制柜(4)获取当前机械臂(1)各轴旋转角度值，根据运动学理论对机械臂(1)进行建模，并计算当前采样点单目深度传感器基于基坐标系的位姿；

步骤S4，移动机械臂(1)使单目深度传感器(2)到达下一个采样位置，拍摄点云并预处理，将其标记为D，根据采样位置基于基坐标系的位姿，计算采样位置拍摄的点云数据间的刚体变换矩阵，对点云进行旋转平移变换，完成点云的初始配准；

步骤S5，使用改进后的迭代最近点算法完成S与D两视角下点云的精确配准，将配准后的点云标记为源点云S；

重复执行步骤S4、S5，再次配准下一视角下的点云，直到目标轮廓的点云模型完整，配准结束。

所述的方法，所述步骤S1的具体过程包括：

根据目标对象的外观形状，先在其四周采集几处点云数据，观察点云中含目标物体的有效点云数量占比，寻找占比较高的点云确定为最佳采样位置，寻找不同视角下的最佳采样位置，直到可以配准出完整的点云轮廓。

所述的方法，所述步骤S2的具体过程包括：

机械臂(1)末端搭载单目深度传感器(2)，上位机(3)控制机械臂精准移动采样位置A，控制单目深度相机(2)拍摄目标(5)轮廓点云，使用直通滤波器和统计滤波器对其滤波处理，去除目标轮廓对象外的背景及其他干扰数据，将当前点云标记为源点云S。

所述的方法，所述步骤S3的具体过程包括：

标定单目深度传感器相对于末端工具坐标系的位姿通过控制柜(4)传输到上位机(3)的数据，获取当前机械臂(1)各轴旋转角度值，根据运动学理论对机械臂(1)进行建模，得到机械臂的正运动学方程，将当前各轴角度值带入该方程，计算出该采样点末端工具坐标系基于基坐标系的位姿根据式推导单目深度传感器相对于基坐标系的位姿

所述的方法，所述步骤S4的具体过程包括：

移动机械臂(1)使单目深度传感器(2)到达下一个采样位置B，拍摄点云并预处理，将其标记为目标点云D，根据步骤S3计算采样位置B处，单目深度传感器基于基坐标系的位姿根据式计算采样位置A拍摄的点云数据S与采样位置B拍摄的点云数据D间的刚体变换矩阵，对点云进行旋转平移变换，完成点云的初始配准。

所述的方法，所述步骤S5的具体过程包括：使用改进的迭代最近点算法完成S与D两视角下点云的精确配准，根据不同采样位置处得到的视角A、B下点云数据间姿态变换矩阵得到两者间的旋转矩阵R与平移向量T，对源点云S＝{s₁,s₂,s₃...s_n}与目标点云D＝{d₁,d₂,d₃...d_n}，其中n为点云中点的个数，具体方法为：

1)对源点云S中的所有点，即s_i，i∈(1,n)分别带入以下式子p_i＝Rs_iT,计算点p_i与邻近范围内点的欧氏距离，找到D点云中距离p_i最近的点，假设为d_i；Pi是源点云Si经过矩阵变换后的点；R为旋转矩阵，T为平移向量；

2)设置匹配点对的最大距离为L，排除所有距离大于L的无效点对；

3)根据新得到匹配点对，求解新的旋转矩阵R和平移向量t，使目标误差函数

4)根据新求出的旋转矩阵R和平移向量T计算出新的p_i＝Rs_i+T，并找到目标点云中与其匹配的点云；

5)重新计算目标误差函数f，并求解本次误差函数与上次误差函数的差值，如果f的差值小于设定的阈值，说明迭代已经收敛，为了进一步减少误差，改进匹配重合度，以固定的数值减小步骤2)中的最大距离L，继续进行迭代；

6)若匹配点对的最大距离L小于所设定的阈值或者迭代次数等于设定的最高迭代次数，则算法计算结束；

完成上述两帧的点云配准后，根据式(1)计算下一个视角C与视角A的相对位置关系，初始配准获取旋转矩阵和平移向量，按照上述步骤进行精确配准，重复该操作直到完成完整目标轮廓点云的拼接。

与现有的技术相比，本发明具有以下优点：

本发明所述的基于单目深度传感器使用机械臂配准目标轮廓点云的方法中，通过对机械臂建模，根据各轴电机反馈回的关节角度值，计算出传感器的采集位姿，获取采集位置的位姿和采集到的点云集合后，使待配准的点云通过矩阵变换获得较为接近的位置关系，然后再利用改进的迭代最近点算法对点云进一步精确配准。通过机械臂精确的完成了点云的初始配准，减少了初始配准过程中的计算，避免了配准过程中陷入局部最小值。

附图说明

图1是本发明实施例的基于单目深度传感器使用机械臂配准目标轮廓点云的方法的流程示意图。

图2是本发明实施例使用机械臂配准点云***的结构示意图。

图2中，1表示机械臂，2表示单目深度传感器，3表示上位机控制器，4表示机械臂控制柜，5表示实验目标。

具体实施方式

以下结合具体实施例，对本发明进行详细说明。

参考图1，本实施例的基于单目深度传感器使用机械臂配准目标轮廓点云的方法包含如下步骤：

步骤S1，根据目标对象的类型及大小，确定最佳采样位置和最少采样点方案，根据目标对象的外观形状，先在其四周采集几处点云数据，观察点云中含目标物体的有效点云数量占比，寻找占比较高的点云确定为最佳采样位置，寻找不同视角下的最佳采样位置，直到可以配准出完整的点云轮廓；

步骤S2，机械臂(1)末端搭载单目深度传感器(2)，上位机(3)控制机械臂精准移动采样位置A，控制单目深度相机拍摄目标轮廓点云并预处理，将当前点云标记为源点云S；

步骤S3，通过控制柜(4)获取当前机械臂(1)各轴旋转角度值，根据运动学理论对机械臂(1)进行D-H建模，计算出该机械臂(1)的正运动学方程，将角度值带入该方程，计算出末端工具坐标系基于基坐标系的位姿标定单目深度相机与末端工具坐标系的相对姿态并计算当前采样点单目深度传感器基于基坐标系的位姿

步骤S4，移动机械臂(1)使单目深度传感器(2)到达下一个采样位置B，拍摄点云并预处理，将其标记为目标点云D，根据采样位置基于基坐标系的位姿计算采样位置拍摄的点云数据间的刚体变换矩阵对点云进行旋转平移变换，完成点云的初始配准；

步骤S5，使用改进的迭代最近点算法(ICP算法)完成点云S与点云D的精确配准，将配准后的点云重新标记为源点云S。

重复执行步骤S4、步骤S5，直到目标轮廓的点云模型完整，配准结束。

参考图2所示出了本实施例的机械臂配准点云***，该***包括：机械臂(1)，单目深度传感器(2)，上位机控制器(3)，机械臂控制柜(4)，待实验目标对象(5)。

图2中的机械臂(1)使用的是工业大型多轴机械臂，活动空间足够大，可以涵盖到目标对象(5)的四周，由上位机控制器(3)设定运动轨迹及运动模式，发送数据到机械臂控制柜(4)，由控制柜(4)内的伺服驱动器驱动机械臂(1)各轴运动，从而带动机械臂(1)末端的单目深度传感器(2)到的目标对象(5)的四周，上位机控制器(3)控制传感器(2)进行点云数据获取并进行下一步的处理及配准。

执行步骤S1，根据目标对象(5)的类型及大小，分析各位置点云信息的有效性，确定最佳采样位置和最少采样方案；

执行步骤S2，机械臂(1)末端搭载单目深度传感器(2)，上位机(3)控制机械臂精准移动采样位置，控制单目深度相机(2)拍摄目标轮廓(5)点云并预处理，将当前点云标记为源点云S；

执行步骤S3，通过控制柜(4)获取当前机械臂(1)各轴旋转角度值，根据运动学理论对机械臂(1)进行建模，并计算当前采样点单目深度传感器基于基坐标系的位姿；

具体而言，控制柜(4)获取机械臂各轴反馈回的数据，发送给上位机，上位机根据机械臂正运动学方程计算出该采样位置处的位姿。

执行步骤S4，移动机械臂(1)使单目深度传感器(2)到达下一个采样位置，拍摄点云并预处理，将其标记为D，根据采样位置基于基坐标系的位姿，计算采样位置拍摄的点云数据间的刚体变换矩阵，对点云进行旋转平移变换，完成点云的初始配准；

执行步骤S5，使用改进的迭代最近点算法完成S与D两视角下点云的精确配准，根据不同采样位置处得到的视角A、B下点云数据间姿态变换矩阵得到两者间的旋转矩阵R与平移向量T，对源点云S＝{s₁,s₂,s₃...s_n}与目标点云D＝{d₁,d₂,d₃...d_n}，其中n为点云中点的个数，具体方法为：

1)对源点云S中的所有点，即s_i，i∈(1,n)分别带入以下式子p_i＝Rs_iT,计算点p_i与邻近范围内点的欧氏距离，找到D点云中距离p_i最近的点，假设为d_i；Pi是源点云Si经过矩阵变换后的点。R为旋转矩阵，T为平移向量。

2)设置匹配点对的最大距离为L，排除所有距离大于L的无效点对。

3)根据新得到匹配点对，求解新的旋转矩阵R和平移向量t，使目标误差函数

4)根据新求出的旋转矩阵R和平移向量T计算出新的p_i＝Rs_i+T，并找到目标点云中与其匹配的点云。

5)重新计算目标误差函数f，并求解本次误差函数与上次误差函数的差值，如果f的差值小于设定的阈值，说明迭代已经收敛，为了进一步减少误差，改进匹配重合度，以固定的数值减小步骤2)中的最大距离L，继续进行迭代。

6)若匹配点对的最大距离L小于所设定的阈值或者迭代次数等于设定的最高迭代次数，则算法计算结束。

完成上述两帧的点云配准后，根据式(1)计算下一个视角C与视角A的相对位置关系，初始配准获取旋转矩阵和平移向量，按照上述步骤进行精确配准，重复该操作直到完成完整目标轮廓点云的拼接。

重复执行步骤S4、步骤S5直到目标轮廓的点云模型完整，配准结束。

综上，采用本实施例的基于单目深度传感器使用机械臂配准目标轮廓点云的方法，相较于其他使用算法估算点云间的刚性变换，可以更精确获取不同视角下点云间的初始配准，为后续ICP配准输入正确的输入参数，较好的重建出目标对象轮廓的点云模型，而且能大大提高点云的配准效率，节约硬件成本。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (6)

1.一种基于单目深度传感器和机械臂配准目标轮廓点云的方法，其特征在于，包含如下步骤：

步骤S1，根据目标对象(5)的类型及大小，确定最佳采样位置和最少采样方案；

步骤S2，机械臂(1)末端搭载单目深度传感器(2)，上位机(3)控制机械臂精准移动采样位置，控制单目深度相机拍摄目标轮廓点云并预处理，将当前点云标记为源点云S；

步骤S3，通过控制柜(4)获取当前机械臂(1)各轴旋转角度值，根据运动学理论对机械臂(1)进行建模，并计算当前采样点单目深度传感器基于基坐标系的位姿；

步骤S4，移动机械臂(1)使单目深度传感器(2)到达下一个采样位置，拍摄点云并预处理，将其标记为D，根据采样位置基于基坐标系的位姿，计算采样位置拍摄的点云数据间的刚体变换矩阵，对点云进行旋转平移变换，完成点云的初始配准；

步骤S5，使用改进后的迭代最近点算法完成S与D两视角下点云的精确配准，将配准后的点云标记为源点云S；

重复执行步骤S4、S5，再次配准下一视角下的点云，直到目标轮廓的点云模型完整，配准结束。

2.根据权利要求1所述的方法，其特征在于，所述步骤S1的具体过程包括：

根据目标对象的外观形状，先在其四周采集几处点云数据，观察点云中含目标物体的有效点云数量占比，寻找占比较高的点云确定为最佳采样位置，寻找不同视角下的最佳采样位置，直到可以配准出完整的点云轮廓。

3.根据权利要求1所述的方法，其特征在于，所述步骤S2的具体过程包括：

机械臂(1)末端搭载单目深度传感器(2)，上位机(3)控制机械臂精准移动采样位置A，控制单目深度相机(2)拍摄目标(5)轮廓点云，使用直通滤波器和统计滤波器对其滤波处理，去除目标轮廓对象外的背景及其他干扰数据，将当前点云标记为源点云S。

4.根据权利要求1所述的方法，其特征在于，所述步骤S3的具体过程包括：

标定单目深度传感器相对于末端工具坐标系的位姿通过控制柜(4)传输到上位机(3)的数据，获取当前机械臂(1)各轴旋转角度值，根据运动学理论对机械臂(1)进行建模，得到机械臂的正运动学方程，将当前各轴角度值带入该方程，计算出该采样点末端工具坐标系基于基坐标系的位姿根据式推导单目深度传感器相对于基坐标系的位姿

5.根据权利要求1所述的方法，其特征在于，所述步骤S4的具体过程包括：

移动机械臂(1)使单目深度传感器(2)到达下一个采样位置B，拍摄点云并预处理，将其标记为目标点云D，根据步骤S3计算采样位置B处，单目深度传感器基于基坐标系的位姿根据式计算采样位置A拍摄的点云数据S与采样位置B拍摄的点云数据D间的刚体变换矩阵，对点云进行旋转平移变换，完成点云的初始配准。

6.根据权利要求1所述的方法，其特征在于，所述步骤S5的具体过程包括：使用改进的迭代最近点算法完成S与D两视角下点云的精确配准，根据不同采样位置处得到的视角A、B下点云数据间姿态变换矩阵得到两者间的旋转矩阵R与平移向量T，对源点云S＝{s₁,s₂,s₃...s_n}与目标点云D＝{d₁,d₂,d₃...d_n}，其中n为点云中点的个数，具体方法为：

1)对源点云S中的所有点，即s_i，i∈(1,n)分别带入以下式子p_i＝Rs_iT,计算点p_i与邻近范围内点的欧氏距离，找到D点云中距离p_i最近的点，假设为d_i；Pi是源点云Si经过矩阵变换后的点；R为旋转矩阵，T为平移向量；

2)设置匹配点对的最大距离为L，排除所有距离大于L的无效点对；

3)根据新得到匹配点对，求解新的旋转矩阵R和平移向量t，使目标误差函数

4)根据新求出的旋转矩阵R和平移向量T计算出新的p_i＝Rs_i+T，并找到目标点云中与其匹配的点云；

5)重新计算目标误差函数f，并求解本次误差函数与上次误差函数的差值，如果f的差值小于设定的阈值，说明迭代已经收敛，为了进一步减少误差，改进匹配重合度，以固定的数值减小步骤2)中的最大距离L，继续进行迭代；

6)若匹配点对的最大距离L小于所设定的阈值或者迭代次数等于设定的最高迭代次数，则算法计算结束；

完成上述两帧的点云配准后，根据式(1)计算下一个视角C与视角A的相对位置关系，初始配准获取旋转矩阵和平移向量，按照上述步骤进行精确配准，重复该操作直到完成完整目标轮廓点云的拼接。