本發(fā)明涉及定位技術(shù)領(lǐng)域,特別涉及一種室內(nèi)定位系統(tǒng)。
背景技術(shù):隨著智能終端的廣泛使用,移動通信快速發(fā)展,同時(shí)數(shù)據(jù)和多媒體業(yè)務(wù)迅速增加,基于位置的服務(wù)(LocationBasedServices,LBS)逐漸受到重視。尤其是作為定位技術(shù)的最后一米,室內(nèi)定位技術(shù)有著廣泛的應(yīng)用前景。比如在地下停車場,可以借助室內(nèi)定位技術(shù)快速找到停車位;在大型商場,可以借助室內(nèi)定位技術(shù)快速找到出口、電梯,同時(shí)商家也可以借助于室內(nèi)定位向用戶介紹商品信息;在火災(zāi)現(xiàn)場,消防人員可以借助室內(nèi)定位技術(shù)快速確定被困人員的位置,從而迅速進(jìn)行救援;在醫(yī)院,可以利用室內(nèi)定位技術(shù)對需要看護(hù)的病人進(jìn)行定位等等。在室外,全球衛(wèi)星導(dǎo)航系統(tǒng)(GlobalNavigationSatelliteSystem,GNSS)基本上可以滿足定位精度的要求,但是在室內(nèi)環(huán)境下,由于衛(wèi)星信號受到遮擋且存在嚴(yán)重的多徑效應(yīng),使得衛(wèi)星定位精度嚴(yán)重下降,甚至出現(xiàn)無法正常定位的情況。目前,針對復(fù)雜的室內(nèi)環(huán)境,現(xiàn)有的室內(nèi)定位技術(shù)主要分為以下幾種1)射頻識別定位技術(shù)射頻識別(RadioFrequencyIdentification,RFID)定位技術(shù)是一種利用射頻信號自動識別目標(biāo)對象并獲取相關(guān)信息的技術(shù)。實(shí)現(xiàn)無接觸信息傳遞并達(dá)到識別對象的目的。RFID定位系統(tǒng)包括電子標(biāo)簽和讀卡器等,該系統(tǒng)利用標(biāo)簽對物體的唯一標(biāo)識特性,通過讀卡器接收到的電子標(biāo)簽發(fā)送的信號來獲得電子標(biāo)簽的位置信息。RFID定位系統(tǒng)優(yōu)點(diǎn)是體積小、造價(jià)低,但是作用范圍小、不具有通信能力,且不便整合到其它系統(tǒng)中。2)藍(lán)牙定位技術(shù)藍(lán)牙(Bluetooth)技術(shù)是一種支持語音和數(shù)據(jù)業(yè)務(wù)的短距離、低功耗無線傳輸技術(shù),并可以實(shí)現(xiàn)短距離不同設(shè)備之間的互聯(lián)。基于藍(lán)牙的室內(nèi)定位技術(shù),利用經(jīng)驗(yàn)測試與信號傳播模型結(jié)合的方式,通過實(shí)時(shí)測量的信號強(qiáng)度進(jìn)行定位。藍(lán)牙定位技術(shù)的優(yōu)點(diǎn)在于設(shè)備體積小,易于集成在移動智能終端上面。理論上,只要集成了藍(lán)牙的移動智能終端且藍(lán)牙功能打開,藍(lán)牙定位系統(tǒng)就能夠?qū)ζ溥M(jìn)行定位。藍(lán)牙定位技術(shù)的缺點(diǎn)在于藍(lán)牙器件設(shè)備的價(jià)格比較昂貴,且對于復(fù)雜的室內(nèi)環(huán)境而言,藍(lán)牙定位系統(tǒng)的穩(wěn)定性比較差。3)紅外線定位技術(shù)紅外線(InfraredRay,IR)是一種介于微波和可見光之間的電磁波。IR室內(nèi)定位方法通過發(fā)射調(diào)制的紅外射線,利用安裝在室內(nèi)的光學(xué)傳感器接收進(jìn)行定位。IR定位具有相對較高的定位精度,但由于光線不能穿過障礙物,使得IR具有傳輸距離短的缺點(diǎn),且需要在室內(nèi)的每個(gè)房間和樓層安裝設(shè)備,成本較高。4)無線局域網(wǎng)定位技術(shù)無線局域網(wǎng)(WirelessLocalAreaNetwork,WLAN)是基于IEEE802.11標(biāo)準(zhǔn),以無線信道作為傳輸媒介的局域網(wǎng)?;赪LAN的定位技術(shù)主要包括信號傳播模型法和位置指紋法,都是利用接收信號的信號強(qiáng)度指示(ReceivedSignalStrengthIndication,RSSI)進(jìn)行位置解算。信號傳播模型法利用室內(nèi)信號的傳播特性,考慮墻壁和地板引起的衰減,建立RSSI與傳播距離d之間的數(shù)學(xué)模型,實(shí)現(xiàn)位置解算。但是由于復(fù)雜的室內(nèi)環(huán)境,很難建立準(zhǔn)確的室內(nèi)信號傳播模型,因此利用該方法進(jìn)行位置解算精度有限?;赪LAN的位置指紋定位技術(shù)包括離線階段和在線階段兩個(gè)階段,離線階段在需要定位的環(huán)境采集相應(yīng)的RSSI信息并存入位置指紋數(shù)據(jù)庫,在線階段利用實(shí)時(shí)測得的RSSI信息與位置指紋數(shù)據(jù)庫進(jìn)行匹配,將匹配最佳位置輸出作為定位結(jié)果?;赪LAN的位置指紋定位技術(shù)具有覆蓋范圍廣、可用性強(qiáng)、低成本、長時(shí)間定位精度高等優(yōu)點(diǎn)。但是由于多徑干擾等引起的RSSI擾動等因素嚴(yán)重影響了WLAN的定位精度。5)MEMS傳感器自主定位技術(shù)隨著微機(jī)電系統(tǒng)(Micro-Electro-MechanicalSystems,MEMS)技術(shù)的快速發(fā)展,各種傳感器,如陀螺儀、加速度計(jì)、磁力計(jì)、氣壓計(jì)等,尺寸不斷變小,成本不斷變低,被廣泛應(yīng)用于各種移動智能終端設(shè)備中。基于MEMS傳感器的定位技術(shù)具有自主性和連續(xù)性,且目前基于MEMS傳感器的個(gè)人導(dǎo)航系統(tǒng)主要有兩種:傳統(tǒng)的慣性積分導(dǎo)航和行人航跡推算導(dǎo)航。傳統(tǒng)的慣性積分導(dǎo)航,根據(jù)牛頓運(yùn)動定律,利用測得3軸加速度計(jì)和3軸陀螺儀的數(shù)據(jù)積分計(jì)算出3維姿態(tài)、速度和位置。理論上這種方式定位精度會很高,但是實(shí)際應(yīng)用中,由于慣性傳感器自身的漂移、測量噪聲、行人的抖動等因素影響,多次積分后會產(chǎn)生累積誤差,幾十秒之內(nèi)誤差可達(dá)幾十米,甚至幾百米。基于行人航跡推算(PedestrianDeadReckoning,PDR)方法利用行人的位移和航向進(jìn)行位置解算,相比于傳統(tǒng)的慣性積分導(dǎo)航更準(zhǔn)確且更適用于行人導(dǎo)航,但是其定位依然受步長、航向、步態(tài)檢測精度影響,隨著時(shí)間的增加,依然存在累積誤差。
技術(shù)實(shí)現(xiàn)要素:本發(fā)明針對現(xiàn)有室內(nèi)定位技術(shù)存在的缺陷和不足,結(jié)合WLAN位置指紋定位和MEMS傳感器定位,提出一種高精度室內(nèi)WLAN/MEMS融合跨樓層3維定位方法,實(shí)現(xiàn)室內(nèi)空間的3維定位。該方法及系統(tǒng)能夠有效的克服MEMS器件定位存在的累積誤差和WLAN定位中存在的RSSI擾動對系統(tǒng)定位精度帶來的影響。本發(fā)明方法及系統(tǒng)利用擴(kuò)展卡爾曼濾波算法中的一步預(yù)測位置信息修正觀測的RSSI,修正由于RSSI擾動給WLAN定位精度帶來的影響;同時(shí)利用WLAN定位結(jié)果補(bǔ)償MEMS的累積誤差,提高定位精度。且該方法還綜合利用氣壓計(jì)、地理位置等解算高度,實(shí)現(xiàn)室內(nèi)跨樓層3維定位。本發(fā)明的技術(shù)方案如下:本發(fā)明提供一種室內(nèi)WLAN/MEMS融合跨樓層3維定位方法,其特征在于,所述方法包括:(1)選擇WLAN位置指紋數(shù)據(jù)庫:計(jì)算前一時(shí)刻得到的行人的3維位置距每個(gè)樓梯口的物理距離,利用計(jì)算出的物理距離是否大于某個(gè)閾值判斷行人是否進(jìn)出樓梯間;如果判斷出沒有進(jìn)出樓梯間,則位置指紋數(shù)據(jù)庫不變,與前一時(shí)刻的位置指紋數(shù)據(jù)庫相同;如果進(jìn)入樓梯間,則選擇樓梯間的位置指紋數(shù)據(jù)庫,如果出樓梯間,則判斷為在樓層中,此時(shí)還需利用前一時(shí)刻的實(shí)際高度判斷所處樓層,選擇相應(yīng)樓層的位置指紋數(shù)據(jù)庫。(2)計(jì)算行人的速度和航向:根據(jù)采集到的MEMS數(shù)據(jù),包括3軸加速度計(jì)、3軸陀螺儀和3軸磁力計(jì)數(shù)據(jù),利用步態(tài)檢測和數(shù)據(jù)更新速率方法計(jì)算速度,四元素更新姿態(tài)矩陣方法計(jì)算航向。(3)WLAN位置指紋定位:首先利用抗差擴(kuò)展卡爾曼組合導(dǎo)航濾波器中的一步預(yù)測信息反匹配位置指紋數(shù)據(jù)庫修正觀測得到RSSI,然后利用修正的RSSI結(jié)合WKNN定位算法進(jìn)行位置解算,克服由于RSSI擾動對WLAN位置指紋定位精度帶來的影響。同時(shí)將抗差M估計(jì)引入室內(nèi)融合定位算法中,形成抗差擴(kuò)展卡爾曼濾波器,解決定位在實(shí)際環(huán)境下受到粗差、干擾、模型誤差等因素的影響,導(dǎo)致位置精度的嚴(yán)重下降。(4)將計(jì)算得到的速度、航向和WLAN指紋定位的位置一起帶入抗差擴(kuò)展卡爾曼組合導(dǎo)航濾波器,解算得到最優(yōu)的二維位置坐標(biāo)。(5)高度計(jì)算:利用氣壓計(jì)輸出數(shù)據(jù)計(jì)算海拔高度,實(shí)際的高度信息則通過判斷上下樓進(jìn)行高度的累加或者累減進(jìn)行計(jì)算,從而實(shí)現(xiàn)室內(nèi)跨樓層3為定位。所述步驟(3)使用的抗差擴(kuò)展卡爾曼組合導(dǎo)航濾波器的設(shè)計(jì)方法是:通過建立觀測方程和狀態(tài)方程,設(shè)計(jì)擴(kuò)展卡爾曼濾波器(EKF)輸出最優(yōu)位置信息,并同時(shí)引入抗差M估計(jì)到室內(nèi)融合定位算法中,形成抗差擴(kuò)展卡爾曼濾波器。進(jìn)一步,所述狀態(tài)變量和狀態(tài)方程如下:選用東位置、北位置、速度和航向作為狀態(tài)變量,即狀態(tài)方程如下:式中xt表示t時(shí)刻待估計(jì)的東位置,yt表示t時(shí)刻待估計(jì)的北位置,vt表示t時(shí)刻待估計(jì)的速度,表示t時(shí)刻待估計(jì)的航向;xt-1表示t-1時(shí)刻的東位置,yt-1表示t-1時(shí)刻的北位置,vt-1表示t-1時(shí)刻的速度,表示t-1時(shí)刻的航向角,Wt-1表示過程噪聲,是均值為0的高斯白噪聲序列。進(jìn)一步,所述的觀測變量和觀測方程如下:選用WLAN輸出的位置信息、MEMS輸出的速度和航向信息作為觀測量,即觀測方程如下:式中表示t時(shí)刻WLAN定位出的東位置,表示t時(shí)刻WLAN定位出的北位置,表示t時(shí)刻MEMS輸出的速度,表示t時(shí)刻MEMS輸出的航向;xt表示t時(shí)刻待估計(jì)的東位置,yt表示t時(shí)刻待估計(jì)的北位置,vt表示t時(shí)刻待估計(jì)的速度,表示t時(shí)刻待估計(jì)的航向,Vt表示觀測噪聲,是均值為0的高斯白噪聲序列。進(jìn)一步,所述步驟(3)修正RSSI的方法具體如下:定義t(t=1,2,K)時(shí)刻擴(kuò)展卡爾曼預(yù)測位置到WLAN位置指紋數(shù)據(jù)庫中參考點(diǎn)的物理距離為式中(xj,yj)為位置指紋數(shù)據(jù)庫中第j個(gè)參考點(diǎn)的位置,n為位置指紋庫中參考點(diǎn)總數(shù),在計(jì)算出t時(shí)刻到所有參考點(diǎn)的距離Dtj后,選取K(K>1)個(gè)物理距離最小的參考點(diǎn)對應(yīng)的RSSI向量,利用類似WKNN算法得到預(yù)測的RSSI向量為式中RSSI1為得到的RSSI預(yù)測向量,RSSIm為t時(shí)刻K個(gè)與擴(kuò)展卡爾曼預(yù)測位置物理距離最小的參考點(diǎn)中第m點(diǎn)對應(yīng)的RSSI向量,Dtm為其對應(yīng)的物理距離,為t時(shí)刻K個(gè)與擴(kuò)展卡爾曼預(yù)測位置物理距離最小的參考點(diǎn)對應(yīng)的物理距離的和,式中q為索引號;得到RSSI1后,對實(shí)測的RSSI2進(jìn)行修正:RSSIt=w×RSSI1+(1-w)×RSSI2(30)式中RSSIt為t時(shí)刻最終的RSSI向量,w為加權(quán)系數(shù),取值范圍為0≤w≤1。更進(jìn)一步,所述步驟(2)中MEMS輸出的速度具體解算如下:首先計(jì)算加速度計(jì)輸出的模值和分別表示加速度計(jì)輸出的3軸加速度。然后通過檢測該模值A(chǔ)norm為峰值且大于某個(gè)設(shè)定的閾值A(chǔ)0來判斷跨步,完成步態(tài)檢測。最后假設(shè)MEMS傳感器的輸出頻率為fs,且通過檢測加速度計(jì)輸出模值A(chǔ)norm連續(xù)兩個(gè)大于閾值A(chǔ)0的峰值之間的數(shù)據(jù)點(diǎn)數(shù)ΔN,可以計(jì)算第k步所需的時(shí)間為假設(shè)行人每一步為勻速運(yùn)動,則可以計(jì)算出第k步的速度為式中Pk為第k步的步長,vk為第k步計(jì)算出的速度。令第k步中每個(gè)采樣時(shí)刻的速度都為vk,則每一秒可計(jì)算出fs個(gè)速度,取每一秒計(jì)算出的所有速度的均值作為該秒計(jì)算出的速度。進(jìn)一步,所述步驟(5)的高度計(jì)算中,僅利用氣壓計(jì)測量的高度值判斷上下樓,而實(shí)際的高度信息則通過判斷上下樓進(jìn)行高度的累加或者累減進(jìn)行計(jì)算,具體方法如下:將行人行走分為3種情況:樓層中行走、上樓梯和下樓梯;通過地理位置信息,判斷出行人所處的大概位置(在樓層中或者在樓梯間),如果在樓梯間,通過高度計(jì)測量的海拔高度信息判斷行人上樓梯或者下樓梯,實(shí)現(xiàn)高度的累加或者累減,如果在樓層中,則將所處樓層的高度直接賦值給當(dāng)前時(shí)刻的高度輸出,實(shí)現(xiàn)室內(nèi)空間跨樓層3維定位。更進(jìn)一步,所述的判斷出行人所處的大概位置(在樓層中或者在樓梯間),其具體判斷如下:測試測試環(huán)境中所有樓梯口處的3維位置坐標(biāo)為(xi,yi,zi),i=1,2K,n,其中n為樓梯口總的個(gè)數(shù)。假設(shè)行人在時(shí)刻t-1的最優(yōu)位置為分別計(jì)算其與每一個(gè)樓梯口的物理距離為式中di為時(shí)刻t-1的最優(yōu)位置與第i個(gè)樓梯口的物理距離;設(shè)定門限r(nóng),如果di<r且判斷為進(jìn)入樓梯間(如果上個(gè)時(shí)刻在樓層中,則判斷為進(jìn)入樓梯間),此時(shí)匹配樓梯間的指紋數(shù)據(jù)庫,并保存此時(shí)的氣壓計(jì)測量高度信息h2;如果di<r且判斷為出樓梯間(如果上個(gè)時(shí)刻在樓梯間,則判斷為出樓梯間),此時(shí)還需利用前一時(shí)刻的實(shí)際高度信息判斷所處樓層,選擇相應(yīng)的位置指紋數(shù)據(jù)庫,如果di>r,則位置指紋數(shù)據(jù)庫不變,與前一時(shí)刻的位置指紋數(shù)據(jù)庫相同。本發(fā)明的優(yōu)點(diǎn)如下:1)高精度,本發(fā)明可以實(shí)現(xiàn)室內(nèi)跨樓層3維定位最小均方根誤差小于1米的高精度定位,并且本發(fā)明室內(nèi)跨樓層3維定位誤差小于1m的置信概率為75%;2)實(shí)現(xiàn)WLAN/MEMS融合定位,克服了WLAN位置指紋定位技術(shù)中存在的RSSI擾動和MEMS傳感器定位技術(shù)中存在的累積誤差對系統(tǒng)定位精度帶來的影響;3)實(shí)現(xiàn)室內(nèi)空間的跨樓層3維定位。附圖說明圖1為本發(fā)明所述系統(tǒng)的總體架構(gòu)流程圖;圖2為本發(fā)明方法中高度計(jì)算的流程圖。具體實(shí)施方式下面結(jié)合附圖和具體實(shí)施例對本發(fā)明進(jìn)行詳細(xì)說明。如圖1所示,本發(fā)明提出的室內(nèi)WLAN/MEMS融合跨樓層3維定位系統(tǒng)主要包括:WLAN位置指紋定位模塊、MEMS數(shù)據(jù)處理模塊、抗差擴(kuò)展卡爾曼濾波器模塊、高度計(jì)算模塊?!馱LAN位置指紋定位模塊。根據(jù)抗差擴(kuò)展卡爾曼濾波器反饋的一步預(yù)測位置信息,利用位置指紋數(shù)據(jù)庫反匹配修正實(shí)測RSSI信息,同時(shí)利用修正的RSSI信息,采用加權(quán)K近鄰(WeightedK-NearestNeighborhood,WKNN)定位算法解算位置?!馦EMS傳感器數(shù)據(jù)處理模塊。根據(jù)加速度計(jì)、陀螺儀、磁力計(jì)輸出的信息,利用步態(tài)檢測、數(shù)據(jù)更新速率、四元素法更新姿態(tài)矩陣等方法計(jì)算行人的速度和航向?!窨共顢U(kuò)展卡爾曼濾波器模塊。建立觀測方程和狀態(tài)方程,設(shè)計(jì)擴(kuò)展卡爾曼濾波器(ExtendedKalmanFilter,EKF)輸出最優(yōu)位置信息,并同時(shí)引入抗差M估計(jì),以解決系統(tǒng)在實(shí)際環(huán)境下受到粗差、干擾、模型誤差等因素的影響,導(dǎo)致位置精度嚴(yán)重下降?!窀叨扔?jì)算模塊。綜合利用地理位置、氣壓計(jì)輸出海拔高度和每一個(gè)樓層的實(shí)際高度實(shí)現(xiàn)高度的解算,并同時(shí)為WLAN定位選擇位置指紋數(shù)據(jù)庫。1、WLAN位置指紋定位對于室內(nèi)WLAN環(huán)境下的實(shí)時(shí)跟蹤定位來說,由于可供采集RSSI的時(shí)間較短、并且RSSI波動性很大,若只使用測量的RSSI信息來確定觀測位置信息,將會存在很大的定位誤差。解決方法為根據(jù)擴(kuò)展卡爾曼濾波器反饋的一步預(yù)測位置信息,利用位置指紋數(shù)據(jù)庫反匹配修正實(shí)測RSSI信息,同時(shí)利用修正的RSSI信息,采用加權(quán)K近鄰(WeightedK-NearestNeighborhood,WKNN)定位算法解算位置。(1)反匹配修正實(shí)測RSSI定義t(t=1,2,K)時(shí)刻擴(kuò)展卡爾曼預(yù)測位置到WLAN位置指紋數(shù)據(jù)庫中參考點(diǎn)的物理距離為式中(xj,yj)為位置指紋數(shù)據(jù)庫中第j個(gè)參考點(diǎn)的位置,n為位置指紋庫中參考點(diǎn)總數(shù)。在計(jì)算出t時(shí)刻到所有參考點(diǎn)的距離Dtj后,選取K(K>1)個(gè)物理距離最小的參考點(diǎn)對應(yīng)的RSSI向量,利用類似WKNN算法得到預(yù)測的RSSI向量為式中RSSI1為得到的RSSI預(yù)測向量,RSSIm為t時(shí)刻K個(gè)與擴(kuò)展卡爾曼預(yù)測位置物理距離最小的參考點(diǎn)中第m點(diǎn)對應(yīng)的RSSI向量,Dtm為其對應(yīng)的物理距離,為t時(shí)刻K個(gè)與擴(kuò)展卡爾曼預(yù)測位置物理距離最小的參考點(diǎn)對應(yīng)的物理距離的和,式中q為索引號。得到RSSI1過后,對實(shí)測的RSSI2進(jìn)行修正有RSSIt=w×RSSI1+(1-w)×RSSI2(3)式中RSSIt為t時(shí)刻最終的RSSI向量,w為加權(quán)系數(shù),取值范圍為0≤w≤1。(2)WKNN定位算法加權(quán)K近鄰(WeightedK-NearestNeighborhood,WKNN)定位算法在定位精度和計(jì)算復(fù)雜度上有一個(gè)良好的折中,因此本文選用WKNN定位算法。定義測試點(diǎn)與參考點(diǎn)之間的歐式距離為式中Sij是第j個(gè)參考點(diǎn)處接收的第i個(gè)AP中的RSSI值,Si是經(jīng)過修正過后的第i個(gè)AP的RSSI值,i=1,2,L,m,j=1,2,L,n,m是AP個(gè)數(shù),n是參考點(diǎn)的數(shù)目。WKNN定位算法是選取K(K>1)個(gè)最小歐式距離對應(yīng)的坐標(biāo),每個(gè)坐標(biāo)根據(jù)歐式距離大小給予權(quán)值,歐式距離越小權(quán)值越大,計(jì)算公式如下式中是t時(shí)刻WLAN定位結(jié)果;ε為一個(gè)極小量,防止出現(xiàn)分母為0的情況;為測試點(diǎn)到第i參考點(diǎn)歐氏距離的倒數(shù),測試點(diǎn)到K個(gè)參考點(diǎn)歐氏距離倒數(shù)之和。2、MEMS傳感器數(shù)據(jù)處理根據(jù)MEMS傳感器,即加速度計(jì)、陀螺儀、磁力計(jì)輸出的信息,利用步態(tài)檢測和四元素法更新姿態(tài)矩陣可以得到行人的步數(shù)和航向信息。首先計(jì)算加速度計(jì)輸出的模值和分別表示加速度計(jì)輸出的3軸加速度。然后通過檢測該模值A(chǔ)norm為峰值且大于某個(gè)設(shè)定的閾值A(chǔ)0來判斷跨步,完成步態(tài)檢測。最后假設(shè)MEMS傳感器的輸出頻率為fs,且通過檢測加速度計(jì)輸出模值A(chǔ)norm連續(xù)兩個(gè)大于閾值A(chǔ)0的峰值之間的數(shù)據(jù)點(diǎn)數(shù)ΔN,可以計(jì)算第k步所需的時(shí)間為假設(shè)行人每一步為勻速運(yùn)動,則可以計(jì)算出第k步的速度為式中Pk為第k步的步長,vk為第k步計(jì)算出的速度。令第k步中每個(gè)采樣時(shí)刻的速度都為vk,則每一秒可計(jì)算出fs個(gè)速度,取每一秒計(jì)算出的所有速度的均值作為該秒計(jì)算出的速度。利用加速度計(jì)、陀螺儀、磁力計(jì)輸出數(shù)據(jù)計(jì)算更新四元素,結(jié)合四元素法更新姿態(tài)矩陣方法計(jì)算每一個(gè)采樣時(shí)刻的航向角,并取每一秒第一個(gè)采樣時(shí)刻的航向角作為MEMS每一秒的航向角。3、抗差擴(kuò)展卡爾曼濾波器(1)抗差卡爾曼濾波器在實(shí)際環(huán)境下,系統(tǒng)將受到粗差、模型誤差等的影響,這些將嚴(yán)重影響融合濾波的精確度。抗差估計(jì),亦稱穩(wěn)健估計(jì),是指在這些影響不可避免的情況下通過選擇適當(dāng)?shù)墓烙?jì)方法,使未知量估值盡可能減少受到影響的程度,從而得出正常模式下的最佳估計(jì)??共頜估計(jì)是使用最為廣泛,計(jì)算較簡明的一種抗差估計(jì)法。M估計(jì)是經(jīng)典的極大似然估計(jì)的推廣,稱為廣義極大似然型估計(jì)。目前常用的抗差估計(jì)方法有Huber法、IGG法、Tukey法等。IGG是中國科學(xué)院測量與地球物理研究所的英文縮寫,相比于其他權(quán)函數(shù),IGG3權(quán)函數(shù)是一個(gè)有界、分段、連續(xù)的函數(shù),如下式所示式中為求得的等價(jià)權(quán)矩陣中的第i個(gè)對角元素,且為對角矩陣;vi為n×1維殘差向量V的第i個(gè)觀測值的測量殘差,n為觀測變量個(gè)數(shù),且V=Z-HX,Z為n×1維觀測向量,X為m×1維待估計(jì)向量,m為需要估計(jì)的變量個(gè)數(shù),H為n×m維量測矩陣;pi為權(quán)矩陣P的第i個(gè)對角元素,這里假設(shè)觀測值獨(dú)立,權(quán)矩陣P為對角矩陣,且P=R-1,R為觀測噪聲協(xié)方差矩陣;ui為標(biāo)準(zhǔn)化的殘差向量,σvi為vi的均方差,實(shí)際情況下qvi為vi的權(quán)倒數(shù),方差因子σ0可以根據(jù)求得,“med”表示取中位數(shù);k0稱為保權(quán)臨界值,k1稱為零權(quán)臨界值,根據(jù)需要k0,k1可分別取1~1.5,2.5~3.0。假設(shè)卡爾曼濾波狀態(tài)方程和觀測方程分別為Xt=Φt-1Xt-1+Γt,t-1Wt-1(8)Zt=HtXt+Vt式中Xt為t時(shí)刻n×1維狀態(tài)向量,n×n維矩陣Φt-1為系統(tǒng)的一步轉(zhuǎn)移矩陣;Wt-1為t-1時(shí)刻n維白噪聲矢量,Γt,t-1為t-1時(shí)刻n×n維系統(tǒng)噪聲矩陣;Zt為t時(shí)刻m×1維觀測矢量,Ht為t時(shí)刻m×m維觀測矩陣,Vt為m維觀測白噪聲矢量。由卡爾曼濾波基本原理和抗差估計(jì)原理可以得到擴(kuò)展抗差卡爾曼濾波基本方程如下所示:狀態(tài)一步預(yù)測式中為t-1時(shí)刻最優(yōu)狀態(tài)估計(jì),Φt-1系統(tǒng)的一步轉(zhuǎn)移矩陣,為t時(shí)刻狀態(tài)的一步預(yù)測值。誤差協(xié)方差矩陣一步預(yù)測式中為t時(shí)刻誤差協(xié)方差矩陣的一步預(yù)測,為t-1時(shí)刻的最優(yōu)估計(jì)誤差協(xié)方差矩陣,Qt-1為t-1時(shí)刻的過程噪聲協(xié)方差矩陣。濾波增益計(jì)算式中為t時(shí)刻求得的卡爾曼濾波增益,Ht為t時(shí)刻觀測矩陣,為通過等價(jià)權(quán)求得的觀測噪聲誤差協(xié)方差矩陣,且為通過抗差M估計(jì)求得的等價(jià)權(quán)矩陣。最優(yōu)狀態(tài)估計(jì)式中為t時(shí)刻求得的最優(yōu)狀態(tài)估計(jì)。誤差協(xié)方差矩陣估計(jì)式中為t時(shí)刻的最優(yōu)估計(jì)誤差協(xié)方差矩陣。(2)抗差擴(kuò)展卡爾曼濾波器設(shè)計(jì)本發(fā)明采用抗差擴(kuò)展卡爾曼濾波器作為組合導(dǎo)航濾波器,選用東位置、北位置、速度和航向作為狀態(tài)量,選用WLAN輸出的位置信息、MEMS輸出的速度和航向信息作為觀測量,狀態(tài)方程如下:式中xt表示t時(shí)刻待估計(jì)的東位置,yt表示t時(shí)刻待估計(jì)的北位置,vt表示t時(shí)刻待估計(jì)的速度,表示t時(shí)刻待估計(jì)的航向,xt-1表示t-1時(shí)刻的東位置,yt-1表示t-1時(shí)刻的北位置,vt-1表示t-1時(shí)刻的速度,表示t-1時(shí)刻的航向角,Wt-1表示過程噪聲,是均值為0的高斯白噪聲序列,且i,j=1,2,...,m,m為需要估計(jì)的變量個(gè)數(shù),δij為Kronecker函數(shù),Q為m維過程噪聲協(xié)方差矩陣。觀測方程如下:式中表示t時(shí)刻WLAN定位出的東位置,表示t時(shí)刻WLAN定位出的北位置,表示t時(shí)刻MEMS輸出的速度,表示t時(shí)刻MEMS輸出的航向;xt表示t時(shí)刻待估計(jì)的東位置,yt表示t時(shí)刻待估計(jì)的北位置,vt表示t時(shí)刻待估計(jì)的速度,表示t時(shí)刻待估計(jì)的航向,Vt表示觀測噪聲,是均值為0的高斯白噪聲序列,且E[V(i)VT(j)]=R(i,j)δij,i,j=1,2,...,n,n為觀測變量個(gè)數(shù),δij為Kronecker函數(shù),R為n維觀測噪聲協(xié)方差矩陣。4、高度計(jì)算(1)氣壓計(jì)測量海拔高度氣壓測高的基本原理是指在重力場內(nèi)的大氣壓力隨著高度的增加而減小,通過氣壓傳感器可以測量氣壓的大小,通過計(jì)算轉(zhuǎn)換可以得到高度信息。在重力勢高度0≤H≤11000米時(shí),國際標(biāo)準(zhǔn)壓高公式如下式中H為計(jì)算出來的重力勢高度,Ps為測得的氣壓值。重力勢高度H和設(shè)備所在高度h的關(guān)系如下式中r為地球公稱半徑,且r=6356766米。由于r?H,所以重力勢高度H和設(shè)備所在高度h近似相等,即h=H。(2)計(jì)算高度和選擇位置指紋數(shù)據(jù)庫正常情況下,將行人在室內(nèi)行走分為:平路行走、上樓梯和下樓梯3種情況,在這3種情況下解算2維位置方法相同(均采用抗差擴(kuò)展卡爾曼濾波進(jìn)行解算),只是在設(shè)定步長上稍有區(qū)別,即上樓梯時(shí)步長應(yīng)該設(shè)為一個(gè)樓梯臺階的寬度。如果直接利用高度計(jì)測量的海拔高度信息作為當(dāng)前時(shí)刻的z軸輸出,由于氣壓計(jì)的測量誤差,測量的高度會出現(xiàn)不準(zhǔn),可能出現(xiàn)大范圍的跳變,而且在不同時(shí)間、不同季節(jié)同一地點(diǎn)的氣壓值也不同。本發(fā)明充分考慮到這些因素的影響,只利用氣壓計(jì)測量的高度值判斷上下樓,而實(shí)際的高度信息則通過判斷上下樓進(jìn)行高度的累加或者累減進(jìn)行計(jì)算。將行人行走分為3種情況:樓層中行走、上樓梯和下樓梯。通過地理位置信息,可以判斷出行人所處的大概位置(在樓層中或者在樓梯間),如果在樓梯間,可以通過高度計(jì)測量的海拔高度信息判斷行人上樓梯或者下樓梯,因此實(shí)現(xiàn)高度的累加或者累減,如果在樓層中,則將所處樓層的高度直接賦值給當(dāng)前時(shí)刻的z軸輸出,實(shí)現(xiàn)室內(nèi)空間跨樓層3維定位。具體的判斷規(guī)則如圖2所示:首先利用地理位置信息,判斷行人是否進(jìn)出樓梯間。測試測試環(huán)境中所有樓梯口出的3維位置坐標(biāo)為(xi,yi,zi),i=1,2K,n,其中n為樓梯口總的個(gè)數(shù)。假設(shè)行人在t-1時(shí)刻的最優(yōu)位置為分別計(jì)算其與每一個(gè)樓梯口的物理距離為式中di為時(shí)刻t-1的最優(yōu)位置與第i個(gè)樓梯口的物理距離。設(shè)定門限r(nóng),如果di<r且判斷為進(jìn)入樓梯間(如果上個(gè)時(shí)刻在樓層中,則判斷為進(jìn)入樓梯間),此時(shí)匹配樓梯間的指紋數(shù)據(jù)庫,并保存此時(shí)的氣壓計(jì)測量高度信息h2;如果di<r且判斷為出樓梯間(如果上個(gè)時(shí)刻在樓梯間,則判斷為出樓梯間),此時(shí)還需利用前一時(shí)刻的實(shí)際高度信息判斷所處樓層,選擇相應(yīng)的位置指紋數(shù)據(jù)庫,如果di>r,則位置指紋數(shù)據(jù)庫不變,與前一時(shí)刻的位置指紋數(shù)據(jù)庫相同。如果判斷為在樓梯間,此時(shí)計(jì)算氣壓計(jì)測量的高度h1和剛進(jìn)入樓梯間的氣壓計(jì)測量高度h2的差值Δh。設(shè)定門限λ,如果|Δh|<λ,則此時(shí)輸出的高度不變,令其等于前一時(shí)刻的實(shí)際高度,如果|Δh|>λ且Δh>0,則此時(shí)輸出的高度在前一時(shí)刻的高度上面累加固定值hstair,如果|Δh|>λ且Δh<0,則此時(shí)輸出的高度在前一時(shí)刻的高度上面累減固定值hstair。如果判斷為在樓層中,則將所處樓層的高度直接賦值給當(dāng)前時(shí)刻的z軸輸出。實(shí)施例:如圖1所示,室內(nèi)WLAN/MEMS融合跨樓層3維定位方法包括以下步驟:1)初始化抗差擴(kuò)展卡爾曼組合導(dǎo)航濾波器,具體為給定初始位置信息(x0,y0),誤差協(xié)方差矩陣P0,過程噪聲協(xié)方差矩陣Q,和觀測噪聲協(xié)方差矩陣R,給定層高度初值z0,并選擇初始位置指紋庫;2)假設(shè)得到t-1時(shí)刻的最優(yōu)估計(jì)位置為t=1,2,K,計(jì)算與樓梯口的物理距離di,選擇WLAN位置指紋數(shù)據(jù)庫,具體如下:測量測試環(huán)境中所有樓梯口出的3維位置坐標(biāo)為(xi,yi,zi),i=1,2K,n,其中n為樓梯口總的個(gè)數(shù)。分別計(jì)算其與每一個(gè)樓梯口的物理距離為式中di為時(shí)刻t-1的最優(yōu)位置與第i個(gè)樓梯口的物理距離。如圖2所示,設(shè)定門限r(nóng),如果di<r且判斷為進(jìn)入樓梯間,此時(shí)匹配樓梯間的指紋數(shù)據(jù)庫;如果di<r且判斷為出樓梯間,此時(shí)還需利用前一時(shí)刻的實(shí)際高度信息判斷所處樓層,選擇相應(yīng)的位置指紋數(shù)據(jù)庫。如果di>r,則位置指紋數(shù)據(jù)庫不變,與前一時(shí)刻的位置指紋數(shù)據(jù)庫相同。3)實(shí)際測量t時(shí)刻RSSI向量,并利用擴(kuò)展卡爾曼濾波器中的一步預(yù)測信息反匹配位置指紋數(shù)據(jù)庫修正RSSI向量,并利用修正的RSSI采用WKNN算法匹配位置指紋數(shù)據(jù)庫,得到t時(shí)刻的WLAN定位結(jié)果具體如下:定義t時(shí)刻擴(kuò)展卡爾曼預(yù)測位置到WLAN位置指紋數(shù)據(jù)庫中參考點(diǎn)的物理距離為式中(xj,yj)為位置指紋數(shù)據(jù)庫中第j個(gè)參考點(diǎn)的位置,n為位置指紋庫中參考點(diǎn)總數(shù)。在計(jì)算出t時(shí)刻到所有參考點(diǎn)的距離Dtj后,選取K個(gè)物理距離最小的參考點(diǎn)對應(yīng)的RSSI向量,利用類似WKNN算法得到預(yù)測的RSSI向量為式中RSSI1為得到的RSSI預(yù)測向量,RSSIm為t時(shí)刻K個(gè)與擴(kuò)展卡爾曼預(yù)測位置物理距離最小的參考點(diǎn)中第m點(diǎn)對應(yīng)的RSSI向量,Dtm為其對應(yīng)的物理距離,為t時(shí)刻K個(gè)與擴(kuò)展卡爾曼預(yù)測位置物理距離最小的參考點(diǎn)對應(yīng)的物理距離的和,式中q為索引號。得到RSSI1過后,對實(shí)測的RSSI2進(jìn)行修正有RSSIt=w×RSSI1+(1-w)×RSSI2(22)式中RSSIi為t時(shí)刻最終的RSSI向量,w為加權(quán)系數(shù),取值范圍為0≤w≤1。計(jì)算與參考點(diǎn)之間的歐式距離式中Sij是第j個(gè)參考點(diǎn)處接收的第i個(gè)AP中的RSSI值,Si是經(jīng)過修正過后的第i個(gè)AP的RSSI值,i=1,2,L,m,j=1,2,L,n,m是AP個(gè)數(shù),n是參考點(diǎn)的數(shù)目。利用WKNN匹配算法得到t時(shí)刻的WLAN定位結(jié)果計(jì)算公式如下式中ε為一個(gè)極小量,防止出現(xiàn)分母為0的情況,為測試點(diǎn)到第i參考點(diǎn)歐氏距離的倒數(shù),測試點(diǎn)到K(K>1)個(gè)參考點(diǎn)歐氏距離倒數(shù)之和。4)處理MEMS傳感器輸出信息,得到t時(shí)刻速度和航向具體如下:首先計(jì)算加速度計(jì)輸出的模值和分別表示加速度計(jì)輸出的3軸加速度。然后通過檢測該模值A(chǔ)norm為峰值且大于某個(gè)設(shè)定的閾值A(chǔ)0來判斷跨步,完成步態(tài)檢測。最后假設(shè)MEMS傳感器的輸出頻率為fs,且通過檢測加速度計(jì)輸出模值A(chǔ)norm連續(xù)兩個(gè)大于閾值A(chǔ)0的峰值之間的數(shù)據(jù)點(diǎn)數(shù)ΔN,可以計(jì)算第k步所需的時(shí)間為假設(shè)行人每一步為勻速運(yùn)動,則可以計(jì)算出第k步的速度為式中Pk為第k步的步長,vk為第k步計(jì)算出的速度。令第k步中每個(gè)采樣時(shí)刻的速度都為vk,取t時(shí)刻計(jì)算出的所有采樣點(diǎn)速度的均值作為MEMS在t時(shí)刻輸出的速度利用t時(shí)刻MEMS測量得到fs組加速度計(jì)、陀螺儀和磁力計(jì)數(shù)據(jù)更新四元素,從而計(jì)算得到fs個(gè)航向角,取t時(shí)刻計(jì)算得到的第一個(gè)航向角作為t時(shí)刻的航向角5)將t時(shí)刻測量得到的WLAN位置信息與MEMS輸出的速度和航向帶入抗差擴(kuò)展卡爾曼濾波器,得到t時(shí)刻2維位置最優(yōu)估計(jì)值6)通過判斷出行人所處的大概位置(在樓層中或者在樓梯間),如果在樓梯間,可以通過高度計(jì)測量的海拔高度信息判斷行人上樓梯或者下樓梯,因此實(shí)現(xiàn)高度的累加或者累減;如果在樓層中,則將所處樓層的高度直接賦值給當(dāng)前時(shí)刻的z軸輸出,具體方法如下:如圖2所示,如果判斷為在樓梯間,此時(shí)計(jì)算氣壓計(jì)測量的高度h1和剛進(jìn)入樓梯間的氣壓計(jì)測量高度h2的差值Δh。設(shè)定門限λ,如果|Δh|<λ,則此時(shí)輸出的高度不變,令其等于前一時(shí)刻的實(shí)際高度,如果|Δh|>λ且Δh>0,則此時(shí)輸出的高度在前一時(shí)刻的高度上面累加固定值hstair,如果|Δh|>λ且Δh<0,則此時(shí)輸出的高度在前一時(shí)刻的高度上面累減固定值hstair。如果判斷為在樓層中,則將所處樓層的高度直接賦值給當(dāng)前時(shí)刻的z軸輸出。最后應(yīng)該說明的是:以上所述僅為本發(fā)明的優(yōu)選實(shí)施例,并不限制本發(fā)明,盡管利用實(shí)施例對本發(fā)明進(jìn)行了詳細(xì)的說明,但對于本領(lǐng)域的技術(shù)人員來說,其依然可以對前述實(shí)施例所記載的技術(shù)方案進(jìn)行修改,或者對其中的部分技術(shù)進(jìn)行等同替換。凡在本發(fā)明的精神和原則之內(nèi),所作的任何修改、等同替換、改進(jìn)等,均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。