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

权利要求书

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为机器人运动持续时间；

所述S1包括：

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

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

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

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

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

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

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

4.如权利要求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)为机器人包围盒的运动轨迹。

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