支 煒，褚金奎，王寅龍

(大連理工大學(xué) 精密與特種加工教育部重點(diǎn)實(shí)驗(yàn)室，遼寧 大連116024)

0 引 言

在飛行器的飛行控制過程中，實(shí)時(shí)準(zhǔn)確地獲得飛行器在空中的航向和位置是保證飛行器安全的重要前提。盡管角速度陀螺可以進(jìn)行單獨(dú)航向角度測量，但是由于飛行器體積與承載能力的限制，飛行器中大多采用MEMS 角速度陀螺，但此種陀螺精度較低，系統(tǒng)誤差較大，并且由于被測角度與陀螺輸出存在積分關(guān)系，角度會(huì)隨時(shí)間積累而不斷增大［1］，不適用于長時(shí)間載體航向角的確定。因此，如何采用低精度陀螺傳感器進(jìn)行長時(shí)間高精度導(dǎo)航成為目前導(dǎo)航領(lǐng)域的熱點(diǎn)。

目前用來修正角速度陀螺的傳感器有很多種，文獻(xiàn)［2］采用磁強(qiáng)計(jì)與角速度陀螺組合測量角度，但由于強(qiáng)磁計(jì)受周圍環(huán)境中磁場的影響較大，穩(wěn)定性不強(qiáng)，并且在地球兩極地區(qū)地球磁場幾乎垂直于地面，因此，在很多情況采用此種方法測算水平航向角并不可靠。文獻(xiàn)［3］采用差分GPS 修正角速度陀螺的測量值，但由于差分GPS 需要配置多天線，要求飛行載體具有較大的安裝空間，同時(shí)GPS 信號(hào)本身受制于人，屬于被動(dòng)式導(dǎo)航，因此，在某些需要保密的場合并不適用。

本文采用自主式導(dǎo)航的偏振光傳感器修正角速度陀螺隨機(jī)誤差，并建立航向角度測量特征模型，采用直接式卡爾曼濾波方法對角速度陀螺和偏振光傳感器的輸出信號(hào)進(jìn)行融合，算法簡單實(shí)用，能夠獲得實(shí)時(shí)高精度無漂移的航向角。

1 偏振光測量原理［4］

偏振光導(dǎo)航傳感器是根據(jù)天空中的偏振光分布模式為依據(jù)來進(jìn)行導(dǎo)航的傳感器，偏振光傳感器模仿沙蟻頭背部的偏振敏感復(fù)眼感知天空偏振光模式，確定體軸與太陽子午線的夾角，可以直接輸出水平面內(nèi)全方位的角度信號(hào)，并且精度不隨溫度、時(shí)間變化而產(chǎn)生累計(jì)誤差。偏振光傳感器輸出信號(hào)是關(guān)于偏振方位角、偏振度的函數(shù)，其關(guān)系滿足以下方程

式中 d 為天空偏振光的偏振程度，α 為偏振方位角，是傳感器的參考方向與太陽子午線之間的夾角，p1(α)，p2(α)，p3(α)為3 個(gè)偏振光檢測單元輸出的帶有天空偏振光信息的電壓信號(hào)。經(jīng)過公式變換整理后得到如下公式

進(jìn)一步可以得到航向角計(jì)算公式

本文采用的偏振光傳感器經(jīng)室外精密轉(zhuǎn)臺(tái)的測試表明，晴朗天空時(shí)該傳感器單周期內(nèi)定向誤差［5］為±0.2°。

2 卡爾曼濾波模型

采用卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合，首先建立系統(tǒng)的狀態(tài)方程和測量方程。

MEMS 陀螺輸出載體的角速度，在采樣時(shí)間為T 的情況下，航向角測量值與陀螺輸出的關(guān)系［6］可用公式(8)所示

其中，˙φ 為陀螺輸出，φk+1與φk分別為k+1 時(shí)刻與k 時(shí)刻的航向角。

由于偏振光傳感器的角度輸出與陀螺的角速度輸出存在導(dǎo)數(shù)關(guān)系，狀態(tài)方程直接選取真實(shí)航向角作為狀態(tài)向量，并將陀螺常值偏差擴(kuò)展為狀態(tài)變量，以便在濾波過程中直接估計(jì)陀螺的常值漂移。偏振光傳感器輸出直接作為量測值，與系統(tǒng)變量呈線性關(guān)系。建立狀態(tài)方程和觀測方程如下

簡寫成

如果要估算k 時(shí)刻的實(shí)際角度，還需已知系統(tǒng)過程噪聲協(xié)方差陣Q 和測量誤差的協(xié)方差矩陣R，這2 個(gè)矩陣分別由系統(tǒng)狀態(tài)變量和測量變量的測量噪聲來決定。Q 與R矩陣的形式如下

式中 Qgyro和Qε分別是陀螺測量和陀螺測量常值誤差的協(xié)方差，Rpol是偏振光傳感器測量的方差，其數(shù)值代表卡爾曼濾波器對其傳感器數(shù)據(jù)的信任程度，值越小，表明信任程度越高。在該系統(tǒng)中偏振光傳感器的值更為接近準(zhǔn)確值，因此，取Rpol的值小于Q 的值。

數(shù)據(jù)融合過程如圖1 所示，陀螺輸出角速度經(jīng)過積分后進(jìn)入卡爾曼濾波器進(jìn)行狀態(tài)更新，當(dāng)發(fā)現(xiàn)新的量測值時(shí)，再進(jìn)行量測更新。狀態(tài)更新頻率與量測更新頻率可以不相等，在實(shí)際運(yùn)用中設(shè)系統(tǒng)狀態(tài)采樣時(shí)間為T1，量測值采樣時(shí)間為T2，一般T2會(huì)大于T1，當(dāng)沒有新的量測值、或者不需要高頻率的量測矯正時(shí)，系統(tǒng)只進(jìn)行時(shí)間更新，當(dāng)發(fā)現(xiàn)新的量測值時(shí)，再進(jìn)行量測更新。這樣設(shè)計(jì)的最大優(yōu)點(diǎn)是能夠充分利用系統(tǒng)資源，提高運(yùn)算速度?？柭鼮V波器輸出的陀螺常值漂移閉環(huán)反饋給角速度陀螺，以便得到更高精度的輸入。

圖1 數(shù)據(jù)融合算法Fig 1 Algorithm of data fusion

3 實(shí)驗(yàn)與分析

為了驗(yàn)證卡爾曼濾波信號(hào)融合方法的有效性，在室內(nèi)進(jìn)行了測試實(shí)驗(yàn)，卡爾曼濾波算法參數(shù)設(shè)置如表1 所示。

表1 濾波參數(shù)Tab 1 Parameters of filtering

實(shí)驗(yàn)過程中，采用北京潤誠時(shí)代科技公司的FMRS 型旋轉(zhuǎn)定位臺(tái)輸出作為測量標(biāo)準(zhǔn)來驗(yàn)證本文提出的算法。此轉(zhuǎn)臺(tái)帶有反饋數(shù)顯裝置，可以直接讀取測量值，定位精度為3'，重復(fù)定位精度為2″，最大轉(zhuǎn)速為10°/s。實(shí)驗(yàn)裝置如圖2所示，MEMS 陀螺型號(hào)為L3G4200D。首先，將實(shí)驗(yàn)采用與偏振光傳感器的中心重合，固定于高精度轉(zhuǎn)臺(tái)之上。之后，轉(zhuǎn)臺(tái)帶動(dòng)陀螺/偏振光導(dǎo)航系統(tǒng)以10°/s 的轉(zhuǎn)速，在水平360°范圍內(nèi)進(jìn)行勻速旋轉(zhuǎn)，每次采樣周期T 大約為52 ms，實(shí)際采樣時(shí)間由采樣處理器計(jì)時(shí)器精確計(jì)時(shí)。歷時(shí)大約300 s，采集約6 000 組數(shù)據(jù)。

圖2 實(shí)驗(yàn)裝置Fig 2 Experimental device

圖3 表示實(shí)驗(yàn)結(jié)果，從圖中可以看出:角速度陀螺單獨(dú)輸出的數(shù)據(jù)積分值隨著時(shí)間的推移誤差逐漸增大，280 s 后測量誤差達(dá)到36.27°，但經(jīng)過偏振光傳感器作為量測值的濾波校正后，估計(jì)角度(▲標(biāo)記)的偏差降為6.05°。

圖3 最優(yōu)估計(jì)航向角Fig 3 Heading angle of the optimal estimation

圖4 表示在濾波過程中估計(jì)出的角速度陀螺的常值漂移，將此常值漂移反饋到角速度陀螺中，即可在線及時(shí)修正陀螺輸出。

圖4 角速度陀螺的常值漂移Fig 4 Constant value drift of angle velocity gyro

4 結(jié)束語

本文研究了采用偏振光導(dǎo)航傳感器對角速度陀螺隨機(jī)漂移的矯正方法，在動(dòng)態(tài)數(shù)據(jù)采集實(shí)驗(yàn)的基礎(chǔ)上，通過直接卡爾曼濾波算法，有效地補(bǔ)償傳感器漂移與測量噪聲等因素對陀螺的影響。實(shí)驗(yàn)結(jié)果表明:實(shí)驗(yàn)進(jìn)行280 s 后，測量誤差由原來的36.27°下降為6.05°。該方法算法簡單實(shí)用，對微處理器的資源占用率低，為進(jìn)一步設(shè)計(jì)微小型機(jī)器人和飛行器組合導(dǎo)航控制系統(tǒng)提供有力依據(jù)。

［1］ 吉訓(xùn)生，王壽榮.MEMS 陀螺儀隨機(jī)漂移誤差研究［J］.宇航學(xué)報(bào)，2006，27(4):640－642.

［2］ 黃 旭，王常虹.磁強(qiáng)計(jì)和微機(jī)械陀螺/加速度計(jì)組合定姿的擴(kuò)展卡爾曼濾波器設(shè)計(jì)［J］.黑龍江大學(xué)自然科學(xué)學(xué)報(bào)，2005(4):454－458.

［3］ 曹 超，馬 磊，趙 舵，等.基于ARM 的移動(dòng)機(jī)器人組合導(dǎo)航系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)［J］.傳感器與微系統(tǒng)，2013，32(2):102－104.

［4］ 張 強(qiáng)，趙開春，褚金奎，等.一種嵌入式仿生測角傳感器的設(shè)計(jì)與實(shí)現(xiàn)［J］.傳感器與微系統(tǒng)，2008，27(9):106－111.

［5］ 褚金奎，王洪青，戎成功，等.基于偏振光傳感器的導(dǎo)航系統(tǒng)實(shí)驗(yàn)測試［J］.宇航學(xué)報(bào)，2011，32(3):489－494.

［6］ 馮智勇，曾 瀚.基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測量［J］.西南師范大學(xué)學(xué)報(bào):自然科學(xué)版，2011，36(4):137－141.