0 引言

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

    隨著安卓智能手機的發(fā)展與普及，為了讓前臂假肢的控制更加方便和標準化，提出了以安卓手機離線識別語音及應用藍牙通信技術實現(xiàn)安卓手機無線控制假肢運動的方案。本文介紹的前臂假肢可實現(xiàn)手部平衡，除此之外，在不需要手部保持平衡的環(huá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)關節(jié)的上部并固定在連桿上，左右位置可以變化，但要確保其所在的平面與手爪虎口所在平面平行。安裝完成后，前臂繞MPU6050的X軸旋轉(zhuǎn)，手腕繞MPU6050的Y軸俯仰。

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

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

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

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

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

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

    設：

    則：

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

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

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

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

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

3 實驗結果分析

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

    將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ā)生相應的變化，俯仰角基本保持不變，如圖6(a)所示。令前臂假肢繞MPU6050的Y軸重復以上實驗，根據(jù)圖6(b)可以看出，俯仰角發(fā)生相應的變化，橫滾角基本保持不變。由于實驗過程中手動旋轉(zhuǎn)前臂假肢，操作者偶有抖動和難做到勻速轉(zhuǎn)動，因此，圖6中的波形中偶有小的波動。實驗表明，本文采用的姿態(tài)估計可以實現(xiàn)前臂假肢動作時姿態(tài)的正確估計，經(jīng)過STM32運算處理后，實現(xiàn)前臂假肢在運動過程中保持手部平衡。

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

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

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

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

4 結論

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

參考文獻

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

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

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

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

[5] 呂印新，肖前貴，胡壽松.基于四元數(shù)互補濾波的無人機姿態(tài)解算[J].燕山大學學報，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] 趙國文.機器人運動學中的四元數(shù)矩陣和基變換矩陣[J].機器人，1993，15（2）：59-64.

[10] 張?zhí)旃?，王秀萍，王麗霞，等，譯.捷聯(lián)慣性導航技術(第二版)[M].北京：國防工業(yè)出版社，2007：29-33.

[11] 秦永元.慣性導航[M].北京：科學出版社，2006：287-305.

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

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

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

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

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

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

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