專利名稱：一種慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法
一種慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法技術(shù)領(lǐng)域
本發(fā)明屬于慣性/衛(wèi)星組合導(dǎo)航技術(shù)領(lǐng)域，具體涉及一種慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法。
背景技術(shù)：
慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)已深入軍事、航空航天領(lǐng)域，在維護國家安全、促進經(jīng)濟增長方面有著重要的作用。
由于衛(wèi)星導(dǎo)航系統(tǒng)易受電磁環(huán)境以及復(fù)雜地形、樹木、建筑的干擾影響，導(dǎo)致可見衛(wèi)星數(shù)減少，不能正常定位，使得慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)在諸如電子對抗戰(zhàn)場、山谷、河床的導(dǎo)航，石油天然氣勘測、都市樓宇間盲人向?qū)Х?wù)場合等復(fù)雜的環(huán)境下的使用受到了極大的限制，傳統(tǒng)的位置、速度組合的松散組合導(dǎo)航算法無法有效解決上述問題。發(fā)明內(nèi)容
本發(fā)明要解決的技術(shù)問題是慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)采用位置/速度松散組合在復(fù)雜環(huán)境中不能正常定位。
本發(fā)明的技術(shù)方案如下所述
該方法以衛(wèi)星定位系統(tǒng)測量的偽距、偽距率與慣導(dǎo)解算出的相對衛(wèi)星的距離信息及速率信息之差為觀測量，利用卡爾曼濾波對各項慣性誤差以及衛(wèi)星定位系統(tǒng)的時鐘參數(shù)進行估計，并采用輸出校正的方式對組合輸出進行修正；
該方法包括以下步驟
步驟1.建立模型；
步驟2.估計系統(tǒng)誤差；
步驟3.自適應(yīng)調(diào)整參數(shù)；
步驟4.輸出校正。
步驟I包括以下步驟
步驟1.1.構(gòu)建狀態(tài)方程；
步驟1. 2.構(gòu)建觀測方程；
步驟1. 2.1坐標(biāo)系轉(zhuǎn)換；
步驟1. 2. 2構(gòu)建偽距觀測方程；
步驟1. 2. 3.構(gòu)建偽距率觀測方程；
步驟1. 2. 4.構(gòu)建系統(tǒng)觀測方程。
步驟1.1構(gòu)建狀態(tài)方程包括以下步驟
選取經(jīng)緯度誤差、速度誤差、失調(diào)角、游動方位角誤差、陀螺隨機漂移、接收機鐘偏和頻漂共13個參量作為狀態(tài)向量X，即
X = [ Δ V AV ΔφΔλ ΔΙ Φ Φ Φ ε ε ε δ ρ δρ]τ 八 L*x * y ιν χ y ζ χ y ζ Κν」
慣導(dǎo)基本誤差方程采用游動方位坐標(biāo)系，構(gòu)建狀態(tài)方程如下
權(quán)利要求
1.一種慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法，其特征在于該方法以衛(wèi)星定位系統(tǒng)測量的偽距、偽距率與慣導(dǎo)解算出的相對衛(wèi)星的距離信息及速率信息之差為觀測量，利用卡爾曼濾波對各項慣性誤差以及衛(wèi)星定位系統(tǒng)的時鐘參數(shù)進行估計，并采用輸出校正的方式對組合輸出進行修正； 該方法包括以下步驟 步驟1.建立模型； 步驟2.估計系統(tǒng)誤差； 步驟3.自適應(yīng)調(diào)整參數(shù)； 步驟4.輸出校正。
2.根據(jù)權(quán)利要求1所述的慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法，其特征在于步驟I包括以下步驟 步驟1.1.構(gòu)建狀態(tài)方程； 步驟1. 2.構(gòu)建觀測方程； 步驟1. 2.1坐標(biāo)系轉(zhuǎn)換； 步驟1. 2. 2構(gòu)建偽距觀測方程； 步驟1. 2. 3.構(gòu)建偽距率觀測方程； 步驟1. 2. 4.構(gòu)建系統(tǒng)觀測方程。
3.根據(jù)權(quán)利要求2所述的慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法，其特征在于步驟1.1構(gòu)建狀態(tài)方程包括以下步驟 選取經(jīng)緯度誤差、速度誤差、失調(diào)角、游動方位角誤差、陀螺隨機漂移、接收機鐘偏和頻漂共13個參量作為狀態(tài)向量X，即 X = [ Δ V Δ V Δ φ Δλ Ak φ Φ Φ ε ε ε δ ρ δ ρ ]τ 慣導(dǎo)基本誤差方程采用游動方位坐標(biāo)系，構(gòu)建狀態(tài)方程如下
4.根據(jù)權(quán)利要求3所述的慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法，其特征在于步驟1. 2構(gòu)建觀測方程包括以下步驟 步驟1. 2.1坐標(biāo)系轉(zhuǎn)換 地理坐標(biāo)下的緯度、經(jīng)度、高度(Φ，λ，H)轉(zhuǎn)換至地心笛卡爾坐標(biāo)系下(X，Y，Ζ)的公式為
5.根據(jù)權(quán)利要求4所述的慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法，其特征在于步驟2利用卡爾曼濾波算法對系統(tǒng)誤差進行估計，具體包括以下步驟 選取狀態(tài)方程中的13維狀態(tài)向量X為待估變量， X = [ Δ Vx Δ Vy Δ φ Δ λ Ak Φχ Φγ Φζ ε χ ε y ε ζ δρ δ ρν]τ 取系統(tǒng)噪聲向量^ = \^χ Vj εχ sy εζ Wp VpvJ 將公式(I) 公式(13)表示成狀態(tài)方程的形式，如下所示X(t) = A(t)X(t) + DW(t)(28)式中 A(t)表示t時刻系統(tǒng)參數(shù)矩陣； D表不系統(tǒng)噪聲矩陣； X(t)表示t時刻狀態(tài)向量X ； W(t)表示t時刻噪聲向量W ; 將公式(25)和公式(27)進行離散化處理，得
6.根據(jù)權(quán)利要求5所述的慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法，其特征在于步驟3具體包括以下步驟當(dāng)可見衛(wèi)星數(shù)不小于4時，偽距、偽距率緊組合導(dǎo)航算法采用當(dāng)前拍偽距、偽距率觀測值可直接實現(xiàn)對慣導(dǎo)速度、位置誤差的估計；當(dāng)可見衛(wèi)星數(shù)等于3時，緊組合算法利用慣導(dǎo)模型短時間內(nèi)的精確性提高系統(tǒng)的可觀性以實現(xiàn)對慣導(dǎo)速度、位置誤差的估計；當(dāng)可見衛(wèi)星數(shù)為2時，緊組合算法利用慣導(dǎo)模型短時間內(nèi)的精確性以及衛(wèi)星定位系統(tǒng)的時鐘穩(wěn)定性提高系統(tǒng)的可觀測性從而實現(xiàn)較長時間內(nèi)慣導(dǎo)速度、位置誤差估計滿足較高的精度要求。
7.根據(jù)權(quán)利要求5所述的慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法，其特征在于步驟4具體包括以下步驟 步驟4.1.位置修正； 步驟4. 2.速度修正。
8.根據(jù)權(quán)利要求7所述的慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法，其特征在于步驟4.1具體包括以下步驟Φ Zh = Φ ins" Δ Φ(31) 入Zh =人ins_ A入 式中 Φζ1ι表示組合緯度； Azh表示組合經(jīng)度； Φ ins表示純慣性緯度； 入ins表示純慣性經(jīng)度。
9.根據(jù)權(quán)利要求8所述的慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法，其特征在于步驟4. 2具體包括以下步驟Δ Vn = Δ Vxcosk+ Δ VysinkAVe= AVxSink-AVyCosk (32) Vzh = yms - AVηηη Vzh = Vms - AV eeη 式中 7廣表示組合X向速度； 「^表示組合Y向速度； 表示純慣性X向速度； 表示純慣性Y向速度； F廣表示組合北向速度； F廣表示組合東向速度。
全文摘要
本發(fā)明屬于慣性/衛(wèi)星組合導(dǎo)航技術(shù)領(lǐng)域，具體涉及一種慣性/衛(wèi)星導(dǎo)航系統(tǒng)偽距/偽距率緊組合方法。本發(fā)明的方法以衛(wèi)星定位系統(tǒng)測量的偽距、偽距率與慣導(dǎo)解算出的相對衛(wèi)星的距離信息及速率信息之差為觀測量，利用卡爾曼濾波對各項慣性誤差以及衛(wèi)星定位系統(tǒng)的時鐘參數(shù)進行估計，并采用輸出校正的方式對組合輸出進行修正。其具體包括以下步驟建立模型；估計系統(tǒng)誤差；自適應(yīng)調(diào)整參數(shù)；輸出校正。本發(fā)明解決了慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)采用位置/速度松散組合在復(fù)雜環(huán)境中不能正常定位的技術(shù)問題，采用偽距/偽距率緊組合，取得了使慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)在復(fù)雜環(huán)境中能夠正常定位的有益效果。
文檔編號G01C21/16GK102998685SQ20111027441
公開日2013年3月27日 申請日期2011年9月15日 優(yōu)先權(quán)日2011年9月15日
發(fā)明者李邦清, 王黎斌, 劉峰, 李文耀, 扈光鋒, 謝仕民 申請人:北京自動化控制設(shè)備研究所