一種基于雙模型切換的二次傳遞對(duì)準(zhǔn)方法
【專利摘要】本發(fā)明提供一種基于雙模型切換的二次傳遞對(duì)準(zhǔn)方法，包括以下步驟：主慣導(dǎo)系統(tǒng)采用固定頻率向子慣導(dǎo)系統(tǒng)發(fā)送導(dǎo)航參數(shù)，子慣導(dǎo)系統(tǒng)利用主慣導(dǎo)的導(dǎo)航參數(shù)完成粗對(duì)準(zhǔn)；子慣導(dǎo)系統(tǒng)基于主慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)構(gòu)造觀測(cè)量；在四元數(shù)誤差模型下使用擴(kuò)展卡爾曼濾波進(jìn)行迭代解算，待失準(zhǔn)角縮小到一預(yù)設(shè)角度α?xí)r，進(jìn)行濾波切換，然后在歐拉角誤差模型下使用標(biāo)準(zhǔn)卡爾曼濾波進(jìn)行迭代解算，直到失準(zhǔn)角估計(jì)值收斂且穩(wěn)定；利用估計(jì)出的失準(zhǔn)角來(lái)修正子慣導(dǎo)系統(tǒng)的姿態(tài)矩陣，得到捷聯(lián)初始姿態(tài)矩陣，以完成二次傳遞對(duì)準(zhǔn)。本發(fā)明的方法同時(shí)具備了線性模型濾波收斂速度快，精度高，非線性模型使用范圍廣的優(yōu)點(diǎn)，同時(shí)可滿足系統(tǒng)快速性，精確性，健壯性的要求。
【專利說(shuō)明】一種基于雙模型切換的二次傳遞對(duì)準(zhǔn)方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于慣性導(dǎo)航系統(tǒng)【技術(shù)領(lǐng)域】，特別是一種基于雙模型切換的二次傳遞對(duì)準(zhǔn) 方法。

【背景技術(shù)】
[0002] 傳遞對(duì)準(zhǔn)是利用主子慣導(dǎo)系統(tǒng)的速度、姿態(tài)等導(dǎo)航參數(shù)進(jìn)行匹配，濾波估計(jì)出子 慣導(dǎo)系統(tǒng)相對(duì)與主慣導(dǎo)系統(tǒng)的失準(zhǔn)角，從而建立精確的子慣導(dǎo)數(shù)學(xué)平臺(tái)，同時(shí)對(duì)子慣導(dǎo)的 導(dǎo)航信息進(jìn)行初始化的過(guò)程。
[0003]目前，在失準(zhǔn)角為小角度條件下的快速傳遞對(duì)準(zhǔn)技術(shù)已經(jīng)較為成熟，在快速性和 精確性方面都能滿足需求。然而，在工程應(yīng)用中經(jīng)常會(huì)出現(xiàn)失準(zhǔn)角為大角度的情況。在這 種情況下傳統(tǒng)的線性誤差模型將不能準(zhǔn)確的描述慣導(dǎo)系統(tǒng)的誤差傳播特性，從而會(huì)給濾波 結(jié)果帶來(lái)較大誤差，甚至導(dǎo)致濾波發(fā)散。
[0004]文獻(xiàn)《IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS》第35卷中， 文獻(xiàn)《Comparison of SDINS In-Flight Alignment Using Equivalent Error Models》中 研究了一種可用于大失準(zhǔn)角條件下的基于四元數(shù)誤差的非線性模型。該模型能應(yīng)用在大失 準(zhǔn)角條件下，但存在著濾波收斂慢，濾波收斂精度略差的缺點(diǎn)。


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

[0005] 本發(fā)明旨在針對(duì)在慣性導(dǎo)航系統(tǒng)動(dòng)基座對(duì)準(zhǔn)中存在大失準(zhǔn)角時(shí)的快速與精確傳 遞對(duì)準(zhǔn)問(wèn)題，提出一種基于雙模型切換的二次傳遞對(duì)準(zhǔn)方法。
[0006] 為實(shí)現(xiàn)上述目的，本發(fā)明所采用的技術(shù)方案如下：
[0007] -種基于雙模型切換的二次傳遞對(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)，前述方法包 括以下步驟：
[0008] 步驟1、主慣導(dǎo)系統(tǒng)采用固定頻率向子慣導(dǎo)系統(tǒng)發(fā)送導(dǎo)航參數(shù)，子慣導(dǎo)系統(tǒng)利用主 慣導(dǎo)的導(dǎo)航參數(shù)完成粗對(duì)準(zhǔn)，前述導(dǎo)航參數(shù)包括速度，姿態(tài)和位置信息；
[0009] 步驟2、子慣導(dǎo)系統(tǒng)基于主慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)構(gòu)造觀測(cè)量；
[0010] 步驟3、在四元數(shù)誤差模型下使用擴(kuò)展卡爾曼濾波進(jìn)行迭代解算，待失準(zhǔn)角縮小到 一預(yù)設(shè)角度a時(shí)，進(jìn)行濾波切換，然后在歐拉角誤差模型下使用標(biāo)準(zhǔn)卡爾曼濾波進(jìn)行迭代 解算，直到失準(zhǔn)角估計(jì)值收斂且穩(wěn)定；以及
[0011] 步驟4、利用步驟3估計(jì)出的失準(zhǔn)角來(lái)修正子慣導(dǎo)系統(tǒng)的姿態(tài)矩陣，得到捷聯(lián)初始 姿態(tài)矩陣，以完成二次傳遞對(duì)準(zhǔn)。
[0012] 進(jìn)一步的實(shí)施例中，前述預(yù)設(shè)角度a的取值范圍為：|aI彡5°。
[0013] 由以上本發(fā)明的技術(shù)方案可知，本發(fā)明的有益效果在于：
[0014]1、相對(duì)于基于四元數(shù)的非線性誤差模型，本發(fā)明的方法具有計(jì)算量小，濾波收斂 快，對(duì)準(zhǔn)精度高的優(yōu)點(diǎn)。
[0015] 2、相對(duì)于基于歐拉角的線性誤差模型，本發(fā)明的方法具有適用范圍廣的優(yōu)點(diǎn)，既 可用于大失準(zhǔn)角條件下，也可用于小失準(zhǔn)角條件下。
[0016] 3、本發(fā)明的方法同時(shí)具備了線性模型濾波收斂速度快，精度高，非線性模型使用 范圍廣的優(yōu)點(diǎn)，同時(shí)滿足了系統(tǒng)的快速性，精確性，健壯性要求。

【專利附圖】

【附圖說(shuō)明】
[0017] 圖1是二次傳遞對(duì)準(zhǔn)仿真試驗(yàn)，基于雙模型切換的二次傳遞對(duì)準(zhǔn)與基于四元數(shù)誤 差的傳遞對(duì)準(zhǔn)結(jié)果對(duì)比圖。
[0018] 圖2是傳遞對(duì)準(zhǔn)跑車試驗(yàn)中，對(duì)主慣導(dǎo)系統(tǒng)補(bǔ)償不同角度時(shí)的姿態(tài)失準(zhǔn)角估計(jì)曲 線。

【具體實(shí)施方式】
[0019] 為了更了解本發(fā)明的技術(shù)內(nèi)容，特舉具體實(shí)施例并配合所附圖式說(shuō)明如下。
[0020] 根據(jù)本發(fā)明的較優(yōu)實(shí)施例，一種基于雙模型切換的二次傳遞對(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)，前述方法包括以下步驟：
[0021] 步驟1、主慣導(dǎo)系統(tǒng)采用固定頻率向子慣導(dǎo)系統(tǒng)發(fā)送導(dǎo)航參數(shù)，子慣導(dǎo)系統(tǒng)利用主 慣導(dǎo)的導(dǎo)航參數(shù)完成粗對(duì)準(zhǔn)，前述導(dǎo)航參數(shù)包括速度，姿態(tài)和位置信息；
[0022] 步驟2、子慣導(dǎo)系統(tǒng)基于主慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)構(gòu)造觀測(cè)量；
[0023] 步驟3、在四元數(shù)誤差模型下使用擴(kuò)展卡爾曼濾波進(jìn)行迭代解算，待失準(zhǔn)角縮小到 一預(yù)設(shè)角度a時(shí)，進(jìn)行濾波切換，然后在歐拉角誤差模型下使用標(biāo)準(zhǔn)卡爾曼濾波進(jìn)行迭代 解算，直到失準(zhǔn)角估計(jì)值收斂且穩(wěn)定；以及
[0024] 步驟4、利用步驟3估計(jì)出的失準(zhǔn)角來(lái)修正子慣導(dǎo)系統(tǒng)的姿態(tài)矩陣，得到捷聯(lián)初始 姿態(tài)矩陣，以完成二次傳遞對(duì)準(zhǔn)。
[0025] 作為一個(gè)較佳的實(shí)施例，前述預(yù)設(shè)角度a的取值范圍為：|a| < 5°。例如可取 值為2°。
[0026] 下面詳細(xì)說(shuō)明上述各步驟的具體實(shí)施。
[0027]1)在四元數(shù)誤差模型下使用擴(kuò)展卡爾曼濾波進(jìn)行迭代解算，其實(shí)現(xiàn)包括以下步 驟：
[0028]11)四元數(shù)誤差與速度誤差方程：

【權(quán)利要求】
1. 一種基于雙模型切換的二次傳遞對(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)，其特征在于，前 述方法包括以下步驟： 步驟1、主慣導(dǎo)系統(tǒng)采用固定頻率向子慣導(dǎo)系統(tǒng)發(fā)送導(dǎo)航參數(shù)，子慣導(dǎo)系統(tǒng)利用主慣導(dǎo) 的導(dǎo)航參數(shù)完成粗對(duì)準(zhǔn)，前述導(dǎo)航參數(shù)包括速度，姿態(tài)和位置信息； 步驟2、子慣導(dǎo)系統(tǒng)基于主慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)構(gòu)造觀測(cè)量； 步驟3、在四元數(shù)誤差模型下使用擴(kuò)展卡爾曼濾波進(jìn)行迭代解算，待失準(zhǔn)角縮小到一 預(yù)設(shè)角度a時(shí)，進(jìn)行濾波切換，然后在歐拉角誤差模型下使用標(biāo)準(zhǔn)卡爾曼濾波進(jìn)行迭代解 算，直到失準(zhǔn)角估計(jì)值收斂且穩(wěn)定；以及 步驟4、利用步驟3估計(jì)出的失準(zhǔn)角來(lái)修正子慣導(dǎo)系統(tǒng)的姿態(tài)矩陣，得到捷聯(lián)初始姿態(tài) 矩陣，以完成二次傳遞對(duì)準(zhǔn)。
2. 根據(jù)權(quán)利要求1所述的基于雙模型切換的二次傳遞對(duì)準(zhǔn)方法，其特征在于，前述預(yù) 設(shè)角度ct的取值范圍為：I a I彡5°。
3. 根據(jù)權(quán)利要求1所述的基于雙模型切換的二次傳遞對(duì)準(zhǔn)方法，其特征在于，前述步 驟3中，在四元數(shù)誤差模型下使用擴(kuò)展卡爾曼濾波進(jìn)行迭代解算包括以下步驟： 11)四元數(shù)誤差與速度誤差方程：
式中，<=[叫叫Iu;!為地理坐標(biāo)系相對(duì)于慣性坐標(biāo)系的角速度在地理坐標(biāo)系中 的投影，^ =[咚％咚]為陀螺儀的輸出在載體坐標(biāo)系中的投影；e b為陀螺的隨機(jī)常 值漂移，F(xiàn)g6表示陀螺測(cè)量白噪聲；為利用速度和位置信息得到的< 的誤差；CT為子 慣導(dǎo)捷聯(lián)矩陣；/6為子慣導(dǎo)加速度計(jì)輸出在載體坐標(biāo)系的投影；V6為加速度計(jì)在載體坐標(biāo) 系下的隨機(jī)常值漂移；< 為地球坐標(biāo)系相對(duì)于慣性坐標(biāo)系的角速度在導(dǎo)航坐標(biāo)系的投影； 為導(dǎo)航坐標(biāo)系相對(duì)于地球坐標(biāo)系的角速度在導(dǎo)航坐標(biāo)系的投影；vn= [w Vu]為子慣 導(dǎo)在導(dǎo)航坐標(biāo)系下的東北天速度；為速度誤差方程中的非線性項(xiàng)； 12)慣性器件誤差方程為：
選取主慣導(dǎo)系統(tǒng)和子慣導(dǎo)系統(tǒng)的速度差作為觀測(cè)量，得到系統(tǒng)觀測(cè)量為： Zobs = [ 5 Ve 5 Vn 5 Vul1 = H * X 式中，H= [04X3 I3x3 06X3]13X3，Sv由子慣導(dǎo)系統(tǒng)與主慣導(dǎo)系統(tǒng)在導(dǎo)航坐標(biāo)系下的速 度做差得到； 15)根據(jù)前述建立的系統(tǒng)狀態(tài)方程、系統(tǒng)觀測(cè)方程以及系統(tǒng)的觀測(cè)量，采用擴(kuò)展卡爾曼 濾波器進(jìn)行迭代解算，完成第一次對(duì)準(zhǔn)，估測(cè)并補(bǔ)償失準(zhǔn)角。
4.根據(jù)權(quán)利要求1所述的基于雙模型切換的二次傳遞對(duì)準(zhǔn)方法，其特征在于，所述步 驟3中，歐拉角誤差模型下的標(biāo)準(zhǔn)卡爾曼濾波迭代解算包括以下步驟： 21) 姿態(tài)誤差與速度誤差方程為：
式中：1是子慣導(dǎo)系統(tǒng)的東向失準(zhǔn)角，&是子慣導(dǎo)系統(tǒng)的北向失準(zhǔn)角，小"是子慣導(dǎo) 系統(tǒng)的天向失準(zhǔn)角；S Ve是子慣導(dǎo)系統(tǒng)的東向速度誤差，S Vn是子慣導(dǎo)系統(tǒng)的北向速度誤 差，S Vu是子慣導(dǎo)系統(tǒng)的天向速度誤差；VE為主慣導(dǎo)系統(tǒng)的東向速度，Vn為主慣導(dǎo)系統(tǒng)的北 向速度，Vu為主慣導(dǎo)系統(tǒng)的天向速度；fE為主慣導(dǎo)系統(tǒng)的東向比力，fN為主慣導(dǎo)系統(tǒng)的北向 比力，f"為主慣導(dǎo)系統(tǒng)的天向比力；L為主慣導(dǎo)系統(tǒng)的緯度，Rm是地球子午圈主曲率半徑，Rn 是地球卯酉圈主曲率半徑，h是高度，《 是地球自轉(zhuǎn)角速度；^為子慣導(dǎo)系統(tǒng)X軸陀螺常 值漂移，4為子慣導(dǎo)系統(tǒng)y軸陀螺常值漂移，<為子慣導(dǎo)系統(tǒng)z軸陀螺常值漂移；為子 慣導(dǎo)系統(tǒng)X軸加速度計(jì)常值偏置，為子慣導(dǎo)系統(tǒng)y軸加速度計(jì)常值偏置，Vf為子慣導(dǎo) 系統(tǒng)z軸加速度計(jì)常值偏置； 22) 慣性器件誤差方程：
式中，入x為子慣導(dǎo)系統(tǒng)X軸安裝誤差角，入y為子慣導(dǎo)系統(tǒng)y軸安裝誤差角，入z為子 慣導(dǎo)系統(tǒng)z軸安裝誤差角； 可得系統(tǒng)方程為：
24)建立系統(tǒng)量測(cè)方程，選取經(jīng)過(guò)補(bǔ)償后的主慣導(dǎo)系統(tǒng)與子慣導(dǎo)系統(tǒng)的姿態(tài)差和速度 差為觀測(cè)變量，系統(tǒng)量測(cè)方程表達(dá)為： Z = HX+V 式中：z = ^v、H 為量測(cè)變量，V是量測(cè)噪聲并且假定其為 均值為零的高斯白噪聲，其協(xié)方差為E[VVT] =R，Sv由主子慣導(dǎo)在導(dǎo)航坐標(biāo)系下的速度相 減得到，為子慣導(dǎo)與經(jīng)過(guò)補(bǔ)償后的主慣導(dǎo)姿態(tài)做差得到，由前述步驟1)得到的失準(zhǔn)角 可得到矩陣<?：：，將主慣導(dǎo)系統(tǒng)的捷聯(lián)矩陣與此矩陣相乘：
25)根據(jù)建立的系統(tǒng)狀態(tài)方程、系統(tǒng)觀測(cè)方程以及系統(tǒng)的觀測(cè)量，采用標(biāo)準(zhǔn)卡爾曼濾波 進(jìn)行濾波迭代解算，直到失準(zhǔn)角估計(jì)值收斂且穩(wěn)定。
【文檔編號(hào)】G01C25/00GK104330092SQ201410356581
【公開(kāi)日】2015年2月4日 申請(qǐng)日期:2014年7月24日 優(yōu)先權(quán)日:2014年7月24日
【發(fā)明者】陳帥, 鐘潤(rùn)伍, 王磊杰, 董亮, 余威, 常耀偉, 金磊 申請(qǐng)人:南京理工大學(xué)