旋轉(zhuǎn)GNSS雙天線/MIMU緊組合系統(tǒng)算法

蔡體菁，趙梓超, 張春霞

(東南大學(xué) 儀器科學(xué)與工程學(xué)院,江蘇 南京 210096)

0 引言

隨著科學(xué)技術(shù)和計算機(jī)技術(shù)的發(fā)展，對導(dǎo)航設(shè)備的成本、精度、體積大小有著越來越高的要求。緊組合算法能夠提高組合系統(tǒng)的精度[1-4]。Emelyantsev等[5-8]提出了一種小型全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)羅盤，實驗結(jié)果表明該羅盤的航向角精度為1°，俯仰角和橫滾角精度為0.3°。2016年，東南大學(xué)開始對旋轉(zhuǎn)北斗短基線雙天線定向開展研究，提出了北斗雙天線基線連續(xù)旋轉(zhuǎn)整周和0°～180°兩位置的兩種快速定向方法[9]，試驗表明，對于0.3 m的北斗短基線雙天線，航向誤差小于1°。2019年,劉瑩等[10]提出了一種旋轉(zhuǎn)短基線北斗雙天線/微慣性測量單元(MIMU)定向方法，試驗表明，當(dāng)基線長度為0.3 m時，航向角誤差小于1°，水平姿態(tài)角誤差小于0.2°。

本文針對旋轉(zhuǎn)短基線GNSS/MIMU組合系統(tǒng)提出了一種緊組合算法，該算法將GNSS的偽距、偽距率和載波相位作為旋轉(zhuǎn)GNSS雙天線/MIMU組合系統(tǒng)的觀測量，用擴(kuò)展卡爾曼濾波進(jìn)行估計，估計得到的狀態(tài)量對系統(tǒng)進(jìn)行閉環(huán)反饋，最后得到組合系統(tǒng)的定向結(jié)果。

1 系統(tǒng)硬件

2018年，東南大學(xué)研制了旋轉(zhuǎn)GNSS短基線雙天線/MIMU緊組合系統(tǒng)的樣機(jī)，如圖1所示。該組合系統(tǒng)的硬件主要由GNSS雙天線、MIMU、旋轉(zhuǎn)機(jī)構(gòu)及由數(shù)字信號處理器(DPS)和現(xiàn)場可編程門陣列(FPGA)芯片構(gòu)成的導(dǎo)航計算機(jī)組成，如圖2所示。旋轉(zhuǎn)機(jī)構(gòu)采用力矩電機(jī)，旋轉(zhuǎn)機(jī)構(gòu)轉(zhuǎn)過的角度用光電編碼器記錄，GNSS雙天線接收機(jī)板卡為OEM617D，MIMU是STIM300，導(dǎo)航計算機(jī)由DSP和FPGA芯片組成，芯片采用TMS320C6748和EP3C25U256I7。

圖1 旋轉(zhuǎn)短基線雙天線/MIMU組合導(dǎo)航系統(tǒng)實物圖

圖2 旋轉(zhuǎn)短基線雙天線/MIMU組合導(dǎo)航硬件框架

2 緊組合算法

緊組合算法將GNSS的偽距、偽距率和載波相位作為旋轉(zhuǎn)GNSS雙天線/MIMU組合系統(tǒng)的觀測量，用擴(kuò)展卡爾曼濾波進(jìn)行估計，估計得到的狀態(tài)量對系統(tǒng)進(jìn)行閉環(huán)反饋，最后得到組合系統(tǒng)的定向結(jié)果。

2.1 旋轉(zhuǎn)MIMU定向算法

(1)

旋轉(zhuǎn)機(jī)構(gòu)轉(zhuǎn)過的角度θ=ωct+θ0(其中，θ0為初始角度，ωc為旋轉(zhuǎn)角速度，t為時間)。

MIMU的姿態(tài)解算采用四元數(shù)法，其微分方程為

(2)

其中：

Q=[q1q2q3q4]T

(3)

(4)

(5)

(6)

由式(6)可得載體的姿態(tài)信息:

(7)

式中：ψ，θ，γ分別為航向角、俯仰角和橫滾角；T12,T22為姿態(tài)矩陣系數(shù)。

捷聯(lián)慣性導(dǎo)航的比力方程為

(8)

(9)

式中：RE為法線平面的曲率半徑；Lk、λk、hk分別為對應(yīng)k時刻的經(jīng)度、緯度和高度；h為橢球高。

2.2 緊組合系統(tǒng)狀態(tài)方程

旋轉(zhuǎn)GNSS短基線雙天線/MIMU緊組合系統(tǒng)擴(kuò)展卡爾曼濾波的狀態(tài)方程為

(10)

其中狀態(tài)向量為

X=[δVE,δVN,δVU,δL,δλ,δh,φE,φN,φU,δGx,

(11)

由陀螺儀和加速計的白噪聲組成的噪聲矩陣：

W=[wgx,wgy,wgz,wax,way,waz]T

(12)

狀態(tài)轉(zhuǎn)移矩陣：

Φk+1/k≈En+Fk·Δt

(13)

2.3 緊組合系統(tǒng)觀測方程

旋轉(zhuǎn)GNSS短基線雙天線/MIMU緊組合系統(tǒng)卡爾曼濾波觀測方程為

Zk+1=Hk+1Xk+1+Vk+1

(14)

式中：Zk+1為觀測向量；Hk+1為觀測矩陣；Vk+1為觀測噪聲矩陣。

旋轉(zhuǎn)GNSS短基線雙天線/MIMU緊組合系統(tǒng)的觀測量為偽距、偽距率和衛(wèi)星載波相位雙差，其表達(dá)式為

(15)

(16)

(17)

將偽距、偽距率做如下線性化處理：

(18)

(19)

其中：

(20)

(21)

2.4 算法流程

圖3 旋轉(zhuǎn)GNSS雙天線/MIMU緊組合系統(tǒng)算法流程圖

3 試驗結(jié)果

2018年,對旋轉(zhuǎn)GNSS雙天線/MIMU組合系統(tǒng)進(jìn)行了船載試驗，該組合系統(tǒng)與GPS/激光捷聯(lián)組合導(dǎo)航系統(tǒng)通過同一個過渡板固定在船的甲板上，兩組合系統(tǒng)的導(dǎo)航坐標(biāo)系保持一個固定值，試驗軌跡如圖4所示。

圖4 多模GNSS/MIMU組合系統(tǒng)船載試驗軌跡圖

對旋轉(zhuǎn)GNSS雙天線/MIMU組合導(dǎo)航系統(tǒng)的船載試驗數(shù)據(jù)進(jìn)行了緊組合離線解算，俯仰角、橫滾角和航向角誤差曲線分別如圖5～7所示。姿態(tài)角誤差如表1所示。經(jīng)計算俯仰角、橫滾角誤差均小于0.1°，航向角誤差小于1°。

圖6 緊組合系統(tǒng)橫滾角誤差曲線

圖7 緊組合系統(tǒng)航向角誤差曲線

表1 姿態(tài)角誤差

4 結(jié)束語

旋轉(zhuǎn)GNSS短基線雙天線/MIMU緊組合系統(tǒng)定向算法是以GNSS雙天線的偽距、偽距率和載波相位作為觀測量，利用擴(kuò)展卡爾曼濾波，估計出載體航向角誤差，然后經(jīng)過誤差補(bǔ)償?shù)玫捷d體的真航向。對旋轉(zhuǎn)GNSS雙天線/MIMU組合導(dǎo)航系統(tǒng)的船載試驗數(shù)據(jù)進(jìn)行了緊組合離線解算，結(jié)果表明,當(dāng)GNSS基線為0.3 m時，組合系統(tǒng)的航向誤差小于1°，俯仰角和橫滾角誤差小于0.1°。緊組合系統(tǒng)可應(yīng)用于無人艇和無人機(jī)領(lǐng)域。