專利名稱：一種基于等值線的輔助導(dǎo)航定位方法
技術(shù)領(lǐng)域：
本發(fā)明涉及的是一種基于等值線的輔助導(dǎo)航定位方法，屬于導(dǎo)航定位技術(shù)領(lǐng)域，適用于航空或航海中INS精度不高或INS存在較大位置誤差情況下的導(dǎo)航定位。
背景技術(shù)：
無源自主導(dǎo)航是目前國內(nèi)外航行器發(fā)展的主要趨勢之一。慣性導(dǎo)航系統(tǒng)(Inertial Navigation System, INS)能夠全天候提供較全面的導(dǎo)航信息,且具有較強的工作自主性和實時性、無源性好、不向外部發(fā)送信號、抗干擾能力強等諸多優(yōu)點，使得INS成為航行器中必備的導(dǎo)航設(shè)備之一。但是由于INS存在位置誤差積累的問題，無法滿足潛器水下長期高精度的導(dǎo)航需求。因此為提高導(dǎo)航精度、獲得高可靠性的位置信息，必須對INS進行定期修正。
輔助導(dǎo)航系統(tǒng)能很好地解決INS因誤差積累而發(fā)散的問題，其系統(tǒng)主要由測量裝置、INS、存儲在數(shù)字計算機中的先驗地形/重力/地磁數(shù)據(jù)庫及輔助導(dǎo)航算法組成，其中輔助導(dǎo)航算法是輔助導(dǎo)航系統(tǒng)中最關(guān)鍵的技術(shù)之一。目前，常用的輔助導(dǎo)航算法主要分為基于地形/重力/地磁輪廓的匹配算法和基于最優(yōu)濾波估計的匹配算法兩大類。最近幾年對輪廓匹配算法的研究主要集中在對等值線迭代最近點算法(ICCP)的研究上，ICCP算法在INS初始位置誤差不大的情況下導(dǎo)航精度較高，但在初始匹配誤差超出ICCP算法容許范圍時，容易導(dǎo)致發(fā)散甚至誤匹配。在這種情況下，申請?zhí)枮?200810118630. 3 的發(fā)明專利中應(yīng)用 TERCOM(The Terrain Contour Matching)和ICCP兩種算法實現(xiàn)了一種組合方法，即用TERCOM算法進行粗匹配，這樣可以很大程度上降低INS的積累誤差；在此基礎(chǔ)上，利用ICCP算法進行精匹配，從而修正慣性導(dǎo)航系統(tǒng)的導(dǎo)航誤差。該組合方法取得了很好的效果，并能在很大程度上抑制ICCP算法在大的匹配誤差情況下發(fā)散的問題。但該組合方式是有局限性的，這是因為TERCOM方法對INS的航向(這里的航向是指航跡向)偏差較為敏感，圖3示出在INS沒有航向偏差時的TERCOM方法匹配示意圖，匹配航跡是與INS指示航跡相平行的一系列高程剖面。實際上，TERCOM方法在匹配過程中隱含了一個條件，即假設(shè)匹配時INS的航向偏差為0或很小的角度，但在潛器的實際航行過程中，INS的航向偏差不可能為0，這就對其精度有較高的要求。當(dāng)航向偏差較大時，INS指示航跡偏離了實際航跡，這時所得到的最佳匹配位置是平行于慣導(dǎo)系統(tǒng)指示航跡的，從而偏離了實際航跡。

發(fā)明內(nèi)容
本發(fā)明的目的是為了解決上述問題，針對傳統(tǒng)輔助導(dǎo)航算法在INS初始誤差較大情況下容易發(fā)散的問題，提出一種基于等值線的輔助導(dǎo)航定位方法，本發(fā)明能大幅度抑制這種發(fā)散并提高INS導(dǎo)航精度的組合導(dǎo)航方法。一種基于等值線的輔助導(dǎo)航定位方法，具體步驟如下步驟一根據(jù)INS的誤差置信范圍劃定搜索區(qū)域；
步驟二 在搜索區(qū)域內(nèi)，根據(jù)測量裝置實時測得的數(shù)據(jù)提取出等值線；步驟三利用價值函數(shù)等值線匹配算法，在等值線上找到離真實航跡較為接近的航跡；步驟四利用ICCP算法不斷地進行旋轉(zhuǎn)和平移變換，獲得最終的航跡向和最近占.步驟五采用最近點加密的方法，獲得最佳匹配航跡，進行導(dǎo)航定位。本發(fā)明的優(yōu)點在于本發(fā)明針對目前常用的輔助導(dǎo)航算法不允許INS有較大的初始匹配誤差的局限性，提出利用價值函數(shù)等值線匹配算法降低INS初始匹配誤差，利用加密的ICCP算法進一步提高匹配精度的方法，本發(fā)明所述方法在大的初始匹配誤差下仍能達到很高的定位精度且增加了系統(tǒng)的可靠性。


圖I是本發(fā)明的方法流程圖；圖2是本發(fā)明的最近點加密方法原理圖；圖3是背景技術(shù)中TERCOM方法匹配示意圖；圖4是在初始匹配誤差為6. 0'時，ICCP算法仿真圖；圖5是在初始匹配誤差為6. 0'時，ICCP算法最終仿真圖；圖6是在初始匹配誤差為6. 0'時，本發(fā)明所述方法的仿真圖。
具體實施例方式下面將結(jié)合附圖和實施例對本發(fā)明作進一步的詳細說明。本發(fā)明的一種基于等值線的輔助導(dǎo)航定位方法，原理是當(dāng)航行器開始進行輔助導(dǎo)航時，在較大的初始匹配誤差下，利用基于價值函數(shù)的等值線匹配算法在置信區(qū)域內(nèi)進行一次大范圍搜索，找到一條離實際航跡較為接近的航跡來降低INS的位置誤差，從而形成待匹配航跡；在此基礎(chǔ)上利用ICCP算法進行進一步匹配，獲得精確的航跡向和最近點信息；加密的方法是利用上述得到的航跡向和最近點信息在等值線上連續(xù)地尋找使價值函數(shù)最小的航跡作為最終匹配航跡，從而很大程度上提高了輔助導(dǎo)航精度及可靠性，可以滿足航行器在INS大的位置誤差情況下的導(dǎo)航需求。本發(fā)明是一種基于等值線的輔助導(dǎo)航定位方法，流程如圖I所示，包括以下幾個步驟步驟一根據(jù)INS (慣性導(dǎo)航系統(tǒng))的誤差置信范圍劃定搜索區(qū)域；搜索區(qū)域是以X為中心，以3倍的INS估計位置誤差為半邊長的正方形區(qū)域，其中，X表示由INS輸出的當(dāng)前的地形/重力/地磁位置點。步驟二 在搜索區(qū)域內(nèi)，根據(jù)測量裝置實時測得的數(shù)據(jù)提取出等值線；具體為航行器勻速行駛進某一匹配區(qū)Q后，測量裝置實時測得的地形/重力/地磁數(shù)據(jù)經(jīng)過數(shù)據(jù)處理后，獲得M個地形/重力/地磁數(shù)據(jù)點序列，利用該M個數(shù)據(jù)點序列從預(yù)存的地形/重力/地磁數(shù)據(jù)庫中提取出相應(yīng)的等值線，并在步驟一的搜索區(qū)域內(nèi)截取等值線，記為 C”i = 1，2，…，M。同時，由INS獲得M個地形/重力/地磁位置點Xi, i = 1，2，…，M和前M_1個位置點對應(yīng)的航向信息，這里M個地形/重力/地磁位置點分別對應(yīng)于M條等值線。將M個相鄰的位置點做差獲得M-I個相對距離,與前M-I個位置點的航向信息一起作為價值函數(shù)等值線匹配算法的輸入量。步驟三利用價值函數(shù)等值線匹配算法，在等值線上找到離真實航跡接近的航跡，從而形成待匹配航跡；具體為在等值線間通過搜索、跟蹤和決策來連續(xù)的尋找使價值函數(shù)最小的兩點作為最佳匹配位置點，在等值線上找到離真實航跡接近的航跡，形成待匹配航跡。所述的價值函數(shù)等值線匹配算法是依據(jù)INS在短時間內(nèi)兩點間相對距離與實際的相對距離偏差較小和航向精度很高的特點而來。價值函數(shù)等值線匹配算法不依賴于INS 初始匹配位置，由此能在INS大的初始位置誤差下工作，同時該算法需要航行器在兩個匹配點保持直線航行。孫楓等人提出價值函數(shù)等值線匹配算法并給出了基本原理及可行性分析可參見，孫楓，王文晶，高偉等.用于無源重力導(dǎo)航的等值線匹配算法.儀器儀表學(xué)報，2009。本發(fā)明利用基于價值函數(shù)等值線匹配算法對初始位置調(diào)優(yōu)，從而降低INS初始定位誤差，形成待匹配航跡，在此基礎(chǔ)上，利用ICCP算法進行精匹配。步驟四利用ICCP算法不斷地進行旋轉(zhuǎn)和平移變換，獲得最終的航跡向和最近占.利用ICCP算法在初始誤差較小時能給出精確的航跡向及最近點信息，從而利用該信息為最近點加密方法做好準備。所述的利用ICCP算法獲得最終的航跡向和最近點具體方法為根據(jù)ICCP算法利用四元數(shù)進行旋轉(zhuǎn)變換的基本原理，對整條待匹配航跡進行旋轉(zhuǎn)平移變換，旋轉(zhuǎn)角度為ak，其公式如下
A、，= R(Ch)WlWlk
I, =X
(cosor, -Sinori')
Riak)=..
I ^I M
Ph = Uyw hp_h X ^-Yw lX , g, Pf/ 今 '* I” 『今 a f Jr
=p^T T|式中Xi,k表示待匹配航跡經(jīng)過第k次旋轉(zhuǎn)變換后的第i個點所在的位置，Xijk+1表示待匹配航跡經(jīng)過第k+1次旋轉(zhuǎn)變換后的第i個點所在的位置，Ti, k表示經(jīng)過第k次旋轉(zhuǎn)后第i個點的平移量，Pg,k、Xg,k分別表示經(jīng)過第k次旋轉(zhuǎn)后的最近點和匹配航跡點的重心，R(ak)為由a,組成的反對稱旋轉(zhuǎn)矩陣，Pi,,表示經(jīng)過第k次旋轉(zhuǎn)后第i個位置點所對應(yīng)的最近點，權(quán)值Wi, k為經(jīng)過第k次旋轉(zhuǎn)后待匹配航跡點Xi, k到相應(yīng)最近點Pi, k距離的倒數(shù)，W為權(quán)值Wi, k之和。經(jīng)過最后一次旋轉(zhuǎn)變換后，獲得最終的位置點Xi, k所對應(yīng)的最近點Pi, k和相鄰兩點Xi^XiIk所形成的航跡向；
步驟五采用最近點加密的方法，獲得最佳匹配航跡。具體包括以下幾個步驟(I)計算相鄰距離，根據(jù)INS給定的位置坐標(biāo)點Xi,Q Ci = 0,1,2,…，M)，分別計算Xij0到Xi+u間的距離Li,其中M表示所需匹配的總點數(shù)，即Li = I I Xpli0-Xi,01(2)分段等值線，在搜索區(qū)域所截取的等值線Ci上找到利用ICCP算法最后一次旋轉(zhuǎn)變換的位置點Xi,k所對應(yīng)的最近點Pi,k，然后采用加密的方法以Pu為中心，以A為步長將等值線Ci劃分為2Xm段，確保真實位置點包含在mX A范圍內(nèi)，每個分段點記為4(/ = 0丄2,-%2/ ),然后以圮為起始點，沿1到\+1,1;的方向(最后一次旋轉(zhuǎn)后的航跡向Hm)向等值線Ci+1分別作射線，如圖2所示，相交點記為，其中j表示存在相交點的段數(shù)，滿足j < I。 其中，所述的求解相交點/仏,—的方法為構(gòu)造兩條直線，即經(jīng)過分段點& 方向為尤的一條直線和由等值線Ci+1上的兩點順次連接所組成的任意一條直線(已用角度判別法去除不相交的線段)，利用上述兩條直線聯(lián)立方程組，從而求得相交點d— o其中，所述的步長A的選取方法為由于方位陀螺漂移和北向陀螺漂移，引起INS在經(jīng)度方向上的位置誤差隨時間增長而發(fā)散，若取一般精度的INS (陀螺漂移為0.01° /h)，其位置誤差約為In mile/h，選取測量裝置采樣周期為5min，則所選取的INS在5min之內(nèi)位置誤差大約為0.083'，本發(fā)明在該直線上取小間隔的間距時，即以該位置誤差為基準，采取折中的辦法，考慮到INS呈震蕩性發(fā)散，則取每個間距為0. 0415'，間隔連續(xù)地取為10個，S卩上下共取20個數(shù)據(jù)點，這樣可以保證定位誤差包含在該區(qū)域內(nèi)，從而降低誤匹配的可能。(3)求取價值函數(shù)，求取匹配時刻所形成的每一條航跡的價值函數(shù)入；化4) 其中a表示可能形成的航跡條數(shù)，Np表示所需采樣點的總個數(shù)。判斷使人、《AM最小的價值函數(shù)Da (Pi, Li)，其公式如下式所示
權(quán)利要求
1.一種基于等值線的輔助導(dǎo)航定位方法，其特征在于，包括以下幾個步驟 步驟一根據(jù)INS的誤差置信范圍劃定搜索區(qū)域； 搜索區(qū)域是以X為中心，以3倍的INS估計位置誤差為半邊長的正方形區(qū)域，其中，X表示由INS輸出的當(dāng)前的地形/重力/地磁位置點； 所述的INS為慣性導(dǎo)航系統(tǒng)； 步驟二 在搜索區(qū)域內(nèi)，根據(jù)測量裝置實時測得的數(shù)據(jù)提取出等值線； 具體為 航行器勻速行駛進某一匹配區(qū)Q后，測量裝置實時測得的地形/重力/地磁數(shù)據(jù)經(jīng)過數(shù)據(jù)處理后，獲得M個地形/重力/地磁數(shù)據(jù)點序列，利用該M個數(shù)據(jù)點序列從預(yù)存的地形/重力/地磁數(shù)據(jù)庫中提取出相應(yīng)的等值線，并在步驟一的搜索區(qū)域內(nèi)截取等值線，記為Ci, i = 同時，由INS獲得M個地形/重力/地磁位置點Xi, i = 1，2，…，M和前M-I個位置點對應(yīng)的航向信息，M個地形/重力/地磁位置點分別對應(yīng)于M條等值線個相鄰的位置點做差獲得M-I個相對距離,與前M-I個位置點的航向信息一起作為步驟三中價值函數(shù)等值線匹配算法的輸入量； 步驟三利用價值函數(shù)等值線匹配算法，在等值線上找到離真實航跡接近的航跡，從而形成待匹配航跡； 在等值線間通過搜索、跟蹤和決策來連續(xù)的尋找使價值函數(shù)最小的兩點作為最佳匹配位置點，在等值線上找到離真實航跡接近的航跡，形成待匹配航跡； 步驟四利用ICCP算法不斷地進行旋轉(zhuǎn)和平移變換，獲得最終的航跡向和最近點； 所述的ICCP為等值線迭代最近點算法，步驟四具體包括 根據(jù)ICCP算法利用四元數(shù)進行旋轉(zhuǎn)變換的基本原理，對整條待匹配航跡進行旋轉(zhuǎn)平移變換，旋轉(zhuǎn)角度為ak，其公式如下 式中Xi，k表示待匹配航跡經(jīng)過第k次旋轉(zhuǎn)變換后的第i個點所在的位置，Xm表示待匹配航跡經(jīng)過第k-l次旋轉(zhuǎn)變換后的第i個點所在的位置，Tu表示經(jīng)過第k次旋轉(zhuǎn)后第i個點的平移量，Pg,k> Xg,k分別表示經(jīng)過第k次旋轉(zhuǎn)后的最近點和匹配航跡點的重心，R(ak)為由a k組成的反對稱旋轉(zhuǎn)矩陣，Pu表示經(jīng)過第k次旋轉(zhuǎn)后第i個位置點所對應(yīng)的最近點，權(quán)值Wu為經(jīng)過第k次旋轉(zhuǎn)后待匹配航跡點Xu到相應(yīng)最近點Pu距離的倒數(shù)，W為權(quán)值Wi, k之和； 經(jīng)過最后一次旋轉(zhuǎn)變換后，獲得最終的位置點所對應(yīng)的最近點Pi, k和相鄰兩點Xi,k、xi+1,k所形成的航跡向t ; 步驟五采用最近點加密的方法，獲得最佳匹配航跡； 具體包括以下幾個步驟 (1)計算相鄰距離，根據(jù)INS給定的位置坐標(biāo)點XiMi = 1，2，…，M，分別計算Xi,。到Xi+1,0間的距離Li,其中M表示所需匹配的總點數(shù)，即 Li = I Xi+lj0-Xi；0 (2)分段等值線，在搜索區(qū)域所截取的等值線Ci上找到利用ICCP算法最后一次旋轉(zhuǎn)變換的位置點Xi,k所對應(yīng)的最近點Pi,k，然后采用加密的方法以Pu為中心，以A為步長將等值線Ci劃分為2Xm段，確保真實位置點包含在mX A范圍內(nèi)，每個分段點記為/ . 1=O，I, 2，…，2m，然后以/匕為起始點，沿Xu到Xi+1,k的方向，即最后一次旋轉(zhuǎn)后的航跡向A^u^,；，向等值線Ci+1分別作射線，相交點記為/^;ti.，其中j表示存在相交點的段數(shù)，滿足j ^ I ; (3)求取價值函數(shù)，求取匹配時刻所形成的每一條航跡的價值函數(shù)人^(6，4)，其中a表示可能形成的航跡條數(shù)，Np表示所需采樣點的總個數(shù)；判斷使最小的價值函數(shù)Da (Pi, Li),其公式如下式所示
2.根據(jù)權(quán)利要求I所述的一種基于等值線的輔助導(dǎo)航定位方法，其特征在于，步驟五(2)中，所述的求解相交點/丨.^的方法為構(gòu)造兩條直線，即經(jīng)過分段點圮,方向的一條直線和由等值線Ci+1上的兩點順次連接所組成的任意一條直線，其中已用角度判別法去除不相交的線段，利用上述兩條直線聯(lián)立方程組，從而求得相交點/丨^
3.根據(jù)權(quán)利要求I所述的一種基于等值線的輔助導(dǎo)航定位方法，其特征在于，步驟五(2)中，所述的步長A的選取方法為取步長A為0.0415'，間隔連續(xù)地取為10個，即上下共取20個數(shù)據(jù)點。
4.根據(jù)權(quán)利要求I所述的一種基于等值線的輔助導(dǎo)航定位方法，其特征在于，步驟五(3)中，在所有的匹配航跡中，當(dāng)使價值函數(shù)人、(盡最小的匹配航跡不止一條時，選取最接近最近點的位置作為最佳匹配點，去除相對較遠的點。
全文摘要
本發(fā)明公開了一種基于等值線的輔助導(dǎo)航定位方法，具體地說，當(dāng)航行器開始進行地形/重力/地磁匹配時，在較大的初始匹配誤差下，利用基于價值函數(shù)的等值線匹配算法在置信區(qū)域內(nèi)找到一條離實際航跡較為接近的航跡來降低慣性導(dǎo)航系統(tǒng)(INS)的初始定位誤差；然后，利用ICCP算法的旋轉(zhuǎn)和平移變換獲得航跡向及最近點序列信息；最后，利用得到的航跡向及最近點信息在等值線上采用加密的方法得到最佳的匹配航跡。本發(fā)明可以解決當(dāng)前輔助導(dǎo)航系統(tǒng)在大的初始定位誤差情況下易發(fā)散的問題，提高輔助導(dǎo)航系統(tǒng)的精度及可靠性，尤其適用于存在探測盲區(qū)情況下的輔助導(dǎo)航定位。
文檔編號G01C21/16GK102809376SQ201210276928
公開日2012年12月5日 申請日期2012年8月6日 優(yōu)先權(quán)日2012年8月6日
發(fā)明者李寧, 張紅偉, 張勇剛, 劉利強, 齊昭 申請人:哈爾濱工程大學(xué)