0 引言

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

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

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

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

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

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

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

    四元數(shù)的數(shù)學(xué)概念是1843年由哈密頓首先提出的,。它是由一個實部單位和3個虛部單位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ù)時必須進行歸一化,，也就是令其模值為1。

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

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

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

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

    設(shè)：

    則：

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

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

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

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

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

3 實驗結(jié)果分析

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

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

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

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

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

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

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

4 結(jié)論

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

參考文獻

[1] 杜海龍，張榮輝,，劉平,，等.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算模塊的實現(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] 楊凌霄,，李曉陽.基于卡爾曼濾波的兩輪自平衡車姿態(tài)解算方法[J].計算機仿真，2014,，31(6)：406-409.

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

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

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

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

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

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

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

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

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

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

[14] 張慧春,，吳簡彤，何昆鵬,，等.四階龍格-庫塔法在捷聯(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] 金光明,，張國良，陳林鵬.MEMS陀螺儀靜態(tài)漂移模型與濾波方法研究[J].傳感器與微系統(tǒng),，2007,，26(11)：48-50.

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

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