CN110978064A - 人机协作中人体安全评估方法及系统 - Google Patents

Abstract

一种人机协作中人体安全评估方法及系统；该方法包括以下步骤：(1)获取人体在全局坐标系下的位姿；(2)获取机器人在全局坐标系下的位姿；(3)根据所获取的位姿信息，计算人体与机器人之间的距离以及机器人的运动速度；(4)根据所计算的人体与机器人之间的距离以及机器人的运动速度，判断人机协作中人体的安全等级；所述系统包括第一获取模块、第二获取模块、计算模块和评估模块。本发明在人机协作过程中，通过监控机器人与人体之间的距离和机器人的运行速度确定当前人体的安全状态，根据不同安全状态做出下一步工作计划，有效避免危险情况发生，使人机协作更安全更高效。

Description

人机协作中人体安全评估方法及系统

技术领域

本发明涉及人机协作领域，具体而言涉及在协作型机器人与人协作过程中的人体安全评估方法及系统。

背景技术

机器人行业的快速发展对人与机器人密切协作提出了新要求，逐渐催生出协作型机器人。传统工业机器人设计有围栏隔离，在机器人工作时不允许工作人员进入，或者在工作人员进入时，机器人必须停止工作，这种方法不仅浪费工作空间，而且降低了整体的工作效率。而协作型机器人，可以实现与人协同工作，不再需要防护栏进行隔离，将人的“知识、分析、决策”能力与机器人的“力量、精度、可重复性”结合在一起。协作型机器人与人共同处于同一工作空间，协作等级不断加强，二者紧密协调，不仅节省了工作空间，而且大大提高了工作效率，并且在确保人员安全的前提下，机器人能够自主提高技能，实现与人自然交互协作。

根据阿西莫夫机器人三定律，不管何种机器人，在任何情况和环境下都不应伤害人类及机器人自身，所以安全性是机器人在人类生产生活领域应用的首要问题和强制性约束，但机器人本身并没有安全意识，人与机器人协同工作时如果发生碰撞，处于弱势地位的操作人员安全隐患相对较大，所以必须针对协作机器人可能出现的各个风险点，进行充分的安全评估，制定相应的保护措施来确保操作人员的安全。

目前存在的人机协作中人体安全评估方法，只通过计算机器人与人体之间的距离来判断人体的安全状况，而忽略机器人的运动速度这一重要参数对人体安全的影响，造成了人机协作中人体安全评估不准确、特定风险无法排查等现象。

发明内容

针对现有上述技术问题，本发明提供一种使人机协作更安全更高效的人机协作中人体安全评估方法，以及实现该方法的系统。

本发明的人机协作中人体安全评估方法，包括以下步骤：

(1)获取人体在全局坐标系下的位姿；

(2)获取机器人在全局坐标系下的位姿；

(3)根据所获取的位姿信息，计算人体与机器人之间的距离以及机器人的运动速度；

(4)根据所计算的人体与机器人之间的距离以及机器人的运动速度，判断人机协作中人体的安全等级。

所述步骤(1)获取人体在全局坐标系下的位姿的具体过程是：

选取人体行走时静止一侧脚踝为人体的基坐标系原点，人行走时，基坐标系原点在左右脚之间切换，首先通过脚踝位置推算出盆骨的位置，再根据盆骨位置推算全身各躯干的位姿；上半身放置5块惯性测量单元，分别置于上身躯干、左右大臂和左右小臂，用于获取上半身的位姿信息；下半身放置6块惯性测量单元，分别置于左右大腿、左右小腿和左右脚背，用于获取下半身的位姿信息；

使用坐标系符号说明：

G：全局坐标系；

B：人体基坐标系；

S：惯性测量单元坐标系；

获取人体基坐标系与全局坐标系之间的姿态关系，所建立的人体基坐标系与全局坐标系姿态一致，人体基坐标系在全局坐标系中的姿态表示为

获取惯性测量单元坐标系与全局坐标系之间的姿态关系，惯性测量单元坐标系在全局坐标系中的姿态表示为

获取人体基坐标系与惯性测量单元坐标系的姿态

通过坐标系之间的变换：

得

在开始运动之前，受试者保持校准姿势，这个校准过程是在所有的惯性测量单元上进行的，其中每个惯性测量单元都与一个具有预定义的坐标系的身体部分相关联，将其用于实时运动捕捉中；

获取人体的姿态：人体肢体长度是确定已知的，对惯性测量单元进行标定之后放置在上身躯干上，以人体直立静止时为初始状态；人在行走时，总会保持一只脚触地，取该侧脚踝为人体基坐标系原点，人体静止时，令每个惯性测量单元坐标系与其参照坐标系之间的旋转矩阵为单位矩阵，人体运动时，通过惯性测量单元返回的四元数(ε₁ ε₂ ε₃ ε₄)，实时得到身体每一躯干的姿态以及位置信息；

左脚触地时，基坐标系(左脚踝坐标系)作为左小腿的参照坐标系，已知人体静止时，在人体基坐标系中左膝盖的坐标为

通过四元数表示的旋转矩阵R_ε1为:

则人体姿态发生变换后的左膝盖坐标为:

P′_L1＝R_ε1P_L1。

以左膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并以此坐标系作为左大腿惯性测量单元运动的参照坐标系。人体静止时，令左大腿惯性测量单元坐标系与左膝盖坐标系之间的旋转矩阵为单位矩阵，在左膝盖坐标系中，左髋关节的坐标为

通过左大腿惯性测量单元传回的四元数表示的旋转矩阵R_ε2，实时得到左大腿的姿态以及位置信息；

则人体姿态发生变换后的左髋关节坐标为：

P′_L2＝R_ε2P_L2,

同时得左髋关节相对于人体基坐标系的坐标：

P′_L2＝P′_L1+P′_L2，

以左髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，假设人在移动过程中，盆骨的姿态不发生变化，则盆骨在左髋关节坐标系中的位置固定，同时得盆骨在人体基坐标系中的位置；

以盆骨中心为坐标系的原点建立坐标系，坐标系姿态与人体基坐标系一致，右髋关节在盆骨坐标系中的位置固定不变，同时得到右髋关节在人体基坐标系中的位置；

以右髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右髋关节坐标系作为右大腿惯性测量单元的参照坐标系，人体静止时，令右大腿惯性测量单元坐标系与右髋关节坐标系之间的旋转矩阵为单位矩阵，在右髋关节坐标系中，通过右大腿惯性测量单元传回的四元数表示的旋转矩阵，实时得到右大腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的右膝盖位置坐标；

以右膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右膝盖坐标系作为右小腿惯性测量单元的参照坐标系，人体静止时，令右小腿惯性测量单元坐标系与右膝盖坐标系之间的旋转矩阵为单位矩阵，在右膝盖坐标系中，通过右小腿惯性测量单元传回的四元数表示的旋转矩阵，实时得到右小腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的右脚踝位置坐标；

右脚触地时，以右脚脚踝为原点，建立人体基坐标系，腿关节坐标系建立方法同上；

同时，以盆骨坐标系作为上身躯干的参照坐标系，人体静止时，令上身躯干惯性测量单元坐标系与盆骨坐标系之间的旋转矩阵为单位矩阵，在盆骨坐标系中，通过上身躯干惯性测量单元传回的四元数表示的旋转矩阵，实时得到上身躯干的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的肩膀位置坐标；

以肩膀为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将肩膀坐标系作为大臂运动的参照坐标系，通过大臂惯性测量单元传回的四元数表示的旋转矩阵，实时得到大臂的姿态以及位置信息，根据坐标变换，获得人体基坐标系下手肘的位置坐标；

以手肘为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将手肘坐标系作为小臂运动的参照坐标系，通过小臂惯性测量单元传回的四元数表示的旋转矩阵，实时得到小臂的姿态以及位置信息，根据坐标变换，获得人体基坐标系下手腕的位置坐标；

从而获得人体在人体基坐标系下的完整姿态以及各个关节的位置坐标；通过人体基坐标系和全局坐标系之间的变换矩阵，得到全局坐标系下的人体完整姿态以及各个关节的位置坐标。

所述步骤(2)获取机器人在全局坐标系下的位姿的具体过程是：

机器人上的惯性测量单元，在基座和每个连杆上分别放置一块惯性测量单元，用于获取机器人的姿态信息；机器人姿态的获取与人体上半身姿态的获取过程一致，以此获得全局坐标系下机器人的姿态。

所述步骤(3)中计算人体与机器人之间的距离以及机器人的运动速度的具体过程是：

将人体各部分躯干以及机械臂的各个连杆视为空间中的圆柱体，利用数学公式求解圆柱体间的最短距离；分别计算人体各部分躯干与机器人各个连杆之间的距离，搜索距离的最小值作为人体和机器人之间的距离；

根据已知的机器人末端连杆的位置，对时间进行微分获得机器人的运动速度。

所述步骤(4)中判断人机协作中人体的安全等级的具体过程是：

当所述人体与机器人之间的距离大于第一预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于安全状态；

当所述人体与机器人之间的距离大于第一预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于相对安全状态；

当所述人体与机器人之间的距离大于第二预设距离阈值同时小于第一预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于相对安全状态；

当所述人体与机器人之间的距离大于第二预设距离阈值同时小于第一预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于相对危险状态；

当所述人体与机器人之间的距离小于第二预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于相对危险状态；

当所述人体与机器人之间的距离小于第二预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于危险状态；

所述距离阈值和速度阈值，根据机器人的外展距离、所安装的工具的长度、机器人的工作性能以及先验经验来确定。具体是第一距离阈值可选取机器人可外展长度的最大值L_max，第二距离阈值可选取机器人可外展长度的最大值的三分之一(即0.33L_max)，速度阈值可选取L_max/1.25。

实现上述方法的人机协作中人体安全评估系统，包括：

第一获取模块，用于获取人体在全局坐标系下的位姿信息；

第二获取模块，用于获取机器人在全局坐标系下的位姿信息；

计算模块，与第一获取模块和第二获取模块连接，根据所获取人体和机器人的位姿信息，计算人体与机器人之间的距离以及机器人的运动速度；

评估模块，与计算模块连接，用于根据所计算的人体和机器人之间的距离以及机器人的运动速度，评价人机协作中人体的安全等级。

所述第一获取模块具体包括：

标定单元，用于对惯性测量单元进行标定，得到惯性测量单元坐标系与其对应人体坐标系之间的坐标变换矩阵；

数据接收单元，用于接收惯性测量单元返回的四元数和角速度；

位姿获取单元，用于根据所得各个惯性测量单元的角速度以及四元数，得到人体在人体基坐标系下的姿态及位置坐标；

坐标转换单元，用于将所述人体在人体基坐标系下的位置坐标转换为人体在全局坐标系下的位置坐标。

本发明通过设置的测试环境，机器人和操作人员均穿戴惯性测量单元，协作型机器人按照预定轨迹进行工作，操作人员与协作型机器人进行协作，在整个过程中，多惯性测量单元首先采集机器人与人体的运动数据，之后对数据进行滤波处理，利用正运动学函数，获取机器人和人体的位置信息及运动姿态，并将其赋值给三维模型，三维模型实时复现机器人与人体的运动状态，同时计算人体和机器人之间的距离及机器人运动速度等评价参数，评估协作型机器人与人员的协作程度及对人员的安全性。

本发明在人机协作过程中，通过监控机器人与人体之间的距离和机器人的运行速度确定当前人体的安全状态，根据不同安全状态做出下一步工作计划，有效避免危险情况发生，使人机协作更安全更高效。

附图说明

图1为测试系统场景示意图。

图2为人体惯性测量单元的放置示意图。

图3为人体左脚触地时全身姿态推算顺序图。

图4为人体右脚触地时全身姿态推算顺序图。

图5为机器人惯性测量传感单元的放置示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

本发明通过设置的测试环境，机器人和操作人员均穿戴惯性测量单元(Inertialmeasurement unit，简称IMU)套装，如图1所示，协作型机器人按照预定轨迹进行工作，操作人员与协作型机器人进行协作，在整个过程中，多惯性测量单元系统首先采集机器人与人体的运动数据，之后对数据进行滤波处理，利用正运动学函数，获取机器人和人体的位置信息及运动姿态，并将其赋值给三维模型，三维模型实时复现机器人与人体的运动状态，同时计算人体和机器人之间的距离及机器人运动速度等评价参数，评估协作型机器人与人员的协作程度及对人员的安全性。

获取人体和机器人在全局坐标系下的位姿信息。

人体上惯性测量单元的放置如图2所示，使用的惯性测量单元为XSENS MTwAwinda，采用链条式机器人模型，选取行走时静止一侧脚踝为人体的基坐标系原点，人行走时，基坐标系原点在左右脚之间切换，首先通过脚踝位置推算出盆骨的位置，再根据盆骨位置推算全身各躯干的位姿；上半身放置5块惯性测量单元，分别置于上身躯干、左右大臂和左右小臂，用于获取上半身的位姿信息；下半身放置6块惯性测量单元，分别置于左右大腿、左右小腿和左右脚背，用于获取下半身的位姿信息，放置在人体上的惯性测量单元位置固定不发生变化。

本实例中所使用坐标系符号说明：

G：全局坐标系；

B：人体基坐标系；

S：惯性测量单元坐标系。

具体的，获取人体基坐标系与全局坐标系之间的姿态关系，简单起见，所建立的人体基坐标系与全局坐标系姿态一致，人体基坐标系在全局坐标系中的姿态表示为

获取惯性测量单元坐标系与全局坐标系之间的姿态关系，惯性测量单元坐标系在全局坐标系中的姿态表示为

获取人体基坐标系与惯性测量单元坐标系的姿态

通过坐标系之间的变换：

可得

在开始运动之前，受试者保持校准姿势，这个校准过程是在所有的惯性测量单元上进行的，其中每个惯性测量单元都与一个具有预定义的坐标系的身体部分相关联，将其用于实时运动捕捉中。

获取人体的姿态。人体肢体长度是确定已知的，对惯性测量单元进行标定之后放置在上身躯干上，以人体直立静止时为初始状态。人在行走时，总会保持一只脚触地，取该侧脚踝为人体基坐标系原点，人体静止时，令每个惯性测量单元坐标系与其参照坐标系之间的旋转矩阵为单位矩阵，人体运动时，通过惯性测量单元返回的四元数(ε₁ ε₂ ε₃ ε₄)，可以实时得到身体每一躯干的姿态以及位置信息。左脚触地时全身姿态的推算顺序如图3所示，右脚触地时全身姿态的推算顺序如图4所示。

现以左脚触地为例，来说明完整人体姿态获取的过程：

基坐标系(左脚踝坐标系)作为左小腿的参照坐标系，已知人体静止时，在人体基坐标系中左膝盖的坐标为

通过四元数表示的旋转矩阵R_ε1为:

则人体姿态发生变换后的左膝盖坐标为:

P′_L1＝R_ε1P_L1。

以左膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并以此坐标系作为左大腿惯性测量单元运动的参照坐标系。人体静止时，令左大腿惯性测量单元坐标系与左膝盖坐标系之间的旋转矩阵为单位矩阵，在左膝盖坐标系中，左髋关节的坐标为

通过左大腿惯性测量单元传回的四元数表示的旋转矩阵R_ε2，可以实时得到左大腿的姿态以及位置信息。

则人体姿态发生变换后的左髋关节坐标为：

P′_L2＝R_ε2P_L2。

同时可得左髋关节相对于人体基坐标系的坐标：

P″_L2＝P′_L1+P′_L2

以左髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，假设人在移动过程中，盆骨的姿态不发生变化，则盆骨在左髋关节坐标系中的位置固定，同时可得盆骨在人体基坐标系中的位置。

以盆骨中心为坐标系的原点建立坐标系，坐标系姿态与人体基坐标系一致，右髋关节在盆骨坐标系中的位置固定不变，同时可得右髋关节在人体基坐标系中的位置。

以右髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右髋关节坐标系作为右大腿惯性测量单元的参照坐标系，人体静止时，令右大腿惯性测量单元坐标系与右髋关节坐标系之间的旋转矩阵为单位矩阵，在右髋关节坐标系中，通过右大腿惯性测量单元传回的四元数表示的旋转矩阵，可以实时得到右大腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的右膝盖位置坐标。

以右膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右膝盖坐标系作为右小腿惯性测量单元的参照坐标系，人体静止时，令右小腿惯性测量单元坐标系与右膝盖坐标系之间的旋转矩阵为单位矩阵，在右膝盖坐标系中，通过右小腿惯性测量单元传回的四元数表示的旋转矩阵，可以实时得到右小腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的右脚踝位置坐标。

右脚触地时，以右脚脚踝为原点，建立人体基坐标系，腿关节坐标系建立方法同上。

同时，以盆骨坐标系作为上身躯干的参照坐标系，人体静止时，令上身躯干惯性测量单元坐标系与盆骨坐标系之间的旋转矩阵为单位矩阵，在盆骨坐标系中，通过上身躯干惯性测量单元传回的四元数表示的旋转矩阵，可以实时得到上身躯干的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的肩膀位置坐标。

以肩膀为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将肩膀坐标系作为大臂运动的参照坐标系，通过大臂惯性测量单元传回的四元数表示的旋转矩阵，可以实时得到大臂的姿态以及位置信息，根据坐标变换，获得人体基坐标系下手肘的位置坐标。

以手肘为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将手肘坐标系作为小臂运动的参照坐标系，通过小臂惯性测量单元传回的四元数表示的旋转矩阵，可以实时得到小臂的姿态以及位置信息，根据坐标变换，获得人体基坐标系下手腕的位置坐标。

从而获得人体在人体基坐标系下的完整姿态以及各个关节的位置坐标。通过人体基坐标系和全局坐标系之间的变换矩阵，可以得到全局坐标系下的人体完整姿态以及各个关节的位置坐标。

机器人上的惯性测量单元放置如图5所示，本发明实例选取6自由度机械臂，在基座和每个连杆上分别放置一块惯性测量单元，用于获取机械臂的姿态信息。

上述提到的获得人体上半身姿态的算法同样适用于机械臂姿态的获取，不再赘述。

以此，获得全局坐标系下机器人的姿态。

根据所获取的位姿信息，计算人体与机器人之间的距离以及机器人的运动速度。

将人体各部分躯干以及机械臂的各个连杆视为空间中的圆柱体，利用数学公式求解圆柱体间的最短距离。

首先求出两个圆柱体中轴线之间的距离，问题转换为求解空间中两条线段之间的距离。以任意两条线段为例，人体上的一条线段AB，A(x_a，y_a，z_a)，B(x_b，y_b，z_b)，机械臂上的一条线段CD，C(x_c，y_c，z_c)，D(x_d，y_d，z_d)。

令H＝x_b-x_a，I＝y_b-y_a，J＝z_b-z_a，K＝x_d-x_c，L＝y_d-y_c，M＝z_d-z_c,

可以得到直线AB与直线CD之间的公垂线方程:

N·x-O·y+P·z+Q＝0，

令

可得公垂线与直线CD的交点E(x_E，y_E，z_E),其中x_E＝K·k′+x_c，y_E＝L·k′+y_c，z_E＝M·k′+z_c。

同理可得公垂线与直线AB的交点F(x_F，y_F，z_F)。

若点E存在于线段CD上，点F存在于线段AB上，则线段AB与线段CD间的最短距离为：

若点E不存在于线段CD上或点F不存在于线段AB上时，则使min{L_AC，L_AD，L_BC，L_BD}为AB与线段CD间的最短距离。

则两个圆柱体之间的距离为两个圆柱中轴线之间的距离与两个圆柱体的半径之差。

分别计算人体各部分躯干与机器人各个连杆之间的距离，搜索距离的最小值作为人体和机器人之间的距离。

根据已知的机器人末端连杆的位置，对时间进行微分获得机器人的运动速度。

根据所述人体与机器人之间的距离以及机器人速度，判断人体的安全程度。

人机协同工作模式下，在该实例中，具体步骤包括：

当所述人体与机器人之间的距离大于第一预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于安全状态；

当所述人体与机器人之间的距离大于第一预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于相对安全状态；

当所述人体与机器人之间的距离大于第二预设距离阈值同时小于第一预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于相对安全状态；

当所述人体与机器人之间的距离大于第二预设距离阈值同时小于第一预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于相对危险状态；

当所述人体与机器人之间的距离小于第二预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于相对危险状态；

当所述人体与机器人之间的距离小于第二预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于危险状态；

具体的，根据机器人的外展距离、所安装的工具的长度、机器人的工作性能以及先验经验来确定距离阈值和速度阈值。例如，第一距离阈值可选取机器人可外展长度的最大值L_max，第二距离阈值可选取机器人可外展长度的最大值的三分之一，即0.33L_max，而正常人的反应能力参数为1.25秒，即发现前方有目标反映到大脑需0.5秒，从大脑反应到手、脚后并采取措施需0.75秒，所以速度阈值可选取L_max/1.25。

通过上述方法，可以在人机协作过程中，通过监控机器人与人体之间的距离和机器人的运行速度确定当前人体的安全状态，根据不同安全状态做出下一步工作计划，有效避免危险情况发生，使人机协作更安全更高效。

相应地，本发明还提供一种人机协作人体安全评估系统，包括：

第一获取模块，用于获取人体在全局坐标系下的位姿信息；

第二获取模块，用于获取机器人在全局坐标系下的位姿信息；

计算模块，用于根据所述人体和机器人的位姿信息，计算人体和机器人之间的距离以及机器人的运动速度；

评估模块，用于根据所述人体与机器人之间的距离以及机器人的运动速度，判断评估人体的安全状态。

可选地，在该实例中，所述第一获取模块具体包括：

标定单元，用于对惯性测量单元进行标定，得到惯性测量单元坐标系与其对应人体坐标系之间的坐标变换矩阵；

数据接收单元，用于接收惯性测量单元返回的四元数和角速度；

位姿获取单元，用于根据所得各个惯性测量单元的角速度以及四元数，得到人体在人体基坐标系下的姿态及位置坐标；

坐标转换单元，用于将所述人体在人体基坐标系下的位置坐标转换为人体在全局坐标系下的位置坐标。

可选地，在该实例中，所述评估模块具体为：

当所述人体与机器人之间的距离大于第一预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于安全状态；

当所述人体与机器人之间的距离大于第一预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于相对安全状态；

当所述人体与机器人之间的距离大于第二预设距离阈值同时小于第一预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于相对安全状态；

当所述人体与机器人之间的距离大于第二预设距离阈值同时小于第一预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于相对危险状态；

当所述人体与机器人之间的距离小于第二预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于相对危险状态；

当所述人体与机器人之间的距离小于第二预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于危险状态；

多人同时进行人机协作时，则计算多个人体与机器人之间的距离和机器人的运行速度，评估判断人体的安全状态。

Claims (8)

1.一种人机协作中人体安全评估方法，其特征是，包括以下步骤：

(1)获取人体在全局坐标系下的位姿；

(2)获取机器人在全局坐标系下的位姿；

(3)根据所获取的位姿信息，计算人体与机器人之间的距离以及机器人的运动速度；

(4)根据所计算的人体与机器人之间的距离以及机器人的运动速度，判断人机协作中人体的安全等级。

2.根据权利要求1所述的人机协作中人体安全评估方法，其特征是：所述步骤(1)获取人体在全局坐标系下的位姿的具体过程是：

选取人体行走时静止一侧脚踝为人体的基坐标系原点，人行走时，基坐标系原点在左右脚之间切换，首先通过脚踝位置推算出盆骨的位置，再根据盆骨位置推算全身各躯干的位姿；上半身放置5块惯性测量单元，分别置于上身躯干、左右大臂和左右小臂，用于获取上半身的位姿信息；下半身放置6块惯性测量单元，分别置于左右大腿、左右小腿和左右脚背，用于获取下半身的位姿信息；

使用坐标系符号说明：

G：全局坐标系；

B：人体基坐标系；

S：惯性测量单元坐标系；

获取人体基坐标系与全局坐标系之间的姿态关系，所建立的人体基坐标系与全局坐标系姿态一致，人体基坐标系在全局坐标系中的姿态表示为

获取惯性测量单元坐标系与全局坐标系之间的姿态关系，惯性测量单元坐标系在全局坐标系中的姿态表示为

获取人体基坐标系与惯性测量单元坐标系的姿态

通过坐标系之间的变换：

得

在开始运动之前，受试者保持校准姿势，这个校准过程是在所有的惯性测量单元上进行的，其中每个惯性测量单元都与一个具有预定义的坐标系的身体部分相关联，将其用于实时运动捕捉中；

获取人体的姿态：人体肢体长度是确定已知的，对惯性测量单元进行标定之后放置在上身躯干上，以人体直立静止时为初始状态；人在行走时，总会保持一只脚触地，取该侧脚踝为人体基坐标系原点，人体静止时，令每个惯性测量单元坐标系与其参照坐标系之间的旋转矩阵为单位矩阵，人体运动时，通过惯性测量单元返回的四元数(ε₁ ε₂ ε₃ ε₄)，实时得到身体每一躯干的姿态以及位置信息；

左脚触地时，基坐标系(左脚踝坐标系)作为左小腿的参照坐标系，已知人体静止时，在人体基坐标系中左膝盖的坐标为

通过四元数表示的旋转矩阵R_ε1为:

则人体姿态发生变换后的左膝盖坐标为:

P′_L1＝R_ε1P_L1；

以左膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并以此坐标系作为左大腿惯性测量单元运动的参照坐标系。人体静止时，令左大腿惯性测量单元坐标系与左膝盖坐标系之间的旋转矩阵为单位矩阵，在左膝盖坐标系中，左髋关节的坐标为

通过左大腿惯性测量单元传回的四元数表示的旋转矩阵R_ε2，实时得到左大腿的姿态以及位置信息；

则人体姿态发生变换后的左髋关节坐标为：

P′_L2＝R_ε2P_L2，

同时得左髋关节相对于人体基坐标系的坐标：

P″_L2＝P′_L1+P′_L2，

以左髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，假设人在移动过程中，盆骨的姿态不发生变化，则盆骨在左髋关节坐标系中的位置固定，同时得盆骨在人体基坐标系中的位置；

以盆骨中心作为坐标系的原点建立坐标系，坐标系姿态与人体基坐标系一致，右髋关节在盆骨坐标系中的位置固定不变，同时得到右髋关节在人体基坐标系中的位置；

以右髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右髋关节坐标系作为右大腿惯性测量单元的参照坐标系，人体静止时，令右大腿惯性测量单元坐标系与右髋关节坐标系之间的旋转矩阵为单位矩阵，在右髋关节坐标系中，通过右大腿惯性测量单元传回的四元数表示的旋转矩阵，实时得到右大腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的右膝盖位置坐标；

以右膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右膝盖坐标系作为右小腿惯性测量单元的参照坐标系，人体静止时，令右小腿惯性测量单元坐标系与右膝盖坐标系之间的旋转矩阵为单位矩阵，在右膝盖坐标系中，通过右小腿惯性测量单元传回的四元数表示的旋转矩阵，实时得到右小腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的右脚踝位置坐标；

右脚触地时，以右脚脚踝为原点，建立人体基坐标系，腿关节坐标系建立方法同上；

同时，以盆骨坐标系作为上身躯干的参照坐标系，人体静止时，令上身躯干惯性测量单元坐标系与盆骨坐标系之间的旋转矩阵为单位矩阵，在盆骨坐标系中，通过上身躯干惯性测量单元传回的四元数表示的旋转矩阵，实时得到上身躯干的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的肩膀位置坐标；

以肩膀为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将肩膀坐标系作为大臂运动的参照坐标系，通过大臂惯性测量单元传回的四元数表示的旋转矩阵，实时得到大臂的姿态以及位置信息，根据坐标变换，获得人体基坐标系下手肘的位置坐标；

以手肘为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将手肘坐标系作为小臂运动的参照坐标系，通过小臂惯性测量单元传回的四元数表示的旋转矩阵，实时得到小臂的姿态以及位置信息，根据坐标变换，获得人体基坐标系下手腕的位置坐标；

从而获得人体在人体基坐标系下的完整姿态以及各个关节的位置坐标；通过人体基坐标系和全局坐标系之间的变换矩阵，得到全局坐标系下的人体完整姿态以及各个关节的位置坐标。

3.根据权利要求1所述的人机协作中人体安全评估方法，其特征是：所述步骤(2)获取机器人在全局坐标系下的位姿的具体过程是：

机器人上的惯性测量单元，在基座和每个连杆上分别放置一块惯性测量单元，用于获取机械臂的姿态信息；机械臂姿态的获取与人体上半身姿态的获取过程一致，以此获得全局坐标系下机械臂的姿态。

4.根据权利要求1所述的人机协作中人体安全评估方法，其特征是：所述步骤(3)中计算人体与机器人之间的距离以及机器人的运动速度的具体过程是：

将人体各部分躯干以及机械臂的各个连杆视为空间中的圆柱体，利用数学公式求解圆柱体间的最短距离；分别计算人体各部分躯干与机器人各个连杆之间的距离，搜索距离的最小值作为人体和机器人之间的距离；

根据已知的机器人末端连杆的位置，对时间进行微分获得机器人的运动速度。

5.根据权利要求1所述的人机协作中人体安全评估方法，其特征是：所述步骤(4)中判断人机协作中人体的安全等级的具体过程是：

当所述人体与机器人之间的距离大于第一预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于安全状态；

当所述人体与机器人之间的距离大于第一预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于相对安全状态；

当所述人体与机器人之间的距离大于第二预设距离阈值同时小于第一预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于相对安全状态；

当所述人体与机器人之间的距离大于第二预设距离阈值同时小于第一预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于相对危险状态；

当所述人体与机器人之间的距离小于第二预设距离阈值并且机器人速度小于预设速度阈值时，视人体处于相对危险状态；

当所述人体与机器人之间的距离小于第二预设距离阈值并且机器人速度大于预设速度阈值时，视人体处于危险状态。

6.根据权利要求5所述的人机协作中人体安全评估方法，其特征是：所述距离阈值和速度阈值，根据机器人的外展距离、所安装的工具的长度、机器人的工作性能以及先验经验来确定，具体是第一距离阈值选取机器人外展长度的最大值L_max，第二距离阈值选取机器人外展长度的最大值的三分之一，速度阈值选取L_max/1.25。

7.一种人机协作中人体安全评估系统，其特征是：包括：

第一获取模块，用于获取人体在全局坐标系下的位姿信息；

第二获取模块，用于获取机器人在全局坐标系下的位姿信息；

计算模块，与第一获取模块和第二获取模块连接，根据所获取人体和机器人的位姿信息，计算人体与机器人之间的距离以及机器人的运动速度；

评估模块，与计算模块连接，用于根据所计算的人体和机器人之间的距离以及机器人的运动速度，评价人机协作中人体的安全等级。

8.根据权利要求7所述的人机协作中人体安全评估系统，其特征是：所述第一获取模块具体包括：

标定单元，用于对惯性测量单元进行标定，得到惯性测量单元坐标系与其对应人体坐标系之间的坐标变换矩阵；

数据接收单元，用于接收惯性测量单元返回的四元数和角速度；

位姿获取单元，用于根据所得各个惯性测量单元的角速度以及四元数，得到人体在人体基坐标系下的姿态及位置坐标；

坐标转换单元，用于将所述人体在人体基坐标系下的位置坐标转换为人体在全局坐标系下的位置坐标。

Images