腰綁式行人導航系統(tǒng)基于緩存區(qū)的啟發(fā)式航向補償算法研究

王鵬宇，孟之棟，鄧志紅

(北京理工大學自動化學院，北京 100081)

0 引言

位置信息在人們?nèi)粘Ｉ詈凸ぷ髦邪l(fā)揮著日益重要的作用。在現(xiàn)代社會，人們大部分時間都處于寫字樓、住宅樓、學校等建筑物內(nèi)部。在復雜的室內(nèi)環(huán)境下，人們對室內(nèi)位置服務的需求迅速增加。目前，室內(nèi)行人導航系統(tǒng)的主流技術方案可分為無線通信定位方案與行人航位推算(Pedestrian Dead Reckoning，PDR)方案兩類。其中，無線通信定位技術包括超寬帶(Ultra Wide Band，UWB)技術[1]、Bluetooth技術[2]、ZigBee技術[3]、WiFi技術[4]和射頻識別技術[5]等。上述方案均需要外部系統(tǒng)設備的支持，在災難救援等應用背景下，一旦外部設備損壞將導致無線通信定位方案無法有效發(fā)揮作用。 PDR方案[6]將微機電慣性傳感器模塊(Micro-Electro-Mechanical System Inertial Measurement Unit，MEMS IMU)固定于行人身體上，基于慣性參量確定步長模型或通過捷聯(lián)慣性導航系統(tǒng)解算距離估計行人的行進步長，并由陀螺儀和電子羅盤等輔助信息組合得到每一步的行進方向，即可得到航位推算系統(tǒng)所需的2個關鍵參數(shù)，是一種完全自主無源的定位導航方式，非常適合在無基礎設施的室內(nèi)環(huán)境使用，因此是行人導航領域的研究熱點之一。目前，MEMS IMU的固定方式有腰綁式、足綁式及肩綁式等。足綁式采集到的慣性數(shù)據(jù)存在較大抖動，需采用零速修正方法抑制誤差發(fā)散；肩綁式采集數(shù)據(jù)更加平穩(wěn)，但上肢對運動感知有延遲且存在特征缺失；相比之下，腰綁式能夠在保證平穩(wěn)采集慣性數(shù)據(jù)的同時保留較多運動特征，且對行人正常運動影響較小，符合實際應用需求，特別是對于災難救援和單兵作戰(zhàn)等任務場景。因此，本文在腰綁式行人導航系統(tǒng)的基礎上設計了PDR算法并對其進行了驗證。

行人行進的步長和方向是PDR系統(tǒng)中2個重要參數(shù)。其中步長的估計主要利用行進過程中產(chǎn)生的慣性參量信息，通過構建步長模型實現(xiàn)。常用的步長模型包括線性模型、非線性模型和機器學習模型等。線性模型利用步頻、加速度方差、加速度最大值和最小值等特征量估計步長[7]，不同的構建方法具有不同的精度。Weinberg[8]利用人體運動學特征，將垂直加速度的二次積分近似為腰部的垂向位移，建立了非線性步長模型。Wang等[9]利用基于長短時記憶模塊和去噪自編碼器的神經(jīng)網(wǎng)絡，估計不同環(huán)境中不同行走模式下的步長值，并利用手機內(nèi)置的慣性傳感器采集數(shù)據(jù)訓練網(wǎng)絡并檢驗步長估計精度。Tian等[10]將磁力計計算所得航向與角速度積分所得航向做差作為航向誤差觀測量，修正導航誤差參數(shù)。Borenstein等[11]利用行人直線行走時天向陀螺輸出理論值為零的約束條件，采用帶有PI控制器的閉環(huán)控制系統(tǒng)補償陀螺漂移，稱為啟發(fā)式陀螺漂移補償算法；之后，根據(jù)大型建筑物內(nèi)部都存在4～8個主方向的事實，將當前航向角與主方向角差值反饋至導航系統(tǒng)中，設計修正量對航向誤差進行補償，稱為啟發(fā)式航向漂移消除算法(Heuristic Drift Elimination, HDE)[12]，是目前主流的精度較高的航向誤差抑制方法之一。

但HDE方法應用范圍有限，如果行人不沿主方向行走或者航向在不斷變化，會出現(xiàn)過度修正或誤修正。本文在HDE算法的基礎上進行改進，提出了一種新的基于緩存區(qū)的啟發(fā)式航向補償算法。具體將N個復步作為一個緩存區(qū)，通過計算緩存區(qū)內(nèi)相鄰復步航向差值的方差，分段執(zhí)行航向修正并控制HDE算法的修正強度，避免過度修正。此外，本文還利用峰值檢測法結合三軸加速度矢量和信號對單步進行劃分，在此基礎上利用步頻和加速度方差信息構建線性步長模型，從而構建出完整的腰綁式PDR解決方案。為了驗證算法的有效性，在操場等環(huán)境開展了定位實驗，結果表明了所提方法的有效性。

1 基于合成加速度峰值檢測的單步劃分

準確的單步劃分是建立步長模型、計算步長的基礎。由于器件在綁定時存在姿態(tài)偏離，且腰部的運動幅度并沒有下肢劇烈。為增強加速度信號周期特征，依據(jù)文獻[13]，對三軸加速度求矢量和并取模值為

(1)

式中，ax(t)、ay(t)、az(t)為三軸加速度輸出。

圖1 單步劃分結果Fig.1 Single step division result

2 基于步頻和加速度方差信息的步長估計

PDR系統(tǒng)通過估計單步步長實現(xiàn)距離更新。為了降低步長計算的復雜度，采用文獻[14]中的線性模型估計單步步長為

S=a·fstep+b·υ+c

(2)

式中，S為單步步長，a、b、c為模型參數(shù)，fstep為步頻，υ為加速度方差，可以分別表示為

(3)

在實際應用中，模型參數(shù)的確定與行人的身高和體重等特征相關[15]。本文提取行人在不同行走頻率下采集到的步長和加速度方差等信息，利用最小二乘法對步頻-步長、加速度方差-步長分別進行擬合，得到結果如圖2和圖3所示。通過對擬合參數(shù)的加權平均確定該行人步長模型參數(shù)。

圖2 步頻-步長擬合結果圖Fig.2 Step frequency versus step length fitting result chart

圖3 加速度方差-步長擬合結果圖Fig.3 Acceleration variance versus step length fitting result chart

3 航向解算與修正

3.1 基于四元數(shù)法的航向角解算

航向解算精度是保證航位推算準確率的關鍵因素。捷聯(lián)慣性導航系統(tǒng)中常用的姿態(tài)更新算法有方向余弦法、歐拉角法和四元數(shù)法等。其中，四元數(shù)法計算量小、精度較高，因此選用該方法進行姿態(tài)更新并解算航向。四元數(shù)可以表示為

Q=q0+q1i+q2j+q3k

(4)

式中，q0、q1、q2、q3為實數(shù)，i、j、k為虛數(shù)單位。

利用四元數(shù)表示的姿態(tài)變換矩陣為

(5)

利用畢卡逼近法由角增量求解四元數(shù)。更新完四元數(shù)后，結合歐拉角表示的姿態(tài)變換矩陣計算航向

(6)

3.2 HDE算法原理

由于MEMS IMU采用的低精度陀螺儀輸出存在較大的隨機漂移，導致慣導解算的航向角隨時間發(fā)散，因此必須要引入航向誤差抑制措施。HDE算法基于行人主要沿主航向行進的假設，將計算所得航向角與最近主航向之間的差值反饋至導航系統(tǒng)對航向角進行修正。

一般設定0°、90°、180°、270°為4個主航向，間隔角度Δ為90°。首先，將前一步航向角ψi-1映射到[0,Δ]區(qū)間中，并將映射結果與Δ/2作差得到控制量為

(7)

式中，MOD(ψi-1,Δ)表示對ψi-1/Δ取余數(shù)。Ei的正負代表與主航向相比航向角向左偏或向右偏。當Ei大于0時，表明前一步航向較主航向左偏，因此角速度需要加上1個修正量；反之則表明前一步航向較主航向右偏，角速度需要減去1個修正量，從而使解算航向角向主航向靠近。所以以控制量Ei作為輸入，設計一個二態(tài)積分控制器，即

Ii=Ii-1+sgn(Ei)·ic

(8)

式中，Ii為反饋量，ic為角速度修正量，與陀螺儀性能有關，這里設為0.005，sgn(Ei)為符號函數(shù)，定義為

(9)

將反饋量作用于角速度進行修正，即

ωi=ωout,i+Ii

(10)

式中，ωout,i為陀螺儀輸出角速度，ωi為修正后的角速度。結合3.1節(jié)航向角解算方法，可以進一步求得修正后的航向角。

3.3 基于緩存區(qū)的啟發(fā)式航向補償算法

在搶險救災和單兵作戰(zhàn)等復雜環(huán)境下，行人行進過程中除沿主航向直行外，還存在許多大轉角轉向和連續(xù)小角度轉向等情況。而HDE算法是基于行人沿主航向行進的假設，通過減小當前航向與主航向之間的差值進行修正，因此在轉彎或小角度轉向時，HDE算法會對航向角的正常變化產(chǎn)生抑制作用，造成過度修正的問題。同時在直線行進時，腰部對于身體晃動造成的航向角突變敏感，因此當遇到異常擺動或避障時，由于每步都進行一次修正，可能導致累積的Ii遠大于ic，使得后續(xù)修正失效。所以本文引入航向緩存區(qū)改進HDE算法的不足。

首先，為解決連續(xù)轉彎時HDE算法存在過度修正的問題，分析相鄰2個復步間航向角差值與轉彎之間的關系。取沿操場環(huán)形跑道行走的慣性數(shù)據(jù)進行航向解算。其中，282步之前為轉彎區(qū)域，之后為直行區(qū)域。利用式(11)求解相鄰步間航向角差值的方差為

(11)

(12)

圖4給出了航向角解算和方差計算結果。可以發(fā)現(xiàn)，在航向角連續(xù)變化時相鄰步間航向角差值的方差較大，而直線行走時航向角差值的方差較小。受該方差特征啟示，將每N個復步視為一個緩存區(qū)，計算在緩存區(qū)內(nèi)航向角差值的方差。該方差特征用于改變HDE算法的修正強度，即相當于引入一個分段式的滑動窗，依據(jù)滑動窗內(nèi)數(shù)據(jù)的方差特征動態(tài)調(diào)整修正量，公式表示為

(13)

式中，i′c為改進后的角速度修正量。

圖4 連續(xù)轉彎時航向角與相鄰步航向角差方差示意圖Fig.4 Heading angle and variance of adjacent steps heading angle differences during continuous turning

從式(13)可以看出，該算法在行人處于轉彎狀態(tài)時，可以減弱HDE算法的修正強度；當行人處于直行狀態(tài)時，HDE算法的修正強度基本不變。其中，計算方差的緩存區(qū)長度N的選取與行人運動速度有關，當行人勻速行走時，N的經(jīng)驗值為5，即此時利用每5步內(nèi)的航向角差值方差特征來修正航向具有較高的精度。

改進HDE算法最終的修正角速度為

(14)

同時，為避免每步執(zhí)行修正可能導致HDE算法失效的問題，將緩存區(qū)作為最小單位，在每個緩存區(qū)內(nèi)的航向角僅執(zhí)行一次修正。由于慣導系統(tǒng)短時精度高，因此在檢測到大角度轉彎時，即δψ(n)大于設定的閾值thδψ時，禁用修正算法且重置緩存區(qū)。

4 實驗驗證

為了驗證所設計PDR導航方案的有效性，選用荷蘭Xsens公司生產(chǎn)的MTi-G-710系列MEMS IMU作為實驗器材。表1給出了傳感器相關參數(shù)指標。

表1 MTi-G-710參數(shù)指標

將傳感器綁定于行人腰側面進行實驗。實驗軌跡分為矩形軌跡和操場軌跡兩種。緩存區(qū)大小設為5個復步，即N取值為5。在矩形軌跡實驗中，令行人從起點出發(fā)，沿著室外廣場的磚縫線行走1個矩形，再回到原點，路徑總長為180m。在操場軌跡實驗中，令行人沿著操場跑道線以均勻的步速行走1圈，全長為400m。圖5和圖6分別給出了兩種軌跡的定位實驗結果。其中，藍色虛線為純慣導解算航向角的軌跡圖，綠色虛線為使用HDE算法修正航向角的軌跡圖，紅色實線為使用所設計的基于緩存區(qū)的啟發(fā)式航向補償算法修正航向角的軌跡圖。

圖5 矩形軌跡定位結果圖Fig.5 Positioning results of rectangular trajectory

圖6 操場軌跡定位結果圖Fig.6 Positioning results of playground trajectory

從圖中可以看出，在矩形軌跡實驗中，不進行任何修正的慣導解算方法精度最低，而利用HDE算法和本文算法所得到的軌跡最貼近真實運動軌跡，說明HDE算法對沿主航向直線行走的航向角具有較好的誤差抑制作用。定義終點與起點位置誤差占行進總距離的百分比為終點定位誤差，則在矩形軌跡實驗中，所設計算法起點與終點的距離為1.09m，終點定位誤差為0.6%。在操場實驗中，由于存在連續(xù)轉彎的2個半圓路徑，所以HDE算法會對航向角誤修正，導致定位軌跡與真實路徑偏差較大。而基于緩存區(qū)的改進HDE算法會根據(jù)路徑特點改變修正量，所以最終的定位軌跡與真實軌跡基本重合，起點與終點的距離為2.12m，終點定位誤差為0.5%。

為了更加直觀地體現(xiàn)所設計算法對航向角的修正效果，圖7給出了操場實驗中使用不同方法得到的航向角。定義直行區(qū)航向角誤差為直行路段(即實際航向角為0°的路段)解算航向與實際航向差值的絕對值，可以看出無修正的慣導解算所得航向角在中期直行路段出現(xiàn)較大偏移，最大直行區(qū)航向角誤差達到28°；HDE算法雖然在直行路段保證了航向角基本維持不變，但在轉彎路段出現(xiàn)了錯誤修正，導致后續(xù)直行區(qū)航向角誤差接近40°；相較于無修正方法和HDE方法，所設計算法在直行和連續(xù)轉彎場景下對航向的修正效果都較為理想，最大直行區(qū)航向角誤差在7°以內(nèi)。

圖7 操場實驗中不同方法所得航向角Fig.7 Heading angles obtained by different methods in playground experiment

5 結論

針對腰綁式行人導航系統(tǒng)，本文設計了一套完整的PDR解決方案，并提出了一種基于緩存區(qū)的啟發(fā)式航向補償算法用于修正航向角。算法分析與實驗結果表明：

1)腰綁式行人導航系統(tǒng)采集的加速度矢量和信號具有明顯的峰值特性，可以用峰值檢測方法準確劃分出單步。在此基礎上，構建線性步長模型準確估計步長。

2)基于緩存區(qū)的啟發(fā)式航向補償算法本質(zhì)上是利用相鄰航向角差值的方差對行人行走方式進行區(qū)分，根據(jù)不同的場景確定HDE算法的修正強度。

3)本文所提出的改進的HDE算法基于行人行進特征對航向角進行修正，如果行人運動狀態(tài)特別復雜，還可以與磁力計解算所得航向角等外部參考信息進行融合，設計全運動模式下的誤差抑制方法。