阅读说明：本技术 一种用于机械臂的防护门防撞方法及系统 (protective door anti-collision method and system for mechanical arm ) 是由 唐露新 张宇维 于 2019-08-21 设计创作，主要内容包括：本发明提供了一种用于机械臂的防护门防撞方法,包括:根据真实机械臂结构,构建得到机械臂模型；利用图像获取装置获取实景防护门图像,并根据防护门模型,并根据所述防护门图像,重构得到防护门点云地图；根据所述机械臂模型和点云地图,判断所述机械臂模型与所述防护门点云地图的相近状态,并根据所述相近状态指令控制所述机械臂的运行。本发明可提高了工业机器人的安全性、工业设备的耐久度、生产安全。(The invention provides a method for preventing a protective door of a mechanical arm from being collided, which comprises the steps of constructing and obtaining a mechanical arm model according to a real mechanical arm structure; acquiring a live-action protective door image by using an image acquisition device, and reconstructing to obtain a protective door point cloud map according to a protective door model and the protective door image; and judging the similar state of the mechanical arm model and the protective door point cloud map according to the mechanical arm model and the point cloud map, and controlling the operation of the mechanical arm according to the similar state instruction. The invention can improve the safety of industrial robot, the durability of industrial equipment and the production safety.)

一种用于机械臂的防护门防撞方法及系统

技术领域

本发明涉及防撞检测领域，具体而言，涉及一种用于机械臂的防护门防撞方法及系统。

背景技术

工业机械臂重点运用在现代工业生产过程中，机械臂操作对象通常是操作仓中或流水线上的工业零部件及工业设备。针对带有高速、高热、辐射等的危险工艺流程及操作，使用机械臂工作能够提高生产线的安全性和工作效率。在操作带有防护门装置的操作仓及流水线时，机械臂常常无法实时监测防护门已经关闭或呈半闭状态，出现失控撞门的故障，如图3所示。不仅对机械臂、工业设备等造成严重损害，也对工业生产安全造成了一定的威胁。

经过大量检索发现一些典型的现有技术，如专利CN108972567A(如图4)，公开了机械臂防碰撞系统、方法及存储介质。其中，方法包括：利用安装在机械臂末端的一个MEMS传感器测量得到机械臂末端的速度和加速度信息，并将所测得的速度和加速度信息与标准的速度和加速度信息进行比较，在比较结果显示偏差超出设定阈值时，通过分析MEMS所测得的速度和加速度在一设定时间段内的变化趋势，来确定是碰撞发生还是其他原因导致的振动问题，并在碰撞发生时，对机械臂进行防护控制。或如专利CN108015774A(如图5)着眼于通过投影方式来检测机械臂的碰撞，且在技术方案中公开了一种无需传感器的机械臂碰撞检测方法，包括：计算机械臂末端负载对机械臂产生的关节力矩矢量的正交投影矩阵，根据该正交投影矩阵得到机械臂末端负载的力矩变换矩阵；建立机械臂本体动力学模型，根据该动力学模型中的各项建立机械臂动量偏差观测器，通过动量偏差观测器得到机械臂关节外力矩矢量，根据该外力矩矢量与机械臂末端负载的力矩变换矩阵计算出机械臂的碰撞力矩；将机械臂的碰撞力矩与预设阈值相比较，判断机械臂是否发生碰撞。又如一种典型的案例CN104537151B，提供了一种基于等效质量的空间机械臂连续碰撞动力学建模方法，也即在建立空间机械臂运动学方程、动力学方程以及能量方程的基础上，计算出空间机械臂末端的等效质量；利用单体之间的连续碰撞Hertz阻尼模型，推导出单体碰撞产生的碰撞力、碰撞持续时间及压缩量；进而，结合空间机械臂末端等效质量和单体连续碰撞Hertz阻尼模型，建立空间机械臂连续碰撞动力学模型，计算出空间机械臂碰撞过程中的碰撞力、碰撞持续时间及最大压缩量。

可见，如何有效解决机械臂在工作过程由于某传感器失灵、实时处理性能不够或控制程序逻辑出现问题导致的碰撞问题，在实际应用中的亟待处理的实际问题还有很多未提出具体的解决方案。

发明内容

为了克服现有技术的不足提供了一种用于机械臂的防护门防撞方法及系统，本发明的具体技术方案如下：

一种用于机械臂的防护门防撞方法，包括:根据真实机械臂结构，构建得到机械臂模型；利用图像获取装置获取实景防护门图像，并根据防护门模型，并根据所述防护门图像，重构得到防护门点云地图；根据所述机械臂模型和点云地图，判断所述机械臂模型与所述防护门点云地图的相近状态，并根据所述相近状态指令控制所述机械臂的运行。

可选的，所述根据真实机械臂结构，构建得到机械臂模型，包括：

根据真实机械臂结构，创建得到虚拟立体机械臂模型；

且根据真实机械臂结构对虚拟立体机械臂模型中的机械臂连杆、关节、待规划的关节组、机械臂初始动作和机械臂末端执行器进行定义操作，得到已定义机械臂模型；

将已定义机械臂模型与场景环境连接，得到完整的机械臂模型。

可选的，所述利用图像获取装置获取实景防护门图像，还包括：

获取所述图像获取装置的标准参数信息，并根据所述标准参数信息对所述图像获取装置进行配置操作；

基于配置完成的图像获取装置，获取得到实景防护门图像。

可选的，所述根据所述防护门图像，重构得到所述防护门的点云地图，包括：

所述图像获取装置获取防护门图像，并提取得到防护门图像的点云数据信息，并根据所述点云数据信息，重构得到防护门点云地图。

可选的，所述点云数据信息为碰撞体信息。

可选的，所述判断所述机械臂模型与所述防护门点云地图的相近状态，并根据所述相近状态，指令控制所述真实机械臂的运行，包括：

根据所述机械臂模型和所述点云数据信息，判断得到所述机械臂模型与所述防护门点云地图是否存在重合，

若重合，则判定所述真实机械臂与所述防护门出现碰撞，并控制停止所述真实机械臂的运行；

若不重合，则获取所述机械臂模型与所述防护门点云地图的相近距离，并将所述相近距离与预警碰撞距离进行比较，判断所述相近距离是否小于或等于所述预警碰撞距离，若所述相近距离小于或等于所述预警碰撞距离，则指令控制所述机械臂停止运行，若所述相近距离大于所述预警碰撞距离，则继续检测获取所述相近距离。

可选的，所述防护门防撞方法还包括基于贝叶斯准则实时更新所述防护门点云地图。

可选的，所述防护门防撞方法还包括基于运动规划信息，控制所述真实机械臂运动。

另外，本发明还提供了一种用于机械臂的防护门防撞系统，应用所述防护门防撞方法，包括：模型构建模块，用于根据真实机械臂结构，构建得到机械臂模型；地图重构模块，用于利用图像获取装置获取实景防护门图像，并根据防护门模型，并根据所述防护门图像，重构得到防护门点云地图；控制中心模块，用于根据所述机械臂模型和点云地图，判断所述机械臂模型与所述防护门点云地图的相近状态，并根据所述相近状态指令控制所述机械臂的运行。

可选的，所述防护门防撞系统还包括参数服务模块，用于输入存储所述机械臂模型的定义参数、机械臂电气的定义参数和场景环境布置的定义参数，以供所述模型构建模块、地图重构模块和控制中心模块的数据调用。

本发明所取得的有益效果包括：1、利用机器视觉技术，检测将要发生的碰撞，不受机械结构、动力数据源准确性的限制，增强了碰撞检测的稳定性与可靠性；2、能够在碰撞发生之前进行检测，提高了工业机器人的安全性、工业设备的耐久度、生产安全；3、可以有效降低载荷，提高了工业机器人的灵活性，降低生产成本及功耗；4、结合动力及图像等多种传感系统，能够有效防止机械臂在工作过程由于某传感器失灵、实时处理性能不够或控制程序逻辑出现问题导致的撞门问题。

附图说明

从以下结合附图的描述可以进一步理解本发明，将重点放在示出实施例的原理上。

图1是本发明实施例之一中防护门防撞方法的流程示意图；

图2是本发明实施例之一中防护门防撞系统的结构示意图；

图3是现有技术中机械臂故障时撞门的事例图；

图4是现有技术CN108972567A中的结构示意图；

图5是现有技术CN108015774A中的机械臂碰撞检测方法的流程示意图。

具体实施方式

为了使得本发明的目的、技术方案及优点更加清楚明白，以下结合其实施例，对本发明进行进一步详细说明；应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。对于本领域技术人员而言，在查阅以下详细描述之后，本实施例的其它系统、方法和/或特征将变得显而易见。旨在所有此类附加的系统、方法、特征和优点都包括在本说明书内、包括在本发明的范围内，并且受所附权利要求书的保护。在以下详细描述描述了所公开的实施例的另外的特征，并且这些特征根据以下将详细描述将是显而易见的。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或组件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

本发明为一种用于机械臂的防护门防撞方法及系统，根据图1-2所示讲述以下实施例：

实施例一：

本实施例提供了一种用于机械臂的防护门防撞方法，包括:

S101:根据真实机械臂结构，构建得到机械臂模型；

S102:利用图像获取装置获取实景防护门图像，并根据防护门模型，并根据所述防护门图像，重构得到防护门点云地图；

S103:根据所述机械臂模型和点云地图，判断所述机械臂模型与所述防护门点云地图的相近状态，并根据所述相近状态指令控制所述机械臂的运行。

其中，所述根据真实机械臂结构，构建得到机械臂模型，包括：

根据真实机械臂结构，创建得到虚拟立体机械臂模型；

且根据真实机械臂结构对虚拟立体机械臂模型中的机械臂连杆、关节、待规划的关节组、机械臂初始动作和机械臂末端执行器进行定义操作，得到已定义机械臂模型；

将已定义机械臂模型与场景环境连接，得到完整的机械臂模型。

其中，所述利用图像获取装置获取实景防护门图像，还包括：

获取所述图像获取装置的标准参数信息，并根据所述标准参数信息对所述图像获取装置进行配置操作；

基于配置完成的图像获取装置，获取得到实景防护门图像。

其中，所述根据所述防护门图像，重构得到所述防护门的点云地图，包括：所述图像获取装置获取防护门图像，并提取得到防护门图像的点云数据信息，并根据所述点云数据信息，重构得到防护门点云地图。其中，所述点云数据信息为碰撞体信息。

其中，所述判断所述机械臂模型与所述防护门点云地图的相近状态，并根据所述相近状态，指令控制所述真实机械臂的运行，包括：

根据所述机械臂模型和所述点云数据信息，判断得到所述机械臂模型与所述防护门点云地图是否存在重合，

若重合，则判定所述真实机械臂与所述防护门出现碰撞，并控制停止所述真实机械臂的运行；

若不重合，则获取所述机械臂模型与所述防护门点云地图的相近距离，并将所述相近距离与预警碰撞距离进行比较，判断所述相近距离是否小于或等于所述预警碰撞距离，若所述相近距离小于或等于所述预警碰撞距离，则指令控制所述机械臂停止运行，若所述相近距离大于所述预警碰撞距离，则继续检测获取所述相近距离。

其中，所述防护门防撞方法还包括基于贝叶斯准则实时更新所述防护门点云地图。

其中，所述防护门防撞方法还包括基于运动规划信息，控制所述真实机械臂运动。

要说明的是，本发明实施例中的防护门的防撞系统可以用于实现上述方法实施例中的全部技术方案，其各个功能模块的功能可以根据下述方法实施例中的方法具体实现，其具体实现过程可参照上述实施例中的相关描述，此处不再赘述。

另外，本发明还提供了一种用于机械臂的防护门防撞系统，应用所述防护门防撞方法，包括：模型构建模块，用于根据真实机械臂结构，构建得到机械臂模型；地图重构模块，用于利用图像获取装置获取实景防护门图像，并根据防护门模型，并根据所述防护门图像，重构得到防护门点云地图；控制中心模块，用于根据所述机械臂模型和点云地图，判断所述机械臂模型与所述防护门点云地图的相近状态，并根据所述相近状态指令控制所述机械臂的运行。

其中，所述防护门防撞系统还包括参数服务模块，用于输入存储所述机械臂模型的定义参数、机械臂电气的定义参数和场景环境布置的定义参数，以供所述模型构建模块、地图重构模块和控制中心模块的数据调用。

实施例二：

本实施例提供了一种用于机械臂的防护门防撞方法，包括:

S101:根据真实机械臂结构，且使用三维建模软件或函数库构建得到机械臂模型；所述三维建模软件可以为ROS机器人操作系统的MoveIt，或3DMax或OpenGL。

S102:利用图像获取装置获取实景防护门图像，并根据防护门模型，并根据所述防护门图像，重构得到防护门的三维八叉树点云地图；其中，所述图像获取装置可以为深度相机、双目摄像头或其他能够获取场景深度信息的传感设备，且所述图像获取装置独立设置有深度信息获取单元与之连接，所述相机或其他深度传感设备搭载位置可更换，但与机械臂的相对位置确定。

S103:根据所述机械臂模型和三维八叉树点云地图，判断所述机械臂模型与所述防护门的三维八叉树点云地图的相近状态，并根据所述相近状态指令控制所述机械臂的运行。

其中，所述根据真实机械臂结构，构建得到机械臂模型，包括：

根据真实机械臂结构，创建得到虚拟立体机械臂模型；

且根据真实机械臂结构对虚拟立体机械臂模型中的机械臂连杆、关节、待规划的关节组、机械臂初始动作和机械臂末端执行器进行定义操作，得到已定义机械臂模型；所述定义操作，根据所述机械臂连杆、关节、待规划的关节组、机械臂初始动作和机械臂末端执行器的不同，设置有不同的定义操作。如所述机械臂连杆定义操作，根据所述机械臂连杆的各类属性信息，对所述虚拟立体机械臂模型中的机械臂连杆进行属性信息定义，包括：名称、转动惯量矩阵、连杆质量、惯性张量矩阵、可视化属性、碰撞体属性；所述关节定义操作，根据所述关节的各类属性信息，对所述虚拟立体机械臂模型中的机械臂关节进行属性信息定义，包括：关节名称、关节类型、连杆转换矩阵、父连杆、子连杆、物理阻尼、物理静摩擦力以及运动上下限；所述待规划的关节组，其定义操作为根据在机械臂实物上可以通过舵机或电机驱动旋转，且可以在所述虚拟立体机械臂模型中同步实现，进行规划路径的定义操作；所述机械臂手臂末端执行器的定义操作，则根据所述末端执行器在实物机械臂中的功能进行定义操作，以使所述虚拟立体机械臂模型中的末端执行器可实现实物机械臂末端执行器的功能，例如，若为抓取器械臂，则定义抓取操作；若为点触机械臂，则定义点触操作。

基于机械臂模型建模工具，配置虚拟关节，将已定义机械臂模型与场景环境连接，以所述场景环境作为所述已定义机械臂模型的底座，得到完整的机械臂模型。具体的，基于ROS系统，运行roslaunch moveit_setup_ssistant setup_assistnt.lanuch,导入已建立的虚拟立体机械臂模型，启动MoveIt设置界面，配置虚拟关节，连接场景环境与机械臂作为机械臂的底座。

其中，所述利用图像获取装置获取实景防护门图像，还包括：

获取所述图像获取装置的标准参数信息，并根据所述标准参数信息对所述图像获取装置进行配置操作；所述配置操作包括所述图像获取装置的深度测量量程、内外参数的配置。

基于配置完成的图像获取装置，获取得到实景防护门图像。

且当所述图像获取装置参数配置完成后，基于ROS系统生成机械臂装置配置文件，此时将会生成下文中所用到的所有xx.lanuch文件，运行roslaunch config demo.launch启动ROS可视化界面(RViz)、仿真控制器以及MoveIt。且行roslaunch gazebo empty_world.launch和roslaunch moveit_config rviz.launch config:＝ture，根据实际场景，创建防护门模型。

其中，所述根据所述防护门图像，重构得到所述防护门的三维八叉树点云地图，包括：

所述图像获取装置获取防护门图像，并提取得到防护门图像的点云数据信息，并根据所述点云数据信息，重构得到防护门的三维八叉树点云地图。所述点云数据信息为碰撞体信息，也即所述机械臂模型与所述三维八叉树点云地图代表着实物机械臂和防护门，且当所述机械臂模型与所述点云数据信息进行靠近重合时，可清晰地获知实物机械臂是否与所述防护门碰撞，从而不受机械结构、动力数据源准确性的限制，增强了碰撞检测的稳定性与可靠性。具体的，ROS系统运行roslanuch gazebo grasping_world.lanuch和roslaunch moveit_config rviz.launch config:＝ture，接收深度相机获取到的点云数据信息并转化为八叉树立体图形，并通过所述ROS用户可视化界面，规划机械臂路径，当点云数据与机械臂本体将要出现重合时，停止机械臂动作，从而实现模拟场景下的防撞功能，还且将规划好的机械臂路径的相关逻辑程序载入实际机械臂的控制中心模块中，完成真实场景下的防护门防撞功能。

其中，所述判断所述机械臂模型与所述防护门的三维八叉树点云地图的相近状态，并根据所述相近状态，指令控制所述真实机械臂的运行，包括：

根据所述机械臂模型和所述点云数据信息，判断得到所述机械臂模型与所述防护门的三维八叉树点云地图是否存在重合，也即根据所述机械臂模型的空间坐标信息和所述防护门的三维八叉树点云地图的坐标信息进行匹配操作，以判断是否存在重合。

若重合，则判定所述真实机械臂与所述防护门出现碰撞，并控制停止所述真实机械臂的运行；

若不重合，则获取所述机械臂模型与所述防护门的三维八叉树点云地图的相近距离，并将所述相近距离与预警碰撞距离进行比较，判断所述相近距离是否小于或等于所述预警碰撞距离，若所述相近距离小于或等于所述预警碰撞距离，则指令控制所述机械臂停止运行，若所述相近距离大于所述预警碰撞距离，则继续检测获取所述相近距离。其中，所述相近距离可通过所述机械臂模型和所述防护门的三维八叉树点云地图的坐标信息计算得到。

其中，所述防护门防撞方法还包括基于贝叶斯准则实时更新所述防护门的三维八叉树点云地图。

其中，所述防护门防撞方法还包括基于运动规划信息，控制所述真实机械臂运动。

要说明的是，本发明实施例中的防护门的防撞系统可以用于实现上述方法实施例中的全部技术方案，其各个功能模块的功能可以根据下述方法实施例中的方法具体实现，其具体实现过程可参照上述实施例中的相关描述，此处不再赘述。

另外，本发明还提供了一种用于机械臂的防护门防撞系统，应用所述防护门防撞方法，包括：模型构建模块，用于根据真实机械臂结构，构建得到机械臂模型；地图重构模块，用于利用图像获取装置获取实景防护门图像，并根据防护门模型，并根据所述防护门图像，重构得到防护门的三维八叉树点云地图；控制中心模块，用于根据所述机械臂模型和三维八叉树点云地图，判断所述机械臂模型与所述防护门的三维八叉树点云地图的相近状态，并根据所述相近状态指令控制所述机械臂的运行。另外，所述控制中心模块还用于向所述机械臂的运动组下达运动组动作指令、放置指令、抓取指令，以及进行所述机械臂路径规划，并执行路径服务，且根据获取规划场景服务信息，检测获取的场景规划。其中，所述进行机械臂路径规划时，根据正反向运动学服务信息和笛卡尔坐标进行路径规划。所述控制中心模块根据待触碰物检测信息或障碍物碰撞检测信息，形成动作指令、放置指令或抓取指令。其中，可通过压力传感器、视觉传感器、动作传感器等设备检测获取所述待触碰物信息、障碍物信息或连杆状态信息、关节状态信息。所述机械臂的运动组包括连杆、关机、舵机、电机等。

另外，所述防护门防撞系统还包括参数服务模块，用于输入存储所述机械臂模型的定义参数、机械臂电气的定义参数和场景环境布置的定义参数，以供所述模型构建模块、地图重构模块和控制中心模块的数据调用。

综上所述，本发明公开的一种用于机械臂的防护门防撞方法及系统，所产生的有益技术效果包括：1、利用机器视觉技术，检测将要发生的碰撞，不受机械结构、动力数据源准确性的限制，增强了碰撞检测的稳定性与可靠性；2、能够在碰撞发生之前进行检测，提高了工业机器人的安全性、工业设备的耐久度、生产安全；3、可以有效降低载荷，提高了工业机器人的灵活性，降低生产成本及功耗；4、结合动力及图像等多种传感系统，能够有效防止机械臂在工作过程由于某传感器失灵、实时处理性能不够或控制程序逻辑出现问题导致的撞门问题。

虽然上面已经参考各种实施例描述了本发明，但是应当理解，在不脱离本发明的范围的情况下，可以进行许多改变和修改。也就是说上面讨论的方法、系统和设备是示例，各种配置可以适当地省略、替换或添加各种过程或组件。例如，在替代配置中，可以以与所描述的顺序不同的顺序执行方法和/或可以添加、省略和/或组合各种部件。而且，关于某些配置描述的特征可以以各种其他配置组合，如可以以类似的方式组合配置的不同方面和元素。此外，随着技术发展其中的元素可以更新，即许多元素是示例，并不限制本发明公开或权利要求的范围。

在说明书中给出了具体细节以提供对包括实现的示例性配置的透彻理解。然而，可以在没有这些具体细节的情况下实践配置,例如已经示出了众所周知的电路、过程、算法、结构和技术而没有不必要的细节，以避免模糊配置。该描述仅提供示例配置，并且不限制权利要求的范围，适用性或配置。相反，前面对配置的描述将为本领域技术人员提供用于实现所描述的技术的使能描述。在不脱离本发明公开的精神或范围的情况下，可以对元件的功能和布置进行各种改变。

综上，其旨在上述详细描述被认为是例示性的而非限制性的，并且应当理解，以下权利要求(包括所有等同物)旨在限定本发明的精神和范围。以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。