一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法
【專利摘要】本發(fā)明公開了一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法，包括以下步驟：粒子初始化，設(shè)定定位對(duì)象初始位置，在定位對(duì)象初始位置按照正態(tài)分布隨機(jī)生成N個(gè)粒子，并載入地圖信息；讀入定位裝置得到的定位對(duì)象第k步的步長(zhǎng)SLk和航向根據(jù)狀態(tài)方程對(duì)粒子預(yù)估計(jì)，得到第i個(gè)粒子的第k步的狀態(tài)變量k為大于等于1的自然數(shù)，k初始值為1；i＝1,2,…,N，表示第i個(gè)粒子；粒子更新：分別判定每個(gè)粒子是否穿過障礙物，如果是，則將粒子權(quán)重設(shè)為0，否則粒子權(quán)重不變；進(jìn)行權(quán)重歸一化，并計(jì)算定位對(duì)象第k步的航位估計(jì)值Xk；判斷權(quán)重不為0的粒子數(shù)量是否大于N/2，如果是，則將k加1后賦值給k，否則進(jìn)行粒子重采樣，并將k加1后賦值給k。
【專利說明】一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種導(dǎo)航【技術(shù)領(lǐng)域】的累積誤差消除方法，具體是一種基于粒子濾波的室內(nèi)定位位置校正方法。
【背景技術(shù)】
[0002]隨著社會(huì)和經(jīng)濟(jì)的飛速發(fā)展，人們對(duì)定位與導(dǎo)航的需求日益增加，而這種需求在消防領(lǐng)域內(nèi)也正日益凸顯。在當(dāng)今重特大惡性災(zāi)害火災(zāi)事故和化學(xué)泄漏事故日益增多的形勢(shì)下，面對(duì)各種類型的嚴(yán)峻災(zāi)害，如何在有效提升災(zāi)害救援的進(jìn)度和效率的同時(shí)，又能最大限度地保障第一線消防員個(gè)人安危，正成為一個(gè)亟待解決的現(xiàn)實(shí)問題。顯然，平時(shí)系統(tǒng)而扎實(shí)的消防演練和培訓(xùn)是解決這個(gè)問題的根本之道，但隨著技術(shù)的進(jìn)步，尤其是定位導(dǎo)航技術(shù)的進(jìn)步，將為這個(gè)問題的解決提供更加科學(xué)的解決方案。
[0003]消防行業(yè)需要在現(xiàn)場(chǎng)緊急情況下保護(hù)消防隊(duì)員的安全，調(diào)度中心知道消防隊(duì)員的位置信息后，可以優(yōu)化調(diào)度。該行業(yè)目前的狀況是消防隊(duì)員靠對(duì)講機(jī)和調(diào)度中心聯(lián)系，匯報(bào)位置很繁瑣，并且得不到實(shí)時(shí)位置信息。重要的是，如果消防隊(duì)員出了問題，調(diào)度中心無(wú)法知道其位置，也無(wú)法調(diào)度其他消防隊(duì)員對(duì)其救援。反恐特警有著類似的需求，當(dāng)特警進(jìn)入建筑物內(nèi)后，調(diào)度中心知道其位置就可以有效的調(diào)度。
[0004]PDR (行人導(dǎo)航推算，Pedestrian Dead Reckoning)系統(tǒng)采用MEMS (微機(jī)電系統(tǒng)，Micro-Electro-Mechanical System)傳感器，以計(jì)步檢測(cè)為基礎(chǔ),從已知的起始位置進(jìn)行航位推算的相對(duì)導(dǎo)航技術(shù)。PDR系統(tǒng)不需要預(yù)裝基礎(chǔ)設(shè)施，可以在沒有GPS信號(hào)的環(huán)境中滿足定位需求。
[0005]地圖匹配算法被廣泛用于車載導(dǎo)航，行駛在道路上的汽車受到道路的約束使地圖匹配算法行之有效。
[0006]現(xiàn)有技術(shù)的缺點(diǎn):l)roR系統(tǒng)的誤差會(huì)隨時(shí)間而累積。2)室內(nèi)地圖約束沒有一成型方法。
[0007]現(xiàn)有的地圖約束修正方法:對(duì)于“穿越墻壁”的軌跡，簡(jiǎn)單的做一些幾何修正，并不能達(dá)到位置校正的理想效果。

【發(fā)明內(nèi)容】

[0008]發(fā)明目的:本發(fā)明所要解決的技術(shù)問題是針對(duì)現(xiàn)有技術(shù)的不足，提供一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法。
[0009]為了解決上述技術(shù)問題，本發(fā)明公開了一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法，包括以下步驟:
[0010]步驟(I )，粒子初始化，設(shè)定定位對(duì)象初始位置，在定位對(duì)象初始位置按照正態(tài)分布隨機(jī)生成N個(gè)粒子，并載入地圖信息；
[0011]步驟(2)，讀入定位裝置得到的定位對(duì)象第k步的步長(zhǎng)SLk和航向奶根據(jù)狀態(tài)方程對(duì)粒子預(yù)估計(jì)，得到第i個(gè)粒子的第k步的狀態(tài)變量\ k為大于等于I的自然數(shù)，k初始值為I ;i = 1，2，…，N，表示第i個(gè)粒子；
[0012]步驟(3)，粒子更新:分別判定每個(gè)粒子是否穿過障礙物，如果是，則將粒子權(quán)重設(shè)為0，否則粒子權(quán)重不變，定位對(duì)象第k步時(shí)第i個(gè)粒子的權(quán)重M汁算公式為:
[0013]
【權(quán)利要求】
1.一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法，其特征在于，包括以下步驟: 步驟(I )，粒子初始化，設(shè)定定位對(duì)象初始位置，在定位對(duì)象初始位置按照正態(tài)分布隨機(jī)生成N個(gè)粒子，并載入地圖信息； 步驟(2)，讀入定位裝置得到的定位對(duì)象第k步的步長(zhǎng)SLk和航向弘，根據(jù)狀態(tài)方程對(duì)粒子預(yù)估計(jì)，得到第i個(gè)粒子的第k步的狀態(tài)變量;^，k為大于等于I的自然數(shù)，k初始值為I ;i = 1，2，…，N，表示第i個(gè)粒子； 步驟(3)，粒子更新:分別判定每個(gè)粒子是否穿過障礙物，如果是，則將粒子權(quán)重設(shè)為O，否則粒子權(quán)重不變，定位對(duì)象第k步時(shí)第i個(gè)粒子的權(quán)重計(jì)算公式為:
2.根據(jù)權(quán)利要求1所述的一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法，其特征在于，粒子個(gè)數(shù)N = 500。
3.根據(jù)權(quán)利要求1所述的一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法，其特征在于，所述地圖信息為建筑三維地圖信息，包括標(biāo)識(shí)了障礙物位置、可以通行的區(qū)域、每一個(gè)房間的ID、房間的區(qū)域、房間門的坐標(biāo)。
4.根據(jù)權(quán)利要求1所述的一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法，其特征在于，步驟(2)中第i個(gè)粒子的狀態(tài)方程的計(jì)算公式為:
5.根據(jù)權(quán)利要求4所述的一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法，其特征在于，權(quán)重歸一化的公式為:
6.根據(jù)權(quán)利要求5所述的一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法，其特征在于，步驟(4)中，定位對(duì)象第k步的航位估計(jì)值Xk的計(jì)算公式為:
7.根據(jù)權(quán)利要求1所述的一種基于室內(nèi)定位裝置的室內(nèi)定位位置校正方法，其特征在于，步驟(5)中，所述重采樣為在當(dāng)前定位對(duì)象的位置周圍按照正態(tài)分布隨機(jī)生成N個(gè)粒子，粒子個(gè)數(shù)N = 500。
【文檔編號(hào)】G01C21/20GK103791906SQ201410060302
【公開日】2014年5月14日 申請(qǐng)日期:2014年2月21日 優(yōu)先權(quán)日:2014年2月21日
【發(fā)明者】申崇江, 馮成濤, 劉楊, 陳婷 申請(qǐng)人:南京北大工道創(chuàng)新有限公司