專利名稱：地形輔助導航方法和設備的制作方法
技術領域：
本發(fā)明提供了一種地形輔助導航方法和設備，該方法和設備 特別在初始誤差較大的情況下，使用低精度慣性導航系統(tǒng)
(Inertial Navigation System, INS)完成匹配，實現(xiàn)巡航導 彈、低空無人機、飛機、潛艇、水下機器人等的低成本高精度導 航。
背景技術：
在巡航導彈等低空飛行器或潛艇的導航中， 一般需要有較高 精度的INS,但是高精度INS造價高，而且，高精度INS的導航誤差 也隨運行時間累積，致使INS無法獨立長時間工作。所以，需要其 它輔助導航方法來修正INS的累積誤差，以保持導航系統(tǒng)的長時間 高精度工作。相對于全球定位系統(tǒng)(Global Positioning System, GPS)等其它輔助導航方法，地形輔助導航具有如下優(yōu) 勢在低空飛行器中，無地形遮蔽，適合于貼地飛行或地形掩 蔽；在水下潛艇中，不受水體遮蔽影響。但是，目前的地形輔助 導航方法對INS精度的依賴性仍比較大。因此，研究一種在低精度 INS條件下的地形輔助導航方法在降低應用成本和小型化等方面都 是有實際意義的。
當載體運行到地形可導航區(qū)之后，INS—般具有較大的位置和 姿態(tài)誤差，因而需要使用適合于大初始誤差的快速定位方式，地 形輪廊匹西己(Terrain Contour Matching, TERC0M)方'法可k乂實 現(xiàn)大初始位置誤差時的快速搜索，可以很快找到大致的匹配區(qū) 域，但其精度受INS精度影響較大，在大航跡形狀誤差條件下無法 正常工作；迭代最近等值點(Iterative Closest Contour Point, ICCP)算法利用高程等值線信息修正INS航跡，使INS航跡 逐漸向真實航跡靠攏，達到降低姿態(tài)誤差的目的，因而減小了匹
配方法對INS精度依賴性，但ICCP算法需要較小的初始定位誤差才 能穩(wěn)定工作。本發(fā)明充分結合了TERCOM和ICCP算法的優(yōu)點，進行 了優(yōu)勢互補，可以應用在巡航導彈和飛機這類空中飛行器上，也 可應用在潛艇和水下機器人等水下載體的地形輔助導航方面，適 合于低精度I NS條件下的長時間航行。
地形輔助導航方法的基本工作原理如圖l所示，首先將匹配區(qū) 域的地形數(shù)字化，并以格網數(shù)字地形的形式存貯在導航計算機 中，當載體通過已經數(shù)字化的匹配地形區(qū)域時，用高程測量設備 101測量載體所在位置處的高程值，得到對應于栽體航跡的一條地 形剖面的地形高程采樣值/序列，另外通過INS 103可知載體真實 位置的大體范圍，在此范圍內將測得的地形高程值/序列與預先存 儲在導航計算機中的數(shù)字地圖104—同輸入導航計算機102中，進 行匹配計算，從而得到匹配的位置，再將該匹配位置反饋給INS 103, 1多正INS累積誤差。
TERCOM的工作過程如圖2所示。該圖是以INS當前輸出位置為 中心，位置估計誤差方差的3倍為半邊長，構成初始搜索窗口，其 中每個格網的頂點表示一個高度數(shù)據(圖2中共有17xl7個高程數(shù) 據)。載體在真實航跡上運行，每隔若干個格網距離采集一個高 程數(shù)據并記錄此時的INS位置，假設截至A時刻累計的高程組成一 個高程采樣序列，將該高程采樣序列與搜索窗口內所有可能的方 向相同、長度一致的基準地圖內的假定航跡逐一進行相關分析 (圖2中共有17xl7條假定航跡)，具有最佳相關值的位置即被確 定為栽體真實位置的估計，即匹配位置，如圖2中的三角形點所示。
TERCOM能夠進行大范圍的搜索并迅速收斂；不過要求INS航跡 形狀和航向都要與真實航跡相近，不能有太大的偏差，否則會因 為搜索窗口內尋找到的假定航跡與真實航跡偏差太大而導致數(shù)據 錯位，匹配誤差加大直至發(fā)散。
如圖3所示是ICCP算法的基本原理圖。載體真實航跡由f' (/ = 1, 2, L,『)點組成，其中『是航跡的長度(點數(shù))；INS 航跡由f點組成；另外，地形測量傳感器測量到當?shù)貙嶋H地形 c,,每一高度數(shù)據對應于地形圖中的一條等高線。由于INS存在測
量誤差，S與f'之間不可避免地存在一定的偏差。ICCP算法的思想就是f 一定位于c,等值線上或附近，那么可以按照一定原 則，使《"靠攏到c,上，找到最優(yōu)估計點,和航跡，實現(xiàn)對INS的誤
差補償。這實際上是一種最優(yōu)化過程，對于由『個點所組成的曲 線段，唯一可以調整的是這些點都相應位于各自等值線，而且可 以在各自等值線上滑動，最終使目標函數(shù)值達到最小。以總的誤 差作為目標函數(shù)，即
<formula>formula see original document page 9</formula> (1)
式中￡為目標函數(shù)，即總誤差；"(P, q)為P與q之間的距離；x,
為估計點,的位置，y,為x,離等值線最近的點的位置；a,為INS指
示位置；《為剛度系數(shù)。
這里認為INS初始誤差為零，如圖3所示c。等高線上的真實位 置、INS位置和估計位置重合；按照最優(yōu)化原理，對式(1)的目 標函數(shù)進行優(yōu)化調整，即改變估計點的位置，使￡最小，并認為 該序列是對當前真實航跡的最優(yōu)估計，即完成對,序列的估計。
如圖4所示為載體的速度誤差和方向誤差對一步航行的影響， 其中A為載體速度誤差，《為載體航向誤差，1),+1為沿航行方向的 單位矢量，^為水平面內與b,w相垂直方向的單位矢量。由圖4可 以得到式(2)。
<formula>formula see original document page 9</formula> ( 2 )
由于A和《均為小量，所以式(2)可以近似為式(3)。
<formula>formula see original document page 9</formula> (3 )
式中<formula>formula see original document page 9</formula>，其中a。表示本次匹配的前面一 點INS的位置。將式(3 )代入式(1 )可得式(4 )。
<formula>formula see original document page 9</formula> (4)
在已知a,、 a和《的前提下，x, (/ = 1， 2, L,『)可以才艮據 式(3)計算出來，因此x,是^和《的函數(shù)。另外，y,是根據x,推 算出來的，因此，y,也是A和e,的函數(shù)。
優(yōu)化過程就是對式(4)進行最優(yōu)化處理，由于x,和y,均是p, 和《的函數(shù)，因此優(yōu)化變量就是^和《，目標函數(shù)是￡。
在《趨于0時，由于航跡的任何小的變化都可能導致目標函數(shù)
值大幅度變化，因此，只能不對航跡進行任何調整，而是通過剛 性變換(旋轉和平移)使目標函數(shù)趨于最優(yōu)值。
如上所述，基本ICCP算法適合于INS航跡形狀偏差較大的情 況；但是該方案要求INS初始誤差很小，另外剛性變換方案的ICCP 算法要求INS的形狀誤差很小，所以目前的ICCP算法都不能夠滿足 實際應用的需求。

發(fā)明內容
本發(fā)明解決的技術問題是在大初始誤差和低精度INS的條件
下，研究一種能夠可靠進行輔助導航的地形匹配方法和設備，優(yōu)
點是能夠大范圍搜索，快速收斂，修正INS的航跡形狀，降低導航
系統(tǒng)的成本，提高導航的精度。
根據本發(fā)明的一個方面，提供了一種適合于空中或水下的地
形輔助導航方法，其可用于大初始誤差和低精度INS的地形輔助導
航，其特征在于包括
確定一個包含當前INS位置的窗口 ；
在所述窗口中，采用TERCOM方法計算粗匹配位置；
利用所述粗匹配位置^"正所述INS位置，在粗匹配位置上疊加
I NS航跡形狀作為待調整航跡；
使用ICCP算法對該待調整航跡進行調整，使待調整航跡向真
實航跡接近。
根據本發(fā)明的一個進一步的方面，提供了一種適合于空中或 水下的地形輔助導航設備，其可用于大初始誤差和低精度INS的地 形輔助導航，其特征在于包括
導航位置窗口確定裝置，用于確定一個包含當前INS位置的窗
o;
粗匹配位置計算裝置，用于在所述窗口中采用TERCOM方法計 算粗匹配位置；
待調整航跡生成裝置，用于利用所述粗匹配位置修正所述INS 位置，在粗匹配位置上疊加INS航跡形狀作為待調整航跡；
航跡調整裝置，用于使用ICCP算法對該待調整航跡進行調 整，使待調整航跡向真實航跡接近。本發(fā)明的原理是利用TERC0M以及加權平均以防止誤匹配的 方法選取粗匹配位置，提高匹配方法的快速性和魯棒性，在粗匹 配位置上疊加INS航跡形狀作為待調整航跡，使用ICCP算法將待調 整航跡向著真實航跡靠攏；利用等值線修正待調整航跡，即是用 高程信息修正INS航跡，利用單純形調優(yōu)法尋找到最小的目標函數(shù) 值，同時利用概率數(shù)據關聯(lián)算法提高精匹配的可靠性，從而獲得 最優(yōu)匹配航跡。
本發(fā)明與現(xiàn)有技術相比的優(yōu)點在于相關方法適合于航跡形 狀誤差較小的情況下，因而要求INS精度較高；卡爾曼濾波方法同 樣要求INS精度較高；單純的ICCP算法需要初始誤差較小或航跡形 狀偏差小。本發(fā)明的方法能夠適應較低精度的INS航跡，能夠對 INS航跡進行修正，這是以前技術中所未曾有的。很多ICCP算法釆 用剛性變換的方法實現(xiàn)待匹配航跡與真實航跡的配準，但是其應 用受到I NS航跡形狀誤差的極大制約；本發(fā)明采用非剛性變換的方 法，可以實現(xiàn)航跡的任意變換，可以更加逼近真實航跡的形狀。


圖1為地形輔助導航方法結構 圖2為TERCOM方法示意圖 圖3為ICCP算法原理圖
圖4為載體速度和方向誤差對位置誤差的影響
圖5為本發(fā)明所使用的ICCP算法原理圖
圖6為(A,《)選取范圍示意圖
圖7為兩次優(yōu)化位置分布示意圖
圖8為方法實現(xiàn)流程圖
圖9為粗匹配實現(xiàn)流程圖
圖10為ICCP調整實現(xiàn)流程圖
圖11為誤匹配判斷實現(xiàn)流程圖
具體實施例方式
本發(fā)明的流程如圖8所示。 (1 )本發(fā)明中TERCOM方法所采用的基本算法包括:
互相關算法(Cross Correlat ion， CC或Product correlation similarity measure, Prod), 歸一化互相關算法 (Normalized Product correlation similarity measure, NProd);絕對差算法(Absolute Difference, AD)，平方差算 法(Square Difference, SD)，平均絕對差算法(Mean Absolute Difference, MAD),平均平方差算法(Mean Square Difference, MSD);改進的絕對差、改進的平方差算法，例如利 用衰減系數(shù)改進的；基于Hausedorff距離的匹配算法。
(2) 本發(fā)明首先進行步驟801確定粗匹配搜索窗口采用3cr 原則，以INS位置為中心開一個6x6o"的方形搜索窗口。
(3) 本發(fā)明采用一種衰減絕對差方法作為TERCOM方法來實現(xiàn) 粗匹配802。圖9是粗匹配802的進一步流程圖。該方法是一種迭代 計算方法，將衰減絕對差方法的量測記為"AD。參考圖2，步驟 901進行獲取搜索窗口內數(shù)字地圖的過程，然后步驟902定義yt時 刻的單點誤差表示為
<formula>formula see original document page 12</formula> ( 5 )
其中附、"表示格網相對窗口中心的位置；w表示A:時刻 的測量高程；^W(A+w,A+")表示地圖中的高程值；A、 A表示的 是搜索窗口的中心位置。
定義aAD如式(6 )所示，圖2中，"AD為一17xl7大小的矩陣。<formula>formula see original document page 12</formula><formula>formula see original document page 12</formula>
步驟903選取矩陣aAD中最小的f個值，步驟904判斷上述^個 點位置的分布方差是否小于設定的閾值，"是"則說明粗匹配方 法收斂，程序進行到步驟9 06以利用加權平均法計算粗匹配位置， "否"則說明粗匹配方法還沒有收斂，程序進行到步驟905,以 INS位置作為定位輸出。
設滿足要求的,個點位置表示為<formula>formula see original document page 12</formula>對應權重 為A(",步驟906中的權重計算式為式(7)。
<formula>formula see original document page 13</formula>最后的估計位置表示為式(8)。
<formula>formula see original document page 13</formula>(8)
步驟803利用粗匹配位置和INS航跡形狀累計待調整航跡，步 驟907判斷ICCP調整的點數(shù)是否累積達到『個，"是"則進行ICCP 調整804,"否"則進行步驟908輸出粗匹配位置作為最終匹配位 置。
(4)所使用的ICCP調整804如下
本發(fā)明中ICCP調整804的過程如圖10所示。ICCP方法是一種批 處理方法，需要進行步驟803累計足夠數(shù)目的待調整航跡點才能進 行ICCP調整804,如果還沒有足夠數(shù)目，此時應以粗匹配位置作為 定位輸出(步驟908 ); —旦數(shù)目達到要求，進行ICCP調整804。
圖5為改進后的ICCP方法，P。"表示上次匹配位置，其范圍依 賴于整個地形輔助導航方法的精度；P。'為真實位置；A表示粗匹 配位置。
改進后的ICCP算法為利用粗匹配位置疊加INS航跡形狀形成 待調整航跡，取代INS航跡；仍采用式(4)所示的目標函數(shù)，不 過此時的3,(/ = 1,2,...,)為待調整航跡，a。為上次最終匹配位置。
步驟1001中A和《的初值隨機選取，但是由于受到粗匹配的 影響，各點的變化范圍有所不同。如圖6所示。戶。"點表示上次最 終匹配位置(即a。)，《"與INS的關系并不顯著，而(A,《) 為兩者之間進行誤差轉移時的參數(shù)， 一般要使估計位置《"位于可 能范圍之內，即圖6中的圓型區(qū)域。因此(化《)的值一般比較 大，視粗匹配精度而定。此后的估計航跡應該與INS航跡具有一定 的相關性，所以(p2,《)可以根據INS速度和航向偏差方差的3cr
原則選取；其它點都是在前一點的基礎上增加一個小隨機量即可 (小隨機數(shù)視INS短時漂移量大小而定)。如式(9)所示。
<formula>formula see original document page 13</formula>x,和y,均為A和《的函數(shù)，利用初始化的A和《可以進行步驟 1002，計算得出估計位置x,和最近點位置y,，繼而利用式(4)進 行步驟1003,計算目標函數(shù)值，最后ICCP算法轉化為多參數(shù)尋優(yōu) 的數(shù)學問題，利用步驟1004進行改進單純形調優(yōu)法經過反射、擴 張和收縮來尋找最優(yōu)解，并在步驟1005中將該最優(yōu)解作為該次的 最優(yōu)位置輸出。
(5)在lCCP調整804過程中調整結果的i吳匹配判斷中，所使 用的概率數(shù)據關聯(lián)防止誤匹配方法如圖ll所示。
為了獲得全局最優(yōu)，對單純形優(yōu)化共計進行e組iccp調整，
其目標函數(shù)收斂到Emn或者迭代次數(shù)超過閾值 ^時，記錄此時的 目標函數(shù)五值作為匹配的指標。
給較小的￡值優(yōu)化位置以較大概率，給較大的￡值優(yōu)化位置 以較小概率，并保證總的概率等于l,首次概率計算見式(IO)。
如。 (10)
其中A, = 1,2,...,2)是各次優(yōu)化位置的概率。
步驟1101:將前次的優(yōu)化位置的概率記為A.U'",2,…,2)，本
次的優(yōu)化位置的概率記為w=i,2,.,e)，將兩次的iNs位置重合，
可以把兩次優(yōu)化的位置放在一個圖中，圖7所示為合為一圖的示意 圖，其中共有前次和本次優(yōu)化的各2個位置以及前次和本次的INS 位置點，各優(yōu)化位置都對應著一個編號。
步驟1102:假定本次的每一個優(yōu)化位置都可能來自于前次的 任何一個優(yōu)化位置，如果INS定位誤差服從N(o,)的高斯分布，那
么前次優(yōu)化位置的第/點對本次優(yōu)化位置的第!點作用的概率密度 如式(ll)所示。
沖|7') =+/^ (11)
其中血為/、 /兩個優(yōu)化位置經過INS整合后的距離，即前次優(yōu)化 第)'點在本次優(yōu)化中的預測位置與本次優(yōu)化位置第/點之間的距 離。
步驟1103:本次的e個優(yōu)化位置都可能來自于前次優(yōu)化位置 中的第/點，可以根據其概率密度來決定本次各點的條件概率，
如式(12)所示。
劇=^^ (12)
步驟l 104:利用式(12)和全概率公式可以獲得前次2個優(yōu)化 位置對本次第/個優(yōu)化位置的影響，即笫/個優(yōu)化位置的聯(lián)合概 率，如式(13)所示。
w)=i;/n('i)') (13)
步驟1105:在g值的基礎上加入聯(lián)合概率，匹配概率的計算 如式(14)所示。
A、攀' (14) 選取最大概率的優(yōu)化位置作為本次最終匹配結果。
權利要求
1、一種適合于空中或水下的地形輔助導航方法，其可用于大初始誤差和低精度慣性導航系統(tǒng)的地形輔助導航，其特征在于包括A)確定一個包含當前慣性導航位置的窗口；B)在所述窗口中，采用TERCOM方法計算粗匹配位置；C)利用所述粗匹配位置修正所述慣性導航位置，在粗匹配位置上疊加慣性導航航跡形狀作為待調整航跡；D)使用ICCP算法對該待調整航跡進行調整，使待調整航跡向真實航跡接近。
2、 根據權利要求l所述的地形輔助導航方法，其特征在于 步驟B)所使用的粗匹配定位方法的包括互相關算法、歸一化互 相關算法、絕對差算法、平方差算法、平均絕對差算法、平均平 方差算法、改進的絕對差、改進的平方差算法、和/或基于 Hausedorff距離的匹配算法。
3、 根據權利要求l所述的地形輔助導航方法，其特征在于 步驟B)進一步包括對于粗匹配位置因為地形相似性而出現(xiàn)的較大跳變，利用加 權平均方法獲得比較可靠的匹配位置；利用TERC0M方法所計算得出的參數(shù)計算權值。
4、 根據權利要求l所述的地形輔助導航方法，其特征在于 步驟D)所使用的ICCP算法進一步包括把粗匹配位置疊加INS航跡形狀，形成待調整航跡； 按照真實航跡位于c,等值線附近的規(guī)律，根據一個預定規(guī) 則，使待調整航跡靠攏到c,上，并確定最優(yōu)估計航跡，從而實現(xiàn) 對INS的誤差補償，其中所述預定原則為下式所表示的目標函數(shù)￡ 取最小值式中五為目標函數(shù)即總誤差；r為待調整航跡所包含的點數(shù)； x,為估計點的位置，y,為x,離等值線最近的點的位置；《為剛度系 數(shù)；a為載體速度誤差；《為載體的航向誤差；<formula>formula see original document page 3</formula>且<formula>formula see original document page 3</formula>；a,為待調整航跡指示位置；a。為上次最終匹配位置；其中a和《的初值隨機選取，且(a,《)的范圍根據粗匹配 精度決定；(a,《)根據INS速度、航向偏差方差的3cr原則選 ??；其它點按照如下原則選取<formula>formula see original document page 3</formula>其中小隨機數(shù)視INS短時漂移量大小而定； 其中x,和y,均為a和《的函數(shù)，其中ICCP算法轉化為多參數(shù)尋優(yōu)的數(shù)學問題，利用改進單純 形調優(yōu)法經過反射、擴張和收縮來尋找最優(yōu)的匹配位置。
5、根據權利要求l所述的地形輔助導航方法，其特征在于 步驟D)所使用的ICCP算法進一步包括Dl)為實現(xiàn)全局最優(yōu)，采用隨機采樣方式，即以待調整航跡為基準，對p,和s進行隨機采樣，共計采樣Q組，然后對這e組優(yōu)化初值分別進行ICCP最優(yōu)化計算，其目標函數(shù)收斂到￡min或者迭 代次數(shù)超過閾值7^時，記錄此時的目標函數(shù)￡值作為匹配的指 標；D2 )如下式計算前次優(yōu)化位置上第/點對本次優(yōu)化位置的第z' 點作用，其中而為前次優(yōu)化第j"點在本次優(yōu)化中的預測位置與本次優(yōu) 化位置第!點之間的距離，^為預測位置誤差方差；D3)在本次的2個優(yōu)化位置都可能來自于前次優(yōu)化位置中的 第尸點的情況下，根據其概率密度來決定本次各點的條件概率；D4)利用全概率公式，獲得前次G個優(yōu)化位置對本次第z個優(yōu) 化位置的影響，即第/個優(yōu)化位置的聯(lián)合概率，其中A,'表示前次各優(yōu)化位置的概率D5)在g值的基礎上加入聯(lián)合概率，匹配概率的計算表示為D6)選取最大概率的優(yōu)化位置作為最終匹配結果，然后再順 序執(zhí)行上述步驟D1)、 D2)、 D3)、 D4)、 D5),從而獲得一系列的更 可靠的匹配結果。
6、 一種適合于空中或水下的地形輔助導航設備，其可用于大 初始誤差和低精度慣性導航系統(tǒng)的地形輔助導航，其特征在于包 括導航位置窗口確定裝置，用于確定一個包含當前慣性導航位 置的窗口；粗匹配位置計算裝置，用于在所述窗口中采用TERCOM方法計 算粗匹配位置；待調整航跡生成裝置，用于利用所述粗匹配位置修正所述慣 性導航位置，在粗匹配位置上疊加慣性導航航跡形狀作為待調整 航跡；航跡調整裝置，用于使用ICCP算法對該待調整航跡進行調 整，使待調整航跡向真實航跡接近。
7、 根據權利要求6所述的地形輔助導航設備，其特征在于 所述所使用的粗匹配定位方法的包括互相關算法、歸一化互相關算法、絕對差算法、平方差算法、平均絕對差算法、平均 平方差算法、改進的絕對差、改進的平方差算法、和/或基于 Hausedorff距離的匹配算法。
8、 根據權利要求6所述的地形輔助導航設備，其特征在于 所述粗匹配位置計算裝置進一步包括跳變4務正部分，用于對于粗匹配位置因為地形相似性而出現(xiàn) 的較大跳變，利用加權平均方法獲得比較可靠的匹配位置；權值計算部分，用于利用TERC0M方法所計算得出的參數(shù)計算 權值。
9、 根據權利要求6所述的地形輔助導航設備，其特征在于 所使用的ICCP算法進一步包括把粗匹配位置疊加INS航跡形狀，形成待調整航跡； 按照真實航跡位于c,等值線附近的規(guī)律，根據一個預定規(guī) 則，使待調整航跡靠攏到c,上，并確定最優(yōu)估計航跡，從而實現(xiàn) 對INS的誤差補償，其中所述預定原則為下式所表示的目標函數(shù)￡ 取最小值<formula>formula see original document page 5</formula>式中E為目標函數(shù)即總誤差；W為待調整航跡所包含的點數(shù)； x1為估計點的位置，y1為x1離等值線最近的點的位置；Q為剛度系數(shù)；p1為栽體速度誤差；Q1為載體的航向誤差；<formula>formula see original document page 5</formula>,且<formula>formula see original document page 5</formula>a1為待調整航跡指示位置；a。為上次最終匹配位置；其中p1和Q1的初值隨機選取，且(a,Q)的范圍根據粗匹配精度決定；(A，Q)根據INS速度、航向偏差方差的3c原則選??；其它點按照如下原則選取<formula>formula see original document page 5</formula>其中小隨機數(shù)視INS短時漂移量大小而定； 其中x1和y1均為p,和Q的函數(shù)，其中ICCP算法轉化為多參數(shù)尋優(yōu)的數(shù)學問題，利用改進單純 形調優(yōu)法經過反射、擴張和收縮來尋找最優(yōu)的匹配位置。
10、 根據權利要求6所述的地形輔助導航設備，其特征在于所 述航跡調整裝置所使用的ICCP算法進一步包括Dl)為實現(xiàn)全局最優(yōu)，采用隨機采樣方式，即以待調整航跡為基準，對a，Q進行隨機釆樣，共計采樣e組，然后對這e組優(yōu)化初值分別進行ICCP最優(yōu)化計算，其目標函數(shù)收斂到￡mm或者迭 代次數(shù)超過閾值7時，記錄此時的目標函數(shù)￡值作為匹配的指 標；D2 )如下式計算前次優(yōu)化位置上第/點對本次優(yōu)化位置的第/ 點作用，<formula>formula see original document page 6</formula>其中""為前次優(yōu)化第/點在本次優(yōu)化中的預測位置與本次優(yōu) 化位置第/點之間的距離，"為預測位置誤差方差；D3)在本次的e個優(yōu)化位置都可能來自于前次優(yōu)化位置中的第尸點的情況下，根據其概率密度來決定本次各點的條件概率；D4)利用全概率公式，獲得前次e個優(yōu)化位置對本次第/個優(yōu) 化位置的影響，即第/個優(yōu)化位置的聯(lián)合概率，其中A.'表示前次 各優(yōu)化位置的概率D5)在A值的基礎上加入聯(lián)合概率，匹配概率的計算表示為A:=lD6)選取最大概率的優(yōu)化位置作為最終匹配結果，然后再順 序執(zhí)行上述步驟D1)、 D2)、 D3)、 D4)、 D5)，從而獲得一系列的更 可靠的匹配結果。
全文摘要
本發(fā)明設計了一種適合于空中(巡航彈、無人飛機、直升機等)或水下(潛艇、水下機器人、AUV、UUV等)的，尤其在初始定位誤差和航向誤差較大的情況下，使用低精度慣性導航系統(tǒng)實現(xiàn)的地形輔助導航方法和設備。該發(fā)明包括首先利用TERCOM方法以及加權平均尋找粗匹配位置修正慣導航跡形成待調整航跡；然后使用迭代最近等值點算法對具有較大航向偏差的待調整航跡進行調整；為了防止出現(xiàn)誤匹配，將多次尋優(yōu)的目標函數(shù)與匹配的位置相結合，引入概率數(shù)據關聯(lián)方法提高匹配的可靠性。本發(fā)明能夠在較低的慣導精度下完成匹配，降低巡航導彈和潛艇等低空飛行器和水下載體的導航成本。
文檔編號G01C21/16GK101339036SQ200810118630
公開日2009年1月7日 申請日期2008年8月20日 優(yōu)先權日2008年8月20日
發(fā)明者勇 楊, 王可東 申請人:北京航空航天大學