多傳感器行人航位推算算法研究*

陳新東 蔡 鵬 高大遠 王 超 朱海榮

（海軍潛艇學院 青島 266000）

1 引言

全球定位系統（GPS）能提供三維位置和速度信息，已經廣泛應用于人們的日常生活中，但GPS信號容易受衛星信號的衰減以及干擾信息的影響，在室內、地下建筑以及城市街道等環境下信號弱，信息可用性差，GPS不能進行準確的定位［1］。目前有很多學者對室內定位技術進行了研究，如WiFi定位技術、超寬帶技術、超聲波技術、視覺定位、紅外技術、FRID定位、藍牙技術等定位傳感技術［2～7］。然而多傳感器的室內定位技術是比較熱門的一種定位手段。當前，國內外有不少學者對多傳感器定位技術進行了研究。Foxlin等開發了”NavSheo”的定位系統［8］；鄧林坤等設計了低成本多傳感器的室內行人航位推算（PDR）系統，并且在ipad上進行了實驗［9］；Ruiz和 Madrid 等利用慣性導航系統與RFID定位系統進行結合實現了室內行人的定位［10］。

本文提出了一種基于足綁式的多傳感器PDR算法，并采用磁角速率更新（MARU）很好地克服了磁干擾的影響，利用慣性測量單元（IMU）對行人的線運動以及角運動等信息進行采集，同時利用三軸磁力計來計算得到對應的磁感應電勢信息，采用EKF算法對各個傳感器的誤差進行補償與修正，最終推算得到行人的位置。

2 初始對準

靜止狀態時，利用加速度計與陀螺儀的輸出進行初始對準。加速度計測量重力加速度，可據此來計算俯仰角和橫滾角。陀螺儀測量地球自轉角速度，可計算得到航向角。但是對于低成本的MEMS IMU來說，MEMS IMU陀螺儀的漂移非常大，用陀螺儀的輸出來計算航向角，其誤差非常大。因此本文采用靜止條件下加速度計和磁力計獲得初始IMU的姿態角，導航坐標系（n系）為北西天坐標系，載體坐標系（b系）固連在傳感器上。一般情況下，由加速度計和磁力計的輸出可得：

3 系統模型

3.1 狀態模型

行人導航系統狀態方程與狀態向量呈非線性關系，可以進行線性化處理［11-13］。系統誤差狀態向量可以表示為

式中：δΨ 、Δω 、δr、δr?、Δa分別代表k時刻的姿態誤差信息，陀螺零偏信息，位置偏差信息，速度偏差信息，加計零偏信息，總共15維信息當做系統的誤差狀態向量。

經過線性化處理得狀態方程：

式中：Xk-1代表k-1時刻的誤差狀態向量后驗估計，Xk代表k時刻誤差狀態向量先驗估計，Wk-1代表k-1時刻的過程噪聲，它的協方差矩陣是Qk=E(WkWkT)，Φk/k-1則代表狀態轉移矩陣。其表達式如下：

其中：

ab(k)=Acc(k)-Δa為EKF修正后的比力，為k時刻加計的量測值，是k時刻b系經過坐標變換到n系方向余弦矩陣的先驗估計。

3.2 量測模型

當足部處于靜止或者速度為0時，此時相對于地面，速度接近于0，ZUPT算法把速度誤差Δvk,k-1=vk,k-1-[0,0,0]T作為EKF的觀測量。

在室內這種環境下，磁的干擾不容易估計，很容易會受到室內建筑等設施的干擾從而引入更多的誤差，為了避免外界磁干擾的影響，引入MARU來進行航向修正，即行人足部處于零速或靜止時，磁感應強度恒定不變。而MEMS轉動時，三軸磁力計分量會發生旋轉，且旋轉速率與陀螺儀角速率ω(k)變化有關，所以將磁感應強度變化作為量測矢量，對ω(k)進行誤差估計補償來抑制航向角的發散。

當MEMS轉動時，磁增量為

式中：Bb(k)和Bb(k-1)分別表示k時刻和k-1時刻的在載體坐標系下磁感應強度。

4 步態檢測

行人行走時，走的步態是周期性變化的；本實驗對行人行走的數據進行采集，每個傳感器的三個軸都具有周期性變化的特征。本文利用三個傳感器，分別是加速度計、陀螺儀以及磁力計，通過三者結合對行人行走進行檢測測定。

其中γα加速度幅值閾值，γω為角速度幅值閾值，Δt為采樣時間間隔。

通過上述判定條件，行人步態檢測如圖1所示。

圖1 步態檢測

文中僅僅截取了一小段步態檢測數據用來進行描述與分析，圖中X軸代表傳感器采樣樣本數，Y軸代表數值大小，無固定含義。從圖中可以看出，行人行走步態呈現出周期性變化規律，步態靜止時，行人靜止，步態移動時，行人則處于移動狀態。

5 實驗數據結果分析

實驗采用的IMU是XSENS公司的MTI產品，實驗在室內完成，本次實驗將IMU綁在實驗人員的足部，依據已經設定好的路線行走，總共做了兩次實驗，分別沿著不同路線行走，采樣頻率設置為100HZ，圖2和圖3分別為Matlab軟件下仿真的實驗結果對比圖。

圖2 總路程28.8m測試對比結果

圖3 總路程110.7m測試對比結果

為了更加清晰地看出文中算法的效果，對兩種方法產生的定位誤差進行相應的統計，如表1所示。定位誤差采用實驗路線終點值減去路線真實終點值，得到的絕對差值占總路程的比值來進行表示。表1記錄了兩種不同方法的對比，其中初始對準時間為120s。從表中定位誤差結果對比可得：在EKF基礎上的ZUPT+MARU融合的方法，對位置誤差具有改善作用，定位精度可以得到一定的提高。

表1 100HZ兩種算法IMU數據的計算結果比較

6 結語

文中提出了一種多傳感器行人航位推算方法，該方法可以減少加計零偏，同時修正行人步態誤差；對陀螺儀角速率進行誤差估計補償，抑制了外界磁干擾，增強了對行人航向跟蹤性能；通過引入ZUPT+MARU融合構建量測模型，有效地修正行人行走時航向的偏差，實驗結果表明：在短時間內，該系統能夠滿足行人在室內的定位需求，可以實現短距離的導航，位置誤差能夠控制在0.56%范圍內，定位效果良好。