基于多傳感器零速修正的消防員定位算法研究

牛仁杰,程 玉,陳 帥,陳 曦,廖澤俁,袁思思

（南京理工大學自動化學院,南京 210094）

0 引言

消防員在大跨度大空間的災害現(xiàn)場因迷失方向而傷亡的案例較多,例如: 2012年2月1日,蘇州工業(yè)園區(qū)廠房發(fā)生火災,1 名消防員在內(nèi)攻偵察時因導向繩被倒塌的貨物埋住而在復雜的環(huán)境中迷失方向英勇犧牲;2013年10月11日,北京石景山區(qū)蘋果園南路喜隆多商場發(fā)生火災,2 名消防員進入火場偵查,因火勢迅速蔓延,建筑突然坍塌,導致2 人在火場迷失方向壯烈犧牲。因此在消防和災難救援中,保障消防員自身安全,對救援人員進行定位是有必要的[1]。

以往的行人定位都是在開闊的室外場景,全球導航衛(wèi)星系統(tǒng)（Global Navigation Satellite System,GNSS）為救援人員提供實時、精準、可靠的定位服務。然而,衛(wèi)星定位也不是無所不能的,在高樓、隧道等環(huán)境中衛(wèi)星信號經(jīng)常受到干擾,災害過后的坍塌環(huán)境衛(wèi)星信號遮擋更嚴重,基本上無法依靠衛(wèi)星[2]。當GNSS 不能正常利用時,捷聯(lián)慣導（Strapdown Inertial Navigation System,SINS）獨特的優(yōu)點就顯現(xiàn)出來了,通過連續(xù)地采樣可以實時獲得救援人員的位置信息。

目前,鄭佳瑤等[3]對于行人導航中的航向角誤差采用有步態(tài)約束的序列檢測算法,修正了航向角的漂移,抑制了發(fā)散,其定位誤差占總路程的0.7%,有效改善了導航精度。Johann 等[4]利用建筑物的方向將人在建筑物內(nèi)的行走分為4 個或8 個主要方向,然后對航向進行約束。尹航[5]提出了利用室內(nèi)地圖修正航向的方法,采用拓撲化規(guī)則提取門、房間、走廊等在空間中具有固定關系的特征并存儲在數(shù)據(jù)庫中,在消防員行走時利用地圖匹配算法修正其位置,由于該算法以原始的空間結構圖作為室內(nèi)地圖,所以消防救災中難以實現(xiàn)。

我國在消防救援環(huán)境的個人定位研究相對較少,本文在零速檢測的基礎上設計了一種基于多傳感器零速修正的消防員定位算法。該方法以零速修正為基礎,將慣性傳感器、磁強計、高度計固連在鞋上測量腳部的運動信息,利用Kalman 濾波算法對導航誤差進行零速修正,可實現(xiàn)無信標環(huán)境下消防員的自主定位,最后對算法精度進行了驗證。

1 消防員定位算法構架

基于多傳感器輔助的消防員定位零速修正算法架構設計如圖1所示,由慣性器件輸出、磁強計輸出、高度計輸出、捷聯(lián)慣導解算單元、步態(tài)檢測單元、Kalman 濾波組成。

圖1 基于多傳感器輔助的消防員定位零速修正算法結構圖Fig.1 Structure diagram of zero-velocity update algorithm for firefighter positioning based on multiple sensors assistance

在行走過程中,消防員腳跟和腳尖完全著地時,慣性器件輸出幾乎為零,根據(jù)這一特點觸發(fā)零速檢測,結合高度計和磁強計輸出的高度信息和航向信息,設計Kalman 濾波器來對速度誤差、航向誤差、高度誤差進行最優(yōu)估計。

本文的消防員定位算法以捷聯(lián)慣導為核心傳感器,因此在慣導解算之前首先要進行初始對準,利用加速度計靜止時三個方向的加速度計輸出即可完成水平對準,計算公式為[6]

式（1）中,θ0和γ0為俯仰角和橫滾角估算值,g0為重力加速度,fbibx、fbiby、f bibz為x軸向、y軸向、z軸向的加速度計輸出。航向角φ0由校正過的磁強計單獨給出。

2 零速區(qū)間檢測算法

圖2所示為簡化版的步伐周期示意圖。步伐周期從右腳邁出階段開始,然后進入右腳落地階段,接著進入左腳邁出階段,最后進入左腳落地階段。在左腳和右腳落地的過程中,只有腳跟和腳尖完全著地階段是本文要檢測的零速時間段。

圖2 行人步伐周期示意圖Fig.2 Schematic diagram of the pedestrian stride cycle

在零速修正方面,步態(tài)檢測是至關重要的一個環(huán)節(jié)。為了保證檢測的準確性,需要同時利用加速度計和陀螺儀輸出的數(shù)據(jù),采用兩個條件（T1和T2）綜合判斷運動狀態(tài),以狀態(tài)“0” 表示運動,狀態(tài)“1” 表示靜止。

條件1（T1）: 加速度計輸出合成幅值,處于給定的上下閾值之間則人體靜止。

定義加速度計的輸出合成幅值為[7]

式（2）中,abkx、abky、abkz分別為加速度計輸出的加速度。

定義給定的閾值為amin=9m/s2和amax=10.9m/s2,并有

條件2（T2）: 陀螺儀輸出合成幅值,低于給定閾值則人體靜止。

定義陀螺儀的輸出合成幅值為[7]

式（4） 中,ωbkx、ωbky、ωbkz分別為陀螺儀輸出的角速度。

給定的閾值為ωmax=10（°） /s,并有

兩個條件之間采用“與” 邏輯,即只有當兩個條件的判斷結果都為“1” 時才認為目標處于靜止狀態(tài)。

從實際步行實驗采集到的數(shù)據(jù)來看,加速度計和陀螺儀的輸出合成幅值和曲線并不光滑,如圖3所示。這是因為安置在腳上的微慣性器件不僅僅隨著腳步做前進且周期性的交替運動,同時它還存在高頻次的左右向和上下向的生理抖動。對這樣的加速度計和陀螺儀信號進行步態(tài)檢測很容易被包含在其中的雜波信息和局部極值點影響造成誤判,解決方法是先對加速度計和陀螺儀進行濾波,再對平滑后的曲線辨識。本文選用Savitzky-Golay 濾波器對數(shù)據(jù)進行平滑處理,圖4為用原始數(shù)據(jù)和濾波后的數(shù)據(jù)分別進行邏輯判斷的對比,可以看出使用Savitzky-Golay 濾波器濾波后的數(shù)據(jù)擁有更好的判斷效果。

圖3 加速度計和陀螺儀的輸出合成幅值Fig.3 Output synthesis amplitude of accelerometer and gyroscope

圖4 零速區(qū)間檢測結果Fig.4 Test results of zero-velocity range

對陀螺儀輸出合成幅值濾波時,Savitzky-Golay濾波器的滑動窗口寬度為71,多項式階次為3 階,濾波前后對比如圖5所示。對加速度計輸出合成幅值濾波時,Savitzky-Golay 濾波器的滑動窗口寬度為85,多項式階次為3 階,濾波前后對比如圖6所示。由圖5、圖6可知,濾波之后傳感器原始數(shù)據(jù)的高頻噪聲明顯減少,為步態(tài)檢測的準確度奠定基礎。

圖5 濾波前后陀螺儀輸出合成幅值對比Fig.5 Comparison of gyroscope output synthesis amplitude before and after filtering

圖6 濾波前后加速度計輸出合成幅值對比Fig.6 Comparison of accelerometer output synthesis amplitude before and after filtering

3 Kalman 濾波器

對系統(tǒng)狀態(tài)量進行分析是設計Kalman 濾波器的前提,零速修正的Kalman 濾波狀態(tài)方程依據(jù)慣性導航誤差方程推導而來,慣性導航誤差方程可以細分為姿態(tài)誤差方程、速度誤差方程與位置誤差方程,在導航坐標系（n）即地理系下可表示為[8]

式（6）中,、、分別為失準角誤差、速度誤差、位置誤差對時間的導數(shù),和δωnin為導航系相對慣性系的角速度及其誤差在導航系下的投影,ωnie和δωnie為地心地固坐標系相對慣性系的角速度及其誤差在導航系下的投影,f n為比力在導航系下的投影,δf b、δωb、δg分別為載體系下加速度計量測誤差、陀螺儀量測誤差以及擾動重力誤差,為導航系相對地球系的角速度在導航系下的投影。

在零速修正時由于載體相對地面靜止,故可將其簡化為

系統(tǒng)的狀態(tài)方程為

式（8）中,X（t）為由SINS 誤差變量構成的15 維狀態(tài)向量,F（t）和G（t）為關于時間參數(shù)t的確定性時變矩陣,W（t）為零均值Gauss 白噪聲向量。

系統(tǒng)的狀態(tài)向量設計為

式（9）中,φe、φn、φu分別為捷聯(lián)式慣性導航系統(tǒng)在東北天上的姿態(tài)角誤差,δve、δvn、δvu分別為捷聯(lián)式慣性導航系統(tǒng)在東北天上的速度誤差,δL、δλ、δh分別為捷聯(lián)式慣性導航系統(tǒng)緯度、經(jīng)度、高度的位置誤差,δωbx、δωby、δωbz分別為陀螺儀在x、y、z三個軸向上的隨機常值誤差,δf bx、δfby、δfbz分別為加速度計在x、y、z三個軸向上的一階Markov 偏移誤差[9]。零速修正的15 維狀態(tài)模型為

本文將純慣性解算的航向角與磁強計航向的差值、純慣性解算的速度vins與零速的差值、純慣性解算的高度與高度計的差值作為系統(tǒng)量測向量,系統(tǒng)量測方程為

式（11）中,φins為捷聯(lián)慣導輸出的航向角信息,φc為磁強計輸出的航向角信息,vinse、vinsn、vinsu為捷聯(lián)慣導輸出的地理系下載體在東北天上的速度信息,hins為捷聯(lián)慣導輸出的高度信息,ha為高度計輸出的高度信息,H（t）為量測矩陣,V（t）為量測噪聲。

定義

則式（11）中,

通過時間更新可以得到估計協(xié)方差為

式（14）中,Φk,k-1為k-1 時刻到k時刻的系統(tǒng)一步轉移矩陣;Γk-1為系統(tǒng)噪聲矩陣,表征由k-1時刻到k時刻的各個系統(tǒng)噪聲分別影響k時刻各個狀態(tài)的程度;Qk-1為系統(tǒng)噪聲的方差矩陣。

在量測更新過程中,根據(jù)誤差協(xié)方差Pk|k-1算出Kalman 濾波增益Kk,然后再結合當前時刻的測量值Zk進行狀態(tài)估計更新。

k時刻的Kalman 濾波增益為

式（15）中,Rk為量測噪聲的方差矩陣。

則狀態(tài)估計為

估計誤差協(xié)方差更新為

4 實驗與分析

為了驗證本文所述基于多傳感器零速修正的消防員定位算法的可靠性和有效性,共設計了兩組半物理仿真實驗。

如圖7所示,實驗1 將慣導、磁強計、高度計固聯(lián)在測試者腳面,以NovAtel-KVH1750 作為基準,通過usb 接口和計算機相連接,通過數(shù)據(jù)采集上位機采樣數(shù)據(jù),后期用Matlab 對數(shù)據(jù)進行仿真處理,設備性能指標如表1所示。本算法的實驗場景選為南京理工大學標準400m 操場,測試者穿戴好裝置,在實驗場地按照逆時針行走一周,黑色實心圓為初始點,向北出發(fā),行走路徑是一個封閉環(huán)形。

圖7 算法驗證設備和行動軌跡Fig.7 Diagram of algorithm verification devices and action trajectory

表1 實驗設備性能指標Table 1 Performance indexes of experiment devices

實驗中,給定陀螺儀的輸出合成幅值閾值為ωmax=10（°） /s,加速度計的輸出合成幅值閾值為amin=9m/s2和amax=10.9m/s2,行走時長約800s,初始對準時長約25s。由圖8可知,傳統(tǒng)ZUPT 算法航向角由于其不可觀測的特性容易發(fā)散,水平誤差為19.74m,約為總路程的4.93%,加入磁強計修正后水平誤差降至2.58m,約為總路程的0.65%,誤差比小于1%D。由圖9可知,加入高度計約束之后,高度誤差減小為原來的16.33%,從而驗證了本文算法的有效性和可行性。

圖8 傳統(tǒng)零速修正、本文算法和基準位置對比Fig.8 Comparison of traditional zero-velocity update,proposed algorithm and benchmark position

圖9 高度誤差對比Fig.9 Comparison of height error

由圖10可知,橫滾角和俯仰角變化很小,航向角由于磁強計的修正變化趨勢正常,經(jīng)歷了0°～180°（-180°）～0°的變化,充分驗證了磁強計修正航向的可行性。圖11為測量的加速度和角速度數(shù)據(jù)（為方便查看,截取前100s 數(shù)據(jù)）。

圖10 行人實時姿態(tài)Fig.10 Diagram of pedestrian real-time attitude

圖11 加速度和角速度測量結果Fig.11 Measurement results of acceleration and angular velocity

實驗2 在室內(nèi)環(huán)境下按照“8” 軌跡行走,實驗行走路徑大約75m,如圖12所示。黑色實心圓為初始點向上出發(fā),經(jīng)過一個“8” 字形從左側繞回來,“?” 為終點結束實驗,行走路徑是一個封閉的雙環(huán)形,場地距離用卷尺進行測量。綠色曲線為傳統(tǒng)零速修正算法軌跡,由于沒有磁強計約束,因此在結尾處航向角有偏移,軌跡起點與終點未能重疊;紅色曲線為本文算法軌跡,由于加上了磁強計的約束,軌跡起點與終點重疊。

圖12 室內(nèi)行人軌跡對比Fig.12 Comparison of indoor pedestrian trajectories

由圖13可知,橫滾角和俯仰角變化很小,航向角由于磁強計的修正變化趨勢正常,經(jīng)歷了0°～90°～0°～-90°～180°（-180°）～90°～180°（-180°）～-90°的變化,驗證了消防員在任意方向行走時航向角依然正確。

圖13 室內(nèi)行人實時姿態(tài)Fig.13 Diagram of indoor pedestrian real-time attitude

5 結論

消防員在大跨度大空間的災害現(xiàn)場因迷失方向而傷亡的案例較多。因此,本文針對消防救援時出現(xiàn)衛(wèi)星拒止導致定位精度差的問題,在傳統(tǒng)零速修正的基礎上增添磁強計、高度計進行輔助定位,解決了傳統(tǒng)算法航向角和高度隨時間發(fā)散的問題,可實現(xiàn)無信標環(huán)境下消防員自主定位。通過操場和室內(nèi)兩種環(huán)境下的半實物仿真實驗對算法有效性進行驗證,結果表明: 本文的多傳感器零速修正定位算法在復雜環(huán)境中精度效果好,魯棒性高,誤差比不超過1%D。