CN105137973A - 一种人机协作场景下的机器人智能躲避人类方法 - Google Patents

Abstract

本发明公开了一种人机协作场景下的机器人智能躲避人类方法，包括以下步骤：1)通过机器人D-H参数构建虚拟场景的机器人圆柱模型；2)通过Kinect实时获取真实场景的RGB图像和操作员的骨骼数据，并构建虚拟场景的操作员圆柱模型；3)真实场景和虚拟场景中机器人和操作员的标定对应；4)机器人模型和操作员模型的实时碰撞检测。采用本发明方法，操作员可以在机器人作业空间中自由运动，而机器人则可以根据Kinect获取的操作员的信息进行主动的避障。

Description

一种人机协作场景下的机器人智能躲避人类方法

技术领域

本发明涉及机器人运动的技术领域，尤其是指一种人机协作场景下的机器人智能躲避人类方法。

背景技术

智能机器人与其他领域的交叉与融合，产生了许多需要人与机器人协作的场景。智能机器人参与到人的作业环境中，在保证工作效率的前提下还可以有效的解放人的劳动力，特殊情况下还可以代替人完成高风险的任务。在人与机器人近距离协作的背景下，如何保证人的安全是至关重要的问题。这篇发明提出了一种人机协作场景下机器人的建模和避障的方法，该方法根据机器人D-H参数构建机器人圆柱模型，借用Kinect传感器采集操作员骨骼位置，以此来构建操作员的圆柱模型，并根据机器人和操作员的碰撞检测结果使机器人智能地识别并躲避人体。

在构建机器人虚拟场景时，通常的做法是利用3dsMax等建模工具构建与真实场景的机器人完全对应的虚拟机器人模型，然而这种方法不具通用性，且建模成本相对较高，因此通过机器人固有的D-H参数来构建机器人模型是有必要的。

在人机协作场景中，机器人根据人体的位置和动作信息，做出避让，避免发生碰撞。然而传统的机器人避碰研究主要是对物体的避碰，与环境物体不同，人具备自身的结构特征，人体状态和运动也具有不可预知性，因此机器人需要对人体进行实时地识别。

在Kinect出现之前，基于多视觉相机的人体信息采集技术相对比较热门，但是该方法需要透视图像，相对成本也较高。在基于标记的多视觉相机人体识别技术中，人体要穿着特定的服装或设备，在活动过程中处于受限的状态，限制着该技术的应用。基于无标记的人体信息提取采集技术则需要将人体图像、三维信息与目标函数等先验知识进行匹配，匹配过程需要足够的样本数量，否则会导致识别准确性较低，也较难保证实时性。

发明内容

本发明的目的在于克服现有技术的不足，提供一种人机协作场景下的机器人智能躲避人类方法，操作员可以在机器人作业空间中自由运动，机器人根据Kinect获取的操作员的信息进行主动的避障。

为实现上述目的，本发明所提供的技术方案为：一种人机协作场景下的机器人智能躲避人类方法，包括以下步骤：

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

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

T₂＝A₁A₂(1)

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

$T_n=A_1{A}_2...A_n=\left(\begin{array}{cc}{q}_n^{3\×3}& p_n^{3\×1}\\ 0& 1\end{array}\right)---\left(2\right)$

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

$A_i=\left(\begin{array}{cccc}\cos \left({\mathrm{\θ}}_i\right)& -\sin \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& \sin \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\cos \left({\mathrm{\θ}}_i\right)\\ \sin \left({\mathrm{\θ}}_i\right)& \cos \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& -\cos \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\sin \left({\mathrm{\θ}}_i\right)\\ 0& \sin \left({\mathrm{\α}}_i\right)& \cos \left({\mathrm{\α}}_i\right)& d_i\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

θ_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在机器人基坐标系下的坐标为在世界坐标系下的坐标为于是它们之间的关系为：

$\left(\begin{array}{c}{X}_p^{E1}\\ Y_p^{E1}\\ Z_p^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E2}}_{E1}& {T_{E2}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)=M_2\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)---\left(4\right)$

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

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

$\left(\begin{array}{c}{X}_{p^{\′}}^{E1}\\ Y_{p^{\′}}^{E1}\\ Z_{p^{\′}}^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E3}}_{E1}& {T_{E3}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)=M_3\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)---\left(5\right)$

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

P_r＝f(P_v)+ε(6)

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

对于Kinect获取操作员的骨骼三维数据和彩色图像二维数据，KinectSDK提供了两者之间的相互转化，即提供了虚拟场景操作员和真实场景操作员的映射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均在线段的端点上，则两圆柱只能是端面相交，此时只需要判断空间上两个圆面是否相交即可。

本发明与现有技术相比，具有如下优点与有益效果：

本发明方法对机器人和操作员采用较规则的几何体形状圆柱体进行建模，具有较好的通用性和适用性，同时圆柱体建模能够在一定程度上提高碰撞检测的效率和精度。使得操作员可以在机器人作业空间中自由运动，机器人可以根据Kinect获取的操作员的信息进行主动的避障，满足操作要求。

附图说明

图1为本发明方法的系统结构图。

图2为6自由度机器人圆柱模型图。

图3为操作员圆柱模型图。

具体实施方式

下面结合具体实施例对本发明作进一步说明。

本实施例所述的人机协作场景下的机器人智能躲避人类方法，使操作员可以在机器人作业空间中自由运动，机器人根据Kinect获取操作员的信息进行主动的避障，其系统结构图如图1所示。本发明方法的关键是构建与真实人机协作场景对应的虚拟机器人模型和操作员模型以及机器人模型和操作员模型的碰撞检测方法，其包括以下步骤：

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

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

T₂＝A₁A₂(1)

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

$T_n=A_1{A}_2...A_n=\left(\begin{array}{cc}{q}_n^{3\×3}& p_n^{3\×1}\\ 0& 1\end{array}\right)---\left(2\right)$

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

$A_i=\left(\begin{array}{cccc}\cos \left({\mathrm{\θ}}_i\right)& -\sin \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& \sin \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\cos \left({\mathrm{\θ}}_i\right)\\ \sin \left({\mathrm{\θ}}_i\right)& \cos \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& -\cos \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\sin \left({\mathrm{\θ}}_i\right)\\ 0& \sin \left({\mathrm{\α}}_i\right)& \cos \left({\mathrm{\α}}_i\right)& d_i\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

θ_i,d_i,a_i,α_i为机器人关节i的D-H参数；

本发明构建虚拟场景的机器人圆柱模型时以机器人基坐标系为模型的坐标系，求解各个关节相对于基坐标系的位置相邻关节间的连杆采用圆柱体进行建模，圆柱体的上下底面圆心为两关节点的位置，圆柱体半径根据实际情况进行调整，构建的6自由度的机器人圆柱模型如图2所示。

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所示。

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

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

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

$\left(\begin{array}{c}{X}_p^{E1}\\ Y_p^{E1}\\ Z_p^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E2}}_{E1}& {T_{E2}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)=M_2\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)---\left(4\right)$

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

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

$\left(\begin{array}{c}{X}_{p^{\′}}^{E1}\\ Y_{p^{\′}}^{E1}\\ Z_{p^{\′}}^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E3}}_{E1}& {T_{E3}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)=M_3\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)---\left(5\right)$

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

P_r＝f(P_v)+ε(6)

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

对于Kinect获取操作员的骨骼三维数据和彩色图像二维数据，KinectSDK提供了两者之间的相互转化，即提供了虚拟场景操作员和真实场景操作员的映射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均在线段的端点上，则两圆柱只能是端面相交，此时只需要判断空间上两个圆面是否相交即可。

以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (1)

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

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

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

T₂＝A₁A₂(1)

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

$T_n=A_1{A}_2...A_n=\left(\begin{array}{cc}{q}_n^{3\×3}& p_n^{3\×1}\\ 0& 1\end{array}\right)---\left(2\right)$

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

$A_i=\left(\begin{array}{cccc}\cos \left({\mathrm{\θ}}_i\right)& -\sin \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& \sin \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\cos \left({\mathrm{\θ}}_i\right)\\ \sin \left({\mathrm{\θ}}_i\right)& \cos \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& -\cos \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\sin \left({\mathrm{\θ}}_i\right)\\ 0& \sin \left({\mathrm{\α}}_i\right)& \cos \left({\mathrm{\α}}_i\right)& d_i\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

θ_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在机器人基坐标系下的坐标为在世界坐标系下的坐标为于是它们之间的关系为：

$\left(\begin{array}{c}{X}_p^{E1}\\ Y_p^{E1}\\ Z_p^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E2}}_{E1}& {T_{E2}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)=M_2\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)---\left(4\right)$

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

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

$\left(\begin{array}{c}{X}_{p^{\′}}^{E1}\\ Y_{p^{\′}}^{E1}\\ Z_{p^{\′}}^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E3}}_{E1}& {T_{E3}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)=M_3\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)---\left(5\right)$

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

P_r＝f(P_v)+ε(6)

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

对于Kinect获取操作员的骨骼三维数据和彩色图像二维数据，KinectSDK提供了两者之间的相互转化，即提供了虚拟场景操作员和真实场景操作员的映射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均在线段的端点上，则两圆柱只能是端面相交，此时只需要判断空间上两个圆面是否相交即可。