CN106384115B - 一种机械臂关节角度检测方法 - Google Patents

Abstract

本发明公开了一种机械臂关节角度检测方法，包括步骤：1)采集RGB‑D原始图像；2)提取机械臂轮廓；3)对提取的机械臂轮廓进行平滑处理；4)提取机械臂轮廓骨架；5)定位机械臂关节点；6)计算机械臂关节角度。机械臂轮廓的提取采用的是彩色图像与深度图像结合的阈值分割技术；机械臂关节点的定位是通过细化算法提取出机械臂轮廓骨架，然后结合机械臂各刚体部件的位置关系和骨架线的长度对其关节点定位；机械臂关节角度是根据关节点的三维信息，由向量的可平移性质及方向性将机械臂关节角度计算简化为对空间向量夹角的计算得到。本发明阈值分割效果好，关节点的定位更加准确，能够良好的计算出机械臂各关节角度，鲁棒性较强，简单有效。

Description

一种机械臂关节角度检测方法

技术领域

本发明涉及图像处理与机器视觉领域，具体涉及一种机械臂关节角度检测方法。

背景技术

随着机器人技术的高速发展和社会工业化进程的加速，机器人逐渐参与到人类的活动中来，并日益成为人类的好帮手。机械臂也是机器人的重要的一种，由于它能完成多种多样的任务，如装配，搬运，焊接等等，还可以代替人类在一些危险的恶劣的工作环境中完成既定的任务等优势。它被广泛应用于工业制造、空间探索、国防科技等领域。

由于工业机器人系统是多输入、多输出、强非线性、强耦合的高度复杂系统，在长时间高速运动的情况下，机器人的非线性动力学特性十分显著，并且在实际工作中存在的各种复杂不确性因素，严重影响着机器人的控制性能，甚至有可能出现故障，破坏物体和伤及工人，变为机器杀手。为此，机械臂角度的检测对机械臂的故障检测、精准度提高和安全保障有着潜在的价值。

传统的机械臂各个关节的角度值的计算是利用已知机械臂的末端执行器的坐标位置(或位姿)通过机械臂的逆向运动学计算求得，但是由于机械臂各关节变量相互耦合，根据逆运动方程求得的解也不是唯一的解，需根据机械臂的组合形态和各关节的运动范围经过多次反复计算再选择一组合理解，通过逆运动学求解机械臂各关节角度十分复杂。

发明内容

本发明要解决的技术问题是，针对现有通过逆运动学求解机械臂各关节角度存在算法非常复杂的不足，提供一种机械臂关节角度检测方法，利用RBG-D图像检测机械臂角度，阈值分割效果好，关节点的定位更加准确，简化了机械臂关节角度求解。

本发明为解决上述技术问题所采用的技术方案是：

一种机械臂关节角度检测方法，包括如下步骤：

步骤一：采集RGB-D原始图像；

步骤二：结合彩色图像与深度图像，用阈值分割的方法提取机械臂目标轮廓图像；

步骤三：对提取的机械臂轮廓进行平滑处理(预处理)；

步骤四：通过Zhang并行快速细化算法提取机械臂轮廓骨架；

步骤五：结合机械臂各刚体部件的位置关系(刚体比例关系)和骨架线长度对机械臂关节点定位，确定机械臂关节点；

步骤六：通过定位出的机械臂关节点的三维信息(深度信息)和空间向量的可平移性质及方向性计算机械臂各关节角度，将机械臂关节角度计算简化为对空间向量夹角的计算。

按上述方案，步骤二中，阈值分割提取机械臂目标轮廓图像具体包括两大块：

a、设定阈值对深度图像进行阈值分割提取机械臂轮廓，对于深度图像，通过深度值的判定把机械臂目标区域与背景区域进行初步区分，深度图像目标提取的公式：

其中，f(x,y)是阈值分割中(x,y)处的逻辑值，d(x,y)为深度图像中(x,y)坐标处的深度值，θ为深度图像的阈值分割的阈值门限，θ根据机械臂的工作空间设置；

b、结合彩色图像进行分割，移除机械臂目标以外物体，对于彩色图像，阈值的选取根据机械臂主体颜色来设定，设定过程如下：首先取彩色图像第一帧作为先验图片，将其由RGB图像转换为YCbCr图像，然后手动选取机械臂主体颜色部分，若选择的矩形宽为w，高为h，p(y,cb,cr)为矩形区域的像素点，(i,j)为图像二维坐标，则矩形区域的颜色平均值为：

取矩形区域的颜色平均值为彩色图像的阈值分割的阈值门限；

彩色图像目标提取的公式：

其中，g(x,y)是阈值分割中(x,y)处的逻辑值，c(x,y)为深度图像中(x,y)坐标处的YcbCr值，为彩色图像的阈值分割的阈值门限，根据手动选取机械臂主体颜色部分的平均值来设置；

最终的阈值分割的逻辑值取深度图像与彩色图像阈值分割的交集。

按上述方案，步骤五机械臂关节点定位具体为：

通过遍历幅机械臂骨架图，细化后的机械臂骨架线机械臂骨架部分像素值为1，背景部分为0，机械臂骨架为单位像素宽，找出像素值为1的八邻域只有一个像素值为1的所有像素点，根据机械臂关节位置关系，像素坐标点纵坐标y最大的像素点为机械臂底座关节点位置，像素点横坐标x最小的为机械臂末端执行器的关节点位置；再从机械臂末端执行器关节点的像素位置开始延骨架线遍历，直到遍历到机械臂底座关节点位置截止，遍历路径的像素点总个数为机械臂骨架线长度，再通过机械臂产品规格图，得到机械臂各个关节的比例，其他关节点按照计算的骨架线长度和各个关节的比例关系就可以定位出来。

按上述方案，步骤六机械臂关节角度的计算具体为：

通过相机建立空间坐标系，将定位出的关节点由图像二维坐标转换为三维坐标，再将机械臂关节角度计算简化为对空间向量夹角的计算；深度图像到空间三维坐标的转换由如下表达式求得：

其中，(i，j)为图像二维坐标，(x，y，z)为空间坐标，(δ_x，δ_y)为相机的镜头畸变，(c_x，c_y)为图像中心点，(f_x，f_y)为比例参数，d是深度图像二维坐标(x,y)处的深度值；根据定位出的关节点像素的图像二维坐标和深度值，按上式算出每个关节点的空间坐标；

对于任意两个不重合的三维坐标点A(x1,y1,z1)、B(x2,y2,z2)，转化到传统空间坐标系中组成向量AB，转化公式为：

根据以上性质，机械臂关节角度的计算简化为空间向量夹角的计算，如定位出的关节坐标点A(x1,y1,z1)、B(x2,y2,z2)、C(x3,y3,z3)之间关节角度，直接通过计算空间向量BA和BC的夹角得出，公式如下：

关节点B、C、D、E之间关节角度依次按上述公式计算，对于机械臂基座转动的关节角度计算按以下方式计算：将关节点A初始位置投影到xOz平面A′，以向量EA′为基准，在机械臂转动的过程中将关节点A继续投影到xOz平面A″，通过计算向量EA′和EA″的夹角获得。

本发明的工作原理：基于RGB-D图像检测机械臂关节角度，首先通过RGB-D图像定位出机械臂关节点，然后通过定位出的关节点的三维信息计算机械臂关节角度。

本发明与现有技术相比具有如下有益效果：

1、通过结合彩色图像和深度图像的阈值分割方法所提取的机械臂目标图像要比单纯的用深度图像或者彩色图像进行阈值分割效果要好；

2、通过结合机械臂各刚体部件的位置关系和骨架线的长度对机械臂关节点进行定位的方式，可以使关节点的定位更加准确；

3、通过向量的性质求解机械臂关节角度的方式简化了计算的复杂性，并且简单有效。

附图说明

图1是本发明机械臂关节角度检测方法流程图；

图2是本发明实施例机械臂关节点空间向量表示图。

具体实施方式

下面根据具体实施例并结合附图，对本发明作进一步详细的说明。

参照图1所示，本发明实施例所述的机械臂关节角度检测方法，包括如下步骤：

步骤一：首先用Kinect采集RGB-D原始图像；

步骤二：设定阈值对深度图像进行阈值分割提取机械臂轮廓，对于深度图像，通过深度值的判定把机械臂目标区域与背景区域进行初步区分，深度图像机械臂目标提取中阈值的限定发挥着关键性的作用，本文采集的场景固定，阈值范围选择机械臂工作空间范围内的深度值，通过多次试验可以得到。

深度图像目标提取的公式：

其中，f(x,y)是阈值分割中(x,y)处的逻辑值，d(x,y)为深度图像中(x,y)坐标处的深度值，θ为深度图像的阈值分割的阈值门限，阈值门限θ根据机械臂的工作空间设置。

单纯的用深度图像阈值分割后的机械臂轮廓图像可能含有机械臂以外物体，于是结合彩色图像进行分割，移除机械臂目标以外物体，对于彩色图像，其阈值的选取根据机械臂主体颜色来设定，其设定过程如下：首先取彩色图像第一帧作为先验图片，将其由RGB图像转换为YCbCr图像，然后手动选取机械臂主体颜色部分，若选择的矩形宽为w，高为h，p(y,cb,cr)为矩形区域的像素点，(i,j)为图像二维坐标，则矩形区域的颜色平均值为：

取矩形区域的颜色平均值为彩色图像的阈值分割的阈值门限，整个过程为了忽略光照强度的影响以cb，cr通道进行分割。

彩色图像目标提取的公式：

其中，g(x,y)是阈值分割中(x,y)处的逻辑值，c(x,y)为深度图像中(x,y)坐标处的YcbCr值，为彩色图像的阈值分割的阈值门限，阈值门限根据手动选取机械臂主体颜色部分的平均值来设置。

最终的阈值分割的逻辑值取深度图像与彩色图像阈值分割的交集。

步骤三：用中值滤波对机械臂轮廓图像就行平滑处理(预处理)。

步骤四：用zhang并行快速细化算法提取机械臂轮廓骨架。

步骤五：机械臂关节点定位。机械臂都会有它的产品说明书和产品技术规格书，机械臂各个关节的结构尺寸都有标注，各部分的关节比例关系为后面关节点定位提供有效的信息。通过遍历幅机械臂骨架图，细化后的机械臂骨架线机械臂骨架部分像素值为1，背景部分为0，机械臂骨架为单位像素宽，找出像素值为1的八邻域只有一个像素值为1的所有像素点，根据机械臂关节位置关系，像素坐标点纵坐标y最大的像素点为机械臂底座关节点位置，像素点横坐标x最小的为机械臂末端执行器的关节点位置。再从机械臂末端执行器关节点的像素位置开始延骨架线遍历，直到遍历到机械臂底座关节点位置截止，遍历路径的像素点总个数为机械臂骨架线长度，再通过机械臂产品规格图，可以得到机械臂各个关节的比例，其他关节点按照计算的骨架线长度和各个关节的比例关系就可以定位出来。

步骤六：机械臂关节角度的计算。通过Kinect相机建立空间坐标系，将定位出的关节点由图像二维坐标转换为三维坐标，再将机械臂关节角度计算简化为对空间向量夹角的计算，该方法简单有效。

深度图像到空间三维坐标的转换由如下表达式求得：

其中，(i，j)为图像二维坐标，(x，y，z)为空间坐标，(δ_x，δ_y)为Kinect相机的镜头畸变，(c_x，c_y)为图像中心点，(f_x，f_y)为比例参数，d是深度图像二维坐标(x,y)处的深度值，根据定位出的关节点像素的图像二维坐标和深度值，按上式即可算出每个关节点的空间坐标；

参照图2所示，对于Kinect坐标系中任意两个不重合的三维坐标点A(x1,y1,z1)、B(x2,y2,z2)，转化到传统空间坐标系中组成向量AB，转化公式为：

根据以上性质，机械臂关节角度的计算可以简化为空间向量夹角的计算，如定位出的关节坐标点A(x1,y1,z1)、B(x2,y2,z2)、C(x3,y3,z3)之间关节角度，可以直接通过计算空间向量BA和BC的夹角得出，公式如下：

关节点B、C、D，E之间关节角度依次按上述公式计算即可，对于机械臂基座转动的关节角度计算按以下方式计算：将关节点A初始位置投影到xOz平面A′，以向量EA′为基准，在机械臂转动的过程中将关节点A继续投影到xOz平面A″，通过上述公式计算向量EA′和EA″的夹角获得。

显然，上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而这些属于本发明的精神所引伸出的显而易见的变化或变动仍处于本发明的保护范围之中。

Claims (2)

1.一种机械臂关节角度检测方法，其特征在于，包括如下步骤：

步骤一：采集RGB-D原始图像；

步骤二：结合彩色图像与深度图像，用阈值分割的方法提取机械臂目标轮廓图像；

步骤三：对提取的机械臂轮廓进行平滑处理；

步骤四：通过Zhang并行快速细化算法提取机械臂轮廓骨架；

步骤五：结合机械臂各刚体部件的位置关系和骨架线长度对机械臂关节点定位，确定机械臂关节点，机械臂关节点定位具体为：

通过遍历幅机械臂骨架图，细化后的机械臂骨架线机械臂骨架部分像素值为1，背景部分为0，机械臂骨架为单位像素宽，找出像素值为1的八邻域只有一个像素值为1的所有像素点，根据机械臂关节位置关系，像素坐标点纵坐标y最大的像素点为机械臂底座关节点位置，像素点横坐标x最小的为机械臂末端执行器的关节点位置；再从机械臂末端执行器关节点的像素位置开始延骨架线遍历，直到遍历到机械臂底座关节点位置截止，遍历路径的像素点总个数为机械臂骨架线长度，再通过机械臂产品规格图，得到机械臂各个关节的比例，其他关节点按照计算的骨架线长度和各个关节的比例关系就可以定位出来；

步骤六：通过定位出的机械臂关节点的三维信息和空间向量的可平移性质及方向性计算机械臂各关节角度，将机械臂关节角度计算简化为对空间向量夹角的计算，机械臂关节角度的计算具体为：

通过相机建立空间坐标系，将定位出的关节点由图像二维坐标转换为三维坐标，再将机械臂关节角度计算简化为对空间向量夹角的计算；深度图像到空间三维坐标的转换由如下表达式求得：

其中，(i，j)为图像二维坐标，(x，y，z)为空间坐标，(δ_x，δ_y)为相机的镜头畸变，(c_x，c_y)为图像中心点，(f_x，f_y)为比例参数，d是深度图像二维坐标(x,y)处的深度值；根据定位出的关节点像素的图像二维坐标和深度值，按上式算出每个关节点的空间坐标；

对于任意两个不重合的三维坐标点A(x1,y1,z1)、B(x2,y2,z2)，转化到传统空间坐标系中组成向量AB，转化公式为：

根据以上性质，机械臂关节角度的计算简化为空间向量夹角的计算，如定位出的关节坐标点A(x1,y1,z1)、B(x2,y2,z2)、C(x3,y3,z3)之间关节角度，直接通过计算空间向量BA和BC 的夹角得出，公式如下：

关节点B、C、D、E之间关节角度依次按上述公式计算，对于机械臂基座转动的关节角度计算按以下方式计算：将关节点A初始位置投影到xOz平面A′，以向量EA′为基准，在机械臂转动的过程中将关节点A继续投影到xOz平面A″，通过计算向量EA′和EA″的夹角获得。

2.如权利要求1所述的机械臂关节角度检测方法，其特征在于，步骤二中，阈值分割提取机械臂目标轮廓图像具体包括两大块：

a、设定阈值对深度图像进行阈值分割提取机械臂轮廓，对于深度图像，通过深度值的判定把机械臂目标区域与背景区域进行初步区分，深度图像目标提取的公式：

其中，f(x,y)是阈值分割中(x,y)处的逻辑值，d(x,y)为深度图像中(x,y)坐标处的深度值，θ为深度图像的阈值分割的阈值门限，θ根据机械臂的工作空间设置；

b、结合彩色图像进行分割，移除机械臂目标以外物体，对于彩色图像，阈值的选取根据机械臂主体颜色来设定，设定过程如下：首先取彩色图像第一帧作为先验图片，将其由RGB图像转换为YCbCr图像，然后手动选取机械臂主体颜色部分，若选择的矩形宽为w，高为h，p(y,cb,cr)为矩形区域的像素点，(i,j)为图像二维坐标，则矩形区域的颜色平均值为：

取矩形区域的颜色平均值为彩色图像的阈值分割的阈值门限；

彩色图像目标提取的公式：

其中，g(x,y)是阈值分割中(x,y)处的逻辑值，c(x,y)为深度图像中(x,y)坐标处的YcbCr值，为彩色图像的阈值分割的阈值门限，根据手动选取机械臂主体颜色部分的平均值来设置；

最终的阈值分割的逻辑值取深度图像与彩色图像阈值分割的交集。