本發(fā)明涉及的是一種水下地形匹配導(dǎo)航方法，具體地說是一種AUV的地形匹配導(dǎo)航濾波方法。

背景技術(shù)：

水下地形匹配導(dǎo)航具有無累計導(dǎo)航偏差的優(yōu)點可作為推算導(dǎo)航的偏差修正，修正過程實際上是地形匹配定位結(jié)果與推算導(dǎo)航結(jié)果的融合濾波過程，由于地形匹配定位通常不具有連續(xù)性以及地形變化的具有很強的隨機性容易導(dǎo)致濾波結(jié)果的發(fā)散。目前地形匹配導(dǎo)航方法主要有TERCOM方法，SIATN方法。TERCOM法屬于批處理算法，通過獲得航線下方的部分地形與載體攜帶的先驗地形配準(zhǔn)獲得定位位置，TERCOM算法不涉及濾波和遞推導(dǎo)航的問題，由于沒有充分利用推算導(dǎo)航的信息，導(dǎo)致算法本身不具備較好的可靠性和連續(xù)導(dǎo)航能力；SITAN方法屬于遞推形式的導(dǎo)航方法，但遞推過程需要進行地形的線性化，由于地形具有很強的非線性特征和較大的測量誤差，這容易導(dǎo)致濾波發(fā)散。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的在于提供一種能實現(xiàn)連續(xù)的地形匹配導(dǎo)航和非連續(xù)的地形匹配導(dǎo)航，提高導(dǎo)航方式的靈活性的AUV地形匹配導(dǎo)航濾波方法。

本發(fā)明的目的是這樣實現(xiàn)的:

AUV在水下航行l(wèi)_t|t距離之后到達地形匹配導(dǎo)航的修正點，推算導(dǎo)航給出了推算導(dǎo)航位置推算導(dǎo)航的誤差并開啟地形匹配導(dǎo)航程序，此時按照如下的步驟完成地形匹配導(dǎo)航的濾波；

步驟一、向地形匹配導(dǎo)航系統(tǒng)輸入初始數(shù)據(jù)，所述初始數(shù)據(jù)包括：推算導(dǎo)航位置推算導(dǎo)航誤差A(yù)UV航程當(dāng)前時刻測量得到的地形Z以及先驗地形圖h；

步驟二、計算地形匹配定位的搜索區(qū)間估計，所述搜索區(qū)間由下式確定：

其中：λ_x，λ_y表示導(dǎo)航偏差占載體的運行里程的百分比，由參導(dǎo)航系統(tǒng)的統(tǒng)計結(jié)果事先給出；α≥1表示放大倍數(shù)，取(1,1.5]；

以為中心，和為x方向和y方向的長度構(gòu)成的矩形區(qū)域就是搜索區(qū)間X_S；

步驟三、地形匹配定位，

根據(jù)步驟二中計算得到的搜索范圍X_S開始進行地形搜索匹配，假設(shè)潮差為t_ij，搜索點ij的先驗地形插值序列與測量地形的高度偏差序列：

假設(shè)地形高度偏差序列滿足白噪聲分布而潮差是常量，得到搜索點ij的潮差和地形測量誤差估計：

得到了潮差的估計值之后，計算測量地形和先驗地形的真實相似程度：

其中：h(x_ij,y_ij)表示測量序列點(x_ij,y_ij)在先驗地形圖h中的插值結(jié)果；z_ij∈Z，Z表示測量地形，z_ij表示測量地形內(nèi)的點；

查找似然函數(shù)L_ij值最大的位置得到地形匹配定位的位置得到定位位置后將定位位置的潮差估計和測量地形的誤差估計作為當(dāng)前定位位置的最優(yōu)潮差估計和測量誤差估計記為：

步驟四、地形匹配地位結(jié)果的有效性檢驗

根據(jù)地形匹配定位位置檢驗該定位點的有效性，建立如下的有效性檢驗?zāi)Ｐ停?/p>

其中C_t|t為有效區(qū)間，由下面的方法得到：

(1)以步驟三中得到的似然函數(shù)L_ij為輸入，計算定位點似然函數(shù)的下界L_low；

其中:σ_p表示地形匹配地位點的測量誤差估計；mn表示地形匹配地位點的個數(shù)；Z_α/2表示以σ_p為正態(tài)分布的上分位點對應(yīng)的值；

(2)以L_low為等高平面截取步驟三中的似然函數(shù)L_ij，得到的等高線其中i表示等高線索引號，假設(shè)共有n條等高線；

(3)查找包圍地形匹配定位點的等高線在等高線所包圍區(qū)域的內(nèi)部，就是地形匹配定位的置信區(qū)間C_t|t；

步驟五、地形匹配定位結(jié)果的定位誤差估計，其估計方法如下，先計算地形匹配定位點的信息量，I₊，I_-表示正向和負向的信息量，先計算I₊：

式中：m,n表示測量地形的節(jié)點數(shù)目，k,l表示地形節(jié)點的索引號，σ_p表示定位點的測量誤差估計，表示測量地形中的(k,l)號節(jié)點在先驗地形中的插值結(jié)果；

由于定位誤差等于信息矩陣的逆P₊＝(I₊)^-1進一步簡化：

其中：

由此得到了定位點在x₊和y₊方向的定位誤差將I₊計算式中的+d換成-d同理計算P_-：

由此得到了定位點在x_-和y_-方向的定位誤差以及x₊和y₊方向的定位誤差

將定位誤差標(biāo)記為：

步驟六、地形匹配定位與推算導(dǎo)航的融合濾波

利用推算導(dǎo)航輸出誤差P_t|t和推算導(dǎo)航的位置地形匹配定位位置地形匹配定位誤差和求得兩個估計值的濾波結(jié)果：

再計算濾波誤差濾波誤差為：

由于地形的強非線性和隨機性特征導(dǎo)致地形匹配導(dǎo)航的濾波難以采用常規(guī)的方法(如Kalman濾波)容易濾波發(fā)散和收斂太慢，通常濾波過程需要計算觀測預(yù)測值，這就需要進行地形的線性化操作，不僅實現(xiàn)困難還會引入更大的誤差。本發(fā)明就是為了簡化濾波過程，實現(xiàn)地形匹配導(dǎo)航的連續(xù)或非連續(xù)遞推而設(shè)計的。本發(fā)明提出了一種新的地形匹配導(dǎo)航的濾波方法。該方法可以實現(xiàn)連續(xù)的地形匹配導(dǎo)航和非連續(xù)的地形匹配導(dǎo)航，并且導(dǎo)航過程中不需要對地形做額外的處理，避免地形的線性化等其他復(fù)雜的操作，導(dǎo)航過程可以對地形匹配定位點進行有效性判定和推算導(dǎo)航與地形匹配定位結(jié)果的遞推融合濾波。

本發(fā)明濾波過程簡單不需要進行額外的地形線性化處理，由于濾波過程包含了地形匹配定位結(jié)果的有效性評估具有較好的魯棒性，不存在濾波結(jié)果發(fā)散的問題。利用本方法不僅可以實現(xiàn)連續(xù)的地形匹配定位與推算導(dǎo)航的融合濾波，而且還可以進行間斷性的融合濾波，也就是說兩次濾波之間可以有一段時間只使用推算導(dǎo)航，當(dāng)獲得有效的地形匹配定位位置后再進行濾波融合，提高了導(dǎo)航方式的靈活性。

附圖說明

圖1是地形匹配導(dǎo)航的濾波過程；

圖2是地形匹配定位過程流程圖；

圖3是地形匹配定位點有效性檢測流程圖；

圖4是地形匹配定位與推算導(dǎo)航定位結(jié)果的相對位置關(guān)系。

具體實施方式

下面舉例對本發(fā)明作更詳細的描述。

AUV在水下航行l(wèi)_t|t距離之后到達地形匹配導(dǎo)航的修正點，推算導(dǎo)航給出了推算導(dǎo)航位置推算導(dǎo)航的誤差并開啟地形匹配導(dǎo)航程序，此時按照如下的步驟完成地形匹配導(dǎo)航的濾波。

步驟100：初始數(shù)據(jù)輸入，向地形匹配導(dǎo)航系統(tǒng)輸入初始數(shù)據(jù)：

推算導(dǎo)航位置

推算導(dǎo)航誤差

AUV航程

當(dāng)前時刻測量得到的地形Z；

先驗地形圖h。

1.計算地形匹配定位的搜索區(qū)間估計(結(jié)合圖中101)

該搜索區(qū)間即使AUV可能的位置區(qū)間，搜索區(qū)間由下式確定：

式中：

λ_x，λ_y表示導(dǎo)航偏差占載體的運行里程的百分比，由參導(dǎo)航系統(tǒng)的統(tǒng)計結(jié)果事先給出；

α≥1表示放大倍數(shù)，一般取(1,1.5]；

以為中心，和為x方向和y方向的長度構(gòu)成的矩形區(qū)域就是搜索區(qū)間X_S。

2.地形匹配定位(結(jié)合圖中102)

根據(jù)發(fā)明步驟1中計算得到的搜索范圍X_S(結(jié)合圖中101)開始進行地形搜索匹配，假設(shè)潮差為t_ij，搜索點ij的先驗地形插值序列(結(jié)合圖中201)與測量地形(結(jié)合圖中103)的高度偏差序列：

假設(shè)地形高度偏差序列滿足白噪聲分布而潮差是常量，得到搜索點ij的潮差和地形測量誤差估計(結(jié)合圖中202)：

得到了潮差的估計值之后，計算測量地形和先驗地形的真實相似程度(結(jié)合圖中203)：

式中：

h(x_ij,y_ij)表示測量序列點(x_ij,y_ij)在先驗地形圖h中的插值結(jié)果；

z_ij∈Z，Z表示測量地形，z_ij表示測量地形內(nèi)的點。

查找似然函數(shù)L_ij值最大的位置(結(jié)合圖中206)可得到地形匹配定位的位置得到定位為之后將定位位置的潮差估計和測量地形的誤差估計作為當(dāng)前定位位置的最優(yōu)潮差估計和測量誤差估計記為：(結(jié)合圖中207)。

3.地形匹配地位結(jié)果的有效性檢驗(結(jié)合圖中107)

根據(jù)步驟2中獲得的地形匹配定位位置下面即使檢驗該定位點的有效性，建立如下的有效性檢驗?zāi)Ｐ停?/p>

(結(jié)合圖中305，結(jié)合圖中306)

式中C_t|t為有效區(qū)間，由下面的方法得到：

(1)以步驟3中得到的似然函數(shù)L_ij為輸入(結(jié)合圖中203)，計算定位點似然函數(shù)的下界L_low(結(jié)合圖中301)；

式中:σ_p表示地形匹配地位點的測量誤差估計；

mn表示地形匹配地位點的個數(shù)；

Z_α/2表示以σ_p為正態(tài)分布的上分位點對應(yīng)的值。

(2)以L_low為等高平面截取步驟2中的似然函數(shù)L_ij，得到的等高線(結(jié)合圖中302)，其中i表示等高線索引號，假設(shè)共有n條等高線；

(3)查找包圍地形匹配定位點(結(jié)合圖中206)的等高線(在等高線所包圍區(qū)域的內(nèi)部)(結(jié)合圖中303)，就是地形匹配定位的置信區(qū)間C_t|t(結(jié)合圖中304)。

4.地形匹配定位結(jié)果的定位誤差估計(結(jié)合圖中105)

其估計方法如下，先計算地形匹配定位點的信息量，I₊，I_-表示正向和負向的信息量，先計算I₊：

式中：m,n表示測量地形的節(jié)點數(shù)目；k,l表示地形節(jié)點的索引號，σ_p表示定位點的測量誤差估計。表示測量地形中的(k,l)號節(jié)點在先驗地形中的插值結(jié)果。由于定位誤差等于信息矩陣的逆P₊＝(I₊)^-1進一步簡化：

式中：

由此得到了定位點在x₊和y₊方向的定位誤差將I₊計算式中的+d換成-d同理可以計算P_-：

由此得到了定位點在x_-和y_-方向的定位誤差以及x₊和y₊方向的定位誤差

將定位誤差標(biāo)記為：

5.地形匹配定位與推算導(dǎo)航的融合濾波(108)

利用推算導(dǎo)航輸出誤差P_t|t和推算導(dǎo)航的位置地形匹配定位位置地形匹配定位誤差和直接求得兩個估計值的濾波結(jié)果：

式中的取值從取得，取值的規(guī)則如下：

A.如果地形匹配定位的位置與推算導(dǎo)航的位置點(結(jié)合圖中402)和點(結(jié)合圖中401)則有：

B.如果地形匹配定位的位置與推算導(dǎo)航的位置點(結(jié)合圖中403)和點(結(jié)合圖中401)則有：

C.如果地形匹配定位的位置與推算導(dǎo)航的位置點(結(jié)合圖中404)和點(結(jié)合圖中401)則有：

D.如果地形匹配定位的位置與推算導(dǎo)航的位置點(結(jié)合圖中405)和點(結(jié)合圖中401)則有：

E.如果地形匹配定位的位置與推算導(dǎo)航的位置點(結(jié)合圖中406)和點(結(jié)合圖中401)則有：

F.如果地形匹配定位的位置與推算導(dǎo)航的位置點(結(jié)合圖中407)和點(結(jié)合圖中401)則有：

G.如果地形匹配定位的位置與推算導(dǎo)航的位置點(結(jié)合圖中408)和點(結(jié)合圖中401)則有：

H.如果地形匹配定位的位置與推算導(dǎo)航的位置點(結(jié)合圖中409)和點(結(jié)合圖中410)則有：

I.如果地形匹配定位的位置與推算導(dǎo)航的位置重合都位于點(結(jié)合圖中401)則有：

下面就可以計算濾波誤差濾波誤差為：

通過以上的步驟就可以實現(xiàn)地形匹配導(dǎo)航的遞推估計。