的效率和精度。在本发明中，对 操作员和机器人均采用较规则的几何形状圆柱体来对机器人和操作员进行建模，基于圆柱 体包围盒的碰撞检测具体方案如下：
[0087] 设A, B为一圆柱的上下底圆圆心，C, D为另一圆柱的上下底圆圆心，空间直线 1AB，Iai为两圆柱的中轴线，主要考虑两直线异面的情况，求取两异面直线1 AB，Iai的公垂线和 垂足P，Q，若P或Q不在线段AB或者线段CD内，则进行边界检测，选取最靠近另外一条直线 的端点替代P或Q点。
[0088] 如果P，Q均在线段AB和线段CD内，则两圆柱只能是侧面相交，此时只需要判断线 段PQ的长度和两圆柱的半径之和即可。
[0089] 如果P，Q中有一个点落在线段内，一个点落在端点上，则两圆柱只能侧面和端面 相交，此时寻找侧面上最靠近端面的母线，判断母线与端面是否相交即可。
[0090] 如果P，Q均在线段的端点上，则两圆柱只能是端面相交，此时只需要判断空间上 两个圆面是否相交即可。
[0091] 以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范 围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。
【主权项】
1. 一种人机协作场景下的机器人智能躲避人类方法，其特征在于，包括以下步骤： 1) 通过机器人D-H参数构建虚拟场景的机器人圆柱模型，如下： 假设机器人是由一系列连杆和关节以任意形式构成的，D-H参数表示的是机器人关节 和连杆之间的关系；为了对机器人进行建模，采用点对关节进行建模，圆柱体对连杆进行建 模；涉及到机器人的正运动学，通过机器人各个关节的初始角度，来求解各个关节的坐标系 相对基坐标系的变换关系；给每个关节定义一个右手坐标系，把一个关节的坐标系和下一 个关节的坐标系之间的齐次变换叫做A矩阵，A 1表示第一个关节相对于在基坐标系的位置 和姿态，那么A2则表示为第二个关节相对于第一个关节的位置和姿态，而第二个关节相对 于在基坐标系的位置和姿态可由如下矩阵乘积组成： T2=A1A2 (1) 以此类推，第n个关节相对于在基坐标系的位置和姿态的公式为：式中，^x3表示第n个关节的姿态，冗xl表示第n个关节相对于基坐标系的位置；其中A 1 可根据D-H参数表示：0 ;，山，a;, a ;为机器人关节i的D-H参数； 构建虚拟场景的机器人圆柱模型时以机器人基坐标系为模型的坐标系，求解各个关节 相对于基坐标系的位置，相邻关节间的连杆采用圆柱体进行建模，圆柱体的上 下底面圆心为两关节点的位置，圆柱体半径根据实际情况进行调整，构建6自由度的机器 人圆柱模型； 2) 通过Kinect实时获取真实场景的RGB图像和操作员的骨骼数据，并构建虚拟场景的 操作员圆柱模型，如下： 操作员在进入到机器人的工作区间时，由固定在操作员前面的Kinect实时获取真实 场景的RGB图像和操作员的骨骼数据，实现对操作员的跟踪和定位；Kinect有三种摄像头： 一种用于采集彩色图像的RGB彩色摄像头和两种用于采集深度图像的红外摄像头； 对于真实场景RGB图像的获取，将Kinect放在环境中的某个位置，打开彩色图像NUI_ INITIALIZE_FLAG_USES_COLOR来初始化Kinect，通过获取的彩色图像帧数据，用OpenCV绘 制出来； 对于骨骼数据的获取，打开骨骼数据NUI_INITIALIZE_FLAG_USES_SKELETON来初始化 Kinect，当人处于站立状态时，Kinect能够获取到人的20个关节点的位置来表示人的骨 架；提取15个关节点来构建虚拟场景中操作员的圆柱模型，这15个关节点从上到下和从左 到右排序为：①头；②肩中心；③右肩；④右手肘；⑤右手；⑥左肩；⑦左手肘；⑧左手；⑨髋 关节中心；⑩右髋；?右膝；?右脚；?左髋；0左膝；?左脚；这些关节点的位置都 是相对于Kinect坐标系的位置； 在构建操作员圆柱模型时以Kinect坐标系为模型的坐标系，通过Kinect对深度图像 的处理，获取操作员的15个关节点的位置，对人体骨架中相邻的关节点采用 圆柱体进行建模，圆柱体的上下底面圆心为两关节点的位置，圆柱体半径根据实际情况进 行调整； 3)真实场景和虚拟场景中机器人和操作员的标定对应，如下： 在上述两个步骤中，对虚拟场景中机器人的建模是以机器人基坐标系为模型的坐标 系，对虚拟场景中操作员的建模是以Kinect坐标系为模型的坐标系，为了将虚拟场景和真 实场景进行对应，选取真实场景中的一个坐标系，称为世界坐标系El，机器人基坐标系称为 坐标系E2, Kinect坐标系称为坐标系E3 ; 机器人基坐标系E2与世界坐标系El之间的关系用旋转矩阵R和平移矩阵T来表示， 设机器人某个关节点P在机器人基坐标系下的坐标为(If 在世界坐标系下的 坐标为(夂^，<i，Zf ) ^于是它们之间的关系为：式中，e2Rei为3X3矩阵，表示机器人基坐标系相对于世界坐标系的姿态变化矩阵；e2Tei 为3X1矩阵，表示机器人基坐标系相对于世界坐标系的位置变化矩阵;M2S 4X4矩阵，表示 机器人基坐标系相对于世界坐标系的位姿变化矩阵； 同理，Kinect坐标系E3与世界坐标系El之间的关系也用旋转矩阵R和平移矩阵T来 表示，设操作员某个关节点P'在Kinect坐标系下的坐标为在世界坐标系 ?， 下的坐标为(If则它们之间的关系为：将机器人基坐标系和Kinect坐标系均转换到世界坐标系之后，接下来就是确定真实 场景和虚拟场景的映射关系以及衡量他们之间的误差；若虚拟场景中的某一点^的坐标为 (-&，& A )，则该点匕在真实场景RGB图像中的坐标为（?，；），则存在一个映射f，使得 虚拟场景中任意的点Pv及其对应真实场景中的点P P存在如下关系： Pr= f(P v)+e (6) 式中，e为虚拟场景和真实场景对应的误差； 对于Kinect获取操作员的骨骼三维数据和彩色图像二维数据，Kinect SDK提供 了两者之间的相互转化，即提供了虚拟场景操作员和真实场景操作员的映射fpCT_; NuiTransformSkeletonToDepthImage提供了三维骨豁数据到二维深度图像的映射关系A， NuiImageGetCoIorPixeICoordinatesFromDepthPixel 提供了 二维深度图像到二维彩色图 像的映射f2，由此可以得知三维骨骼数据到二维彩色图像的映射关系： fperson= f 2 * fl0) 至此，虚拟场景的操作员真实场景的操作员已实现了对应，对于机器人，由于机器人基 坐标系和Kinect坐标系均已转化到世界坐标系，则机器人和操作员所处的虚拟场景和真 实场景的坐标系是一样的，则虚拟场景机器人和真实场景机器人的映射关系1^与虚拟场 景操作员和真实场景操作员的映射关系f PCT_是一样的： frobot f person (8) 则对于整个虚拟场景和真实场景而言，它们之间的映射关系f?的公式为： f = frobot= f person= f 2 * ^ 0) 结合上面公式（6)和公式（9)，可得衡量虚拟场景和真实场景的误差e : e = Pr-f2 * f! (Pv) (10) 4)机器人模型和操作员模型的实时碰撞检测，如下： 当操作员进入机器人的作业环境中时，操作员的信息对于机器人来说具有不可预知 性，对操作员和机器人的建模策略会直接影响到碰撞检测的效率和精度；对操作员和机器 人均采用较规则的几何形状圆柱体来对机器人和操作员进行建模，基于圆柱体包围盒的碰 撞检测具体方案如下： 设A, B为一圆柱的上下底圆圆心，C, D为另一圆柱的上下底圆圆心，空间直线1AB, Icd 为两圆柱的中轴线，主要考虑两直线异面的情况，求取两异面直线1AB，1")的公垂线和垂足 P，Q，若P或Q不在线段AB或者线段CD内，则进行边界检测，选取最靠近另外一条直线的端 点替代P或Q点； 如果P，Q均在线段AB和线段CD内，则两圆柱只能是侧面相交，此时只需要判断线段PQ 的长度和两圆柱的半径之和即可； 如果P，Q中有一个点落在线段内，一个点落在端点上，则两圆柱只能侧面和端面相交， 此时寻找侧面上最靠近端面的母线，判断母线与端面是否相交即可； 如果P，Q均在线段的端点上，则两圆柱只能是端面相交，此时只需要判断空间上两个 圆面是否相交即可。
【专利摘要】本发明公开了一种人机协作场景下的机器人智能躲避人类方法，包括以下步骤：1)通过机器人D-H参数构建虚拟场景的机器人圆柱模型；2)通过Kinect实时获取真实场景的RGB图像和操作员的骨骼数据，并构建虚拟场景的操作员圆柱模型；3)真实场景和虚拟场景中机器人和操作员的标定对应；4)机器人模型和操作员模型的实时碰撞检测。采用本发明方法，操作员可以在机器人作业空间中自由运动，而机器人则可以根据Kinect获取的操作员的信息进行主动的避障。
【IPC分类】G05D1/02
【公开号】CN105137973
【申请号】CN201510518563
【发明人】张平, 杜广龙, 金培根, 高鹏, 刘欣, 李备
【申请人】华南理工大学
【公开日】2015年12月9日
【申请日】2015年8月21日