基于DSP+FPGA的超緊組合導(dǎo)航系統(tǒng)研究*

余 威 陳 帥 陸 帥 王磊杰

南京理工大學(xué)，南京210094

基于DSP+FPGA的硬件平臺(tái)搭建分布式超緊組合導(dǎo)航系統(tǒng)，利用慣性信息輔助GNSS接收機(jī)進(jìn)行信號(hào)跟蹤，提高了系統(tǒng)的魯棒性。從提高動(dòng)態(tài)條件下GNSS接收機(jī)跟蹤環(huán)路性能的角度出發(fā)，通過(guò)曲線擬合的方法對(duì)慣性輔助信息進(jìn)行數(shù)據(jù)遞推以消除加速度殘差，并對(duì)該方法進(jìn)行了仿真驗(yàn)證。對(duì)于少星情況下系統(tǒng)觀測(cè)量維數(shù)降低、組合導(dǎo)航精度下降的問(wèn)題，提出了一種通過(guò)引入外部高度信息、采用全維濾波方式進(jìn)行信息融合的算法。

1 系統(tǒng)方案設(shè)計(jì)與硬件實(shí)現(xiàn)

1.1 方案設(shè)計(jì)

分布式GNSS/SINS超緊組合導(dǎo)航系統(tǒng)根據(jù)多普勒效應(yīng)，利用衛(wèi)星的星歷信息解算出的衛(wèi)星位置和速度以及組合導(dǎo)航輸出的載體位置和速度，計(jì)算由于載體和衛(wèi)星之間的相對(duì)運(yùn)動(dòng)所造成的載波多普勒頻移，并對(duì)載波環(huán)進(jìn)行輔助，以提高跟蹤環(huán)路的動(dòng)態(tài)性能。同時(shí)為了降低收星數(shù)過(guò)少帶來(lái)的影響，引入了外部高度表信息作為濾波器輸入，重新構(gòu)建觀測(cè)方程。整體結(jié)構(gòu)如圖1所示。

1.2 硬件實(shí)現(xiàn)

系統(tǒng)基于DSP+FPGA硬件平臺(tái)搭建，按功能劃分為以下模塊:INS模塊、接收機(jī)模塊、高度表模塊、數(shù)據(jù)通信模塊、數(shù)據(jù)處理模塊和上位機(jī)監(jiān)控模塊。其中，INS模塊和接收機(jī)模塊分別指MEMS慣導(dǎo)和GNSS接收機(jī);高度表模塊采用氣壓式高度表;數(shù)據(jù)通信模塊分為UART模塊和FIFO模塊，由FPGA硬件實(shí)現(xiàn);數(shù)據(jù)處理模塊為T(mén)I公司生產(chǎn)的數(shù)字信號(hào)處理器TMS320C6713;上位機(jī)監(jiān)控模塊由PC機(jī)和監(jiān)控軟件組成。

在系統(tǒng)中INS模塊使用RS-422傳輸數(shù)據(jù)，而高度表模塊、接收機(jī)模塊和上位機(jī)模塊的信息傳輸采用RS-232，面臨信息源多，數(shù)據(jù)量大的問(wèn)題。為了減輕DSP負(fù)擔(dān)，采用FPGA對(duì)串口進(jìn)行擴(kuò)展并通過(guò)EMIF總線接口與DSP通信，使得DSP能專(zhuān)注于數(shù)據(jù)處理，提高實(shí)時(shí)性。硬件結(jié)構(gòu)如圖2所示。

圖1 系統(tǒng)總體結(jié)構(gòu)圖

圖2 系統(tǒng)硬件結(jié)構(gòu)圖

2 系統(tǒng)狀態(tài)方程與觀測(cè)方程

組合導(dǎo)航系統(tǒng)通過(guò)卡爾曼濾波器對(duì)系統(tǒng)中的誤差量進(jìn)行估計(jì)，并將估計(jì)值作為校正量反饋到系統(tǒng)中，實(shí)現(xiàn)組合校正。系統(tǒng)狀態(tài)方程與觀測(cè)方程如下［1-3］:

基于偽距和偽距率的超緊組合導(dǎo)航系統(tǒng)在收星數(shù)少于4顆的情況下仍能繼續(xù)工作，但由于觀測(cè)維數(shù)的減少削弱了狀態(tài)變量的觀測(cè)性，導(dǎo)致定位精度出現(xiàn)下降。高度表是一種在飛行器上廣泛使用的測(cè)量器件，其中氣壓式高度表利用大氣靜壓隨高度的變化規(guī)律間接測(cè)量高度，具有自主性好，低空測(cè)量準(zhǔn)確等優(yōu)點(diǎn)。由于接收機(jī)天線受到遮擋基本上出現(xiàn)在近地面附近，所以氣壓式高度表不僅可以在少星情況下增強(qiáng)系統(tǒng)的觀測(cè)性，還彌補(bǔ)了GNSS接收機(jī)在近地面時(shí)高度信息準(zhǔn)確度相對(duì)較差的缺點(diǎn)。

引入外部高度信息之后，系統(tǒng)的狀態(tài)方程不變，觀測(cè)方程增加了高度誤差信息作為觀測(cè)量。設(shè)慣導(dǎo)解算得到的高度為hins，氣壓式高度表測(cè)得的高度為halt，則高度信息的觀測(cè)量h為:

3 組合導(dǎo)航系統(tǒng)工作模式

3.1 收星數(shù)大于4顆

當(dāng)收星數(shù)大于4顆時(shí)系統(tǒng)將面臨2種選擇:通過(guò)選星算法選擇4顆衛(wèi)星作為導(dǎo)航星;使用全部可見(jiàn)星的觀測(cè)量作為濾波器輸入。

由于精度因子表征了定位誤差的放大倍數(shù)，選星算法通過(guò)計(jì)算得到一組使精度因子最佳的衛(wèi)星組合，可以在不增加濾波器系統(tǒng)維數(shù)的情況下提高導(dǎo)航精度。缺點(diǎn)是此時(shí)其他衛(wèi)星提供的有效信息會(huì)被浪費(fèi)，從而降低系統(tǒng)的能觀性。與之相反，全維濾波使用所有可見(jiàn)衛(wèi)星進(jìn)行組合濾波，增強(qiáng)了系統(tǒng)的觀測(cè)性，但觀測(cè)維數(shù)的增加導(dǎo)致濾波時(shí)間加長(zhǎng)。

對(duì)于“DSP+FPGA”平臺(tái)，數(shù)據(jù)的接收和發(fā)送均由FPGA完成，DSP與FPGA間通過(guò)EMIF連接，傳輸耗時(shí)可忽略不計(jì)。不妨設(shè)卡爾曼濾波器全維濾波時(shí)間為tk，數(shù)據(jù)的同步時(shí)間為tc，慣導(dǎo)解算時(shí)間為tins，其他數(shù)據(jù)運(yùn)算時(shí)間為tother。若慣導(dǎo)的解算周期為T(mén)，那么需滿足下列公式成立:

組合導(dǎo)航系統(tǒng)中各項(xiàng)所占時(shí)間如表1所示。

表1 時(shí)間測(cè)試

表2 不同收星數(shù)下的濾波時(shí)間

若慣導(dǎo)的解算周期設(shè)為10ms，從上表可以看到，即使是全維濾波，總時(shí)間仍小于解算周期，說(shuō)明采用全維濾波完全可行。從提高系統(tǒng)性能的角度出發(fā)，選用全維濾波方式進(jìn)行組合導(dǎo)航。跑車(chē)試驗(yàn)中2種方式的誤差對(duì)比如圖3和4所示。

由圖可見(jiàn):全維濾波位置誤差在5m以?xún)?nèi)，速度誤差在1m/s以?xún)?nèi);而采用選星算法位置誤差在10m以?xún)?nèi)，速度誤差在2m/s以?xún)?nèi)。試驗(yàn)結(jié)果表明采用全維濾波可改善導(dǎo)航精度。

圖3 位置誤差對(duì)比圖

圖4 速度誤差對(duì)比圖

3.2 收星數(shù)小于4顆

當(dāng)可見(jiàn)衛(wèi)星數(shù)為3顆時(shí)，系統(tǒng)狀態(tài)變量的能觀性遭到削弱，此時(shí)加入高度表信息可增強(qiáng)觀測(cè)性。在靜態(tài)試驗(yàn)中位置、速度信息長(zhǎng)時(shí)間內(nèi)無(wú)發(fā)散，且短時(shí)間內(nèi)仍能保持較高定位精度。

導(dǎo)航誤差隨時(shí)間積累，當(dāng)收星數(shù)為2顆時(shí)，系統(tǒng)的能觀性進(jìn)一步下降，以該種情況作為典型的少星情況予以分析。靜態(tài)實(shí)驗(yàn)結(jié)果如圖5和6所示。

由圖5和6可知，在無(wú)高度表信息輔助情況下，200s后緯度誤差累積到50m，經(jīng)度誤差達(dá)到90m，高度誤差達(dá)到40m，速度誤差超過(guò)1m/s;加入高度表信息后200s以?xún)?nèi)緯度誤差小于10m，經(jīng)度誤差小于10m，高度誤差小于2m，速度誤差在0.2m/s以?xún)?nèi)。試驗(yàn)結(jié)果表明引入外部高度信息可改善少星情況下的導(dǎo)航精度。

圖5 無(wú)高度表輔助的位置曲線圖

圖6 高度表輔助下的位置曲線圖

4 慣性信息輔助跟蹤環(huán)路

4.1 慣性輔助載波跟蹤環(huán)路數(shù)學(xué)模型

接收機(jī)載波環(huán)是一個(gè)由鑒相器、環(huán)路濾波器和數(shù)控振蕩器構(gòu)成的負(fù)反饋閉環(huán)控制系統(tǒng)。將慣性輔助環(huán)路加入該系統(tǒng)后得到的數(shù)學(xué)模型如圖7［4-8］。

圖7 慣性輔助載波環(huán)數(shù)學(xué)模型

4.2 高動(dòng)態(tài)條件下的動(dòng)態(tài)應(yīng)力誤差

從上式我們可以看出:當(dāng)載體作加加速度運(yùn)動(dòng)時(shí)，由于載波環(huán)路無(wú)法跟蹤高階次的運(yùn)動(dòng)，使得輔助間隔內(nèi)鎖相環(huán)相位誤差累積，可能導(dǎo)致環(huán)路失鎖，因此需要對(duì)輔助信息進(jìn)行插值或擬合。

4.3 加加速度曲線擬合方法

對(duì)于上式可使用最小二乘法求解［9］。此外，由于高動(dòng)態(tài)往往發(fā)生在載體飛行的主動(dòng)段，發(fā)動(dòng)機(jī)的工作由火控系統(tǒng)控制，飛行阻力與大氣稀薄程度和速度有關(guān)。通過(guò)對(duì)發(fā)動(dòng)機(jī)推力和飛行阻力變化的合理建模，也可以在一定范圍內(nèi)對(duì)加加速度進(jìn)行擬合，減小其帶來(lái)的不利影響。載體運(yùn)動(dòng)數(shù)學(xué)模型如下:

式中，Ju為多項(xiàng)式擬合得到的加加速度;αu，Vu，Pu分別是采用串口實(shí)時(shí)接收的載體三維、速度、位置信息;t為傳輸延遲時(shí)間;α'u，V'u，P'u分別為根據(jù)數(shù)學(xué)模型遞推得到的觀測(cè)時(shí)刻載體三維加速度、速度和位置信息。在每次跟蹤環(huán)路更新時(shí)，利用遞推得到的慣性輔助全信息在輔助間隔內(nèi)對(duì)跟蹤環(huán)路進(jìn)行輔助，從而消除加速度殘差。

4.4 慣性信息輔助跟蹤環(huán)路仿真分析

出于簡(jiǎn)化考慮，載體的運(yùn)動(dòng)選擇最簡(jiǎn)單的勻加加速運(yùn)動(dòng)，此時(shí)僅需要對(duì)慣性輔助信息進(jìn)行線性擬合即可。在DSP+FPGA硬件平臺(tái)的基礎(chǔ)上利用衛(wèi)星導(dǎo)航信號(hào)模擬器仿真相應(yīng)的運(yùn)動(dòng)軌跡和衛(wèi)星信號(hào)，并搭建半實(shí)物仿真系統(tǒng)進(jìn)行高動(dòng)態(tài)實(shí)驗(yàn)。接收機(jī)定位后載體的實(shí)際運(yùn)動(dòng)軌跡如圖8～10所示。

由圖8～10可知，載體在接收機(jī)定位后首先以100m/s的速度勻速飛行220s;然后以20g/s的加加速度作勻加加速度運(yùn)動(dòng)3s;再以60g的加速度作勻加速運(yùn)動(dòng)7s;最后轉(zhuǎn)為勻速飛行。

組合導(dǎo)航系統(tǒng)定位結(jié)果如圖11～13所示。

圖8 軌跡位置曲線圖

圖9 軌跡速度曲線圖

圖10 軌跡合速度與加速度曲線圖

圖11 接收機(jī)位置曲線圖

從圖8～9和圖11～12的對(duì)比可以看到:當(dāng)載體進(jìn)行高動(dòng)態(tài)飛行時(shí)，接收機(jī)仍然能保持對(duì)衛(wèi)星信號(hào)的穩(wěn)定跟蹤，實(shí)現(xiàn)正常定位;以第4號(hào)衛(wèi)星為例，從圖13可以看到，在加入線性擬合后解算多普勒頻移曲線與實(shí)際多普勒頻移曲線基本重合，兩者的誤差保持在10Hz以?xún)?nèi)。

圖12 接收機(jī)速度曲線圖

圖13 多普勒頻移及誤差曲線圖

5 結(jié)束語(yǔ)

設(shè)計(jì)了一種基于DSP+FPGA的GNSS/SINS超緊組合導(dǎo)航系統(tǒng)。針對(duì)導(dǎo)航過(guò)程中出現(xiàn)的高動(dòng)態(tài)、障礙遮擋等情況提出了相應(yīng)的解決方法。通過(guò)相關(guān)試驗(yàn)驗(yàn)證，為下一步的工程實(shí)現(xiàn)提供了重要依據(jù)。結(jié)果表明:采用慣性輔助載波跟蹤環(huán)路可改善接收機(jī)的動(dòng)態(tài)性能，引入外部高度信息可改善少星情況下的導(dǎo)航精度。

［1］ 羅建軍，馬衛(wèi)華，袁建平，岳曉奎.組合導(dǎo)航原理與運(yùn)用［M］.西安:西北工業(yè)大學(xué)出版社，2012.(Luo Jianjun，Ma Weihua，Yuan Jianping，Yue Xiaokui.Integrated Navigation System:Principle and Application［M］.Xi'an:Northwestern Polytechnical University Press Lo.Ltd，2012.)

［2］ 葉萍.MEMS IMU/GNSS超緊組合導(dǎo)航技術(shù)研究［D］.上海交通大學(xué)，2011:70-77.(Ye Ping.Research on M-EMS IMU/GNSS Ultra-tight Integration Navigation Technology ［D］.Shanghai Jiao Tong University，2011:70-77.)

［3］ 袁信，俞濟(jì)祥，陳哲.導(dǎo)航系統(tǒng)［M］.北京:航空工業(yè)出版社，1993.(Yuan Xin，Yu Jixiang，Chen Zhe.Navigation System ［M］.Beijing:Aviation Industry Press，1993.)

［4］ 余忠清，張捷，龔誠(chéng).GNSS接收機(jī)在慣性速度輔助下跟蹤環(huán)路的研究［J］.無(wú)線通信技術(shù)，2008，17(1):34-36，41.(Yu Zhongqing，Zhang Jie，Gong Cheng.Research on GNSS receiver tracking loop using inertial velocity［J］.Wireless Communication Technolog，2008，17(1):34-36，41.)

［5］ 董曉明，孫迅.GPS/INS超緊耦合方法及其應(yīng)用分析［J］. 遙測(cè)遙控，2010，31(6):6-10.(Dong Xiaoming，Sun Xun，GPS/INSUltra-Tightly coupled technique and its application analysis［J］.Journal of Telemetry;Tracking and Command，2010，31(6):6-10.)

［6］ 謝鋼.GPS原理與接收機(jī)設(shè)計(jì)［M］.北京:電子工業(yè)出版社，2009.(Xie Gang，Principles of GPS and Receiver Dsign［M］.Beijing:Publishing House of Electronics Industry，2009.)

［7］ 馬云峰.MSINS/GPS組合導(dǎo)航系統(tǒng)及其數(shù)據(jù)融合技術(shù)研究［D］.東南大學(xué)，2006:82-84.(Ma Yunfeng.Research on MSINS/GPS Integrated Navigation System and Its Data FusionTechnology［D］.Southeast University，2006:82-84.)

［8］ 于潔，王新龍.SINS輔助GPS跟蹤環(huán)路超緊耦合系統(tǒng)設(shè)計(jì)［J］.北京航空航天大學(xué)學(xué)報(bào)，2010，36(5):606-609.(Yu Jie，Wang Xinlong.Ultra-tightly coupled navigation system design based on SINS aiding GPS tracking loops［J］.Journal of Beijing University of Aeronautics and Astronautics，2010，36(5):606-609.

［9］ 蔡山，張浩，陳洪輝，沙基昌.基于最小二乘法的分段三次曲線擬合方法研究［J］.科學(xué)技術(shù)與工程，2007，7(3):352-355.(Cai Shan，Zhang Hao，Chen Hong hui，Sha Jichang.Research of piecewise cubic curvefitting method based on least-square principle［J］.Science Technology and Engineering，2007，7(3):352-355.