光學(xué)捷聯(lián)基準(zhǔn)導(dǎo)航計算機設(shè)計與實現(xiàn)

霍 庚，黃平華，張興凡

（中國人民解放軍92941 部隊，葫蘆島 125000）

1 引 言

捷聯(lián)基準(zhǔn)系統(tǒng)的主要任務(wù)是為大型艦船武器系統(tǒng)的基座提供姿態(tài)基準(zhǔn)。 基準(zhǔn)信息主要包括三軸姿態(tài)信息和角速度信息，是一種純自主的捷聯(lián)式慣性導(dǎo)航系統(tǒng)。 在衛(wèi)星導(dǎo)航定位系統(tǒng)與測流儀輔助下進(jìn)行導(dǎo)航解算，從而構(gòu)成導(dǎo)航系統(tǒng)，以提高捷聯(lián)基準(zhǔn)的精度。 作為捷聯(lián)基準(zhǔn)的數(shù)學(xué)計算平臺，導(dǎo)航計算機需要完成對慣性器件的數(shù)據(jù)采集，并根據(jù)相應(yīng)的數(shù)據(jù)及參數(shù)完成實時導(dǎo)航解算，并將導(dǎo)航姿態(tài)參數(shù)提供給外部使用。 因此，它需要實時處理大量原始數(shù)據(jù)，如慣性測量單元中陀螺儀和加速度計的原始數(shù)據(jù)預(yù)處理、衛(wèi)星導(dǎo)航定位系統(tǒng)位置信息解析與融合、導(dǎo)航解算，還要完成與外部導(dǎo)航應(yīng)用設(shè)備的數(shù)據(jù)交互等任務(wù)。

光學(xué)捷聯(lián)基準(zhǔn)的慣性器件是光纖陀螺，可靠性、實時性較高，對導(dǎo)航計算機（ENC）的功能、性能需求也越來越高。 以往基準(zhǔn)系統(tǒng)的導(dǎo)航計算機主要以PC104 處理器為體系架構(gòu)，但是PC104 架構(gòu)的計算機體積較大、功耗高、系統(tǒng)程序復(fù)雜、可移植性低、成本高，計算性能受主計算機的硬件性能限制。 目前通常采用DSP ＋MCU（微控制器）組合的方式作為ENC 的核心，DSP 作為主計算機，負(fù)責(zé)數(shù)據(jù)的處理與導(dǎo)航解算，MCU 負(fù)責(zé)數(shù)據(jù)的采集，該方式處理速度慢、可靠性低，實用性還有待提高。針對新型光學(xué)捷聯(lián)基準(zhǔn)對ENC 實時性要求高、工作功耗低、尺寸小的需求，本文設(shè)計了一種基于DSP和FPGA 為核心處理器（DSP ＋FPGA）的嵌入式導(dǎo)航計算機。 在硬件設(shè)計方面對信息處理模塊和數(shù)據(jù)通信采集模塊進(jìn)行了考慮，凸顯了DSP 高頻運算實時性及FPGA 多路并行的優(yōu)點，兩者相結(jié)合的應(yīng)用，體現(xiàn)了嵌入式組合ENC 的優(yōu)勢。 在軟件上，重點研究了基于船用主慣導(dǎo)航向與計程儀速度組合的Kalman 濾波算法設(shè)計，并將該ENC 應(yīng)用到實際樣機中，進(jìn)行了半物理仿真驗證試驗。

2 ENC 硬件設(shè)計

新型光學(xué)捷聯(lián)基準(zhǔn)導(dǎo)航計算機實時性很高，需要同時完成傳感器的原始數(shù)據(jù)同步采集、實時導(dǎo)航及姿態(tài)解算、導(dǎo)航姿態(tài)信息輸出等多個任務(wù)。 ENC系統(tǒng)包括：慣性傳感器IMU、DSP ＋FPGA 雙架構(gòu)處理器、外掛FLASH 及DDR、外圍設(shè)備接口電路。 陀螺儀及加速度計數(shù)據(jù)經(jīng)FPGA 按照1 kHz 的頻率進(jìn)行循環(huán)采樣處理后，先進(jìn)行信號的預(yù)補償，然后將數(shù)據(jù)信號進(jìn)行整合和處理，并將數(shù)據(jù)寫入到FPGA內(nèi)部的雙口隨機存取存儲器（RAM）中，DSP 則通過外部存儲器接口（EMIF）總線直接讀取相應(yīng)RAM地址中的各種數(shù)據(jù)，并在內(nèi)部進(jìn)行濾波及導(dǎo)航解算，待解算完成后，DSP 將實時計算出的航向角、水平姿態(tài)角、角速度等信息寫入雙口RAM 回傳給FPGA，F(xiàn)PGA 將導(dǎo)航數(shù)據(jù)通過外圍接口進(jìn)行對外發(fā)送。

按照系統(tǒng)功能分類，可將ENC 硬件系統(tǒng)主要劃分為兩個核心模塊：導(dǎo)航信息處理模塊（MPM）和數(shù)據(jù)采集通信模塊（DCM），如圖1 所示。

圖1 導(dǎo)航計算機硬件結(jié)構(gòu)框圖Fig．1 Hardware structure diagram of navigation computer

2．1 MPM 的設(shè)計

MPM 主要由DSP、DDR2、NAND Flash、晶振電路、總線驅(qū)動電路等組成。 采用基于嵌入式DSP處理器設(shè)計的高性能DSP 模塊作為導(dǎo)航系統(tǒng)計算單元的核心，主要用來快速完成對實時數(shù)據(jù)的處理及導(dǎo)航解算，需要具備高性能浮點運算能力；擴展內(nèi)存采用高性能的DDR2 以輔助DSP 在進(jìn)行大量數(shù)據(jù)運算時需要的計算空間；NAND Flash 為外擴存儲器，主要存儲軟件編譯文件及導(dǎo)航系統(tǒng)的各類補償參數(shù)；晶振時鐘電路為DSP 和FPGA 提供穩(wěn)定可靠的時鐘基準(zhǔn)信息；所采用的EMIF 總線驅(qū)動電路主要用于外部FLASH 及DDR 的讀寫，同時實現(xiàn)對FPGA 內(nèi)部各雙口RAM 模塊的讀寫，以實現(xiàn)DSP 和FPGA 之間的高速數(shù)據(jù)傳輸。

本模塊的設(shè)計在選用核心處理器時，考慮了導(dǎo)航信息計算機要進(jìn)行各種航姿信息解算時的運行速度及運算量。 因此，將選用高性能支持浮點運算的DSP 芯片作為核心處理器。 同時需要兼顧處理器本身正常使用時的功耗、尺寸、價格等多種因素。目前常用的DSP 主要有TI 公司的TMS320C6713、TMS320C6748、TMS320C6678 等型號。 其中，C6713為單核處理器，主頻為300 MHz，其性能有限，不符合高性能解算的要求，且封裝尺寸較大，不符合小型化的要求；C6678 為多核處理器，單核主頻能夠達(dá)到1 GHz，具備高性能的處理能力，但其功耗較大，價格昂貴，不適用于慣性導(dǎo)航系統(tǒng)使用；C6748 具有專用浮點數(shù)計算的硬件乘法器，支持高速的浮點數(shù)運算，主頻最高為每秒456 MHz，具有高達(dá)3648 MIPS 的性能和最高2746 位MFLOPS 的高速運算的能力，能滿足設(shè)計對運算實時性的要求，目前廣泛應(yīng)用于慣性導(dǎo)航領(lǐng)域。 C6748 在成本與功耗方面的優(yōu)勢也是本設(shè)計選擇的理由之一，它是目前最低成本與最低功耗的C6000 系列處理器以及最低功耗的浮點DSP。 因此，本設(shè)計采用了TMS320C6748芯片來作為光纖捷聯(lián)基準(zhǔn)導(dǎo)航計算機的主處理器。

導(dǎo)航信息處理模塊導(dǎo)航運算量較大，但DSP 處理器自帶的RAM 空間局限，不能滿足運算需求，需要擴展外部同步動態(tài)隨機存取內(nèi)存（SDRAM），增大存儲空間。 主處理器TMS320C6748 芯片有支持JESD79－2A 標(biāo)準(zhǔn)的DDR2 接口，因此，本方案將選擇外擴DDR2 來增大運算空間，其芯片型號為MT47H128M16RT，該DDR2 支持?jǐn)U展存儲容量達(dá)256 MB，可滿足導(dǎo)航信息處理器對數(shù)據(jù)存儲的緩存處理需求。

TMS320C6748 芯片由于自帶的只讀存儲器（ROM）空間比較有限，難以同時滿足導(dǎo)航計算機中對程序數(shù)據(jù)存儲功能和復(fù)雜系統(tǒng)參數(shù)信息存儲能力的擴展需求，需要適當(dāng)擴展一些外部空間或者其他外部存儲器，實現(xiàn)復(fù)雜程序的存儲擴展和復(fù)雜系統(tǒng)參數(shù)存儲。 當(dāng)導(dǎo)航計算機上電時，從外部FLASH將用戶程序?qū)氲絻?nèi)部RAM 運行，同時將誤差參數(shù)從外部FLASH 讀取至軟件內(nèi)，配合導(dǎo)航解算，保證導(dǎo)航系統(tǒng)正常輸出精度。 本設(shè)計選用的NAND Flash 外擴存儲器芯片為三星公司的K9F4G08U0M，用于導(dǎo)航程序和解算數(shù)據(jù)的存儲，擴展存儲空間為512 M，工作額定電壓為2.7 V ～3.6 V。

2．2 DCM 的模塊設(shè)計

DCM 包括FPGA、AD 采樣電路、SPI 通訊模塊等部分。 DCM 可以完成對光纖陀螺儀、加速度計、衛(wèi)星導(dǎo)航定位系統(tǒng)接收器等多種外部高精度導(dǎo)航測量傳感器信息的采集。 同時完成與上位機的數(shù)據(jù)交互（作用指令的接收及數(shù)據(jù)輸出），供終端用戶使用和分析信息。

本設(shè)計中的FPGA 采用賽靈思Spartan－6 系列XC6SLX45T 芯片，該型號器件采用高性能低功耗45 nm 芯片技術(shù)，多層金屬布線、雙柵極氧化層工藝，實現(xiàn)了低風(fēng)險、低功耗、高性能的平衡處理需求。 XC6SLX45T 的Logic Cells 數(shù)量為43 661 個，Max Distributed RAM 可達(dá)401 kB，最大用戶I／O 可達(dá)296 個，完全滿足數(shù)據(jù)采集通信模塊的設(shè)計要求。

ENC 系統(tǒng)采用RS－422 接口作為與其他外圍設(shè)備數(shù)據(jù)通信的接口，RS－422 傳輸線纜采用雙絞方式，具有信號傳輸距離遠(yuǎn)、抗環(huán)境干擾性強、速率高等優(yōu)點。

模塊軟件設(shè)計基于Verilog 語言，在FPGA 芯片內(nèi)部針對不同功能需求進(jìn)行邏輯編程，如串口、SPI、雙口RAM 模塊的編程。 擴展的SPI 通信模塊，用FPGA 芯片內(nèi)部的雙口RAM 數(shù)據(jù)緩存，通過調(diào)節(jié)相應(yīng)的通信管腳輸入以及通信協(xié)議，來滿足不同接口的傳感器的數(shù)據(jù)采集。 FPGA 可進(jìn)行重新編譯，不僅使整個系統(tǒng)接口多樣化、功能性能穩(wěn)定可靠，同時方便升級硬件電路，具有良好的移植性和擴展性。

數(shù)據(jù)通信模塊內(nèi)部關(guān)系如圖1 中數(shù)據(jù)采集模塊部分所示，SPI 采集陀螺輸出數(shù)據(jù)及控制AD 采樣電路實現(xiàn)對加速計的信號采集，采集數(shù)據(jù)存放在雙口RAM 中。 RS－422 通信通過UART 串口模塊完成數(shù)據(jù)收發(fā)并通過雙口RAM 緩存。

3 基于主慣導(dǎo)和速度組合的Kalman濾波器設(shè)計

本設(shè)計中考慮了為解決由于中低精度慣性器件（陀螺儀和加速度計）長時間工作誤差積累造成系統(tǒng)精度下降的問題，結(jié)合捷聯(lián)基準(zhǔn)設(shè)備的使用條件，采用將外部信息源與捷聯(lián)基準(zhǔn)進(jìn)行航向與速度組合導(dǎo)航的方案來抑制工作時間對系統(tǒng)精度的影響。 同時，利用Kalman 濾波技術(shù)將主慣導(dǎo)航向與捷聯(lián)基準(zhǔn)輸出的航向及計程儀速度與捷聯(lián)基準(zhǔn)輸出的速度誤差列為觀測量，并在進(jìn)行試驗的過程中對導(dǎo)航解算過程中的誤差量實時估計，同時對誤差估計值進(jìn)行修正、補償。

3．1 基于主慣導(dǎo)組合導(dǎo)航的狀態(tài)方程

系統(tǒng)的狀態(tài)方程為

式中： F——狀態(tài)轉(zhuǎn)移矩陣； X——狀態(tài)變量；G——系統(tǒng)噪聲分配矩陣；W——系統(tǒng)噪聲。

式中：φ——姿態(tài)誤差；v——速度誤差；ε——光纖陀螺儀常值漂移量；ΔA——加速度計的零偏；λ——陀螺安裝誤差。

狀態(tài)量中包含了3 個速度誤差、3 個姿態(tài)誤差、3 個光纖陀螺儀常值漂移量、3 個加速度計的零偏、陀螺儀正交誤差和隨機誤差，本方案只考慮加速度計的隨機誤差和陀螺儀隨機漂移量誤差模型，則

噪聲方面考慮在載體坐標(biāo)系下加速度計與光纖陀螺的隨機漂移分量。 為了簡化模型，初步認(rèn)為陀螺的隨機漂移為白噪聲加上隨機常數(shù)，即

式中：ε——隨機常值；w——白噪聲。 其中，ε的每次的啟動值是隨機變化的，但每次啟動后其值是不變的，因此

由于機械的應(yīng)力釋放會導(dǎo)致慣性測量單元（IMU）中陀螺正交誤差發(fā)生變化，尤其在海上動態(tài)搖擺條件下，由于傾斜角度變大，該誤差被放大，正常情況安裝誤差應(yīng)該是6 個量，本方案以陀螺X 軸為約束軸，選3 個安裝誤差量進(jìn)行在線估計補償，即λ ＝［λλλ］，這樣可以優(yōu)化Kalman 濾波器維數(shù)，減小計算量。

3．2 主慣導(dǎo)航向＋速度組合導(dǎo)航量測方程

艦船上通常使用主慣導(dǎo)系統(tǒng)提供精度較高的導(dǎo)航信息，但由于捷聯(lián)基準(zhǔn)安裝位置和主慣導(dǎo)之間會存在甲板變形和安裝誤差，導(dǎo)致捷聯(lián)基準(zhǔn)測量局部姿態(tài)與主慣導(dǎo)存在差異，因此，將主慣導(dǎo)的航向與捷聯(lián)基準(zhǔn)的航向誤差取為觀測量，同時，將捷聯(lián)基準(zhǔn)輸出的速度與計程儀的速度差值也作為觀測量，這樣可以提高對陀螺漂移和航向失準(zhǔn)角的估計精度，既可以抑制捷聯(lián)長時間工作航向的漂移，也可以抑制水平姿態(tài)振蕩。

系統(tǒng)的觀測方程為

式中：Z——觀測向量；V——捷聯(lián)基準(zhǔn)輸出的速度與計程儀的速度差值；φ——主慣導(dǎo)的航向與捷聯(lián)基準(zhǔn)的航向誤差。

捷聯(lián)系統(tǒng)輸出的航向角方程

式中：?——航向角，（）；?——真實航向角，（°）；δ?——捷聯(lián)系統(tǒng)航向角誤差，（°）。

主慣導(dǎo)輸出的航向角方程為

式中：?——輸出航向角，（°）δ?——主慣導(dǎo)航向角誤差，（°）。

4 半實物仿真及驗證

根據(jù)建立的模型，運用半實物仿真，為了驗證ENC 硬件方案的可行性和基于主慣導(dǎo)航向和速度組合Kalman 濾波器設(shè)計的正確性，在基于FPGA 和DSP 雙處理器的ENC 上實現(xiàn)該功能，并將該ENC應(yīng)用到捷聯(lián)基準(zhǔn)樣機中，通過半實物仿真試驗進(jìn)行驗證。

將樣機安裝在三軸轉(zhuǎn)臺上，裝訂航向信息和速度信息。 光纖捷聯(lián)基準(zhǔn)在組合模式下，將轉(zhuǎn)臺內(nèi)框設(shè)置為±22°／0.1 Hz，中框設(shè)置為±7°／0.143 Hz，外框設(shè)置為±5°／0.2 Hz，模擬艦船在六級海況條件下的工作情況，陀螺零偏0.03°／h，加表零偏50 μg，數(shù)據(jù)更新頻率為200 Hz，仿真時間為600 s，得到航向角、縱橫搖角誤差仿真曲線如圖2 ～圖4 所示?？v搖（Pitch）、橫搖（Roll）及航向角（Yaw）誤差在10 min內(nèi)能快速收斂。 經(jīng)測試，設(shè)備與轉(zhuǎn)臺數(shù)據(jù)同步時差控制在0.1 ms 以內(nèi)，遠(yuǎn)低于PC104 方案的1.2 ms。

圖2 捷聯(lián)基準(zhǔn)縱橫搖角和航向角搖擺曲線Fig．2 Swing curve of strapdown reference attitude angle

圖3 三軸轉(zhuǎn)臺搖擺曲線Fig．3 Swing curve of three axis turntable

圖4 捷聯(lián)基準(zhǔn)以轉(zhuǎn)臺做參考誤差曲線Fig．4 Strapdown standard simulation error curve

捷聯(lián)基準(zhǔn)樣機安裝在艦船的某一基座上，主慣導(dǎo)提供航向、縱橫搖信息，電磁計程儀提供速度信息。 在系泊的狀態(tài)下，對捷聯(lián)基準(zhǔn)進(jìn)行數(shù)據(jù)錄取，對錄取的IMU 數(shù)據(jù)進(jìn)行解算并將與主慣導(dǎo)輸出的姿態(tài)值進(jìn)行比較。 誤差曲線如圖5 所示，縱、橫搖角誤差及航向角誤差均控制在航向角隨機誤差0.039 mrad，縱搖角綜合誤差0.163 mrad；橫搖角綜合誤差0.097 mrad 的范圍內(nèi)。

圖5 航向角誤差曲線Fig．5 Strapdown standard error curve

5 結(jié)束語

本文針對光學(xué)捷聯(lián)基準(zhǔn)解算精度高、工作功耗低、外形尺寸小型化的需求，設(shè)計了一種基于DSP與FPGA 作為核心單元的嵌入式導(dǎo)航計算機。 該硬件方案采用DSP 芯片TMS320C6748 實現(xiàn)導(dǎo)航解算，采用Xilinx Spartan －6 系列FPGA 芯片實現(xiàn)數(shù)據(jù)采集和通信，在計算能力、硬件升級等方面都有很好的性能，克服了導(dǎo)航計算機平臺在運行能力、體積、功耗和成本等方面的限制。 同時，為解決長時間工作對系統(tǒng)精度的影響，研究了基于主慣導(dǎo)航向和計程儀速度組合的Kalman 濾波器設(shè)計。 通過三軸轉(zhuǎn)臺和系泊狀態(tài)下半實物仿真試驗，結(jié)果表明該導(dǎo)航計算機系統(tǒng)數(shù)據(jù)更新率和同步實時性高，可以滿足光學(xué)捷聯(lián)基準(zhǔn)系統(tǒng)的使用要求，同時驗證了基于主慣導(dǎo)航向和計程儀速度組合的Kalman 濾波器設(shè)計的科學(xué)性。