基于dsptms320f28335的全自主捷聯(lián)慣性導航系統(tǒng)的制作方法
【專利摘要】基于DSPTMS320F28335的全自主捷聯(lián)慣性導航系統(tǒng)，本系統(tǒng)以DSPTMS320F28335作為控制器，將全自主捷聯(lián)慣性導航算法嵌入到浮點型DSP內(nèi)的軟件實現(xiàn)，實時采集、分析慣性測量單元IMU中光纖陀螺和加速度計的輸出信號并控制算法運行，以保證全自主捷聯(lián)慣性導航系統(tǒng)的高速處理能力、高實時性、高精度、高性能、低成本、低功耗。本發(fā)明適合應用于不提供精確定位信息、受外界干擾載體姿態(tài)時刻都在發(fā)生變化的場合下載體的全自主實時捷聯(lián)慣性系統(tǒng)，諸如直接不提供緯度信息，或者緯度信息不可測的隧道深處、深山密林、深海海底情況下的載體全自主實時捷聯(lián)慣性導航。
【專利說明】
基于DSPTMS320F28335的全自主捷聯(lián)慣性導航系統(tǒng)
技術領域
[0001 ]本發(fā)明提供的是一種基于DSPTMS320F28335的全自主捷聯(lián)慣性導航系統(tǒng)，精確度 高，數(shù)據(jù)處理效率高，涉及算法嵌入浮點型DSPTMS320F28335內(nèi)的軟件實現(xiàn)、晃動基座下載 體所在位置煒度信息計算、動態(tài)干擾情況下捷聯(lián)姿態(tài)矩陣的解算，自對準完成后的捷聯(lián)慣 性自主導航。本發(fā)明適合應用于不提供精確定位信息、受外界干擾載體姿態(tài)時刻都在發(fā)生 變化的場合下載體的全自主實時捷聯(lián)慣性系統(tǒng)，諸如直接不提供煒度信息，或者煒度信息 不可測的隧道深處、深山密林、深海海底情況下的載體全自主實時捷聯(lián)慣性導航。
【背景技術】
[0002]導航就是以一定的精度、在規(guī)定的時間內(nèi)將載體沿著預定的路線引導至目的地的 過程。慣性導航是一種自主式導航技術，它不依靠外界信息，也不向外界輻射任何能量，僅 利用陀螺儀和加速度計等慣性測量器件以及初始的導航信息來確定載體運行期間的各項 導航參數(shù)，隱蔽性好、抗干擾性強，能夠全天時、全天候為載體提供完備的運動信息。
[0003]慣性導航中的捷聯(lián)式慣性導航技術將陀螺儀和加速度計直接安裝在載體上，得到 載體系下的加速度和角速度，通過導航計算機將測得的數(shù)據(jù)轉(zhuǎn)換至導航坐標系完成導航， 它不需要實體的穩(wěn)定平臺，成本低、體積小、重量輕、可靠性高。
[0004]慣性導航系統(tǒng)在進入導航工作之前，需要獲得初始姿態(tài)以及位置信息，即進行初 始對準過程。初始對準通常分為兩個階段:粗對準階段和精對準階段。諸如發(fā)動機處于高頻 振動的汽車、浪涌下的艦船等受干擾較大的載體，角振動和線振動會導致基座晃動，捷聯(lián)慣 導系統(tǒng)在自對準過程中的姿態(tài)時刻都在發(fā)生變化，初始對準的時間和精度會受到影響。因 此，在運載體晃動干擾環(huán)境下完成初始對準過程，就必須屏蔽這些無法消除的擾動影響。
[0005] 無論是導航過程還是對準過程，無論是靜基座自對準方法還是動基座自對準方 法，無論粗對準還是精對準，都需要外界輔助信息提供當?shù)販蚀_的地理煒度信息，如果地理 煒度存在誤差，將可能影響到初始對準的精度。這種依賴輔助信息的捷聯(lián)慣性導航是不自 主導航，然而在深山荒野，海底等環(huán)境中，想要獲得當?shù)氐牡乩砦恢眯畔⒉⒉皇且患苋菀?的事情，或者某些應用場合，如通信衛(wèi)星跟蹤或者武器平臺的穩(wěn)定，有較高的定向要求，而 沒有必要提供精確的定位要求，或者在某些應急場合，如軍事戰(zhàn)爭中，受到敵方攻擊，需要 迅速做出反擊，而定位系統(tǒng)來不及工作，卻急需定向。在上述煒度未知的條件下，想要完成 導航工作，就必須利用系統(tǒng)自身的慣性測量單元輸出解算出載體初始時刻所在位置的煒度 信息，利用自身解算煒度信息值完成導航過程，實現(xiàn)載體的全自主捷聯(lián)慣性導航。
[0006] 另外，隨著捷聯(lián)式慣性導航技術的不斷應用，對系統(tǒng)的精度、算法、數(shù)據(jù)處理能力、 實時性、穩(wěn)定性都有了相應的進一步的要求，且大多相關應用場合都提出了系統(tǒng)小型化、低 功耗等要求。從處理器硬件平臺選擇上考慮，在大量的浮點數(shù)運算或者復雜算法運算時，對 于一般的單片機或定點型處理器，程序執(zhí)行效率較低，數(shù)據(jù)更新較慢，執(zhí)行周期較長，不僅 不能滿足對運載體姿態(tài)矩陣實時高效地解算，同時數(shù)據(jù)處理精度低，運算能力也不能完全 滿足。
[0007] 針對上述問題，本發(fā)明應用TI公司的TMS320F28335浮點型DSP控制器作為硬件平 臺，來控制數(shù)據(jù)采集和算法解算，完成載體的實時快速捷聯(lián)慣性導航系統(tǒng)。在本發(fā)明中， TMS320F28335型數(shù)字信號處理器的高速處理能力、高精度、高性能、低成本、低功耗、外設集 成度高、數(shù)據(jù)以及程序存儲量大、32位浮點運算單元等性能在解算方面得到了充分發(fā)揮，從 而滿足數(shù)據(jù)處理效率高，實時性好，精確度高，有效在動態(tài)干擾情況下解算煒度信息值，有 效隔離晃動對載體初始對準的影響，有效實現(xiàn)全自主捷聯(lián)慣性導航系統(tǒng)的實時高精度快速 自對準，進入自主導航狀態(tài)。

【發(fā)明內(nèi)容】

[0008] 在本發(fā)明方法中，全自主捷聯(lián)慣性導航算法嵌入到浮點型DSPTMS320F28335內(nèi)的 軟件實現(xiàn)。本發(fā)明的目的在于，針對需要外界輔助信息提供當?shù)氐乩頍樁刃畔⒌膶Ш江h(huán)境， 以及對導航系統(tǒng)數(shù)據(jù)處理能力、實時性、穩(wěn)定性、小型化、低功耗的進一步高要求，提出一種 基于DSPTMS320F28335的全自主捷聯(lián)慣性導航系統(tǒng)，不依外界賴輔助信息，僅利用系統(tǒng)自身 的慣性測量單元輸出，高精確度、高數(shù)據(jù)處理效率地在動態(tài)干擾情況計算煒度信息，有效隔 離晃動對載體初始對準的影響，有效實現(xiàn)實時快速高精度全自主式導航過程。
[0009] 實現(xiàn)本發(fā)明目的的技術解決方案為：
[0010] 本發(fā)明提出一種基于DSPTMS320F28335的全自主捷聯(lián)慣性導航系統(tǒng)，包括以下方 面：
[0011] (1)上位導航計算機開啟，CCS5.3集成開發(fā)環(huán)境打開，建立工程并編寫編譯算法程 序，鏈接SEED-XDS510PLUS仿真器，將算法嵌入到DSPTMS320F28335內(nèi)的軟件，DSP控制算法 運行；
[0012] (2)32位浮點型DSPTMS320F28335控制慣性全自主導航系統(tǒng)，向?qū)Ш较到y(tǒng)發(fā)送命令 協(xié)議，利用SCI通信，通過RS232通信串口，采集慣性測量單元頂U中光纖陀螺和加速度計的 輸出信號；
[0013] ⑶不依外界賴輔助信息，僅利用系統(tǒng)自身的慣性測量單元輸出，以慣性空間為參 考基準，利用不同時刻重力投影之間的夾角，確定晃動基座下運載體初始時刻所在位置的 煒度信息；
[0014] (4)不依外界賴輔助信息，帶入計算所得煒度信息，利用系統(tǒng)自身的慣性測量單元 輸出，基于雙矢量定姿原理，利用重力加速度在慣性空間方位的改變包含地球北向信息這 一特性，求解慣性系下粗略的初始姿態(tài)陣，接著采用基于新息的自適應濾波方法獲取載體 的失準角，用失準角修正捷聯(lián)姿態(tài)矩陣，得到三軸姿態(tài)信息，實時記錄數(shù)據(jù)，完成初始對準， 進入全自主導航狀態(tài)。
[0015] 本發(fā)明使用基于DSPTMS320F28335的全自主捷聯(lián)慣性導航系統(tǒng)，裝置總體結(jié)構(gòu)框 圖如圖1，裝置總體實物圖如圖2。系統(tǒng)裝置包括：浮點型DSPTMS320F28335、SEED-XDS510PLUS仿真器、光纖陀螺、加速度計、SCI通信電路、RS232及RS422轉(zhuǎn)USB通信串口、上位 導航計算機、數(shù)據(jù)電纜部分、電源。以DSPTMS320F28335作為控制器，將全自主捷聯(lián)慣性導航 算法嵌入到浮點型DSP內(nèi)的軟件實現(xiàn)，實時采集、分析慣性測量單元頂U中光纖陀螺和加速 度計的輸出信號并控制算法運行，以保證全自主捷聯(lián)慣性導航系統(tǒng)的高速處理能力、高實 時性、高精度、高性能、低成本、低功耗。慣性測量IMU單元中，光纖陀螺儀的零點偏置穩(wěn)定性 是0.5度/h，零點偏置重復性是0.5度/h，加速度計的零點偏置小于lmg，零點偏置穩(wěn)定性小 于lmg，數(shù)據(jù)更新速率100HZ，波特率115200bps;數(shù)據(jù)電纜部分，供電電壓VDC9至36VDC可選， 輸出方式RS-232、USB或者RS-422，可通過具體的命令協(xié)議，在上位機或者DSP軟件部分，得 到MU信息。5V2A電源與浮點型DSP模塊連接，18VDC電源與導航系統(tǒng)連接。數(shù)字信號處理器 選擇TI公司的TMS320F28335浮點DSP控制器作為硬件平臺，實時性好，精確度高，數(shù)據(jù)處理 效率高，用來控制數(shù)據(jù)采集和算法解算；仿真器使用TI公司的SEED-XDS510PLUS，用于在 CCS5.3集成開發(fā)環(huán)境內(nèi)，將算法嵌入到DSP內(nèi)，；DSP模塊與導航系統(tǒng)通過RS232通信串口 SCI 通信方式連接，上位導航計算機與導航系統(tǒng)通過RS422轉(zhuǎn)USB通信串口連接。
[0016] 本發(fā)明的優(yōu)點在于：
[0017] (1)針對需要外界輔助信息提供當?shù)氐乩頍樁刃畔⒌膶Ш江h(huán)境，本發(fā)明提出全自 主捷聯(lián)慣性導航算法。不需要外界輔助信息，僅利用系統(tǒng)自身的慣性測量單元輸出，首先以 慣性空間為參考基準，利用重力投影，確定晃動基座下載體初始時刻所在位置的煒度信息， 然后帶入計算所得煒度信息，基于雙矢量定姿原理，利用重力加速度在慣性空間方位的改 變包含地球北向信息這一特性，求解慣性系下粗略的初始姿態(tài)陣，最后在慣性系下粗略的 初始姿態(tài)陣已求得的基礎上，建立了煒度未知動態(tài)干擾條件下的精對準誤差模型，采用基 于新息的自適應濾波方法解算載體的失準角，用失準角修正捷聯(lián)姿態(tài)矩陣，有效隔離晃動 對載體初始對準的影響，完成初始對準，進入自主導航狀態(tài)。系統(tǒng)不依賴外界賴輔助信息， 僅利用系統(tǒng)自身的陀螺儀和加速度計等慣性測量器件輸出完成導航解算過程，不向外界輻 射任何能量，隱蔽性好，抗干擾性強，能夠全天時、全天候為載體提供完備的運動信息，完成 全自主導航。
[0018] (2)針對捷聯(lián)慣性導航系統(tǒng)對數(shù)據(jù)處理能力、實時性、穩(wěn)定性、小型化、低功耗的進 一步高要求，從處理器硬件平臺選擇上考慮，本發(fā)明應用TI公司的TMS320F28335浮點型DSP 控制器作為硬件平臺，來控制數(shù)據(jù)采集和算法解算，完成載體的實時快速捷聯(lián)慣性導航。系 統(tǒng)在大量的浮點運算時有很大優(yōu)勢，對復雜的算法程序執(zhí)行效率高，TMS320F28335型數(shù)字 信號處理器的高速處理能力、高精度、高性能、低成本、低功耗、外設集成度高、數(shù)據(jù)以及程 序存儲量大、32位浮點運算單元等性能在解算方面得到了充分發(fā)揮，實現(xiàn)高精確度、高數(shù)據(jù) 處理效率地在動態(tài)干擾情況計算煒度信息，有效隔離晃動對載體初始對準的影響，實現(xiàn)捷 聯(lián)慣導系統(tǒng)的實時高精度快速導航。
[0019] (3)在本發(fā)明方法的最后，算法解算姿態(tài)信息與實際姿態(tài)信息進行比較，增加了對 算法和系統(tǒng)的驗證部分，可以實時監(jiān)控并證明本系統(tǒng)的可行性和有效性，實現(xiàn)全自主捷聯(lián) 慣性導航系統(tǒng)的實時高精度快速解算。
【附圖說明】
[0020] 圖1為本發(fā)明基于DSPTMS320F28335的全自主捷聯(lián)慣性導航系統(tǒng)裝置總體結(jié)構(gòu)框 圖；
[0021] 圖2為本發(fā)明基于DSPTMS320F28335的全自主捷聯(lián)慣性導航系統(tǒng)裝置總體實物圖； [0022]圖3為本發(fā)明系統(tǒng)的流程圖；
[0023] 圖4為慣性坐標系下煒度估計示意圖；
[0024] 圖5為基于新息的自適應濾波方程計算流程示意圖；
[0025] 圖6為煒度信息仿真結(jié)果示意圖；
[0026] 圖7為煒度信息仿真結(jié)果分析示意圖；
[0027] 圖8為慣性系粗對準仿真結(jié)果示意圖；
[0028] 圖9為慣性系粗對準仿真結(jié)果分析示意圖；
[0029] 圖10為精對準仿真結(jié)果分析示意圖；
[0030] 圖11為上位機采集真實姿態(tài)信息頁面示意圖；
[0031]圖12為實驗結(jié)果示意圖。
[0032]圖13為dsp算法解算流程圖。
【具體實施方式】
[0033]本發(fā)明是基于32位浮點型DSPTMS320F28335控制數(shù)據(jù)采集和算法解算的實時快全 自主捷聯(lián)慣性導航系統(tǒng)，下面結(jié)合本發(fā)明系統(tǒng)流程圖3對本發(fā)明的具體實施步驟進行詳細 的描述：
[0034]在下文實施步驟的詳細描述中坐標系定義如下:地球坐標系e系，原點選取地球中 心，X軸位于赤道平面內(nèi)，從地心指向載體所在點經(jīng)線，Z軸沿地球自轉(zhuǎn)軸方向，隨地球自轉(zhuǎn) 而轉(zhuǎn)動，X軸、Y軸和Z軸構(gòu)成右手坐標系，隨地球自轉(zhuǎn)而轉(zhuǎn)動;地心慣性坐標系i系，是在粗對 準起始時刻將地球坐標系e系慣性凝固后形成的坐標系；導航坐標系n系，即導航基準的坐 標系，導航相關運算都在該坐標系下進行，原點位于艦載機重心，X軸指向東向（E)，Y軸指向 北向(N)，Z軸指向天向(U);載體坐標系b系，原點位于艦載機重心，X軸、Y軸、Z軸分別沿艦載 機機體橫軸指向右、沿縱軸指向前、沿立軸指向上;載體慣性坐標系ibO系，是在對準初始時 刻將載體坐標系經(jīng)慣性凝固后的坐標系。
[0035]步驟1:系統(tǒng)準備階段，硬件模塊連接，DSP上電，導航系統(tǒng)預熱；
[0036]步驟2:上位導航計算機開啟，CCS5.3集成開發(fā)環(huán)境打開，建立工程并編寫編譯算 法程序，鏈接SEED-XDS510PLUS仿真器，將算法嵌入到DSPTMS320F28335內(nèi)的軟件，DSP控制 全自主捷聯(lián)慣性導航算法運行；
[0037]步驟3: DSP初始化，完成系統(tǒng)及各個模塊的初始化工作，正確配置狀態(tài)寄存器、中 斷、引腳、看門狗等整個系統(tǒng)；
[0038]步驟4:導航算法初始化，正確設置相關參數(shù)；
[0039] 步驟5:DSPTMS320F28335控制導航系統(tǒng)，利用SCI通信，通過RS232通信串口，向?qū)?航系統(tǒng)發(fā)送命令協(xié)議"$GCCMD，OUTPUT，COMA，GTIMU，0.01*FF"，以100HZ的數(shù)據(jù)更新速率， 115200bps的波特率，采集慣性測量單元MU數(shù)據(jù)，分析并解包得到光纖陀螺儀的三軸測量 值gyout_b和加速度計的三軸測量值acout_b。其SCI通信電路原理圖如圖6;
[0040]步驟6:不依外界賴輔助信息，僅利用系統(tǒng)自身的慣性測量單元輸出，在得到光纖 陀螺儀的三軸測量值gy〇ut_b和加速度計的三軸測量值&〇〇此_13后，以慣性空間為參考基 準，采用重力投影，利用兩個不同時刻慣性坐標系下重力加速度之間的夾角求取載體所在 位置的煒度信息，隔離晃動干擾對煒度求解的影響，完成晃動基座下運載體初始時刻所在 位置煒度信息的求解過程，其具體步驟如下：
[0041 ]設起始時刻t慣性坐標系i系與載體坐標系b系重合，載體坐標系隨地球一起轉(zhuǎn) 動，轉(zhuǎn)動至時間t2，慣性坐標系沒有轉(zhuǎn)動保持不變。由于地球的轉(zhuǎn)動，重力加速度g在慣性空 間i系內(nèi)的方向投影由時刻的gUtd變成了 t2時刻的gi(t2)，設gUtO與gi(t2)之間的夾角 為0。如圖4所示，0與煒度L之間存在幾何關系，因此通過求取0可以間接地求取煒度信息L， 下面敘述具體過程。
[0042] 確定起始時刻tjP煒度估計時間t2，《 為地球坐標系e系相對于地心慣性坐標系 i系的地球自轉(zhuǎn)角速度大小，在t、t2時間差內(nèi)地球轉(zhuǎn)過的角度a可求得：
[0043] a= o ie(t2~ti) (1)
[0044]在。時刻，i系與b系重合，重力加速度g在b系內(nèi)的方向投影為ght)，重力加速度g 在慣性空間內(nèi)的方向gi ( )可求得：
[0045] g1 (ti) = gb (ti) = -acout_b (ti) (2)
[0046]在t2時刻，重力加速度g在b系內(nèi)的方向投影為gb(t2)，重力加速度g在慣性空間內(nèi) 的方向gi(t2)由下式求取：
[0047] g' (/：) =(q- (/： ))* ? gh {>：) ? q (>z) = -(q (/： )V ? ac〇m _ h{t2) ? 〇;; u2) ⑴
[0048] 式中，《疼)為〇時刻b系相對于i系的關系四元數(shù)值，(q丨(yf是的共輒， q'Q >可由下式微分方程式求得：
[0050] 式中，< 為光纖陀螺輸出值，表示b系相對于i系的載體運動角速率在b系內(nèi)的投 影。
[0051] 由⑵式求解的矢量gUti)和⑶式求解的矢量gUt2)，可以求得兩矢量gUt)與g1 (t2)之間的夾角0:
[0053]式中，?為內(nèi)積符號，| IgUtOl |表示求取gUtO的模值，| |gUt2)| |表示求取g1 (t2)的模值。
[0054] 0與煒度L之間存在幾何關系，下面通過求取0來間接地求取煒度信息L:
[0055] 由圖4可知幾何關系如下：
[0056] | Ao，| = | Bo，| = | Ao | cos L (6)
[0059]將(6)式和(7)式代入(8)式，可求得晃動基座下煒度信息L的計算公式：
[0061]將（1)式所求得角度a和(5)式所求得夾角0代入(9)式，即可求得煒度信息L，隔離 了晃動干擾的影響，完成了晃動基座下煒度值的估計過程。
[0062] 步驟7:不依外界賴輔助信息，帶入計算所得煒度信息，利用系統(tǒng)自身的慣性測量 單元輸出，基于雙矢量定姿原理，利用重力加速度在慣性空間方位的改變包含地球北向信 息這一特性，求解慣性系下粗略的初始姿態(tài)陣，姿態(tài)誤差角可視為小角，接著采用基于新息 的自適應濾波方法獲取載體的失準角，用失準角修正捷聯(lián)姿態(tài)矩陣，得到三軸姿態(tài)信息航 向角L俯仰角9、橫滾角Y，實時記錄數(shù)據(jù)，完成初始對準，進入全自主導航狀態(tài)。其具體步 驟如下：
[0063] 1)載體所在位置的煒度信息計算完成后，利用計算所得煒度值，進入晃動基座初 始對準粗對準過程，以慣性空間為參考基準，利用雙矢量定姿算法計算出的初始姿態(tài)陣粗 略值，完成粗對準步驟。
[0064] 在慣性系下捷聯(lián)慣導系統(tǒng)的自對準粗對準算法中，以慣性空間為參考基準，導航 坐標系n系與載體坐標系b系之間的初始姿態(tài)矩陣的實現(xiàn)分解成和常 值矩陣三部分的求解過程，姿態(tài)矩陣的分解表達式為：
[_] q(，) <(0(〇(，） （動
[0066] 其中，導航坐標系n系與地心慣性坐標系i系之間的轉(zhuǎn)換矩陣由所 求得的運載體所在對準點的地理位置煒度信息和初始對準時間確定：
[0067] 導航坐標系n系與地球坐標系e系之間的轉(zhuǎn)換矩陣：
[0069]地球坐標系e系與地心慣性坐標系i系之間的轉(zhuǎn)換矩陣：
[0071] 載體坐標系b系與載體慣性坐標系為ibO系之間的轉(zhuǎn)換矩陣⑷的初值 = 其更新利用陀螺測量輸出角速度信息<(〇跟蹤求解，且其更新求解過程隱含了 對角運動的隔離：
[0072] =C^[(0l(t)x\ (|3)
[0073] 式中，[4(加]為向量<(〇的叉乘反對稱矩陣。
[0074] 地心慣性坐標系i系與載體慣性坐標系ibO系之間的轉(zhuǎn)動關系Ct是不隨時間變化 的常值矩陣，
[0075] 其確定過程是姿態(tài)矩陣 估算問題的關鍵，是初始對準的核心。
[0076] 因為地心慣性坐標系i系內(nèi)的速度投影V1與載體慣性坐標系ibO系內(nèi)的速度投影 r-有關系式^，因此cf9可根據(jù)速度在兩慣性坐標系中的投影v1和構(gòu)造矢量 實現(xiàn)計算。在兩個不同時刻，根據(jù)i系下的重力加速度的積分V1和ibO系下的重力加速度的 積分rM1分別構(gòu)建矢量，采用雙矢量定姿法求解矩陣求逆得到C:。具體求解過程如下：
[0077]設n系下的重力加速度值為gn，i系下的重力加速度值g1為：
[0079] 設A tk = tk-to,積分得i系下的速度投影VS
[0081] 在粗對準過程中忽略加速度計的刻度系數(shù)誤差和安裝誤差角，如3財_6包含重力 加速度gb、載體震蕩縱蕩橫蕩引起的干擾加速度桿臂干擾加速度^4如下式所示： 、 ?
[0084]則ibQ坐標系下陀螺輸出積分所得速度的投影為：
[0086]由于為線振動干擾速度，與重力加速度積分的速度相比較小，粗對準過程中 可以將其忽略。同理，由煒度誤差引起的重力加速度誤差引起的誤差可以忽略，故求解ib〇系 下的速度的公式可化簡為：
[0088]在兩個不同時刻，根據(jù)式（15)求得i系下的速度投影V1，根據(jù)式（17)和式（18)求得 ib〇系下的速度"。后，分別構(gòu)建矢量，采用雙矢量定姿法求解gQ。
[0089] 首先，在tkl、tk2時刻（切〈七1^1〈七1^2)有：
[0090] =
[0091] r卞2) = c;f (，,f2) :(19):
[0092] 構(gòu)造矩陣：
[0094] 由此，采用雙矢量定姿法求解過程完成。將式(11)式(12)所得C(t^式(13)所 得々u(t\式(20)所得C|fl代入式（10)計算，即可計算出G，完成慣性系下捷聯(lián)慣導系統(tǒng)的 粗對準對準過程，得到粗略的初始姿態(tài)陣。
[0095] 2)通過上述粗對準方法計算出姿態(tài)陣的粗略值，姿態(tài)誤差角可視為小角，在此基 礎上利用基于新息的自適應濾波獲取載體的失準角，用失準角修正捷聯(lián)姿態(tài)矩陣，對姿態(tài) 陣做精確估計以進一步提高對準精度，結(jié)束自對準，進入自主導航狀態(tài)。
[0096] 在精對準階段，姿態(tài)矩陣C((〇分為3個矩陣求取，如下所示：
[0097] ci{t)=c：q{twb{t) m)
[0098] 式中，C：和按照上述粗對準方案中的計算方法進行求取。因此的確定 關鍵在于地心慣性坐標系i系與載體坐標系b系之間的轉(zhuǎn)換矩陣C;,(/)的確定。
[0099] 設i'系是由陀螺輸出計算得到的存在失準角的慣性坐標系，i系是真實的慣性坐 標系。由于慣性器件存在各種誤差，i'系和i系并不重合，設i'系與i系之間的失準角為#， 其三軸值分別表示為<和?6i'系與i系之間的轉(zhuǎn)換關系為<：,:(〇，1'系與13系之間的轉(zhuǎn) 換矩陣為G 0) 〇?
[0102] 其中，Ct'(0)是通過粗對準過程建立的載體坐標系與計算慣性坐標系之間的轉(zhuǎn)換 矩陣，作為精對準開始時的轉(zhuǎn)換矩陣。ACi根據(jù)陀螺和加速度計輸出通過四元數(shù)算法求得。 因此，的確定關鍵在于從i系到i'系的轉(zhuǎn)換矩陣的求取，也就是說關鍵在于i'系與i系 之間的失準角#的求取。
[0103] 針對上述所需過程，在這一步驟中，首先建立系統(tǒng)狀態(tài)方程和量測方程，創(chuàng)建系統(tǒng) 狀態(tài)空間模型，利用基于新息的自適應濾波器精確估計載體的失準角#，然后用失準角以修 正捷聯(lián)姿態(tài)矩陣，計算準確地姿態(tài)矩陣，完成自對準的精對準過程，進入自主導航狀態(tài)。
[0104] ①建立系統(tǒng)狀態(tài)方程
[0105] 在精對準過程中，加速度計的刻度系數(shù)誤差和安裝誤差不可忽略。設加表常值偏 置向量= + Vb為加速度計的常值偏置誤差，其三軸值分別為\^和Vfc， 為高斯白噪聲，其三軸值分別為％?、Vw>1和D加表刻度系數(shù)誤差矩陣為SKA = diag(S KAx，SKAy，SKAz)，加表安裝誤差矩陣.
，則加速度計輸出的比力 在V系中的投影為：
[0106] Qacoul_b = Cf[(/ +^,)(/ + 4, +?0) +Vb] (24)
[0107] 忽略二階小項之后，可得：
[0109]設& = 4 + <4 + 44，為等效干擾加速度。將上式整理可 得：
[0111]對上式積分，可得：
[0118] 設慣性系i系下的速度誤差為SVSSV1的三軸值分別為#；⑴、和^^⑴，由 式(32)得到慣性系下的速度誤差方程：
[0119] sf:(t)=-ig; it )x]^ (/)+q (0 v;, -t- (; (n v w (33)
[0120] 設陀螺的常值漂移和量測高斯白噪聲分別為一和co b，eb的三軸值分別表示為 44和<且礦=〇,《 b的三軸值分別表示為<、<和<。由姿態(tài)轉(zhuǎn)換陣Ci和C丨' 的微分方 程，得到慣性系下的失準角方程：
[0121] m=-Q{t)sh-Q(t)G/ (34)
[0122] 根據(jù)慣性系下的速度誤差方程和失準角方程，系統(tǒng)的狀態(tài)方程是如下形式：
[0123] X{() = A(t)X(n + B(t)lV (35)
[0132] ②建立系統(tǒng)量測方程
[0133] 設量測矢量表示為：
[0134] Z = P F』-% =5F!+Fw+d>= (4〇;
[0135] 根據(jù)式(27)至式(32)，得到系統(tǒng)的量測方程：
[0136] Z⑴：-/認(/) +匕十汾(41)
[0137] 式中，量測矩陣為H=[I3X3 03x9];#g為不確定性量測干擾，主要由垂蕩、縱蕩、橫 蕩產(chǎn)生的干擾速度構(gòu)成;Vw為量測高斯白噪聲。
[0138] ③基于新息的自適應濾波算法
[0139] 系統(tǒng)狀態(tài)方程和量測方程完成求解，由（35)時和(41)式，系統(tǒng)的狀態(tài)空間模型也 就建立了。系統(tǒng)空間模型離散化后的形式如下：
[0141 ]式中，Xk為k時刻的狀態(tài)矢量;Zk為k時刻的量測矢量;FKK-Ak-1時刻到k時刻的一 步轉(zhuǎn)移矩陣；r^Sk-i時刻的系統(tǒng)噪聲矩陣;Hk為k時刻的量測矩陣;Wk-dPVk為隨機干擾的 白噪聲;Uk為不確定性量測干擾。
[0142] 經(jīng)典卡爾曼濾波采用新息的理論協(xié)方差計算濾波增益矩陣Kk，但是新息的理論協(xié) 方差無法直接反映出外部量測噪聲的變化，所以該算法使用新息的實際協(xié)方差代替理論協(xié) 方差來計算濾波增益矩陣Kk。
[0143] 對新息％ 進行加權(quán)求和：
[0145] 由極大似然估計可知系統(tǒng)噪聲協(xié)方差矩陣Rk的估計值為：
[0146] 4 = "/"I (My
[0147] 用此處Rk的估計值代替經(jīng)典卡爾曼濾波公式中的Rk，可得到：
[0148] Kk = Pk\i.-iHlCh
[0149] 由于新息的不穩(wěn)定性，進一步對其采用自適應的方式進行計算：
[0151] 式中a為調(diào)節(jié)因子，用于調(diào)節(jié)新的新息在新息加權(quán)求和過程中所占的比例，可根據(jù) 經(jīng)驗選擇默認值。當新的新息比新息邊界大的時候，認為此次的新息誤差過大，降低其比例 降低a，反之a(chǎn)采用默認值，以此抑制濾波器發(fā)散。
[0152] a的計算方法如下：
[0154] 其中G為允許的誤差邊界。從公式可以看出，超過誤差允許邊界，距離越遠，a越 小，則新的新息在計算過程中所占比例越小。
[0155] 結(jié)合圖5可知，系統(tǒng)的基于新息的自適應濾波方程如下：
[0163]新息的計算方程:匕=石(48h)
[0164] 利用基于新息的自適應濾波可估計出系統(tǒng)狀態(tài)矢量，系統(tǒng)狀態(tài)矢量的第四項、第 五項、第六項即為載體的失準角<(/)、0)、<(十
[0165] ④用失準角修正捷聯(lián)姿態(tài)矩陣
[0166] 從i系到i'系的轉(zhuǎn)換矩陣：
[0168] Cf代入(22)式求得C；U)，€：、€(〇與代入(21)式求得精確的<=(〇,完成 了用失準角修正捷聯(lián)姿態(tài)矩陣，對姿態(tài)矩陣做了精確估計，結(jié)束了自對準，進入自主導航狀 〇
[0169] 步驟8:終止程序，關閉系統(tǒng)。
[0170] 本發(fā)明的有益效果如下：
[0171] 本發(fā)明首先在仿真環(huán)境下對全自主捷聯(lián)慣性導航算法進行仿真實驗，證明該算法 的有效性。然后通過真實實驗對本發(fā)明提出的基于DSPTMS320F28335的全自主捷聯(lián)慣性導 航系統(tǒng)進行驗證，實驗結(jié)果與實際載體姿態(tài)信息對比，證明本系統(tǒng)的可行性和有效性，證明 本發(fā)明實時性好，精確度高，數(shù)據(jù)處理效率高，能有效在動態(tài)干擾情況下計算煒度信息，能 有效隔離晃動對載體初始對準的影響，有效實現(xiàn)捷聯(lián)慣性導航系統(tǒng)的實時快速全自主導 航。
[0172] (1)在仿真環(huán)境下對該算法進行仿真實驗，驗證算法。
[0173]首先進行仿真環(huán)境設置：
[0174]①設載體受到外界擾動，其航向角iK俯仰角0、橫滾角Y作周期性變化，如下：
[0176]②存在橫蕩、縱蕩和垂蕩引起的線速度：
[0178] Adx = 0 ? 02m，ADy = 0 ? 03m，ADz = 0? 3m; ?Di = 2Ji/TDi，且Tdx = 7s，TDy = 6s，Tdz = 8s ;供a 為[0,23i]上服從均勻分布的隨機相位；
[0179]③假設陀螺三個方向軸上的隨機常值漂移為0.01°/h，陀螺的隨機游走系數(shù)為 o.oor/V^.加速度計三個方向軸上的常值偏差為1 x io-4g，加速度計量測白噪聲標準差為 1 / 1()〇*'^：
[0180]④常數(shù)設置
[0181] 赤道半徑為Re = 6378165.0m，橢球扁率為e = 3.352e-3，由萬有引力可得地球表面 重力加速度為g〇 = 9 ? 78049，地球自轉(zhuǎn)角速度是單位是7 ? 2921158e-5;常數(shù)31 = 3 ? 1415926
[0182] 算法仿真結(jié)果如下：
[0183] 煒度信息真值為北煒39.78度。維度信息仿真估計進行了十次，每次煒度估計仿真 時間為ll〇s，且以取4〇8，^取11〇 8。仿真結(jié)果如圖6所示。統(tǒng)計出十次煒度估計仿真實驗對應 煒度誤差的均值、最大值、最小值，如圖7所示。可以清楚地看出，該煒度估計結(jié)果滿足誤差 要求，可為后續(xù)自對準提供初始煒度信息。
[0184] 粗對準仿真采用慣性系粗對準算法，進行了 50次，每次粗對準仿真時間為80s，且 tkl取40s，tk2取80s。仿真結(jié)果如圖8所示。以粗對準結(jié)束時刻的姿態(tài)誤差角<(0、<的、 作為衡量粗對準精度的指標，統(tǒng)計出五十次粗對準仿真實驗對應的姿態(tài)誤差角的均值、最 大值和最小值，如圖9所示?？梢郧宄乜闯?，航向姿態(tài)誤差小于0.76度，俯仰姿態(tài)誤差小于 0.23度，橫滾姿態(tài)誤差小于0.1度，該粗對準的結(jié)果滿足了精對準的要求，可為后續(xù)精對準 提供初始姿態(tài)。
[0185] 進行了 840s精對準仿真，精對準仿真結(jié)果如圖10所示。以姿態(tài)誤差角的估計誤差 <(〇、<(〇、&⑴作為衡量精對準的指標。可以看得出，俯仰姿態(tài)在60s內(nèi)完成精對準，穩(wěn) 定在0.07度;橫滾姿態(tài)在60s內(nèi)完成精對準，穩(wěn)定在0.045度;航向姿態(tài)在210s內(nèi)完成精對 準，精度為〇.〇9度以內(nèi)。
[0186] 由仿真結(jié)果可知，在動態(tài)干擾情況下，不依賴任何外界輔助信息，本算法可以自主 估計出載體所在位置的煒度信息值，自主解算精度要求范圍內(nèi)的粗略初始姿態(tài)陣，快速完 成精對準過程，完成全自主自對準，精度達到了要求，進入自主導航狀態(tài)。
[0187] (2)通過真實實驗對本發(fā)明提出的基于DSPTMS320F28335的全自主捷聯(lián)慣性導航 系統(tǒng)進行驗證。真實試驗中，不提任何供外界輔助信息，系統(tǒng)裝置放置在車上，有人員上下 車、開關車門、對車進行晃動等干擾。實驗歷時980s，試驗地點在北京工業(yè)大學羽毛球館南 廣場位置。上位導航計算機控制導航系統(tǒng)，通過RS422轉(zhuǎn)USB串口，向?qū)Ш较到y(tǒng)發(fā)送命令協(xié)議 "$GCCMD，OUTPUT，COMC，GPFPD，0.01*FF"，以 100HZ的數(shù)據(jù)更新速率，115200bps的波特率，采 集航向精度達〇. 1度、姿態(tài)精度達〇. 05度的實際三軸姿態(tài)信息，其上位機采集頁面示意圖如 圖11。解算獲得的載體姿態(tài)信息與本步驟中得到的高精度真實載體姿態(tài)信息做比較，證明 本算法和系統(tǒng)的可行性和有效性。
[0188] 系統(tǒng)實驗結(jié)果如下：
[0189] 煒度信息真值為北煒39.8709度。維度信息估計在120s內(nèi)完成，以取4〇8，t2取120s。 仿真估計值為北煒39.706790度，仿真估計誤差為0.164110度，在誤差允許范圍內(nèi)，可為后 續(xù)自對準提供初始煒度信息。
[0190] 粗對準過程采用慣性系粗對準算法，歷時80s，tkl取40s，tk2取80s。粗對準實驗結(jié) 果為航向姿態(tài)180.122500度、俯仰姿態(tài)0.513390度、橫滾姿態(tài)0.082630度，此時刻真實姿態(tài) 信息輸出值為航向姿態(tài)179.87度、俯仰姿態(tài)0.40度、橫滾姿態(tài)0.21度，以姿態(tài)誤差角的估計 誤差作為衡量精對準的指標，航向姿態(tài)誤差小于0.252500度，俯仰姿態(tài)誤差小于0.113390 度，橫滾姿態(tài)誤差小于0.12737度，該粗對準的結(jié)果滿足了精對準的要求，可為后續(xù)精對準 提供初始姿態(tài)。
[0191] 精對準過程歷時780s，結(jié)果如圖12。以姿態(tài)誤差角的估計誤差作為衡量精對準的 指標?？梢钥吹贸?，俯仰姿態(tài)、橫滾姿態(tài)和航向姿態(tài)均快速完成對準工作，俯仰姿態(tài)精度達 到〇. 06度，橫滾姿態(tài)精度達到0.07度，航向姿態(tài)精度達到0.15度，收斂速度、穩(wěn)態(tài)誤差均符 合要求，完成自對準過程，進入自主導航狀態(tài)。
[0192] 實驗結(jié)果表明：不依賴任何外界輔助信息的情況下，針對導航系統(tǒng)對數(shù)據(jù)處理能 力、實時性、穩(wěn)定性、小型化、低功耗的進一步高要求，本發(fā)明基于DSPTMS320F28335的全自 主捷聯(lián)慣性導航系統(tǒng)僅利用自身的慣性測量單元輸出，能自主有效在動態(tài)干擾情況下計算 煒度信息，能自主有效隔離晃動對載體初始對準的影響，能自主有效實現(xiàn)捷聯(lián)慣性導航系 統(tǒng)的實時高精度快速自對準，進入全自主導航狀態(tài)。
【主權(quán)項】
1.基于DSPTMS320F28335的全自主捷聯(lián)慣性導航系統(tǒng)，其特征在于： 本發(fā)明使用基于DSPTMS320F28335的全自主捷聯(lián)慣性導航系統(tǒng)，系統(tǒng)裝置包括:浮點型 DSPTMS320F28335、SEED-XDS510PLUS仿真器、光纖陀螺、加速度計、SCI通信電路、RS232及 RS422轉(zhuǎn)USB通信串口、上位導航計算機、數(shù)據(jù)電纜部分、電源；以DSPTMS320F28335作為控制 器，將全自主捷聯(lián)慣性導航算法嵌入到浮點型DSP內(nèi)的軟件實現(xiàn)，實時采集、分析慣性測量 單元IMU中光纖陀螺和加速度計的輸出信號并控制算法運行，以保證全自主捷聯(lián)慣性導航 系統(tǒng)的高速處理能力、高實時性、高精度、高性能、低成本、低功耗;慣性測量MU單元中，光 纖陀螺儀的零點偏置穩(wěn)定性是〇. 5度/h，零點偏置重復性是0.5度/h，加速度計的零點偏置 小于lmg，零點偏置穩(wěn)定性小于lmg，數(shù)據(jù)更新速率100HZ，波特率115200bps;數(shù)據(jù)電纜部分， 供電電壓VDC9至36VDC可選，輸出方式RS-232、USB或者RS-422，可通過具體的命令協(xié)議，在 上位機或者DSP軟件部分，得到MU信息；5V2A電源與浮點型DSP模塊連接，18VDC電源與導航 系統(tǒng)連接;數(shù)字信號處理器選擇TI公司的TMS320F28335浮點DSP控制器作為硬件平臺，實時 性好，精確度高，數(shù)據(jù)處理效率高，用來控制數(shù)據(jù)采集和算法解算；仿真器使用TI公司的 SEED-XDS510PLUS，用于在CCS5.3集成開發(fā)環(huán)境內(nèi)，將算法嵌入到DSP內(nèi)，；DSP模塊與導航系 統(tǒng)通過RS232通信串口 SCI通信方式連接，上位導航計算機與導航系統(tǒng)通過RS422轉(zhuǎn)USB通信 串口連接； 坐標系定義如下：地球坐標系e系，原點選取地球中心，X軸位于赤道平面內(nèi)，從地心指 向載體所在點經(jīng)線，Z軸沿地球自轉(zhuǎn)軸方向，隨地球自轉(zhuǎn)而轉(zhuǎn)動，X軸、Y軸和Z軸構(gòu)成右手坐 標系，隨地球自轉(zhuǎn)而轉(zhuǎn)動;地心慣性坐標系i系，是在粗對準起始時刻將地球坐標系e系慣性 凝固后形成的坐標系；導航坐標系n系，即導航基準的坐標系，導航相關運算都在該坐標系 下進行，原點位于艦載機重心，X軸指向東向(E)，Y軸指向北向（N)，Z軸指向天向（U);載體坐 標系b系，原點位于艦載機重心，X軸、Y軸、Z軸分別沿艦載機機體橫軸指向右、沿縱軸指向 前、沿立軸指向上;載體慣性坐標系ibO系，是在對準初始時刻將載體坐標系經(jīng)慣性凝固后 的坐標系； 步驟1:系統(tǒng)準備階段，硬件模塊連接，DSP上電，導航系統(tǒng)預熱； 步驟2:上位導航計算機開啟，CCS5.3集成開發(fā)環(huán)境打開，建立工程并編寫編譯算法程 序，鏈接SEED-XDS510PLUS仿真器，將算法嵌入到DSPTMS320F28335內(nèi)的軟件，DSP控制全自 主捷聯(lián)慣性導航算法運行； 步驟3:DSP初始化，完成系統(tǒng)及各個模塊的初始化工作，正確配置狀態(tài)寄存器、中斷、弓丨 腳、看門狗等整個系統(tǒng)； 步驟4:導航算法初始化，正確設置相關參數(shù)； 步驟5:DSPTMS320F28335控制導航系統(tǒng)，利用SCI通信，通過RS232通信串口，向?qū)Ш较?統(tǒng)發(fā)送命令協(xié)議"$GCCMD，OUTPUT，COMA，GTIMU，0.01*FF"，以100HZ的數(shù)據(jù)更新速率， 115200bps的波特率，采集慣性測量單元MU數(shù)據(jù)，分析并解包得到光纖陀螺儀的三軸測量 值gyout_b和加速度計的三軸測量值acout_b; 步驟6:不依外界賴輔助信息，僅利用系統(tǒng)自身的慣性測量單元輸出，在得到光纖陀螺 儀的三軸測量值gyout_b和加速度計的三軸測量值8(3〇此_13后，以慣性空間為參考基準，采 用重力投影，利用兩個不同時刻慣性坐標系下重力加速度之間的夾角求取載體所在位置的 煒度信息，隔離晃動干擾對煒度求解的影響，完成晃動基座下運載體初始時刻所在位置煒 度信息的求解過程，其具體步驟如下： 設起始時刻ti慣性坐標系i系與載體坐標系b系重合，載體坐標系隨地球一起轉(zhuǎn)動，轉(zhuǎn)動 至時間t2，慣性坐標系沒有轉(zhuǎn)動保持不變；由于地球的轉(zhuǎn)動，重力加速度g在慣性空間i系內(nèi) 的方向投影由乜時刻的gi(t〇變成了 t2時刻的gi(t2)，設gi(t〇與gi(t2)之間的夾角為0;0與 煒度L之間存在幾何關系，因此通過求取0可以間接地求取煒度信息L，下面敘述具體過程； 確定起始時刻以和煒度估計時間t 2，CO i(5為地球坐標系e系相對于地心慣性坐標系i系的 地球自轉(zhuǎn)角速度大小，在ti、t 2時間差內(nèi)地球轉(zhuǎn)過的角度a可求得： a=?ie(t2_tl) (1) 在。時刻，i系與b系重合，重力加速度g在b系內(nèi)的方向投影為ghti)，重力加速度g在慣 性空間內(nèi)的方向gi ( ti )可求得： g1 (ti) = gb (ti) = -acout_b (ti) (2) 在t2時刻，重力加速度g在b系內(nèi)的方向投影為gb(t2)，重力加速度g在慣性空間內(nèi)的方 向gVts)由下式求取：式中，q>2)St2時刻b系相對于i系的關系四元數(shù)值，(q〖(r2)f是q冰)的共輒，q> 2)可由 下式微分方程式求得：式中，< 為光纖陀螺輸出值，表示b系相對于i系的載體運動角速率在b系內(nèi)的投影； 由（2)式求解的矢量gUtd和(3)式求解的矢量gUt2)，求得兩矢量gUtd與gUt2)之間 的夾角9:式中，?為內(nèi)積符號，I IgUtdl |表示求取gUtO的模值，| |gUt2)| |表示求取gVts)的 模值； 9與煒度L之間存在幾何關系，下面通過求取9來間接地求取煒度信息L: 幾何關系如下： |Ao '| = |Bo '| = |Ao|cosL(6)將(6)式和(7)式代入(8)式，可求得晃動基座下煒度信息L的計算公式：將（1)式所求得角度a和（5)式所求得夾角0代入(9)式，即可求得煒度信息L，隔離了晃 動干擾的影響，完成了晃動基座下煒度值的估計過程； 步驟7:不依外界賴輔助信息，帶入計算所得煒度信息，利用系統(tǒng)自身的慣性測量單元 輸出，基于雙矢量定姿原理，利用重力加速度在慣性空間方位的改變包含地球北向信息這 一特性，求解慣性系下粗略的初始姿態(tài)陣，姿態(tài)誤差角可視為小角，接著采用基于新息的自 適應濾波方法獲取載體的失準角，用失準角修正捷聯(lián)姿態(tài)矩陣，得到三軸姿態(tài)信息航向角 也、俯仰角9、橫滾角Y，實時記錄數(shù)據(jù)，完成初始對準，進入全自主導航狀態(tài);其具體步驟如 下： 1)載體所在位置的煒度信息計算完成后，利用計算所得煒度值，進入晃動基座初始對 準粗對準過程，以慣性空間為參考基準，利用雙矢量定姿算法計算出的初始姿態(tài)陣粗略值， 完成粗對準步驟； 在慣性系下捷聯(lián)慣導系統(tǒng)的自對準粗對準算法中，以慣性空間為參考基準，導航坐標 系n系與載體坐標系b系之間的初始姿態(tài)矩陣的實現(xiàn)分解成⑴和常值矩 陣三部分的求解過程，姿態(tài)矩陣的分解表達式為：其中，導航坐標系n系與地心慣性坐標系i系之間的轉(zhuǎn)換矩陣G _由所求得 的運載體所在對準點的地理位置煒度信息和初始對準時間確定： 導航坐標系n系與地球坐標系e系之間的轉(zhuǎn)換矩陣：地球坐標系e系與地心慣性坐標系i系之間的轉(zhuǎn)換矩陣：載體坐標系b系與載體慣性坐標系為ibO系之間的轉(zhuǎn)換矩陣的初值= 其 更新利用陀螺測量輸出角速度信息⑴跟蹤求解，且其更新求解過程隱含了對角運動的隔 離：式中，[4(〇x]為向量的叉乘反對稱矩陣； 地心慣性坐標系i系與載體慣性坐標系ib〇系之間的轉(zhuǎn)動關系是不隨時間變化的常 值矩陣，其確定過程是姿態(tài)矩陣:C|估算問題的關鍵，是初始對準的核心； 因為地心慣性坐標系i系內(nèi)的速度投影V1與載體慣性坐標系ibO系內(nèi)的速度投。有 關系式，因此Cf°可根據(jù)速度在兩慣性坐標系中的投影V1和F~構(gòu)造矢量實現(xiàn)計 算;在兩個不同時刻，根據(jù)i系下的重力加速度的積分V1和ibO系下的重力加速度的積分 分別構(gòu)建矢量，采用雙矢量定姿法求解C,ib°矩陣求逆得到^^ .具體求解過程如下： 設n系下的重力加速度值為gn，i系下的重力加速度值g1為：設A tk = tk-to，積分得i系下的速度投影V1:在粗對準過程中忽略加速度計的刻度系數(shù)誤差和安裝誤差角包含重力加速 度813、載體震蕩縱蕩橫蕩引起的干擾加速度、桿臂干擾加速度^4，如下式所示：則ib〇坐標系下陀螺輸出積分所得速度的投影為：由于為線振動干擾速度，與重力加速度積分的速度相比較小，粗對準過程中可以 將其忽略；同理，由煒度誤差引起的重力加速度誤差引起的誤差可以忽略，故求解ib〇系下的 速度的公式可化簡為：在兩個不同時刻，根據(jù)式（15)求得i系下的速度投影V1，根據(jù)式（17)和式（18)求得ib0系 下的速度后，分別構(gòu)建矢量，采用雙矢量定姿法求解 首先，在 tkl、tk2 時刻（t0〈tkl〈tk2)有： r-{tkl)=crv,-{tkl) vi-(til)=crvi{ti2) 值) 構(gòu)造矩陣：由此，采用雙矢量定姿法求解clM過程完成；將式（li)式（12)所得式（13)所得 式(20)所得代入式（10)計算，即可計算出G，完成慣性系下捷聯(lián)慣導系統(tǒng)的粗 對準對準過程，得到粗略的初始姿態(tài)陣； 2)通過上述粗對準方法計算出姿態(tài)陣的粗略值，姿態(tài)誤差角可視為小角，在此基礎上 利用基于新息的自適應濾波獲取載體的失準角，用失準角修正捷聯(lián)姿態(tài)矩陣，對姿態(tài)陣做 精確估計以進一步提高對準精度，結(jié)束自對準，進入自主導航狀態(tài)； 在精對準階段，姿態(tài)矩陣C/1 /)分為3個矩陣求取，如下所示： Cl(t) = C：C：{t)Ch^ 式中，和CTW按照上述粗對準方案中的計算方法進行求??；因此，C7⑴的確定關鍵 在于地心慣性坐標系i系與載體坐標系b系之間的轉(zhuǎn)換矩陣q⑴的確定； 設i '系是由陀螺輸出計算得到的存在失準角的慣性坐標系，i系是真實的慣性坐標系； 由于慣性器件存在各種誤差，i'系和i系并不重合，設i'系與i系之間的失準角為￥，其三軸 值分別表示為成、<和€，i'系與i系之間的轉(zhuǎn)換關系為Cf的，i'系與b系之間的轉(zhuǎn)換矩陣 為(:⑴：其中，ct'(〇)是通過粗對準過程建立的載體坐標系與計算慣性坐標系之間的轉(zhuǎn)換矩陣， 作為 精對準開始時的轉(zhuǎn)換矩陣；AC【根據(jù)陀螺和加速度計輸出通過四元數(shù)算法求得；因此， <：,;(/)的確定關鍵在于從i系到i'系的轉(zhuǎn)換矩陣的求取，也就是說關鍵在于i'系與i系之間的 失準角#的求??； 針對上述所需過程，在這一步驟中，首先建立系統(tǒng)狀態(tài)方程和量測方程，創(chuàng)建系統(tǒng)狀態(tài) 空間模型，利用基于新息的自適應濾波器精確估計載體的失準角#，然后用失準角￥修正 捷聯(lián)姿態(tài)矩陣，計算準確地姿態(tài)矩陣，完成自對準的精對準過程，進入自主導航狀態(tài)； ①建立系統(tǒng)狀態(tài)方程 在精對準過程中，加速度計的刻度系數(shù)誤差和安裝誤差不可忽略；設加表常值偏置向 量v6=vb+vw，vb為加速度計的常值偏置誤差，其三軸值分別為I、，和，▽: w為高 斯白噪聲，其三軸值分別為I、和;加表刻度系數(shù)誤差矩陣為SKA=diag(SK Ax，S KAy，SKAz)，加表安裝誤差矩陣為則加速度計輸出的比力在V系中的投影為：忽略二階小項之后，可得：設dV +心!)(-y + + 為等效干擾加速度;將上式整理可得：對上式積分，可得：設慣性系i系下的速度誤差為SVSSV1的三軸值分別為和&<(〇,由式 (32)得到慣性系下的速度誤差方程：設陀螺的常值漂移和量測高斯白噪聲分別為^和^13，^的三軸值分別表示為4、<和 <且礦=0、《b的三軸值分別表示為4 <和烤.由姿態(tài)轉(zhuǎn)換陣C;和C丨:的微分方程，得到 慣性系下的失準角方程： <p'{t) = -Ch{t)sb -Cl(t)a>h ('4) 根據(jù)慣性系下的速度誤差方程和失準角方程，系統(tǒng)的狀態(tài)方程是如下形式： 式中，狀態(tài)矢量設為： 系統(tǒng)噪聲是：② 建立系統(tǒng)量測方程 設量測矢量表示為：根據(jù)式(27)至式(32 )，得到系統(tǒng)的量測方程： Z(t) = IIX(l) + V, +SV：, (4]) 式中，量測矩陣為H= [ 13X3 〇3X9] ; 為不確定性量測干擾，主要由垂蕩、縱蕩、橫蕩產(chǎn) 生的干擾速度構(gòu)成;Vw為量測高斯白噪聲； ③ 基于新息的自適應濾波算法 系統(tǒng)狀態(tài)方程和量測方程完成求解，由（35)時和(41)式，系統(tǒng)的狀態(tài)空間模型也就建 立了；系統(tǒng)空間模型離散化后的形式如下：式中，Xk為k時刻的狀態(tài)矢量;Zk為k時刻的量測矢量;FKK-Ak-1時刻到k時刻的一步轉(zhuǎn) 移矩陣；r k-Ak-l時刻的系統(tǒng)噪聲矩陣;Hk為k時刻的量測矩陣;Wk-jPVk為隨機干擾的白噪 聲;Uk為不確定性量測干擾； 經(jīng)典卡爾曼濾波采用新息的理論協(xié)方差計算濾波增益矩陣Kk，但是新息的理論協(xié)方差 無法直接反映出外部量測噪聲的變化，所以該算法使用新息的實際協(xié)方差代替理論協(xié)方差 來計算濾波增益矩陣Kk; 對新息K =4 -巧兄M進行加權(quán)求和：由極大似然估計可知系統(tǒng)噪聲協(xié)方差矩陣Rk的估計值為： 鳥=凡-/( 44 ) 用此處Rk的估計值代替經(jīng)典卡爾曼濾波公式中的Rk，可得到： K^P^HlC^ (45) 由于新息的不穩(wěn)定性，進一步對其采用自適應的方式進行計算：式中a為調(diào)節(jié)因子，用于調(diào)節(jié)新的新息在新息加權(quán)求和過程中所占的比例，可根據(jù)經(jīng)驗 選擇默認值;當新的新息比新息邊界大的時候，認為此次的新息誤差過大，降低其比例降低 a，反之a(chǎn)采用默認值，以此抑制濾波器發(fā)散； a的計算方法如下：其中G為允許的誤差邊界;從公式可以看出，超過誤差允許邊界，距離越遠，a越小，則 新的新息在計算過程中所占比例越??； 系統(tǒng)的基于新息的自適應濾波方程如下： 狀態(tài)一步預測方程：^ kxjn (48 a) 均方誤差一步預測方程+込-！ (48b) 濾波增益方程：A = Ax-1凡-(I. (48c) 狀態(tài)估計方程：尤:--1 (48d) 均方誤差估計方程:Pk = (I-KkHk ) PKK-i (48e)新息的計算方程:K = Za--(48b)： 利用基于新息的自適應濾波可估計出系統(tǒng)狀態(tài)矢量，系統(tǒng)狀態(tài)矢量的第四項、第五項、 第六項即為載體的失準角#(〇、<(〇、穴(乂 ④用失準角修正捷聯(lián)姿態(tài)矩陣 從i系到i'系的轉(zhuǎn)換矩陣：Cf代入(22)式求得4(〇, C：、與(7丨(〇代入(21)式求得精確的€：(〇,完成了用失 準角修正捷聯(lián)姿態(tài)矩陣，對姿態(tài)矩陣做了精確估計，結(jié)束了自對準，進入自主導航狀態(tài)； 步驟8:終止程序，關閉系統(tǒng)。
【文檔編號】G01C21/16GK106052686SQ201610540172
【公開日】2016年10月26日
【申請日】2016年7月10日
【發(fā)明人】裴?？? 智巖, 梁青琳, 魏曉麗
【申請人】北京工業(yè)大學