基于慣性傳感器的行人室內(nèi)位置跟蹤方法
【專利摘要】基于慣性傳感器的行人室內(nèi)位置跟蹤方法，本發(fā)明涉及一種行人室內(nèi)位置跟蹤方法，具體涉及基于慣性傳感器的PDR方法和PF融合地圖信息算法。本發(fā)明是要解決單純使用PDR對行人位置跟蹤時(shí)，因?yàn)閼T性傳感器自身所具有的長時(shí)漂移性而造成位置估計(jì)誤差大甚至錯(cuò)誤估計(jì)等情況。一、根據(jù)加速度傳感器進(jìn)行檢步及步長估計(jì)；二、根據(jù)陀螺儀測量數(shù)據(jù)中的三軸角速度變化進(jìn)行航向角估計(jì)，對航向角進(jìn)行校正，然后根據(jù)校正后的航向角與第一步步長進(jìn)行航跡推算，最后根據(jù)步長、航向角通過PDR方法估計(jì)位置：三、通過粒子濾波將地圖信息和PDR估計(jì)結(jié)果融合，即完成了基于慣性傳感器的行人室內(nèi)位置跟蹤方法。本發(fā)明應(yīng)用于室內(nèi)定位【技術(shù)領(lǐng)域】。
【專利說明】基于慣性傳感器的行人室內(nèi)位置跟蹤方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種行人室內(nèi)位置跟蹤方法，具體涉及基于慣性傳感器的PDR方法和 PF融合地圖信息算法。

【背景技術(shù)】
[0002] 近幾年，微電子機(jī)械系統(tǒng)（Micro-ElectroMechanical Systems, MEMS)的發(fā)展，使 得慣性傳感器在智能移動(dòng)終端的應(yīng)用得到普及?；趹T性傳感器的行人室內(nèi)導(dǎo)航系統(tǒng)， 以其無需鋪設(shè)外部設(shè)施的低成本優(yōu)勢，逐漸成為研究熱門。該系統(tǒng)的基本原理是采用行 人航跡推算法（PDR, Pedestrian Dead-Reckoning),根據(jù)慣性傳感器（如加速度計(jì)、陀螺 儀）的測量數(shù)據(jù)，主要涉及到檢步、估計(jì)步長、估計(jì)航向角等技術(shù)，因此這種系統(tǒng)有時(shí)也稱 為SHSs(Step-and_Heading Systems)。早期采用的是可穿戴設(shè)備，將傳感器安裝于鞋子、 頭盔、置于口袋、腰部等，行走中腳部能更多的反映運(yùn)動(dòng)特征，所以基于腳部傳感器能更好 的檢步，然而需要額外購置專門設(shè)備且不便攜帶，不適用于普通的行人室內(nèi)導(dǎo)航系統(tǒng)。在 行走過程中，加速度傳感器會輸出一定的行走特征，通過對測量數(shù)據(jù)進(jìn)行峰值檢測、過零檢 測、自相關(guān)匹配、頻譜分析等，其中一種或幾種相結(jié)合來識別每一步，后兩種因?yàn)楹臅r(shí)長而 導(dǎo)致實(shí)時(shí)性不強(qiáng)。對于步長估計(jì)，最粗略的是直接設(shè)定步長為一個(gè)常數(shù)，因?yàn)樾腥嗽诔Ｋ?行走下有一個(gè)平均步長，但是現(xiàn)實(shí)中，步長受一個(gè)人的身高、體型、換步頻率等因素影響，不 可以一概為均值。研究者們普遍接受一個(gè)最大最小加速度差值四次根方的公式計(jì)算，但 也有人提出其它如基于步頻和步長有線性關(guān)系，穿戴腳部的傳感器采用零速更新（ZUPT， Zero-velocity UPdaTe)，基于行人身高等步長估計(jì)方法。而航向估計(jì)中，最簡單的估計(jì)方 法是將傳感器固定于行人，使傳感器坐標(biāo)系與地球坐標(biāo)系（指南針坐標(biāo)系，N-E坐標(biāo)系）三 軸重合，這樣傳感器測得的方位角即是行人的航向角，則經(jīng)常通過借助重力在加速度傳感 器三軸上的分量以及磁強(qiáng)計(jì)（電子羅盤）來計(jì)算求得，而一般情況下，傳感器的放置是無固 定姿態(tài)的（即姿態(tài)任意性），這是需要借助其它方法來估計(jì)航向，比如主成份分析法（PCA， Primary Components Analyze)估計(jì)航向角。
[0003] 粒子濾波（PF，Particle Filter)是在Kalman濾波的狀態(tài)模型的高斯、線性假設(shè) 條件不滿足的情況下興起的濾波跟蹤算法，它通過使用蒙特卡洛法抽樣最大后驗(yàn)估計(jì)，從 而逼近貝葉斯最優(yōu)估計(jì)。目前各種室內(nèi)定位如WLAN室內(nèi)定位、超寬帶室內(nèi)定位、基于視覺 室內(nèi)定位、慣性傳感器室內(nèi)定位等都采用粒子濾波來改善定位精度以及解決多目標(biāo)跟蹤問 題，并且使用粒子濾波來融合不同定位系統(tǒng)的數(shù)據(jù)，從而相互輔助提高定位結(jié)果。經(jīng)常采用 粒子濾波來將地圖信息加入到位置估計(jì)中，根據(jù)地圖信息來調(diào)整粒子濾波中的參數(shù)，從而 不僅提高精度，也防止了跟蹤目標(biāo)出現(xiàn)穿墻等特殊情況的發(fā)生。


【發(fā)明內(nèi)容】

[0004] 本發(fā)明是要解決單純使用PDR對行人位置跟蹤時(shí)，因?yàn)閼T性傳感器自身所具有的 長時(shí)漂移性而造成位置估計(jì)誤差大甚至錯(cuò)誤估計(jì)等情況，而提供了基于慣性傳感器的行人 室內(nèi)位置跟蹤方法。
[0005] 基于慣性傳感器的行人室內(nèi)位置跟蹤方法按以下步驟實(shí)現(xiàn)：
[0006] -、根據(jù)加速度傳感器進(jìn)行檢步及步長估計(jì)；
[0007] 二、根據(jù)陀螺儀測量數(shù)據(jù)中的三軸角速度變化進(jìn)行航向角估計(jì)，對航向角進(jìn)行校 正，然后根據(jù)校正后的航向角與第一步步長進(jìn)行航跡推算，最后根據(jù)步長、航向角通過roR 方法估計(jì)位置：

【權(quán)利要求】
1. 基于慣性傳感器的行人室內(nèi)位置跟蹤方法，其特征在于它按以下步驟實(shí)現(xiàn)： 一、 根據(jù)加速度傳感器進(jìn)行檢步及步長估計(jì)； 二、 根據(jù)陀螺儀測量數(shù)據(jù)中的三軸角速度變化進(jìn)行航向角估計(jì)，對航向角進(jìn)行校正，然 后根據(jù)校正后的航向角與第一步步長進(jìn)行航跡推算，最后根據(jù)步長、航向角通過PDR方法 估計(jì)位置：
(1) 其中，
表示第k步的PDR估計(jì)位置
hk代表第k步估計(jì)的航向角， sLenk代表第k步的步長； 三、 通過粒子濾波將地圖信息和PDR估計(jì)結(jié)果融合，即完成了基于慣性傳感器的行人 室內(nèi)位置跟蹤方法。
2. 根據(jù)權(quán)利要求1中所述的基于慣性傳感器的行人位置跟蹤方法，其特征在于所述步 驟一具體過程如下： 采用峰值-零值-谷值-時(shí)間間隔的方法進(jìn)行檢步，即每行走一步包含1個(gè)最大加速 度、2個(gè)零值、1個(gè)最小加速度，并且滿足時(shí)間間隔合理，按照成人常速行走的速度為2?4 步每秒，設(shè)置時(shí)間間隔下限為250暈秒，S。表不起步，Si, i = 1…9表不第i步，步長估計(jì)通 過下式來獲得
(2) 其中sLen是估計(jì)的步長，acCi、accAve;分別代表每一步中的加速度值、平均加速度值，N 表示每步中采集到的數(shù)據(jù)個(gè)數(shù)。
3. 根據(jù)權(quán)利要求2中所述的基于慣性傳感器的行人位置跟蹤方法，其特征在于所述步 驟二中航向角估計(jì)方法為： 首先對圍繞X、y、z的三軸角速度進(jìn)行時(shí)間積分，分別得到俯仰角、滾動(dòng)角、方位角，記 作 Pitch, Roll, Azimuth ; 采用（3)對航向角進(jìn)行第一步校正： heading = q · Pitch+c2 · Roll+c3 · Azimuth (3) 其中，俯仰角Pitch表示圍繞著x軸的旋轉(zhuǎn)量，滾動(dòng)角Roll表示圍繞著y軸的旋轉(zhuǎn)量， 方位角Azimuth表示圍繞著z三軸的旋轉(zhuǎn)量，其中Cl，c2, c3為對應(yīng)的加權(quán)系數(shù)； 對第一步校正后的航向角進(jìn)行第二步校正，即進(jìn)行逐次平滑，具體通過下式計(jì)算：
(4) mean( ·)表示取平均值。
4. 根據(jù)權(quán)利要求3中所述的基于慣性傳感器的行人位置跟蹤方法，其特征在于所述步 驟三的通過粒子濾波將地圖信息和PDR估計(jì)結(jié)果融合方法如下： 粒子濾波是一種采用蒙特卡洛方法來逼近后驗(yàn)概率的貝葉斯濾波方法，不要求運(yùn)動(dòng)系 統(tǒng)是線性或者高斯的； 一、 采用的粒子濾波的狀態(tài)方程為：
(5) 其中，xk, yk表示第k步時(shí)的位置坐標(biāo)，Xh, yH表示第k-Ι步的位置坐標(biāo)，hk是估計(jì)出 的航向角，
為步長估計(jì)誤差，
為航向角估計(jì)誤差，lk表示？，sin(*)，c 〇s(·)分別表示 求對應(yīng)角度的正弦值、余弦值； 二、 對第k步的所有粒子進(jìn)行加權(quán)值&計(jì)算：
(6) 其中
表示第k步的PDR估計(jì)位置
表示第k步的第i個(gè)粒子的 位置，σ表示PDR位置估計(jì)的標(biāo)準(zhǔn)差，當(dāng)?shù)趉步中生成的某個(gè)粒子不在有效位置范圍內(nèi)時(shí)， 則K取0值，否則按照condition_l計(jì)算權(quán)值； 三、 通過粒子濾波將地圖信息和PDR估計(jì)結(jié)果融合： 在得到第k步的所有粒子加權(quán)值&后，根據(jù)&計(jì)算使用了粒子濾波的第k步的估計(jì)位 置：
(7) 其中，Np代表第k步中的粒子個(gè)數(shù)，
代表第k步中對第i個(gè)粒子的估計(jì)位置
，根據(jù)式（5)計(jì)算得到，為歸一化加權(quán)值，通過下式計(jì)算：
(8) 不斷的對PDR估計(jì)出的位置進(jìn)行粒子濾波，得到新的估計(jì)位置。
【文檔編號】G01C21/18GK104061934SQ201410255619
【公開日】2014年9月24日 申請日期:2014年6月10日 優(yōu)先權(quán)日:2014年6月10日
【發(fā)明者】馬琳, 鄧仲哲, 秦丹陽, 何晨光, 徐玉濱, 崔揚(yáng) 申請人:哈爾濱工業(yè)大學(xué)