王鵬宇，孟之棟，鄧志紅

(北京理工大學(xué)自動(dòng)化學(xué)院，北京 100081)

0 引言

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

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

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

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

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

(1)

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

圖1 單步劃分結(jié)果Fig.1 Single step division result

2 基于步頻和加速度方差信息的步長(zhǎng)估計(jì)

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

S=a·fstep+b·υ+c

(2)

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

(3)

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

圖2 步頻-步長(zhǎng)擬合結(jié)果圖Fig.2 Step frequency versus step length fitting result chart

圖3 加速度方差-步長(zhǎng)擬合結(jié)果圖Fig.3 Acceleration variance versus step length fitting result chart

3 航向解算與修正

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

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

Q=q0+q1i+q2j+q3k

(4)

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

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

(5)

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

(6)

3.2 HDE算法原理

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

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

(7)

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

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

(8)

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

(9)

將反饋量作用于角速度進(jìn)行修正，即

ωi=ωout,i+Ii

(10)

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

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

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

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

(11)

(12)

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

(13)

式中，i′c為改進(jìn)后的角速度修正量。

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

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

改進(jìn)HDE算法最終的修正角速度為

(14)

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

4 實(shí)驗(yàn)驗(yàn)證

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

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

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

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

圖6 操場(chǎng)軌跡定位結(jié)果圖Fig.6 Positioning results of playground trajectory

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

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

圖7 操場(chǎng)實(shí)驗(yàn)中不同方法所得航向角Fig.7 Heading angles obtained by different methods in playground experiment

5 結(jié)論

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

1)腰綁式行人導(dǎo)航系統(tǒng)采集的加速度矢量和信號(hào)具有明顯的峰值特性，可以用峰值檢測(cè)方法準(zhǔn)確劃分出單步。在此基礎(chǔ)上，構(gòu)建線(xiàn)性步長(zhǎng)模型準(zhǔn)確估計(jì)步長(zhǎng)。

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

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