一種低成本水下潛器地形輔助慣性組合導(dǎo)航定位方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種水下潛器組合導(dǎo)航定位方法，具體涉及一種低成本水下潛器地形 輔助慣性組合導(dǎo)航定位方法。
【背景技術(shù)】
[0002] 水下潛器經(jīng)過長(zhǎng)時(shí)間航行后，慣性導(dǎo)航系統(tǒng)輸出的位置已經(jīng)積累較大誤差?？紤] 到安全性、隱蔽性和長(zhǎng)時(shí)間水下作業(yè)的實(shí)際情況，采用地球物理場(chǎng)輔助慣性導(dǎo)航的無源導(dǎo) 航技術(shù)，主要包括慣性/地形、慣性/重力和慣性/地磁組合導(dǎo)航。其基本原理都是通過潛 器裝備的傳感器測(cè)量航行軌跡上的地球物理場(chǎng)數(shù)據(jù)，再與已有的海圖數(shù)據(jù)庫(kù)進(jìn)行匹配，確 定水下潛器的最佳位置。
[0003] 地形導(dǎo)航最早應(yīng)用在航空領(lǐng)域，作為戰(zhàn)機(jī)和巡航導(dǎo)彈的一種輔助導(dǎo)航方法，其中 最著名的有TERC0M和SITAN。目前，水下地形輔助慣性組合導(dǎo)航的研究多用于裝備有導(dǎo)航 級(jí)慣性系統(tǒng)、多波束測(cè)深儀和高分辨率高精度海圖數(shù)據(jù)庫(kù)的高成本水下潛器。由于水下環(huán) 境的特殊性，高分辨率的海圖數(shù)據(jù)庫(kù)往往很難獲取，因此，針對(duì)裝備低分辨率海圖及單波束 測(cè)深儀的低成本水下潛器地形輔助慣性組合導(dǎo)航定位的研究具有實(shí)際意義。
[0004] 海底地形的變化通過水深的分布情況反映，在地形輔助慣性組合導(dǎo)航的研究中， 海圖水深模型建立的準(zhǔn)確性是影響導(dǎo)航精度的主要因素之一。在海圖分辨率較高情況下 (優(yōu)于10m)，海圖水深建模多采用最鄰近點(diǎn)插值法和雙線性插值法。但在海圖分辨率較低 情況下（大于l〇m小于30m)，以上方法構(gòu)建的水深模型準(zhǔn)確度差，導(dǎo)致組合導(dǎo)航定位精度急 劇下降。因此，建立一種適于低分辨率海圖的水深模型，有助于提高組合導(dǎo)航的定位精度， 同時(shí)可擴(kuò)展地形輔助慣性組合導(dǎo)航定位在低成本水下潛器中的應(yīng)用。

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

[0005] 本發(fā)明的目的是為了解決高分辨率海圖采用的海圖水深模型不再適用于低分辨 率海圖水深建模，導(dǎo)致組合導(dǎo)航定位精度下降的問題，提供一種低成本水下潛器地形輔助 慣性組合導(dǎo)航定位方法，該方法采用高斯過程回歸建立海圖水深模型，利用高斯過程回歸 在海圖采樣點(diǎn)學(xué)習(xí)和預(yù)測(cè)的特點(diǎn)，可準(zhǔn)確構(gòu)建海圖水深模型，進(jìn)而提高低成本水下潛器的 組合導(dǎo)航定位精度。
[0006] 本發(fā)明采用的技術(shù)方案為：一種低成本水下潛器地形輔助慣性組合導(dǎo)航定位方 法，具體步驟如下：
[0007] 步驟1)初始化組合導(dǎo)航定位系統(tǒng)，為系統(tǒng)的導(dǎo)航參數(shù)更新提供初始值：使用位置 傳感器獲取潛器的初始位置參數(shù)；根據(jù)慣性測(cè)量單元采集的潛器角速率和比力信息，以及 多普勒測(cè)速儀測(cè)得的載體系速度，進(jìn)行捷聯(lián)慣性導(dǎo)航系統(tǒng)的初始對(duì)準(zhǔn)，獲取潛器的初始姿 態(tài)矩陣和導(dǎo)航系初始速度信息；
[0008] 步驟2)根據(jù)潛器的運(yùn)動(dòng)模型，建立以潛器位置誤差為狀態(tài)量的3維線性離散狀態(tài) 方程；
[0009] 步驟3)采用高斯過程回歸建立以水深值為觀測(cè)量的1維非線性離散量測(cè)方程；
[0010] 步驟4)由步驟2)和步驟3)組成非線性組合導(dǎo)航系統(tǒng)模型，構(gòu)建粒子濾波算法；
[0011] 步驟5)計(jì)算位置誤差并修正捷聯(lián)慣性導(dǎo)航系統(tǒng)位置，完成組合導(dǎo)航位置參數(shù)更 新。
[0012] 作為優(yōu)選，所述步驟2)中，根據(jù)潛器的運(yùn)動(dòng)模型，建立以潛器位置誤差為狀態(tài)量 的3維線性離散狀態(tài)方程，具體過程為：
[0013] 以東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，以潛器右前上方向構(gòu)成的坐標(biāo)系作為載體 坐標(biāo)系；
[0014] 潛器的運(yùn)動(dòng)模型為
[0015] xt+1=Xt+ut+vt (1)
[0016]式中：
[0017]xJPXt+1分別為t時(shí)刻和t+1時(shí)刻潛器的水平位置向量，Xt=[XE,t，xN,t]T，xE,t和 xNit分別為t時(shí)刻潛器的東向和北向位置分量；[· ]T表示矩陣轉(zhuǎn)置；ut為慣性導(dǎo)航系統(tǒng)計(jì) 算的從t時(shí)刻到t+Ι時(shí)刻潛器的水平位置變化量；^是t時(shí)刻的系統(tǒng)噪聲；
[0018] 定義和，=%-毛，貝1J
[0019]δxt+1=δχt+vt (2)
[0020] 式中：
[0021] 筆為慣性導(dǎo)航系統(tǒng)計(jì)算的t時(shí)刻潛器的水平位置向量；δ\和δχt+1分別為t時(shí) 刻和t+1時(shí)刻潛器的水平位置誤差向量；
[0022] 由式（2)擴(kuò)展的以潛器位置誤差為狀態(tài)量的3維線性離散狀態(tài)方程為
[0023] ξ?+1=ξt+vt ⑶
[0024]式中：
[0025]ξ廊ξt+1分別為t時(shí)刻和t+1時(shí)刻潛器的位置誤差向量，ξt= [δxE,t，δxN,t，bt] T，δχΕ?，SxN，t，bt分別為t時(shí)刻潛器的東向、北向和天向位置誤差分量。
[0026] 作為優(yōu)選，所述步驟3)中，采用高斯過程回歸建立以水深值為觀測(cè)量的1維非線 性離散量測(cè)方程，具體過程為：
[0027] 3. 1)粒子濾波器選取Μ個(gè)粒子，以第m個(gè)粒子，m= 1，2,…，M，的導(dǎo)航系水平位置 向量為中心，以海圖上3X3鄰域內(nèi)η個(gè)節(jié)點(diǎn)的3維位置向量作為該粒子海圖水深建模的訓(xùn) 練樣本；
[0028] 3. 2)高斯過程回歸模型初始化：設(shè)定協(xié)方差函數(shù)和超參數(shù)，采用神經(jīng)網(wǎng)絡(luò)協(xié)方差 函數(shù)，變量xp與Xq的神經(jīng)網(wǎng)絡(luò)協(xié)方差函數(shù)為
[0029]
I)
[0030]式中：
[0031]xp和Xq為訓(xùn)練樣本水平位置向量中的任意元素；為信號(hào)方差；
為尺度矩陣；^和^分別為水平東向和北向的特征尺度；超參數(shù)Θ= {1E，1N，Of,ση}，其 中σn為樣本噪聲標(biāo)準(zhǔn)差；
[0032] 3.3)高斯過程回歸模型訓(xùn)練：分別以第m個(gè)粒子訓(xùn)練樣本中水平位置向量集 和含噪聲的垂直水深向量集作為該粒子高斯過程回歸的輸入數(shù)據(jù)和輸出數(shù) 據(jù)，建立訓(xùn)練樣本條件概率ρΟΟΧ^，θη)的對(duì)數(shù)似然函數(shù)為
[0033]
[0034]式中：
[0035]
}為式⑷確 定的ηΧη階無噪聲對(duì)稱正定的協(xié)方差矩陣；Θm為第m個(gè)粒子的超參數(shù)；I。為η維單位矩 陣；
[0036] 通過最大化似然函數(shù)設(shè)置超參數(shù)最優(yōu)解，似然函數(shù)對(duì)超參數(shù)求偏導(dǎo)為
[0037]
[0038]式中：
[0039]
表示矩陣的跡；
[0040] 3. 4)組合導(dǎo)航過程中，將粒子濾波器的第m個(gè)粒子在t時(shí)刻的水平位置向量 輸入t時(shí)刻該粒子建立的高斯過程回歸模型，則可得到海圖上t時(shí)刻對(duì)應(yīng)位置的水深值/^ 及其方差c〇v(/；CJ分別為
[0041]
[0042]
[0043]式中：
[0044]X"^P別為t時(shí)刻第m個(gè)粒子高斯過程回歸的輸入數(shù)據(jù)和輸出數(shù)據(jù)；
[0045] 勺t時(shí)刻第m個(gè)粒子的x*和X" t之 mt， 間的ΙΧη階協(xié)方差矩陣，為和\"^的協(xié)方差函數(shù)，其表達(dá)式取式（4); K(Xn,t，Xn,t) = {k(xp,n,t，xq,n,t) |p,q= 1，2, ···，]!}為t時(shí)刻第m個(gè)粒子的Xn,t和Xn,t 之間的ηXn階協(xié)方差矩陣，k(x^t，t)為xp^ ,和xt的神經(jīng)網(wǎng)絡(luò)協(xié)方差函數(shù)； 為自身的協(xié)方差函數(shù)，表達(dá)式取式（4)中的神經(jīng)網(wǎng)絡(luò)協(xié)方差函數(shù)表達(dá)式；
[0046] 3. 5)非線性離散量測(cè)方程為
[0047]
[0048]式中：
[0049]^為t時(shí)刻潛器測(cè)量的水深值，具體為單波束測(cè)深儀測(cè)量的潛器距離海底的高度 和壓力傳感器測(cè)量的潛器距離海面的深度之和；海圖模型
表示潛器在海圖上的水深值，分別為慣性導(dǎo)航系統(tǒng)計(jì)算的t時(shí)刻潛器的東向和北 向位置分量，f( ·)采用高斯過程回歸建立；η,為t時(shí)刻的量測(cè)噪聲。
[0050] 作為優(yōu)選，所述步驟4)中，構(gòu)建非線性組合導(dǎo)航系統(tǒng)的粒子濾波算法，具體過程 為：
[0051] 4. 1)由步驟2)和步驟3)組成的非線性組合導(dǎo)航系統(tǒng)模型為
[0052]
[0053]式中：
[0054]ξ種ξt+1分別為t和t+Ι時(shí)刻的系統(tǒng)狀態(tài)向量；zAt時(shí)刻的量測(cè)量；(ξ,:交,） 為t時(shí)刻的非線性觀測(cè)函數(shù)；^和IIt分別為t時(shí)刻的系統(tǒng)噪聲向量和量測(cè)噪聲向量，且νt 和nt滿足
^為δ函 數(shù)；
[0055] 4. 2)粒子濾波初始化：由先驗(yàn)概率ρ(ξ。）產(chǎn)生粒子群，所有粒子權(quán)值為1/ Μ；
[0056] 4. 3)更新：在t時(shí)刻更新粒子權(quán)值，重要性權(quán)值為