专利摘要

本发明公开了一种复杂三维环境中机械臂防碰撞方法及系统，通过深度图像信息标记出所有疑似障碍区，在机械臂往目标位置移动的过程中，通过深度图像中的深度信息得到所有疑似障碍区的点云，计算出每个疑似障碍区的点云的几何中心映射到地平面的点与机械臂末端映射到地平面的点的空间坐标的直线距离作为安全距离，当机械臂与疑似障碍区之间的安全距离小于距离阈值时，将机械臂末端的空间坐标与疑似障碍区的点云的几何中心的直线之间的夹角作为电机转角；驱动机械臂的电机按照电机转角驱动机械臂转向并继续移动；能够降低机械臂在运动轨迹中发生碰撞的概率，从而提高机械臂的使用寿命，从而满足在复杂的三维环境中主动避开障碍的应用要求，本发明应用于机械臂领域。

权利要求

1.一种复杂三维环境中机械臂防碰撞方法，其特征在于，所述方法包括以下步骤：

S100：实时采集三维环境场景的深度图像；

S200：标记出深度图像中的RGB图像的像素矩阵的所有疑似障碍区；

S300：实时的通过位置传感器采集机械臂末端的空间坐标；所述位置传感器设置于机械臂的末端；

S400：在机械臂往目标位置移动的过程中，通过深度图像中的深度信息得到所有疑似障碍区的点云，将每个疑似障碍区的点云的几何中心映射到地平面的点作为点云端点；将机械臂末端映射到地平面的点作为机械臂端点；将每个点云端点与机械臂端点的直线距离作为安全距离；

S500：当机械臂与疑似障碍区之间的安全距离小于距离阈值时，将连接机械臂端点与当前疑似障碍区的点云端点的直线作为第一直线，沿机械臂往目标位置移动的前进方向，选取所述疑似障碍区的下一个疑似障碍区，将；连接机械臂端点与下一个疑似障碍区的点云端点的直线作为第二直线；

S600：将第一直线与第二直线之间的夹角作为电机转角；驱动机械臂的电机按照电机转角驱动机械臂转向并继续移动。

2.根据权利要求1所述的一种复杂三维环境中机械臂防碰撞方法，其特征在于，在S100中，采集三维环境场景的深度图像的方法为通过TOF相机、Kinect传感器或激光雷达测距对场景进行深度图像采集。

3.根据权利要求1所述的一种复杂三维环境中机械臂防碰撞方法，其特征在于，在S300中，所述位置传感器包括直线位移传感器和角位移传感器中任意一种。

4.根据权利要求1所述的一种复杂三维环境中机械臂防碰撞方法，其特征在于，在S400中，在机械臂往目标位置移动的过程，通过滚动路径规划法、遗传算法、蚁群算法中任意一种方法规划的路径进行移动，所述目标位置为机械臂的移动目标。

5.一种复杂三维环境中机械臂防碰撞系统，其特征在于，所述系统包括：存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序运行在该系统的以下单元中：

深度图像采集单元，用于实时采集三维环境场景的深度图像；

疑似障碍标记单元，用于分别从深度图像中的RGB图像的像素矩阵的横坐标和纵坐标开始搜索与第1个像素的RGB颜色值不相同的RGB颜色值所在的坐标区域，将所述不相同的RGB颜色值所在的坐标区域标记为疑似障碍区；遍历完一个疑似障碍区后，再从像素矩阵中剩下的像素点找下一个疑似障碍区，一直到搜索完像素矩阵为止，标记出所有疑似障碍区；

空间坐标采集单元，用于实时的通过位置传感器采集机械臂末端的空间坐标；所述位置传感器设置于机械臂的末端；

安全距离计算单元，用于在机械臂往目标位置移动的过程中，通过深度图像中的深度信息得到所有疑似障碍区的点云，将每个疑似障碍区的点云的几何中心映射到地平面的点作为点云端点；将机械臂末端映射到地平面的点作为机械臂端点；将每个点云端点与机械臂端点的直线距离作为安全距离；

夹角线选取单元，用于当机械臂与疑似障碍区之间的安全距离小于距离阈值时，将连接机械臂端点与当前疑似障碍区的点云端点的直线作为第一直线，沿机械臂往目标位置移动的前进方向，选取所述疑似障碍区的下一个疑似障碍区，将连接机械臂端点与下一个疑似障碍区的点云端点的直线作为第二直线；

机械臂转向单元，用于将第一直线与第二直线之间的夹角作为电机转角；驱动机械臂的电机按照电机转角驱动机械臂转向并继续移动。

说明书

技术领域

本发明涉及机械臂防碰撞领域，具体涉及一种复杂三维环境中机械臂防碰撞方法及系统。

背景技术

在复杂的三维环境中，比如有各种仪器设备车间、大规模生产线上，有存在有大量的机械臂和阻碍机械臂移动的障碍物，机械臂难免会发生互相碰撞和运动轨迹冲突。而机械臂防碰撞是在复杂的三维环境中的障碍条件以及起始位姿和目标位姿，要求选择一条从起始点到目标点的路径，使机械臂能实时的、安全的降低碰撞障碍的风险。

现有的关于机械臂防碰撞一般通过规划好的路径进行防碰撞，路径规划有很多方法，如滚动路径规划法、遗传算法、蚁群算法。但是，这些方法仅能用于二维平面中，复杂的三维环境中实现这些算法避障需要增加大量的计算。因此现有的这些方法难以保证机械臂运动轨迹的精确度和稳定性，机械臂在运动轨迹中发生碰撞的概率很大，难以达到在复杂的三维环境中的应用要求。

发明内容

本发明提供一种复杂三维环境中机械臂防碰撞方法及系统，通过深度图像信息标记出所有疑似障碍区，在机械臂往目标位置移动的过程中，通过深度图像中的深度信息得到所有疑似障碍区的点云，计算出每个疑似障碍区的点云的几何中心映射到地平面的点与机械臂末端映射到地平面的点的空间坐标的直线距离作为安全距离，当机械臂与疑似障碍区之间的安全距离小于距离阈值时，将机械臂末端的空间坐标与疑似障碍区的点云的几何中心的直线作为第一直线，沿机械臂往目标位置移动的前进方向，选取所述疑似障碍区的下一个疑似障碍区，将机械臂末端的空间坐标与下一个疑似障碍区的点云的几何中心的直线作为第二直线，将第一直线与第二直线之间的夹角作为电机转角；驱动机械臂的电机按照电机转角驱动机械臂转向并继续移动。

本发明的目的是针对上述问题，提供一种复杂三维环境中机械臂防碰撞方法及系统，具体包括以下步骤：

S100：实时采集三维环境场景的深度图像；

S200：标记出深度图像中的RGB图像的像素矩阵的所有疑似障碍区；

S300：实时的通过位置传感器采集机械臂末端的空间坐标；所述位置传感器设置于机械臂的末端；

S400：在机械臂往目标位置移动的过程中，通过深度图像中的深度信息得到所有疑似障碍区的点云，将每个疑似障碍区的点云的几何中心映射到地平面的点作为点云端点；将机械臂末端映射到地平面的点作为机械臂端点；将每个点云端点与机械臂端点的直线距离作为安全距离；

其中，疑似障碍区的点云的几何中心映射到地平面的点为：以点云疑似障碍区的点云的几何中心投影到地平面上的点；

其中，机械臂末端映射到地平面的点为:以机械臂末端投影到地平面上的点；

S500：当机械臂与疑似障碍区之间的安全距离小于距离阈值时，将机械臂端点与当前疑似障碍区的点云端点的直线作为第一直线，沿机械臂往目标位置移动的前进方向，选取所述疑似障碍区的下一个疑似障碍区，将机械臂端点与下一个疑似障碍区的点云端点的直线作为第二直线；

S600：将第一直线与第二直线之间的夹角作为电机转角；驱动机械臂的电机按照电机转角驱动机械臂转向并继续移动。

进一步地，在S100中，采集三维环境场景的深度图像的方法为通过设置于三维环境场景上方的TOF相机、Kinect传感器或激光雷达测距对场景进行深度图像采集，三维环境场景中包括有一个或多个机械臂。

进一步地，在S100中，深度图像(RGB-D图像)包含了普通的RGB图像和深度图，深度图是与视点的场景对象的表面的距离有关的信息的图像或图像通道，其中，深度图的每个像素值是传感器距离物体的实际距离，RGB图像和深度图是配准的，RGB图像和深度图的像素点之间具有一对一的对应关系。

进一步地，在S200中，标记出深度图像中的RGB图像的像素矩阵的所有疑似障碍区的方法为：分别从深度图像中的RGB图像的像素矩阵的横坐标和纵坐标开始搜索与像素矩阵的第1个像素的RGB颜色值不相同的RGB颜色值所在的坐标区域，将所述不相同的RGB颜色值所在的坐标区域标记为疑似障碍区；遍历完一个疑似障碍区后，再从像素矩阵中剩下的像素点找下一个疑似障碍区，一直到搜索完像素矩阵为止，标记出所有疑似障碍区。

进一步地，在S200中，标记出深度图像中的RGB图像的像素矩阵的所有疑似障碍区的方法为：人工对深度图像中的RGB图像进行标注，标注出RGB图像上疑似障碍区域。

进一步地，在S200中，标记出深度图像中的RGB图像的像素矩阵的所有疑似障碍区的方法为：将深度图像中的RGB图像进行二值化处理，筛选出灰度值为0的区域表示疑似障碍区域。

进一步地，在S300中，所述位置传感器包括直线位移传感器和角位移传感器中任意一种。

进一步地，在S400中，在机械臂往目标位置移动的过程，通过滚动路径规划法、遗传算法、蚁群算法中任意一种方法规划的路径进行移动，所述目标位置为机械臂的移动目标。

进一步地，在S400中，深度图像中的深度信息(即到场景中各点的距离(深度值))是与视点的场景对象的表面的距离有关的信息的图像或图像通道，是传感器距离物体的实际距离；深度图像经过坐标转换可以计算为点云数据。

进一步地，在S500中，所述距离阈值为机械臂与疑似障碍区之间的安全距离的1/5长度。

本发明还提供了一种复杂三维环境中机械臂防碰撞系统，所述系统包括：存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序运行在以下系统的单元中：

深度图像采集单元，用于实时采集三维环境场景的深度图像；

疑似障碍标记单元，用于分别从深度图像中的RGB图像的像素矩阵的横坐标和纵坐标开始搜索与第1个像素的RGB颜色值不相同的RGB颜色值所在的坐标区域，将所述不相同的RGB颜色值所在的坐标区域标记为疑似障碍区；遍历完一个疑似障碍区后，再从像素矩阵中剩下的像素点找下一个疑似障碍区，一直到搜索完像素矩阵为止，标记出所有疑似障碍区；

空间坐标采集单元，用于实时的通过位置传感器采集机械臂末端的空间坐标；所述位置传感器设置于机械臂的末端；

安全距离计算单元，用于在机械臂往目标位置移动的过程中，通过深度图像中的深度信息得到所有疑似障碍区的点云，将每个疑似障碍区的点云的几何中心映射到地平面的点作为点云端点；将机械臂末端映射到地平面的点作为机械臂端点；将每个点云端点与机械臂端点的直线距离作为安全距离；

夹角线选取单元，用于当机械臂与疑似障碍区之间的安全距离小于距离阈值时，将机械臂端点与当前疑似障碍区的点云端点的直线作为第一直线，沿机械臂往目标位置移动的前进方向，选取所述疑似障碍区的下一个疑似障碍区，将机械臂端点与下一个疑似障碍区的点云端点的直线作为第二直线；

机械臂转向单元，用于将第一直线与第二直线之间的夹角作为电机转角；驱动机械臂的电机按照电机转角驱动机械臂转向并继续移动。

本发明的有益效果为：本发明公开了一种复杂三维环境中机械臂防碰撞方法，能够降低机械臂在运动轨迹中发生碰撞的概率，提高了机械臂移动的稳定性，从而提高机械臂的使用寿命，从而满足在复杂的三维环境中主动避开障碍的应用要求。

附图说明

通过对结合附图所示出的实施方式进行详细说明，本发明的上述以及其他特征将更加明显，本发明附图中相同的参考标号表示相同或相似的元素，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，在附图中：

图1所示为本发明的一种复杂三维环境中机械臂防碰撞方法的流程图；

图2所示为本发明实施方式的一种复杂三维环境中机械臂防碰撞系统。

具体实施方式

以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本发明的目的、方案和效果。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

如图1所示为根据本发明的一种复杂三维环境中机械臂防碰撞方法的流程图，下面结合图1来阐述根据本发明的实施方式的方法。

本发明提出一种复杂三维环境中机械臂防碰撞方法，具体包括以下步骤：

S100：实时采集三维环境场景的深度图像；

S200：分别从深度图像中的RGB图像的像素矩阵的横坐标和纵坐标开始搜索与像素矩阵的第1个像素的RGB颜色值不相同的RGB颜色值所在的坐标区域，将所述不相同的RGB颜色值所在的坐标区域标记为疑似障碍区；遍历完一个疑似障碍区后，再从像素矩阵中剩下的像素点找下一个疑似障碍区，一直到搜索完像素矩阵为止，标记出所有疑似障碍区；

S300：实时的通过位置传感器采集机械臂末端的空间坐标；所述位置传感器设置于机械臂的末端；

S400：在机械臂往目标位置移动的过程中，通过深度图像中的深度信息得到所有疑似障碍区的点云，将每个疑似障碍区的点云的几何中心映射到地平面的点作为点云端点；将机械臂末端映射到地平面的点作为机械臂端点；将每个点云端点与机械臂端点的直线距离作为安全距离；

其中，疑似障碍区的点云的几何中心映射到地平面的点为:以点云疑似障碍区的点云的几何中心作投影到地平面上的点；

其中，机械臂末端映射到地平面的点为:以机械臂末端作投影到地平面上的点；

进一步地，在S400中，深度图像中的深度信息(即到场景中各点的距离(深度值))是与视点的场景对象的表面的距离有关的信息的图像或图像通道，是传感器距离物体的实际距离；深度图像经过坐标转换可以计算为点云数据。

进一步地，通过深度图像中的深度信息得到所有疑似障碍区的点云通过以下C++代码得以实现：

S500：当机械臂与疑似障碍区之间的安全距离小于距离阈值时，将机械臂端点与当前疑似障碍区的点云端点的直线作为第一直线，沿机械臂往目标位置移动的前进方向，选取所述疑似障碍区的下一个疑似障碍区，将机械臂端点与下一个疑似障碍区的点云端点的直线作为第二直线；

S600：将第一直线与第二直线之间的夹角作为电机转角；驱动机械臂的电机按照电机转角驱动机械臂转向并继续移动。

进一步地，在S100中，采集三维环境场景的深度图像的方法为通过TOF相机、Kinect传感器或激光雷达测距对场景进行深度图像采集。

进一步地，在S100中，深度图像(RGB-D图像)包含了普通的RGB图像和深度图，深度图是与视点的场景对象的表面的距离有关的信息的图像或图像通道，其中，深度图的每个像素值是传感器距离物体的实际距离，RGB图像和深度图是配准的，RGB图像和深度图的像素点之间具有一对一的对应关系。

进一步地，在S200中，标记出深度图像中的RGB图像的像素矩阵的所有疑似障碍区的方法为：人工对深度图像中的RGB图像进行标注，标注出RGB图像上疑似障碍区域。

进一步地，在S200中，标记出深度图像中的RGB图像的像素矩阵的所有疑似障碍区的方法为：将深度图像中的RGB图像进行二值化处理，筛选出灰度值为0的区域表示疑似障碍区域。

进一步地，在S300中，所述位置传感器包括直线位移传感器和角位移传感器中任意一种。

进一步地，在S400中，在机械臂往目标位置移动的过程，通过滚动路径规划法、遗传算法、蚁群算法中任意一种方法规划的路径进行移动，所述目标位置为机械臂的移动目标。

进一步地，在S500中，所述距离阈值为机械臂与疑似障碍区之间的安全距离的1/5长度。

本发明的实施例提供的一种复杂三维环境中机械臂防碰撞系统，如图2所示为本发明的一种复杂三维环境中机械臂防碰撞系统结构图，该实施例的一种复杂三维环境中机械臂防碰撞系统包括：处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述一种复杂三维环境中机械臂防碰撞系统实施例中的步骤。

所述系统包括：存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序运行在以下系统的单元中：

深度图像采集单元，用于实时采集三维环境场景的深度图像；

疑似障碍标记单元，用于分别从深度图像中的RGB图像的像素矩阵的横坐标和纵坐标开始搜索与第1个像素的RGB颜色值不相同的RGB颜色值所在的坐标区域，将所述不相同的RGB颜色值所在的坐标区域标记为疑似障碍区；遍历完一个疑似障碍区后，再从像素矩阵中剩下的像素点找下一个疑似障碍区，一直到搜索完像素矩阵为止，标记出所有疑似障碍区；

空间坐标采集单元，用于实时的通过位置传感器采集机械臂末端的空间坐标；所述位置传感器设置于机械臂的末端；

安全距离计算单元，用于在机械臂往目标位置移动的过程中，通过深度图像中的深度信息得到所有疑似障碍区的点云，计算出每个疑似障碍区的点云的几何中心映射到地平面的点与机械臂末端映射到地平面的点的空间坐标的直线距离作为安全距离；

夹角线选取单元，用于当机械臂与疑似障碍区之间的安全距离小于距离阈值时，将机械臂末端的空间坐标与疑似障碍区的点云的几何中心的直线作为第一直线，沿机械臂往目标位置移动的前进方向，选取所述疑似障碍区的下一个疑似障碍区，将机械臂末端的空间坐标与下一个疑似障碍区的点云的几何中心的直线作为第二直线；

机械臂转向单元，用于将第一直线与第二直线之间的夹角作为电机转角；驱动机械臂的电机按照电机转角驱动机械臂转向并继续移动。

所述一种复杂三维环境中机械臂防碰撞系统可以运行于桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备中。所述一种复杂三维环境中机械臂防碰撞系统可运行的系统可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，所述例子仅仅是一种复杂三维环境中机械臂防碰撞系统的示例，并不构成对一种复杂三维环境中机械臂防碰撞系统的限定，可以包括比例子更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述一种复杂三维环境中机械臂防碰撞系统还可以包括输入输出设备、网络接入设备、总线等。所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述一种复杂三维环境中机械臂防碰撞系统运行系统的控制中心，利用各种接口和线路连接整个一种复杂三维环境中机械臂防碰撞系统可运行系统的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述一种复杂三维环境中机械臂防碰撞系统的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

尽管本发明的描述已经相当详尽且特别对几个所述实施例进行了描述，但其并非旨在局限于任何这些细节或实施例或任何特殊实施例，从而有效地涵盖本发明的预定范围。此外，上文以发明人可预见的实施例对本发明进行描述，其目的是为了提供有用的描述，而那些目前尚未预见的对本发明的非实质性改动仍可代表本发明的等效改动。

一种复杂三维环境中机械臂防碰撞方法及系统专利购买费用说明

Q：办理专利转让的流程及所需资料

A：专利权人变更需要办理著录项目变更手续，有代理机构的，变更手续应当由代理机构办理。

1：专利变更应当使用专利局统一制作的“著录项目变更申报书”提出。

2：按规定缴纳著录项目变更手续费。

3：同时提交相关证明文件原件。

4：专利权转移的，变更后的专利权人委托新专利代理机构的，应当提交变更后的全体专利申请人签字或者盖章的委托书。

Q:专利著录项目变更费用如何缴交

A：（1）直接到国家知识产权局受理大厅收费窗口缴纳,（2）通过代办处缴纳，（3）通过邮局或者银行汇款,更多缴纳方式

Q：专利转让变更，多久能出结果

A：著录项目变更请求书递交后，一般1-2个月左右就会收到通知，国家知识产权局会下达《转让手续合格通知书》。