CN109465835A - 一种动态环境下双臂服务机器人作业的事前安全预测方法 - Google Patents

Abstract

本发明属于服务机器人的安全预测领域，并公开了一种动态环境下双臂服务机器人作业的事前安全预测方法。该方法包括：(a)获取待预测对象的骨骼模型和点云图，以此获得待预测对象的空间位置信息；(b)构建双臂机器人的运动学模型和三维结构模型，以此获取双臂机器人的空间位置信息；将待预测对象和双臂机器人的位置信息统一到同一个世界坐标系中；(c)对于待预测对象选择球形包围盒或胶囊形包围盒构建其包围盒模型，对于双臂机器人选择胶囊形包围盒代表每个部分其包围盒模型；(d)计算待预测对象和双臂机器人包围盒模型之间的距离，根据距离值判断预测对象是否安全。通过本发明，提高计算效率低、缩短计算时间长，提高计算精度。

Description

一种动态环境下双臂服务机器人作业的事前安全预测方法

技术领域

本发明属于服务机器人的安全预测领域，更具体地，涉及一种动态环境下双臂服务机器人作业的事前安全预测方法。

背景技术

服务机器人作为人工智能、物联网等高新技术的载体，或将成为下一个互联网终端。所以服务机器人在未来的应用场景会非常的广泛，并且目前社会的人口老龄化问题逐渐严重，老年人群体对于护理、陪伴、康复医疗等方面有着巨大的需求，然而与之相反的是人力成本保持着较高的增长趋势，进一步扩大对服务机器人的需求，但是，服务机器人还不能真正的进入人类的生活之中，其中主要的原因之一就是服务机器人的安全性问题。

目前对于服务机器人安全性问题的解决方案大致可分为两种类型：一种为事后安全控制，即在人与机器人发生碰撞后采取控制策略来消除碰撞会对人体造成的伤害；另一种为事前安全控制，即在人与机器人发生碰撞之前主动采取相应的安全措施来避免或者降低人机碰撞的危害。

针对服务机器人的事后安全控制方案，UR机器人与ABB机器人通过采集机器人各关节的电流来估算关节实时力矩，以此判断机器人是否发生碰撞。该方法由于在碰撞前没有任何安全措施，所以对于机器人的运行速度有着较大的限制，同时为了降低冲撞力，对机器人的质量以及刚度也有很大的限制，另外由于是通过电流推算出电机的输出力矩，由于需要通过减速机构连接负载端，会因为减速机构的摩擦导致理论的关节力矩与实际的关节力矩有较大的误差，从而导致判断机器人是否发生碰撞时会出现误判的情况。博世机器人采取的方案是给机器人的所有关节都穿上一层保护衣，这些外衣实质就是触觉传感器，能够检测到任何外界非正常的碰撞力，并实施的将检测信息反馈给控制系统，从而实现机器人的安全策略。但是这种方法的数据处理量非常大，对于计算机的硬件要求较高，而且传感器成本很高，经济性并不好。

针对服务机器人的事前安全控制方案，ABB公司利用激光传感器为固定工作空间的机器人建立虚拟栅栏，以此检测是否有人员进入机器人工作空间。该方法对人与机器人的工作空间有着极大的限制，有学者研究使用红外传感器与超声传感器分别进行人体的粗定位与精定位。该方法依然存在限制人与机器人工作空间的问题，并且红外传感器与超声波传感器易受环境影响，且测量精度不高。有学者使用体感相机对人机的骨架进行定位，将人机简化为球体模型进行距离检测，该方法可以使得距离检测计算精度大大提高，但是由于球体模型较多，计算效率较低，不能满足系统对于实时性的要求。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种动态环境下双臂服务机器人作业的事前安全预测方法，通过获取待预测对象与双臂机器人的空间位置信息后，然后根据这些信息建立待预测对象与双臂机器人的包围盒模型，最后通过计算包围盒模型之间的距离获得待预测对象与双臂机器人之间的距离，以此判断待预测对象是否处于安全距离之内，该过程中采用包围盒模型代替待预测对象和双臂机器人，一方面大大地简化了二者的三维结构，另一方面也极大地简化了求解二者之间的距离时的计算，且计算过程中也不需要限制机器人的工作空间、质量、刚度、工作速度等情况，由此解决事前预测安全中的计算效率低、计算时间长，以及计算精度差的技术问题。

为实现上述目的，按照本发明，提供了一种动态环境下双臂服务机器人作业的事前安全预测方法，其特征在于，该方法包括下列步骤：

(a)针对在双臂服务机器人作业中的安全待预测对象，获取该待预测对象的骨骼模型和点云图，以此分别获得该待预测对象的空间位置信息，包括所述骨骼模型中各个关节点的坐标和该待预测对象的点云坐标；

(b)构建所述双臂机器人的运动学模型和三维结构模型，以此获取该双臂机器人的空间位置信息，包括各个关节点坐标和各个连杆参数；将所述待预测对象和双臂机器人的位置信息进行坐标转换，以此统一到同一个世界坐标系中；

(c)根据所述待预测对象的骨骼模型和所述双臂机器人的三维结构模型分别将二者划分为多个部分，对于待预测对象，选择球形包围盒或胶囊形包围盒代表每个所述部分，对于所述双臂机器人，选择胶囊形包围盒代表每个所述部分，分别对待预测对象和双臂机器人的所有部分建立包围盒后即获得所述待预测对象和双臂机器人各自的包围盒模型，其中，所述胶囊形包括圆柱部分和两个半球形部分，所述两个半球形部设置在所述圆柱部分的两端，且所述半球形部分的底面与所述圆柱部分的底面重合，所述球形或胶囊形的尺寸分别根据所述待预测对象和双臂机器人的空间位置信息计算获得；

(d)计算所述待预测对象和双臂机器人包围盒模型之间的距离，将该距离值与预设可接受安全阈值进行比较，在该阈值范围内则安全，否则不安全，以此实现待预测对象安全的事前预测。

进一步优选地，在步骤(c)中，所述球形包围盒优选选取相应部分的最小外接球作为所述球形包围盒。

进一步优选地，在步骤(c)中，对于待预测对象，所述胶囊形包围盒中圆柱的半径优选相应部分边界上的点到该部分中心轴线之间的垂直距离中的最大值。

进一步优选地，在步骤(c)中，所述双臂机器人的胶囊包围盒圆柱部分的直径径优选为相应连杆的最大横截面处的宽度。

进一步优选地，在步骤(d)中，所述计算所述待预测对象和双臂机器人包围盒模型之间的距离具体为：计算所述待预测对象上的每个部分对应的包围盒与所述双臂机器人末端连杆对应的包围盒之间的距离，即计算所述待预测对象上的球形包围盒或胶囊形包围盒与所述双臂机器人上的胶囊形包围盒之间的距离。

进一步优选地，在步骤(d)中，计算所述待预测对象上的球形包围盒或胶囊形包围盒与所述双臂机器人上的胶囊形包围盒之间的距离分别按照下列方式计算：

i当计算球形包围盒到胶囊形包围盒之间的距离时，通过计算球形包围盒的中心到所述胶囊形包围盒中心轴线之间的距离获得；

ii当计算胶囊形包围盒到胶囊包围盒之间的距离时，通过计算两个胶囊包围盒中心轴线之间的距离获得。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1、本发明通过采用构建包围盒的形式代替待预测对象与双臂机器人的三维结构模型，一方面包围盒的形式构建过程简单，另一方面大大地简化了待预测对象与双臂机器人之间安全距离检测的计算过程，缩短即计算时间，提高了计算效率和精度，大大提高了事前安全预测的可靠性；

2、本发明采用胶囊形和球形构建包围盒，一方面构建胶囊形圆柱体包围盒也较为简单，另一方面当被包围的物体处于运动状态时也只需要实时更新圆柱体包围盒的两端端点的坐标，能够保证实时重构包围盒模型，由此简化计算过程；

3、本发明提出的服务机器人人机安全策略，一方面不需要再限制人与机器人的工作空间，使得人与机器人能够更近距离的接触；另外由于视觉的加入，则可以限制人机发生碰撞时的冲撞力，所以也不需要限制机器人质量，刚度、运行速度，增大了服务机器人的设计空间。

附图说明

图1是按照本发明的优选实施例所构建的一种动态环境下双臂服务机器人作业的事前安全预测方法的流程图；

图2是按照本发明的优选实施例所构建的人体骨骼模型与关节点的结构示意图；

图3是按照本发明的优选实施例所构建的深度相机的空间坐标系的结构示意图；

图4是按照本发明的优选实施例所构建的双臂机器人连杆的坐标系与连杆参数示意图；

图5是按照本发明的优选实施例所构建的相机的针孔成像模型的示意图；

图6是按照本发明的优选实施例所构建的手眼标定的示意图；

图7是按照本发明的优选实施例所构建的人体和机器人的刚体连杆模型示意图；

图8是按照本发明的优选实施例所构建的胶囊型包围盒结构示意图；

图9是按照本发明的优选实施例所构建的构建人体包围盒的结构示意图；

图10是按照本发明的优选实施例所构建的人体躯干切片的示意图；

图11是按照本发明的优选实施例所构建的四根刚体连杆机器人的胶囊型圆柱体的包围盒模型示意图；

图12是按照本发明的优选实施例所构建的包围盒最小半径示意图；

图13是按照本发明的优选实施例所构建的距离检测过程的示意图。

具体实施方式

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

本发明是根据待预测对象与机器人的结构特点分别建立胶囊形圆柱体包围盒模型，然后通过计算包围盒的空间位置关系从而判断待预测对象与机器人的实时距离。

图1是按照本发明的优选实施例所构建的一种动态环境下双臂服务机器人作业的事前安全预测方法的流程图，本实施例中，待预测对象为人，具体的技术路线如下：

(1)利用深度相机捕获人体的骨架信息，得到人体各关节点在深度相机坐标系下的空间三维坐标；利用人体的表面点云数据与各关节点的位置信息获得人体点云坐标；

(2)对机器人进行运动学分析，获取机器人的各个关节点在机器人基坐标系下的空间三维坐标；对机器人进行三角面片的划分，获得各个三角面片顶点坐标；

(3)通过相机标定的方法，将人体的空间位置信息与机器人空间位置信息统一到一个世界坐标系下；

(4)利用得到的人体与机器人的空间位置信息与各关节的尺寸信息分别对人与机器人构建胶囊形圆柱体包围盒模型，利用人体关节点坐标和人体点云坐标获得人体包围盒的位置和尺寸参数，利用机器人的关节坐标和三角面片的顶点坐标获得机器人包围盒的位置和尺寸参数；

(5)通过对人与机器人包围盒之间的距离检测实时获取人体与机器人的空间实时距离。

下面将结合具体的实施过程进一步说明上述技术方案。

(1)人体各关节点位置信息的获取

获取人体关节数据的方法是在OpenNI的开发框架下，使用中间件NITE利用深度相机实时的获取人体的骨骼模型。图2是按照本发明的优选实施例所构建的人体骨骼模型与关节点的结构示意图，如图2所示，在OpenNI的开发框架下，能够获取人体的15个关节点，并且可以得到各个关节点的空间坐标，图3是按照本发明的优选实施例所构建的深度相机的空间坐标系的结构示意图，如图3所示，坐标原点就在深度相机的接受红外光的摄像头处，其中Z轴方向为沿着摄像头方向，Y轴方向为竖直向上的方向，X轴方向为沿着Z轴方向看时的左边指向。

最后基于OpenNI框架通过深度相机获取到的人体各关节的三维数据的形式如下所示：

jiont.x＝SkeletonFrame.SkeletonData[m].SkeletonPositions[n].x；

jiont.y＝SkeletonFrame.SkeletonData[m].SkeletonPositions[n].y；

jiont.z＝SkeletonFrame.SkeletonData[m].SkeletonPositions[n].z；

其中jiont是关节，SkeletionFrameSkeletonData是骨骼数据，SkeletonPositions是骨架位置。m表示深度相机进行人体检测的对象，n表示人体的关节，x、y、z表示关节点n在深度相机坐标系下的三维坐标。

(2)机器人各关节点位置信息获取

机器人各关节点位置信息获取的方法是对机器人进行运动学分析，本实施例的硬件实验平台是七自由度机器人，使用齐次变换法将机器人的运动学机器人的几何关系联系起来，由于机器人的左右两臂为对称结构，其运动学模型的建立过程是一样的，所以可以通过对右臂的分析来建立出整个机器人的运动学模型。图4是按照本发明的优选实施例所构建的双臂机器人个连杆的坐标系与连杆参数示意图，如图4所示，通过上述对机器人的各连杆的参数以及建立的坐标系进行分析可以得到机器人的D-H参数表如表所示：

表1机器人单臂连杆参数

a_i-1和α_i-1分别表示连杆i-1的长度和扭角，d_i表示从x_i-1到x_i沿z_i测量的距离，θ_i表示从x_i-1到x_i绕z_i旋转的角度。通过机器人的模型可以知道表中的机器人连杆参数分别为a₁＝0.069m，a₃＝0.069m，a₅＝0.010m，d₁＝0.27035m，d₃＝0.36442m，d₅＝0.37429m，d₇＝0.387225m。

通过对机械臂运动学的研究可知，变换矩阵可由如下公式求得：

由此便可以得到机器人各关节的坐标系相对于基坐标系的变换关系为：

其中，即为机器人第i个连杆的坐标系原点在机器人基坐标系下的坐标值。

(3)统一世界坐标系

步骤(1)中获得人体关节点坐标是在深度相机坐标系下的，机器人各关节点坐标是在机器人基坐标下的，所以需要对两者的坐标系进行转换，统一到一个世界坐标系下，从而得到人与机器人之间的空间位置关系，也就是需要对相机进行标定。

首先需要对相机进行内参的标定，对深度相机的标定，需要将标定光源换成红外光源。本实施例中采用的标定方法是棋盘格标定法，建立相机的针孔成像模型，图5是按照本发明的优选实施例所构建的相机的针孔成像模型的示意图，如图5所示，对于相机的标定，就是根据相机的成像模型建立相机坐标系与世界坐标系之间的关系，其标定方程如下所示：

其中M₁就是相机的内参矩阵，即为相机的固有属性，M₂为相机的外参矩阵，即为相机坐标系与标定板坐标系之间的变换关系。坐标轴按顺序绕xyz轴分别旋转的角度为ψ，φ，τ，世界坐标系下坐标用(Xw，Yw，Zw)来表示，相机坐标系下的坐标用(Xc，Yc，Zc)来表示，物理坐标系下的坐标用(X，Y)来表示，像素坐标系下的坐标用(u，v)来表示。

利用上述的方法对相机进行标定，最终可以得到的相机内参以及畸变系数如下表所示，其中f_x，f_y是焦距的坐标，u₀，v₀是投影中心的坐标，k₁，k₂为径向畸变系数；p₁，p₂为切向畸变系数。

表2相机内参及畸变系数

然后需要对相机与机器人进行手眼标定，就是建立相机坐标系与机器人基坐标系之间的变换关系。需要标定的深度相机是固定在双臂七自由度机器人的基座本体上，相对于机器人的基坐标系保持相对静止状态，需要标定的就是机器人基坐标系与相机坐标系之间的关系。标定的基本原理就是将标定板固定在机器人末端的夹手之上，然后以不同的姿态出现在固定相机的视场之中，以此可以完成机器人与相机之间的手眼标定。图6是按照本发明的优选实施例所构建的手眼标定的示意图，如6所示，根据图6中的各坐标系之间的变换关系，我们可以得到相机坐标系与机器人基坐标系的转换关系^baseH_cam的表达式为：

^baseH_cam＝^baseH_tool·^toolH_obj·^objH_cam

其中^baseH_tool是机器人末端坐标系相对于机器人的基坐标系的变换关系，^toolH_obj是标定板坐标系相对于机器人末端坐标系的变换关系，^objH_cam是相机坐标系相对于标定板坐标系的变换关系，^baseH_cam是相机坐标系相对于机器人基坐标系的变换关系。

让机械臂处于两个不同的位置，分别由相机拍摄标定板的图片，可以得到两个状态下的变换方程组如下：

综上可以得到手眼标定的结果，相机坐标系相对于机器人基坐标系的变换关系^baseH_cam为：

利用该转换关系可以将人体各关节点坐标转换到机器人基坐标系下，从而将人与机器人的坐标系转换到一个世界坐标系下。

(4)人与机器人包围盒模型的构建

利用深度相机能够得到人体的骨架模型，可以将人体抽象为一个个关节连杆互相连接而成；然后通过对双臂机器人进行运动学分析，可以得到机器人的简化刚体模型。图7是按照本发明的优选实施例所构建的人体和机器人的刚体连杆模型示意图，如图7所示，根据图中的刚体模型提出一种基于中心轴的圆柱形的包围盒模型，并且根据人体与机器人的结构特点，在圆柱形包围盒两端加上半球形组合成胶囊形的包围盒模型，并且在适当的位置比如人体的头部加上球体包围盒模型。通过实时检测两者的包围盒模型之间的距离就可以判断人与机器人的空间位置关系，从而满足人机安全这一要求。

根据人体与机器人的结构特点可以知道，胶囊形圆柱体对于两者包裹紧密性是较好的。图8是按照本发明的优选实施例所构建的胶囊型包围盒结构示意图，如图8所示，描述一个空间的球体包围盒只需要两个参数分别为球心C的坐标以及球体半径r，而对于空间胶囊形圆柱体包围盒的描述也较为简单，分别为圆柱体两端中心的坐标A，B以及圆柱体的半径r，所以构建胶囊形圆柱体包围盒也较为简单，并且当被包围的物体处于运动状态时也只需要实时更新圆柱体包围盒的两端端点的坐标，能够保证实时重构包围盒模型。

i人体包围盒构建

图9是按照本发明的优选实施例所构建的构建人体包围盒的结构示意图，如图9所示，根据胶囊形包围盒的构建特性可以以人体的各个关节连杆为包围盒的中心轴线来建立人体的包围盒模型，由于人体头部与手部的结构特点，对其建立球体包围盒，最终的人体包围盒模型如图9中所示，主要包含躯干，四肢，头部的12个包围盒模型，分为9个胶囊形圆柱体包围盒模型H₁～H₉和3个球体包围盒模型H₁₀～H₁₂。

对于胶囊形圆柱体包围盒模型H₁～H₉的中心轴线的定位只需要确定两端的端点的空间位置坐标，而通过人体的骨骼模型可以知道，各个中轴线的两端点就是人体的各个关节的两端的关节点，其空间位置信息可以通过深度相机获取，对于躯干的包围盒H₅的下端点可以通过两髋关节的中心位置求得。同理对于球体包围盒H₁₀～H₁₂的中心即为骨骼模型中头部与手部的空间位置坐标。

对人人体包围盒半径r的确定，本专利采用的方法是利用人体的骨骼模型以及人体的点云数据来计算得到。利用骨骼模型定位人体的不同区域，再利用点云图像确定不同区域的尺寸，由此便可以根据人体的骨骼模型与点云图像来计算人体各包围盒半径R_ht。接下来以人体躯干为例进行介绍，为了简化计算，可以将躯干进行切片处理，通过比较所有切片的最大半径，便可以求出人体躯干的包围盒。设躯干的包围盒中轴线两端点分别为H₅₁(x₅₁，y₅₁，z₅₁)，H₅₂(x₅₂，y₅₂，z₅₂)，若将躯干区域切为k片，则第i个切片与中轴线的交点M_i(x_i，y_i，z_i)的坐标为：

设第i个切片的人体外表面具有n个点，任意一点的坐标为Ps(x_s，y_s，z_s)，图10是按照本发明的优选实施例所构建的人体躯干切片的示意图，如图10所示，则人体躯干区域的第i个切片的Ps点与中轴线的距离L_is为：

则人体躯干的包围盒半径为：

R_h5＝max{L_is|1≤i≤k，1≤s≤n}

一般为了计算的方便，根据人体的不同部位，切片数n选择2～4，至此已经可以确定躯干包围盒的三个参数，人体的其他区域的包围盒半径R_ht计算方法类似，

ii机器人包围盒构建

对于机器人包围盒的构建，经分析可知机械臂的第3、5、7号关节都是绕中心轴线旋转，对该关节的空间位置并没有影响，所以可以将机械臂简化成4个刚体连杆机构分别为L₁、L₃、L₃、L₄，然后对于4根刚体连杆可以依据上述方法建立机械臂的胶囊形圆柱体的包围盒模型，图11是按照本发明的优选实施例所构建的四根刚体连杆机器人的胶囊型圆柱体的包围盒模型示意图，如图11所示，针对一个胶囊形圆柱体的包围盒模型需要知道三个参数，分别为两端点的空间坐标以及胶囊的半径，设第i个连杆的两端点分别为L_i1(x_i1，y_i1，z_i1)，L_i2(x_i2，y_i2，z_i2)，半径为R_ri，通过对机械臂的运动学分析，由公式(3-3)可以得到各个连杆的两端坐标分别为：

通过上述的方法可以得到各包围盒的两端的空间坐标，对于半径r_i的获取，可以将机械臂各刚体连杆沿轴线方向投影到一个平面，找到一个最小的半径r将整个关节都包裹起来。图12是按照本发明的优选实施例所构建的包围盒最小半径示意图，如12所示，为了确定某一关节的R值，可以采用三角面片的方法来求解，同样的可以设关节表面被划分为n个三角面片，用p_i，q_i，r_i表示第i个三角面片的三个顶点，则可以得到半径R如下：

综上对于机器人的包围盒已经构建完成，当机器人各关节在做旋转运动时都能够快速地确定各包围盒的三个参数，保证了实时性的要求。

(4)人机距离检测

通过上述建立包围盒模型的过程建立的人体包围盒有两种元素：胶囊型圆柱体与球体，机器人包围盒模型只包含一种元素：胶囊型圆柱体。所以距离检测模型有两种，一种是球体包围盒与胶囊形圆柱体包围盒距离检测；另一种是胶囊形圆柱体包围盒与胶囊形圆柱体包围盒距离检测。图13是按照本发明的优选实施例所构建的距离检测过程的示意图，如图13所示。

i球体包围盒与胶囊形圆柱体包围盒距离检测

对于球体包围盒与胶囊形圆柱体包围盒距离检测，可以转化为中心点与中轴线之间的距离，也就是空间中一点与一条线段之间的距离，由此可以将复杂的包围盒碰撞问题转化为空间几何问题，计算复杂度得到大幅度的降低。

设机器人的一个包围盒模型的两端点分别为A(x_a，y_a，z_a)，B(x_b，y_b，z_b)，半径为R_r；人体的包围盒模型中心点为O(x_o，y_o，z_o)，半径为R_h，根据前文中包围盒的构建可知包围盒参数都是已经确定的。点0到线段AB的最短距离的计算过程如下：

首先是过点0作直线AB的垂线，设在直线AB上的垂点为C，即另外可由AB点的坐标求得AB的方向向量为：

已知点0与直线的AB的方向向量，则可以利用平面点法式的方法来表示过点0且垂直于直线AB的平面α为：

(x-x_O)(x_B-x_A)+(y-y_O)(y_B-y_A)+(z-z_O)(z_B-z_A)＝0

另外直线AB的表示形式如下：

则直线AB上的任意点可以表示为：

由平面α的方程式与直线AB的方程式可以求得点C处直线AB中m的值为：

根据计算得到的m值可以确定点C与线段AB的位置关系，从而可以得到两者之间的最短距离L_min如下：

ii两胶囊形圆柱体包围盒距离检测

对于两胶囊形圆柱体包围盒距离检测，同样的可以转化为两包围盒中心轴线之间的距离，也就是空间中两条线段之间的距离，距离检测模型如图13所示。同样的可以设机器人的一个包围盒模型的两端点分别为A(x_A，y_A，z_A)、B(x_B，y_B，z_B)，半径为R_r；人体的包围盒模型，两端点分别为C(x_C，y_C，z_C)、D(x_D，y_D，z_D)，半径为R_h，根据前文中包围盒的构建可知包围盒参数都是已经确定的。则线段AB与线段CD之间的最短距离的计算过程如下：

在直线AB上任找一点E，设点E的坐标为E(x_E，y_E，z_E)，则点E可以表示为

同样的在直线CD上任找一点F，设点F的坐标为F(x_F，y_F，z_F)，则点F可以表示为

由空间几何的方法可以知道E、F两点之间的距离L_EF为

由上式知道L_EF中只有s、t两个参数，这两个参数决定点E、F分别在直线AB与直线CD上的位置。所以可以将距离表示为关于s、t两个参数的函数，则距离的L² _EF可以用函数f(s，t)表示如下

求直线AB之间的最短距离，也就是求函数f(s，t)的最小值，所以可以让函数f(s，t)分别对参数s、t求偏导数，并令偏导数等于零求取极值点，计算过程如下：

展开上式并整理得到关于参数s、t的方程组如下：

通过对上式的计算可以得到参数s、t的值。从而可以确定点E、F两点在直线AB、CD上的位置，但是由于直线与线段的区别，参数s、t值得不同，点E、F不一定会落在线段AB、CD上，所以对于线段AB与线段CD的最短距离L_min存在如下情况：

结合上述的两种距离检测模型，可以得到人与机器人的最短距离如下所示，从而实现了基于视觉的人与机器人之间的实时距离检测。

D_min＝L_min-(R_h+R_r)

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

Claims (6)

1.一种动态环境下双臂服务机器人作业的事前安全预测方法，其特征在于，该方法包括下列步骤：

(a)针对在双臂服务机器人作业中的安全待预测对象，获取该待预测对象的骨骼模型和点云图，以此分别获得该待预测对象的空间位置信息，包括所述骨骼模型中各个关节点的坐标和该待预测对象的点云坐标；

(b)构建所述双臂机器人的运动学模型和三维结构模型，以此获取该双臂机器人的空间位置信息，包括各个关节点坐标和各个连杆参数；将所述待预测对象和双臂机器人的位置信息进行坐标转换，以此统一到同一个世界坐标系中；

(c)根据所述待预测对象的骨骼模型和所述双臂机器人的三维结构模型分别将二者划分为多个部分，对于待预测对象，选择球形包围盒或胶囊形包围盒代表每个所述部分，对于所述双臂机器人，选择胶囊形包围盒代表每个所述部分，分别对待预测对象和双臂机器人的所有部分建立包围盒后即获得所述待预测对象和双臂机器人各自的包围盒模型，其中，所述胶囊形包括圆柱部分和两个半球形部分，所述两个半球形部设置在所述圆柱部分的两端，且所述半球形部分的底面与所述圆柱部分的底面重合，所述球形或胶囊形的尺寸分别根据所述待预测对象和双臂机器人的空间位置信息计算获得；

(d)计算所述待预测对象和双臂机器人包围盒模型之间的距离，将该距离值与预设可接受安全阈值进行比较，在该阈值范围内则安全，否则不安全，以此实现待预测对象安全的事前预测。

2.如权利要求1所述的方法，其特征在于，在步骤(c)中，所述球形包围盒优选选取相应部分的最小外接球作为所述球形包围盒。

3.如权利要求1或2所述的方法，其特征在于，在步骤(c)中，对于待预测对象，所述胶囊形包围盒中圆柱的半径优选相应部分边界上的点到该部分中心轴线之间的垂直距离中的最大值。

4.如权利要求1-3任一项所述的方法，其特征在于，在步骤(c)中，所述双臂机器人的胶囊包围盒圆柱部分的直径径优选为相应连杆的最大横截面处的宽度。

5.如权利要求1-4任一项所述的方法，其特征在于，在步骤(d)中，所述计算所述待预测对象和双臂机器人包围盒模型之间的距离具体为：计算所述待预测对象上的每个部分对应的包围盒与所述双臂机器人末端连杆对应的包围盒之间的距离，即计算所述待预测对象上的球形包围盒或胶囊形包围盒与所述双臂机器人上的胶囊形包围盒之间的距离。

6.如权利要求5所述的方法，其特征在于，在步骤(d)中，计算所述待预测对象上的球形包围盒或胶囊形包围盒与所述双臂机器人上的胶囊形包围盒之间的距离分别按照下列方式计算：

ⅰ当计算球形包围盒到胶囊形包围盒之间的距离时，通过计算球形包围盒的中心到所述胶囊形包围盒中心轴线之间的距离获得；

ⅱ当计算胶囊形包围盒到胶囊包围盒之间的距离时，通过计算两个胶囊包围盒中心轴线之间的距离获得。