一种机械臂交互控制方法及系统与流程

本发明属于无障碍工程技术领域，涉及一种机械臂交互控制方法及系统。

背景技术：

主从遥操作机械臂控制系统广泛应用于核反应堆维护、载人航天、医疗手术的模拟训练等高危险性作业中，手部是人体最常用的一个部位。因此，实现机械臂跟随手部运动可以很大程度上提高了人的感知能力和行为能力。研究手部的各种运动，手部与机械臂的交互方式，具有重要的理论意义和现实意义。

目前，现有的手部跟踪定位技术主要有两种，基于数据手套技术和基于图像视觉处理的手势识别。数据手套技术由于本身及其电路连线较为笨重，不方便使用，加上价格昂贵等因素限制了其应用。weinland等人提出将3d建模和hmm方法识别人手的运动。liu等利用rfid方法在试验者手腕处贴标签的方法来跟踪和识别手部状态，这种方法可以确定试验者的手腕位置。但是，彩色图像容易受光照条件影响，而且传统的基于视觉的运动目标跟踪需要对摄像头内外参数进行标定，但在实际情况下无法精确标定。

技术实现要素：

有鉴于此，本发明的目的在于提供一种基于kinect的无标定人机交互控制系统及方法，kinect是一台体感摄像机，可以在获取的每一帧深度图像上进行背景相减获得人体各关节3d坐标，即人体骨骼信息，并从人体骨骼信息中无标定获取人手的位置信息。鉴于此，本发明利用kinect骨骼追踪技术获取人手运动信息，并构建其与机械臂的运动映射关系，就它们之间的主从控制方式进行研究。

为达到上述目的，本发明提供如下技术方案：

一种机械臂交互控制方法，该方法包括以下步骤：

s1：人体深度图像信息获取；

s2：图像预处理、人体关节点识别和骨骼数据的平滑处理；

s3：人手相对于自身髋骨中心的运动与机械臂相对于基座运动之间的映射转换以及机械臂逆运动学分析与求解；

s4：控制指令的生成、规范指令格式和指令的收发。

进一步，所述步骤s1具体为：

利用kinect人体骨骼追踪技术非接触获取人手运动信息，具体过程是：将分割化的人体深度图像传送到辨别人体部位的动作捕捉机器学习系统中，利用随机森林的分类方法设计身体组件的中间表示,将姿势估计问题映射为逐像素分类问题；通过重投影分类器的结果，生成人体关节的可信3d估计。

进一步，所述步骤s2具体为：

在进行逆运动学求解前先对异常值予以识别和剔除，并且对骨骼数据进行降噪和平滑处理；

假定轨迹平滑处理算法的时间平移步长为n，i为当前位置信息在规划位置信息中的权重，则当k≥n时，k时刻的规划位置由下式计算获得：

当k＜n时，k时刻的规划位置为：

在求解位置增量的过程中，对相邻时刻人手位置的增量进行平滑处理。

进一步，所述步骤s3具体为：

采取笛卡尔空间映射模式，即将手部的位姿作为机械臂末端执行器位姿的输入；在控制方式选择上采用增量式控制，将人手的运动位姿增量叠加到当前机器人的末端执行器的位姿上，实现手部对机械臂的实时引导控制；

人手与髋骨中心的位置信息是在kinect坐标系下，为建立上述的运动映射关系，将其坐标转换到机器人坐标系下，具体实现方法如下：

s31：从骨骼信息流skeletonframe提取出右手在kinect坐标系{k}下的位置矢量^kp1＝[x1,y1,z1]^t，以及髋骨中心在{k}下的位置矢量^kp2＝[x2,y2,z2]^t；得到kinect坐标系{k}下操作者右手相对于自身髋骨中心的位置^kp：

^kp＝^kp1-^kp2(3)

s32：骨骼跟踪坐标系都是在kinect摄像头坐标系，原点为红外摄像头中心，z轴为红外摄像头光轴，x轴方向为水平方向，y轴为垂直方向；为得到机械臂末端相对于基座的坐标^bp，需求出kinect摄像头坐标系{k}转换到机器人坐标系{b}的旋转矩阵kinect摄像头坐标系{k}按右手定则绕z轴旋转-90°，再绕x轴旋转-90°，得到旋转矩阵

s33：最终得到机械臂末端相对于基座的坐标^bp为：

采用反变换法,即在变换矩阵前乘一个或若干个逆变换矩阵，通过比较方程两边对应元素，达到求解运动学逆解的目的；如上所述，由公式(5)得到机械臂末端在基坐标系下的位置矢量^bp＝[x,y,z]^t，基于d-h参数机械臂末端坐标系相对于基座坐标系的齐次变换矩阵为：

其中，r11＝c1c234c5+s1s5；r12＝-c1c234c5+s1s5；r13＝-c1s234；r21＝s1c234c5-c1s5；r22＝-s1c234c5-c1s5；r23＝-s1s234；r31＝-s234c5；r32＝s234c5；r33＝-c234；px＝c1(a2c2+a3c23-d5s234)；py＝s1(a2c2+a3c23-d5s234)；pz＝-a2s2-a3s23+d1-d5c234.

式中s234＝sin(θ2+θ3+θ4)、c234＝cos(θ2+θ3+θ4)，其它情况同理；

运动学方程为：

由(6)、(7)通过代数法求出机械臂各关节角的表达式如下：

θ1＝atan2(px,py)(8)

θ2＝atan2(s2,c2)(9)

θ5＝atan2(r11py-r21px,r12py-r22px)(12)

其中

m＝(r13px+r23py+r33(pz-d1)；

利用代数法求出五自由度机械臂的所有关节角。

基于所述方法的机械臂交互控制系统，该系统包括kinect传感器、计算机软件系统和机械臂；

kinect传感器用于人体深度图像信息的获取；

计算机软件系统运用骨骼追踪技术处理获取的景深数据，建立人体20个骨骼点的3d坐标；

机械臂的控制系统获取计算机系统转换发出的控制指令，控制末端执行器跟随手部运动，实现人手对机械臂末端执行器的实时引导。

本发明的有益效果在于：

本发明是一种新型的人机交互方式，利用kinect传感器骨骼追踪技术处理景深数据获取手部位置，构建手部相对于髋骨中心与末端执行器相对于机械臂基座的运动映射关系进行主从控制，实现人手与机器臂的实时交互。并针对手部抖动消除和异常值处理，提出了一种基于位置增量的移动平均轨迹平滑算法。实验结果表明该系统能够很好的完成手部跟踪和主从控制任务，具有较高的实时性和交互性。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

图1为本发明主从控制系统框架；

图2为本发明系统流程图；

图3为平滑处理实现过程；

图4为连杆坐标系。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

一种基于kinect的无标定人机交互控制系统，包括人体骨骼信息获取、计算机软件系统、机械臂，人体深度图像信息获取主要由kinect传感器完成。计算机软件系统运用骨骼追踪技术处理获取的景深数据，建立人体20个骨骼点的3d坐标。机械臂的控制系统获取计算机系统转换发出的控制指令，控制末端执行器跟随手部运动，实现人手对机械臂末端执行器的实时引导。

1、系统框架

整个系统可分为三大部分，即人体骨骼信息获取、计算机软件系统、机械臂。系统框图如下图1所示，系统运行框图如图2所示。

人体深度图像信息获取主要由kinect传感器完成。计算机软件系统运用骨骼追踪技术处理获取的景深数据，建立人体20个骨骼点的3d坐标。机械臂的控制系统获取计算机系统转换发出的控制指令，控制末端执行器跟随手部运动，实现人手对机械臂末端执行器的实时引导。

2、获取手部信息及主从控制方法

2.1kinect简介

kinect是微软公司开发的一款能够实时获取彩色图像和深度图像数据的设备，同时支持实时的全身和半身骨骼跟踪模式，并可以识别一系列的动作。它由rgb彩色摄像头、红外线发射器和红外线coms摄像机(ir)三部分组成。

2.2人体检测及手部信息的获取

本发明主从控制系统中，一个关键技术就是人手位姿的精确获取。目前运动目标检测和跟踪常用的方法有：帧间差分法、光流法和背景差分法。光流法的缺点是需要多次迭代才可以收敛，计算量大，难以满足实时性要求；帧差法的问题是前后帧运动重叠的区域会有空洞产生，检测效果不精确，而且阈值的选择对结果影响较大，阈值过大会形成空洞，阈值过小则会产生噪声。传统的背景差分法通过建立背景模型，将当前帧图像与背景模型相减，可以确定目标的位置以及形状信息。然而彩色图像易受光照变化、复杂背景的影响，导致目标追踪困难以及分割不精确。有一些学者利用kinect生成的深度图像上进行背景相减，得到了分割化的人体深度图像，取得很好的效果。为此，作者提出了一种利用kinect人体骨骼追踪技术非接触获取人手运动信息的方法，具体过程是：将分割化的人体深度图像传送到辨别人体部位的动作捕捉机器学习系统中，利用随机森林的分类方法设计了身体组件的中间表示,将姿势估计问题映射为逐像素分类问题。通过重投影分类器(身体组件估计)的结果，生成人体关节的可信3d估计。

本发明省去复杂的摄像头标定过程，相对于通过摄像头标定获取手部位置方法能够获得精度较高的手部位置信息。

2.3坐标数据平滑处理

由于kinect硬件的性能不稳定、操作者动作不够连贯等问题，通过上述方法估计骨骼关节点的相对位置可能在帧与帧之间变动很大，骨骼数据序列中可能含有异常值。因此，在进行逆运动学求解前必须要先对异常值予以识别和剔除，并且对骨骼数据进行降噪和平滑处理。

移动平均轨迹平滑算法的原理是将当前时刻的人手位置信息与前n-1个采样周期内的人手位置信息的平均值作为当前时刻的主从控制位置信息，并按照时间序列以步长n对kinect采集到的位置信息进行逐项推移，最终得到整个主从控制过程中期望的位置值，实现对从机械臂末端位置的控制。该算法可以滤除周期性的变动对位置曲线平滑性的影响，轨迹平滑处理的实现过程如图3所示。

移动平均轨迹平滑处理算法在消除人手周期性抖动及其他随机干扰的同时，也有可能会滤除操作者期望的位置操作信息。为了在消除人手抖动的同时，尽可能保留操作者期望的位置信息，本发明在移动平滑算法的基础上提高当前位置信息在平滑处理后位置中所占的权重。假定轨迹平滑处理算法的时间平移步长为n，i为当前位置信息在规划位置信息中的权重，则当k≥n时，k时刻的规划位置可由下式计算获得：

当k＜n时，k时刻的规划位置为：

采集到的人手空间运动位置信息中，除了人手部动作不够连贯和抖动产生的干扰，也包含随机性的异常值的干扰。由于本发明是基于位置增量的主从控制策略，因此作者在求解位置增量的过程中，不是直接对采集到的手部位置信息进行轨迹平滑处理算法，而是对相邻时刻人手位置的增量进行平滑处理。采用此方式可以更好的利用移动平均算法滤除采集到的手部位置信息的不规则变动。经验证，采用基于位置增量的移动平均轨迹平滑算法进行处理能够消除手部运动过程中大部分抖动以及异常值的干扰。

2.4手部对从动机械臂的主从控制方法

手部控制机械臂时人手的运动必须和机械臂的运动进行映射。主从控制系统中运动映射模式通常有两种，一种是关节空间映射，另一种是笛卡尔空间映射。本发明操作任务主要是针对机械臂末端位姿进行控制，因此系统采取笛卡尔空间映射模式，即：将手部的位姿作为机械臂末端执行器位姿的输入。在控制方式选择上采用增量式控制，将人手的运动位姿增量叠加到当前机器人的末端执行器的位姿上，实现手部对机械臂的实时引导控制。

人手与髋骨中心的位置信息是在kinect坐标系下，为了建立上述的运动映射关系，需将其坐标转换到机器人坐标系下，具体实现方法如下：

s31：从骨骼信息流skeletonframe提取出右手在kinect坐标系{k}下的位置矢量^kp1＝[x1,y1,z1]^t，以及髋骨中心在{k}下的位置矢量^kp2＝[x2,y2,z2]^t。由此无需对摄像头进行标定就可以得到kinect坐标系{k}下操作者右手相对于自身髋骨中心的位置^kp：

^kp＝^kp1-^kp2(3)

s32：如图1所示，骨骼跟踪坐标系都是在kinect摄像头坐标系，原点为红外摄像头中心，z轴为红外摄像头光轴，x轴方向为水平方向，y轴为垂直方向。为了得到机械臂末端相对于基座的坐标^bp，需求出kinect摄像头坐标系{k}转换到机器人坐标系{b}的旋转矩阵kinect摄像头坐标系{k}按右手定则绕z轴旋转-90°，再绕x轴旋转-90°，可得到旋转矩阵

s33：最终得到机械臂末端相对于基座的坐标^bp为：

3、机械臂逆运动学求解

本发明使用的五自由度机械臂的连杆坐标系如图4所示，d-h参数如表1所示：

表1机械臂的d-h参数表

在笛卡尔空间中，机器臂运动学逆问题是已知末端相对基座坐标系的姿态和位置、机器臂几何参数，确定其各关节变量的过程。本发明采用反变换法,即在变换矩阵前乘一个或若干个逆变换矩阵，通过比较方程两边对应元素，达到求解运动学逆解的目的。如上所述,由公式(5)可以得到机械臂末端在基坐标系下的位置矢量^bp＝[x,y,z]^t，根据表1得到，基于d-h参数机械臂末端坐标系相对于基座坐标系的齐次变换矩阵为:

其中，r11＝c1c234c5+s1s5；r12＝-c1c234c5+s1s5；r13＝-c1s234；r21＝s1c234c5-c1s5；r22＝-s1c234c5-c1s5；r23＝-s1s234；r31＝-s234c5；r32＝s234c5；r33＝-c234；px＝c1(a2c2+a3c23-d5s234)；py＝s1(a2c2+a3c23-d5s234)；pz＝-a2s2-a3s23+d1-d5c234.

式中s234＝sin(θ2+θ3+θ4)、c234＝cos(θ2+θ3+θ4)，其它情况类似。

运动学方程为：

由(6)、(7)通过代数法可以求出机械臂各关节角的表达式如下：

θ1＝atan2(px,py)(8)

θ2＝atan2(s2,c2)(9)

θ5＝atan2(r11py-r21px,r12py-r22px)(12)

其中

m＝(r13px+r23py+r33(pz-d1)。

这样，利用代数法就求出五自由度机械臂的所有关节角。由于机械臂某些关节存在对称的角度，上述方法求得机械臂末端执行器的某个位姿可能存在多组逆解的情况。然而关节运动受到自身运动范围的限制，所以某些逆解对应的位姿是不可能实现的。在对机械臂进行实际控制时，本发明利用关节限位和从最优路径的角度来对多组逆解进行了取舍。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。