減小失鎖時速度誤差對組合導(dǎo)航精度影響的方法及系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開了一種減小失鎖時速度誤差對組合導(dǎo)航精度影響的方法及系統(tǒng)，包括全球衛(wèi)星定位系統(tǒng)接收機(jī)、微機(jī)械慣組、可編程邏輯控制器、數(shù)字信號處理器和CAN總線，全球衛(wèi)星定位系統(tǒng)接收機(jī)的信號輸出端連接可編程邏輯控制器的信號輸入端，微機(jī)械慣組的信號輸出端連接數(shù)字信號處理器的信號輸入端，可編程邏輯控制器的信號輸出端連接數(shù)字信號處理器的信號輸入端，數(shù)字信號處理器的信號輸出端連接CAN總線。該方法和系統(tǒng)能明顯提高衛(wèi)星失鎖狀態(tài)下慣導(dǎo)/全球衛(wèi)星定位組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。
【專利說明】減小失鎖時速度誤差對組合導(dǎo)航精度影響的方法及系統(tǒng)
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及組合導(dǎo)航【技術(shù)領(lǐng)域】，具體地指一種通過減小失鎖時速度誤差提高組合導(dǎo)航精度的方法及系統(tǒng)。
【背景技術(shù)】
[0002]慣導(dǎo)系統(tǒng)是一種自主的導(dǎo)航系統(tǒng)，具有工作時不依賴任何外界導(dǎo)航設(shè)備的特點，且隱蔽性好，數(shù)據(jù)更新率高。但是慣導(dǎo)系統(tǒng)的導(dǎo)航誤差會隨時間積累。衛(wèi)星導(dǎo)航GPS(GlobalPositioning System,全球衛(wèi)星定位系統(tǒng))能夠全天候地提供高精度的三維位置、速度和時間基準(zhǔn)信息，具有靜態(tài)定位、動態(tài)導(dǎo)航及精密授時的功能，但是衛(wèi)星導(dǎo)航容易受到干擾，短期穩(wěn)定低，更新率較低。針對上述慣導(dǎo)系統(tǒng)和衛(wèi)星導(dǎo)航GPS的優(yōu)點和缺點，目前設(shè)計出了一種慣導(dǎo)/全球衛(wèi)星定位組合導(dǎo)航系統(tǒng)。該組合系統(tǒng)將衛(wèi)星和慣導(dǎo)系統(tǒng)相結(jié)合，能夠發(fā)現(xiàn)并測量慣性導(dǎo)航的系統(tǒng)誤差，彌補(bǔ)衛(wèi)星導(dǎo)航的信號缺失，提高采樣率，提高模糊度的搜索速度，提高周期性數(shù)據(jù)跳變的檢測能力，使組合后的導(dǎo)航精度高于兩個系統(tǒng)單獨工作的精度，增加了觀測冗余度和系統(tǒng)的抗干擾能力。
[0003]在慣導(dǎo)/全球衛(wèi)星定位組合導(dǎo)航系統(tǒng)進(jìn)行組合導(dǎo)航的過程中，當(dāng)衛(wèi)星出現(xiàn)失鎖狀態(tài)時，由于衛(wèi)星個數(shù)的不斷減少且高動態(tài)全球衛(wèi)星定位系統(tǒng)的數(shù)據(jù)要求實時性高，使得高動態(tài)全球衛(wèi)星定位系統(tǒng)的速度輸出會產(chǎn)生較大誤差。而對于慣導(dǎo)/全球衛(wèi)星定位組合導(dǎo)航系統(tǒng)，慣導(dǎo)/全球衛(wèi)星定位組合導(dǎo)航系統(tǒng)無法分辨這種誤差，所以會將衛(wèi)星失鎖的誤差直接引入慣導(dǎo)/全球衛(wèi)星定位組合導(dǎo)航系統(tǒng)，導(dǎo)致慣導(dǎo)/全球衛(wèi)星定位組合導(dǎo)航系統(tǒng)的精度明顯降低。

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

[0004]本發(fā)明的目的就是要提供一種減小失鎖時速度誤差對組合導(dǎo)航精度影響的方法及系統(tǒng)，該方法和系統(tǒng)能明顯提高衛(wèi)星失鎖狀態(tài)下慣導(dǎo)/全球衛(wèi)星定位組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。
[0005]為實現(xiàn)此目的，本發(fā)明所設(shè)計的減小失鎖時速度誤差對組合導(dǎo)航精度影響的方法，其特征在于，它包括如下步驟:
[0006]步驟1:全球衛(wèi)星定位系統(tǒng)接收機(jī)實時監(jiān)測全球衛(wèi)星定位系統(tǒng)的衛(wèi)星個數(shù)信息，同時通過全球衛(wèi)星定位系統(tǒng)得到全球衛(wèi)星定位系統(tǒng)接收機(jī)自身的實時位置信息和實時速度信息，并且，將全球衛(wèi)星定位系統(tǒng)實時的衛(wèi)星個數(shù)信息、全球衛(wèi)星定位系統(tǒng)接收機(jī)自身的實時位置信息和實時速度信息通過可編程邏輯控制器傳輸給數(shù)字信號處理器；
[0007]步驟2:微機(jī)械慣組將自身的實時位置信息、實時速度信息和實時姿態(tài)信息也傳輸給數(shù)字信號處理器；
[0008]步驟3:在數(shù)字信號處理器中得到微機(jī)械慣組自身的實時位置信息和實時速度信息與全球衛(wèi)星定位系統(tǒng)接收機(jī)自身的實時位置信息和實時速度信息之間的差值，并將該差值輸入到常規(guī)的卡爾曼濾波算法，同時將微機(jī)械慣組自身的實時姿態(tài)信息也輸入到常規(guī)的卡爾曼濾波算法；
[0009]此時數(shù)字信號處理器判斷全球衛(wèi)星定位系統(tǒng)實時的衛(wèi)星個數(shù)信息，當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)的衛(wèi)星個數(shù)> 5顆星時，在不干預(yù)卡爾曼濾波算法量測噪聲中的全球衛(wèi)星定位系統(tǒng)速度噪聲分量的情況下，將全球衛(wèi)星定位系統(tǒng)速度噪聲分量及步驟3中得到的差值和微機(jī)械慣組自身的實時姿態(tài)信息代入現(xiàn)有卡爾曼濾波算法內(nèi)的最優(yōu)估計算法，從而得到微機(jī)械慣組自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差；并將上述微機(jī)械慣組自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差輸入微機(jī)械慣組進(jìn)行導(dǎo)航參數(shù)校正，校正之后得到組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)；
[0010]當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)的衛(wèi)星個數(shù)由5顆星開始變?yōu)?顆星的5秒鐘之內(nèi)包括第5秒時，將常規(guī)卡爾曼濾波算法量測噪聲中的全球衛(wèi)星定位系統(tǒng)速度噪聲分量調(diào)整為兩倍的全球衛(wèi)星定位系統(tǒng)速度噪聲分量，然后將兩倍的全球衛(wèi)星定位系統(tǒng)速度噪聲分量及步驟3中得到的差值和微機(jī)械慣組自身的實時姿態(tài)信息代入常規(guī)卡爾曼濾波算法內(nèi)的最優(yōu)估計算法，從而得到微機(jī)械慣組自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差；將上述微機(jī)械慣組自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差輸入微機(jī)械慣組進(jìn)行導(dǎo)航參數(shù)校正，校正之后得到組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)；
[0011]當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)的衛(wèi)星個數(shù)保持在4顆星5秒鐘以上或者衛(wèi)星個數(shù)恢復(fù)到5顆星及以上時，將兩倍全球衛(wèi)星定位系統(tǒng)速度噪聲分量，恢復(fù)到原始全球衛(wèi)星定位系統(tǒng)速度噪聲分量，然后將原始的全球衛(wèi)星定位系統(tǒng)速度噪聲分量及步驟3中得到的差值和微機(jī)械慣組自身的實時姿態(tài)信息代入常規(guī)卡爾曼濾波算法內(nèi)的最優(yōu)估計算法，從而得到微機(jī)械慣組自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差；將上述微機(jī)械慣組自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差輸入微機(jī)械慣組進(jìn)行導(dǎo)航參數(shù)校正，校正之后得到組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)；
[0012]當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)的衛(wèi)星個數(shù)< 4顆星時，此時全球衛(wèi)星定位系統(tǒng)無數(shù)據(jù)輸出，常規(guī)卡爾曼濾波算法此時按全球衛(wèi)星定位系統(tǒng)失鎖處理。
[0013]本發(fā)明所設(shè)計能減小失鎖速度誤差對組合導(dǎo)航精度影響的系統(tǒng)，它包括全球衛(wèi)星定位系統(tǒng)接收機(jī)和微機(jī)械慣組，其特征在于:它還包括可編程邏輯控制器、數(shù)字信號處理器和CAN (Controller Area Network,控制器局域網(wǎng)絡(luò))總線,其中，所述全球衛(wèi)星定位系統(tǒng)接收機(jī)的信號輸出端連接可編程邏輯控制器的信號輸入端，微機(jī)械慣組的信號輸出端連接數(shù)字信號處理器的信號輸入端，所述可編程邏輯控制器的信號輸出端連接數(shù)字信號處理器的信號輸入端，所述數(shù)字信號處理器的信號輸出端連接CAN總線。
[0014]進(jìn)一步地，所述數(shù)字信號處理器中內(nèi)嵌有卡爾曼濾波算法。
[0015]本發(fā)明的有益效果:
[0016]I)本發(fā)明通過判斷全球衛(wèi)星定位系統(tǒng)實時的衛(wèi)星個數(shù)，并根據(jù)衛(wèi)星個數(shù)來相應(yīng)調(diào)整卡爾曼濾波算法量測噪聲中的全球衛(wèi)星定位系統(tǒng)速度噪聲分量，從而最終實現(xiàn)減小全球衛(wèi)星定位系統(tǒng)失鎖速度誤差對組合導(dǎo)航精度的影響。
[0017]2)本發(fā)明的實時性好、精度高，完全能滿足動態(tài)性要求較高的場合。
[0018]3)本發(fā)明有效的解決了目前高動態(tài)全球衛(wèi)星定位系統(tǒng)在失鎖過程中引入全球衛(wèi)星定位系統(tǒng)速度誤差到卡爾曼濾波算法的問題，本發(fā)明根據(jù)衛(wèi)星失鎖的具體情況，在卡爾曼濾波算法中設(shè)置了相應(yīng)的補(bǔ)償，這樣明顯提高了失鎖過程中組合導(dǎo)航系統(tǒng)的精度。[0019]4)本發(fā)明采用的卡爾曼濾波算法為現(xiàn)有成熟算法，利用該算法補(bǔ)償微機(jī)械慣組誤差的方法成熟可靠。
【專利附圖】

【附圖說明】
[0020]圖1為本發(fā)明的使用狀態(tài)結(jié)構(gòu)示意圖；
[0021]其中，I一全球衛(wèi)星定位系統(tǒng)接收機(jī)、2—全球衛(wèi)星定位系統(tǒng)、3—數(shù)字信號處理器、4一微機(jī)械慣組、5—可編程邏輯控制器、6- CAN總線。
【具體實施方式】
[0022]以下結(jié)合附圖和具體實施例對本發(fā)明作進(jìn)一步的詳細(xì)說明:
[0023]如圖1所示能減小失鎖速度誤差對組合導(dǎo)航精度影響的系統(tǒng)，它包括全球衛(wèi)星定位系統(tǒng)接收機(jī)1、微機(jī)械慣組4、可編程邏輯控制器5、數(shù)字信號處理器3和CAN總線6，其中，所述全球衛(wèi)星定位系統(tǒng)接收機(jī)I的信號輸出端連接可編程邏輯控制器5的信號輸入端，微機(jī)械慣組4的信號輸出端連接數(shù)字信號處理器3的信號輸入端，所述可編程邏輯控制器5的信號輸出端連接數(shù)字信號處理器3的信號輸入端，所述數(shù)字信號處理器3的信號輸出端連接CAN總線6。所述數(shù)字信號處理器3中內(nèi)嵌有卡爾曼濾波算法。
[0024]上述能減小失鎖速度誤差對組合導(dǎo)航精度影響的系統(tǒng)在工作時，它包括如下步驟:
[0025]步驟1:全球衛(wèi)星定位系統(tǒng)接收機(jī)I實時監(jiān)測全球衛(wèi)星定位系統(tǒng)2的衛(wèi)星個數(shù)信息，同時通過全球衛(wèi)星定位系統(tǒng)2得到全球衛(wèi)星定位系統(tǒng)接收機(jī)I自身的實時位置信息和實時速度信息，并且，將全球衛(wèi)星定位系統(tǒng)2實時的衛(wèi)星個數(shù)信息、全球衛(wèi)星定位系統(tǒng)接收機(jī)I自身的實時位置信息和實時速度信息通過可編程邏輯控制器5傳輸給數(shù)字信號處理器3 ；
[0026]步驟2:微機(jī)械慣組4將自身的實時位置信息、實時速度信息和實時姿態(tài)信息也傳輸給數(shù)字信號處理器3 ；
[0027]步驟3:在數(shù)字信號處理器3中得到微機(jī)械慣組4自身的實時位置信息和實時速度信息與全球衛(wèi)星定位系統(tǒng)接收機(jī)I自身的實時位置信息和實時速度信息之間的差值，并將該差值輸入到常規(guī)的卡爾曼濾波算法，同時將微機(jī)械慣組4自身的實時姿態(tài)信息也輸入到常規(guī)的卡爾曼濾波算法；此時下述卡爾曼濾波算法公式(4)中全球衛(wèi)星定位系統(tǒng)速度噪聲分量v(t)按照以下步驟進(jìn)行相應(yīng)設(shè)定:
[0028]此時數(shù)字信號處理器3判斷全球衛(wèi)星定位系統(tǒng)2實時的衛(wèi)星個數(shù)信息，當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)2的衛(wèi)星個數(shù)> 5顆星時，在不干預(yù)卡爾曼濾波算法量測噪聲中的全球衛(wèi)星定位系統(tǒng)速度噪聲分量V (t)的情況下，將全球衛(wèi)星定位系統(tǒng)速度噪聲分量V (t)及步驟3中得到的差值和微機(jī)械慣組4自身的實時姿態(tài)信息代入現(xiàn)有卡爾曼濾波算法內(nèi)的最優(yōu)估計算法，從而得到微機(jī)械慣組4自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差；并將上述微機(jī)械慣組4自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差輸入微機(jī)械慣組4進(jìn)行導(dǎo)航參數(shù)校正，校正之后得到組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)；
[0029]當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)2的衛(wèi)星個數(shù)由5顆星開始變?yōu)?顆星的5秒鐘之內(nèi)包括第5秒時(4顆星是全球衛(wèi)星定位系統(tǒng)能否定位的臨界狀態(tài)，5秒鐘是根據(jù)試驗實際測得整體精度最好的時間長度)，將常規(guī)卡爾曼濾波算法量測噪聲中的全球衛(wèi)星定位系統(tǒng)速度噪聲分量V (t)調(diào)整為兩倍的全球衛(wèi)星定位系統(tǒng)速度噪聲分量V (t)，然后將兩倍的全球衛(wèi)星定位系統(tǒng)速度噪聲分量V(t)及步驟3中得到的差值和微機(jī)械慣組4自身的實時姿態(tài)信息代入常規(guī)卡爾曼濾波算法內(nèi)的最優(yōu)估計算法，從而得到微機(jī)械慣組4自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差；將上述微機(jī)械慣組4自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差輸入微機(jī)械慣組4進(jìn)行導(dǎo)航參數(shù)校正，校正之后得到組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)；
[0030]當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)2的衛(wèi)星個數(shù)保持在4顆星5秒鐘以上或者衛(wèi)星個數(shù)恢復(fù)到5顆星及以上時(因為這種情況下，衛(wèi)星定位系統(tǒng)已經(jīng)重新獲得精確速度位置信息)，將兩倍全球衛(wèi)星定位系統(tǒng)速度噪聲分量V (t)，恢復(fù)到原始全球衛(wèi)星定位系統(tǒng)速度噪聲分量V (t)，然后將原始的全球衛(wèi)星定位系統(tǒng)速度噪聲分量V (t)及步驟3中得到的差值和微機(jī)械慣組4自身的實時姿態(tài)信息代入常規(guī)卡爾曼濾波算法內(nèi)的最優(yōu)估計算法，從而得到微機(jī)械慣組4自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差；將上述微機(jī)械慣組4自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差輸入微機(jī)械慣組4進(jìn)行導(dǎo)航參數(shù)校正，校正之后得到組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)；
[0031]當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)2的衛(wèi)星個數(shù)< 4顆星時(如3顆星，4顆星是衛(wèi)星定位系統(tǒng)能否定位的臨界狀態(tài))，此時全球衛(wèi)星定位系統(tǒng)2無數(shù)據(jù)輸出，常規(guī)卡爾曼濾波算法此時按全球衛(wèi)星定位系統(tǒng)失鎖處理。
[0032]通過以上步驟，可以根據(jù)衛(wèi)星數(shù)情況判斷全球衛(wèi)星定位系統(tǒng)在失鎖過程中的速度誤差估計，從而設(shè)置正確的全球衛(wèi)星定位系統(tǒng)速度噪聲分量參數(shù)，估計出速度、位置誤差，確?？柭鼮V波在整個失鎖過程都處在線性估計狀態(tài)，得到最優(yōu)的導(dǎo)航結(jié)果。若在失鎖過程中，使用原始的全球衛(wèi)星定位系統(tǒng)速度噪聲分量參數(shù)，而速度的誤差估計是變化的，卡爾曼濾波將按線性方程參數(shù)估計整個失鎖過程的非線性狀態(tài)，導(dǎo)航結(jié)果就會出現(xiàn)偏差。
·[0033]上述技術(shù)方案中，校正之后得到組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)由數(shù)字信號處理器3輸出給CAN總線6。
[0034]具體實施時選取東北天坐標(biāo)系為導(dǎo)航坐標(biāo)系，將上述能減小失鎖時速度誤差對組合導(dǎo)航精度影響的裝置安裝于試驗車上，以車體的右前上坐標(biāo)系為載體坐標(biāo)系建立姿態(tài)矩陣，微機(jī)械慣組4導(dǎo)航姿態(tài)解算采用雙子樣算法，同時進(jìn)行速度更新和位置更新。卡爾曼濾波算法的狀態(tài)向量選為:
[0035]Χ = \_φ'!! ?>ν"Τ ?μ i,\ ▽,.]( I )
[0036]其中，X為卡爾曼濾波算法狀態(tài)向量，ΦηΤ為姿態(tài)誤差，δνηΤ為速度誤差，δ ρτ為位置誤差W力陀螺常值漂移，Vf為加表偏置，T為矩陣轉(zhuǎn)置符號。
[0037]相關(guān)誤差模型如下:
r=-kx^,,+^<-c:kb+<)
[0038]紀(jì)=(/；；-(2< H-ω:?)χ V + V" X[?δω? + δω:)+ C；(Vbb + w:) C2；
φ = δνη[0039]其中，φη、&Β、孕分別為姿態(tài)誤差、速度誤差、位置誤差微分，Φη為當(dāng)前姿態(tài)，ω ω 為不同坐標(biāo)系下地球轉(zhuǎn)速，We、如:為不同坐標(biāo)系下地球轉(zhuǎn)速偏差，Cnb為姿態(tài)轉(zhuǎn)移矩陣，<、丨分別表示微機(jī)械慣組4陀螺噪聲和加表噪聲，/ ,為加速度輸
出，為加速度計偏差，δ vn為速度誤差。
[0040]卡爾曼濾波算法量測矩陣為:
[0041]H(t) = [06X3 I6x6 06X6]T (3)
[0042]其中，H(t)為量測矩陣，06X3、06X6為全零矩陣，I6x6為對角單位矩陣。
[0043]則卡爾曼濾波算法狀態(tài)空間方程為:
【權(quán)利要求】
1.一種減小失鎖時速度誤差對組合導(dǎo)航精度影響的方法，其特征在于，它包括如下步驟: 步驟1:全球衛(wèi)星定位系統(tǒng)接收機(jī)(I)實時監(jiān)測全球衛(wèi)星定位系統(tǒng)(2)的衛(wèi)星個數(shù)信息，同時通過全球衛(wèi)星定位系統(tǒng)(2)得到全球衛(wèi)星定位系統(tǒng)接收機(jī)(I)自身的實時位置信息和實時速度信息，并且，將全球衛(wèi)星定位系統(tǒng)(2)實時的衛(wèi)星個數(shù)信息、全球衛(wèi)星定位系統(tǒng)接收機(jī)(I)自身的實時位置信息和實時速度信息通過可編程邏輯控制器(5)傳輸給數(shù)字信號處理器(3)； 步驟2:微機(jī)械慣組(4)將自身的實時位置信息、實時速度信息和實時姿態(tài)信息也傳輸給數(shù)字信號處理器(3); 步驟3:在數(shù)字信號處理器(3)中得到微機(jī)械慣組(4)自身的實時位置信息和實時速度信息與全球衛(wèi)星定位系統(tǒng)接收機(jī)(I)自身的實時位置信息和實時速度信息之間的差值，并將該差值輸入到常規(guī)的卡爾曼濾波算法，同時將微機(jī)械慣組(4)自身的實時姿態(tài)信息也輸入到常規(guī)的卡爾曼濾波算法； 此時數(shù)字信號處理器(3)判斷全球衛(wèi)星定位系統(tǒng)(2)實時的衛(wèi)星個數(shù)信息，當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)(2)的衛(wèi)星個數(shù)> 5顆星時，在不干預(yù)卡爾曼濾波算法量測噪聲中的全球衛(wèi)星定位系統(tǒng)速度噪聲分量的情況下，將全球衛(wèi)星定位系統(tǒng)速度噪聲分量及步驟3中得到的差值和微機(jī)械慣組(4)自身的實時姿態(tài)信息代入現(xiàn)有卡爾曼濾波算法內(nèi)的最優(yōu)估計算法，從而得到微機(jī)械慣組(4)自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差；并將上述微機(jī)械慣組(4)自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差輸入 微機(jī)械慣組(4)進(jìn)行導(dǎo)航參數(shù)校正，校正之后得到組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)； 當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)(2)的衛(wèi)星個數(shù)由5顆星開始變?yōu)?顆星的5秒鐘之內(nèi)包括第5秒時，將常規(guī)卡爾曼濾波算法量測噪聲中的全球衛(wèi)星定位系統(tǒng)速度噪聲分量調(diào)整為兩倍的全球衛(wèi)星定位系統(tǒng)速度噪聲分量，然后將兩倍的全球衛(wèi)星定位系統(tǒng)速度噪聲分量及步驟3中得到的差值和微機(jī)械慣組(4)自身的實時姿態(tài)信息代入常規(guī)卡爾曼濾波算法內(nèi)的最優(yōu)估計算法，從而得到微機(jī)械慣組(4)自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差；將上述微機(jī)械慣組(4)自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差輸入微機(jī)械慣組(4)進(jìn)行導(dǎo)航參數(shù)校正，校正之后得到組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)； 當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)(2)的衛(wèi)星個數(shù)保持在4顆星5秒鐘以上或者衛(wèi)星個數(shù)恢復(fù)到5顆星及以上時，將兩倍全球衛(wèi)星定位系統(tǒng)速度噪聲分量，恢復(fù)到原始全球衛(wèi)星定位系統(tǒng)速度噪聲分量，然后將原始的全球衛(wèi)星定位系統(tǒng)速度噪聲分量及步驟3中得到的差值和微機(jī)械慣組(4)自身的實時姿態(tài)信息代入常規(guī)卡爾曼濾波算法內(nèi)的最優(yōu)估計算法，從而得到微機(jī)械慣組(4)自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差；將上述微機(jī)械慣組(4)自身的實時位置信息誤差、實時速度信息誤差和實時姿態(tài)信息誤差輸入微機(jī)械慣組(4)進(jìn)行導(dǎo)航參數(shù)校正，校正之后得到組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)； 當(dāng)?shù)玫饺蛐l(wèi)星定位系統(tǒng)(2)的衛(wèi)星個數(shù)<4顆星時，此時全球衛(wèi)星定位系統(tǒng)(2)無數(shù)據(jù)輸出，常規(guī)卡爾曼濾波算法此時按全球衛(wèi)星定位系統(tǒng)失鎖處理。
2.一種為實現(xiàn)權(quán)利要求1所述方法而設(shè)計的能減小失鎖速度誤差對組合導(dǎo)航精度影響的系統(tǒng)，它包括全球衛(wèi)星定位系統(tǒng)接收機(jī)(I)和微機(jī)械慣組(4)，其特征在于:它還包括可編程邏輯控制器(5)、數(shù)字信號處理器(3)和CAN總線(6)，其中，所述全球衛(wèi)星定位系統(tǒng)接收機(jī)(I)的信號輸出端連接可編程邏輯控制器(5)的信號輸入端，微機(jī)械慣組(4)的信號輸出端連接數(shù)字信號處理器(3)的信號輸入端，所述可編程邏輯控制器(5)的信號輸出端連接數(shù)字信號處理器(3)的信號輸入端，所述數(shù)字信號處理器(3)的信號輸出端連接CAN總線(6)。
3.根據(jù)權(quán)利要求2所述的能減小失鎖時速度誤差對組合導(dǎo)航精度影響的系統(tǒng)，其特征在于:所述數(shù)字信號處理器(3)中內(nèi)嵌有卡爾曼濾波算法。
【文檔編號】G01S19/47GK103592669SQ201310596586
【公開日】2014年2月19日 申請日期:2013年11月22日 優(yōu)先權(quán)日:2013年11月22日
【發(fā)明者】何健偉, 呂江濤, 陸俊清 申請人:湖北航天技術(shù)研究院總體設(shè)計所