黃 洋，姜文剛

江蘇科技大學(xué) 電子信息學(xué)院，江蘇 鎮(zhèn)江 212003

1 引言

基于慣性傳感器的機械臂示教系統(tǒng)是通過采集固定于人體手臂各個位置的慣性傳感器數(shù)據(jù)，經(jīng)過姿態(tài)解算與空間位置計算得到機械臂姿態(tài)運動軌跡，其核心在于慣性傳感器解算姿態(tài)角度的精確性。Rehbinder和Hu在其研究姿態(tài)解算算法的時候，采用了基于MEMS（微機電傳感器）技術(shù)的陀螺儀和加速度計傳感器組合的姿態(tài)解算算法，俯仰角和橫滾角得到了一定的校正，但是航向角容易發(fā)生漂移。Bachmann在進行三維空間姿態(tài)測量的研究中加入了磁力計，由磁力計數(shù)據(jù)對航向角進行校正。由于姿態(tài)測量環(huán)境中的隨機干擾對MEMS傳感器測量精度的影響較大，同時受制于其本身的精度問題，導(dǎo)致經(jīng)過誤差標(biāo)定的MEMS傳感器在采集數(shù)據(jù)以及之后的姿態(tài)解算過程中不可避免的引起姿態(tài)角較大的偏差，最終導(dǎo)致姿態(tài)數(shù)據(jù)的漂移。

針對上述問題，參考捷聯(lián)慣導(dǎo)相關(guān)算法[1-2]，設(shè)計了一種采用組合傳感器測量[3]和組合濾波的姿態(tài)解算方法。使用六個慣性傳感器單元對載體坐標(biāo)系的三軸正負(fù)軸向分別測量加速度，磁場強度和角速度數(shù)據(jù)，采用四元數(shù)法對姿態(tài)角進行求取。將磁羅盤算法和四元數(shù)估計法得到的兩組姿態(tài)角的誤差分別作為模糊調(diào)節(jié)器和PI調(diào)節(jié)器的輸入?yún)?shù)，使用互補濾波[4-5]分別修正兩組角速度的輸出數(shù)據(jù)，通過卡爾曼濾波器[6]減小誤差，得到更精確的姿態(tài)角數(shù)據(jù)。該方法結(jié)合了模糊調(diào)節(jié)的快速性和PI調(diào)節(jié)的精確性，通過互補濾波來抑制陀螺儀角速度漂移，保證姿態(tài)解算系統(tǒng)的穩(wěn)定性，在姿態(tài)解算過程中通過卡爾曼濾波能有效抑制噪聲[7]，通過迭代估計提高姿態(tài)角解算的精度。該方法相較于普通互補濾波解算姿態(tài)的方法更適合于示教機器人的姿態(tài)解算系統(tǒng)。

2 人體姿態(tài)解算方法原理

人體姿態(tài)變化即人體坐標(biāo)系相對于基坐標(biāo)系的空間轉(zhuǎn)動。假設(shè)人體運動為剛體運動，在慣性導(dǎo)航系統(tǒng)中用俯仰角θ，橫滾角γ和航向角ψ作為姿態(tài)角的表示方法。取人體坐標(biāo)系為b系，參考坐標(biāo)系為n系。xnynzn為參考坐標(biāo)系的東北地方向，初始xbybzb與參考坐標(biāo)系三軸重合。經(jīng)過旋轉(zhuǎn)后參考坐標(biāo)系與人體坐標(biāo)系有如下關(guān)系[8]：

其中Cbn以歐拉角形式表示為：器之間存在的耦合等因素，常規(guī)使用單組MEMS傳感器進行姿態(tài)解算的方法難以保證示教機械臂姿態(tài)角解算的精度和穩(wěn)定性。而采用六組傳感器可以有效減小微傳感器之間的耦合所產(chǎn)生的量測誤差，組合調(diào)節(jié)能夠結(jié)合模糊調(diào)節(jié)的快速性和PI調(diào)節(jié)的精確性，經(jīng)過互補濾波對正負(fù)兩組三軸角速度數(shù)據(jù)同時調(diào)節(jié)來抑制陀螺儀漂移，得到更精確的角速度數(shù)據(jù)，EKF（拓展卡爾曼濾波）濾波器可以根據(jù)調(diào)節(jié)后的角速度數(shù)據(jù)進行姿態(tài)估計并抑制噪聲，能夠滿足系統(tǒng)精確性和穩(wěn)定性的要求，因此設(shè)計了一種采用模糊和PI組合調(diào)節(jié)，互補濾波和EKF組合濾波的多組傳感器姿態(tài)解算方法。

3 算法結(jié)構(gòu)及分析過程

3.1 算法結(jié)構(gòu)

設(shè)計的算法總體結(jié)構(gòu)如圖1所示。載體坐標(biāo)系中按坐標(biāo)軸固定的六組傳感器分別測得各個軸向上加速度，磁場強度和角速度數(shù)據(jù)，可歸為正負(fù)兩組數(shù)據(jù)。將傳感器測量數(shù)據(jù)與四元數(shù)法估計數(shù)據(jù)的向量積分別作為模糊調(diào)節(jié)器和PI調(diào)節(jié)器的輸入，分別與兩組角速度數(shù)據(jù)進行互補濾波，然后通過EKF濾波進行四元數(shù)求解，進而轉(zhuǎn)化為姿態(tài)角。

由此可以計算出歐拉角方式下姿態(tài)角數(shù)據(jù)，即：

圖1 算法總體結(jié)構(gòu)框圖

式（3）即為示教機器人姿態(tài)解算的歐拉角方程。在示教機器人勻速運動的過程中，傳統(tǒng)磁羅盤算法可以通過各傳感器數(shù)據(jù)得出姿態(tài)角。載體坐標(biāo)系b下加速度計測得加速度ab=[]T，磁力計測得磁場強度mb=[]T，考慮東向磁場強度近似為0，參考坐標(biāo)系下加速度與磁場強度an=[0 0 g]T，mn=[0]T，根據(jù)坐標(biāo)變換矩陣可以得到姿態(tài)角數(shù)據(jù)，即：

其中角速度調(diào)節(jié)部分是根據(jù)傳感器量測誤差，利用模糊調(diào)節(jié)和PI調(diào)節(jié)，根據(jù)互補濾波原理可以調(diào)節(jié)角速度原始數(shù)據(jù)，能夠快速并精確抑制陀螺儀在運動過程中的漂移，減小角速度誤差，提高系統(tǒng)穩(wěn)定性。EKF濾波部分是根據(jù)調(diào)節(jié)后得到更精確的角速度數(shù)據(jù)來構(gòu)建狀態(tài)方程，傳感器量測數(shù)據(jù)構(gòu)建觀測方程，利用EKF濾波器進行姿態(tài)估計并抑制噪聲。在循環(huán)迭代的過程中，得到更精確的四元數(shù)值，能夠有效地減小傳感器量測誤差對姿態(tài)解算精度的影響，提高系統(tǒng)精度。

3.2 傳感器組合測量

由于低成本MEMS傳感器本身的精度局限，陀螺儀的漂移，外界的隨機干擾和同一傳感器單元中微傳感

設(shè)計的算法采用按坐標(biāo)軸固定的六組傳感器分別測得各個軸向的傳感器數(shù)據(jù)，安裝位置如圖2所示。

圖2 傳感器安裝位置示意圖

由示意圖所示，傳感器1～3可以測得人體加速度a1=[]T，磁場m1=[]T，角速度ω1=[]T，傳感器4～6可以測得符號相反的加速度，磁力和角速度數(shù)據(jù)。根據(jù)坐標(biāo)變換原理可得：

Δax,Δay,Δaz為加速度計測量噪聲。由式（7）、（8）可以求出俯仰角θ與橫滾角γ。

與上述方法一致，考慮東向磁場近似為0，可以得到航向角ψ：

Δmx,Δmy,Δmz為磁力計測量噪聲。根據(jù)組合方式解算出的歐拉角可以通過推導(dǎo)得到四元數(shù)[9-10]初值，即：

3.3 角速度調(diào)節(jié)

在實際應(yīng)用過程中，由于加速度計和磁力計靜態(tài)特性較好，在解算過程中不會產(chǎn)生累計誤差，航時間的穩(wěn)定性較好，但是短時間內(nèi)精度較差。陀螺儀動態(tài)特性較好，短時間內(nèi)精度較高，但是在積分過程中會產(chǎn)生累計誤差，長時間工作會引起漂移，最終導(dǎo)致解算方法失效。因此，可以采用互補濾波的方法對傳感器信息進行融合[11]?；パa濾波公式變換可得：

轉(zhuǎn)換到時域為：

由公式（14）可知互補濾波為通過磁羅盤算法和四元數(shù)估計法得到的姿態(tài)角差值作為輸入量調(diào)節(jié)陀螺儀角速度輸出[12]。由于對陀螺儀角速度的校正中誤差的計算通常采用的梯度下降法比較復(fù)雜且計算量大，因此在低速小范圍運動中采用向量積法代替來計算誤差量[13-14]，即：

其中，eθγ,eψ為加速度計和磁力計的測量向量與四元數(shù)下的觀測向量的叉乘，an,hn為參考坐標(biāo)系下加速度和磁力單位向量，,為載體坐標(biāo)系下傳感器測量值單位向量。加速度計1～3得到，加速度計4～6得到，磁力計1～3得到，磁力計4～6得到，e1=+，e2=+，通過采用模糊調(diào)節(jié)器[15]對陀螺儀1～3輸出的角速度進行調(diào)節(jié)，誤差e1和其在采樣周期內(nèi)的積分為輸入量，u1為輸出量，則：

通過PI調(diào)節(jié)器對陀螺儀4～6輸出的角速度進行調(diào)節(jié)，誤差e2為輸入量，u2為輸出量，則：

由于陀螺儀1～3與陀螺儀4～6安裝方向相反，則載體角速度可由均值得到：

3.4 EKF姿態(tài)解算

由四元數(shù)作為狀態(tài)向量，根據(jù)四元數(shù)微分方程可以建立EKF的狀態(tài)方程，即：

其中

其中ω(k)為根據(jù)式（20）得到的調(diào)節(jié)后的角速度數(shù)據(jù)，W(k -1)近似為白噪聲矩陣，Γ(k -1)為噪聲驅(qū)動矩陣。

以傳感器1～3測量的加速度和磁力數(shù)據(jù)的單位向量為觀測量，建立觀測方程：

在小范圍手臂示教運動情況下，可近似認(rèn)為有害加速度趨近于0，磁場無變化，即：

則可以得到

4 仿真驗證

為了驗證所設(shè)計的姿態(tài)解算方法，MATLAB中設(shè)定六組傳感器兩兩相差90°，固定于手臂同一位置，其中加速度計隨機誤差為1×10-4g，陀螺儀隨機誤差為0.1°/h，磁力計隨機誤差為50 nt，各傳感器誤差相互獨立，量測過程中互不影響。人體手臂運動屬于低速小范圍運動，頻率一般低于15 Hz，則當(dāng)截止頻率 fc為15 Hz時，K為0.8，采樣周期ts為0.1 s，仿真時長30 s。在陀螺儀角速度校正部分，模糊調(diào)節(jié)器設(shè)定誤差，誤差積分和輸出量的量化因子分別為3，1，1。論域分別為[- 6 6]，[-3 3]，[- 9 9]。模糊化過程采用三角形隸屬度函數(shù)，模糊規(guī)則表根據(jù)經(jīng)驗設(shè)計。PI調(diào)節(jié)器設(shè)定P為0.8，I為0.02。卡爾曼濾波器中，根據(jù)文獻[12]的方法取各協(xié)方差矩陣[16]為 Q=0.000 1I3×3，R=0.001I6×6，P0=0.001I4×4。分別采用文獻[11]中PI調(diào)節(jié)方法和設(shè)計的組合調(diào)節(jié)方法對姿態(tài)角靜態(tài)和動態(tài)情況下姿態(tài)角變化做仿真，得到姿態(tài)角誤差曲線圖，靜態(tài)仿真圖如圖3～5所示。

靜態(tài)情況下，調(diào)節(jié)前后的姿態(tài)角誤差如圖3～5所示。圖3為使用單組傳感器無調(diào)節(jié)姿態(tài)角誤差，圖4為加入PI調(diào)節(jié)后的姿態(tài)角誤差，圖5為使用多組傳感器，并采用模糊PI組合調(diào)節(jié)的姿態(tài)角誤差，誤差參數(shù)見表1，range表示誤差范圍，std表示誤差標(biāo)準(zhǔn)差。

圖3 無調(diào)節(jié)靜態(tài)姿態(tài)角誤差

圖4 PI調(diào)節(jié)靜態(tài)姿態(tài)角誤差

圖5 模糊PI組合調(diào)節(jié)靜態(tài)姿態(tài)角誤差

表1 靜態(tài)姿態(tài)角誤差參數(shù)比較

由表1中可以看出，圖4中采用文獻[11]中PI調(diào)節(jié)的方法，姿態(tài)角誤差的標(biāo)準(zhǔn)差從0.507°、0.643°、0.519°降低到0.136°、0.157°、0.206°。圖5中PI調(diào)節(jié)與模糊調(diào)節(jié)進行組合后，姿態(tài)角標(biāo)準(zhǔn)差進一步變?yōu)?.096°、0.098°、0.142°，姿態(tài)角誤差標(biāo)準(zhǔn)差有進一步減小。表明在靜態(tài)情況下，設(shè)計的算法相比文獻[11]中的算法能夠減小姿態(tài)角誤差和提高系統(tǒng)穩(wěn)定性的效果。

在動態(tài)仿真的過程中，假設(shè)航向角由0°勻速變化為－90°，同時適當(dāng)增大各傳感器的隨機誤差，動態(tài)仿真圖如圖6所示。

圖6 模糊PI組合調(diào)節(jié)航向角變化曲線

圖6 為航向角變化的曲線，即從0°勻速變化為－90°。從圖中可以看出，雖然航向角在變化過程中存在微小的誤差波動，但是曲線整體變化近似于真實曲線。

動態(tài)情況下，調(diào)節(jié)前后的姿態(tài)角誤差如圖7～9所示。圖7為使用單組傳感器無調(diào)節(jié)姿態(tài)角誤差，圖8為加入PI調(diào)節(jié)后的姿態(tài)角誤差，圖9為使用多組傳感器，并采用模糊PI組合調(diào)節(jié)的姿態(tài)角誤差，誤差參數(shù)見表2。

圖7 無調(diào)節(jié)動態(tài)姿態(tài)角誤差

圖8 PI調(diào)節(jié)動態(tài)姿態(tài)角誤差

圖9 模糊PI組合調(diào)節(jié)動態(tài)姿態(tài)角誤差

表2 動態(tài)姿態(tài)角誤差參數(shù)比較

從表2中可以看出，圖8中采用文獻[11]中PI調(diào)節(jié)的方法，動態(tài)姿態(tài)角誤差的標(biāo)準(zhǔn)差分別從0.792°、0.997°、1.135°降低為0.298°、0.275°、0.608°。圖9中采用模糊PI組合調(diào)節(jié)方法后，姿態(tài)角誤差的標(biāo)準(zhǔn)差進一步減小為0.193°、0.188°、0.308°，這表明在航向角動態(tài)變化過程中采用模糊PI組合調(diào)節(jié)的方法相比在單一PI調(diào)節(jié)能進一步減小姿態(tài)角誤差，并且提高系統(tǒng)穩(wěn)定性，尤其對變化的航向角有更好的效果。

組合了模糊調(diào)節(jié)和PI調(diào)節(jié)，融合了互補濾波和卡爾曼濾波的多組傳感器姿態(tài)解算方法對靜態(tài)和動態(tài)情況下姿態(tài)角的誤差消除都有比較好的效果，尤其是在航向角變化的情況下相比使用單組傳感器PI調(diào)節(jié)的姿態(tài)角的精度和穩(wěn)定性有進一步的提高，使得航向角變化接近于真值。因此，設(shè)計的算法在示教機械臂姿態(tài)解算中能有效提高姿態(tài)解算的精度，提高系統(tǒng)的穩(wěn)定性。

5 結(jié)束語

在傳統(tǒng)加速度計，磁力計和陀螺儀信息融合算法的基礎(chǔ)上，將傳感器組增加到六個，使用六軸向安裝的MEMS傳感器分別測得各自軸向的傳感器數(shù)據(jù)。以該方法得到的傳感器量測向量與四元數(shù)法得到的估計向量的兩個向量積分別作為模糊調(diào)節(jié)器和PI調(diào)節(jié)器的輸入量，輸出量經(jīng)過互補濾波分別調(diào)節(jié)兩組角速度向量，取均值后輸入卡爾曼濾波器中，得到更精確的四元數(shù)估計值，再經(jīng)過變換得到姿態(tài)角。仿真結(jié)果表明，設(shè)計的算法在靜態(tài)和動態(tài)情況下均能有效提高姿態(tài)角解算精度，并且對變化的航向角精度和穩(wěn)定性提高有比較好的效果。