CN112757274A - 一种面向人机协同操作的动力学融合行为安全算法及系统 - Google Patents

Abstract

本发明公开了一种面向人机协同操作的动力学融合行为安全算法及系统，属于机器人任务规划及碰撞检测领域，方法包括：计算人体各关节点的三维坐标；搜索与人体各关节点的三维坐标最接近的相似构型，以计算人体各关节的运动关节角度；根据人体各关节的运动关节角度分别生成包围人体各关节的虚拟人包围盒，计算机器人包围盒与各虚拟人包围盒之间的最小距离；计算各最小距离对应的虚拟力，根据各虚拟力的平方和与机器人各关节末端力的平方和之间的比值设置控制参数；根据控制参数、虚拟力的作用时长以及机器人运动持续时间控制机器人的状态。根据人的动作自主判断协作场景中的人机安全状态，从而控制机器人进行安全响应，避让规划迅速。

Description

一种面向人机协同操作的动力学融合行为安全算法及系统

技术领域

本发明属于机器人任务规划及碰撞检测领域，更具体地，涉及一种面向人机协同操作的动力学融合行为安全算法及系统。

背景技术

随着协作机器人应用越来越广泛，人机协同操作在智能制造业出现的频次也越来越高，这对协作机器人的人机协同操作能力提出了更越高的需求。为了保证人的安全，传统电子围栏式的速度级别设置控制模式或机器人停止等待等被动模式在应对具有物理交互的人机协同操作时，已经变得越来越低效。

现有的人机协作空间内的安全算法研究仍是基于距离阈值设置速度等级的模式。此种模式只考虑到人的安全而忽略了协作任务的效率，并且在近距离人机协作过程中，未考虑到人大部分处于被部分遮挡的状态，因此很难真正应用于人机协作任务。

人机安全一直是协作机器人的研究重点之一。现有的研究重点都是针对工业机器人添加柔性皮肤或柔性原件等添加安全硬件的方式；或者采用超声波、红外、视觉传感器或力传感器等多传感融合的方式构建不同等级的电子围栏，以根据人机距离实现分级调速。上述两种模式很难辅助协作机器人高效的完成近距离的人机协作任务。因此，亟待研发一种通过安全状态判断可进行自主避障规划的人机安全算法，实现协作机器人与人之间的精准、高效、安全地协同操作。

发明内容

针对现有技术的缺陷和改进需求，本发明提供了一种面向人机协同操作的动力学融合行为安全算法及系统，其目的在于根据人的动作自主判断协作场景中的人机安全状态，从而控制机器人进行安全响应，满足人机协同操作的安全性和协作任务的连续性，在保障人安全的前提下提升人机协同操作的效率。

为实现上述目的，按照本发明的一个方面，提供了一种面向人机协同操作的动力学融合行为安全算法，包括：S1，识别人体各关键点的像素坐标，并根据所述像素坐标计算人体各关节点的三维坐标；S2，在人体运动构型数据集中搜索与所述人体各关节点的三维坐标最接近的相似构型，并计算所述相似构型中人体各关节的运动关节角度；S3，根据所述人体各关节的运动关节角度分别生成包围所述人体各关节的虚拟人包围盒，计算机器人包围盒与各所述虚拟人包围盒之间的最小距离；S4，计算各所述最小距离对应的虚拟力，计算各所述虚拟力的平方和与机器人各关节末端力的平方和之间的比值，并将1与所述比值之间的差值设置为控制参数；S5，当w_v＜k_d且t_c＜t_i时，控制机器人处于避让运动状态；当w_v＜k_d且t_c≥t_i时，控制所述机器人处于暂停状态；当w_v≥k_d时，控制所述机器人处于协作运动状态，其中，w_v为所述控制参数，k_d为预置阈值，t_i为虚拟力的作用时长，t_c为机器人运动持续时间。

更进一步地，所述S1包括：识别人体各关键点的像素坐标，并将所述像素坐标映射至相机坐标系，得到人体各关节点在相机坐标系下的三维坐标信息；对所述人体各关节点在相机坐标系下的三维坐标信息进行限速滤波；将滤波后人体各关节点在相机坐标系下的三维坐标信息转换至机器人坐标系下，得到所述人体各关节点的三维坐标。

更进一步地，滤波后人体各关节点在相机坐标系下的三维坐标信息P为：

其中，P_t1、P_t2和P_t3分别为t1时刻、t2时刻和t3时刻人体各关节点在相机坐标系下的三维坐标，Δd为更新阈值。

更进一步地，所述S2中利用kd树从所述人体运动构型数据集中搜索出所述相似构型。

更进一步地，所述人体各关节的运动关节角度为：

其中，θ_i为人体第i个关节的运动关节角度，P_bi为所述相似构型中人体第i个关节点的三维坐标，P_ki为利用kd树搜索出与P_bi最接近的点，i＝1,2,…,k，k为人体的关节数。

更进一步地，所述S3中实时计算运动过程中所述机器人包围盒与各所述虚拟人包围盒之间的最小距离，所述最小距离为：

d_i＝inf{||x_i-y||₂:x_i∈traA_i(t),y∈traB(t)}

其中，d_i为机器人包围盒与第i个关节对应的虚拟人包围盒之间的最小距离，x_i为traA_i(t)中的点，y为traB(t)，traA_i(t)为第i个关节对应的虚拟人包围盒的运动轨迹，traB(t)为机器人包围盒的运动轨迹。

更进一步地，所述机器人包围盒根据FCL库中的数据生成。

更进一步地，所述虚拟力为：

其中，F_vti为人体第i个关节在t时刻对应的虚拟力，F_ei为机器人第i个关节的末端力，d_ti为第i个关节在t时刻对应的最小距离，k_a为第一调节参数，k_b为第二调节参数，k_c为第三调节参数。

更进一步地，k_a、k_b、k_c的取值分别为1、0.2、10。

按照本发明的另一个方面，提供了一种面向人机协同操作的动力学融合行为安全系统，包括：识别模块，用于识别人体各关键点的像素坐标，并根据所述像素坐标计算人体各关节点的三维坐标；搜索模块，用于在人体运动构型数据集中搜索与所述人体各关节点的三维坐标最接近的相似构型，并计算所述相似构型中人体各关节的运动关节角度；第一计算模块，用于根据所述人体各关节的运动关节角度分别生成包围所述人体各关节的虚拟人包围盒，计算机器人包围盒与各所述虚拟人包围盒之间的最小距离；第二计算模块，用于计算各所述最小距离对应的虚拟力，计算各所述虚拟力的平方和与机器人各关节末端力的平方和之间的比值，并将1与所述比值之间的差值设置为控制参数；控制模块，用于当w_v＜k_d且t_c＜t_i时，控制机器人处于避让运动状态；当w_v＜k_d且t_c≥t_i时，控制所述机器人处于暂停状态；当w_v≥k_d时，控制所述机器人处于协作运动状态，其中，w_v为所述控制参数，k_d为预置阈值，t_i为虚拟力的作用时长，t_c为机器人运动持续时间。

总体而言，通过本发明所构思的以上技术方案，能够取得以下有益效果：根据人体各关节点在机器人坐标系下各关节的运动关节角度生成包围人体各关节的虚拟人包围盒，将虚拟人包围盒与机器人包围盒之间的最小距离转化为虚拟力，构建虚拟人-机器人-动力学数字孪生系统，实现视觉信息到力信息的转化及融合，通过虚拟力信息判断人机协作任务的安全状态，保障协作任务中人的安全；通过安全状态估计实现安全规划，实现快速行为安全响应，从状态变化到机器人完成安全规划耗时不超过0.05s；通过三种安全规划实现尽量不中断的人机协作，以接触式机器人维护为验证实验的实验结果表明，较传统的停机-维护-评估的模式，此算法效率可提升26.4％。

附图说明

图1为本发明实施例提供的面向人机协同操作的动力学融合行为安全算法的流程图；

图2为本发明实施例提供的面向人机协同操作的动力学融合行为安全算法中状态切换的示意图；

图3为本发明实施例提供的面向人机协同操作的动力学融合行为安全算法的控制框架图；

图4为本发明实施例提供的面向人机协同操作的动力学融合行为安全算法中构建动力学-数字孪生人体模型的过程示意图；

图5为本发明实施例提供的面向人机协同操作的动力学融合行为安全算法中构建的人-机器人动力学系统包围盒模型，

图6为本发明实施例提供的面向人机协同操作的动力学融合行为安全系统的框图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

在本发明中，本发明及附图中的术语“第一”、“第二”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。

图1为本发明实施例提供的面向人机协同操作的动力学融合行为安全算法的流程图。参阅图1，结合图2-图5，对本实施例中面向人机协同操作的动力学融合行为安全算法进行详细说明。方法包括操作S1-操作S5。

操作S1，识别人体各关键点的像素坐标，并根据像素坐标计算人体各关节点的三维坐标。

根据本发明实施例，操作S1包括子操作S11-子操作S13。

在子操作S11中，识别人体各关键点的像素坐标，并将像素坐标映射至相机坐标系，得到人体各关节点在相机坐标系下的三维坐标信息。

具体地，例如利用机器学习算法从RGB图像中识别人体各关键点的像素坐标，并通过蒙版映射的方式结合深度图像估算出人体各关键点的三维坐标信息。关节点为关键点的子集。

以右手腕部点的三维坐标信息估算为例，假设估算出右手腕部点P_w在RGB图像中的像素坐标为[u,v]，其在深度图中投影为d_p，将该点在相机坐标系下对应点的三维坐标信息设置为P(x_p,y_p,z_p)，则x_p、y_p、z_p可由下式求解：

z_p＝d_p/s

其中，C为相机的固有参数，f_x和f_y表示相机坐标，c_x和c_y表示像素坐标，s表示深度图像的缩放因子。

在子操作S12中，对人体各关节点在相机坐标系下的三维坐标信息进行限速滤波。

本实施例中，利用限速滤波降低深度图孔洞信息或传感器噪声引起的人体关键点估算误差。滤波后人体各关节点在相机坐标系下的三维坐标信息P为：

其中，P_t1、P_t2和P_t3分别为t1时刻、t2时刻和t3时刻人体各关节点在相机坐标系下的三维坐标，Δd为更新阈值。

在子操作S13中，将滤波后人体各关节点在相机坐标系下的三维坐标信息转换至机器人坐标系下，得到人体各关节点的三维坐标。

相机坐标系与机器人坐标系下坐标信息的转换关系为：

其中，(x_b,y_b,z_b)表示机器人坐标系下的三维坐标，R表示从相机坐标系到机器人坐标系的旋转矩阵，t表示从相机坐标系到机器人坐标系的平移矩阵。

进一步地，将人体各关节点的三维坐标通过ROS master发送到控制机器人的系统中。ROS master为机器人操作系统的通讯中心。

操作S2，在人体运动构型数据集中搜索与人体各关节点的三维坐标最接近的相似构型，并计算相似构型中人体各关节的运动关节角度。

订阅人体各关节点的三维坐标，利用kd树从人体运动构型数据集中搜索出相似构型。具体地，通过正运动学在各关节限位内采样构建动力学-数字孪生人体模型的可达空间S，S的数据量可通过采样密度调节；然后，将可达空间S构建成kd树所对应的数据格式S_kd；当根据视觉信息估算出数据点坐标后，通过k临近搜索在S_kd中迅速搜索出最相近的构型，该构型即为相似构型。

通过相似构型确定的人体各关节的运动关节角度为：

其中，θ_i为人体第i个关节的运动关节角度，P_bi为相似构型中人体第i个关节点的三维坐标，P_ki为利用kd树搜索出与P_bi最接近的点，i＝1,2,…,k，k为人体的关节数。

进一步地，通过视觉与力觉融合的方式将人体动力学系统和机器人动力学系统进行整合，构建虚拟人-机器人-动力学数字孪生系统，如图4所示。该系统可根据视觉信息与机器人传感信息实时感知人与机器人之间的虚拟作用力，从而根据虚拟作用力的大小对机器人进行安全规划，实现人机安全。当获取到传感器采集到的人体各关节角和机器人各关节角后，人-机器人-动力学数字孪生系统便可根据视觉信息实时更新人体各关节的运动关节角度。

操作S3，根据人体各关节的运动关节角度分别生成包围人体各关节的虚拟人包围盒，计算机器人包围盒与各虚拟人包围盒之间的最小距离。

本发明实施例中，根据柔性碰撞库(Flexible Collision Library，FCL)构建包围盒规则，从而生成机器人包围盒。本实施例中，构建的机器人包围盒和虚拟人包围盒如图5所示。实时计算运动过程中机器人包围盒与各虚拟人包围盒之间的最小距离，最小距离为：

d_i＝inf{||x_i-y||₂:x_i∈traA_i(t),y∈traB(t)}

其中，d_i为机器人包围盒与第i个关节对应的虚拟人包围盒之间的最小距离，x_i为traA_i(t)中的点，y为traB(t)，traA_i(t)为第i个关节对应的虚拟人包围盒的运动轨迹，traB(t)为机器人包围盒的运动轨迹。

操作S4，计算各最小距离对应的虚拟力，计算各虚拟力的平方和与机器人各关节末端力的平方和之间的比值，并将1与比值之间的差值设置为控制参数。

本发明实施例中，虚拟力为：

其中，F_vti为人体第i个关节在t时刻对应的虚拟力，F_ei为机器人第i个关节的末端力，d_ti为第i个关节在t时刻对应的最小距离，k_a为第一调节参数，k_b为第二调节参数，k_c为第三调节参数。本实施例中，将k_a、k_b、k_c的取值分别设置为1、0.2、10。可以理解的是，k_a、k_b、k_c也可以有其他取值。

控制参数w_v为：

操作S5，当w_v＜k_d且t_c＜t_i时，控制机器人处于避让运动状态；当w_v＜k_d且t_c≥t_i时，控制机器人处于暂停状态；当w_v≥k_d时，控制机器人处于协作运动状态，其中，w_v为控制参数，k_d为预置阈值，t_i为虚拟力的作用时长，t_c为机器人运动持续时间。

本发明实施例中，根据虚拟力的大小、机器人关节当前末端力的大小、虚拟力的作用时长作为安全状态的判断指标进行状态判断，针对不同的状态进行安全规划，驱动机器人进行避让、暂停以及协作运动等状态的调整，状态估计结果为：

其中，s为状态估计结果，0、1、2分别表示避让运动状态、协作运动状态、暂停状态。根据该状态估计结果调整机器人的状态，机器人的状态切换如图3所示。

参阅图2，本发明实施例中面向人机协同操作的动力学融合行为安全算法，通过构建虚拟人-机器人-动力学数字孪生系统，实现视觉信息到力信息的转化及融合，通过虚拟力信息判断人机协作任务的安全状态，保障协作任务中人的安全；通过安全状态估计实现安全规划，实现快速行为安全响应。实验表明，从状态变化到机器人完成安全规划耗时不超过0.05s；通过三种安全规划实现尽量不中断的人机协作，以接触式机器人维护为验证实验的实验结果表明，较传统的停机-维护-评估的模式，此算法效率可提升26.4％。

图6为本发明实施例提供的面向人机协同操作的动力学融合行为安全系统的框图。参阅图6，该面向人机协同操作的动力学融合行为安全系统600包括识别模块610、搜索模块620、第一计算模块630、第二计算模块640以及控制模块650。

识别模块610例如执行操作S1，用于识别人体各关键点的像素坐标，并根据像素坐标计算人体各关节点的三维坐标。

搜索模块620例如执行操作S2，用于在人体运动构型数据集中搜索与人体各关节点的三维坐标最接近的相似构型，并计算相似构型中人体各关节的运动关节角度。

第一计算模块630例如执行操作S3，用于根据人体各关节的运动关节角度分别生成包围人体各关节的虚拟人包围盒，计算机器人包围盒与各虚拟人包围盒之间的最小距离。

第二计算模块640例如执行操作S4，用于计算各最小距离对应的虚拟力，计算各虚拟力的平方和与机器人各关节末端力的平方和之间的比值，并将1与比值之间的差值设置为控制参数。

控制模块650例如执行操作S5，用于当w_v＜k_d且t_c＜t_i时，控制机器人处于避让运动状态；当w_v＜k_d且t_c≥t_i时，控制机器人处于暂停状态；当w_v≥k_d时，控制机器人处于协作运动状态，其中，w_v为控制参数，k_d为预置阈值，t_i为虚拟力的作用时长，t_c为机器人运动持续时间。

面向人机协同操作的动力学融合行为安全系统600用于执行上述图1-图5所示实施例中的面向人机协同操作的动力学融合行为安全算法。本实施例未尽之细节，请参阅前述图1-图5所示实施例中的面向人机协同操作的动力学融合行为安全算法，此处不再赘述。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种面向人机协同操作的动力学融合行为安全算法，其特征在于，包括：

S1，识别人体各关键点的像素坐标，并根据所述像素坐标计算人体各关节点的三维坐标；

S2，在人体运动构型数据集中搜索与所述人体各关节点的三维坐标最接近的相似构型，并计算所述相似构型中人体各关节的运动关节角度；

S3，根据所述人体各关节的运动关节角度分别生成包围所述人体各关节的虚拟人包围盒，计算机器人包围盒与各所述虚拟人包围盒之间的最小距离；

S4，计算各所述最小距离对应的虚拟力，计算各所述虚拟力的平方和与机器人各关节末端力的平方和之间的比值，并将1与所述比值之间的差值设置为控制参数；

S5，当w_v＜k_d且t_c＜t_i时，控制机器人处于避让运动状态；当w_v＜k_d且t_c≥t_i时，控制所述机器人处于暂停状态；当w_v≥k_d时，控制所述机器人处于协作运动状态，其中，w_v为所述控制参数，k_d为预置阈值，t_i为虚拟力的作用时长，t_c为机器人运动持续时间。

2.如权利要求1所述的面向人机协同操作的动力学融合行为安全算法，其特征在于，所述S1包括：

识别人体各关键点的像素坐标，并将所述像素坐标映射至相机坐标系，得到人体各关节点在相机坐标系下的三维坐标信息；

对所述人体各关节点在相机坐标系下的三维坐标信息进行限速滤波；

将滤波后人体各关节点在相机坐标系下的三维坐标信息转换至机器人坐标系下，得到所述人体各关节点的三维坐标。

3.如权利要求2所述的面向人机协同操作的动力学融合行为安全算法，其特征在于，滤波后人体各关节点在相机坐标系下的三维坐标信息P为：

其中，P_t1、P_t2和P_t3分别为t1时刻、t2时刻和t3时刻人体各关节点在相机坐标系下的三维坐标，Δd为更新阈值。

4.如权利要求1所述的面向人机协同操作的动力学融合行为安全算法，其特征在于，所述S2中利用kd树从所述人体运动构型数据集中搜索出所述相似构型。

5.如权利要求4所述的面向人机协同操作的动力学融合行为安全算法，其特征在于，所述人体各关节的运动关节角度为：

其中，θ_i为人体第i个关节的运动关节角度，P_bi为所述相似构型中人体第i个关节点的三维坐标，P_ki为利用kd树搜索出与P_bi最接近的点，i＝1,2,…,k，k为人体的关节数。

6.如权利要求1所述的面向人机协同操作的动力学融合行为安全算法，其特征在于，所述S3中实时计算运动过程中所述机器人包围盒与各所述虚拟人包围盒之间的最小距离，所述最小距离为：

d_i＝inf{||x_i-y||₂:x_i∈traA_i(t),y∈traB(t)}

其中，d_i为机器人包围盒与第i个关节对应的虚拟人包围盒之间的最小距离，x_i为traA_i(t)中的点，y为traB(t)，traA_i(t)为第i个关节对应的虚拟人包围盒的运动轨迹，traB(t)为机器人包围盒的运动轨迹。

7.如权利要求1所述的面向人机协同操作的动力学融合行为安全算法，其特征在于，所述机器人包围盒根据FCL库中的数据生成。

8.如权利要求1-7任一项所述的面向人机协同操作的动力学融合行为安全算法，其特征在于，所述虚拟力为：

其中，F_vti为人体第i个关节在t时刻对应的虚拟力，F_ei为机器人第i个关节的末端力，d_ti为第i个关节在t时刻对应的最小距离，k_a为第一调节参数，k_b为第二调节参数，k_c为第三调节参数。

9.如权利要求8所述的面向人机协同操作的动力学融合行为安全算法，其特征在于，k_a、k_b、k_c的取值分别为1、0.2、10。

10.一种面向人机协同操作的动力学融合行为安全系统，其特征在于，包括：

识别模块，用于识别人体各关键点的像素坐标，并根据所述像素坐标计算人体各关节点的三维坐标；

搜索模块，用于在人体运动构型数据集中搜索与所述人体各关节点的三维坐标最接近的相似构型，并计算所述相似构型中人体各关节的运动关节角度；

第一计算模块，用于根据所述人体各关节的运动关节角度分别生成包围所述人体各关节的虚拟人包围盒，计算机器人包围盒与各所述虚拟人包围盒之间的最小距离；

第二计算模块，用于计算各所述最小距离对应的虚拟力，计算各所述虚拟力的平方和与机器人各关节末端力的平方和之间的比值，并将1与所述比值之间的差值设置为控制参数；

控制模块，用于当w_v＜k_d且t_c＜t_i时，控制机器人处于避让运动状态；当w_v＜k_d且t_c≥t_i时，控制所述机器人处于暂停状态；当w_v≥k_d时，控制所述机器人处于协作运动状态，其中，w_v为所述控制参数，k_d为预置阈值，t_i为虚拟力的作用时长，t_c为机器人运动持续时间。

Images