0 引言

    世界上每年因戰(zhàn)爭(zhēng)、自然災(zāi)害、交通事故等原因引起的殘疾人數(shù)多達(dá)千萬(wàn)人以上，其中大部分為肢體殘疾，而目前市場(chǎng)上的大部分假肢都是作為裝飾性用品，真正實(shí)現(xiàn)功能代償?shù)募僦珨?shù)量較少。為滿足殘疾人日常生活的需求，本系統(tǒng)對(duì)前臂假肢做了姿態(tài)規(guī)劃，即控制前臂旋轉(zhuǎn)和手腕俯仰兩個(gè)電機(jī)，使得假肢末端手爪的虎口平面與大地水平面始終保持平行，如圖1所示。這樣假肢在抓取和移動(dòng)目標(biāo)物體的過(guò)程中，可以保證運(yùn)動(dòng)平穩(wěn)，對(duì)移動(dòng)水杯等類似操作尤其重要。在前臂假肢的平衡控制過(guò)程中，姿態(tài)傳感器將檢測(cè)到的前臂假肢的姿態(tài)信號(hào)送入STM32，經(jīng)算法運(yùn)算得到控制量驅(qū)動(dòng)舵機(jī)來(lái)控制前臂的扭轉(zhuǎn)和手部的俯仰，從而使前臂假肢的手部恢復(fù)到平衡狀態(tài)?？梢钥闯?，獲取實(shí)時(shí)的前臂假肢的姿態(tài)是保持手部平衡的前提。因此，實(shí)現(xiàn)一種實(shí)時(shí)可靠的前臂假肢姿態(tài)估計(jì)算法尤為重要，它直接影響其手部自平衡性能。

    姿態(tài)解算的算法有多種，常用的算法有歐拉角法、方向余弦法與四元數(shù)法，由于歐拉角法求解時(shí)，方程中存在奇點(diǎn)，方向余弦法計(jì)算量大，因此現(xiàn)在常用的方法為四元數(shù)法，其計(jì)算量小，無(wú)奇點(diǎn)^[1-2]。姿態(tài)估計(jì)算法必須考慮到傳感器容易受溫度及噪聲的影響而產(chǎn)生不同程度的漂移，它會(huì)影響姿態(tài)估計(jì)的準(zhǔn)確性。針對(duì)這一問(wèn)題，本文采用卡爾曼濾波的方法來(lái)融合陀螺儀和加速度計(jì)輸出的數(shù)據(jù)，修正了陀螺儀輸出量中的隨機(jī)漂移分量^[3]。

1 系統(tǒng)硬件設(shè)計(jì)

    隨著安卓智能手機(jī)的發(fā)展與普及，為了讓前臂假肢的控制更加方便和標(biāo)準(zhǔn)化，提出了以安卓手機(jī)離線識(shí)別語(yǔ)音及應(yīng)用藍(lán)牙通信技術(shù)實(shí)現(xiàn)安卓手機(jī)無(wú)線控制假肢運(yùn)動(dòng)的方案。本文介紹的前臂假肢可實(shí)現(xiàn)手部平衡，除此之外，在不需要手部保持平衡的環(huán)境下，安卓手機(jī)通過(guò)語(yǔ)音識(shí)別可控制前臂假肢關(guān)閉手部自平衡功能和實(shí)現(xiàn)手部上抬、下落、內(nèi)轉(zhuǎn)、外轉(zhuǎn)和張合等動(dòng)作。該系統(tǒng)由安卓手機(jī)、藍(lán)牙模塊、MPU6050、STM32微控制器、MX-28高性能舵機(jī)組成，系統(tǒng)的控制流程如圖2所示。

    其中，選用MPU6050做為前臂假肢的姿態(tài)檢測(cè)系統(tǒng)，MPU6050在單個(gè)產(chǎn)品封裝內(nèi)集成了3軸陀螺儀和3軸加速度計(jì)。由于前臂假肢不需獲知偏航角，所以本文采用了MPU6050來(lái)獲取前臂假肢的俯仰角和橫滾角。整個(gè)控制過(guò)程由MPU6050測(cè)量姿態(tài)變化，并通過(guò)前臂扭轉(zhuǎn)以及手腕俯仰來(lái)根據(jù)姿態(tài)變化實(shí)時(shí)進(jìn)行調(diào)整，使其達(dá)到自平衡狀態(tài)。

    MPU6050安裝在假肢的腕部，其安裝位置如圖3所示。將MPU6050安裝在控制前臂旋轉(zhuǎn)關(guān)節(jié)的上部并固定在連桿上，左右位置可以變化，但要確保其所在的平面與手爪虎口所在平面平行。安裝完成后，前臂繞MPU6050的X軸旋轉(zhuǎn)，手腕繞MPU6050的Y軸俯仰。

2 四元數(shù)姿態(tài)解算算法

    四元數(shù)的數(shù)學(xué)概念是1843年由哈密頓首先提出的。它是由一個(gè)實(shí)部單位和3個(gè)虛部單位i、j、k組成，其形式為：q=q₀+q₁i+q₂j+q₃k。四元數(shù)可以表示旋轉(zhuǎn)，其標(biāo)量部分表示旋轉(zhuǎn)角度的大小，矢量部分表示轉(zhuǎn)軸的方向^[4-5]，應(yīng)用四元數(shù)時(shí)必須進(jìn)行歸一化，也就是令其模值為1。

    前臂假肢的定位由固定的內(nèi)部的三維坐標(biāo)確定，一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的變換，可以通過(guò)繞不同坐標(biāo)軸的3次連續(xù)轉(zhuǎn)動(dòng)來(lái)實(shí)現(xiàn)。順時(shí)針繞橫滾軸，即繞載體坐標(biāo)系的X軸旋轉(zhuǎn)，得到橫滾角φ；順時(shí)針繞俯仰軸，即繞載體坐標(biāo)系的Y軸旋轉(zhuǎn)，得到俯仰角θ；順時(shí)針繞偏航軸，即繞載體坐標(biāo)系的?爪軸旋轉(zhuǎn)，得到偏航角γ^[6]。

    四個(gè)參數(shù)的初始值是由歐拉角的初始值經(jīng)過(guò)以下計(jì)算得到的^[6-9]：

    利用四元數(shù)微分方程可以實(shí)現(xiàn)四元數(shù)的更新，其中，ω是陀螺儀測(cè)量的角速度。

    應(yīng)用四階龍格—庫(kù)塔算法解微分方程，得到4個(gè)參數(shù)的更新值。具體過(guò)程如下^[12-14]：

    設(shè)：

    則：

其中，T為更新周期，q(t)為初始四元數(shù)，q(t+T)為更新后的四元數(shù)，K₁是時(shí)間段T開始時(shí)的斜率，K₂是時(shí)間段中點(diǎn)的斜率，K₃也是中點(diǎn)的斜率，K₄是時(shí)間段終點(diǎn)的斜率。

    計(jì)算得到這4個(gè)參數(shù)之后，可由姿態(tài)矩陣T更新歐拉角[15]：

    得到姿態(tài)角，即可得到前臂假肢的姿態(tài)估計(jì)。

    前臂假肢在平衡的情況下，歐拉角的理想輸出值應(yīng)該是0°，但由于陀螺儀有隨機(jī)漂移誤差，歐拉角會(huì)一直積累^[16]。實(shí)驗(yàn)結(jié)果圖4所示。

    針對(duì)這一問(wèn)題，提出了應(yīng)用卡爾曼濾波的數(shù)據(jù)融合算法來(lái)解決?？柭鼮V波可以將陀螺儀的輸出數(shù)據(jù)和加速度計(jì)的輸出數(shù)據(jù)融合起來(lái)。陀螺儀用于測(cè)量角速度，動(dòng)態(tài)跟蹤性能好，但陀螺儀受溫度、不穩(wěn)定力矩等因素的影響，會(huì)產(chǎn)生隨機(jī)漂移誤差；加速度計(jì)可用于測(cè)量加速度，通過(guò)測(cè)得的加速度值與重力加速度的比值得到傾角值，加速度計(jì)靜態(tài)性能好，但是動(dòng)態(tài)跟蹤性能差?？梢钥闯?，陀螺儀和加速度計(jì)優(yōu)缺點(diǎn)互補(bǔ)，結(jié)合起來(lái)會(huì)有好的效果^[17-18]。

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

3.1 四元數(shù)法和卡爾曼濾波實(shí)驗(yàn)結(jié)果分析

    將MPU6050靜止平放在桌上，采集其經(jīng)四元數(shù)姿態(tài)解算和卡爾曼濾波后輸出的數(shù)據(jù)，通過(guò)MATLAB工具對(duì)實(shí)測(cè)數(shù)據(jù)進(jìn)行仿真分析，如圖5所示，俯仰角和橫滾角都在附近，對(duì)比圖4發(fā)現(xiàn)卡爾曼濾波有效解決了陀螺儀的隨機(jī)漂移誤差，可以準(zhǔn)確地獲取靜態(tài)時(shí)前臂假肢的姿態(tài)。

    將MPU6050安裝到前臂假肢上，令前臂假肢繞MPU6050的X軸從平衡位置順時(shí)針逐步旋轉(zhuǎn)到-40°，一段時(shí)間后，回到平衡位置，再沿X軸逆時(shí)針逐步旋轉(zhuǎn)到40°，采集這一過(guò)程MPU6050經(jīng)四元數(shù)姿態(tài)解算和卡爾曼濾波后輸出的數(shù)據(jù)，通過(guò)MATLAB工具對(duì)實(shí)測(cè)數(shù)據(jù)進(jìn)行仿真分析，橫滾角發(fā)生相應(yīng)的變化，俯仰角基本保持不變，如圖6(a)所示。令前臂假肢繞MPU6050的Y軸重復(fù)以上實(shí)驗(yàn)，根據(jù)圖6(b)可以看出，俯仰角發(fā)生相應(yīng)的變化，橫滾角基本保持不變。由于實(shí)驗(yàn)過(guò)程中手動(dòng)旋轉(zhuǎn)前臂假肢，操作者偶有抖動(dòng)和難做到勻速轉(zhuǎn)動(dòng)，因此，圖6中的波形中偶有小的波動(dòng)。實(shí)驗(yàn)表明，本文采用的姿態(tài)估計(jì)可以實(shí)現(xiàn)前臂假肢動(dòng)作時(shí)姿態(tài)的正確估計(jì)，經(jīng)過(guò)STM32運(yùn)算處理后，實(shí)現(xiàn)前臂假肢在運(yùn)動(dòng)過(guò)程中保持手部平衡。

3.2 前臂假肢手部自平衡實(shí)驗(yàn)結(jié)果分析

    對(duì)前臂假肢系統(tǒng)組裝并調(diào)試，通過(guò)安卓手機(jī)軟件向假肢發(fā)出保持手部平衡的命令。具體測(cè)試步驟:連接好線路，打開電源，觀察指示燈正常亮起，觸擊控制界面的藍(lán)牙連接按鈕，觀察指示燈，以確保手機(jī)藍(lán)牙和藍(lán)牙模塊的配對(duì)連接成功；否則，檢查系統(tǒng)的連接是否正確。藍(lán)牙配對(duì)成功后，向安卓手機(jī)喊出“平衡”指令，手機(jī)通過(guò)藍(lán)牙將指令傳到單片機(jī)，從而控制前臂假肢運(yùn)動(dòng)。實(shí)驗(yàn)結(jié)果如圖7、圖8所示。

    圖7(a)～圖7(e)展示了前臂假肢從下往上轉(zhuǎn)動(dòng)過(guò)程中，其手部可以實(shí)現(xiàn)自平衡，手部虎口一直保持與大地平行。

    圖8中以箭頭指向的地點(diǎn)為參考點(diǎn)，可以看出圖8(a)～圖8(c)為前臂假肢由內(nèi)往外轉(zhuǎn)動(dòng)過(guò)程，同樣假肢手部能實(shí)現(xiàn)自平衡。

4 結(jié)論

    此前臂假肢采用四元數(shù)法的姿態(tài)解算算法，并利用卡爾曼濾波融合MPU6050中陀螺儀和加速度計(jì)的數(shù)據(jù)，得到其正確的姿態(tài)估計(jì)。經(jīng)實(shí)驗(yàn)測(cè)試，該系統(tǒng)運(yùn)行穩(wěn)定、可靠，所設(shè)計(jì)的姿態(tài)估計(jì)算法能夠很好地跟蹤前臂假肢的真實(shí)姿態(tài)，實(shí)現(xiàn)了假肢手部的姿態(tài)自平衡控制。此前臂假肢可以輔助殘疾人完成喝水等動(dòng)作，方便了他們的生活，也減輕了家屬的負(fù)擔(dān)。

參考文獻(xiàn)

[1] 杜海龍，張榮輝，劉平，等.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算模塊的實(shí)現(xiàn)[J].光學(xué)精密工程，2008，16(10)：1956-1962.

[2] 張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程，2008，16(10)：1963-1970.

[3] 楊凌霄，李曉陽(yáng).基于卡爾曼濾波的兩輪自平衡車姿態(tài)解算方法[J].計(jì)算機(jī)仿真，2014，31(6)：406-409.

[4] 葉锃鋒，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法[J].傳感技術(shù)學(xué)報(bào)，2012，25(4)：524-528.

[5] 呂印新，肖前貴，胡壽松.基于四元數(shù)互補(bǔ)濾波的無(wú)人機(jī)姿態(tài)解算[J].燕山大學(xué)學(xué)報(bào)，2014,38（2）：175-180.

[6] 張思奇，趙憶文.便攜式移動(dòng)機(jī)器人姿態(tài)解算方法[J].微計(jì)算機(jī)信息，2007，23(10-2)：188-190.

[7] 祝喬，汪旭東，崔家瑞.基于四元數(shù)的航天器姿態(tài)非線性控制器[J].信息與控制，2012，41(2)：170-179.

[8] 賈秋玲，閆建國(guó)，王建民.空間多剛體系統(tǒng)姿態(tài)的協(xié)同控制[J].信息與控制，2005，34(4)：385-388.

[9] 趙國(guó)文.機(jī)器人運(yùn)動(dòng)學(xué)中的四元數(shù)矩陣和基變換矩陣[J].機(jī)器人，1993，15（2）：59-64.

[10] 張?zhí)旃猓跣闫?，王麗霞，等，譯.捷聯(lián)慣性導(dǎo)航技術(shù)(第二版)[M].北京：國(guó)防工業(yè)出版社，2007：29-33.

[11] 秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2006：287-305.

[12] 崔璐璐.基于MEMS器件的姿態(tài)測(cè)量系統(tǒng)研究與實(shí)現(xiàn)[D].大連：大連理工大學(xué)，2009.

[13] 王勵(lì)揚(yáng)，霍昆明，何文濤，等.四階龍格庫(kù)塔算法在捷聯(lián)慣性導(dǎo)航中的應(yīng)用[J].計(jì)算機(jī)仿真，2014，31(11)：56-59.

[14] 張慧春，吳簡(jiǎn)彤，何昆鵬，等.四階龍格-庫(kù)塔法在捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算中的應(yīng)用[J].應(yīng)用科技，2005，32(6)：37-39.

[15] 鄧正隆.慣性技術(shù)[M].哈爾濱：哈爾濱工業(yè)大學(xué)出版社，2006：14-164.

[16] 金光明，張國(guó)良，陳林鵬.MEMS陀螺儀靜態(tài)漂移模型與濾波方法研究[J].傳感器與微系統(tǒng)，2007，26(11)：48-50.

[17] 馮智勇，曾瀚，張力，等.基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測(cè)量[J].西南師范大學(xué)學(xué)報(bào)(自然科學(xué)版)，2011，36(4)：137-141.

[18] 劉星.多維MEMS慣性傳感器的姿態(tài)解算算法研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.