CN103112007A

CN103112007A - 基于混合传感器的人机交互方法

Info

Publication number: CN103112007A
Application number: CN201310047285XA
Authority: CN
Inventors: 张平; 杜广龙; 卢晓敏
Original assignee: South China University of Technology SCUT
Current assignee: South China University of Technology SCUT
Priority date: 2013-02-06
Filing date: 2013-02-06
Publication date: 2013-05-22
Anticipated expiration: 2033-02-06
Also published as: CN103112007B

Abstract

本发明提供了基于混合传感器的人机交互方法，包括步骤：（1）实时获取并处理操作员通过手部运动进行机器人操作任务过程中的RGB图像和深度图像，得到操作员的手部位置；（2）使用惯性测量仪测量操作员的手部姿态，并结合四元数算法(FQA)和卡尔曼滤波（KFs）对姿态数据进行估计，从而得到稳定的操作员的手部姿态；（3）根据上述步骤获取的操作员手部位姿驱动机器人。本发明允许操作者通过三维的手部动作来控制机器人。本发明允许操作员使用手部和手臂的动作控制机器人而没有物理装置的限制，只需要把精力集中在手头的任务也不用把任务都分解为那些有限的指令。

Description

基于混合传感器的人机交互方法

技术领域

本发明属于机器人运动领域，特别涉及一种基于混合传感器的人机交互方法。

背景技术

当人类不适合出现在机器人现场，在非结构化动态环境中利用操作员操纵远程机器人是十分必要的。为达成目标操作任务，遥操作使用的机械和其他接触接口需要不自然的人类动作，它们往往会妨碍人体动作。现有的基于视觉的人机交换方式要么只用到人手的少数自由度，要么需要操作者做大幅度运动，操作起来非常费力。这篇发明提出了一种不接触的基于多传感器的机器人遥操作方法，它允许操作者通过自然的人手三维动作控制六自由度虚拟机器人来完成操作任务。基于多传感器的人机接口帮助操作者使用人手动作控制机器人和了解机器人动作和周围环境。

通常使用的遥操作机械式人机接口包括了多种操作员佩戴的装置，比如骨骼式机械装置，工具手套，电磁或惯性动作跟踪传感器，或者肌电灵敏度传感器。然而，这些佩戴的装置，传感器还有它们的连线可能妨碍操作员灵活的动作。其他接触式的接口比如仿机器人机械手，转盘，摇杆，电脑鼠标和计算机图形界面等都需要操作员不自然的动作或者需要操作者学习如何操作。基于视觉的交互技术使用了手势识别和语音识别结合的控制方式，它允许操作者更自然直观的给出命令而没有物理装置上的限制。然而在这种控制下，被控的机器人的动作都是由一些简单的命令组成，比如确定末端执行器的位置和方位，抓取还有一系列更小的任务如上移下移，旋转，暂停，继续还有改变操作的模式。当要求的机器人操作器动作的复杂度比较高的时候，这个技术都会难以操作。单摄像头的手部动作跟踪已经被应用在机器人遥操作和遥操作的模拟上。然而，动作命令只局限在真正的末端执行器自由度的子集里。此外，那些与特定的操作任务无关的手势被用来改变机器人的操作模式，所以这些都造成了操作任务的交流不自然。理想地，一种自然的不用将复杂的任务分解为有限指令和没有接触式物理装置限制的方法被人们所需要。

发明内容

这个发明提出了一种基于混合传感器的人机交互方法，允许操作者通过三维的手部动作来控制机器人。这个方法使用了非接触式的基于基于混合传感器的人机接口，它可以通过操作者动作控制机器人还有反馈机器人动作和机器人与周围环境物体的交互的情况，具体技术方案如下。

基于混合传感器的人机交互方法，其包括如下步骤：

S1、实时获取并处理操作员通过手部运动进行机器人操作任务过程中的RGB图像和深度图像，得到操作员的手部位置；

S2、使用惯性测量仪测量操作员的手部姿态，并结合四元数算法(FQA)和卡尔曼滤波（KFs）对姿态数据进行估计，从而得到稳定的操作员的手部姿态；

S3、根据上述步骤获取的操作员手部位姿驱动机器人。

进一步的，所述步骤S1包括以下步骤：

操作员通过手部运动进行机器人操作任务时，由固定在操作员前面的Kinect捕获RGB图像和深度图像，连续处理该过程中的RGB图像和深度图像实现人手跟踪和定位；Kinect有三种自动对焦摄像头：两个优化深度检测的红外摄像头和一个标准的用于视觉识别的视觉光谱摄像头；

从深度图像中提取人体骨架关节点；所提取出来的这十五个关节点如图1所示，从上到下和从左到右排序为：（1）头；（2）肩中心；（3）右肩；（4）右手肘；（5）右手腕；（6）左肩；（7）左手肘；（8）左手腕；（9）髋关节中心；（10）右髋；（11）右膝；（12）右脚；（13）左髋；（14）左膝；（15）左脚；这十五个关节点的坐标被称为Kinect坐标；如图2所示，操作员右手的位置可定义为右手腕P₅的位置，但这样直接利用P₅的位置信息来控制机器人可能导致机器人的位置受操作员身体移动或颤抖的影响；为了解决这个问题，将肩关节P₃作为参考点并构建参考点

的右手框架；

相对于参考点

的P₅位置P′₅为：

P′₅=P₅-P₃ (1)

定义右手相对于右肩的移动范围为

W_{RH}^{1} < x < W_{RH}^{2},

H_{RH}^{1} < y < H_{RH}^{2},

L_{RH}^{1} < z < L_{RH}^{2},

而机器人的移动范围为

W_{RR}^{1} < x < W_{RR}^{2},

H_{RR}^{1} < y < H_{RR}^{2},

L_{RR}^{1} < z < L_{RR}^{2},

机器人移动范围的映射点为：

\{\begin{matrix} R_{x}^{R} = \frac{(P_{8_x}^{'} - W_{RH}^{1})}{(W_{RH}^{2} - W_{RH}^{1})} \cdot (W_{RR}^{2} - W_{RR}^{1}) + W_{RR}^{1} \\ R_{y}^{R} = \frac{(P_{8_y}^{'} - H_{RH}^{1})}{(H_{RH}^{2} - H_{RH}^{1})} \cdot H_{RR}^{2} - H_{RR}^{1} + H_{RR}^{1} \\ R_{z}^{R} = \frac{(P_{8_z}^{'} - L_{RH}^{1})}{(L_{RH}^{2} - L_{RH}^{1})} \cdot (L_{RR}^{2} - L_{RR}^{1}) + L_{RR}^{1} \end{matrix} - - - (2)

所述步骤S2包括以下步骤：

操作员通过手部运动进行机器人操作任务时，右手拿着一个惯性测量仪，这个惯性测量仪可以确定操作员右手的滚动、俯仰和偏转姿态；惯性测量仪由一个三轴加速计，两个两轴陀螺仪和一个三轴磁力计组成；

定义三个坐标系：操作员本体坐标系x_by_bz_b，惯性测量仪坐标系x_sy_sz_s和地球坐标系x_ey_ez_e；惯性测量仪坐标系x_sy_sz_s对应三个正交安装的加速计和磁力计的轴；由于惯性测量仪被刚性地安装在机器人末端上，假定操作员本体坐标系x_by_bz_b与惯性测量仪坐标系x_sy_sz_s结合；地球固定坐标系x_ey_ez_e遵循北-东-下(NED)公约，其中x_e指向北，y_e指向东而z_e指向下；惯性测量仪能测量其自身的滚动、俯仰和偏转姿态；定义绕x_e的旋转φ表示滚动，绕y_e轴的旋转θ表示俯仰而绕z_e轴的旋转ψ表示偏转；根据欧拉旋转定理，从欧拉角φ、θ和ψ到四元数q的转换为：

q = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}] = [\begin{matrix} \cos (φ / 2) \cos (θ / 2) \cos (ψ / 2) + \sin (φ / 2) \sin (θ / 2) \sin (ψ / 2) \\ \sin (φ / 2) \cos (θ / 2) \cos (ψ / 2) - \cos (φ / 2) \sin (θ / 2) \sin (ψ / 2) \\ \cos (φ / 2) \sin (θ / 2) \cos (ψ / 2) + \sin (φ / 2) cso (θ / 2) \sin (ψ / 2) \\ \cos (φ / 2) \cos (θ / 2) \sin (ψ / 2) - \sin (φ / 2) \sin (θ / 2) \cos (ψ / 2) \end{matrix}] - - - (3)

而四个欧拉参数的约束为：

q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2} = 1 - - - (4)

这里q₀是标量部分而(q₁,q₂,q₃)是矢量部分；所以从惯性测量仪坐标系到地球固定坐标系的姿态余弦矩阵可以被表示为：

M_{s}^{e} = [\begin{matrix} q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2 (q_{1} q_{2} - q_{0} q_{3}) & 2 (q_{0} q_{2} + q_{1} q_{3}) \\ 2 (q_{1} q_{2} + q_{0} q_{3}) & q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} & 2 (q_{2} q_{3} - q_{0} q_{1}) \\ 2 (q_{1} q_{3} - q_{0} q_{2}) & 2 (q_{0} q_{1} + q_{2} q_{3}) & q_{0}^{2} - q_{1}^{2} - q_{2}^{2} - q_{3}^{2} \end{matrix}] - - - (5);

由于陀螺仪和磁力计都有白噪声和随机游走，为了得到稳定的数据，利用卡尔曼滤波从一组嘈杂和不完整的测量中估计惯性测量仪的状态；从时刻k-1的惯性测量仪状态x_k-1估计时刻k的惯性测量仪状态x_k的转换方程为：

x_k=A_k·x_k-1+B·u_k-1+w_k-1 (6)

z_k=H·x_k+v_k

这里x_k是n×1的状态向量，代表时刻k的惯性测量仪状态；A是n×n的状态转换矩阵；B是n×p的***输入矩阵；u_k-1是p×1的k-1时刻确定性输入向量，p为确定性输入的个数；w_k-1是k-1时刻的n×1过程噪声向量；z_k是k时刻的m×1测量向量；H是m×n的观察矩阵而v_k是m×1的测量噪声向量；此处n=7，m=6；与时间相关的四元数微分方程为：

[\begin{matrix} {&PartialD; q}_{0} / &PartialD; t \\ {&PartialD; q}_{1} / &PartialD; t \\ {&PartialD; q}_{2} / &PartialD; t \\ {&PartialD; q}_{3} / &PartialD; t \end{matrix}] = [\begin{matrix} q_{0} & {- q}_{1} & {- q}_{2} & {- q}_{3} \\ q_{1} & q_{0} & {- q}_{3} & q_{2} \\ q_{2} & q_{3} & q_{0} & {- q}_{1} \\ q_{3} & {- q}_{2} & q_{1} & q_{0} \end{matrix}] \cdot \begin{matrix} [\begin{matrix} 0 \\ v_{x} / 2 \\ v_{y} / 2 \\ v_{z} / 2 \end{matrix}] - - - (7) \end{matrix}

其中v_x,v_y,v_z是惯性测量仪在轴x_s,y_s,z_s的角速度；由于状态x_k包括四元数状态和角速度，所以x_k有以下形式：

x_k=[q_0,kq_1,kq_2,kq_3,kv_x,kv_y,kv_z,k] (8)

这里q₀，_k，q₁，_k，q₂，_k，q₃，_k，v_x，_k，v_y，_k，v_z,_k是k时刻的四元数状态和角速度，从方程(7)可得，状态转换方程为：

A_{k} = [\begin{matrix} 1 & 0 & 0 & 0 & - q_{1, k} \cdot Δt / 2 & - q_{2, k} \cdot Δt / 2 & - q_{3, k} \cdot Δt / 2 \\ 0 & 1 & 0 & 0 & q_{0, k} \cdot Δt / 2 & q_{3, k} \cdot Δt / 2 & q_{2, k} \cdot Δt / 2 \\ 0 & 0 & 1 & 0 & q_{3, k} \cdot Δt / 2 & q_{0, k} \cdot Δt / 2 & - q_{1, k} \cdot Δt / 2 \\ 0 & 0 & 0 & 1 & - q_{2, k} \cdot Δt / 2 & q_{1, k} \cdot Δt / 2 & q_{0, k} \cdot Δt / 2 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] - - - (9)

这里Δt是取样时间，由于没有控制输入，让B=0^n×p，则B·u_k-1=0^n×1；使用角速度来估计四元数状态，得到过程噪声向量为

w_k=[0000 w_x w_y w_z]^T (10)

这里w_x,w_y,w_z是角速度的过程噪声；由于使用陀螺仪来测量角速度，观察矩阵H为

H=[0^3×4I^3×3] (11)

为了满足(4)，定义

k时刻的确定性四元数q_k可被标准化为：

q_k=[q_0,k/D q_1,k/D q_2,k/D q_3,k/D] (12)

结合公式(6)到公式(12)，可从时刻k-1的惯性测量仪状态x_k-1估计出时刻k的惯性测量仪状态x_k。

所述步骤S3包括以下步骤：

利用机器人的末端位姿进行反解，然后每个关节按反解的角度运动，当全部关节运动完毕虚拟机器人的手抓末端就到达指定的位置。这种情况适合于周围没有障碍物的情况，虚拟机器人手臂运动不会与周围环境障碍物发生碰撞。

而在Denavit-Hartenberg表示法中，A_i表示从坐标系i-1到坐标系i的齐次坐标变换矩阵，一般地有：

A_{i} = [\begin{matrix} {\cos θ}_{i} & {- \sin}_{i} {\cos α}_{i} & {\sin θ}_{i} {\sin α}_{i} & l_{i} {\cos θ}_{i} \\ {\sin θ}_{i} & {\cos θ}_{i} {\cos α}_{i} & {- \cos θ}_{i} {\sin α}_{i} & l_{i} {\sin θ}_{i} \\ 0 & {\sin α}_{i} & {\cos α}_{i} & r_{i} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (13)

对于一个具有六关节的机器人，定义机座坐标系为坐标系1，最后一个坐标系为坐标系6，从机座坐标架到最后一个坐标架的齐次变换矩阵T₆定义为：

T_{6} = A_{1} A_{2} {. . . A}_{6} = [\begin{matrix} n_{6}^{0} & s_{6}^{0} & a_{6}^{0} & p_{6}^{0} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (14)

式中

为手爪的法向矢量，

为滑动矢量，

为接近矢量，

为位置矢量。

利用(14),(5)有：

T₆=M (15)

通过求解(15)得到六个关节运动角度值：(θ₁，θ₂,...θ₆)。

本发明相对于现有技术具有如下的优点及效果：

本发明提出了一种基于混合传感器的人机交互方法，允许操作者通过三维的手部动作来控制机器人。这种基于混合传感器的接口允许操作员使用手部和手臂的动作控制机器人而没有物理装置的限制，只需要把精力集中在手头的任务也不用把任务都分解为那些有限的指令。

附图说明

图1是骨架关节点图；

图2是右手关节点图；

图3是基于混合传感器的人机交互流程图。

具体实施方式

下面结合实施例对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例：

本发明基于混合传感器进行人机交互方法包括如下步骤：

S3、根据上述步骤获取的操作员手部位姿驱动机器人。

所述步骤S1包括以下步骤：

的右手框架；

相对于参考点

的P₅位置P′₅为：

P₅=P₅-P₃ (1)

定义右手相对于右肩的移动范围为

w_{Rh}^{1} < x < w_{RH}^{2},

H_{RH}^{1} < y < H_{RH}^{2},

L_{RH}^{1} < z < L_{RH}^{2},

而机器人的移动范围为

W_{RR}^{1} < x < W_{RR}^{2},

H_{RR}^{1} < y < H_{RR}^{2},

L_{RR}^{1} < z < L_{RR}^{2},

机器人移动范围的映射点为：

\{\begin{matrix} R_{x}^{R} = \frac{(P_{8_x}^{'} - W_{RH}^{1})}{(W_{RH}^{2} - W_{RH}^{1})} \cdot (W_{RR}^{2} - W_{RR}^{1}) + W_{RR}^{1} \\ R_{y}^{R} = \frac{(P_{8_y}^{'} - H_{RH}^{1})}{(H_{RH}^{2} - H_{RH}^{1})} \cdot H_{RR}^{2} - H_{RR}^{1} + H_{RR}^{1} \\ R_{z}^{R} = \frac{(P_{8_z}^{'} - L_{RH}^{1})}{(L_{RH}^{2} - L_{RH}^{1})} \cdot (L_{RR}^{2} - L_{RR}^{1}) + L_{RR}^{1} \end{matrix} - - - (2)

所述步骤S2包括以下步骤：

q = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}] = [\begin{matrix} \cos (φ / 2) \cos (θ / 2) \cos (ψ / 2) + \sin (φ / 2) \sin (θ / 2) \sin (ψ / 2) \\ \sin (φ / 2) \cos (θ / 2) \cos (ψ / 2) - \cos (φ / 2) \sin (θ / 2) \sin (ψ / 2) \\ \cos (φ / 2) \sin (θ / 2) \cos (ψ / 2) + \sin (φ / 2) cso (θ / 2) \sin (ψ / 2) \\ \cos (φ / 2) \cos (θ / 2) \sin (ψ / 2) - \sin (φ / 2) \sin (θ / 2) \cos (ψ / 2) \end{matrix}] - - - (3)

而四个欧拉参数的约束为：

q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2} = 1 - - - (4)

这里q₀是标量部分而(q₁,q₂,q₃)是矢量部分；所以从惯性测量仪坐标系到地球固定坐标系的姿态余弦矩阵

可以被表示为：

M_{s}^{e} = [\begin{matrix} q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2 (q_{1} q_{2} - q_{0} q_{3}) & 2 (q_{0} q_{2} + q_{1} q_{3}) \\ 2 (q_{1} q_{2} + q_{0} q_{3}) & q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} & 2 (q_{2} q_{3} - q_{0} q_{1}) \\ 2 (q_{1} q_{3} - q_{0} q_{2}) & 2 (q_{0} q_{1} + q_{2} q_{3}) & q_{0}^{2} - q_{1}^{2} - q_{2}^{2} - q_{3}^{2} \end{matrix}] - - - (5);

x_k=A_k·x_k-1+B·u_k-1+w_k-1 (6)

z_k=H·x_k+v_k

[\begin{matrix} {&PartialD; q}_{0} / &PartialD; t \\ {&PartialD; q}_{1} / &PartialD; t \\ {&PartialD; q}_{2} / &PartialD; t \\ {&PartialD; q}_{3} / &PartialD; t \end{matrix}] = [\begin{matrix} q_{0} & {- q}_{1} & {- q}_{2} & {- q}_{3} \\ q_{1} & q_{0} & {- q}_{3} & q_{2} \\ q_{2} & q_{3} & q_{0} & {- q}_{1} \\ q_{3} & {- q}_{2} & q_{1} & q_{0} \end{matrix}] \cdot \begin{matrix} [\begin{matrix} 0 \\ v_{x} / 2 \\ v_{y} / 2 \\ v_{z} / 2 \end{matrix}] - - - (7) \end{matrix}

其中v_x,v_y，v_z是惯性测量仪在轴x_s,y_s,z_s的角速度；由于状态x_k包括四元数状态和角速度，所以x_k有以下形式：

x_k=[q_0,kq_1，kq_2,kq_3，kv_x,kv_y，kv_z,k] (8)

这里q_0，k，q_1，k，q_2，k，q_3，k，v_x，k，v_y，k，v_z,k是k时刻的四元数状态和角速度，从方程(7)可得，状态转换方程为：

A_{k} = [\begin{matrix} 1 & 0 & 0 & 0 & - q_{1, k} \cdot Δt / 2 & - q_{2, k} \cdot Δt / 2 & - q_{3, k} \cdot Δt / 2 \\ 0 & 1 & 0 & 0 & q_{0, k} \cdot Δt / 2 & q_{3, k} \cdot Δt / 2 & q_{2, k} \cdot Δt / 2 \\ 0 & 0 & 1 & 0 & q_{3, k} \cdot Δt / 2 & q_{0, k} \cdot Δt / 2 & - q_{1, k} \cdot Δt / 2 \\ 0 & 0 & 0 & 1 & - q_{2, k} \cdot Δt / 2 & q_{1, k} \cdot Δt / 2 & q_{0, k} \cdot Δt / 2 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] - - - (9)

w_k=[0000w_x w_y w_z]^T (10)

H=[0^3×4 I^3×3] (11)

为了满足(4)，定义

k时刻的确定性四元数q_k可被标准化为：

q_k=[q_0，k/D q_1,k/D q_2,k/D q_3,k/D] (12)

所述步骤S3包括以下步骤：

A_{i} = [\begin{matrix} {\cos θ}_{i} & {- \sin}_{i} {\cos α}_{i} & {\sin θ}_{i} {\sin α}_{i} & l_{i} {\cos θ}_{i} \\ {\sin θ}_{i} & {\cos θ}_{i} {\cos α}_{i} & {- \cos θ}_{i} {\sin α}_{i} & l_{i} {\sin θ}_{i} \\ 0 & {\sin α}_{i} & {\cos α}_{i} & r_{i} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (13)

T_{6} = A_{1} A_{2} . . . A_{6} = [\begin{matrix} n_{6}^{0} & s_{6}^{0} & a_{6}^{0} & p_{6}^{0} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (14)

式中为手爪的法向矢量，为滑动矢量，

为接近矢量，

为位置矢量。

利用(14),(5)有：

T₆=M (15)

通过求解(15)得到六个关节运动角度值：(θ₁,θ₂,...,θ₆)。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims

1.基于混合传感器的人机交互方法，其特征在于，包括以下步骤：

S2、使用惯性测量仪测量操作员的手部姿态，并结合四元数算法和卡尔曼滤波对姿态数据进行估计，从而得到稳定的操作员的手部姿态；

S3、根据上述步骤获取的操作员手部位姿驱动机器人。

2.根据权利要求1所述的基于混合传感器的人机交互方法，其特征在于，所述步骤S1包括：

从深度图像中提取人体骨架关节点；所提取出来的这十五个关节点从上到下和从左到右排序为：（1）头；（2）肩中心；（3）右肩；（4）右手肘；（5）右手腕；（6）左肩；（7）左手肘；（8）左手腕；（9）髋关节中心；（10）右髋；（11）右膝；（12）右脚；（13）左髋；（14）左膝；（15）左脚；这十五个关节点的坐标被称为Kinect坐标；操作员右手的位置定义为右手腕P₅的位置，将肩关节P₃作为参考点并构建参考点

的右手框架；

相对于参考点

的P₅位置P′₅为：

P′₅=P₅-P₃ (1)

设定右手相对于右肩的移动范围为

W_{RH}^{1} < x < W_{RH}^{2},

H_{RH}^{1} < y < H_{RH}^{2},

L_{RH}^{1} < z < L_{RH}^{2},

而机器人的移动范围为

W_{RR}^{1} < x < W_{RR}^{2},

H_{RR}^{1} < y < H_{RR}^{2},

L_{RR}^{1} < z < L_{RR}^{2},

机器人移动范围的映射点为：

\{\begin{matrix} R_{x}^{R} = \frac{(P_{8_x}^{'} - W_{RH}^{1})}{(W_{RH}^{2} - W_{RH}^{1})} \cdot (W_{RR}^{2} - W_{RR}^{1}) + W_{RR}^{1} \\ R_{y}^{R} = \frac{(P_{8_y}^{'} - H_{RH}^{1})}{(H_{RH}^{2} - H_{RH}^{1})} \cdot H_{RR}^{2} - H_{RR}^{1} + H_{RR}^{1} \\ R_{z}^{R} = \frac{(P_{8_z}^{'} - L_{RH}^{1})}{(L_{RH}^{2} - L_{RH}^{1})} \cdot (L_{RR}^{2} - L_{RR}^{1}) + L_{RR}^{1} \end{matrix} . - - - (2)

3.根据权利要求1所述的基于混合传感器的人机交互方法，其特征在于，所述步骤S2包括以下步骤：

q = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}] = [\begin{matrix} \cos (φ / 2) \cos (θ / 2) \cos (ψ / 2) + \sin (φ / 2) \sin (θ / 2) \sin (ψ / 2) \\ \sin (φ / 2) \cos (θ / 2) \cos (ψ / 2) - \cos (φ / 2) \sin (θ / 2) \sin (ψ / 2) \\ \cos (φ / 2) \sin (θ / 2) \cos (ψ / 2) + \sin (φ / 2) cso (θ / 2) \sin (ψ / 2) \\ \cos (φ / 2) \cos (θ / 2) \sin (ψ / 2) - \sin (φ / 2) \sin (θ / 2) \cos (ψ / 2) \end{matrix}] - - - (3)

而四个欧拉参数的约束为：

q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2} = 1 - - - (4)

可以被表示为：

M_{s}^{e} = [\begin{matrix} q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2 (q_{1} q_{2} - q_{0} q_{3}) & 2 (q_{0} q_{2} + q_{1} q_{3}) \\ 2 (q_{1} q_{2} + q_{0} q_{3}) & q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} & 2 (q_{2} q_{3} - q_{0} q_{1}) \\ 2 (q_{1} q_{3} - q_{0} q_{2}) & 2 (q_{0} q_{1} + q_{2} q_{3}) & q_{0}^{2} - q_{1}^{2} - q_{2}^{2} - q_{3}^{2} \end{matrix}] - - - (5);

x_k=A_k·x_k-1+B·u_k-1+w_k-1 (6)

z_k=H·x_k+v_k

[\begin{matrix} {&PartialD; q}_{0} / &PartialD; t \\ {&PartialD; q}_{1} / &PartialD; t \\ {&PartialD; q}_{2} / &PartialD; t \\ {&PartialD; q}_{3} / &PartialD; t \end{matrix}] = [\begin{matrix} q_{0} & {- q}_{1} & {- q}_{2} & {- q}_{3} \\ q_{1} & q_{0} & {- q}_{3} & q_{2} \\ q_{2} & q_{3} & q_{0} & {- q}_{1} \\ q_{3} & {- q}_{2} & q_{1} & q_{0} \end{matrix}] \cdot \begin{matrix} [\begin{matrix} 0 \\ v_{x} / 2 \\ v_{y} / 2 \\ v_{z} / 2 \end{matrix}] - - - (7) \end{matrix}

x_k=[q_0,kq_1，kq_2,kq_3，kv_x,kv_y，kv_z,k] (8)

其中q_0，k，q_1，k，q_2，k，q_3，k，v_x，k，v_y，k，v_z,k是k时刻的四元数状态和角速度，从方程(7)可得，状态转换方程为：

A_{k} = [\begin{matrix} 1 & 0 & 0 & 0 & - q_{1, k} \cdot Δt / 2 & - q_{2, k} \cdot Δt / 2 & - q_{3, k} \cdot Δt / 2 \\ 0 & 1 & 0 & 0 & q_{0, k} \cdot Δt / 2 & q_{3, k} \cdot Δt / 2 & q_{2, k} \cdot Δt / 2 \\ 0 & 0 & 1 & 0 & q_{3, k} \cdot Δt / 2 & q_{0, k} \cdot Δt / 2 & - q_{1, k} \cdot Δt / 2 \\ 0 & 0 & 0 & 1 & - q_{2, k} \cdot Δt / 2 & q_{1, k} \cdot Δt / 2 & q_{0, k} \cdot Δt / 2 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] - - - (9)

其中Δt是取样时间，由于没有控制输入，让B=0^n×p，则B·u_k-1=0^n×1；使用角速度来估计四元数状态，得到过程噪声向量为

w_k=[0000w_xw_yw_z]^T (10)

其中w_x,w_y,w_z是角速度的过程噪声；由于使用陀螺仪来测量角速度，观察矩阵H为

H=[0^3×4I^3×3] (11)

为了满足(4)，定义

k时刻的确定性四元数q_k可被标准化为：

q_k=[q_0,k/D q_1,k/D q_2,k/D q_3,k/D] (12)

4.根据权利要求1所述的基于混合传感器的人机交互方法，其特征在于，所述步骤S3包括以下步骤：

利用机器人的末端位姿进行反解，然后每个关节按反解的角度运动，当全部关节运动完毕虚拟机器人的手抓末端就到达指定的位置；这种情况适合于周围没有障碍物的情况，虚拟机器人手臂运动不会与周围环境障碍物发生碰撞；

在Denavit-Hartenberg表示法中，A_i表示从坐标系i-1到坐标系i的齐次坐标变换矩阵，则有：

A_{i} = [\begin{matrix} {\cos θ}_{i} & {- \sin}_{i} {\cos α}_{i} & {\sin θ}_{i} {\sin α}_{i} & l_{i} {\cos θ}_{i} \\ {\sin θ}_{i} & {\cos θ}_{i} {\cos α}_{i} & {- \cos θ}_{i} {\sin α}_{i} & l_{i} {\sin θ}_{i} \\ 0 & {\sin α}_{i} & {\cos α}_{i} & r_{i} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (13)

T_{6} = A_{1} A_{2} . . . A_{6} = [\begin{matrix} n_{6}^{0} & s_{6}^{0} & a_{6}^{0} & p_{6}^{0} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (14)

式中为手爪的法向矢量，为滑动矢量，

为接近矢量，

为位置矢量。

利用(14),(5)有：

T₆=M (15)

通过求解(15)得到六个关节运动角度值：(θ₁,θ₂,...,θ₆)。