專利名稱：一種基于三角形約束模型的無源導(dǎo)航的等值線匹配方法
技術(shù)領(lǐng)域：
本發(fā)明涉及了地球物理場(地形/地磁/重力)輔助慣性導(dǎo)航，主要針對輔助導(dǎo)航系統(tǒng)中匹配算法的初始匹配方法。
背景技術(shù)：
地球物理場(地形/地磁/重力)輔助導(dǎo)航方法是一種十分有效的對慣性導(dǎo)航系統(tǒng)誤差進(jìn)行重調(diào)校正的方法，屬于真正意義上的無源導(dǎo)航，因此受到了越來越多的關(guān)注。對于無源導(dǎo)航系統(tǒng)，通常在進(jìn)入地球物理場導(dǎo)航圖適配區(qū)域時，運載體在經(jīng)過了相當(dāng)長時間航行后由單純慣性導(dǎo)航輸出的導(dǎo)航位置和航向角都已積累了較大的誤差，因此，利用地球物理場信息來校正慣導(dǎo)系統(tǒng)的大位置誤差是地球物理場輔助慣性導(dǎo)航應(yīng)首要解決的問題。目前地球物理場輔助導(dǎo)航方法主要有分批處理的TERCOM算法(Terrain Contour Matching)和遞歸處理的 SITAN 算法(Sandia Inertial Terrain Aided Nayi gat ion)兩種。TERCOM 算法主要以 ICCP 算法(Iterated Closest Contour Point)為代表，ICCP 算法首先在地球物理場觀測值所在的等值線上尋找距離慣導(dǎo)指示位置最近的點集，在此基礎(chǔ)上進(jìn)行迭代，求取每次迭代的旋轉(zhuǎn)和平移變換，使上次迭代點集經(jīng)變換后與該點集在等值線上的最近點點集的距離平方價值函數(shù)最小，將經(jīng)過若干次迭代后的結(jié)果作為匹配點集。 ICCP算法是在將慣導(dǎo)指示位置在對應(yīng)等值線上的最近點作為真實位置的基礎(chǔ)上建立起來的，因此算法的應(yīng)用受到慣導(dǎo)指示位置與載體真實位置之間的誤差必須足夠小的假設(shè)條件的約束。SITAN算法利用Kalman濾波技術(shù)對實時采集的測量數(shù)據(jù)進(jìn)行遞歸處理，從而得到慣導(dǎo)系統(tǒng)誤差的最優(yōu)估計。但是，載體位置誤差與地球物理場異常值變化量成線性關(guān)系的假設(shè)只有當(dāng)真實位置在慣導(dǎo)指示位置不遠(yuǎn)處時才成立，在慣導(dǎo)位置誤差很大的情況下會引起誤匹配。由以上的分析可知，無論是TERCOM算法還是SITAN算法，在慣導(dǎo)定位誤差較大的情況下均不可用。

發(fā)明內(nèi)容
本發(fā)明的技術(shù)解決問題克服現(xiàn)有技術(shù)的不足，提供一種基于三角形約束模型的無源導(dǎo)航的等值線匹配方法，它利用慣導(dǎo)系統(tǒng)連續(xù)兩次采樣間的距離信息和相對轉(zhuǎn)向角構(gòu)建高精度三角形幾何約束模型，再基于三角形約束模型進(jìn)行等值線匹配，故而保證三角形的精度只與慣導(dǎo)短時精度有關(guān)，而不受慣導(dǎo)系統(tǒng)長期累積誤差的影響，具有可靠的置信度。本發(fā)明采用的技術(shù)方案步驟如下一種基于三角形約束模型的無源導(dǎo)航的等值線匹配方法，實現(xiàn)步驟如下第一步，判斷運載體已進(jìn)入地球物理場適配區(qū)域Ω，選取運載體所承載的地球物理場傳感器連續(xù)的三個采樣點，三個采樣點所對應(yīng)的時間分別為、、t2和t3時刻；第二步，構(gòu)建三角形幾何約束模型，并結(jié)合地球物理場參考圖所提供的場值與實測地球物理場值之間的尺度變化構(gòu)建幾何約束模型的三角形頂點位置誤差置信區(qū)間δ ； 所述構(gòu)建三角形幾何約束模型的過程為首先獲取慣導(dǎo)系統(tǒng)在時間段、t2、t2t3內(nèi)輸出的運載體相對行駛距離li、I2及在t2時刻的相對轉(zhuǎn)向角θ ；然后，利用Ipl2和θ三個參數(shù)構(gòu)建三角形幾何約束模型；第三步，獲取運載體承載的地球物理場傳感器在、、t2和t3時刻三個采樣點地球物理場值分別對應(yīng)的全部等值線Cp C2和C3 ；第四步，構(gòu)建運載體在采樣點、時刻慣導(dǎo)系統(tǒng)提供的航向角扇形誤差置信區(qū)間V， 然后以等值線C1的點集P1內(nèi)任意一點為起點，基于航向角扇形誤差置信區(qū)間V和三角形幾何約束模型，尋找運載體在等值線C2上與之對應(yīng)的初始匹配點，最終獲取運載體在等值線
C2上的初始匹配點集尸2'；第五步，以初始匹配點集P2'內(nèi)任意一點為起點，基于三角形幾何約束模型，尋找運
載體在等值線C3上與之對應(yīng)的初始匹配點，獲取運載體在等值線C3上的初始匹配點集P3'第六步，獲取同時滿足tp t2和t3時刻匹配點形成初始匹配集；第七步，采用加權(quán)概率估計模型算法，計算初始匹配集中各組數(shù)據(jù)的代價值，取代價值最小的那組數(shù)據(jù)為運載體最終初始匹配位置。所述第四步構(gòu)建運載體在采樣點、時刻慣導(dǎo)系統(tǒng)提供的航向角扇形誤差置信區(qū)間V為首先，根據(jù)慣導(dǎo)系統(tǒng)的角度誤差特性，估計采樣點、時刻慣導(dǎo)系統(tǒng)輸出的運載體航向角的誤差置信區(qū)間φ ；然后，以ti時刻運載體的航向為扇形頂角的角平分線，扇形的頂角大小為2 Φ，構(gòu)建、時刻運載體的航向角扇形誤差置信區(qū)間V。所述第四步尋找運載體在等值線C2上與之對應(yīng)的初始匹配點，最終獲取運載體在等值線C2上的初始匹配點集尸2'的過程如下首先，取等值線C1的點集P1 (A、Pn--Pu、…、化,，i為自然數(shù)，l<i<Li山為點集P1所包含的點數(shù))內(nèi)任意一點Pli為扇形誤差置信區(qū)間V的頂點；其次搜索等值線C2 的點集P2 ( Pn、…、Pik、…、P2L2，k為自然數(shù)，1 < k < L2, L2為點集P2所包含的點數(shù)) 內(nèi)位于扇形誤差置信區(qū)間V內(nèi)的所有點，生成點集仏(如、仏2、···、qtJ、…、為自然數(shù)， 1 < i < L1, j為自然數(shù)，1 < j < N2, N2為點集Qi所包含的點數(shù))；再次，利用點Pli與點集 A內(nèi)所有點生成匹配對集P1A (凡如、1U,2、…、“…、斗^v2 )，并計算匹配對集P1A內(nèi)
每個匹配對所對應(yīng)的兩點間的距離/ “；最后，搜索匹配對集？工兌內(nèi)滿足不等式 (I1為第二步提到的三角形約束模型的一條邊的邊長，δ為第二步提到的三角形頂點位置誤差置信區(qū)間S)的所有匹配對，生成匹配對集(^、^、…、^、…、^；，η為自然數(shù)，1< <》2，冗為匹配對所包含的點數(shù))。遍歷點集？工內(nèi)所有點，重復(fù)上面的過程，最終獲取運載體在等值線C2上的初始匹配點集ρ2'(Α、Ρ2'2、…、Ρ2；、...、P‘)(k為自然
m,l<k<L'2丄丨為點集巧所包含的點數(shù))，以及所有匹配對集初(Plq” p2q2.....pmqm.....
P q ) (m為自然數(shù)，1 < i < M，M為匹配對集@所包含的匹配對數(shù)，pm、Qffl分別表示等值線 C1、等值線C2上的點)。所述第五步以初始匹配點集內(nèi)任意一點為起點，基于三角形幾何約束模型，尋找運載體在等值線C3上與之對應(yīng)的初始匹配點，獲取運載體在等值線C3上的初始匹配點集P/的過程如下從匹配對集@中選取任意一個匹配對 ，以等值線(2上的點(^為起點，搜索等值線(3的點集P3(&、Py1、…、Pv、…、&，1為自然數(shù)，1<士<1^山表示點集？3所包含的點數(shù))內(nèi)所有滿足^ -^卜^且幺!^!^^ =…。。-Θ條件的點，從而獲取運載體在等值線C3上的初始匹配點集/>/。所述第六步獲取同時滿足、、t2和t3時刻匹配點形成初始匹配點集的過程如下 在匹配對集@中任選一組匹配對;^，以匹配對位于等值線C2上的點qm為起點，搜索等值線C3的點集P3內(nèi)滿足I^p3, ~h\ < 5且Z PfflQfflP3i = 180° - θ條件的點，從而形成滿足三角形幾何約束模型的一組匹配對^^，遍歷匹配對集^^內(nèi)所有匹配對，從而獲取同時滿足 、、、和 t3 時刻匹配點的匹配對集/QPi ( J^,p2q2p'2、…、pm.qm,p'm,、...、pM,qM.p: ) (m'為自然數(shù)，l<m' <M'，M'為匹配對集_所包含的匹配對數(shù)，pm,、cv、/^.分別表示等值線C1、等值線C2、等值線C3上的點)。所述第七步，采用加權(quán)概率估計模型算法，計算初始匹配集中各組數(shù)據(jù)的代價值， 取代價值最小的那組數(shù)據(jù)為運載體最終初始匹配位置的具體過程如下構(gòu)建的代價函數(shù)為
權(quán)利要求
1.一種基于三角形約束模型的無源導(dǎo)航的等值線匹配方法，其特征在于實現(xiàn)步驟如下第一步，判斷運載體已進(jìn)入地球物理場適配區(qū)域Ω，選取運載體所承載的地球物理場傳感器連續(xù)的三個采樣點，三個采樣點所對應(yīng)的時間分別為、、t2和t3時刻；第二步，構(gòu)建三角形幾何約束模型，并結(jié)合地球物理場參考圖所提供的場值與實測地球物理場值之間的尺度變化構(gòu)建幾何約束模型的三角形頂點位置誤差置信區(qū)間S ；所述構(gòu)建三角形幾何約束模型的過程為首先獲取慣導(dǎo)系統(tǒng)在時間段、t2、t2t3內(nèi)輸出的運載體相對行駛距離I:、I2及在t2時刻的相對轉(zhuǎn)向角θ ；然后，利用Ipl2和θ三個參數(shù)構(gòu)建三角形幾何約束模型；第三步，獲取運載體承載的地球物理場傳感器在、、t2和t3時刻三個采樣點地球物理場值分別對應(yīng)的全部等值線Cp C2和C3 ；第四步，構(gòu)建運載體在采樣點、時刻慣導(dǎo)系統(tǒng)提供的航向角扇形誤差置信區(qū)間V，然后以等值線(^的點集P1內(nèi)任意一點為起點，基于航向角扇形誤差置信區(qū)間V和三角形幾何約束模型，尋找運載體在等值線C2上與之對應(yīng)的初始匹配點，最終獲取運載體在等值線C2上的初始匹配點集P2';第五步，以初始匹配點集P2'內(nèi)任意一點為起點，基于三角形幾何約束模型，尋找運載體在等值線C3上與之對應(yīng)的初始匹配點，獲取運載體在等值線C3上的初始匹配點集P;,第六步，獲取同時滿足ti、t2和t3時刻匹配點形成初始匹配集； 第七步，采用加權(quán)概率估計模型算法，計算初始匹配集中各組數(shù)據(jù)的代價值，取代價值最小的那組數(shù)據(jù)為運載體最終初始匹配位置。
2.根據(jù)權(quán)利要求1所述的基于三角形約束模型的無源導(dǎo)航的等值線匹配方法，其特征在于所述第四步構(gòu)建運載體在采樣點、時刻慣導(dǎo)系統(tǒng)提供的航向角扇形誤差置信區(qū)間V 為首先，根據(jù)慣導(dǎo)系統(tǒng)的角度誤差特性，估計采樣點、時刻慣導(dǎo)系統(tǒng)輸出的運載體航向角的誤差置信區(qū)間Φ ；然后，以、時刻運載體的航向為扇形頂角的角平分線，扇形的頂角大小為2 Φ，構(gòu)建、時刻運載體的航向角扇形誤差置信區(qū)間V。
3.根據(jù)權(quán)利要求1所述的基于三角形約束模型的無源導(dǎo)航的等值線匹配方法，其特征在于所述第四步尋找運載體在等值線C2上與之對應(yīng)的初始匹配點，最終獲取運載體在等值線C2上的初始匹配點集P2'的過程如下a.取等值線C1的點集P1，設(shè)定點集P1包含的點有Λ、Pn-Pu、…、A,，i為自然數(shù)， 1 < i < L1, L1為點集P1所包含的點數(shù)，內(nèi)任意一點Pli為扇形誤差置信區(qū)間V的頂點；b.搜索等值線C2的點集P2內(nèi)位于扇形誤差置信區(qū)間V內(nèi)的所有點，生成點集Qi，設(shè)定點集Qi包含的點有知<1丨2、…、^lj、…、h，i為自然數(shù)，1 < i < L1, j為自然數(shù)，1 < j < N2, N2為點集Qi所包含的點數(shù)；c.利用點Pli與點集A內(nèi)所有點生成匹配對集PliQi,設(shè)定匹配對集P1A包含的匹配對有徹、ρ—、…、·、p^lN2，并計算匹配對集PliGii內(nèi)每個匹配對所對應(yīng)的兩點間的距離;d.搜索匹配對集PliQi內(nèi)滿足不等式、％-A (I1為第二步提到的三角形約束模型的一條邊的邊長，s為第二步提到的三角形頂點位置誤差置信區(qū)間)的所有匹配對， 生成匹配對集，設(shè)定匹配對集,包含的匹配對有^^、...、^...、，η 為自然數(shù)，，》2為匹配對集所包含的點數(shù)，遍歷點集P1內(nèi)所有點，重復(fù)上面的過程，最終獲取運載體在等值線C2上的初始匹配點集尸2'，設(shè)定點集P2'包含的點有 AP Ki、…、Kk、…、di，k為自然數(shù)，1<眾<《4為點集T32'所包含的點數(shù)，以及所有匹配對集歷，設(shè)定匹配對集取包含的匹配對有=P1QpPW2.....PmQm.....P q , m為自然數(shù)，1 < i< M，M為匹配對集@所包含的匹配對數(shù)，pm、Qffl分別表示等值線C1、等值線C2上的點。
4.根據(jù)權(quán)利要求1所述的基于三角形約束模型的無源導(dǎo)航的等值線匹配方法，其特征在于所述第五步以初始匹配點集P2'內(nèi)任意一點為起點，基于三角形幾何約束模型，尋找運載體在等值線C3上與之對應(yīng)的初始匹配點，獲取運載體在等值線C3上的初始匹配點集 P3’的過程如下從匹配對集⑩中選取任意一個匹配對^；，以等值線C2上的點qm為起點， 搜索等值線C3的點集P3內(nèi)所有滿足-l2\< 5且Z PfflQfflP3i = 180° -θ條件的點，從而獲取運載體在等值線C3上的初始匹配點集戶/，i為自然數(shù)，1 < i < L3, L3表示點集P3所包含的點數(shù)。
5.根據(jù)權(quán)利要求1所述的基于三角形約束模型的無源導(dǎo)航的等值線匹配方法，其特征在于所述第六步獲取同時滿足、、t2和t3時刻匹配點形成初始匹配點集的過程如下 在匹配對集@中任選一組匹配對i，以匹配對位于等值線C2上的點1為起點，搜索等值線C3的點集P3內(nèi)滿足|/9』,-/2|<5且ZPmqmP3i = 180° -θ條件的點，從而形成滿足三角形幾何約束模型的一組匹配對凡札A ,遍歷匹配對集@內(nèi)所有匹配對，從而獲取同時滿足、、t2和t3時刻匹配點的匹配對集P1 QP3，設(shè)定匹配對集^^包含的匹配對有 p^p2q2p'r..·、Pm,qm.P'm‘、..·、PM.qM,P'M.，m‘為自然數(shù)，l<m' <M' ,M'為匹配對集_ 所包含的匹配對數(shù)，Pm,、qm,、；^分別表示等值線C1、等值線C2、等值線C3上的點。
6.根據(jù)權(quán)利要求1所述的基于三角形約束模型的無源導(dǎo)航的等值線匹配方法，其特征在于所述第七步，采用加權(quán)概率估計模型算法，計算初始匹配集中各組數(shù)據(jù)的代價值，取代價值最小的那組數(shù)據(jù)為運載體最終初始匹配位置的具體過程如下a.構(gòu)建的代價函數(shù)為cos t = f (1, α ) (1)式中1表示匹配點與慣導(dǎo)系統(tǒng)所提供的對應(yīng)位置點之間的距離參數(shù)，α表示匹配點的方向角與慣導(dǎo)系統(tǒng)所提供的對應(yīng)點的方向角之間的夾角；b.從匹配對集中任選一組匹配對^^，由公式(1)分別計算匹配對K^S 所對應(yīng)的三個匹配點Pm,、qm,和Λ的匹配代價值Costpm,、COStqm,和COS爐:^，可得匹配對 pm,qm,p'm,的匹配代價值costj^ = Costpm, + Costqm, + cos^，選取cosi^；^*最小值所對應(yīng)的一組匹配對為運載體最終的初始匹配值。
全文摘要
一種基于三角形約束模型的無源導(dǎo)航的等值線匹配方法，本發(fā)明針對無源導(dǎo)航(地形/地磁/重力輔助慣形導(dǎo)航)系統(tǒng)，當(dāng)運載體剛進(jìn)入地球物理場適配區(qū)域時，慣導(dǎo)系統(tǒng)提供的運載體位置信息存在大誤差條件下，為了快速獲取高精度的初始匹配值而提出的一種等值線匹配方法，其特征在于利用已知的高精度三角形約束模型進(jìn)行等值線匹配。本發(fā)明是一種自主式匹配方法，具有精度高、魯棒性好的特點，滿足無源導(dǎo)航系統(tǒng)地球物理場匹配技術(shù)對運載體初始位置精度的要求以及運載體對無源導(dǎo)航系統(tǒng)實時性要求，可應(yīng)用于地形/地磁/重力輔助慣性導(dǎo)航等無源導(dǎo)航系統(tǒng)中的初始匹配。
文檔編號G01C21/20GK102168979SQ201010592980
公開日2011年8月31日 申請日期2010年12月8日 優(yōu)先權(quán)日2010年12月8日
發(fā)明者房建成, 朱莊生 申請人:北京航空航天大學(xué)