吉寒冬，姜文剛

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

0 引言

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

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

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

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

設(shè)地理坐標(biāo)系為xeyeze東北地方向，載體坐標(biāo)系xbybzb，空間關(guān)系如下：

(1)

(2)

(3)

(4)

(5)

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

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

2.1 算法的總體結(jié)構(gòu)

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

圖1 姿態(tài)解算總體結(jié)構(gòu)圖

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

2.2 多傳感器信息融合

當(dāng)載體靜止或勻速運(yùn)動(dòng)，MEMS傳感器中加速度計(jì)測(cè)量值與空間變換矩陣之間的關(guān)系：

(6)

(7)

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

(8)

(9)

由于參考坐標(biāo)系選擇東北地，東向磁場(chǎng)近似為0，可以求得φ1：

(10)

同理可得第2組對(duì)角安裝傳感器的姿態(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為磁力計(jì)測(cè)量噪聲，傳感器組合安裝能夠減小內(nèi)部各微傳感器之間的耦合誤差。

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

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

(17)

轉(zhuǎn)換成時(shí)域?yàn)?/p>

(18)

當(dāng)載體為低速運(yùn)動(dòng)時(shí)，采用向量積法求得各誤差量，即：

(19)

(20)

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

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

(21)

(22)

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

(23)

(24)

(25)

(26)

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

(27)

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

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

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

預(yù)測(cè)計(jì)算：

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

(28)

Zk=HXk+vk

(29)

卡爾曼量測(cè)更新校正計(jì)算：

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)

自適應(yīng)估計(jì)器：

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ū)動(dòng)矩陣;ek為殘差;Hk為量測(cè)矩陣；Wk近似為白噪聲;Qk為系統(tǒng)噪聲協(xié)方差矩陣;Rk為量測(cè)噪聲協(xié)方差矩陣。

3 仿真驗(yàn)證

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

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

圖5 俯仰角變化曲線

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

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

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

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

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

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

4 結(jié)束語

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

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

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

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

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