本發(fā)明屬于一種純慣導(dǎo)導(dǎo)航方式，具體地說是一種基于M估計(jì)不完全約束的車輛導(dǎo)航方法。

背景技術(shù)：

對于作戰(zhàn)車輛來說，出于隱蔽性考慮，在進(jìn)行導(dǎo)航時(shí)，不會依賴衛(wèi)星導(dǎo)航，只采用慣性導(dǎo)航、多普勒導(dǎo)航等方式，但采用慣性導(dǎo)航存在兩個(gè)缺點(diǎn)，首先，慣導(dǎo)誤差隨時(shí)間發(fā)散，這就導(dǎo)致采用純慣導(dǎo)方式進(jìn)行導(dǎo)航定位精度差，穩(wěn)定性低，其次，慣性器件成本偏高，若采用戰(zhàn)術(shù)級精度慣導(dǎo)系統(tǒng)進(jìn)行導(dǎo)航，一套慣導(dǎo)系統(tǒng)可達(dá)數(shù)十萬元，而這對于作戰(zhàn)車輛來說顯然無法負(fù)擔(dān)。

對于上述問題，有一種解決辦法是采用不完全約束算法進(jìn)行導(dǎo)航信息校正，該方法可以抑制慣導(dǎo)系統(tǒng)誤差發(fā)散，同時(shí)，在采用低成本微慣性測量單元(MIMU)情況下，定位精度也可以得到保證。但該方法存在一個(gè)很大的問題就是必須保證車輛在運(yùn)行時(shí)，不發(fā)生側(cè)滑、跳躍等活動(dòng)，即保證MIMU輸出的載體系上的信息中，天向速度信息和側(cè)向速度信息理論上為零，但對于作戰(zhàn)車輛而言，因其工作環(huán)境所限，不完全約束條件經(jīng)常不能得到滿足，這將導(dǎo)致導(dǎo)航結(jié)果精度大幅度降低。

為解決上述問題，本發(fā)明將M估計(jì)算法與不完全約束算法結(jié)合，構(gòu)建一種基于M估計(jì)不完全約束算法的車輛導(dǎo)航方法，克服不完全約束條件不滿足的問題，提高純慣導(dǎo)導(dǎo)航條件下車輛定位精度。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種基于M估計(jì)不完全約束的車輛導(dǎo)航方法，以解決作戰(zhàn)車輛只能依賴純慣導(dǎo)定位下誤差發(fā)散、高成本的問題，同時(shí)對不完全約束算法進(jìn)行改進(jìn)，改善其在各個(gè)場合下的通用性差，穩(wěn)定性低的問題。

為了解決背景技術(shù)所存在的問題，本發(fā)明采取以下技術(shù)方案：

一種基于M估計(jì)不完全約束的車輛導(dǎo)航方法，它包括以下步驟：

MIMU傳感器安放：將MIMU固定在車輛上，使其不發(fā)生滑動(dòng)，從而輸出相對于車輛載體系的導(dǎo)航信息；

在車輛正常行駛時(shí)，采用不完全約束算法對MIMU輸出導(dǎo)航信息進(jìn)行解算，得到車輛導(dǎo)航信息；

當(dāng)車輛在行駛過程中發(fā)生側(cè)滑、跳躍等動(dòng)作時(shí)，不完全約束條件不再滿足，此時(shí)利用M估計(jì)算法對MIMU信息進(jìn)行修正，利用修正后信息進(jìn)行不完全約束算法解算，得到車輛導(dǎo)航信息。

在步驟(2)中，當(dāng)車輛正常行駛時(shí)，通過不完全約束算法可直接利用MIMU輸出的純慣導(dǎo)信息進(jìn)行車輛位置解算，且定位誤差不隨時(shí)間發(fā)散，極大的提高了純慣導(dǎo)系統(tǒng)的導(dǎo)航能力。

在步驟(3)中，因作戰(zhàn)車輛的工作環(huán)境較為惡劣，車輛側(cè)滑、起跳等現(xiàn)象經(jīng)常出現(xiàn)，導(dǎo)致不完全約束條件不在滿足，算法解算效果變差，針對這一問題，可以將MIMU輸出的不滿足約束條件的導(dǎo)航信息視為粗差，利用M估計(jì)算法對粗差進(jìn)行降權(quán)甚至剔除，然后再對處理后信息進(jìn)行不完全約束算法解算。

在步驟(3)中，對于粗差的識別，可以將MIMU輸出的導(dǎo)航信息進(jìn)行預(yù)判，利用概率統(tǒng)計(jì)的方法識別，即根據(jù)正態(tài)分布原理，粗差數(shù)據(jù)主要集中在正態(tài)分布曲線的兩側(cè)，通過對概率密度的劃分即可實(shí)現(xiàn)粗差識別。

在步驟(3)中，若車輛配備有GPS，可利用本發(fā)明估計(jì)出的導(dǎo)航信息輔助GPS進(jìn)行導(dǎo)航。

本發(fā)明相較于現(xiàn)有技術(shù)有如下有益效果：采用本發(fā)明中的基于M估計(jì)不完全約束的車輛導(dǎo)航方法時(shí)，首先在車輛行駛過程中實(shí)時(shí)提供準(zhǔn)確的車輛位置信息，且定位誤差不發(fā)散；其次，采用MIMU作為傳感器，極大降低了導(dǎo)航成本；然后本發(fā)明所提算法具有普適性，可以應(yīng)用在各類陸地車輛導(dǎo)航系統(tǒng)中，定位精度高，穩(wěn)定性好，最后，若車輛裝有GPS，可利用本發(fā)明估計(jì)出的導(dǎo)航信息輔助GPS進(jìn)行導(dǎo)航，增強(qiáng)導(dǎo)航效果。

附圖說明

圖1為MIMU在車輛上安裝示意圖。

圖2為不完全約束條件示意圖。

圖3為本發(fā)明中采用的諾亞得公司生產(chǎn)的ADIS16490型MIMU。

圖4為本發(fā)明中采用的佳維恒信公司生產(chǎn)的ET02A型GPS接收機(jī)。

圖5為模擬條件下采用基于M估計(jì)不完全約束與采用不完全約束算法下的車輛導(dǎo)航速度誤差對比圖。

圖6為車載試驗(yàn)條件下采用基于M估計(jì)不完全約束的車輛導(dǎo)航速度誤差曲線。

圖7為本發(fā)明流程圖。

具體實(shí)施方式

步驟一：數(shù)學(xué)模型建立

選取東北天(E,N,U)地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系(n系)，取車輛重心作為原點(diǎn)，橫軸指右方向作為x軸正向，y軸正向沿車輛縱軸指向前，z軸垂直于Oxy，并沿車輛的豎軸指向上，建立載體坐標(biāo)系(b系)。據(jù)此建立車輛導(dǎo)航模型。

步驟二：狀態(tài)方程建立

依據(jù)慣導(dǎo)系統(tǒng)誤差方程建立狀態(tài)方程：

其中，A為狀態(tài)矩陣，B為系統(tǒng)噪聲矩陣，W為系統(tǒng)噪聲，X為狀態(tài)向量，即：

步驟三：不完全約束方程建立

與傳統(tǒng)卡爾曼濾波相比，不完全約束算法的區(qū)別在于量測方程的建立。

所謂車輛的不完全約束條件是：

基于地面車輛運(yùn)動(dòng)的特點(diǎn)，在以下兩個(gè)假設(shè)條件成立時(shí)：

a).車輛運(yùn)動(dòng)時(shí)無測向滑動(dòng)；

b).車輛運(yùn)動(dòng)時(shí)一直不離開地面，即無跳起運(yùn)動(dòng)；

即如圖2所示，在b系上x軸與z軸的速度理論上應(yīng)該為零，即下式成立

但由于慣導(dǎo)系統(tǒng)存在導(dǎo)航誤差積累的問題，實(shí)際解算b系上的x軸與z軸的速度均不為零，從而可以利用兩軸上計(jì)算的速度誤差作為觀測量，進(jìn)行輔助導(dǎo)航。

利用不完全約束條件作為外部輔助信息時(shí)，觀測量信息建立如下：

其中，為b系上x軸與z軸的速度誤差，由式(2)可得

其中，是捷聯(lián)解算得到的b系上兩軸速度。

利用捷聯(lián)矩陣確定n系和b系的關(guān)系：

對式(6)加入誤差擾動(dòng)，整理有：

根據(jù)式(7)得到量測方程如下

其中，量測噪聲e是零均值白噪聲，量測矩陣H如下

其中，Vⁿ×表示Vⁿ的反對稱陣，表示取矩陣的第一行，其余符號含義類似。

步驟四：M估計(jì)不完全約束方程建立

當(dāng)車輛在行駛過程中發(fā)生側(cè)滑、跳躍等動(dòng)作時(shí)，不完全約束條件不再滿足時(shí)，采用M估計(jì)不完全約束方程，方程建立如下：

根據(jù)M估計(jì)的思想，為使?fàn)顟B(tài)變量X_k獲得最優(yōu)估計(jì)，只需讓誤差參量取最小值即可，所以，利用式(8)構(gòu)造似然函數(shù)方程，有

對式(10)求導(dǎo)，并令其為零有

對式(11)整理有：

其中，稱為極值函數(shù)，設(shè)稱為得分函數(shù)。

將式(12)改寫成

其中，稱為權(quán)值函數(shù)。

將式(13)寫成矩陣形式

其中，

利用Newton法解方程，得

其中，

利用Newton法解方程，得

其中，j為迭代次數(shù)。

利用式(16)進(jìn)行迭代即可得到的最優(yōu)值，此時(shí)，協(xié)方差陣為

對于極值函數(shù)ρ的選取，本發(fā)明選取IGG法作為極值函數(shù)，表示如下：

其中，a,b,c,d均為常數(shù)，各常數(shù)的取值，一般利用正態(tài)分布的知識按經(jīng)驗(yàn)求取。

步驟五：反饋校正過程

利用步驟三、步驟四算法，得到車輛導(dǎo)航信息誤差，利用該誤差對MIMU輸出參數(shù)進(jìn)行反饋校正，獲得正確的車輛導(dǎo)航信息。

本發(fā)明的效果通過如下方法得到驗(yàn)證：

為驗(yàn)證本發(fā)明的有效性，采用Matlab進(jìn)行仿真分析。仿真初始設(shè)置位置為[125.6705°E,45.7796°N,1.2m]，初始航向角為[0°,0°,0°]，仿真時(shí)間1500s，陀螺常值漂移0.01°/h，加速度計(jì)零偏10^-4g，采樣周期1s。仿真過程如表1所示，包含車輛靜止、勻變速、勻速和轉(zhuǎn)向等行駛狀態(tài)，在此基礎(chǔ)上，在400s-403s時(shí)間內(nèi)于b系x方向人為加入加速度為2m/s²，在403s-405s時(shí)間內(nèi)加入加速度為-2m/s²的側(cè)向速度，來模擬車輛發(fā)生側(cè)滑情況；在900s-903s時(shí)間內(nèi)于b系z方向人為加入加速度為2m/s²，在903s-905s時(shí)間內(nèi)加入加速度為-2m/s²的縱向速度，來模擬車輛發(fā)生跳躍情況；從而模擬實(shí)際狀態(tài)下車輛運(yùn)動(dòng)形式。

在模擬時(shí)，采用不完全約束算法和本發(fā)明算法分別對MIMU導(dǎo)航信息進(jìn)行處理，得到導(dǎo)航誤差結(jié)果。

表1車輛仿真過程設(shè)置

從圖4可以看出，兩種算法都能在車輛正常行駛提供較準(zhǔn)確的導(dǎo)航信息，但在發(fā)生車輛發(fā)生跳躍、側(cè)滑等情況時(shí)，本發(fā)明所提到的算法能有效解決時(shí)導(dǎo)航信息發(fā)散的問題，而傳統(tǒng)方法失效。