劉 宇，周 帆，李云梅，路永樂(lè)，彭 慧，張 欣

（1. 重慶郵電大學(xué) 光電信息感測(cè)與傳輸技術(shù)重慶市重點(diǎn)實(shí)驗(yàn)室，重慶 400065；2. 中國(guó)電子科技集團(tuán)第二十六研究所，重慶 400065）

基于人體多方位運(yùn)動(dòng)的三維自主導(dǎo)航定位算法

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

（1. 重慶郵電大學(xué) 光電信息感測(cè)與傳輸技術(shù)重慶市重點(diǎn)實(shí)驗(yàn)室，重慶 400065；2. 中國(guó)電子科技集團(tuán)第二十六研究所，重慶 400065）

基于MEMS傳感器的行人導(dǎo)航通常利用行人航跡推算算法解算出人體位置的坐標(biāo)對(duì)行人定位。傳統(tǒng)行人航跡推算算法只能用于單一前進(jìn)行走運(yùn)動(dòng)模式，不適用于人體實(shí)際的多方位運(yùn)動(dòng)模式。因此，提出了一種多方位運(yùn)動(dòng)三維自主導(dǎo)航定位算法，利用三軸加速度計(jì)數(shù)據(jù)對(duì)行人進(jìn)行步態(tài)檢測(cè)和步長(zhǎng)估計(jì)，在分析了加速度計(jì)單軸數(shù)據(jù)的基礎(chǔ)上，討論了4類運(yùn)動(dòng)模式的識(shí)別方法，推導(dǎo)出適用于人體多方位運(yùn)動(dòng)模式的航跡推算公式。在智能手機(jī)平臺(tái)上進(jìn)行測(cè)試驗(yàn)證，實(shí)驗(yàn)結(jié)果表明，所提出的多方位運(yùn)動(dòng)模式自主導(dǎo)航定位算法在人體實(shí)際運(yùn)動(dòng)中，定位誤差小于3.2%，很大程度上提高了導(dǎo)航精度。

MEMS傳感器；三維自主導(dǎo)航；航跡推算；多方位

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

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

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

1 系統(tǒng)介紹

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

多方位運(yùn)動(dòng)三維自主導(dǎo)航定位算法的系統(tǒng)架構(gòu)如圖1所示。首先，對(duì)三軸加速度計(jì)、三軸磁力、計(jì)三軸陀螺儀和氣壓計(jì)進(jìn)行數(shù)據(jù)預(yù)處理；然后，通過(guò)數(shù)據(jù)融合得到行人每一步的航向，利用加速度數(shù)據(jù)檢測(cè)行人的步態(tài)和步長(zhǎng)。使用峰值檢測(cè)法與相位反轉(zhuǎn)法區(qū)分前進(jìn)行走、后退行走、左橫向行走，右橫向行走4類行走運(yùn)動(dòng)模式，氣壓計(jì)用來(lái)解算高度；最后，通過(guò)本文提出的多方位運(yùn)動(dòng)三維定位算法得到行人的坐標(biāo)進(jìn)行定位。

圖1 系統(tǒng)框架Fig.1 System architecture

2 導(dǎo)航算法

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

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

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

2.1 步態(tài)檢測(cè)

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

圖3 步態(tài)判別Fig.3 Detection of step

2.2 步長(zhǎng)估計(jì)

步長(zhǎng)估計(jì)是影響定位精度的關(guān)鍵因素，因此步長(zhǎng)估計(jì)成為航跡推算算法的研究重點(diǎn)。步長(zhǎng)估計(jì)的算法有線性模型、人體運(yùn)動(dòng)模型、經(jīng)驗(yàn)?zāi)Ｐ?、神?jīng)網(wǎng)絡(luò)模型等[11]。

本文使用一種常用且精度相對(duì)較高的線性模型來(lái)進(jìn)行步長(zhǎng)估計(jì)，影響行人步長(zhǎng)的因素主要是步頻和行人身高，因此本文使用了行人身高與步頻來(lái)計(jì)算行人步長(zhǎng)，計(jì)算公式為

式中，h為行人的身高，fstep為行人行走的步頻，a、b、c為線性模型的校準(zhǔn)系數(shù)，通過(guò)大量的訓(xùn)練數(shù)據(jù)可以得到校準(zhǔn)系數(shù)a、b、c的值。

2.3 高度解算

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

式中：Xk為第k個(gè)均值，M為時(shí)間窗的長(zhǎng)度，wi為權(quán)值；iP為t時(shí)刻的原始?xì)鈮褐担琍(t)為t時(shí)刻濾波后的氣壓值，單位為Pa；Xmid為5個(gè)均值的中值。

氣壓計(jì)測(cè)量的氣壓值經(jīng)過(guò)濾波，然后利用式(5)把氣壓值轉(zhuǎn)換成高度，直接用于人體高度的判別。

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

3 多方位運(yùn)動(dòng)航跡推算

3.1 縱向航跡推算

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

如圖4所示為二維運(yùn)動(dòng)模型，行人從A點(diǎn)前進(jìn)行走到B點(diǎn)，再?gòu)腂點(diǎn)后退行走到C點(diǎn)，這個(gè)過(guò)程有兩次跨步且航向不變。如果使用傳統(tǒng)的航跡推算算法，行人的位置會(huì)在D點(diǎn)，這與實(shí)際運(yùn)動(dòng)軌跡相反，這種情況下傳統(tǒng)的航跡推算算法不再適用。

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

式(6)適用于前進(jìn)行走和后退行走運(yùn)動(dòng)模式的航跡推算法，Lk為每一步的步長(zhǎng)，θ為每一步的航向，Vk為高度差。Sk為運(yùn)動(dòng)模式標(biāo)志，當(dāng)前進(jìn)行走時(shí)Sk=1，后退行走時(shí)Sk=-1，其他運(yùn)動(dòng)模式Sk=0。

3.2 橫向航跡推算

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

如圖5所示為二維運(yùn)動(dòng)模型，當(dāng)行人從A點(diǎn)前進(jìn)行走到B點(diǎn)，再?gòu)腂點(diǎn)左橫向行走到C點(diǎn)，這個(gè)過(guò)程航向不變，E(k)減少了Lkcosθk，N(k)增加了Lksinθk；或行人從A點(diǎn)前進(jìn)行走到B點(diǎn)，再?gòu)腂點(diǎn)右橫向行走到C′點(diǎn)時(shí)，航向同樣不變，E(k)增加了Lkcosθk，N(k)減少了Lksinθk。如果使用傳統(tǒng)的航跡推算算法，行人的位置會(huì)在D點(diǎn)，這與實(shí)際軌跡垂直，傳統(tǒng)的航跡推算算法也不再適用。

當(dāng)只考慮橫向行走時(shí)，航跡推算算法改為

式中：Lk為每一步的步長(zhǎng)，θ為每一步的航向，Vk為高度差；Uk為運(yùn)動(dòng)模式標(biāo)志，左橫向行走時(shí)Uk=-1，右橫向行走時(shí)Uk=1，其他運(yùn)動(dòng)模式Uk=0。

3.3 多方位運(yùn)動(dòng)航跡推算

行人實(shí)際運(yùn)動(dòng)是包括前進(jìn)行走、后退行走、左橫向行走和右橫向行走的多方位混合運(yùn)動(dòng)模式，本文提出的多方位運(yùn)動(dòng)航跡推算算法為

式中：Lk為每一步的步長(zhǎng)，θ為每一步的航向，Vk為高度差，Sk為前、后行走運(yùn)動(dòng)模式標(biāo)志，Uk為左、右橫向行走運(yùn)動(dòng)模式標(biāo)志。前進(jìn)行走時(shí)，Sk=1，Uk=0；后退行走時(shí)，Sk=-1，Uk=0；左橫向行走時(shí)Sk=0，Uk=-1；右橫向行走時(shí)Sk=0，Uk=1；其他運(yùn)動(dòng)模式，Sk=0，Uk=0。Sk和Uk的值可以由方位識(shí)別確定。

3.4 方位識(shí)別

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

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

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

4 實(shí)驗(yàn)結(jié)果

本文選用重慶郵電大學(xué)標(biāo)準(zhǔn)足球場(chǎng)為測(cè)試地點(diǎn)，進(jìn)行多方位運(yùn)動(dòng)的二維自主導(dǎo)航定位驗(yàn)證，行人手持智能手機(jī)圍繞足球場(chǎng)行走一圈，即起點(diǎn)和終點(diǎn)重合。如圖7所示，測(cè)試路線為，從A點(diǎn)前進(jìn)行走到B點(diǎn)，接著從B點(diǎn)右橫向行走到C點(diǎn)，然后從C點(diǎn)后退行走到D點(diǎn)，最后從D點(diǎn)左橫向行走到A點(diǎn)。這時(shí)，可由行人的起點(diǎn)與終點(diǎn)的距離誤差作為判斷定位精度的標(biāo)準(zhǔn)，起點(diǎn)與終點(diǎn)的距離越小，就證明系統(tǒng)的定位精度越高。

圖7 測(cè)試場(chǎng)景-足球場(chǎng)Fig.7 Test scenarios

圖 8(a)是通過(guò)傳統(tǒng)的航跡推算算法得到的軌跡圖，圖8(b)是通過(guò)本文提出的多方位運(yùn)動(dòng)自主導(dǎo)航定位算法軌跡圖，可見(jiàn)，本文提出的多方位運(yùn)動(dòng)自主導(dǎo)航定位算法在多方位運(yùn)動(dòng)切換行走過(guò)程得到更可靠的定位信息。

選用重慶郵電大學(xué)第三教學(xué)樓進(jìn)行三維定位測(cè)試，第三教學(xué)樓共5層，矩形走廊的長(zhǎng)度為62 m，寬度為43 m。測(cè)試過(guò)程中行人從起點(diǎn)依次在每層樓行走，最終回到起點(diǎn)。通過(guò)對(duì)來(lái)自10個(gè)人的10組數(shù)據(jù)進(jìn)行統(tǒng)計(jì)，如表1所示，本文提出的算法定位誤差在3.2%以內(nèi)，基本滿足行人多方位運(yùn)動(dòng)三維自主導(dǎo)航需求。

圖8 傳統(tǒng)定位算法及本文改進(jìn)算法定位結(jié)果對(duì)比Fig.8 Comparison on positioning results between traditional algorithm and improved algorithm

表1 測(cè)試結(jié)果Tab.1 Tracking results

5 結(jié) 論

針對(duì)傳統(tǒng)行人航跡推算算法只能適用于單一前進(jìn)行走運(yùn)動(dòng)模式的問(wèn)題，分析了行人多方位運(yùn)動(dòng)的特征，提出了多方位運(yùn)動(dòng)模式三維自主導(dǎo)航算法。不僅提高了混合運(yùn)動(dòng)模式下步態(tài)檢測(cè)的正確率，而且能夠區(qū)分4個(gè)方位的運(yùn)動(dòng)模式。大大提高了定位的精度，達(dá)到 96.8%以上。因此，本文所提出的多方位運(yùn)動(dòng)模式三維自主導(dǎo)航定位算法可以真正有效地應(yīng)用到人類實(shí)際的生活中，具有較好的應(yīng)用價(jià)值。

（References）：

[1] 姬振龍. 低成本組合導(dǎo)航系統(tǒng)研究[D]. 上海: 上海交通大學(xué), 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òng)作分類和步數(shù)檢[D].天津: 天津大學(xué), 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] 劉宇, 江宏毅, 王仕亮, 等. 基于加速度時(shí)域特征的實(shí)時(shí)人體行為模式識(shí)別[J]. 上海交通大學(xué)學(xué)報(bào), 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] 錢偉行, 曾慶化, 萬(wàn)駿煒, 等. 基于人體下肢運(yùn)動(dòng)學(xué)機(jī)理的行人導(dǎo)航方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 陳興秀, 張金藝, 晏理, 等. 三維復(fù)雜運(yùn)動(dòng)模式航跡推算慣性導(dǎo)航室內(nèi)定位[J]. 應(yīng)用科學(xué)學(xué)報(bào), 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

國(guó)家自然科學(xué)基金資助項(xiàng)目（51175535）；國(guó)際聯(lián)合研究中心科技平臺(tái)與基地建設(shè)（cstc2014gjhz0038）；重慶市基礎(chǔ)與前沿研究計(jì)劃項(xiàng)目（cstc2015jcyjBX0068）

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

1005-6734(2016)04-0449-05

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