雙天線GPS/SINS組合導(dǎo)航系統(tǒng)設(shè)計

現(xiàn)代導(dǎo)航對系統(tǒng)有能夠?qū)崟r、準(zhǔn)確地獲取載體運動信息的要求。在組合導(dǎo)航領(lǐng)域，衛(wèi)星導(dǎo)航(目前最常用的是GPS)與捷聯(lián)式慣性導(dǎo)航(SINS)組合的導(dǎo)航系統(tǒng)能夠彌補各自單獨導(dǎo)航情況下的不足，而且具有高精度、低成本、結(jié)構(gòu)簡單等優(yōu)點，目前已經(jīng)成為最熱門的研究方向。GPS/SINS組合導(dǎo)航一般是以位置，速度作為量測量來修正慣性導(dǎo)航系統(tǒng)，隨著GPS定向系統(tǒng)的逐漸發(fā)展，將姿態(tài)與位置和速度同樣作為量測量的全組合方法正在成為研究熱點。本文根據(jù)GPS/SINS全組合導(dǎo)航系統(tǒng)的機理和特點在基于DSP+FPGA的導(dǎo)航計算機平臺上設(shè)計了可靠的信息融合技術(shù)，通過相關(guān)的試驗證明本系統(tǒng)具有較好的效果。

組合導(dǎo)航系統(tǒng)軟硬件設(shè)計

導(dǎo)航計算機總體設(shè)計

捷聯(lián)式慣性導(dǎo)航系統(tǒng)中將陀螺儀、加速度計、電子線路和金屬臺體合稱為慣性測量裝置(IMU)，它是整個系統(tǒng)的核心部件。本文中導(dǎo)航系統(tǒng)采用方便野外跑車實驗的一體化、模塊化設(shè)計，通過接口將導(dǎo)航計算機固連于IMU器件內(nèi)，計算機與IMU方便更換。本系統(tǒng)中設(shè)置IMU數(shù)據(jù)更新率為200Hz，GPS數(shù)據(jù)輸出頻率為1Hz，因此計算機必須能夠在5ms內(nèi)完成整個系統(tǒng)算法運行，這樣的速率要求使得單純采用一個核心處理器的方法已無法滿足系統(tǒng)實時性要求，故本系統(tǒng)中計算機采用了DSP+FPGA雙核設(shè)計方式，DSP選用TI公司的TMS320C6747，TMS320C6747具有高性能的浮點運算能力，最高主頻可達到450MHz，能夠?qū)崿F(xiàn)高速運算;而FPGA型號為Xilinx公司的XC3S500E。系統(tǒng)硬件結(jié)構(gòu)如圖1所示，其中，DSP6747主要負(fù)責(zé)IMU解算、卡爾曼濾波等數(shù)據(jù)處理算法，F(xiàn)PGA主要完成外部接口擴展(2路RS232口，1路RS422口)、數(shù)據(jù)的采集與傳輸及邏輯時序控制的功能。

圖1 導(dǎo)航計算機結(jié)構(gòu)圖

導(dǎo)航系統(tǒng)軟件設(shè)計

導(dǎo)航計算機軟件設(shè)計必須滿足實時性與準(zhǔn)確性的要求。計算機上電后會執(zhí)行整個系統(tǒng)的初始化，通過后開始捷聯(lián)慣導(dǎo)算法運算，組合導(dǎo)航算法在GPS數(shù)據(jù)獲取后將其作為初始數(shù)據(jù)進行運算，如果有GPS數(shù)據(jù)丟失狀況發(fā)生，系統(tǒng)將單獨運行捷聯(lián)慣導(dǎo)解算，經(jīng)解算后的數(shù)據(jù)被送入Kalman濾波器中處理。系統(tǒng)軟件流程圖如圖2所示。

圖2 系統(tǒng)軟件流程圖

組合導(dǎo)航系統(tǒng)算法

(1)組合導(dǎo)航的狀態(tài)方程

目前，GPS/SINS組合導(dǎo)航方式主要分為松散組合和緊密組合。而松散組合大多采用位置、速度組合模式，而本文中加入雙天線GPS姿態(tài)角作為量測量進而實現(xiàn)全組合方式。其狀態(tài)方程如公式(1)所示：

其中F(t)是18*18階的狀態(tài)轉(zhuǎn)移矩陣;W(t)為18維的狀態(tài)噪聲向量;下列各式給出了狀態(tài)方程的系統(tǒng)噪聲性質(zhì)：

系統(tǒng)狀態(tài)X(t)選取為：

其中δL、δλ、δH為經(jīng)度、緯度、高度誤差，δVe、δVn、δVu分別為

東北天三個方向上的速度誤差，Φe、Φn、Φu為平臺誤差角，εbx、εby、εbz，εrx、εry、εrz，Δax、Δay、Δaz分別為陀螺常值漂移，相關(guān)漂移與加速度計的零偏。

而其中失準(zhǔn)角誤差方程為：

速度誤差方程為：

位置誤差方程為：

(2)組合導(dǎo)航系統(tǒng)的量測方程

由于系統(tǒng)將定向GPS給出的數(shù)據(jù)作為修正數(shù)據(jù)，系統(tǒng)量測值在位置和速度值的基礎(chǔ)上加入了姿態(tài)信息。位置量測值為慣導(dǎo)系統(tǒng)與GPS給出的緯度、經(jīng)度和高度差，相應(yīng)的速度量測值為慣導(dǎo)系統(tǒng)與GPS給出的在慣性系統(tǒng)中各坐標(biāo)下的差值，而慣導(dǎo)系統(tǒng)與雙天線GPS給出的姿態(tài)差值作為第三組量測值。

慣導(dǎo)系統(tǒng)與GPS位置量測信息在地理坐標(biāo)系以真值和誤差表示如下：

式中L、λ、h表示運載體真實位置，Ne、Nn、NH為GPS東北天方向位置誤差。

位置量測方程如下：

與位置量測方程形式相似，速度量測方程表示如下：

下式為姿態(tài)量測方程：

將(2)(3)(4)式組合后可以得到整個系統(tǒng)的量測方程如下

本文采用卡爾曼濾波算法對GPS，IMU數(shù)據(jù)進行融合。將式(1)(8)離散化后方程為：

一步預(yù)測狀態(tài)方程為：

狀態(tài)估計方程為：

濾波增益為：

計算得到一步預(yù)測均方差：

則均方差誤差估計：

試驗結(jié)果

為了驗證本文提出的雙天線GPS/SINS組合導(dǎo)航系統(tǒng)的可行性，在南京某地進行了跑車實驗。其中姿態(tài)GPS模塊定位精度小于5m，速度精度0.1m/s，航向精度0.1°。IMU常值漂移小于0.1°/h。跑車過程中以高精度GPS作為真實值，將組合導(dǎo)航系統(tǒng)輸出值與高精度GPS數(shù)據(jù)進行比較。整個系統(tǒng)中每5ms進行一次慣導(dǎo)數(shù)據(jù)解算，通過卡爾曼濾波算法進行數(shù)據(jù)修正的頻率為1次/s。將跑車過程中導(dǎo)航計算機輸出數(shù)據(jù)存入文件，通過MATLAB進行數(shù)據(jù)處理。圖3中給出一次跑車中組合導(dǎo)航系統(tǒng)與高精度GPS同一時刻輸出數(shù)據(jù)的差值，結(jié)果如圖3所示。

圖3 組合導(dǎo)航與GPS誤差曲線

從圖3可以看出，在動態(tài)情況下本文提出的組合導(dǎo)航系統(tǒng)具有較高的定位精度，從與高精度GPS數(shù)據(jù)對比中可以看，此系統(tǒng)定位誤差在4m以內(nèi)，而速度誤差在0.1m/s左右，達到了系統(tǒng)的要求，充分滿足了實際應(yīng)用需求。