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

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

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

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

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

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

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

導(dǎo)航計(jì)算機(jī)軟件設(shè)計(jì)必須滿足實(shí)時(shí)性與準(zhǔn)確性的要求。計(jì)算機(jī)上電后會(huì)執(zhí)行整個(gè)系統(tǒng)的初始化，通過后開始捷聯(lián)慣導(dǎo)算法運(yùn)算，組合導(dǎo)航算法在GPS數(shù)據(jù)獲取后將其作為初始數(shù)據(jù)進(jìn)行運(yùn)算，如果有GPS數(shù)據(jù)丟失狀況發(fā)生，系統(tǒng)將單獨(dú)運(yùn)行捷聯(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)角作為量測(cè)量進(jìn)而實(shí)現(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分別為

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

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

速度誤差方程為：

位置誤差方程為：

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

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

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

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

位置量測(cè)方程如下：

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

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

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

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

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

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

濾波增益為：

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

則均方差誤差估計(jì)：

試驗(yàn)結(jié)果

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

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

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