CN111185906A - 一种基于Leap Motion的灵巧手主从控制方法 - Google Patents

Abstract

本发明公开了一种基于Leap Motion的灵巧手主从控制方法，在五指伸展的状态下获取人手和灵巧手的五指指尖和指掌关节位置信息，由这些信息执行映射算法，计算出实现人手与灵巧手之间精确的运动映射所需要的变换矩阵；接下来通过Leap Motion视觉传感器实时获取人手五指指尖在参考坐标系下的笛卡尔空间坐标，通过变换矩阵变换到灵巧手的笛卡尔空间，实现重映射，重映射得到的空间坐标就是灵巧手对应五指指尖的目标位置，然后根据重映射得到的灵巧手指尖的目标位置，对灵巧手五指进行逆运动学解算，得到五指各关节的角度值，最后通过灵巧手的驱动机构驱动五指各关节运动对应的角度值，使灵巧手指尖运动到目标位置，完成基于人手运动映射的灵巧手抓持规划。

Description

一种基于Leap Motion的灵巧手主从控制方法

技术领域

本发明属于人机交互技术领域，具体涉及一种基于Leap Motion的灵巧手主从控制方法。

背景技术

灵巧手的抓持规划是灵巧手相关技术研究中的关键技术。由于灵巧手所处的工作环境具有高度复杂性、动态性和不可预测性，且灵巧手对于环境的感知也存在不可靠性，要求灵巧手实现完全自主的抓持规划是一项极具挑战性的课题。而主从控制则是通过人手直接控制灵巧手的一种抓持方案，该方案结合了人的经验智慧和灵巧手的智能，将人作为控制系统中的一个环节参与到灵巧手的控制中，利用人进行高层次的任务规划和命令设计，利用机器人完成任务执行工作，是目前较为成熟可行的方案。

主从控制中的一个关键问题是人手运动采集，传统的运动采集系统如数据手套，除了佩戴不方便以外，更严重的缺点是传感器采集的信息误差很大，而采用Kinect相机作为运动采集系统虽然摆脱了穿戴手套的不方便问题，但仍然存在精度低且对于手部的动作细节信息不够等问题。

主从控制中的一个关键问题是运动映射，即将人手的运动转换为灵巧手的运动。为了实现精确抓持，常采用基于笛卡尔空间的指尖位置映射。但由于人手和灵巧手在外形和结构尺寸上存在差异，导致二者指尖在笛卡尔空间的活动范围也存在差异，直接的运动映射无法实现精确的抓持规划，甚至出现目标位置不可解的情况。

因此，需要采用更为精确、方便的人手运动采集系统，同时也需要对运动映射的过程加以改进，以实现精确的抓持规划。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于Leap Motion的灵巧手主从控制方法，通过该方法对灵巧手进行主从控制，可以实现精确的人手与灵巧手之间的运动映射，完成精确的灵巧手抓持规划。

为达到上述目的，本发明构思如下：

本发明在五指伸展的状态下获取人手和灵巧手的五指指尖和指掌关节位置信息，由这些信息执行映射算法，计算出实现人手与灵巧手之间精确的运动映射所需要的变换矩阵；接下来通过Leap Motion视觉传感器实时获取人手五指指尖在参考坐标系下的笛卡尔空间坐标，通过变换矩阵变换到灵巧手的笛卡尔空间，实现重映射，重映射得到的空间坐标就是灵巧手对应五指指尖的目标位置，然后根据重映射得到的灵巧手指尖的目标位置，对灵巧手五指进行逆运动学解算，得到五指各关节的角度值，最后通过灵巧手的驱动机构驱动五指各关节运动对应的角度值，使灵巧手指尖运动到目标位置，完成基于人手运动映射的灵巧手抓持规划。

根据上述构思，本发明采用如下技术方案：

一种基于Leap Motion的灵巧手主从控制方法，包括以下步骤：

步骤1：获取人手和InMoov灵巧手的相关点坐标；

步骤2：执行映射算法，得到从人手空间到灵巧手空间映射的变换矩阵T(M)；

步骤3：重映射：通过Leap Motion实时采集人手五指指尖相对于参考坐标系的笛卡尔空间坐标^up_ds，根据步骤2映射算法得到的变换矩阵T(M)，将^up_ds变换到灵巧手的笛卡尔空间，变换后的笛卡尔空间坐标为

即为灵巧手指尖的目标位置；

步骤4：根据步骤3得到的灵巧手指尖的目标位置信息，对灵巧手五指进行逆运动学解算，得到五指各关节角度值θ，通过灵巧手的驱动机构驱动五指各关节运动对应的角度值，使灵巧手指尖运动到目标位置，完成基于人手运动映射的灵巧手抓持规划。

所述步骤1的具体过程如下：

步骤1.1：建立人手的参考坐标系；

步骤1.2：建立灵巧手的参考坐标系；

步骤1.3：使人手处于五指伸展状态，通过Leap Motion获取人手五指的指尖和指掌关节相对于参考坐标系的笛卡尔空间坐标，分别为^up_ds和^up_mc；

步骤1.4：使灵巧手处于五指伸展的状态，获取灵巧手指尖和指掌关节相对于灵巧手参考坐标系的笛卡尔空间坐标，分别为^rp_ds和^rp_mc。

所述步骤2中，所述映射算法具体流程为：

步骤2.1：计算变换矩阵T(M)：

从人手到灵巧手的运动映射包括笛卡尔空间的平移变换、旋转变换和线性比例缩放关系变换，定义运动映射参数矩阵：

M＝[S T_x T_y T_z R_x R_y R_z]

参数矩阵M的初始值为M₀＝[1 0 0 0 0 0 0]，在算法第一次迭代执行时使用，根据M中各参数具体值，计算从人手笛卡尔空间到灵巧手笛卡尔空间的变换矩阵T(M)：

T(M)＝T_scale(S)T_trans(T_x,T_y,T_z)T_rot(R_x,R_y,R_z)

其中T_scale(S)为在x，y，z方向进行线性缩放的齐次变换矩阵，T_trans(T_x,T_y,T_z)为进行平移变换的齐次变换矩阵，T_rot(R_x,R_y,R_z)为进行旋转变换的齐次变换矩阵；

步骤2.2：坐标变换：

根据步骤2.1得到的变换矩阵T(M)，将人手笛卡尔空间的指尖和指掌关节坐标变换到灵巧手笛卡尔空间中，变换操作如下：

其中，

和

分别为变换后的指尖和指掌关节坐标；

步骤2.3：计算误差向量e：

根据步骤1和步骤2.2得到的信息，计算灵巧手指尖和指掌关节的笛卡尔空间坐标与变换后人手指尖和指掌关节的笛卡尔空间坐标之间的误差向量e：

步骤2.4：计算逆雅可比矩阵

根据步骤2.2得到的

对参数矩阵M求偏导，得到

相对参数矩阵M中各元素变化的雅可比矩阵J，由于J非方阵，其不存在逆矩阵，因此取J的伪逆矩阵

步骤2.5：计算参数矩阵M；

步骤2.6：回到步骤2.1，计算变换矩阵T(M)，进行下一次迭代，映射算法的迭代终止条件是误差向量e为零向量。

与现有技术相比，本发明方法具有以下有益效果：

1.本发明采用Leap Motion视觉传感器作为人手运动采集系统来获取人手的运动信息，能够精确捕捉人手五指的运动状态和各关节的空间坐标，基于视觉的控制方式也更加方便高效。

2.本发明采用的映射算法能够准确计算出人手和灵巧手之间的运动映射的变换矩阵，通过此变换矩阵可以使人手指尖的运动范围与灵巧手之间的运动范围重合，完成精确的基于人手运动映射的灵巧手抓持规划。

附图说明

图1是本发明一种基于Leap Motion的灵巧手主从控制方法流程图。

图2是本发明采用的映射算法流程图。

图3是人手模型图。

图4是InMoov灵巧手模型图。

图5是人手骨骼图。

具体实施方式

下面结合附图对本发明的具体实施例进行进一步地描述。

本实施例需要采集指尖和指掌关节的笛卡尔空间坐标，其具体位置如图5所示。本实施例采用图4所示的InMoov灵巧手进行主从控制，映射算法和主从控制都在MatlabSimulink环境下实现，首先需要在Simulink环境下建立InMoov灵巧手仿真模型，并通过MatLeap接口实现Leap Motion与Matlab之间的通信。

如图1所示，一种基于Leap Motion的灵巧手主从控制方法，包括以下步骤：

步骤1：获取人手和InMoov灵巧手的相关点坐标。

步骤1.1：建立人手的参考坐标系，如图3所示，人手坐标系建立在手腕处，为右手坐标系，其y方向和z方向如图所示；

步骤1.2：建立InMoov灵巧手的参考坐标系，如图4所示，InMoov灵巧手坐标系建立在灵巧手的基座处，为右手坐标系，其y方向和z方向如图所示；

步骤1.3：使人手处于五指伸展状态，即图3所示状态，通过Leap Motion获取人手五指的指尖和指掌关节相对于参考坐标系的笛卡尔空间坐标，分别为^up_ds和^up_mc；

步骤1.4：将Simulink环境下的InMoov灵巧手模型各关节角设为0°，即图2所示的五指伸展状态，通过Transform Sensor模块得到灵巧手指尖和指掌关节相对于灵巧手参考坐标系的笛卡尔空间坐标，分别为^rp_ds和^rp_mc。

步骤2：执行映射算法，得到从人手空间到灵巧手空间映射的变换矩阵T(M)。

根据步骤1获取的信息，执行映射算法，图2为映射算法流程图，结合图2对映射算法进行详细说明，具体包括以下步骤：

步骤2.1：计算变换矩阵T(M)：

从人手到灵巧手的运动映射包括笛卡尔空间的平移变换、旋转变换和线性比例缩放关系变换，定义运动映射参数矩阵：

M＝[S T_x T_y T_z R_x R_y R_z]

参数矩阵M的初始值为M₀＝[1 0 0 0 0 0 0]，在算法第一次迭代执行时使用，根据M中各参数具体值，计算从人手笛卡尔空间到灵巧手笛卡尔空间的变换矩阵T(M)：

T(M)＝T_scale(S)T_trans(T_x,T_y,T_z)T_rot(R_x,R_y,R_z)

其中T_scale(S)为在x，y，z方向进行线性缩放的齐次变换矩阵，其具体形式为：

其中T_trans(T_x,T_y,T_z)为进行平移变换的齐次变换矩阵，其具体形式为：

其中T_rot(R_x,R_y,R_z)为进行旋转变换的齐次变换矩阵，其具体形式为：

式中

分别为绕x、y、z轴旋转R_x、R_y、R_z角度的齐次变换矩阵。

步骤2.2：坐标变换：

根据步骤2.1得到的变换矩阵T(M)，将人手笛卡尔空间的指尖和指掌关节坐标变换到InMoov灵巧手笛卡尔空间中，变换操作如下：

其中，

和

分别为变换后的指尖和指掌关节坐标。

步骤2.3：计算误差向量e

根据步骤1和步骤2.2得到的信息，计算灵巧手指尖和指掌关节的笛卡尔空间坐标与变换后人手指尖和指掌关节的笛卡尔空间坐标之间的误差向量e：

步骤2.4：计算逆雅可比矩阵

根据步骤2.2得到的

对参数矩阵M求偏导，得到

相对参数矩阵M中各元素变化的雅可比矩阵J：

对于变换后的一点p＝[p_x p_y p_z]^T,J中各元素的计算公式为：

J₁₁＝p_x cos R_y cos R_z-p_y cos R_y sin R_z+p_z sin R_y+T_x

J₁₆＝-Sp_x sin R_y cos R_z+Sp_y sin R_y sin R_z+Sp_z cos R_y

J₁₇＝-Sp_x cos R_y sin R_z-Sp_y cos R_y cos R_z

J₂₁＝p_x(sin R_x sin R_y cos R_z+cos R_x sin R_z)

+p_y(cos R_x cos R_z-sin R_x sin R_y sin R_z)

-p_z sin R_x cos R_y+T_y

J₂₅＝Sp_x(cos R_x sin R_y cos R_z-sin R_x sin R_z)

+Sp_y(-sin R_x cos R_z-cos R_x sin R_y sin R_z)

-Sp_z cos R_x cos R_y

J₂₆＝Sp_x sin R_x cos R_y cos R_z-Sp_y sin R_x sin R_y sin R_z+Sp_z sin R_x sin R_y

J₂₇＝Sp_x(-sin R_x sin R_y sin R_z+cos R_x cos R_z)

+Sp_y(-cos R_x sin R_z-sin R_x sin R_y cos R_z)

J₃₁＝p_x(sin R_x sin R_z-cos R_x sin R_y cos R_z)

+p_y(cos R_x sin R_y sin R_z+sin R_x cos R_z)

+p_z cos R_x cos R_y+T_z

J₃₅＝Sp_x(cos R_x sin R_z+sin R_x sin R_y cos R_z)

+Sp_y(-sin R_x sin R_y sin R_z+cos R_x cos R_z)

-Sp_z sin R_x cos R_y

J₃₆＝-Sp_x cos R_x cos R_y cos R_z+Sp_y cos R_x cos R_y sin R_z-Sp_z cos R_x sin R_y

J₃₇＝Sp_x(sin R_x cos R_z+cos R_x sin R_y sin R_z)

+Sp_y(cos R_x sin R_y cos R_z-sin R_x sin R_z)

由于J非方阵，其不存在逆矩阵，因此这里取J的伪逆矩阵

步骤2.5：计算参数矩阵M：

根据步骤2.3和步骤2.4得到的信息，可以计算得到M的微分形式

因此对

进行积分就能得到M；

需要说明的是，当误差向量e对时间求导时，可以得到：

其中

为参数矩阵M的微分形式，当选择

按如下方式计算时：

可以使映射算法构成形式为

的闭环线性系统，其中K为正定矩阵，用于调节算法的收敛速度，该系统在K为正定矩阵时是渐进稳定的，误差沿轨迹趋于零，因此选择合适K，映射算法总是会收敛的。

步骤2.6：回到步骤2.1，计算变换矩阵T(M)，进行下一次迭代；

需要说明的是，根据图2所示算法流程图可以看出，映射算法是一个迭代形式的算法，该算法迭代终止的条件是误差向量为零向量，即该算法最终会找到一个参数矩阵M，使得按照M计算得到的变换矩阵T(M)能保证变换后的人手五指指尖和指掌关节与InMoov灵巧手的五指指尖和指掌关节重合。

需要说明的是，该映射算法是应用于单根手指上的，即对于InMoov灵巧手来说，需要将该算法分别应用到拇指、食指、中指、无名指和小指上。

步骤3：重映射：通过Leap Motion实时采集人手五指指尖相对于参考坐标系的笛卡尔空间坐标，根据步骤2得到的变换矩阵T(M)变换到InMoov灵巧手的笛卡尔空间。

步骤4：控制InMoov灵巧手运动；根据步骤3得到的信息，可以确定InMoov灵巧手五指指尖此时应该达到的位置，对InMoov灵巧手五指进行逆运动学求解，可以得到每根手指的各关节角度值，将该值分别传给灵巧手各关节的执行机构，驱动执行机构运动相应角度，可使灵巧手指尖运动到指定位置，完成基于人手运动映射的灵巧手抓持规划。

Claims (3)

1.一种基于Leap Motion的灵巧手主从控制方法，其特征在于，包括以下步骤：

步骤1：获取人手和InMoov灵巧手的相关点坐标；

步骤2：执行映射算法，得到从人手空间到灵巧手空间映射的变换矩阵T(M)；

步骤3：重映射：通过Leap Motion实时采集人手五指指尖相对于参考坐标系的笛卡尔空间坐标^up_ds，根据步骤2映射算法得到的变换矩阵T(M)，将^up_ds变换到灵巧手的笛卡尔空间，变换后的笛卡尔空间坐标为

即为灵巧手指尖的目标位置；

步骤4：根据步骤3得到的灵巧手指尖的目标位置信息，对灵巧手五指进行逆运动学解算，得到五指各关节角度值θ，通过灵巧手的驱动机构驱动五指各关节运动对应的角度值，使灵巧手指尖运动到目标位置，完成基于人手运动映射的灵巧手抓持规划。

2.根据权利要求1所述的基于Leap Motion的灵巧手主从控制方法，其特征在于，所述步骤1的具体过程如下：

步骤1.1：建立人手的参考坐标系；

步骤1.2：建立灵巧手的参考坐标系；

步骤1.3：使人手处于五指伸展状态，通过Leap Motion获取人手五指的指尖和指掌关节相对于参考坐标系的笛卡尔空间坐标，分别为^up_ds和^up_mc；

步骤1.4：使灵巧手处于五指伸展的状态，获取灵巧手指尖和指掌关节相对于灵巧手参考坐标系的笛卡尔空间坐标，分别为^rp_ds和^rp_mc。

3.根据权利要求1所述的基于Leap Motion的灵巧手主从控制方法，其特征在于，所述步骤2中，所述映射算法具体流程为：

步骤2.1：计算变换矩阵T(M)：

从人手到灵巧手的运动映射包括笛卡尔空间的平移变换、旋转变换和线性比例缩放关系变换，定义运动映射参数矩阵：

M＝[S T_x T_y T_z R_x R_y R_z]

参数矩阵M的初始值为M₀＝[1 0 0 0 0 0 0]，在算法第一次迭代执行时使用，根据M中各参数具体值，计算从人手笛卡尔空间到灵巧手笛卡尔空间的变换矩阵T(M)：

T(M)＝T_scale(S)T_trans(T_x,T_y,T_z)T_rot(R_x,R_y,R_z)

其中T_scale(S)为在x，y，z方向进行线性缩放的齐次变换矩阵，T_trans(T_x,T_y,T_z)为进行平移变换的齐次变换矩阵，T_rot(R_x,R_y,R_z)为进行旋转变换的齐次变换矩阵；

步骤2.2：坐标变换：

根据步骤2.1得到的变换矩阵T(M)，将人手笛卡尔空间的指尖和指掌关节坐标变换到灵巧手笛卡尔空间中，变换操作如下：

其中，

和

分别为变换后的指尖和指掌关节坐标；

步骤2.3：计算误差向量e：

根据步骤1和步骤2.2得到的信息，计算灵巧手指尖和指掌关节的笛卡尔空间坐标与变换后人手指尖和指掌关节的笛卡尔空间坐标之间的误差向量e：

步骤2.4：计算逆雅可比矩阵

根据步骤2.2得到的

对参数矩阵M求偏导，得到

相对参数矩阵M中各元素变化的雅可比矩阵J，由于J非方阵，其不存在逆矩阵，因此取J的伪逆矩阵

步骤2.5：计算参数矩阵M；

步骤2.6：回到步骤2.1，计算变换矩阵T(M)，进行下一次迭代，映射算法的迭代终止条件是误差向量e为零向量。

Images