吉寒冬，姜文剛

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

0 引言

近年來，微機電系統(tǒng)(MESM)慣性傳感器以及計算機技術快速發(fā)展[1]，由于MEMS傳感器體積小、功耗低、成本低，被廣泛運用于航海、機器人以及無人機等領域。但由于傳感器本身精度低，陀螺儀長時間會產(chǎn)生累積誤差，發(fā)生漂移，同時會受到載體運動加速度和磁場環(huán)境等因素影響，導致姿態(tài)角解算精度低、穩(wěn)定性差。

為了解決上述問題,陳灣灣使用十二位置標定法，標定MIMU加速度計和陀螺儀的零偏穩(wěn)定性和安裝誤差角，對加速度計和陀螺儀的輸出值進行補償，提高姿態(tài)角解算精度[2];魏志方利用時間序列自回歸滑動平均模型分析方法對原始角速度進行誤差建模，通過卡爾曼濾波算法進行誤差補償[3];李景輝采用PI和互補濾波算法，抑制陀螺儀漂移，提高角速度精度[4]；Z. X. Hu將拓展卡爾曼濾波應用于傳感器姿態(tài)解算和漂移補償[5]；黃洋使用多組傳感器進行數(shù)據(jù)采集，運用模糊和PI組合算法對數(shù)據(jù)進行調(diào)節(jié)，采用互補濾波與EKF算法分別對誤差進行補償，對姿態(tài)數(shù)據(jù)進行去噪以及預測估計[6]；Y. L. Wang研究了利用GPS與MEMS傳感器的組合定姿算法[7]；孟秀云改進自適應擴展卡爾曼濾波算法，解決傳統(tǒng)自適應擴展卡爾曼(AEKF)濾波發(fā)散問題[8]。

文中提出一種基于多MEMS傳感器組合姿態(tài)解算方法。采用12組MEMS傳感器，兩兩對角安裝在載體坐標系各軸上，使用兩組模糊和兩組PI算法對測量模塊的測量值與觀測值之間的向量積進行調(diào)節(jié)；利用互補濾波對角速度進行誤差補償，抑制陀螺儀漂移，最后通過自適應擴展卡爾曼濾波對姿態(tài)進行預測估計，求得精確的姿態(tài)角數(shù)據(jù)。

1 初始姿態(tài)角模型

設地理坐標系為xeyeze東北地方向，載體坐標系xbybzb，空間關系如下：

(1)

(2)

(3)

(4)

(5)

由于MEMS傳感器本身工藝的局限性，導致其精度低，陀螺儀漂移，加之外部環(huán)境的隨機干擾，MEMS慣性傳感器中各微傳感器模塊之間存在耦合誤差，因此傳統(tǒng)方法已經(jīng)難以滿足實際需求。所以提出一種多MEMS組合傳感器姿態(tài)解算算法方法，來進行姿態(tài)解算求解。

2 數(shù)據(jù)信息融合

2.1 算法的總體結構

載體坐標系各軸上兩兩對角安裝4組傳感器，將采集的數(shù)據(jù)與四元數(shù)估計數(shù)據(jù)的向量積，通過兩組模糊和兩組PI算法組合調(diào)節(jié)，互補濾波調(diào)節(jié)角速度，利用AEKF對姿態(tài)進行預測估計，進而轉(zhuǎn)化為高精度姿態(tài)角。姿態(tài)解算總體結構圖如圖1所示。

圖1 姿態(tài)解算總體結構圖

傳感器安裝的優(yōu)點：減小傳感器內(nèi)部的耦合誤差；確保系統(tǒng)可靠性；利用模糊算法的快速性和PI算法的準確性調(diào)節(jié)數(shù)據(jù)偏差，互補濾波可以抑制角速度漂移，減小角速度誤差，利用AEKF抑制噪聲，通過迭代估計提高姿態(tài)角數(shù)據(jù)。

2.2 多傳感器信息融合

當載體靜止或勻速運動，MEMS傳感器中加速度計測量值與空間變換矩陣之間的關系：

(6)

(7)

其中加速計1和加速計2對角安裝。Δax、Δay、Δaz為加速度計的測量噪聲，通過式(6)和式(7)之間的關系可求得第1組對角安裝的θ1和γ1。

(8)

(9)

由于參考坐標系選擇東北地，東向磁場近似為0，可以求得φ1：

(10)

同理可得第2組對角安裝傳感器的姿態(tài)角數(shù)據(jù)如下：

(11)

(12)

(13)

由上式可得系統(tǒng)的姿態(tài)角分別為：

θ=(θ1+θ2)/2

(14)

γ=(γ1+γ2)/2

(15)

φ=(φ1+φ2)/2

(16)

Δmx、Δmy、Δmz為磁力計測量噪聲，傳感器組合安裝能夠減小內(nèi)部各微傳感器之間的耦合誤差。

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

在研究中發(fā)現(xiàn)，加速度計和磁力計動態(tài)響應差，靜態(tài)響應好，在姿態(tài)解算中不會產(chǎn)生累積誤差，而陀螺儀在進行姿態(tài)解算時，會產(chǎn)生累積誤差，但動態(tài)響應良好，根據(jù)它們在頻域上有互補特性，采用互補濾波融合所測得的數(shù)據(jù)[9]，提高系統(tǒng)的穩(wěn)定性?；パa濾波可變換得：

(17)

轉(zhuǎn)換成時域為

(18)

當載體為低速運動時，采用向量積法求得各誤差量，即：

(19)

(20)

式中：eθφ、eφ為加速度計和磁力計的測量數(shù)據(jù)與四元數(shù)觀測數(shù)據(jù)的向量積運算；ha和hm為在載體坐標系下加速度和磁力單位向量。

通過模糊算法對傳感器1和傳感器3陀螺儀輸出角速度進行調(diào)節(jié)，誤差e1和e3與其在采樣周期內(nèi)的積分為輸入量，u1和u3為輸出量，得：

(21)

(22)

傳感器2和傳感器4由PI控制器對其進行調(diào)節(jié)，誤差e2和e4作為輸入量，u2和u4作為PI控制器的輸出量，得：

(23)

(24)

(25)

(26)

載體的角速度可由均值得到：

(27)

設定截止頻率fc=15 Hz，k為0.8，kp、ki取值為1.5、0.03。

2.4 自適應擴展卡爾曼濾波姿態(tài)解算

自適應擴展卡爾曼濾波能抑制噪聲[10]，對非線性系統(tǒng)具有較好的估計與修正作用，迭代估計提高姿態(tài)角精度，具體濾波算法如下：

預測計算：

Xk,k-1=Φk,k-1Xk-1+Γk-1Wk-1

(28)

Zk=HXk+vk

(29)

卡爾曼量測更新校正計算：

Kk=Pk,k-1HT(HPk,k-1HT+Rk)-1

(30)

ek=Zk-HXk,k-1-rk

(31)

Xk=Xk,k-1+Kkek

(32)

(33)

Pk=(I-KkHk)Pk,k-1

(34)

自適應估計器：

Wk=(1-dk-1)Wk-1+dk-1Gk(Xk-Φk,k-1Xk-1)

(35)

(36)

(37)

rk=(1-dk-1)rk-1+dk-1(Zk-HkXk,k-1)

(38)

(39)

(40)

式中：Φk,k-1為tk-1到tk的一步轉(zhuǎn)移矩陣;Γk-1為系統(tǒng)噪聲驅(qū)動矩陣;ek為殘差;Hk為量測矩陣；Wk近似為白噪聲;Qk為系統(tǒng)噪聲協(xié)方差矩陣;Rk為量測噪聲協(xié)方差矩陣。

3 仿真驗證

為了驗證所提出的方法，在MATLAB中設載體坐標系各軸采用4組傳感器兩兩對角安裝，設定加速度傳感器的隨機誤差為1×10-4g(g表示重力加速度)，磁阻尼傳感器的隨機誤差為50 nT，靜態(tài)測量陀螺儀隨機誤差為1/900°/h，MEMS傳感器內(nèi)部各微傳感器之間誤差相互獨立，互不干擾，采用二維模糊控制器，模糊調(diào)節(jié)器設定誤差、誤差變化率和輸出量的論域分別為[-6，6]、[-6，6]、[-3，3]，模糊集均為{NB,NM,NS,ZO,PS,PM,PB}，模糊推理語句為If A and B then C；模糊規(guī)則表根據(jù)專家經(jīng)驗設計；選用三角形隸屬函數(shù)trimf(x,[a,b,c])；選擇Mamdani模糊系統(tǒng)進行推理；反模糊化選用面積重心法centroid。設定自適應擴展卡爾曼濾波中各協(xié)方差矩陣初始值為Q0=0.000 1I3×3，P0=0.001I4×4，R0=0.001I6×6，分別在靜態(tài)和動態(tài)兩種情況下對傳統(tǒng)姿態(tài)角解算以及所提方法姿態(tài)角解算進行仿真數(shù)據(jù)分析，在靜態(tài)時理想狀態(tài)下，各姿態(tài)角應都為0。靜態(tài)仿真結果如圖2～圖4所示。靜態(tài)姿態(tài)角誤差參數(shù)分析見表1。

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

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

圖4 靜態(tài)模糊和PI組合調(diào)節(jié)以及本文所提方法姿態(tài)誤差

表1 靜態(tài)姿態(tài)角誤差參數(shù)分析 (°)

在動態(tài)調(diào)節(jié)情況下，設定俯仰角θ由0°勻速變化為90°，陀螺儀的隨機誤差增加到1/400°/h，動態(tài)仿真圖如圖5所示。

圖5 俯仰角變化曲線

圖5為俯仰角變化曲線，θ從0°勻速變化為90°，從圖中可以看出俯仰角變化過程波動非常小，有微小誤差，曲線的整體變化趨于真實曲線。

動態(tài)情況下仿真對比姿態(tài)角誤差如圖6～圖8所示，姿態(tài)誤差參數(shù)見表2。

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

通過表1～表2可得，MEMS傳感器在無調(diào)節(jié)時，誤差很大，尤其在動態(tài)的情況下，通過數(shù)據(jù)可以看出陀螺儀漂移大，穩(wěn)定性低；當加入了PI調(diào)節(jié)，誤差得到抑制，數(shù)據(jù)波動減小，姿態(tài)角精度得到提高，穩(wěn)定性也得到提高；而通過模糊和PI組合調(diào)節(jié)，系統(tǒng)誤差進一步減??；而文中所提的方法：采用多組MEMS傳感器組合采集數(shù)據(jù)，通過兩組模糊和兩組PI進行組合調(diào)節(jié)，互補濾波與自適應擴展卡爾曼濾波組合對角速度進行補償，系統(tǒng)誤差更進一步減小，尤其在動態(tài)情況下，模擬俯仰角由0°勻速變化為90°，可以從動態(tài)圖看出，曲線變化接近真實曲線，從圖8可以得出，所提出的方法，穩(wěn)定性更好，通過以上數(shù)據(jù)分析對比更能凸顯出文中所提出方法的優(yōu)越性和先進性，更能滿足實際需求。

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

表2 動態(tài)姿態(tài)角誤差參數(shù)分析 (°)

4 結束語

文中對傳統(tǒng)MEMS傳感器姿態(tài)解算所存在的問題進行分析，并提出一種多MEMS組合傳感器姿態(tài)解算方法，具體步驟如下：

(1)在載體坐標系各軸上分別采用4組傳感器兩兩對角安裝，多傳感器組合安裝的優(yōu)點：能夠減小各慣性模塊之間的耦合誤差問題；提高系統(tǒng)的可靠性；提高輸入、輸出信息可信度。

(2)采用兩組模糊和兩組PI算法組合調(diào)節(jié)，相較于使用單PI調(diào)節(jié)，更加精準，而采用互補濾波可以抑制角速度漂移。

(3)利用自適應擴展卡爾曼濾波一方面能夠?qū)ψ藨B(tài)角進行預測估計，更新四元數(shù)微分方程，提高姿態(tài)角精度，另一方面能夠減弱PI算法和互補濾波算法相結合可能導致的超調(diào)和振蕩問題。

后續(xù)需要針對自適應擴展卡爾曼濾波所存在的問題進行進一步研究。這些問題包括公式矩陣較多，計算量較大；系統(tǒng)噪聲協(xié)方差與量測噪聲協(xié)方差同時更新會引起濾波發(fā)散；系統(tǒng)中仍存在超調(diào)和振蕩等。