基于慣性元件的實(shí)時(shí)三維室內(nèi)定位系統(tǒng)設(shè)計(jì)*

白夢(mèng)帥， 曾祥燁， 王靜宜， 蘇彥莽

(河北工業(yè)大學(xué) 電子信息工程學(xué)院 天津市電子材料與器件重點(diǎn)實(shí)驗(yàn)室，天津 300401)

0 引 言

隨著經(jīng)濟(jì)社會(huì)的發(fā)展，城市中大型建筑物的數(shù)量不斷增加，隨之而來的是以火災(zāi)為主的突發(fā)性災(zāi)害事件的發(fā)生概率逐漸提高[1]?；馂?zāi)發(fā)生時(shí)，實(shí)時(shí)的室內(nèi)定位可以提供必要的位置信息，進(jìn)而提高救援效率，在最大限度上保證人民的生命財(cái)產(chǎn)安全，因此室內(nèi)定位的研究具有重要的理論意義和應(yīng)用價(jià)值。本文設(shè)計(jì)了一種以慣性元件和氣壓計(jì)進(jìn)行數(shù)據(jù)采集，然后利用樹莓派進(jìn)行處理，最后通過無線通信將解算后的數(shù)據(jù)發(fā)送到上位機(jī)，在上位機(jī)上利用基于粒子濾波的地圖匹配來進(jìn)行實(shí)時(shí)位置顯示的三維室內(nèi)定位系統(tǒng)。

1 慣性定位技術(shù)的理論基礎(chǔ)

慣性定位技術(shù)以牛頓運(yùn)動(dòng)定律為理論基礎(chǔ)，首先通過姿態(tài)傳感器采集被定位目標(biāo)的慣性信息，然后將載體坐標(biāo)系與地理坐標(biāo)系進(jìn)行轉(zhuǎn)換[2]，轉(zhuǎn)換后對(duì)慣性數(shù)據(jù)進(jìn)行位移和姿態(tài)解算，最后得到被定位目標(biāo)的位置信息。

1.1 坐標(biāo)系之間的轉(zhuǎn)換

在慣性導(dǎo)航技術(shù)中，要想由原始數(shù)據(jù)得到位置信息，首先需要進(jìn)行坐標(biāo)系的轉(zhuǎn)換。實(shí)時(shí)更新運(yùn)動(dòng)載體的姿態(tài)矩陣是進(jìn)行坐標(biāo)系轉(zhuǎn)換的前提，常用的姿態(tài)矩陣更新方法有歐拉角法、方向余弦算法和四元數(shù)法[3]，與前兩種方法相比，四元數(shù)算法具有計(jì)算量較小，并且可以避免死鎖問題的優(yōu)點(diǎn)[4]。所以本文采用四元數(shù)法來完成坐標(biāo)系的轉(zhuǎn)換。由四元數(shù)表示的坐標(biāo)轉(zhuǎn)換矩陣如下

(1)

q=q0+q1i+q2j+q3k

(2)

式中q0為四元數(shù)的實(shí)部，q1，q2，q3為三個(gè)相互正交的虛部單位向量。

1.2 位置解算

在位置解算方面，慣性導(dǎo)航技術(shù)常用的方法有二次積分法和行人航位推算(pedestrian dead reckoning，PDR)算法，本文采用PDR算法來解算位置，與二次積分法相比，PDR最大的優(yōu)勢(shì)是可以避免累計(jì)誤差[5,6]，其基本原理如圖1所示。

圖1 PDR基本原理

如圖1所示,建立直角坐標(biāo)系OXY，其中X軸與地理坐標(biāo)系的東向重合，Y軸與北向重合，假設(shè)(X0,Y0)為被定位目標(biāo)的初始位置，目標(biāo)從初始位置開始運(yùn)動(dòng)，di為每次運(yùn)動(dòng)的單步步長(zhǎng)，m，ai為當(dāng)前步長(zhǎng)方向與Y軸的夾角(°)，則運(yùn)動(dòng)目標(biāo)從第(i-1)步運(yùn)動(dòng)到第i步的步長(zhǎng)在X軸方向上可由下式表示

ΔXi-(i-1)=disinαi

(3)

同理,在Y軸方向上的步長(zhǎng)可由下式表示

ΔYi-(i-1)=dicosαi

(4)

由上式可得，被定位目標(biāo)邁出第n步后的坐標(biāo)(Xn,Yn)可作如下表示

(5)

(6)

2 系統(tǒng)框架與硬件方案設(shè)計(jì)

本系統(tǒng)能夠完成數(shù)據(jù)的采集、處理和發(fā)送以及最終位置的解算和顯示，系統(tǒng)框架主要分為下位機(jī)、上位機(jī)以及上位機(jī)和下位機(jī)之間的無線通信三大部分。其中下位機(jī)以樹莓派3B+作為核心主控設(shè)備，可以滿足本系統(tǒng)大量數(shù)據(jù)運(yùn)算的需求。慣性模塊采用JY901，氣壓采集模塊采用GY68均通過I2C與樹莓派連接，無線通信模塊采用AS07-M1101S，利用自定義協(xié)議完成無線數(shù)據(jù)通信。

3 系統(tǒng)軟件方案設(shè)計(jì)

本系統(tǒng)軟件設(shè)計(jì)主要分為上位機(jī)和下位機(jī)以及二者之間的無線通信。下位機(jī)主要的功能為采集原始數(shù)據(jù)，然后進(jìn)行處理運(yùn)算，最后將處理過的位移數(shù)據(jù)發(fā)送給上位機(jī)進(jìn)行基于粒子濾波的位置顯示。軟件總體工作流程如圖2所示。

圖2 系統(tǒng)軟件設(shè)計(jì)流程

為了保證數(shù)據(jù)傳輸?shù)姆€(wěn)定性和有效性，本系統(tǒng)將無線通信的波特率設(shè)置為38 400 Baud/s，每一幀數(shù)據(jù)共包含32個(gè)字節(jié)，分別是：數(shù)據(jù)類型(占1個(gè)字節(jié))，數(shù)據(jù)序號(hào)(占1個(gè)字節(jié))，終端地址(占1個(gè)字節(jié))，步數(shù)(占4個(gè)字節(jié))，位移(占24個(gè)字節(jié))，校驗(yàn)位(占1個(gè)字節(jié))。

4 定位數(shù)據(jù)的處理

由JY901和GY68采集到的數(shù)據(jù)需要進(jìn)一步處理才可用于具體的定位，本文的數(shù)據(jù)處理由以樹莓派為核心的下位機(jī)系統(tǒng)完成。

4.1 原始數(shù)據(jù)的預(yù)處理

本文所用JY901屬低成本慣性元件，對(duì)抖動(dòng)高度敏感[7]，經(jīng)實(shí)測(cè)角速度存在靜態(tài)漂移，為減小誤差，采用中值均值濾波來進(jìn)行預(yù)處理，處理結(jié)果如圖3所示。

圖3 經(jīng)中值均值濾波處理后的角速度效果

由氣壓計(jì)GY68采集到的氣壓數(shù)據(jù)也存在跳變，同樣采用中值均值濾波來降低數(shù)據(jù)跳變的頻率，降低對(duì)高度定位效果的影響，效果如圖4所示。

圖4 經(jīng)中值均值濾波處理后的氣壓數(shù)據(jù)效果

4.2 運(yùn)動(dòng)過程中的濾波處理

單步檢測(cè)的準(zhǔn)確率直接影響到最終定位效果，本文利用三軸合加速度來檢測(cè)單步運(yùn)動(dòng)，經(jīng)實(shí)測(cè)運(yùn)動(dòng)中的合加速度數(shù)據(jù)存在大量噪聲，對(duì)步態(tài)檢測(cè)的精度有較大影響，為了降低誤差，提高精度，利用低通濾波對(duì)三軸合加速度進(jìn)行平滑濾波，濾波效果如圖5所示。

圖5 經(jīng)低通濾波處理后的合加速度效果

本系統(tǒng)的慣性模塊JY901采樣率設(shè)置為100 Hz，即每秒鐘可完成100組數(shù)據(jù)的采集，每組數(shù)據(jù)包括三軸角速度和三軸加速度，實(shí)測(cè)采集15 s運(yùn)動(dòng)數(shù)據(jù)，共計(jì)1 500組。由圖5可知，將低通濾波器的截止頻率設(shè)為3.5，濾波后的運(yùn)動(dòng)規(guī)律更明顯，可以容易的檢測(cè)出單步運(yùn)動(dòng)的波峰以及波谷，有利于提高步態(tài)檢測(cè)的精度。

5 測(cè)試結(jié)果與分析

5.1 步數(shù)檢測(cè)和位移計(jì)算

本文采用設(shè)置閾值的方法來檢測(cè)步態(tài)，根據(jù)行人邁步規(guī)律，單步運(yùn)動(dòng)可分為開始，步中，結(jié)束三個(gè)關(guān)鍵節(jié)點(diǎn)[8]，首先檢測(cè)單步運(yùn)動(dòng)的開始，即三軸合加速度波形的波峰，然后檢測(cè)單步的步中，即波谷，如果峰谷時(shí)間差大于0.3 s且小于0.8 s，便可認(rèn)為檢測(cè)到了一步運(yùn)動(dòng)，然后將檢測(cè)到的單步運(yùn)動(dòng)的相關(guān)數(shù)據(jù)信息保存到列表中，用于后期的位移計(jì)算。每當(dāng)檢測(cè)到單步運(yùn)動(dòng)時(shí)，即在程序中調(diào)用步長(zhǎng)計(jì)算函數(shù)，將單步內(nèi)的X,Y軸位移數(shù)據(jù)進(jìn)行二次積分運(yùn)算，得到具體的單步位移。為了適應(yīng)復(fù)雜的應(yīng)用環(huán)境和進(jìn)一步降低傳感器的抖動(dòng)誤差，本文將設(shè)備佩戴于腰部右側(cè)，X,Y,Z軸分別對(duì)應(yīng)運(yùn)動(dòng)方向的前、右、上，步數(shù)的檢測(cè)結(jié)果如表1所示。

由表1可知，步數(shù)檢測(cè)的準(zhǔn)確度可達(dá)95%以上，具有良好的檢測(cè)精度。

表1 步數(shù)檢測(cè)結(jié)果

5.2 樓層檢測(cè)

傳統(tǒng)的三維定位中，大多使用氣壓計(jì)所測(cè)得的絕對(duì)氣壓值來進(jìn)行海拔高度的換算，此方法不僅需要提前設(shè)置參考高度，還可能受到溫度變化的影響，因此通用性較差，為解決此問題，本文采用通過氣壓相對(duì)變化量來表示樓層變化的方法，由氣壓計(jì)算海拔高度的公式可知

(7)

式中h為當(dāng)前海拔高度，p為氣壓測(cè)量值，p0為海平面所對(duì)應(yīng)的氣壓值。高度每升高8.43 m，氣壓降低1 hPa。本文測(cè)試所在樓層的高度為4.3 m左右，且兩層樓中間設(shè)有樓梯轉(zhuǎn)向平臺(tái)，實(shí)測(cè)結(jié)果如圖6所示。

圖6 氣壓值的相對(duì)變化量和樓層的對(duì)應(yīng)關(guān)系

經(jīng)實(shí)測(cè)可知，根據(jù)氣壓值的累計(jì)變化來進(jìn)行樓層檢測(cè)，可以達(dá)到半層樓的檢測(cè)精度，具有一定的實(shí)用性。

5.3 系統(tǒng)定位效果驗(yàn)證

在上位機(jī)的位置顯示部分，本系統(tǒng)采用基于粒子濾波的地圖匹配算法，首先利用由Python編寫的地圖生成器生成室內(nèi)建筑物的二維平面地圖，預(yù)先規(guī)定的運(yùn)動(dòng)路線如圖7所示。

圖7 實(shí)際測(cè)試規(guī)畫路線

通過設(shè)置一系列的特定地圖標(biāo)定元素，將實(shí)際的墻壁和電梯、樓梯處分別用不同元素來限定，只有當(dāng)被定位目標(biāo)運(yùn)動(dòng)到預(yù)先標(biāo)定的高度可改變區(qū)域，即電梯、樓梯處，氣壓的變化才可以用來表示樓層變化，然后二維地圖隨即切換到對(duì)應(yīng)樓層，二維地圖中的軌跡跟蹤實(shí)測(cè)效果如圖8所示。

圖8 實(shí)際測(cè)試運(yùn)動(dòng)軌跡

圖7中灰線為已規(guī)劃的行走路線，方向如箭頭所示，地點(diǎn)為河北工業(yè)大學(xué)電子信息工程學(xué)院樓四樓樓道，圖8為按照規(guī)劃路線進(jìn)行軌跡追蹤的效果圖，其中方塊色起點(diǎn)，灰色粒子為運(yùn)動(dòng)過程中的實(shí)際軌跡。

6 結(jié) 論

本文的主要研究?jī)?nèi)容是通過姿態(tài)傳感器采集慣性信息，通過氣壓計(jì)采集氣壓信息，然后由樹莓派進(jìn)行數(shù)據(jù)處理，把處理過的兩軸位移數(shù)據(jù)和氣壓數(shù)據(jù)通過無線通信模塊發(fā)送到上位機(jī)，由上位機(jī)進(jìn)行基于粒子濾波的實(shí)時(shí)位置的顯示。系統(tǒng)整體結(jié)構(gòu)設(shè)計(jì)簡(jiǎn)單可靠，且能夠?qū)崿F(xiàn)真正的實(shí)時(shí)定位，水平定位誤差達(dá)到了0.5 m以內(nèi)，樓層檢測(cè)精度可以達(dá)到半層樓，有一定的應(yīng)用價(jià)值。