一種基于改進的iccp算法的重力匹配輔助慣性導(dǎo)航方法
【專利摘要】本發(fā)明公開了一種基于改進的ICCP算法的重力匹配輔助慣性導(dǎo)航方法。使用本發(fā)明能夠有效地提高重力輔助慣性導(dǎo)航系統(tǒng)的精度和實時性，從而減小慣導(dǎo)系統(tǒng)隨時間積累誤差及在慣導(dǎo)系統(tǒng)存在大誤差條件下重力匹配失效的缺陷。本發(fā)明對匹配算法模塊中的ICCP算法進行改進，用采樣單個點數(shù)據(jù)代替序列采樣的方法，將單個點數(shù)據(jù)替換序列數(shù)據(jù)中的首位數(shù)據(jù)，形成新的序列，從而縮短數(shù)據(jù)采樣時間，提高匹配效率。
【專利說明】—種基于改進的ICCP算法的重力匹配輔助慣性導(dǎo)航方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及慣性導(dǎo)航【技術(shù)領(lǐng)域】，具體涉及一種基于改進的ICCP算法的重力匹配輔助慣性導(dǎo)航方法。
【背景技術(shù)】
[0002]慣性導(dǎo)航系統(tǒng)具有無源性和自主性，因此水下航行器一般采用以慣性導(dǎo)航為核心的導(dǎo)航系統(tǒng)，以實現(xiàn)水下自主隱蔽導(dǎo)航。但慣性導(dǎo)航系統(tǒng)存在誤差隨時間積累而發(fā)散的缺陷，如果不定期修正，就無法保證其水下航行器長航時及高精度兩個要求，因此必須要通過其他導(dǎo)航方式實時或定時修正慣性導(dǎo)航系統(tǒng)來抑制導(dǎo)航系統(tǒng)位置誤差的增長，提高長航時導(dǎo)航精度。目前輔助慣導(dǎo)系統(tǒng)常采用的無源方法是天文導(dǎo)航、地磁匹配及地形匹配等技術(shù)。近年來，借助地形匹配的技術(shù)成果，重力/慣性、地磁/慣性、GPS/慣性等組合導(dǎo)航的理論方法也相繼成為了國內(nèi)外研究和開放的重點。
[0003]重力匹配輔助慣性導(dǎo)航系統(tǒng)目前獲得各國的廣泛關(guān)注。地球重力場的變化是地球固有的物理信息，穩(wěn)定性較好，重力數(shù)據(jù)的獲得是一個無源的過程，最大程度保證了潛器導(dǎo)航的隱蔽性和自主性。將重力信息引入慣導(dǎo)系統(tǒng)，通過與慣導(dǎo)系統(tǒng)組合修正，可以形成自主、高精度、高隱蔽性、抗干擾能力強的組合導(dǎo)航系統(tǒng)。重力匹配的核心是算法，經(jīng)典的方法是地形輪廓匹配方法(TERC0M)、桑迪亞慣性地形輔助導(dǎo)航方法(SITAN)和基于等值線的最近點迭代方法(ICCP)。ICCP算法是近年來使用較多的一類匹配算法，最初它是用于圖像配準中，Behzad首次將其引入到水下輔助導(dǎo)航,并給出了具體的實現(xiàn)形式，Bishop則通過仿真試驗系統(tǒng)地驗證了該算法的可行性及各種誤差影響。ICCP算法是ICP算法以等值線為匹配單元的特例，它原理簡單，易于實現(xiàn)。
[0004]ICCP算法不需要進行事先估計分析，只需對實測點值不斷重復(fù)迭代，以求得最佳航跡。主要問題涉及等值線提取、最近點的求解和坐標值轉(zhuǎn)換計算。ICCP算法是針對序列數(shù)據(jù)進行的運算，在使用前，需采樣出足夠的慣導(dǎo)指示航跡點和重力測量值。地球重力場在小范圍內(nèi)變化并不明顯，因此航行器需要大范圍搜索采樣點才能滿足算法需要特別變化明顯的重力值的要求。潛器在水下航行速度較慢，導(dǎo)致算法在取采樣點和重力值的環(huán)節(jié)上耗費大量時間。而此期間慣導(dǎo)誤差繼續(xù)隨時間積累，如若再進行匹配，勢必造成一次完整的重力匹配修正慣導(dǎo)耗時長、精度低，相鄰匹配相隔時間也過長，不能達到最佳匹配的效果。如果將采樣時間縮短，可以提高匹配精度，充分達到修正慣性導(dǎo)航的目的。

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

[0005]有鑒于此，本發(fā)明提供了一種基于改進的ICCP算法的重力匹配輔助慣性導(dǎo)航方法，能夠有效地提高重力輔助慣性導(dǎo)航系統(tǒng)的精度和實時性，從而減小慣導(dǎo)系統(tǒng)隨時間積累誤差及在慣導(dǎo)系統(tǒng)存在大誤差條件下重力匹配失效的缺陷。
[0006]本發(fā)明的基于改進的ICCP算法的重力匹配輔助慣性導(dǎo)航方法中，匹配算法模塊將重力儀器實測得的重力值與預(yù)存重力基準圖中讀取的重力值進行改進的ICCP算法匹配，得到接近真實軌跡的匹配軌跡，將匹配軌跡反饋給慣性導(dǎo)航系統(tǒng)進行軌跡修正，其中，改進的ICCP算法包括如下步驟:
[0007]步驟一，匹配算法模塊利用重力儀器采集的η個航跡點作為初始序列PJp1,P2,…，PnI，進行第一次匹配，第一次匹配時采用的剛性變換記為T1,得到航跡M1 K1,m2;1，mn；1}；
[0008]步驟二,采集新的航跡點pn+1,對pn+1進行T1變換得到IVu,將mn+i，i代入航跡M1中，替換航跡點,形成航跡P2Imiu, m3；1,..., mn；1, ,將P2作為初始序列進行第二次匹配，第二次匹配時采用的剛性變換記為1~2，得到航跡M2{m2，2，m3，2，…，mn+1，2}；
[0009]步驟三，采集新的航跡點pn+2，對pn+2先進行T1變換后再進行T2變換，得到mn+2，2，將％+2，2代入航跡仏中，替換航跡點m2，2,形成航跡P3{m3，2, m4；2,..., mn+2，2},將航跡P3作為第三次匹配的初始航跡進行第三次匹配，第三次匹配時采用的剛性變換記為T3,得到航跡M3lm3’3, m4’3，…，mn+2’3}；
[0010]步驟四，按照以上做法，采集新的航跡點pn+i，對pn+i進行T1, T2,…，Ti變換，得到mn+i； i，將mn+i，i代入航跡Mi中，替換航跡點^，形成航跡Pi+1 {m i+1；i,m i+2；i；...,mn+i； J，將航跡Pi+1作為第i+1次匹配的初始航跡進行第i+Ι次匹配，第i+Ι次匹配時采用的剛性變換
;己為 T i+i，丫守至丨M j+i {ill i+i’i+ι, Π i+2, i+1,...， Π i+n, i+ll ;
[0011]步驟五， 不斷重復(fù)上述采樣、迭代步驟，得到的結(jié)果為匹配算法求解出的最佳匹配航跡。
[0012]有益效果:
[0013]修改后的ICCP算法相對于傳統(tǒng)的ICCP算法在計算上更具有連貫性，這就使得匹配誤差相對減少，匹配軌跡精確度相對提高。由于只有第一次采樣是序列值，之后的采樣都是單個點值，這就滿足了實時測量的要求，大大減少匹配算法前采樣數(shù)據(jù)的消耗時間，提高了匹配效率和精度，對測量系統(tǒng)的修正也更精確。
【專利附圖】

【附圖說明】
[0014]圖1為重力匹配輔助慣導(dǎo)原理圖。
[0015]圖2為ICCP算法示意圖。
[0016]圖3為ICCP算法流程圖。
[0017]圖4為初始序列采樣匹配示意圖。
[0018]圖5為單點采樣二次匹配示意圖。
[0019]圖6為單點采樣第三次匹配示意圖。
[0020]圖7為傳統(tǒng)ICCP算法采樣匹配示意圖。
[0021]圖8為改進的ICCP算法采樣匹配示意圖。
【具體實施方式】
[0022]下面結(jié)合附圖并舉實施例，對本發(fā)明進行詳細描述。
[0023]本發(fā)明提供了一種基于改進的ICCP算法的重力匹配輔助慣性導(dǎo)航方法，對傳統(tǒng)的ICCP算法進行改進。
[0024]如圖1所示，傳統(tǒng)的重力輔助慣性導(dǎo)航系統(tǒng)一般主要分成慣性導(dǎo)航系統(tǒng)、預(yù)存重力基準圖、基于動基座的實測重力參數(shù)系統(tǒng)和匹配算法四個模塊。慣導(dǎo)系統(tǒng)模塊的主要目的是對航行器在航行過程中進行自主隱蔽的導(dǎo)航定位；預(yù)存重力圖模塊的主要目的是利用慣導(dǎo)系統(tǒng)指示出的位置在重力圖上找到重力值，作為匹配值；實測重力系統(tǒng)模塊的主要目的是實時測量航行器所在位置的重力、水深等信息，利用重力值畫出等值線，并找出慣導(dǎo)指示位置對應(yīng)的最近等值線；匹配算法模塊的主要目的是將實測重力值與預(yù)存重力圖上讀取的重力值利用ICCP算法匹配，使得匹配后得到的軌跡值最接近真實軌跡值。本發(fā)明主要是針對匹配算法模塊中ICCP算法的改進問題，以提高重力輔助慣性導(dǎo)航系統(tǒng)的可靠性和實時性。
[0025]傳統(tǒng)ICCP算法中，匹配數(shù)據(jù)為序列值，因此在每次匹配之前需要采樣固定數(shù)目的序列數(shù)據(jù)之后才能進行匹配計算，由于慣導(dǎo)誤差隨時間積累，因此長時間序列值采樣會導(dǎo)致匹配后的結(jié)果誤差較大。本發(fā)明提出了用采樣單個點數(shù)據(jù)代替序列采樣的方法，將單個點數(shù)據(jù)替換序列數(shù)據(jù)中的首位數(shù)據(jù)，形成新的序列，從而縮短數(shù)據(jù)采樣時間，提高匹配效率。即在跟蹤模式下，與目標對象同步，實時追蹤以獲取一組采樣序列值，作為初始匹配序列進行匹配；第二次待匹配值采樣時只取單個采樣點，將其加入上一次匹配得到的序列中代替首位的值形成新的待匹配序列進行匹配計算。這種待匹配值只取一次序列而后用單個點值替換組成新序列的方式可以縮短算法前采樣的時間及完整匹配的時間，減小誤差積累的影響，在短時間內(nèi)獲得較高精度的估計值，并達到了實時測量的目的，即便導(dǎo)航一段時間后，第k次匹配時需要將新采集的點進行！\、T2,……、Th次變換，變換計算所需的時間也小于傳統(tǒng)的采集10個數(shù)目的序列進行匹配所需的時間。
[0026]改進的ICCP算法具體步驟如下:
[0027]第一步，利用重力儀器實時測得的值繪出等值線圖。
[0028]ICCP算法假設(shè)真實軌跡Xi —定位于等值線C上，因此在匹配計算前，利用重力儀器實時測得的重力數(shù)據(jù)值繪出重力等值線圖，如圖2所示。
`[0029]第二步，采集測量序列，尋找其對應(yīng)最近等值線點，經(jīng)剛性變換T迭代得到匹配軌跡。
[0030]采集η個航跡點作為初始序列P1 {p1； P2,..., ρη}。找到個各航跡點對應(yīng)的最近等值線點，將初始序列進行第一次匹配T1，直到T1收斂或最優(yōu)目標函數(shù)值小于預(yù)設(shè)的閾值即停止迭代，得到新的航跡M1 {mu，my，…，my}。寄存器一般工作方程為:
[0031]Mx = JR + KPx
[0032]初始序列采樣和匹配與傳統(tǒng)方法相同，尋找最近等值線點是匹配算法的關(guān)鍵，它基于慣性導(dǎo)航定位誤差不大的假設(shè)，即真實位置在慣導(dǎo)給出位置的附近?？梢酝ㄟ^向等值線做垂線的方法尋找最近等值線點，但是真正的重力圖是以離散的形式存儲在計算機中的，這樣我們需要在慣導(dǎo)位置的附近尋找重力值相等的最近點，故算法假設(shè)慣導(dǎo)誤差不大。在匹配過程中，剛性變換分為旋轉(zhuǎn)(R)和平移(t)兩部分，先旋轉(zhuǎn)使測量點Xi對準估計值Ji方向，再平移使兩級和質(zhì)心重合，匹配結(jié)束的標志是剛性變換T1收斂或最優(yōu)目標函數(shù)值小于預(yù)設(shè)的閾值S。最優(yōu)目標函數(shù)為:
η2
[0033]M = y.-Rp1-t

/-1 “[0034]第三步,在測量航跡上取新的單點pn+1，對單點pn+1進行T1變換得到IVu,將mn+i，i代入航跡M1中，替換航跡點Hi1,1;形成新的航跡P2 {m2； ^m3jl,…,mn；1,mn+1； J ,將新航跡P2作為初始序列進行第二次匹配T2，直到T2收斂或最優(yōu)目標函數(shù)值小于預(yù)設(shè)的閾值即停止迭代，得到新的航跡 M2Im2,2，m2，2,..., mn+1，2}。
[0035]改進的方法中，只有初始匹配的采樣點是以序列的形式獲取，第二次匹配開始單個點采樣，將單個點用過移位寄存與之前序列的η-1個數(shù)據(jù)形成新的匹配序列進行匹配計算。而單個點不是直接加入序列中，而是先經(jīng)過T1變換，與之前序列值同步，再進行T2變換。
[0036]第四步，按照第三步的作法，取新采樣點pn+2，先進行T1變換后再T2變換，得到1Vpdt mn+2，2代入航跡M2中，替換航跡點m2，2,形成新的航跡P3 {m3；2, m4；2,..., mn+2；2},將航跡P3作為第三次匹配的初始航跡進行匹配T3，T3呈現(xiàn)收斂或最優(yōu)目標函數(shù)值小于預(yù)設(shè)的閾值即停止迭代，得到新的航跡M3 {m3，3, m4，3,..., mn+2，3}。
[0037]第五步，按照以上做法，采集新的航跡點pn+i，對pn+i進行T1, T2,…，Ti變換，得到mn+i，i，將mn+i，i代入航跡Mi中，替換航跡點,形成航跡Pi+1{m i+1；i, m i+2；i,…，1?+^},將航跡Pi+1作為第i+1次匹配的初始航跡進行第i+1次匹配T i+1，得到航跡M i+1{m i+1；i+1, m
i+2, i+1? …，m i+n； i+1}
[0038]第六步，不斷重復(fù)上述采樣、迭代步驟，得到的結(jié)果為匹配算法求解出的最佳匹配航跡，將最佳匹配航跡反饋給慣性導(dǎo)航系統(tǒng)進行軌跡修正。
[0039]綜上所述，以上僅為本發(fā)明的較 佳實施例而已，并非用于限定本發(fā)明的保護范圍。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進等，均應(yīng)包含在本發(fā)明的保護范圍之內(nèi)。
【權(quán)利要求】
1.一種基于改進的ICCP算法的重力匹配輔助慣性導(dǎo)航方法，其特征在于，匹配算法模塊將重力儀器實測得的重力值與預(yù)存重力基準圖中讀取的重力值進行改進的ICCP算法匹配，得到接近真實軌跡的匹配軌跡，將匹配軌跡反饋給慣性導(dǎo)航系統(tǒng)進行軌跡修正，其中，改進的ICCP算法包括如下步驟:步驟一，匹配算法模塊利用重力儀器采集的η個航跡點作為初始序列P1 {Ρ1，Ρ2，-,ρη}，進行第一次匹配，第一次匹配時采用的剛性變換記為T1，得到航跡M1 Irnlil, my，…，HinJ ；步驟二，采集新的航跡點ρη+1，對ρη+1進行T1變換得到mn+1，!,將mn+1，I代入航跡M1中，替換航跡點,形成航跡P2Imiu, m3；1,..., mn；1, 1?+^},將P2作為初始序列進行第二次匹配，第二次匹配時采用的剛性變換記為T2,得到航跡M2{m2，2, m3，2,…，mn+1，2}； 步驟三，采集新的航跡點pn+2，對pn+2先進行T1變換后再進行T2變換，得到mn+2，2，將mn+2，2代入航跡M2中，替換航跡點m2，2，形成航跡P3Im3 2, m4 2，…，mn+2，2}，將航跡P3作為第三次匹配的初始航跡進行第三次匹配，第三次匹配時采用的剛性變換記為T3,得到航跡M3 {m3；3，m4’3,…，mn+2’3}； 步驟四，按照以上做法，采集新的航跡點Pn+i，對Pn+i進行T1, T2,…，Ti變換，得到πιη+Μ，將mn+i，i代入航跡Mi中，替換航跡點,形成航跡Pi+1{m i+1；i,m i+2；i,…,!!!~},將航跡Pi+1作為第i+Ι次匹配的初始航跡進行第i+Ι次匹配，第i+1次匹配時采用的剛性變換記為Ti+l，Y守至iJjl/Lji M i+i {m i+l’i+l，m i+2’i+i， * * *? Π i+n； i+j}； 步驟五，不斷重復(fù)上述采樣、迭代步驟，得到的結(jié)果為匹配算法求解出的最佳匹配航跡。
【文檔編號】G01C21/20GK103822634SQ201410062405
【公開日】2014年5月28日 申請日期:2014年2月24日 優(yōu)先權(quán)日:2014年2月24日
【發(fā)明者】付夢印, 王博, 鄧志紅, 劉美琪, 肖烜 申請人:北京理工大學(xué)