CN107564065B - 一种协作环境下人机最小距离的测算方法 - Google Patents

Abstract

本发明公开了一种协作环境下人机最小距离的测算方法，根据机器人各部件之间的相对位置关系与部件旋转轴线，建立机器人的广义运动模型，构建人机最小距离的机器人运动节点迭代数据集。同时通过3D视觉传感器采集人机协作空间的图像数据以跟踪并识别协作环境中的人体信息，提取人体骨骼节点数据，构建人机最小距离的人体骨骼节点迭代数据集。根据得到的迭代数据集迭代计算协作环境中人与机器人间的最小距离及对应点空间位置坐标。本发明的协作环境下人机最小距离的迭代计算方法可以实时构建精确的并易于实现的人机距离计算模型，实时的计算协作环境中的人机最小距离与对应点，用于提高机器人的安全性，可以实时高效的跟踪环境中的人机最小距离。

Description

一种协作环境下人机最小距离的测算方法

技术领域

本发明涉及机器人技术领域，特别是一种协作环境下人机最小距离的测算方法。

背景技术

为了弥补传统机器人灵活性与智能性的不足，使机器人适应更广泛、更复杂的任务需求，协作机器人技术应运而生。人机协作与共融机制将机器人的“计算能力、力量、精度、可重复性”与人的“灵活性、经验知识、分析、决策能力”相结合，实现优势互补。

与传统工业机器人相比，人与机器人之间的协作与共融解除了人机之间的隔离保护、合并了人机优势、增加了灵活性与安全性，进而提升了生产效能。而为了使人与机器人实现协作与共融，精确并实时的计算人与机器人之间的相对距离则成为了亟待解决的问题。构建实时精确并易于实现的人机距离计算模型，不仅可以提高机器人的安全性，也可以通过对人体的识别与特征提取，识别人体构型预判人体行为，为设计机器人的智能式反应策略提供参考。针对这一问题目前有两种主流的解决方案是：1)基于图形的人机距离计算方法：利用预先建立的凸体模型，进行凸体之间距离的计算，但此方法均基于模型已知的条件下。而在协作环境中，人体的位置与姿态处理动态变化中，相对应的构成人体的凸体图形也处于变化之中，在这种场景下若动态的构建人体的凸体模型计算量较大，不利于实时性的实现；2)基于平面图像的人机距离计算方法：通过安装在协作空间顶部的平面视觉相机，获取协作空间内的场景图像，经过图像分割与形态学处理获得机器人与人的最小距离，但这种方法相当于把人与机器人看作是分别独立的连通区，只能计算出两个整体之间的距离，每个整体的内部姿态却是未知的，同时形态处理之后的图像映射到人机空间距离时，精度也会大为降低。

发明内容

本发明所要解决的技术问题是克服现有技术的不足而提供一种协作环境下人机最小距离的测算方法，本方法简单易于实现，采用该方法测量得到的人机最小距离结果准确度更高、实时性高。

本发明为解决上述技术问题采用以下技术方案：

根据本发明提出的一种协作环境下人机最小距离的测算方法，包括以下步骤：

步骤S1、使用位置测量与标定***确定机器人与3D视觉传感器的相对位姿关系，得到机器人基坐标系和3D视觉传感器坐标系之间的转换关系；

S2、根据机器人各部件之间的相对位置关系与部件旋转关系，建立部件的基准坐标系，基准坐标系的原点即为机器人的关键运动节点，并根据机器人的关键运动节点之间的相对位姿关系，建立机器人广义坐标模型，测算机器人各部件的计算余量，设定机器人的迭代次序向量，实时读取机器人运动过程中的关节角数据，根据运动过程中的关节角数据，利用机器人广义坐标模型与设定的机器人的迭代次序向量构建人机最小距离的机器人运动节点迭代数据集；

S3、根据3D视觉传感器跟踪并识别协作环境中的人体信息，提取人体信息中的人体骨骼节点数据，计算人体的安全余量与人体骨骼的迭代次序向量，此后，根据人体骨骼节点数据与人体骨骼的迭代次序向量构建人机最小距离的人体骨骼节点迭代数据集；

S4、根据得到的机器人运动节点迭代数据集、人体骨骼节点迭代数据集、机器人各部件的计算余量和人体的安全余量，迭代计算协作环境中人与机器人间的最小距离及与最小距离相对应的空间点的坐标。

作为本发明所述的一种协作环境下人机最小距离的测算方法进一步优化方案，所述步骤S1中，通过标定***标定机器人坐标系与3D视觉传感器所在的传感器坐标系的相对位置与姿态，并确定二者之间的转换矩阵。

作为本发明所述的一种协作环境下人机最小距离的测算方法进一步优化方案，所述步骤S2中，

机器人各部件间的相对位置关系是指按照机器人基座到末端法兰的先后连接次序，后连接的部件相对于前一个部件在机器人广义坐标模型中部件的基准坐标系沿前一个部件基准坐标系三坐标轴方向的相对平移关系，用^i-1A_i＝(x y z)表示，^i-1A_i为机器人的第i个部件相对于第i-1个部件的相对平移关系，x、y、z分别为x,y,z轴上的平移量；

部件的旋转关系是指部件在部件的基准坐标系中的旋转轴线，用R_i＝(r_x r_y r_z)表示，R_i表示第i个部件的旋转轴线，r_x、r_y、r_z分别表示部件的旋转轴线的方向向量在部件的基准坐标系中沿三坐标轴的分量；

机器人各部件的计算余量是按以下准则确定：以机器人广义坐标模型中的相邻部件的基准坐标系的原点为端点，两端点的连线为圆柱体的中心轴线，构建圆柱体，则机器人各部件的计算余量为能够将两圆柱体上下底面之间的机器人部件完全包络的圆柱体半径的下确界；

所述实时读取机器人运动过程中的关节角数据，根据运动过程中的关节角数据，利用机器人广义坐标模型与设定的机器人的迭代次序向量构建人机最小距离的机器人运动节点迭代数据集；具体如下：

实时读取机器人运动过程中的关节角数据，并由机器人广义坐标模型得到当前时刻的机器人的关键运动节点的位置，按照机器人的从基座到末端法兰的机械连接顺序建立机器人的链式结构关键运动节点数据，建立机器人的迭代次序向量，构造人机最小距离的机器人运动节点迭代数据集L。

作为本发明所述的一种协作环境下人机最小距离的测算方法进一步优化方案，所述步骤S3具体如下：

S3.1：根据3D视觉传感器获取的人机协作场景的图像数据和深度信息，通过图像分析与深度数据处理得到人体的识别结果并跟踪协作环境中的人体信息，并将人体从环境中分割出来，结合深度信息计算人体的骨骼节点数据，通过图像与空间的映射关系，提取人体的骨骼节点的空间位置信息；

S3.2：根据3D视觉生成的人体识别结果与人体的骨骼节点数据，确定识别的人体骨骼节点信息在实际物理空间中对应人体部位的相应位置，并据此计算人体的安全余量；

S3.3：根据S3.1中提取的人体骨骼节点数据，以相邻人体骨骼关节所对应的节点在骨骼节点数据中的索引为次序标记，计算人体骨骼的迭代次序向量；人体骨骼的迭代次序向量与人体的骨骼节点数据构成人体骨骼节点迭代数据集H。

作为本发明所述的一种协作环境下人机最小距离的测算方法进一步优化方案，所述S4中迭代计算包括以下要素：

S4.1：输入机器人运动节点迭代数据集L和人体骨骼节点迭代数据集H，初始化筛选矩阵E；判断数据集L和H是否为空集，若为空集，则结束以下计算过程；否则执行S4.2；

S4.2：按照机器人的迭代次序向量提取机器人运动节点迭代数据集L中的元素L_r′，按照人体骨骼的迭代次序向量提取人体骨骼节点迭代数据集H中的元素L_h′，则L_r′与L_h′分别对应空间中的一条线段，由此得到两条空间线段，L_r′对应的空间线段用L_r表示，L_h′对应的空间线段用L_h表示；

S4.3：迭代常量与方向向量初始化；用L_h的两个端点初始化s₀,s₁，则s₀,s₁为与L_h对应的空间中的两个点；用L_r的两个端点初始化t₀,t₁，则t₀,t₁为与L_r对应的空间中的两个点；令向量u和v分别为L_h与L_r的方向向量，w₀为两条空间线段端点之间的方向向量，则其表达式如式

(1)(2)(3)所示：

u＝s₁-s₀ (1)

v＝t₁-t₀ (2)

w₀＝s₀-t₀ (3)

S4.4：迭代变量初始化：令S_temp＝s_j＝s₀,T_temp＝t_j＝t₀，其中，S_temp,T_temp为迭代过程中的临时变量，s_j,t_j为迭代过程中的计算所得变量；

S4.5：计算向量系数c_t：

若c_t∈(-∞，0)则t_j＝t₀；若c_t∈(1,∞)则t_j＝t₁；若c_t∈[0，1]则执行式(5)；

计算向量系数c_s：

若c_s∈(-∞，0)则s_j＝s₀；若c_s∈(1,∞)则s_j＝s₁；若c_s∈[0，1]则执行式(7)；

s_j＝s₀+c_su (7)

S4.6：计算式(8)，式(8)为迭代停止条件；

(||S_temp-s_j||≤δ)∩(||T_temp-t_j||≤δ) (8)

其中，δ为设定的迭代阈值，若式(8)的计算结果为真，则采用式(9)计算L_r与L_h之间的最小距离d(L_r,L_h)，并将式(9)所得d(L_r,L_h)与s_j,t_j组合成列向量存入筛选矩阵E，之后判断数据集L和H是否为空集，若不为空集则执行S4.2；若为空集，则表示本次数据集中的所有元素均迭代完毕，执行S4.7；

d(L_r,L_h)＝||s_j-t_j|| (9)

若式(8)计算结果为假，则更新迭代变量如式(10)(11)所示：

S_temp＝s_j (10)

T_temp＝t_j (11)

之后执行S4.5；

S4.7：对筛选矩阵E进行式(12)所示计算，则人机最小距离d_min(L,H)如下式所示：

d_min(L,H)＝min{d|d∈C,C＝[1 0 0]E} (12)

其中，d为矩阵C中的元素，由式(12)所得人机最小距离d_min(L,H)在筛选矩阵E中查找到d_min(L,H)对应的索引，并由索引读取人机最小距离d_min(L,H)所对应的两个空间点。

作为本发明所述的一种协作环境下人机最小距离的测算方法进一步优化方案，迭代停止条件为：当两条空间线段迭代中前后两次所返回的迭代空间点相同或之差小于一个给定无穷小值时，停止迭代过程。

作为本发明所述的一种协作环境下人机最小距离的测算方法进一步优化方案，机器人的关键运动节点为预先选取的机器人的运动节点。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

(1)实时高效的跟踪环境中的人机最小距离，解决实际人机协作中的问题：剖析现在实际人机协作中存在的人机距离与相对位置关系问题，分析其本质特点，提出对应的迭代计算方法，避免了传统理论分析的复杂性与对距离问题分析的穷举特性，简化了计算过程，更有利于编程实现；

(2)本发明方法简便易行，具有通用性：对机器人的建模方面，相比于传统的机器人建模方法，大大的简化了建模的过程，降低了建模的难度；对人体的建模方面，避免了传统方法计算量大的特点；对于结构与机械臂类似的机构都可用此种方法进行建模分析；

(3)用这种方法所测量的人机最小距离结果准确度更高，计算量小，实时性高。相比于传统的直接构建凸体模型或是直接通过图像处理计算人机最小距离的方法，此方法改善了计算性能，大大减小了计算量，并能满足人机协作中的实时性要求；并且由于构建的目标的关键运动节点模型与运动链模型的固有特性，可以准确分析目标间各部分间的相互关系。

附图说明

图1a为本发明具体实施方式针对的人机协作场景之一。

图1b为本发明具体实施方式针对的人机协作场景特征提取示意图。

图2为本发明具体实施方式的整体结构要素示意图。

图3a～图3f为本发明具体实施方式中机器人部件基准坐标系建立方法示意图。

图4a为本发明具体实施方式的机器人示意图。

图4b为本发明具体实施方式的机器人关键运动节点示意图。

图5为本发明具体实施方式的人体关键运动节点示意图。

图6为本发明具体实施方式的人机最小距离迭代算法原理图。

图7为本发明具体实施方式的人机最小距离迭代算法流程图。

图8a～图8f为本发明具体实施方式的基于人机距离模型的迭代算法结果示意图；图8a～图8f对应的时间分别为：t＝0ms，t＝2344ms，t＝4396ms，t＝6072ms，t＝8204ms，t＝10000ms。

图中的附图标记解释为：图4b中A～G为机器人的关键运动节点，H为附加节点。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

本具体实施方式公开了一种协作环境下人机最小距离的迭代计算方法，包括以下步骤：

S1：使用位置测量与标定***确定机器人与3D视觉传感器的相对位姿关系，得到机器人基坐标系和3D视觉传感器坐标系之间的转换关系；

S2：根据机器人各部件之间的相对位置关系与部件旋转关系，建立部件的基准坐标系，基准坐标系的原点即为机器人的关键运动节点，并根据机器人的关键运动节点之间的相对位姿关系，建立机器人广义坐标模型，测算机器人各部件的计算余量，设定机器人的迭代次序向量，实时读取机器人运动过程中的关节角数据，根据运动过程中的关节角数据，利用机器人广义坐标模型与设定的机器人的迭代次序向量构建人机最小距离的机器人运动节点迭代数据集；

S3：根据3D视觉传感器跟踪并识别协作环境中的人体信息，提取人体信息中的人体骨骼节点数据，计算人体的安全余量与人体骨骼的迭代次序向量，此后，根据人体骨骼节点数据与人体骨骼的迭代次序向量构建人机最小距离的人体骨骼节点迭代数据集；

S4：根据得到的机器人运动节点迭代数据集、人体骨骼节点迭代数据集、机器人各部件的计算余量和人体的安全余量，迭代计算协作环境中人与机器人间的最小距离及与最小距离相对应的空间点的坐标。

图1a为人机协作场景之一，图1b为人机协作场景的特征提取示意图。对于人与机器人实现协作与共融的工作场景，机器人的数量或是机器人所拥有的机械臂的数量没有限制，但为了实现人机协作的目的，构建实时精确并易于实现的人机距离计算模型具有必要性，不仅可以提高机器人的安全性，也可以通过对人体的识别与特征提取，识别人体构型预判人体行为，为设计机器人的智能式反应策略提供参考。

图2为本方法的整体结构要素示意图。在进行协作任务之前，首先要解决的问题就是确定3D传感器与机器人本体之间的位姿转换关系。由于传感器安装的位置具有任意性，所以在环境中需要选择一个基准坐标系，也称之为世界坐标系，来描述传感器的位置，同时也用不超过描述环境中的信息。传感器坐标系与世界坐标系之间的转换关系可以用齐次变换矩阵来表示。空间中任意一点P在世界坐标系与传感器坐标系中的齐次坐标分别为P_w＝(x_w y_w z_w 1)^T与P＝(x y z 1)^T则，其坐标之间存在如下关系：

其中R为3×3正交单位矩阵，T为三维平移向量，0＝(0 0 0)^T，M_s为4×4矩阵。综合以上考虑并结合传感器的内部参数，可最终确定二者之间的变换矩阵。

机器人广义坐标模型的建立过程中，需要将机器人的各部件按照统一的方式建立基准坐标系，按照从机器人的基座到末端法兰的次序，在机器人初如状态下，机器人基座的部件基准坐标系原点位于机器人第一关节旋转轴线与基座下底面的交点处，机器人第二部件的基准坐标系原点位于机器人基座与第二部件的配合平面与第一关节的旋转轴线交点处，机器人的第三部件基准坐标系原点位于机器人第二关节的旋转轴线与第三部件的对称平面的交点处，机器人第四部件的基准坐标系原点位于机器人第三关节的旋转轴线与第四部件的对称平面的交点处，机器人第五部件的基准坐标系原点位于机器人第四关节的旋转轴线与第四第五部件配合平面的交点处，机器人第六部件的基准坐标系的原点位于机器人第五关节与第五部件的对称平面交点处，机器人第七部件的基准坐标系原点位于末端法兰中心轴线与外法兰面的交点处，所有部件基准坐标系的X轴方向皆指向机器人的正前方向，Z轴方向皆垂直向上，Y轴方向由右手笛卡尔坐标系确定，如图3a～图3f所示，之后测算各部件的空间平移位置关系。

确定部件间的空间相对平移位置关系是指后一个部件相对于前一个部件在笛卡尔空间中的平移关系，用^i-1A_i＝(x y z)表示，^i-1A_i为机器人的第i个部件相对于第i-1个部件的相对平移关系，x、y、z分别为x,y,z轴上的平移量。则部件间的平移关系Transl(A_i)定义如式(2)所示：

同时确定部件的旋转关系，具体是指在笛卡尔坐标系中确定部件的旋转轴线，用R_i＝(r_x r_y r_z)表示，R_i表示第i个部件的旋转轴线，r_x、r_y、r_z分别表示部件的旋转轴线的方向向量在部件的基准坐标系中沿三坐标轴的分量，例如当r_x取值为-1，其它两元素为0时，表示机器人部件沿其笛卡尔坐标系的x轴负方向旋转，当r_x取值为1，其它两元素为0时，表示机器人部件沿其笛卡尔坐标系的x轴正方向旋转。以R_i＝(0 0 1)且旋转角度为θ为例，其旋转关系如式(3)所示：

由于机器人每个部件的基准坐标系建立方法相同，坐标系的姿态也相同，每个部件的基准坐标系之间只存在空间平移关系。所以可以简化建模过程与模型表达形式，则根据以上定义与方法建立机器人的广义坐标模型如式(4)～(11)所示。

T_A＝Transl(A₁)*Trot(R₁(θ₀)) (4)

T_B＝T_A*Transl(A₂)*Trot(R₂(θ₁)) (5)

T_C＝T_B*Transl(A₃)*Trot(R₃(θ₂)) (6)

T_D＝T_C*Transl(A₄)*Trot(R₄(θ₃)) (7)

T_E＝T_D*Transl(A₅)*Trot(R₅(θ₄)) (8)

T_F＝T_E*Transl(A₆)*Trot(R₆(θ₅)) (9)

T_G＝T_F*Transl(A₇) (10)

T_H＝T_D*Transl(S_Z·A₅) (11)

其中S_Z＝(0 0 1)为坐标选择算子，如式(12)所示：

S_Z·A₅＝(0 0 1)·(x₅ y₅ z₅)＝(0 0 z₅) (12)

则可以得到机器人的关键运动节点与机器人的关节角之间的映射关系，图4a为本发明具体实施方式的机器人示意图，机器人的关键运动节点示意图如图4b所示，A～G为机器人的关键运动节点，H为附加的节点，用来拟合机器人的部件形状。

机器人各部件计算余量的测算按以下准则进行。以机器人广义坐标模型中的相邻关键运动节点为圆心，连线为法线，构建圆柱体，则部件计算余量为能够将两圆心之间的机器人部件完全包络的圆柱体半径的下确界。

对于机器人运动节点迭代数据集。在协作环境中要实时的读取机器人的运动关节角数据，并由广义坐标模型得到当前时刻的机器人各关键运动节点位置值，按照机器人的机械连接顺序建立机器人的链式结构特征点数据，建立迭代次序向量，构造人机最小距离的机器人运动节点迭代数据集L。

同时，根据3D视觉传感器的反馈数据，通过分析彩色图像识别并跟踪协作环境中的人体信息，并将人体从环境中分割出来，计算其骨骼节点数据，通过3D点云数据与彩色图像的映射关系，提取人体骨骼节点的空间位置信息。根据3D视觉生成的点云数据与人体识别结果，结合点云数据的信息，确定识别的人体骨骼信息在实际物理空间中对应人体部位的相应位置，并据此设定计算中的人体安全余量。

通过以上过程可以获得人与机器人的运动关键节点数据，但数据的存储是散乱的，需要对数据进行处理与标记生成迭代索引信息，并与原始数据相结合以构建人机最小距离的人体骨骼节点迭代数据集。

对于人体骨骼节点数据，提取的骨骼节点示意图如图5所示，按照人体骨骼的分布方式与邻接关系，以相邻人体骨骼关节所对应的节点在骨骼节点数据中的索引为次序标记，以识别的人体骨骼特征中单一骨骼为单位，构造骨骼数据集，并按照以头部节点为根节点的树形结构对所有的节点进行重新排布，则树形结构的数据代表每一对应的骨骼特征，按照树形结构的遍历次序，存储每一树枝的父节点与子节点对应的骨骼节点索引，形成迭代次序向量。迭代次序向量与人体骨骼节点数据构成人体骨骼节点迭代数据集H。

在此之后，根据以上过程得到的迭代数据集迭代计算协作环境中人与机器人间的最小距离及对应点空间位置坐标，其迭代计算的原理图如图6所示。

由迭代数据集，建立迭代元素间距离的数学关系。令L_r∈L为机器人运动节点迭代数据集中的任意迭代元素，L_h∈H为人体骨骼节点迭代数据集中任意的迭代元素，其分别对应上空间中的一条线段，t_j,s_j分别为L_r与L_h上任意一点。则迭代元素之间的最小距离定义如式(13)所示：

进一步的，建立空间点到线之间的距离关系。令s₀,s₁,t₀,t₁分别为两条线段的端点，则有如下关系：

s_j＝s₀+c_su c_s∈[0,1] (14)

t_j＝t₀+c_tv c_t∈[0,1] (15)

u＝s₁-s₀ (16)

v＝t₁-t₀ (17)

w₀＝s₀-t₀ (18)

其中c_s,c_t为向量系数，u和v分别为对应线段的方向向量，w₀两线段端点之间的向量。则空间某一线段的点到另一线段之间存在如下关系，以s₀为例：

(s₀-t_j)·v＝0 (19)

在迭代计算中，每一迭代过程中若c_t∈(-∞，0)则在空间点线计算中，用空间线段的左侧端点更新迭代空间点，若c_t∈[0，1]则用式(21)的方法计算出用于更新的空间点，若c_t∈(1,∞)则用空间线段的右侧端点更新迭代空间点。若(20)式中||v||＝0则表示空间线段收缩为空间中一点，反过来计算此收缩的点到另一条线段的距离即可。

空间任意两条线段之间的最小距离存在且唯一。通过有限次迭代可以求得最小距离，并且存在迭代次数N，当迭代次数n≥N时，两条线段最小距离对应的空间点不再改变。基于以上理论，迭代停止条件设定为当两条空间线段迭代中前后两次所返回的迭代空间点相同或之差小于一个给定无穷小值时，停止迭代过程。

则协作环境下人机最小距离迭代计算结果由式(22)得到，对应的最小距离空间点坐标为迭代过程停止时对应的迭代变量值。

具体的迭代计算流程如图7所示。详细的迭代步骤如下：

Step 1：输入机器人运动节点迭代数据集L和人体骨骼节点迭代数据集H，初始化筛选矩阵E；判断数据集L和H是否为空集，若为空集，则结束以下计算过程；否则执行Step2；

Step 2：按照机器人的迭代次序向量提取机器人运动节点迭代数据集L中的元素L_r′，按照人体骨骼的迭代次序向量提取人体骨骼节点迭代数据集H中的元素L_h′，则L_r′与L_h′分别对应空间中的一条线段，由此得到两条空间线段，L_r′对应的空间线段用L_r表示，L_h′对应的空间线段用L_h表示；

Step 3：迭代常量与方向向量初始化；用L_h的两个端点初始化s₀,s₁，则s₀,s₁为与L_h对应的空间中的两个点；用L_r的两个端点初始化t₀,t₁，则t₀,t₁为与L_r对应的空间中的两个点；令向量u和v分别为L_h与L_r的方向向量，w₀为两条空间线段端点之间的方向向量，则其表达式如式(23)(24)(25)所示：

u＝s₁-s₀ (23)

v＝t₁-t₀ (24)

w₀＝s₀-t₀ (25)

Step 4：迭代变量初始化：令S_temp＝s_j＝s₀,T_temp＝t_j＝t₀，其中，S_temp,T_temp为迭代过程中的临时变量，s_j,t_j为迭代过程中的计算所得变量；

Step 5：计算向量系数c_t：

若c_t∈(-∞，0)则t_j＝t₀；若c_t∈(1,∞)则t_j＝t₁；若c_t∈[0，1]则执行式(27)；

计算向量系数c_s：

若c_s∈(-∞，0)则s_j＝s₀；若c_s∈(1,∞)则s_j＝s₁；若c_s∈[0，1]则执行式(29)；

s_j＝s₀+c_su (29)

Step 6：计算式(30)，式(30)为迭代停止条件；

(||S_temp-s_j||≤δ)∩(||T_temp-t_j||≤δ) (30)

其中，δ为设定的迭代阈值，若式(30)的计算结果为真，则采用式(31)计算L_r与L_h之间的最小距离d(L_r,L_h)，并将式(31)所得d(L_r,L_h)与s_j,t_j组合成列向量存入筛选矩阵E，之后判断数据集L和H是否为空集，若不为空集则执行Step 2；若为空集，则表示本次数据集中的所有元素均迭代完毕，执行Step 7；

d(L_r,L_h)＝||s_j-t_j|| (31)

若式(30)计算结果为假，则更新迭代变量如式(32)(33)所示：

S_temp＝s_j (32)

T_temp＝t_j (33)

之后执行Step 5；

Step 7：对筛选矩阵E进行式(34)所示计算，则人机最小距离d_min(L,H)如下式所示：

d_min(L,H)＝min{d|d∈C,C＝[1 0 0]E} (34)

其中，d为矩阵C中的元素，由式(12)所得人机最小距离d_min(L,H)在筛选矩阵E中查找到d_min(L,H)对应的索引，并由索引读取人机最小距离d_min(L,H)所对应的两个空间点。

迭代算法结果示意图可见图8a～图8f。经过以上迭代计算终可获得人与机器人之间的最小距离与对应的空间点坐标，并附加的在迭代过程中可获得机器人与人的关键运动节点以及由运动节点构成的运动链之间的详细的相对位置关系，最终完成协作环境下人机最小距离及对应点的实时计算。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替代，都应当视为属于本发明的保护范围。

Claims (7)

1.一种协作环境下人机最小距离的测算方法，其特征在于，包括以下步骤：

步骤S1、使用位置测量与标定***确定机器人与3D视觉传感器的相对位姿关系，得到机器人基坐标系和3D视觉传感器坐标系之间的转换关系；

S2、根据机器人各部件之间的相对位置关系与部件旋转关系，建立部件的基准坐标系，基准坐标系的原点即为机器人的关键运动节点，并根据机器人的关键运动节点之间的相对位姿关系，建立机器人广义坐标模型，测算机器人各部件的计算余量，设定机器人的迭代次序向量，实时读取机器人运动过程中的关节角数据，根据运动过程中的关节角数据，利用机器人广义坐标模型与设定的机器人的迭代次序向量构建人机最小距离的机器人运动节点迭代数据集；

S3、根据3D视觉传感器跟踪并识别协作环境中的人体信息，提取人体信息中的人体骨骼节点数据，计算人体的安全裕量与人体骨骼的迭代次序向量，此后，根据人体骨骼节点数据与人体骨骼的迭代次序向量构建人机最小距离的人体骨骼节点迭代数据集；

S4、根据得到的机器人运动节点迭代数据集、人体骨骼节点迭代数据集、机器人各部件的计算余量和人体的安全裕量，迭代计算协作环境中人与机器人间的最小距离及与最小距离相对应的空间点的坐标；

机器人各部件的计算余量是按以下准则确定：以机器人广义坐标模型中的相邻部件的基准坐标系的原点为端点，两端点的连线为圆柱体的中心轴线，构建圆柱体，则机器人各部件的计算余量为能够将两圆柱体上下底面之间的机器人部件完全包络的圆柱体半径的下确界。

2.根据权利要求1所述的一种协作环境下人机最小距离的测算方法，其特征在于，所述步骤S1中，通过标定***标定机器人坐标系与3D视觉传感器所在的传感器坐标系的相对位置与姿态，并确定二者之间的转换矩阵。

3.根据权利要求1所述的一种协作环境下人机最小距离的测算方法，其特征在于，所述S2中，

机器人各部件间的相对位置关系是指按照机器人基座到末端法兰的先后连接次序，后连接的部件相对于前一个部件在机器人广义坐标模型中部件的基准坐标系沿前一个部件基准坐标系三坐标轴方向的相对平移关系，用^i-1A_i＝(x y z)表示，^i-1A_i为机器人的第i个部件相对于第i-1个部件的相对平移关系，x、y、z分别为x,y,z轴上的平移量；

部件的旋转关系是指部件在部件的基准坐标系中的旋转轴线，用R_i＝(r_x r_y r_z)表示，R_i表示第i个部件的旋转轴线，r_x、r_y、r_z分别表示部件的旋转轴线的方向向量在部件的基准坐标系中沿三坐标轴的分量；

所述实时读取机器人运动过程中的关节角数据，根据运动过程中的关节角数据，利用机器人广义坐标模型与设定的机器人的迭代次序向量构建人机最小距离的机器人运动节点迭代数据集；具体如下：

实时读取机器人运动过程中的关节角数据，并由机器人广义坐标模型得到当前时刻的机器人的关键运动节点的位置，按照机器人的从基座到末端法兰的机械连接顺序建立机器人的链式结构关键运动节点数据，建立机器人的迭代次序向量，构造人机最小距离的机器人运动节点迭代数据集L。

4.根据权利要求1所述的一种协作环境下人机最小距离的测算方法，其特征在于，所述S3具体如下：

S3.1：根据3D视觉传感器获取的人机协作场景的图像数据和深度信息，通过图像分析与深度数据处理得到人体的识别结果并跟踪协作环境中的人体信息，并将人体从环境中分割出来，结合深度信息计算人体的骨骼节点数据，通过图像与空间的映射关系，提取人体的骨骼节点的空间位置信息；

S3.2：根据3D视觉生成的人体识别结果与人体的骨骼节点数据，确定识别的人体骨骼节点信息在实际物理空间中对应人体部位的相应位置，并据此计算人体的安全裕量；

S3.3：根据S3.1中提取的人体骨骼节点数据，以相邻人体骨骼关节所对应的节点在骨骼节点数据中的索引为次序标记，计算人体骨骼的迭代次序向量；人体骨骼的迭代次序向量与人体的骨骼节点数据构成人体骨骼节点迭代数据集H。

5.根据权利要求1所述的一种协作环境下人机最小距离的测算方法，其特征在于，所述S4中迭代计算包括以下要素：

S4.1：输入机器人运动节点迭代数据集L和人体骨骼节点迭代数据集H，初始化筛选矩阵E；判断数据集L和H是否为空集，若为空集，则结束以下计算过程；否则执行S4.2；

S4.2：按照机器人的迭代次序向量提取机器人运动节点迭代数据集L中的元素L′_r，按照人体骨骼的迭代次序向量提取人体骨骼节点迭代数据集H中的元素L′_h，则L′_r与L′_h分别对应空间中的一条线段，由此得到两条空间线段，L′_r对应的空间线段用L_r表示，L′_h对应的空间线段用L_h表示；

S4.3：迭代常量与方向向量初始化；用L_h的两个端点初始化s₀,s₁，则s₀,s₁为与L_h对应的空间中的两个点；用L_r的两个端点初始化t₀,t₁，则t₀,t₁为与L_r对应的空间中的两个点；令向量u和v分别为L_h与L_r的方向向量，w₀为两条空间线段端点之间的方向向量，则其表达式如式(1)(2)(3)所示：

u＝s₁-s₀ (1)

v＝t₁-t₀ (2)

w₀＝s₀-t₀ (3)

S4.4：迭代变量初始化：令S_temp＝s_j＝s₀,T_temp＝t_j＝t₀，其中，S_temp,T_temp为迭代过程中的临时变量，s_j,t_j为迭代过程中的计算所得变量；

S4.5：计算向量系数c_t：

若c_t∈(-∞，0)则t_j＝t₀；若c_t∈(1,∞)则t_j＝t₁；若c_t∈[0，1]则执行式(5)；

计算向量系数c_s：

若c_s∈(-∞，0)则s_j＝s₀；若c_s∈(1,∞)则s_j＝s₁；若c_s∈[0，1]则执行式(7)；

s_j＝s₀+c_su (7)

S4.6：计算式(8)，式(8)为迭代停止条件；

(||S_temp-s_j||≤δ)∩(T_temp-t_j||≤δ) (8)

其中，δ为设定的迭代阈值，若式(8)的计算结果为真，则采用式(9)计算L_r与L_h之间的最小距离d(L_r,L_h)，并将式(9)所得d(L_r,L_h)与s_j,t_j组合成列向量存入筛选矩阵E，之后判断数据集L和H是否为空集，若不为空集则执行S4.2；若为空集，则表示本次数据集中的所有元素均迭代完毕，执行S4.7；

d(L_r,L_h)＝||s_j-t_j|| (9)

若式(8)计算结果为假，则更新迭代变量如式(10)(11)所示：

S_temp＝s_j (10)

T_temp＝t_j (11)

之后执行S4.5；

S4.7：对筛选矩阵E进行式(12)所示计算，则人机最小距离d_min(L,H)如下式所示：

d_min(L,H)＝min{d|d∈C,C＝[1 0 0]E} (12)

其中，d为矩阵C中的元素，由式(12)所得人机最小距离d_min(L,H)在筛选矩阵E中查找到d_min(L,H)对应的索引，并由索引读取人机最小距离d_min(L,H)所对应的两个空间点。

6.根据权利要求5所述的一种协作环境下人机最小距离的测算方法，其特征在于，迭代停止条件为：当两条空间线段迭代中前后两次所返回的迭代空间点相同或之差小于一个给定无穷小值时，停止迭代过程。

7.根据权利要求1所述的一种协作环境下人机最小距离的测算方法，其特征在于，机器人的关键运动节点为预先选取的机器人的运动节点。