遙操作護理機器人系統(tǒng)的操作者姿態(tài)解算方法研究

左國玉 于雙悅 龔道雄

遙操作護理機器人系統(tǒng)的操作者姿態(tài)解算方法研究

左國玉1,2于雙悅1,2龔道雄1,2

設(shè)計了一種遙操作護理機器人系統(tǒng),為實現(xiàn)從端同構(gòu)式機器人的隨動運動控制,對主端操作者人體姿態(tài)解算方法進行了研究.首先,構(gòu)建由慣性傳感單元構(gòu)成的動作捕捉系統(tǒng),對用作從端機器人動作指令的操作者人體姿態(tài)信息進行采集,采用四元數(shù)法對人體運動原始數(shù)據(jù)進行初步求解.其次,將四元數(shù)法得到的姿態(tài)數(shù)據(jù)解算成依據(jù)仿人結(jié)構(gòu)設(shè)計的護理機器人各關(guān)節(jié)運動的目標(biāo)姿態(tài)角,實現(xiàn)人體姿態(tài)到機器人動作的同構(gòu)性映射.最后,為驗證本文所提姿態(tài)解算方法的性能,設(shè)計了操作者控制護理機器人完成遞送和拿取藥瓶動作的實驗.結(jié)果表明,本文姿態(tài)解算方法的解算性能與參考系統(tǒng)基本相同;在操作者動作姿態(tài)快速變化的時間段,系統(tǒng)仍可獲得較高精度的目標(biāo)姿態(tài)數(shù)據(jù),其誤差在動態(tài)條件下依舊能保持在2%以下;護理機器人可較好地實時復(fù)現(xiàn)操作者的人體動作.本文方法能滿足機器人進行一般護理作業(yè)時對人體姿態(tài)數(shù)據(jù)處理的快速性和準(zhǔn)確性要求.

遙操作護理機器人,姿態(tài)解算,同構(gòu)映射,人體運動跟蹤

DOI10.16383/j.aas.2016.c160137

我國社會的人口老齡化問題日益嚴(yán)重,伴隨而來的醫(yī)療、養(yǎng)老及救助等社會保障問題日益凸顯.同時,由疾病和災(zāi)害等造成的殘障病患也需要充沛的社會保障資源.對于這些有大量護理服務(wù)需求的群體,很多低復(fù)雜度護理工作若也雇用專人護理,則會占用大量而寶貴的人力資源,社會成本高而效率低下.因此研究開發(fā)遙操作護理機器人,以相對低的成本,實現(xiàn)如幫助病患或老人拿藥送水甚至翻身等常規(guī)性護理工作,并遠程集中實施護理作業(yè),將是未來家庭護理工作的發(fā)展方向[1?2].當(dāng)前,由于機器人技術(shù)成熟度還比較低,全自主護理機器人在醫(yī)療康復(fù)領(lǐng)域的普及應(yīng)用短期內(nèi)還很難實現(xiàn).采用機器人遙操作控制方法,依據(jù)護理人員的經(jīng)驗做出決策,對護理機器人進行遙操作控制,實現(xiàn)病患及老人的遠程護理,是現(xiàn)有智能機器人技術(shù)得到實際應(yīng)用的現(xiàn)實而有效的途徑[3?4].

國內(nèi)外有很多機構(gòu)對用于醫(yī)療健康領(lǐng)域的遙操作機器人技術(shù)進行了大量的研究,其中部分研究成果已進入臨床應(yīng)用階段.日本岐阜大學(xué)研制的手指康復(fù)機器人,能幫助患者自行完成手指康復(fù)訓(xùn)練,患者可利用穿戴在健側(cè)的數(shù)據(jù)手套控制穿戴在患側(cè)的康復(fù)機器人,實現(xiàn)雙邊式康復(fù)訓(xùn)練,同時該系統(tǒng)可實時監(jiān)測患者的運動狀態(tài)以獲取訓(xùn)練數(shù)據(jù)[5].瑞士蘇黎世大學(xué)ARMin外骨骼上肢康復(fù)機器人系統(tǒng),可根據(jù)患者需求選擇合適模式進行康復(fù)訓(xùn)練并根據(jù)患者上肢尺寸進行自動調(diào)節(jié)[6].達芬奇手術(shù)機器人[7?8]和宙斯手術(shù)機器人[9]可實現(xiàn)遠距離高精度臨床手術(shù).Melkong機器人可將病人平穩(wěn)托起并送至衛(wèi)生間[10],DEKA機器人可根據(jù)患者殘肢肌電收縮信號控制假肢運動[11].

在機器人遙操作護理作業(yè)過程中,護理員人體動作姿態(tài)的準(zhǔn)確獲取與解算是機器人遙操作主端系統(tǒng)的主要研究內(nèi)容之一.根據(jù)姿態(tài)數(shù)據(jù)獲取方法的作用范圍與可移動屬性,有非穿戴式和穿戴式兩類.Vicon系統(tǒng)是典型的非穿戴式方法,通過在工作空間內(nèi)布置多個攝像頭對被測對象采集數(shù)據(jù),采用圖像識別技術(shù),利用光學(xué)標(biāo)記點位置直接構(gòu)建人體的物理姿態(tài),而無需求解算法解算姿態(tài)[12];NDI Aurora系統(tǒng)基于電磁定位識別方法,通過分布在周圍的三軸線圈產(chǎn)生特定頻率電磁場對被測對象身上的三軸磁傳感器的磁感應(yīng),利用電磁感應(yīng)信號之間的耦合關(guān)系確定被測對象的姿態(tài)[13];用于虛擬現(xiàn)實的Logitech運動捕捉系統(tǒng)基于超聲波聲學(xué)定位方法,實現(xiàn)被測對象姿態(tài)的檢測與獲取[14].在可移動式姿態(tài)獲取方法方面,加州理工大學(xué)研制的手部外骨骼系統(tǒng)利用連桿上布置的應(yīng)變計測量關(guān)節(jié)位置,它是一種可穿戴機械式捕獲系統(tǒng),無需復(fù)雜算法進行姿態(tài)解算[15];PrioVR系統(tǒng)采用多種電子傳感器組成測量裝置,也多用于體感與虛擬現(xiàn)實領(lǐng)域[16].

為使從端護理機器人快速準(zhǔn)確地復(fù)現(xiàn)主端護理員的護理動作,本文針對課題開發(fā)的一種同構(gòu)式遙操作機器人系統(tǒng),研究了操作者動作姿態(tài)的獲取與解算方法,并以護理員人體姿態(tài)作為動作指令輸入,實現(xiàn)從端護理機器人對應(yīng)關(guān)節(jié)的隨動運動控制.這種方法能直接對人體進行接觸測量,在光線、角度和障礙物等外部條件受限的情況下穩(wěn)定可靠地獲取姿態(tài)數(shù)據(jù),可免于分布式視覺采集系統(tǒng)對空間環(huán)境以及識別算法有效性的嚴(yán)格要求,也不受機械式可穿戴式姿態(tài)采集系統(tǒng)結(jié)構(gòu)復(fù)雜、移動不便以及采集數(shù)量有限且精度不高的局限.另一方面,本文姿態(tài)解算方法結(jié)合自主研制的仿人機械臂結(jié)構(gòu),將經(jīng)過處理的人體姿態(tài)數(shù)據(jù)映射成機械臂各關(guān)節(jié)運動信息,從而實現(xiàn)同構(gòu)式從端護理機器人的隨動運動控制.本文內(nèi)容組織如下:第1節(jié)簡要介紹遙操作護理機器人系統(tǒng)的構(gòu)成;第2節(jié)介紹人體運動數(shù)據(jù)的獲取與預(yù)處理;第3節(jié)描述了基于上述運動數(shù)據(jù)的機器人各關(guān)節(jié)目標(biāo)位置的姿態(tài)解算算法;最后通過遙操作機器人遞送和抓取藥瓶的實驗對本文方法的有效性進行了驗證.

1 遙操作護理機器人系統(tǒng)的構(gòu)成

遙操作護理機器人是一種人機協(xié)作機器人系統(tǒng),主要由主端的人體運動捕捉系統(tǒng)和從端與人體同構(gòu)的護理機器人兩部分組成.系統(tǒng)的主端部分獲取護理員的動作姿態(tài)信息后,將其作為實際動作指令傳送給從端的同構(gòu)式護理機器人,機器人根據(jù)指令信息同步跟隨護理員執(zhí)行相同的護理操作;護理機器人同時將現(xiàn)場工況以視覺的形式回送至遠端的護理員,護理員根據(jù)獲得的信息進一步調(diào)整機器人的動作姿態(tài)使機器人實時完成護理作業(yè)任務(wù).護理機器人進行工作時,就如同護理員在被護人員家中進行現(xiàn)場工作一樣.圖1為遙操作護理機器人系統(tǒng)的工作原理.

圖1 “人在環(huán)內(nèi)”的護理機器人遙操作控制Fig.1 Human-in-the-loop tele-control for the nursing robot

圖2所示為本文課題研發(fā)的基于人體姿態(tài)控制的遙操作隨動護理機器人系統(tǒng).系統(tǒng)主端的人體動作捕捉系統(tǒng)采用模塊化設(shè)計方法,由數(shù)據(jù)手套、背心、頭帶、腕帶等穿戴部分構(gòu)成,其上分別安裝數(shù)量不等的獨立數(shù)據(jù)采集模塊(節(jié)點).各節(jié)點單元與主控單元一起,共同組成了完整的人體運動捕捉系統(tǒng).這些節(jié)點單元通過CAN總線進行連接,并將各自采集的數(shù)據(jù)存放在主控單元中,經(jīng)過處理后,再由主控單元通過無線網(wǎng)絡(luò)發(fā)送至從端護理機器人并控制其操作.從端護理機器人由半身仿人機器人和可自主移動的地面移動平臺構(gòu)成,機器人的機械臂采用與人臂同構(gòu)的仿生設(shè)計方法.機器人上半身共有25個自由度,其中頭部的左右和上下2個自由度用于調(diào)整機器人的視覺范圍;腰部1個自由度完成機器人軀干部分的直立和前傾動作;機體兩側(cè)的機械臂各有6個自由度,手臂的仿生化設(shè)計可以使機器人完成模擬人手臂的運動,其中人體肩部球狀關(guān)節(jié)在仿人機械臂設(shè)計上以串聯(lián)的三自由度形式表示;左右手指各5個自由度可完成模擬人手的抓取動作.護理機器人的控制單元根據(jù)接收到的主端控制信號完成護理操作.

圖2 遙操作護理機器人系統(tǒng)Fig.2 Telerobotic nursing robot system

由于工作對象的特殊性,在人機交互過程中,護理機器人相對一般工業(yè)機器人,其作業(yè)方式對機器人的類人動作特性要求很高[17?18].從端的護理機器人采用了同構(gòu)化設(shè)計,從物理形態(tài)上能夠采用主從兩端系統(tǒng)對應(yīng)關(guān)節(jié)直接映射的方法,這樣機器人各關(guān)節(jié)的運動可在主端通過姿態(tài)求解得到.相較于主從端異構(gòu)的機器人遙操作系統(tǒng),無需專門算法克服人與機器人的主從臂之間的運動學(xué)差異,即可實現(xiàn)從臂依據(jù)指令隨主臂沿著相同的軌跡進行運動.因此,整個系統(tǒng)的遙操作控制過程符合人的日常動作習(xí)慣,使遙操作控制過程更加直觀和方便,能降低遙操作控制的難度和護理人員的工作負擔(dān),較好地保證系統(tǒng)工作的低失誤率.而機器人的類人動作效果也能使被護人員心理上更容易接受機器人的護理服務(wù).進一步,可以用一對多的方式使更多被護人共享主端的護理資源,降低護理成本.

2 人體運動數(shù)據(jù)的預(yù)處理

2.1 原始數(shù)據(jù)的采集與處理

如上一節(jié)所述,遙操作機器人護理系統(tǒng)的主端運動捕捉采集服部分由多個獨立慣性測量單元組成,用于獲取人體各關(guān)節(jié)的姿態(tài)信息.每個測量單元由9軸慣性導(dǎo)航系統(tǒng)構(gòu)成,包含三軸陀螺儀、加速度計及磁力計.利用加速度計和磁力計計算得到陀螺儀的誤差,通過補償器與陀螺儀測量的數(shù)據(jù)進行融合,從而求出實時準(zhǔn)確的角速度值.人體動作姿態(tài)數(shù)據(jù)可以采用歐拉角法、方向余弦法及四元數(shù)法來表示[19?21].相比于歐拉角法存在萬向鎖現(xiàn)象,以及方向余弦法具有較大的運算量,四元數(shù)法既可表達全姿態(tài),所需的計算量又相對較小,因此選用四元數(shù)法可保證姿態(tài)解算的實時性[22?23].

在數(shù)據(jù)解算和融合處理過程中,要對原始數(shù)據(jù)進行濾波處理.目前,主要的濾波方法有擴展卡爾曼濾波法[24]、梯度下降法[25]和互補濾波法[26?27]等.其中,卡爾曼濾波器設(shè)計較為復(fù)雜且運算量較大,而且由于人體運動姿態(tài)較為復(fù)雜,不同關(guān)節(jié)的運動特征也不盡相同,這將進一步增加卡爾曼濾波器的設(shè)計復(fù)雜度;梯度下降法雖然在姿態(tài)解算過程中只涉及加法和乘法運算,但對于不同關(guān)節(jié)的運動特征,使用該方法困難的地方在于整體姿態(tài)解算系統(tǒng)需要確立多個目標(biāo)函數(shù);而互補濾波器結(jié)構(gòu)設(shè)計簡單,而且對高頻和低頻的噪聲輸入信號進行融合處理具有更好的效果.因此,互補濾波法非常適合用于本系統(tǒng)所用到的加速度計、磁力計和陀螺儀器件在不同的高、低頻運動特性下的數(shù)據(jù)融合.本文采用基于四元數(shù)法結(jié)合互補濾波的方式對原始人體姿態(tài)數(shù)據(jù)進行初步的姿態(tài)解算.

基于四元數(shù)法的姿態(tài)解算如圖3所示.

圖3 四元數(shù)姿態(tài)解算框圖Fig.3 Block diagram of attitude calculation based on quaternion

圖3中,?0,θ0,γ0為慣導(dǎo)系統(tǒng)的初始姿態(tài)角;q0為初始四元數(shù);F(ω)為角速度矩陣;為通過解算實時更新的四元數(shù).將?0,θ0,γ0代入C0(s)中得到四元數(shù)初值:

系統(tǒng)每進行一次解算過程,得到一組新的角速度矩陣F(ω),可完成一次四元數(shù)?q的更新.

式中,ωgx,ωgy,ωgz是對于x,y,z三軸的角速度.

在實際應(yīng)用中,陀螺儀通過積分可在較短時間內(nèi)獲得準(zhǔn)確姿態(tài)信息,但由于器件自身特性,受溫度影響較大,易產(chǎn)生漂移,通過積分過程不斷累積誤差,而且在低頻區(qū)產(chǎn)生很大的噪聲,使采集的數(shù)據(jù)嚴(yán)重偏離真實值.相對于陀螺儀、三軸加速度計和三軸磁力計不能直接獲得角速度,且有短期內(nèi)精度差、動態(tài)性能慢的劣勢,但其測量誤差不會隨時間進行累積,低頻特性較好.因此計算過程中,通過多傳感器融合和補償求得準(zhǔn)確的角速度值,利用一階龍格-庫塔法取代式(2)實現(xiàn)四元數(shù)?q的更新.

其中,ωx,ωy,ωz表示角速度,T為慣導(dǎo)系統(tǒng)的采樣周期.

2.2 數(shù)據(jù)融合處理

根據(jù)前一節(jié)所述,陀螺儀和加速度、磁力計具有不同的頻率特性.通過恰當(dāng)構(gòu)造互補濾波器,將三軸陀螺儀測量的角速度值ωg、三軸加速度計的加速度值αg和三軸磁力計的磁強值mg這三種數(shù)據(jù)進行融合補償處理,可以得到準(zhǔn)確的角速度值ωx,ωy,ωz.數(shù)據(jù)融合與補償過程如圖4所示.

圖4 慣導(dǎo)四元數(shù)求解框圖Fig.4 Quaternion calculation based on inertial sensors

圖4中,ag,mg,ωg為加速度、磁強、陀螺儀測量值;為測量值標(biāo)定值;為標(biāo)準(zhǔn)化后值;為通過互補濾波器的加速度、磁強最優(yōu)估計值;ea為俯仰角Pitch和橫滾角Roll的誤差;em為偏航角Yaw的誤差;e為通過加速度計和磁力計求得的總誤差;δ為補償后的角速度值;F(ω)為角速度矩陣;為通過解算實時更新的四元數(shù).

在計算過程中,由于傳感器直接測量值ag,mg,ωg存在一定的隨機誤差和噪聲,而與航天器、平衡車等應(yīng)用相比,人體姿態(tài)數(shù)據(jù)所需采樣頻率低,可通過設(shè)計IIR Butterworth數(shù)字濾波器對測量值進行預(yù)處理,消除交變噪聲.對各傳感器進行標(biāo)定,得到進行標(biāo)準(zhǔn)化處理后,得到用于后續(xù)計算的

以“東北地”作為導(dǎo)航坐標(biāo)系,由于重力加速度方向指向z軸,可求出加速度測量值通過互補濾波器的最優(yōu)估計值

將磁強值轉(zhuǎn)換到該坐標(biāo)系得到mh.由于地球磁場為南北向,東西方向為0,可求出校準(zhǔn)后的地磁場矢量表達式進而求出磁力計通過互補濾波器的最優(yōu)估計值

將加速度測量值與加速度最優(yōu)估計值叉乘,得到的法向量與俯仰角Pitch和橫滾角Roll的誤差成正比,用ea表示.同理得到偏航角Yaw的誤差em.兩者疊加可得姿態(tài)的誤差為e.

根據(jù)陀螺儀與加速度計、磁羅盤之間的不同特性,設(shè)計互補濾波器

式中,δ為誤差e通過一個比例積分模塊后生成的對陀螺漂移后的補償量,Kp為比例系數(shù),Ki為積分系數(shù).

調(diào)節(jié)Kp和Ki這兩個參數(shù)可以得到陀螺儀參數(shù)的修正量,進行疊加得到補償后的角速度值,將經(jīng)過補償后的角速度值代入式(3)中,完成一次四元數(shù)?q的更新.如此這樣,就可實現(xiàn)對原始姿態(tài)數(shù)據(jù)的四元數(shù)表示.

3 操作者運動信息的目標(biāo)姿態(tài)解算

由于以四元數(shù)表示的人體姿態(tài)并不能直接作為從端護理機器人各關(guān)節(jié)控制的給定輸入,因此需將初步解算后的人體運動信息進一步解算為機器人各關(guān)節(jié)的運動信息,使從端機器人與護理員的肢體運動軌跡直接對應(yīng),實現(xiàn)機器人的同構(gòu)化遙操作控制.這樣,整個機器人遙操作系統(tǒng)的人機同構(gòu)映射的姿態(tài)解算方法如圖5所示.

圖5 人機同構(gòu)運動的姿態(tài)解算流程Fig.5 Motion mapping method from human operator to slave robot

上一節(jié)的預(yù)處理過程將原始人體姿態(tài)數(shù)據(jù)解算為不同的四元數(shù)組,下面將詳細描述由人體姿態(tài)四元數(shù)組到機器人各關(guān)節(jié)位置信息的姿態(tài)映射方法,分別實現(xiàn)頸、肩、肘和腕等關(guān)節(jié)的各姿態(tài)角的求解.

不失一般性,我們以機器人右大臂為例進行機器人與人體運動進行同構(gòu)性映射的姿態(tài)解算.圖6為從端護理機器人上的6自由度仿人機械臂,其中,大臂肩關(guān)節(jié)含有3自由度(俯仰、橫滾、偏航)、肘關(guān)節(jié)含有2自由度(俯仰、橫滾)和手腕關(guān)節(jié)含有1自由度(偏航),可以使機械臂的肩、肘、腕三個關(guān)節(jié)在空間內(nèi)獨立運動.

圖6 從端機器人的6自由度仿人機械臂Fig.6 6-DOF humanoid mechanical arm on the slave robot

為了基于四元數(shù)求解各自由度的運動,先選取特定參考系.對于從機器人機械臂,選擇以順向指向機械臂末端方向為x軸,根據(jù)右手定則,得到相應(yīng)的y軸和z軸.對于人體,以豎直向下為x軸,右側(cè)為y軸,正對方向為z軸.

對于機器人大臂關(guān)節(jié)的運動姿態(tài),由于人體和機器人軀干相對靜止,只需利用大臂上測量的姿態(tài)信息和固定方向向量進行求解.

將大臂關(guān)節(jié)其中一個自由度的運動方向記為右大臂與機體坐標(biāo)系的偏航角?rb,其求解過程如下:

記大臂單位四元數(shù)為qrb,求取大臂四元數(shù)的逆

其中,qrb0,qrb1,qrb2,qrb3分別為qrb的各分項.

計算右大臂關(guān)于機體x軸的方向向量qrbX:

其中,qtemp為中間計算量:

式中,mx=(0 1 0 0)為x軸單位四元數(shù).于是可求取右大臂關(guān)節(jié)的偏航角?rb:

其中,d1為點乘得到的中間量:

式中,my=(0 0 1 0)為y軸單位四元數(shù).

通過上述計算過程,即可求取式(16)所示的大臂關(guān)節(jié)偏航角的目標(biāo)值?rb.

將右大臂關(guān)節(jié)中另一個自由度的運動方向記為大臂與機體坐標(biāo)系的俯仰角θrb,按如下過程求取θrb:

由右大臂x軸的方向向量qrbX叉乘y軸單位向量my=(0 0 1 0),取結(jié)果后三個數(shù)即得法向量

將法向量nbx?by與z軸單位向量mz=(0 0 0 1)中的方向元素進行點乘,得到大臂俯仰角關(guān)節(jié)的目標(biāo)值θrb:

其中,|nbx?by|為式(18)所求法向量的模,d2為計算過程的中間量:

對于肘關(guān)節(jié)運動姿態(tài)的解算,由于肘關(guān)節(jié)連接大臂和小臂,且兩者隨手臂運動而同時發(fā)生變化,故需分別獲取大、小臂上的兩個測量單元測量值.

記右小臂的單位四元數(shù)為qrs.根據(jù)式(13)求出qrs的逆根據(jù)式(14)和式(15)求出小臂關(guān)于機體x軸的方向向量qrsX,再將qrsX與大臂qrbX點乘,可解出肘關(guān)節(jié)的俯仰角θrs:

肘關(guān)節(jié)的橫滾角αrs由大小臂關(guān)于機體X軸方向向量叉乘得到的法向量nbx?sx,點乘右小臂沿y軸方向的單位向量qrsy而得到,計算過程如下:

同理,采用上述姿態(tài)求解算法,還可分別實現(xiàn)右肘部偏航角、右腕部橫滾角、頸部關(guān)節(jié)兩個自由度、腰關(guān)節(jié)一個自由度以及對稱左臂6自由度的運動姿態(tài)解算.這樣,我們基于慣性測量單元的遙操作護理機器人系統(tǒng)，就可完成從主端護理員到從端護理機器人15個自由度的目標(biāo)姿態(tài)求解,從而實現(xiàn)護理員遠程控制機器人進行護理作業(yè).

圖7為遙操作護理機器人系統(tǒng)操作者的動作姿態(tài)經(jīng)解算后形成從端機器人隨動運動的仿真效果,可以看到機器人肢體的動作范圍完全符合人體運動生理特性.

圖7 主端控制下的從端機器人隨動運動仿真Fig.7 Motion simulation on slave robot under master control

4 實驗驗證

4.1 實驗設(shè)計

為驗證所研制的同構(gòu)式遙操作機器人系統(tǒng)姿態(tài)解算方法的有效性以及護理機器人的實時跟隨運動特性,設(shè)計了如圖8所示的護理員遙操作護理機器人遞送和抓取藥瓶實驗.實驗環(huán)境設(shè)定為機器人系統(tǒng)的主從端處于同一房間內(nèi),主、從兩端通過無線方式發(fā)送和接收信息.

圖8 遙操作護理機器人系統(tǒng)的藥瓶抓取和遞送實驗Fig.8 Experiment on delivering and taking medicine bottle of telerobotic nursing system

機器人系統(tǒng)工作時,主端通過姿態(tài)解算方法將護理員的人體動作姿態(tài)映射成護理機器人各關(guān)節(jié)的目標(biāo)運動位置,發(fā)送到從端護理機器人,機器人根據(jù)接收到的目標(biāo)位置控制機器人身體各關(guān)節(jié)進行運動.同時,機器人工作場景的視頻信息回傳給護理員,用以調(diào)整機器人手臂姿態(tài),從而完成從端護理機器人的遞送和抓取藥瓶任務(wù).

4.2 姿態(tài)解算精度測試

遙操作護理機器人系統(tǒng)的主端動作捕捉部分,測量節(jié)點分別由集成了三軸陀螺儀和三軸加速度計的MPU6050與三軸磁力計HMC5883構(gòu)成.在采集到各節(jié)點數(shù)據(jù)后,使用前文所述方法求解機器人機械臂的目標(biāo)姿態(tài).為驗證該法解算性能,實驗采用Honeywell三軸姿態(tài)慣性參考系統(tǒng)3DM-GX3-25(AHRS)的輸出角度作為參考角度,該參考系統(tǒng)的解算精度為對于靜態(tài)條件誤差小于0.5°,動態(tài)條件誤差小于2°.實驗過程中,將本文設(shè)計的姿態(tài)測量單元與上述參考系統(tǒng)一同固定在同一穿戴設(shè)備上,并將基準(zhǔn)初始狀態(tài)調(diào)至一致.

實驗設(shè)定主端系統(tǒng)的數(shù)據(jù)采樣頻率為200Hz,在每16個采樣數(shù)據(jù)中取出1個作為有效實驗數(shù)據(jù),這樣主端系統(tǒng)實際數(shù)據(jù)采集的時間周期為0.08秒.對采集到的原始人體姿態(tài)數(shù)據(jù)經(jīng)過解算,可以獲得仿人機械臂各關(guān)節(jié)姿態(tài)的解算角度(機器人目標(biāo)位置角度).

圖9～13分別為同一9.12秒時間范圍的各關(guān)節(jié)姿態(tài)角的數(shù)據(jù)變化曲線.由曲線圖可知,經(jīng)本文方法解算得到的各關(guān)節(jié)姿態(tài)角,雖然因系統(tǒng)本身器件響應(yīng)速度等原因而相對于AHRS參考系統(tǒng)存在一定的滯后,但與參考角度的時間曲線基本一致,仍能很好地反映人體動作姿態(tài)變化.特別是角度變化速率較小時,解算角度與參考角度幾乎完全重合.

圖9 肩部俯仰角變化曲線Fig.9 Curve of shoulder pitch angle

圖10 肩部橫滾角變化曲線Fig.10 Curve of shoulder roll angle

圖11 肩部偏航角變化曲線Fig.11 Curve of shoulder yaw angle

圖12 肘部俯仰角變化曲線Fig.12 Curve of elbow pitch angle

圖13 肘部橫滾角變化曲線Fig.13 Curve of elbow roll angle

以上結(jié)果表明,基于慣性測量單元的數(shù)據(jù)采集和處理方法,可以快速完成人體姿態(tài)數(shù)據(jù)采集和各關(guān)節(jié)角度的姿態(tài)解算,主端系統(tǒng)可以較好地采集操作者人體各關(guān)節(jié)的運動信息,能完全滿足遙操作機器人護理系統(tǒng)執(zhí)行護理動作的時間要求.

由于在執(zhí)行遞送藥瓶動作過程中,不同關(guān)節(jié)的動作幅度不同.為比較不同關(guān)節(jié)的測量精度,實驗對誤差進行規(guī)整化處理,用相對誤差來評價各關(guān)節(jié)的姿態(tài)解算精度,以進一步準(zhǔn)確評估系統(tǒng)姿態(tài)解算精度.實驗選擇姿態(tài)快速變化的時間段內(nèi)的數(shù)據(jù)進行處理.各關(guān)節(jié)角度數(shù)據(jù)的處理過程如下:

觀察本系統(tǒng)解算數(shù)據(jù)和參考系統(tǒng)數(shù)據(jù),它們之間平均有相鄰1個時間單位的時延.將圖中的解算角度數(shù)據(jù)進行平移1個時間單位,以消除本系統(tǒng)與AHRS系統(tǒng)響應(yīng)時間不同而帶來的固有干擾.

對于上述5個關(guān)節(jié)角度,分別取其角度變化較快的一個時間段內(nèi)的數(shù)據(jù),求取該時段內(nèi)全部的每兩個相鄰時間點參考數(shù)據(jù)的差值,對全部差值求平均,得到該段內(nèi)數(shù)值變化的平均值,記為角度變化均值;將每個時間點解算角度與參考角度的差值記為比較誤差均值;將比較誤差均值與角度變化均值的比值記為相對誤差,它表示在各關(guān)節(jié)姿態(tài)快速改變時,解算角度與AHRS參考角度的相對偏差,其大小能更好地反映本文解算方法的解算精度.根據(jù)參考數(shù)據(jù)和系統(tǒng)解算數(shù)據(jù)進行計算,表1為上述3個參數(shù)的計算結(jié)果.

表1 對應(yīng)機械臂各關(guān)節(jié)的解算姿態(tài)數(shù)據(jù)精度Table 1 Average errors of fve calculated joint angles

肩部3個關(guān)節(jié)角的相對誤差約為0.112～0.146,相對較小;肘部兩關(guān)節(jié)角的相對誤差為0.194～0.255,相對較大.出現(xiàn)這樣的差異,一是在機械臂各關(guān)節(jié)角度解算中,肩部各關(guān)節(jié)的解算僅采用大臂姿態(tài)測量單元數(shù)據(jù),結(jié)合人體軀干的固定坐標(biāo)系進行解算,快速運動時傳感器自身特性影響較為單一;二是肘部各關(guān)節(jié)的姿態(tài)解算不僅需要使用小臂姿態(tài)數(shù)據(jù),還需結(jié)合大臂姿態(tài)數(shù)據(jù)進行聯(lián)合求解,兩個測量單元的傳感器數(shù)據(jù)誤差相互疊加,導(dǎo)致肘關(guān)節(jié)數(shù)據(jù)的整體誤差相較于肩關(guān)節(jié)誤差略有變大.

根據(jù)上述5個關(guān)節(jié)角度在其各自角度變化較快時間段的數(shù)據(jù),計算在動態(tài)條件下的絕對誤差η:

以上實驗過程表明,在機器人系統(tǒng)進行設(shè)定任務(wù)時,采用本文方法解算得到的各關(guān)節(jié)角度與參考系統(tǒng)測得角度相比,其相對誤差很小.在人體姿態(tài)快速改變時,雖然解算角度變化與參考角度變化存在一定誤差,但總體趨勢能保持一致,系統(tǒng)的誤差在動態(tài)條件下依舊能保持2%以下.

4.3 機器人隨動運動測試

本項實驗內(nèi)容為護理機器人的隨動運動性能測試.護理機器人各關(guān)節(jié)輸出軸末端都裝有測量關(guān)節(jié)角度的12位高精度增量式磁編碼器AS5045.在護理員控制護理機器人抓取藥瓶的動作過程中,實驗分別取得護理機器人實時接收的肩部三個關(guān)節(jié)姿態(tài)和相同時刻通過磁編碼器實際測量得到的機器人對應(yīng)關(guān)節(jié)位置.護理員控制護理機器人抓取藥瓶的動作用時9.6秒,采樣方法同第4.2節(jié).

圖14～16為截取的9.6秒主從端兩種姿態(tài)數(shù)據(jù)的變化曲線.其中,目標(biāo)角度為護理機器人肩部各關(guān)節(jié)實時給定目標(biāo)角度,當(dāng)前角度為從端護理機器人各關(guān)節(jié)的實際測量值.由圖可見,實際測得的當(dāng)前角度與目標(biāo)角度相比出現(xiàn)一定的滯后.因為人體動作姿態(tài)需要進行獲取和處理,從端機器人從主端接收目標(biāo)角度值,機器人執(zhí)行機構(gòu)運動到目標(biāo)位置,這些過程是造成遙操作機器人系統(tǒng)時延的主要因素.機器人系統(tǒng)主從端處于同一房間并采用無線方式收發(fā)信息時,系統(tǒng)延時小于200ms.

圖14 肩部俯仰角變化曲線Fig.14 Curve of shoulder pitch angle

圖15 肩部橫滾角變化曲線Fig.15 Curve of shoulder roll angle

圖16 肩部偏航角變化曲線Fig.16 Curve of shoulder yaw angle

通過以上分析可知,采用本文提出的姿態(tài)解算方法,從端護理機器人各關(guān)節(jié)能實時快速地跟隨人體進行動作.從曲線圖中也能看到,在拿藥瓶動作實驗中,系統(tǒng)剛上電時,機器人肢體各部分的初始位置與操作者初始位置相差很多,但其能迅速回到操作者肢體的起始位置,與人體姿態(tài)保持一致.

5 結(jié)論

為實現(xiàn)護理機器人快速準(zhǔn)確地跟隨護理員進行護理作業(yè),本文研究了一種同構(gòu)式遙操作護理機器人系統(tǒng)的操作者人體姿態(tài)解算方法,實現(xiàn)了主端人體動作姿態(tài)到從端機器人姿態(tài)的同構(gòu)性映射.實驗分別設(shè)計了護理員遙操作控制護理機器人遞送和抓取藥瓶作業(yè)任務(wù).實驗結(jié)果表明,本文方法滿足護理機器人執(zhí)行護理動作時所需處理數(shù)據(jù)的快速性和準(zhǔn)確性要求,護理機器人能實時準(zhǔn)確地復(fù)現(xiàn)護理員的動作姿態(tài).但是也可看到,實驗中護理機器人復(fù)現(xiàn)的動作與人體動作姿態(tài)仍然存在一定的時間滯后,因此解決護理機器人遙操作的時延問題,特別是遠距離網(wǎng)絡(luò)傳輸環(huán)境下的時延解決方法,將是我們下一步將要研究的主要內(nèi)容之一.

1 Wang Tian-Miao,Tao Yong,Chen Yang.Research status and development trends of the service robotic technology.Scientia Sinica Informationis,2012,42(9):1049?1066 (王田苗,陶永,陳陽.服務(wù)機器人技術(shù)研究現(xiàn)狀與發(fā)展趨勢.中國科學(xué):信息科學(xué),2012,42(9):1049?1066)

2 Tan Min,Wang Shuo.Research progress on robotics.Acta Automatica Sinica,2013,39(7):963?972 (譚民,王碩.機器人技術(shù)研究進展.自動化學(xué)報,2013,39(7):963?972)

3 Joubair A,Zhao L F,Bigras P,Bonev I.Absolute accuracy analysis and improvement of a hybrid 6-DOF medical robot.Industrial Robot:An International Journal,2015,42(1):44?53

4 Chen T L,Kemp C C.A direct physical interface for navigation and positioning of a robotic nursing assistant.Advanced Robotics,2011,25(5):605?627

5 Kawasaki H,Kimura H,Ito S,Nishimoto Y,Hayashi H, Sakaeda H.Hand rehabilitation support system based on self-motion control,with a clinical case report.In:Proceedings of the 2006 World Automation Congress.Budapest, Hungary:IEEE,2006.1?6

6 Mihelj M,Nef T,Riener R.ARMin-toward a six DoF upper limb rehabilitation robot.In:Proceedings of the 1st IEEE/RAS-EMBS International Conference on Biomedical Robotics and Biomechatronics.Pisa,Italy:IEEE,2006. 1154?1159

7 Li J M,Wang S X,Wang X F,He C.Optimization of a novel mechanism for a minimally invasive surgery robot.The International Journal of Medical Robotics and Computer Assisted Surgery,2010,6(1):83?90

8 Miyamoto H,Leechavengvongs S,Atik T,Facca S,Liverneaux P.Nerve transfer to the deltoid muscle using the nerve to the long head of the triceps with the da vinci robot:six cases.Journal of Reconstructive Microsurgery, 2014,30(6):375?380

9 Ma G W,Pytel M,Trejos A L,Hornblower V,Smallwood J, Patel R,Fenster A,Malthaner R A.Robot-assisted thoracoscopic brachytherapy for lung cancer:comparison of the ZEUS robot,VATS,and manual seed implantation.Computer Aided Surgery,2007,12(5):270?277

10 Mukai T,Hirano S,Nakashima H,Sakaida Y,Guo S J.Realization and safety measures of patient transfer by nursingcare assistant robot RIBA with tactile sensors.Journal of Robotics and Mechatronics,2011,23(3):360?369

11 Wester B A,Para M P,Sivakumar A,Kutzer M D,Katyal K D,Ravitz A D,Beaty J D,McLoughlin M P,Johannes M S. Experimental validation of imposed safety regions for neural controlled human patient self-feeding using the modular prosthetic limb.In:Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Tokyo,Japan:IEEE,2013.877?884

13 Seeberger R,Kane G,Hofmann J,Eggers G.Accuracy assessment for navigated maxillo-facial surgery using an electromagnetic tracking device.Journal of Cranio-Maxillofacial Surgery,2012,40(2):156?161

14 Hess W.Head-tracking techniques for virtual acoustics applications.In:Proceedings of the 2012 Audio Engineering Society Convention 133.Erlangen,Germany:Fraunhofer Institute for Integrated Circuits IIS,2012.(8782):1?15

16 Simeone A L.Substitutional reality:towards a research agenda.In:Proceedings of the 1st IEEE Workshop on Everyday Virtual Reality.Arles,France:IEEE,2015.19?22

17 Zhang F,DiSanto W,Ren J,Dou Z,Yang Q,Huang H. A novel CPS system for evaluating a neural-machine interface for artifcial legs.In:Proceedings of the 2011 IEEE/ ACM International Conference on Cyber-Physical Systems. Chicago,IL,USA:IEEE,2011.67?76

18 Ogawa M,Honda K,Sato Y,Kudoh S,Oishi T,Ikeuchi K. Motion generation of the humanoid robot for teleoperation by task model.In:Proceedings of the 24th IEEE International Symposium on Robot and Human Interactive Communication.Kobe,Japan:IEEE,2015.71?76

19 Dai J S.Euler-Rodrigues formula variations,quaternion conjugation and intrinsic connections.Mechanism and Machine Theory,2015,92:144?152

20 Xinjilefu X,Feng S Y,Huang W W,Atkeson C G.Decoupled state estimation for humanoids using full-body dynamics. In:Proceedings of the 2014 IEEE International Conference on Robotics and Automation.Hong Kong,China:IEEE, 2014.195?201

21 Zhang Z Q,Meng X L,Wu J K.Quaternion-based Kalman flter with vector selection for accurate orientation tracking.IEEE Transactions on Instrumentation and Measurement, 2012,61(10):2817?2824

22 Fresk E,Nikolakopoulos G.Full quaternion based attitude control for a quadrotor.In:Proceedings of the 2013 European Control Conference.Zurich,Switzerland:IEEE,2013. 3864?3869

23 Carminati M,Ferrari G,Grassetti R,Sampietro M.Realtime data fusion and MEMS sensors fault detection in an aircraft emergency attitude unit based on Kalman fltering.IEEE Sensors Journal,2012,12(10):2984?2992

24 Ge Quan-Bo,Li Wen-Bin,Sun Ruo-Yu,Xu Zi.Centralized fusion algorithms based on EKF for multisensor non-linear systems.Acta Automatica Sinica,2013,39(6):816?825 (葛泉波,李文斌,孫若愚,徐姿.基于EKF的集中式融合估計研究.自動化學(xué)報,2013,39(6):816?825)

25 Peng Xiao-Dong,Zhang Tie-Min,Li Ji-Yu,Chen Yu.Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration.Acta Automatica Sinica, 2015,41(4):854?860 (彭孝東,張鐵民,李繼宇,陳渝.基于傳感器校正與融合的農(nóng)用小型無人機姿態(tài)估計算法.自動化學(xué)報,2015,41(4):854?860)

26 Zhao H,Wang Z Y.Motion measurement using inertial sensors,ultrasonic sensors,and magnetometers with extended Kalman flter for data fusion.IEEE Sensors Journal,2012,12(5):943?953

27 Valenti R G,Dryanovski I,Xiao J Z.Keeping a good attitude:a quaternion-based orientation flter for IMUs and MARGs.Sensors,2015,15(8):19302?19330

左國玉博士,北京工業(yè)大學(xué)電子信息與控制工程學(xué)院副教授.主要研究方向為遙操作機器人和仿生機器人.

E-mail:zuoguoyu@bjut.edu.cn

(ZUOGuo-YuPh.D.,associate professor at the College of Electronic Information and Control Engineering, Beijing University of Technology.His research interest covers telerobotics and bionic robotics.)

于雙悅北京工業(yè)大學(xué)電子信息與控制工程學(xué)院碩士研究生.主要研究方向為機器人控制.

E-mail:ysy@emails.bjut.edu.cn

(YU Shuang-YueMaster student at the College of Electronic Information and Control Engineering,Beijing University of Technology.His main research interest is robot control.)

龔道雄博士,北京工業(yè)大學(xué)電子信息與控制工程學(xué)院副教授.主要研究方向為計算智能與機器人.本文通信作者.

E-mail:gongdx@bjut.edu.cn

(GONG Dao-XiongPh.D.,associate professor at the College of Electronic Information and Control Engineering,Beijing University of Technology.His research interest covers computational intelligence and robotics.Corresponding author of this paper.)

Operator Attitude Algorithm for Telerobotic Nursing System

ZUO Guo-Yu1,2YU Shuang-Yue1,2GONG Dao-Xiong1,2

This paper presents a telerobotic nursing system using inertial navigation information.In order to realize following control of the slave nursing robot,an attitude solution method is proposed for operator attitude in the master system.First,a motion capture system based on inertial units is built.To meet real-time demand,quaternion method with relatively low computation is performed in attitude solution for human data acquired from the master system,and then data fusion and compensation are conducted.Second,the attitude data obtained by quaternion method are further calculated as attitude of each joint of the slave robot with a humanoid structure,and thus an isomorphic motion mapping from human operator to slave robot is completed.Finally,experiments of taking and delivering bottle of the telerobotic system are described,and solution performance analysis on the attitude data from the master motion capture system are presented.The results show that there is a small measuring deviation with an absolute error less than 2%between the proposed method and the reference system.The attitude solving method can satisfy the requirements for fast and accurate attitude processing when the slave robot conducts a nursing task by following the nurse operator.

Telerobotic nursing system,attitude algorithm,isomorphic mapping,human motion tracking

左國玉,于雙悅,龔道雄.遙操作護理機器人系統(tǒng)的操作者姿態(tài)解算方法研究.自動化學(xué)報,2016,42(12): 1839?1848

Zuo Guo-Yu,Yu Shuang-Yue,Gong Dao-Xiong.Operator attitude algorithm for telerobotic nursing system.Acta Automatica Sinica,2016,42(12):1839?1848

2016-02-03 錄用日期2016-08-15

Manuscript received February 3,2016;accepted August 15, 2016

國家自然科學(xué)基金(61375086),北京工業(yè)大學(xué)智能機器人領(lǐng)域大科研推進計劃(002000514316008),中國科學(xué)研究院沈陽自動化所機器人學(xué)國家重點實驗室開放課題(2014338)資助

Supported by National Natural Science Foundation of China (61375086),Beijing University of Technology Big Scientifc Promoting Plan on Intelligent Robot(002000514316008),and Foundation of the State Key Laboratory of Robotics in Shenyang Institute of Automation(SIA)of Chinese Academy of Sciences(20 14338)

本文責(zé)任編委趙新剛

Recommended by Associate Editor ZHAO Xin-Gang

1.北京工業(yè)大學(xué)電子信息與控制工程學(xué)院 北京 100124 2.計算智能與智能系統(tǒng)北京市重點實驗室北京100124

1.College of Electronic Information and Control Engineering, Beijing University of Technology,Beijing 100124 2.Beijing Key Laboratory of Computational Intelligence and Intelligent System,Beijing 100124