CN106313049B

CN106313049B - 一种仿人机械臂体感控制系统及控制方法

Info

Publication number: CN106313049B
Application number: CN201610876399.9A
Authority: CN
Inventors: 刘世平; 赫向阳; 李世其; 付艳; 甘凯; 汤贤
Original assignee: Huazhong University of Science and Technology
Current assignee: Huazhong University of Science and Technology
Priority date: 2016-10-08
Filing date: 2016-10-08
Publication date: 2017-09-26
Anticipated expiration: 2036-10-08
Also published as: CN106313049A

Abstract

本发明公开了一种仿人机械臂体感控制系统及控制方法，引入臂型角来定义手臂肩关节、肘关节和腕关节形成的平面与参考平面的夹角大小。通过手掌和肩关节的位置差来确定手臂末端的位置，手臂末端的姿态通过手掌、拇指和手掌末端形成的平面与肩关节坐标系的相对姿态来确定。得到人体手臂的臂型角以及手臂末端的位置和姿态，就可以由控制系统计算出机械臂的七个自由度的角度，从而实现机械臂的精确控制。本发明与现有技术相比，具有以下优点：基于手臂臂型角的仿人机械臂体感的控制方法，将人体手臂的位置和姿态进行了完整的定义，避免了由于体感设备获取的骨骼数据不足，仿人机械臂自由度过多，反而导致机械臂控制不精确的缺陷。

Description

一种仿人机械臂体感控制系统及控制方法

技术领域

本发明属于机器人控制领域，更具体地，涉及一种仿人机械臂体感控制系统及控制方法。

背景技术

多数交互设备需要操作者掌握专业知识才能进行相应的交互操作。而体感技术降低了操作者对外设专业知识的掌握，简化了学习和训练的过程，依靠实时肢体(手势)识别、面部识别及语音识别就可以实现对机器的直接控制。用这种最自然的方式与机器进行交互，可以减轻操作者的负荷。自然的人机交互方式，越来越受到重视。

通常，基于体感设备的控制方法采用可穿戴设备来采集骨骼信息，这种设备穿戴繁琐，降低了体感控制的可操作性。目前获取的人体骨骼信息后，大多通过空间向量法计算人体关节角，直接映射到机器人关节角用于控制机器人。但是这种控制方法，由于获取关节数量较少，计算自由度多，操作复杂，控制不精确，缺乏对机器人的完整描述。

发明内容

针对现有技术的上述问题，本发明旨在克服传统机械臂控制方法操作复杂的缺陷，减少控制机器人所需计算和输出的自由度，使机器人控制更加智能、准确和快速。

为了实现上述目的，本发明提供了一种仿人机械臂体感控制系统，包括：体感检测设备、机械臂控制器、机械臂和数据交互平台；体感检测设备的输出端连接数据交互平台的输入端，数据交互平台与机械臂控制器双向连接，机械臂控制器的输出端连接机械臂；

体感检测设备，用于提取人体的肩关节中点B、肩关节位置S、肘关节位置E、腕关节位置W、手掌位置H、手末端位置H_T和拇指位置T；

机械臂，具有对应模拟人体的肩关节中点B'、肩关节位置S'、肘关节位置E'、腕关节位置W'、手掌位置H'；1号关节位于B'和S'之间，1号连杆连接1号关节和S'，2号连杆连接S'和3号关节，3号连杆连接3号关节和E'，4号连杆连接E'和5号关节，5号连杆连接5号关节和W'，6号连杆连接W'和7号关节，7号连杆连接7号关节和H'；S'、E'、W'分别是2号、4号、6号关节；

数据交互平台，用于接收和处理体感检测设备上传的人体各位置数据，并据此计算出人体臂型角ψ、手掌位置^BP_H、手掌姿态^BR_H，从而根据ψ、^BP_H、^BR_H结合机械臂的运动参数计算出机械臂的角度θ_i，θ_i表示i号连杆绕i号关节轴线的转角，i＝1,2,…,7；数据交互平台还用于将θ_i发送给机械臂控制器；

机械臂控制器用于将θ_i转换成运动指令以控制机械臂的运动并将机械臂的运动信息反馈给数据交互平台。

进一步地，还包括虚拟场景显示控制台，虚拟场景显示控制台连接在数据交互平台和体感检测设备之间，用于模拟机械臂所处环境以及机械臂的运行结果并根据模拟结果决定机械臂是否执行数据交互平台的运动指令。

进一步地，数据交互平台用于按照如下方式确定臂型角ψ：

将B、S、W三点确定的平面作为参考平面，将S、W、E三点确定的平面作为臂平面，计算参考平面和臂平面的夹角，记为臂型角ψ。

进一步地，数据交互平台用于按照如下方式确定手掌位置^BP_H：

根据B点和H点的坐标，得出手掌相对肩关节中点的位置^BP_H，

^BP_H＝[(x_H-x_B),(y_H-y_B),(z_H-z_B)]

其中，x_H、y_H、z_H是H点的坐标，x_B、y_B、z_B是B点的坐标。

根据腕关节W和手掌位置H的坐标，求得向量该方向记为Z_H；手掌点H、拇指点T和手掌末端H_T三点构成一个平面，X_H垂直于该平面；Y_H是根据Z_H和X_H由笛卡尔坐标系确定；

手臂末端的姿态描述为^BR_H＝[X_H,Y_H,Z_H]。

为了实现上述目的，本发明还提供了一种用于上述仿人机械臂体感控制系统的控制方法，包括如下步骤：

(1)人体臂型角ψ、手掌位置^BP_H、手掌姿态^BR_H的读取及确认

体感检测设备提取人体上B、S、E、W、H、H_T和T七个位置并上传给数据交互平台，在数据交互平台中进行如下定义和计算：

将B、S、W三点确定的平面作为参考平面，将S、W、E三点确定的平面作为臂平面，计算参考平面和臂平面的夹角，记为臂型角ψ；

根据B点和H点的坐标，得出手掌相对肩关节中点的位置^BP_H，

^BP_H＝[(x_H-x_B),(y_H-y_B),(z_H-z_B)]

其中，x_H、y_H、z_H是H点的坐标，x_B、y_B、z_B是B点的坐标；

手臂末端的姿态描述为^BR_H＝[X_H,Y_H,Z_H]。

(2)数据交互平台根据人体臂型角ψ、手掌位置^BP_H、手掌姿态^BR_H结合机械臂的运动学参数计算机械臂的角度θ_i，θ_i表示i号连杆绕i号关节轴线的转角，i＝1,2,…,7；

(3)数据交互平台将θ_i输入到机械臂控制器，并由机械臂控制器控制机械臂运动，i＝1,2,…,7。

进一步地，还包括虚拟场景显示控制台，在步骤(2)和步骤(3)之间增加如下判定步骤：

在虚拟场景显示控制台中模拟机械臂所处环境，以及根据步骤(2)中得到的θ_i模拟机械臂的运行结果，i＝1,2,…,7，

如果模拟结果为机械臂运行有障碍，则虚拟场景显示控制台反馈障碍信息，提示捕捉对象变更动作，返回步骤(1)重新从步骤(1)开始执行；

如果模拟结果为机械臂正常运行，则执行步骤(3)。

进一步地，步骤(2)中，θ_i的获得方法如下：

(a)坐标系的建立

以i号关节+1的轴线为Z_i轴，平行于i号关节的轴线的方向为Y_i轴建立关于i号连杆的i号坐标系，i＝1,2,…,6；

以B'为原点，1号关节的轴线为Z₀轴，平行于1号关节的轴线的方向为Y₀轴建立0号坐标系作为基坐标系；

以H'为原点，7号关节的轴线为Z₀轴，平行于6号关节的轴线的方向为Y₇轴建立7号坐标系作为机械臂末端坐标系；X_i是根据Z_i和Y_i由右手定则确定，i＝0,1,…,7；

(b)根据捕捉得到的人体手掌位置^BP_H、人体手掌姿态^BR_H得到基坐标系中机械臂末端位置⁰P₇和机械臂末端姿态⁰R₇；由人体臂型角ψ得到机械臂的臂型角ψ′，ψ′＝ψ；

(c)θ₄求解

S'、E'、W'在初始状态下的位置用S₀'、E₀'和W₀'表示，与的夹角即为肘部关节角θ₄；

由于θ₄在机械臂的臂平面上，θ₄与ψ的大小无关：

其中，

表示初始状态下和的夹角，

表示在t时刻时和的夹角；

(d)θ₁、θ₂和θ₃求解

3号坐标系相对于基坐标系的姿态⁰R₃的第一个表达式如下：

其中，

⁰R_ψ-机械臂的臂平面相对于机械臂的参考平面的空间向量关系的旋转矩阵；

-当3号坐标系的X₃O₃Y₃平面与参考平面重合时，3号坐标系相对于基座标系的旋转矩阵；

而根据正运动学方程可得⁰R₃用θ₁、θ₂和θ₃来表达的第二个表达式如下：

式中，s_i＝sinθ_i，c_i＝cosθ_i，i＝1、2、3；

方程(13)(14)右侧的对应项相等，求出θ₁、θ₂和θ₃；

(e)θ₅、θ₆和θ₇求解；

4号坐标系相对于7号坐标系的姿态⁷R₄有如下两种表达式：

式中，s_i＝sinθ_i，c_i＝cosθ_i，i＝5、6、7；

-机械臂的末端位置⁰P₇的转置矩阵；

³R₄-4号坐标系相对于3号坐标系的旋转矩阵；

方程(15)(16)右侧的对应项相等，求出θ₅、θ₆和θ₇。

本发明提出的体感控制仿人机械臂的控制系统和控制方法与现有技术相比，具有以下优点：基于手臂臂型角的仿人机械臂体感的控制方法，将人体手臂的位置和姿态进行了完整的定义，避免了由于体感设备获取的骨骼数据不足，仿人机械臂自由度较多，机械臂控制不精确的缺陷。

附图说明

图1是体感设备获取的人体骨骼位置点；

图2是人体骨骼信息基坐标系示意图；

图3是人体手臂臂型角的定义示意图；

图4是人体手掌坐标系的定义示意图；

图5是S-R-S仿人机械臂构型及7个自由度示意图；

图6是机械臂臂型角定义示意图；

图7是机械臂DH坐标系示意图；

图8是机械臂的臂型角计算示意图；

图9是仿人机械臂体感控制系统示意图；

图10是仿人机械臂控制流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

1、人体骨骼信息的获取

人体骨骼信息的获取方式通常为基于视觉识别测量或者通过佩戴测量节点来实现。Kinect V2是微软开发的新一代3D体感摄像机，用于体感游戏和科研人员开发。KinectSDK包含两个重要的API，分别是NUI API(自然用户界面)和Audio API。UNI API是KinectAPI的核心，实现从图像传感器中提取数据并控制Kinect设备。Kinect通过红外线发生装置发射红外线，并探测红外光反射，从而可以计算出视场范围内每一个像素的深度值，即获得深度数据，从深度数据中最先提取出来的是物体主体和形状。然后用这些形状信息来匹配人体的各个部分，最后计算匹配出来的各个关节在人体中的位置，Kinect获取的人体骨骼信息如图1所示。

这些关节点的三维坐标可以通过Skeleton的Joints属性得到。通过Joints属性获得的关节三维坐标，坐标系方向如图2所示，Z向表示Kinect传感器的方向。在控制机械臂运动过程中，只提取主要的7个关节信息，分别是肩关节中点B，肩关节位置S，肘关节位置E，腕关节位置W，手掌位置H，手末端位置H_T和拇指位置T。

2、臂型角参数化的人体骨骼信息

人体在自然状态下的一种姿态如图3所示。图中点B，S，E，W和H分别表示颈部，肩关节，肘关节，腕关节和手掌的位置。从当前的姿态可以发现，肩关节S、肘关节E和腕关节W可以构成一个平面。人体手臂在运动过程中，该平面会随着手臂姿态变化。通过描述该平面的位置，结合手掌的位置，可以准确的表达手臂的运动状态。

为描述肩关节、肘关节和腕关节构成的平面的运动，引入人体手臂的臂型角定义。肩关节、肘关节和腕关节可以构成一个平面，定义这个平面为臂平面。颈部和肩关节形成的向量记为肩关节和腕关节形成的向量记为定义向量和形成的平面为参考平面。臂平面与参考平面生成的夹角，定义为臂型角，记作ψ。通过臂型角ψ可以描述肘关节绕方向的旋转运动。

如图3，已知S点、E点和W点的坐标，即可求得E点到直线SW的垂足O点的坐标，这样向量就可以确定。已知B点和S点的坐标，可得向量将向量平移并通过O点，可得向量向量和向量的夹角即为手臂臂型角ψ，则

已知B点和H点的坐标，可以求得手相对肩关节中点的位置，记为^BP_H，

^BP_H＝[(x_H-x_B),(y_H-y_B),(z_H-z_B)]

式中，x_H，y_H和z_H为手的坐标，x_B，y_B和z_B为肩关节中心的坐标。

^BP_H描述了手臂末端相对于肩部中心的位置，缺少对手臂末端姿态的描述。根据手掌，手末端和拇指的位置关系，定义手掌的坐标系如图所示。其中，根据已知腕关节W和手掌位置H的坐标，可以求得向量该方向记为Z_H。手掌点H，拇指点T和手掌末端H_T可以构成一个平面，X_H垂直于该平面，Y_H是根据Z_H和X_H由笛卡尔坐标系确定。

手臂末端的姿态可以描述为^BR_H＝[X_H,Y_H,Z_H]。

这样，基于臂型角的人体手臂骨骼，通过臂型角和手臂末端坐标系完整的描述出来了。运用手臂臂型角ψ和手掌位置^BP_H和姿态^BR_H，即可计算本发明所针对的仿人机械臂控制所需的七个自由度，将这七个自由度输入到仿人机械臂运动学模型中可以控制机械臂的运动。

3、臂型角参数化的仿人机械臂

仿人机械臂采用串联球形关节-旋转关节-球形关节的形式(简称S-R-S)，上述三个关节互相垂直相交于一点，形成球铰，机械臂的肩关节、肘关节、腕关节(如图5所示)，即采用这种S-R-S形式的球铰构型。这种构型能够最大程度的模拟人体的手臂，根据这种构型设计了人形机械臂。在本实施例中，为了增加机械臂的运动范围，肘关节设计有一定的位置偏置。

机械臂在工作过程中的一种姿态可以用图6表示，其中等效的肩关节中点、肩关节、肘关节、腕关节和末端分别记为B'、S'、E'、W'和H'。1号关节位于B'和S'之间，1号连杆连接1号关节和S'，2号连杆连接S'和3号关节，3号连杆连接3号关节和E'，4号连杆连接E'和5号关节，5号连杆连接5号关节和W'，6号连杆连接W'和7号关节，7号连杆连接7号关节和H'；S'、E'、W'分别是2号、4号、6号关节。

由此，已可以定义出能够准确、完整地描述该仿人机械臂运动形态的自由度角度θ_i(i＝1,2,…,7)，θ_i表示i号连杆绕i号关节轴线的转角。

根据上述S-R-S构型的仿人机械臂，可以构建与人体手臂等效的机械臂运动学求解算法。定义机械臂的臂型角如下，机械臂等效的肩关节S'、肘关节E'和腕关节W'构成的平面称为臂平面，机械臂等效的肩关节中点B'、肩关节S'和腕关节W'构成的平面称为参考平面，臂平面与参考平面形成的角度称为机械臂的臂型角ψ′，ψ′＝ψ且ψ′取值范围与ψ一样为[-π,π]。在后文的计算中，将直接用ψ来替代ψ′进行计算。

如图8已知仿人机械臂末端的位置和姿态(前面已由体感设备读取人体动作经映射得到)，可以得到肩关节S'到腕关节W'的向量初始状态用S₀'、E₀'和W₀'表示，t时刻的状态用S'、E'和W'表示。则与的夹角即为肘部关节角θ₄，由于θ₄在机械臂的臂平面上，θ₄不会引起ψ的变化，ψ也不会引起θ₄的变化，所以θ₄与ψ无关，根据余弦定理可以求得初始状态下和的夹角以及t时刻时和的夹角则肘关节的角度值为

已知臂型角ψ，可以得到臂平面相对于参考平面的空间向量关系的旋转矩阵，记为⁰R_ψ。同样，当3号坐标系的X₃O₃Y₃平面与参考平面重合时，可以得到3号坐标系相对于基座标系的旋转矩阵，记为当3号坐标系的X₃O₃Y₃平面与参考平面不重合时，根据所求的臂平面相对于参考平面的旋转矩阵和3号坐标系相对于基座标系的旋转矩阵，由此3号坐标系相对于基坐标系的姿态⁰R₃可以表示为⁰R_ψ与的点乘乘积。而根据机械臂正向运动学，同样可以获得3号坐标系相对于基坐标系的旋转矩阵⁰R₃的以θ₁、θ₂和θ₃来表达的另一种表达式。两种不同表达式的对应项相等，可以求得关节角θ₁、θ₂和θ₃。

同理，可以构建4号坐标系相对于7号坐标系的旋转矩阵⁷R₄。而根据机械臂正向运动学，也可以获得4号坐标系相对于7号坐标系的旋转矩阵⁷R₄的以θ₅、θ₆和θ₇来表达的另一种表达式，⁷R₄的两个不同的旋转矩阵表达式对应项相等，可以计算得到θ₅、θ₆和θ₇的值。至此，已经获得仿人机械臂的全部七个运动学解。图9、图10分别是仿人机械臂体感控制系统示意图和仿人机械臂控制流程示意图。

更具体的计算过程如下所示。

1、建立机械臂的数学模型

根据机械臂本体的结构构型，建立相对应的DH(Denavit-Hartenberg)坐标系。机械臂的数学模型及DH坐标系如图6所示。

如图7，以i号关节+1的轴线为Z_i轴，平行于i号关节的轴线的方向为Y_i轴建立关于i号连杆的i号坐标系(i＝1～6)；以B'为原点，1号关节的轴线为Z₀轴，平行于1号关节的轴线的方向为Y₀轴建立0号坐标系作为基坐标系(又称世界坐标系)；以H'为原点，7号关节的轴线为Z₀轴，平行于6号关节的轴线的方向为Y₇轴建立7号坐标系作为机械臂末端坐标系。X_i是根据Z_i和Y_i由右手定则确定，i＝0,1,…,7。

通过建立以上坐标系，定义如下参数：

a_i等于Z_i-1和Z_i之间沿X_i方向的距离，i＝1～7；

d_i等于X_i-1和X_i之间沿Z_i方向的距离，i＝1～7。

DH坐标系建立以后，a_i和d_i即为已知。

在本实施例中，由于机械臂上1号连杆、2号连杆、2号关节没有偏置，所以反应在DH坐标系中，Z₁、Y₂均与2号关节的轴线重合，1号坐标系与2号坐标系的原点重合，即a₁、a₂＝0，d₂＝0。同理，a₅、a₆、a₇＝0，d₂、d₄、d₆＝0。

2、求解θ₄

在已知基坐标系中机械臂的末端位置⁰P₇时(由人体的^BP_H直接映射得到)，S'点、W'点以及向量的值是唯一的。根据前面的定义，实际上1号关节轴线的方向与肩关节S'和腕关节W'方向形成的平面即为机械臂的参考平面。

已知基坐标系中机械臂末端位姿⁰P₇和⁰R₇(分别由人体的^BP_H和^BR_H映射得到)，向量可以表示为：

请参考图7、8，任意状态下，肩部、肘部和腕部的几何关系可以用图8表示。其中虚线部分为机械臂的初始状态，初始状态用S₀'、E₀'和W₀'表示。实线部分为机械臂在t时刻的状态，用S'、E'和W'表示。图中与的夹角即为肘部关节角θ₄。

由于θ₄在机械臂的臂平面上，θ₄不会引起ψ的变化，ψ也不会引起θ₄的变化，所以θ₄与ψ无关：

其中，

表示初始状态下和的夹角；

表示在t时刻时和的夹角。

由图8中的几何关系可知，

S₀W₀＝d₃+d₅，

在本实施例中，由图8可以看出，初始状态下，θ₄＝0，机械臂伸直。

3、求解θ₁、θ₂、θ₃、θ₅、θ₆、θ₇

请参考图7，在基坐标系中，臂平面相对于参考平面的旋转矩阵为：

式中，I₃为3×3的单位阵，为向量的叉乘因子。

当3号坐标系的X₃O₃Y₃平面与参考平面重合时，ψ＝0，3号坐标系与基座标系之间的空间向量关系用来表示，⁰x₃，⁰y₃和⁰z₃分别表示3号坐标系的三个方向向量：

向量l是垂直于参考平面的向量：

由于肘关节偏置a₃的存在，⁰y₃相对于基坐标系的角度γ实际上由两部分组成，分别是转动形成的角度α和偏置产生的角度β：

γ＝α+β (8)

⁰y₃相对于基坐标系的旋转矩阵为：

R(l,γ)＝I₃+[l^×]sin(γ)+[l^×]²(1-cos(γ)) (9)

式中，I₃为3×3的单位阵，l^×为向量l的叉乘因子

⁰x₃＝⁰y₃×⁰z₃ (11)

根据公式(5)(10)(11)可得：

则当3号坐标系与参考平面不重合时，3号坐标系相对于基坐标系的姿态可以表示为：

而根据正运动学方程可得⁰R₃的另一种表达式如下：

式中，s_i＝sinθ_i，c_i＝cosθ_i，i＝1、2、3。

由于方程(13)(14)右侧的对应项相等，即可据此求出θ₁、θ₂和θ₃。

同理，4号坐标系相对于7号坐标系的姿态有如下两种表达方式：

式中，s_i＝sinθ_i，c_i＝cosθ_i，i＝5、6、7；

-机械臂的末端位置⁰P₇的转置矩阵；

³R₄-4号坐标系相对于3号坐标系的旋转矩阵。

由于方程(15)(16)右侧的对应项相等，即可据此求出θ₅、θ₆和θ₇。

按照以上计算过程，根据体感控制方法获取的人体手臂臂型角ψ和手掌位置^BP_H、姿态^BR_H，输入到仿人机械臂的运动学参数中按照相应公式进行计算，可以获得机械臂对应姿态下的运动学解，即获得此状态下的7个关节角度θ₁～θ₇，如图5。这种仿人机械臂体感控制方法和对应的仿人机械臂的算法结合，能够最大程度的模拟人体手臂的姿态从而实现运动控制。

4具有虚拟场景显示的控制平台

本实施例的机械臂体感控制平台，基于MFC开发。Kinect V2 SDK提供了基于VC++的开发接口，通过调用SDK的函数实现获取人体骨骼数据。遥操作开始和停止功能通过体感来控制，当系统处于待机状态时，通过体感捕捉人体手臂动作，当手臂动作符合预定动作后，遥操作端会发出系统开始体感控制指令。在人体手臂动作后，向仿人机械臂系统发送相应的动作指令。机械臂控制都是通过左臂来完成，当右臂向上举起时，系统判断为停止。

机械臂体感控制平台的虚拟模型显示，通过OpenInvertor开发包来实现。OpenInvertor通过建立节点信息来更新虚拟场景，各节点的位置信息通过机械臂的连杆的相对位置矩阵来获得。

机械臂体感控制平台的虚拟模型，还具有碰撞检测的功能。安全预警模型旨在预测机器人的运动情况，通过预测，可以预先知道机器人哪些动作是安全的，哪些动作是不安全的，从而指导用户快速实时地做出合理的决策，提高交互效率。安全预警算法是建立在虚拟现实技术上的一套安全保障算法，在位置信息可以预测的情况下，运用碰撞检测算法等手段进行安全预测。在虚拟场景中，计算机可以根据虚拟场景提供的机器人以及环境物体的位置信息，预测将要发生的碰撞，从而及早提醒操作者进行相应策略的调整。

5基于EntherCAT总线通信方式

所述机械臂体感控制平台通过EntherCAT总线方式与仿人机械臂的控制系统进行通信。使用倍福公司开发的TwinCAT软件作为开发包，在遥操作端进行总线通信。经过TwinCAT封装的EntherCAT通信方式，通过调用ADSnet来实现。经过ADS通信的方式，通信周期可以缩小到1ms，提高了体感控制系统的跟随速度。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims

1.一种仿人机械臂体感控制系统，其特征在于，包括：体感检测设备、机械臂控制器、机械臂和数据交互平台；体感检测设备的输出端连接数据交互平台的输入端，数据交互平台与机械臂控制器双向连接，机械臂控制器的输出端连接机械臂；

2.如权利要求1所述的一种仿人机械臂体感控制系统，其特征在于，还包括虚拟场景显示控制台，虚拟场景显示控制台连接在数据交互平台和体感检测设备之间，用于模拟机械臂所处环境以及机械臂的运行结果并根据模拟结果决定机械臂是否执行数据交互平台的运动指令。

3.如权利要求1所述的一种仿人机械臂体感控制系统，其特征在于，数据交互平台用于按照如下方式确定臂型角ψ：

4.如权利要求1所述的一种仿人机械臂体感控制系统，其特征在于，数据交互平台用于按照如下方式确定手掌位置^BP_H：

根据B点和H点的坐标，得出手掌相对肩关节中点的位置^BP_H，

^BP_H＝[(x_H-x_B),(y_H-y_B),(z_H-z_B)]

其中，x_H、y_H、z_H是H点的坐标，x_B、y_B、z_B是B点的坐标。

5.如权利要求1所述的一种仿人机械臂体感控制系统，其特征在于，数据交互平台用于按照如下方式确定手掌位置^BP_H：

手臂末端的姿态描述为^BR_H＝[X_H,Y_H,Z_H]。

6.一种用于权利要求1所述仿人机械臂体感控制系统的控制方法，其特征在于，包括如下步骤：

(1)人体臂型角ψ、手掌位置^BP_H、手掌姿态^BR_H的读取及确认

根据B点和H点的坐标，得出手掌相对肩关节中点的位置^BP_H，

^BP_H＝[(x_H-x_B),(y_H-y_B),(z_H-z_B)]

其中，x_H、y_H、z_H是H点的坐标，x_B、y_B、z_B是B点的坐标；

手臂末端的姿态描述为^BR_H＝[X_H,Y_H,Z_H]；

7.如权利要求6所述的用于仿人机械臂体感控制系统的控制方法，其特征在于，还包括虚拟场景显示控制台，在步骤(2)和步骤(3)之间增加如下判定步骤：

如果模拟结果为机械臂正常运行，则执行步骤(3)。

8.如权利要求6或7所述的用于仿人机械臂体感控制系统的控制方法，其特征在于，步骤(2)中，θ_i的获得方法如下：

(a)坐标系的建立

以i+1号关节的轴线为Z_i轴，平行于i号关节的轴线的方向为Y_i轴建立关于i号连杆的i号坐标系，i＝1,2,…,6；

以H'为原点，7号关节的轴线为Z₀轴，平行于6号关节的轴线的方向为Y₇轴建立7号坐标系作为机械臂末端坐标系；

X_i是根据Z_i和Y_i由右手定则确定，i＝0,1,…,7；

(c)θ₄求解

由于θ₄在机械臂的臂平面上，θ₄与ψ的大小无关：

<mrow> <msub> <mi>&theta;</mi> <mn>4</mn> </msub> <mo>=</mo> <msubsup> <mi>&theta;</mi> <mn>4</mn> <mn>0</mn> </msubsup> <mo>-</mo> <msubsup> <mi>&theta;</mi> <mn>4</mn> <mi>t</mi> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>

其中，

表示初始状态下和的夹角，

表示在t时刻时和的夹角；

(d)θ₁、θ₂和θ₃求解

<mrow> <msub> <mmultiscripts> <mi>R</mi> <mn>0</mn> </mmultiscripts> <mn>3</mn> </msub> <mo>=</mo> <msub> <mmultiscripts> <mi>R</mi> <mn>0</mn> </mmultiscripts> <mi>&psi;</mi> </msub> <mo>&CenterDot;</mo> <mmultiscripts> <mi>R</mi> <mn>3</mn> <mrow> <mi>&psi;</mi> <mo>=</mo> <mn>0</mn> </mrow> <mn>0</mn> </mmultiscripts> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>

其中，

式中，s_i＝sinθ_i，c_i＝cosθ_i，i＝1、2、3；

方程(13)(14)右侧的对应项相等，求出θ₁、θ₂和θ₃；

(e)θ₅、θ₆和θ₇求解；

4号坐标系相对于7号坐标系的姿态⁷R₄有如下两种表达式：

<mrow> <msub> <mmultiscripts> <mi>R</mi> <mn>7</mn> </mmultiscripts> <mn>4</mn> </msub> <mo>=</mo> <mmultiscripts> <mi>R</mi> <mn>7</mn> <mi>T</mi> <mn>0</mn> </mmultiscripts> <mo>&CenterDot;</mo> <msub> <mmultiscripts> <mi>R</mi> <mn>0</mn> </mmultiscripts> <mi>&psi;</mi> </msub> <mo>&CenterDot;</mo> <mmultiscripts> <mi>R</mi> <mn>3</mn> <mrow> <mi>&psi;</mi> <mo>=</mo> <mn>0</mn> </mrow> <mn>0</mn> </mmultiscripts> <mo>&CenterDot;</mo> <msub> <mmultiscripts> <mi>R</mi> <mn>3</mn> </mmultiscripts> <mn>4</mn> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>

式中，s_i＝sinθ_i，c_i＝cosθ_i，i＝5、6、7；

-机械臂的末端位置⁰P₇的转置矩阵；

³R₄-4号坐标系相对于3号坐标系的旋转矩阵；

方程(15)(16)右侧的对应项相等，求出θ₅、θ₆和θ₇。