胡旭輝 王鈺

摘要：針對(duì)上肢康復(fù)機(jī)器人的被動(dòng)康復(fù)訓(xùn)練，建立了一套基于慣性傳感器且用于UR協(xié)作機(jī)器人的實(shí)時(shí)路徑規(guī)劃的示教系統(tǒng)。利用固定于示教者手心處的慣性傳感器，實(shí)時(shí)獲取手部相對(duì)于大地坐標(biāo)系的位姿數(shù)據(jù)，經(jīng)過坐標(biāo)變化，轉(zhuǎn)變?yōu)闄C(jī)器人末端在機(jī)器人機(jī)架坐標(biāo)系中的位姿數(shù)據(jù)，再由通訊系統(tǒng)傳給機(jī)器人系統(tǒng)，最后經(jīng)逆向運(yùn)動(dòng)學(xué)轉(zhuǎn)變?yōu)闄C(jī)器人各關(guān)節(jié)坐標(biāo)數(shù)據(jù)，進(jìn)而實(shí)現(xiàn)機(jī)器人帶動(dòng)患者上肢進(jìn)行康復(fù)運(yùn)動(dòng)?，F(xiàn)場測試驗(yàn)證了該系統(tǒng)功能的有效性。

關(guān)鍵詞：康復(fù)機(jī)器人;慣性傳感器;被動(dòng)運(yùn)動(dòng);坐標(biāo)變換

中圖分類號(hào)：TP24???????? 文獻(xiàn)標(biāo)志碼：A

目前，中國腦卒中的發(fā)病率逐年增長，且趨于年輕化，預(yù)計(jì)到2030年將比2010年升高近50%，其中70%～85%的卒中后患者會(huì)伴有偏癱后遺癥[1]，即半側(cè)肢體的運(yùn)動(dòng)失能。肢體失能的程度分為三級(jí)，一級(jí)是最嚴(yán)重的情況[2]，此時(shí)患者的運(yùn)動(dòng)康復(fù)需采用被動(dòng)運(yùn)動(dòng)康復(fù)模式，即患者的上肢或下肢需在外力的幫助下進(jìn)行長時(shí)間的特定運(yùn)動(dòng)。顯然，輔助患者運(yùn)動(dòng)的外力若靠人工來實(shí)施，勞動(dòng)強(qiáng)度非常大，且難以長時(shí)間保持規(guī)定的運(yùn)動(dòng)動(dòng)作而導(dǎo)致康復(fù)效果欠佳，因而機(jī)器人輔助患者實(shí)施被動(dòng)運(yùn)動(dòng)康復(fù)就成為康復(fù)領(lǐng)域的有效手段[3-7]。機(jī)器人輔助患者做被動(dòng)運(yùn)動(dòng)康復(fù)，無論采用何種結(jié)構(gòu)，都需要先規(guī)劃機(jī)器人的運(yùn)動(dòng)軌跡[8-10]。而傳統(tǒng)示教方式是以屏控設(shè)置軌跡坐標(biāo)點(diǎn)[11]和末端拖拽為主[12-13]，實(shí)踐發(fā)現(xiàn)這種示教方法難于滿足安全性、高效性和快速適應(yīng)性的基本要求[14]。對(duì)此，本文給出了一種基于慣性傳感器示教的方法。先將一只慣性傳感器置于示教者左或右手的中心（綁縛于手背上），通過校準(zhǔn)獲得初始位姿。然后獲取傳感器隨示教者手部運(yùn)動(dòng)生成于大地坐標(biāo)系的方位數(shù)據(jù)，并實(shí)時(shí)變換到手部坐標(biāo)系，再實(shí)時(shí)變換為機(jī)器人的TCP方位坐標(biāo)，通過逆向運(yùn)動(dòng)學(xué)實(shí)時(shí)計(jì)算出機(jī)器人的關(guān)節(jié)坐標(biāo)，以此驅(qū)動(dòng)機(jī)器臂帶動(dòng)患者上肢做被動(dòng)康復(fù)運(yùn)動(dòng)。實(shí)測驗(yàn)證了機(jī)器人末端TCP的運(yùn)動(dòng)方位跟隨慣性傳感器運(yùn)動(dòng)方位的實(shí)時(shí)有效性，并給出了實(shí)際示教運(yùn)動(dòng)過程的演示實(shí)例。

1 慣性示教系統(tǒng)的組成

為了易于普及使用，本文選用了市面上常見的肢體運(yùn)動(dòng)慣性捕捉設(shè)備，型號(hào)為VDSuit，屬于成本和精度都適中的產(chǎn)品，由17只慣性傳感器、1只通訊轉(zhuǎn)換器、若干連線和綁帶所組成，如圖1所示。

該運(yùn)動(dòng)慣性捕捉系統(tǒng)可以完整捕捉上、下肢體關(guān)節(jié)的整體空間運(yùn)動(dòng)，可以選用1只固定于手背中心，專門捕捉左或右上肢手部中心的軌跡數(shù)據(jù)，也可同時(shí)將兩只傳感器各置于左、右手中心，同時(shí)捕捉兩條上肢末端的軌跡方位數(shù)據(jù)。當(dāng)然，也可以全穿戴同時(shí)捕捉上下肢體各關(guān)節(jié)的運(yùn)動(dòng)數(shù)據(jù)，為未來實(shí)現(xiàn)上、下肢協(xié)同運(yùn)動(dòng)康復(fù)提供全面的運(yùn)動(dòng)數(shù)據(jù)。本文的研究只限于1只固定于左手或右手中心的慣性傳感器，運(yùn)動(dòng)數(shù)據(jù)將變換給UR10機(jī)器人，最終驅(qū)動(dòng)機(jī)器人的TCP沿慣性傳感器的運(yùn)動(dòng)軌跡來運(yùn)動(dòng)，以此實(shí)現(xiàn)左上肢或右上肢的被動(dòng)運(yùn)動(dòng)康復(fù)。

2 示教控制算法的實(shí)現(xiàn)

如圖2所示，為該康復(fù)系統(tǒng)的初始狀態(tài)，其中站立者為示教康復(fù)師。有4組坐標(biāo)描述該系統(tǒng)的空間狀態(tài)：慣性系統(tǒng)的大地坐標(biāo)Ce、位于示教者手部的傳感器坐標(biāo)Ch、綁縛患者手部的機(jī)器人末端TCP坐標(biāo)Ct和機(jī)器人機(jī)架坐標(biāo)Cb。傳感器的運(yùn)動(dòng)數(shù)據(jù)首先被描述在Ce中，然后經(jīng)坐標(biāo)變換鏈Ce→Ch→Ct→Cb被映射為機(jī)架坐標(biāo)數(shù)據(jù)，總變換矩陣

EAB? = EAH×HAt×tAB （1）

最后由上位機(jī)進(jìn)行逆向運(yùn)動(dòng)學(xué)計(jì)算出機(jī)器臂的關(guān)節(jié)坐標(biāo)數(shù)據(jù)，并經(jīng)由通訊口發(fā)送給機(jī)器人控制系統(tǒng)，進(jìn)而實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)控制。

2.1 示教控制流程設(shè)計(jì)

基于慣性傳感器控制康復(fù)機(jī)器人的整體算法流程如圖3所示。手部傳感器獲取操作者手部運(yùn)動(dòng)數(shù)據(jù)，在慣性捕捉系統(tǒng)下生成相對(duì)于大地坐標(biāo)系的運(yùn)動(dòng)數(shù)據(jù)，將這些運(yùn)動(dòng)數(shù)據(jù)通過藍(lán)牙通訊傳送給上位機(jī)，在上位機(jī)上實(shí)現(xiàn)一系列坐標(biāo)變換及逆向運(yùn)動(dòng)學(xué)計(jì)算，且時(shí)刻判斷示教動(dòng)作是否超出安全區(qū)域或安全速度，若是，則生成康復(fù)機(jī)器人安全制動(dòng)指令，通過Socket通訊發(fā)送給康復(fù)機(jī)器人，以保護(hù)患者免受二次傷害;反之，生成康復(fù)機(jī)器人運(yùn)動(dòng)控制指令，使得康復(fù)機(jī)器人跟隨操作者做被動(dòng)康復(fù)運(yùn)動(dòng)。

2.2 大地坐標(biāo)系轉(zhuǎn)換為手部初始坐標(biāo)系

如圖4所示，虛線表示操作者手部任意運(yùn)動(dòng)軌跡，點(diǎn)o為操作者手部初始位置，即手部坐標(biāo)系Ch的位置，點(diǎn)i為運(yùn)動(dòng)過程中的任意點(diǎn)，通過固定在操作者手背中部的慣性傳感器，可以獲取任意時(shí)刻操作者手部相對(duì)于大地坐標(biāo)系Ce的方位數(shù)據(jù)。其中，手部坐標(biāo)系Ch的位置Pe=（px，py，pz），其對(duì)應(yīng)的平移變化矩陣為

ETH? = Trans（px ，py ，pz ）（2）

以及歐拉角Ee=（φe，θe，ψe），本文姿態(tài)變換采用歐拉角中ZYX型，其對(duì)應(yīng)的旋轉(zhuǎn)矩陣為

ERH? = RPY（ψe ，θe ，φe ）（3）

由式（2）、（3）可得到操作者手部坐標(biāo)系Ch在大地坐標(biāo)系下的位姿矩陣為

EAH? = ETH ×ERH? = cos ψe cos θe? cos ψe sin θe sin φe -sin ψe cos φe? cos ψe sin θe cos φe? + sin ψe sin φe? px sin ψe cos θe? sin ψe sin θe sin φe? + cos ψe cos φe? sin ψe sin θe cos φe -cos ψe sin φe? py -sin θe? cos θe sin φe? cos θe cos φe? pz 0 0 0 1（4）

同理，得到在大地坐標(biāo)系Ce下操作者手部任意點(diǎn)i的位姿EAi。設(shè)手部任意點(diǎn)i在手部初始坐標(biāo)系Ch中的位姿為HAi

EAi? = EAH ×HAi （5）

則手部任意點(diǎn)i在手部初始坐標(biāo)系Ch下的位姿矩陣為

HAi? = （EAH ）-1×EAi （6）

因此可得到手部任意點(diǎn)i在手部初始坐標(biāo)系Cn下的位姿為

HAi? = （hpxi ，hpyi ，hpzi ，φhi，θhi ，ψhi ）（7）

2.3 手部坐標(biāo)系轉(zhuǎn)換為工具末端坐標(biāo)系

由于要使機(jī)器人TCP與帶有慣性傳感器的手部進(jìn)行同步運(yùn)動(dòng)，以及位置與姿態(tài)運(yùn)動(dòng)變化同步，即要使機(jī)器人末端在TCP初始坐標(biāo)系Ct中的運(yùn)動(dòng)狀態(tài)完全與手部在手部初始坐標(biāo)系Ch中的運(yùn)動(dòng)狀態(tài)相同，于是可得到機(jī)器人末端在坐標(biāo)系Ct下隨手部運(yùn)動(dòng)的位姿為Ptcp=（xtcp，ytcp，ztcp，φtcp，θtcp，ψtcp）。其中

xtcp ytcpztcpφtcpθtcpψtcp = 0 -1 0 0 0 0-1 0 0 0 0 00 0 -1 0 0 00 0 0 0 -1 00 0 0 -1 0 00 0 0 0 0 -1×hpxi hpyi hpzi φhi θhi ψhi （8）

2.4 TCP坐標(biāo)系轉(zhuǎn)換為機(jī)器人機(jī)架坐標(biāo)系

本文所選用的機(jī)器人為UR10，建立其初始關(guān)節(jié)坐標(biāo)系如圖5所示，x0y0z0為機(jī)器人機(jī)架坐標(biāo)系，x6 y6 z6為機(jī)器人末端TCP坐標(biāo)系。

根據(jù)鏈?zhǔn)綑C(jī)器人臂的運(yùn)動(dòng)學(xué)理論，得到機(jī)器人機(jī)架坐標(biāo)系到機(jī)器人末端TCP坐標(biāo)系的齊次變換矩陣

BTtcp=A1 A2 A3 A4 A5 A6 （9）

其中

Ai+1=cos θi+1-sin θi+1cos αi+1sin θi+1sin αi+1ai+1cos θi+1sin θi+1cos θi+1cos αi+1-cos θi+1sin αi+1ai+1sin θi+10sin αi+1cos αi+1di+10001

可得在機(jī)架坐標(biāo)系下，機(jī)器人TCP隨操作者手部運(yùn)動(dòng)的位姿，通過控制器，求出該位姿所對(duì)應(yīng)的各關(guān)節(jié)旋轉(zhuǎn)角，使得康復(fù)機(jī)器人跟隨著示教者做相應(yīng)的康復(fù)運(yùn)動(dòng)。

3 示教運(yùn)動(dòng)過程測試

3.1 跟隨精度與重復(fù)精度

在坐標(biāo)變換系統(tǒng)，操作者帶動(dòng)康復(fù)機(jī)器人做一組連續(xù)運(yùn)動(dòng)，以檢測：（1）跟隨精度，即機(jī)器人末端TCP能否準(zhǔn)確地跟隨操作者手部進(jìn)行運(yùn)動(dòng);（2）重復(fù)精度，即在運(yùn)行一段時(shí)間后，操作者回到初始方位時(shí)，康復(fù)機(jī)器人TCP能否回到所對(duì)應(yīng)的初始方位。具體測量結(jié)果如圖6（a）～（f）所示，其中實(shí)線表示手部傳感器在各方向的位置與姿態(tài)，虛線表示康復(fù)機(jī)器人TCP在各方向的位置與姿態(tài)，水平直線表示手部傳感器與康復(fù)機(jī)器人的初始位姿。

（a）手部傳感器沿x方向及所對(duì)應(yīng)TCP的位置;（b）手部傳感器繞x方向及TCP所對(duì)應(yīng)的姿態(tài);（c）手部傳感器沿y方向及所對(duì)應(yīng)TCP的位置;（d）手部傳感器繞y方向及TCP所對(duì)應(yīng)的姿態(tài);（e） 手部傳感器沿z方向及所對(duì)應(yīng)TCP的位置;（f） 手部傳感器繞z方向及TCP所對(duì)應(yīng)的姿態(tài)在圖6（a）～（f）中，實(shí)線與虛線基本重合，各方向的最大位置跟隨精度為±4 mm，最大姿態(tài)跟隨精度為-0.4°～0.6°，說明機(jī)器人末端TCP能夠準(zhǔn)確地跟隨操作者手部進(jìn)行康復(fù)運(yùn)動(dòng)，因此本文所建立的整套坐標(biāo)變換是正確的。對(duì)于重復(fù)精度，在運(yùn)動(dòng)一段時(shí)間之后，操作者手部回到初始位置，機(jī)器人末端TCP在x、y、z方向的位置與其初始位置的誤差分別為19.54 mm、30.87 mm、8.55 mm，各方向的姿態(tài)誤差分別為-0.18°、0.278°、-1.13°，因?yàn)槭植總鞲衅鲾?shù)據(jù)漂移與誤差累積的緣故。因此，可通過誤差補(bǔ)償?shù)姆椒p小各方向的位置誤差，且示教過程最好在5 min內(nèi)完成，不易過長。

由于本文所選用的是在能夠滿足精度的前提下，成本較低的慣性傳感器，其在測量時(shí)會(huì)產(chǎn)生數(shù)據(jù)漂移，造成數(shù)據(jù)不準(zhǔn)確，且隨測量時(shí)間的變長，也會(huì)產(chǎn)生誤差積累。

3.2 實(shí)際康復(fù)動(dòng)作測試

為了更加直觀，操作者帶動(dòng)患者做一組倒水的康復(fù)運(yùn)動(dòng)，如圖7（a）～（i）所示，操作者帶動(dòng)患者從初始位置出發(fā)，去抓取裝有水的杯子，帶著患者將水倒入另外一個(gè)空杯中，倒完水后，將杯子放回原來的位置，最后，操作者帶著患者回到初始位置。

4 安全保護(hù)功能的設(shè)計(jì)

安全性是康復(fù)機(jī)器人需要考慮的重要因素之一。示教過程中，示教者穿戴慣性傳感器的手有可能意外失速或超出患者的安全運(yùn)動(dòng)區(qū)域，造成患者的二次傷害，故該系統(tǒng)采用了速度安全保護(hù)與限位空間保護(hù)的控制措施。

4.1 速度安全保護(hù)

速度安全保護(hù)的關(guān)鍵點(diǎn)在于如何給定安全速度值。本文根據(jù)調(diào)研與實(shí)際測試可知，當(dāng)機(jī)器人TCP的加速度不超過重力加速度的30%時(shí)，手部綁縛于機(jī)器人TCP處的患者在感覺上是可接受的，此時(shí)若TCP的速度為零，則1 s后其速度將接近300 mm/s，故為了簡化判斷過程，認(rèn)為TCP在任意時(shí)刻的速度達(dá)到300 mm/s，控制系統(tǒng)就應(yīng)制動(dòng)。進(jìn)一步測試發(fā)現(xiàn)，盡管TCP的加速度很小，但讓其長時(shí)間保持一個(gè)較高速度運(yùn)動(dòng)，則該速度不宜超過275 mm/s為合適。本文選定最大安全速度為275 mm/s，如圖8所示?？刂葡到y(tǒng)實(shí)時(shí)監(jiān)測康復(fù)機(jī)器人末端x、y和z方向的合成速度，若合成速度達(dá)到最大安全速度時(shí)，即可對(duì)康復(fù)機(jī)器人發(fā)出停止運(yùn)動(dòng)指令，以達(dá)到保護(hù)患者的作用。

4.2 限位空間保護(hù)

對(duì)于限位空間保護(hù)，以患者的肩關(guān)節(jié)為中心，患者的手伸向遠(yuǎn)端時(shí)，不得超過上肢伸直的長度，建立約束平面Ⅰ;患者的手向近端移動(dòng)時(shí)，不得觸及自己的軀干，建立約束平面Ⅱ;上肢向下屈的范圍，則不能越過桌面或觸及患者自身的下肢，建立約束平面Ⅲ，而向上展的范圍，則根據(jù)患者的具體情況設(shè)定，但不能超過180°，建立約束平面Ⅳ。如圖9（a）、（b）所示，以肩關(guān)節(jié)為原點(diǎn)，肩部方向?yàn)閤軸，手臂前伸方向?yàn)閥軸，垂直向上為z軸，建立空間坐標(biāo)系，則約束平面Ⅰ的參數(shù)方程為

x=Lcos αy=Lsin αz=t （10）

其中，α∈[π6，5π6]，t∈（-SymboleB@，+SymboleB@），L表示臂長，α表示手臂繞Z軸旋轉(zhuǎn)角度。

約束平面II的方程為

A2x+B2y+C2z-d2=0A2=C2=0B2=1 （11）

其中，d2表示近端安全距離。

約束平面Ⅲ、Ⅳ的方程為

A3x+B3y+C3z+d3=0A4x+B4y+C4z-d4=0A3=B3=A4=B4=0C3=C4=1 （12）

其中，d3、d4表示手臂下屈與上展的安全高度。

限位空間的實(shí)際尺寸，可根據(jù)不同患者的實(shí)際肢體尺寸具體設(shè)定，控制系統(tǒng)實(shí)時(shí)監(jiān)測機(jī)器人末端的位置，若TCP超出限位空間，即刻對(duì)康復(fù)機(jī)器人發(fā)出停止運(yùn)動(dòng)指令。

5 結(jié)論

本文基于慣性傳感器建立了一套用于被動(dòng)運(yùn)動(dòng)康復(fù)的示教系統(tǒng)，該系統(tǒng)對(duì)安全性的實(shí)現(xiàn)給予了充分的重視，通過對(duì)速度安全與限位空間安全的實(shí)現(xiàn)，避免了因示教者的意外失誤對(duì)患者可能造成的二次傷害。同時(shí)，該系統(tǒng)在一定的時(shí)間范圍內(nèi)，能夠更加安全、更加有效地進(jìn)行康復(fù)示教，解決了傳統(tǒng)康復(fù)機(jī)器人示教難的問題。后續(xù)研究將通過誤差補(bǔ)償?shù)姆椒ㄟM(jìn)一步減小傳感器的累積誤差，使得該系統(tǒng)能夠更加精確、更加持久地對(duì)康復(fù)機(jī)器人進(jìn)行示教。

參考文獻(xiàn)

[1]王隴德，劉建民，楊弋，等.我國腦卒中預(yù)防仍面臨巨大挑戰(zhàn)——《中國腦卒中防治報(bào)告2018》概要[J].中國循環(huán)雜志，2019，34（2）：105-119.

[2]張晶，王斌全，魏邵輝.腦卒中失能老年患者輔助器具使用情況的調(diào)查分析[J].護(hù)理學(xué)雜志，2017，32（1）：85-88.

[3]楊啟志，曹電鋒，趙金海.上肢康復(fù)機(jī)器人研究現(xiàn)狀的分析[J].機(jī)器人，2013，35（5）：630-640.

[4]KWAKKEL G， WAGENAAR R C， TWISK J W R， et al. Intensity of leg and arm training after primary middle-cerebral-artery stroke： A randomized trial[J].The Lancet，1999，354（9174）：191-196.

[5]王娟.上肢康復(fù)機(jī)器人訓(xùn)練對(duì)腦卒中后偏癱患者上肢功能的影響[J].按摩與康復(fù)醫(yī)學(xué)，2017，8（7）：21-22.

[6]RHON D I， GILL N， TEYHEN D， et al. Clinician perception of the impact of deployed physical therapists as physician extenders in a combat environment[J].Military Medicine，2010，175（5）：305-312.

[7]孫超，苑明海，周灼，等. 外骨骼上肢康復(fù)機(jī)器人的結(jié)構(gòu)設(shè)計(jì)與仿真研究[J]. 機(jī)電工程，2019，36（4）：383-386.

[8]KIM H， MILLER L M， FEDULOW I， et al. Kinematic data analysis for post-strokepatients following bilateral versus unilateral rehabilitation with an upper limb wearable robotic system[J]. IEEE Transactions on Neural Systems and Rehabilitation Engineering， 2013， 21（2）：153-164.

[9]TANG L W， TANG X Q， JIANG X L， et al. Dynamic trajectory planning study of planar two-dof redundantly actuated cable-suspended parallel robots[J]. Mechatronics， 2015， 30： 187-197.

[10] ABU-DAKKA F J， RUBIO F， VALERO F， et al. Evolutionary indirect approach to solving trajectory planning problem for industrial robots operating in workspaces with obstacles[J]. European Journal of Mechanics A-Solids， 2013， 42： 210-218.

[11] 計(jì)時(shí)鳴，黃希歡.工業(yè)機(jī)器人技術(shù)的發(fā)展與應(yīng)用綜述[J].機(jī)電工程，2015，32（1）：1-12.

[12] 李占賢，李興龍.一種新型手把手示教型施釉機(jī)器人[J].機(jī)械，2015，42（1）：67-69.

[13] 劉乃軍，魯濤，蔡瑩皓，等.機(jī)器人操作技能學(xué)習(xí)方法綜述[J].自動(dòng)化學(xué)報(bào)，2019，45（3）：458-470.

[14] KAWASAKI H， NANMO S， MOURI T，et al. Virtual robot teaching for humanoid hand robot using muti-fingered haptic interface[C]// 2011 International Conference on Communications， Computing and Control Applications （CCCA），Tunisia，2011： 1-6.

Research on Remote Teaching and Rehabilitation System Based on Inertial Sensor

HU Xu-hui， WANG Yu

（College of Mechanical and Electrical Engineering， Qingdao University， Qingdao 266071， China）

Abstract：

Aiming at the passive rehabilitation training of upper limb rehabilitation robots， a teaching system based on inertial sensors and used for real-time path planning of UR collaborative robots was established in this survey. Firstly， an inertial sensor fixed on the palm of the teacher was used to obtain the pose data of the hand relative to the earth coordinate system in real time. After a series of coordinate changes， it was transformed into the position and posture data of the robot′s tool in the coordinate system of the robot's frame. Then， it was transferred from the communication system to the robot system. Finally， it was transformed into the coordinate data of each joint of the robot through reverse kinematics. By testing， the rehabilitation system has shown an excellent result.

Keywords：

rehabilitation robot; inertial sensor; passive motion; coordinate transformation

收稿日期：2021-06-29

基金項(xiàng)目：

山東省科技發(fā)展計(jì)劃項(xiàng)目（批準(zhǔn)號(hào)：40214010075）資助。

通信作者：

王鈺，男，副教授，主要研究的方向?yàn)榭祻?fù)機(jī)器人與打磨機(jī)器人。E-mail：ywang@qdu.edu.cn

2843501186293