馬 偉, 李 沅, 康 健, 侯 琪, 李 皓, 王 豐

(中北大學(xué) 信息與通信工程學(xué)院,山西 太原 030051)

0 引 言

世界上單一的導(dǎo)航信息源建立較早的有衛(wèi)星導(dǎo)航、慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)、地磁導(dǎo)航等[1]。INS不依賴外界環(huán)境可實(shí)現(xiàn)完全自主導(dǎo)航,精度高，穩(wěn)定性好,但其定位誤差隨時(shí)間積累,長(zhǎng)時(shí)間工作易產(chǎn)生較大的誤差[2]；衛(wèi)星導(dǎo)航可實(shí)現(xiàn)全天候連續(xù)精密導(dǎo)航與定位,但衛(wèi)星易受人為或自然因素影響[3]；地磁導(dǎo)航可實(shí)現(xiàn)全天候、全地域?qū)Ш?但需存儲(chǔ)大量地磁數(shù)據(jù),且易受外界電磁干擾[4]；多源組合導(dǎo)航可以充分利用單一導(dǎo)航的優(yōu)勢(shì),提高導(dǎo)航的容錯(cuò)性與魯棒性,增加觀測(cè)冗余度[5]。偏振光導(dǎo)航具有抗干擾性強(qiáng)、誤差不隨時(shí)間積累、易于微小型化等優(yōu)點(diǎn),因此，國(guó)內(nèi)外學(xué)者結(jié)合偏振光進(jìn)行了一系列組合導(dǎo)航研究[6]。

本文主要采用偏振光/捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system,SINS)/北斗衛(wèi)星導(dǎo)航系統(tǒng)(BeiDou navigation satellite system,BDS)/地磁的多源組合導(dǎo)航模型,針對(duì)多源融合非線性特點(diǎn),設(shè)計(jì)了一種基于聯(lián)邦卡爾曼濾波的偏振光/SINS/BDS/地磁組合導(dǎo)航算法,算法中主濾波器負(fù)責(zé)最優(yōu)融合,子濾波器進(jìn)行姿態(tài)、位置局部估計(jì),采用偏振光、地磁輔助SINS定姿,地磁、BDS輔助SINS定位,提高組合導(dǎo)航的姿態(tài)、定位測(cè)量精度,最后進(jìn)行試驗(yàn),其結(jié)果表明,該方法能夠滿足平臺(tái)、地面、空間導(dǎo)航。

1 聯(lián)邦濾波組合導(dǎo)航系統(tǒng)模型

1.1 聯(lián)邦濾波組合導(dǎo)航設(shè)計(jì)方案

本文設(shè)計(jì)的聯(lián)邦濾波器中,以SINS為公共的信息參考系統(tǒng),使用4個(gè)信息源,因此設(shè)計(jì)3個(gè)子濾波器,1個(gè)主濾波器。每一個(gè)子濾波器都將各自的量測(cè)信息處理得到局部估計(jì)量,主濾波器將各子濾波器的局部估計(jì)量進(jìn)行最優(yōu)估計(jì),圖1為整個(gè)系統(tǒng)的結(jié)構(gòu)圖。

圖1 多源組合導(dǎo)航系統(tǒng)框圖

1.2 聯(lián)邦濾波組合導(dǎo)航狀態(tài)方程

SINS作為公共的信息參考系統(tǒng),采用東北天地理參考系作為導(dǎo)航坐標(biāo)系。系統(tǒng)的公共狀態(tài)方程為

X(t)=[δL,δλ,δh,δVE,δVN,δVU,φE,φN,φU,εrx,εry,

(1)

X=F(t)X(t)+G(t)W(t)

(2)

式中F(t)為SINS的狀態(tài)轉(zhuǎn)移矩陣,G(t)為SINS的系統(tǒng)噪聲驅(qū)動(dòng)矩陣,系統(tǒng)噪聲為W(t)。

1.3 聯(lián)邦濾波組合導(dǎo)航量測(cè)方程

本文設(shè)計(jì)的聯(lián)邦濾波器主濾波器無信息分配,主濾波器不進(jìn)行濾波任務(wù),主濾波器只針對(duì)各子濾波器的估計(jì)值進(jìn)行最優(yōu)融合得到全局的最優(yōu)估計(jì)。以下為各子濾波器的量測(cè)方程。

1)SINS/北斗組合的量測(cè)方程

SINS與BDS的位置量測(cè)方程

(3)

(4)

式中λ,L,h為經(jīng)度、緯度和高度的參考值；Me,Mn,Mu為BDS在東北天3個(gè)方向上的位置誤差。

2)SINS/偏振光組合的量測(cè)方程

由于姿態(tài)角的誤差角度為小角度,把偏振光解算出的航向角φp和系統(tǒng)解算出來的航向角φx之差作為航向角的觀測(cè)量。航向角的誤差關(guān)系式如式(5)所示

(5)

式中θx與φx分別為系統(tǒng)解算出的俯仰角與航向角。偏振光的量測(cè)方程為

(6)

3)SINS/地磁組合的量測(cè)方程

磁力計(jì)給出的航向角解算值是φ′,實(shí)際的航向角為φ,因此,航向角的計(jì)算誤差為δφ,所以有φ′=φ+δφ。由姿態(tài)矩陣與姿態(tài)角,坐標(biāo)變換理論的關(guān)系能夠得到

(7)

式中t為數(shù)學(xué)平臺(tái)坐標(biāo)系。式(8)如下

(8)

聯(lián)立式(7)與式(8)可得

(9)

tanφ′=tan(φ+δφ)=-12/22

(10)

由于δφ是一個(gè)小量,因此,可近似認(rèn)為tanδφ=δφ,所以,將式(10)用泰勒級(jí)數(shù)展開得出

(11)

tan(φ+δφ)=tanφ+(1+tan2φ)δφ

(12)

聯(lián)立式(11)與式(12)可得

(13)

故SINS/地磁組合量測(cè)方程為

(14)

2 聯(lián)邦濾波器設(shè)計(jì)

本文設(shè)計(jì)的聯(lián)邦濾波器,子濾波器進(jìn)行量測(cè)更新,主濾波器進(jìn)行噪聲協(xié)方差陣更新和誤差協(xié)方差陣時(shí)間更新。將上述狀態(tài)方程和量測(cè)方程離散化為

(15)

整個(gè)系統(tǒng)中子濾波器與主濾波器是存在信息分配守恒定律,但主濾波器不參與濾波,所以主濾波器分配系數(shù)為零,子濾波器之間的分配系數(shù)關(guān)系為

(16)

協(xié)方差陣時(shí)間更新為

(17)

狀態(tài)估計(jì)更新和濾波估計(jì)為

(18)

子濾波器的增益為

(19)

子濾波器的估計(jì)均方差為

(20)

子濾波器完成相應(yīng)的局部估計(jì)后主濾器進(jìn)行全局的最優(yōu)估計(jì),主濾波器的最優(yōu)估計(jì)為

(21)

3 實(shí)驗(yàn)結(jié)果與分析

為了對(duì)本文設(shè)計(jì)的聯(lián)邦卡爾曼濾波器的性能進(jìn)行測(cè)試與驗(yàn)證,進(jìn)行了基于車載運(yùn)行軌跡的多源融合實(shí)驗(yàn)。 實(shí)驗(yàn)地點(diǎn)為中北大學(xué) (東經(jīng)：112.451 52°,北緯：38.018 68°),實(shí)驗(yàn)時(shí)間為2021年3月5日,實(shí)驗(yàn)天氣晴朗無云,實(shí)驗(yàn)條件為空曠地域進(jìn)行車載2 300 m的往返實(shí)驗(yàn),具體實(shí)驗(yàn)結(jié)果如圖2～圖6所示。圖7為實(shí)驗(yàn)場(chǎng)地示意。

圖2 導(dǎo)航軌跡

圖3 融合軌跡

圖4 航向角解算曲線

圖5 單一導(dǎo)航誤差曲線

圖6 組合導(dǎo)航誤差曲線

圖7 實(shí)驗(yàn)場(chǎng)地示意

由圖5(a)可知在數(shù)據(jù)長(zhǎng)度在200～300時(shí)地磁東向誤差曲線發(fā)生突變,是由于剛好路過一處電磁干擾地,因此，地磁信號(hào)受到干擾導(dǎo)致誤差急劇加大。

從圖2～圖6可以看出：?jiǎn)我粚?dǎo)航情況下地磁的位置誤差較大,兩兩組合導(dǎo)航的情況下較單一信息源導(dǎo)航的定位精度高,在兩個(gè)不同傳感器的組合情況下,偏振光/地磁的組合誤差較其他兩兩組合的誤差大,從表1～表2數(shù)據(jù)可知SINS/偏振光組合比SINS/地磁、偏振光/地磁組合導(dǎo)航的定位精度高，且定位穩(wěn)定性也相對(duì)可靠,而偏振光/SINS/BDS/地磁多個(gè)信息源的組合導(dǎo)航其定位精度明顯提高,其東向誤差為3.230 5 m,北向誤差為3.741 9 m。整個(gè)實(shí)驗(yàn)表明本算法可以實(shí)現(xiàn)多種傳感器較為可靠的融合定位,可以有效地提高測(cè)量精度,得到可靠的定位信息。

表1 偏振光/SINS/BDS/地磁組合導(dǎo)航與單一導(dǎo)航最大誤差對(duì)比 m

表2 兩兩組合導(dǎo)航誤差對(duì)比

4 結(jié) 論

本文針對(duì)高穩(wěn)定目標(biāo)導(dǎo)航參數(shù)獲取對(duì)多傳感器融合算法的需求,利用偏振光的偏振信息與地磁信息,提出一種偏振光/SINS/BDS/地磁組合導(dǎo)航方法。多源組合導(dǎo)航充分利用偏振光與地磁誤差不隨時(shí)間積累,可以提供高精度的姿態(tài)信息,結(jié)合SINS,BDS可以在不同環(huán)境情況下優(yōu)勢(shì)互補(bǔ),提高組合導(dǎo)航的可靠性與姿態(tài)測(cè)量精度。實(shí)驗(yàn)表明:本文算法能夠提供精準(zhǔn)的航向角和位置估計(jì),偏振光/SINS/BDS/地磁組合導(dǎo)航能夠可靠提供定位信息,綜上所述本方法適用于平臺(tái)、地面、空間導(dǎo)航。