[发明专利]一种人机协作场景下的机器人智能躲避人类方法

权利要求书

1.一种人机协作场景下的机器人智能躲避人类方法，其特征在于，包括以下步骤：

1)通过机器人D-H参数构建虚拟场景的机器人圆柱模型，如下：

假设机器人是由一系列连杆和关节以任意形式构成的，D-H参数表示的是机器人关节和连杆之间的关系；为了对机器人进行建模，采用点对关节进行建模，圆柱体对连杆进行建模；涉及到机器人的正运动学，通过机器人各个关节的初始角度，来求解各个关节的坐标系相对基坐标系的变换关系；给每个关节定义一个右手坐标系，把一个关节的坐标系和下一个关节的坐标系之间的齐次变换叫做A矩阵，A₁表示第一个关节相对于在基坐标系的位置和姿态，那么A₂则表示为第二个关节相对于第一个关节的位置和姿态，而第二个关节相对于在基坐标系的位置和姿态由如下公式(1)所示的矩阵乘积组成：

T₂＝A₁A₂(1)

以此类推，第n个关节相对于在基坐标系的位置和姿态的公式为：

Tn=A1A2...An=qn3×3pn3×101---(2)]]>

式中，表示第n个关节的姿态，表示第n个关节相对于基坐标系的位置；其中A_i根据D-H参数表示：

Ai=cos(θi)-sin(θi)cos(αi)sin(θi)sin(αi)aicos(θi)sin(θi)cos(θi)cos(αi)-cos(θi)sin(αi)aisin(θi)0sin(αi)cos(αi)di0001---(3)]]>

θ_i,d_i,a_i,α_i为机器人关节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个关节点从上到下和从左到右排序为：①头；②肩中心；③右肩；④右手肘；⑤右手；⑥左肩；⑦左手肘；⑧左手；⑨髋关节中心；⑩右髋；右膝；右脚；左髋；左膝；左脚；这些关节点的位置都是相对于Kinect坐标系的位置；

在构建操作员圆柱模型时以Kinect坐标系为模型的坐标系，通过Kinect对深度图像的处理，获取操作员的15个关节点的位置对人体骨架中相邻的关节点采用圆柱体进行建模，圆柱体的上下底面圆心为两关节点的位置，圆柱体半径根据实际情况进行调整；

3)真实场景和虚拟场景中机器人和操作员的标定对应，如下：

在上述两个步骤中，对虚拟场景中机器人的建模是以机器人基坐标系为模型的坐标系，对虚拟场景中操作员的建模是以Kinect坐标系为模型的坐标系，为了将虚拟场景和真实场景进行对应，选取真实场景中的一个坐标系，称为世界坐标系E1，机器人基坐标系称为坐标系E2，Kinect坐标系称为坐标系E3；

机器人基坐标系E2与世界坐标系E1之间的关系用旋转矩阵R和平移矩阵T来表示，设机器人某个关节点P在机器人基坐标系下的坐标为在世界坐标系下的坐标为于是它们之间的关系为：

XpE1YpE1ZpE11=RE2E1TE2E101·XpE2YpE2ZpE21=M2·XpE2YpE2ZpE21---(4)]]>

式中，^E2R_E1为3X3矩阵，表示机器人基坐标系相对于世界坐标系的姿态变化矩阵；^E2T_E1为3X1矩阵，表示机器人基坐标系相对于世界坐标系的位置变化矩阵；M₂为4X4矩阵，表示机器人基坐标系相对于世界坐标系的位姿变化矩阵；

同理，Kinect坐标系E3与世界坐标系E1之间的关系也用旋转矩阵R和平移矩阵T来表示，设操作员某个关节点P’在Kinect坐标系下的坐标为在世界坐标系下的坐标为则它们之间的关系为：

Xp′E1Yp′E1Zp′E11=RE3E1TE3E101·Xp′E3Yp′E3Zp′E31=M3·Xp′E3Yp′E3Zp′E31---(5)]]>

将机器人基坐标系和Kinect坐标系均转换到世界坐标系之后，接下来就是确定真实场景和虚拟场景的映射关系以及衡量他们之间的误差；若虚拟场景中的某一点P_v的坐标为则虚拟场景中任意的点P_v对应真实场景中的点P_r在真实场景RGB图像中的坐标为则存在一个映射f(·)，使得虚拟场景中任意的点P_v及其对应真实场景中的点P_r，存在如下关系：

P_r＝f(P_v)+ε(6)

式中，ε为虚拟场景和真实场景对应的误差；

对于Kinect获取操作员的骨骼三维数据和彩色图像二维数据，Kinect SDK提供了两者之间的相互转化，即提供了虚拟场景操作员和真实场景操作员的映射f_person；NuiTransformSkeletonToDepthImage提供了三维骨骼数据到二维深度图像的映射关系f₁，NuiImageGetColorPixelCoordinatesFromDepthPixel提供了二维深度图像到二维彩色图像的映射f₂，由此可以得知三维骨骼数据到二维彩色图像的映射关系：

f_person＝f₂·f₁(7)

至此，虚拟场景的操作员和真实场景的操作员已实现了对应，对于机器人，由于机器人基坐标系和Kinect坐标系均已转化到世界坐标系，则机器人和操作员所处的虚拟场景和真实场景的坐标系是一样的，则虚拟场景机器人和真实场景机器人的映射关系f_robot与虚拟场景操作员和真实场景操作员的映射关系f_person是一样的：

f_robot＝f_person(8)

则对于整个虚拟场景和真实场景而言，它们之间的映射关系f的公式为:

f＝f_robot＝f_person＝f₂·f₁(9)

结合上面公式(6)和公式(9)，可得衡量虚拟场景和真实场景的误差ε：

ε＝P_r-f₂·f₁(P_v)(10)

4)机器人模型和操作员模型的实时碰撞检测，如下：

当操作员进入机器人的作业环境中时，操作员的信息对于机器人来说具有不可预知性，对操作员和机器人的建模策略会直接影响到碰撞检测的效率和精度；对操作员和机器人均采用较规则的几何形状圆柱体来对机器人和操作员进行建模，基于圆柱体包围盒的碰撞检测具体方案如下:

设A,B为一圆柱的上下底圆圆心，C,D为另一圆柱的上下底圆圆心，空间直线l_AB,l_CD为两圆柱的中轴线，主要考虑两直线异面的情况，求取两异面直线l_AB,l_CD的公垂线和垂足P,Q，若P或Q不在线段AB或者线段CD内，则进行边界检测，选取最靠近另外一条直线的端点替代P或Q点；

如果P,Q均在线段AB和线段CD内，则两圆柱只能是侧面相交，此时只需要判断线段PQ的长度和两圆柱的半径之和即可；

如果P,Q中有一个点落在线段内，一个点落在端点上，则两圆柱只能侧面和端面相交，此时寻找侧面上最靠近端面的母线，判断母线与端面是否相交即可；

如果P,Q均在线段的端点上，则两圆柱只能是端面相交，此时只需要判断空间上两个圆面是否相交即可。