苑 朝，王少君，蔣 陽(yáng)，王 祝，翟永杰

(華北電力大學(xué)自動(dòng)化系，河北保定 071000)

0 引言

近年來(lái)，隨著機(jī)器人技術(shù)的發(fā)展，其應(yīng)用場(chǎng)景越來(lái)越復(fù)雜多樣，這對(duì)機(jī)器人操作提出了更高的要求。由于受到機(jī)構(gòu)、控制、人工智能和傳感技術(shù)水平的限制，發(fā)展能在未知或復(fù)雜環(huán)境下工作的全自主式智能機(jī)器人很難實(shí)現(xiàn)[1]，基于人機(jī)交互的機(jī)器人遙操作技術(shù)成為了解決當(dāng)前問(wèn)題的方案之一。

體感人機(jī)交互是一種自然、高效的人機(jī)交互方式，而人手作為人體最靈活的部位之一，更加適合作為人機(jī)交互系統(tǒng)的主操作端。相較于傳統(tǒng)的鼠標(biāo)、鍵盤(pán)、手柄等交互方式，數(shù)據(jù)手套可以獲取手部更多的運(yùn)動(dòng)信息，使操作更加靈活。數(shù)據(jù)手套通常以在手套背部布置傳感器的方式來(lái)實(shí)現(xiàn)對(duì)穿戴者手指關(guān)節(jié)角度的測(cè)量。文獻(xiàn)[2]中設(shè)計(jì)了一款基于壓阻式彎曲傳感器的數(shù)據(jù)手套，用于機(jī)械手的遙操作，但該手套每根手指只布置了1個(gè)彎曲傳感器，不能準(zhǔn)確測(cè)量手指每個(gè)關(guān)節(jié)的角度。文獻(xiàn)[3]中基于Cyber GloveII數(shù)據(jù)手套設(shè)計(jì)了1個(gè)多關(guān)節(jié)靈巧手混合遙操作系統(tǒng)，Cyber GloveII數(shù)據(jù)手套使用光纖傳感器測(cè)量手指彎曲角度，價(jià)格較高，提高了系統(tǒng)的成本。

近年來(lái)微機(jī)電系統(tǒng)(micro-electro-mechanical system，MEMS)技術(shù)的進(jìn)步推動(dòng)了傳感器向小型化、高精度、高可靠性、低成本方向發(fā)展，采用慣性測(cè)量單元制作數(shù)據(jù)手套成為了主要選擇之一。2017年，趙裕沛采用11個(gè)9軸慣性測(cè)量單元設(shè)計(jì)了一款面向手部康復(fù)的數(shù)據(jù)手套[4]，由于遠(yuǎn)節(jié)指骨背部沒(méi)有布置傳感器，遠(yuǎn)指關(guān)節(jié)的角度由近指關(guān)節(jié)的角度通過(guò)數(shù)學(xué)關(guān)系計(jì)算得到，對(duì)一些特殊手勢(shì)，不能很準(zhǔn)確得到。清華大學(xué)設(shè)計(jì)了一款名為YoBu的數(shù)據(jù)手套用于遙操作機(jī)械手和機(jī)械臂[5-6]，該手套使用了18個(gè)慣性測(cè)量單元，分別測(cè)量人手部和手臂的姿態(tài)，但在傳感器校正部分只考慮了主要誤差。相似地，文獻(xiàn)[7]也基于18個(gè)慣性測(cè)量單元設(shè)計(jì)了一款數(shù)據(jù)手套，對(duì)于傳感器的校正也只校正了零偏誤差。

針對(duì)面向遙操作數(shù)據(jù)手套存在的精度低、成本高、基于慣性測(cè)量單元的數(shù)據(jù)手套校正難、校正不完全等問(wèn)題，本文面向機(jī)器人遙操作仿真方向，基于慣性測(cè)量單元設(shè)計(jì)了一款數(shù)據(jù)手套，將15個(gè)慣性測(cè)量單元布置于手套背部用于采集人手指骨的運(yùn)動(dòng)信息；采用Levenberg-Marquardt(LM)算法和橢球擬合算法對(duì)傳感器數(shù)據(jù)進(jìn)行了校正，校正后的數(shù)據(jù)通過(guò)擴(kuò)展卡爾曼濾波進(jìn)行融合；然后由融合后的數(shù)據(jù)計(jì)算得到各關(guān)節(jié)角，并輸入到Simscape Multibody中建立的虛擬靈巧手，以控制其進(jìn)行動(dòng)作。Simscape Multibody是Simulink中廣泛應(yīng)用于機(jī)械仿真的部分，能夠很方便地反映機(jī)械結(jié)構(gòu)的運(yùn)動(dòng)角度、角速度等參數(shù)，還可以進(jìn)行動(dòng)力學(xué)仿真，便于對(duì)控制操作進(jìn)行評(píng)估和分析。

1 數(shù)據(jù)手套系統(tǒng)設(shè)計(jì)

1.1 人手部結(jié)構(gòu)與運(yùn)動(dòng)分析

人手的骨骼結(jié)構(gòu)由腕骨、掌骨、指骨和關(guān)節(jié)構(gòu)成[8]，如圖1所示。指骨包括近節(jié)指骨、中節(jié)指骨和遠(yuǎn)節(jié)指骨，大拇指只有近節(jié)指骨和遠(yuǎn)節(jié)指骨，其他4根手指都由上述3種指骨構(gòu)成。骨骼間由關(guān)節(jié)連接，其中掌指關(guān)節(jié)(metacarpophalangeal joint，MPJ)連接掌骨與指骨；大拇指的指間關(guān)節(jié)(interphalangeal joint，IPJ)連接近節(jié)指骨與遠(yuǎn)節(jié)指骨；其余4根手指的指骨之間分別由近指關(guān)節(jié)(proximal interphalangeal joint，PIPJ)、遠(yuǎn)指關(guān)節(jié)(distal interphalangeal joint，DIPJ)連接。

圖1 人手骨骼與關(guān)節(jié)結(jié)構(gòu)

人手到靈巧手的運(yùn)動(dòng)映射方法分為4類[9]：指尖映射、關(guān)節(jié)映射、關(guān)鍵點(diǎn)映射和基于被操作物體的映射，本文采用便于控制的關(guān)節(jié)角映射。為了便于進(jìn)行運(yùn)動(dòng)分析，將人手骨骼結(jié)構(gòu)簡(jiǎn)化為連桿結(jié)構(gòu)，如圖2所示。每個(gè)指間關(guān)節(jié)、近指關(guān)節(jié)、遠(yuǎn)指關(guān)節(jié)都只有1個(gè)彎曲自由度，而掌指關(guān)節(jié)有2個(gè)自由度，即彎曲自由度和外展自由度。調(diào)查發(fā)現(xiàn)部分常見(jiàn)的五指靈巧機(jī)械手不具備掌指關(guān)節(jié)的外展自由度[10-11]，并且在沒(méi)有外展自由度的情況下，五指靈巧機(jī)械手也能很靈活地完成任務(wù)[12]，因此在手部建模過(guò)程中，只關(guān)注掌指關(guān)節(jié)的彎曲自由度，忽略其外展自由度。簡(jiǎn)化后的手部結(jié)構(gòu)只需要得到14個(gè)關(guān)節(jié)(如圖1)的彎曲角度就能得到各手指的姿態(tài)，這14個(gè)關(guān)節(jié)彎曲角度的范圍如表1所示。

圖2 人手手指簡(jiǎn)化結(jié)構(gòu)及傳感器安裝方式

表1 手指關(guān)節(jié)彎曲角度的范圍

1.2 數(shù)據(jù)手套系統(tǒng)結(jié)構(gòu)

面向機(jī)器人遙操作的數(shù)據(jù)手套及其仿真系統(tǒng)的整體結(jié)構(gòu)如圖3所示。

圖3 系統(tǒng)整體結(jié)構(gòu)圖

數(shù)據(jù)手套的硬件部分主要包括15個(gè)JY901慣性測(cè)量單元、1個(gè)STM32F103單片機(jī)和1對(duì)藍(lán)牙模塊。為了更準(zhǔn)確地測(cè)得手指關(guān)節(jié)的彎曲角度，各JY901以X軸互相平行的方式布置于掌骨和14個(gè)手指指骨對(duì)應(yīng)的手套背部位置，具體布置方向如圖2所示。STM32布置在手腕背部以不影響手部運(yùn)動(dòng)。STM32通過(guò)IIC總線采集15個(gè)JY901測(cè)得的加速度、角速度和磁場(chǎng)的原始數(shù)據(jù)，并把這些數(shù)據(jù)通過(guò)藍(lán)牙模塊傳輸至PC端處理中心。

數(shù)據(jù)手套的數(shù)據(jù)處理和對(duì)虛擬靈巧手的控制在Simulink中實(shí)現(xiàn)，結(jié)構(gòu)如圖4所示。在Simulink中，讀取并解析藍(lán)牙模塊傳輸?shù)臄?shù)據(jù)，分離得到每個(gè)JY901的加速度、角速度和磁場(chǎng)的原始數(shù)據(jù)。傳感器的原始數(shù)據(jù)經(jīng)過(guò)校正、數(shù)據(jù)融合，得到每個(gè)傳感器的實(shí)時(shí)姿態(tài)，再由每個(gè)傳感器的實(shí)時(shí)姿態(tài)計(jì)算得到每個(gè)關(guān)節(jié)的旋轉(zhuǎn)角，數(shù)據(jù)具體的處理的流程見(jiàn)圖5，把關(guān)節(jié)旋轉(zhuǎn)角傳輸至虛擬靈巧手的旋轉(zhuǎn)關(guān)節(jié)，實(shí)現(xiàn)對(duì)虛擬靈巧手手指運(yùn)動(dòng)的控制。其中，校正補(bǔ)償矩陣離線得到，數(shù)據(jù)解析、校正、數(shù)據(jù)融合在Matlab-Function中完成。

圖4 上位機(jī)結(jié)構(gòu)

圖5 數(shù)據(jù)處理過(guò)程

2 傳感器數(shù)據(jù)校正

本文采用的JY901傳感器，其核心部分MPU9250由加速度計(jì)、陀螺儀、磁力計(jì)集成，由于制作工藝及使用環(huán)境的影響，3種類型的傳感器會(huì)存在誤差，從而導(dǎo)致手指姿態(tài)解算過(guò)程中出現(xiàn)誤差。因此，在進(jìn)行姿態(tài)解算之前，需要對(duì)加速度計(jì)、陀螺儀、磁力計(jì)的測(cè)量數(shù)據(jù)進(jìn)行校正。

2.1 傳感器誤差模型

加速度計(jì)和陀螺儀測(cè)量原理相似，它們的系統(tǒng)誤差都包括非正交誤差、刻度因子誤差、零偏誤差[13]。磁力計(jì)的誤差除了上述3種誤差外，還包含由周圍磁場(chǎng)與磁化物質(zhì)引起的誤差，即硬磁誤差和軟磁誤差[14-15]。在對(duì)傳感器校正之前需要對(duì)每種類型傳感器的誤差進(jìn)行建模。

2.1.1 加速度計(jì)誤差模型

加速度計(jì)的非正交誤差Ta、刻度因子誤差Ka、零偏誤差ba的補(bǔ)償矩陣可以表示為

(1)

式中αij為加速度計(jì)的i軸繞正交坐標(biāo)系j軸旋轉(zhuǎn)的角度。

(2)

(3)

根據(jù)3種誤差對(duì)傳感器的影響情況，可以得到加速度計(jì)的誤差校正模型：

ao=Ga(as+ba)+εa

(4)

2.1.2 陀螺儀誤差模型

陀螺儀的非正交誤差、刻度因子誤差和零偏誤差的補(bǔ)償矩陣表示如下：

(5)

式中：βij為陀螺儀的i軸繞正交坐標(biāo)系j軸旋轉(zhuǎn)的角度。

(6)

(7)

陀螺儀的誤差校正模型為

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

(8)

2.1.3 磁力計(jì)誤差模型

磁力計(jì)的誤差主要包括系統(tǒng)誤差和環(huán)境誤差，其中系統(tǒng)誤差包括非正交誤差、刻度因子誤差和零偏誤差，環(huán)境誤差包括硬磁誤差和軟磁誤差。

磁力計(jì)的非正交誤差矩陣Tm可以表示為3×3的一般矩陣：

(9)

刻度因子誤差矩陣Km表示為

(10)

零偏誤差矩陣bm由x、y、z三軸數(shù)據(jù)的誤差參數(shù)構(gòu)成：

(11)

磁力計(jì)的硬磁誤差矩陣bhi可以表示為由三軸硬磁誤差數(shù)據(jù)構(gòu)成的向量，軟磁誤差矩陣Msi表示為3×3的一般矩陣：

(12)

(13)

根據(jù)不同誤差對(duì)磁力計(jì)數(shù)據(jù)的影響情況，磁力計(jì)的誤差模型表示為

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

(14)

式(14)可以簡(jiǎn)化為

hs=Gmho+b

(15)

所以磁力計(jì)的誤差補(bǔ)償公式為

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

(16)

2.2 傳感器數(shù)據(jù)校正

加速度計(jì)和陀螺儀的系統(tǒng)誤差主要來(lái)源于非正交誤差、刻度因子誤差和零偏誤差，理想情況下，靜止的加速度計(jì)只測(cè)量當(dāng)?shù)氐闹亓铀俣?，因此可以?jù)此構(gòu)造損失函數(shù)，用優(yōu)化算法求誤差矩陣的最優(yōu)值。陀螺儀可以利用校正后的加速度計(jì)數(shù)據(jù)構(gòu)造損失函數(shù)，同樣用優(yōu)化算法求誤差矩陣的最優(yōu)值[13]。磁力計(jì)的誤差除了系統(tǒng)誤差外，還包含硬磁、軟磁誤差，采用橢球擬合算法對(duì)磁力計(jì)數(shù)據(jù)進(jìn)行校正。

2.2.1 加速度計(jì)數(shù)據(jù)校正

在進(jìn)行加速度計(jì)、陀螺儀的校正之前，需要采集每個(gè)傳感器的原始數(shù)據(jù)。首先把JY901靜止放置時(shí)間t，然后把傳感器置于另一個(gè)姿態(tài)再靜止時(shí)間t，這樣重復(fù)N次，得到用于傳感器校正的數(shù)據(jù)集。其中靜止時(shí)間t不少于10 s，N不少于20次。通過(guò)給陀螺儀數(shù)據(jù)向量的模長(zhǎng)設(shè)置閾值將獲得的數(shù)據(jù)集劃分為動(dòng)、靜態(tài)數(shù)據(jù)區(qū)間。加速度計(jì)采用靜態(tài)區(qū)間的數(shù)據(jù)進(jìn)行校正，陀螺儀采用動(dòng)態(tài)區(qū)間的數(shù)據(jù)進(jìn)行校正。

加速度計(jì)的誤差校正模型如式(4)所示，Ga和ba共有9個(gè)未知數(shù)，用向量θa表示：

(17)

加速度計(jì)的誤差校正公式可以寫(xiě)成與θa相關(guān)的函數(shù)形式：

ao=f(as,θa)

(18)

理想情況下，靜態(tài)區(qū)間的加速度數(shù)據(jù)校正后應(yīng)滿足‖ao‖=‖g‖，構(gòu)建加速度計(jì)數(shù)據(jù)的損失函數(shù)，損失函數(shù)最小時(shí)向量θa即為最優(yōu)。

(19)

式中：N為靜態(tài)區(qū)間個(gè)數(shù)；g為本地重力加速度。

針對(duì)損失函數(shù)La(θa)，采用LM算法求其最優(yōu)解，更新公式為

(20)

2.2.2 陀螺儀數(shù)據(jù)校正

(21)

式中M為N個(gè)靜態(tài)區(qū)間的陀螺儀數(shù)據(jù)組總數(shù)。

補(bǔ)償系數(shù)矩陣Gg共有9個(gè)未知參數(shù)，用向量θg表示如下：

(22)

損失函數(shù)定義為

(23)

對(duì)損失函數(shù)Lg(θg)進(jìn)行最優(yōu)化計(jì)算時(shí)，首先需要計(jì)算旋轉(zhuǎn)矩陣Ri。通過(guò)式(24)可以由陀螺儀測(cè)得的角速度計(jì)算得到四元數(shù)的微分形式，再根據(jù)RK4積分可以得到第i個(gè)動(dòng)態(tài)區(qū)間的四元數(shù)，最后根據(jù)四元數(shù)與旋轉(zhuǎn)矩陣的轉(zhuǎn)換關(guān)系可以得到Ri。

四元數(shù)的微分方程形式為：

(24)

(25)

式中：ωx、ωy、ωz為三軸角速度。

(26)

式中：Δt為動(dòng)態(tài)區(qū)間中，采集到相鄰2組數(shù)據(jù)的時(shí)間差。

陀螺儀數(shù)據(jù)的校正也采用LM算法，損失函數(shù)如式(23)，迭代公式為：

(27)

2.2.3 磁力計(jì)數(shù)據(jù)校正

理想情況下，磁力計(jì)三軸的輸出數(shù)據(jù)在空間中的分布應(yīng)為一個(gè)球心在原點(diǎn)的球體，但是由于存在非正交誤差、刻度因子誤差、零偏誤差以及環(huán)境磁場(chǎng)的干擾，其畸變成了球心偏離原點(diǎn)的橢球體。本文采用橢球擬合算法來(lái)校正磁力計(jì)，算法主要分為2步：通過(guò)擬合橢球得到橢球的參數(shù)；把橢球參數(shù)轉(zhuǎn)化為磁力計(jì)的誤差補(bǔ)償矩陣參數(shù)。

校正磁力計(jì)之前，需要重新采集9軸慣性測(cè)量單元的數(shù)據(jù)。在數(shù)據(jù)手套工作的空間內(nèi)，手持JY901在空間中任意旋轉(zhuǎn)3 min，使采集的數(shù)據(jù)盡可能覆蓋整個(gè)橢球面。

誤差存在的情況下，磁力計(jì)數(shù)據(jù)畸變?yōu)橐话愕臋E球，設(shè)橢球方程的一般方程式為

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

(28)

上式中有10個(gè)未知參數(shù)，使用橢球擬合算法找到滿足式(28)的最優(yōu)的參數(shù)組合。

記未知參數(shù)向量為θm：

θm=[abcdefpqrs]T

(29)

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

(30)

(θm)TCθm=1

(31)

若要保證損失函數(shù)Lm(θm)最小時(shí)求得的θm為橢球的方程系數(shù)，需要滿足式(31)的約束條件。根據(jù)拉格朗日乘子法，需要求解的函數(shù)與約束條件寫(xiě)成：

(32)

式中λ為拉格朗日乘子。

(33)

(34)

根據(jù)文獻(xiàn)[16]，式(33)只有一個(gè)正特征根，該特征根對(duì)應(yīng)的特征向量為ν1，再根據(jù)式(34)可以得到ν2，由此可以得到θm的最優(yōu)值。

橢球的方程還可以寫(xiě)成以下的矩陣方程形式：

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

(35)

校正后的磁場(chǎng)數(shù)據(jù)模長(zhǎng)的平方可以表示為

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

(36)

傳統(tǒng)的求解校正矩陣的方法需要測(cè)得當(dāng)?shù)氐牡卮艌?chǎng)強(qiáng)度，而在研究過(guò)程中發(fā)現(xiàn)，在進(jìn)行數(shù)據(jù)融合時(shí)，真正起作用的不是校正后磁場(chǎng)強(qiáng)度的大小，而是磁場(chǎng)矢量的方向，所以本文對(duì)校正矩陣的求解方法進(jìn)行了簡(jiǎn)化：

(37)

對(duì)矩陣A進(jìn)行奇異值分解得到(Gm)-1。求得的(Gm)-1和b是相對(duì)值，所以由式(16)計(jì)算得到的校正后磁場(chǎng)數(shù)據(jù)也是相對(duì)值，但對(duì)于后續(xù)的姿態(tài)解算沒(méi)有影響。

3 測(cè)量單元姿態(tài)解算

擴(kuò)展卡爾曼濾波是卡爾曼濾波在非線性系統(tǒng)中的應(yīng)用[17]，它由預(yù)測(cè)和修正兩部分組成。在預(yù)測(cè)過(guò)程中，當(dāng)前狀態(tài)的預(yù)測(cè)通過(guò)濾波器對(duì)上一時(shí)刻的狀態(tài)值估計(jì)得到，在修正過(guò)程中，需要由觀測(cè)值得到卡爾曼增益，進(jìn)而對(duì)上一步得到的預(yù)測(cè)值進(jìn)行修正。在9軸慣性測(cè)量單元系統(tǒng)中，預(yù)測(cè)值由陀螺儀測(cè)得的角速度數(shù)據(jù)計(jì)算得到，加速度計(jì)和磁力計(jì)數(shù)據(jù)計(jì)算得到的卡爾曼增益用來(lái)修正預(yù)測(cè)值[18]。

擴(kuò)展卡爾曼濾波系統(tǒng)的動(dòng)態(tài)模型和觀測(cè)模型為

(38)

式中：xk為k時(shí)刻的狀態(tài)向量；Zk為k時(shí)刻系統(tǒng)的觀測(cè)量；wk和vk分別為服從N(0，Qk)、N(0，Rk)分布的高斯白噪聲。

根據(jù)四元數(shù)的微分方程可以得到狀態(tài)轉(zhuǎn)移矩陣Φk-1：

(39)

式中：I為單位矩陣；T為時(shí)間。

針對(duì)本系統(tǒng)，擴(kuò)展卡爾曼濾波的核心公式如下：

(40)

式中：上標(biāo)“-”表示預(yù)測(cè)過(guò)程變量；上標(biāo)“+”表示修正過(guò)程變量；觀測(cè)值Zk1由加速度計(jì)的數(shù)據(jù)計(jì)算求得；Zk2由磁力計(jì)的數(shù)據(jù)計(jì)算求得。

式(40)為一次迭代過(guò)程，由此可以得到實(shí)時(shí)的姿態(tài)四元數(shù)qk=[q0q1q2q3]。根據(jù)式(41)把四元數(shù)轉(zhuǎn)化為歐拉角：

(41)

4 實(shí)驗(yàn)結(jié)果與分析

4.1 傳感器數(shù)據(jù)校正結(jié)果

4.1.1 加速度計(jì)數(shù)據(jù)校正結(jié)果

把加速度計(jì)采集的數(shù)據(jù)換算成以“ m/s2”為單位的數(shù)據(jù)，誤差參數(shù)的初始值設(shè)置為θa=[1 0 0 1 0 1 0 0 0]，經(jīng)過(guò)迭代得到加速度計(jì)校正系數(shù)矩陣Ga、零偏誤差補(bǔ)償矩陣ba如下：

(42)

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

(43)

將傳感器以x軸、y軸、z軸分別朝上靜止放置，加速度數(shù)據(jù)校正前后的矢量和見(jiàn)圖6、均方根誤差見(jiàn)表2，校正后的加速度數(shù)據(jù)很接近本地實(shí)際的重力加速度。

圖6 3種靜止姿態(tài)下校正前后的加速度對(duì)比

表2 加速度計(jì)數(shù)據(jù)校正前后的均方根誤差 m/s2

4.1.2 陀螺儀數(shù)據(jù)校正結(jié)果

陀螺儀的數(shù)據(jù)換算成以“rad/s”為單位的數(shù)據(jù)，根據(jù)式(21)，可以計(jì)算得到零偏誤差補(bǔ)償矩陣bg。誤差參數(shù)矩陣的初始值設(shè)為θg=[1 0 0 0 1 0 0 0 1]，經(jīng)過(guò)迭代得到陀螺儀系數(shù)補(bǔ)償矩陣Gg。

(44)

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

(45)

將傳感器分別x軸朝上、y軸朝上、z軸朝上靜止放置得到的陀螺儀數(shù)據(jù)，校正前后的矢量和如圖7所示，表3為陀螺儀數(shù)據(jù)校正前后的均方根誤差，可以看出經(jīng)過(guò)校正，陀螺儀靜止?fàn)顟B(tài)的數(shù)據(jù)誤差有明顯的減小。

圖7 3種靜止姿態(tài)下校正前后角速度對(duì)比

表3 陀螺儀數(shù)據(jù)校正前后的均方根誤差 rad/s

4.1.3 磁力計(jì)數(shù)據(jù)校正結(jié)果

磁力計(jì)測(cè)得的以“μT”為單位的數(shù)據(jù)，取C1中k=4擬合得到如圖8(a)的橢球，經(jīng)式(27)計(jì)算得到橢球的修正矩陣(Gm)-1和b，校正后的數(shù)據(jù)三維散點(diǎn)圖如圖8(b)，圖9為校正得到的球在3個(gè)坐標(biāo)平面的投影，可以看出畸變成橢球的原始數(shù)據(jù)被校正成了球心在原點(diǎn)的球。由于求得的矩陣為相對(duì)值，所以校正后的磁場(chǎng)大小略小于實(shí)際磁場(chǎng)。

(a)原始磁場(chǎng)數(shù)據(jù)

(b)校正后磁場(chǎng)數(shù)據(jù)圖8 校正前后磁場(chǎng)數(shù)據(jù)分布對(duì)比

(a)校正前后數(shù)據(jù)在X-Y平面投影

(b)校正前后數(shù)據(jù)在X-Z平面投影

(c)校正前后數(shù)據(jù)在Y-Z平面投影圖9 校正前后的磁場(chǎng)數(shù)據(jù)點(diǎn)在坐標(biāo)平面的投影

(46)

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

(47)

4.2 精度測(cè)試

4.2.1 靜態(tài)測(cè)試

虛擬靈巧手的單個(gè)關(guān)節(jié)由2個(gè)JY901傳感器橫滾角的角度差控制，把2個(gè)JY901傳感器的X軸分別以0°、30°、60°、90°的角度差固定，實(shí)際采集到的角度數(shù)據(jù)見(jiàn)圖10，4組角度數(shù)據(jù)的平均值與標(biāo)準(zhǔn)差見(jiàn)表4?？梢钥闯?，4組靜態(tài)測(cè)試角度平均誤差的絕對(duì)值均小于1°，且數(shù)據(jù)比較平穩(wěn)。

圖10 靜態(tài)角度測(cè)試

表4 靜態(tài)角度數(shù)據(jù)的平均值與標(biāo)準(zhǔn)差 (°)

4.2.2 動(dòng)態(tài)測(cè)試

動(dòng)態(tài)測(cè)試以高精度、高穩(wěn)定性姿態(tài)傳感器LPMS-IG1的數(shù)據(jù)作為參考標(biāo)準(zhǔn)。LPMS-IG1高精度姿態(tài)傳感器的解析度為0.01°，靜態(tài)誤差小于0.3°，動(dòng)態(tài)誤差小于1°。設(shè)置JY901和LMPS-IG1的傳輸頻率均為100 Hz，把二者固定在一起同步運(yùn)動(dòng)，得到三軸的動(dòng)態(tài)角度曲線如圖11所示。以LPMS-IG1的數(shù)據(jù)為參考數(shù)據(jù)，對(duì)比得到JY901三軸動(dòng)態(tài)角度的誤差見(jiàn)表5。與高精度LPMS-IG1的數(shù)據(jù)相比，JY901三軸動(dòng)態(tài)數(shù)據(jù)的均方根誤差均小于1.5°，最大誤差絕對(duì)值均小于3°，動(dòng)態(tài)性能良好。

圖11 動(dòng)態(tài)角度測(cè)試

表5 動(dòng)態(tài)角度數(shù)據(jù)的均方根誤差與最大誤差 (°)

4.3 系統(tǒng)有效性仿真驗(yàn)證

圖12是數(shù)據(jù)手套的實(shí)物圖，在藍(lán)牙有效傳輸距離內(nèi)，操作者佩戴數(shù)據(jù)手套，做出特定手勢(shì)，人手動(dòng)作對(duì)虛擬手動(dòng)作控制效果如圖13所示?？梢钥闯?，當(dāng)人手做出特定手勢(shì)的動(dòng)作，數(shù)據(jù)手套將測(cè)得的數(shù)據(jù)經(jīng)過(guò)處理后映射到虛擬模型中，虛擬機(jī)械手也做出相應(yīng)的動(dòng)作。

圖12 數(shù)據(jù)手套實(shí)物圖

圖13 數(shù)據(jù)手套與虛擬靈巧手動(dòng)作的對(duì)應(yīng)

以食指關(guān)節(jié)為例，當(dāng)食指從伸直狀態(tài)到彎曲狀態(tài)、再恢復(fù)至伸直狀態(tài)，掌指關(guān)節(jié)MPJ、近指關(guān)節(jié)PIPJ、遠(yuǎn)指關(guān)節(jié)DIPJ的關(guān)節(jié)角度、角速度隨時(shí)間的變化情況如圖14所示。由Simscape Mulitybody搭建的系統(tǒng)，可以很直觀地得到關(guān)節(jié)的實(shí)時(shí)角度與角速度，便于進(jìn)行對(duì)數(shù)據(jù)手套系統(tǒng)的評(píng)估。把JY901自身輸出時(shí)間與CPU時(shí)間進(jìn)行對(duì)比，測(cè)得系統(tǒng)的延遲時(shí)間小于0.06 s。

(a)角度隨時(shí)間變化圖

(b)角速度隨時(shí)間變化圖圖14 食指關(guān)節(jié)角度、角速度隨時(shí)間變化情況

5 結(jié)論

本文基于15個(gè)9軸慣性測(cè)量單元設(shè)計(jì)了一款應(yīng)用于機(jī)器人遙操作的數(shù)據(jù)手套，并在Simulink搭建的系統(tǒng)中進(jìn)行了實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明：采用LM算法對(duì)加速度計(jì)和陀螺儀數(shù)據(jù)進(jìn)行校正能有效減小系統(tǒng)誤差；因誤差影響而畸變成橢球面的磁力計(jì)數(shù)據(jù)，經(jīng)橢球擬合算法校正后，數(shù)據(jù)點(diǎn)分布在以球心為原點(diǎn)的球體表面；校正后的傳感器數(shù)據(jù)經(jīng)擴(kuò)展卡爾曼濾波算法融合得到傳感器姿態(tài)數(shù)據(jù)，與高精度LPMS-IG1姿態(tài)傳感器相比，JY901三軸動(dòng)態(tài)數(shù)據(jù)的均方根誤差小于1.5°，最大誤差的絕對(duì)值小于3°，靜態(tài)關(guān)節(jié)角度誤差小于1°；計(jì)算得到的手指關(guān)節(jié)數(shù)據(jù)輸入到仿真系統(tǒng)中，能夠較準(zhǔn)確地控制虛擬靈巧手的動(dòng)作；系統(tǒng)的延遲時(shí)間小于0.06 s，實(shí)時(shí)性較好。

基于Simscape Mulitybody的虛擬靈巧手仿真系統(tǒng)，可以較真實(shí)地模擬機(jī)器人的操作環(huán)境，便于進(jìn)行動(dòng)力學(xué)仿真及獲取相關(guān)數(shù)據(jù)，本文設(shè)計(jì)的數(shù)據(jù)手套及其仿真系統(tǒng)可以為下一步進(jìn)行真實(shí)機(jī)械手的控制提供參考。