基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí)延補(bǔ)償方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于慣性導(dǎo)航技術(shù)領(lǐng)域，特別是一種基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn) 時(shí)延補(bǔ)償方法。
【背景技術(shù)】
[0002] 慣性導(dǎo)航系統(tǒng)在進(jìn)入導(dǎo)航狀態(tài)之前，必須經(jīng)過初始對準(zhǔn)。慣導(dǎo)系統(tǒng)傳遞對準(zhǔn)是子 慣導(dǎo)系統(tǒng)數(shù)據(jù)動(dòng)態(tài)地匹配主慣導(dǎo)系統(tǒng)數(shù)據(jù)的過程，即慣導(dǎo)系統(tǒng)用主慣導(dǎo)系統(tǒng)的速度和姿態(tài) 信息對準(zhǔn)子慣導(dǎo)系統(tǒng)。傳遞對準(zhǔn)過程中，由于主慣導(dǎo)的解算和傳輸延遲，子慣導(dǎo)解算信息與 進(jìn)行匹配的基準(zhǔn)信息不能完全同步，有些情況下時(shí)間延遲較大，從而會(huì)影響傳遞對準(zhǔn)中卡 爾曼濾波器的收斂速度和精度。
[0003] 文獻(xiàn)1;《計(jì)算機(jī)仿真》第25卷第2期中"傳遞對準(zhǔn)中時(shí)間延遲的補(bǔ)償方法"中提 出了直接利用主慣導(dǎo)姿態(tài)四元素、載機(jī)角速度和延遲時(shí)間等主慣導(dǎo)數(shù)據(jù)進(jìn)行四元素計(jì)算， 得出延遲后的主慣導(dǎo)姿態(tài)四元素，進(jìn)而獲得補(bǔ)償后的量測失準(zhǔn)角的算法。但是此算法中時(shí) 間延遲是依靠硬件設(shè)備測量出來的，并且沒有考慮到存在大安裝角的情況。

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

[0004] 本發(fā)明的目的在于提供一種速度快、精度高的基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對 準(zhǔn)時(shí)延補(bǔ)償方法。
[0005] 實(shí)現(xiàn)本發(fā)明目的的技術(shù)解決方案為：一種基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí) 延補(bǔ)償方法，包括W下步驟：
[0006] 步驟1，利用主慣導(dǎo)系統(tǒng)的導(dǎo)航信息對子慣導(dǎo)系統(tǒng)進(jìn)行粗對準(zhǔn)；
[0007] 步驟2,主慣導(dǎo)系統(tǒng)和子慣導(dǎo)系統(tǒng)分別進(jìn)行導(dǎo)航解算，主慣導(dǎo)系統(tǒng)將得到的速度與 姿態(tài)信息傳輸給子慣導(dǎo)系統(tǒng)；
[0008] 步驟3,根據(jù)導(dǎo)航解算結(jié)果分別得到主、子慣導(dǎo)系統(tǒng)的捷聯(lián)矩陣，同時(shí)構(gòu)造子慣導(dǎo) 系統(tǒng)的安裝角補(bǔ)償矩陣，在子慣導(dǎo)系統(tǒng)中構(gòu)造觀測量，即得到主、子慣導(dǎo)系統(tǒng)之間的速度差 和量測失準(zhǔn)角；
[0009] 步驟4,進(jìn)行標(biāo)準(zhǔn)卡爾曼濾波迭代解算；建立捷聯(lián)慣導(dǎo)系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方 程W及系統(tǒng)觀測量，并進(jìn)行卡爾曼濾波迭代解算，得到主、子慣導(dǎo)系統(tǒng)傳遞對準(zhǔn)的時(shí)延估計(jì) 值并進(jìn)行補(bǔ)償，得到對時(shí)間延遲補(bǔ)償后的子慣導(dǎo)系統(tǒng)的姿態(tài)失準(zhǔn)角。
[0010] 本發(fā)明與現(xiàn)有技術(shù)相比，其顯著優(yōu)點(diǎn)是；（1)將時(shí)間延遲作為狀態(tài)變量加入了濾 波之中，可W精確有效地估計(jì)出時(shí)間延遲并進(jìn)行補(bǔ)償，顯著提高了傳遞對準(zhǔn)精度；（2)解決 了時(shí)間延遲問題，避免了利用硬件解決時(shí)間延遲帶來的麻煩；（3)最優(yōu)姿態(tài)算法與其他傳 統(tǒng)傳遞對準(zhǔn)算法相比具有形式簡單、易于理解，可大大減小計(jì)算量的優(yōu)點(diǎn)，能夠在子慣導(dǎo)安 裝角為大角度的環(huán)境下正常工作。
【附圖說明】
[0011] 圖1是本發(fā)明基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí)延補(bǔ)償方法的工作流程圖。
[0012] 圖2是實(shí)施例1中傳遞對準(zhǔn)仿真試驗(yàn)對時(shí)間延遲補(bǔ)償后載體飛行軌跡圖。
[0013] 圖3是實(shí)施例1中傳遞對準(zhǔn)仿真試驗(yàn)對時(shí)間延遲補(bǔ)償前、后姿態(tài)失準(zhǔn)角曲線對比 圖。
[0014] 圖4是本發(fā)明傳遞對準(zhǔn)仿真試驗(yàn)對時(shí)間延遲估計(jì)曲線圖。
【具體實(shí)施方式】
[0015] 下面結(jié)合附圖及具體實(shí)施例對本發(fā)明作進(jìn)一步詳細(xì)說明。
[0016] 結(jié)合圖1，本發(fā)明基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí)延補(bǔ)償方法，包括W下步 驟：
[0017] 步驟1，利用主慣導(dǎo)系統(tǒng)的導(dǎo)航信息對子慣導(dǎo)系統(tǒng)進(jìn)行粗對準(zhǔn)，即將主慣導(dǎo)系統(tǒng)的 速度、姿態(tài)、位置信息傳輸給子慣導(dǎo)系統(tǒng)，子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)傳輸?shù)男畔⑼瓿沙跏蓟?作。
[0018] 步驟2,主慣導(dǎo)系統(tǒng)和子慣導(dǎo)系統(tǒng)分別進(jìn)行導(dǎo)航解算，主慣導(dǎo)系統(tǒng)將得到的速度與 姿態(tài)信息傳輸給子慣導(dǎo)系統(tǒng)；
[0019] 步驟3,根據(jù)導(dǎo)航解算結(jié)果分別得到主、子慣導(dǎo)系統(tǒng)的捷聯(lián)矩陣，同時(shí)構(gòu)造子慣導(dǎo) 系統(tǒng)的安裝角補(bǔ)償矩陣，在子慣導(dǎo)系統(tǒng)中構(gòu)造觀測量，即得到主、子慣導(dǎo)系統(tǒng)之間的速度差 和量測失準(zhǔn)角；
[0020] 步驟4,進(jìn)行標(biāo)準(zhǔn)卡爾曼濾波迭代解算；建立捷聯(lián)慣導(dǎo)系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方 程W及系統(tǒng)觀測量，并進(jìn)行卡爾曼濾波迭代解算，得到主、子慣導(dǎo)系統(tǒng)傳遞對準(zhǔn)的時(shí)延估計(jì) 值并進(jìn)行補(bǔ)償，得到對時(shí)間延遲補(bǔ)償后的子慣導(dǎo)系統(tǒng)的姿態(tài)失準(zhǔn)角；所述進(jìn)行標(biāo)準(zhǔn)卡爾曼 濾波迭代解算，具體如下：
[0021] 第1步，根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)的力學(xué)編排建立慣導(dǎo)誤差模型，得到系統(tǒng)的誤差方程， 包括姿態(tài)誤差方程、速度誤差方程及慣性器件誤差方程，具體如下：
[0022] ( 1. 1)姿態(tài)誤差方程與速度誤差方程為：
[0023]
【主權(quán)項(xiàng)】
1. 一種基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí)延補(bǔ)償方法，其特征在于，包括以下步 驟： 步驟1，利用主慣導(dǎo)系統(tǒng)的導(dǎo)航信息對子慣導(dǎo)系統(tǒng)進(jìn)行粗對準(zhǔn)； 步驟2,主慣導(dǎo)系統(tǒng)和子慣導(dǎo)系統(tǒng)分別進(jìn)行導(dǎo)航解算，主慣導(dǎo)系統(tǒng)將得到的速度與姿態(tài) 信息傳輸給子慣導(dǎo)系統(tǒng)； 步驟3,根據(jù)導(dǎo)航解算結(jié)果分別得到主、子慣導(dǎo)系統(tǒng)的捷聯(lián)矩陣，同時(shí)構(gòu)造子慣導(dǎo)系統(tǒng) 的安裝角補(bǔ)償矩陣，在子慣導(dǎo)系統(tǒng)中構(gòu)造觀測量，即得到主、子慣導(dǎo)系統(tǒng)之間的速度差和量 測失準(zhǔn)角； 步驟4,進(jìn)行標(biāo)準(zhǔn)卡爾曼濾波迭代解算：建立捷聯(lián)慣導(dǎo)系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方程以 及系統(tǒng)觀測量，并進(jìn)行卡爾曼濾波迭代解算，得到主、子慣導(dǎo)系統(tǒng)傳遞對準(zhǔn)的時(shí)延估計(jì)值并 進(jìn)行補(bǔ)償，得到對時(shí)間延遲補(bǔ)償后的子慣導(dǎo)系統(tǒng)的姿態(tài)失準(zhǔn)角。
2. 根據(jù)權(quán)利要求1所述的基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí)延補(bǔ)償方法，其特征 在于，步驟1所述利用主慣導(dǎo)系統(tǒng)的導(dǎo)航信息對子慣導(dǎo)系統(tǒng)進(jìn)行粗對準(zhǔn)，即將主慣導(dǎo)系統(tǒng) 的速度、姿態(tài)、位置信息傳輸給子慣導(dǎo)系統(tǒng)，子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)傳輸?shù)男畔⑼瓿沙跏蓟?工作。
3. 根據(jù)權(quán)利要求1所述的基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí)延補(bǔ)償方法，其特征 在于，步驟4所述進(jìn)行標(biāo)準(zhǔn)卡爾曼濾波迭代解算，具體如下： 第1步，根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)的力學(xué)編排建立慣導(dǎo)誤差模型，得到系統(tǒng)的誤差方程，包括 姿態(tài)誤差方程、速度誤差方程及慣性器件誤差方程； 第2步，根據(jù)慣導(dǎo)誤差模型及系統(tǒng)誤差方程建立捷聯(lián)慣導(dǎo)系統(tǒng)的狀態(tài)方程； 第3步，選取主、子慣導(dǎo)系統(tǒng)之間的速度差和量測失準(zhǔn)角作為觀測量，推導(dǎo)出平臺失準(zhǔn) 角、姿態(tài)失準(zhǔn)角與量測失準(zhǔn)角之間的關(guān)系，進(jìn)而得到最優(yōu)姿態(tài)匹配算法； 第4步，根據(jù)建立的系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方程以及系統(tǒng)的觀測量進(jìn)行卡爾曼濾波 迭代解算，得到主、子慣導(dǎo)系統(tǒng)傳遞對準(zhǔn)的時(shí)延估計(jì)值以及對時(shí)間延遲補(bǔ)償后的子慣導(dǎo)系 統(tǒng)的姿態(tài)失準(zhǔn)角。
4. 根據(jù)權(quán)利要求3所述的基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí)延補(bǔ)償方法，其特征 在于，第1步所述系統(tǒng)的誤差方程包括姿態(tài)誤差方程、速度誤差方程及慣性器件誤差方程， 具體如下： (1. 1)姿態(tài)誤差方程與速度誤差方程為：
式中，小。小1^、?。?分別為東、北、天方向的平臺失準(zhǔn)角，3￥^6￥1<、6￥ 11分別為載體東、 北、天方向的速度誤差，VE、VN、Vu分別為東、北、天方向的速度，L、h分別表示緯度和高度，R m 為橢球子午圈上各點(diǎn)的曲率半徑，Rn為卯酉圈上各點(diǎn)的曲率半徑，Wie為地球轉(zhuǎn)動(dòng)角速率， fE、fN、f；是慣導(dǎo)系統(tǒng)加速度計(jì)測量到的比力通過坐標(biāo)轉(zhuǎn)換后得到的導(dǎo)航系下東、北、天三個(gè) 方向上的比力分量，ε Ε、εΝ、％分別為地理坐標(biāo)系內(nèi)陀螺的等效漂移在東、北、天方向的分 量，▽ E、V N、V υ分別為地理坐標(biāo)系內(nèi)加速度計(jì)的等效偏置在東、北、天方向的分量； 速度Vi及比力&均由主慣導(dǎo)系統(tǒng)提供，其中i = E，N，U ;在地理坐標(biāo)系內(nèi)陀螺的等效 漂移ε i和加速度計(jì)的等效偏置^ i，由于對準(zhǔn)時(shí)間短，這些等效量為隨機(jī)常值，即有：
(1.2)安裝誤差角方程為：
將傳遞對準(zhǔn)中的時(shí)間延遲作為隨機(jī)常值處理，即主慣導(dǎo)系統(tǒng)時(shí)間延遲At的模型為：
其屮，λ Ε、λ Ν、λ u為子慣導(dǎo)系統(tǒng)相對主慣導(dǎo)系統(tǒng)的安裝誤差角在載體系在x、y、z軸上 的分量。
5.根據(jù)權(quán)利要求3所述的基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí)延補(bǔ)償方法，其特征 在于，第2步所述捷聯(lián)慣導(dǎo)系統(tǒng)的狀態(tài)方程如下式所示：
式中X為系統(tǒng)狀態(tài)矢量，Z表示系統(tǒng)狀態(tài)矢量的導(dǎo)數(shù)，F(xiàn)為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，G為系統(tǒng) 噪聲驅(qū)動(dòng)矩陣，W為系統(tǒng)噪聲矢量，具體如下： (2. I) X為系統(tǒng)狀態(tài)矢量： χ=[φι SVi Δ t]16X1，i = E，N，U (2. 2) F為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣：
系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣中矩陣塊F1與F2如下所示：
并假設(shè)其為零均值高斯白噪聲，協(xié)方差陣為E(WWt) = Q，Q為系統(tǒng)過程噪聲方差陣； 式中：^為姿態(tài)矩陣^^^^^^分別為陀螺隨機(jī)漂移在義^^軸上的分量^^^^、 Wvz分別為加速度計(jì)常值偏置在x、y、z軸上的分量。
6.根據(jù)權(quán)利要求3所述的基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí)延補(bǔ)償方法，其特征 在于，第3步所述最優(yōu)姿態(tài)匹配算法，具體為： (3. 1)選取主、子慣導(dǎo)系統(tǒng)之間的速度差和量測失準(zhǔn)角Φπ作為觀測量，得到系統(tǒng)量測 方程為： Z = HX+V 式中，Z= [Φ" ΦΠ? Φπζ δνΕ δνΝ SVu]為觀測量，φ"、φν、φπζ*量測失準(zhǔn)角在 x、y、z軸上的分量；V是量測噪聲且為均值為零的高斯白噪聲，其協(xié)方差為E[VVT] =R，R為 量測噪聲方差陣； (3. 2)量測失準(zhǔn)角Φπ由以下推導(dǎo)得到：
式中，為主、子慣導(dǎo)系統(tǒng)體坐標(biāo)系間的轉(zhuǎn)換矩陣，C:為子慣導(dǎo)系統(tǒng)姿態(tài)矩陣的轉(zhuǎn) 置，為主慣導(dǎo)系統(tǒng)姿態(tài)矩陣，C7是子慣導(dǎo)系統(tǒng)安裝角姿態(tài)矩陣的補(bǔ)償值，經(jīng)過該矩陣補(bǔ) 償后還存在不能直接補(bǔ)償?shù)陌惭b誤差角λ，且有：
即九=^ ，得到最優(yōu)姿態(tài)匹配算法； 式中C：為矩陣CT啲轉(zhuǎn)置，cr為矩陣C的轉(zhuǎn)置，c：為子慣導(dǎo)數(shù)學(xué)平臺坐標(biāo)系到導(dǎo)航 坐標(biāo)系的轉(zhuǎn)換矩陣，Φ為子慣導(dǎo)平臺失準(zhǔn)角； (3. 3)考慮時(shí)間延遲后，量測失準(zhǔn)角Φπ表示為：
速度誤差為： δ V = Vs (t+ Δ t) -Vm (t) -fm (t) Δ t 式中，vs(t+At)為子慣導(dǎo)在t+At時(shí)刻的速度，Vni(t)為主慣導(dǎo)在t時(shí)刻的速度，乙⑴ 為主慣導(dǎo)在t時(shí)刻的比力； 由此可得系統(tǒng)的量測矩陣：
式中fiE、fiN、分別為主慣導(dǎo)對準(zhǔn)時(shí)刻的比力信息在東、北、天三個(gè)方向上的比力分 量，《mx、ωην、為主慣導(dǎo)對準(zhǔn)時(shí)刻在機(jī)體坐標(biāo)系X、y、Z軸上的角速度。
【專利摘要】本發(fā)明公開了一種基于最優(yōu)姿態(tài)匹配的動(dòng)基座傳遞對準(zhǔn)時(shí)延補(bǔ)償方法。包括以下步驟：利用主慣導(dǎo)系統(tǒng)的導(dǎo)航信息對子慣導(dǎo)系統(tǒng)進(jìn)行粗對準(zhǔn)；主慣導(dǎo)系統(tǒng)和子慣導(dǎo)系統(tǒng)分別進(jìn)行導(dǎo)航解算，主慣導(dǎo)系統(tǒng)將得到的速度與姿態(tài)信息傳輸給子慣導(dǎo)系統(tǒng)；根據(jù)導(dǎo)航解算結(jié)果分別得到主、子慣導(dǎo)系統(tǒng)的捷聯(lián)矩陣，同時(shí)構(gòu)造子慣導(dǎo)系統(tǒng)的安裝角補(bǔ)償矩陣，在子慣導(dǎo)系統(tǒng)中構(gòu)造觀測量得到主、子慣導(dǎo)系統(tǒng)之間的速度差和量測失準(zhǔn)角；建立捷聯(lián)慣導(dǎo)系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方程以及系統(tǒng)觀測量，并進(jìn)行卡爾曼濾波迭代解算，得到主、子慣導(dǎo)系統(tǒng)傳遞對準(zhǔn)的時(shí)延估計(jì)值并進(jìn)行補(bǔ)償，得到對時(shí)間延遲補(bǔ)償后的子慣導(dǎo)系統(tǒng)的姿態(tài)失準(zhǔn)角。本發(fā)明能夠進(jìn)行精確的時(shí)延估計(jì)及補(bǔ)償，具有廣泛的用途。
【IPC分類】G01C21-16, G01C25-00
【公開號】CN104748761
【申請?zhí)枴緾N201310731634
【發(fā)明人】陳帥, 鐘潤伍, 王磊杰, 常耀偉, 金磊, 余威, 徐芹麗
【申請人】南京理工大學(xué)
【公開日】2015年7月1日
【申請日】2013年12月26日