基于AHDE和手機陀螺儀的行人航向修正方法

郭 英，劉寒爍，葉 瑾，盛方園

（山東科技大學 測繪與空間信息學院，青島266590）

近年來，位置服務成為了人們?nèi)粘Ｉ钪斜夭豢缮俚姆?，全球衛(wèi)星導航系統(tǒng)(Global Navigation Satellite System,GNSS)在位置服務中占據(jù)著重要的位置，然而導航衛(wèi)星信號的可用性在室內(nèi)不能得到保證，GNSS 在室內(nèi)環(huán)境中的性能嚴重下降。因此，建立一個準確、可靠、實時的室內(nèi)定位系統(tǒng)，滿足公眾的室內(nèi)定位需求具有重要意義[1]。

隨著微機電技術(shù)(Micro-Electro-Mechanical Systems,MEMS)的不斷發(fā)展，慣性測量單元(Inertial Measurement Units,IMU)的體積越來越小，價格越來越便宜，并且隨著智能手機的普及和改進，IMU 被普遍應用于智能手機中，使基于智能手機的行人導航定位成為室內(nèi)定位領域的一個重要新分支[2]。

然而普通的IMU 均存在一定程度的累積誤差影響，為了在誤差影響下提高行人航向估算精度，國內(nèi)外學者提出了一些航向修正方法：Borenstein 等人[3]提出了基于陀螺儀航向估計的啟發(fā)式漂移消除算法（Heuristic Drift Elimination,HDE），該方法利用室內(nèi)環(huán)境大多為矩形，走廊一般為直線型的特性，預先設定好四個主導方向，當檢測到行人靠近主導方向行走時，航向偏差會由HDE 算法來修正；Jimenez 等人[4]提出了改進的啟發(fā)式漂移消除算法（improved Heuristic Drift Elimination,iHDE），該方法在HDE 算法原有的主導方向上再加入四個，每個主導方向相隔45 °，相較于HDE 算法，iHDE 算法在非理想的室內(nèi)環(huán)境中的表現(xiàn)要優(yōu)于HDE 算法[5]。為了進一步提高航向估算精度，一些基于固定式IMU 的航向修正算法被提出。Muhammad 等人[6]提出了骨盆旋轉(zhuǎn)-零速率更新旋轉(zhuǎn)檢測算法（Pelvic Rotation-ZUPT Turn Detection,PZTD），該算法基于腰部穿戴式慣性傳感設備，檢測行人是否直行和處于主導方向，對行人沿主導方向直行和未在主導方向直行兩種情況進行航向修正，不僅對主導方向的航向修正也提高了非主導方向上的航向精度。王鵬宇等人[7]基于iHDE 算法，使用腰綁式慣性傳感器，對航向建立緩沖區(qū)來抑制iHED 算法在特定場景下的過度修正問題。Ju 等人[12]提出了基于足綁式 IMU 的高級啟發(fā)式漂移消除算法（Advanced Heuristic Drift Elimination,AHDE），該算法利用行人連續(xù)六步的位置數(shù)據(jù)判斷是否直線行走或者沿主導方向直線行走。相比iHDE 算法，AHDE 加入了非主導方向直行的航向修正。在實際中，手機相較于固定式IMU 更加靈活，一些基于固定式IMU 的航向修正算法在手機端難以實現(xiàn)；室內(nèi)環(huán)境復雜，非主導方向直行的情況較多，iHDE 算法難以發(fā)揮優(yōu)勢；室內(nèi)直行的路線長短不一，AHDE 算法使用連續(xù)多步數(shù)據(jù)檢測是否直行會直接影響計算效率和修正效果。

基于此，本文首先分析基于智能手機框架的互補濾波航向推算原理，然后根據(jù)一秒時域內(nèi)航向變化幅度判斷行人是否直行，針對直線行走和主導方向下直線行走建立航向修正算法，并且對行人停頓和姿態(tài)轉(zhuǎn)換進行識別與處理，最后實驗驗證該方法在常規(guī)和復雜環(huán)境中的有效性。

1 基于互補濾波的航向推算基本原理

描述姿態(tài)常用的方法有歐拉角法、四元數(shù)法、羅德里格斯參數(shù)、方向余弦法等。四元數(shù)法相較于歐拉角法計算量大但計算精度更高[9]。而相較于羅德里格斯參數(shù)和方向余弦法，四元數(shù)法計算量小、簡單高效，并且可以全姿態(tài)求解。而且四元數(shù)法避免了產(chǎn)生的奇點的問題[14]。因此這里使用四元數(shù)法來描述姿態(tài)，根據(jù)式(1)將初始歐拉角轉(zhuǎn)換為四元數(shù)。

為了減弱陀螺儀漂移誤差，提升航向推算精度，這里使用互補濾波融合陀螺儀和加速度計數(shù)據(jù)[10]。即通過四元數(shù)計算得到載體坐標系下的重力矢量其中為重力場在導航坐標系下的投影，單位為m/s2，為導航坐標系轉(zhuǎn)換為載體坐標系的旋轉(zhuǎn)矩陣；加速度計數(shù)據(jù)計算得到載體本身的重力矢量；實際上，也就是陀螺儀數(shù)據(jù)在一定程度上存在偏差，將上述兩矢量叉乘得到偏差矢量e：

根據(jù)矢量偏差e及控制參數(shù)修正陀螺儀偏差：

式中，kp為比例控制參數(shù)，ki為積分控制參數(shù)，ω為三軸陀螺儀數(shù)據(jù)。根據(jù)二階龍格庫塔法使用修正后的陀螺儀數(shù)據(jù)更新的四元數(shù)，并轉(zhuǎn)換為姿態(tài)角。

2 基于高級啟發(fā)式漂移消除算法（AHDE）航向修正方法

由于航向漂移誤差會隨著行走距離的增加不斷增大，且行人在走路的過程中以直線行走居多，故有必要加入一定的航向修正算法。本文利用AHDE 算法中將行人行走狀態(tài)分為非直線、直線、沿主導方向直線行走的思想建立航向修正算法。

2.1 AHDE 算法原理及分析

AHDE算法是由Ju等人[12]提出的高級HDE算法，該算法通過判別行人行走的方式將行人行走分為三種模式，即非直線行走、直線行走和沿著主導方向直線行走，改進了iHDE 算法缺少非主導方向直線行走時修正航向的問題[11]。在非直線行走時AHDE 算法不生效，誤差與基于足綁式慣性單元 PDR 算法框架（INS-EKF-ZUPT,IEZ）一致[8]；在直線行走時，將當前行進航向與五步前的航向作差，使用該差值作為擴展卡爾曼濾波（Extended Kalman Filter,EKF）的測量值估計航向；在沿著主導方向直線行走時，將當前航向與主導航向的差值作為EKF 的測量值估計航向。

相比較于HDE 算法，AHDE 算法不僅能在簡單的主導方向上起到明顯效果，而且能在復雜的路線上起到一定作用。而AHDE 算法應用于IEZ 框架中，在手機端不適用；AHDE 算法在判定行走狀態(tài)時用到連續(xù)六步位置數(shù)據(jù)，由于步數(shù)較大，航向修正將會延遲[6]；同時，AHDE 算法使用五步的位置數(shù)據(jù)估算直線行走的航向也會引起不必要的誤差。而且，AHDE算法未考慮到主導方向偏離時過度修正問題。

2.2 航向修正算法改進

考慮到手機陀螺儀自身誤差，這里以互補濾波推算得到的航向為輸入值，按照時域分析航向角的變化情況判斷是否直行；對直線行走的航向加入直線修正估算行人航向，而非使用第一步的航向值作為直線行走的航向；沿主導方向直線行走時加入直線和主導方向修正算法估算行人航向，將直線修正后的航向與主導方向的差值作為輸入量估計行人航向；對非直線行走航向加入航向抖動檢測及消弱算法，避免手機抖動過大引起的誤判；最后對停頓點和姿態(tài)轉(zhuǎn)換進行識別與處理，提高行人在復雜室內(nèi)環(huán)境中的航向精度。

2.2.1 行走模式識別

傳統(tǒng)的AHDE 算法中，確定直線行走所需要的步數(shù)相當大，導致檢測滯后，引起不必要的延遲和誤差。室內(nèi)環(huán)境下直行路線長短不一，可能軌跡的末端是非主導方向，或者直行步數(shù)達不到檢測所需步數(shù)，這種情況下航向修正效果可能更差。為了避免這種情況，通過式(4)計算航向變化幅度，通過記錄航向和1 s 內(nèi)航向變化幅度確定是否直行。

式中，m為1 s 窗口大?。ㄔ谶@里m=50），為歷元i下的航向角，為歷元i下的航向變化幅度。一些學者分析行人在走路的過程中盆腔最大旋轉(zhuǎn)的幅度在4 °到9 °之間。在行人前進的過程中手持的手機會隨之產(chǎn)生一定程度的抖動，如果按照最大旋轉(zhuǎn)幅度來考慮行人在行走過程中的抖動情況，可以認為一步的航向變化在9 °內(nèi)。

為了能更加準確的判別行人的行走模式，這里通過實驗來驗證行人行走時的航向變化幅度。實驗地點為山東科技大學J6 樓，實驗中使用的智能手機為OPPO R9s，該智能手機的慣性傳感器為博世公司的BMI160，該傳感器的部分參數(shù)見表1。

表1 BMI 160 傳感器技術(shù)參數(shù)Tab.1 Specifications of the BMI 160

在實驗中，我們招募了5 名志愿者覆蓋了不同的年齡身高以及性別，其中包括男性A，175 cm/70 kg/50歲；男性 B，180 cm/90 kg/29 歲；女性 C，174 cm/63 kg/26 歲；男性D，167 cm/56 kg/16 歲；女性E，163 cm/58 kg/50 歲。志愿者被要求在40 m-100 m的直線中行走，傳感器采樣頻率為50 Hz，實驗中將手機平放在胸前，保持導航模式行走。為了直觀展示航向變化幅度的分布情況，該部分數(shù)據(jù)以箱型圖的形式展示，如圖1所示，其中A1 和B1 為志愿者A 和B非勻速行走數(shù)據(jù)。在箱型圖中，中心標記為數(shù)據(jù)中值，箱的上下端分別表示第25%和75%位的數(shù)據(jù)，“+”表示異常值。

圖1 行人航向變化幅度箱型圖Fig.1 The box-plot of change of pedestrians heading

從圖1 和表2 可以看出，勻速行走下的航向變化幅度平均集中在3 °以下，即便是最大異常值仍在4 °以內(nèi)；非勻速行走的數(shù)據(jù)是為了模擬直線行走過程中手機不平穩(wěn)的情況，平均最大異常值在4.5 °左右，在非勻速A1 實驗中超出了4.5 °，雖然在實驗處理中將非連續(xù)性超出閾值的異常數(shù)據(jù)認作噪音去除掉，但也說明過度的航向抖動可能會引起誤判，為減弱這一情況的影響，后續(xù)的航向修正會針對誤判情況進行修正。根據(jù)盆腔旋轉(zhuǎn)幅度以及實際直線行走的結(jié)果可以得出合適的判斷直行的閾值DTh。

表2 行人航向變化幅度數(shù)據(jù)表（單位：（°））Tab.2 Table of pedestrian course change amplitude data（Unit：（°））

2.2.2 直線行走航向修正

傳統(tǒng)的直線航向修正是在判斷行人連續(xù)幾步的航向變化不大的情況下，將后續(xù)的幾步航向角與判斷為直線行走起的航向保持一致，直至脫離直線行走。傳統(tǒng)方法判定為直線行走時行人的航向是保持不變的（即，直到脫離直線行走），這與實際并不相符（行人直線行走時航向會小幅度波動）。為了降低直線行走過程中航向偏差，以及考慮到閾值判斷時產(chǎn)生的誤判情況，公式(5)將當前航向與直線方向的偏差作為輸入量對直線行走模式下的航向偏差分等級修正，其修正示意圖見圖2。

圖2 直線行走下航向修正對比圖Fig.2 Comparison chart of heading correction when walking straight

其中小角度偏差被完全減弱，大角度偏差保留一定原有特性，若超出直線行走的正常航向變化范圍，結(jié)束直線修正。這樣不僅修正直線行走航向偏差而且降低誤判造成的過度修正影響。

2.2.3 主導方向航向修正

主導方向是事先建立好的方向，如圖3所示，當行人沿著主導方向直線行走時，航向被主導方向修正算法修正。

圖3 主導方向示意圖Fig.3 The chart of dominant direction

圖3 將主導方向定為8 個，行人直線行走時的航向在主導方向附近時會被識別算法檢測，式(6)[6]為主導方向判別公式。

當前航向與主導航向偏差量ic為：

通過以上公式進行兩圈矩形100 m 行走測試，如圖4所示?；パa濾波計算得到的航向偏差會隨著時間不斷變大，圖中行走狀態(tài)為1 則行人轉(zhuǎn)彎；為0 則行人直行，可以看出行走模式能夠被正確判斷，航向偏差能夠被檢測并且被修正算法補償。

圖4 主導方向直線行走航向修正對比圖Fig.4 Heading correction comparison chart for straight walk along the dominant directions

2.2.4 航向抖動識別與減弱

考慮到在修正航向角的過程中可能因各種影響造成航向抖動導致直線行走未被正確識別，通過加權(quán)平均模型建立航向抖動減弱公式(9)，該公式使用非直線行走下連續(xù)三步的航向數(shù)據(jù)來判斷航向抖動情況，即連續(xù)三步航向相互作差，將抖動情況分為三類并分別修正，其修正示意圖見圖5。

圖5 航向抖動優(yōu)化示意圖Fig.5 The chart of heading jitter optimization

行人步頻使用傳統(tǒng)的峰值檢測法計算，步幅模型為非線性模型中的Weinberg 方法[13]，最后結(jié)合步長和航向信息推算行人航跡。

2.2.5 停頓點識別與處理

為了解決行人在行走的過程中遇到障礙物，或者因其他原因在原地停頓而產(chǎn)生位置誤差的問題，在推算行人航向的同時加入了停頓點檢測。

由于行人在某一位置停頓時加速度計數(shù)據(jù)趨于穩(wěn)定，因此選擇建立加速度數(shù)據(jù)方差識別行人的停頓位置。

圖6(a)展示了手機在平放狀態(tài)下行人停頓和行走時的加速度計數(shù)據(jù)，停頓狀態(tài)下明顯穩(wěn)定于行走狀態(tài)，從圖6(b)可以看出根據(jù)其3 軸加速度計計算的窗口方差，在停頓區(qū)域的方差值十分穩(wěn)定，相互配合可以比較準確地確定出停頓點的位置。

圖6 停頓檢測示意圖Fig.6 The chart of pause detection

為此通過研究建立了行人停頓時的檢測識別方法，如下式：

由于舍去停頓區(qū)間分段建立行走路線，需要匹配停頓區(qū)間前后的航向，選取停頓前短時間內(nèi)未發(fā)生大幅度變化的四元數(shù)的均值，使用該四元數(shù)來推算下一段路線的行人航向。

2.2.6 手機姿態(tài)轉(zhuǎn)換識別與處理

圖7 不同手機位置下加速度數(shù)據(jù)對比圖Fig.7 The chart of acceleration data in different mobile phone position

手機平放查閱及打電話模式之間的轉(zhuǎn)換為手機導航常見的姿態(tài)轉(zhuǎn)換，所以這里主要研究行人在行走的過程中手機的手持姿態(tài)和打電話姿態(tài)轉(zhuǎn)換的情況。

由于手機的平放姿態(tài)和打電話狀態(tài)時的傾斜角度完全不同，因此三軸加速度計的大小就會根據(jù)姿態(tài)轉(zhuǎn)換發(fā)生變化，如圖7所示，接打電話狀態(tài)和手持平放狀態(tài)下的加速度計數(shù)據(jù)有著顯著的區(qū)別，根據(jù)這種變化判斷是否在打電話。

在姿態(tài)轉(zhuǎn)換過程結(jié)束以后的三軸加速度計和陀螺儀數(shù)據(jù)難以計算真實的行走航向和步長，為了得到真實的航向數(shù)據(jù)和步長數(shù)據(jù)，采用旋轉(zhuǎn)矩陣的方法進行數(shù)據(jù)轉(zhuǎn)換，將加速度計數(shù)據(jù)和陀螺儀數(shù)據(jù)恢復到手機平放狀態(tài)下。公式如下：

式中，φ為在轉(zhuǎn)化到打電話前一時刻的手機航向，ψ和β是在打電話狀態(tài)下的手機俯仰角和橫滾角，建立得到的即為在行人打電話狀態(tài)下相對于手機平放狀態(tài)的旋轉(zhuǎn)矩陣，'g為修正后的三軸陀螺儀數(shù)據(jù)，g為原始三軸陀螺儀數(shù)據(jù)。通過旋轉(zhuǎn)矩陣將打電話狀態(tài)下的加速度計和陀螺儀數(shù)據(jù)轉(zhuǎn)換到手機導航坐標系下，進而推算真實行人航向。

3 實驗分析

為了驗證本文提出算法的有效性與可靠性，針對不同的行走情況采集了多種路線下的數(shù)據(jù)進行對比，其中包括了常規(guī)性路線，以及針對特定情況的復雜性實驗，對實驗數(shù)據(jù)分別使用改進的航向修正算法以及互補濾波算法。使用閉合差、點位誤差、航向誤差綜合比較實驗的精度，最后對各組實驗路線全長相對閉合差進行比較。

3.1 常規(guī)實驗

常規(guī)性實驗包括96 m 矩形行走路線實驗和主導方向偏離實驗，實驗過程中均保持手機平放在胸前，該部分實驗可以直觀地反映出航向修正算法在主導方向直線行走時的航向糾正能力，以及偏離主導方向時的避免修正過度能力。

在矩形行走路線中，行人在主導方向直線行走，通過改進的航向修正算法修正后的航向以及路線都有著明顯的精度提升，如圖8所示。航向修正后的平均航向誤差為0.1 °，相較于PDR 互補濾波算法航向精度提高97%，其相對閉合差為0.1%，精度提升94%（見表3 和表5）。

主導方向偏離誤差測試實驗旨在檢測主導方向修正算法是否會過度修正主導方向邊緣的航向。實驗路線為先直線行走，經(jīng)過兩個90 °轉(zhuǎn)彎，然后轉(zhuǎn)向靠近主導方向的方向直行回到原點。由于前半段沿著主導方向行走，航向偏差能夠被正確補償，后半段沿著主導方向邊緣行走，航向修正算法被干擾，如圖9所示，但航向修正算法精度仍優(yōu)于PDR 互補濾波算法。其中航向修正算法的平均航向誤差為0.7 °，相對閉合差為0.2%D，精度提升71%（見表3 和表5）。

圖9 主導方向偏離實驗對比圖Fig.9 The comparison chart of dominant direction deviation experiment

表3 航向誤差對比表Tab.3 The comparison table of heading error

3.2 停頓實驗

停頓實驗包含三角形行走實驗，以及含有曲線的矩形行走實驗，實驗過程中手機平放于胸前，兩個實驗中均包含了停頓點。

在三角形行走實驗中，無論是否沿主導方向直行均能發(fā)揮出航向修正算法的優(yōu)勢，如圖10(a)所示，并且點位精度優(yōu)于互補濾波算法，如圖10(b)所示。航向修正算法的平均點位誤差為0.1 m，精度提升77%；相對閉合差為0.7%D，較與未修正航向路線精度提升71%（見表4 和表5）。

帶有曲線的停頓實驗中包含了直線和曲線行走，體現(xiàn)航向修正算法在含有曲線行走的路線中同樣有效，避免因識別錯誤或者過度修正而導致嚴重誤差。實驗中PDR 互補濾波算法的路線產(chǎn)生了偏移，如圖11(a)所示。航向修正后的路線平均點位誤差為0.03 m，精度提升95%；相對閉合差為0.7%D，精度提升22%（見表4 和表5）。

未加入停頓識別處理算法的路線均在停頓后出現(xiàn)較大的位置偏差，其中三角形路線閉合差為4.39 m，帶有圓形路線閉合差為1.95 m。

圖10 帶有停頓點的三角形路線對比分析圖Fig.10 The comparison chart of triangular route with paused point

圖11 帶有停頓點和曲線路線的實驗路線對比分析圖Fig.11 The comparison chart of experiment route with paused point and curve route

表4 點位誤差對比表Tab.4 The comparison table of point error

3.3 姿態(tài)轉(zhuǎn)換實驗

該部分實驗包含了姿態(tài)轉(zhuǎn)換和停頓檢測的內(nèi)容，路線為多個轉(zhuǎn)彎的行走路線，整個過程先從手持平放狀態(tài)轉(zhuǎn)換為打電話狀態(tài)，中途停頓三次，再轉(zhuǎn)換為手持平放狀態(tài)直到終點，路線對比圖如圖12所示。

未加入停頓識別算法和姿態(tài)轉(zhuǎn)換算法的解算路線出現(xiàn)了嚴重的誤差；由于路線較短、手機抖動較小等原因PDR 互補濾波算法也起到了良好的修正作用，但仍然有一定程度的航偏。經(jīng)過航向修正的路線相對閉合差為0.7%D，相較于PDR 互補濾波算法提升了60%。姿態(tài)轉(zhuǎn)換實驗中未處理停頓點的路線閉合差為5.9 m，未處理姿態(tài)轉(zhuǎn)換和停頓點的路線閉合差為21.7 m。

為了更直觀地比較航向修正算法對位置精度的影響，表5 對比了上述實驗各種路線的閉合差以及路線 全長相對閉合差。兩種算法均加入姿態(tài)轉(zhuǎn)換處理與停頓處理，因而不包含停頓識別和姿態(tài)轉(zhuǎn)換帶來的誤差。

圖12 帶有姿態(tài)轉(zhuǎn)換和停頓點的實驗路線對比圖Fig.12 The comparison chart of experiment route with attitude change and paused point

表5 各實驗位置精度分析表Tab.5 Position accuracy analysis table for each experiment

4 結(jié)論

本文分析當前航向修正算法HDE、iHDE 以及AHDE 的特點和優(yōu)勢，在此基礎上建立改進的航向修正算法，針對航向漂移、航向抖動、行人停頓和姿態(tài)轉(zhuǎn)換引起的誤差，分別建立識別和修正模型。結(jié)合多種行走路線以及行人行走狀態(tài)實驗，結(jié)果表明：

(1)互補濾波算法在短距離內(nèi)能夠起到一定的效果，但其航向誤差會隨著距離的增加不斷增加，而航向修正算法能夠一定程度消除陀螺儀漂移偏差，在本文實驗中，不同的環(huán)境中的航向修正后路線相對閉合差范圍為0.1%～0.7%，相較于互補濾波算法精度提升范圍為22%～94%，平均提升68%。

(2)停頓識別與消除算法能夠有效處理行人停頓問題，本文的實驗中加入該算法的行人停頓行走路線閉合差精度提高范圍為80%～96%。

(3)綜合路線包含了航向修正、停頓識別及處理和姿態(tài)轉(zhuǎn)換內(nèi)容，本文綜合路線閉合差精度提高范圍為50%～98%。實驗表明本文研究內(nèi)容在三種方法相互配合的復雜行走狀態(tài)路線下保持良好的精度和穩(wěn)定性。

本文研究的算法在主導方向邊緣時修正能力會受到影響，航向修正算法的優(yōu)勢受到限制，接下來的研究會考慮改進主導方向邊緣修正精度以及加入磁力計來提升定位精度。