MPOS聯(lián)邦實(shí)時(shí)組合算法

李建利, 魏夢(mèng)笛, 王其朋, 張 武

(北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院, 北京 100191)

0 引 言

微小型位置姿態(tài)測(cè)量系統(tǒng)(micro position and orientation system, MPOS)是機(jī)載高分辨率對(duì)地觀測(cè)載荷運(yùn)動(dòng)補(bǔ)償?shù)膽T性組合測(cè)量設(shè)備,由微慣性測(cè)量單元(micro inertial measurement unit,MIMU)、全球?qū)Ш叫l(wèi)星系統(tǒng)(global navi-gation satellite system,GNSS)、微磁強(qiáng)計(jì)(micro magne-tometer sensor,MMS)等傳感器組成[1-3]。MPOS通過(guò)濾波進(jìn)行有機(jī)組合,彌補(bǔ)各自的不足,得到載體的實(shí)時(shí)運(yùn)動(dòng)參數(shù)信息,具有低成本、低功耗、體積小等優(yōu)勢(shì),因此在航空測(cè)繪、自然環(huán)境監(jiān)測(cè)等對(duì)地觀測(cè)領(lǐng)域起到了至關(guān)重要的作用[4-5]。其測(cè)量精度嚴(yán)重制約成像精度的提升,由此需要選擇合適的濾波方法來(lái)提升系統(tǒng)整體性能[6-7]。常見(jiàn)濾波結(jié)構(gòu)有集中式濾波,其采用一個(gè)濾波器處理所有信息,得到最佳估計(jì)結(jié)果[8-10],但計(jì)算量大,實(shí)時(shí)性差。另外,由于所有的量測(cè)信息集中在一個(gè)濾波器處理,當(dāng)某個(gè)量測(cè)信息發(fā)生異常時(shí),將會(huì)對(duì)其他無(wú)異常的量測(cè)信息造成污染,系統(tǒng)的容錯(cuò)性能較差[11-13]。與之相對(duì)的是分散式濾波,其中最典型實(shí)用的是聯(lián)邦濾波[14-16],聯(lián)邦濾波包含多個(gè)并行子濾波器和一個(gè)主濾波器,由于各個(gè)子濾波器獨(dú)立,可以有效地避免交叉污染,同時(shí)由于并行處理,有利于進(jìn)行實(shí)時(shí)運(yùn)算[17-20]。本文設(shè)計(jì)了基于MIMU/雙天線GNSS/微磁強(qiáng)計(jì)的MPOS聯(lián)邦實(shí)時(shí)組合算法,MIMU作為主參考系統(tǒng),雙天線GNSS與磁強(qiáng)計(jì)提供量測(cè)信息,以此設(shè)計(jì)主子濾波器,并成功應(yīng)用于基于雙ARM(advanced reduced instruction set computing machines)架構(gòu)的MPOS硬件平臺(tái),最后通過(guò)動(dòng)態(tài)實(shí)驗(yàn)驗(yàn)證了MPOS基于聯(lián)邦濾波算法的組合精度。

1 MPOS硬件平臺(tái)

MPOS硬件平臺(tái)主要由MIMU模塊、GNSS模塊、磁強(qiáng)計(jì)模塊、數(shù)據(jù)處理電路4部分組成,其整體MPOS硬件集成如圖1所示。

圖1 MPOS硬件平臺(tái)Fig.1 MPOS hardware platform

慣性器件選用中國(guó)電子科技集團(tuán)公司第十三研究所研制的MIMU。其內(nèi)部集成了三軸陀螺儀和加速度計(jì),零偏穩(wěn)定性分別優(yōu)于5°/h,100 μg。GNSS選用Novatel 718D多模高精度雙天線定位模塊,水平定位精度為1.5 m,提供位置、速度、航向角量測(cè)信息。磁強(qiáng)計(jì)模塊選用采用美國(guó)Honeywell公司研制生產(chǎn)的HMC系列磁傳感器,選擇HMC1001/1002單雙軸磁傳感器組成三軸磁傳感器,測(cè)量三軸的磁感應(yīng)強(qiáng)度,從而可以計(jì)算得出磁航向角。數(shù)據(jù)處理電路采用雙ARM架構(gòu),能夠支持快速導(dǎo)航運(yùn)算的功能。其中,ARM1主要負(fù)責(zé)數(shù)據(jù)采集、時(shí)鐘同步、數(shù)據(jù)存儲(chǔ)等功能。MIMU的陀螺和加速度計(jì)數(shù)據(jù)、GNSS數(shù)據(jù)和磁強(qiáng)計(jì)數(shù)據(jù)通過(guò)數(shù)據(jù)輸入模塊的串行外設(shè)接口(serial peripheral interface, SPI)、晶體管-晶體管邏輯(transisor-transisor logic, TTL)串口和模擬數(shù)字(analog digital, AD)電路進(jìn)行定時(shí)采集,并向ARM2發(fā)送原始數(shù)據(jù),同時(shí)在安全數(shù)碼(secure digital, SD)卡進(jìn)行數(shù)據(jù)存儲(chǔ)。ARM2則通過(guò)響應(yīng)ARM1中斷負(fù)責(zé)數(shù)據(jù)處理功能,包括誤差補(bǔ)償、初始對(duì)準(zhǔn)、捷聯(lián)解算、聯(lián)邦濾波、反饋校正等模塊,并實(shí)時(shí)將解算后的位置速度姿態(tài)信息通過(guò)RS422/232通信接口輸送給上位機(jī)進(jìn)行監(jiān)控和存儲(chǔ)。MPOS工作時(shí)需要4個(gè)模塊共同協(xié)調(diào)工作,其整體工作流程如圖2所示。

圖2 MPOS工作流程圖Fig.2 MPOS work flow chart

2 MPOS聯(lián)邦濾波算法

MPOS利用聯(lián)邦濾波實(shí)現(xiàn)多傳感器的信息融合,其中MIMU為主參考系統(tǒng),MIMU與雙天線GNSS 構(gòu)成子濾波器1,MIMU與MMS 構(gòu)成子濾波器2,其量測(cè)信息輸入到相應(yīng)的子濾波器中,進(jìn)行局部濾波后,其估計(jì)結(jié)果輸入到主濾波器,實(shí)現(xiàn)全局的狀態(tài)估計(jì)。

其系統(tǒng)狀態(tài)空間模型如下:

Xk=Φk,k-1Xk-1+Γk-1Wk-1

(1)

設(shè)每個(gè)子系統(tǒng)的狀態(tài)向量和量測(cè)向量為

(2)

式中:i=1,2。其中,子系統(tǒng)狀態(tài)變量與主濾波器相同。

MIMU/MMS構(gòu)成子濾波器2,MMS提供由三軸磁場(chǎng)強(qiáng)度解算得到的磁航向角信息,則子濾波器2的系統(tǒng)觀測(cè)量為Z2,k=[δφ2],δφ2為MIMU與磁強(qiáng)計(jì)之間的航向角之差;系統(tǒng)量測(cè)噪聲為V2,k=[vδφ2],vδφ2為磁強(qiáng)計(jì)解算出的航向角噪聲,由磁強(qiáng)計(jì)的精度決定。

聯(lián)邦濾波算法包括給定初值、信息更新、信息融合、信息重置4個(gè)過(guò)程[21-24]。其具體計(jì)算流程如下。

步驟 1給定初值

(3)

式中:P和Q分別為MIMU協(xié)方差陣和過(guò)程噪聲陣;βi為信息分配系數(shù),i=1,2代表子濾波器,i=m為主濾波器。

步驟 2信息更新

步驟 2.1時(shí)間更新

(4)

步驟 2.2量測(cè)更新

(5)

步驟 3信息融合

(6)

步驟 4信息重置

通過(guò)全局濾波解進(jìn)行信息重置:

(7)

信息分配系數(shù)的選取對(duì)聯(lián)邦濾波的性能有很大影響。為了兼顧容錯(cuò)性和精度選取部分重置結(jié)構(gòu)(rescale mode,RS)[28-31],即主濾波器其信息分配系數(shù)βm=0,且滿足β1+β2=1。

MPOS聯(lián)邦實(shí)時(shí)組合算法流程圖如圖3所示。

圖3 MPOS聯(lián)邦實(shí)時(shí)組合算法流程圖Fig.3 Flow chart of MPOS federated real-time integration algorithm

3 實(shí)驗(yàn)驗(yàn)證

為了測(cè)試基于聯(lián)邦濾波實(shí)時(shí)組合算法的MPOS精度,進(jìn)行動(dòng)態(tài)實(shí)驗(yàn)。將高精度光纖位置姿態(tài)測(cè)量系統(tǒng)(position and orientation system,POS)基準(zhǔn)與MPOS固定在結(jié)構(gòu)梁上,并將結(jié)構(gòu)梁放置在大理石臺(tái)上保持穩(wěn)定,兩端分別安裝GNSS主子天線,連接上位機(jī),電源,設(shè)備安裝完畢后開(kāi)始上電工作,進(jìn)行動(dòng)態(tài)實(shí)驗(yàn)。系統(tǒng)實(shí)際安裝如圖4所示。

圖4 實(shí)驗(yàn)實(shí)際安裝圖Fig.4 Actual installation diagram of the experiment

該實(shí)驗(yàn)全程運(yùn)動(dòng)軌跡圖如圖5所示,實(shí)驗(yàn)基準(zhǔn)采用高精度光纖POS系統(tǒng)后處理數(shù)據(jù),其全程運(yùn)動(dòng)誤差如圖6～圖8所示。

圖5 實(shí)驗(yàn)軌跡圖Fig.5 Experimental trajectory diagram

圖6 位置誤差圖Fig.6 Position error diagram

圖7 速度誤差圖Fig.7 Velocity error diagram

圖8 姿態(tài)誤差圖Fig.8 Attitude error diagram

對(duì)全程的位置、速度、姿態(tài)估計(jì)誤差進(jìn)行標(biāo)準(zhǔn)差(stan-dard deviation,STD)統(tǒng)計(jì),以高精度POS后處理數(shù)據(jù)作為基準(zhǔn),計(jì)算公式為

與加拿大Applanix公司APX-15 UAV產(chǎn)品的實(shí)時(shí)單點(diǎn)精度相對(duì)比,精度統(tǒng)計(jì)結(jié)果如表1所示。

表1 位置、速度、姿態(tài)STD精度統(tǒng)計(jì)結(jié)果Table 1 STD precision statistical results of position, speed and attitude

由上述STD精度統(tǒng)計(jì)結(jié)果可以得到,在動(dòng)態(tài)平穩(wěn)運(yùn)動(dòng)環(huán)境下,基于聯(lián)邦濾波的MPOS位置、速度、航向角、水平姿態(tài)角精度分別可以達(dá)到0.6 m、0.03 m/s、0.15°、0.04°,由于精度統(tǒng)計(jì)為水平勻速直線段,其精度優(yōu)于APX-15 UAV精度,驗(yàn)證了MPOS聯(lián)邦實(shí)時(shí)組合算法的有效性。

4 結(jié) 論

MPOS是為成像載荷提供實(shí)時(shí)高精度運(yùn)動(dòng)補(bǔ)償信息的關(guān)鍵裝置,其測(cè)量精度嚴(yán)重制約成像精度的提升,主要由MIMU、GNSS、MMS等傳感器組成,選擇合適的組合濾波方法是測(cè)量精度的關(guān)鍵。聯(lián)邦濾波與集中濾波相比實(shí)時(shí)性強(qiáng),容錯(cuò)性高,由此本文提出了基于MIMU/雙天線GNSS/MMS的 MPOS聯(lián)邦實(shí)時(shí)組合算法。設(shè)計(jì)以MIMU為公共參考系統(tǒng),雙天線GNSS和MMS作為子系統(tǒng)的聯(lián)邦濾波結(jié)構(gòu),并成功應(yīng)用于雙AMR架構(gòu)的MPOS硬件平臺(tái)。通過(guò)動(dòng)態(tài)實(shí)驗(yàn)驗(yàn)證了MPOS聯(lián)邦實(shí)時(shí)組合算法的精度,其位置、速度、航向角、水平姿態(tài)角精度分別可以達(dá)到0.6 m、0.03 m/s、0.15°、0.04°,該精度與加拿大Applanix公司APX-15 UAV產(chǎn)品的實(shí)時(shí)單點(diǎn)精度相當(dāng),為機(jī)載對(duì)地觀測(cè)實(shí)際應(yīng)用提供了理論和實(shí)驗(yàn)基礎(chǔ)。