基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn)方法
【專利摘要】本發(fā)明提供一種基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn)方法，包括以下步驟：步驟1、子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)系統(tǒng)采用固定頻率發(fā)送的導(dǎo)航參數(shù)完成粗對(duì)準(zhǔn)，前述導(dǎo)航參數(shù)包括速度、姿態(tài)和位置信息；步驟2、采用“量測失準(zhǔn)角+速度”匹配方式建立系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方程；步驟3、子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)以固定頻率發(fā)送給子慣導(dǎo)系統(tǒng)的速度與姿態(tài)信息構(gòu)造觀測量；以及步驟4、采用卡爾曼濾波進(jìn)行迭代解算，估算子慣導(dǎo)系統(tǒng)的姿態(tài)失準(zhǔn)角及速度、位置誤差的狀態(tài)估算值，并據(jù)此對(duì)子慣導(dǎo)系統(tǒng)的導(dǎo)航信息進(jìn)行修正，完成傳遞對(duì)準(zhǔn)。
【專利說明】基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn)方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于慣性導(dǎo)航【技術(shù)領(lǐng)域】，是一種基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn)方法。

【背景技術(shù)】
[0002] 傳遞對(duì)準(zhǔn)是將子慣導(dǎo)輸出的導(dǎo)航信息與主慣導(dǎo)輸出的相應(yīng)導(dǎo)航信息相比較，并以 此對(duì)子慣導(dǎo)姿態(tài)誤差角進(jìn)行最優(yōu)估計(jì)的一種方法。它是解決機(jī)載武器和任務(wù)傳感器在空中 動(dòng)基座條件下初始對(duì)準(zhǔn)問題的主要方法。
[0003] 目前，在失準(zhǔn)角為小角度條件下的快速傳遞對(duì)準(zhǔn)技術(shù)，在國內(nèi)，一般采用"速度+ 姿態(tài)"匹配方式，在快速性和精確性方面都能滿足需求。但在某些特殊的姿態(tài)角時(shí)，使部分 量測矩陣為0,導(dǎo)致某些失準(zhǔn)角誤差難以估計(jì)。在國外文獻(xiàn)中，使用主、子慣導(dǎo)姿態(tài)矩陣的乘 積結(jié)果作為量測量的方法得到廣泛的使用。量測失準(zhǔn)角匹配法是在文獻(xiàn)《Rapidtransfer alignmentfortacticalweaponapplications》中提出的量測失準(zhǔn)角的基礎(chǔ)上發(fā)展和推 導(dǎo)的。
[0004] 在《西北工業(yè)大學(xué)學(xué)報(bào)》第25卷第5期中，文獻(xiàn)《傳遞對(duì)準(zhǔn)中一種新的姿態(tài)匹配算 法》中，推導(dǎo)了平臺(tái)失準(zhǔn)角，量測失準(zhǔn)角和實(shí)際失準(zhǔn)角三者間的關(guān)系，并采用該方法進(jìn)行仿 真，精度和快速性均能滿足要求。但不能滿足大失準(zhǔn)角的情況。


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

[0005] 本發(fā)明目的在于提供一種可用于大失準(zhǔn)角條件下的基于量測失準(zhǔn)角的動(dòng)基座傳 遞對(duì)準(zhǔn)方法，能夠滿足快速性與精確性要求。
[0006] 本發(fā)明的上述目的通過獨(dú)立權(quán)利要求的技術(shù)特征實(shí)現(xiàn)，從屬權(quán)利要求以另選或有 利的方式發(fā)展獨(dú)立權(quán)利要求的技術(shù)特征。
[0007] 為達(dá)成上述目的，本發(fā)明所采用的的技術(shù)方案如下：
[0008] -種基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn)方法，包括以下步驟：
[0009] 步驟1、子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)系統(tǒng)采用固定頻率發(fā)送的導(dǎo)航參數(shù)完成粗對(duì)準(zhǔn)，前 述導(dǎo)航參數(shù)包括速度、姿態(tài)和位置信息；
[0010] 步驟2、采用"量測失準(zhǔn)角+速度"匹配方式建立系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方程；
[0011] 步驟3、子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)以固定頻率發(fā)送給子慣導(dǎo)系統(tǒng)的速度與姿態(tài)信息 構(gòu)造觀測量；以及
[0012] 步驟4、采用卡爾曼濾波進(jìn)行迭代解算，估算子慣導(dǎo)系統(tǒng)的姿態(tài)失準(zhǔn)角及速度、位 置誤差的狀態(tài)估算值，并據(jù)此對(duì)子慣導(dǎo)系統(tǒng)的導(dǎo)航信息進(jìn)行修正，完成傳遞對(duì)準(zhǔn)。
[0013] 由以上本發(fā)明的技術(shù)方案可知，本發(fā)明的有益效果在于：
[0014] 1、相對(duì)于傳統(tǒng)的傳遞對(duì)準(zhǔn)算法，本發(fā)明的方法具有形式簡單、易于理解，可大大減 小計(jì)算量的優(yōu)點(diǎn)。
[0015] 2、本發(fā)明所提出的對(duì)準(zhǔn)方法適用于大失準(zhǔn)角條件，對(duì)主慣導(dǎo)姿態(tài)進(jìn)行補(bǔ)償構(gòu)造觀 測量后，可在線性模型下直接實(shí)現(xiàn)。

【專利附圖】

【附圖說明】
[0016] 圖1為本發(fā)明一實(shí)施方式基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn)方法的實(shí)現(xiàn)流程圖。
[0017] 圖2、3、4分別為利用圖1實(shí)施例的方法實(shí)現(xiàn)的經(jīng)過卡爾曼濾波對(duì)準(zhǔn)后子慣導(dǎo)的姿 態(tài)角估計(jì)誤差曲線示意圖，其中：圖2為x軸姿態(tài)角估計(jì)誤差曲線，圖3為y軸姿態(tài)角估計(jì) 誤差曲線，圖4為z軸姿態(tài)角估計(jì)誤差曲線。

【具體實(shí)施方式】
[0018]為了更了解本發(fā)明的技術(shù)內(nèi)容，特舉具體實(shí)施例并配合所附圖式說明如下。
[0019] 如圖1所示，根據(jù)本發(fā)明的較優(yōu)實(shí)施例，一種基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn) 方法，應(yīng)用于由主慣導(dǎo)系統(tǒng)和子慣導(dǎo)系統(tǒng)組成的慣性導(dǎo)航系統(tǒng)，前述主慣導(dǎo)系統(tǒng)和子慣導(dǎo) 系統(tǒng)均為捷聯(lián)式慣性導(dǎo)航系統(tǒng)，該方法的實(shí)現(xiàn)包括以下步驟：
[0020] 步驟1、子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)系統(tǒng)采用固定頻率發(fā)送的導(dǎo)航參數(shù)完成粗對(duì)準(zhǔn)，前 述導(dǎo)航參數(shù)包括速度、姿態(tài)和位置信息；
[0021] 步驟2、采用"量測失準(zhǔn)角+速度"匹配方式建立系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方程；
[0022] 步驟3、子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)以固定頻率發(fā)送給子慣導(dǎo)系統(tǒng)的速度與姿態(tài)信息 構(gòu)造觀測量；以及
[0023] 步驟4、采用卡爾曼濾波進(jìn)行迭代解算，估算子慣導(dǎo)系統(tǒng)的姿態(tài)失準(zhǔn)角及速度、位 置誤差的狀態(tài)估算值，并據(jù)此對(duì)子慣導(dǎo)系統(tǒng)的導(dǎo)航信息進(jìn)行修正，完成傳遞對(duì)準(zhǔn)。
[0024] 下面結(jié)合附圖1-4所示，詳細(xì)說明上述各步驟的具體實(shí)施。
[0025] 在前述步驟1中，子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)的導(dǎo)航參數(shù)完成粗對(duì)準(zhǔn)，即初始化過程。
[0026] 在前述步驟2中，采用"量測失準(zhǔn)角+速度"匹配方式建立系統(tǒng)狀態(tài)方程、系統(tǒng)觀 測方程，具體如下：
[0027] 步驟2-1、根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)的力學(xué)編排建立慣導(dǎo)誤差模型，得到系統(tǒng)的誤差方 程，包括姿態(tài)誤差方程、速度誤差方程及慣性器件誤差方程；
[0028] 步驟2-2、根據(jù)慣導(dǎo)誤差模型及系統(tǒng)誤差方程建立捷聯(lián)慣導(dǎo)系統(tǒng)的狀態(tài)方程。
[0029] 作為可選的方式，前述步驟2-1中，姿態(tài)誤差方程、速度誤差方程及慣性器件誤差 方程建立如下：

【權(quán)利要求】
1. 一種基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn)方法，應(yīng)用于由主慣導(dǎo)系統(tǒng)和子慣導(dǎo)系統(tǒng)組 成的慣性導(dǎo)航系統(tǒng)，前述主慣導(dǎo)系統(tǒng)和子慣導(dǎo)系統(tǒng)均為捷聯(lián)式慣性導(dǎo)航系統(tǒng)，其特征在于， 該方法的實(shí)現(xiàn)包括以下步驟： 步驟1、子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)系統(tǒng)采用固定頻率發(fā)送的導(dǎo)航參數(shù)完成粗對(duì)準(zhǔn)，前述導(dǎo) 航參數(shù)包括速度、姿態(tài)和位置信息； 步驟2、采用"量測失準(zhǔn)角+速度"匹配方式建立系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方程； 步驟3、子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)以固定頻率發(fā)送給子慣導(dǎo)系統(tǒng)的速度與姿態(tài)信息構(gòu)造 觀測量；以及 步驟4、采用卡爾曼濾波進(jìn)行迭代解算，估算子慣導(dǎo)系統(tǒng)的姿態(tài)失準(zhǔn)角及速度、位置誤 差的狀態(tài)估算值，并據(jù)此對(duì)子慣導(dǎo)系統(tǒng)的導(dǎo)航信息進(jìn)行修正，完成傳遞對(duì)準(zhǔn)。
2. 根據(jù)權(quán)利要求1所述的基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn)方法，其特征在于，在前 述步驟2中，采用"量測失準(zhǔn)角+速度"匹配方式建立系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方程，具體如 下： 步驟2-1、系統(tǒng)姿態(tài)誤差方程、速度誤差方程及慣性器件誤差方程建立如下：
式中，φn為子慣導(dǎo)系統(tǒng)的失準(zhǔn)角，為地球自傳角速率在導(dǎo)航坐標(biāo)下 的投影，<為導(dǎo)航系相對(duì)地球系的旋轉(zhuǎn)角速率，G為子慣導(dǎo)的姿態(tài)矩陣，SV=[δ\δVn SVu]為子慣導(dǎo)系統(tǒng)的東、北、天方向的速度誤差，F(xiàn)n =|>7G 分別為主慣導(dǎo)系統(tǒng)的 東、北、天方向的速度，I為地球轉(zhuǎn)動(dòng)角速率，r=[/rC 是主慣導(dǎo)系統(tǒng)加速度計(jì) 測量到的比力通過坐標(biāo)轉(zhuǎn)換后得到的導(dǎo)航系下東、北、天三個(gè)方向上的比力分量，ε= [εχ eyεζ]為子慣導(dǎo)陀螺在體坐標(biāo)系下的隨機(jī)常值漂移，▽= [VxVyVJ為子慣導(dǎo)加速 度計(jì)在體坐標(biāo)系下的隨機(jī)常值偏置，λ= [λχAyλJ為子慣導(dǎo)系統(tǒng)相對(duì)主慣導(dǎo)系統(tǒng)的安 裝誤差角在體坐標(biāo)系下的分量； <#'我4,4分別表示對(duì)對(duì)應(yīng)參數(shù)的微分； 前述的系統(tǒng)狀態(tài)方程建立如下： j=FX+GW 式中，X為系統(tǒng)狀態(tài)矢量，F(xiàn)為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，G為系統(tǒng)噪聲驅(qū)動(dòng)矩陣，W為系統(tǒng)噪 聲矢量，且： 1. X為系統(tǒng)狀態(tài)矢量： χ=[φδνε V λ]15Χ1 2. F為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣：
式中，矩陣塊F1與F2表達(dá)如下：
其中，Rm為子午圈曲率半徑；Rn為卯酉圈曲率半徑山為載體當(dāng)前的高度；L為載體當(dāng)前 所處位置的緯度； 步驟2-2、系統(tǒng)觀測方程建立如下： Z=HX+V 式中，RΛ?]為觀測量，!爐,.、釋2為量測失準(zhǔn)角在x、y、z 軸上的分量；V是量測噪聲且為均值為零的高斯白噪聲，其協(xié)方差為E[VVT] =R，R為量測 噪聲方差陣；H為系統(tǒng)的觀測矩陣：
其中：1為單位矩陣，C：為主慣導(dǎo)系統(tǒng)姿態(tài)矩陣的轉(zhuǎn)置。
3.根據(jù)權(quán)利要求2所述的基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn)方法，其特征在于，所述步驟3中,按照下述關(guān)系構(gòu)造觀測量Z, _ =i-，且λ] 由此可得：
由上式可得到觀測量Z中的且觀測量δνε，δνη，SVu由主慣導(dǎo)的速度減去子 (^K)iCl(C) 慣導(dǎo)的速度得到，BP|呎=Ct-Ie,其中，--=〇χeΓ二]為主慣導(dǎo)的東北天速 ?汾;JU:JU:J' 度，fG 為子慣導(dǎo)的東北天速度，由此可得到前述系統(tǒng)觀測方程中的全部的 觀測量。
4.根據(jù)權(quán)利要求3所述的基于量測失準(zhǔn)角的動(dòng)基座傳遞對(duì)準(zhǔn)方法，其特征在于，所述 步驟4的實(shí)現(xiàn)包括： 將前述系統(tǒng)狀態(tài)方程、系統(tǒng)觀測方程離散化后得到： Xk=Φ^-ι^-1+Γ,-Λ-ι Zk =HkVVk 其中，Xk表示被估計(jì)狀態(tài)，為V1時(shí)刻的一步轉(zhuǎn)移矩陣，Γη為k_i時(shí)刻系統(tǒng)噪 聲驅(qū)動(dòng)陣，Wk為系統(tǒng)激勵(lì)噪聲序列，Zk為觀測量，Hk為量測陣，Vk為量測噪聲序列； 根據(jù)適用于離散系統(tǒng)的卡爾曼濾波器基本方程的卡爾曼濾波算法具體如下： 狀態(tài)一步預(yù)測方程： Xk/k-Ι- 其中，為k_l時(shí)刻到k時(shí)刻的系統(tǒng)狀態(tài)一步預(yù)測值，Xlrt為k-l時(shí)刻系統(tǒng)狀態(tài)估計(jì) 值，Φ,η為k-Ι時(shí)刻到k時(shí)刻的系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣； 一步預(yù)測均方誤差方程： I=m+rMm, 其中，Pk/H為k-l時(shí)刻到k時(shí)刻的系統(tǒng)狀態(tài)協(xié)方差陣，Plrl為k-l時(shí)刻的系統(tǒng)狀態(tài)協(xié)方 差陣，Qlrt為k-Ι時(shí)刻系統(tǒng)噪聲矩陣； 最優(yōu)濾波增益方程： Kk=Pte^HllHkPklkβΙ+Ε,?1 其中，Kk為k時(shí)刻系統(tǒng)增益矩陣，Hk為k時(shí)刻系統(tǒng)量測矩陣，Rk為k時(shí)刻系統(tǒng)量測噪聲 矩陣； 狀態(tài)估計(jì)方程： Xk - ^k/k-l+Kk (Zk-HkXkZ^1) 估計(jì)均方誤差方程： Pk=U-K1H1 Wk：1 ,(/^^/Zi)r +KkRkKl, 其中，Qk是系統(tǒng)過程噪聲方差陣，Pk為均方誤差陣； 聯(lián)立前述狀態(tài)一步預(yù)測方程、一步預(yù)測均方誤差方程、最優(yōu)濾波增益方程、狀態(tài)估計(jì)方 程和估計(jì)均方誤差方程，采用卡爾曼濾波算法進(jìn)行狀態(tài)估計(jì)，估計(jì)出狀態(tài)矢量X=[ΦSV ε▽?duì)薦15X1的狀態(tài)估計(jì)值； 然后，利用狀態(tài)矢量Χ=[ΦSVε▽入]15>〇中的姿態(tài)失準(zhǔn)角φ的估計(jì)值和速度 誤差SV的估計(jì)值對(duì)子慣導(dǎo)系統(tǒng)的導(dǎo)航信息進(jìn)行修正，從而完成傳遞對(duì)準(zhǔn)。
【文檔編號(hào)】G01C25/00GK104236586SQ201410453907
【公開日】2014年12月24日 申請(qǐng)日期:2014年9月5日 優(yōu)先權(quán)日:2014年9月5日
【發(fā)明者】陳帥, 董亮, 鐘潤伍, 余威, 常耀偉, 王磊杰, 金磊, 丁翠玲, 劉亞玲 申請(qǐng)人:南京理工大學(xué)