基于慣性傳感器的行人室內定位算法*

寇彩云， 張會清， 王 普

(北京工業大學 信息學部，北京 100124)

0 引 言

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

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

1 數據采集

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

圖1 Android手機數據采集畫面

2 數據處理

2.1 零偏置校正算法

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

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

(1)

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

2.2 改進零速度檢測算法

積分誤差會隨著時間的推移而累積，用改進的條件判斷法進行零速度檢測，當檢測到行人運動的“零速度”時，觸發零速度修正，對數據進行置零校正；對行人運動的三軸加速度和三軸角速度進行閾值設定，當小于一定范圍時視為靜止狀態。

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

(2)

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

對處于零速度時刻的數據進行置零校正。

2.3 頻域積分

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

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

(3)

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

(4)

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

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

首先用8字校正法校正手機中的電子羅盤[8]，手機電子羅盤的優點是能提供絕對的航向值且使用方便。對電子羅盤獲取的數據首先使用滑動均值濾波，減小數據的波動，由于電子羅盤本身的原因，在向南方向行走時，所獲取的電子羅盤的數據會突然地跳變，對獲取的航向會有很大的影響，針對此問題采用角度補償的方法進行校正。滑動均值處理的階數選為50，當前后時刻的角度突然地跳變超過180°時，是在向南的方向，此時在[-180°,-170°]之間產生一個隨機的數作為此時的航向角，當前后時刻的角度跳變超過30°而小于180°時，則使用陀螺儀的角度作為此時的航向角。

3 定位算法設計

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

圖2 行人導航定位算法流程

4 實驗測試仿真

算法設計驗證及分析平臺為MATLAB R2010a。

1)零偏置校正算法驗證

校正結果如圖3所示，經過零偏置校正后，靜止狀態下加速度數據值基本為零，數據準確度得到了提升，對零速度校正的閾值設定也提供了方便。

圖3 零偏置校正結果

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

對所采集的加速度、角速度、加速度方差、角速度方差進行閾值設定，當零速度時刻c=1時，將這一時刻的加速度值置零，所得到零速度檢測結果和姿態角檢測如圖4。

圖4 零速度校正和姿態角驗證結果

3)頻域積分驗證

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

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

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

圖6 行人航向角結果

對行人的x，y軸加速度數據進行時域內二次積分，將x，y軸積分數據作為橫縱坐標畫出其軌跡，如圖7所示。

圖7 無向時域和頻域積分

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

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

圖8 改進算法前軌跡

圖9 算法1軌跡

圖10 算法2軌跡

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

表1 拐點誤差絕對值對比

5 結束語

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