水下潛器地形輔助慣性導(dǎo)航系統(tǒng)的位置匹配方法
【專利摘要】本發(fā)明公開一種水下潛器地形輔助慣性導(dǎo)航系統(tǒng)的位置匹配方法，包括：1)在進(jìn)入海圖覆蓋區(qū)域后，利用深度計(jì)和多波束掃描測(cè)深儀，測(cè)量潛器在起始時(shí)間段Δt1內(nèi)航行區(qū)域的水深值；2)計(jì)算時(shí)間段Δt1的水深累積直方圖H1、地形粗糙度Kr1和下次水深測(cè)量的時(shí)間段Δt2；3)確定潛器在Δt1/2時(shí)刻的可能位置集L1；4)測(cè)量潛器在時(shí)間段Δtn(n＝2，3...，N)內(nèi)航行區(qū)域的水深值，計(jì)算時(shí)間段Δtn的水深累積直方圖Hn、地形粗糙度Krn和下次水深測(cè)量的時(shí)間段Δtn+1等步驟。木發(fā)明是一種自主式匹配方法具有精度高、魯棒性好的特點(diǎn)，可應(yīng)用于地形、地磁或者重力輔助慣性導(dǎo)航定位。
【專利說明】水下潛器地形輔助慣性導(dǎo)航系統(tǒng)的位置匹配方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種水下潛器地形匹配方法，主要針對(duì)水下潛器地形輔助慣性導(dǎo)航系 統(tǒng)的位置匹配方法。

【背景技術(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ù)庫進(jìn)行匹配，確定 潛器的最佳位置。
[0003] 地形匹配方法最早應(yīng)用在航空領(lǐng)域，作為戰(zhàn)機(jī)和巡航導(dǎo)彈的一種輔助導(dǎo)航方法， 其中最著名的有TERCOM和SITAN。由于水下環(huán)境的特殊性，Besl和Mckay等人提出了 ICP 圖像對(duì)準(zhǔn)方法。近年來，很多學(xué)者將ICP算法與其他方法進(jìn)行組合以改進(jìn)地形匹配導(dǎo)航方 法。
[0004] 為保證慣性導(dǎo)航系統(tǒng)長(zhǎng)時(shí)間、高精度的工作，地形匹配算法應(yīng)具有實(shí)時(shí)性和可靠 性。實(shí)時(shí)性方面，ICP算法的旋轉(zhuǎn)、平移計(jì)算量較大，并且實(shí)際應(yīng)用中要求潛器初始誤差較 小?？煽啃苑矫?，目前大多數(shù)地形導(dǎo)航確定搜索區(qū)域的方法是采用慣導(dǎo)系統(tǒng)輸出的潛器位 置為中心，搜索半徑根據(jù)實(shí)際情況確定。這種方法在慣性導(dǎo)航初始誤差不大的情況下是可 取的，在初始誤差較大時(shí)，確定的搜索區(qū)域可能是不正確的或范圍很大。


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

[0005] 發(fā)明目的：本發(fā)明的目的在于克服現(xiàn)有技術(shù)的不足，提出一種水下潛器地形輔助 慣性導(dǎo)航系統(tǒng)的位置匹配方法，可有效地提高地形輔助慣性導(dǎo)航系統(tǒng)快速、準(zhǔn)確定位，從而 消除和克服在慣導(dǎo)系統(tǒng)初始誤差大的情況下地形匹配算法失效的缺陷。
[0006] 技術(shù)方案：本發(fā)明所述的水下潛器地形輔助慣性導(dǎo)航系統(tǒng)的位置匹配方法包括以 下步驟：
[0007] 1)在進(jìn)入海圖覆蓋區(qū)域后，利用潛器所裝備的深度計(jì)和多波束掃描測(cè)深儀，測(cè)量 潛器在起始時(shí)間段At 1內(nèi)航行區(qū)域的水深值；
[0008] 2)計(jì)算時(shí)間段At1的水深累積直方圖H1和地形粗糙度K rt，并由Krt計(jì)算下次水深 測(cè)量的時(shí)間段At2;
[0009] 3)將時(shí)間段At1內(nèi)掃描區(qū)域的大小作為整個(gè)海圖搜索的網(wǎng)格單元，采用滑動(dòng)窗的 方式搜索，計(jì)算第m個(gè)滑動(dòng)窗的水深累積直方圖H lmOn = 1，2,…，M)，并計(jì)算H1與Hlm之間 的距離，將其中距離滿足匹配準(zhǔn)則的第m個(gè)滑動(dòng)窗的中心位置作為潛器在Λ tl/2時(shí)刻的可 能位置集L1 ;
[0010] 4)測(cè)量潛器在時(shí)間段Atn(η = 2, 3···，N)內(nèi)航行區(qū)域的水深值，計(jì)算時(shí)間段Atn 的水深累積直方圖Hn和地形粗糙度Km，并由Km計(jì)算下次水深測(cè)量的時(shí)間段△ tn+1 ;
[0011] 5)計(jì)算慣導(dǎo)系統(tǒng)在時(shí)間段Atlri內(nèi)航行的相對(duì)距離Dn_ w計(jì)算更新的可能位置集 L' n_i(當(dāng)n = 2時(shí)，L' i = !^)中各位置點(diǎn)之間的最大距離Dlri，確定時(shí)間段Atn內(nèi)水深測(cè) 量的搜索區(qū)域，采用滑動(dòng)窗的方式搜索，計(jì)算第m個(gè)滑動(dòng)窗的水深累積直方圖H?，并計(jì)算H n 與Hnm之間的距離，將其中距離滿足匹配準(zhǔn)則的第m個(gè)滑動(dòng)窗的中心位置作為潛器在Λ tn/2 時(shí)刻的可能位置集Ln ;
[0012] 6)計(jì)算L' n_i和Ln中各位置點(diǎn)之間的距離，并與DnM比較，將滿足乂 A 與Dn_1;n的絕對(duì)差值小于三倍海圖分辨率條件的L' n_i集作為潛器在Λ t^/2時(shí)刻的參考位 置集Plri，同時(shí)將滿足上述條件的Ln集作為潛器在Λ tn/2時(shí)刻的更新的可能位置集L' n ;
[0013] 7)依據(jù)地形粗糙度Km決定實(shí)測(cè)總次數(shù)N，并判斷實(shí)測(cè)次數(shù)是否達(dá)到N，若未達(dá)到， 將η加1，并重復(fù)步驟4)?步驟6);若實(shí)測(cè)次數(shù)達(dá)到N，則計(jì)算參考位置集P 1至Pim的價(jià)值 函數(shù)，獲取潛器航跡，修正慣導(dǎo)系統(tǒng)的位置參數(shù)。
[0014] 進(jìn)一步地，所述步驟1)中潛器在測(cè)量起始時(shí)間段內(nèi)航行的區(qū)域應(yīng)盡量保持正方 形，此外，在后續(xù)每次測(cè)量時(shí)間段內(nèi)航行的區(qū)域也同樣保持正方形。
[0015] 進(jìn)一步地，所述步驟2)中計(jì)算時(shí)間段At1的水深累積直方圖H1具體過程為：根據(jù) 整個(gè)海圖的水深量程，將水深值平均分成10組到20組不重疊區(qū)間，確定水深間隔范圍，統(tǒng) 計(jì)步驟1)測(cè)量的水深值在每個(gè)水深間隔范圍內(nèi)的水深值個(gè)數(shù)，累積各水深間隔范圍的水 深值概率分布情況，得到時(shí)間段At 1的水深累積直方圖H1 ;
[0016] 地形粗糙度定義為單位面積的水深值方差，Krt表示時(shí)間段At1的地形粗糙度，依 據(jù)1^確定下次水深測(cè)量的時(shí)間段At 2，其中，At2取大于1/Krt的最小整數(shù)。
[0017] 進(jìn)一步地，所述步驟3)中時(shí)間段At1內(nèi)掃描區(qū)域的大小具體為：
[0018] S = (kd)2 (1)
[0019] 式中，k為與潛器航跡垂直的平面內(nèi)一次給出的多波束測(cè)深點(diǎn)個(gè)數(shù)，d為k個(gè)多波 束測(cè)深點(diǎn)的中心點(diǎn)間距，S為時(shí)間段At 1內(nèi)掃描區(qū)域的大小，也稱為網(wǎng)格單元；
[0020] 第m個(gè)滑動(dòng)窗的搜索步進(jìn)值為海圖的分辨率，其中，m = 1，2, "·，Μ，M由搜索區(qū)域 和網(wǎng)格單元大小決定，水深累積直方圖之間的距離定義為相同水深間隔范圍內(nèi)累積概率值 的均方誤差，計(jì)算H 1與Hlm之間的距離，H1與Hlm之間的距離滿足的匹配準(zhǔn)則為：
[0021]

【權(quán)利要求】
1. 水下潛器地形輔助慣性導(dǎo)航系統(tǒng)的位置匹配方法，其特征在于包括以下步驟： 1) 在進(jìn)入海圖覆蓋區(qū)域后，利用潛器所裝備的深度計(jì)和多波束掃描測(cè)深儀，測(cè)量潛器 在起始時(shí)間段At1內(nèi)航行區(qū)域的水深值； 2) 計(jì)算時(shí)間段Λ tl的水深累積直方圖H1和地形粗糙度Krt，并由Krt計(jì)算下次水深測(cè)量 的時(shí)間段At 2; 3) 將時(shí)間段At1內(nèi)掃描區(qū)域的大小作為整個(gè)海圖搜索的網(wǎng)格單元，采用滑動(dòng)窗的方式 搜索，計(jì)算第m個(gè)滑動(dòng)窗的水深累積直方圖H lmOn = 1，2, "·，Μ)，并計(jì)算H1與Hlm之間的距 離，將其中距離滿足匹配準(zhǔn)則的第m個(gè)滑動(dòng)窗的中心位置作為潛器在Λ tl/2時(shí)刻的可能位 直集L1 ; 4) 測(cè)量潛器在時(shí)間段Λ?η(η = 2,3···，Ν)內(nèi)航行區(qū)域的水深值，計(jì)算時(shí)間段八乂的水 深累積直方圖Hn和地形粗糙度Km，并由K m計(jì)算下次水深測(cè)量的時(shí)間段Λ tn+1 ; 5) 計(jì)算慣導(dǎo)系統(tǒng)在時(shí)間段Atlri內(nèi)航行的相對(duì)距離Dn_lin，計(jì)算更新的可能位置集 L' n_i(當(dāng)n = 2時(shí)，L' i = !^)中各位置點(diǎn)之間的最大距離Dlri，確定時(shí)間段Atn內(nèi)水深測(cè) 量的搜索區(qū)域，采用滑動(dòng)窗的方式搜索，計(jì)算第m個(gè)滑動(dòng)窗的水深累積直方圖H?，并計(jì)算H n 與Hnm之間的距離，將其中距離滿足匹配準(zhǔn)則的第m個(gè)滑動(dòng)窗的中心位置作為潛器在Λ tn/2 時(shí)刻的可能位置集Ln ; 6) 計(jì)算L' n_jPLn中各位置點(diǎn)之間的距離義^,,，并與D1^n比較，將滿足'^"與D 1^n 的絕對(duì)差值小于三倍海圖分辨率條件的L' n_i集作為潛器在Λ t^/2時(shí)刻的參考位置集 Plri，同時(shí)將滿足上述條件的Ln集作為潛器在Λ tn/2時(shí)刻的更新的可能位置集L' n ; 7) 依據(jù)地形粗糙度Km決定實(shí)測(cè)總次數(shù)N，并判斷實(shí)測(cè)次數(shù)是否達(dá)到N，若未達(dá)到，將η 加1，并重復(fù)步驟4)?步驟6);若實(shí)測(cè)次數(shù)達(dá)到Ν，則計(jì)算參考位置集P1至？^的價(jià)值函數(shù)， 獲取潛器航跡，修正慣導(dǎo)系統(tǒng)的位置參數(shù)。
2. 根據(jù)權(quán)利要求1所述的水下潛器地形輔助慣性導(dǎo)航系統(tǒng)的位置匹配方法，其特征在 于：所述步驟1)中潛器在測(cè)量起始時(shí)間段內(nèi)航行的區(qū)域應(yīng)盡量保持正方形，此外，在后續(xù) 每次測(cè)量時(shí)間段內(nèi)航行的區(qū)域也同樣保持正方形。
3. 根據(jù)權(quán)利要求1所述的水下潛器地形輔助慣性導(dǎo)航系統(tǒng)的位置匹配方法，其特征在 于：所述步驟2)中計(jì)算時(shí)間段△ h的水深累積直方圖H1具體過程為：根據(jù)整個(gè)海圖的水 深量程，將水深值平均分成10組到20組不重疊區(qū)間，確定水深間隔范圍，統(tǒng)計(jì)步驟1)測(cè)量 的水深值在每個(gè)水深間隔范圍內(nèi)的水深值個(gè)數(shù)，累積各水深間隔范圍的水深值概率分布情 況，得到時(shí)間段At 1的水深累積直方圖H1 ; 地形粗糙度定義為單位面積的水深值方差，Krt表示時(shí)間段Λ tl的地形粗糙度，依據(jù)Krt 確定下次水深測(cè)量的時(shí)間段At2，其中，At2取大于1/Krt的最小整數(shù)。
4. 根據(jù)權(quán)利要求1所述的水下潛器地形輔助慣性導(dǎo)航系統(tǒng)的位置匹配方法，其特征在 于：所述步驟3)中時(shí)間段Λ tl內(nèi)掃描區(qū)域的大小具體為： S = (kd)2 (1) 式中，k為與潛器航跡垂直的平面內(nèi)一次給出的多波束測(cè)深點(diǎn)個(gè)數(shù)，d為k個(gè)多波束測(cè) 深點(diǎn)的中心點(diǎn)間距，S為時(shí)間段At1內(nèi)掃描區(qū)域的大小，也稱為網(wǎng)格單元； 第m個(gè)滑動(dòng)窗的搜索步進(jìn)值為海圖的分辨率，其中，m = 1，2, "·，Μ，M由搜索區(qū)域和網(wǎng) 格單元大小決定，水深累積直方圖之間的距離定義為相同水深間隔范圍內(nèi)累積概率值的均 方誤差，計(jì)算H1與Hlm之間的距離，H1與Hlm之間的距離滿足的匹配準(zhǔn)則為：
(2) 式中，η表示測(cè)量次數(shù)，此處為1，num表示水深累積直方圖的區(qū)間數(shù)，Hni表示測(cè)量時(shí)間 段Atn內(nèi)第i個(gè)水深間隔范圍的累積概率值，Hnmi表示時(shí)間段Atn內(nèi)，第m個(gè)滑動(dòng)窗的第i 個(gè)水深間隔范圍的累積概率值，Km表示時(shí)間段Atn的地形粗糙度，ε表示調(diào)節(jié)因子，調(diào)節(jié) ε，使得（ε /Krn)在12?25范圍內(nèi)，式⑵的左邊為水深累積直方圖Hn與Hmi之間的距 離，H niin表示扎與民^!!! = 1，2, "·，Μ)之間的最小距離，將滿足式（2)的第m個(gè)滑動(dòng)窗的中 心位置作為潛器在Λ tl/2時(shí)刻的可能位置集U。
5. 根據(jù)權(quán)利要求1所述的水下潛器地形輔助慣性導(dǎo)航系統(tǒng)的位置匹配方法，其特征在 于：所述步驟5)中搜索區(qū)域的確定方法具體為：計(jì)算L' n_i集中各位置點(diǎn)之間的最大距離 Dlri，取Dlri和2Dn_1;n中較大值為R，得到搜索區(qū)域?yàn)椋褐M度范圍為[Latimin-R，Latimax+R]， 經(jīng)度范圍為[longimin-R, longimax+R]，其中，Latimax和Latimin分別是L' n_l集中諱度 最大值和最小值，Iongimax和Iongimin分別是L' Jri集中經(jīng)度最大值和最小值，如果搜索 區(qū)域的某邊界超出海圖的邊界，以海圖邊界為最大值或者最小值； 同步驟3)計(jì)算扎與^之間的距離，式⑵中n = 2,3···，N，將滿足式⑵的第m個(gè)滑 動(dòng)窗的中心位置作為潛器在Λ tn/2時(shí)刻的可能位置集Ln。
6. 根據(jù)權(quán)利要求1所述的水下潛器地形輔助慣性導(dǎo)航系統(tǒng)的位置匹配方法，其特征在 于：所述步驟7)中價(jià)值函數(shù)具體為：
(3) 式中，D(PnJn)表示參考位置集Plri與Pn各位置點(diǎn)之間的距離，K為價(jià)值函數(shù)；依據(jù)地 形粗糙度1^確定實(shí)測(cè)總次數(shù)N，具體確定原則為：Km>l時(shí)，取N 3 3，Km〈l時(shí)，取N 3 4 ;根 據(jù)式（3)計(jì)算參考位置集P1至？^的價(jià)值函數(shù)K，取價(jià)值函數(shù)為最小值時(shí)的參考位置連線 為最終獲取的潛器航跡，由匹配的各時(shí)間段中點(diǎn)位置，結(jié)合潛器短時(shí)內(nèi)勻速直航的特點(diǎn)，推 算潛器當(dāng)前的實(shí)際位置，最后通過位置重置修正慣導(dǎo)系統(tǒng)的位置參數(shù)。
【文檔編號(hào)】G01C21/16GK104390646SQ201410469998
【公開日】2015年3月4日 申請(qǐng)日期:2014年9月15日 優(yōu)先權(quán)日:2014年9月15日
【發(fā)明者】程向紅, 周玲, 陸源, 冉昌艷, 王磊, 朱倚嫻 申請(qǐng)人:東南大學(xué)