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

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

【技术实现步骤摘要】
一种面向人机协同操作的动力学融合行为安全算法及系统
本专利技术属于机器人任务规划及碰撞检测领域，更具体地，涉及一种面向人机协同操作的动力学融合行为安全算法及系统。
技术介绍
随着协作机器人应用越来越广泛，人机协同操作在智能制造业出现的频次也越来越高，这对协作机器人的人机协同操作能力提出了更越高的需求。为了保证人的安全，传统电子围栏式的速度级别设置控制模式或机器人停止等待等被动模式在应对具有物理交互的人机协同操作时，已经变得越来越低效。现有的人机协作空间内的安全算法研究仍是基于距离阈值设置速度等级的模式。此种模式只考虑到人的安全而忽略了协作任务的效率，并且在近距离人机协作过程中，未考虑到人大部分处于被部分遮挡的状态，因此很难真正应用于人机协作任务。人机安全一直是协作机器人的研究重点之一。现有的研究重点都是针对工业机器人添加柔性皮肤或柔性原件等添加安全硬件的方式；或者采用超声波、红外、视觉传感器或力传感器等多传感融合的方式构建不同等级的电子围栏，以根据人机距离实现分级调速。上述两种模式很难辅助协作机器人高效的完成近距离的人机协作任务。因此，亟待研发一种通过安全状态判断可进行自主避障规划的人机安全算法，实现协作机器人与人之间的精准、高效、安全地协同操作。
技术实现思路
针对现有技术的缺陷和改进需求，本专利技术提供了一种面向人机协同操作的动力学融合行为安全算法及系统，其目的在于根据人的动作自主判断协作场景中的人机安全状态，从而控制机器人进行安全响应，满足人机协同操作的安全性和协作任务的连续性，在保障人安全的前提下提升人机协同操作的效率。为实现上述目的，按照本专利技术的一个方面，提供了一种面向人机协同操作的动力学融合行为安全算法，包括：S1，识别人体各关键点的像素坐标，并根据所述像素坐标计算人体各关节点的三维坐标；S2，在人体运动构型数据集中搜索与所述人体各关节点的三维坐标最接近的相似构型，并计算所述相似构型中人体各关节的运动关节角度；S3，根据所述人体各关节的运动关节角度分别生成包围所述人体各关节的虚拟人包围盒，计算机器人包围盒与各所述虚拟人包围盒之间的最小距离；S4，计算各所述最小距离对应的虚拟力，计算各所述虚拟力的平方和与机器人各关节末端力的平方和之间的比值，并将1与所述比值之间的差值设置为控制参数；S5，当wv＜kd且tc＜ti时，控制机器人处于避让运动状态；当wv＜kd且tc≥ti时，控制所述机器人处于暂停状态；当wv≥kd时，控制所述机器人处于协作运动状态，其中，wv为所述控制参数，kd为预置阈值，ti为虚拟力的作用时长，tc为机器人运动持续时间。更进一步地，所述S1包括：识别人体各关键点的像素坐标，并将所述像素坐标映射至相机坐标系，得到人体各关节点在相机坐标系下的三维坐标信息；对所述人体各关节点在相机坐标系下的三维坐标信息进行限速滤波；将滤波后人体各关节点在相机坐标系下的三维坐标信息转换至机器人坐标系下，得到所述人体各关节点的三维坐标。更进一步地，滤波后人体各关节点在相机坐标系下的三维坐标信息P为：其中，Pt1、Pt2和Pt3分别为t1时刻、t2时刻和t3时刻人体各关节点在相机坐标系下的三维坐标，Δd为更新阈值。更进一步地，所述S2中利用kd树从所述人体运动构型数据集中搜索出所述相似构型。更进一步地，所述人体各关节的运动关节角度为：其中，θi为人体第i个关节的运动关节角度，Pbi为所述相似构型中人体第i个关节点的三维坐标，Pki为利用kd树搜索出与Pbi最接近的点，i＝1,2,…,k，k为人体的关节数。更进一步地，所述S3中实时计算运动过程中所述机器人包围盒与各所述虚拟人包围盒之间的最小距离，所述最小距离为：di＝inf{||xi-y||2:xi∈traAi(t),y∈traB(t)}其中，di为机器人包围盒与第i个关节对应的虚拟人包围盒之间的最小距离，xi为traAi(t)中的点，y为traB(t)，traAi(t)为第i个关节对应的虚拟人包围盒的运动轨迹，traB(t)为机器人包围盒的运动轨迹。更进一步地，所述机器人包围盒根据FCL库中的数据生成。更进一步地，所述虚拟力为：其中，Fvti为人体第i个关节在t时刻对应的虚拟力，Fei为机器人第i个关节的末端力，dti为第i个关节在t时刻对应的最小距离，ka为第一调节参数，kb为第二调节参数，kc为第三调节参数。更进一步地，ka、kb、kc的取值分别为1、0.2、10。按照本专利技术的另一个方面，提供了一种面向人机协同操作的动力学融合行为安全系统，包括：识别模块，用于识别人体各关键点的像素坐标，并根据所述像素坐标计算人体各关节点的三维坐标；搜索模块，用于在人体运动构型数据集中搜索与所述人体各关节点的三维坐标最接近的相似构型，并计算所述相似构型中人体各关节的运动关节角度；第一计算模块，用于根据所述人体各关节的运动关节角度分别生成包围所述人体各关节的虚拟人包围盒，计算机器人包围盒与各所述虚拟人包围盒之间的最小距离；第二计算模块，用于计算各所述最小距离对应的虚拟力，计算各所述虚拟力的平方和与机器人各关节末端力的平方和之间的比值，并将1与所述比值之间的差值设置为控制参数；控制模块，用于当wv＜kd且tc＜ti时，控制机器人处于避让运动状态；当wv＜kd且tc≥ti时，控制所述机器人处于暂停状态；当wv≥kd时，控制所述机器人处于协作运动状态，其中，wv为所述控制参数，kd为预置阈值，ti为虚拟力的作用时长，tc为机器人运动持续时间。总体而言，通过本专利技术所构思的以上技术方案，能够取得以下有益效果：根据人体各关节点在机器人坐标系下各关节的运动关节角度生成包围人体各关节的虚拟人包围盒，将虚拟人包围盒与机器人包围盒之间的最小距离转化为虚拟力，构建虚拟人-机器人-动力学数字孪生系统，实现视觉信息到力信息的转化及融合，通过虚拟力信息判断人机协作任务的安全状态，保障协作任务中人的安全；通过安全状态估计实现安全规划，实现快速行为安全响应，从状态变化到机器人完成安全规划耗时不超过0.05s；通过三种安全规划实现尽量不中断的人机协作，以接触式机器人维护为验证实验的实验结果表明，较传统的停机-维护-评估的模式，此算法效率可提升26.4％。附图说明图1为本专利技术实施例提供的面向人机协同操作的动力学融合行为安全算法的流程图；图2为本专利技术实施例提供的面向人机协同操作的动力学融合行为安全算法中状态切换的示意图；图3为本专利技术实施例提供的面向人机协同操作的动力学融合行为安全算法的控制框架图；图4为本专利技术实施例提供的面向人机协同操作的动力学融合行为安全算法中构建动力学-数字孪生人体模型的过程示意图；图5为本专利技术实施例提供的面向人机协同操作的动力学融合行为安全算法中构建的人-机器人动力学系统包围盒模型，图6为本专利技术实施例提供的面向人机协同操作的动力学融合行为安全系统的框图。具体实施方式为本文档来自技高网...

【技术保护点】
1.一种面向人机协同操作的动力学融合行为安全算法，其特征在于，包括：/nS1，识别人体各关键点的像素坐标，并根据所述像素坐标计算人体各关节点的三维坐标；/nS2，在人体运动构型数据集中搜索与所述人体各关节点的三维坐标最接近的相似构型，并计算所述相似构型中人体各关节的运动关节角度；/nS3，根据所述人体各关节的运动关节角度分别生成包围所述人体各关节的虚拟人包围盒，计算机器人包围盒与各所述虚拟人包围盒之间的最小距离；/nS4，计算各所述最小距离对应的虚拟力，计算各所述虚拟力的平方和与机器人各关节末端力的平方和之间的比值，并将1与所述比值之间的差值设置为控制参数；/nS5，当w

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


2.如权利要求1所述的面向人机协同操作的动力学融合行为安全算法，其特征在于，所述S1包括：
识别人体各关键点的像素坐标，并将所述像素坐标映射至相机坐标系，得到人体各关节点在相机坐标系下的三维坐标信息；
对所述人体各关节点在相机坐标系下的三维坐标信息进行限速滤波；
将滤波后人体各关节点在相机坐标系下的三维坐标信息转换至机器人坐标系下，得到所述人体各关节点的三维坐标。


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



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


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


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



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

【专利技术属性】
技术研发人员：李世其，张帅，李肖，熊友军，谢峥，
申请(专利权)人：华中科技大学，
类型：发明
国别省市：湖北;42