面向机器人遥操作的数据手套设计及仿真验证

苑 朝，王少君，蒋 阳，王 祝，翟永杰

(华北电力大学自动化系，河北保定 071000)

0 引言

近年来，随着机器人技术的发展，其应用场景越来越复杂多样，这对机器人操作提出了更高的要求。由于受到机构、控制、人工智能和传感技术水平的限制，发展能在未知或复杂环境下工作的全自主式智能机器人很难实现[1]，基于人机交互的机器人遥操作技术成为了解决当前问题的方案之一。

体感人机交互是一种自然、高效的人机交互方式，而人手作为人体最灵活的部位之一，更加适合作为人机交互系统的主操作端。相较于传统的鼠标、键盘、手柄等交互方式，数据手套可以获取手部更多的运动信息，使操作更加灵活。数据手套通常以在手套背部布置传感器的方式来实现对穿戴者手指关节角度的测量。文献[2]中设计了一款基于压阻式弯曲传感器的数据手套，用于机械手的遥操作，但该手套每根手指只布置了1个弯曲传感器，不能准确测量手指每个关节的角度。文献[3]中基于Cyber GloveII数据手套设计了1个多关节灵巧手混合遥操作系统，Cyber GloveII数据手套使用光纤传感器测量手指弯曲角度，价格较高，提高了系统的成本。

近年来微机电系统(micro-electro-mechanical system，MEMS)技术的进步推动了传感器向小型化、高精度、高可靠性、低成本方向发展，采用惯性测量单元制作数据手套成为了主要选择之一。2017年，赵裕沛采用11个9轴惯性测量单元设计了一款面向手部康复的数据手套[4]，由于远节指骨背部没有布置传感器，远指关节的角度由近指关节的角度通过数学关系计算得到，对一些特殊手势，不能很准确得到。清华大学设计了一款名为YoBu的数据手套用于遥操作机械手和机械臂[5-6]，该手套使用了18个惯性测量单元，分别测量人手部和手臂的姿态，但在传感器校正部分只考虑了主要误差。相似地，文献[7]也基于18个惯性测量单元设计了一款数据手套，对于传感器的校正也只校正了零偏误差。

针对面向遥操作数据手套存在的精度低、成本高、基于惯性测量单元的数据手套校正难、校正不完全等问题，本文面向机器人遥操作仿真方向，基于惯性测量单元设计了一款数据手套，将15个惯性测量单元布置于手套背部用于采集人手指骨的运动信息；采用Levenberg-Marquardt(LM)算法和椭球拟合算法对传感器数据进行了校正，校正后的数据通过扩展卡尔曼滤波进行融合；然后由融合后的数据计算得到各关节角，并输入到Simscape Multibody中建立的虚拟灵巧手，以控制其进行动作。Simscape Multibody是Simulink中广泛应用于机械仿真的部分，能够很方便地反映机械结构的运动角度、角速度等参数，还可以进行动力学仿真，便于对控制操作进行评估和分析。

1 数据手套系统设计

1.1 人手部结构与运动分析

人手的骨骼结构由腕骨、掌骨、指骨和关节构成[8]，如图1所示。指骨包括近节指骨、中节指骨和远节指骨，大拇指只有近节指骨和远节指骨，其他4根手指都由上述3种指骨构成。骨骼间由关节连接，其中掌指关节(metacarpophalangeal joint，MPJ)连接掌骨与指骨；大拇指的指间关节(interphalangeal joint，IPJ)连接近节指骨与远节指骨；其余4根手指的指骨之间分别由近指关节(proximal interphalangeal joint，PIPJ)、远指关节(distal interphalangeal joint，DIPJ)连接。

图1 人手骨骼与关节结构

人手到灵巧手的运动映射方法分为4类[9]：指尖映射、关节映射、关键点映射和基于被操作物体的映射，本文采用便于控制的关节角映射。为了便于进行运动分析，将人手骨骼结构简化为连杆结构，如图2所示。每个指间关节、近指关节、远指关节都只有1个弯曲自由度，而掌指关节有2个自由度，即弯曲自由度和外展自由度。调查发现部分常见的五指灵巧机械手不具备掌指关节的外展自由度[10-11]，并且在没有外展自由度的情况下，五指灵巧机械手也能很灵活地完成任务[12]，因此在手部建模过程中，只关注掌指关节的弯曲自由度，忽略其外展自由度。简化后的手部结构只需要得到14个关节(如图1)的弯曲角度就能得到各手指的姿态，这14个关节弯曲角度的范围如表1所示。

图2 人手手指简化结构及传感器安装方式

表1 手指关节弯曲角度的范围

1.2 数据手套系统结构

面向机器人遥操作的数据手套及其仿真系统的整体结构如图3所示。

图3 系统整体结构图

数据手套的硬件部分主要包括15个JY901惯性测量单元、1个STM32F103单片机和1对蓝牙模块。为了更准确地测得手指关节的弯曲角度，各JY901以X轴互相平行的方式布置于掌骨和14个手指指骨对应的手套背部位置，具体布置方向如图2所示。STM32布置在手腕背部以不影响手部运动。STM32通过IIC总线采集15个JY901测得的加速度、角速度和磁场的原始数据，并把这些数据通过蓝牙模块传输至PC端处理中心。

数据手套的数据处理和对虚拟灵巧手的控制在Simulink中实现，结构如图4所示。在Simulink中，读取并解析蓝牙模块传输的数据，分离得到每个JY901的加速度、角速度和磁场的原始数据。传感器的原始数据经过校正、数据融合，得到每个传感器的实时姿态，再由每个传感器的实时姿态计算得到每个关节的旋转角，数据具体的处理的流程见图5，把关节旋转角传输至虚拟灵巧手的旋转关节，实现对虚拟灵巧手手指运动的控制。其中，校正补偿矩阵离线得到，数据解析、校正、数据融合在Matlab-Function中完成。

图4 上位机结构

图5 数据处理过程

2 传感器数据校正

本文采用的JY901传感器，其核心部分MPU9250由加速度计、陀螺仪、磁力计集成，由于制作工艺及使用环境的影响，3种类型的传感器会存在误差，从而导致手指姿态解算过程中出现误差。因此，在进行姿态解算之前，需要对加速度计、陀螺仪、磁力计的测量数据进行校正。

2.1 传感器误差模型

加速度计和陀螺仪测量原理相似，它们的系统误差都包括非正交误差、刻度因子误差、零偏误差[13]。磁力计的误差除了上述3种误差外，还包含由周围磁场与磁化物质引起的误差，即硬磁误差和软磁误差[14-15]。在对传感器校正之前需要对每种类型传感器的误差进行建模。

2.1.1 加速度计误差模型

加速度计的非正交误差Ta、刻度因子误差Ka、零偏误差ba的补偿矩阵可以表示为

(1)

式中αij为加速度计的i轴绕正交坐标系j轴旋转的角度。

(2)

(3)

根据3种误差对传感器的影响情况，可以得到加速度计的误差校正模型：

ao=Ga(as+ba)+εa

(4)

2.1.2 陀螺仪误差模型

陀螺仪的非正交误差、刻度因子误差和零偏误差的补偿矩阵表示如下：

(5)

式中：βij为陀螺仪的i轴绕正交坐标系j轴旋转的角度。

(6)

(7)

陀螺仪的误差校正模型为

ωo=Gg(ωs+bg)+εg

(8)

2.1.3 磁力计误差模型

磁力计的误差主要包括系统误差和环境误差，其中系统误差包括非正交误差、刻度因子误差和零偏误差，环境误差包括硬磁误差和软磁误差。

磁力计的非正交误差矩阵Tm可以表示为3×3的一般矩阵：

(9)

刻度因子误差矩阵Km表示为

(10)

零偏误差矩阵bm由x、y、z三轴数据的误差参数构成：

(11)

磁力计的硬磁误差矩阵bhi可以表示为由三轴硬磁误差数据构成的向量，软磁误差矩阵Msi表示为3×3的一般矩阵：

(12)

(13)

根据不同误差对磁力计数据的影响情况，磁力计的误差模型表示为

hs=TmKm(Msiho+bhi)+bm+εm

(14)

式(14)可以简化为

hs=Gmho+b

(15)

所以磁力计的误差补偿公式为

ho=(Gm)-1(hs-b)

(16)

2.2 传感器数据校正

加速度计和陀螺仪的系统误差主要来源于非正交误差、刻度因子误差和零偏误差，理想情况下，静止的加速度计只测量当地的重力加速度，因此可以据此构造损失函数，用优化算法求误差矩阵的最优值。陀螺仪可以利用校正后的加速度计数据构造损失函数，同样用优化算法求误差矩阵的最优值[13]。磁力计的误差除了系统误差外，还包含硬磁、软磁误差，采用椭球拟合算法对磁力计数据进行校正。

2.2.1 加速度计数据校正

在进行加速度计、陀螺仪的校正之前，需要采集每个传感器的原始数据。首先把JY901静止放置时间t，然后把传感器置于另一个姿态再静止时间t，这样重复N次，得到用于传感器校正的数据集。其中静止时间t不少于10 s，N不少于20次。通过给陀螺仪数据向量的模长设置阈值将获得的数据集划分为动、静态数据区间。加速度计采用静态区间的数据进行校正，陀螺仪采用动态区间的数据进行校正。

加速度计的误差校正模型如式(4)所示，Ga和ba共有9个未知数，用向量θa表示：

(17)

加速度计的误差校正公式可以写成与θa相关的函数形式：

ao=f(as,θa)

(18)

理想情况下，静态区间的加速度数据校正后应满足‖ao‖=‖g‖，构建加速度计数据的损失函数，损失函数最小时向量θa即为最优。

(19)

式中：N为静态区间个数；g为本地重力加速度。

针对损失函数La(θa)，采用LM算法求其最优解，更新公式为

(20)

2.2.2 陀螺仪数据校正

(21)

式中M为N个静态区间的陀螺仪数据组总数。

补偿系数矩阵Gg共有9个未知参数，用向量θg表示如下：

(22)

损失函数定义为

(23)

对损失函数Lg(θg)进行最优化计算时，首先需要计算旋转矩阵Ri。通过式(24)可以由陀螺仪测得的角速度计算得到四元数的微分形式，再根据RK4积分可以得到第i个动态区间的四元数，最后根据四元数与旋转矩阵的转换关系可以得到Ri。

四元数的微分方程形式为：

(24)

(25)

式中：ωx、ωy、ωz为三轴角速度。

(26)

式中：Δt为动态区间中，采集到相邻2组数据的时间差。

陀螺仪数据的校正也采用LM算法，损失函数如式(23)，迭代公式为：

(27)

2.2.3 磁力计数据校正

理想情况下，磁力计三轴的输出数据在空间中的分布应为一个球心在原点的球体，但是由于存在非正交误差、刻度因子误差、零偏误差以及环境磁场的干扰，其畸变成了球心偏离原点的椭球体。本文采用椭球拟合算法来校正磁力计，算法主要分为2步：通过拟合椭球得到椭球的参数；把椭球参数转化为磁力计的误差补偿矩阵参数。

校正磁力计之前，需要重新采集9轴惯性测量单元的数据。在数据手套工作的空间内，手持JY901在空间中任意旋转3 min，使采集的数据尽可能覆盖整个椭球面。

误差存在的情况下，磁力计数据畸变为一般的椭球，设椭球方程的一般方程式为

ax2+by2+cz2+dxy+exz+fyz+px+qy+rz+s=0

(28)

上式中有10个未知参数，使用椭球拟合算法找到满足式(28)的最优的参数组合。

记未知参数向量为θm：

θm=[abcdefpqrs]T

(29)

Lm(θm)=‖Dθm‖2

(30)

(θm)TCθm=1

(31)

若要保证损失函数Lm(θm)最小时求得的θm为椭球的方程系数，需要满足式(31)的约束条件。根据拉格朗日乘子法，需要求解的函数与约束条件写成：

(32)

式中λ为拉格朗日乘子。

(33)

(34)

根据文献[16]，式(33)只有一个正特征根，该特征根对应的特征向量为ν1，再根据式(34)可以得到ν2，由此可以得到θm的最优值。

椭球的方程还可以写成以下的矩阵方程形式：

(X-X0)TA(X-X0)=1

(35)

校正后的磁场数据模长的平方可以表示为

‖ho‖2=(hs-b)T((Gm)-1)T(Gm)-1(hs-b)

(36)

传统的求解校正矩阵的方法需要测得当地的地磁场强度，而在研究过程中发现，在进行数据融合时，真正起作用的不是校正后磁场强度的大小，而是磁场矢量的方向，所以本文对校正矩阵的求解方法进行了简化：

(37)

对矩阵A进行奇异值分解得到(Gm)-1。求得的(Gm)-1和b是相对值，所以由式(16)计算得到的校正后磁场数据也是相对值，但对于后续的姿态解算没有影响。

3 测量单元姿态解算

扩展卡尔曼滤波是卡尔曼滤波在非线性系统中的应用[17]，它由预测和修正两部分组成。在预测过程中，当前状态的预测通过滤波器对上一时刻的状态值估计得到，在修正过程中，需要由观测值得到卡尔曼增益，进而对上一步得到的预测值进行修正。在9轴惯性测量单元系统中，预测值由陀螺仪测得的角速度数据计算得到，加速度计和磁力计数据计算得到的卡尔曼增益用来修正预测值[18]。

扩展卡尔曼滤波系统的动态模型和观测模型为

(38)

式中：xk为k时刻的状态向量；Zk为k时刻系统的观测量；wk和vk分别为服从N(0，Qk)、N(0，Rk)分布的高斯白噪声。

根据四元数的微分方程可以得到状态转移矩阵Φk-1：

(39)

式中：I为单位矩阵；T为时间。

针对本系统，扩展卡尔曼滤波的核心公式如下：

(40)

式中：上标“-”表示预测过程变量；上标“+”表示修正过程变量；观测值Zk1由加速度计的数据计算求得；Zk2由磁力计的数据计算求得。

式(40)为一次迭代过程，由此可以得到实时的姿态四元数qk=[q0q1q2q3]。根据式(41)把四元数转化为欧拉角：

(41)

4 实验结果与分析

4.1 传感器数据校正结果

4.1.1 加速度计数据校正结果

把加速度计采集的数据换算成以“ m/s2”为单位的数据，误差参数的初始值设置为θa=[1 0 0 1 0 1 0 0 0]，经过迭代得到加速度计校正系数矩阵Ga、零偏误差补偿矩阵ba如下：

(42)

ba=[0.201 8 -0.293 0 -0.042 0]T

(43)

将传感器以x轴、y轴、z轴分别朝上静止放置，加速度数据校正前后的矢量和见图6、均方根误差见表2，校正后的加速度数据很接近本地实际的重力加速度。

图6 3种静止姿态下校正前后的加速度对比

表2 加速度计数据校正前后的均方根误差 m/s2

4.1.2 陀螺仪数据校正结果

陀螺仪的数据换算成以“rad/s”为单位的数据，根据式(21)，可以计算得到零偏误差补偿矩阵bg。误差参数矩阵的初始值设为θg=[1 0 0 0 1 0 0 0 1]，经过迭代得到陀螺仪系数补偿矩阵Gg。

(44)

bg=[0.005 6 0.002 5 -0.005 5]T

(45)

将传感器分别x轴朝上、y轴朝上、z轴朝上静止放置得到的陀螺仪数据，校正前后的矢量和如图7所示，表3为陀螺仪数据校正前后的均方根误差，可以看出经过校正，陀螺仪静止状态的数据误差有明显的减小。

图7 3种静止姿态下校正前后角速度对比

表3 陀螺仪数据校正前后的均方根误差 rad/s

4.1.3 磁力计数据校正结果

磁力计测得的以“μT”为单位的数据，取C1中k=4拟合得到如图8(a)的椭球，经式(27)计算得到椭球的修正矩阵(Gm)-1和b，校正后的数据三维散点图如图8(b)，图9为校正得到的球在3个坐标平面的投影，可以看出畸变成椭球的原始数据被校正成了球心在原点的球。由于求得的矩阵为相对值，所以校正后的磁场大小略小于实际磁场。

(a)原始磁场数据

(b)校正后磁场数据图8 校正前后磁场数据分布对比

(a)校正前后数据在X-Y平面投影

(b)校正前后数据在X-Z平面投影

(c)校正前后数据在Y-Z平面投影图9 校正前后的磁场数据点在坐标平面的投影

(46)

b=[-22.068 3 9.872 4 18.362 8]T

(47)

4.2 精度测试

4.2.1 静态测试

虚拟灵巧手的单个关节由2个JY901传感器横滚角的角度差控制，把2个JY901传感器的X轴分别以0°、30°、60°、90°的角度差固定，实际采集到的角度数据见图10，4组角度数据的平均值与标准差见表4。可以看出，4组静态测试角度平均误差的绝对值均小于1°，且数据比较平稳。

图10 静态角度测试

表4 静态角度数据的平均值与标准差 (°)

4.2.2 动态测试

动态测试以高精度、高稳定性姿态传感器LPMS-IG1的数据作为参考标准。LPMS-IG1高精度姿态传感器的解析度为0.01°，静态误差小于0.3°，动态误差小于1°。设置JY901和LMPS-IG1的传输频率均为100 Hz，把二者固定在一起同步运动，得到三轴的动态角度曲线如图11所示。以LPMS-IG1的数据为参考数据，对比得到JY901三轴动态角度的误差见表5。与高精度LPMS-IG1的数据相比，JY901三轴动态数据的均方根误差均小于1.5°，最大误差绝对值均小于3°，动态性能良好。

图11 动态角度测试

表5 动态角度数据的均方根误差与最大误差 (°)

4.3 系统有效性仿真验证

图12是数据手套的实物图，在蓝牙有效传输距离内，操作者佩戴数据手套，做出特定手势，人手动作对虚拟手动作控制效果如图13所示。可以看出，当人手做出特定手势的动作，数据手套将测得的数据经过处理后映射到虚拟模型中，虚拟机械手也做出相应的动作。

图12 数据手套实物图

图13 数据手套与虚拟灵巧手动作的对应

以食指关节为例，当食指从伸直状态到弯曲状态、再恢复至伸直状态，掌指关节MPJ、近指关节PIPJ、远指关节DIPJ的关节角度、角速度随时间的变化情况如图14所示。由Simscape Mulitybody搭建的系统，可以很直观地得到关节的实时角度与角速度，便于进行对数据手套系统的评估。把JY901自身输出时间与CPU时间进行对比，测得系统的延迟时间小于0.06 s。

(a)角度随时间变化图

(b)角速度随时间变化图图14 食指关节角度、角速度随时间变化情况

5 结论

本文基于15个9轴惯性测量单元设计了一款应用于机器人遥操作的数据手套，并在Simulink搭建的系统中进行了实验。实验结果表明：采用LM算法对加速度计和陀螺仪数据进行校正能有效减小系统误差；因误差影响而畸变成椭球面的磁力计数据，经椭球拟合算法校正后，数据点分布在以球心为原点的球体表面；校正后的传感器数据经扩展卡尔曼滤波算法融合得到传感器姿态数据，与高精度LPMS-IG1姿态传感器相比，JY901三轴动态数据的均方根误差小于1.5°，最大误差的绝对值小于3°，静态关节角度误差小于1°；计算得到的手指关节数据输入到仿真系统中，能够较准确地控制虚拟灵巧手的动作；系统的延迟时间小于0.06 s，实时性较好。

基于Simscape Mulitybody的虚拟灵巧手仿真系统，可以较真实地模拟机器人的操作环境，便于进行动力学仿真及获取相关数据，本文设计的数据手套及其仿真系统可以为下一步进行真实机械手的控制提供参考。