曾慶化，曾世杰，劉建業(yè)，陳銳志，孟 騫，王敬賢

（1. 南京航空航天大學(xué) 導(dǎo)航研究中心，南京 210016；2. 衛(wèi)星通信與導(dǎo)航協(xié)同創(chuàng)新中心，南京 210016；3. 武漢大學(xué)測繪遙感信息工程國家重點實驗室，武漢 430079）

基于位置的服務(wù)（Location Based Service, LBS）可以根據(jù)人們不同需求，在任何時間、地點，確定用戶實際地理位置，為用戶提供與位置相關(guān)的信息與服務(wù)[1-2]。隨著智能手機的普及和功能的不斷完善，基于手機的行人導(dǎo)航定位技術(shù)成為導(dǎo)航領(lǐng)域中的一個重要的新興分支，可實時確定并監(jiān)測行人的位置與人體狀態(tài)，從而有效提高搶險搜救人員、特定功能行動的快速反應(yīng)能力與任務(wù)執(zhí)行效率，具有廣闊的應(yīng)用前景。行人手機導(dǎo)航已成為當(dāng)前導(dǎo)航領(lǐng)域研究熱點之一，國內(nèi)外各高校、研究所都在此領(lǐng)域進(jìn)行了深入的研究。2017年，美國商務(wù)部國家標(biāo)準(zhǔn)技術(shù)研究院（National Institute of Standards and Technology, NIST）舉辦了名為“PerfLoc”的大獎賽，為參賽者提供NIST的智能手機數(shù)據(jù)信息，便于參賽者挖掘智能手機在室內(nèi)定位導(dǎo)航應(yīng)用中的潛力。

室外環(huán)境下的行人導(dǎo)航研究以衛(wèi)星導(dǎo)航或者是GNSS/INS組合導(dǎo)航為主，近些年取得了較好的成果[3]。而室內(nèi)環(huán)境下導(dǎo)航研究，針對行人導(dǎo)航技術(shù)的研究主要集中于以下兩類：一類是基站式定位，主要基于室內(nèi)UWB、ZigBee、無線網(wǎng)絡(luò)、WiFi以及藍(lán)牙等無線射頻設(shè)備[4]；一類是慣性為主的傳感器定位方式，主要基于人體腳部，腿部、腰部、肩部等部位安裝慣性傳感器[5]，通過觀測上述部位的運動特征，研究零速修正（Zero Velocity Update, ZUPT）、步態(tài)識別等方法，實現(xiàn)基于慣性傳感器的行人導(dǎo)航。上述的兩類方法各有優(yōu)劣，前者需要事先在室內(nèi)布置諸如WiFi路由器、信標(biāo)基站等作為導(dǎo)航信息源，增加了成本，適用環(huán)境受局限，后者需要在人體上添置額外的傳感器設(shè)備，其性能精度受傳感器誤差影響較大。

除了單獨使用上述兩種導(dǎo)航定位方法外，組合導(dǎo)航也是室內(nèi)行人導(dǎo)航領(lǐng)域中常用的方式，如INS/UWB、INS/WiFi、INS/磁場匹配（Magnetic Matching, MM）[6-8]等。德國航空航天中心通信與導(dǎo)航研究所的研究人員提出了一種判斷步伐、姿態(tài)和步長的方法，優(yōu)化了行人慣性導(dǎo)航的結(jié)果[9]；西班牙自動化與機器人中心的研究人員提出了使用磁力計、陀螺儀和加速度計配合擴(kuò)展卡爾曼濾波的方式為行人航跡推算（Pedestrian Dead Reckoning, PDR）算法獲得更精確的航向信息[10]。上述的組合導(dǎo)航算法需要慣性傳感器以外的傳感器配合，且在陌生環(huán)境和建筑中，其導(dǎo)航效果會大打折扣。

近年來基于智能手機的行人導(dǎo)航定位技術(shù)發(fā)展迅速。南京航空航天大學(xué)的導(dǎo)航研究中心的研究人員基于智能手機設(shè)計并實現(xiàn)了室內(nèi)外的無縫導(dǎo)航系統(tǒng)[11]；愛丁堡大學(xué)的研究人員提出使用手機作為導(dǎo)航設(shè)備和WiFi指紋識別系統(tǒng)相結(jié)合的組合導(dǎo)航算法HiMLoc[12]；德國卡爾斯魯厄理工學(xué)院的研究人員提出混合定位概念，使用手機和UWB設(shè)備搭建了組合導(dǎo)航平臺[13]。在使用手機進(jìn)行純慣性導(dǎo)航定位的研究中，法國GEOLOC實驗室的研究人員設(shè)計了一種雜交濾波器用于標(biāo)定通常PDR方法中的行人步長[14]。韓國高等科學(xué)技術(shù)研究所的研究人員提出并搭建了一種基于手機的SmartPDR導(dǎo)航系統(tǒng)，能利用手機完成PDR導(dǎo)航解算，但是該系統(tǒng)要求手機的使用方式保持固定，不能有太大的移動幅度[15]，以減少不同手機手持方式對導(dǎo)航定位解算的影響。

將手機保持在一個固定的握持姿態(tài)，不僅給行人增加了使用困難，也限制了手機導(dǎo)航的適用性，降低了用戶體驗，因此迫切需要一種能自動判別手機姿態(tài)，并根據(jù)姿態(tài)變化自動修正航向的算法，同時需要提高航向補償性能，以提高純慣性手機導(dǎo)航的適用性和定位精度。為此，提出了一種新的航向補償算法，通過對MEMS慣性器件的輸出信息采集分析，判斷手機的使用方式和當(dāng)前姿態(tài)，并對行人使用手機導(dǎo)航時的航向誤差進(jìn)行補償。

1 行人航跡推算算法原理

自主性更強的慣性定位技術(shù)在無GNSS信號的區(qū)域具有巨大的發(fā)展?jié)摿?。傳統(tǒng)的捷聯(lián)慣導(dǎo)系統(tǒng)（Strapdown Inertial Navigation System, SINS）通過對加速度計和陀螺儀輸出進(jìn)行分析處理，得到導(dǎo)航坐標(biāo)系下的位置、速度和航向三維信息。但由于手機內(nèi) MEMS慣性器件精度低、穩(wěn)定性差，其航向和定位結(jié)果容易迅速發(fā)散。而傳統(tǒng)行人PDR導(dǎo)航方法是通過在某一時刻對行人行走的步數(shù)、步長和方向進(jìn)行測量和統(tǒng)計，推算出下一時刻行人行走軌跡和位置等信息，所以比SINS具有更高的定位精度，一般應(yīng)用于低成本下的導(dǎo)航定位研究。

傳統(tǒng)PDR包含三個重要內(nèi)容[16]：1）步伐檢測；2）步長估計；3）航向估計。

式(1)為PDR算法的遞推公式，E和N分別為東向和北向位置的坐標(biāo)，Sk、qk分別為tk-1到tk時刻行人從位置(Ek-1,Nk-1)到位置(Ek,Nk)的行走距離和方位角。

由式(1)可得行人在tk時刻東向和北向位置信息。

2 模擬零速修正算法

PDR方法的整體航位推算結(jié)果受低精度手機陀螺儀影響較大。足部或者車輛載體中應(yīng)用的 ZUPT算法是一種簡便、高效的慣性導(dǎo)航誤差補償方法，可有效減緩慣性導(dǎo)航誤差積累，提高慣性導(dǎo)航的定位精度[17]，已經(jīng)在車載導(dǎo)航和行人足部導(dǎo)航中獲得應(yīng)用。然而，在手機行人導(dǎo)航中，當(dāng)行人手持手機時，手機一般位于行人腰部以上的位置，不包含明顯的零速時刻，很難將傳統(tǒng)的ZUPT算法應(yīng)用進(jìn)來。因此，本文提出了一種在行人腳部落地時刻進(jìn)行卡爾曼濾波的模擬零速修正（Simulated Zero-Velocity Update,S-ZUPT）方法。

為了提高步伐檢測算法的準(zhǔn)確性，對三軸加速度計輸出的信號進(jìn)行如下預(yù)處理：

其中，A為加速度峰值閾值，tmin和tmax為時間間隔的區(qū)間值。行人的行走頻率一般在2 Hz到4 Hz之間，因此選擇的時間間隔為

對步伐檢測算法進(jìn)行多次實驗結(jié)果如表1所示，其中，單次實驗的準(zhǔn)確率基本能達(dá)到98%以上，平均準(zhǔn)確率為98.975%。

圖1所示為編號為1的實驗結(jié)果圖，圖中的三角形表示探測出的峰值點，一個三角形代表一步。

本文提出的S-ZUPT算法利用人體運動特點構(gòu)建步伐觀測模型，采用卡爾曼濾波器估計姿態(tài)和傳感器誤差，達(dá)到抑制航向角度誤差發(fā)散的目的。設(shè)置濾波系統(tǒng)的系統(tǒng)方程和觀測方程為：

表1 步伐檢測實驗結(jié)果Tab.1 Experiment results of step detection

圖1 步伐探測結(jié)果圖Fig.1 Step detection results

式中，F(xiàn)(t)為系統(tǒng)狀態(tài)矩陣，H(t)為系統(tǒng)觀測矩陣，G(t)為系統(tǒng)噪音驅(qū)動矩陣，w(t)為系統(tǒng)噪音，v(t)為觀測噪音。通過解算可得離散情況下k+1時刻的系統(tǒng)狀態(tài)估計方程和系統(tǒng)觀測方程。

系統(tǒng)狀態(tài)估計方程如下：

其中，時刻k為步伐檢測算法判斷出的步伐時刻，分別為該時刻下的橫滾角、俯仰角和航向角以及手機陀螺儀三個軸輸出的角速度值。

將行人航位推算系統(tǒng)獲得的水平姿態(tài)角度和航向角度以及實時陀螺輸出角速度值作為系統(tǒng)的觀測量可以獲得k+1時的觀測方程如下：

受限于行人行走的特點，航向角度輸出周期具有非一致性，在分析了行人步伐的周期特點基礎(chǔ)上，選擇了時變離散的航向角輸出周期，即：

經(jīng)由S-ZUPT算法濾波后的姿態(tài)角信息和角速度信息即為每個步伐時刻的最優(yōu)估計值。

3 基于重力的手機姿態(tài)判斷和航向補償方法

在日常生活的應(yīng)用中，手機常處于多種狀態(tài)，一般可以分為豎屏擺放、橫屏擺放以及通話模式等，如圖2所示。本文將手機使用的方式大致分為三種方式：豎屏姿態(tài)、橫屏姿態(tài)、通話姿態(tài)。

圖2 手機持握方式示意圖Fig.2 Ways to use smartphone

當(dāng)使用手機的姿態(tài)從圖2提及的姿態(tài)中相互切換時，手機都會有較大的位置和姿態(tài)變動，并且手機內(nèi)部傳感器一并記錄了輸出姿態(tài)變換過程中的加速度、角速度等信息。假如姿態(tài)切換過程中的角速度進(jìn)入了導(dǎo)航系統(tǒng)，這對依賴MEMS慣性器件的手機導(dǎo)航而言無疑是有害的，會誤導(dǎo)導(dǎo)航系統(tǒng)認(rèn)為是行人的航向發(fā)生了變化，從而給出錯誤的導(dǎo)航定位結(jié)果。在手機行人導(dǎo)航時，僅使用S-ZUPT算法依然無法補償由于手機使用方式改變造成的姿態(tài)角變化，因此本文根據(jù)行人對手機使用方式以及傳感器輸出數(shù)據(jù)分析，在實現(xiàn)S-ZUPT算法的基礎(chǔ)上，提出了一種重力輔助航向補償（Heading Compensation with Gravity Assisted,HCGA）方法對手機的航向進(jìn)行補償。

HCGA方法分為兩個部分，分別為重力球區(qū)域姿態(tài)判斷法和航向補償過程。

手機重力計探測到的重力數(shù)據(jù)具有以下特點：

式中，為重力加速度合矢量，分別為手機重力計輸出的各軸重力加速度矢量。

若將采集到的重力計數(shù)據(jù)投影到三維坐標(biāo)系中并且建立一個半徑大小為當(dāng)?shù)刂亓铀俣鹊那蛎?，便得到了如圖3所示的球面點云圖。圖中的I、II、III區(qū)域為分別豎屏姿態(tài)、橫屏姿態(tài)、通話姿態(tài)下重力數(shù)據(jù)在三維坐標(biāo)系中的投影圖。從圖中能夠發(fā)現(xiàn)不同姿態(tài)下手機重力計輸出數(shù)據(jù)在球面上的投影區(qū)域有明顯的區(qū)別，即通過計算重力計數(shù)據(jù)在球面坐標(biāo)系下的方位角和仰角便可區(qū)分手機當(dāng)前的姿態(tài)。

通過公式(10)得到三軸重力數(shù)據(jù)在球面坐標(biāo)系下的方位角和仰角：

圖3 球面點云圖Fig.3 Spherical point cloud

為了方便計算，本文中實驗以右手使用手機進(jìn)行實驗。在實際使用時，由于坐標(biāo)軸的對稱性，使用左手進(jìn)行實驗時只需將判斷的區(qū)域進(jìn)行對稱變換即可。

式(11)展示了不同姿態(tài)下方位角和仰角的范圍，其中，J為姿態(tài)標(biāo)識，1、2、3分別表示豎屏姿態(tài)、橫屏姿態(tài)、通話姿態(tài)三種姿態(tài)。

基于對行人使用手機方式同手機姿態(tài)之間關(guān)系的研究，本文提出了基于手機姿態(tài)對航向進(jìn)行補償?shù)姆椒?。本方法具體過程分為以下4步：

1）獲取手機MEMS器件的數(shù)據(jù)，并判斷步伐時刻；

2）實時監(jiān)控兩次步伐之間航向的改變值；

3）根據(jù)航向改變值大小決定是否進(jìn)行前后兩次步伐的手機姿態(tài)判斷；

4）對比前后兩次的手機姿態(tài)，根據(jù)對比結(jié)果對航向進(jìn)行修正。

4 實驗與分析

為了對本文提出的基于重力輔助和模擬零速修正的手機無約束航向修正方法進(jìn)行驗證，在南京航空航天大學(xué)自動化學(xué)院樓群進(jìn)行了室內(nèi)行走實驗。本次實驗所用設(shè)備為ONEPLUS A3000手機，設(shè)置采集傳感器信號的頻率為 50 Hz，攜帶方式為右手手持，豎屏模式下的手機頭部朝向行人行走方向。

實驗在自動化學(xué)院一號樓5樓走廊進(jìn)行。行走矩形路線45 m′53 m，在行走時依次使用了圖2中的三種手機使用模式，并采用傳統(tǒng)PDR算法、HCGA算法、S-ZUPT算法和S-ZUPT+HCGA算法對采集數(shù)據(jù)進(jìn)行解算。其中，HCGA算法對于手機姿態(tài)的判斷結(jié)果如圖4所示，HCGA算法成功判斷出了三次姿態(tài)變化的時刻，并且姿態(tài)判斷的準(zhǔn)確率為100%。

圖4 手機使用模式判斷圖Fig.4 Judgment of smartphone using-mode

圖5所示為行走軌跡復(fù)現(xiàn)圖，其中，綠色實線代表實際的行走軌跡，藍(lán)色下三角和紅色上三角分別代表為未添加HCGA的PDR算法和S-ZUPT算法解算結(jié)果，藍(lán)色點線和紅色虛線代表PDR+HCGA和S-ZUPT+HCGA算法解算結(jié)果。

從圖5可以看出，未添加HCGA的解算結(jié)果在第一次改變手機姿態(tài)后，行走軌跡就出現(xiàn)了較大的偏差，航向偏移接近 60°。在三次姿態(tài)改變后，行走軌跡已經(jīng)完全偏移實際行走路線。對輔助了HCGA算法的兩組結(jié)果而言，行走軌跡基本貼近實際路線。說明本文提出的HCGA算法能夠有效判斷手機姿態(tài)變化，并能對由于手機姿態(tài)改變而造成的航向變化進(jìn)行補償。

圖5 軌跡對比圖Fig.5 Walking experiments comparison

此外，從圖5中第一次改變姿態(tài)之前的路線結(jié)果就能明顯看出S-ZUPT算法在對誤差抑制的效果上要優(yōu)于傳統(tǒng)PDR算法。由圖5可知，在同樣輔助了HCGA算法的情況下，S-ZUPT算法最后的定位結(jié)果為（?1.674, 1.543），與起始點之間的距離為2.28 m，對于196 m的行走距離而言誤差為1.2%。相較于PDR算法的（?8.161, 3.047）最后定位結(jié)果，精度提高了74%。且從整體的復(fù)現(xiàn)圖中，S-ZUPT+HCGA算法的導(dǎo)航路線圖和實際行走路線圖更為貼近，說明本文設(shè)計的S-ZUPT+HCGA算法在僅使用 MEMS器件采集到的導(dǎo)航傳感器信息時，能有效提高導(dǎo)航精度，抑制慣性導(dǎo)航的誤差發(fā)散，并且能對手機姿態(tài)進(jìn)行判斷，補償因手機姿態(tài)改變造成的航向誤差。

圖6所示為使用四種算法得到的航向值曲線。其中，灰色點虛線代表行走過程中實驗人員轉(zhuǎn)彎時刻，而淡藍(lán)色虛線代表手機姿態(tài)發(fā)生變化的時刻。從圖6可以明顯看出：在行人行走轉(zhuǎn)彎時，HCGA算法不影響航向的變化，但當(dāng)手機姿態(tài)發(fā)生變化時，未使用HCGA輔助的算法會認(rèn)為是行人行走進(jìn)行了轉(zhuǎn)彎，從而輸出錯誤的航向；而輔助HCGA算法后，系統(tǒng)通過對使用姿態(tài)的判斷進(jìn)行了航向的補償，保持了航向，且S-ZUPT算法得到的航向結(jié)果與傳統(tǒng)PDR算法的航向結(jié)果相比，對誤差的抑制效果更明顯。

5 總 結(jié)

用手機進(jìn)行行人導(dǎo)航時，手機的姿態(tài)由于正常使用而發(fā)生改變，傳統(tǒng)導(dǎo)航算法易受姿態(tài)改變的影響，給出錯誤的導(dǎo)航結(jié)果。本文采用基于模擬零速修正的PDR系統(tǒng)進(jìn)行導(dǎo)航，以卡爾曼濾波算法對陀螺儀輸出的角速度進(jìn)行濾波處理，以抑制慣導(dǎo)誤差隨時間發(fā)散。同時，在每次探測到步伐時，對手機姿態(tài)進(jìn)行判斷，并根據(jù)判斷的結(jié)果進(jìn)行航向的補償。

從實驗結(jié)果中可以看出，本文提出的算法在200 m的矩形行走實驗中，定位誤差為 1.63%，能夠有效減少誤差積累，抑制航向發(fā)散，提高導(dǎo)航精度，使得導(dǎo)航復(fù)現(xiàn)路線更加貼合真實行走軌跡；結(jié)合HCGA算法后能夠動態(tài)補償由于手機姿態(tài)改變導(dǎo)致的航向誤差，提高了手機導(dǎo)航的精度。

目前本文專門針對三種手機使用模式進(jìn)行判斷，且采用經(jīng)驗閾值分析方法獲得統(tǒng)計數(shù)據(jù)。后繼將引入機器學(xué)習(xí)算法，增加相關(guān)的判斷模型，提高手機使用模式的分析效率，增強用戶體驗。