CN109822579A - 基于视觉的协作机器人安全控制方法 - Google Patents

Abstract

本发明公开了一种基于视觉的协作机器人安全控制方法。该方法结合深度视觉三维重建与轨迹规划技术，在仿真环境中对人机协作场景进行实时三维重建，并结合机器人自身模型进行避障轨迹规划。本发明可以使协作作业机器人具备对场景的感知能力，从而避免人机协作作业中危险碰撞的发生，实现对协作机器人的安全控制。

Description

基于视觉的协作机器人安全控制方法

技术领域

本发明涉及机器人安全控制领域，具体涉及一种基于视觉的协作机器人安全控制方法。

背景技术

协作机器人(Collaborative Robot)是一种可以安全的与人类进行直接交互的机器人。与传统工业机器人不同，协作机器人拓展了机器人功能内涵中“人”的属性，使机器人具备一定的自主行为和协作能力，可在非结构的环境下与人配合完成复杂的动作和任务，使机器人真正成为人的合作伙伴，结合人的智力、灵巧性和机器的体力、力量和准确性，人机协作完成诸如精密装配等工作，解决传统工业机器人应用的局限性。根据机器人与人的协作程度，协作等级从工人与机器人共享工作区域但没有直接接触或同步任务，最高到机器人会实时调整其运动以配合工人的动作。

近年来国内外许多机器人公司先后推出协作机器人产品。美国机器人公司Rethink在2012年推出协作机器人Baxter，它能够对人进行感知。2015年公布了新一代协作机器人Sawyer，它体型更小，能够处理高精度工业任务。丹麦机器人公司Universal Robots推出的UR系列协作机器人是目前市场上最流行的协作机器人，该系列机器人具有轻量、易用、灵活的特点，在协作性和安全性方面有着优秀的表现。国内近年来也研发出一些协作机器人产品，新松机器人2015年推出国内首台七自由度双臂协作机器人，它具有柔性多关节技术和可动双目视觉系统。这些机器人均面向人机协作进行了特殊设计，作业的灵活性和安全性较高，但一般负载能力有限，且具有较高的使用成本，目前在工业生产中尚未得到大范围推广应用。

协作机器人应用在人机协作的工作环境中，因此安全是重中之重。传统工业机器人为了保证快速高效运行，设计中不考虑有人存在的环境，使用时需要用安全围栏将机器人工作环境围起来。协作机器人的应用环境中需要考虑到人的存在，目前大多数协作机器人采用基于力反馈的方式感知与人的非安全接触，这种方法能够有效的降低人机碰撞带来的伤害，但力传感器造价较高，导致协作机器人成本提升，一定程度上阻碍了协作机器人的推广。同时力反馈的方法无法对环境进行感知，进而无法提前对危险进行预判。

因此有必要开发一种低成本，通用性较好，针对人机协作场景的机器人安全控制方法。

发明内容

为了提高人机协作环境中机器人的安全性，降低协作机器人的应用成本，本发明提出一种基于视觉的协作机器人的安全控制方法，该方法通过采集协作环境深度信息在仿真环境中进行重建，在仿真环境中检测安全状况，在检测到机器人将会发生碰撞时重规划机器人路径，实现对机器人的安全控制。

本发明提出一种基于视觉的协作机器人安全控制方法，该方法由多个软件模块协同实现，包括：仿真模块、安全检测模块与轨迹规划模块。仿真模块用于对协作作业场景进行仿真，仿真内容包括机械臂模型与其周围环境的三维重建模型。安全检测模块用于对当前作业环境的安全程度进行检测，通过仿真环境中机械臂与动态障碍的相对状态来判断安全级别，从而采取相应的应对措施。轨迹规划模块用于求解机械臂从起始点到目标点的无碰撞运动路径，采用基于随机扩展的RRT算法，在机器人状态空间中规划末端运动路径。

该方法各模块按照以下步骤进行：

步骤1：协作机器人正常运行过程中，通过深度相机获取协作作业场景的深度图像，并将图像信息传递给上位机中的仿真模块；

步骤2：仿真模块根据步骤1获得的深度图像信息对场景进行三维重建，并结合已有的机器人模型，实时重构出机器人及作业现场的仿真虚拟场景；

步骤3：安全检测模块对步骤2的实时仿真进行安全程度判定，根据人机相对状态将分为安全、需谨慎、危险三个安全等级，分别对应等级1到3，三种状态将触发不同措施：安全状态机器人全速运行，需谨慎状态机器人减速运行，危险状态机器人停止运行并重新规划路径；

步骤4：根据步骤3的安全判断结果，当出现危险状态时，将会触发轨迹规划模块对机器人末端运行路径重新进行规划，轨迹规划模块在状态空间规划无碰撞且不发生干涉的路径，并进行平滑细插值处理。生成的新路径点将分别发送给仿真模块中的机器人模型和实际环境中协作机器人的运动控制器，控制仿真机器人模型与实际机器人同步运行。

所述的仿真模块通过一个架设在协作作业区域侧上方的深度相机获取作业现场的深度图像，深度相机选用基于ToF的Kinect V2，其视野范围覆盖协作作业区域、机械臂、协作者工作区域，深度相机与协作机器人经过手眼标定。

仿真模块仿真的内容包括机械臂模型与其周围环境的三维重建模型。机械臂模型根据轨迹规划模块发出的路径指令进行运动。环境三维重建模型由通过视觉传感器得到的深度点云信息进行VoxelGrid下采样得到的体素构成。仿真环境由机器人操作系统ROS中的MoveIt！模块提供的Rviz实现，三维重建具体包括如下步骤：

步骤1：ToF深度相机实时采集人机协作作业现场的RGBD图像信息，对图像进行滤波去噪；

步骤2：通过已标定相机的内外参数，将步骤1采集的图像像素坐标经过坐标转换转为世界坐标系中的点，形成点云；

步骤3：在仿真环境中建立VoxelGrid，并导入点云信息，若网格中含有点云则判定其为填充，若无点云则判定为空白，对视觉范围内网格进行遍历判定，可以得到体素表示的场景三维模型，由此完成对场景的三维重建。

所述的安全检测模块，其关键部分在于对作业场景安全程度进行实时的检测。根据仿真模块中三维重建场景与机器人模型的相对位置与速度进行评判，具体评判指标与措施为：

等级1，场景未出现动态障碍，机器人按全速运行；

等级2，场景中出现动态障碍，但未与机器人运行路径重合，机器人末端运行速度降速至50％；

等级3，场景中出现动态障碍，且与机器人运行路径重合，机器人在当前位置停止运动，并触发对路径的重新规划。

所述的轨迹规划模块根据仿真环境信息使用RRT算法对机械臂的运动路径进行规划，使机械臂末端在不发生碰撞的情况下运动到目标位姿，该模块具体步骤如下：

步骤1：在状态空间中，机械臂末端现有位姿速度加速度状态x_init为起始点，设定目标位姿速度加速度状态点x_goal，其中x_init计入生成树T中；

步骤2：在状态空间生成新的状态点x_rand，找到T中距x_rand欧氏距离最近的点x_near，从x_near向x_rand按固定步长k延伸出新点x_new；

步骤3：判断从x_near到x_new的路径是否出现奇异点、自干涉和碰撞，若出现奇异点、自干涉或碰撞则舍弃此次树的生长，若未出现则将x_near到x_new之间的路径和x_new内入T中；

步骤4：判断x_goal与T中距离最近的状态点的欧式距离是否小于阈值，若否则重复步骤2、3，若是则连接x_goal与T中最近点，并将T中从x_init到x_goal的状态点路径作为规划轨迹。

步骤5：对上述生成轨迹使用Hermite曲线进行平滑细插值处理，将生成的轨迹点分别发送给仿真模块和机器人运动控制器，控制仿真机器人模型与实际机器人同步运行。

本发明的有益效果：

本发明提供了一种低成本，通用性较好的基于视觉的协作机器人安全控制方法。该方法使协作机器人具备一定的环境感知与应对能力，能够有效保障人机协作作业过程中操作者的安全，并且不依赖特定型号的机器人，不需过多调整就能应用在任意的人机协作生产环境中，从而促进协作机器人的应用推广。

附图说明

图1是本发明使用场景结构图。

图2是本发明方法结构图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明作进一步的详细说明。

参照图1，本方法适用的实施例程包括：深度视觉传感器、工业机器人及其运动控制器、上位机。其中，深度视觉传感器安装在协作作业场景侧上方，视野范围覆盖协作者、机械臂、作业区域，通过网线接口将深度视觉信息发送给上位机。工业机器人作为实际控制对象，其运动控制器接收上位机发送的路径点并进行细插补和关节空间转换，控制机器人按规划轨迹运动。本方法运行在上位机中，方法由多个软件模块协同实现，包括：仿真模块、安全检测模块、路径规划模块与运动控制模块。仿真模块用于对协作作业场景进行仿真，仿真内容包括机械臂模型与其周围环境的三维重建模型。安全检测模块用于对当前作业环境的安全程度进行检测，通过仿真环境中机械臂与动态障碍的相对状态来判断安全级别，从而采取相应的应对措施。轨迹规划模块用于求解机械臂从起始点到目标点的无碰撞运动路径，采用基于随机扩展的RRT算法，在机器人状态空间中规划末端运动路径。

各模块按照下述过程运行：协作机器人正常运行过程中，通过深度相机获取协作作业场景的深度图像，并将图像信启、传递给上位机中的仿真模块。仿真模块根据获得的深度图像信息对场景进行三维重建，并结合已有的机器人模型，实时重构出机器人及作业现场的仿真虚拟场景。安全检测模块对实时仿真场景进行安全程度判定，根据人机相对状态将分为安全、需谨慎、危险三个安全等级，分别对应等级1到3，三种状态将触发不同措施：安全状态机器人全速运行，需谨慎状态机器人减速运行，危险状态机器人停止运行并重新规划路径。根据安全判断结果，当出现危险状态时，将会触发轨迹规划模块对机器人末端运行路径重新进行规划，轨迹规划模块在状态空间规划无碰撞且不发生干涉的路径，并进行平滑细插值处理。生成的新路径点将分别发送给仿真模块中的机器人模型和实际环境中协作机器人的运动控制器，控制仿真机器人模型与实际机器人同步运行。本发明方法运行结构如图2所示。

所述的仿真模块通过一个架设在协作作业区域侧上方的深度相机获取作业现场的深度图像，深度相机选用基于ToF的Kinect V2，深度相机与协作机器人事先通过棋盘格标定法进行手眼标定，将棋盘标定板固定在机械臂末端以多个不同位姿对相机进行标定，从而在仿真环境中能够使用统一的世界坐标系描述。

仿真模块仿真的内容包括机械臂模型与其周围环境的三维重建模型。机械臂模型根据路径规划模块发出的路径点进行运动。环境三维重建模型由通过视觉传感器得到的深度点云信息进行VoxelGrid下采样得到的体素构成。仿真环境由机器人操作系统ROS中的MoveIt！模块提供的Rviz实现。该模块主要包括以下步骤：

步骤1：ToF深度相机实时采集人机协作作业现场的RGBD图像信息，该外设在ROS中以节点的形式存在，仿真模块节点通过订阅的方式以一定的时间间隔不断的获取其发布的深度图像。图像通过网线发送给上位机，借助OpenCV库的滤波函数对深度图像进行滤波去噪；

步骤2：通过已标定相机的内外参数，将步骤1采集的图像像素坐标经过坐标转换转为世界坐标系中的点，形成点云；

步骤3：在仿真环境中建立边长为3cm的VoxelGrid，并导入点云信息，若网格中含有点云则判定其为填充，若无点云则判定为空白，对视觉范围内网格进行遍历判定，可以得到体素表示的场景三维模型，由此完成对场景的三维重建。将重建场景导入到机器人模型所在仿真环境中。

所述的安全检测模块，其关键部分在于对作业场景安全程度进行实时的检测。根据仿真模块中三维重建场景与机器人模型的相对状态进行评判。具体评判指标与措施为：

等级1，场景未出现动态障碍，机器人按全速运行；

等级2，场景中出现动态障碍，但未与机器人运行路径重合，机器人末端运行速度降速至50％；

等级3，场景中出现动态障碍，且与机器人运行路径重合，机器人在当前位置停止运动，并触发对路径的重新规划。

具体实施时，仿真环境背景固定，当检测到无新增障碍物时，判断场景安全等级1。当出现新增障碍物，调用PlanningSceneMonitor类的isStateValid()函数对当前场景下未运行的各路径点进行检测，该函数返回的是当前的场景是否合法，如果发生自碰撞或者机器人与场景的碰撞等，该函数就回返回False。因此，若未出现机器人与场景的碰撞，判断场景安全等级2，否则判断当前安全等级3。

所述的轨迹规划模块根据仿真环境信息使用RRT算法对机械臂的运动路径进行规划，使机械臂末端在不发生碰撞的情况下运动到目标位姿，该模块包括如下步骤：

步骤1：在状态空间中，机械臂末端现有位姿速度加速度状态x_init为起始点，设定目标位姿速度加速度状态点x_goal，其中x_init计入生成树T中；

步骤2：在状态空间生成新的状态点x_rand，找到T中距x_rand欧氏距离最近的点x_near，从x_near向x_rand按固定步长k延伸出新点x_new；

步骤3：判断从x_near到x_new的路径是否出现奇异点、自干涉和碰撞，这里使用backKineticsXPcore()函数进行运动学反解，若反解失败证明该点奇异则舍弃该点，使用isStateValid()函数进行干涉碰撞检测，若出现碰撞则舍弃此次树的生长，若未出现则将x_near到x_new之间的路径和x_new纳入T中；

步骤4：判断x_goal与T中距离最近的状态点的欧式距离是否小于阈值，判断公式为dist(x_new，x_goal)≤L，若否则重复步骤2、3，若是则连接x_goal与T中最近点，并将T中从x_init到x_goal的状态点路径作为规划轨迹。

步骤5：对上述生成轨迹进行平滑细插值处理，将生成的轨迹点分别发送给仿真模块和机器人运动控制器，控制仿真机器人模型与实际机器人同步运行。

轨迹平滑采用Hermite曲线平滑处理，当出现相邻的三个点p0，p1，p2连线角度过于尖锐时需要对其进行曲线平滑。选取p0，p1，p2，p3为控制点。p0、p2端点处的切向矢量为r0＝p1-p0、r1＝p3-p2。将p0、p1、r0、r1带入Hermite几何矢量G中，根据Hermite方程公式Q(t)＝T·M·G，其中T＝[t³+t²+t+1]，M为常数，G＝[p0 p1 r0 r1]^T可求出Hermite曲线方程，在该曲线上每两个点之间进行均匀细插值得到最终输出的路径点。

Claims (5)

1.一种基于视觉的协作机器人安全控制方法，其特征在于，包括：仿真模块、安全检测模块与轨迹规划模块。

仿真模块用于对协作作业场景进行仿真，仿真内容包括机械臂模型与其周围环境的三维重建模型。

安全检测模块用于对当前作业环境的安全程度进行检测评判，通过仿真环境中机械臂与动态障碍的相对状态来判断安全级别，从而采取相应的应对措施。

轨迹规划模块用于求解机械臂从起始点到目标点的无碰撞运动路径，采用基于随机扩展的RRT算法，在机器人状态空间中规划末端运动轨迹。

该方法中各模块按照以下步骤运行：

步骤1：协作机器人正常运行过程中，通过深度相机获取协作作业场景的深度图像，并将图像信息传递给上位机中的仿真模块；

步骤2：仿真模块根据步骤1获得的深度图像信息对场景进行三维重建，并结合已有的机器人模型，实时重构出机器人及作业现场的仿真虚拟场景；

步骤3：安全检测模块对步骤2的实时仿真进行安全程度判定，根据人机相对状态将分为安全、需谨慎、危险三个安全等级，分别对应等级1到3，三种状态将触发不同措施：安全状态机器人全速运行，需谨慎状态机器人减速运行，危险状态机器人停止运行并重新规划路径；

步骤4：根据步骤3的安全判断结果，当出现危险状态时，将会触发轨迹规划模块对机器人末端运行路径重新进行规划，轨迹规划模块在状态空间规划无碰撞且不发生干涉的路径，并进行平滑细插值处理。生成的新路径点将分别发送给仿真模块中的机器人模型和实际环境中协作机器人的运动控制器，控制仿真机器人模型与实际机器人同步运行。

2.根据权利要求1所述的协作机器人安全控制方法，其特征在于，所述的仿真模块通过一个架设在协作作业区域侧上方的深度相机获取作业现场的深度图像，深度相机选用基于ToF的KinectV2，其视野范围覆盖协作者、机械臂、协作作业区域。

3.根据权利要求1所述的协作机器人安全控制方法，其特征在于，所述的仿真模块仿真的内容包括机械臂模型与其周围环境的三维重建模型。机械臂模型根据轨迹规划模块发出的路径指令进行运动。环境三维重建模型由通过视觉传感器得到的深度点云信息进行VoxelGrid下采样得到的体素构成。仿真环境由机器人操作系统ROS中的MoveIt！模块提供的Rviz实现，三维重建具体运行步骤如下：

步骤1：ToF深度相机实时采集人机协作作业现场的RGBD图像信息，对图像进行滤波去噪；

步骤2：通过已标定相机的内外参数，将步骤1采集的图像像素坐标经过坐标转换转为世界坐标系中的点，形成点云；

步骤3：在仿真环境中建立VoxelGrid，并导入点云信息，若网格中含有点云则判定其为填充，若无点云则判定为空白，对视觉范围内网格进行遍历判定，可以得到体素表示的场景三维模型，由此完成对场景的三维重建。

4.根据权利要求1所述的协作机器人安全控制方法，其特征在于，所述的安全检测模块，其关键部分在于对作业场景安全程度进行实时的检测。根据仿真模块中三维重建场景与机器人模型的相对位置与速度进行评判，具体评判指标与措施为：

等级1，场景未出现动态障碍，机器人按全速运行；

等级2，场景中出现动态障碍，但未与机器人运行路径重合，机器人末端运行速度降速至50％；

等级3，场景中出现动态障碍，且与机器人运行路径重合，机器人在当前位置停止运动，并触发对路径的重新规划。

5.根据权利要求1所述的协作机器人安全控制方法，其特征在于，所述的轨迹规划模块根据仿真环境信息使用RRT算法对机械臂的运动路径进行规划，使机械臂末端在不发生碰撞的情况下运动到目标位姿，该模块具体运行步骤如下：

步骤1：在状态空间中，机械臂末端现有状态x_init为起始点，设定目标状态点x_goal，其中x_init计入生成树T中；

步骤2：在状态空间生成新的状态点x_rand，找到T中距x_rand欧氏距离最近的点x_near，从x_near向x_rand按固定步长k延伸出新点x_new；

步骤3：判断从x_near到x_new的路径是否出现奇异点、自干涉和碰撞，若出现奇异点、自干涉或碰撞则舍弃此次树的生长，若未出现则将x_near到x_new之间的路径和x_new纳入T中；

步骤4：判断x_goal与T中距离最近的状态点的欧式距离是否小于阈值，若否则重复步骤2、3，若是则连接x_goal与T中最近点，并将T中从x_init到x_goal的状态点路径作为规划轨迹。

步骤5：对上述生成轨迹使用Hermite曲线进行平滑细插值处理，将生成的轨迹点分别发送给仿真模块和机器人运动控制器，控制仿真机器人模型与实际机器人同步运行。