基于四元數(shù)互補(bǔ)濾波算法的前臂假肢手部自平衡系統(tǒng)設(shè)計(jì)

王傳江 王棟 侯鵬亮 袁振 張遠(yuǎn)來

（1山東科技大學(xué)機(jī)器人研究中心，山東青島,266590； 2山東省機(jī)器人與智能技術(shù)重點(diǎn)實(shí)驗(yàn)室，山東青島，266590；3山東大學(xué)控制科學(xué)與工程學(xué)院，濟(jì)南,250061）

0 引言

目前，市場上大部分前臂假肢多為裝飾假肢，僅僅起到美觀、裝飾的作用，并不能真正實(shí)現(xiàn)其代償功能。為了滿足前臂截肢者端水等基本日常生活需要，本文設(shè)計(jì)了一種兩自由度電動(dòng)前臂假肢手部自平衡系統(tǒng)，通過控制前臂旋轉(zhuǎn)和手腕俯仰，使得患者前臂假肢在運(yùn)動(dòng)過程中其假肢末端手部虎口處始終與水平面保持平行。

前臂假肢手部自平衡系統(tǒng)主要包括姿態(tài)解算和手臂控制兩部分，由于人類在運(yùn)動(dòng)時(shí)所做動(dòng)作的復(fù)雜性和不確定性，因此要使假肢手部能夠在運(yùn)動(dòng)時(shí)始終保持平衡，姿態(tài)解算則是實(shí)現(xiàn)該系統(tǒng)自平衡的關(guān)鍵問題之一。

姿態(tài)解算的速度和精度會(huì)直接影響整個(gè)系統(tǒng)的穩(wěn)定性和可靠性，是實(shí)現(xiàn)手臂控制的重要前提和基礎(chǔ)。姿態(tài)解算需要依靠慣性測量器件，而MEMS器件由于具有小體積、低成本、低功耗等特點(diǎn)，因此成為前臂假肢慣性測量器件的首選。但是，由于MEMS器件容易受溫度和外界噪聲的影響，需根據(jù)姿態(tài)解算算法對其進(jìn)行多傳感器的數(shù)據(jù)融合，才能得到實(shí)時(shí)準(zhǔn)確的姿態(tài)。

目前，常用的多傳感器姿態(tài)解算算法有基于四元數(shù)的卡爾曼濾波[1]、基于四元數(shù)的擴(kuò)展卡爾曼濾波[2]、歐拉角法等。其中，卡爾曼濾波所需計(jì)算量較大，在低頻處理器上運(yùn)行，其輸出姿態(tài)的實(shí)時(shí)性和可靠性較低；而歐拉角法則存在萬向節(jié)死鎖的問題。

針對上述問題，本文采用了互補(bǔ)濾波算法對前臂假肢手部進(jìn)行姿態(tài)估計(jì)，實(shí)驗(yàn)表明，該算法通過對加速度計(jì)和陀螺儀進(jìn)行互補(bǔ)融合，有效地消除了漂移誤差，能夠快速、可靠地跟蹤手部姿態(tài)，并且實(shí)現(xiàn)前臂假肢手部的自平衡。

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

本文設(shè)計(jì)的前臂假肢除了能夠?qū)崿F(xiàn)手部自平衡，還可實(shí)現(xiàn)手部的張合、內(nèi)外轉(zhuǎn)動(dòng)等動(dòng)作，為了使假肢使用者能夠更加簡單、有效地對其進(jìn)行操控，該前臂假肢采用LD3320語音識別模塊來接收使用者的控制指令。LD3320語音識別精度高、功耗低，可對多條關(guān)鍵詞進(jìn)行識別，能夠滿足該前臂假肢設(shè)計(jì)需要。

慣性測量單元采用MPU6050傳感器，其集成了3軸加速度計(jì)和3軸陀螺儀，免除了組合加速度計(jì)與陀螺儀時(shí)間軸之差的問題。前臂假肢硬件系統(tǒng)結(jié)構(gòu)示意圖如圖1所示。

圖1 前臂假肢硬件結(jié)構(gòu)

在該系統(tǒng)中，MPU6050傳感器安裝在前臂旋轉(zhuǎn)電機(jī)固定支架上，前臂假肢機(jī)械結(jié)構(gòu)示意圖如圖2所示。初始條件下，MPU6050傳感器應(yīng)與手部虎口處保持平行。當(dāng)主控芯片STM32接收到語音識別單元的“自平衡”指令后，前臂假肢進(jìn)入自平衡模式，此時(shí)STM32通過采集MPU6050的原始數(shù)據(jù)進(jìn)行姿態(tài)解算，然后根據(jù)解算姿態(tài)計(jì)算出舵機(jī)所需控制角度，并通過RS485通信把控制量發(fā)送給舵機(jī)，從而使手部虎口處始終與水平面保持水平。

圖2 前臂假肢機(jī)械結(jié)構(gòu)

2 互補(bǔ)濾波姿態(tài)估計(jì)算法

2.1 坐標(biāo)系描述

為了描述前臂假肢手部的姿態(tài)信息，本文定義了2個(gè)不同的空間三維坐標(biāo)系，分別為參考坐標(biāo)系 和載體坐標(biāo)系 ，如圖3所示。

圖3 參考坐標(biāo)系與載體坐標(biāo)系的關(guān)系

2.2 坐標(biāo)系間的轉(zhuǎn)換關(guān)系

式（1）中， 為坐標(biāo)轉(zhuǎn)換矩陣，也稱為姿態(tài)矩陣[3-4]，歐拉角所表征的 表達(dá)式為：

式（2）中所表示的姿態(tài)矩陣，由于計(jì)算量大，在微控制器中運(yùn)行效率低，無法滿足姿態(tài)的實(shí)時(shí)性要求，所以本文采用四元數(shù)法進(jìn)行姿態(tài)解算。四元數(shù)定義為 ：

式（3）中， ，其為四元數(shù) 的矢量部分；為四元數(shù) 的標(biāo)量部分。由四元數(shù)運(yùn)算法則可知，四元數(shù) 滿足的歸一化約束條件為：

則坐標(biāo)系 和 之間四元數(shù)轉(zhuǎn)換矩陣為[5]：

由式（2）和式（4）可得載體姿態(tài)角為：

2.3 四元數(shù)姿態(tài)更新算法

由四元數(shù)性質(zhì)可知，四元數(shù)的更新本質(zhì)就是求解四元數(shù)微分方程，四元數(shù)微分方程表示為[6]：

假設(shè)系統(tǒng)采樣周期為T，在(t，t+T)時(shí)間間隔內(nèi)，角速度 、 、 為三軸陀螺儀的輸出，采用一階龍格庫塔法求解四元數(shù)微分方程為：

根據(jù)公式（9）和公式（10），可得t+T時(shí)刻的四元數(shù)(t+T)，將(t+T)代入公式（6）可得t+T時(shí)刻的三軸歐拉角

2.4 互補(bǔ)濾波姿態(tài)融合

綜上可知，姿態(tài)更新算法主要是對陀螺儀數(shù)據(jù)進(jìn)行迭代運(yùn)算。雖然陀螺儀的動(dòng)態(tài)性能好，短時(shí)間內(nèi)數(shù)據(jù)精度高，但隨著時(shí)間的延長，其容易受外界環(huán)境、溫度等影響而產(chǎn)生隨機(jī)漂移[8]，出現(xiàn)積分誤差。而加速度計(jì)雖然短時(shí)間內(nèi)精度差，但長時(shí)間測量不會(huì)產(chǎn)生積分誤差，所以可通過重力加速度在當(dāng)前載體坐標(biāo)系的分量與加速度計(jì)測得的當(dāng)前重力加速度分量做向量積運(yùn)算，修正陀螺儀引起的誤差[9]。其中由公式（1）和公式（5）可得：

加速度計(jì)所測重力加速度經(jīng)單位化后為 ，則加速度計(jì)對陀螺儀的修正誤差為：

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

3.1 姿態(tài)解算實(shí)驗(yàn)結(jié)果分析

為便于控制器安裝在前臂假肢上，本文所設(shè)計(jì)的前臂假肢手部自平衡系統(tǒng)將STM32、MPU6050、RS485等器件均集成在一塊PCB電路板上，其實(shí)物圖如圖4所示。

圖4 硬件實(shí)物圖

為了測試互補(bǔ)濾波算法的性能，本文分別在靜態(tài)和動(dòng)態(tài)環(huán)境下對有/無互補(bǔ)濾波進(jìn)行了姿態(tài)解算算法的測試和對比。

1）靜態(tài)測試：將控制電路板水平靜止放置在桌面上，采集姿態(tài)解算輸出數(shù)據(jù)并對數(shù)據(jù)進(jìn)行波形繪制，如圖5所示。分別為比例系數(shù)和積分系數(shù)。

圖5 互補(bǔ)濾波前后的靜態(tài)測試對比

由圖5可知，互補(bǔ)濾波前的姿態(tài)角會(huì)隨著時(shí)間漂移，而互補(bǔ)濾波后的姿態(tài)角雖然有小范圍波動(dòng)，但基本保持在水平位置。

2）動(dòng)態(tài)測試：將控制電路板進(jìn)行俯仰和翻滾轉(zhuǎn)動(dòng)后水平放置在桌面上，其動(dòng)作姿態(tài)波形如圖6所示。

圖6 互補(bǔ)濾波前后的動(dòng)態(tài)測試對比

由圖6可知，互補(bǔ)濾波前的姿態(tài)角經(jīng)過一段時(shí)間轉(zhuǎn)動(dòng)后不能回到水平初始角度，而互補(bǔ)濾波后的姿態(tài)角經(jīng)過一段時(shí)間轉(zhuǎn)動(dòng)后均能保持在水平初始角度。

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

在上述實(shí)驗(yàn)的基礎(chǔ)上，將控制電路板安裝在前臂假肢上，對手部進(jìn)行自平衡控制調(diào)試。調(diào)試步驟如下：首先向語音識別模塊發(fā)出“自平衡”指令，指示燈點(diǎn)亮，前臂假肢進(jìn)入手部自平衡模式，然后對假肢進(jìn)行俯仰、橫滾動(dòng)作，觀察手部虎口處是否處于水平狀態(tài)。

經(jīng)過測試，前臂假肢在進(jìn)行俯仰、橫滾動(dòng)作時(shí)，手部虎口處一直保持水平狀態(tài)，實(shí)驗(yàn)結(jié)果如圖7所示。

圖7 前臂假肢俯仰、橫滾測試實(shí)驗(yàn)

4 結(jié)論

本文所介紹的前臂假肢手部自平衡系統(tǒng)采用互補(bǔ)濾波姿態(tài)估計(jì)算法，將加速度計(jì)和陀螺儀的數(shù)據(jù)進(jìn)行互補(bǔ)融合，有效地補(bǔ)償了因陀螺儀漂移造成的姿態(tài)誤差，從而得到了精確、實(shí)時(shí)、可靠的姿態(tài)角度，并通過對前臂假肢姿態(tài)的跟蹤和控制，成功實(shí)現(xiàn)了前臂假肢手部自平衡動(dòng)作。

[[1]薛亮,苑偉政,常洪龍,等.用于微小型飛行器姿態(tài)估計(jì)的四元數(shù)擴(kuò)展卡爾曼濾波算法[J].納米技術(shù)與精密工程,2009,7(2)∶163-167.

[2]賈瑞才.基于四元數(shù)EKF的低成本MEMS姿態(tài)估計(jì)算法[J].傳感技術(shù)學(xué)報(bào),2014(1)∶90-95.

[3] 宋宇,翁新武,郭昕剛.基于四元數(shù)EKF算法的小型無人機(jī)姿態(tài)估計(jì)[J].吉林大學(xué)學(xué)報(bào)(理學(xué)版), 2015,53(3)∶511-518.

[4]梁延德,程敏,何福本,等.基于互補(bǔ)濾波器的四旋翼飛行器姿態(tài)解算[J].傳感器與微系統(tǒng),2011,30(11)∶ 56-58,61.

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

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

[7]關(guān)越魏,何波賢,于仁清,等.基于四元數(shù)解算陀螺儀姿態(tài)角算法的實(shí)現(xiàn)[J].電腦編程技巧與維護(hù), 2015(9)∶32-34.

[8]樊炳輝,張凱麗,王傳江,等.基于四元數(shù)的前臂假肢手部自平衡的設(shè)計(jì)[J].電子技術(shù)應(yīng)用,2016,42(5)∶78-81.

[9]王立,章政,孫平.一種自適應(yīng)互補(bǔ)濾波姿態(tài)估計(jì)算法[J].控制工程,2015,22(5)∶881-886.

[10]孫金秋,游有鵬,傅忠云.基于自適應(yīng)顯式互補(bǔ)濾波的姿態(tài)解算方法[J].測控技術(shù),2015,34(4)∶24-27,31.