基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法
【專利摘要】本發(fā)明公開一種基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法，其特征在于，該導(dǎo)航方法應(yīng)用于一基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航系統(tǒng)，該導(dǎo)航系統(tǒng)包括多普勒計(jì)程儀、捷聯(lián)慣性測(cè)量單元、捷聯(lián)慣性測(cè)量單元處理模塊和中央處理單元；所述的多普勒計(jì)程儀包括收發(fā)器、四個(gè)換能器和接口單元；所述多普勒計(jì)程儀通過該接口單元與中央處理單元相連接；所述捷聯(lián)慣性測(cè)量單元包括三軸陀螺和三軸加速度計(jì)；所述捷聯(lián)慣性測(cè)量單元與捷聯(lián)慣性測(cè)量單元處理模塊相連接；所述捷聯(lián)慣性測(cè)量單元處理模塊與中央處理單元相連接；該導(dǎo)航方法包括若干步驟，它克服了AUV導(dǎo)航系統(tǒng)易受環(huán)境影響以及捷聯(lián)慣性導(dǎo)航系統(tǒng)的導(dǎo)航定位誤差隨時(shí)間延續(xù)不斷累積達(dá)不到精度要求的缺陷。
【專利說明】
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法，屬于水下及水面 航行器的導(dǎo)航及定位跟蹤領(lǐng)域。 基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法

【背景技術(shù)】
[0002] AUV(Autonomous Underwater Vehicle自主水下航行器）是探索和開發(fā)利用海洋 環(huán)境資源的重要工具，也在海洋軍事應(yīng)用方面發(fā)揮著重要作用。因其遠(yuǎn)程性、隱蔽性和自主 性，使得AUV的導(dǎo)航問題仍是目前面臨的主要技術(shù)挑戰(zhàn)之一。由于水下環(huán)境復(fù)雜，常采用組 合導(dǎo)航方式對(duì)AUV進(jìn)行導(dǎo)航定位，而組合導(dǎo)航系統(tǒng)濾波器設(shè)計(jì)是保證導(dǎo)航精度的關(guān)鍵。目 前常規(guī)AUV的導(dǎo)航定位精度可達(dá)到〈0. 8nmile / h左右，在一定程度上滿足了 AUV導(dǎo)航定 位的精度要求。
[0003] SINS (Strapdown Inertial Navigation Systems 捷聯(lián)慣性導(dǎo)航系統(tǒng)）系統(tǒng)具有自 主導(dǎo)航能力，不受環(huán)境、載體機(jī)動(dòng)及無線電干擾的影響，能夠連續(xù)的提供載體位置、速度和 姿態(tài)等導(dǎo)航定位信息，其數(shù)據(jù)更新頻率快，且在短時(shí)間內(nèi)具有較高的相對(duì)精度。但是，隨著 系統(tǒng)工作時(shí)間的延長(zhǎng)，捷聯(lián)慣性導(dǎo)航系統(tǒng)的導(dǎo)航誤差會(huì)隨之積累增長(zhǎng)，此時(shí)就需要利用外 部傳感器的觀測(cè)信息通過濾波算法來修正補(bǔ)償捷聯(lián)慣性導(dǎo)航系統(tǒng)，以抑制其隨時(shí)間積累的 誤差。
[0004] DVL(Doppler Velocity Log多普勒計(jì)程儀）是廣泛應(yīng)用于水下及水面航行器組合 系統(tǒng)的速度測(cè)量裝置，具有實(shí)時(shí)輸出載體三維速度與航程能力的聲學(xué)導(dǎo)航定位系統(tǒng)。多普 勒系統(tǒng)通過安裝于AUV底部的四個(gè)換能器信息求得聲波頻移，進(jìn)而計(jì)算得到載體在載體系 的三維速度信息，具有精度高、可靠性、實(shí)時(shí)性等優(yōu)點(diǎn)。但是由于水下環(huán)境復(fù)雜、航位推算系 統(tǒng)位置誤差隨時(shí)間累積以及AUV的長(zhǎng)航時(shí)、隱蔽性要求，基于捷聯(lián)慣性導(dǎo)航/多普勒的組合 系統(tǒng)的常規(guī)濾波算法精度難以達(dá)到高精度導(dǎo)航定位功能需求。
[0005] 常規(guī)H c?濾波算法對(duì)系統(tǒng)統(tǒng)計(jì)特性不作任何假設(shè)，具有濾波模型簡(jiǎn)單、魯棒性好 的優(yōu)點(diǎn)，因此常應(yīng)用于AUV導(dǎo)航系統(tǒng)。但是常規(guī)H c?濾波器跟蹤機(jī)動(dòng)過程能力較弱，實(shí)時(shí)性 難以保證。當(dāng)由于模型不確定性等因素的影響，造成濾波器的狀態(tài)估計(jì)值偏離系統(tǒng)狀態(tài)時(shí)， 必然會(huì)在輸出殘差序列的均值與幅值上有所表現(xiàn)，此時(shí)若在線調(diào)整時(shí)變?cè)鲆婢仃?，使得?統(tǒng)估計(jì)誤差方差最小的同時(shí)保持殘差序列仍然相互正交，即滿足正交性原理，則可迫使濾 波器保持對(duì)實(shí)際系統(tǒng)狀態(tài)的高精度跟蹤。本發(fā)明通過在He?濾波算法中引入次優(yōu)漸消因子 對(duì)過去的數(shù)據(jù)實(shí)施漸消，以減弱老數(shù)據(jù)對(duì)當(dāng)前濾波估計(jì)值的影響，即通過實(shí)時(shí)調(diào)節(jié)估計(jì)誤 差方差矩陣以及相應(yīng)的增益矩陣以滿足正交性原理，迫使濾波器對(duì)實(shí)際系統(tǒng)狀態(tài)的高精度 跟蹤，并最大程度地提取輸出殘差中一切有效信息，得到一種高跟蹤性能的濾波算法。該濾 波算法具有較強(qiáng)的關(guān)于模型不確定性的魯棒性以及對(duì)系統(tǒng)機(jī)動(dòng)過程的跟蹤能力，并保持了 H c?濾波模型簡(jiǎn)單的優(yōu)點(diǎn)，實(shí)現(xiàn)濾波器對(duì)于AUV機(jī)動(dòng)過程系統(tǒng)狀態(tài)的跟蹤，具有重要的理論 意義和工程實(shí)用價(jià)值。
[0006] 基于捷聯(lián)慣性導(dǎo)航/多普勒的AUV組合導(dǎo)航系統(tǒng)將捷聯(lián)慣性導(dǎo)航、多普勒與本發(fā) 明所提出的濾波算法的優(yōu)勢(shì)結(jié)合在一起，可提供載體高精度的實(shí)時(shí)導(dǎo)航跟蹤參數(shù)，解決了 AUV導(dǎo)航系統(tǒng)易受環(huán)境影響以及捷聯(lián)慣性導(dǎo)航的導(dǎo)航定位誤差隨時(shí)間延續(xù)不斷累積達(dá)不到 精度要求的問題。


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

[0007] 發(fā)明目的：本發(fā)明的目的在于，克服現(xiàn)有技術(shù)的不足，提出了一種用于捷聯(lián)慣性導(dǎo) 航/多普勒的AUV組合導(dǎo)航系統(tǒng)的濾波方法。利用所設(shè)計(jì)的濾波器將多普勒測(cè)速信息引入 AUV組合系統(tǒng)，以輔助捷聯(lián)慣性導(dǎo)航進(jìn)行導(dǎo)航定位。該方法可以整合多個(gè)子導(dǎo)航信息，克服 了 AUV導(dǎo)航系統(tǒng)易受環(huán)境影響以及捷聯(lián)慣性導(dǎo)航系統(tǒng)的導(dǎo)航定位誤差隨時(shí)間延續(xù)不斷累 積達(dá)不到精度要求的缺陷。
[0008] 技術(shù)方案：本發(fā)明所述的一種基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法，該 導(dǎo)航方法應(yīng)用于一基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航系統(tǒng)，該導(dǎo)航系統(tǒng)包括多普勒 計(jì)程儀、捷聯(lián)慣性測(cè)量單元、捷聯(lián)慣性測(cè)量單元處理模塊和中央處理單元；所述的多普勒計(jì) 程儀包括收發(fā)器、四個(gè)換能器和接口單元；所述多普勒計(jì)程儀通過該接口單元與中央處理 單元相連接；所述捷聯(lián)慣性測(cè)量單元包括三軸陀螺和三軸加速度計(jì)；所述捷聯(lián)慣性測(cè)量單 元與捷聯(lián)慣性測(cè)量單元處理模塊相連接；所述捷聯(lián)慣性測(cè)量單元處理模塊與中央處理單元 相連接；該導(dǎo)航方法包括以下步驟：
[0009] (1)通過捷聯(lián)慣性測(cè)量單元組件中的三軸陀螺測(cè)得三軸角速度信息和加速度計(jì)測(cè) 得三軸加速度信息，捷聯(lián)慣性測(cè)量單元處理模塊接收捷聯(lián)慣性測(cè)量單元輸出的導(dǎo)航信息， 通過導(dǎo)航積分計(jì)算獲得載體位置、速度和姿態(tài)等導(dǎo)航信息；
[0010] (2)通過多普勒計(jì)程儀的收發(fā)器中發(fā)射電振蕩信號(hào)，送給換能器；
[0011] (3)通過多普勒計(jì)程儀的四個(gè)換能器發(fā)射超聲波和接收具有頻移特性的反射回 波；
[0012] (4)利用收發(fā)器中接收系統(tǒng)將換能器送來的回波信號(hào)經(jīng)放大處理后求得多普勒頻 移并轉(zhuǎn)換為航速模擬信號(hào)送給接口單元。設(shè)載體航行速度為V，波束發(fā)射俯角為Θ，聲速 C ~ 1500m / S，則單波束頻移計(jì)算公式為：Af = ZVfdCos Θ / C，由此可得速度計(jì)算公式 為：V = Λ fC / (2fQcos Θ );
[0013] (5)利用收發(fā)器中接口單元將收發(fā)器送來的四個(gè)航速模擬信號(hào)轉(zhuǎn)換為航速，并以 數(shù)字方式向中央處理單元輸出；
[0014] (6)中央處理單元中的運(yùn)算器根據(jù)多普勒計(jì)程儀的接口單元輸出的速度信息周期 性地計(jì)算載體的實(shí)時(shí)三維載體系速度量；
[0015] (7)中央處理單元中的濾波模塊對(duì)接收到的捷聯(lián)慣性導(dǎo)航導(dǎo)航信息、多普勒計(jì)程 儀的三維速度信息進(jìn)行濾波融合計(jì)算得到k時(shí)刻的慣性捷聯(lián)測(cè)量系統(tǒng)校正量又 A，校正后得 到最終高精度的導(dǎo)航定位信息；
[0016] 所述的濾波融合計(jì)算得到慣性捷聯(lián)測(cè)量系統(tǒng)校正量的方法為：
[0017] 將捷聯(lián)慣性導(dǎo)航作為參考系統(tǒng)，狀態(tài)變量X取速度誤差(汾姿態(tài)角誤差 (Φε，Φ η，Φ》、加速度計(jì)隨機(jī)常值偏置(V, )和陀螺隨機(jī)常值漂移（ε χ，ε y，ε z)，共10 維：義=[汾/，汾7二，s,. A-]Γ ; W=[wax，way，wgx，wgy，w gz，0,0,0,0,0]T 為系

【權(quán)利要求】
1. 一種基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法，其特征在于，該導(dǎo)航方法應(yīng)用 于一基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航系統(tǒng)，該導(dǎo)航系統(tǒng)包括多普勒計(jì)程儀、捷聯(lián) 慣性測(cè)量單元、捷聯(lián)慣性測(cè)量單元處理模塊和中央處理單元；所述的多普勒計(jì)程儀包括收 發(fā)器、四個(gè)換能器和接口單元；所述多普勒計(jì)程儀通過該接口單元與中央處理單元相連接； 所述捷聯(lián)慣性測(cè)量單元包括三軸陀螺和三軸加速度計(jì)；所述捷聯(lián)慣性測(cè)量單元與捷聯(lián)慣性 測(cè)量單元處理模塊相連接；所述捷聯(lián)慣性測(cè)量單元處理模塊與中央處理單元相連接；該導(dǎo) 航方法包括以下步驟： (1) 通過捷聯(lián)慣性測(cè)量單元組件中的三軸陀螺測(cè)得三軸角速度信息和加速度計(jì)測(cè)得三 軸加速度信息，捷聯(lián)慣性測(cè)量單元處理模塊接收捷聯(lián)慣性測(cè)量單元輸出的導(dǎo)航信息，通過 導(dǎo)航積分計(jì)算獲得載體位置、速度和姿態(tài)等導(dǎo)航信息； (2) 通過多普勒計(jì)程儀的收發(fā)器中發(fā)射電振蕩信號(hào)，送給換能器； (3) 通過多普勒計(jì)程儀的四個(gè)換能器發(fā)射超聲波和接收具有頻移特性的反射回波； (4) 利用收發(fā)器中接收系統(tǒng)將換能器送來的回波信號(hào)經(jīng)放大處理后求得多普勒頻移 并轉(zhuǎn)換為航速模擬信號(hào)送給接口單元。設(shè)載體航行速度為V，波束發(fā)射俯角為Θ，聲速 C ~ 1500m / s，則單波束頻移計(jì)算公式為：Af = ZVi^cos Θ / C，由此可得速度計(jì)算公式 為：V = Λ fC / (2fQcos Θ ); (5) 利用收發(fā)器中接口單元將收發(fā)器送來的四個(gè)航速模擬信號(hào)轉(zhuǎn)換為航速，并以數(shù)字 方式向中央處理單元輸出； (6) 中央處理單元中的運(yùn)算器根據(jù)多普勒計(jì)程儀的接口單元輸出的速度信息周期性地 計(jì)算載體的實(shí)時(shí)三維載體系速度量； (7) 中央處理單元中的濾波模塊對(duì)接收到的捷聯(lián)慣性導(dǎo)航導(dǎo)航信息、多普勒計(jì)程儀的 三維速度信息進(jìn)行濾波融臺(tái)計(jì)算得到k時(shí)刻的慣性捷聯(lián)測(cè)量系統(tǒng)校正量，校正后得到最 終高精度的導(dǎo)航定位信息； 所述的濾波融合計(jì)算得到慣性捷聯(lián)測(cè)量系統(tǒng)校正量的方法為： 將捷聯(lián)慣性導(dǎo)航作為參考系統(tǒng)，狀態(tài)變量X取速度誤差(Μ:沉了）、姿態(tài)角誤差（Φ Φη，Φ")、加速度計(jì)隨機(jī)常值偏置（Vx，Vv)和陀螺隨機(jī)常值漂移（ε χ，ey，εζ)，共10維： X = [5VenySx ,sy ,ε: ]τ ； W=[wax，way，wgx，wgy，w gz，0,0,0,0,0]T 為系統(tǒng)噪聲向量； 以捷聯(lián)慣性導(dǎo)航解算出的速度與多普勒計(jì)程儀測(cè)得的載體系速度經(jīng)轉(zhuǎn)換到導(dǎo)航系后 兩者之差作為系統(tǒng)的量測(cè)值，則量測(cè)方程表示為：

上式中，殘差n -ΑΛ-,?；-,; 〇· 95彡P(guān)彡1為遺忘因子,一般取p = 0· 95 ; β彡1 為弱化因子，一般取β=1。
2. 根據(jù)權(quán)利要求1所述的一種基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法，其特征 在于，所述的捷聯(lián)慣性測(cè)量單元的三軸陀螺儀三軸正交安裝，所述的三軸加速度計(jì)三軸正 交安裝。
3. 根據(jù)權(quán)利要求1所述的一種基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法，其特征 在于，所述的捷聯(lián)慣性測(cè)量單元的三軸陀螺儀為光纖陀螺儀。
4. 根據(jù)權(quán)利要求1所述的一種基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法，其特征 在于，所述的三軸加速度計(jì)為硅微加速度計(jì)。
5. 根據(jù)權(quán)利要求1所述的一種基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法，其特征 在于，所述多普勒計(jì)程儀的收發(fā)器包括發(fā)射系統(tǒng)、接收系統(tǒng)和計(jì)算補(bǔ)償電路。
6. 根據(jù)權(quán)利要求1所述的一種基于捷聯(lián)慣性制導(dǎo)和多普勒計(jì)程儀的導(dǎo)航方法，其特征 在于，所述導(dǎo)航系統(tǒng)還包括顯示器，所述顯示器與運(yùn)算器相連接，顯示中央處理器運(yùn)算得到 的導(dǎo)航信息。
【文檔編號(hào)】G01C21/16GK104061930SQ201310653821
【公開日】2014年9月24日 申請(qǐng)日期:2013年12月5日 優(yōu)先權(quán)日:2013年12月5日
【發(fā)明者】程向紅, 許立平 申請(qǐng)人:東南大學(xué)