基于改進(jìn)零速檢測(cè)方法的室內(nèi)定位系統(tǒng)①

郭春麗,毛素梅,黃瑞航,丁建旭

1(廣東財(cái)貿(mào)職業(yè)學(xué)院 信息技術(shù)學(xué)院,廣州 510445)

2(中科云圖智能科技有限公司,廣州 510030)

3(廣州特種機(jī)電設(shè)備檢測(cè)研究院 國(guó)家防爆設(shè)備質(zhì)量監(jiān)督檢驗(yàn)中心,廣州 510180)

導(dǎo)航定位服務(wù)具有很好的實(shí)用價(jià)值和應(yīng)用前景[1].當(dāng)前常用的定位產(chǎn)品,大部分采用非自主式定位技術(shù),這類技術(shù)主要包括衛(wèi)星導(dǎo)航、WiFi、藍(lán)牙、超寬帶、蜂窩移動(dòng)網(wǎng)絡(luò).在復(fù)雜的建筑物或礦井里,衛(wèi)星導(dǎo)航信號(hào)易被遮擋或缺失,提供的定位服務(wù)精準(zhǔn)度較低;WiFi、藍(lán)牙都是利用接收端與信號(hào)源的信號(hào)衰減強(qiáng)度換算成兩者之間的距離,再采用三角定位法對(duì)位置進(jìn)行估算,但在復(fù)雜多變的室內(nèi)環(huán)境中很難建立準(zhǔn)確的信號(hào)衰減模型;超寬帶利用脈沖信號(hào)在接收端與發(fā)射端的時(shí)間差來測(cè)量它們的距離,再采用三角定位法估算位置,在視距范圍內(nèi)可達(dá)厘米級(jí)定位精度,但超寬帶設(shè)備價(jià)格昂貴,部署成本較高;隨著5G 網(wǎng)絡(luò)技術(shù)的大力發(fā)展,蜂窩移動(dòng)網(wǎng)絡(luò)可以便捷使用搭建的基礎(chǔ)設(shè)施,利用移動(dòng)通信系統(tǒng)的體系結(jié)構(gòu)和傳輸信息實(shí)現(xiàn)用戶的位置坐標(biāo)推算,但受基站密度的影響,目前蜂窩移動(dòng)網(wǎng)絡(luò)定位精度較低,無(wú)法實(shí)現(xiàn)精確的定位.當(dāng)人員處于火災(zāi)救援、礦井搶險(xiǎn)等極端或未知環(huán)境時(shí),無(wú)法獲得非自主式定位技術(shù)所需的有效信息,這就對(duì)導(dǎo)航方式的自主性提出了更高要求[2].慣性導(dǎo)航技術(shù)不依賴于外部信息,是當(dāng)前實(shí)現(xiàn)室內(nèi)人員定位的主要研究方向.

慣性導(dǎo)航技術(shù)依靠自身的慣性傳感器,能夠敏感載體運(yùn)動(dòng)的角速度和加速度,根據(jù)牛頓運(yùn)動(dòng)定律,通過3 個(gè)方向的加速度數(shù)據(jù)積分計(jì)算出三維速度和位置進(jìn)行導(dǎo)航推算.該技術(shù)能夠?qū)崿F(xiàn)連續(xù)定位,且不受外界環(huán)境干擾,但是由于慣性傳感器的器件誤差,長(zhǎng)時(shí)間的誤差積累嚴(yán)重影響定位的精度,需要采用主動(dòng)或被動(dòng)的方式來抑制誤差發(fā)散[3].零速修正技術(shù)是進(jìn)行誤差抑制的一種簡(jiǎn)單而且有效的手段,利用人員行走過程中的零速作為觀測(cè)量,通過周期性的誤差修正以提高定位精度.

人員在運(yùn)動(dòng)過程中,腳部與地面保持相對(duì)靜止的時(shí)間十分短暫,一種準(zhǔn)確率高、魯棒性強(qiáng)的零速檢測(cè)方法是基于慣性導(dǎo)航技術(shù)的室內(nèi)定位算法的核心.目前典型的零速檢測(cè)方法有:(1)固定閾值法[4],通過對(duì)單一傳感器或某種組合觀測(cè)量設(shè)定閾值,滿足閾值要求即檢測(cè)得到零速區(qū)間,這種方法僅在正常行走模式下檢測(cè)效果較好,但是對(duì)于快走、跑步等運(yùn)動(dòng)模式,該方法的檢測(cè)效果并不理想;(2)外部傳感器檢測(cè)法[5],利用壓力傳感器或超聲波數(shù)據(jù)輔助檢測(cè)得到零速區(qū)間,外部傳感器需要綁在腳上或腿上,這使得穿戴更加繁瑣,增加外部傳感器使得數(shù)據(jù)處理量增加且成本較高,在實(shí)際應(yīng)用中并不常見.

本研究提出了基于改進(jìn)零速檢測(cè)方法的室內(nèi)定位系統(tǒng),適用于正常行走、小跑、上下樓等多種運(yùn)動(dòng)模式,經(jīng)多次試驗(yàn)驗(yàn)證,該方法可以控制導(dǎo)航誤差在1%里程以內(nèi),實(shí)時(shí)將位置信息傳送至服務(wù)端,圖形界面顯示人員軌跡,為一些安全生產(chǎn)和緊急救援提供了重要的技術(shù)支撐.

1 室內(nèi)系統(tǒng)定位設(shè)計(jì)

1.1 系統(tǒng)架構(gòu)設(shè)計(jì)

本系統(tǒng)由設(shè)備端、手機(jī)端、云端3 部分組成,主要完成慣性傳感器數(shù)據(jù)的采集、處理及導(dǎo)航解算,解算得到的位置數(shù)據(jù)上傳到云端保存后,可視化顯示人員的實(shí)時(shí)位置與航跡,具體如圖1所示.

圖1 系統(tǒng)架構(gòu)設(shè)計(jì)

(1)設(shè)備端:由三軸陀螺儀、三軸加速度計(jì)、藍(lán)牙等組成[6],負(fù)責(zé)采集運(yùn)動(dòng)時(shí)的數(shù)據(jù),并通過藍(lán)牙通信協(xié)議將導(dǎo)航解算后的數(shù)據(jù)傳送到手機(jī)端;

(2)手機(jī)端:安裝對(duì)應(yīng)的APP,主要是通過手機(jī)藍(lán)牙,接受設(shè)備端的數(shù)據(jù)通信,再通過WiFi、4G 等網(wǎng)絡(luò)通信方式,將數(shù)據(jù)實(shí)時(shí)上傳到云端.

(3)云端:包括數(shù)據(jù)存儲(chǔ)模塊和應(yīng)用模塊[7].數(shù)據(jù)存儲(chǔ)模塊負(fù)責(zé)保存上傳的原始數(shù)據(jù),應(yīng)用模塊是對(duì)位置數(shù)據(jù)進(jìn)行處理、分析和顯示,比如根據(jù)定位數(shù)據(jù)推算確定人員的位置和方向,顯示在已導(dǎo)入的建筑模型中,以及安全軌跡預(yù)警等.

1.2 數(shù)據(jù)協(xié)議

傳感器收集的數(shù)據(jù)處理后,再經(jīng)導(dǎo)航解算,按規(guī)定的協(xié)議格式通過藍(lán)牙通信協(xié)議,傳輸?shù)绞謾C(jī)APP.協(xié)議具體如表1所示.

表1 協(xié)議格式

表1中,數(shù)據(jù)類型以及其對(duì)應(yīng)數(shù)據(jù)長(zhǎng)度如表2所定義.

表2 數(shù)據(jù)類型定義

2 室內(nèi)人員定位慣性導(dǎo)航算法

2.1 基本原理

人員在運(yùn)動(dòng)過程中,腳部會(huì)周期性出現(xiàn)離地、擺動(dòng)、觸地、靜止4 個(gè)階段,也就是一個(gè)步態(tài)周期,如圖2所示.當(dāng)腳部處于靜止?fàn)顟B(tài)時(shí),此時(shí)人員的理論速度為零.慣性傳感器固定在人員腳部,采集x、y、z三個(gè)方向的角速度(單位為度/秒,即deg/s),當(dāng)3 個(gè)方向角速度在某段時(shí)間內(nèi)均為零時(shí),則把此段稱為靜止區(qū)間,如圖3所示.

圖2 人員步態(tài)示意圖

圖3 靜止區(qū)間示意圖

把速度為零作為卡爾曼濾波的虛擬觀測(cè),可以建立卡爾曼濾波方程,修正慣性導(dǎo)航誤差[8].以常用的9 狀態(tài)濾波器為例,設(shè)模型的系統(tǒng)狀態(tài)為導(dǎo)航系統(tǒng)誤差,如式(1)所示:

式中,δ φ=[φEφNφU]為姿態(tài)角誤差,δv=[vE vN vU]為速度誤差,δr=[rE rN rU]為位置誤差.

建立卡爾曼濾波狀態(tài)方程和量測(cè)方程[9]:

系統(tǒng)的觀測(cè)量z如下:

觀測(cè)矩陣為:

這就是基于慣性導(dǎo)航技術(shù)的室內(nèi)定位算法基本原理,其中零速檢測(cè)的準(zhǔn)確性是影響系統(tǒng)定位精度的重要原因之一.

2.2 改進(jìn)的零速檢測(cè)算法

零速檢測(cè)算法是基于零速檢測(cè)的人員定位算法中重要部分,當(dāng)慣性傳感器被固定在鞋上時(shí),三軸陀螺儀和三軸加速度計(jì)可以敏感到腳部的運(yùn)動(dòng)狀態(tài),實(shí)時(shí)檢測(cè)腳部離地、擺動(dòng)、觸地、靜止的周期性變化.在目前的應(yīng)用中,常采用加速度計(jì)和陀螺儀固定閾值法,利用統(tǒng)計(jì)學(xué)特性來完成靜止區(qū)間的檢測(cè).

式中,||fk||表示加速度計(jì)模值,||fx/y/z||表示加速度計(jì)測(cè)量值,||wk||表示陀螺儀模值,||wx/y/z||表示陀螺儀測(cè)量值.

當(dāng)||fk||和||wk||分別小于其設(shè)定閾值,同時(shí)滿足判斷條件時(shí),即判定此時(shí)間段為靜止區(qū)間,但是在實(shí)際應(yīng)用中,由于人員腳部運(yùn)動(dòng)的隨機(jī)性,在一個(gè)步態(tài)周期內(nèi)常常出現(xiàn)漏檢,如圖4所示.由于x軸陀螺儀數(shù)據(jù)較大,會(huì)造成大量數(shù)據(jù)漏檢,但此時(shí)腳部仍然處于靜止階段.

圖4 一個(gè)步態(tài)周期內(nèi)靜止區(qū)間漏檢示意圖

針對(duì)以上漏檢的情況,本文對(duì)基于固定閾值的零速檢測(cè)算法進(jìn)行改進(jìn).由于人員運(yùn)動(dòng)屬于低速運(yùn)動(dòng),毫秒級(jí)的延遲不會(huì)影響定位精度,故采用滑動(dòng)窗的方式,緩存需判斷時(shí)刻及其前后各3 個(gè)時(shí)刻的傳感器數(shù)據(jù),設(shè)滑動(dòng)窗緩存陀螺儀測(cè)量值為ωxi,ωyi,ωzi,i=1,2,···,7,緩存加速度計(jì)測(cè)量值為axi,ayi,azi,i=1,2,···,7,分別設(shè)定陀螺儀判斷閾值Thre_Gyro和加速度計(jì)判斷閾值Thre_Acc,判斷條件如式(8)和式(9):

當(dāng)式(8)和式(9)滿足邏輯與的條件下,判斷此時(shí)刻為靜止區(qū)間,當(dāng)式(8)和式(9)滿足邏輯或的條件下,進(jìn)行重新判斷.

由圖4可以看出,出現(xiàn)漏檢的原因是由于x軸陀螺儀變化較大,造成此現(xiàn)象的原因是腳踝有輕微的抖動(dòng),但腳部實(shí)際仍處于靜止?fàn)顟B(tài).針對(duì)此干擾項(xiàng),選取當(dāng)前時(shí)刻與緩存區(qū)其它時(shí)刻數(shù)據(jù)進(jìn)行冗余判斷,若當(dāng)前時(shí)刻與其它時(shí)刻的變化較小,即認(rèn)為該時(shí)刻為靜止區(qū)間.分別設(shè)定陀螺儀冗余判斷閾值ThreDiff_Gyro和加速度計(jì)冗余判斷閾值ThreDiff_Acc,判斷條件如式(10)和式(11)所示.

當(dāng)式(10)和式(11)同時(shí)滿足條件情況下,判斷此時(shí)刻為靜止區(qū)間,改進(jìn)零速檢測(cè)方法流程圖如圖5.

圖5 改進(jìn)零速檢測(cè)方法流程圖

利用以上改進(jìn)的零速檢測(cè)方法后,測(cè)出結(jié)果如圖6所示,可以看出圖4中出現(xiàn)的漏檢數(shù)據(jù)被正確檢測(cè)判斷為靜止區(qū)間.

圖6 改進(jìn)算法靜止區(qū)間檢測(cè)結(jié)果

在水平地面,選擇步行、跑步兩種狀態(tài)進(jìn)行測(cè)試,結(jié)果分別如圖7、圖8所示.從這兩個(gè)圖可以看出,改進(jìn)零速檢測(cè)算法比原零速檢測(cè)算法定位更準(zhǔn)確,在走路和跑步情況下均提高了定位精度,達(dá)到了預(yù)期效果.

圖7 步行測(cè)試結(jié)果對(duì)比圖

圖8 跑步測(cè)試結(jié)果對(duì)比圖

3 室內(nèi)定位系統(tǒng)實(shí)現(xiàn)

室內(nèi)定位系統(tǒng)由設(shè)備端、手機(jī)端和云端組成.設(shè)備端是集成硬件于一體的裝置,人員可隨身攜帶;手機(jī)端安裝對(duì)應(yīng)的APP,打開藍(lán)牙,選用4G 網(wǎng)絡(luò)用于數(shù)據(jù)的通信;云端是部署在服務(wù)器上,有固定的IP,用戶可隨時(shí)訪問.

將室內(nèi)建筑模型坐標(biāo)保存到系統(tǒng),設(shè)備上的位置數(shù)據(jù)上傳之后可以實(shí)時(shí)繪制人員軌跡,也可以瀏覽歷史軌跡,方便在特殊情況下進(jìn)行任務(wù)調(diào)配和危險(xiǎn)處置,如圖9所示.還支持劃定安全區(qū)電子圍欄、警報(bào)區(qū)和禁區(qū)電子圍欄,進(jìn)入則觸發(fā)相應(yīng)級(jí)別的報(bào)警,有益于安全生產(chǎn)和緊急救援.

圖9 室內(nèi)人員定位系統(tǒng)云端界面

4 實(shí)驗(yàn)與分析

實(shí)驗(yàn)人員佩戴此系統(tǒng)裝置進(jìn)入測(cè)試建筑物,從A 點(diǎn)出發(fā),穿越一層長(zhǎng)廊到達(dá)北側(cè)消防樓梯間(B 點(diǎn)),向上爬行至3 層,到達(dá)C 點(diǎn),再穿越長(zhǎng)廊到達(dá)南側(cè)防煙樓梯間(D 點(diǎn)),從此處消防樓梯間下至1 層,回到起始A 點(diǎn)結(jié)束,完成一個(gè)閉環(huán)測(cè)試路徑,期間包括正常行走、小跑、上下樓等運(yùn)動(dòng)方式.

從圖10可以看出,測(cè)試全程約為330 m,閉合誤差為1 m,閉合誤差為0.4%里程,滿足1%里程的設(shè)計(jì)精度,證明該室內(nèi)定位系統(tǒng)在不依賴于外界信息的情況下具有良好的實(shí)時(shí)自主導(dǎo)航性能.

圖10 測(cè)試路徑圖

5 結(jié)論

基于慣性導(dǎo)航技術(shù)的室內(nèi)定位系統(tǒng)為人員定位提供了新的解決方案,作為一種自主式導(dǎo)航方式,不需要外界基站的輔助,提高了環(huán)境適用性,在消防救援、搶險(xiǎn)救災(zāi)等未知環(huán)境下具有不可替代的作用.

基于零速修正的慣性導(dǎo)航算法中航向角是不可觀的,故無(wú)法對(duì)航向角誤差進(jìn)行修正,后期需繼續(xù)針對(duì)航向角的誤差修正進(jìn)行研究,同磁力計(jì)[10]、雷達(dá)[11]、Slam[12]、UWB[13]等多種導(dǎo)航信息源進(jìn)行數(shù)據(jù)融合是進(jìn)一步提高室內(nèi)定位精度、適用性和可靠性的有效途徑,本文的研究也為室內(nèi)定位多傳感器融合技術(shù)奠定了研究基礎(chǔ).