一種基于行人航位推算的改進航跡跟蹤算法

胡 銳 吳 飛 通雁輝 朱 海 鄢 松 陸雯霞

1 上海工程技術大學電子電氣工程學院，上海市龍騰路333號，201620

室外環境中的行人導航研究以衛星導航或全球導航衛星系統(GNSS)/慣性導航(INS)組合導航為主，近年取得了較好的成果[1]。對于行人定位導航技術的研究主要分為兩類：一類是利用外部部署設備定位，主要基于室內超寬帶(UWB)、Wi-Fi(wireless fidelity)及藍牙等無線射頻設備；另一類則是以慣性傳感器定位，主要基于慣性導航技術等。目前，已有較多針對步數估計[2-9]、步長估計[10-12]及航向角估計[13-16]等方面的研究，并取得了不錯的進展。

為提高行人在有磁場干擾環境中軌跡跟蹤的準確性，本文在步數估計階段采用雙重閾值二值化算法提高計步的準確性；同時在航向估計階段，利用自適應融合算法對加速度數據、角速度數據及磁力計數據融合得出更精準的航向；最后設計在有磁干擾和無磁干擾環境中進行對比實驗，證明本文算法在航跡跟蹤上的有效性。

1 行人位置推算

行人航位推算時通過計步檢測、步長估計、航向估計3個步驟完成用戶位置的更新，并據此繪制出移動軌跡。在計步檢測中，通常利用三軸加速度信息檢測步數，當檢測到用戶移動1步時，步長估計和航向估計模塊發生作用，開始進行位置的更新，其過程見圖1。假設初始時刻為t0，行人的初始位置為p0(e0，n0)，沿北偏東α0的方向移動，在t1時刻行人的運動距離為S0(步長)，此時行人的位置為p1(e1，n1)。同理可得，在t2時刻行人的位置為p2(e2，n2)，距離為S1(步長)。

圖1 PDR位置更新Fig.1 PDR location update

推廣到一般情況，根據t時刻的步長St和行進方向αt，計算t+1時刻行人的位置為：

(1)

式中，nt為t時刻行人在全局坐標系中的橫坐標，et為t時刻行人在全局坐標系中的縱坐標。

由于傳統的計步檢測過程中會出現偽峰谷值的影響，因此提出利用雙重閾值二值化算法避免誤判；傳統的航向估計精度不高，本文采用自適應融合算法進行航向估計。

2 計步檢測

傳統的波峰檢測和閾值檢測算法均無法處理行人在行走過程中出現的抖動情況，表現在加速度數據上的特征是出現偽波峰或偽波谷。本文提出利用雙重閾值二值化算法進行計步檢測，可精準計步，并處理出現偽波峰或偽波谷的情況。利用改進的步數算法進行計步檢測的步驟為：

1)求加速度模值。通過手機加速度傳感器獲取三軸加速度(圖2)，再利用三軸加速度信息求加速度模值，其公式為：

圖2 沿矩形路徑行走的三軸加速度Fig.2 Triaxial acceleration along rectangular path

(2)

式中，ax、ay、az為三軸加速度。

2)低通濾波。利用四階的低通濾波器對加速度模值進行濾波，正常人勻速行走的頻率為每秒2步，變速行走平均為每秒3步，截止頻率可設置為2 Hz或3 Hz，濾波前后的數據見圖3。

圖3 低通濾波后的加速度Fig.3 Low-pass filtered acceleration

3)去除重力。將濾波之后的加速度模值去除重力的影響：

acc_detrend=acc_mag_filt-g

(3)

式中，acc_detrend為去除重力影響的加速度，acc_mag_filt為濾波后的加速度，g為重力加速度(取9.8 m/s2)。

4)雙重閾值二值化。由于傳統閾值方法對偽峰谷值的判斷不準確，因此設計兩重閾值細化加速度信息，其上限根據去除重力后加速度模值的最大值設置，下限根據行走多組實驗取平均得出。同時，為從細化的加速度信息中提取峰谷值，采用二值化方法進行計算。雙重閾值二值化流程見圖4，二值化后的加速度為：

圖4 雙重閾值二值化Fig.4 Double threshold binarization

(4)

(5)

式中，low_acc和up_acc分別為閾值下限和上限，B1i、B2i分別為第i個樣本在二值化后的加速度，B1i-1為前一個樣本二值化后的值。

5)計步。需同時滿足從高加速度到低加速度，如B1i從1到0，且在1個時間窗口內加速度出現0，即可記為1步。

3 航向估計

3.1 基于歐拉角的坐標變換

由于手機在行走過程中會不斷發生移動，加速度傳感器采集的數據因手機姿態的不斷變化很難同前一時刻保持一致，因此采用空間坐標系轉換的方法將數據從機體坐標系(body frame, b)映射至靜態的導航坐標系(navigation frame，n)。機體坐標系如圖5所示。

圖5 機體坐標系Fig.5 Body frame

在初始時刻，機體坐標系xbybzb與導航坐標系xnynzn是重合的，行走過程中設備載體的姿態會隨行人的運動發生改變。導航坐標系可經過航向角(機體繞z軸旋轉)、橫滾角(機體繞y軸旋轉)及俯仰角(機體繞x軸旋轉)變化后得到機體坐標系，該旋轉過程可表示為：

(6)

式中，θ、φ、φ分別為航向角、橫滾角及俯仰角。

3.2 改進的航跡推算算法描述

針對航向估計準確性低的問題，傳統算法利用陀螺儀積分得到角度，但積分會使角度誤差隨時間不斷累積，導致航向錯誤，同時周圍磁場也在發生變化，使得方向估計更加困難。因此，本文先將加速度計及陀螺儀數據進行融合，再根據磁感應強度閾值確定是否需要融合磁力計數據，具體流程見圖6。

圖6 融合框架Fig.6 Fusion framework

具體算法步驟如下。

1)根據加速度求取角度。根據加速度傳感器采集的信息初始化手機的姿態信息，默認初始狀態的yaw(航向角)為0°，roll(橫滾角)和pitch(俯仰角)為：

(7)

2)坐標轉換。根據§3.1的描述，得出機體坐標系轉換到導航坐標系下的初始旋轉矩陣。

3)由微分方程確定旋轉矩陣，進行姿態解算。對姿態的解算本質上是求旋轉矩陣，由于姿態隨時間變化，在時間t處姿態的變化率為：

(8)

式中，δt為陀螺儀的周期。式(8)可利用極限性質進行簡化：

(9)

(10)

式中，δΨ為t到t+δt時間段內的旋轉矩陣，δθ為該時間段內微小的偏航角，δφ為該時間段內微小的橫滾角，δφ為該時間段內微小的俯仰角。

(11)

(12)

式中，Ω(t)為陀螺儀角速率矩陣，t為陀螺儀周期開始時刻，ωbx(t)、ωby(t)、ωbz(t)分別為三軸角速率。由式(9)～(12)可得：

(13)

陀螺儀實際上是不提供連續信號ωb(t)的，而是通常以固定頻率提供角速度的樣本，必須使用積分方案來積分采樣信號。因此式(13)可改寫為：

(14)

由式(14)的微分方程可解得旋轉矩陣為：

(15)

式中，C(0)由步驟1)中加速度初始化的旋轉矩陣確定，因此更新過程中融合了加速度信息。

4)根據旋轉矩陣求航向角。由步驟3)求得的旋轉矩陣可以得出加速度融合陀螺儀的航向角為：

(16)

式中，C21為旋轉矩陣2行1列處的元素，同理C11為矩陣中1行1列處的元素。

5)根據磁力計數據求航向角。由磁力計估計的航向角為：

ψmag=

(17)

式中，hx、hy、hz為磁力計測得的三軸磁感應強度，D為當地磁偏角。

6)融合策略。根據是否受磁場干擾確定加速度計、陀螺儀及磁力計的權重分配，其中有、無磁場干擾是根據傳感器采集的磁感應強度模值決定的，大于閾值時認為有磁場干擾，反之則認為無磁場干擾。本文采取的策略為：如果受到磁場干擾，則磁力計的權重小，加速度計和陀螺儀融合結果權重大；如果沒有受到磁場干擾，則磁力計權重大，加速度數據和陀螺儀融合結果權重小，以適應有磁場干擾的環境。

4 實驗與結果分析

4.1 實驗條件及數據采集

為對本文提出的改進計步算法和航向估計算法在航跡跟蹤上的效果進行驗證，設計多組實驗，并讓行人按照標定好的矩形路徑行走，比較航跡的誤差。

實驗的采集設備為華為P20手機，加速度計、陀螺儀和磁力計數據的采集利用LOPSI實驗室(http://car.upm-csic.es/lopsi)開發的GetSensorData2.0應用程序，數據采集頻率均為50 Hz，采集數據時右手持手機放在胸前。

4.2 步數估計實驗

在計步檢測實驗中，采用本文提出的雙重閾值二值化算法進行計步，閾值上限根據去除重力影響后加速度模值的最大值設置為2.2 m/s2，下限根據加速度模值的最小值設置為0.4 m/s2，實驗過程中行人可以不同速度行走，以檢驗算法的魯棒性。以直線行走和沿矩形路徑行走為例，分析加速度變化特性并可視化，如圖7所示。可以看出，相鄰的波峰與波谷之間為1步，但由于行人在運動過程中的擺動可能出現偽波峰或偽波谷，采用二值化的方式并結合時間約束可避免誤判。從圖7(b)中可清晰地看出，在5 200～5 300 ms之間加速度模值出現偽波峰谷，二值化算法在該時段沒有計步。

圖7 步數可視化Fig.7 Visualization of the number of steps

為驗證算法的有效性，通過計步準確性衡量指標分析本文算法與文獻[8]中動態閾值算法在計步檢測上的表現，結果見表1。根據行人在行走過程中速度的變化將實驗分為勻速運動和變速運動，行人在勻速行走時兩種算法的計步準確率都在95%以上；行人在變速行走時，基于動態閾值波峰檢測算法的計步準確率明顯低于本文提出的雙閾值二值化算法。由此可知，雙閾值二值化算法可避免傳統方法遇到的偽峰谷值的影響，進而提高計步的準確率，同時本文算法對行人的行走速度要求不高，更具有普適性。

表1 步數判斷結果Tab.1 Step judgment result

4.3 航向估計實驗

由于地球表面任一點的地球磁場強度矢量方向與水平面存在夾角，在航向估計中需要移除本地磁偏角(5°)。為驗證自適應融合算法在航向估計中的準確性，設計行人沿著矩形路徑行走，通過平均角度誤差將本文改進的航向估計算法同傳統算法以及文獻[17]中的多傳感器融合算法進行比對。圖8展示了行人沿矩形路徑行走37步中角度的變化情況，其中自適應融合算法即為本文算法。由圖可知，本文算法和文獻[17]算法均在第20步時判斷發生第1次轉彎，但傳統算法在行走到第18步時判斷發生第1次轉彎，從參考航向來看，第1次轉彎發生在第20步，因此傳統算法的轉彎識別錯誤。在發生第1次轉彎過程中3種算法的精度各不相同，本文算法在第1次轉彎處與參考角度之間的誤差為5.7°，文獻[17]算法的誤差為8.6°，傳統算法的誤差為11.4°；在第2次轉彎時，傳統算法判斷在第20步時發生第2次轉彎，而參考航向證明發生第2次轉彎是在第21步，傳統算法轉彎識別錯誤，本文算法與文獻[17]算法均在第21步時識別轉彎，文獻[17]算法與參考角度之間的誤差為12.5°，本文算法的誤差為5.4°。從圖中還可看出，在行走到第24步時3種算法均判斷發生第3次轉彎，即3種算法在第3次轉彎識別上的判斷上是一致的，傳統算法與參考角度之間的誤差為5.7°，文獻[17]算法的誤差為2.5°，本文算法的誤差為2.7°。為量化誤差及衡量算法的準確性，利用平均角度誤差衡量在3次轉彎過程中的角度誤差，由于傳統算法未能正確識別第1次和第2次轉彎，所以在平均角度誤差衡量過程中不考慮傳統算法。本文算法的平均角度誤差為4.5°，文獻[17]算法的平均角度誤差為6.8°。綜上所述，本文算法在航向估計中較文獻[17]算法的角度誤差更小，更有利于航跡跟蹤時準確捕獲航向變化。

圖8 航向估計結果Fig.8 Heading estimation result

4.4 航跡跟蹤實驗

為得到更精確的位置和航向信息，實驗通過計算最終定位結果與起始點之間的誤差來比較算法的優劣。將磁感應強度閾值設置為30 μT，以鑒別是否存在磁場干擾；實驗設計行人分別在有磁場干擾和無磁場干擾的環境中沿矩形路徑行走，有磁場干擾的實驗場地選在有鐵質和鋼質及無線通信設備的環境中，無磁場干擾實驗場則是在空曠環境中，盡量屏蔽手機和鋼鐵帶來的影響。矩形路徑長為11 m，寬為5 m，其中步長估計采用文獻[10]中的經典Weinberg步長模型，計步檢測和航向軌跡利用本文的雙重閾值二值化和自適應融合算法，并將本文算法與傳統PDR算法及PDR+Mahony融合算法進行對比，圖9為在不同場景下對行人運動軌跡的跟蹤情況。

圖9 行人軌跡跟蹤Fig.9 Pedestrian tracking

從圖9可以看出，在無磁場的環境下，傳統算法和本文算法對行人軌跡的跟蹤總體一致；但在有磁場環境中，傳統算法未能對磁場干擾進行有效排除，因此最終的定位結果為(-0.88 m，0.22 m)，與起始點的距離為0.91 m，對于32 m的行走距離而言誤差為2.8%；PDR+Mahony融合算法在航跡上的跟蹤結果為(-0.23 m, 0.12 m)，與起始點的距離為0.26 m，誤差為1.4%；而本文通過自適應融合算法，根據磁場強弱分配權重，最終的航跡跟蹤定位結果為(-0.15 m, 0.14 m)，與起始點的距離為0.21 m，誤差為1.1%，精度分別提高了55%和25%。選取參考路徑中的部分實驗點作為誤差分析參考點，將行人的二維定位坐標與對應參考點之間的歐氏距離定義為定位誤差，其計算公式為：

(18)

圖10為在有磁場干擾和無磁場干擾下根據定位誤差繪制的累積概率分布，可以看出，本文算法定位誤差小于0.75 m的概率為59%，PDR+Mahony融合算法定位誤差小于0.75 m的概率為55%，傳統算法定位誤差小于0.75m的概率為22%；本文算法在定位誤差小于0～1.0 m之間的概率均大于或等于PDR+Mahony融合算法；3種算法在無磁場干擾情況下的定位誤差大致一致。

圖10 不同環境下的定位誤差累積概率分布Fig.10 Probability distribution of positioning errors in different environments

為了說明在不同環境下各算法的平均定位誤差，分別讓行人沿矩形路徑行走32 m、64 m及96 m，其余實驗變量與行走32 m的相同，表2～4為在不同環境中的平均定位誤差統計。從3個表中均可以看出，在有磁場干擾環境下，本文算法的定位誤差更小；在無磁場干擾環境下，3種算法的性能大體一致。由此可知，本文算法對于航跡的跟蹤更貼近真實軌跡，同時適應在有磁場干擾環境中的跟蹤，并在定位精度方面有所提高。

表2 行走32 m平均定位誤差統計Tab.2 Statistics of average positioning error of 32 m walking

表3 行走64 m平均定位誤差統計Tab.3 Statistics of average positioning error of 64 m walking

表4 行走96 m平均定位誤差統計Tab.4 Statistics of average positioning error of 96 m walking

5 結 語

本文提出一種基于行人航位推算的改進航跡跟蹤算法，該算法利用雙重閾值二值化算法對加速度數據進行處理，避免偽峰谷值的影響，并利用自適應融合算法對行人航向進行準確估計，最終推算出行人的運動軌跡。實驗結果表明，本文算法在32 m矩形行走實驗中定位誤差為1.1%，能夠有效減少誤差積累，抑制航向發散，提高導航精度，使導航復現路線更加貼合真實行走軌跡。