一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法
【專利摘要】本發(fā)明涉及一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法，包括：?jiǎn)伪鴮?dǎo)航系統(tǒng)配有一個(gè)小型GPS接收機(jī)和兩個(gè)MIMU傳感器。GPS與雙MIMU系統(tǒng)在人體安置的位置選擇以便最大限度的克服GPS動(dòng)態(tài)性能不良以及MEMS隨時(shí)間導(dǎo)航精度急劇降低的缺點(diǎn)。利用各導(dǎo)航器件之間的位置關(guān)系，構(gòu)造兩個(gè)非線性等式約束方程。將約束方程與卡爾曼濾波相結(jié)合，擺脫微弱信號(hào)下GPS定位精度不夠的問題。在惡劣的封閉室內(nèi)導(dǎo)航環(huán)境下，采用本發(fā)明可以充分增加微弱GPS信號(hào)的可利用性，在一定程度上補(bǔ)償MEMS的累積誤差，并且在衛(wèi)星信號(hào)失鎖的緊急時(shí)刻也能利用MEMS獨(dú)立輔助單兵執(zhí)行任務(wù)，因此大大的提高了組合導(dǎo)航系統(tǒng)的可靠性。
【專利說明】一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法
【技術(shù)領(lǐng)域】:
[0001]本發(fā)明涉及的是一種導(dǎo)航方法，特別是涉及一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法。
【背景技術(shù)】:
[0002]對(duì)于單兵導(dǎo)航來說，一個(gè)既可靠又準(zhǔn)確且能夠?qū)崿F(xiàn)導(dǎo)航模式由室外到室內(nèi)無縫切換的定位系統(tǒng)是必不可少的。由于綜合導(dǎo)航環(huán)境的限制，需要的傳感器件必須具有輕便、簡(jiǎn)小、價(jià)格低的優(yōu)點(diǎn)，但須至少提供米級(jí)以上的精度。因此小型GPS接收機(jī)與慣導(dǎo)器件必不可少，除此之外磁力計(jì)、氣壓計(jì)、UWB無線設(shè)備、圖像傳感器、多普勒雷達(dá)、以及超聲波傳感器均可作為多傳感器融合系統(tǒng)的選擇。
[0003]多種傳感器信息可供選擇，本發(fā)明選取其中最基本的兩項(xiàng)，即慣導(dǎo)與衛(wèi)導(dǎo)結(jié)合。將多傳感器信息進(jìn)行高精度融合成為一個(gè)亟待解決的問題。因此如何改進(jìn)濾波方法，使得這兩種導(dǎo)航系統(tǒng)的精度更高成為研究重點(diǎn)。本發(fā)明基于此目的利用單兵的人體特征，如單腳的長度、身高的最大值等構(gòu)造約束方程，抑制獲取優(yōu)于直接卡爾曼濾波的結(jié)果。

【發(fā)明內(nèi)容】
:
[0004]本發(fā)明的目的在于提供一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法，以解決現(xiàn)有技術(shù)中MEMS傳感器在導(dǎo)航過程中誤差積累較大，造成隨時(shí)間導(dǎo)航精度急劇降低以及在室內(nèi)GPS信號(hào)過于微弱造成輸出結(jié)果可信度不高以及信號(hào)失鎖后由于動(dòng)態(tài)性能差造成的搜星困難等問題。
[0005]為了解決【背景技術(shù)】所存在的問題，本發(fā)明采用以下技術(shù)方案:
[0006]一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法，它包括如下步驟:
[0007](I)將GPS安置在單兵頭部，兩個(gè)MIMU導(dǎo)航器件分別放置在單兵的單腳腳尖與腳后跟；
[0008](2)測(cè)量三個(gè)導(dǎo)航子系統(tǒng)所構(gòu)成的空間三角形邊長；
[0009](3)根據(jù)三邊關(guān)系，構(gòu)造兩個(gè)非線性等式約束方程，其中第一個(gè)等式約束即為雙MU之間的相對(duì)距離固定；
[0010](4)運(yùn)用基于等式約束的卡爾曼濾波方法，對(duì)各導(dǎo)航子系統(tǒng)分別校正與解算。
[0011]在所述步驟(4)中，當(dāng)單兵行走時(shí)，腳部接觸地面的瞬間，傳感器對(duì)地絕對(duì)速度為零，則可以將對(duì)地絕對(duì)零速設(shè)為觀測(cè)量，進(jìn)行濾波估計(jì)，實(shí)現(xiàn)對(duì)陀螺加速度計(jì)零位誤差的反饋矯正，即觸發(fā)兩個(gè)MIMU的零速校正。
[0012]在所述步驟(4)中，初始對(duì)準(zhǔn)可通過采集陀螺x、y、ζ軸前100個(gè)角速率求得，SP
[0013]fx=mean (gyro (1,1:100))；
[0014]y，ζ 軸類似。
[0015]在所述步驟(3)中，，雙MMU系統(tǒng)會(huì)相互抑制其誤差發(fā)散，腳負(fù)載雙MMU系統(tǒng)，其相對(duì)位置固定，即相對(duì)距離的二范數(shù)為定值。在單兵運(yùn)動(dòng)過程中，經(jīng)過零速校正的雙MMU分別繪制出兩條軌跡，但是其相對(duì)距離卻可能并不滿足條件，因此可以將等式約束與卡爾曼濾波相結(jié)合，將雙MMU系統(tǒng)的位置狀態(tài)變量利用投影法進(jìn)行二次估計(jì)，即為雙MIMU系統(tǒng)會(huì)相互抑制其誤差發(fā)散。
[0016]在所述步驟(4)中，，GPS信號(hào)微弱時(shí)定位誤差較大，類似于雙MIMU系統(tǒng)相互抑制誤差發(fā)散，對(duì)于相對(duì)位置可以近似為固定的GPS天線和腳部負(fù)載MMU也可以構(gòu)造基于等式約束卡爾曼濾波，將含有較大誤差的衛(wèi)星信號(hào)與一個(gè)MIMU的位置狀態(tài)變量用投影法進(jìn)行估計(jì)，可對(duì)GPS位置、速度信息進(jìn)行進(jìn)一步估計(jì)。
[0017]在所述步驟(4)中，，當(dāng)GPS信號(hào)完全失鎖時(shí)可利用慣導(dǎo)系統(tǒng)獨(dú)立導(dǎo)航，即兩個(gè)MIMU進(jìn)行單等式約束卡爾曼濾波，同時(shí)慣導(dǎo)信息也可輸出用于輔助GPS信號(hào)的再次捕獲。
[0018]本發(fā)明對(duì)比現(xiàn)有技術(shù)有如下的有益效果:利用本發(fā)明中的基于等式約束卡爾曼濾波進(jìn)行估計(jì)的過程中，既可對(duì)零速校正后的MEMS器件的位置進(jìn)行約束，減小其誤差漂移，同時(shí)又可以對(duì)輸出結(jié)果穩(wěn)定但是誤差較大的GPS進(jìn)行校正，并且能夠保證實(shí)現(xiàn)由信號(hào)微弱至信號(hào)失鎖的無縫切換。
【專利附圖】

【附圖說明】:
[0019]圖1為本發(fā)明中的單兵導(dǎo)航示意圖。
[0020]圖2示為本發(fā)明中采用的XSENS公司生產(chǎn)的MT1-G型IMU0
[0021]圖3為MEMS在單兵腳上的實(shí)例圖，單腳負(fù)載雙MMU示意圖
[0022]圖4為雙MIMU的位置約束關(guān)系。
[0023]圖5為單兵導(dǎo)航的坐標(biāo)系與軌跡。
[0024]圖6為慣導(dǎo)輔助衛(wèi)星導(dǎo)航的原理圖。
[0025]圖7為單MIMU進(jìn)行單兵導(dǎo)航解算定位圖。
[0026]圖8為采用基于等式約束卡爾曼濾波的單兵導(dǎo)航解算定位圖。
[0027]圖9為本發(fā)明方法流程圖。
【具體實(shí)施方式】:
[0028]下面結(jié)合附圖和【具體實(shí)施方式】對(duì)本發(fā)明作進(jìn)一步描述:
[0029]針對(duì)單兵導(dǎo)航精度高、可靠性好等要求，本發(fā)明提出一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法。具體結(jié)構(gòu)裝置為將GPS安置在單兵頭部，兩個(gè)MIMU導(dǎo)航器件分別放置在單兵的單腳腳尖與腳后跟，如圖1為本發(fā)明提出的單兵導(dǎo)航示意圖。單兵導(dǎo)航進(jìn)行時(shí)，與傳統(tǒng)的導(dǎo)航要得到精確的經(jīng)度與緯度不同。封閉的室內(nèi)導(dǎo)航空間有限，若單兵移動(dòng)100米左右，那么對(duì)應(yīng)的經(jīng)度、緯度的變化幾乎為零，因此圖5表示的導(dǎo)航坐標(biāo)系可以選取為室內(nèi)平面的延墻面的基線分別為X、1、z軸，根據(jù)單兵導(dǎo)航需求設(shè)置初始位置為原點(diǎn)0(0，0，0)。圖2為本發(fā)明所采用XSENS公司生產(chǎn)的的MIMU傳感器示意圖，設(shè)繞Z軸逆時(shí)針旋轉(zhuǎn)為航向正方向。圖3為單腳雙MMU示意圖，雙MMU均固連在載體上，在運(yùn)動(dòng)過程中MMUl的三軸陀螺輸出三個(gè)方向的角速率，三軸加速度計(jì)輸出沿載體系I的線加速度，MMU2同理。圖4為單腳雙MIMU構(gòu)成的等式約束示意圖，通過圖中可以直接看出兩個(gè)IMU之間的位置約束關(guān)系，在卡爾曼濾波過程中構(gòu)造等式約束方程。圖6為慣導(dǎo)輔助衛(wèi)星導(dǎo)航的原理圖，在衛(wèi)星信號(hào)失鎖的時(shí)候，單兵導(dǎo)航系統(tǒng)只有依靠自主式的慣性單元完成導(dǎo)航任務(wù)，MMUl與MMU2在基于等式約束的卡爾曼濾波估計(jì)后分別可以得到兩個(gè)位置，由于其與GPS天線的距離均為定值，則以兩個(gè)微慣導(dǎo)IMU組件為圓心作圓的交點(diǎn)即為GPS預(yù)測(cè)位置。根據(jù)GPS天線預(yù)測(cè)位置可以輔助衛(wèi)導(dǎo)快速搜星，大大提高室內(nèi)單兵導(dǎo)航的可靠性與可利用性。
[0030]首先，分析等式約束與卡爾曼結(jié)合的濾波過程，具體分析如下:
[0031]步驟一:雙MIMU與GPS構(gòu)造聯(lián)合狀態(tài)向量
[0032]如上圖所示,兩個(gè)IMU與GPS衛(wèi)星導(dǎo)航系統(tǒng)相互獨(dú)立,令
【權(quán)利要求】
1.一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法，其特征在于，包括以下步驟: (1)將GPS安置在單兵頭部，兩個(gè)MIMU導(dǎo)航器件分別放置在單兵的單腳腳尖與腳后跟； (2)測(cè)量三個(gè)導(dǎo)航子系統(tǒng)所構(gòu)成的空間三角形邊長； (3)根據(jù)三邊關(guān)系，構(gòu)造兩個(gè)非線性等式約束方程，其中第一個(gè)等式約束即為雙IMU之間的相對(duì)距離固定； (4)運(yùn)用基于等式約束的卡爾曼濾波方法，對(duì)各導(dǎo)航子系統(tǒng)分別校正與解算。
2.根據(jù)權(quán)利要求1所述的一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法，其特征在于，在所述步驟(4)中，當(dāng)單兵行走時(shí)，腳部接觸地面的瞬間，傳感器對(duì)地絕對(duì)速度為零，則可以將對(duì)地絕對(duì)零速設(shè)為觀測(cè)量，進(jìn)行濾波估計(jì)，實(shí)現(xiàn)對(duì)陀螺加速度計(jì)零位誤差的反饋矯正，即觸發(fā)兩個(gè)MIMU的零速校正。
3.根據(jù)權(quán)利要求1所述的一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法，其特征在于，在所述步驟(4)中，初始對(duì)準(zhǔn)可通過采集陀螺x、y、z軸前100個(gè)角速率求得，SP
fx=mean(gyro(1,1: 100))； y，z軸類似。
4.根據(jù)權(quán)利要求1所述的一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法，在所述步驟(3)中，其特征在于，雙MIMU系統(tǒng)會(huì)相互抑制其誤差發(fā)散，單腳負(fù)載雙MIMU系統(tǒng)，其相對(duì)位置固定，即相對(duì)距離的二范數(shù)為定值；在單兵運(yùn)動(dòng)過程中，經(jīng)過零速校正的雙MMU分別繪制出兩條軌跡，但是其相對(duì)距離卻可能并不滿足條件，因此可以將等式約束與卡爾曼濾波相結(jié)合，將雙MMU系統(tǒng)的位置狀態(tài)變量利用投影法進(jìn)行二次估計(jì)，即為雙MMU系統(tǒng)會(huì)相互抑制其誤差發(fā)散。
5.根據(jù)權(quán)利要求1所述的一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法，在所述步驟(4)中,其特征在于,GPS信號(hào)微弱時(shí)定位誤差較大,類似于雙MIMU系統(tǒng)相互抑制誤差發(fā)散，對(duì)于相對(duì)位置可以近似為固定的GPS天線和腳部負(fù)載MMU也可以構(gòu)造基于等式約束卡爾曼濾波，將含有較大誤差的衛(wèi)星信號(hào)與一個(gè)MIMU的位置狀態(tài)變量用投影法進(jìn)行估計(jì)，可對(duì)GPS位置、速度信息進(jìn)行進(jìn)一步估計(jì)。
6.根據(jù)權(quán)利要求 1所述的一種基于等式約束卡爾曼濾波的單兵導(dǎo)航方法，在所述步驟(4)中，其特征在于，當(dāng)GPS信號(hào)完全失鎖時(shí)可利用慣導(dǎo)系統(tǒng)獨(dú)立導(dǎo)航，即兩個(gè)MIMU進(jìn)行單等式約束卡爾曼濾波，同時(shí)慣導(dǎo)信息也可輸出用于輔助GPS信號(hào)的再次捕獲。
【文檔編號(hào)】G01S19/49GK103744099SQ201310581902
【公開日】2014年4月23日 申請(qǐng)日期:2013年11月20日 優(yōu)先權(quán)日:2013年11月20日
【發(fā)明者】高偉, 李佳璇, 周廣濤, 于春陽, 蘭海鈺, 林萌萌, 劉曉旭, 白紅美, 梁宏, 孫夢(mèng) 申請(qǐng)人:哈爾濱工程大學(xué)