基于DSP的捷聯(lián)慣導(dǎo)系統(tǒng)組合初始對準(zhǔn)算法設(shè)計?

（海軍工程大學(xué)電氣工程學(xué)院 武漢 430033）

1 引言

慣性導(dǎo)航作為當(dāng)今各種導(dǎo)航方式之一，具有抗干擾能力強，不需要外界提供信息等諸多優(yōu)勢，具有不可替代作用。而光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)通過數(shù)學(xué)平臺替代物理平臺，具有結(jié)構(gòu)簡單，成本低，重量輕等特點，是慣性導(dǎo)航發(fā)展的必然趨勢［1］。

在平臺慣導(dǎo)系統(tǒng)中，運載體的姿態(tài)角以及航向角可通過平臺框架上得到，計算量小。而在捷聯(lián)慣導(dǎo)系統(tǒng)中由于沒有平臺慣導(dǎo)類似的物理平臺隔離角運動，載體的晃動干擾直接作用于慣性元件，并采用數(shù)學(xué)平臺得到載體坐標(biāo)系和導(dǎo)航坐標(biāo)系之間的方向余弦矩陣，姿態(tài)角和航向角都需要經(jīng)過計算獲得，這導(dǎo)致捷聯(lián)慣導(dǎo)的導(dǎo)航解算部分計算量龐大［2］。而高精度捷聯(lián)慣導(dǎo)系統(tǒng)又對系統(tǒng)處理數(shù)據(jù)的實時性要求很高，這就需要選取合適處理器來完成精度高、計算量大的導(dǎo)航解算任務(wù)。另外慣性導(dǎo)航的初始對準(zhǔn)關(guān)系到導(dǎo)航的精度和使用前的準(zhǔn)備時間，初始對準(zhǔn)誤差會隨著導(dǎo)航解算過程進行累積，制約著導(dǎo)航的精度［2～6］?；诖吮疚耐ㄟ^TMS320F28335 高速 DSP［7～8］完成導(dǎo)航系統(tǒng)初始對準(zhǔn)工作，采用慣性系粗對準(zhǔn)和卡爾曼精對準(zhǔn)完成初始對準(zhǔn)，通過設(shè)計軟件程序設(shè)計等步驟流程驗證算法的可行性。

2 捷聯(lián)慣導(dǎo)組合初始對準(zhǔn)技術(shù)

捷聯(lián)慣性導(dǎo)航系統(tǒng)的初始對準(zhǔn)是系統(tǒng)正常進入導(dǎo)航解算階段的必要前提。初始對準(zhǔn)分為粗對準(zhǔn)和精對準(zhǔn)兩個階段，對準(zhǔn)過程中首先利用粗對準(zhǔn)在短時間內(nèi)得到載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的姿態(tài)矩陣的估計粗略值，再通過精對準(zhǔn)計算得到精確的姿態(tài)矩陣［5］。而初始位置信息一般通過外界輸入得到（如通過GPS獲?。?］）。對于初始速度信息，由于一般是在靜止?fàn)顟B(tài)下進行初始對準(zhǔn)，初始速度為零。本文所用的組合對準(zhǔn)是將對準(zhǔn)速度快的慣性系對準(zhǔn)和精度高的卡爾曼濾波對準(zhǔn)有機結(jié)合，達到將二者算法優(yōu)勢結(jié)合的目的。

2.1 慣性系快速粗對準(zhǔn)

傳統(tǒng)解析式粗對準(zhǔn)是基于地理系，慣性系粗對準(zhǔn)是基于慣性系，但本質(zhì)上都是雙矢量定姿［3］。慣性系對準(zhǔn)是通過重力加速度信息在慣性坐標(biāo)系內(nèi)進行相應(yīng)的解算得到姿態(tài)矩陣。慣性系對準(zhǔn)較傳統(tǒng)的對準(zhǔn)方法可以在短時間內(nèi)得到導(dǎo)航前初始姿態(tài)角，并可以較好地適應(yīng)晃動基座環(huán)境下的初始對準(zhǔn)，具有較好地抗干擾能力，能夠適用于系泊晃動的搖擺基座環(huán)境。解決了傳統(tǒng)解析粗對準(zhǔn)僅能夠適用于載體靜態(tài)或輕微晃動環(huán)境下實現(xiàn)對準(zhǔn)的局限性［10～12］。

在慣性坐標(biāo)系下將姿態(tài)矩陣分裂成三個矩陣［3］分別求解：

式中，i系為地心慣性坐標(biāo)系（初始對準(zhǔn)時刻的地球坐標(biāo)系）；ib0系為基座慣性坐標(biāo)系（初始時刻的載體坐標(biāo)系，相對慣性空間無轉(zhuǎn)動）。

為載體相對基座慣性坐標(biāo)系的姿態(tài)陣，其初值是單位矩陣，即，可以體現(xiàn)出晃動基座的姿態(tài)變化，而可由陀螺輸出利用旋轉(zhuǎn)矢量法實時更新得到［2，10］。

可以根據(jù)載體所在位置的緯度信息L及時間t計算得到：

對于求解陣，由于i和ib0均為慣性坐標(biāo)系，可選取慣性坐標(biāo)系下的兩個不共線的參考矢量加以確定。

由于在晃動基座下加計輸出因載體晃動會產(chǎn)生干擾線加速度，將這種大致呈現(xiàn)周期性變化的干擾通過積分可在一定程度上降低此干擾產(chǎn)生的速度誤差。因此對式（3）和式（4）進行積分得到式（5）和（6）：

構(gòu)建兩組速度適量v(tk1)、v(tk2)以及兩組速度矢量的叉乘矢量v(tk1)×v(tk2)，以此建立ib0和i的關(guān)系：

考慮到在對重力加速度在ib0系內(nèi)進行積分運算時沒有對垂蕩、縱蕩和橫蕩的擾動作補償處理，在晃動基座環(huán)境下這會導(dǎo)致的求取產(chǎn)生較大誤差，因此只用慣性系對準(zhǔn)作為粗對準(zhǔn)的方法。

2.2 卡爾曼濾波精對準(zhǔn)

卡爾曼精對準(zhǔn)解算過程是在粗對準(zhǔn)得到的粗略姿態(tài)信息的基礎(chǔ)上，采用卡爾曼濾波器精確估計出失準(zhǔn)角［13～14］，再對粗對準(zhǔn)得到的姿態(tài)矩陣作修正，進而得到更精確的姿態(tài)信息。

經(jīng)典卡爾曼濾波器包含兩個回路：狀態(tài)估計回路（主回路）和誤差方差回路（引導(dǎo)回路），通過卡爾曼增益，將兩個回路連接。根據(jù)是否引入量測量將濾波器的更新方式分為兩類：時間更新和量測更新?？柭鼮V波方程可劃分為五個基本公式。

在卡爾曼濾波過程中，考慮到簡化計算復(fù)雜程度，建立了9維的卡爾曼濾波方程。選取姿態(tài)誤差、速度誤差和位置誤差作為狀態(tài)變量X，將系統(tǒng)解算得到的速度Vn和位置POS與GPS提供的準(zhǔn)確的速度vnGPS和位置posGPS作差得到觀測量Z：

狀態(tài)轉(zhuǎn)移矩陣Φ由捷聯(lián)慣導(dǎo)系統(tǒng)的誤差方程經(jīng)過離散化處理后獲得的9×9維矩陣。觀測矩陣H為

DSP對IMU的數(shù)據(jù)采集頻率為50Hz，設(shè)置GPS發(fā)送頻率為1Hz，GPS信號通過串口被DSP采集并解碼提取速度和位置信息。在GPS傳輸間隔只進行卡爾曼濾波器的時間更新，在獲取到GPS提供的速度和位置信息后進行一次量測更新。每次狀態(tài)變量更新后，通過得到的9個誤差量對當(dāng)前解算得到的導(dǎo)航結(jié)果進行一次反饋修正。

系統(tǒng)噪聲Q陣，由實測數(shù)據(jù)反推得到陀螺的角度隨機游走和加速度計的速率隨機游走。

觀測噪聲R陣，根據(jù)GPS標(biāo)稱的定位誤差設(shè)置。

3 基于CCS的組合對準(zhǔn)程序設(shè)計

DSP通常使用TI公司提供的集成開發(fā)環(huán)境CCS［7］，該環(huán)境為用戶提供了環(huán)境配置、源程序編輯、源程序鏈接、源程序調(diào)試、運行結(jié)果分析等功能，可為用戶提供與硬件開發(fā)板結(jié)合的在線編程調(diào)試。在開發(fā)過程中，可使用TI為F28335用戶提供的利用結(jié)構(gòu)體和聯(lián)合體定義片內(nèi)各個寄存器的頭文件庫，用戶可以方便地用C語言對DSP內(nèi)部的寄存器進行訪問。DSP28335程序空間和數(shù)據(jù)空間是統(tǒng)一編址的，通過配置cmd文件完成對存儲空間的劃分［8］。

在編寫主程序前通常先對系統(tǒng)配置進行初始化，GPIO以及串口初始化要結(jié)合硬件的連接進行。本系統(tǒng)采用SCI串口進行數(shù)據(jù)通信，F(xiàn)28335提供給用戶3個可使用的SCI串口。SCI_A和IMU導(dǎo)航原始數(shù)據(jù)進行通信，SCI_B和上位機進行通信，SCI_C和外部GPS模塊進行通信。通過最大可達16級深度的FIFO，設(shè)置中斷接收函數(shù)，減少CPU不必要的開銷，提高效率。對準(zhǔn)過程中由上位機輸出導(dǎo)航參數(shù)。圖1為慣性系粗對準(zhǔn)流程圖，圖2為卡爾曼精對準(zhǔn)流程圖，圖3為從GPS信號中解碼出位置和速度信息流程圖。

圖1 慣性系粗對準(zhǔn)流程

4 實測試驗及結(jié)果分析

在調(diào)試代碼通過后，通過F28335自帶的FLASH完成程序固化，并上轉(zhuǎn)臺試驗?；贔28335DSP慣導(dǎo)實驗平臺如圖4所示。值得注意的是由于FLASH中指令運行速度遠不如RAM調(diào)試中的運行速度，需要將部分對時間較敏感的函數(shù)在系統(tǒng)開機后自動復(fù)制到F28335的RAM中運行，提高運行速度。

為驗證組合對準(zhǔn)算法的有效性，以項目組自研光纖陀螺捷聯(lián)慣導(dǎo)樣機為測試平臺，完成傳統(tǒng)的羅經(jīng)對準(zhǔn)方法和本文采用的組合對準(zhǔn)方法的比較驗證。

由于實驗條件限制，無法使用高精度定向測試轉(zhuǎn)臺，只將慣導(dǎo)系統(tǒng)放置于實驗臺上進行靜態(tài)初始對準(zhǔn)測試，水平姿態(tài)角和航向角未知。

圖2 卡爾曼精對準(zhǔn)流程

圖3 提取GPS速度和位置信息流程

首先采用傳統(tǒng)的羅經(jīng)對準(zhǔn)方法（下文稱作羅經(jīng)法）對系統(tǒng)進行30min靜基座的初始對準(zhǔn)測試。由于傳統(tǒng)羅經(jīng)法的水平對準(zhǔn)能夠在短時間內(nèi)收斂至較高精度，而方位對準(zhǔn)難以在短時間內(nèi)達到較高精度要求，因此設(shè)置水平對準(zhǔn)2min，航向?qū)?zhǔn)28min，對準(zhǔn)結(jié)果如圖5和圖6所示。

圖4 DSP調(diào)試平臺

圖5 傳統(tǒng)羅經(jīng)法水平對準(zhǔn)

圖6 傳統(tǒng)羅經(jīng)法航向?qū)?zhǔn)

圖7 基于卡爾曼濾波的水平對準(zhǔn)

然后再采用本文提出的組合對準(zhǔn)方法進行對比測試。鑒于慣性系對準(zhǔn)具有對準(zhǔn)速度快，抗擾動能力強的特性，設(shè)置慣性系粗對準(zhǔn)時間為2min，卡爾曼濾波精對準(zhǔn)時間為18min。其中基于卡爾曼濾波的精對準(zhǔn)結(jié)果如圖7和圖8所示。

圖8 基于卡爾曼濾波的航向?qū)?zhǔn)

組合對準(zhǔn)方法的粗對準(zhǔn)和精對準(zhǔn)過程分別獲得的姿態(tài)角見表1。兩種對準(zhǔn)方法完成后獲得的姿態(tài)角見表2。

表1 組合法對準(zhǔn)結(jié)果

表2 兩種方法對準(zhǔn)結(jié)果對比

由于放置慣導(dǎo)系統(tǒng)的實驗臺水平誤差未知，并且北向基準(zhǔn)的不確定，因此無法得知理想的具體對準(zhǔn)精度，只能相對而言進行評估。由表1可以看到，在靜態(tài)環(huán)境下由于外部干擾小，在慣性系快速對準(zhǔn)中陣計算誤差小，因而經(jīng)過粗對準(zhǔn)估計出的三個姿態(tài)角已經(jīng)接近了精對準(zhǔn)估計出的姿態(tài)角，驗證了慣性系粗對準(zhǔn)的高效性，同時為后續(xù)的精對準(zhǔn)打下了良好的基礎(chǔ)。由表2可以看出，本文使用的組合對準(zhǔn)算法和羅經(jīng)法對準(zhǔn)結(jié)果在水平姿態(tài)角上精度相當(dāng)。兩種方法獲得的航向角相差較大，但由于無精確的北向基準(zhǔn)，無法判定航向角精度的好壞。

通過對比圖6和圖8，傳統(tǒng)的羅經(jīng)法航向角在1500s后波動性有所減弱但仍不穩(wěn)定，短時間對準(zhǔn)精度較差。而組合法的航向角在400s后波動性減弱，800s后趨于穩(wěn)定?？梢钥闯鲈诤较蚪菍?zhǔn)上組合對準(zhǔn)方式不管是在速度還是穩(wěn)定性上都要優(yōu)于傳統(tǒng)羅經(jīng)對準(zhǔn)。通過對比圖5和圖7，在水平角對準(zhǔn)上兩種對準(zhǔn)方式精度相似，但組合法輸出姿態(tài)角更加平穩(wěn)。兩種方法在800s后的估計的姿態(tài)角標(biāo)準(zhǔn)差見表3。

表3 兩種方法800s后估計的姿態(tài)角標(biāo)準(zhǔn)差

綜上分析，本文采用的慣性系快速粗對準(zhǔn)可以在短時間內(nèi)得到較為精確的姿態(tài)角，精度和傳統(tǒng)的羅經(jīng)法對準(zhǔn)相當(dāng)。粗對準(zhǔn)后再通過卡爾曼濾波器對失準(zhǔn)角進行精確估計。從試驗結(jié)果看驗證了傳統(tǒng)的羅經(jīng)對準(zhǔn)方法在航向?qū)?zhǔn)上存在收斂速度較慢、穩(wěn)定性較差等問題。而組合對準(zhǔn)方法可以有效解決傳統(tǒng)方式存在的問題，在短時間內(nèi)航向角標(biāo)準(zhǔn)差比傳統(tǒng)法降低兩個數(shù)量級，水平角標(biāo)準(zhǔn)差比傳統(tǒng)法降低一個數(shù)量級。另外組合對準(zhǔn)在對準(zhǔn)速度上比傳統(tǒng)方法更快，可以在16min內(nèi)完成。

5 結(jié)語

初始對準(zhǔn)作為捷聯(lián)慣性導(dǎo)航系統(tǒng)進入工作的必要前提，對準(zhǔn)誤差會在解算過程中進行傳遞累積，因此對準(zhǔn)精度是提高導(dǎo)航精度的前提。通過采用慣性系粗對準(zhǔn)和基于卡爾曼濾波精對準(zhǔn)的組合方式完成捷聯(lián)慣導(dǎo)系統(tǒng)的初始對準(zhǔn)，可以增強系統(tǒng)對外界的抗擾動能力，同時兼顧對準(zhǔn)精度和對準(zhǔn)時間?；贒SP28335作為初始對準(zhǔn)的處理器，利用CCS軟件完成了組合對準(zhǔn)程序設(shè)計，以光纖陀螺捷聯(lián)慣導(dǎo)樣機以及本文設(shè)計程序算法為平臺完成比對驗證試驗，證明了方法的有效性。