專利名稱：一種面向消防和救援的危險(xiǎn)作業(yè)人員無盲區(qū)定位方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種人員跟蹤定位技術(shù)，尤其涉及一種面向消防、救援等危險(xiǎn)作業(yè)人員的無盲區(qū)三維定位和狀態(tài)識(shí)別技術(shù)。
背景技術(shù)：
雖然隨著社會(huì)和科技的進(jìn)步，各種危險(xiǎn)作業(yè)移動(dòng)機(jī)器人的研究如火如荼日趨深入，應(yīng)用前景廣闊美好，但不可否認(rèn)，無法預(yù)期如何久遠(yuǎn)的未來，消防、地震、礦難等突發(fā)事故的處置還是依賴作業(yè)人員深入事故現(xiàn)場進(jìn)行處置和救援。在此過程中，保證作業(yè)人員的生命安全、保持戰(zhàn)斗力是首要任務(wù)。因此在作業(yè)人員深入事故現(xiàn)場，全身心、全力以赴進(jìn)行事故處置的過程，對(duì)其所處位置、作業(yè)狀態(tài)進(jìn)行遠(yuǎn)程監(jiān)控和指揮無疑是非常必要、亟待展開，而且也是存在諸多技術(shù)問題需要攻克。關(guān)于定位系統(tǒng)，世界上發(fā)展較為成熟與精確的就是全球衛(wèi)星定位系統(tǒng)(GPS，Global Positioning System), GPS利用多顆GPS衛(wèi)星聯(lián)合對(duì)持有接收器的節(jié)點(diǎn)協(xié)作定位，現(xiàn)在GPS定位已經(jīng)可以達(dá)到很高的精度了。但是在某些特定的場景下，如在有遮擋的場合如建筑物內(nèi)，茂密的森林等情況，GPS無法使用，這就需要利用盲區(qū)自定位的方法進(jìn)行節(jié)點(diǎn)的自定位。目前國內(nèi)危險(xiǎn)作業(yè)人員還沒有佩戴此類的定位和監(jiān)控系統(tǒng)，人員的生命安全無法得到有效的保證。本發(fā)明是基于MEMS、GPS、3G、Virtual Reality以及智能控制等技術(shù)實(shí)現(xiàn)消防、救援人員在廣域危險(xiǎn)室外環(huán)境和復(fù)雜非結(jié)構(gòu)化室內(nèi)環(huán)境的全地域無盲區(qū)定位和狀態(tài)監(jiān)測(正常、跌倒、受困等)，從事實(shí)現(xiàn)對(duì)救援消防作業(yè)人員的遠(yuǎn)程監(jiān)控、調(diào)度和安全保障。

發(fā)明內(nèi)容
本發(fā)明的目的是，提供一種面向消防、救援等危險(xiǎn)作業(yè)人員的無盲區(qū)定位方法。本發(fā)明的技術(shù)方案如下:一種面向消防和救援的危險(xiǎn)作業(yè)人員無盲區(qū)定位方法，該方法在GPS信號(hào)有效時(shí)，利用GPS模塊直接得到經(jīng)緯度和高度，當(dāng)GPS信號(hào)無效時(shí)，以GPS無效點(diǎn)為原點(diǎn)，進(jìn)行盲區(qū)相對(duì)位置的定位，定位方法如下:第一步:將人的運(yùn)動(dòng)方式大體分為靜止，跑，走，上樓，下樓五個(gè)動(dòng)作，對(duì)加速度傳感器采集的加速度信號(hào)進(jìn)行軟件濾波，然后采用滑動(dòng)時(shí)間窗的方法進(jìn)行加速度信號(hào)的特征提取，相鄰的窗之間重疊半個(gè)窗長，先對(duì)窗口進(jìn)行標(biāo)準(zhǔn)差處理，得到標(biāo)準(zhǔn)差#^;根據(jù)標(biāo)準(zhǔn)差#^的大小和表征每一步動(dòng)作的波峰和波谷之間的采樣點(diǎn)大小，進(jìn)行動(dòng)作識(shí)別；第二步:根據(jù)動(dòng)作分類和加速度信號(hào)，再加上電子羅盤采集的方位角信息，進(jìn)行相對(duì)位移的計(jì)算；第三步:將方位角信息和相對(duì)位移轉(zhuǎn)化為相應(yīng)的經(jīng)緯度輸出。作為優(yōu)選實(shí)施方式，其中的第一步采用如下的方法進(jìn)行動(dòng)作識(shí)別:(I)設(shè)采樣頻率為l OOhz，將一個(gè)窗口長度為80個(gè)樣本點(diǎn)的矩形窗滑過原始的加速度號(hào)，得到標(biāo)準(zhǔn)差>/ D(X)；(2)對(duì)于每一步的動(dòng)作，應(yīng)該有I個(gè)波谷和I個(gè)波峰，當(dāng)#^ <250mg時(shí)，若每個(gè)波谷和波峰之間有15到30個(gè)采樣點(diǎn)，且波峰和波谷的差值T大于200mg，則判斷為“走”的動(dòng)作；(3) ^VD(X) >250mg，每個(gè)波谷和波峰之間有8到20個(gè)采樣點(diǎn)，且波峰和波谷的差值T大于500mg，則判斷為“跑”的動(dòng)作；(4)若^/5^ >250mg，每個(gè)波谷和波峰之間有20到40個(gè)采樣點(diǎn)，且波峰和波谷的差值T大于400mg，據(jù)此判斷為上下樓動(dòng)作；(5)若判斷為上樓或下樓動(dòng)作時(shí)，結(jié)合氣壓傳感器的采集數(shù)據(jù)區(qū)分下樓動(dòng)作和上樓動(dòng)作；(6)當(dāng)一秒內(nèi)沒有任何動(dòng)作符合上述的動(dòng)作判定時(shí)，則判定為靜止。本發(fā)明避免對(duì)加速度觀測值積分，而基于行人步行的運(yùn)動(dòng)生理學(xué)特性，利用行走時(shí)加速度波形的周期性特征和統(tǒng)計(jì)值與行走速度相關(guān)的特點(diǎn)，直接估計(jì)行走步長，使其可以使低成本傳感器也獲得高精度的位置信息，并加入了對(duì)電子羅盤的修正方法使其輸出方向更為準(zhǔn)確，本發(fā)明更是采用gps和盲區(qū)定位相結(jié)合的方式，使其可以長時(shí)間準(zhǔn)確的輸出定位信息。


圖1gps定位流 程圖。圖2盲區(qū)定位流程圖。圖3DR算法原理圖。 圖4動(dòng)作分類流程圖。圖5盲區(qū)推估功能框圖。
具體實(shí)施例方式下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明進(jìn)行說明。實(shí)現(xiàn)本發(fā)明的硬件包括:一個(gè)GPS模塊，I塊stm32芯片，一塊51單片機(jī)，I個(gè)3軸數(shù)字加速度計(jì)ADXL345，I個(gè)3軸數(shù)字羅盤HMC5883，I個(gè)3軸數(shù)字陀螺儀L3G4200D，一個(gè)BMP085氣壓傳感器。stm32芯片為主要功能元件，GPS模塊，加速度計(jì)，氣壓傳感器和51單片機(jī)都直接與其相連，它實(shí)時(shí)檢測GPS信號(hào)。本發(fā)明在GPS信號(hào)有效時(shí)，利用GPS模塊直接得到經(jīng)緯度和高度，當(dāng)GPS信號(hào)無效時(shí)，以GPS無效點(diǎn)為原點(diǎn)，進(jìn)行盲區(qū)相對(duì)位置的定位。我們從中提取經(jīng)緯度和高度信息(B，L, H) B為緯度，L為經(jīng)度，H為高度，第6字段為定位質(zhì)量指示，0為無效，I為有效，當(dāng)有效時(shí)直接輸出經(jīng)緯度和高度，當(dāng)數(shù)據(jù)無效時(shí)，盲區(qū)定位功能啟動(dòng)。功能框圖如圖1。軟件系統(tǒng)采用uc/os-1I實(shí)時(shí)操作系統(tǒng)，定位方面主要有兩個(gè)任務(wù)，任務(wù)一負(fù)責(zé)GPS數(shù)據(jù)的讀取，當(dāng)數(shù)據(jù)有效時(shí)直接輸出，當(dāng)數(shù)據(jù)無效時(shí)，任務(wù)一掛起，低于它優(yōu)先級(jí)的任務(wù)2開始執(zhí)行，任務(wù)2即盲區(qū)推估功能，盲區(qū)推估功能實(shí)現(xiàn)方法
I盲區(qū)推估定位當(dāng)GPS數(shù)據(jù)無效時(shí)，盲區(qū)推估功能啟動(dòng)，獲取相對(duì)位移，方位角，然后通過大地主題正解算法轉(zhuǎn)化為相應(yīng)的經(jīng)緯度(B，L，H)輸出，每走五步，進(jìn)行一次轉(zhuǎn)換，輸出相應(yīng)的經(jīng)緯度(B，L，H)，具體流程圖如圖2所示。2，求相對(duì)位移盲區(qū)推估主要是運(yùn)用傳感器去估計(jì)其位移，本發(fā)明采取的是動(dòng)作識(shí)別的方法進(jìn)行盲區(qū)推估的，設(shè)GPS無效點(diǎn)為原點(diǎn)，采取行走航跡推算算法roR，即從一個(gè)已知坐標(biāo)開始，通過傳感器獲取人在該位置的方向，速度等數(shù)據(jù)來推算行人在下一時(shí)刻出現(xiàn)未知的過程。如圖3所示E、N分別表示東向、北向位置分量，(E0, N0)是行人在h時(shí)刻的初始位置，S。和0 Q分別表示行人從t0時(shí)刻至IJ 時(shí)刻從位置(E0, N0)行走到位置(E1, N1)的移動(dòng)距離和絕對(duì)航向，則行人在h時(shí)刻的位置可表示為:E1=Ec^S0Sin 0 0N1=Nc^S0COS 0 0同理，行人在t2時(shí)刻的位置(E2，N2)可表示為:
「00351 \ E2 = E1+ sIsin0I = E0+ Si1=O Si Sinei (N2 = N1 + S1CosG1 = N0 + Ei=o Si cos Bi 根據(jù)上述原理，可獲得行人在tk時(shí)刻位置的航位推算公式為:
「00371 f Ek = Ek-1 + Sk sin 0k = E0 + Efc Si sin Bi I Nk = Nk_i + Skcos0k = N0 + Eto1 Si cos 0;然而由于步行行為的多樣性與多變性，使得DR在應(yīng)用于行人導(dǎo)航系統(tǒng)時(shí)，其具體的實(shí)現(xiàn)形式略有變化。根據(jù)步行特點(diǎn)以及行人導(dǎo)航系統(tǒng)低成本、易佩戴的要求，行人的行走距離很難直接通過傳感器直接測量，也不容易根據(jù)加速度積分的方式進(jìn)行計(jì)算。因此，行人導(dǎo)航系統(tǒng)中主要采用步長獲得相對(duì)位移量，首先將人的運(yùn)動(dòng)方式大體分為靜止，跑，走，上樓，下樓五個(gè)動(dòng)作，根據(jù)其每個(gè)動(dòng)作的行走距離不同賦予不同的值再根據(jù)其行走的方向求出位移。3動(dòng)作分類為了區(qū)分這五種動(dòng)作，我們利用了加速度和氣壓2種傳感器，加速度計(jì)原始輸出雖然具有一定的周期性，但受噪音影響變化復(fù)雜，不易于直接進(jìn)行計(jì)步計(jì)算，需對(duì)信號(hào)進(jìn)行濾波，盡可能消除噪音影響。采用平均法進(jìn)行濾波，屬于軟件濾波平均濾波法的公式為
/3—1
-——7’-’這樣得出的加速度曲線較為平滑，五種特征的提取都是釆
Yj
戶用滑動(dòng)時(shí)間窗的方法，將一個(gè)窗口長度為80個(gè)樣本點(diǎn)的矩形窗滑過原始的加速
度信號(hào)，相鄰的窗之間重疊半個(gè)窗長。因?yàn)榧铀俣炔蓸宇l率為lOOhz，所以一個(gè)窗長為0.8秒，對(duì)于加速度的判定，采用矢量和的形式a = jai +畤+ a|，這消除由于儀器佩戴不垂直所帶來的誤差，加速度傳感器是以mg為單位的,靜止時(shí)為IOOOmg就是Ig,
I)本發(fā)明采用基于時(shí)域的方法進(jìn)行檢測的，走和上下樓，跑最主要的區(qū)別是比較平穩(wěn)，所以首先對(duì)窗口進(jìn)行標(biāo)準(zhǔn)差處理，D(X) = Eli1fe^1E(X)為窗口平均數(shù)即為標(biāo)準(zhǔn)差，小于閾值的進(jìn)入行走判定，否則歸于另一類。2)對(duì)于一步行走應(yīng)該有I個(gè)波谷一個(gè)波峰，將波峰和波谷的差值T作為閾值，每個(gè)波谷和波峰之間有15到30個(gè)采樣點(diǎn)，大于閾值并且在采樣點(diǎn)范圍內(nèi)則屬于一步行走。3)當(dāng)標(biāo)準(zhǔn)差判定時(shí)歸于上下樓和跑步那一類時(shí)，每一步也有I個(gè)波谷一個(gè)波峰，將波峰和波谷的差值作T為閾值，但是跑步每個(gè)波谷和波峰之間有8到20個(gè)采樣點(diǎn)，而上下樓每個(gè)波谷和波峰之間有20到40個(gè)采樣點(diǎn)4)當(dāng)?shù)谌奖慌袛酁樯舷聵鞘?，再用氣壓傳感器進(jìn)行判定，想將氣壓根據(jù)公式
(l Xalt為高度，p為當(dāng)前大氣壓，P0為標(biāo)準(zhǔn)大氣壓，單位為厘米，
alt= 4433000 x VJ
當(dāng)高度下降10厘米時(shí)判定為下樓，否則為上樓。5)當(dāng)一秒內(nèi)沒有任何動(dòng)作符合時(shí)，判定為靜止。具體算法流程如圖4所示。圖中，Su為上樓步長，Hu上樓高度，Sd為下樓步長，Hd下樓高度，Sz為行走步長，Sp為跑步步長。4電子羅盤的校準(zhǔn)51單片機(jī)主要進(jìn)行電子羅盤校正于在電子羅盤方面采用HMC5883L三軸電子羅盤，這種電子羅盤有著一個(gè)和一般電子羅盤相同的缺點(diǎn)，那就是在校準(zhǔn)之后，不能有任何傾斜，否則將會(huì)造成較大數(shù)值偏差，為克服這一缺點(diǎn)，采用卡爾曼濾波的方法進(jìn)行修正，參見圖5,具體措施如下:I構(gòu)建方位角的狀態(tài)方程；使用3軸數(shù)字陀螺儀L3G4200D，獲得航向角方向的角加速度ax，那么狀態(tài)方程為X(k)=X(k_l)+axkt+W(k) ;X(k)為k時(shí)刻的航向角，X (k_l)為k_l時(shí)刻的航向角，axk為k時(shí)刻的角加速度，t為陀螺儀每次讀取數(shù)據(jù)的時(shí)間，W(k)和下面所說的V(k)分別為過程和測量的噪聲，我們假定其恒定不變都為4度。2方位角的測量值由電子羅盤進(jìn)行觀測，Z (k)系統(tǒng)的觀測量，V(k)為測量噪聲，3首先預(yù)計(jì)k時(shí)刻的航向角X (k I k_l) =X (k_l | k_l) +axkt, X (k | k_l)是利用上一狀態(tài)預(yù)測的結(jié)果，X(k-l|k-l)是上一狀態(tài)最優(yōu)的結(jié)果，匕為卡爾曼增益，X(k-llk-l)的協(xié)方差為 P (k-11 k-1)，X (k I k-1)的協(xié)方差為 P (k I k-1)，P (k I k-1) = (P (k-11 k-1) 2+42)1/2,Kg (K) = (P (k I k-1)2/ (P (k I k-1) 2+42))1/2，X (k I k) =X (k I k-1) +Kg (k) (Z (k) -X (k I k-1))，P (k I k)= ((l-Kg(k))*P(k|k-l)2)1/2.
這樣就可以修正因傾斜引起的偏差，但是在人體行走過程中，也有航向角方面的晃動(dòng)，比如，邁右腿身體自然就會(huì)往左邊傾斜，而實(shí)際上還是屬于直線行走，這樣我們采取如下方法進(jìn)行校正，下圖所有參數(shù)初始值都為零。關(guān)于大地主題正解算法是已知一點(diǎn)經(jīng)緯度，距離，方位角，求另一點(diǎn)的經(jīng)緯度的方法，詳見大地測量學(xué)中有關(guān)大地主題正反算中的正算。
權(quán)利要求
1.一種面向消防和救援的危險(xiǎn)作業(yè)人員無盲區(qū)定位方法，該方法在GPS信號(hào)有效時(shí)，利用GPS模塊直接得到經(jīng)緯度和高度，當(dāng)GPS信號(hào)無效時(shí)，以GPS無效點(diǎn)為原點(diǎn)，進(jìn)行盲區(qū)相對(duì)位置的定位，定位方法如下: 第一步:將人的運(yùn)動(dòng)方式大體分為靜止，跑，走，上樓，下樓五個(gè)動(dòng)作，對(duì)加速度傳感器采集的加速度信號(hào)進(jìn)行軟件濾波，然后采用滑動(dòng)時(shí)間窗的方法進(jìn)行加速度信號(hào)的特征提取，相鄰的窗之間重疊半個(gè)窗長，先對(duì)窗口進(jìn)行標(biāo)準(zhǔn)差處理，得到標(biāo)準(zhǔn)差#^:根據(jù)標(biāo)準(zhǔn)差#^的大小和表征每一步動(dòng)作的波峰和波谷之間的采樣點(diǎn)大小，進(jìn)行動(dòng)作識(shí)別； 第二步:根據(jù)動(dòng)作分類和加速度信號(hào)，再加上電子羅盤采集的方位角信息，進(jìn)行相對(duì)位移的計(jì)算； 第三步:將方位角信息和相對(duì)位移轉(zhuǎn)化為相應(yīng)的經(jīng)緯度輸出。
2.根據(jù)權(quán)利要求1所述的無盲區(qū)定位方法，其特征在于，其中的第一步采用如下的方法進(jìn)行動(dòng)作識(shí)別: (1)設(shè)采樣頻率為lOOhz，將一個(gè)窗口長度為80個(gè)樣本點(diǎn)的矩形窗滑過原始的加速度Ih號(hào)，得到標(biāo)準(zhǔn)差D PO ； (2)對(duì)于每一步的動(dòng)作，應(yīng)該有I個(gè)波谷和I個(gè)波峰，當(dāng)#^<25(tog時(shí)，若每個(gè)波谷和波峰之間有15到30個(gè)采樣點(diǎn)，且波峰和波谷的差值T大于200mg，則判斷為“走”的動(dòng)作； (3)^VD(X) >250mg，每個(gè)波谷和波峰之間有8到20個(gè)采樣點(diǎn)，且波峰和波谷的差值T大于500mg，則判斷為“跑”的動(dòng)作； (4)若#^>250mg，每個(gè)波谷和波峰之間有20到40個(gè)采樣點(diǎn)，且波峰和波谷的差值T大于400mg，據(jù)此判斷為上下 樓動(dòng)作； (5)若判斷為上樓或下樓動(dòng)作時(shí)，結(jié)合氣壓傳感器的采集數(shù)據(jù)區(qū)分下樓動(dòng)作和上樓動(dòng)作； (6)當(dāng)一秒內(nèi)沒有任何動(dòng)作符合上述的動(dòng)作判定時(shí)，則判定為靜止。
3.根據(jù)權(quán)利要求1所述的無盲區(qū)定位方法，其特征在于，第三步中，通過大地主題正解算法將方位角信息和相對(duì)位移轉(zhuǎn)化為相應(yīng)的經(jīng)緯度輸出。
全文摘要
本發(fā)明屬于跟蹤定位技術(shù)領(lǐng)域，涉及一種面向消防和救援的危險(xiǎn)作業(yè)人員無盲區(qū)定位方法，該方法在GPS信號(hào)無效時(shí)，以GPS無效點(diǎn)為原點(diǎn)，進(jìn)行盲區(qū)相對(duì)位置的定位，方法如下將人的運(yùn)動(dòng)方式大體分為靜止，跑，走，上樓，下樓五個(gè)動(dòng)作，對(duì)加速度傳感器采集的加速度信號(hào)進(jìn)行軟件濾波，然后采用滑動(dòng)時(shí)間窗的方法進(jìn)行加速度信號(hào)的特征提取，相鄰的窗之間重疊半個(gè)窗長，先對(duì)窗口進(jìn)行標(biāo)準(zhǔn)差處理，得到標(biāo)準(zhǔn)差；根據(jù)標(biāo)準(zhǔn)差的大小和表征每一步動(dòng)作的波峰和波谷之間的采樣點(diǎn)大小，進(jìn)行動(dòng)作識(shí)別；再加上電子羅盤采集的方位角信息，進(jìn)行相對(duì)位移的計(jì)算；將方位角信息和相對(duì)位移轉(zhuǎn)化為相應(yīng)的經(jīng)緯度輸出。本發(fā)明可以長時(shí)間準(zhǔn)確的輸出定位信息。
文檔編號(hào)G01C21/10GK103234540SQ20131008964
公開日2013年8月7日 申請(qǐng)日期2013年3月20日 優(yōu)先權(quán)日2013年3月20日
發(fā)明者丁承君, 李 根, 段萍, 申敏 申請(qǐng)人:河北工業(yè)大學(xué)