基于多傳感器的室內三維定位算法研究*

陳友榮,陳 浩,鐘陳健,劉半藤,任條娟,周駿華

(1.浙江樹人大學信息科技學院,杭州 310015;2.常州大學信息科學與工程學院,江蘇 常州 213164; 3.中移(杭州)信息技術有限公司,杭州 311100)

近年來,城市的建筑結構日益復雜,這給消防員的偵察、滅火和搜救等工作帶來極大的挑戰。在復雜的建筑結構下開展救援行動,需要獲知消防員的實時位置信息,從而保障消防員的自身安全和協調整體救援行動。但是目前在日常救援過程中,消防員和指揮員只采用對講機進行人工對話,簡單收集消防員的位置和所在位置的火情信息,沒有考慮消防員的精準定位問題,指揮員和消防員難以準確獲知各自的位置坐標[1]。因此需要一種現場救援環境下的消防員實時定位技術。

目前實時定位主要分為室內定位和室外定位。全球衛星定位系統已經廣泛應用到室外目標的定位和跟蹤。但是由于衛星信號在室內嚴重衰減,衛星定位系統不能應用到室內環境,而且較多購物中心、大型超市、地下車庫等建筑物內部結構復雜,室內人員容易迷路,因此對室內精準定位的需求較大,且受到學術界和產業界的高度關注。目前較多學者側重于利用WIFI信號、攝像頭信號、無線信號、慣性導航等信號,研究室內二維定位算法[2-3],如文獻[4]提出了基于零速度更新的腳踏式慣性導航算法FINS(Foot-mounted INS algorithm),實現自身的二維定位。文獻[5]提出基于鞋上慣性傳感器的室內步行導航算法IPN_S(Indoor Pedestrian Navigation algorithm with shoe-mounted inertial sensors),采用零速度更新和9維擴展卡爾曼濾波器估計自身二維定位。一部分學者側重于利用單個或多個信號研究室內三維定位算法,如利用現場部署攝像頭的圖像數據[6],激光信號[7],聲音信號[8],LED燈光信號[9]和超寬帶信號[10]等單一信號實現室內三維定位。文獻[11]利用地磁信號、轉向角傳感器和氣壓傳感器等傳感器融合計算三維定位。文獻[12]提出一種三維室內定位算法IPA_3D(3D Indoor Positioning Algorithm),即利用加速度計、陀螺儀和氣壓計等低成本傳感器,采用15維擴展卡爾曼濾波器融合計算室內人員的相對三維位置。

目前關于消防員的室內定位算法的研究結果非常少,主要集中在RFID、慣性導航方面的定位算法研究。如文獻[13-14]采集所有消防員的信息,采用集中式方法計算消防員的位置。集中式方法需要收集所有傳感節點信息。文獻[15-16]利用RFID[15]、聲音[16]等信號對室內人員進行定位。文獻[17]考慮野外火災救援場景,考慮部署在貨車上的基站設備和消防員身上的beacon節點,根據基站設備信號計算自身位置坐標。

由表1可知,上述算法存在一定缺點,不能有效解決缺電、出現大量濃煙、烈火等火災現場下的室內定位問題。因此在上述文獻的基礎上,結合三軸加速度計、三軸陀螺儀、三軸磁力計、氣壓計等多個傳感器,提出一種基于多傳感器的室內三維定位算法ITPA(Indoor Three-dimensional Positioning Algorithm based on multi-sensor)。即當消防員進入室內進行救援行動時,通過氣壓計輸出數據和Kalman濾波,計算消防員所在的高度和樓層,判斷平地行走、上下樓梯、上下電梯等行為。通過三軸加速度計、三軸陀螺儀和三軸磁力計的數據,采用過零檢測識別消防員靜止和運動情況,并通過捷聯慣導處理和航向角計算移動偏移的坐標。最終選擇第一層的樓梯位置作為原點,以樓梯為參照點,采用Kalman融合算法計算消防員的三維位置坐標。總之,當消防員處于復雜室內環境下運動時,本算法不僅能夠實時判別其行為情況,還能計算3維位置,可降低定位誤差,同時解決火災復雜環境下室內消防員的定位問題,為其他算法提供位置信息。

表1 室內定位算法分析

1 定位算法原理

基于多傳感器的室內三維定位算法的原理如圖1所示。首先將加速度計等傳感器綁在腳上獲取消防員行走的各個數據。其次通過氣壓計的值,采用Kalman濾波算法對當前的高度數據進行濾波,并通過行為閾值判斷方法判斷平地行走、上下樓梯、上下電梯等行為。由于三軸加速度計、三軸陀螺儀和三軸磁力計的輸出數據存在漂移誤差,即當消防員在靜止的狀態下仍輸出明顯波動的數據,因此采用過零檢測判斷消防員是否處于靜止狀態,如果處于靜止狀態,則將該三個傳感器的輸出值歸零。如果不為零,則把加速度計,陀螺儀和磁力計數據經過捷聯慣導計算處理,同時結合航向角數據,計算平面二維位置信息。最后采用Kalman融合估計消防員的三維定位坐標,從而提高定位的準確度。但是ITPA算法仍需要解決以下3個問題:①如何通過氣壓計判斷當前的高度、移動行為和當前樓層;②如何根據三軸加速度計、三軸陀螺儀和三軸磁力計,計算當前平面的二維坐標;③如何采用Kalman融合算法融合估計當前信息,獲取消防員的室內三維坐標,這三個問題的具體解決如下。

圖1 室內三維定位算法原理

1.1 傳感器數據輸出

1.2 過零檢測

在消防員行走過程中,每個跨步中腳都要經歷平穩、邁步、再次平穩的周期性變化過程。但是當消防員的腳處于平穩狀態時,傳感器采集到的數據仍會出現一定的漂移,會影響后續的定位效果,提高了定位誤差。因此考慮根據加速度計和陀螺儀輸出的信息,考慮加速度的幅值均方根、加速度的幅值方差和角速度的幅值均方根的情況,實現過零監測,判斷消防員是否靜止在地面上,從而控制和消除數據漂移誤差。具體條件如下:

t時刻的合加速度值為

(1)

其中,at表示t時刻的三個方向的合加速度大小。

(2)

(3)

其中,C1表示加速度幅值均方根條件的判斷指示符,thmin和thmax表示根據實際情況設置的幅值均方根條件最小和最大閾值。

令平均加速度為

(4)

(5)

加速度的幅值方差條件為

(6)

其中,C2表示加速度幅值方差條件的判斷指示符,thσ-min表示根據實際情況設置的幅值方差閾值。

令角速度的幅值均方根為

(7)

(8)

其中,C3表示角速度的幅值均方根條件的判斷指示符號,thw-max表示根據實際情況設置的角速度閾值。

上述3個條件中,C1表示加速度是在一定的范圍內,接近重力值。C2表示加速度的波動不能太大,保持穩定。C3表明角速度小于閾值,沒有發生轉動。只有當C1,C2和C3同時為1時,才判定消防員的腳落在地面上,沒有移動,此時腳處于平穩態。

1.3 高度和移動行為判斷

高度和移動行為判斷包括高度獲取,卡爾曼濾波和異常數據處理、移動行為判斷和修正3個方面。

1.3.1 高度獲取

采用氣壓計獲取消防員位置的高度信息,并結合該高度信息判斷移動行為和所在樓層。即根據大氣壓力,計算海拔高度。

(9)

其中,ρt表示t時刻消防員所處位置的大氣壓力值,ρ0表示標準大氣壓力值1.013 25×105Pa,Ht表示t時刻消防員所處的海拔高度,單位m。

由于三維定位所需的是相對海拔高度值,因此采用以下公式計算Δht。

Δht=Ht-h0

(10)

其中,Δht表示t時刻消防員所處的相對海拔高度,h0表示起始位置的海拔高度。

1.3.2 卡爾曼濾波和異常數據處理

由于相對海拔高度值Δht中具有一定噪聲,因此需要對Δht進行卡爾曼濾波處理,其所對應的空間模型為:

(11)

建立觀測方程為:

(12)

(13)

結合時間更新方程和測量更新方程進行迭代計算,獲得濾波后的相對海拔高度值。

通過過零檢測可將每一腳步劃分為邁步期與平穩期兩種狀態。由于氣壓計存在一定的不穩定性,且考慮腳步邁步期的高度變化較大,因此根據過零檢測的結果,分析當前時刻是否處于平穩期。如果處于平穩期且該步平穩期內Kalman濾波器輸出的相對海拔高度數據個數大于或等于100個,選擇100個相對海拔高度值,否則如果該步平穩期較短且開始進入邁步期,即相對海拔高度數據個數小于100個時,選擇該步平穩期內的所有相對海拔高度數據。采用箱型圖方法對所選擇的相對海拔高度數據進行異常點檢測。令H表示所選擇的相對海拔高度數據集合。對集合H內所有元素進行排序,選擇排序第25%的相對海拔高度值作為上四分位值U,選擇排序第75%的相對海拔高度值作為下四分位值L,即集合H內只有1/4個數大于U,只有1/4個數小于L。則定義上界高度和下界高度為

hupper=U+1.5(U-L)

(14)

hlower=U-1.5(U-L)

(15)

其中,hupper表示上界高度數值,hlower表示下界高度數值。

(16)

(17)

1.3.3 移動行為判斷和修正

根據經過濾波和異常點處理后的高度數據,判斷消防員的移動行為。為區分平地行動、上下樓梯和上下電梯的行為,引入λh和μh兩個正閾值。如果前后兩個時刻的平均高度變化的差值不大于λh,則表示消防員在平面上行動。差值大于λh,則表示消防員在走樓梯或乘坐電梯。而μh區分消防員是在走樓梯還是乘坐電梯。具體算法偽代碼如下:

1. if |h′t-h′t-1|≤λhthen在平面上行動

2. else if (h′t-h′t-1)≥μhthen乘坐電梯且上行

3. else if (h′t-h′t-1)≤-μhthen乘坐電梯且下行

4. else if (h′t-h′t-1)>λhthen向上走樓梯

5. else向下走樓梯

6. end if

在實際的判斷過程中,由于沒有考慮以下情況,會出現錯誤判斷,需要加入一定的結果修正,從而保證判斷的準確度。

情況1電梯啟動是一個慢慢加速過程。在電梯啟動和停止部分會出現上下樓梯的錯誤判斷。修正辦法:在判斷行為為乘坐電梯后,則計算開始乘樓梯行動的判斷部分的時間長度。若時間長度大于η1,則判斷消防員的行為為乘坐電梯,將在開始,中間和結束階段中走樓梯的判斷結果直接修正為乘坐電梯。如果在結束部分出現在平面上行動的判斷結果,則結束修正。

情況2移動行為判斷為上下樓梯,但在中間部分可能會出現乘坐電梯的誤判斷。修正辦法:在判斷行為為上下樓梯后,則計算上下樓梯行動的判斷部分的時間長度。若時間長度大于η2,則判斷消防員的行為為上下樓梯,將在開始,中間和結束階段中乘坐電梯的判斷結果直接修正為上下樓梯。如果在結束部分出現在平面上行動的判斷結果,則結束修正。

情況3移動行為判斷為上下樓梯,中間部分會出現在平面上行動的判斷,無法區分是在樓梯上停留還是錯誤判斷。修正辦法:計算平面上行動的判斷部分的時間長度。若時間長度大于η3,則保留判斷結果,否則修正為上下樓梯行為。

情況4移動行為判斷為平面上行動,中間部分會出現部分判定為上下樓梯或乘坐電梯的錯誤判斷。修正辦法:如果移動行為判斷為平面上行動且中間部分出現上下樓梯或乘坐電梯,則計算上下樓梯或乘坐電梯判斷部分的時間長度。若時間長度小于η4,則修正為平面上行動。

1.4 二維坐標計算

二維坐標計算包括移動距離計算和位置獲取兩個方面。

1.4.1 移動距離計算

考慮到定位對象是消防員,結合其日常工作環境分析發現:消防員在救災現場不可能像普通行人那樣走路,他們的動作更復雜,跑步、不平坦的地勢和擺動幅度大的行走是消防員不可避免的。因此提出一種新的移動距離計算方法,它除了能計算普通行人的移動距離外,更適用于像消防員這類人的移動距離計算。下面就是這種算法的步驟:

(18)

(19)

其中,q0、q1、q0、q0表示四元數。通過加速度計和陀螺儀兩個傳感器,采用畢卡逼近法更新四元數,可獲得當前時刻的旋轉矩陣。

(20)

其中,T是采樣周期。

步驟3修正速度。根據過零檢測的結果,分析當前時刻是否處于平穩期。如果處于平穩期且該步平穩期內的速度數據個數大于或等于100個,選擇100個速度數據,否則如果該步平穩期較短且開始進入邁步期,即速度數據個數小于100個,選擇該步平穩期內的所有相對速度數據。令Vpi表示所選擇平穩期的速度數據集合,計算Vpi的速度平均值,即

(21)

根據平均速度βt,通過式(22)修正邁步期的速度。

(22)

根據平均速度βt,修正平穩期的速度為:

(23)

步驟4計算位移增量ΔPt:

(24)

步驟5計算平面上移動距離:

(25)

其中,SLt表示第t時刻的移動距離。

循環執行步驟1～步驟5,則可以獲得在二維平面上移動的距離。

1.4.2 位置獲取

假定載體坐標系的X軸與腳相一致后,根據陀螺儀和磁力計兩個傳感器輸出的值,采用互補濾波修正航向角,并通過移動距離計算自身二維位置。即:

(26)

(27)

(28)

其中,θs=arctan(|T22/T12|)。

1.5 Kalman融合估計

在獲得高度值和二維坐標的信息后,采用Kalman融合算法將信息進行融合處理。令Xt表示t時刻Kalman融合算法的狀態向量估計值,由位置、速度、姿態角、加速度和角速度構成,即Xt=[rt,ct,φt,bt,ηt]T,其中rt表示3維位置估計值,ct表示3維速度估計值,φt表示3維姿態角估計值,bt表示3維加速度計零偏誤差值,ηt表示3維陀螺儀零偏值。其所對應的空間模型為:

Xt=FtXt-1+Γt-1Wt-1

(29)

其中,Xt表示t時刻融合濾波狀態向量的估計值,Ft表示狀態轉移矩陣,Wt-1表示t-1時刻的過程噪聲,Γt-1表示系統噪聲增益矩陣。Ft可表示為:

(30)

其中,ξ表示t時刻的旋轉矩陣,ψ表示加速度an的反對稱矩陣,Δt表示采樣間隔,I3×3表示3×3單位矩陣,O3×3表示全為0的3×3矩陣。ψ可表示為:

(31)

系統噪聲增益矩陣Γt-1為

(32)

建立觀測方程為:

Zt=HtXt+nt

(33)

其中,Zt表示t時刻融合濾波的測量值,nt表示t時刻的測量噪聲,Ht表示濾波算法的測量矩陣。Ht可表示為:

Ht=[I3×3ΔtI3×3O3×3O3×3O3×3O3×3]

(34)

2 算法實現

本算法可直接應用到消防員上的嵌入式設備中,實現消防員的實時三維定位。如圖2所示,其三維定位算法的具體實現步驟如下:

步驟1初始化各個參數,包括起始位置的海拔高度、橫軸、縱軸和豎軸的方向、二維平面坐標和航向角等信息。

步驟2實時采集三軸加速度計、三軸陀螺儀、三軸磁力計的姿態傳感器和氣壓計數值,并存儲到存儲器中。

步驟3根據加速度計和陀螺儀輸出的信息,計算當前時刻的加速度幅值均方根條件C1、加速度幅值方差條件C2和角速度幅值均方根條件C3。如果C1,C2和C3的值都為1時,判斷當前時刻處于平穩期,否則處于邁步期。

步驟4根據當前的氣壓計值,參考起始位置的海拔高度,計算相對海拔高度值。采用卡爾曼濾波算法對高度數據進行濾波,消除噪聲。

步驟5根據過零檢測的結果,分析當前時刻是否處于平穩期。如果處于平穩期且該平穩期內卡爾曼濾波后輸出的相對海拔高度數據個數大于或等于100個,則選擇該100個相對海拔高度數據,否則如果處于平穩期且該平穩期內卡爾曼濾波后輸出的相對海拔高度數據個數小于100個,則直接選擇該平穩期內所有相對海拔高度數據,否則直接跳到步驟2。采用箱型圖方法對所選擇的相對海拔高度數據進行異常點檢測和修正,將這些相對海拔數據的平均值作為當前時刻的消防員的高度值,跳到步驟6;

步驟6根據前后高度值,識別平面行動、上下樓梯和上下電梯等消防員的行為,并對識別結果進行修正,從而保證判斷的準確度。

步驟7根據加速度計和陀螺儀的值,計算和修正當前二維的移動距離。根據陀螺儀和磁力計的值,采用互補濾波修正航向角,并通過移動距離計算自身的二維位置坐標。

步驟8采用Kalman融合方法對信息進行融合處理,計算和輸出消防員的室內三維坐標。

圖2 工作流程圖

步驟9如果消防員手動結束室內三維定位功能,則退出,否則跳到步驟2。

循環執行步驟2～步驟9,實現消防員室內實時三維定位。

3 算法實驗

3.1 實驗參數

在實驗過程中,選擇MPU9025姿態傳感器和MS5611氣壓計。將上述兩個模塊固定在實驗人員的腳部,并通過藍牙模塊、STM32開發板和上位機軟件,實現實驗人員的運動數據采集,且每次測試時記錄下經過測量點的真實步數,位置和移動距離等信息,方便后續誤差分析。MPU9250姿態傳感器由3軸加速度、3軸陀螺儀和3軸磁力計組成。其中3軸陀螺儀輸出具有三個可選擇量程(±250 °/s,±500 °/s,±1 000 °/s)的16 位三軸角速度數據;3軸加速度輸出具有三個可選擇量程(±2gn,±4gn,±8gn,±16gn)的16位三軸加速度數據;3軸磁力計輸出具有14位和16位分辨率的磁力計數據,同時最大測量范圍在±4 800 μT。MS5611氣壓計內置24位模數轉換器,支持IIC和SPI通訊協議,測量/工作輸出范圍:1 000 mPa～1 200 mPa,分辨率10 cm。根據實驗人員的實測運動數據,選擇以下參數,通過MATLAB軟件計算消防員室內三維位置、步數誤差、距離誤差和漂移誤差:thmin為0,thmax為1,thσ-min為1,thw-max為1,λh為0.14,μh為0.6,nt、wt-1、qt和wt-1為隨機高斯噪聲的協方差,采樣周期T為0.01 s。

其中,定義步數誤差errorzupt為計算所得的當前步數和真實步數差值的絕對值與真實步數的比值,可表示為:

errorzupt=(|steptrue-stepcal|)/steptrue

(35)

其中,steptrue表示測量過程中的真實步數。stepcal表示算法計算所得的步數。定義移動距離誤差errordis為計算所得的移動距離和真實距離差值的絕對值與真實距離的比值,可表示為:

errordis=(|distrue-discal|)/distrue

(36)

其中,distrue表示測量過程中的真實移動距離。discal表示算法計算所得的移動路程。定義漂移誤差 errordrift為計算所得的三維坐標和真實三維位置坐標差值的模值與真實距離的比值,可表示為:

(37)

其中,(xtrue,ytrue,ztrue)表示測量過程中的真實坐標。(xcal,ycal,zcal)表示算法計算所得的坐標。

3.2 實驗結果分析

選擇3.1節中參數,比較FINS[4],IPN_S[5],IPA_3D[12]和ITPA算法的消防員室內三維位置、步數誤差、距離誤差和漂移誤差。其中FINS和IPN_S算法是二維定位算法,在算法比較中直接選擇ITPA算法的高度計算方法計算其高度值,從而獲得三維位置坐標。

3.2.1 直線行走

如圖3所示,起始點矩形經過11個中間測量點(菱形),到達終點測量點圓形。真實軌跡是朝一個方向直線行走30 m。從圖中可以發現軌跡在x軸方向上穩定增加,而在y軸方向上幾乎沒有出現明顯的波動,較接近真實路線。

圖3 直線行走的定位坐標圖

如圖4所示,ITPA算法的直線行走步數誤差在8%以下,遠低于FINS,IPN_S和IPA_3D的直線行走步數誤差。這是因為:FINS,IPN_S和IPA_3D算法采用單個條件或結合瞬時時刻的數據分析行人行走的邁步態和平穩態,從而判斷其行走步數,而ITPA算法選取加速度的幅值均方根、加速度的幅值方差和角速度的幅值均方根的情況,實現過零監測,降低了步數判斷的誤差。

圖4 直線行走的步數誤差

如圖5所示,ITPA算法的直線行走距離誤差低于FINS,IPN_S和IPA_3D的直線行走距離誤差,但是當移動距離較大時,ITPA算法和FINS算法的距離誤差相差不大。這是因為:在直線行走時,ITPA算法和FINS算法基本上采用相同的方法,但是由于ITPA算法根據平穩期的平均速度修正邁步期和平穩期的速度,從而獲得較接近實際的速度,降低了距離誤差,因此其距離誤差略低于FINS算法的距離誤差。IPN_S 和IPA_3D采用靜止時的速度作為觀測量修正位置,其距離誤差略高。

圖5 直線行走的距離誤差

如圖6所示,ITPA算法的直線行走漂移誤差低于FINS,IPN_S,IPA_3D的直線行走漂移誤差。這是因為:ITPA算法的步數判斷較準確,高度且2維坐標獲取的誤差較小,同時采用Kalman融合算法對由位置、速度、姿態角、加速度和角速度構成的狀態向量進行融合估計,降低了系統噪聲,并有效防止誤差的累計。

圖6 直線行走的漂移誤差

3.2.2 樓梯行走

如圖7所示,起始點(矩形),先向上經過一段樓梯和中間平臺,再換一個方向向上經過另一段樓梯,最終行走到上一層樓,即經過11個中間測量點(菱形),到達終點測量點(圓形)。從圖中可以發現軌跡在z軸上穩定增加,而在x軸和y軸上有規律的變化,其行走的路線較接近真實路線。

圖7 樓梯行走的定位坐標圖

如圖8所示,ITPA算法的樓梯行走步數誤差遠低于FINS,IPN_S和IPA_3D的樓梯行走步數誤差。這是因為:ITPA算法選取加速度的幅值均方根、加速度的幅值方差和角速度的幅值均方根作為消防員邁出一步的判斷標準,不受高度對步數判斷的影響,其步數誤差與直線行走的步數誤差相差不大。而FINS,IPN_S和IPA_3D只是考慮部分條件,走樓梯的行為對其步數判斷影響較大,其步數誤差明顯高于直線行走的步數誤差。

圖8 樓梯行走的步數誤差

如圖9所示,ITPA算法通過分析人行走時的狀態,及時對不同階段的速度進行修正,同時根據高度值判斷行人所在樓層高度,并及時修正樓層高度,并根據移動行為對三維坐標觀測值進行修正,有效降低了累計誤差,同時結合樓層高度和移動行為進行數據融合,降低了數據的漂移對算法影響,因此ITPA算法的樓梯行走距離誤差低于FINS,IPN_S和IPA_3D的樓梯行走距離誤差,且與其直線行走距離誤差相差不大。但是FINS,IPN_S和IPA_3D的樓梯行走距離誤差比其直線行走距離誤差提高較多。

圖9 樓梯行走的距離誤差

如圖10所示,FINS和IPN_S算法是二維定位,其高度計算方法采用ITPA算法的高度計算方法,對氣壓計的輸出值進行Kalman濾波和異常數據處理等,提高了高度精度,因此其漂移誤差遠低于IPA_3D算法。由于樓梯上行走x和y軸的變化相對不大,主要是在z軸上逐漸增加,因此FINS,IPN_S和ITPA算法的漂移誤差相差不大,且與其各自直線行走的漂移誤差提高不多。由于ITPA算法采用了Kalman融合估計,并根據移動行為對觀測值進行修正,從而降低了算法的漂移誤差,因此ITPA算法略低于FINS和IPN_S算法。

圖10 樓梯行走的漂移誤差

3.3.3 綜合行走

如圖11所示,起始點(矩形),先沿樓梯行走至第二層,再在該層樓行走一個矩形后,經過樓梯返回至第一層,即經過11個中間測量點(菱形)到達終點測量點(圓形)。從圖11可以發現軌跡在樓梯行走的時候z軸上穩定增加,而x軸和y軸上有規律的變化,在平面行走時,z軸基本無變化,且其計算路徑較接近真實路線。

圖11 綜合行走的定位圖

圖12 綜合行走的步數誤差

如圖12所示,ITPA算法不受行走路徑的影響,其綜合行走的步數誤差低于FINS,IPN_S和IPA_3D的綜合行走步數誤差,且比樓梯行走和直線行走的步數誤差略高。IPN_S和IPA_3D受高度變化的影響較大,其綜合行走步數誤差遠大于樓梯行走和直線行走的步數誤差。其原因與樓梯行走的類似,請參考3.2.2節。

如圖13所示,ITPA算法不受行走路徑的影響,其綜合行走距離誤差遠低于FINS,IPN_S和IPA_3D的綜合行走距離誤差,且與其樓梯行走和直線行走的距離誤差相差不大。FINS,IPN_S和IPA_3D的綜合行走距離誤差比其樓梯行走和直線行走的距離誤差提高較多,其中IPN_S達到200%以上,距離誤差較大。其原因與樓梯行走的類似,請參考3.2.2節。

圖13 綜合行走的距離誤差

如圖14所示,IPA_3D算法的漂移誤差較大,遠大于其樓梯行走和直線行走的漂移誤差。FINS,IPN_S和ITPA算法選用相同的高度計算方法,其綜合行走漂移誤差相對較小,其中ITPA算法的綜合行走漂移誤差最小,且與其樓梯行走和直線行走的漂移誤差相差不大。其原因與樓梯行走的類似,請參考3.2.2節。

圖14 綜合行走的漂移誤差

4 總結

利用氣壓計和包含三軸加速度計、三軸陀螺儀和三軸磁力計的姿態傳感器數據,提出一種基于多傳感器的室內三維定位算法。首先,提出了傳感器數據輸出和過零檢測方法,其次,根據氣壓計數據,獲得海拔值,并采用卡爾曼濾波和異常數據處理等方法,獲得消防員的當前高度,并提出消防員移動行為判斷和修正方法。接著,通過移動距離計算和位置獲取,計算二維坐標,并采用Kalman融合估計方法,利用移動行為修正觀測值,降低定位誤差。最后給出算法的仿真參數,比較直行行走、樓梯行走和綜合行走的FINS,IPN_S,IPA_3D和ITPA算法的性能。

總之,在直行行走、樓梯行走和綜合行走下,ITPA算法都能獲得較接近真實路線的移動軌跡和消防員室內三維位置,降低了算法的步數誤差、距離誤差和漂移誤差。但是算法目前根據真實實驗數據進行仿真分析和比較,因此下一階段目標是將在嵌入式設備上實現ITPA算法,實現算法的應用。