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

李建利, 魏夢笛, 王其朋, 張 武

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

0 引 言

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

1 MPOS硬件平臺

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

圖1 MPOS硬件平臺Fig.1 MPOS hardware platform

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

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

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

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

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

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

(1)

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

(2)

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

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

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

步驟 1給定初值

(3)

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

步驟 2信息更新

步驟 2.1時間更新

(4)

步驟 2.2量測更新

(5)

步驟 3信息融合

(6)

步驟 4信息重置

通過全局濾波解進行信息重置:

(7)

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

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

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

3 實驗驗證

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

圖4 實驗實際安裝圖Fig.4 Actual installation diagram of the experiment

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

圖5 實驗軌跡圖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

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

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

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

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

4 結(jié) 論

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