基于人體多方位運動的三維自主導航定位算法

劉 宇，周 帆，李云梅，路永樂，彭 慧，張 欣

（1. 重慶郵電大學 光電信息感測與傳輸技術重慶市重點實驗室，重慶 400065；2. 中國電子科技集團第二十六研究所，重慶 400065）

基于人體多方位運動的三維自主導航定位算法

劉 宇1，周 帆1，李云梅1，路永樂1，彭 慧2，張 欣1

（1. 重慶郵電大學 光電信息感測與傳輸技術重慶市重點實驗室，重慶 400065；2. 中國電子科技集團第二十六研究所，重慶 400065）

基于MEMS傳感器的行人導航通常利用行人航跡推算算法解算出人體位置的坐標對行人定位。傳統行人航跡推算算法只能用于單一前進行走運動模式，不適用于人體實際的多方位運動模式。因此，提出了一種多方位運動三維自主導航定位算法，利用三軸加速度計數據對行人進行步態檢測和步長估計，在分析了加速度計單軸數據的基礎上，討論了4類運動模式的識別方法，推導出適用于人體多方位運動模式的航跡推算公式。在智能手機平臺上進行測試驗證，實驗結果表明，所提出的多方位運動模式自主導航定位算法在人體實際運動中，定位誤差小于3.2%，很大程度上提高了導航精度。

MEMS傳感器；三維自主導航；航跡推算；多方位

慣性系統不需要外部參考系、不向外部輻射能量，也不依賴于外部信息，是一種自主導航系統，可全時間地工作于空中、地球表面乃至水下。慣性系統能提供位置、速度、航向和姿態角信息，所產生的導航信息連續性好并且數據更新率高[1]。

很多提高行人導航的精度的研究，主要集中在傳感器校準及算法優化的角度。Nilsson研究了基于鞋式的行人導航系統，文章中采用了兩個慣性測量單元綁在腳上，雖然降低了算法復雜度，但是在硬件上產生了很大的開銷[2]。Fan Li采用粒子濾波對步長和航向進行估計，但是算法中需要有本地地圖對行人的行走軌跡進行限制，對于缺少室內地圖的情況則不適用[3]。Schindhelm采用慣性傳感器判別行人的運動模式，但只分析了前進行走運動模式[4]。李昊提出了前進行走和后退行走的方位識別，但相對人體實際的多方位運動模式仍不完善[5]。

人體實際運動包括4個方位的運動模式，前進行走、后退行走、左橫向行走和右橫向行走。如果不進行區分，將導致導航軌跡與實際相反或垂直，引起不可估量的誤差。為了解決這個問題，本文提出一種基于人體多方位運動的三維自主導航定位算法。

1 系統介紹

智能手機的底層硬件、通信能力和計算能力完全能夠滿足定位與導航需求。本系統采用華為智能手機作為行人自主導航系統的硬件測試平臺，該設備集成了多個傳感器，如加速度傳感器、磁力傳感器、陀螺儀、距離傳感器和氣壓計等。

多方位運動三維自主導航定位算法的系統架構如圖1所示。首先，對三軸加速度計、三軸磁力、計三軸陀螺儀和氣壓計進行數據預處理；然后，通過數據融合得到行人每一步的航向，利用加速度數據檢測行人的步態和步長。使用峰值檢測法與相位反轉法區分前進行走、后退行走、左橫向行走，右橫向行走4類行走運動模式，氣壓計用來解算高度；最后，通過本文提出的多方位運動三維定位算法得到行人的坐標進行定位。

圖1 系統框架Fig.1 System architecture

2 導航算法

式(1)是傳統的航跡推算算法，該算法利用航向、步長和高度計算出行人的坐標對行人定位，算法模型如圖2所示[6-7]。

式中，Lk為每一步的步長，θ為每一步的航向，Vk為高度差，E(k)、N(k)、H(k)分別表示東向、北向、高度。

圖2 航跡推算算法模型Fig.2 Model for PDR algorithm

2.1 步態檢測

加速度計原始值輸出的波形會產生多峰值現象，易造成計步誤判，不利于步態的判別，因此，需要對加速度信號做預處理[8]。對于原始數據的預處理，需要消除各種噪聲，并盡量多地保留原始數據的特征。通過濾波處理可以使加速度曲線更加平滑，更適合特征提取和分類[9]。本文采取的是漢明窗濾波，窗口大小為 5。濾波后加速度的數據呈規律性變化，每一個跨步對應一個峰值、一個零點和一個谷值，類似于一個正弦波。本文利用峰值檢測法來檢測步態，圖3是加速度計濾波后的波形圖，行人每一步的峰值都被標記了出來，從而驗證了該步態檢測算法的有效性[10]。

圖3 步態判別Fig.3 Detection of step

2.2 步長估計

步長估計是影響定位精度的關鍵因素，因此步長估計成為航跡推算算法的研究重點。步長估計的算法有線性模型、人體運動模型、經驗模型、神經網絡模型等[11]。

本文使用一種常用且精度相對較高的線性模型來進行步長估計，影響行人步長的因素主要是步頻和行人身高，因此本文使用了行人身高與步頻來計算行人步長，計算公式為

式中，h為行人的身高，fstep為行人行走的步頻，a、b、c為線性模型的校準系數，通過大量的訓練數據可以得到校準系數a、b、c的值。

2.3 高度解算

由于外界環境的影響使得在相同樓層大氣壓也會有跳變，造成求出的高度不穩定。因此需要對氣壓值濾波，首先利用式(3) 滑動加權平均法對氣壓計數據處理，然后用式(4)中值濾波算法濾波，最后把氣壓值轉換成高度。

式中：Xk為第k個均值，M為時間窗的長度，wi為權值；iP為t時刻的原始氣壓值，P(t)為t時刻濾波后的氣壓值，單位為Pa；Xmid為5個均值的中值。

氣壓計測量的氣壓值經過濾波，然后利用式(5)把氣壓值轉換成高度，直接用于人體高度的判別。

式中：H(t)為高度，單位為m；P(t)是t時刻濾波后的氣壓值，單位為Pa；P(0)是起始時刻的氣壓值，單位為Pa。

3 多方位運動航跡推算

3.1 縱向航跡推算

傳統的航跡推算算法存在的缺陷是，行人后退行走、左橫向行走及右橫向行走時與前進行走的航向一樣，定位軌跡一直向前更新，造成導航軌跡與實際相反或垂直，引起較大的誤差。

如圖4所示為二維運動模型，行人從A點前進行走到B點，再從B點后退行走到C點，這個過程有兩次跨步且航向不變。如果使用傳統的航跡推算算法，行人的位置會在D點，這與實際運動軌跡相反，這種情況下傳統的航跡推算算法不再適用。

圖4 航跡推算算法模型Fig.4 Model for PDR algorithm

式(6)適用于前進行走和后退行走運動模式的航跡推算法，Lk為每一步的步長，θ為每一步的航向，Vk為高度差。Sk為運動模式標志，當前進行走時Sk=1，后退行走時Sk=-1，其他運動模式Sk=0。

3.2 橫向航跡推算

圖5 航跡推算算法模型Fig.5 Model for PDR algorithm

如圖5所示為二維運動模型，當行人從A點前進行走到B點，再從B點左橫向行走到C點，這個過程航向不變，E(k)減少了Lkcosθk，N(k)增加了Lksinθk；或行人從A點前進行走到B點，再從B點右橫向行走到C′點時，航向同樣不變，E(k)增加了Lkcosθk，N(k)減少了Lksinθk。如果使用傳統的航跡推算算法，行人的位置會在D點，這與實際軌跡垂直，傳統的航跡推算算法也不再適用。

當只考慮橫向行走時，航跡推算算法改為

式中：Lk為每一步的步長，θ為每一步的航向，Vk為高度差；Uk為運動模式標志，左橫向行走時Uk=-1，右橫向行走時Uk=1，其他運動模式Uk=0。

3.3 多方位運動航跡推算

行人實際運動是包括前進行走、后退行走、左橫向行走和右橫向行走的多方位混合運動模式，本文提出的多方位運動航跡推算算法為

式中：Lk為每一步的步長，θ為每一步的航向，Vk為高度差，Sk為前、后行走運動模式標志，Uk為左、右橫向行走運動模式標志。前進行走時，Sk=1，Uk=0；后退行走時，Sk=-1，Uk=0；左橫向行走時Sk=0，Uk=-1；右橫向行走時Sk=0，Uk=1；其他運動模式，Sk=0，Uk=0。Sk和Uk的值可以由方位識別確定。

3.4 方位識別

通過實驗對比分析，當人體前進行走和后退行走時，加速度Y軸正弦波相位正好相差180°；左橫向行走和右橫向行走時，加速度X軸正弦波相位正好相差180°，即加速度值總是沿著行走方向先增大后減小。于是提出了相位反轉法以區分4類運動模式，只需判斷加速度X軸或Y軸的正弦波相位是0°還是180°即可[12]。

行人行走時手機X軸向右，Y軸指向前進方向，Z軸向下。如圖6所示，前進和后退行走時Y軸的波形相位相差180°，加速度計Y軸輸出波形先增大后減小是前進行走，先減小后增大是后退行走。

圖6 Y軸波形Fig.6 Y-axis acceleration waveform

4 實驗結果

本文選用重慶郵電大學標準足球場為測試地點，進行多方位運動的二維自主導航定位驗證，行人手持智能手機圍繞足球場行走一圈，即起點和終點重合。如圖7所示，測試路線為，從A點前進行走到B點，接著從B點右橫向行走到C點，然后從C點后退行走到D點，最后從D點左橫向行走到A點。這時，可由行人的起點與終點的距離誤差作為判斷定位精度的標準，起點與終點的距離越小，就證明系統的定位精度越高。

圖7 測試場景-足球場Fig.7 Test scenarios

圖 8(a)是通過傳統的航跡推算算法得到的軌跡圖，圖8(b)是通過本文提出的多方位運動自主導航定位算法軌跡圖，可見，本文提出的多方位運動自主導航定位算法在多方位運動切換行走過程得到更可靠的定位信息。

選用重慶郵電大學第三教學樓進行三維定位測試，第三教學樓共5層，矩形走廊的長度為62 m，寬度為43 m。測試過程中行人從起點依次在每層樓行走，最終回到起點。通過對來自10個人的10組數據進行統計，如表1所示，本文提出的算法定位誤差在3.2%以內，基本滿足行人多方位運動三維自主導航需求。

圖8 傳統定位算法及本文改進算法定位結果對比Fig.8 Comparison on positioning results between traditional algorithm and improved algorithm

表1 測試結果Tab.1 Tracking results

5 結 論

針對傳統行人航跡推算算法只能適用于單一前進行走運動模式的問題，分析了行人多方位運動的特征，提出了多方位運動模式三維自主導航算法。不僅提高了混合運動模式下步態檢測的正確率，而且能夠區分4個方位的運動模式。大大提高了定位的精度，達到 96.8%以上。因此，本文所提出的多方位運動模式三維自主導航定位算法可以真正有效地應用到人類實際的生活中，具有較好的應用價值。

（References）：

[1] 姬振龍. 低成本組合導航系統研究[D]. 上海: 上海交通大學, 2010. Ji Zheng-long. Research of low-cost integrated navigation system[D]. Shanghai: Shanghai Jiao Tong University, 2010.

[2] Nilsson J O, Zachariah D, Skog I, et al. Cooperative localization by dual foot-mounted inertial sensors and inter-agent ranging[J]. EURASIP: Journal on Advances in Signal Processing, 2013, 2013(1): 1-17.

[3] Li F, Zhao C, Ding G, et al. A reliable and accurate indoor localization method using phone inertial sensors[C]// Proceedings of the 2012 ACM Conference on Ubiquitous Computing. 2012: 421-430.

[4] Schindhelm C K. Activity recognition and step detection with smartphones: Towards terminal based indoor positioning system[C]//2012 IEEE 23rd International Symposium on Personal Indoor and Mobile Radio Communications. 2012: 2454-2459.

[5] 李昊. 基于三軸加速度傳感器的動作分類和步數檢[D].天津: 天津大學, 2010. Li Hao. Study of activities classify and step detection base on the three-dimensional acceleration sensor[D]. Tianjin: Tianjin University, 2010.

[6] Levi R W, Judd T. Dead reckoning navigational system using accelerometer to measure foot impacts: U.S., 5583776[P]. 1996-12-10.

[7] Zhang H, Zhang J, Zhou D, et al. Axis-Exchanged compensation and gait parameters analysis for high accuracy indoor pedestrian dead reckoning[J]. Journal of Sensors, 2015, 2015(2): 1-13.

[8] Xu Yuan, Chen Xi-yuan, Li Qing-hua, et al. Indoor pedestrian navigation based on double-IMU framework [J]. Journal of Chinese Inertial Technology, 2015, 23(6): 714-717.

[9] 劉宇, 江宏毅, 王仕亮, 等. 基于加速度時域特征的實時人體行為模式識別[J]. 上海交通大學學報, 2015, 49(2): 169-172. Liu Yu, Jiang Hong-yi, Wang Shi-liang, et al. Real-time human activity pattern recognition based on time domain features of acceleration[J]. Journal of Shanghai Jiaotong University, 2015, 49(2): 169-172.

[10] Kang W, Han Y. SmartPDR: smartphone-based pedestrian dead reckoning for indoor localization[J]. IEEE Sensors Journal, 2015, 15(5): 2906-2916.

[11] 錢偉行, 曾慶化, 萬駿煒, 等. 基于人體下肢運動學機理的行人導航方法[J]. 中國慣性技術學報, 2015, 23(1): 24-28. Qian Wei-xing, Zeng Qing-hua, Wan Jun-wei, et al. Pedestrian navigation method based on kinematic mechanism of human lower limb[J]. Journal of Chinese Inertial Technology, 2015, 23(1): 24-28.

[12] 陳興秀, 張金藝, 晏理, 等. 三維復雜運動模式航跡推算慣性導航室內定位[J]. 應用科學學報, 2014, 32(4): 349-356. Chen Xing-xiu, Zhang Jin-yi, Yan Li, et al. Inertial indoor navigation with 3D complex motion mode of pedestrian dead reckoning[J]. Journal of Applied Sciences, 2014, 32(4): 349-356.

Three-dimensional autonomous navigation algorithm based on multi-azimuth movement pattern

LIU Yu1, ZHOU Fan1, LI Yun-mei1, LU Yong-le1, PENG Hui2, ZHANG Xin1
(1. Chongqing Municipal Level Key laboratory of Photoelectronic Information Sensing and Transmitting Technology, Chongqing University of Post and Telecommunications, Chongqing 400065, China; 2. The 26th Institute of China Electronics Technology Corporation, Chongqing 400065, China)

The pedestrian navigation based on MEMS sensor usually achieves real-time pedestrian positioning by using pedestrian dead reckoning (PDR) algorithm to calculate the coordinate of pedestrian’s position. Traditional PDR only considers forward movement pattern, which is not applicable to the actual multi-azimuth human movement pattern. Hence, a novel algorithm of three-dimensional autonomous navigation based on multi-azimuth movement pattern is proposed by using the data of three-axis accelerometer to implement step detection and stride-length estimation. Based on the analysis of the accelerometer data, a pattern recognition method for four types of movement patterns is discussed, and the dead reckoning equation applied to pedestrian multi-azimuth movement pattern is derived. Verification experiments are made on smartphone platform by using the proposed algorithm into the actual autonomous navigation based on multi-azimuth movement pattern, which show that the positioning error is less than 3.2%, significantly improving the navigation accuracy.

MEMS sensor; three-dimensional autonomous navigation; dead reckoning; multi-azimuth

U666.1

：A

2016-04-27；

：2016-06-30

國家自然科學基金資助項目（51175535）；國際聯合研究中心科技平臺與基地建設（cstc2014gjhz0038）；重慶市基礎與前沿研究計劃項目（cstc2015jcyjBX0068）

劉宇（1972—），男，碩士生導師，教授，主要從事慣性導航及傳感器件的研究。E-mail: mianfeiyangpian@163.com

1005-6734(2016)04-0449-05

10.13695/j.cnki.12-1222/o3.2016.04.006