寇彩云， 張會清， 王 普

(北京工業(yè)大學(xué) 信息學(xué)部，北京 100124)

0 引 言

目前，以微機電系統(tǒng)(micro-electro-mechanical system,MEMS)慣性傳感器進行室內(nèi)定位[1～4]的技術(shù)可以分為兩類，以行人運動姿態(tài)為研究的行人航跡推測和以慣性導(dǎo)航解算算法的行人室內(nèi)定位。Levi R W和Judd T在1996年提出了行人航跡推算法[5]，根據(jù)人員行走的步態(tài)特征，通過加速度傳感器獲得室內(nèi)人員的運動數(shù)據(jù)，通過算法估計行人的單步步長和行走的步數(shù)，由陀螺儀獲得人體姿態(tài)信息。2012年瑞典皇家工學(xué)院提出了一個開源的、實時的以鞋綁式慣性傳感器為基礎(chǔ)，并應(yīng)用零速度算法修正的慣性定位系統(tǒng)[6]，且其設(shè)備成本低、短距離定位導(dǎo)航準(zhǔn)確度高。國內(nèi)很多高校如西北工業(yè)大學(xué)、國防科技大學(xué)等也進行了一些基礎(chǔ)理論、算法的研究實驗工作。導(dǎo)航、制導(dǎo)與控制專業(yè)的科研人員和一些公司也在逐步跟隨國外的研究步伐。

由于鞋綁式的慣性器件固定于腳面，每次固定的水平位置不同會導(dǎo)致采集的數(shù)據(jù)有誤差，本文提出采用零偏置校正算法解決上述問題；在進行長距離加速度積分時會有累積誤差的存在，本文采用改進的零速度檢測算法和頻域積分算法分解決上述問題。

1 數(shù)據(jù)采集

將慣性器件盡量呈水平狀態(tài)固定到腳面上，安卓手機開發(fā)的APP軟件通過藍牙連接到慣性器件，將行人行走時的三軸加速度數(shù)據(jù)，手機內(nèi)置的電子羅盤的方向數(shù)據(jù)和三軸角速度數(shù)據(jù)通過藍牙傳輸?shù)绞謾C上并進行存儲。如圖1。

圖1 Android手機數(shù)據(jù)采集畫面

2 數(shù)據(jù)處理

2.1 零偏置校正算法

由于慣性器件采用鞋綁式，每次固定的位置無法達到理想的水平狀態(tài)，造成了靜止?fàn)顟B(tài)時所測得的三軸加速度值不為零，且對零速度檢測算法的閾值設(shè)定也會因每次固定位置不理想而造成閾值每次都會變，閾值的變化也會造成最終結(jié)果的誤差。

零偏置校正算法是對每次測量數(shù)據(jù)的初始靜態(tài)數(shù)據(jù)求均值，行人行走時的動態(tài)數(shù)據(jù)減去初始靜態(tài)數(shù)據(jù)均值，以此保證靜態(tài)時三軸加速度數(shù)據(jù)為零，滿足慣性器件理想狀態(tài)下的水平固定位置狀態(tài)。因為采集數(shù)據(jù)的設(shè)備每秒采集100組數(shù)據(jù)，為了保證采集到的數(shù)據(jù)為靜止?fàn)顟B(tài)的數(shù)據(jù)，所以本文取采集數(shù)據(jù)時的前50個數(shù)取求均值作為初始誤差

(1)

以采集的數(shù)據(jù)減去初始誤差作為行人真實行走數(shù)據(jù)。

2.2 改進零速度檢測算法

積分誤差會隨著時間的推移而累積，用改進的條件判斷法進行零速度檢測，當(dāng)檢測到行人運動的“零速度”時，觸發(fā)零速度修正，對數(shù)據(jù)進行置零校正；對行人運動的三軸加速度和三軸角速度進行閾值設(shè)定，當(dāng)小于一定范圍時視為靜止?fàn)顟B(tài)。

由于腳著地的時候，只受到重力的作用，此時利用合加速度的幅值、方差、合角速度的幅值和方差在給定的閾值范圍內(nèi)來判斷靜止時刻。

(2)

最后，將單個條件結(jié)果進行“與”運算，求出腳著地，即“零速度”的時刻c=c1&c2&c3&c4。

對處于零速度時刻的數(shù)據(jù)進行置零校正。

2.3 頻域積分

將校正后的數(shù)據(jù)進行沒有累積誤差頻域積分[7]，進行傅里葉逆變換后得到行人更準(zhǔn)確的位移信息。

設(shè)一次積分前的信號為v(n)(0

(3)

將每個頻率分量的信號值V(k)轉(zhuǎn)換為一次積分后有

(4)

再將D(k)逆變換，即可得到輸入信號v(n)的一次精準(zhǔn)的積分。

2.4 均值濾波與角度補償算法

首先用8字校正法校正手機中的電子羅盤[8]，手機電子羅盤的優(yōu)點是能提供絕對的航向值且使用方便。對電子羅盤獲取的數(shù)據(jù)首先使用滑動均值濾波，減小數(shù)據(jù)的波動，由于電子羅盤本身的原因，在向南方向行走時，所獲取的電子羅盤的數(shù)據(jù)會突然地跳變，對獲取的航向會有很大的影響，針對此問題采用角度補償?shù)姆椒ㄟM行校正?；瑒泳堤幚淼碾A數(shù)選為50，當(dāng)前后時刻的角度突然地跳變超過180°時，是在向南的方向，此時在[-180°,-170°]之間產(chǎn)生一個隨機的數(shù)作為此時的航向角，當(dāng)前后時刻的角度跳變超過30°而小于180°時，則使用陀螺儀的角度作為此時的航向角。

3 定位算法設(shè)計

系統(tǒng)使用了集成藍牙HC—06模塊和MPU6050慣性傳感器，集成了加速度計、角速度計和磁力計，不需要借助于外界任何的信號與裝置，且結(jié)構(gòu)簡單、抗干擾性強等優(yōu)點。根據(jù)行人的運動特點，系統(tǒng)采用鞋綁式MPU6050慣性傳感器和手機相結(jié)合的數(shù)據(jù)采樣方式。模塊固定在行人腳面，行人手持手機正對行走方向。定位算法的流程如圖2。

圖2 行人導(dǎo)航定位算法流程

4 實驗測試仿真

算法設(shè)計驗證及分析平臺為MATLAB R2010a。

1)零偏置校正算法驗證

校正結(jié)果如圖3所示，經(jīng)過零偏置校正后，靜止?fàn)顟B(tài)下加速度數(shù)據(jù)值基本為零，數(shù)據(jù)準(zhǔn)確度得到了提升，對零速度校正的閾值設(shè)定也提供了方便。

圖3 零偏置校正結(jié)果

2)改進的零速度校正算法驗證

對所采集的加速度、角速度、加速度方差、角速度方差進行閾值設(shè)定，當(dāng)零速度時刻c=1時，將這一時刻的加速度值置零，所得到零速度檢測結(jié)果和姿態(tài)角檢測如圖4。

圖4 零速度校正和姿態(tài)角驗證結(jié)果

3)頻域積分驗證

對行人行走的二維平面的x軸和y軸的速度數(shù)據(jù)進行離散化并進行積分，積分結(jié)果如圖5所示。

圖5 x和y軸加速度頻域積分驗證

4)均值濾波與角度補償算法驗證得到的行人航向角數(shù)據(jù)如圖6所示。

圖6 行人航向角結(jié)果

對行人的x，y軸加速度數(shù)據(jù)進行時域內(nèi)二次積分，將x，y軸積分?jǐn)?shù)據(jù)作為橫縱坐標(biāo)畫出其軌跡，如圖7所示。

圖7 無向時域和頻域積分

由圖7可以看出，時域積分畫出的無向運動軌跡褶皺較多，而頻域積分畫出的無向運動軌跡則比較順滑流暢，因為行人在行走過程中是按直線行走，不會不停地左右變動，因此頻域積分?jǐn)?shù)據(jù)更準(zhǔn)確一些。

圖8為改進前軌跡。圖9為經(jīng)過零速度檢測算法，時域和頻域積分算法，航向角使用滑動均值濾波處理后的行人行走的軌跡(算法1)，從圖中可以看出在行人行走時偏離了實際的路線。但頻域積分比時域積分更接近實際路線。圖10為經(jīng)過改進零速度檢測算法，頻域積分算法，航向角使用滑動均值濾波和角度補償算法處理后的行人行走的軌跡(算法2)，其中，加速度幅值取2.4 m/s2，加速度方差取0.7 m/s2，角速度幅值取25°/s，角速度方差取2 000°/s；由圖可知，所得的運動結(jié)果比改進算法前更準(zhǔn)確。

圖8 改進算法前軌跡

圖9 算法1軌跡

圖10 算法2軌跡

表1為實際值、算法改進前、算法1、算法2各拐點值與實際值誤差絕對值。本文所用的算法有效提高了運動軌跡的準(zhǔn)確度。

表1 拐點誤差絕對值對比

5 結(jié)束語

本文分析了基于零偏置校正算法、改進零速度檢測算法和頻域積分算法的原理和實現(xiàn)方法，指出了零偏置校正對加速度數(shù)據(jù)的影響，并提出了運用改進零速度檢測算法和頻域積分算法減小累積誤差對運動結(jié)果的影響。并提出了運用滑動均值濾波和角度補償法處理電子羅盤的數(shù)據(jù)，結(jié)合陀螺儀得到更準(zhǔn)確的行人航向角，測試表明，零偏置校正算法提高了原始采樣數(shù)據(jù)的準(zhǔn)確度，改進零速度檢測算法和頻域積分減小了累積誤差，滑動均值濾波和角度補償算法提高了航向角的準(zhǔn)確度，將算法融合后得到行人運動軌跡，實驗結(jié)果表明，本文算法提升了行人運動軌跡的準(zhǔn)確度。