專利名稱：一種基于粒子濾波的慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)方法
技術(shù)領(lǐng)域：
本發(fā)明屬于慣性導(dǎo)航技術(shù)領(lǐng)域，是一種用于慣性導(dǎo)航系統(tǒng)的初始對(duì)準(zhǔn)方法，適用于慣性導(dǎo)航系統(tǒng)工作前的靜基座自主初始對(duì)準(zhǔn)。

背景技術(shù)：
慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的目的是在慣性導(dǎo)航系統(tǒng)進(jìn)入導(dǎo)航工作狀態(tài)之前，建立起慣性導(dǎo)航系統(tǒng)載體坐標(biāo)系相對(duì)于某個(gè)參考坐標(biāo)系的姿態(tài)關(guān)系的過(guò)程。初始對(duì)準(zhǔn)誤差是慣性導(dǎo)航系統(tǒng)的主要誤差源之一，直接影響著慣性導(dǎo)航系統(tǒng)的工作精度，同時(shí)初始對(duì)準(zhǔn)時(shí)間是反映武器系統(tǒng)快速反應(yīng)能力的重要戰(zhàn)術(shù)指標(biāo)。較高的對(duì)準(zhǔn)精度往往需要較長(zhǎng)的對(duì)準(zhǔn)時(shí)間，解決對(duì)準(zhǔn)精度和對(duì)準(zhǔn)速度的矛盾是慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)研究與發(fā)展的重要方向。
慣性導(dǎo)航系統(tǒng)自主對(duì)準(zhǔn)的困難是系統(tǒng)不完全可觀，初始對(duì)準(zhǔn)的結(jié)果很大程度上依賴于系統(tǒng)狀態(tài)的可觀性。當(dāng)可觀測(cè)矩陣的秩相同時(shí)，卡爾曼濾波的效果并不相同，原因在于系統(tǒng)狀態(tài)變量的可觀測(cè)度不同。為了提高系統(tǒng)狀態(tài)的可觀測(cè)度，通過(guò)加入基座運(yùn)動(dòng)的方法進(jìn)行初始對(duì)準(zhǔn)能收到較好的效果，如二位置對(duì)準(zhǔn)。二位置方法能夠有效提高系統(tǒng)狀態(tài)的可觀測(cè)度，提高慣性導(dǎo)航系統(tǒng)的初始對(duì)準(zhǔn)精度、縮短初始對(duì)準(zhǔn)時(shí)間。
粒子濾波是一種針對(duì)非線性非高斯系統(tǒng)的狀態(tài)估計(jì)方法，具有優(yōu)異的濾波性能。作為解決非線性、非高斯動(dòng)態(tài)系統(tǒng)最優(yōu)估計(jì)的最有效方法之一，粒子濾波已經(jīng)成為研究的熱點(diǎn)。粒子濾波的缺陷在于其計(jì)算量對(duì)系統(tǒng)維數(shù)極其敏感，粒子數(shù)量隨維數(shù)增加呈指數(shù)規(guī)律增加。常規(guī)的初始對(duì)準(zhǔn)模型采用慣性導(dǎo)航系統(tǒng)的誤差方程，系統(tǒng)維數(shù)高，不適合粒子濾波的工程應(yīng)用。
本發(fā)明針對(duì)常規(guī)初始對(duì)準(zhǔn)模型維數(shù)高的特點(diǎn)，基于二位置對(duì)準(zhǔn)方法提出了一種適用于粒子濾波的二位置非線性初始對(duì)準(zhǔn)模型，該模型利用二位置方法消除慣性器件的常值誤差，因此無(wú)需對(duì)慣性器件誤差進(jìn)行狀態(tài)擴(kuò)展，從而在保證對(duì)準(zhǔn)精度的前提下降低了初始對(duì)準(zhǔn)模型的維數(shù)。將粒子濾波應(yīng)用于二位置非線性初始對(duì)準(zhǔn)模型，能夠提高初始對(duì)準(zhǔn)的精度和速度，滿足粒子濾波在工程實(shí)際中的應(yīng)用。


發(fā)明內(nèi)容
技術(shù)問(wèn)題本發(fā)明的目的是提供一種慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的方法，該方法可以實(shí)現(xiàn)慣性導(dǎo)航系統(tǒng)的高精度快速靜基座自主初始對(duì)準(zhǔn)。
技術(shù)方案本發(fā)明為實(shí)現(xiàn)上述目的，采用如下技術(shù)方案 本發(fā)明是一種基于粒子濾波的慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)方法，其特征在于包括下列步驟 1)建立基于二位置的水平姿態(tài)角計(jì)算模型； 2)建立基于二位置的航向角計(jì)算模型； 3)建立基于二位置的非線性初始對(duì)準(zhǔn)模型； 4)構(gòu)建用于初始對(duì)準(zhǔn)的高斯粒子濾波器； 5)由導(dǎo)航計(jì)算機(jī)，采集慣性器件輸出信息，并完成初始對(duì)準(zhǔn)濾波。
優(yōu)選地，所述水平姿態(tài)角計(jì)算模型的建立方法如下 開始靜基座初始對(duì)準(zhǔn)時(shí)，慣性導(dǎo)航系統(tǒng)具有一定的姿態(tài)角俯仰角為θ，橫滾角為γ，航向角為ψ，則加速度計(jì)輸出為 式中，f為機(jī)體系下的加速度計(jì)輸出，g為當(dāng)?shù)刂亓铀俣?，Δf代表加速度計(jì)誤差，

為機(jī)體系下的加速度計(jì)隨機(jī)常值，w為機(jī)體系下的加速度計(jì)隨機(jī)漂移，考慮為白噪聲，下標(biāo)x，y，z為機(jī)體軸向；Cnb表示了從導(dǎo)航坐標(biāo)系n系到機(jī)體坐標(biāo)系b系的轉(zhuǎn)換關(guān)系矩陣，滿足 將(2)式代人(1)式，并移項(xiàng)可以得到 式中，上標(biāo)1表示在慣性導(dǎo)航系統(tǒng)在靜基座初始時(shí)，即位置1處的值； 將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度，則有 式中，上標(biāo)2表示將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度后，即位置2處的值； 將(3)式減去(4)式可得 由(5)式可以得到 優(yōu)選地，所述航向角計(jì)算模型的建立方法如下 有機(jī)體系下的陀螺輸出 式中，ω為機(jī)體系下的陀螺輸出，ωie為地球自轉(zhuǎn)角速率，L為當(dāng)?shù)鼐暥?，Δω為陀螺誤差，ε為機(jī)體系下陀螺的隨機(jī)常值，v為機(jī)體系下陀螺的隨機(jī)漂移為白噪聲，下標(biāo)x，y，z為機(jī)體軸向；Cnb表示了從導(dǎo)航坐標(biāo)系n系到機(jī)體坐標(biāo)系b系的轉(zhuǎn)換關(guān)系矩陣 將Cnb代人(7)式，并移項(xiàng)可以得到 式中，上標(biāo)1表示在慣性導(dǎo)航系統(tǒng)在靜基座初始時(shí)，即位置1處的值； 將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度，則有 式中，上標(biāo)2表示將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度后，即位置2處的值； 將(8)式減去(9)式可得 由(10)式可以得到 優(yōu)選地，所述的建立基于二位置的非線性初始對(duì)準(zhǔn)模型的方法如下 初始對(duì)準(zhǔn)狀態(tài)方程如下 式中，θk為k時(shí)刻即第k次濾波的俯仰角，γk為k時(shí)刻橫滾角，ψk為k時(shí)刻航向角，θk-1為k-1時(shí)刻俯仰角，γk-1為k-1時(shí)刻橫滾角，ψk-1為k-1時(shí)刻航向角，wθ，wγ，wψ為三個(gè)姿態(tài)角的干擾為白噪聲； 取(5)式前兩行和(10)式第一行，并移項(xiàng)整理后有 其中所述(5)式和(10)式如下 式中，fxk1-fxk2為x軸向加速度計(jì)k時(shí)刻的二位置量測(cè)值，fyk1-fyk2為y軸向加速度計(jì)k時(shí)刻的二位置量測(cè)值，ωxk1-ωxk2為x軸向陀螺k時(shí)刻的二位置量測(cè)值，wxk1-wxk2為x軸向加速度計(jì)k時(shí)刻的二位置量測(cè)噪聲，wyk1-wyk2為y軸向加速度計(jì)k時(shí)刻的二位置量測(cè)噪聲，vxk1-vxk2為x軸向陀螺k時(shí)刻的二位置量測(cè)噪聲，量測(cè)噪聲都為白噪聲，上標(biāo)1表示在慣性導(dǎo)航系統(tǒng)在靜基座初始時(shí)，即位置1處的值，上標(biāo)2表示將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度后，即位置2處的值；k時(shí)刻即當(dāng)前時(shí)刻，k-1時(shí)刻即前一時(shí)刻。
優(yōu)選地，所述的初始對(duì)準(zhǔn)的高斯粒子濾波器的構(gòu)建包括如下步驟 Step1初始化，在p(x0)抽取N個(gè)樣本點(diǎn)x0i，相應(yīng)權(quán)值1/N； Step2抽樣 Step3計(jì)算權(quán)值并歸一化， Step4計(jì)算估計(jì)值， Step5對(duì)離散的濾波密度進(jìn)行高斯近似； Step6從近似的高斯分布中抽取新的粒子xkj，相應(yīng)權(quán)值1/N； Step7將新的粒子xkj進(jìn)行下一周期的濾波； 其中，p(·)為概率密度函數(shù)，q(·)為重要密度函數(shù)，N為大于1的自然數(shù)，x0為濾波初值，x0i為濾波初始粒子，xki為k時(shí)刻粒子，xk為k時(shí)刻狀態(tài)，xk-1為k-1時(shí)刻狀態(tài)，zk為k時(shí)刻二位置觀測(cè)量，z1:k-1為1時(shí)刻到k-1時(shí)刻的二位置觀測(cè)量，

為k時(shí)刻未歸一化的粒子權(quán)值，wki為k時(shí)刻歸一化后粒子的權(quán)值，

為k時(shí)刻的濾波估值，xkj為近似高斯分布中抽取的新粒子，i，j＝1，2，3......N。
優(yōu)選地，步驟5)所述的初始對(duì)準(zhǔn)完成后，在位置1處采集P次慣性器件輸出數(shù)據(jù)，慣性導(dǎo)航系統(tǒng)在靜基座初始時(shí)，即位置1處；然后，將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度即位置2處，基座穩(wěn)定后再采集P次慣性器件輸出數(shù)據(jù)；然后將兩次采集的數(shù)據(jù)依次構(gòu)造二位置量測(cè)量，并對(duì)采集數(shù)據(jù)進(jìn)行濾波處理，其中P為大于1的自然數(shù)。
有益效果 本發(fā)明的方法具有如下優(yōu)點(diǎn)利用二位置方法消除慣性器件的常值誤差，因此無(wú)需對(duì)慣性器件誤差進(jìn)行狀態(tài)擴(kuò)展，從而在保證對(duì)準(zhǔn)精度的前提下降低了初始對(duì)準(zhǔn)模型的維數(shù)；由于二位置非線性初始對(duì)準(zhǔn)模型的維數(shù)僅為三維，適合粒子濾波算法在初始對(duì)準(zhǔn)中的工程應(yīng)用，較好的滿足了粒子濾波在慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)中的實(shí)時(shí)性要求；提高了慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的精度和速度。



圖1是用于初始對(duì)準(zhǔn)的高斯粒子濾波流程。
圖2是俯仰角誤差曲線。
圖3是橫滾角誤差曲線。
圖4是航向角誤差曲線。

具體實(shí)施例方式 下面結(jié)合附圖對(duì)發(fā)明的技術(shù)方案進(jìn)行詳細(xì)說(shuō)明 如圖1所示，是用于初始對(duì)準(zhǔn)的高斯粒子濾波流程。
首先根據(jù)慣性導(dǎo)航系統(tǒng)的工作原理建立基于二位置的水平姿態(tài)角計(jì)算模型；然后建立基于二位置的航向角計(jì)算模型；在此基礎(chǔ)上建立基于二位置的非線性初始對(duì)準(zhǔn)模型；根據(jù)所確定的非線性初始對(duì)準(zhǔn)模型設(shè)計(jì)高斯粒子濾波器；最后由導(dǎo)航計(jì)算機(jī)采集慣性器件輸出信息，并完成初始對(duì)準(zhǔn)的濾波解算。
1)建立基于二位置的水平姿態(tài)角計(jì)算模型 初始對(duì)準(zhǔn)中假定慣性導(dǎo)航系統(tǒng)經(jīng)過(guò)標(biāo)定，常值漂移已經(jīng)進(jìn)行了補(bǔ)償，并考慮初始對(duì)準(zhǔn)時(shí)間較短，將加速度計(jì)誤差模型近似為隨機(jī)常值加隨機(jī)漂移，陀螺誤差模型近似為隨機(jī)常值加隨機(jī)漂移。開始靜基座初始對(duì)準(zhǔn)時(shí)，慣性導(dǎo)航系統(tǒng)具有一定的姿態(tài)角，其俯仰角為θ，橫滾角為γ，航向角為ψ，則加速度計(jì)輸出為 式中，f為機(jī)體系下的加速度計(jì)輸出，g為當(dāng)?shù)刂亓铀俣龋代表加速度計(jì)誤差，

為機(jī)體系下的加速度計(jì)隨機(jī)常值，w為機(jī)體系下的加速度計(jì)隨機(jī)漂移，考慮為白噪聲，下標(biāo)x，y，z為機(jī)體軸向。Cnb表示了從導(dǎo)航坐標(biāo)系n系到機(jī)體坐標(biāo)系b系的轉(zhuǎn)換關(guān)系矩陣，滿足 將(2)式代人(1)式，并移項(xiàng)可以得到 式中，上標(biāo)1表示在位置1處的值。
將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度，則有 式中，上標(biāo)2表示將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度后，即位置2處的值。
將(3)式減去(4)式可得 由(5)式可以得到 2)建立基于二位置的航向角計(jì)算模型 與水平姿態(tài)角求解方法類似，有 式中，ω為機(jī)體系下的陀螺輸出，ωie為地球自轉(zhuǎn)角速率，L為當(dāng)?shù)鼐暥龋う貫橥勇菡`差，ε為機(jī)體系下陀螺的隨機(jī)常值，v為機(jī)體系下陀螺的隨機(jī)漂移，考慮為白噪聲，下標(biāo)x，y，z為機(jī)體軸向。Cnb表示了從導(dǎo)航坐標(biāo)系n系到機(jī)體坐標(biāo)系b系的轉(zhuǎn)換關(guān)系矩陣。
將(2)式代人(7)式，并移項(xiàng)可以得到 式中，上標(biāo)1表示在位置1處的值。
將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度，則有 式中，上標(biāo)2表示將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度后，即位置2處的值。
將(8)式減去(9)式可得 由(10)式可以得到 3)建立基于二位置的非線性初始對(duì)準(zhǔn)模型 常規(guī)初始對(duì)準(zhǔn)模型中系統(tǒng)狀態(tài)維數(shù)為10維(采用加性四元數(shù)時(shí)為11維)，包含3個(gè)平臺(tái)誤差角(加性四元數(shù)誤差模型中為四個(gè)加性四元數(shù))、2個(gè)水平速度和5個(gè)慣性器件誤差狀態(tài)。對(duì)粒子濾波而言，采用如此高維的誤差模型，不能滿足初始對(duì)準(zhǔn)的實(shí)時(shí)性要求，因此必須在滿足初始對(duì)準(zhǔn)精度的前提下，對(duì)系統(tǒng)進(jìn)行降維，從而降低粒子濾波應(yīng)用于慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的計(jì)算量。
考慮初始對(duì)準(zhǔn)時(shí)慣性導(dǎo)航系統(tǒng)具有一定的姿態(tài)角，各個(gè)姿態(tài)角可認(rèn)為是一固定不變的角度，由于受外界干擾，表現(xiàn)為圍繞某一固定角度微小波動(dòng)。因此初始對(duì)準(zhǔn)狀態(tài)方程如下 式中，θk為k時(shí)刻即第k次濾波的俯仰角，γk為k時(shí)刻橫滾角，ψk為k時(shí)刻航向角，θk-1為k-1時(shí)刻俯仰角，γk-1為k-1時(shí)刻橫滾角，ψk-1為k-1時(shí)刻航向角，wθ，wγ，wψ為三個(gè)姿態(tài)角的干擾，考慮為白噪聲。
取(5)式前兩行和(10)式第一行，并移項(xiàng)整理后有 式中，fxk1-fxk2為x軸向加速度計(jì)k時(shí)刻的二位置量測(cè)值，fyk1-fyk2為y軸向加速度計(jì)k時(shí)刻的二位置量測(cè)值，ωxk1-ωxk2為x軸向陀螺k時(shí)刻的二位置量測(cè)值，wxk1-wxk2為x軸向加速度計(jì)k時(shí)刻的二位置量測(cè)噪聲，wyk1-wyk2為y軸向加速度計(jì)k時(shí)刻的二位置量測(cè)噪聲，vxk1-vxk2為x軸向陀螺k時(shí)刻的二位置量測(cè)噪聲，量測(cè)噪聲都考慮為白噪聲。
式(12)和(13)構(gòu)成了初始對(duì)準(zhǔn)的狀態(tài)方程和二位置非線性量測(cè)方程，狀態(tài)方程和量測(cè)方程維數(shù)都為3，遠(yuǎn)遠(yuǎn)低于常規(guī)二位置方法狀態(tài)方程的維數(shù)，有利于減少粒子濾波的計(jì)算量；其次，該初始對(duì)準(zhǔn)模型未進(jìn)行小角度線性化假設(shè)，模型準(zhǔn)確性要高于常規(guī)初始對(duì)準(zhǔn)方法，其對(duì)準(zhǔn)精度高于常規(guī)初始對(duì)準(zhǔn)方法；最后，由于三個(gè)姿態(tài)角都是直接可觀測(cè)的，因此其對(duì)準(zhǔn)速度要高于常規(guī)初始對(duì)準(zhǔn)方法。
由于粒子濾波初始賦值偏差過(guò)大時(shí)(大失準(zhǔn)角情況)，需要增加大量的粒子來(lái)保證濾波的收斂，因此可采用式(6)和(11)在精對(duì)準(zhǔn)前進(jìn)行粗對(duì)準(zhǔn)，式中的白噪聲分量可以采用多次測(cè)量取平均的方式來(lái)進(jìn)行消除。
4)構(gòu)建用于初始對(duì)準(zhǔn)的高斯粒子濾波器 粒子濾波采用蒙特卡洛積分方法實(shí)現(xiàn)最優(yōu)貝葉斯估計(jì)。最為典型的粒子濾波算法是重要性采樣重采樣SIR算法，該算法重要性函數(shù)計(jì)算簡(jiǎn)單，容易采樣，在各個(gè)領(lǐng)域得到了廣泛的應(yīng)用。
SIR算法存在一些問(wèn)題，如經(jīng)過(guò)重采樣步驟之后，各粒子在統(tǒng)計(jì)意義上將不再獨(dú)立，不能滿足樣本獨(dú)立同分布的要求，從而使得Monte Carl積分的收斂性不能保證；經(jīng)過(guò)重采樣過(guò)程之后，權(quán)值較大的粒子將會(huì)被多次復(fù)制，使得重采樣后的粒子集中包含了許多重復(fù)粒子，從而失去了粒子的多樣性；重采樣過(guò)程會(huì)損失信息，對(duì)于任何基于Monte Carl方法的估計(jì)問(wèn)題，經(jīng)過(guò)重采樣之后其估計(jì)精度會(huì)有所下降。為了解決這些問(wèn)題，目前提出了各種粒子濾波改進(jìn)算法，如高斯粒子濾波。
本發(fā)明中初始對(duì)準(zhǔn)的濾波算法采用高斯粒子濾波實(shí)現(xiàn)。下面給出用于初始對(duì)準(zhǔn)的高斯粒子濾波算法基本步驟 Step1初始化，在p(x0)抽取N個(gè)樣本點(diǎn)x0i，相應(yīng)權(quán)值1/N； Step2抽樣 Step3計(jì)算權(quán)值并歸一化， Step4計(jì)算估計(jì)值， Step5對(duì)離散的濾波密度進(jìn)行高斯近似； Step6從近似的高斯分布中抽取新的粒子xkj，相應(yīng)權(quán)值1/N； Step7將新的粒子xkj進(jìn)行下一周期的濾波； 其中，p(·)為概率密度函數(shù)，q(·)為重要密度函數(shù)，N為大于1的自然數(shù)，x0為濾波初值，x0i為濾波初始粒子，xki為k時(shí)刻粒子，xk為k時(shí)刻狀態(tài)，xk-1為k-1時(shí)刻狀態(tài)，zk為k時(shí)刻二位置觀測(cè)量，z1:k-1為1時(shí)刻到k-1時(shí)刻的二位置觀測(cè)量，

為k時(shí)刻未歸一化的粒子權(quán)值，wki為k時(shí)刻歸一化后粒子的權(quán)值，

為k時(shí)刻的濾波估值，xkj為近似高斯分布中抽取的新粒子，i，j＝1，2，3......N。
5)由導(dǎo)航計(jì)算機(jī)，采集慣性器件輸出信息，并完成初始對(duì)準(zhǔn)濾波。
導(dǎo)航計(jì)算機(jī)根據(jù)慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的狀態(tài)方程和觀測(cè)方程，采集慣性器件輸出信息，并完成初始對(duì)準(zhǔn)的高斯粒子濾波解算。
采用非線性初始對(duì)準(zhǔn)模型時(shí)各姿態(tài)角的收斂時(shí)間除了與二位置量測(cè)次數(shù)有關(guān)，而且與二位置量測(cè)的周期也有關(guān)，尤其是航向角的收斂速度還是要略低于水平姿態(tài)角的收斂速度。每次二位置量測(cè)時(shí)需要將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度，由于基座轉(zhuǎn)動(dòng)、停止等運(yùn)動(dòng)狀態(tài)需要較長(zhǎng)時(shí)間，因此影響了慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的時(shí)間。
考慮到初始對(duì)準(zhǔn)時(shí)間較短，對(duì)于中高精度的陀螺來(lái)說(shuō)，在初始對(duì)準(zhǔn)這段時(shí)間陀螺的零偏穩(wěn)定性對(duì)精度的影響較小，為縮短慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的時(shí)間，此處二位置量測(cè)時(shí)僅將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)1次，將數(shù)據(jù)采集和濾波分成兩步進(jìn)行，具體方法如下完成粗對(duì)準(zhǔn)后，在位置1處采集多次慣性器件輸出數(shù)據(jù)，仿真中設(shè)置為400次，實(shí)際中可根據(jù)需要設(shè)置；然后，將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度到位置2處，基座穩(wěn)定后再采集400次慣性器件輸出數(shù)據(jù)；然后將兩次采集的數(shù)據(jù)依次構(gòu)造二位置量測(cè)量，并對(duì)采集數(shù)據(jù)進(jìn)行濾波處理。當(dāng)陀螺精度較差時(shí)，可減少采集數(shù)據(jù)的次數(shù)，增加基座旋轉(zhuǎn)的次數(shù)。
綜上所述，對(duì)該方法進(jìn)行效果分析。從圖2和圖3中可以看出采用二位置非線性初始對(duì)準(zhǔn)模型的初始對(duì)準(zhǔn)方法水平姿態(tài)角能很快收斂，經(jīng)過(guò)幾次二位置量測(cè)之后其水平姿態(tài)誤差便收斂至6個(gè)角秒以內(nèi)。圖4顯示，航向角的收斂速度略低于水平姿態(tài)角，但也能夠迅速收斂至10個(gè)角分以內(nèi)。由于在二位置非線性初始對(duì)準(zhǔn)模型中，加速度計(jì)隨機(jī)常值和陀螺儀隨機(jī)常值通過(guò)二位置方法進(jìn)行了消除，不需要將他們擴(kuò)展入系統(tǒng)狀態(tài)方程，因此有效地降低了系統(tǒng)的維數(shù)，有利于粒子濾波在初始對(duì)準(zhǔn)中的應(yīng)用。
本發(fā)明說(shuō)明書中未作詳細(xì)描述的內(nèi)容屬于本領(lǐng)域?qū)I(yè)技術(shù)人員公知的現(xiàn)有技術(shù)。
權(quán)利要求
1.一種基于粒子濾波的慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)方法，其特征在于包括下列步驟
1)建立基于二位置的水平姿態(tài)角計(jì)算模型；
2)建立基于二位置的航向角計(jì)算模型；
3)建立基于二位置的非線性初始對(duì)準(zhǔn)模型；
4)構(gòu)建用于初始對(duì)準(zhǔn)的高斯粒子濾波器；
5)由導(dǎo)航計(jì)算機(jī)，采集慣性器件輸出信息，并完成初始對(duì)準(zhǔn)濾波。
2.根據(jù)權(quán)利要求1所述的一種基于粒子濾波的慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)方法，其特征在于步驟1)所述水平姿態(tài)角計(jì)算模型的建立方法如下
開始靜基座初始對(duì)準(zhǔn)時(shí)，慣性導(dǎo)航系統(tǒng)具有一定的姿態(tài)角俯仰角為θ，橫滾角為γ，航向角為ψ，則加速度計(jì)輸出為
式中，f為機(jī)體系下的加速度計(jì)輸出，g為當(dāng)?shù)刂亓铀俣?，Δf代表加速度計(jì)誤差，
為機(jī)體系下的加速度計(jì)隨機(jī)常值，w為機(jī)體系下的加速度計(jì)隨機(jī)漂移，考慮為白噪聲，下標(biāo)x，y，z為機(jī)體軸向；Cnb表示了從導(dǎo)航坐標(biāo)系n系到機(jī)體坐標(biāo)系b系的轉(zhuǎn)換關(guān)系矩陣，滿足
將(2)式代人(1)式，并移項(xiàng)可以得到
式中，上標(biāo)1表示在慣性導(dǎo)航系統(tǒng)在靜基座初始時(shí)，即位置1處的值；
將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度，則有
式中，上標(biāo)2表示將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度后，即位置2處的值；
將(3)式減去(4)式可得
由(5)式可以得到
3.根據(jù)權(quán)利要求1所述的一種基于粒子濾波的慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)方法，其特征在于步驟2)所述航向角計(jì)算模型的建立方法如下
有機(jī)體系下的陀螺輸出
式中，ω為機(jī)體系下的陀螺輸出，ωie為地球自轉(zhuǎn)角速率，L為當(dāng)?shù)鼐暥龋う貫橥勇菡`差，ε為機(jī)體系下陀螺的隨機(jī)常值，v為機(jī)體系下陀螺的隨機(jī)漂移為白噪聲，下標(biāo)x，y，z為機(jī)體軸向；Cnb表示了從導(dǎo)航坐標(biāo)系n系到機(jī)體坐標(biāo)系b系的轉(zhuǎn)換關(guān)系矩陣
將Cnb代人(7)式，并移項(xiàng)可以得到
式中，上標(biāo)1表示在慣性導(dǎo)航系統(tǒng)在靜基座初始時(shí)，即位置1處的值；
將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度，則有
式中，上標(biāo)2表示將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度后，即位置2處的值；
將(8)式減去(9)式可得
由(10)式可以得到
4.根據(jù)權(quán)利要求1所述的一種基于粒子濾波的慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)方法，其特征在于步驟3)所述的建立基于二位置的非線性初始對(duì)準(zhǔn)模型的方法如下
初始對(duì)準(zhǔn)狀態(tài)方程如下
式中，θk為k時(shí)刻即第k次濾波的俯仰角，γk為k時(shí)刻橫滾角，ψk為k時(shí)刻航向角，θk-1為k-1時(shí)刻俯仰角，γk-1為k-1時(shí)刻橫滾角，ψk-1為k-1時(shí)刻航向角，wθ，wγ，wψ為三個(gè)姿態(tài)角的干擾為白噪聲；
取(5)式前兩行和(10)式第一行，并移項(xiàng)整理后有
其中所述(5)式和(10)式如下
式中，fxk1-fxk2為x軸向加速度計(jì)k時(shí)刻的二位置量測(cè)值，fyk1-fyk2為y軸向加速度計(jì)k時(shí)刻的二位置量測(cè)值，ωxk1-ωxk2為x軸向陀螺k時(shí)刻的二位置量測(cè)值，wxk1-wxk2為x軸向加速度計(jì)k時(shí)刻的二位置量測(cè)噪聲，wyk1-wyk2為y軸向加速度計(jì)k時(shí)刻的二位置量測(cè)噪聲，vxk1-vxk2為x軸向陀螺k時(shí)刻的二位置量測(cè)噪聲，量測(cè)噪聲都為白噪聲，上標(biāo)1表示在慣性導(dǎo)航系統(tǒng)在靜基座初始時(shí)，即位置1處的值，上標(biāo)2表示將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度后，即位置2處的值；k時(shí)刻即當(dāng)前時(shí)刻，k-1時(shí)刻即前一時(shí)刻。
5.根據(jù)權(quán)利要求1所述的一種基于粒子濾波的慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)方法，其特征在于步驟4)所述的初始對(duì)準(zhǔn)的高斯粒子濾波器的構(gòu)建包括如下步驟
Step1初始化，在p(x0)抽取N個(gè)樣本點(diǎn)x0i，相應(yīng)權(quán)值1/N；
Step2抽樣
Step3計(jì)算權(quán)值并歸一化，
Step4計(jì)算估計(jì)值，
Step5對(duì)離散的濾波密度進(jìn)行高斯近似；
Step6從近似的高斯分布中抽取新的粒子xkj，相應(yīng)權(quán)值1/N；
Step7將新的粒子xkj進(jìn)行下一周期的濾波；
其中，p(·)為概率密度函數(shù)，q(·)為重要密度函數(shù)，N為大于1的自然數(shù)，x0為濾波初值，x0i為濾波初始粒子，xki為k時(shí)刻粒子，xk為k時(shí)刻狀態(tài)，xk-1為k-1時(shí)刻狀態(tài)，zk為k時(shí)刻二位置觀測(cè)量，z1:k-1為1時(shí)刻到k-1時(shí)刻的二位置觀測(cè)量，
為k時(shí)刻未歸一化的粒子權(quán)值，wki為k時(shí)刻歸一化后粒子的權(quán)值，
為k時(shí)刻的濾波估值，xkj為近似高斯分布中抽取的新粒子，i，j＝1，2，3......N。
6.根據(jù)權(quán)利要求1所述的一種基于粒子濾波的慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)方法，其特征在于步驟5)所述的初始對(duì)準(zhǔn)完成后，在位置1處采集P次慣性器件輸出數(shù)據(jù)，慣性導(dǎo)航系統(tǒng)在靜基座初始時(shí)，即位置1處；然后，將慣性導(dǎo)航系統(tǒng)繞基座的天向軸旋轉(zhuǎn)180度即位置2處，基座穩(wěn)定后再采集P次慣性器件輸出數(shù)據(jù)；然后將兩次采集的數(shù)據(jù)依次構(gòu)造二位置量測(cè)量，并對(duì)采集數(shù)據(jù)進(jìn)行濾波處理，其中P為大于1的自然數(shù)。
全文摘要
本發(fā)明公布了一種基于粒子濾波的慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)方法，包括下列步驟建立基于二位置的水平姿態(tài)角計(jì)算模型；建立基于二位置的航向角計(jì)算模型；建立基于二位置的非線性初始對(duì)準(zhǔn)模型；構(gòu)建用于初始對(duì)準(zhǔn)的高斯粒子濾波器；由導(dǎo)航計(jì)算機(jī)，采集慣性器件輸出信息，并完成初始對(duì)準(zhǔn)濾波。本發(fā)明利用二位置方法消除慣性器件的常值誤差，因此無(wú)需對(duì)慣性器件誤差進(jìn)行狀態(tài)擴(kuò)展，從而在保證對(duì)準(zhǔn)精度的前提下降低了初始對(duì)準(zhǔn)模型的維數(shù)；由于二位置非線性初始對(duì)準(zhǔn)模型的維數(shù)僅為三維，適合粒子濾波算法在初始對(duì)準(zhǔn)中的工程應(yīng)用，較好的滿足了粒子濾波在慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)中的實(shí)時(shí)性要求；提高了慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的精度和速度。
文檔編號(hào)G01C21/16GK101813493SQ201010142839
公開日2010年8月25日 申請(qǐng)日期2010年4月9日 優(yōu)先權(quán)日2010年4月9日
發(fā)明者熊劍, 劉建業(yè), 賴際舟, 周翟和, 曾慶化, 趙偉, 呂品, 鄭智明, 萬(wàn)輝, 鐘麗娜 申請(qǐng)人:南京航空航天大學(xué)