基于慣性元件的實時三維室內定位系統設計*

白夢帥， 曾祥燁， 王靜宜， 蘇彥莽

(河北工業大學 電子信息工程學院 天津市電子材料與器件重點實驗室，天津 300401)

0 引 言

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

1 慣性定位技術的理論基礎

慣性定位技術以牛頓運動定律為理論基礎，首先通過姿態傳感器采集被定位目標的慣性信息，然后將載體坐標系與地理坐標系進行轉換[2]，轉換后對慣性數據進行位移和姿態解算，最后得到被定位目標的位置信息。

1.1 坐標系之間的轉換

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

(1)

q=q0+q1i+q2j+q3k

(2)

式中q0為四元數的實部，q1，q2，q3為三個相互正交的虛部單位向量。

1.2 位置解算

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

圖1 PDR基本原理

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

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

(3)

同理,在Y軸方向上的步長可由下式表示

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

(4)

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

(5)

(6)

2 系統框架與硬件方案設計

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

3 系統軟件方案設計

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

圖2 系統軟件設計流程

為了保證數據傳輸的穩定性和有效性，本系統將無線通信的波特率設置為38 400 Baud/s，每一幀數據共包含32個字節，分別是：數據類型(占1個字節)，數據序號(占1個字節)，終端地址(占1個字節)，步數(占4個字節)，位移(占24個字節)，校驗位(占1個字節)。

4 定位數據的處理

由JY901和GY68采集到的數據需要進一步處理才可用于具體的定位，本文的數據處理由以樹莓派為核心的下位機系統完成。

4.1 原始數據的預處理

本文所用JY901屬低成本慣性元件，對抖動高度敏感[7]，經實測角速度存在靜態漂移，為減小誤差，采用中值均值濾波來進行預處理，處理結果如圖3所示。

圖3 經中值均值濾波處理后的角速度效果

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

圖4 經中值均值濾波處理后的氣壓數據效果

4.2 運動過程中的濾波處理

單步檢測的準確率直接影響到最終定位效果，本文利用三軸合加速度來檢測單步運動，經實測運動中的合加速度數據存在大量噪聲，對步態檢測的精度有較大影響，為了降低誤差，提高精度，利用低通濾波對三軸合加速度進行平滑濾波，濾波效果如圖5所示。

圖5 經低通濾波處理后的合加速度效果

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

5 測試結果與分析

5.1 步數檢測和位移計算

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

由表1可知，步數檢測的準確度可達95%以上，具有良好的檢測精度。

表1 步數檢測結果

5.2 樓層檢測

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

(7)

式中h為當前海拔高度，p為氣壓測量值，p0為海平面所對應的氣壓值。高度每升高8.43 m，氣壓降低1 hPa。本文測試所在樓層的高度為4.3 m左右，且兩層樓中間設有樓梯轉向平臺，實測結果如圖6所示。

圖6 氣壓值的相對變化量和樓層的對應關系

經實測可知，根據氣壓值的累計變化來進行樓層檢測，可以達到半層樓的檢測精度，具有一定的實用性。

5.3 系統定位效果驗證

在上位機的位置顯示部分，本系統采用基于粒子濾波的地圖匹配算法，首先利用由Python編寫的地圖生成器生成室內建筑物的二維平面地圖，預先規定的運動路線如圖7所示。

圖7 實際測試規畫路線

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

圖8 實際測試運動軌跡

圖7中灰線為已規劃的行走路線，方向如箭頭所示，地點為河北工業大學電子信息工程學院樓四樓樓道，圖8為按照規劃路線進行軌跡追蹤的效果圖，其中方塊色起點，灰色粒子為運動過程中的實際軌跡。

6 結 論

本文的主要研究內容是通過姿態傳感器采集慣性信息，通過氣壓計采集氣壓信息，然后由樹莓派進行數據處理，把處理過的兩軸位移數據和氣壓數據通過無線通信模塊發送到上位機，由上位機進行基于粒子濾波的實時位置的顯示。系統整體結構設計簡單可靠，且能夠實現真正的實時定位，水平定位誤差達到了0.5 m以內，樓層檢測精度可以達到半層樓，有一定的應用價值。