微慣性組件多傳感器數(shù)據(jù)融合測姿方法研究

王 磊，孟志敏，關(guān) 英，宋 博

（沈陽理工大學(xué)機(jī)械工程學(xué)院，遼寧 沈陽 110159）

1 引言

微機(jī)電系統(tǒng)（MEMS）慣性器件及其組成的各類慣性測量系統(tǒng)一直是各國大力發(fā)展的核心和前沿技術(shù)，歐美和日本等顯示出了明顯的領(lǐng)先優(yōu)勢。MEMS 慣性測量單元（MEMS IMU）成為了MEMS 領(lǐng)域研究和發(fā)展最重要的方向之一[1]。由微慣性組件構(gòu)成的導(dǎo)航系統(tǒng)往微小型、中高等精度的組合導(dǎo)航方向發(fā)展。

以MEMS 陀螺儀、加速度計(jì)等微慣性組件所構(gòu)成微慣導(dǎo)系統(tǒng)，可廣泛應(yīng)用于機(jī)器人、無人飛行器、人體姿態(tài)檢測、飛機(jī)/導(dǎo)彈飛行控制、姿態(tài)控制等領(lǐng)域，相關(guān)理論及應(yīng)用研究具有重要意義[2-5]。

采用微慣性測量組件與其他傳感器，衛(wèi)星接收系統(tǒng)，諸如GPS、北斗、地磁傳感器，高度氣壓計(jì)等進(jìn)行組合，多傳感器數(shù)據(jù)融合[3-5]、MEMS 慣性器件標(biāo)定與補(bǔ)償[6-7]、高精度濾波、姿態(tài)測量[8-10]是研究的重點(diǎn)和熱點(diǎn)。

針對(duì)由MEMS 陀螺儀、加速度計(jì)、磁阻傳感器所構(gòu)成多傳感器數(shù)據(jù)融合系統(tǒng)，根據(jù)MEMS 微慣性器件不同噪聲的頻率特點(diǎn)，通過加速度計(jì)和陀螺儀融合測姿得到滾轉(zhuǎn)角和俯仰角，通過磁阻傳感器計(jì)算航向角。利用不同傳感器得到的姿態(tài)數(shù)據(jù)，作為觀測數(shù)據(jù)，構(gòu)建無跡卡爾曼濾波器，有效融合三種傳感器信息，對(duì)載體姿態(tài)預(yù)測估計(jì)，進(jìn)一步提高測姿精度。相關(guān)算法在實(shí)驗(yàn)室開發(fā)的微慣導(dǎo)系統(tǒng)中進(jìn)行了實(shí)驗(yàn)驗(yàn)證。

2 低成本微慣性組件開發(fā)

針對(duì)微慣導(dǎo)系統(tǒng)在諸如無人飛行器等環(huán)境下的位姿檢測，實(shí)驗(yàn)室自主研發(fā)了微慣導(dǎo)系統(tǒng)，包括數(shù)字信號(hào)處理器（DSP28335），電源模塊、Flash、SDRAM、三個(gè)單軸陀螺儀、加速度計(jì)和磁阻傳感器等。微慣導(dǎo)系統(tǒng)尺寸（45×45×30）mm，系統(tǒng)采樣率 100Hz，如圖1 所示。

微慣導(dǎo)系統(tǒng)包括6 層PCB 布線的彈載機(jī)和微慣性組件構(gòu)成。其中陀螺儀通過SPI 總線接收、加速度計(jì)和磁力計(jì)通過16 位A/D 采集數(shù)據(jù)，提出的算法通過Matlab 仿真驗(yàn)證后，轉(zhuǎn)化為C 語言，并移植到DSP 嵌入式系統(tǒng)CCS3.3 中實(shí)時(shí)運(yùn)行。

圖1 微慣性導(dǎo)航系統(tǒng)Fig.1 Micro Inertial Navigation System

3 多傳感器姿態(tài)測量方法

微慣性測量組件常由陀螺儀，加速度計(jì)輔以磁阻傳感器構(gòu)成。四元數(shù)和歐拉角方法可通過陀螺儀輸出完成姿態(tài)更新[8]，求解載體的橫滾、俯仰和偏航角度。

式中：q—四元數(shù)向量，q=［q0q1q2q3］T；運(yùn)算符“?”—四元數(shù)乘法—導(dǎo)航和載體坐標(biāo)系的相對(duì)角速率。

靜止或低動(dòng)態(tài)條件下，可由加速度計(jì)輸出計(jì)算俯仰和滾轉(zhuǎn)角。

式中：mb—載體坐標(biāo)系下磁阻傳感器輸出；mp—地磁分量在水平坐標(biāo)系的投影，滿足：

航向角ψm可由下式計(jì)算

考慮到不同MEMS 微慣性器件不同噪聲的頻率特點(diǎn)，對(duì)于陀螺儀在低頻情況下會(huì)產(chǎn)生較大漂移，會(huì)噪聲誤差積累；對(duì)于加速度計(jì)/磁阻傳感器，易受到高頻干擾。采用互補(bǔ)濾波器對(duì)陀螺儀、加速度計(jì)和磁力計(jì)測量數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，可以提高載體姿態(tài)的測量精度。動(dòng)態(tài)環(huán)境下，滾轉(zhuǎn)角和俯仰角通過加速度計(jì)和陀螺儀融合測姿得到，航向角通過磁阻傳感器計(jì)算得到。

4 多傳感器信息融合與濾波算法

微慣導(dǎo)系統(tǒng)中，單一傳感器的測姿方法均存在缺陷，由陀螺儀更新計(jì)算姿態(tài)角的誤差會(huì)累積，由加速度計(jì)/磁阻傳感器計(jì)算姿態(tài)角誤差存在高頻噪聲。為克服以上不足，利用不同傳感器得到的姿態(tài)數(shù)據(jù)，構(gòu)建卡爾曼濾波器可以有效的融合三種傳感器信息，獲取更高的測姿精度。采用多傳感器組合測姿與無跡卡爾曼濾波（UKF）相結(jié)合方法預(yù)測和估計(jì)載體姿態(tài)。

設(shè)系統(tǒng)的狀態(tài)方程和觀測方程分別為：

式中：Xk、Zk—狀態(tài)向量和量測向量；f（Xk-1）、h（Xk）—非線性狀態(tài)函數(shù)和量測函數(shù)；wk-1、vk—服從于 N（q，Q）和 N（r，R）的系統(tǒng)噪聲和觀測噪聲，它們是互不相關(guān)的高斯白噪聲。UKF的具體控制流程如下：

采用UT 變換對(duì)每個(gè)Sigma 點(diǎn)進(jìn)行非線性變換，計(jì)算k 時(shí)刻的一步預(yù)測模型：

將系統(tǒng)狀態(tài)向量和協(xié)方差矩陣進(jìn)行增廣處理，計(jì)算k 時(shí)刻的一步預(yù)測增廣樣本點(diǎn)和觀測量。

計(jì)算增益矩陣：

使用Kalman 增益對(duì)系統(tǒng)的均值和協(xié)方差矩陣進(jìn)行更新，得到后驗(yàn)的狀態(tài)估計(jì)

式中：q—四元數(shù)；

b0—陀螺儀的常值零偏，狀態(tài)方程，如式（22）所示。

式中：τ—高斯馬爾可夫時(shí)間常數(shù)；

ωr—高斯白噪聲。

為有效融合陀螺儀、加速度計(jì)、磁阻傳感器信息，觀測量可采用加速度計(jì)，陀螺儀，磁阻傳感器計(jì)算的姿態(tài)角，觀測方程為

式中：φacc&gyro，θacc&gyro—通過加速度計(jì)和陀螺儀，應(yīng)用互補(bǔ)濾波器計(jì)算的姿態(tài)角；ψmag—磁阻傳感器計(jì)算的航向角—測量噪聲。

5 人體姿態(tài)測量實(shí)驗(yàn)

為驗(yàn)證上述所提姿態(tài)測量方法有效，實(shí)驗(yàn)室開展了人體姿態(tài)測量實(shí)驗(yàn)，如圖2 所示。實(shí)驗(yàn)選用了一款商用的航姿系統(tǒng)MTi，該系統(tǒng)是荷蘭XSENS 公司生產(chǎn)的一款集成了陀螺儀、加速度計(jì)、磁阻傳感器等微型慣性測試系統(tǒng)，具有較高的姿態(tài)測量精度。

圖2 人體姿態(tài)測試系統(tǒng)Fig.2 Testing System of Body Attitude

將微慣性組件、MTi 固定在手臂上，經(jīng)由RS-232 接口與上位機(jī)連接。實(shí)驗(yàn)開始后，人體隨意搖擺手臂，做大幅度姿態(tài)變化，同時(shí)采集傳感器數(shù)據(jù)。為驗(yàn)證算法的有效性，采用MTi 提供的姿態(tài)信息作為參考姿態(tài)，用于實(shí)驗(yàn)比對(duì)。實(shí)驗(yàn)中，分別采用基于四元數(shù)解算的陀螺儀測姿方法，加速度計(jì)和磁阻傳感器組合測姿方法，提出的不同傳感器組合測試與無跡卡爾曼濾波（UKF）融合的測姿方法分別對(duì)載體姿態(tài)進(jìn)行解算，實(shí)驗(yàn)結(jié)果，如圖3～圖5 所示。滾轉(zhuǎn)角、俯仰角和航向角的變化曲線，如圖3（a）～圖5（a）所示。相應(yīng)的誤差曲線，如圖3（b）、圖4（b）、圖5（b）所示?；谒脑獢?shù)的陀螺儀測姿方法，受到了其漂移誤差的影響，隨著時(shí)間積累，誤差增加，如圖3～圖5 所示。以滾轉(zhuǎn)角測姿為例，如圖4 所示。測姿結(jié)果在（0～10）s 內(nèi)的姿態(tài)變化與MTi 的參考曲線很接近，而在（20～30）s 時(shí)間段內(nèi)解算曲線雖能跟隨參考軌跡變化趨勢，但解算誤差逐步增加。加速度計(jì)/磁阻傳感器組合算法得到的姿態(tài)變化曲線圍繞參考姿態(tài)上下波動(dòng)，解算姿態(tài)角雖然不會(huì)累積，但易受到高頻噪聲的較大影響，如圖3（b）、圖4（b）和圖5（b）誤差曲線所示。如圖3（b）所示，當(dāng)航向角發(fā)生反向變化時(shí)（在9s 和18s時(shí)），加速度計(jì)/磁阻傳感器組合算法計(jì)算的航向角噪聲明顯比航向角同向變化時(shí)大。因此，依靠單一傳感器很難取得滿意的測姿精度?；パa(bǔ)濾波器分別采用低通和高通濾波分別對(duì)不同MEMS 微慣性器件濾波處理，提高了一定的測姿精度。提出的UKF 融合測姿方法，克服了單一傳感器測姿的不足，充分利用不同傳感器的測姿結(jié)果作為觀測量，可以實(shí)現(xiàn)對(duì)量測噪聲和方差的有效跟蹤，姿態(tài)估計(jì)值逐漸趨于收斂，可以較好完成對(duì)參考姿態(tài)軌跡的跟蹤。俯仰，滾轉(zhuǎn)和航向角的動(dòng)態(tài)誤差分別為0.71°，0.88°和 1.10°。

圖3 航向角及誤差Fig.3 Yaw Angle and Yaw Angle Error

圖4 滾轉(zhuǎn)角及誤差Fig.4 Roll Angle and Roll Angle Error

圖5 俯仰角及誤差Fig.5 Pitch Angle and Pitch Angle Error

6 結(jié)論

針對(duì)低成本微慣導(dǎo)系統(tǒng)，提出了一種利用不同微慣性傳感器組合測量與UKF 融合濾波方法，該方法可有效測量載體姿態(tài)。搭建了實(shí)驗(yàn)平臺(tái)，分別應(yīng)用基于四元數(shù)的陀螺儀測姿，基于加速度計(jì)和磁力計(jì)的組合測姿方法，提出的融合濾波方法進(jìn)行了人體姿態(tài)測試實(shí)驗(yàn)，通過實(shí)驗(yàn)對(duì)比結(jié)果，UKF 融合測姿方法可以較好的實(shí)現(xiàn)對(duì)參考姿態(tài)的跟蹤，俯仰，滾轉(zhuǎn)和航向角的動(dòng)態(tài)誤差分別為 0.71°，0.88°和 1.10°。