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

余 威 陳 帥 陸 帥 王磊杰

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

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

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

1.1 方案設(shè)計

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

1.2 硬件實現(xiàn)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

表1 時間測試

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

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

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

圖3 位置誤差對比圖

圖4 速度誤差對比圖

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

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

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

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

圖5 無高度表輔助的位置曲線圖

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

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

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

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

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

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

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

4.3 加加速度曲線擬合方法

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

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

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

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

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

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

圖8 軌跡位置曲線圖

圖9 軌跡速度曲線圖

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

圖11 接收機位置曲線圖

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

圖12 接收機速度曲線圖

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

5 結(jié)束語

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

［1］ 羅建軍，馬衛(wèi)華，袁建平，岳曉奎.組合導(dǎo)航原理與運用［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］ 袁信，俞濟祥，陳哲.導(dǎo)航系統(tǒng)［M］.北京:航空工業(yè)出版社，1993.(Yuan Xin，Yu Jixiang，Chen Zhe.Navigation System ［M］.Beijing:Aviation Industry Press，1993.)

［4］ 余忠清，張捷，龔誠.GNSS接收機在慣性速度輔助下跟蹤環(huán)路的研究［J］.無線通信技術(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］. 遙測遙控，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原理與接收機設(shè)計［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］.北京航空航天大學(xué)學(xué)報，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.