基于DSP的導(dǎo)航計(jì)算機(jī)數(shù)據(jù)處理

周珍娟

摘 要： 研究了以數(shù)字信號(hào)處理芯片作為數(shù)字硬件平臺(tái)，實(shí)現(xiàn)捷聯(lián)式慣性導(dǎo)航系統(tǒng)（SINS）數(shù)據(jù)處理的軟件設(shè)計(jì)。分析了捷聯(lián)式慣性導(dǎo)航系統(tǒng)的動(dòng)力學(xué)方程，模擬了陀螺儀漂移的數(shù)據(jù)，并用系統(tǒng)辨識(shí)中的時(shí)間序列分析法對(duì)漂移數(shù)據(jù)建模，之后進(jìn)行了卡爾曼濾波。最后，在CC環(huán)境下，運(yùn)用C語(yǔ)言和匯編語(yǔ)言混合編程的方式，將位移解算和卡爾曼濾波程序移植到開(kāi)發(fā)板上，利用CC環(huán)境自帶的優(yōu)化編譯器，對(duì)所編寫(xiě)的數(shù)據(jù)處理程序進(jìn)行優(yōu)化，使運(yùn)行速度達(dá)到最快。

關(guān)鍵詞： 數(shù)字信號(hào)處理； 捷聯(lián)式慣性導(dǎo)航； 卡爾曼濾波； 系統(tǒng)辨識(shí)

中圖分類(lèi)號(hào)： TN96?34； TM417 文獻(xiàn)標(biāo)識(shí)碼： A 文章編號(hào)： 1004?373X（2016）07?0044?04

Abstract： DSP chip taken as the digital hardware platform is researched to realize the software design of SINS data proces?sing. The kinetic equation of SINS is analyzed. The gyroscopes drift data was simulated， and modeled with the time series ana?lysis method in system identification. And then Kalman filtering was conducted for it. The hybrid programming method of C language and assembler language is utilized in CC environment to transplant the displacement solution and Kalman filtering programming on development board. The optimization compiler owned by CC environment itself is utilized to optimize the compiled data processing program， which can make the running speed fastest.

Keywords： DSP； strapdown inertial navigation； Kalman filtering； system identification

0 引 言

獲取導(dǎo)彈的飛行姿態(tài)信息和位置信息對(duì)分析其飛行動(dòng)力學(xué)特性具有重要意義。本文的研究?jī)?nèi)容是在火箭彈的3個(gè)軸向上安裝3個(gè)速率陀螺和加速度計(jì)，獲取火箭彈的角速度和線(xiàn)加速度信息，通過(guò)導(dǎo)航計(jì)算機(jī)對(duì)得到的信息進(jìn)行解算，得到火箭彈在飛行過(guò)程中的姿態(tài)和位置信息。慣性測(cè)量元件存在隨機(jī)誤差，這種誤差在工藝上是不可消除的，其中陀螺儀的漂移誤差是導(dǎo)致系統(tǒng)精度下降的重要原因。不管在何種應(yīng)用還是何種形式的陀螺儀，陀螺漂移始終是制約其應(yīng)用系統(tǒng)精度的關(guān)鍵問(wèn)題。因此，為了提高導(dǎo)航系統(tǒng)的精度，有必要建立陀螺漂移的數(shù)學(xué)模型，對(duì)其進(jìn)行有效濾波，或在系統(tǒng)中進(jìn)行補(bǔ)償。

近些年來(lái)，導(dǎo)航計(jì)算機(jī)廣泛應(yīng)用的是Intel80x86系列芯片和MCS?51單片機(jī)，運(yùn)算的實(shí)時(shí)性不容易滿(mǎn)足，針對(duì)這一問(wèn)題，研究了基于DSP的導(dǎo)航計(jì)算機(jī)數(shù)據(jù)處理技術(shù)，包括仿真陀螺儀和加速度計(jì)的輸出信號(hào)，對(duì)陀螺的漂移進(jìn)行卡爾曼濾波后將數(shù)據(jù)送入以DSP 為處理芯片的導(dǎo)航計(jì)算機(jī)，選擇合適的解算方法解算出火箭彈在飛行過(guò)程中的姿態(tài)和位置，并對(duì)程序進(jìn)行優(yōu)化，滿(mǎn)足導(dǎo)航計(jì)算要求的實(shí)時(shí)性和準(zhǔn)確性。

1 捷聯(lián)慣導(dǎo)系統(tǒng)數(shù)據(jù)處理技術(shù)研究

1.1 捷聯(lián)慣導(dǎo)系統(tǒng)的計(jì)算原理

系統(tǒng)第一步通過(guò)[Cbp]（姿態(tài)矩陣）推演得到[apib]（游移方位坐標(biāo)系），第二步，根據(jù)加速度計(jì)測(cè)量的慣性空間相對(duì)機(jī)體坐標(biāo)系在[abib]分量的比例（機(jī)體坐標(biāo)系軸向上的分量），得出導(dǎo)航位置參數(shù)。進(jìn)一步，通過(guò)補(bǔ)償角加速度和重力加速度，對(duì)[apib]用速度方程進(jìn)行積分運(yùn)算得到速度分量[Vpep]。[Vpep]（速度分量）不僅可以作位置速率計(jì)算的輸入，同時(shí)也可以用作系統(tǒng)的輸出，為了得到[ωpep]（位置角速率），可以使用位置速率方程計(jì)算，一方面通過(guò)位置微分方程的積分去更新位置矩陣[Cpe，]以便由位置矩陣中的元素[Cij]按照導(dǎo)航位置參數(shù)計(jì)算公式，解算出任意時(shí)刻的導(dǎo)航位置參數(shù)[φ，λ，α；]另一方面又與地球角速率[ωpie]迭加，通過(guò)[ωbib]（陀螺輸出的角速率）變換后和[ωbpb]（姿態(tài)矩陣）一起構(gòu)成姿態(tài)角速率，并實(shí)時(shí)更新姿態(tài)矩陣[Cbp，]同時(shí)使用姿態(tài)微分方程的積分運(yùn)算。[Cbp]（姿態(tài)矩陣）不僅可以擔(dān)負(fù)起整體運(yùn)作的作用，同時(shí)還可以進(jìn)行由機(jī)體坐標(biāo)系到游移方位坐標(biāo)系的坐標(biāo)變換。

1.2 用四元數(shù)法進(jìn)行位置解算

所需初始數(shù)據(jù)：積分計(jì)算時(shí)需要的初始數(shù)據(jù)包括初始速度[Vx0，Vy0，Vz0，]初始經(jīng)度[φ0，]初始緯度[λ0，]初始游移方位角[α0，]采樣時(shí)間，積分步長(zhǎng)等。在計(jì)算時(shí)，由于積分誤差的存在，破壞了四元數(shù)變換的正交性，使四元數(shù)范數(shù)[N≠1。]因此，需要對(duì)范數(shù)進(jìn)行修正，將四元數(shù)歸一化。四元數(shù)歸一化后即可得到姿態(tài)矩陣[Tij]中的各個(gè)元素，在彈道仿真時(shí)，得到的是在機(jī)體坐標(biāo)系下的加速度計(jì)信息，姿態(tài)矩陣得到后即可將機(jī)體坐標(biāo)系下的加速度信息轉(zhuǎn)換為平臺(tái)坐標(biāo)系下的加速度信息，在位置解算時(shí)直接使用。

2 卡爾曼濾波算法的實(shí)現(xiàn)

2.1 時(shí)間序列分析法建立陀螺儀漂移模型

確定模型信號(hào)具有零均值、平穩(wěn)、正態(tài)分布的特性。陀螺儀隨機(jī)漂移是去掉均值的陀螺漂移信號(hào)，因此具備了零均值特性。對(duì)于通過(guò)測(cè)試系統(tǒng)測(cè)量得到的陀螺隨機(jī)漂移信號(hào)，這時(shí)候系統(tǒng)的正態(tài)性也可以得到進(jìn)一步的保證。因?yàn)槲粗獠凯h(huán)境及內(nèi)部因素存在各種干擾和不確定性，目前很難保證陀螺漂移信號(hào)的平穩(wěn)性。因此在系統(tǒng)建模的時(shí)候，就應(yīng)該通過(guò)統(tǒng)計(jì)檢驗(yàn)的科學(xué)數(shù)學(xué)方法，評(píng)判隨機(jī)陀螺漂移信號(hào)的平衡穩(wěn)定性。

系統(tǒng)假設(shè)條件如下：漂移數(shù)據(jù)去除了線(xiàn)性趨勢(shì)項(xiàng)、常值項(xiàng)、周期趨勢(shì)項(xiàng)，系統(tǒng)以動(dòng)力調(diào)協(xié)式陀螺儀漂移為對(duì)象，得到了零均值、平穩(wěn)、正態(tài)分布的白噪聲序列。這個(gè)序列主要是用來(lái)驗(yàn)證模型參數(shù)辨識(shí)和卡爾曼濾波方法的有效性。

2.2 卡爾曼濾波初始值的確定

初始誤差協(xié)方差矩陣[P0]與初始估計(jì)[X0]要求在卡爾曼估計(jì)開(kāi)始前決定，預(yù)先根據(jù)已確定信息得出，同樣可以通過(guò)觀(guān)測(cè)得出。如果卡爾曼濾波估計(jì)方法應(yīng)用于結(jié)構(gòu)系統(tǒng)的識(shí)別，[X0]和[P0]的初值設(shè)定其實(shí)不會(huì)對(duì)濾波誤差產(chǎn)生影響。此處設(shè)初始估計(jì)[X0]為漂移序列的初始值，初始誤差協(xié)方差矩陣[P0=0。]

2.3 卡爾曼濾波程序的實(shí)現(xiàn)

3 在DSP開(kāi)發(fā)板上的數(shù)據(jù)程序優(yōu)化過(guò)程

在編寫(xiě)程序時(shí)，必須首先實(shí)現(xiàn)程序應(yīng)該完成的功能，在此基礎(chǔ)上才可以使用優(yōu)化工具。下面同樣以對(duì)位置解算程序的優(yōu)化過(guò)程為例。

在C32的開(kāi)發(fā)環(huán)境中，使用Profiling（剖析）功能，可以設(shè)置斷點(diǎn)進(jìn)行執(zhí)行周期數(shù)的測(cè)試。因?yàn)槭褂玫腃32芯片主頻為40 MHz，所以clock setup如圖2所示，操作為profiler>clock setup。對(duì)時(shí)鐘使能，觀(guān)察時(shí)鐘運(yùn)行情況。

C32編譯優(yōu)化器的選項(xiàng)分為Optimization Level和Inline Option兩個(gè)大類(lèi)，他們各自又分為不同的級(jí)別。Optimization Level包括前面所說(shuō)的優(yōu)化等級(jí)，即Disable，Level 0?Registers Only，Level 1? Local， Level 2? Global， Level 3?File；Inline Option包括Disable，Intrinsic Operator Only，F(xiàn)ull，其中Intrinsic Operator Only 的含義是指將C32特有的內(nèi)聯(lián)函數(shù)功能進(jìn)行行內(nèi)擴(kuò)展，內(nèi)聯(lián)函數(shù)是專(zhuān)門(mén)為該芯片具體編寫(xiě)的，而且是已經(jīng)過(guò)優(yōu)化的常用函數(shù)。操作為：project>option…>compiler>optimization level，界面見(jiàn)圖3。

下面對(duì)這些優(yōu)化條件進(jìn)行不同的組合，得到的求解四元數(shù)的核心程序dery的最短運(yùn)行周期數(shù)，如表1所示。從表1中可以看出，若只使用Inline Option 中的Full選項(xiàng)，對(duì)程序的優(yōu)化效果很顯著，而Intrinsic Operator Only無(wú)任何效果，這是因?yàn)楸疚闹械膁ery子函數(shù)未使用到C32的內(nèi)聯(lián)函數(shù)。另外，使用Optimization Level而不使用Inline Option時(shí)，在Level?0時(shí)會(huì)有很好的效果，而其他選項(xiàng)的效果并不明顯，這是因?yàn)長(zhǎng)evel?0包括了將運(yùn)行調(diào)用變?yōu)樾袃?nèi)擴(kuò)展的功能，與Inline Option的功能相同。

5 結(jié) 論

通過(guò)彈道仿真得到了捷聯(lián)慣導(dǎo)系統(tǒng)中陀螺儀和加速度計(jì)的輸出信號(hào)，根據(jù)得到的輸出數(shù)據(jù)，編寫(xiě)位置解算程序?qū)楏w在空中的位置信息進(jìn)行解算，然后用系統(tǒng)辨識(shí)中的時(shí)間序列分析法對(duì)陀螺儀的漂移建模，通過(guò)卡爾曼濾波程序，對(duì)陀螺儀的漂移數(shù)據(jù)進(jìn)行濾波，最后將程序從PC機(jī)上移植到DSP 開(kāi)發(fā)板上，并利用優(yōu)化編譯器對(duì)位置解算程序進(jìn)行了優(yōu)化。

受C32SS開(kāi)發(fā)板內(nèi)存空間所限，目前只能存儲(chǔ)彈道仿真程序步長(zhǎng)為0.5 ms的數(shù)據(jù)，在今后的研究中，如果能夠解決內(nèi)存限制的問(wèn)題，或在由實(shí)際的陀螺輸出數(shù)據(jù)的情況下，可以縮短計(jì)算周期進(jìn)行解算驗(yàn)證。本文采用的是最基礎(chǔ)的卡爾曼濾波算法，為了得到更好的濾波效果，使濾波算法更加完善，在以后的工作中，可采用如擴(kuò)展的卡爾曼濾波算法，自適應(yīng)卡爾曼濾波算法等，并要考慮卡爾曼濾波的穩(wěn)定性，對(duì)濾波中的發(fā)散現(xiàn)象進(jìn)行抑制。

參考文獻(xiàn)

[1] 陳哲.捷聯(lián)慣導(dǎo)系統(tǒng)原理[M].北京：宇航出版社，1986.

[2] 孟秀云.導(dǎo)彈制導(dǎo)與控制系統(tǒng)原理[M].北京：北京理工大學(xué)出版社，2003：25?27.

[3] 付夢(mèng)印，鄧志紅，張繼偉.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京：科學(xué)出版社，2003：8?10.

[4] VAN CLEVE D P. Weapon interface simulation [C]// Procee?dings of 1999 IEEE Conference on Systems Readiness Techno?logy. San Antonio： IEEE， 1999： 593?597.

[5] 尹勇，歐光軍，關(guān)榮鋒.DSP集成開(kāi)發(fā)環(huán)境CCS開(kāi)發(fā)指南[M].北京：北京航空航天大學(xué)出版社，2003.

[6] 曾廣達(dá).系統(tǒng)辨識(shí)與仿真[M].成都：電子科技大學(xué)出版社，1995：66?68.

[7] 林瑞陽(yáng)，楊東升，邱鋒.Unscented卡爾曼濾波對(duì)目標(biāo)位置預(yù)測(cè)[J].現(xiàn)代電子技術(shù)，2014，37（1）：34?37.

[8] Texas Instruments. TMS320C3X general applications [R]. US： Texas Instruments， 1998： 78?79.

[9] 張兆亮，趙偉，趙大建，等.導(dǎo)航計(jì)算機(jī)系統(tǒng)中CPLD配置軟件串口更新研究[J].現(xiàn)代電子技術(shù)，2011，34（24）：56?59.

[10] 陳亮，劉金學(xué).基于DSP和FPGA的導(dǎo)航計(jì)算機(jī)系統(tǒng)設(shè)計(jì)[J].現(xiàn)代電子技術(shù)，2012，35（2）：1?3.