CN103170973A - 基于Kinect摄像机的人机协作装置及方法 - Google Patents

Abstract

本发明主要基于近期微软公司推出的一款Kinect?的体感摄像机，来实现人体目标的检测和跟踪，本文利用Kinect?的人体骨架检测技术能精确判断出目标物的位置以及试验者的意图，避免交接时候出现危险，而且提出了基于工作空间的RRT算法来实现路径规划。通过反复试验验证本系统可以安全的进行人际交接。

Description

基于Kinect摄像机的人机协作装置及方法

技术领域

本发明涉及图像处理和目标跟踪技术，特别涉及一种安全的人机协作。

背景技术

传统的研究人机交接系统，主要是依靠参与者来配合机器人的工作，而且要求试验者高度的集中精力，以免在实验的过程中发生危险，这样给试验者带来了很大的压力。

一般的跟踪技术大都是基于二维图片技术来的，以前大多数利用HMM方法来识别人手的动作，Weinland等在2007年将HMM方法和三维建模结合起来识别人的运动，由于上述是基于2维空间即使没有干扰下精度也不是很高。Wu等在2007年利用RFID方法在试验者手腕贴标签的方法来跟踪和识别。

常用来进行目标检测的方法主要有三种，包括背景相减法、光流计算法和帧差法。其中光流法的优势是在摄像机运动的情况下检测出独立运动的目标，但其抗噪性差且计算量较大，不适合实时处理；帧差法的优势是可以很好的适应动态环境，但由于位置不准确，不能很好的提取出运动目标的所有相关点；传统的背景相减法首先要在没有目标出现的场景中提取一个背景模型，并保存为背景图像，然后将当前帧与背景模型相减。 如果象素值相减的结果大于某一特定值(即阈值), 则可确定该象素点是运动目标上的某点, 否则属于背景图像。背景相减法得到的结果可以确定出目标的位置以及形状等信息,然而此方法对外界环境比如光线、天气等的变化比较敏感,同时容易受到目标运动过程阴影的影响。

传统的路径规划算法有多边形拟合法、栅格法、人工势场法、遗传算法等。但这些方法都需要在个确定的空间内对障碍物进行建模，计算复杂度与机器人自由度呈指数关系，不适合解决多自由度机器人在复杂环境中的规划。新的基于快速扩展随机树（RRT）的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。

但是，基于关节空间的RRT 方法有以下的不足：

1. 基于关节空间的RRT法无法解决CP（Continue-path-problem，连续路径问题）问题和准CT问题(quasi-continuoustrajectory problem，即仅部分限定末端执行器的运动轨迹)。

2.  基于关节空间的RRT 法无法预知机械臂末端执行器运动轨迹，造成末端执行器运动与关节运动分离，不利于任务的完成。

总的来说，如果需要控制或者约束机械臂末端执行器的运动轨迹，基于关节空间的RRT 法是无能为力的。

发明内容

针对上述问题，本发明所涉及的一种基于

摄像机的人机协作的装置及其方法，可以精确的对空间位置进行定位，可通过人体骨骼的提取方法可以容易的获取人体信息，可通过图像计算中的聚类方法可以精确的得到目标物的位置信息，可利用基于工作空间的快速扩展随机算法可以得到较好的壁障效果，可以为双臂机器人协作打下基础。

本发明为了实现上述目的，可以使用以下方案：

本发明提供了一种与试验员协作从而实现对目标物进行跟踪和协作的人机协作装置，其特征在于，具有：至少1个机械手，用于抓握目标物；至少1个机械臂，机械臂的底部为基座，机械臂的末端安装有机械手；至少1个

摄像机，对空间位置进行立体定位，采集彩色图像信息、深度图像信息和试验员的骨骼信息；至少1个棋盘格，用于标定机械臂末端的位置；以及至少1台计算机，其中，计算机用于处理彩色图像信息、深度图像信息和骨骼信息，处理空间位置的相互关系，实现对目标物进行跟踪和与试验员进行协作，计算机与机械臂通过控制器局域网络总线连接，并且控制机械臂的运动与机械手的运动。

本发明所涉及的人机协作装置所实现的人机协作的方法，其特征在于，具有以下步骤：将基座的中心点作为三维空间坐标系的基准点建立基准坐标系，将末端作为中心点建立末端坐标系；采用摄像机发射红外激光，采集彩色图像信息和深度图像信息，然后对空间位置进行立体定位，从而建立以

摄像机为中心点的

摄像机坐标系；将棋盘格安装在末端，采用

摄像机采集棋盘格的格点的彩色图像信息和深度图像信息，得到棋盘格在

摄像机坐标系的坐标位置，从而标定末端在

摄像机坐标系的坐标；采用摄像机发射红外激光识别目标物和试验员的运动，

摄像机将采集的骨骼信息，实现对目标物和试验员的人体骨骼的跟踪，快速获取试验员的手腕位置，定位目标物的大致位置信息；采用

摄像机获取目标物周围的彩色图像信息和深度图像信息，采用计算机基于目标物周围的彩色图像信息和深度图像信息通过图像处理技术中的聚类算法提取试验员手中目标物的坐标位置信息；采用计算机将基准坐标系，末端坐标系和摄像机坐标系相互之间通过旋转矩阵和平移矩阵的方法统一到一个坐标系中；采用计算机根据末端相对于基准坐标系的坐标位置、姿态和速度关系，并根据平移矩阵，旋转矩阵和速度矩阵的参数，精确控制机械臂的坐标位置、姿态、速度；采用计算机根据目标物的坐标位置，调整末端姿态使得机械手靠近目标物，并完成机械手对目标物的抓取。

在本发明所涉及的人机协作方法，计算机对机械臂的运动进行基于工作空间的快速扩展随机树路径规划算法，先确定机械臂末端的运动路径，然后确定机械臂各个关节的运动路径。

发明效果

本发明提供的人机协作的装置及方法，其具有：机械臂，机械手，

摄像机，棋盘格以及计算机。通过

摄像机可以对空间位置进行精确定位，并通过获得人体骨骼运动的节点和聚类的图像处理技术，快速确定目标物的精确位置，解决了抗噪性差、计算量较大和定位不准确的问题，同时不容易受到目标物运动过程阴影的影响，利用基于工作空间的快速扩展随机数路径规划算法，模拟了人的手臂的运动规律，适合解决多自由度机器人在复杂环境中的规划。

附图说明

图1是人机协作装置的示意图。

图2是人机协作装置中Power Cube模块化机械臂通讯结构图。

图3是人机协作装置中Barrett Hand机械手的外形及尺寸图。

图4是人机协作装置中及其图像坐标系与各坐标系的变换关系图。

图5是人机协作装置中 

人体骨骼检测流程图。

图6是8×8棋盘格标定示意图。

图7是

人体骨骼检测示意图。

图8是基于工作空间的快速扩展随机树路径规划算法(RRT算法)节点扩展过程示意图。

具体实施方法

以下结合附图对本发明涉及的一种基于

摄像机的人机协作装置及方法的优选实施例做详细阐述，但本发明并不仅限于该实施例。为了使公众对本发明有彻底的了解，在以下本发明优选实施例中详细说明了就具体的细节。

图1为人机协作装置的示意图。

如图1所示，本发明所涉及的人机协作装置包括了

摄像机101，计算机102，模块化机械臂103，以及机械手104。在以

摄像机为中心点建立了摄像机坐标系，以位于机械臂底部的基座的中心为中心点建立基准坐标系，计算机通过控制器局域网络总线(CAN)对机械臂的姿态，速度和运动轨迹进行规划和控制。

图2为人机协作装置中Power Cube模块化机械臂通讯结构图。

如图2所示，本发明所涉及的机械臂是德国Schunk公司开发并生产七自由度机械手臂，由7个完全独立的PowerCube模块组合而成，每一个关节模块都是完全独立的运动单元。所有的模块都通过CAN总线与控制计算机相连，模块之间采用串行通讯接口，这个接口传递所有的运动控制、参数设置以及监控命令。采用CAN通讯可增加其抗干扰能力，而工控机一般只有PCI插槽，通过转接卡可将CAN信号直接传递给工业控制计算机，电源系统采用自保护电路，提供驱动电源与逻辑电源。该计算机可以通过控制每个Power Cube模块来实现对机械臂运动姿态、速度和运动轨迹进行规划和控制。

图3为人机协作装置中Barrett Hand机械手的外形及尺寸图。

如图3所示，该机械手为美国Barrett Technology公司生产的BH8.280灵巧手，该灵巧手是一个可编程的多手指抓持器，功能强大，结构紧凑，具有非常高的灵活性，可以抓取不同形状、大小及姿态的目标物体，并且手指和手掌表面有压力传感器，可以用于目标物抓取好坏的判断。

图4为人机协作装置中及其图像坐标系与各坐标系的变换关系图。

如图4所示，

摄像机是微软公司推出的一款用于X-BOX360的体感摄像机。使用

摄像机，我们可以获得一幅分辨率为640*480的RGB彩色图像和一幅与之对相应的深度图像。利用获得的深度信息，我们可以通过转换得到相应环境的3D模型，此时的三维坐标系为世界坐标系E2,转换后的三维坐标系如图4，原点为摄像机所在位置，Z轴正方向为摄像机正前方，为摄像机坐标系E2。

摄像机的标定及坐标系之间的转换：由于

摄像机可安放在环境中的任意位置，在环境中选择一个坐标系来描述摄像机的位置，并用它描述环境中任何物体的位置，该坐标系称为世界坐标系E1。它由xR_wR、yR_wR、zR_wR轴组成。摄像机坐标系E2与世界坐标系E1之间的关系可以用旋转矩阵R与平移向量T来描述。空间中某一点P在世界坐标系与摄像机坐标系下的齐次坐标如果分别是X=(x_W,y_W,z_W,1)^T与x=(x,y,z,1)^T，于是存在如下关系：

$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=M_2\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]---\left(1\right)$

其中，R为3×3正交单位矩阵；T为三维平移向量；0=(0,0,0)^T；M₂为4×4矩阵。

从世界坐标系E1到图像坐标系E5的完整变换可以分如下步骤进行：

物点从世界坐标系E1到摄像机三维坐标系E2的平移矩阵T和旋转变换矩阵R中的外参数共有6个，它们是相应于R的用欧拉角表示的侧倾角φ、俯仰角θ、旋转角

，以及相应于平移矢量T的三个分量T_x,T_y,T_z。旋转矩阵R可以表示为φ、θ、的函数。

由摄像机坐标系E2与世界坐标系E1关系式（1）和（2）可以得到

$\left\{\begin{array}{c}x=r_1{x}_w+r_2{y}_w+r_3{z}_w+T_x\\ y=r_4{x}_w+r_5{y}_w+r_6{z}_w+T_y\\ z=r_7{x}_w+r_8{y}_w+r_9{z}_w+T_z\end{array}\right.---\left(3\right)$

式中(x_W,y_W,z_W)是P点在世界坐标系中的坐标，（x，y，z）是P点在摄像机坐标中的坐标。

摄像机内部参数有以下几项，f：有效焦距，即图像平面到投影中心距离；k：透镜畸变系数；N_x：x方向的比例系数；N_v：y方向的比例系数；X_c,Y_c：图像平面原点的计算机图像坐标。根据

摄像机本身内部参数的设定，并利用投影原理可以将摄像机坐标系E2转换成理想成像平面坐标系E3。

实际图像平面坐标系E4（X_d，Y_d）到计算机图像坐标E5（u_d，v_d）的变换为：

$\left\{\begin{array}{c}{u}_d=s_x{d}_x^{\′-1}{X}_d+u_0\\ v_d=d_y^{\′-1}{Y}_d+v_0\end{array}\right.---\left(4\right)$

其中，d＇_x=dN_cx/N_fx，d_x为摄像机在X方向的像素间距，d_y为摄像机在Y方向的像素间距，N_cx为摄像机在X方向的像素数，N_fx为计算机在X方向采集到的行像素数，s_x为图像尺度因子或称为纵横比，（u₀，v₀）为光学中心。这里先推导一般的非共面标定点的摄像机参数求解方法。

此外通过公式（1）和对图像坐标系E5（加上深度信息）的转换处理，可以将图像坐标系E5转换到基准坐标系E6,基准坐标系E6为机械臂基座的中心为中心点建立的三维空间坐标系。

图5为人机协作装置中 

摄像机人体骨骼检测流程图。

如图5所示，基于

摄像机对人体骨骼进行检测，并实现机械臂对人体运动的跟踪具有以下步骤：

步骤201：

判断是否有人进入。

摄像机可以拍摄二维彩色图像，通过RGB传感器获得彩色图像的信息、即一副分辨率为640*480的RGB彩色图像，同时还可以拍摄显示深度的图像。

摄像机的摄像头能够发射红外激光，并通过反射对空间位置进行定位，由于空间中任意两处不同区域反射回来的激光散斑图案都不相同，从而通过分析反射图案，可以活动一一对应的空间信息。当还没有任何跟踪目标在场景中时，需要预先记录整个空间的散斑图案，即每隔固定的距离，选取一个垂直于摄像头光轴的参考平面，将该平面上的散斑图案保存于设备内。

步骤202：

人体跟踪。当目标物或者有测试人员出现之后，红外激光在照射到目标物后，会反射红外线形成散斑图像，由于该散斑图像具有高度的单一性，会随着距离的不同而变换图案，将该图像和设备内存储的已有记录图像进行相关比对，得到一些列距离值，其中峰值代表的位置即为该目标物所处的位置。将这些峰值位置叠加，再使用线性或非线性插值运算，从而重构三维场景。

三维重建是基于OpenNI技术实现的，OpenNI软件内部的SetViewPoint()函数可以将深度图像通过一定的变换对应到RGB图像中。步骤203：

触发姿势检测。

步骤204：

程序驱动成功。

步骤205：

人体骨骼显示及跟踪。基于OpenNI的人体骨架的检测技术，可以快速精准的得到人体骨骼的位置，从而实现目标物的跟踪以及精准抓取。

步骤206：

跟踪丢失。

图6为8×8棋盘格标定示意图。

如图6所示，本实施例的标定模板是8x8的标准棋盘格，每格长度为6cm。首先将棋盘格安装在机械臂末端，则棋盘格的格点在末端坐标系的坐标可以直接算出，并且通过末端位姿(或关节转角)计算其格点相对于基坐标系的坐标；使用opencv接口检测棋盘格角点，得到其在

光学摄像机中的图像坐标，之后使用opencv中的cv::calibrateCamera计算

光学摄像机的内外参数矩阵。则优化好的外参数矩阵即是基坐标系到

坐标系的变换矩阵。

棋盘格的角点就是白色各点与黑色各点相交的点，由于在彩色图中黑白两色具有最高的对比点，通过计算这些角点的位置来确定几幅不同方位的棋盘角点的位置信息，从而标定机械臂末端在摄像机坐标系E2的位置。

利用公式(1)和位置可以求得

摄像机与机械臂末端坐标系的转换关系(transform mat 的逆矩阵)，同时根据运动学控制可以得到机械臂的基准坐标系E6到末端坐标系的转换矩阵，从而得到

与机械臂的基准坐标系E6的转换关系矩阵T。

图7为

人体骨骼检测示意图。

如图7所示，

摄像机能够基于OpenNI的人体骨架的检测技术，可以很快和很好的得到人体骨骼的位置，比如，人的头部，颈部和手臂关节和手腕位置，腹部，髋关节及膝盖和脚等能够描述人体运动特征的部位以节点的形式展现出来，实现对人体骨骼的跟踪，可以使图像遍历的范围缩小，从而减少了工控计算机的庞大的计算量。

首先，利用

摄像机通过人体骨骼检测技术快速得到试验者左手的手腕位置，这样可以定位目标物的大致位置信息，之后可以在小范围内，由于计算的范围小可以提高检测的速度和精确度。

然后利用k-means 聚类的算法来提取出手中物体的位置信息，以便于配合机械臂与人的协作、跟踪和机械手的精确抓取。

k-means 算法的工作过程说明如下：首先从n个数据对象任意选择k 个对象作为初始聚类中心；而对于所剩下其它对象，则根据它们与这些聚类中心的相似度（距离），分别将它们分配给与其最相似的（聚类中心所代表的）聚类；然后再计算每个所获新聚类的聚类中心（该聚类中所有对象的均值）；不断重复这一过程直到标准测度函数开始收敛为止。一般都采用均方差作为标准测度函数. k个聚类具有以下特点：各聚类本身尽可能的紧凑，而各聚类之间尽可能的分开。当实现机械臂对于目标物的跟踪之后，工控计算机可以控制机械臂和机械手完成对目标物的抓取。通过运动学控制，完成对机械臂运动的规划，实现机械手与试验员之间进行目标物的交接。

运动学控制描述的是机器臂的末端执行器相对于参考坐标系的位置、姿态和速度关系，包括正运动学和逆运动学。在本发明中涉及到的是逆运动学，就是根据已知的末端位置、姿态信息或者是速度和机械臂各个关节的几何关系，求解各个机械臂关节角值的变化。通常在实际情况中，已知的总是机械臂的末端姿态，再根据末端姿态解算机械臂的各个关节角。 例如：可以把机器臂看作是一系列由关节连接起来的连杆构成的。我们将为机械臂的每个连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对位置和姿态。通常把描述一个连杆和下个连杆间相对关系的齐次变换叫做A矩阵。如果A₁表示第一个连杆对于基系的位置和姿态，A₂表示第二个连杆相对于第一个连杆的位置和姿态，那么第二个连杆在基坐标系的位置和姿态可由下列矩阵乘积给出T₂= A₁ A₂，同理，当机械臂为七自由度的时候有T₇= A₁ A₂ A₃A₄A₅A₆A₇，这样就可以知道了机械臂的末端相对于基坐标系的位置和姿态。公式T₇= A₁ A₂ A₃A₄A₅A₆A₇即为运动学方程。通过对目标物位置、姿态信息和速度等参数进行机械臂的运动轨迹的规划，可以快速、高效、精确的实现人机协作和目标物的交接。

图8为基于工作空间的快速扩展随机树路径规划算法(RRT算法)节点扩展过程示意图。

如图8所示，工作状态空间下RRT算法的基本原理：初始位姿x_ini为树的第一个节点。在每一迭代步中，随机生成状态点x_rand，参照ρ找到离x_rand最近的一点x_near；然后，从x_near和x_rand中找到节点x_new，原则是x_new到x_near的距离比x_rand到x_near的距离更近。之后。判断x_new是否在障碍区域内，若不在，则把它纳入T中，形成新的树；若在，则继续寻找新的节点。重复迭代直到T中有一点x达到目标区域Q_gool。

工作状态空间下RRT算法的基本原理是借鉴人的生理反应，人的手臂从一个地方移动到另一个地方，是先确定手的运动路径，然后决定手臂各关节的运动。这给我们的启示是，RRT 树的规划空间应该是工作空间，而不是关节空间。这样，既符合人手臂的运动规律，又能把末端执行器的运动路径和关节轨迹结合起来，从而有效解决CP、准CT 等对末端执行器路径有一定约束的轨迹规划问题。

用上述方法获取目标物的中心点，进行轨迹规划。得到的结果如图8所示，包括RRT树轨迹（本次实验没有设置障碍物）、机械臂末端运动轨迹、各关节轨迹和末端坐标分量轨迹。

具体实施例的作用与效果

根据具体实施例所涉及的一种基于摄像机的人机协作装置及方法，由于使用了

摄像机，可以根据彩色图像和深度信息可以快速方便的进行三维建模，该三维建模所确定的空间位置十分精确，且解决了抗噪性差，计算量大，不适合实时处理，定位不准确，和受外界环境影响的问题。由于摄像机的OpenNI技术可以捕捉人体运动的骨骼信息，无需使用RFID的方法在试验员手腕处贴标签的方法实现跟踪，同时可以使图像遍历的范围缩小，减少了计算机的计算量。由于对机械臂的运动轨迹的规划是根据机械臂末端姿态、位置和速度进行规划的，并提出了基于工作空间的快速扩展随机树的路径规划算法的壁障规划，适合解决多自由度的机械臂在复杂环境中的规划问题，使得本发明所涉及的人机协作的装置能够实现更高效、快速和安全的人机协作。

Claims (3)

1.一种与试验员协作从而实现对目标物进行跟踪和协作的人机协作装置，其特征在于，具有：

至少1个机械手，用于抓握所述目标物；

至少1个机械臂，所述机械臂的底部为基座，所述机械臂的末端安装有所述机械手；

至少1个 摄像机，对空间位置进行立体定位，采集彩色图像信息、深度图像信息和所述试验员的骨骼信息；

至少1个棋盘格，用于标定所述末端的位置；以及

至少1台计算机，

其中，所述计算机用于处理所述彩色图像信息、所述深度图像信息和所述骨骼信息，处理所述空间位置的相互关系，实现对所述目标物进行跟踪和与所述试验员进行协作，所述计算机与所述机械臂通过控制器局域网络总线连接，并且控制所述机械臂的运动与所述机械手的运动。

2.一种用根据权利要求1所述的人机协作装置实现人机协作的方法，其特征在于，具有以下步骤：

将所述基座的中心点作为三维空间坐标系的基准点建立基准坐标系，将所述末端作为中心点建立末端坐标系；

采用所述

 摄像机发射红外激光，采集彩色图像信息和深度图像信息，然后对所述空间位置进行立体定位，从而建立以 摄像机为中心点的

 摄像机坐标系；

将所述棋盘格安装在所述末端，采用所述

 摄像机采集所述棋盘格的格点的彩色图像信息和深度图像信息，得到所述棋盘格在所述

 摄像机坐标系的坐标位置，从而标定所述末端在所述 摄像机坐标系的坐标；

采用所述

 摄像机发射红外激光识别所述目标物和所述试验员的运动，所述 摄像机将采集的所述骨骼信息，实现对所述目标物和所述试验员的人体骨骼的跟踪，快速获取所述试验员的手腕位置，定位 所述目标物的大致位置信息；

采用所述

 摄像机获取所述目标物周围的彩色图像信息和深度图像信息，采用所述计算机基于所述目标物周围的彩色图像信息和深度图像信息通过图像处理技术中的聚类算法提取所述试验员手中所述目标物的坐标位置信息；

采用所述计算机将所述基准坐标系，所述末端坐标系和所述 摄像机坐标系相互之间通过旋转矩阵和平移矩阵的方法统一到一个坐标系中；

采用所述计算机根据所述末端相对于所述基准坐标系的坐标位置、姿态和速度关系，并根据平移矩阵，旋转矩阵和速度矩阵的参数，精确控制所述机械臂的坐标位置、姿态、速度；

采用所述计算机根据所述目标物的坐标位置，调整所述末端姿态使得所述机械手靠近所述目标物，并完成所述机械手对所述目标物的抓取。

3.根据权利要求2所述的人机协作方法，其特征在于：

所述计算机对所述机械臂的运动进行基于工作空间的快速扩展随机树路径规划算法，先确定所述机械臂末端的运动路径，然后确定所述机械臂各个关节的运动路径。 

Images