王 磊,孟志敏,關(guān) 英,宋 博
(沈陽理工大學(xué)機(jī)械工程學(xué)院,遼寧 沈陽 110159)
微機(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)證。
針對(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
微慣性測量組件常由陀螺儀,加速度計(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ì)算得到。
微慣導(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ì)算的航向角—測量噪聲。
為驗(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
針對(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°。