本發(fā)明涉及一種導(dǎo)航技術(shù)，特別是一種基于mems慣性測量單元的純慣性車輛導(dǎo)航方法。

背景技術(shù)：

mems慣性器件具有傳統(tǒng)慣性器件無法比擬的優(yōu)勢，受到了各國研究機構(gòu)的高度重視，產(chǎn)品層出不窮。美國draper實驗室的產(chǎn)品體積小于4in³，功率小于3w，可承受20000g的過載，陀螺零偏穩(wěn)定性達到了1°/h，加速度的零偏穩(wěn)定性小于100ug；美國的analogdevices公司2002年生產(chǎn)的adxr150陀螺的量程為±150°/s，靈敏度為12.5mv/°/s，角度隨機游走為0.05°/s/hz^1/2，實現(xiàn)了量產(chǎn)化；德國的robertbosch公司的drs-mm3陀螺，零偏穩(wěn)定性達到了1.35°/h，角度隨機游走為0.004°/s/hz^1/2；此外，美國的jpl、crossbowtechnology、honeywell、litton、rockwell、northrop，英國的bae，德國的imar公司，瑞典imego等均擁有量產(chǎn)化的mems慣性器件。

mems慣性器件的高速發(fā)展，使mimu在集成度、體積、精度等方面有了很大的提高。mimu由三個微機械陀螺儀與三個微加速度計構(gòu)成，敏感軸相互垂直，組成三維坐標系，處理器接收mimu的輸出信號，根據(jù)導(dǎo)航算法可以解算載體的方位角、姿態(tài)角、緯度、經(jīng)度、速度等參數(shù)。draper實驗室于2004年研制的mimu的體積為133cm³、重量0.272kg、功耗3.1w、零偏重復(fù)性為3°/h，可承受高達20000g的過載。國外的許多知名公司已有商品化的mimu系列產(chǎn)品，在飛機和導(dǎo)彈的導(dǎo)航、制導(dǎo)與控制領(lǐng)域獲得了廣泛的應(yīng)用，其中，典型的有honeywell公司的hg1900系列、hg1930系列，美國analogdevices公司的ads163xx系列，英國utcaerospacesystems的siimuxx系列等。

國內(nèi)于20世紀90年代開始mems慣性器件的研究，主要研究力量集中于清華大學(xué)、東南大學(xué)、哈爾濱工業(yè)大學(xué)、中科院上海微系統(tǒng)與信息技術(shù)研究所、中國電子科技集團13所等高校與科研院所，其中，清華大學(xué)研制的mems陀螺、電子13所的msi3100型微慣性測量單元的陀螺零偏穩(wěn)定性與重復(fù)性均達到了10°/h，其精度已接近國外同類產(chǎn)品。并且在尺寸和價格上，現(xiàn)在的mimu已經(jīng)適用于車載導(dǎo)航系統(tǒng)的應(yīng)用，但是另外一個問題是由于陀螺噪聲引起的航向誤 差，典型的mems陀螺噪聲特性會導(dǎo)致在積分成姿態(tài)角時出現(xiàn)較大的計算誤差。

由于mems技術(shù)的發(fā)展，慣性導(dǎo)航系統(tǒng)(ins)的成本大大降低。低成本、短時精度高、自主式的慣性導(dǎo)航系統(tǒng)得到了迅猛的發(fā)展。但是mems器件測量精度低、誤差大，ins對測量值進行積分會導(dǎo)致最后的姿態(tài)和位置信息快速發(fā)散。由于這些特點，ins和gps結(jié)合的組合導(dǎo)航系統(tǒng)成為了現(xiàn)在的研究熱點。通過設(shè)計一個卡爾曼濾波器可以利用gps的測量信息校正慣導(dǎo)系統(tǒng)輸出，抑制慣導(dǎo)系統(tǒng)的發(fā)散。但是gps也有一些缺點，在高樓、隧道或者叢林中，它常常會接收不到信號，無法給ins提供測量信息，導(dǎo)致ins很快的發(fā)散。

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

本發(fā)明的目的在于提供一種基于mems慣性測量單元的純慣性車輛導(dǎo)航方法，采用數(shù)據(jù)挖掘算法svm辨識出車輛的靜止、直行和轉(zhuǎn)彎狀態(tài)，并在不同運動狀態(tài)下，使用磁和車輛運動學(xué)輔助算法估計車輛的俯仰、橫滾、航向或速度等導(dǎo)航量，然后進行濾波補償，全面提高車輛純慣性導(dǎo)航系統(tǒng)導(dǎo)航精度。該方法包括：

mems慣性測量單元獲取的角速度和加速度數(shù)值并存儲，

每一角速度和加速度數(shù)值經(jīng)過高頻慣導(dǎo)解算輸出第一導(dǎo)航信息，

存儲一定時間的角速度和加速度數(shù)值經(jīng)過低頻導(dǎo)航狀態(tài)量計算輸出第二導(dǎo)航信息，

第一導(dǎo)航信息和第二導(dǎo)航信息進行相減獲得導(dǎo)航信息誤差后作為卡爾曼濾波觀測量進行卡爾曼濾波獲取導(dǎo)航信息誤差估計值，

導(dǎo)航信息誤差估計值校正第一導(dǎo)航信息后輸出校正后的導(dǎo)航信息。

本發(fā)明與現(xiàn)有技術(shù)相比，具有以下優(yōu)點：

(1)能夠在不需要先驗信息的情況下只利用極短時間的靜止和運動數(shù)據(jù)訓(xùn)練svm(實驗證明運動和靜止數(shù)據(jù)各采集1分鐘即可)，就能夠在后續(xù)導(dǎo)航過程中有效的區(qū)分車輛的靜止、直行和轉(zhuǎn)彎狀態(tài)。

(2)基于確定的運動狀態(tài)能夠獲得更多的不隨時間發(fā)散的導(dǎo)航狀態(tài)量，從而能夠更有效的校正慣導(dǎo)系統(tǒng)，抑制慣導(dǎo)系統(tǒng)誤差發(fā)散速度。

附圖說明

圖1是本發(fā)明的方法流程圖。

具體實施方式

結(jié)合圖1，本發(fā)明中每隔一個濾波周期解算出車輛的導(dǎo)航狀態(tài)量，并與慣導(dǎo)系統(tǒng)的輸出進行卡爾曼濾波，將估計出的導(dǎo)航誤差量反饋給慣性導(dǎo)航系統(tǒng)，利用誤差量補償后輸出導(dǎo)航狀態(tài)量：姿態(tài)角、速度和位置。具體方法為：

步驟1，對mems慣性測量單元獲取的每一角速度和加速度數(shù)值經(jīng)過慣導(dǎo)解算輸出第一導(dǎo)航信息；

步驟2，存儲一定時間的角速度和加速度數(shù)值經(jīng)過低頻導(dǎo)航狀態(tài)量計算輸出第二導(dǎo)航信息；

步驟3，第一導(dǎo)航信息和第二導(dǎo)航信息進行相減獲得導(dǎo)航信息誤差后作為卡爾曼濾波觀測量進行卡爾曼濾波獲取導(dǎo)航信息誤差估計值；

步驟4，導(dǎo)航信息誤差估計值校正第一導(dǎo)航信息后輸出校正后的導(dǎo)航信息；所述導(dǎo)航信息包括姿態(tài)角、速度和位置。

所述步驟2為獲取車輛不同行使狀態(tài)下的導(dǎo)航狀態(tài)量估計值，其具體過程包括：

步驟2.1，設(shè)定一閾值，基于存儲的一定時間內(nèi)航向陀螺的輸出角速度均值的絕對值判斷車輛行使狀態(tài)：若絕對值小于閾值，則車輛處于直行或靜止狀態(tài)，否則車輛處于轉(zhuǎn)彎狀態(tài)。具體地，選定轉(zhuǎn)彎判定閾值ωz0，計算出一個滑動窗口內(nèi)z軸陀螺儀輸出平均值，將這個平均值的絕對值與閾值比較，判斷是否轉(zhuǎn)彎。判斷準則為：若abs(mean(ωz(n))＞ωz0，車輛處于轉(zhuǎn)彎狀態(tài)；否則車輛處于靜止或直行狀態(tài)。具體計算如下

其中ωz(i)為i時刻z軸角速度的輸出；k為滑動窗口長度；mean(ωz(n))為窗口長度k內(nèi)z軸角速度輸出的平均值。

步驟2.2，對車輛處于靜止或直行狀態(tài)時，由存儲的mems慣性測量單元輸出信息獲取mems慣性測量單元輸出的特征量，采用基于線性核函數(shù)的硬閾值svm再次進行分類。

步驟2.2.1，由存儲的mems慣性測量單元輸出信息獲取mems慣性測量 單元輸出的特征量。

所述特征量包括三軸加速度計輸出的模值mf(n)、x軸方向加速度計輸出信息mfx、頻率信號幅值的均值x軸方向加速度和角速度的幅值均值其中

(1)地球重力場產(chǎn)生的重力加速度大小為g，從而只要有機動，都會體現(xiàn)在由三軸加速度計輸出值計算得到的模值中，使模值不等于g。車輛在運動過程中很難保證絕對的沒有機動，因此可以利用三軸加速度計輸出的模值信息作為第一類特征，具體特征計算如下

其中：

fx(i)為i時刻x軸加速度計的輸出；fy(i)為i時刻y軸加速度計的輸出；fz(i)為i時刻z軸加速度計的輸出。

(2)在某些特殊情況下，即時車輛存在機動，也有可能三軸加速度計輸出的模值等于重力加速度。考慮到車輛運動時只有一個方向的動力，而且人腳控制油門很難保證勻速運動，因此車輛運動時，前進方向的加速度值相對靜止狀態(tài)應(yīng)該有一定的特點，不管是突然的加速還是減速都會體現(xiàn)在前進方向的加速度值中。故可以選擇x軸方向加速度計輸出的信息作為另一特征，對第一類特征存在的缺陷進行補充，具體特征計算如下

式中參數(shù)定義同上式。

(3)頻域特征選擇

信號的頻域特征已經(jīng)被廣泛應(yīng)用于人體運動識別中，許多在時域上體現(xiàn)不出來的信號特征在頻域上都會體現(xiàn)的非常清楚。比如人步行、跑步對加速度計信號所造成的影響在時域上很難看清，只能看到不斷上下跳動的加速度 值，但當(dāng)轉(zhuǎn)換到頻域上時，特征就會非常明顯，因為步行和跑步造成的加速度計值變化擁有不同的頻率。

與人體運動類似，車輛靜止時mimu輸出的值主要來自于傳感器本身的噪聲；當(dāng)車輛運動時，車輛機動的信息就會作用在x軸方向。而道路不會完全平整又會造成車輛繞x軸方向顛簸，也就是車輛左右搖晃。車輛機動的頻率和道路不平整引起的車輛搖晃的頻率顯然會大大低于發(fā)動機的轉(zhuǎn)動頻率。這類低頻信號可以被加速度計和陀螺儀敏感到。因此車輛靜止和運動時x軸方向加速度和角速度的頻譜和靜止時會有很大不同。

基于上述分析，選取x軸方向加速度計信息和陀螺儀信息的頻域特征作為第三類特征。

為了得到信號的頻率譜，可以采用離散傅里葉變換(dft)。dft的定義為：

其中x(f)為時域信號x中頻率為f的信號；k為滑動窗口長度。

式(4)也可以寫成復(fù)數(shù)形式如下：

其中

利用式(5)可以計算出各個頻率信號的幅值大?。?/p>

選取各個頻率信號幅值的均值作為第三類特征，具體計算如式(7)

其中|xf(i)|為第i個頻率下信號的幅值，k為滑動窗口長度。

具體對應(yīng)到x軸方向加速度和角速度的幅值均值，如下式：

其中為x軸方向加速度頻域信號的幅值，為x軸方向角速度頻域信號的幅值，將x軸方向加速度和角速度的幅值均值作為第四類特征。

已經(jīng)經(jīng)過實驗驗證，在選取上述四個特征量后，利用線性核函數(shù)下的硬閾值svm即可有效辨識出車輛的靜止和運動狀態(tài)。

步驟2.2.2，采用基于線性核函數(shù)的硬閾值svm再次進行分類。

步驟2.2.2.1，首先利用基于線性核函數(shù)的硬閾值svm進行訓(xùn)練。

基于提取的上述四項特征，可以建立特征向量數(shù)組如下：

其中：

mf＝[mf(1)mf(2)...mf(s)]

mfx＝[mfx(1)mfx(2)...mfx(s)]

s為訓(xùn)練樣本數(shù)，這里的訓(xùn)練樣本是一組帶標簽的樣本，對應(yīng)的標簽數(shù)組如下：

[y1...ypyp＋1...ys]

其中p為第一類標簽的樣本數(shù)(例如靜止狀態(tài))，剩余另一類標簽的樣本數(shù)(例如直行狀態(tài))。一般取

svm利用上述建立的特征向量進行分類，即找到向量w和b，使得：

滿足上式的向量可能有若干個，svm的目的是尋找其中使兩類特征向量分隔得最開的超平面，即最小化下面關(guān)于w的表達式

這是一個帶約束的最優(yōu)化問題，利用拉格朗日方程可以求解，獲得最優(yōu)的w和b值。

步驟2.2.2.2，存儲一定時間的角速度和加速度數(shù)值帶入訓(xùn)練后的基于線性核函數(shù)的硬閾值svm，判斷在該時間內(nèi)車輛處于的狀態(tài)。

步驟2.3，根據(jù)慣性導(dǎo)航比力方程推導(dǎo)的導(dǎo)航狀態(tài)量估計值。

傳統(tǒng)的慣導(dǎo)系統(tǒng)假設(shè)導(dǎo)航坐標系為n系，本發(fā)明中n系均取為n-e-d坐標系，假設(shè)自地心至導(dǎo)航坐標系的支點引位置矢量根據(jù)哥氏定理可以得到比力方程：

其中是在地球上觀察到的位置矢量的變化率，所以是運載體相對地球的運動速度，簡稱為地速，記作

這里考慮到車輛運動的特殊性，它在載體系中的運動特性更清楚，只沿載體前進方向有速度和加速度，現(xiàn)在對式(12)兩邊求絕對變化率，并再次使用哥氏定理，其中相對變化率對車輛載體系b系求?。?/p>

mems陀螺敏感不到地球自轉(zhuǎn)，忽略可得：

其中，左邊項可表示為

是單位質(zhì)量上作用的非引力外力，為重力加速度。

將式(16)向b系內(nèi)投影，則比力方程為：

其中即為車輛加速度，方向沿車輛前進方向；為加速度計測得比力；為陀螺儀測得角速度。

現(xiàn)在，以車輛前進方向為x軸，車輛側(cè)向向右為y軸，縱向向下為z軸。根據(jù)汽車理想狀態(tài)下不會側(cè)滑，不會離開地面，假設(shè)車輛橫向和縱向速度為0，假設(shè)汽車前進速度為vx，加速度為x軸方向比力fx，y軸方向比力為fy，z軸方向比力為fz，可以寫出比力方程矩陣形式為：

化簡式(18)可得：

其中為姿態(tài)轉(zhuǎn)移矩陣：

為航向角，θ為俯仰角，γ為滾轉(zhuǎn)角。

將代入式(19)，化簡可得：

vx*ωz-fy-g·sinγ·cosθ＝0(21)

vx·ωy＋fz+g·cosγ·cosθ＝0(22)

由式(20)、(21)、(22)即可以進行導(dǎo)航狀態(tài)量的估計。

(1)若判定車輛處于靜止狀態(tài)，則可以根據(jù)(20)估計出俯仰角：

進而根據(jù)(21)估計出滾轉(zhuǎn)角：

根據(jù)前面估算出的俯仰角θ和滾轉(zhuǎn)角γ對磁數(shù)據(jù)進行投影，轉(zhuǎn)換到水平坐標系中：

m'x＝mxcosθ＋mysinγsinθ＋mzcosγsinθ(25)

m'y＝mycosγ-mzsinγ(26)

其中mx，my，mz為載體系中傳感器測得的磁場值，m'x，m'y為投影后的水平坐標系中的磁場值。

然后進一步估算出航向角：

d為磁偏角(27)

同時，車輛靜止時前進、橫向、縱向速度為0：

vx＝0(28)

vy＝0(29)

vz＝0(30)

(2)若判定車輛處于直線行駛狀態(tài)，則可以根據(jù)(21)、(22)估計出滾轉(zhuǎn)角：

橫向、縱向速度為0：

vy＝0(32)

vz＝0(33)

(3)若判定車輛處于轉(zhuǎn)彎狀態(tài)，則根據(jù)式(22)可估計出車輛速度：

橫向、縱向速度為0：

vy＝0(35)

vz＝0(36)

步驟3中，根據(jù)估計出的導(dǎo)航狀態(tài)量，與慣導(dǎo)系統(tǒng)輸出的導(dǎo)航狀態(tài)量作差，進行卡爾曼濾波。

(1)卡爾曼濾波狀態(tài)方程：

選取卡爾曼濾波狀態(tài)量為

x＝[δvnδveδvdφnφeφdδlδλδh]^t

其中δvn、δve、δvd分別為北向、東向、地向速度誤差，φn、φe、φd分別為北向、東向、地向姿態(tài)角誤差，δl、δλ、δh分別為緯度、經(jīng)度、高度誤差。

根據(jù)慣導(dǎo)系統(tǒng)誤差傳遞方程，狀態(tài)方程為

x為狀態(tài)向量，f為狀態(tài)轉(zhuǎn)移矩陣，w為狀態(tài)噪聲方差陣，其中

其中rm為子午圈面內(nèi)主曲率半徑，rn為卯酉圈內(nèi)主曲率半徑，ωie為地球自轉(zhuǎn)角速度。

其中fn為北向比力值，fe為東向比力值，fd為地向比力值。

f32＝03×3

(2)卡爾曼濾波觀測方程：

z＝hx＋v

z為觀測向量，h為觀測矩陣，v為觀測噪聲方程陣。

a)車輛為靜止狀態(tài)時

假設(shè)由磁和車輛運動學(xué)信息算出的虛擬觀測量為θd、γd、vxd、vyd、vzd。慣導(dǎo)解算出的狀態(tài)量為θi、γi、vxi、vyi、vzi，其中vxi、vyi、vzi由得到。觀測向量為：

觀測矩陣為：

b)車輛為直行狀態(tài)時

假設(shè)由車輛運動學(xué)信息算出的虛擬觀測量為γd、vyd、vzd；慣導(dǎo)解算出的狀態(tài)量為γi、vyi、vzi，其中vyi、vzi由得到。觀測向量為：

觀測矩陣為：

c)車輛為轉(zhuǎn)彎時

假設(shè)由車輛運動學(xué)信息算出的虛擬觀測量為vxd、vyd、vzd；慣導(dǎo)解算出的狀態(tài)量為vxi、vyi、vzi，其中vxi、vyi、vzi由得到。觀測向量為：

觀測矩陣為：

根據(jù)慣導(dǎo)系統(tǒng)狀態(tài)方程和車輛不同運動狀態(tài)下的觀測方程利用下面5個卡爾曼濾波方程對前面選擇的9維狀態(tài)量進行估計。

狀態(tài)一步預(yù)測

狀態(tài)估計

濾波增益

一步預(yù)測均方誤差

估計均方誤差

pk+1/k+1＝pk+1/k-kk+1hk+1pk+1/k(41)

假設(shè)t時刻濾波估計出的狀態(tài)量為：

步驟4，利用該估計結(jié)果可以對慣導(dǎo)系統(tǒng)t時刻的輸出進行校正，假設(shè)慣導(dǎo)系統(tǒng)t時刻的輸出的導(dǎo)航信息分別為：

地理系下北-東-地三個方向的速度：

經(jīng)、緯度和高度：

姿態(tài)角矩陣為

則可按下面方式進行補償：

經(jīng)過校正后的信息誤差會大大降低，因此上述方法在沒有g(shù)ps輸出作為外觀測的情況下可以有效的抑制導(dǎo)航誤差的發(fā)散。