基于DSP+FPGA的SINS/GPS組合導(dǎo)航系統(tǒng)設(shè)計(jì)?

閆強(qiáng) 程國建

（1.西安石油大學(xué)西安710065）（2.中興通訊股份有限公司西安710065）

基于DSP+FPGA的SINS/GPS組合導(dǎo)航系統(tǒng)設(shè)計(jì)?

閆強(qiáng)1，2程國建1

（1.西安石油大學(xué)西安710065）（2.中興通訊股份有限公司西安710065）

小型化、低成本組合導(dǎo)航系統(tǒng)是現(xiàn)今慣性導(dǎo)航系統(tǒng)的重要發(fā)展趨勢，論文設(shè)計(jì)了基于DSP+FPGA技術(shù)的SINS/ GPS組合導(dǎo)航系統(tǒng)。完成了計(jì)算機(jī)硬件電路的設(shè)計(jì)，用DSP+FPGA替代了原微處理器作為核心處理器工作模式的同時(shí)，設(shè)計(jì)了捷聯(lián)解算算法，實(shí)現(xiàn)了系統(tǒng)通訊、數(shù)據(jù)采集以及導(dǎo)航數(shù)據(jù)解算的功能，并通過初始對準(zhǔn)試驗(yàn)對設(shè)計(jì)方案進(jìn)行了驗(yàn)證。

SINS/GPS；組合導(dǎo)航系統(tǒng)；DSP+FPGA；捷聯(lián)解算算法

Class NumberTN96.2

1 引言

SINS/GPS組合導(dǎo)航是軍用領(lǐng)域主要的導(dǎo)航方法［1～8］。作為外部信息源的GPS與捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）結(jié)合的組合導(dǎo)航系統(tǒng)可以解決它們單獨(dú)工作的缺陷，相互彌補(bǔ)。SINS在很大程度上彌補(bǔ)了GPS信號易受干擾的缺陷，GPS則給出了SINS所需的外界校正信息，兩者組合可實(shí)現(xiàn)系統(tǒng)在滿足小型化、低成本要求的同時(shí)完成較高精度的長航時(shí)導(dǎo)航任務(wù)。計(jì)算機(jī)電路作為組合導(dǎo)航系統(tǒng)的大腦，要同時(shí)完成外部大量數(shù)據(jù)的采集、捷聯(lián)慣性導(dǎo)航數(shù)據(jù)的解算、多數(shù)據(jù)的融合與計(jì)算結(jié)果的輸出等工作。目前，大多數(shù)SINS仍采用PC104處理器，其體積大、集成度低、速度慢、實(shí)時(shí)性差，無法大批量的采集和解算數(shù)據(jù)。數(shù)字信號處理器（DSP）具有高集成度、高運(yùn)算速率和高精度的特點(diǎn)［9～14］，現(xiàn)場可編程門陣列（FPGA）擁有編程靈活、集成度高、設(shè)計(jì)開發(fā)周期短等特點(diǎn)，因此本文采用DSP+FPGA的方案實(shí)現(xiàn)計(jì)算機(jī)電路的設(shè)計(jì)，DSP作為導(dǎo)航解算和控制的核心處理器，F(xiàn)PGA作為輔助設(shè)備，用于實(shí)現(xiàn)緩沖數(shù)據(jù)、擴(kuò)展端口和邏輯控制等功能。

2 系統(tǒng)的硬件設(shè)計(jì)

SINS/GPS組合導(dǎo)航系統(tǒng)由加速度計(jì)、陀螺儀、電源電路、計(jì)算機(jī)電路等模塊組成。計(jì)算機(jī)電路設(shè)計(jì)原理圖如圖1所示，包含最小系統(tǒng)電路與數(shù)據(jù)采集電路兩部分。其中，加速度計(jì)采用石英加速度計(jì)，陀螺儀采用三軸一體激光陀螺儀。

圖1計(jì)算機(jī)電路整體設(shè)計(jì)框圖

2.1 DSP最小系統(tǒng)

DSP最小系統(tǒng)主要完成組合導(dǎo)航解算功能［15～17］，包括TMS320C6713、外擴(kuò)FLASH存儲器、外擴(kuò)SDRAM存儲器以及電源電路、時(shí)鐘電路、復(fù)位電路、JTAG仿真接口等幾大部分。除了獨(dú)立組成一個(gè)子系統(tǒng)外，DSP最小系統(tǒng)還必須提供與FPGA的接口，完成與外界的信息交換。圖2為DSP最小系統(tǒng)的結(jié)構(gòu)示意圖。

圖2DSP最小系統(tǒng)框圖

其中，復(fù)位電路完成整個(gè)系統(tǒng)的復(fù)位操作，復(fù)位時(shí)間大于DSP TMS320C6713芯片10個(gè)時(shí)鐘周期，采用手動復(fù)位和系統(tǒng)上電復(fù)位兩種模式。時(shí)鐘電路主要由鎖相環(huán)（PLL）及鎖相環(huán)控制器、鎖相環(huán)電源濾波電路和時(shí)鐘發(fā)生電路三個(gè)部分組成。鎖相環(huán)電源濾波電路給鎖相環(huán)控制器提供穩(wěn)定的電源，鎖相環(huán)控制器將外部輸入的標(biāo)準(zhǔn)時(shí)經(jīng)過倍頻和分頻后產(chǎn)生DSP內(nèi)核時(shí)鐘、EMIF總線時(shí)鐘和兩路外部輸出時(shí)鐘。最小系統(tǒng)板中使用三種電源電壓：+5V為電壓調(diào)整芯片輸入電壓，是整個(gè)系統(tǒng)的電源，+1.2V為DSP核心電源，+3.3V為DSP I/O口、PLL、FLASH和SDRAM電源，根據(jù)芯片的實(shí)際電氣特性分析，電源電路分別選取了LD1117#12和LD1117#33兩個(gè)具有固定輸出電壓的電源芯片來提供DSP芯片所需的核電壓和I/O口電壓。

2.2 數(shù)據(jù)采集系統(tǒng)

數(shù)據(jù)采集電路的功能是定時(shí)采集3路陀螺、3路加速度計(jì)的輸入信號，并對輸入的信號進(jìn)行計(jì)數(shù)和鎖存處理。通過串口接收GPS信號并與外界交換信息。數(shù)據(jù)采集電路主要包括輸入信號整形、輸入信號光耦隔離、串口通訊、FPGA模塊等部分。除了獨(dú)立完成組合導(dǎo)航數(shù)據(jù)采集功能外，還必須提供與DSP的接口，完成將數(shù)據(jù)采集結(jié)果上傳給DSP的任務(wù)。圖3為數(shù)據(jù)采集電路的結(jié)構(gòu)示意圖。

圖3數(shù)據(jù)采集電路框圖

數(shù)據(jù)采集電路的復(fù)位電路和時(shí)鐘電路與DSP最小系統(tǒng)的相同。數(shù)據(jù)采集系統(tǒng)需要對3路加速度計(jì)的輸入信號和1路頻標(biāo)輸入信號進(jìn)行整形處理，使輸入信號滿足TTL電平標(biāo)準(zhǔn)，提高系統(tǒng)工作可靠性。本系統(tǒng)采用RC濾波加施密特觸發(fā)器實(shí)現(xiàn)對輸入信號進(jìn)行整形的目的。FPGA上電后通過專用引腳從PROM芯片中獲取程序?qū)ζ鋬?nèi)部實(shí)施編程操作，完成對FPGA的配置。電源電路有三種電源電壓，+5V是整個(gè)電路板的輸入電壓，+1.8V是FPGA芯片電源電壓，+3.3V是施密特觸發(fā)器、光耦芯片輸出端上拉電源、串口芯片電源和PROM芯片電源。

3 系統(tǒng)的軟件設(shè)計(jì)

計(jì)算機(jī)電路的功能需要通過軟件設(shè)計(jì)進(jìn)行實(shí)現(xiàn)。分別在最小系統(tǒng)電路和數(shù)據(jù)采集電路兩個(gè)硬件平臺進(jìn)行［18～20］。

3.1 最小系統(tǒng)電路軟件設(shè)計(jì)

最小系統(tǒng)電路主要完成導(dǎo)航數(shù)據(jù)的融合處理。其軟件主要實(shí)現(xiàn)最小系統(tǒng)的引導(dǎo)裝載以及將從數(shù)據(jù)采集電路得到的數(shù)據(jù)進(jìn)行導(dǎo)航解算，并保存相關(guān)數(shù)據(jù)的功能。其中，主程序部分實(shí)現(xiàn)整個(gè)導(dǎo)航算法，包括SINS的誤差補(bǔ)償、初始對準(zhǔn)、捷聯(lián)慣性導(dǎo)航算法和組合導(dǎo)航融合算法等程序模塊。主程序的編程語言采用C語言和匯編語言。SINS導(dǎo)航算法采用C語言編寫，并嵌入了硬件電路控制的匯編語言。這樣的編程方式有效利用了C語言的高效可讀性，又利用了匯編語言的準(zhǔn)確性、及時(shí)性，提高了程序的執(zhí)行速度和可移植性，便于實(shí)現(xiàn)硬件的控制。

SINS/GPS組合導(dǎo)航系統(tǒng)初始對準(zhǔn)主要任務(wù)是在對SINS進(jìn)行誤差補(bǔ)償后為捷聯(lián)解算算法確定載體坐標(biāo)系和真實(shí)導(dǎo)航坐標(biāo)系之間的捷聯(lián)矩陣初始值，為后續(xù)組合導(dǎo)航算法提供準(zhǔn)確的基準(zhǔn)。初始對準(zhǔn)流程圖如圖4所示。SINS系統(tǒng)初始對準(zhǔn)分為粗對準(zhǔn)和精對準(zhǔn)兩個(gè)過程。粗對準(zhǔn)用來縮短對準(zhǔn)時(shí)間，它要求在較短的時(shí)間內(nèi)粗略地計(jì)算出初始姿態(tài)矩陣，在此基礎(chǔ)上再進(jìn)一步進(jìn)行精對準(zhǔn)，以達(dá)到對準(zhǔn)的精度要求。

圖4初始對準(zhǔn)流程圖

3.2 數(shù)據(jù)采集電路軟件設(shè)計(jì)

數(shù)據(jù)采集電路FPGA的設(shè)計(jì)流程有設(shè)計(jì)輸入、功能仿真、綜合、實(shí)現(xiàn)、時(shí)序仿真、配置下載六個(gè)步驟，主要實(shí)現(xiàn)譯碼器、分頻器、計(jì)數(shù)器、鎖存器等功能，其中加速度計(jì)的計(jì)數(shù)鎖存通道同光纖陀螺儀的計(jì)數(shù)鎖存通道相似。當(dāng)數(shù)據(jù)采集板開始工作時(shí)，各通道計(jì)數(shù)器分別對3路加速度計(jì)、3路陀螺的輸入脈沖信號進(jìn)行計(jì)數(shù)，同時(shí)分頻器在接收到DSP輸入的計(jì)數(shù)值后對外部輸入的頻標(biāo)信號進(jìn)行減1計(jì)數(shù)，當(dāng)分頻器數(shù)值為“0”時(shí)，向各路鎖存器發(fā)出鎖存信號，將相應(yīng)的計(jì)數(shù)器數(shù)值鎖存，同時(shí)向DSP發(fā)出中斷請求信號，請求DSP讀取鎖存器中的計(jì)數(shù)值。DSP響應(yīng)中斷后由譯碼器依次選通各路鎖存器，將鎖存器中的數(shù)據(jù)讀取到DSP中進(jìn)行處理。程序采用比較通用的VHDL語言，程序的編寫只需直接對對象進(jìn)行系統(tǒng)級的邏輯行為描述，從而避開具體的器件結(jié)構(gòu)來進(jìn)行系統(tǒng)的設(shè)計(jì)，它的可移植能力和多層次設(shè)計(jì)描述能力比較強(qiáng)。

4 試驗(yàn)驗(yàn)證及結(jié)果分析

SINS/GPS組合導(dǎo)航系統(tǒng)設(shè)計(jì)完成后進(jìn)行初始對準(zhǔn)試驗(yàn)。實(shí)驗(yàn)地點(diǎn)選在試驗(yàn)室進(jìn)行。當(dāng)?shù)鼐暥龋?4.1767°，經(jīng)度：108.9511°。誤差補(bǔ)償完成后進(jìn)行粗對準(zhǔn)，時(shí)間為120s，可得到粗對準(zhǔn)結(jié)束之后的三個(gè)方向上的失準(zhǔn)角誤差為：[-0.2353°，-2.8088°，-0.7530°]，即為精對準(zhǔn)的初始失準(zhǔn)角誤差。精對準(zhǔn)采用靜基座條件下卡爾曼濾波方法來完成。在此設(shè)計(jì)了初始對準(zhǔn)誤差模型的卡爾曼濾波器，將SINS的速度作為量測量，在精對準(zhǔn)過程中采用卡爾曼濾波器來估計(jì)失準(zhǔn)角誤差。濾波器每秒濾波一次，濾波時(shí)間總共300s。試驗(yàn)結(jié)果估計(jì)圖如圖5所示。從圖中可以看出，東北天三個(gè)方向失準(zhǔn)角的估計(jì)效果較好，收斂速度較快，其時(shí)間和精度均可滿足實(shí)際使用要求。

圖5失準(zhǔn)角誤差估計(jì)圖

5 結(jié)語

本文設(shè)計(jì)完成了一種以DSP+FPGA為核心的SINS/GPS組合導(dǎo)航系統(tǒng)，用DSP+FPGA技術(shù)替代傳統(tǒng)使用的微處理器作為核心處理器的工作模式，采用工程上較為實(shí)用的卡爾曼濾波方法進(jìn)行初始對準(zhǔn)試驗(yàn)，有效地驗(yàn)證了DSP最小系統(tǒng)和FPGA數(shù)據(jù)采集系統(tǒng)功能的有效性以及設(shè)計(jì)靈活性和高拓展性，實(shí)現(xiàn)了在保持系統(tǒng)精度的同時(shí)小型化、低成本的設(shè)計(jì)目的。

［1］柳貴福，張樹俠.光纖陀螺零漂數(shù)據(jù)濾波方法的研究［J］.中國慣性技術(shù)學(xué)報(bào)，2001，9（4）：66. LIU Guifu，ZHANG Shuxia.Gyro random drift data fi?ber-optic gyroscope zero drift data filtering method re?search［J］.Journal of Chinese Inertial Technology，2001，9（4）：66.

［2］R.B.Vaganov，B.G.Klevitskiy.Sagnac Effect in a Fi?ber-ring Interferometer［J］.Radio Engineering and Elec?tronic Physics，1984，29（3）：149-153.

［3］H.J.Arditty，H.C.Lefovre.Sagnac Effect in Fiber Gyro?scopes［J］.Optics Letters，1981，6：401-403.

［4］董緒榮，張守信，華仲春.GPS/SINS組合導(dǎo)航定位及其應(yīng)用［M］.長沙：國防科技大學(xué)出版社，1998：33-54. DONG Xurong，ZHANG Shouxin，HUA Zhongchun.GPS/ SINS integrated navigation positioning and its application［M］.Changsha：National university of defense technology press，1998：33-54.

［5］于旭東，張鵬飛，謝元平.單軸旋轉(zhuǎn)慣導(dǎo)系統(tǒng)軸向陀螺漂移預(yù)測方法［J］.強(qiáng)激光與粒子束，2013，22（4）：399-402. YU Xudong，ZHANG Pengfei，XIE Yuanping.Uniaxial ro?tating inertial navigation system prediction method of axial gyro drift［J］.High Power Laser and Particle Beams，2013，22（4）：399-402.

［6］黃磊，王巍.閉環(huán)光纖陀螺的輸出誤差特性研究［J］.中國慣性技術(shù)學(xué)報(bào)，2005，13（3）：52-55. HUANG Lei，WANG Wei.Closed loop of fiber-optic gyro?scope output error characteristics research［J］.Journal of Chinese Inertial Technology，2005，13（3）：52-55.

［7］Donohodl.De-noising by soft-thresholding［J］.IEEE Transactions on Information Theory，2005，41（3）：613-626.

［8］Daubechies.Orthogonal bases of compactly sup-ported wavelets［J］.Comm Pure Appl Math，1998（41）：909-996.

［9］張濤，徐曉蘇.基于小波和人工智能的艦載捷聯(lián)系統(tǒng)動基座對準(zhǔn)［J］.吉林大學(xué)學(xué)報(bào)，2010，40（2）：549-554. ZHANG Tao，XU Xiaosu.Based on wavelet and artificial intelligence carrier moving base alignment strapdown sys?tem［J］.Journal of Jilin university，2010，40（2）：549-554.

［10］JIANG Y P.Error analysis of analytic coarse alignment methods［J］.IEEE Transactions on Aerospace and Elec?tronic Systems，1998，34（1）：334-337.

［11］Kevin J.Walchko，Michael C.Nechyba，Eric Schwartz，et al.Embedded Low Cost Inertial Navigation System［C］// Florida conference on Recent Adwances in Robotics，2003.

［12］張巖.激光陀螺捷聯(lián)慣導(dǎo)系統(tǒng)數(shù)據(jù)采集電路分析與設(shè)計(jì)［D］.北京：國防科技大學(xué)，2005：5-9. ZHANG Yan.Laser gyro strap down inertial navigation system，data acquisition circuit analysis and design［D］. Beijing：National University of Defense Technology，2005：5-9.

［13］宋東清.基于DSP的導(dǎo)航計(jì)算機(jī)系統(tǒng)設(shè)計(jì)［D］.哈爾濱：哈爾濱工程大學(xué)，2006：35-48. SONG Dong qing.The navigation computer system de?sign based on DSP［D］.Harbin：Harbin Engineering Uni?versity，2006：35-48.

［14］薛雷，張金藝，彭之威.DSP原理及應(yīng)用教程［M］.北京：清華大學(xué)出版社，2007：44-45. XUE Lei，ZHANG Jinyi，PENG Zhiwei.The principle and application of DSP tutorial［M］.Beijing：Tsinghua university press，2007：44-45.

［15］Li Xuyou，Zhang Chen，He Zhouet al..Temperature per?formance research of fiber coil in fiber optic gyroscope based on polarization coupling theory［J］.Chinese J.La?sers，2010，37（4）：1053-1057.

［16］王念旭等.DSP基礎(chǔ)與應(yīng)用系統(tǒng)設(shè)計(jì)［M］.北京：北京航空航天大學(xué)出版社，2000：144-169. WANG Nianxu.DSP based and application system design［M］.Beijing：Beijing university of aeronautics and astro?nautics press，2000：144-169.

［17］賈子申，李淑清，王冠雅.基于FPGA的UART控制器設(shè)計(jì)［J］.電子測量技術(shù)，2008：31（3）：80-91. JIA zishen，LI Shuqing，WANG Guanya.UART control?ler design based on FPGA［J］.Electronic measurement techniques.2008：31（3）：80-91.

［18］秦永元.慣性導(dǎo)航［M］.北京：科學(xué)出版社，2006：203-204. QIN Yongyuan.Inertial navigation［M］.Beijing：Science press，2006：203-204.

［19］Mallat S.A Theory for Multiresolution Signal Decomposi?tion［C］//The Wavelet Representation.IEEE Trans.On Pattern Anal.Mach Intell，1989，11（7）：674-691.

［20］LI Qing-wu，HE Chun-yuan.A new thresholding method in wavelet packet analysis for image denoising［C］//Pro?ceedings of the IEEE International Conference on Mecha?tronics and Automation，2006：2074-2078.

Design of Strapdown Inertial Navigation System Based on DSP/ FPGA

YAN Qiang1，2CHENG Guojian1
（1.Xi'an Shiyou University，Xi'an710065）（2.ZTE Corporation，Xi'an710065）

That the integrated navigation system has the miniaturization and low cost is an important development trend of cur?rent.This paper designed the SINS/GPS integrated navigation system based on technology of DSP+FPGA.It designed the hardware circuit，using DSP+FPGA to replace the original microprocessor as the core processor working mode.At the same time，the strap?down decoding algorithm was designed.The design implemented the function about the system communication，data acquisition and navigation data decoding，and were verified by the initial alignment test.

SINS/GPS，integrated navigation system，DSP+FPGA，strapdown decoding algorithm

TN96.2

10.3969/j.issn.1672-9722.2017.07.024

2017年1月3日，

2017年2月19日

程國建，男，教授，博士，研究方向：計(jì)算智能、機(jī)器學(xué)習(xí)、模式識別、數(shù)據(jù)挖掘、圖像處理、智能油藏工程、商業(yè)智能、清潔能源及CO2封存及利用等。閆強(qiáng)，男，碩士研究生，研究方向：計(jì)算機(jī)應(yīng)用技術(shù)研究。