專利名稱：基于混合地形輪廓匹配tercom算法和粒子濾波的地形輔助導(dǎo)航方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種慣性地形輔助導(dǎo)航方法，特別是一種基于混合地形輪廓匹配 TERCOM算法和粒子濾波的地形輔助導(dǎo)航方法，適用于無(wú)人飛行器和有人飛行器地形輔助導(dǎo)航定位系統(tǒng)。
背景技術(shù)：
在現(xiàn)代軍民用領(lǐng)域中應(yīng)用的無(wú)人飛行器(無(wú)人機(jī)和巡航導(dǎo)彈)對(duì)自主導(dǎo)航系統(tǒng)精度和可靠性要求越來(lái)越高，要求實(shí)時(shí)、可靠地提供無(wú)人飛行器的位置、速度和姿態(tài)信息。依靠單一的導(dǎo)航系統(tǒng)很難滿足要求。將慣性導(dǎo)航系統(tǒng)與衛(wèi)星定位系統(tǒng)信息進(jìn)行融合來(lái)提供載體的運(yùn)動(dòng)信息已被廣泛應(yīng)用。但由于衛(wèi)星定位信號(hào)易受干擾或欺騙，甚至信號(hào)轉(zhuǎn)發(fā)源被摧毀，不能夠長(zhǎng)期提供穩(wěn)定可靠的定位信息，在軍民用領(lǐng)域中尚不能完全依賴于衛(wèi)星定位來(lái)輔助慣性導(dǎo)航實(shí)現(xiàn)飛行器的精確導(dǎo)航定位。因此，發(fā)展具有自主性強(qiáng)和較高精度的地形輔助導(dǎo)航技術(shù)已成為提高飛機(jī)、巡航導(dǎo)彈等武器系統(tǒng)戰(zhàn)斗力的有效手段之一。經(jīng)過(guò)近60年的發(fā)展，已經(jīng)發(fā)展成熟的地形輔助導(dǎo)航方法有兩類，一種是基于相關(guān)分析原理的地形輪廓匹配算法的導(dǎo)航方法，例如TERCOM算法；另一類是基于擴(kuò)展Kalman濾波的地形輔助慣性導(dǎo)航算法的導(dǎo)航方法，例如SITAN算法。兩種導(dǎo)航方法已被成功應(yīng)用在多種型號(hào)的精確制導(dǎo)武器和戰(zhàn)術(shù)飛機(jī)上。然而，當(dāng)飛行器在平坦區(qū)域、地形梯度變化劇烈的區(qū)域或者飛行器有大的機(jī)動(dòng)時(shí)，兩種導(dǎo)航方法的定位精度下降，甚至產(chǎn)生偽定位。為了克服傳統(tǒng)地形匹配算法的不足，提出了一些改進(jìn)的導(dǎo)航方法，例如基于多模自適應(yīng)估計(jì)技術(shù)的HELI/SITAN算法、AFTI/SITAN算法、北航慣性地形輔助導(dǎo)航(BUAA Inertial Terrain Aided Navigation, BITAN, BITAN)算法和北航慣性地形輔助導(dǎo)航算法(BITAN)II的導(dǎo)航方法，但這些導(dǎo)航方法實(shí)質(zhì)都是將非線性的系統(tǒng)狀態(tài)方程和觀測(cè)方程進(jìn)行線性化處理后， 利用擴(kuò)展卡爾曼濾波方法對(duì)地形高度匹配問(wèn)題進(jìn)行處理，因此只適用于濾波誤差和預(yù)測(cè)誤差很小的情況。然而由于地形特征是非線性的，當(dāng)初始位置誤差較大時(shí)，在地形線性化時(shí)會(huì)造成擬合區(qū)域線性化誤差增大，無(wú)法滿足卡爾曼濾波對(duì)局部線性化的要求，使得濾波發(fā)散， 而且當(dāng)?shù)匦螛?biāo)高變化過(guò)于劇烈、不能滿足線性化假設(shè)時(shí)，定位精度將下降，甚至不能進(jìn)行定位。本發(fā)明中提到的一種基于混合地形輪廓匹配TERCOM算法和粒子濾波的地形輔助導(dǎo)航方法，利用基于相關(guān)分析原理的TERCOM算法實(shí)現(xiàn)跟蹤模式，能夠快速、準(zhǔn)確地完成初始定位，在跟蹤模式下利用能充分反映地形非線性特性的粒子濾波實(shí)現(xiàn)連續(xù)快速的跟蹤， 再輔以模式邏輯轉(zhuǎn)換，保證了算法的定位精度和可靠性。本發(fā)明與其它慣性地形輔助導(dǎo)航不同在于在當(dāng)前已公開(kāi)發(fā)表的資料中，慣性地形輔助導(dǎo)航集中在對(duì)傳統(tǒng)算法的改進(jìn)，在搜索模式和跟蹤模式都采用卡爾曼濾波，或是在搜索模式采用TERCOM算法，跟蹤模式采用擴(kuò)展卡爾曼濾波等，但這些方法無(wú)一例外均無(wú)法解決在大的初始位置誤差，平坦地形區(qū)域以及飛行器高機(jī)動(dòng)飛行時(shí)濾波器的發(fā)散問(wèn)題。

發(fā)明內(nèi)容
本發(fā)明要解決的技術(shù)問(wèn)題克服現(xiàn)有的慣性地形輔助導(dǎo)航系統(tǒng)在大的初始位置誤差，平坦地形區(qū)域以及飛行器高機(jī)動(dòng)飛行時(shí)無(wú)法定位，甚至濾波器發(fā)散問(wèn)題，提出一種基于混合地形輪廓匹配TERCOM算法和粒子濾波的地形輔助導(dǎo)航方法。本發(fā)明采用的技術(shù)方案為一種基于混合地形輪廓匹配TERCOM算法和粒子濾波的地形輔助導(dǎo)航方法，該導(dǎo)航方法包含搜索模式、跟蹤模式以及丟失模式和模式邏輯裝換， 在搜索模式下，分別利用平均絕對(duì)差MAD算法、均方差MSD算法和交叉相關(guān)COR算法批處理測(cè)量信息獲得位置估計(jì)信息，并根據(jù)一致性表決算法，在大的初始位置誤差下快速、準(zhǔn)確地實(shí)現(xiàn)初始定位；在跟蹤模式下采用粒子濾波遞推處理測(cè)量信息實(shí)現(xiàn)連續(xù)匹配定位；丟失模式和模式邏輯轉(zhuǎn)換實(shí)現(xiàn)初始定位后由搜索模式轉(zhuǎn)入跟蹤模式，出現(xiàn)偽定位后由跟蹤模式轉(zhuǎn)換為搜索模式。保證在大的初始位置誤差穩(wěn)定可靠地實(shí)現(xiàn)連續(xù)定位。具體實(shí)現(xiàn)步驟為(1)進(jìn)入地形匹配區(qū)域判斷根據(jù)慣導(dǎo)系統(tǒng)的指示位置和數(shù)字高程基準(zhǔn)圖左上角和右下角的坐標(biāo)進(jìn)行判斷，飛行器是否進(jìn)入地形匹配區(qū)域；利用坐標(biāo)變換公式將每一個(gè)匹配區(qū)左上角和右下角的高斯直角坐標(biāo)轉(zhuǎn)換為地理坐標(biāo)，并分別記為(starts λ ,Start^L)和(end^ λ , end^L), i = 1,2,…為匹配區(qū)編號(hào)；根據(jù)慣導(dǎo)系統(tǒng)anertial Navigation System, INS)指示的位置(λ，L)，判斷飛行器是否到達(dá)或進(jìn)入第i個(gè)匹配區(qū)start^A ( λ Sendi _λ(1)start^L ^ L < end^L(2)式中，λ為經(jīng)度，L為緯度，上述兩個(gè)公式同時(shí)成立且等號(hào)不成立時(shí)表明飛行器已進(jìn)入匹配區(qū)，當(dāng)?shù)忍?hào)剛好成立時(shí)，表明飛行器到達(dá)匹配區(qū)的邊緣；⑵搜索模式進(jìn)入匹配區(qū)后，利用高度表連續(xù)獲取高度測(cè)量信息，分別利用基于相關(guān)分析原理的交叉相關(guān)COR算法、平均絕對(duì)差MAD算法和均方差MSD算法進(jìn)行批處理運(yùn)算，獲得飛行器的位置的估計(jì)值、和；根據(jù)位置的估計(jì)值構(gòu)造位置一致性判斷準(zhǔn)則為Ps = ^[Pcor - Pmad )2 + (Pcor - Pmsd )2 + (Pmsd - Pmad )2 (3)當(dāng)為《15 ( δ為數(shù)字高程圖的分辨率)時(shí)，則獲得飛行器位置估計(jì)值是正確的，完成一次位置一致性的判別；當(dāng)連續(xù)三次一致性判別一致時(shí)，則轉(zhuǎn)入跟蹤模式，此時(shí)通過(guò)相關(guān)分析算法獲得飛行器的位置估計(jì)值是實(shí)際位置的概率為99. 7% ；為保證每次一致性判別的獨(dú)立性，每次匹配定位的間隔必須大于連續(xù)匹配時(shí)的匹配間隔，保證每次匹配時(shí)地形輪廓是無(wú)關(guān)的；(3)跟蹤模式針對(duì)地形輔助導(dǎo)航的非線性問(wèn)題，建立一個(gè)能夠描述地形輔助導(dǎo)航的概率模型， 通過(guò)對(duì)該模型的隨機(jī)采樣得到大量樣本點(diǎn)，并通過(guò)計(jì)算樣本點(diǎn)的統(tǒng)計(jì)特征估計(jì)飛行器的位
6置信息，即利用狀態(tài)空間的一組加權(quán)隨機(jī)粒子樣本逼近狀態(tài)變量的后驗(yàn)概率密度；基于粒子濾波的地形輔助導(dǎo)航系統(tǒng)的狀態(tài)取飛行器在水平面上的位置信息，即在東北天坐標(biāo)系下，系統(tǒng)狀態(tài)取為東向位置和北向位置，其狀態(tài)方程和量測(cè)方程為Xk = Xh+VHT+QH (4)Zk = h (Xk) +rk(5)式中，X= [χ y]T，其中X表示東向位置，y表示北向位置；V= [vx vy]T，其中\(zhòng)表示東向速度，Vy表示北向速度；Qlri為系統(tǒng)噪聲；hOQ是地形高程值關(guān)于位置(x，y)的非線性函數(shù)；Α為量測(cè)誤差，為高斯噪聲，包括氣壓高度表誤差，雷達(dá)高度噪聲以及數(shù)字地圖制作噪聲；(4)丟失模式和模式邏輯轉(zhuǎn)換丟失模式是一種特殊的處理模式，包括真丟失和假丟；真丟失是指慣導(dǎo)系統(tǒng)的位置誤差很大，導(dǎo)致其指示位置與真實(shí)位置間距超過(guò)了搜索范圍，主要原因是系慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)問(wèn)題和系統(tǒng)硬件故障；假丟失實(shí)際上是由于算法判據(jù)不完善或是地形平坦或重復(fù)造成的偽定位。如果不及時(shí)處理，會(huì)導(dǎo)致搜索模式下獲得位置估值與真實(shí)位置有很大的誤差，導(dǎo)致飛行器定位精度下降，甚至出現(xiàn)定位錯(cuò)誤；在丟失模式下，根據(jù)模式轉(zhuǎn)換算法使其轉(zhuǎn)入丟失模式，重新進(jìn)行搜索，獲得正確的飛行器位置估計(jì)值后再轉(zhuǎn)入跟蹤模式；模式轉(zhuǎn)換包括兩種形式，一種是系統(tǒng)完成初始定位后由搜索模式轉(zhuǎn)入跟蹤模式； 另一種是在跟蹤模式下出現(xiàn)偽定位，由跟蹤模式轉(zhuǎn)入搜索模式。本發(fā)明的優(yōu)點(diǎn)在于采用TERCOM算法實(shí)現(xiàn)跟蹤模式，能夠快速、準(zhǔn)確地實(shí)現(xiàn)飛行器初始定位，而且初始位置是飛行器實(shí)際位置的概率為99. 7% ；采用粒子濾波實(shí)現(xiàn)飛行器連續(xù)定位，充分利用了地形的非線性信息，可實(shí)現(xiàn)在平坦區(qū)域以及飛行器高機(jī)動(dòng)飛行時(shí)進(jìn)行連續(xù)定位，提高了系統(tǒng)的可靠性和定位精度，并拓展了其適用范圍；設(shè)置丟失模式和模式邏輯轉(zhuǎn)換進(jìn)一步保證了算法的可靠性?？梢詫?shí)現(xiàn)在大的初始位置誤差、平坦地形區(qū)域以及飛行器高機(jī)動(dòng)飛行時(shí)實(shí)現(xiàn)連續(xù)可靠地定位。


圖1為本發(fā)明的一種基于混合地形輪廓匹配TERCOM算法和粒子濾波的地形輔助導(dǎo)航方法的原理流程圖；圖2為本發(fā)明的測(cè)量量間的關(guān)系圖；圖3為本發(fā)明的地形匹配區(qū)域判斷示意圖；圖4為本發(fā)明的粒子濾波算法及重采樣原理圖；圖5(a)為本發(fā)明與基于BITANII算法的導(dǎo)航方法的東向位置誤差比較結(jié)果；圖5(b)為本發(fā)明與BITANII算法的導(dǎo)航方法的北向位置誤差比較結(jié)果。
具體實(shí)施例方式下面結(jié)合附圖和具體實(shí)施方式
對(duì)本發(fā)明進(jìn)一步說(shuō)明，本發(fā)明的具體實(shí)現(xiàn)步驟如下
7
(1)算法初始化算法初始化包括匹配區(qū)搜索大小、初始粒子分布、慣導(dǎo)系統(tǒng)的初始位置、速度和姿態(tài)以及數(shù)字高程基礎(chǔ)圖的原點(diǎn)坐標(biāo)以及地圖大小等信息。(2)進(jìn)入匹配區(qū)域判斷如圖2所示，慣性導(dǎo)航系統(tǒng)作為主導(dǎo)航系統(tǒng)提供全部導(dǎo)航信息，雷達(dá)高度表輸出的高度、是飛行器到地面的瞬時(shí)距離，氣壓高度表或大氣數(shù)據(jù)計(jì)算機(jī)單獨(dú)或與慣性導(dǎo)航系統(tǒng)組合輸出的高度h是飛行器到海平面的瞬時(shí)距離，根據(jù)慣導(dǎo)系統(tǒng)的指示位置和數(shù)字高程基準(zhǔn)圖左上角和右下角的坐標(biāo)進(jìn)行判斷，飛行器是否進(jìn)入地形匹配區(qū)域；利用坐標(biāo)變換公式將每一個(gè)匹配區(qū)左上角和右下角的高斯直角坐標(biāo)轉(zhuǎn)換為地理坐標(biāo)，并分別記為(starts λ ,start^L)和(end^ λ , end^L), i = 1,2,…為匹配區(qū)編號(hào)；根據(jù)慣導(dǎo)系統(tǒng)anertial Navigation System, INS)指示的位置(λ，L)，判斷飛行器是否到達(dá)或進(jìn)入第i個(gè)匹配區(qū)start^A ( λ < end^ λ(1)StartiJj 彡 L < end^L(2)式中，λ為經(jīng)度，L為緯度，上述兩個(gè)公式同時(shí)成立且等號(hào)不成立時(shí)表明飛行器已進(jìn)入匹配區(qū)，當(dāng)?shù)忍?hào)剛好成立時(shí)，表明飛行器到達(dá)匹配區(qū)的邊緣，如圖3所示，其中大方框代表地形匹配區(qū)，細(xì)實(shí)線代表慣導(dǎo)指示航線，粗實(shí)線代表飛行器真實(shí)航線；飛行器在A點(diǎn)時(shí)，系統(tǒng)判斷剛進(jìn)入匹配區(qū)，此時(shí)系統(tǒng)開(kāi)始加載數(shù)字高程基準(zhǔn)圖，當(dāng)飛行器飛到B點(diǎn)時(shí)，小方框代表根據(jù)誤差帶估計(jì)設(shè)定的搜索區(qū)，當(dāng)搜索區(qū)全部位于地形匹配區(qū)，開(kāi)始進(jìn)入搜索模式。(3)搜索模式進(jìn)入匹配區(qū)后，利用高度表連續(xù)獲取高度測(cè)量信息，分別利用基于相關(guān)分析原理的交叉相關(guān)COR算法、平均絕對(duì)差MAD算法和均方差MSD算法進(jìn)行批處理運(yùn)算，獲得飛行器的位置的估計(jì)值、么_和4皿，其計(jì)算公式為交叉相關(guān)(CrossCorrelation, COR)算法
權(quán)利要求
1. 一種基于混合地形輪廓匹配TERCOM算法和粒子濾波的地形輔助導(dǎo)航方法，其特征在于，該導(dǎo)航方法包括搜索模式、跟蹤模式以及丟失模式和模式邏輯轉(zhuǎn)換，在飛行器進(jìn)入地形匹配區(qū)域后，該導(dǎo)航方法開(kāi)始工作，其具體實(shí)現(xiàn)步驟(1)進(jìn)入地形匹配區(qū)域判斷根據(jù)慣導(dǎo)系統(tǒng)的指示位置和數(shù)字高程基準(zhǔn)圖左上角和右下角的坐標(biāo)進(jìn)行判斷，飛行器是否進(jìn)入地形匹配區(qū)域；利用坐標(biāo)變換公式將每一個(gè)地形匹配區(qū)左下上角和右下角的高斯直角坐標(biāo)轉(zhuǎn)換為地理坐標(biāo)，并分別記為(startsλ，StartiJJ和(end^A，end^L)，i = 1，2，…為匹配區(qū)編號(hào)；根據(jù)慣導(dǎo)系統(tǒng)指示的位置(λ，L)，判斷飛行器是否到達(dá)或進(jìn)入第i個(gè)地形匹配區(qū) starti_A ( λ < end^ λ ⑴ startj_L ^ L < end^L (2)式中，λ為經(jīng)度，L為緯度，上述兩個(gè)公式同時(shí)成立且等號(hào)不成立時(shí)表明飛行器已進(jìn)入匹配區(qū)，當(dāng)?shù)忍?hào)剛好成立時(shí)，表明飛行器到達(dá)匹配區(qū)的邊緣；(2)搜索模式進(jìn)入匹配區(qū)后，利用高度表連續(xù)獲取高度測(cè)量信息，分別利用基于相關(guān)分析原理的交叉相關(guān)COR算法、平均絕對(duì)差MAD算法和均方差MSD算法進(jìn)行批處理運(yùn)算，獲得飛行器的位置的估計(jì)值、和；根據(jù)位置的估計(jì)值構(gòu)造位置一致性判斷準(zhǔn)則為
2.根據(jù)權(quán)利要求1所述的一種基于混合地形輪廓匹配TERCOM算法和粒子濾波的地形輔助導(dǎo)航方法，其特征在于所述步驟C3)跟蹤模式，其具體方法為①初始化設(shè)置適當(dāng)?shù)腜(Xtl)分布，生成N個(gè)服從ρ (Xtl)分布的隨機(jī)樣本&(1)，其中i = 1，…N， 即對(duì)P (Xtl)進(jìn)行抽樣，令P ( )服從正態(tài)分布；②時(shí)間更新生成N個(gè)服從Pq(Qh)的隨機(jī)數(shù)込―丨，并根據(jù)狀態(tài)方程對(duì)粒子進(jìn)行一步預(yù)測(cè)Xl(I) = Xk^+Vk^T+ QU(6)③測(cè)量更新在得到當(dāng)前測(cè)量值4后，計(jì)算各粒子的似然比，從而獲得歸一化的權(quán)值和更新粒子后的狀態(tài)估計(jì)值為Prk(Zk-KXld))W =---! ix&m.))i = 1，…，N ⑵
3.根據(jù)權(quán)利要求1所述的一種基于混合地形輪廓匹配TERCOM算法和粒子濾波的地形輔助導(dǎo)航方法，其特征在于所述步驟(4)丟失模式和模式邏輯轉(zhuǎn)換，其具體方法為a、記錄搜索模式下連續(xù)三次通過(guò)表決的匹配結(jié)果和時(shí)間，并以第三次匹配的匹配結(jié)果作為系統(tǒng)跟蹤算法的初始位置；b、依據(jù)系統(tǒng)跟蹤算法計(jì)算出當(dāng)前時(shí)刻的匹配位置，經(jīng)度λ(k)和緯度L(k)；C、根據(jù)前5次匹配定位結(jié)果，推算出5個(gè)當(dāng)前時(shí)刻的位置估計(jì)值，經(jīng)度㈨和緯度 L{k_n)(k) (η = 1,2,…，5)，其計(jì)算公式為
全文摘要
本發(fā)明一種基于混合地形輪廓匹配TERCOM算法和粒子濾波的地形輔助導(dǎo)航方法，目的是解決在大的初始位置誤差，平坦地形區(qū)域以及飛行器高機(jī)動(dòng)飛行時(shí)濾波器的發(fā)散問(wèn)題。該導(dǎo)航方法包含搜索模式、跟蹤模式以及丟失模式和模式邏輯裝換，在搜索模式下，分別利用平均絕對(duì)差MAD算法、均方差MSD算法和交叉相關(guān)COR算法批處理測(cè)量信息獲得位置估計(jì)信息，并根據(jù)一致性表決算法，在大的初始位置誤差下快速、準(zhǔn)確地實(shí)現(xiàn)初始定位；在跟蹤模式下采用粒子濾波遞推處理測(cè)量信息實(shí)現(xiàn)連續(xù)匹配定位；丟失模式和模式邏輯轉(zhuǎn)換實(shí)現(xiàn)初始定位后由搜索模式轉(zhuǎn)入跟蹤模式，出現(xiàn)偽定位后由跟蹤模式轉(zhuǎn)換為搜索模式。本發(fā)明能夠保證在大的初始位置誤差穩(wěn)定可靠地實(shí)現(xiàn)連續(xù)定位。
文檔編號(hào)G01C21/24GK102426018SQ20111024004
公開(kāi)日2012年4月25日 申請(qǐng)日期2011年8月19日 優(yōu)先權(quán)日2011年8月19日
發(fā)明者劉昊, 李萌, 趙君力, 趙龍, 陸守雷 申請(qǐng)人:北京航空航天大學(xué)