孫 偉,吳家驥

(遼寧工程技術(shù)大學(xué)測(cè)繪與地理科學(xué)學(xué)院,遼寧 阜新 123000)

基于MEMS的測(cè)姿系統(tǒng)在虛擬現(xiàn)實(shí)、無(wú)人機(jī)和自動(dòng)車(chē)輛導(dǎo)航等領(lǐng)域有著廣泛應(yīng)用[1-3]。國(guó)內(nèi)外有學(xué)者圍繞MEMS慣性測(cè)姿算法開(kāi)展大量對(duì)應(yīng)研究工作,Madgwick提出采用PID控制理論的梯度下降算法[4],存在比例系數(shù)難以精確問(wèn)題;Mahony提出互補(bǔ)濾波算法[5],但是當(dāng)載體處于靜止、勻速運(yùn)動(dòng)或緩慢變速運(yùn)動(dòng)環(huán)境下時(shí)存在著互補(bǔ)濾波系數(shù)難調(diào)整問(wèn)題;而基于四元數(shù)的EKF方法[6-8]應(yīng)用最為廣泛,當(dāng)狀態(tài)方程和測(cè)量方程為非線性方程時(shí),應(yīng)用EKF模型獲取的是狀態(tài)向量的次優(yōu)估計(jì)[9],同時(shí)存在釋放四元數(shù)歸一化約束的缺點(diǎn)。

圍繞上述問(wèn)題,論文將狀態(tài)擾動(dòng)作為狀態(tài)向量,構(gòu)建線性狀態(tài)方程并通過(guò)廣義加法設(shè)計(jì)一種間接EKF姿態(tài)估計(jì)方案。通過(guò)建立合理的慣性器件輸出模型補(bǔ)償各類(lèi)誤差的影響。在選擇姿態(tài)描述方法時(shí),考慮單一姿態(tài)描述方法存在自身缺陷的問(wèn)題[10-11],選擇用歸一化約束的四元數(shù)進(jìn)行姿態(tài)解算,用方向余弦陣進(jìn)行二者間的轉(zhuǎn)換[12]。與EKF姿態(tài)估計(jì)算法相比,在水平方向上姿態(tài)估計(jì)收斂快且航向誤差小,姿態(tài)估計(jì)更加可靠。

1 四元數(shù)運(yùn)動(dòng)模型建立

(1)

根據(jù)歐拉定理:假設(shè)三維空間里剛體位移時(shí),剛體內(nèi)部至少有一點(diǎn)固定不動(dòng),則此位移等價(jià)于一個(gè)繞著包含那固定點(diǎn)的固定軸的旋轉(zhuǎn)。旋轉(zhuǎn)四元數(shù)的數(shù)學(xué)形式為:

(2)

如圖1所示。

圖1中,單位矢量rn=[rxryrz]。在b系相對(duì)n系運(yùn)動(dòng)過(guò)程中,采用極限思想計(jì)算轉(zhuǎn)動(dòng)速率:

(3)

并代入式(3):

(4)

式中:θ=rn·θ;ω=[ωxωyωz]T表示投影在b系下的IMU旋轉(zhuǎn)角速率:

(5)

四元數(shù)的積分過(guò)程相當(dāng)于求解一階微分方程。假設(shè)在很短的積分周期內(nèi)b系相對(duì)于n系的旋轉(zhuǎn)角速率呈線性變化。則ti+1時(shí)刻的四元數(shù)在ti時(shí)刻的鄰域內(nèi)的Taylor公式如式(6),出于簡(jiǎn)潔省去坐標(biāo)系信息。

(6)

將式(4)四元數(shù)微分結(jié)果代入式(6),整理得到一階四元數(shù)積分:

(7)

2 MEMS輸出建模

由于低成本MEMS制造工藝等因素的影響,其輸出信號(hào)包含多種噪聲[13],如果不進(jìn)行誤差校正而直接使用,會(huì)使估計(jì)姿態(tài)誤差累積甚至解算失效。所以需要對(duì)MEMS慣性器件量測(cè)值進(jìn)行補(bǔ)償校正。建立如式(8)包含常見(jiàn)噪聲的馬爾科夫過(guò)程模型[14]:

(8)

相關(guān)協(xié)方差分別為:

(9)

3 間接EKF原理

(10)

等式右邊由Taylor定理可知:

(11)

顧及上述等式得到線性連續(xù)時(shí)間系統(tǒng)

(12)

式中:F(t)

3.1 狀態(tài)方程

為推導(dǎo)出擾動(dòng)狀態(tài)方程及非線性離散系統(tǒng),設(shè)計(jì)包含四元數(shù)以及陀螺儀偏差的七參數(shù)狀態(tài)向量:

(13)

由式(4)四元數(shù)導(dǎo)數(shù)方程和式(8)陀螺儀輸出數(shù)學(xué)模型可知:

(14)

(15)

由前面推導(dǎo)知擾動(dòng)四元數(shù)微分:

(16)

(17)

結(jié)合式(14)和Δbg的定義:

(18)

進(jìn)一步得到連續(xù)系統(tǒng)擾動(dòng)狀態(tài)方程:

(19)

選定離散化時(shí)間間隔T,得到狀態(tài)轉(zhuǎn)移矩陣離散化過(guò)程:

(20)

噪聲協(xié)方差離散化過(guò)程:

(21)

式中:

(22)

Ql表示連續(xù)系統(tǒng)噪聲協(xié)方差矩陣,根據(jù)建立的MEMS輸出模型得:

(23)

3.2 量測(cè)方程

連續(xù)系統(tǒng)量測(cè)方程設(shè)計(jì)為:

(24)

MEMS慣性傳感器靜止或勻速直線運(yùn)動(dòng)時(shí),加速度測(cè)量值對(duì)姿態(tài)估計(jì)起平衡穩(wěn)定的作用,取n系下重力投影相對(duì)值作為觀測(cè)向量

(25)

為得到量測(cè)方程相對(duì)擾動(dòng)狀態(tài)的Jacobi矩陣,采用鏈?zhǔn)椒▌t求偏導(dǎo)數(shù)如下

H

(26)

式中:Hx取決于加速度計(jì)的量測(cè)值,由式(25)知

Hx

(27)

(28)

(29)

3.3 間接EKF方程

根據(jù)EKF理論[15],采用迭代更新求取擾動(dòng)狀態(tài)向量估計(jì)值,進(jìn)一步求得系統(tǒng)狀態(tài)向量的估計(jì)值:

①狀態(tài)一步預(yù)測(cè)

③狀態(tài)一步預(yù)測(cè)均方誤差

④濾波增益

⑤狀態(tài)估計(jì)

代入式(15)得出最優(yōu)估計(jì)姿態(tài)。

⑥狀態(tài)估計(jì)均方誤差

Pk=(I-KkHk)Pk/k-1。

由此可知間接EKF方塊圖如圖2所示。

圖2 間接EKF方塊圖

4 實(shí)驗(yàn)結(jié)果及分析

將 MTi-G-710慣性測(cè)量單元采樣頻率設(shè)置為100 Hz。實(shí)驗(yàn)開(kāi)始時(shí)將MTi-G-710靜止一段時(shí)間后開(kāi)始緩慢無(wú)規(guī)則運(yùn)動(dòng),然后再靜止一段時(shí)間后繼續(xù)緩慢無(wú)規(guī)則運(yùn)動(dòng)。將傳感器采集的原始數(shù)據(jù)通過(guò)串口傳至PC機(jī),利用MATLAB軟件編寫(xiě)誤差狀態(tài)EKF程序,獲取準(zhǔn)確的姿態(tài)估計(jì)。

三種不同方法解算的載體姿態(tài)對(duì)比結(jié)果如圖3所示。

圖3 姿態(tài)解算對(duì)比結(jié)果

通過(guò)對(duì)圖3分析可看出,MTi-G-710緩慢無(wú)規(guī)則動(dòng)態(tài)運(yùn)動(dòng)400 s過(guò)程中,間接EKF算法有較好的姿態(tài)估計(jì)誤差優(yōu)于傳統(tǒng)EKF方案,根據(jù)圖3(b)求取間接EKF與EKF方法獲取的三個(gè)姿態(tài)角平均誤差、均方誤差及最大誤差結(jié)果如表1和表2所示。

表2 EKF姿態(tài)解算誤差統(tǒng)計(jì) 單位:°

對(duì)比表1和表2可看出,采用間接EKF方法獲取的載體姿態(tài)最大平均誤差、均方誤差和最大誤差分別小于0.25°、0.5°和8.1°,該結(jié)果優(yōu)于傳統(tǒng)EKF方法解算的載體姿態(tài)誤差。實(shí)驗(yàn)結(jié)果驗(yàn)證了論文提出的基于間接EKF姿態(tài)解算算法具有更高的姿態(tài)解決可信度,且對(duì)于運(yùn)動(dòng)環(huán)境變化具有較好的適用性。

5 結(jié)論

提出間接EKF姿態(tài)更新方法。將姿態(tài)誤差和陀螺儀偏差設(shè)計(jì)為六參數(shù)擾動(dòng)狀態(tài)向量,通過(guò)應(yīng)用Taylor定理確定六參數(shù)擾動(dòng)狀態(tài)方程為線性方程,應(yīng)用標(biāo)準(zhǔn)線性Kalman濾波理論使得擾動(dòng)狀態(tài)及其協(xié)方差最優(yōu)更新。與原有的七參數(shù)狀態(tài)向量EKF姿態(tài)估計(jì)算法不同,間接EKF無(wú)需為獲得可接受估計(jì)性能而對(duì)噪聲協(xié)方差進(jìn)行重復(fù)調(diào)整,且不釋放四元數(shù)歸一化約束。對(duì)比性實(shí)驗(yàn)結(jié)果表明,在水平方向上姿態(tài)估計(jì)收斂快且航向誤差小,姿態(tài)估計(jì)更加可靠,與成熟商用軟件解算結(jié)果精度相當(dāng)。間接EKF姿態(tài)更新方法可滿足運(yùn)動(dòng)狀態(tài)變化環(huán)境下載體姿態(tài)信息的連續(xù)準(zhǔn)確獲取。