CN104626206A - 一种非结构环境下机器人作业的位姿信息测量方法 - Google Patents

Abstract

本发明公开了一种非结构环境下机器人作业相对位姿信息的测量方法，包括：创建机器人当前工作空间的实时三维模型；在机器人机械臂末端夹具上设置位姿标记板；在位姿标记板上画标记直线箭头；根据标记板颜色检索位姿标记板；根据标记板三维点云轮廓信息获取标记直线箭头的矢量坐标；根据直线箭头的矢量坐标，计算机械臂末端夹具在相机坐标系中的位置和姿态；计算目标物与机械臂末端夹具位置和姿态、障碍物与机械臂相对位置以及双臂之间的相姿态。本方法可以为移动机器人在非结构化环境下对目标物实现无碰撞的自主抓取提供相应的位姿信息。

Description

一种非结构环境下机器人作业的位姿信息测量方法

技术领域

本发明属于机器人视觉伺服及目标信息获取领域，具体涉及一种非结构环境下机器人作业的位姿信息测量方法。

背景技术

“福岛核电站事故”对核电的安全可靠性提出了警钟，“福岛核电站事故”各国均对核电的安全可靠性及发展核电的必要性进行了深入探讨。在这种情况下，通过装载视觉系统的智能特种机器人替代人力进入这些高危的环境中开展自主监测和维护工作成为一种有效方法和热门趋势。

由于机器人工作的环境是一种非结构化环境，非结构化环境的特征是表面材质性能不均，结构及尺寸变化不规律且不稳定，环境信息非固定、不可知、不可描述。因此，机器人工作的环境是一种未知、复杂的环境。

现有的装载视觉系统自主抓取作业机器人中根据摄像机与机器人的相互位置的不同，分类分为Eye-in-Hand系统和Eye-to-Hand系统。Eye-in-Hand系统的摄像机安装在机器人手部末端，在工业机器人中的应用比较广泛。但是在非结构化环境应用中，系统视场处在变化之中，不能保证作业目标一直在视场中，有时会存在丢失作业目标的现象。Eye-to-Hand系统的摄像机安装在视野包含目标物区域的某一固定位置，由于摄像机不随之一起移动，使用该方案的机器人进入非结构化环境进行自主作业的空间范围有限。

同时，传统机械臂自主控制中，机械臂末端夹具的控制依靠机械臂运动控制电机转动实现，并且只能通过计算控制电机转动过程及转动量计算当前机械臂末端夹具在空间移动量和旋转量，因此，末端夹具在当前环境中的相对位置和姿态是未知的。同时，传统的机械臂末端的动作完成，依据机械臂的路径规划伺服发出指令直接执行相应动作，无法实现实时反馈动作执行过程是否会发生碰撞等意外情况。

发明内容

本发明提供了一种非结构环境下机器人作业的位姿信息测量方法，可以为移动机器人在非结构化环境下对目标物实现无碰撞的自主抓取提供相应的位姿信息。

本发明的一个实施例提供了非结构环境下机器人作业的位姿信息测量方法，包括：

创建机器人当前工作空间的实时三维模型；

在机器人机械臂末端夹具上设置位姿标记板，位姿标记板与夹具同轴转动；

在位姿标记板上画标记直线箭头，标记直线箭头与夹具的夹缝呈垂直关系；

获取标定直线箭头的矢量坐标，计算得到机械臂末端夹具当前的位置和夹具旋转角度；

测量机器人机械臂伸缩的最长纵向和横向距离，提取三维点云坐标值中Z方向绝对值小于最长纵向距离并且Y方向绝对值小于最长横向距离三维点云进行位姿标记板检索；

利用三维点云图存储的彩色信息，将RGB值归一化，根据标定板颜色，对比CIE1931-RGB标准色度表成功检索到位姿标记板；

对检索到的位姿标记板三维点云图区域进行灰度化和滤波去噪处理，并采用Canny算子对所述图像进行二值化处理，及检测图像的轮廓；

利用霍夫变换方法提取二值化图像中直线箭头的三维点云信息；以及

根据直线箭头的三维点云信息，计算机械臂末端夹具在相机坐标系中的位置和姿态。

本发明的有益效果为：

1.本发明的位姿测量方法提高了机器人在非结构化环境下自主抓取的可行性和精确度，既可以避免Eye-in-Hand系统的位姿测量方法存在的丢失作业目标的缺点，也可以避免Eye-to-Hand系统的位姿测量方法作业的空间范围有限的缺点；可实时反馈机械臂在机器人当前工作区间的相对位置和姿态，有效实现机械臂对目标物实现无碰撞的自主抓取。

2.基于视觉的机械臂末端夹具的位姿测量方式适用于任意形状的夹具，具有应用广泛的优点。

3.视觉系统在安装选定的相机之初进行一次相机参数标定后，可在任意未知非结构化环境中，无需相机标定，实时为机械臂自主抓取提供目标物相对位置和姿态信息。

附图说明

图1所示为本发明的一种非结构环境下机器人作业的位姿信息测量方法100的流程图；

图2所示为本发明的创建机器人当前工作空间的实时三维模型的方法200的流程图；

图3所示为本发明的直线箭头姿态测量示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明具体实施例及相应的附图对本发明技术方案进行清楚、完整地描述。

参考图1，图1所示为本发明的一种非结构环境下机器人作业的位姿信息测量方法100的流程图。实施例100包括如下步骤101至109。

在步骤101中，创建机器人当前工作空间的实时三维模型。

机器人当前工作空间的实时三维模型是指用于对机器人当前工作区域建立适合计算机表示和处理的数学模型，对其进行处理、操作和分析。工作区域重建不仅能够为机器人的导航、定位以及避障提供必要的信息，而且大大提升了机器人感知外部环境、辨识真实场景中物体几何特性的能力，从而使移动机器人机械臂自主作业过程更好避障、定位以及对作业目标的操作。

参考图2，在本发明的一个实施例中，步骤101可以进一步包括步骤201～207。

在步骤201中，用两个相机同时采集机器人当前工作空间的实时图像，得到两个彩色图像。

相机成像过程是将客观场景的三维物体投影到二维图像平面的过程。因此，在本发明的一个实施例中，在采集图像之前，需要对视觉系统相机进行参数标定。

在本发明的一个实施例中，采用张正友的双目标定方法对视觉系统相机进行摄像机内参数标定和摄像机之间的结构参数(即外参数)标定。

在步骤202中，获取两个彩色图像中某一相同像点的二维图像坐标信息。

在本发明的一个实施例中，可以利用图像匹配算法得出两个图像中某一相同像点的二维图像坐标信息。

在步骤203中，计算所述像点在两个彩色图像中的视差，得到像点在机器人当前工作空间中的深度信息。

在步骤204中，组合二维图像坐标信息和深度信息，获取像点在机器人当前工作空间中的三维坐标信息。

在步骤205中，组合机器人当前工作空间实时图像中所有点的三维坐标信息，生成机器人当前工作空间的三维点云图。

在步骤206中，将彩色图像的颜色信息映射到三维点云图中，得到机器人当前工作空间的彩色三维点云图。

在步骤207中，根据彩色三维点云图重建机器人当前的当前工作空间。

在步骤102中，在机器人机械臂末端夹具上设置位姿标记板，位姿标记板需要与夹具同轴转动。

在步骤103中，在位姿标记板上画标记直线箭头，标记直线箭头需要与夹具的夹缝呈垂直关系。

在步骤104中，获取标定直线箭头的矢量坐标，计算得到机械臂末端夹具当前的位置和夹具旋转角度。

由于位姿标记板与夹具为同轴转动，位姿标记板的标记直线箭头与夹缝呈垂直关系。因此，在本发明的一个实施例中，通过获取标记直线箭头的位置和姿态即可获得机械臂末端夹具当前的位置和夹具旋转角度。

在步骤105中，测量机器人机械臂伸缩的最长纵向和横向距离，提取三维点云坐标值中Z方向绝对值小于最长纵向距离并且Y方向绝对值小于最长横向距离三维点云进行位姿标记板检索。

在本发明的一个实施例中，测量机器人机械臂伸缩的最长纵向和横向距离，分别用K1和K2表示，提取坐标值中|Z|＜K1且|Y|＜K2的三维点云进行位姿标记板检索；假设未知的非结构化环境出现与位姿标记板体征相近物体，此方案将有效降低位姿标记板检测错误率。

在步骤106中，利用三维点云图存储的彩色信息，将RGB值归一化，根据标定板颜色，对比CIE1931-RGB标准色度表成功检索到位姿标记板。

在步骤107中，对检索到的位姿标记板三维点云图区域进行灰度化和滤波去噪处理，并采用Canny算子对所述图像进行二值化处理，及检测图像的轮廓。

在步骤108中，利用霍夫变换方法提取二值化图像中直线箭头的三维点云信息。

直线位姿测量原理如图3所示。在直线L_K上，任选两点P₁和P₂作为特征点。这两个特征点构成的向量，在摄像机坐标系C₁和C₂中分别表示为¹L_P12和²L_P12。点P_i在摄像机坐标系C₁和C₂中的位置向量，分别表示为P_c1j和P_c2j。利用¹P_c2j表示P_c2j在摄像机坐标系C₁中的向量。因此，在本发明的一个实施例中，两台摄像机之间的姿态变换矩阵记为¹R₂，为已知向量。由图3可知，向量¹L_P12、¹P_c11和¹P_c12构成的向量三角形是线性相关的。因此有：

$\left|\begin{array}{ccc}{P}_{C_{1}1x}& P_{C_{1}2x}& L_{P12x}^1\\ P_{C_{1}1y}& P_{C_{1}2y}& L_{P12y}^1\\ P_{C_{1}1z}& P_{C_{1}2z}& L_{P12z}^1\end{array}\right|=0$

类似的，向量¹L_P12、¹P_c11和¹P_c12同样构成如同上式的线性相关的向量三角形。又有步骤104中获得的两个摄像机外参，用表示。由上述条件和向量约束||¹L_p12||＝1可求出单位向量₁L_P12即获得了该标记直线箭头在相机坐标系中的位姿信息。

在步骤109中，根据直线箭头的三维点云信息，计算机械臂末端夹具在相机坐标系中的位置和姿态。

至此完成了机器人在非结构环境下作业的位姿信息测量。

机器人在非结构环境下作业，需要抓取目标物，因此，需要获取目标物与机械臂末端夹具之间的相对位姿信息。因此，在本发明的一个实施例中，本发明的方法还进一步包括在线实时提供目标物与机械臂末端夹具在机械臂控制端坐标系中的相对位置和姿态的步骤，该步骤包括：

以相机的云台的旋转中心为原点建立相机坐标系；

以机械臂控制端旋转中心为原点建立机械臂控制端坐标系；

获取机械臂末端夹具在机械臂控制端坐标系中的三维坐标值；

获取目标物在相机坐标系中的三维坐标值，计算得到目标物在机械臂控制端坐标系中的三维坐标值；以及

计算得到目标物与机械臂末端夹具在机械臂控制端坐标系中的三维相对位置和姿态。

在机器人抓取目标物的过程中，需要时刻防止机器人机械臂与工作空间突出物碰撞以及机械臂之间相互碰撞。因此，在本发明的一个实施例中，本发明的方法还进一步包括实时测量所述环境障碍物突出菱角到机械臂各关的距离以及双臂相互位姿关系的步骤，该步骤包括：

获取机械臂末端夹具在相机坐标系中的三维坐标值，依据D-H坐标逆变换原理由机械臂末端夹具在相机坐标系中的三维坐标求取机械臂每个关节点在相机坐标系中的三维坐标值；

以左机械臂各关节三维坐标为原点，分别与右机械臂对应关节建立三维矢量坐标，实时计算反馈矢量方向，控制矢量方向小于90度；

获取当前工作空间实时三维模型中障碍物突出菱角在相机坐标系中的三维坐标值；

建立障碍物突出菱角与每个关节点的三维坐标矢量并计算矢量模长，实时反馈矢量模长。

虽然以上述较佳的实施例对本发明做出了详细的描述，但并非用上述实施例限定本发明。本领域的技术人员应当意识到在不脱离本发明技术方案所给出的技术特征和范围的情况下，对技术特征所作的增加、以本领域一些同样内容的替换，均应属本发明的保护范围。

Claims (4)

1.一种非结构环境下机器人作业的位姿信息测量方法，包括：

创建机器人当前工作空间的实时三维模型；

在机器人机械臂末端夹具上设置位姿标记板，所述位姿标记板与所述夹具同轴转动；

在所述位姿标记板上画标记直线箭头，所述标记直线箭头与所述夹具的夹缝呈垂直关系；

获取所述标定直线箭头的矢量坐标，计算得到机械臂末端夹具当前的位置和夹具旋转角度；

测量机器人机械臂伸缩的最长纵向和横向距离，提取三维点云坐标值中Z方向绝对值小于最长纵向距离并且Y方向绝对值小于最长横向距离三维点云进行位姿标记板检索；

利用三维点云图存储的彩色信息，将RGB值归一化，根据标定板颜色，对比CIE1931-RGB标准色度表成功检索到位姿标记板；

对所述检索到的位姿标记板三维点云图区域进行灰度化和滤波去噪处理，并采用Canny算子对所述图像进行二值化处理，及检测图像的轮廓；

利用霍夫变换方法提取二值化图像中直线箭头的三维点云信息；以及

根据所述直线箭头的三维点云信息，计算机械臂末端夹具在相机坐标系中的位置和姿态。

2.根据权利要求1所述的方法，其特征是，所述创建机器人当前工作空间的实时三维模型的步骤包括：

用两个相机同时采集所述当前工作空间的实时图像，得到两个彩色图像；

获取所述两个彩色图像中某一相同像点的二维图像坐标信息；

计算所述像点在两个彩色图像中的视差，得到所述像点在所述当前工作空间中的深度信息；

组合所述二维图像坐标信息和所述深度信息，获取所述像点在所述当前工作空间中的三维坐标信息；

组合所述当前工作空间实时图像中所有点的所述三维坐标信息，生成机器人当前工作空间的三维点云图；

将所述彩色图像的颜色信息映射到所述三维点云图中，得到机器人当前工作空间的彩色三维点云图；以及

根据所述彩色三维点云图重建机器人当前的当前工作空间。

3.根据权利要求1或2所述的方法，其特征是，还包括在线实时提供目标物与机械臂末端夹具在机械臂控制端坐标系中的相对位置和姿态的步骤，包括：

以所述相机的云台的旋转中心为原点建立相机坐标系；

以机械臂控制端旋转中心为原点建立机械臂控制端坐标系；

获取所述机械臂末端夹具在机械臂控制端坐标系中的三维坐标值；

获取目标物在相机坐标系中的三维坐标值，计算得到目标物在机械臂控制端坐标系中的三维坐标值；以及

计算得到目标物与机械臂末端夹具在机械臂控制端坐标系中的三维相对位置和姿态。

4.根据权利要求3所述的方法，其特征是，还包括实时测量所述环境障碍物突出菱角到机械臂各关的距离以及双臂相互位姿关系的步骤，包括：

获取所述机械臂末端夹具在相机坐标系中的三维坐标值，依据D-H坐标逆变换原理由机械臂末端夹具在相机坐标系中的三维坐标求取机械臂每个关节点在相机坐标系中的三维坐标值；

以左机械臂各关节三维坐标为原点，分别与右机械臂对应关节建立三维矢量坐标，实时计算反馈矢量模长及方向；

获取当前工作空间实时三维模型中障碍物突出菱角在相机坐标系中的三维坐标值；

建立障碍物突出菱角与每个关节点的三维坐标矢量并计算矢量模长，实时反馈矢量模长。