面向機器人遙操作的數據手套設計及仿真驗證

苑 朝，王少君，蔣 陽，王 祝，翟永杰

(華北電力大學自動化系，河北保定 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的虛擬靈巧手仿真系統，可以較真實地模擬機器人的操作環境，便于進行動力學仿真及獲取相關數據，本文設計的數據手套及其仿真系統可以為下一步進行真實機械手的控制提供參考。