一種imu室內(nèi)定位方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于室內(nèi)定位技術(shù)領(lǐng)域，尤其涉及一種IMU室內(nèi)定位方法及系統(tǒng)。
【背景技術(shù)】
[0002] 隨著物聯(lián)網(wǎng)和智慧城市建設(shè)的興起和快速發(fā)展，人們對(duì)室內(nèi)位置服務(wù)例如目標(biāo)發(fā) 現(xiàn)、醫(yī)療服務(wù)和智能家居等的需求與日倶增。
[0003] 傳統(tǒng)的GPS和蜂窩網(wǎng)定位技術(shù)在室外能達(dá)到較高的定位精度，但是在室內(nèi)環(huán)境 下，由于射頻信號(hào)受到阻隔而無(wú)法有效定位。
[0004] 目前常用的室內(nèi)定位技術(shù)主要是基于各種無(wú)線網(wǎng)絡(luò)，如WLAN、RFID、WIFI等，利用 接收信號(hào)強(qiáng)度（ReceivedSignalStrength，RSS)來(lái)實(shí)現(xiàn)室內(nèi)定位。但是由于多徑效應(yīng)、墻 壁衰減、人的走動(dòng)等的影響，室內(nèi)環(huán)境比較復(fù)雜，RSS的變化比較大，因此基于無(wú)線網(wǎng)絡(luò)的室 內(nèi)定位精度受到限制，定位精度很少能達(dá)到1米以下，并且這些定位技術(shù)需要輔助相應(yīng)的 信號(hào)基站。
[0005] 基于以上原因，考慮使用IMU(InertialMeasurementUnit，IMU)定位，它利用 慣性測(cè)量單元推算下一時(shí)刻用戶的位置，估計(jì)行走軌跡，不需要輔助信號(hào)基站，具有完全自 主、不受外界環(huán)境影響、數(shù)據(jù)更新速度快等特點(diǎn)。但是，目前傳統(tǒng)MU室內(nèi)定位的航位推算 模型中還存在數(shù)據(jù)處理誤差大，定位精度低的問(wèn)題。

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

[0006] 本發(fā)明的目的在于提供一種基于改進(jìn)航位推算模型和粒子濾波的IMU室內(nèi)定位 系統(tǒng)，屬于室內(nèi)定位領(lǐng)域，旨在改進(jìn)航位推算模型和粒子濾波的基礎(chǔ)上設(shè)計(jì)出室內(nèi)定位系 統(tǒng)，減少室內(nèi)定位誤差。
[0007] 本發(fā)明是這樣實(shí)現(xiàn)的，一種IMU室內(nèi)定位方法，該方法包括以下步驟：
[0008] S1、步數(shù)檢測(cè)：通過(guò)加速度傳感器獲取Z軸方向的加速度數(shù)據(jù)，利用步數(shù)檢測(cè)算法 計(jì)算行走步數(shù)；
[0009] S2、自適應(yīng)步長(zhǎng)估計(jì)：根據(jù)沿著行走方向的相關(guān)數(shù)據(jù)，結(jié)合自適應(yīng)步長(zhǎng)估計(jì)算法得 出用戶行走一步的距離，所述數(shù)據(jù)包括加速度和步頻；
[0010] S3、航向估計(jì)：通過(guò)方向傳感器和陀螺儀計(jì)算得出用戶的行走方向；
[0011] S4、室內(nèi)定位：基于幾何中心的部分重采樣粒子濾波算法對(duì)用戶進(jìn)行室內(nèi)定位。
[0012] 優(yōu)選地，在步驟S1中，所述步數(shù)檢測(cè)算法包括以下步驟：
[0013] 對(duì)Z軸方向的加速度值進(jìn)行濾波去噪、峰谷值檢測(cè)算法，得到Z軸加速度的波形 圖；
[0014] 對(duì)此波形進(jìn)行窗口檢測(cè)算法，對(duì)不滿足窗口檢測(cè)的波形段標(biāo)記為無(wú)效步伐；
[0015] 通過(guò)對(duì)波形進(jìn)行DTW算法檢測(cè)，通過(guò)檢測(cè)相鄰的波峰和波谷的相近度來(lái)確定其是 否為真實(shí)的一步。
[0016] 優(yōu)選地，在步驟S2中，所述自適應(yīng)步長(zhǎng)估計(jì)算法包括以下步驟：
[0017] (1)初始化：在未采集到用戶的步伐相關(guān)數(shù)據(jù)前，先使用通用步長(zhǎng)模型的a和0， 根據(jù)步頻和步長(zhǎng)模型獲得步長(zhǎng)；
[0018] (2)自適應(yīng)估計(jì)：在收集到i_l時(shí)刻步伐相關(guān)數(shù)據(jù)后，根據(jù)其步長(zhǎng)與步頻的關(guān)系， 利用線性回歸可以得到i時(shí)刻的a和0，再根據(jù)i時(shí)刻的步頻可得出相應(yīng)的步長(zhǎng)。
[0019] 優(yōu)選地，在步驟S4中，所述部分重采樣粒子濾波算法包括以下步驟：
[0020] (1)根據(jù)預(yù)先設(shè)定的粒子初始化狀態(tài)分別對(duì)粒子進(jìn)行初始化t= 0,根據(jù)p(X。）生 成1')個(gè)粒子2?，#==1，2，~，#。^(1。）分布通常為高斯白噪聲分布 ；
[0021] ⑵設(shè)定t=t+1，根據(jù)狀態(tài)轉(zhuǎn)移公式| 采樣粒子樣本 = If表示t時(shí)刻的第i個(gè)粒子；對(duì)每個(gè)粒子XX2,...,I分配相應(yīng)的權(quán) 值
其中|X/j是已知t-1時(shí)刻的狀態(tài)量去估計(jì)t時(shí)刻 狀態(tài)量的后驗(yàn)概率密度函數(shù)，I是已知t時(shí)刻的狀態(tài)量去估計(jì)t時(shí)刻的觀測(cè)量的后 驗(yàn)概率密度函數(shù)，4表示X&粒子對(duì)應(yīng)的權(quán)重；
[0022] 對(duì)每個(gè)粒子的權(quán)重進(jìn)行歸一化：
[0023]
[0024] (3)基于幾何中心部分重采樣的原理對(duì)A類和C類粒子尤，1 = 1，2，...，N進(jìn)行 重采樣，并重新分配權(quán)重，對(duì)A類和C類粒子根據(jù)其加權(quán)后的權(quán)重進(jìn)行重采樣得 到Ns個(gè)新粒子，并對(duì)其重新分配權(quán)重1/Ns;其中，所述A類粒子為距離小于H的粒子，所述C類粒子為距離大于Th的粒子；
[0025] (4)根據(jù)新生成的粒子進(jìn)行狀態(tài)估計(jì)
[0026] 針對(duì)現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種基于改進(jìn)航位推算模型和粒子濾波的 MU室內(nèi)定位方法及系統(tǒng)。本發(fā)明針對(duì)原航位推算模型數(shù)據(jù)處理誤差大，設(shè)計(jì)了步數(shù)檢測(cè) 算法和自適應(yīng)步長(zhǎng)算法；粒子濾波算法完成了IMU定位系統(tǒng)的信息整合和優(yōu)化過(guò)程，針對(duì) 其自身粒子出現(xiàn)的貧化和退化問(wèn)題，本發(fā)明提出了基于幾何中心的部分重采樣粒子濾波算 法。本發(fā)明MU室內(nèi)定位過(guò)程如圖1所示；其中，改進(jìn)的航位推算模型包括步數(shù)檢測(cè)算法和 自適應(yīng)步長(zhǎng)算法。
[0027] 通常，步數(shù)檢測(cè)算法的一般過(guò)程為：
[0028] (1)濾波去噪：為了表現(xiàn)信號(hào)的周期性，步數(shù)檢測(cè)算法首先使用低通FIR數(shù)字濾波 器去除高頻率噪聲。低通頻率下限設(shè)為3Hz;
[0029] (2)峰谷值檢測(cè)：峰谷值檢測(cè)用于計(jì)算一個(gè)單步內(nèi)的一個(gè)峰值和谷值，判斷其是 否完成一步，如圖2。
[0030] 通過(guò)對(duì)加速度數(shù)據(jù)平滑處理后，算法找到波形的峰值和谷值去判斷是否完成一 步。但是，并不是每一個(gè)符合峰谷值特征的波形都是真實(shí)的一步，手機(jī)的偶爾抖動(dòng)也會(huì)產(chǎn)生 錯(cuò)誤的一步。本發(fā)明通過(guò)增加時(shí)間窗口檢測(cè)和動(dòng)態(tài)時(shí)間規(guī)整（DTW)距離算法來(lái)減少錯(cuò)誤的 估計(jì)。
[0031] 時(shí)間窗口檢測(cè)：時(shí)間窗口用于排除無(wú)效抖動(dòng)。假設(shè)用戶最快的行走速度為每秒 三步，最慢的步行速度為每?jī)擅胍徊?。因此，兩個(gè)有效的步數(shù)之間時(shí)間間隔在時(shí)間窗口 [0. 3s~2.Os]之內(nèi)，如果時(shí)間間隔小于0. 3s或者大于2s的步伐是無(wú)效步伐。
[0032] DTW算法：DTW已經(jīng)被廣泛作為檢測(cè)兩個(gè)離散波形的相似度的有效方法。低的DTW 距離代表著高相似度。考慮到當(dāng)一個(gè)人正常行走時(shí)是左右腳交替前行，所以可以看成是相 似的波形在重復(fù)?；诖?，設(shè)計(jì)了DTW算法來(lái)確定通過(guò)峰谷值檢測(cè)的一步是真正的一步。具 體操作過(guò)程為：假設(shè)通過(guò)峰谷值檢測(cè)的步行{Sl，S2,S3,. . .，Sn}。那么，對(duì)于Si，算法可以 計(jì)算Si與Si-2之間的DTW距離。如果它低于給定的門限值，那么Si看做是真實(shí)的一步， 否則視為無(wú)效的一步。
[0033] 自適應(yīng)步長(zhǎng)估計(jì)算法：基于步長(zhǎng)與步頻、加速度的關(guān)系，本發(fā)明選擇步長(zhǎng)模型：
[0034] Lg=aXf+PXv+y；
[0035] 其中，、是步長(zhǎng)，f是步頻（即每秒行走的步數(shù)），a、0和y是可變參數(shù)，在每 一步完成之后可根據(jù)相關(guān)數(shù)據(jù)對(duì)其修正：
[0036]fx=l/a-t,i)；
[0037]
[0038] 其中，h是檢測(cè)到行走完第i步的時(shí)間。at是時(shí)刻t的加速度值，％.是一步之內(nèi) 的加速度平均值，N是一步內(nèi)的信號(hào)輸出個(gè)數(shù)。
[0039] 本發(fā)明的自適應(yīng)步長(zhǎng)估計(jì)算法過(guò)程具體為：
[0040] (1)初始化：在未采集到用戶的步伐相關(guān)數(shù)據(jù)前，先使用通用步長(zhǎng)模型的a和0， 根據(jù)步頻和步長(zhǎng)模型獲得步長(zhǎng)；
[0041] (2)自適應(yīng)估計(jì)：在收集到i_l時(shí)刻步伐相關(guān)數(shù)據(jù)后，根據(jù)其步長(zhǎng)與步頻的關(guān)系， 利用線性回歸可以得到i時(shí)刻的a和0，再根據(jù)i時(shí)刻的步頻可得出相應(yīng)的步長(zhǎng)。
[0042] 此外，相對(duì)于本發(fā)明改進(jìn)粒子濾波算法部分而言，現(xiàn)有粒子濾波算法幾乎都是對(duì) 所有粒子進(jìn)行的完全重采樣，這種"過(guò)度"重采樣極易引發(fā)粒子貧化。若將完全重采樣用部 分重采樣替代，則參與重采樣的粒子有助于緩解退化問(wèn)題，未參與重采樣的粒子有利于保 持粒子的多樣性?；谶@種思想，本發(fā)明提出了基于幾何中心的部分重采樣粒子濾波算法， 先通過(guò)幾何方法尋找粒子集的中心，按照粒子與其中心的歐式距離的大小確定需要重采樣 的粒子，解決了粒子濾波的貧化和退化的問(wèn)題，最后通過(guò)為了保證濾波算法的有效性通過(guò) 設(shè)置濾波閥值，高于此閥值說(shuō)明濾波算法失去其有效性，重新初始化重新濾波。
[0043] 本發(fā)明的粒子濾波系統(tǒng)模型為：
[0044] Xt=ft(Xt1;utj)；
[0045] Zt=ht(Xt,yt);
[0046] 其中，\和Zt分別是t時(shí)刻的狀態(tài)量和觀測(cè)量，uti和yt分別為系統(tǒng)過(guò)程噪音 和觀測(cè)噪音，其概率密度函數(shù)相互獨(dú)立其已知。濾波的目的就是根據(jù)給定的狀態(tài)初值X。和 序貫獲取的量測(cè)序列Z1:t={ZZ2, . . .，ZJ遞推估計(jì)Xt。粒子濾波通過(guò)狀態(tài)方程來(lái)預(yù)測(cè)狀 態(tài)的先驗(yàn)概率模型，再用觀測(cè)值對(duì)其進(jìn)行修正，得到狀態(tài)的后驗(yàn)概率模型，從而得到系統(tǒng)狀 態(tài)值的最優(yōu)估計(jì)。
[0047]粒子濾波的思路就是從選取的重要性密度函數(shù)q(Xt|XQ:tdZ1:t)中抽取N個(gè)加權(quán)粒 子i= 1，2, . . .，N，經(jīng)過(guò)權(quán)值歸一化及再抽樣來(lái)估計(jì)t時(shí)刻的后驗(yàn)概率分布：
[0048]
[0049] 其中，5為Dirac函數(shù)。
[0050] 基于幾何中心的部分重采樣的主要思想是：通過(guò)幾何方法找到粒子集的中心點(diǎn)， 使中心點(diǎn)與各個(gè)粒子的歐式距離之和為最小。設(shè)置距離閥值Th、Tl(0〈Il〈Th)，關(guān)于如何設(shè) 置在下文中說(shuō)明。根據(jù)粒子與中心點(diǎn)的距離，粒子可以分為3類：
[0051] A類：距離小于n的粒子；
[0052] B類：距離大于II小于Th的粒子；
[0053] C類：距離大于Th的粒子。
[0054] B類的粒子是比較穩(wěn)健的，不需要重采樣，因此重采樣只需要對(duì)A類和C類的粒子 進(jìn)行，如圖3。
[0055] 距離閥值的大小選擇對(duì)于重采樣算法的計(jì)算時(shí)間、粒子的多樣性以及粒子濾波的 性能有著重要影響，設(shè)Th= 0. 8L，II= 0. 2L。
[0056] 假設(shè)A類和C類粒子的總數(shù)目為Ns，在重采樣之前A類和C類粒子的權(quán)重為 = ，其中％是粒子的權(quán)重。易知，A類的權(quán)重大，C類的權(quán)重小，為了 減少每次重采樣中C類粒子的損失，我們對(duì)A類和C類粒子群權(quán)重M，/= 取平均得 $，再將g與?丨進(jìn)行組合，得到新的粒