本發(fā)明涉及一種室內(nèi)定位方法與裝置，尤其涉及一種基于慣導(dǎo)與基站數(shù)據(jù)融合的室內(nèi)定位方法與裝置。本發(fā)明屬于室內(nèi)慣性定位領(lǐng)域，特別是以慣性導(dǎo)航定位為基礎(chǔ)的數(shù)據(jù)融合輔助定位領(lǐng)域。

背景技術(shù)：

在消防救援等應(yīng)急任務(wù)場(chǎng)景中，獲取消防員、搜救員、警用機(jī)器人等待監(jiān)測(cè)物的實(shí)時(shí)位置可以極大的提高救援與工作效率，并且可有效避免救援人員在火場(chǎng)迷失方向而造成的不必要人員傷亡。慣性定位作為一種在應(yīng)急任務(wù)中獲取消防員等待監(jiān)測(cè)物位置的重要方法，由于其不依靠外部設(shè)備，定位自主性強(qiáng)，短時(shí)定位精度高，在消防等應(yīng)急突發(fā)事件應(yīng)用中有不可替代的重要作用。

同時(shí)，慣性定位也存在如下的不足之處：(1)慣性器件受加工工藝與安裝因素影響存在測(cè)量誤差，測(cè)量誤差會(huì)隨著定位時(shí)間增長(zhǎng)而累積，從而導(dǎo)致對(duì)所需監(jiān)測(cè)物的定位精度下降和定位可靠性降低；(2)在實(shí)際應(yīng)用中由于使用前準(zhǔn)備時(shí)間的限制，不能充分獲取慣性器件的初始偏置誤差，導(dǎo)致航向上的誤差增大，定位可靠性下降。因此，現(xiàn)有的慣性定位方法與裝置無(wú)法滿足要求。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明要解決的問題是針對(duì)現(xiàn)有慣性定位存在的問題，提供一種基于慣導(dǎo)的室內(nèi)定位方法及裝置，使得定位結(jié)果更加精確，獲取長(zhǎng)時(shí)間可靠的定位結(jié)果。

為解決上述技術(shù)問題，本發(fā)明采用的技術(shù)方案是：一種基于慣導(dǎo)的室內(nèi)定位方法，待監(jiān)測(cè)載體行進(jìn)區(qū)域內(nèi)固定設(shè)置有基站，所述基站為ibeacon傳感器基站和/或超寬帶傳感器基站和/或wifi傳感器基站和/或地磁傳感器基站和/或rfid基站，待監(jiān)測(cè)載體上設(shè)置有加速度計(jì)、陀螺儀，所述基于慣導(dǎo)的室內(nèi)定位方法包括如下步驟：

(1)待監(jiān)測(cè)載體行進(jìn)中，采集待監(jiān)測(cè)載體在載體坐標(biāo)系下的陀螺儀測(cè)量數(shù)據(jù)、載體坐標(biāo)系下的加速度計(jì)測(cè)量數(shù)據(jù)；

(2)利用陀螺儀測(cè)量數(shù)據(jù)、加速度計(jì)測(cè)量數(shù)據(jù)計(jì)算待監(jiān)測(cè)載體在導(dǎo)航坐標(biāo)系下的位置(xk,yk)；若待監(jiān)測(cè)載體為人員，則k為采樣時(shí)刻或人員行進(jìn)的步數(shù)；若待監(jiān)測(cè)載體為非人員，

則k為采樣時(shí)刻；

(3)若檢測(cè)到基站的信號(hào)，則跳轉(zhuǎn)到步驟(4)，否則，跳轉(zhuǎn)到步驟(1)；

(4)利用待監(jiān)測(cè)載體在導(dǎo)航坐標(biāo)系下的位置(xk,yk)計(jì)算粒子傳播距離lk、粒子傳播方向θk、粒子傳播方向變化量δθk，其中θk∈[-π,π]：

(5)利用下式進(jìn)行迭代運(yùn)算，計(jì)算粒子位置集合

其中i為粒子個(gè)數(shù)，i＝1,2,…,n，為粒子傳播距離噪聲，為粒子傳播方向噪聲，為陀螺誤差量，為陀螺誤差的噪聲，δt為粒子更新時(shí)間間隔，δt由獲取的基站2的信號(hào)的更新率確定；可采用有色噪聲或白噪聲；

(6)根據(jù)檢測(cè)到的基站的信號(hào)計(jì)算粒子權(quán)值

(7)根據(jù)步驟(6)得到的粒子權(quán)值對(duì)所有粒子進(jìn)行重采樣得到新的粒子位置集合并利用下式對(duì)粒子位置求均值，得到待監(jiān)測(cè)載體的位置估計(jì)

本發(fā)明中，利用基站測(cè)量數(shù)據(jù)與慣性定位結(jié)果相融合，使得定位結(jié)果更加精確，獲取長(zhǎng)時(shí)間可靠的定位結(jié)果。本發(fā)明中，利用陀螺儀測(cè)量數(shù)據(jù)、加速度計(jì)測(cè)量數(shù)據(jù)計(jì)算待監(jiān)測(cè)載體在導(dǎo)航坐標(biāo)系下的位置；利用待監(jiān)測(cè)載體在導(dǎo)航坐標(biāo)系下的位置計(jì)算粒子傳播距離、粒子傳播方向、粒子傳播方向變化量，得到計(jì)算粒子位置集合；根據(jù)檢測(cè)到的基站的信號(hào)計(jì)算粒子權(quán)值；根據(jù)粒子權(quán)值對(duì)所有粒子進(jìn)行重采樣得到新的粒子位置集合，并對(duì)粒子位置求均值，得到待監(jiān)測(cè)載體的位置估計(jì)。本發(fā)明中，可設(shè)置一種類型或多種類型的基站，各個(gè)基站位于不同的作用范圍內(nèi)，當(dāng)待監(jiān)測(cè)載體行進(jìn)到某個(gè)基站的作用范圍時(shí)，即利用此基站的數(shù)據(jù)對(duì)待監(jiān)測(cè)載體的位置進(jìn)行修正。由于基站的位置是確定的，因此基站周圍的距離與信號(hào)強(qiáng)度的關(guān)系也是確定的，因此利用基站的數(shù)據(jù)的修正，可以大大減小隨著定位時(shí)間增長(zhǎng)而累積的誤差，使得定位精度大大提高。

上述技術(shù)方案中，所述步驟(2)包括如下子步驟：

(2.1)利用陀螺儀測(cè)量數(shù)據(jù)計(jì)算姿態(tài)變換矩陣；

(2.2)利用姿態(tài)變換矩陣和加速度計(jì)測(cè)量數(shù)據(jù)計(jì)算導(dǎo)航坐標(biāo)系下的加速度計(jì)測(cè)量數(shù)據(jù)；

(2.3)將步驟(2.2)計(jì)算得到的導(dǎo)航坐標(biāo)系下的加速度計(jì)測(cè)量數(shù)據(jù)進(jìn)行積分運(yùn)算得到待監(jiān)測(cè)載體在導(dǎo)航坐標(biāo)系下載體的原始計(jì)算位置(x'k,y'k)；當(dāng)載體有零速狀態(tài)時(shí)，利用零速修正技術(shù)對(duì)(x'k,y'k)進(jìn)行修正得到待監(jiān)測(cè)載體在導(dǎo)航坐標(biāo)系下的位置(xk,yk)。

上述技術(shù)方案中，所述待監(jiān)測(cè)載體為人員，所述步驟(2)包括如下子步驟：

(2.1)利用下式計(jì)算人員行進(jìn)中第k步的步長(zhǎng)lk

其中，為人員行進(jìn)中第k步的步頻，τ1、τ2分別為第一相關(guān)系數(shù)、第二相關(guān)系數(shù)；

(2.2)利用下式計(jì)算待監(jiān)測(cè)載體在導(dǎo)航坐標(biāo)系下的位置(xk,yk)

其中，φk為人員行進(jìn)中第k-1步到第k步的步態(tài)方向變化角。

本發(fā)明中，可利用加速度計(jì)測(cè)量數(shù)據(jù)計(jì)算人員的步頻利用陀螺儀測(cè)量數(shù)據(jù)計(jì)算人員行進(jìn)中第k-1步到第k步的步態(tài)方向變化角φk。

上述技術(shù)方案中，所述待監(jiān)測(cè)載體為輪式機(jī)器人，所述步驟(2)中，利用下式計(jì)算輪式機(jī)器人在導(dǎo)航坐標(biāo)系下的位置(xk,yk)

其中，lk為k-1時(shí)刻到k時(shí)刻輪式機(jī)器人的水平位移增量，為k時(shí)刻的輪式方向角。

本發(fā)明中，可利用陀螺儀測(cè)量數(shù)據(jù)或車輪之間的差速計(jì)算k時(shí)刻的輪式方向角

上述技術(shù)方案中，所述步驟(6)中，檢測(cè)到的信號(hào)為ibeacon傳感器基站的信號(hào)強(qiáng)度srss,k，若srss,k＞sthres，則利用下式計(jì)算粒子權(quán)值

其中，(xibeacon,yibeacon)為ibeacon傳感器基站的位置坐標(biāo)，sthres為信號(hào)強(qiáng)度門限，dthres為距離門限。

本發(fā)明中，待監(jiān)測(cè)載體行進(jìn)到ibeacon傳感器基站的作用范圍時(shí)，判斷檢測(cè)到的信號(hào)為ibeacon傳感器基站的信號(hào)強(qiáng)度srss,k是否滿足要求，若滿足要求，則利用ibeacon傳感器基站計(jì)算粒子權(quán)值，從而實(shí)現(xiàn)利用ibeacon傳感器基站的數(shù)據(jù)對(duì)待監(jiān)測(cè)載體位置進(jìn)行修正的目的。

上述技術(shù)方案中，所述步驟(6)中，檢測(cè)到的信號(hào)為超寬帶傳感器基站的信噪比參數(shù)ζk，若ζk＞ζthres，則利用下式計(jì)算粒子權(quán)值

其中，(xubw,yuwb)為超寬帶傳感器基站的位置坐標(biāo)，di為超寬帶傳感器基站與待監(jiān)測(cè)載體(4)之間的相對(duì)距離，ζthres為信噪比門限。

本發(fā)明中，待監(jiān)測(cè)載體行進(jìn)到超寬帶傳感器基站的作用范圍時(shí)，判斷檢測(cè)到的信號(hào)為超寬帶傳感器基站的信噪比參數(shù)ζk是否滿足要求，若滿足要求，則利用超寬帶傳感器基站計(jì)算粒子權(quán)值，從而實(shí)現(xiàn)利用超寬帶傳感器基站的數(shù)據(jù)對(duì)待監(jiān)測(cè)載體位置進(jìn)行修正的目的。

上述技術(shù)方案中，所述步驟(6)中，檢測(cè)到的信號(hào)為wifi傳感器基站的wifi指紋，利用下式計(jì)算粒子權(quán)值

其中，(xwifi,ywifi)為利用wifi指紋計(jì)算得到的待監(jiān)測(cè)載體的位置，為位置測(cè)量方差。

本發(fā)明中，待監(jiān)測(cè)載體行進(jìn)到wifi傳感器基站的作用范圍時(shí)，則利用wifi傳感器基站計(jì)算粒子權(quán)值，從而實(shí)現(xiàn)利用wifi傳感器基站的數(shù)據(jù)對(duì)待監(jiān)測(cè)載體位置進(jìn)行修正的目的。

本發(fā)明還提供一種實(shí)現(xiàn)上述方法的基于慣導(dǎo)的室內(nèi)定位裝置，包括固定設(shè)置于待監(jiān)測(cè)載體行進(jìn)區(qū)域內(nèi)的至少一個(gè)基站，所述基站為ibeacon傳感器基站和/或超寬帶傳感器基站和/或wifi傳感器基站和/或地磁傳感器基站和/或rfid基站，還包括設(shè)置于待監(jiān)測(cè)載體上的加速度計(jì)、陀螺儀，還包括與所述加速度計(jì)、陀螺儀、基站連接的信號(hào)處理單元，所述基站為超寬帶傳感器基站和/或ibeacon傳感器基站和/或wifi傳感器基站和/或地磁傳感器基站和/或rfid基站。

本發(fā)明具有的優(yōu)點(diǎn)和積極效果是：本發(fā)明建立了一個(gè)基于粒子濾波為核心的結(jié)合慣性定位結(jié)果與其它基站的數(shù)據(jù)相融合的柔性定位框架，使得多種基站的數(shù)據(jù)能夠有效的與慣性數(shù)據(jù)相融合提高定位的精確度和可靠性。本發(fā)明作為一種柔性的方法框架可以根據(jù)實(shí)際需求，整合特定的慣性定位裝置和基站。本發(fā)明具有適用性強(qiáng)，通用性好的優(yōu)點(diǎn)，可以很大提高定位的長(zhǎng)短時(shí)精度與可靠性。本發(fā)明可以柔性的利用不同的基站的信息，因此在基站的布置上可以采用稀疏布置的方法，根據(jù)實(shí)際中的成本要求，不要求在同一區(qū)域中布置兩種類型或更多類型的基站，即各個(gè)基站的作用范圍互相獨(dú)立，而且可以根據(jù)實(shí)際需求可布置成本較為低廉的基站，這樣可以極大的降低布置的硬件成本。

附圖說明

圖1為本發(fā)明的基于慣導(dǎo)的室內(nèi)定位方法的處理流程示意圖；

圖2為本發(fā)明的基于慣導(dǎo)的室內(nèi)定位裝置的結(jié)構(gòu)示意圖；

圖3為本發(fā)明的ibeacon預(yù)測(cè)作用距離示意圖；

圖4為本發(fā)明的ubw視距條件下測(cè)距精度可靠性示意圖；

圖5為本發(fā)明的空間內(nèi)多個(gè)wifi傳感器基站信號(hào)強(qiáng)度在空間分布示意圖；

圖6(a)、(b)、(c)為本發(fā)明的實(shí)際行走軌跡、慣性定位的軌跡、融合慣性定位與超寬帶傳感器基站定位的軌跡的結(jié)果對(duì)比圖，圖中的矩形塊的標(biāo)注代表超寬帶傳感器基站；

圖6(a)為本發(fā)明的實(shí)際運(yùn)動(dòng)軌跡指示圖；

圖6(b)為本發(fā)明的純慣導(dǎo)計(jì)算軌跡；

圖6(c)為本發(fā)明的融合慣性定位與超寬帶傳感器基站的定位結(jié)果圖；

圖7(a)、(b)分別為本發(fā)明的慣性定位的軌跡、融合慣性定位與ibeacon傳感器基站定位的軌跡的結(jié)果，圖中的矩形塊、三角形塊的標(biāo)注分別代表超寬帶傳感器基站、wifi傳感器基站；

圖7(a)為本發(fā)明的慣性定位的軌跡的結(jié)果圖；

圖7(b)為本發(fā)明的融合慣性定位與ibeacon傳感器基站定位的定位軌跡的結(jié)果圖；

圖8(a)、(b)分別為本發(fā)明的慣性定位的軌跡與融合慣性定位、超寬帶傳感器基站與ibeacon傳感器基站定位的軌跡的結(jié)果圖。

圖8(a)為本發(fā)明的慣性定位的結(jié)果圖。

圖8(b)為本發(fā)明的融合慣性定位、超寬帶傳感器基站定位、ibeacon傳感器基站定位的結(jié)果對(duì)比圖。

上述附圖中，11、加速度計(jì)，12、陀螺儀，13、基站信號(hào)標(biāo)簽，2、基站，3、信號(hào)處理單元，4、待監(jiān)測(cè)載體。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明作更詳細(xì)地解釋。圖1所示基于粒子濾波為核心的結(jié)合慣性定位結(jié)果與其它基站數(shù)據(jù)相融合的柔性定位框架。

如圖1、圖2、圖3、圖4、圖5、圖6(a)-(c)、圖7(a)-(b)、圖8(a)-(b)所示，本發(fā)明是一種慣性定位與基站(例如多源輔助傳感器基站)測(cè)量數(shù)據(jù)融合的定位方法，其對(duì)慣性定位裝置的型式無(wú)特定要求，對(duì)所需要的基站種類與數(shù)目不做特定要求。專利提出的方法作為一種柔性的方法框架可以根據(jù)實(shí)際需求，整合特定的慣性定位轉(zhuǎn)置和基站。本發(fā)明所提的方法具有適用性強(qiáng)，通用性好的優(yōu)點(diǎn)，可以很大提高定位的長(zhǎng)短時(shí)精度與可靠性。基站可采用輔助傳感器基站或其他類型的基站。

本發(fā)明的方法主要包括：慣性航跡數(shù)據(jù)獲取；粒子傳播模型建立；基站數(shù)據(jù)獲取及粒子權(quán)值更新；粒子濾波粒子重采樣獲取估計(jì)位置。

如圖2所示，本發(fā)明還提供一種基于慣導(dǎo)的室內(nèi)定位裝置，待監(jiān)測(cè)載體上設(shè)置有加速度計(jì)11、陀螺儀12，待監(jiān)測(cè)載體4行進(jìn)區(qū)域內(nèi)固定設(shè)置有至少一個(gè)基站2，待監(jiān)測(cè)載體4上設(shè)置有基站信號(hào)標(biāo)簽13，若基站2采用輔助傳感器基站，則基站信號(hào)標(biāo)簽13為輔助傳感器標(biāo)簽，輔助傳感器標(biāo)簽為超寬帶(uwb)傳感器標(biāo)簽或ibeacon傳感器標(biāo)簽或wifi傳感器標(biāo)簽。所述基站為ibeacon傳感器基站和/或超寬帶傳感器基站和/或wifi傳感器基站和/或地磁傳感器基站和/或rfid基站。

本發(fā)明中，待監(jiān)測(cè)載體4行進(jìn)區(qū)域內(nèi)固定設(shè)置有基站2，即只要在待監(jiān)測(cè)載體4行進(jìn)過程中可以經(jīng)過接收到的基站2信號(hào)強(qiáng)度高的區(qū)域，即可實(shí)現(xiàn)對(duì)待檢測(cè)載體4的位置的修正。在實(shí)際應(yīng)用過程中，待監(jiān)測(cè)載體4并非必須每個(gè)時(shí)刻都在接收到的輔助傳感器2的信號(hào)強(qiáng)度高的區(qū)域活動(dòng)，只要待監(jiān)測(cè)載體4在運(yùn)動(dòng)過程中偶爾到達(dá)可接收到輔助傳感器2的信號(hào)強(qiáng)度高的區(qū)域，就能達(dá)到修正慣性定位結(jié)果的作用。

基站2與基站信號(hào)標(biāo)簽13通過無(wú)線方式連接?；拘盘?hào)標(biāo)簽13檢測(cè)到基站2的信號(hào)。檢測(cè)到的基站2的信號(hào)為基站2與待監(jiān)測(cè)載體4之間的信號(hào)強(qiáng)度srss或相對(duì)距離d或相對(duì)位置(xwifi,ywifi)，還包括與所述加速度計(jì)11、陀螺儀12、基站2連接的信號(hào)處理單元3，所述信號(hào)處理單元3用于：以固定周期采集時(shí)刻待監(jiān)測(cè)載體4在載體坐標(biāo)系下的陀螺儀測(cè)量數(shù)據(jù)、加速度計(jì)測(cè)量數(shù)據(jù)；利用陀螺儀測(cè)量數(shù)據(jù)、加速度計(jì)測(cè)量數(shù)據(jù)計(jì)算待監(jiān)測(cè)載體4在導(dǎo)航坐標(biāo)系下的位置；利用待監(jiān)測(cè)載體4在導(dǎo)航坐標(biāo)系下的位置(xk,yk)計(jì)算粒子傳播距離lk、粒子傳播方向θk、粒子傳播方向變化量δθk；計(jì)算粒子傳播距離噪聲、粒子傳播方向噪聲；計(jì)算粒子位置集合；根據(jù)檢測(cè)到的基站2的信號(hào)計(jì)算粒子權(quán)值；根據(jù)粒子權(quán)值對(duì)所有粒子進(jìn)行重采樣得到新的粒子位置集合，并對(duì)粒子位置求均值，得到待監(jiān)測(cè)載體4的位置估計(jì)。信號(hào)處理單元3可采用數(shù)字信號(hào)處理芯片，例如dsp、單片機(jī)、arm等。

本發(fā)明的基本思路是：首先，利用慣性定位裝置獲取慣性定位信息，利用固定在待監(jiān)測(cè)載體上的慣性定位裝置獲取載體在導(dǎo)航坐標(biāo)系下的實(shí)時(shí)位置輸出。通過捷聯(lián)慣性導(dǎo)航算法或者航跡推算算法獲得待監(jiān)測(cè)載體的慣性定位結(jié)果。慣性定位的信息應(yīng)用于柔性框架的核心粒子濾波的粒子傳播模型的建立；然后利用獲取的基站的測(cè)量數(shù)據(jù)對(duì)粒子濾波的粒子權(quán)值進(jìn)行更新，實(shí)現(xiàn)數(shù)據(jù)融合；最后對(duì)粒子濾波的粒子進(jìn)行重采樣操作，得到融合當(dāng)前數(shù)據(jù)下的優(yōu)化估計(jì)定位結(jié)果。本發(fā)明中，利用固定在待監(jiān)測(cè)載體上的慣性定位裝置獲取載體在導(dǎo)航坐標(biāo)系下的實(shí)時(shí)位置輸出。通過捷聯(lián)慣性導(dǎo)航算法或者航跡推算算法獲得待監(jiān)測(cè)載體的慣性定位結(jié)果。

基站2可采用輔助傳感器構(gòu)成的基站，也可采用其他類型的非輔助傳感器構(gòu)成的基站。在本專利中，可僅布置一種類型的基站，如ibeacon傳感器基站或超寬帶傳感器基站或wifi傳感器基站或地磁傳感器基站或rfid基站，也可同時(shí)布置上述基站類型中的兩種類型或三種類型的基站，例如同時(shí)布置超寬帶傳感器基站和ibeacon傳感器基站，或同時(shí)布置超寬帶傳感器基站和wifi傳感器基站，或同時(shí)布置ibeacon傳感器基站和wifi傳感器基站，或同時(shí)布置超寬帶傳感器基站、ibeacon傳感器基站、wifi傳感器基站。實(shí)際應(yīng)用中，測(cè)量相對(duì)信號(hào)強(qiáng)度或相對(duì)距離或相對(duì)位置的傳感器都可以用作本發(fā)明中的基站。

布置基站的數(shù)量多少根據(jù)實(shí)際應(yīng)用決定。例如，根據(jù)成本或?qū)嶋H定位過程所涉及的面積決定，面積越大，可布置越多的基站2，即先確定人員的行進(jìn)區(qū)域范圍，根據(jù)人員的行進(jìn)區(qū)域范圍、成本確定布置基站2的數(shù)量。在待監(jiān)測(cè)載體4的行進(jìn)區(qū)域范圍內(nèi)，可以通過基站2的信號(hào)及時(shí)對(duì)待檢測(cè)載體4的定位結(jié)果進(jìn)行修正，使得定位結(jié)果更為準(zhǔn)確。

布置多個(gè)基站時(shí)，優(yōu)選將各個(gè)基站進(jìn)行稀疏布置，即所布置的各個(gè)基站的作用范圍不重合，即在同一時(shí)刻，僅利用一個(gè)輔助傳感器對(duì)粒子權(quán)值進(jìn)行計(jì)算。本發(fā)明的優(yōu)勢(shì)就是可以柔性的利用不同的輔助傳感器信息，因此在傳感器的布置上可以采用稀疏布置的方法，不要求在同一區(qū)域中布置兩種及其以上的傳感器，這樣可以極大的降低布置的硬件成本。本發(fā)明中，并不是需要每個(gè)時(shí)刻都通過外界的基站2修正定位結(jié)果，外界的基站2只是用來(lái)修正慣導(dǎo)的位置數(shù)據(jù)的，在沒有外界基站的信息時(shí)，依靠慣性定位確定待監(jiān)測(cè)載體的位置。由于基站數(shù)據(jù)的可信定位精度是會(huì)隨著距離的由近到遠(yuǎn)而衰減惡化的，所以有時(shí)候收到基站的數(shù)據(jù)并不代表這個(gè)數(shù)據(jù)是可用的。僅在待監(jiān)測(cè)載體進(jìn)入到基站的作用范圍時(shí)，才進(jìn)行修正，否則，仍采用慣性測(cè)量的數(shù)據(jù)。

各個(gè)基站都有一定的作用范圍，由于基站的作用范圍有限，僅布置基站，需要的基站的數(shù)量較多，成本較高，而僅布置慣性器件，又容易造成誤差累積。本發(fā)明中，將慣性器件與基站結(jié)合起來(lái)進(jìn)行使用，既降低了應(yīng)用的成本，又通過布設(shè)基站對(duì)累積誤差進(jìn)行了修正，保證了定位精度。距離基站2越近的位置，對(duì)待監(jiān)測(cè)載體4的位置估計(jì)越準(zhǔn)確，若待監(jiān)測(cè)載體與基站之間的距離超過一定值，也可能來(lái)自收到基站的信號(hào)，但由于相對(duì)距離較遠(yuǎn)，可能出現(xiàn)建筑物的阻擋或其他情況，有可能影響信號(hào)的準(zhǔn)確性，從而影響待檢測(cè)載體的位置的估計(jì)。因此，本發(fā)明中，僅當(dāng)接收到來(lái)自基站的信號(hào)滿足一定條件時(shí)，才利用基站2的信號(hào)對(duì)待監(jiān)測(cè)載體的位置進(jìn)行估計(jì)。

若未接收到來(lái)自基站的信號(hào)，或來(lái)自各個(gè)基站的接收信號(hào)均不可用，則仍跳轉(zhuǎn)到步驟(1)，利用慣性定位確定待監(jiān)測(cè)載體4的位置。

本發(fā)明中，慣性定位裝置的安裝與載體的類型不做特定要求。

在各個(gè)實(shí)施例中，給出載體位置獲取的三種典型類型加以說明：足部航跡推算(foot-ins)、腰部航跡推算(pdr-ins)、輪式機(jī)器人航跡推算(agv-ins)，其中前兩種適用在人，后一種適用在輪式機(jī)器人。

在實(shí)施例四至六中，將檢測(cè)到的基站2的信號(hào)給出三種典型類型加以說明：ibeacon，uwb，wifi。

實(shí)施例一

待監(jiān)測(cè)載體為人員，利用人員足部航跡推算(foot-ins)計(jì)算待監(jiān)測(cè)載體的位置；此時(shí)k為采樣時(shí)刻。優(yōu)選在人員足部設(shè)置加速度計(jì)、陀螺儀。

本實(shí)施例中，基于慣導(dǎo)的室內(nèi)定位方法的具體步驟如下：

(1)待監(jiān)測(cè)載體行進(jìn)中，以固定周期采集待監(jiān)測(cè)載體4在載體坐標(biāo)系下的陀螺儀測(cè)量數(shù)據(jù)載體坐標(biāo)系下的加速度計(jì)測(cè)量數(shù)據(jù)優(yōu)選為足部的加速度計(jì)、陀螺儀測(cè)量數(shù)據(jù)；其中，上標(biāo)b或下標(biāo)b表示數(shù)據(jù)是載體坐標(biāo)系下的數(shù)據(jù)；

(2)利用陀螺儀測(cè)量數(shù)據(jù)加速度計(jì)測(cè)量數(shù)據(jù)計(jì)算待監(jiān)測(cè)載體4在導(dǎo)航坐標(biāo)系下的位置(xk,yk)；若待監(jiān)測(cè)載體4為人員，則k為采樣時(shí)刻或人員行進(jìn)的步數(shù)；若待監(jiān)測(cè)載體4為非人員，則k為采樣時(shí)刻；若k為采樣時(shí)刻，則k＝t，則加速度計(jì)測(cè)量數(shù)據(jù)利用捷聯(lián)慣性導(dǎo)航算法獲得人的位置的具體步驟如下：

(2.1)利用陀螺儀測(cè)量數(shù)據(jù)計(jì)算姿態(tài)變換矩陣

由加速度計(jì)測(cè)量數(shù)據(jù)、陀螺儀測(cè)量數(shù)據(jù)解算得到姿態(tài)角，姿態(tài)角包括橫滾角、俯仰角、航向角；利用姿態(tài)角計(jì)算足部的初始姿態(tài)變換矩陣；利用如下的姿態(tài)變換矩陣微分方程計(jì)算姿態(tài)變換矩陣

其中，上標(biāo)nv表示數(shù)據(jù)是在導(dǎo)航坐標(biāo)系下的數(shù)據(jù)，為陀螺數(shù)據(jù)的反對(duì)稱矩陣，其表達(dá)式為

(2.2)利用姿態(tài)變換矩陣和加速度計(jì)測(cè)量數(shù)據(jù)計(jì)算導(dǎo)航坐標(biāo)系下的加速度計(jì)測(cè)量數(shù)據(jù)：

利用下式通過姿態(tài)矩陣將加速度投影到導(dǎo)航坐標(biāo)系下，得到導(dǎo)航坐標(biāo)系下的加速度計(jì)測(cè)量數(shù)據(jù)

(2.3)將導(dǎo)航坐標(biāo)系下的加速度計(jì)測(cè)量數(shù)據(jù)進(jìn)行積分運(yùn)算得到待監(jiān)測(cè)載體4在導(dǎo)航坐標(biāo)系下載體的原始計(jì)算位置：

對(duì)加速度分別進(jìn)行一次和二次積分分別得到速度{vx,k,vy,k}和待監(jiān)測(cè)載體4在導(dǎo)航坐標(biāo)系下的原始計(jì)算位置(x'k,y'k)，具體公式如下

其中，當(dāng)δt_sample為k時(shí)刻和k-1時(shí)刻之間的采樣時(shí)間間隔，下標(biāo)k表示不同時(shí)刻。δt為δt_sample的整數(shù)倍。

當(dāng)載體有零速狀態(tài)時(shí)，利用零速修正技術(shù)對(duì)步驟(2.3)得到的(x'k,y'k)進(jìn)行修正得到待監(jiān)測(cè)載體4在導(dǎo)航坐標(biāo)系下的位置(xk,yk)。

利用零速修正技術(shù)對(duì)原始計(jì)算位置(x'k,y'k)進(jìn)行修正得到foot-ins慣性航跡數(shù)據(jù)，即得到待監(jiān)測(cè)載體4在導(dǎo)航坐標(biāo)系下的位置(xk,yk)。零速修正技術(shù)是利用載體停車時(shí)慣性導(dǎo)航系統(tǒng)的速度輸出作為系統(tǒng)速度誤差的觀測(cè)量，進(jìn)而對(duì)其他各項(xiàng)誤差進(jìn)行修正。利用現(xiàn)有的零速修正技術(shù)即可進(jìn)行計(jì)算。

(3)若檢測(cè)到基站2的信號(hào)，則跳轉(zhuǎn)到步驟(4)，否則，跳轉(zhuǎn)到步驟(1)；

實(shí)際使用過程中，基站2不斷掃描周圍環(huán)境的基站信號(hào)標(biāo)簽13。當(dāng)待監(jiān)測(cè)載體4未進(jìn)入到基站的作用范圍時(shí)，即設(shè)置于待監(jiān)測(cè)載體4上的基站信號(hào)標(biāo)簽13接收到來(lái)自待監(jiān)測(cè)載體4的第一信號(hào)未滿足相應(yīng)條件時(shí)，待監(jiān)測(cè)載體4的位置依然通過設(shè)置于待監(jiān)測(cè)載體上的加速度計(jì)11、陀螺儀12計(jì)算。當(dāng)待監(jiān)測(cè)載體4上的基站信號(hào)標(biāo)簽13接收到來(lái)自待監(jiān)測(cè)載體4的信號(hào)滿足相應(yīng)條件時(shí)，利用基站信號(hào)標(biāo)簽13收到的來(lái)自于輔助傳感器4的信號(hào)對(duì)待監(jiān)測(cè)載體4的位置進(jìn)行修正。

(4)利用待監(jiān)測(cè)載體4在導(dǎo)航坐標(biāo)系下的位置(xk,yk)計(jì)算粒子傳播距離lk、粒子傳播方向θk、粒子傳播方向變化量δθk：

若計(jì)算粒子傳播模型，則需構(gòu)建粒子傳播模型所需的粒子傳播距離lk、粒子傳播方向θk、粒子傳播方向變化量δθk。當(dāng)k代表幀數(shù)時(shí)，在滿足設(shè)定頻率(幾hz到幾百hz)下輸出慣性航跡數(shù)據(jù){xk,yk}，輸出的最大頻率要比慣性定位裝置內(nèi)部計(jì)算頻率要小。根據(jù)當(dāng)前幀k的位置與前一幀k-1的位置得到相對(duì)變化長(zhǎng)度lk與相對(duì)轉(zhuǎn)向角δθk。因此這里統(tǒng)一將lk定義為粒子濾波傳播的距離，將δθk定義為粒子濾波傳播方向變化量，當(dāng)前粒子集合的傳播過程，其中表示粒子集合中所有粒子的位置，n代表粒子集合中粒子總數(shù)，為粒子傳播的距離噪聲和方向噪聲。

根據(jù)如上所述慣性定位裝置輸出的航跡信息{xk,yk}建立粒子傳播模型，選取任意相鄰兩個(gè)時(shí)刻或相鄰兩個(gè)步子的位置信息{xk,yk}與{xk-1,yk-1}，計(jì)算得到粒子濾波傳播的距離lk與粒子濾波傳播的方向δθk作為粒子濾波狀態(tài)輸入，計(jì)算方法如下

其中θk∈[-π,π]；

(4)利用待監(jiān)測(cè)載體4在導(dǎo)航坐標(biāo)系下的位置(xk,yk)計(jì)算粒子傳播距離lk、粒子傳播方向θk、粒子傳播方向變化量δθk，其中θk也可采用θk＝atan2(δyk,δxk)進(jìn)行計(jì)算，即θk∈[-π,π]，將生成的角度θk控制在[-π,π]的范圍內(nèi)；

(5)計(jì)算粒子傳播距離噪聲粒子傳播方向噪聲利用下式進(jìn)行迭代運(yùn)算，計(jì)算粒子位置集合

其中i為粒子個(gè)數(shù)，i＝1,2,…,n，為陀螺誤差量，為陀螺誤差的噪聲，通過有色噪聲或白噪聲計(jì)算得到，δt為粒子更新時(shí)間間隔，δt由獲取的基站2的信號(hào)的更新率確定。本發(fā)明中，的取值范圍為0.001-0.05度/每秒。

考慮到粒子傳播距離lk與傳播方向δθk由慣性定位裝置獲取，其存在誤差，這里假設(shè)lk和δθk的誤差為高斯噪聲。對(duì)整個(gè)粒子集合而言，粒子傳播距離噪聲為粒子傳播方向噪聲為由概率分布p(lr,k|lk)經(jīng)過抽樣得到，由概率分布p(δθr,k|δθk)經(jīng)過抽樣得到

其中，

其中，n為粒子集合中包含的粒子數(shù)目，(σl)²與(σδθ)²表示傳播距離與傳播方向的噪聲方差，噪聲方差的取值分別與傳播距離與傳播方向成正比關(guān)系，比例系數(shù)為c與d。c取值范圍為[0.01,0.02]，d取值范圍為[0.02,0.06]。

利用噪聲粒子與k-1時(shí)刻第i個(gè)粒子的位置經(jīng)計(jì)算后可得到新的粒子集粒子集的傳播過程包含如下迭代步驟

其中，δθk數(shù)值是將陀螺z軸輸出進(jìn)行積分運(yùn)算的結(jié)果，且需要考慮陀螺的誤差后，即

(6)根據(jù)檢測(cè)到的基站2的信號(hào)計(jì)算粒子權(quán)值

各個(gè)粒子權(quán)值的初始值相等，即若存在n個(gè)粒子，那么每個(gè)粒子對(duì)應(yīng)的權(quán)值為1/n。通過粒子傳播距離、粒子傳播方向可以更新粒子的位置狀態(tài)，而粒子權(quán)值的更新是需要通過檢測(cè)到的基站2的信號(hào)進(jìn)行計(jì)算。

所檢測(cè)的基站2的信號(hào)為基站與待監(jiān)測(cè)載體之間的信號(hào)強(qiáng)度srss和/或相對(duì)距離d和/或相對(duì)位置(xwifi,ywifi)，檢測(cè)到的基站2的信號(hào)可表示為z*(*＝r,d,p)；不同類型的輔助測(cè)量傳感器2獲得傳感器相對(duì)載體的信號(hào)強(qiáng)度zr(rssi)信息，或相對(duì)距離信息zd，或相對(duì)位置信息zp。對(duì)于基站2的布設(shè)以及選擇的傳感器類型不做特殊要求。檢測(cè)到的基站2的信號(hào)用于更新粒子濾波粒子的權(quán)重。

僅在待監(jiān)測(cè)載體4進(jìn)入到基站2的作用范圍時(shí)才對(duì)位置進(jìn)行修正，否則，仍根據(jù)慣性測(cè)量方式確定待監(jiān)測(cè)載體4在導(dǎo)航坐標(biāo)系下的位置。

根據(jù)輔助傳感器測(cè)量結(jié)果z*(*＝r,d,p)對(duì)粒子濾波的粒子權(quán)值進(jìn)行更新。更新的方式為：

在實(shí)施例四至六中，將獲取的檢測(cè)到的基站2的信號(hào)給出三種典型類型加以說明：ibeacon，uwb，wifi。

(7)待監(jiān)測(cè)載體4的位置估計(jì)

根據(jù)得到的權(quán)值按照剩余重采樣(residualresampling)算法對(duì)所有粒子進(jìn)行重采樣得到新的粒子位置并且對(duì)整個(gè)粒子集合中的粒子求均值，得到數(shù)據(jù)融合后的載體的位置估計(jì)。

由基站2的信號(hào)z*(*＝r,d,p)對(duì)粒子的權(quán)值進(jìn)行更新后。對(duì)粒子進(jìn)行重采樣操作并計(jì)算融合后的定位結(jié)果。

(7.1)利用下式對(duì)當(dāng)前粒子集合中的所有粒子的權(quán)值進(jìn)行歸一化處理：

(7.2)根據(jù)步驟(7.1)中得到的權(quán)值按照剩余重采樣算法對(duì)所有粒子進(jìn)行重采樣，得到更新后的位置粒子集合本發(fā)明的重采樣方法利用comparisonofresamplingschemesforparticlefiltering(作者rdouc，ocappe，出版源computerscience,2005:64-69)中的方法。確定粒子權(quán)值后，采用剩余重采樣法去掉權(quán)值低的粒子，復(fù)制權(quán)值高的粒子，再對(duì)粒子的位置取均值，即根據(jù)粒子的權(quán)值決定是否舍棄該粒子或采用該粒子。這時(shí)的粒子集合就代表了對(duì)位置狀態(tài)信息的概率分布；

(7.3)對(duì)步驟(7.2)中得到的重采樣后的粒子位置數(shù)據(jù)取均值，得到融合后的位置估計(jì)

實(shí)施例二

待監(jiān)測(cè)載體4為人員，利用腰部航跡推算(pdr-ins)計(jì)算待監(jiān)測(cè)載體的位置；此時(shí)k為人員行進(jìn)中的步數(shù)。本實(shí)施例與其他實(shí)施例相同的步驟可采用其他實(shí)施例中的步驟中的內(nèi)容。本實(shí)施例二中，步驟(2)中，pdr-ins利用航跡推算模型計(jì)算載體的運(yùn)動(dòng)位置變化。加速度計(jì)、陀螺儀優(yōu)選安裝于人員腰部，獲取的慣性測(cè)量數(shù)據(jù)為人員腰部的陀螺儀測(cè)量數(shù)據(jù)和加速度計(jì)測(cè)量數(shù)據(jù)

本實(shí)施例中，步驟(2)中的具體分步驟如下：

(2.1)利用下式計(jì)算人員行進(jìn)中第k步的步長(zhǎng)lk

其中，為人員行進(jìn)中第k步的步頻，τ1、τ2分別為第一相關(guān)系數(shù)、第二相關(guān)系數(shù)；

本發(fā)明中，利用加速度計(jì)測(cè)量數(shù)據(jù)計(jì)算人員的步頻通過對(duì)加速度計(jì)測(cè)量數(shù)據(jù)執(zhí)行過零檢測(cè)操作，實(shí)現(xiàn)運(yùn)動(dòng)中的計(jì)步功能獲取步頻根據(jù)加速度計(jì)測(cè)量數(shù)據(jù)計(jì)算靜止的時(shí)刻和運(yùn)動(dòng)的時(shí)刻，根據(jù)靜止時(shí)刻和運(yùn)動(dòng)時(shí)刻的采樣點(diǎn)獲取人員行進(jìn)中第k步的步頻過零檢測(cè)是指：在運(yùn)動(dòng)過程中，人員完成一個(gè)完整步子的過程中，加速度計(jì)對(duì)腰部的運(yùn)動(dòng)的測(cè)量得到的加速度計(jì)測(cè)量數(shù)據(jù)是一個(gè)從正值到負(fù)值的數(shù)據(jù)變化，因此通過判斷加速度計(jì)測(cè)量數(shù)據(jù)由正值到負(fù)值的變化，即通過過零檢測(cè)即可實(shí)現(xiàn)計(jì)步功能。

采用典型的步頻——步長(zhǎng)模型獲得步長(zhǎng)其中τ1和τ2分別為第一相關(guān)系數(shù)、第二相關(guān)系數(shù)，τ1、τ2均通過實(shí)際試驗(yàn)中確定，這里給出實(shí)驗(yàn)中使用的值τ1＝20，τ2＝0.54。為人員行進(jìn)中第k步的步頻。根據(jù)停、走、停判斷方法，計(jì)算第k-1步、第k步對(duì)應(yīng)的靜止時(shí)刻之差的倒數(shù)即為第k步的步頻。

(2.2)利用下式計(jì)算待監(jiān)測(cè)載體(4)在導(dǎo)航坐標(biāo)系下的位置(xk,yk)

其中，φk為人員行進(jìn)中第k-1步到第k步的步態(tài)方向變化角。

實(shí)施例三

待監(jiān)測(cè)載體4為輪式機(jī)器人，利用輪式機(jī)器人航跡推算(agv-ins)計(jì)算待監(jiān)測(cè)載體4的位置。本實(shí)施例中，k為采樣時(shí)刻。本實(shí)施例與其他實(shí)施例相同的步驟可采用其他實(shí)施例中的步驟中的內(nèi)容。本實(shí)施例中，步驟(2)中，agv-ins利用航跡推算算法獲得輪式機(jī)器人的位置變化。

本實(shí)施例中，步驟(2)中，利用下式計(jì)算輪式機(jī)器人在導(dǎo)航坐標(biāo)系下的位置(xk,yk)

其中，lk為k-1時(shí)刻到k時(shí)刻輪式機(jī)器人的水平位移增量，為k時(shí)刻的輪式方向角，k為固定采樣頻率下采樣點(diǎn)。(xk,yk)即為agv-ins慣性航跡數(shù)據(jù)。

其中，采集k-1時(shí)刻到k時(shí)刻輪式機(jī)器人車輪編碼器的脈沖數(shù)nk，依據(jù)下式計(jì)算k-1時(shí)刻到k時(shí)刻輪式機(jī)器人的水平位移增量lk

其中，rk為輪子的半徑；ntol為輪式機(jī)器人車輪的輪子轉(zhuǎn)一圈總脈沖數(shù)。利用陀螺儀測(cè)量數(shù)據(jù)計(jì)算k時(shí)刻的輪式方向角即對(duì)陀螺儀測(cè)量數(shù)據(jù)進(jìn)行積分得到輪式方向角

輪式機(jī)器人的位置變化推移，可以分解為兩部分：方向變化和距離的變化。方向變化可以利用慣性測(cè)量數(shù)據(jù)給出，也可以利用車輪之間的差速給出。利用車輪之間的差速計(jì)算方向變化是現(xiàn)成的技術(shù)方案，查找資料可以獲取。距離變化可以利用在輪式機(jī)器人上設(shè)置的里程計(jì)獲取，這里的獲取方法根據(jù)所使用的輪式機(jī)器人形式不同有不同的計(jì)算方法，所使用的方法在相關(guān)輪式機(jī)器人的資料中都有明確的說明。實(shí)際應(yīng)用中，兩輪機(jī)器人、基于四輪平臺(tái)的輪式機(jī)器人均可應(yīng)用于本申請(qǐng)。

實(shí)施例四

本實(shí)施例中，待監(jiān)測(cè)載體4可為人員或輪式機(jī)器人。

本實(shí)施例與其他實(shí)施例相同的步驟可采用其他實(shí)施例中的步驟中的內(nèi)容。本實(shí)施例的步驟(2)可采用實(shí)施例一、二、三中任一種方法。本實(shí)施例四中，基站為ibeacon傳感器基站，在待檢測(cè)載體4行進(jìn)區(qū)域內(nèi)固定設(shè)置ibeacon傳感器基站，即設(shè)置ibeacon傳感器基站，待監(jiān)測(cè)載體上設(shè)置有ibeacon標(biāo)簽。檢測(cè)到的基站2的信號(hào)為基站2與待監(jiān)測(cè)載體之間的信號(hào)強(qiáng)度srss，k，利用信號(hào)強(qiáng)度srss計(jì)算粒子權(quán)值超寬帶標(biāo)簽接收到的信號(hào)為ibeacon傳感器基站的信號(hào)強(qiáng)度srss,k。設(shè)置于人員腰部的藍(lán)牙標(biāo)簽根據(jù)接收到來(lái)自于ibeacon傳感器基站的信號(hào)強(qiáng)度rssi計(jì)算出距離。

ibeacon傳感器是使用藍(lán)牙低功耗標(biāo)準(zhǔn)的廣播信號(hào)，其確定范圍是一個(gè)近似和定性方法。本發(fā)明使用ibeacon傳感器作為輔助傳感器標(biāo)簽獲取一個(gè)特定的范圍內(nèi)的指示。人員攜帶的具有藍(lán)牙功能的工具(如手機(jī)等)能夠從ibeacon傳感器獲取rssi(接收信號(hào)強(qiáng)度)信息。區(qū)域范圍信號(hào)強(qiáng)度與相對(duì)距離之間的關(guān)系基本符合圖3所示，從圖中可以看出ibeacon可以提供:立即，遠(yuǎn)，近三種信息,其中立即表示范圍小于0.5米,靠近表示介于0.5米和2.5米左右。srss,k為k時(shí)刻接收的信號(hào)強(qiáng)度，sthres為信號(hào)強(qiáng)度門限，若srss,k＞sthres，則按下式對(duì)粒子權(quán)值進(jìn)行更新

其中(xibeacon,yibeacon)為ibeacon傳感器基站的位置坐標(biāo)，dthres為距離門限。如果srss,k小于門限sthres，則忽略本次測(cè)量信息。僅當(dāng)待監(jiān)測(cè)載體進(jìn)入到ibeacon傳感器基站的作用范圍時(shí)，即srss,k＞sthres時(shí)，才進(jìn)行修正，否則，仍采用慣性測(cè)量的數(shù)據(jù)。本實(shí)施例中，步驟(6)中，檢測(cè)到的信號(hào)為ibeacon傳感器基站的信號(hào)強(qiáng)度srss,k，若srss,k＞sthres，則利用下式計(jì)算粒子權(quán)值

其中，(xibeacon,yibeacon)為ibeacon傳感器基站的位置坐標(biāo)，sthres為信號(hào)強(qiáng)度門限，dthres為距離門限。其中信號(hào)強(qiáng)度門限sthres、距離門限dthres均通過實(shí)際試驗(yàn)或在實(shí)際使用環(huán)境中確定；

若srss的值不滿足式子srss,k＞sthres，則不采用ibeacon傳感器基站的信號(hào)更新粒子權(quán)值，則檢測(cè)是否接收到其他基站(例如輔助傳感器基站)的信號(hào)，若未檢測(cè)到任何基站(例如輔助傳感器基站)的信號(hào)，則回到步驟(1)，即仍采用慣性定位的結(jié)果確定待監(jiān)測(cè)載體4的位置。

iibeacon傳感器基站不斷的廣播自身所在位置，設(shè)置于人員腰部的藍(lán)牙標(biāo)簽根據(jù)接收到來(lái)自于ibeacon傳感器基站的信號(hào)強(qiáng)度rssi計(jì)算出相對(duì)距離。ibeacon傳感器標(biāo)簽可采用ti公司的cc2541、cc2540或nordic的nrf51822。

實(shí)施例五

本實(shí)施例中，待監(jiān)測(cè)載體4可為人員或輪式機(jī)器人。

本實(shí)施例與其他實(shí)施例相同的步驟可采用其他實(shí)施例中的步驟中的內(nèi)容。本實(shí)施例的步驟(2)可采用實(shí)施例一、二、三中任一種方法。本實(shí)施例中，基站2為超寬帶(uwb)傳感器基站，在待檢測(cè)載體4行進(jìn)區(qū)域內(nèi)固定設(shè)置超寬帶傳感器基站2，待監(jiān)測(cè)載體4上設(shè)置有超寬帶傳感器標(biāo)簽，即uwb標(biāo)簽。超寬帶傳感器標(biāo)簽接收到的信號(hào)為超寬帶傳感器基站2的信噪比參數(shù)ζk，獲取接收到的基站2的信噪比參數(shù)ζk是超寬帶的一個(gè)信噪比指標(biāo)，信噪比參數(shù)ζk可以由超寬帶基站發(fā)送，反映了當(dāng)前uwb測(cè)距的結(jié)果可信度。

檢測(cè)到的基站2的信號(hào)為基站與待監(jiān)測(cè)載體之間的相對(duì)距離，利用相對(duì)距離計(jì)算粒子權(quán)值通過超寬帶(uwb)傳感器能夠獲取超寬帶基站與人員身上的超寬帶標(biāo)簽的距離信息di，其測(cè)距獲取的距離信息可靠度基本符合圖4所示的高斯分布。

本實(shí)施例中，檢測(cè)到的信號(hào)為超寬帶傳感器基站(2)的信噪比參數(shù)ζk，若ζk＞ζthres，則利用下式計(jì)算粒子權(quán)值

其中，(xubw,yuwb)為超寬帶傳感器基站的位置坐標(biāo)，di為超寬帶傳感器基站與待監(jiān)測(cè)載體4之間的相對(duì)距離，ζthres為信噪比門限，其中信噪比門限ζthres通過實(shí)際試驗(yàn)或在實(shí)際使用環(huán)境中確定。p()是一個(gè)均值為方差為σ²的高斯分布。方差σ²根據(jù)實(shí)際基站的特性確定，即通過超寬帶傳感器本身確定。方差σ²可采用如下方法通過實(shí)驗(yàn)確定：首先依據(jù)超寬帶傳感器基站最大測(cè)距范圍劃歸距離測(cè)試區(qū)段，依次在近距離、中等距離、遠(yuǎn)距離分別進(jìn)行測(cè)試，對(duì)比實(shí)際距離和uwb測(cè)試距離之差，利用多組實(shí)驗(yàn)統(tǒng)計(jì)得到方差σ²的值。

本實(shí)施例中，利用設(shè)置于人員腰部的超寬帶標(biāo)簽，以tof方式與外部的超寬帶傳感器基站測(cè)距，人員腰部的超寬帶標(biāo)簽可通過信號(hào)的飛行時(shí)間計(jì)算得到人員與基站2的距離。常見的超寬帶標(biāo)簽包括有decawave公司的dw1000，法國(guó)的bespoon的超寬帶模組。

若ζk的值不滿足式子ζk＞ζthres，則不采用超寬帶傳感器基站的信號(hào)更新粒子權(quán)值，則檢測(cè)是否接收到其他基站(例如輔助傳感器基站)的信號(hào)，若未檢測(cè)到任何基站(例如輔助傳感器基站)的信號(hào)，則回到步驟(1)，即仍采用慣性定位的結(jié)果確定待監(jiān)測(cè)載體4的位置。

實(shí)施例六

本實(shí)施例中，待監(jiān)測(cè)載體4可為人員或輪式機(jī)器人。

本實(shí)施例與其他實(shí)施例相同的步驟可采用其他實(shí)施例中的步驟中的內(nèi)容。本實(shí)施例的步驟(2)可采用實(shí)施例一、二、三中任一種方法。本實(shí)施例中，基站2為wifi傳感器基站，在待檢測(cè)載體4行進(jìn)區(qū)域內(nèi)固定設(shè)置wifi傳感器基站，待監(jiān)測(cè)載體4上設(shè)置有wifi傳感器標(biāo)簽，檢測(cè)到的基站2的信號(hào)為基站2與待監(jiān)測(cè)載體4之間的相對(duì)位置(xwifi,ywifi)，利用相對(duì)位置(xwifi,ywifi)計(jì)算粒子權(quán)值所述步驟(3)中，還包括獲取wifi傳感器基站周邊區(qū)域的位置與wifi指紋的關(guān)系，接收到的信號(hào)為wifi指紋。其他可以獲取相對(duì)位置信息的基站也可以應(yīng)用于本專利中。

圖5給出了一個(gè)空間內(nèi)三個(gè)wifi傳感器基站輻射信號(hào)強(qiáng)度圖，圖中是wifi信號(hào)強(qiáng)度的覆蓋的區(qū)域示范圖。圖中，位于同一曲線上的點(diǎn)即為具有相同信號(hào)強(qiáng)度的點(diǎn)。對(duì)于wifi傳感器基站，輻射信號(hào)強(qiáng)度隨著待監(jiān)測(cè)載體4與wifi傳感器基站之間的相對(duì)距離而變化，相對(duì)距離越遠(yuǎn)，則輻射信號(hào)強(qiáng)度越小。在本專利的方法中，依據(jù)本方法實(shí)現(xiàn)時(shí)使用場(chǎng)景區(qū)域的大小，可選擇布置wifi傳感器基站的數(shù)量。在本實(shí)施例中，根據(jù)實(shí)際情況，優(yōu)選布置3個(gè)wifi傳感器基站，并記錄各個(gè)wifi傳感器基站的位置。布置好的3個(gè)wifi傳感器基站的位置在當(dāng)前運(yùn)動(dòng)區(qū)域內(nèi)是需要提前確定好的。

wifi指紋庫(kù)是指wifi的信號(hào)在其所布置空間內(nèi)的一個(gè)分布。wifi指紋庫(kù)即為無(wú)線信號(hào)強(qiáng)度分布圖或信號(hào)強(qiáng)度經(jīng)驗(yàn)值數(shù)據(jù)庫(kù)。實(shí)際應(yīng)用中，先布設(shè)wifi傳感器基站，并確定wifi傳感器基站的位置。在進(jìn)行定位之前，先通過試驗(yàn)得到wifi傳感器基站周圍的信號(hào)強(qiáng)度與距離的對(duì)應(yīng)關(guān)系，即通過對(duì)采集的各個(gè)wifi傳感器基站的wifi信號(hào)指紋進(jìn)行訓(xùn)練，可以得到wifi傳感器基站所在區(qū)域各個(gè)位置和輻射信號(hào)強(qiáng)度之間的非線性關(guān)系的統(tǒng)計(jì)模型，即得到wifi傳感器基站周邊區(qū)域的wifi指紋庫(kù)，根據(jù)該統(tǒng)計(jì)模型，在實(shí)際定位過程中，通過測(cè)量輻射信號(hào)強(qiáng)度，即可得到wifi傳感器基站在其所在區(qū)域內(nèi)的信號(hào)強(qiáng)度的分布。布置好的3個(gè)wifi傳感器基站的位置在當(dāng)前運(yùn)動(dòng)區(qū)域內(nèi)是需要提前確定好的。通過wifi信號(hào)指紋訓(xùn)練得到wifi傳感器基站在其所在區(qū)域內(nèi)的信號(hào)強(qiáng)度的分布。

本實(shí)施例中，檢測(cè)到的信號(hào)為wifi傳感器基站的wifi指紋，利用下式計(jì)算粒子權(quán)值

其中，(xwifi,ywifi)為利用wifi指紋計(jì)算得到的待監(jiān)測(cè)載體的位置，為位置測(cè)量方差，

在獲取wifi指紋時(shí)就已經(jīng)確定了信號(hào)與位置的相關(guān)關(guān)系，在有些區(qū)域即使收到了wifi信號(hào)，但是沒有與之相匹配的指紋信息，那么這樣的信息是不會(huì)被使用的，因此可設(shè)置wifi信號(hào)強(qiáng)度門限，若接收到的wifi指紋大于wifi信號(hào)強(qiáng)度門限，則利用接收到的wifi指紋計(jì)算待監(jiān)測(cè)載體的位置。也可不設(shè)置wifi信號(hào)強(qiáng)度門限，若wifi指紋未包含在所獲取的wifi指紋庫(kù)的范圍內(nèi)，則不采用wifi傳感器基站的信號(hào)更新粒子權(quán)值，則檢測(cè)是否接收到其他基站(例如輔助傳感器基站)的信號(hào)，若未檢測(cè)到任何基站(例如輔助傳感器基站)的信號(hào)，則回到步驟(1)，即仍采用慣性定位的結(jié)果確定待監(jiān)測(cè)載體4的位置。

wifi傳感器標(biāo)簽無(wú)需采用額外的模塊，例如具有wifi功能的手持終端(如三防機(jī)、手機(jī))即可，本專利使用的可集成wifi傳感器標(biāo)簽是esp8266。

實(shí)施例七

本實(shí)施例中，待監(jiān)測(cè)載體4可為人員或輪式機(jī)器人。

本實(shí)施例與其他實(shí)施例相同的步驟可采用其他實(shí)施例中的步驟中的內(nèi)容。本實(shí)施例的步驟(2)可采用實(shí)施例一、二、三中任一種方法。本實(shí)施例中，基站2為rfid基站。

rfid信標(biāo)的作用機(jī)理與ibeacon的作用機(jī)理是相同的，即將實(shí)施例四中各個(gè)公式的ibeacon的數(shù)據(jù)替換為rfid信標(biāo)的數(shù)據(jù)即可，且只是這里rfid信標(biāo)可以提供的位置和信號(hào)強(qiáng)度的范圍更大。

實(shí)施例八

本實(shí)施例中，待監(jiān)測(cè)載體4可為人員或輪式機(jī)器人。

本實(shí)施例與其他實(shí)施例相同的步驟可采用其他實(shí)施例中的步驟中的內(nèi)容。本實(shí)施例的步驟(2)可采用實(shí)施例一、二、三中任一種方法。本實(shí)施例中，基站2為地磁傳感器基站。

地磁傳感器的作用機(jī)理與wifi方案作用機(jī)理相同，地磁使用是通過前期的測(cè)量獲取需要定位的空間內(nèi)，位置與地磁強(qiáng)度之間的相對(duì)數(shù)據(jù)關(guān)系，這里可以把這樣的關(guān)系理解為地磁指紋，地磁指紋的使用和wifi指紋使用是相同的。實(shí)際應(yīng)用中，先布設(shè)地磁傳感器基站，并確定地磁傳感器基站的位置。在進(jìn)行定位之前，先通過試驗(yàn)得到地磁傳感器基站周圍的信號(hào)強(qiáng)度與距離的對(duì)應(yīng)關(guān)系，即通過對(duì)采集的各個(gè)地磁傳感器基站的地磁信號(hào)指紋進(jìn)行訓(xùn)練，可以得到地磁傳感器基站所在區(qū)域各個(gè)位置和輻射信號(hào)強(qiáng)度之間的非線性關(guān)系的統(tǒng)計(jì)模型，即得到地磁傳感器基站周邊區(qū)域的地磁指紋庫(kù)，根據(jù)該統(tǒng)計(jì)模型，在實(shí)際定位過程中，通過測(cè)量輻射信號(hào)強(qiáng)度，即可得到地磁傳感器基站在其所在區(qū)域內(nèi)的信號(hào)強(qiáng)度的分布。利用接收到的地磁指紋計(jì)算粒子權(quán)值，進(jìn)行后續(xù)計(jì)算。在計(jì)算過程中，只要將實(shí)施例六中wifi傳感器的數(shù)據(jù)替換為地磁傳感器的數(shù)據(jù)即可。

實(shí)施例九

本實(shí)施例中，待監(jiān)測(cè)載體4可為人員或輪式機(jī)器人。

本實(shí)施例與其他實(shí)施例相同的步驟可采用其他實(shí)施例中的步驟中的內(nèi)容。本實(shí)施例的步驟(2)可采用實(shí)施例一、二、三中任一種方法。本實(shí)施例中，步驟(6)中，檢測(cè)到的基站2的信號(hào)為基站2與待監(jiān)測(cè)載體4之間的相對(duì)位置(xwifi,ywifi)和/或相對(duì)距離d和/或相對(duì)位置(xwifi,ywifi)，利用相對(duì)位置(xwifi,ywifi)和/或相對(duì)距離d和/或相對(duì)位置(xwifi,ywifi)計(jì)算粒子權(quán)值本實(shí)施例中，分別布置一個(gè)ibeacon傳感器基站、一個(gè)wifi傳感器基站，優(yōu)選布置為各個(gè)基站2的作用范圍不重疊。

圖8(a)-(b)給出，采用超寬帶傳感器基站與ibeacon傳感器基站同時(shí)布置的情況。同時(shí)融合超寬帶傳感器基站和ibeacon傳感器基站就精度而言相比較與單個(gè)基站相融合在精度上沒有較大的區(qū)別，但是由于超寬帶傳感器基站的布置成本較高，ibeacon傳感器基站成本低，因此對(duì)于傳感器基站的布置要求降低，使靈活性更高。

最后給出專利提出的方法經(jīng)過試驗(yàn)驗(yàn)證的結(jié)果，圖6(a)-(c)給出，慣性定位與超寬帶傳感器融合后的定位結(jié)果與只用慣性定位的結(jié)果比較，定位誤差明顯減弱，使得定位結(jié)果更加精確。圖6(a)為實(shí)際的行走軌跡，真實(shí)軌跡為沿箭頭方向反復(fù)行走；圖6(b)為本發(fā)明的純慣導(dǎo)計(jì)算軌跡；圖6(c)為本發(fā)明的融合慣性定位與超寬帶傳感器基站的定位結(jié)果圖。如圖6(a)中，真實(shí)的行走軌跡為，先在a-b-c段行走直線軌跡，然后在圓形的d區(qū)域行走多圈，繞圈后，沿之前的路徑c-b-a段返回。圖6(b)中可以看出，在開始的a-b-c段及在d區(qū)域繞圈的階段，慣性定位基本與真實(shí)軌跡重合。在繞圈完成后，沿之前的路徑c-b-a段運(yùn)動(dòng)時(shí)，慣性定位方法的軌跡明顯偏離真實(shí)軌跡，即隨著時(shí)間越長(zhǎng)，慣性定位的累積誤差越大，使得定位產(chǎn)生錯(cuò)誤。如圖6(c)所示，根據(jù)本方法，在繞圈完成后，沿之前的路徑c-b-a段運(yùn)動(dòng)時(shí)，定位結(jié)果也會(huì)與真實(shí)運(yùn)動(dòng)路徑產(chǎn)生偏移，但由于設(shè)置了超寬帶基站，因此會(huì)逐漸將產(chǎn)生誤差的定位軌跡拉回真實(shí)軌跡，消除了運(yùn)動(dòng)過程中產(chǎn)生的誤差。

圖7(a)-(b)給出，慣性定位與ibeacon傳感器融合后的定位結(jié)果與只采用慣性定位的結(jié)果比較。隨著時(shí)間的推移，慣性定位軌跡相對(duì)于真實(shí)行進(jìn)路線存在較大的慣性漂移，而經(jīng)過ibeacon傳感器融合之后的定位軌跡幾乎與原行進(jìn)路線重合，結(jié)果更加精確。慣性定位與ibeacon傳感器融合后糾正了慣性定位慣性漂移，定位更準(zhǔn)確。根據(jù)圖7(a)所示，m1、m2、m3的位置分別表示時(shí)間依次變長(zhǎng)的慣性定位的軌跡，可以看出，m1基本與真實(shí)軌跡相符，但m2已出現(xiàn)偏差，m3的位置與真實(shí)運(yùn)動(dòng)區(qū)域完全偏離，定位結(jié)果已經(jīng)不可信。因此圖7(a)也驗(yàn)證了慣性定位的誤差隨時(shí)間累積越來(lái)越大，完全偏離了真實(shí)軌跡。根據(jù)圖7(b)所示，本發(fā)明的方法，在相同的時(shí)間過程中，未出現(xiàn)誤差累積的過程，始終處于真實(shí)軌跡，未出現(xiàn)明顯的累積誤差，定位精度較高。圖7(a)為純慣導(dǎo)計(jì)算軌跡，從圖中可以看出純慣導(dǎo)運(yùn)動(dòng)軌跡相對(duì)實(shí)際運(yùn)動(dòng)建筑物存在較大的誤差；圖7(b)為采用了慣導(dǎo)與ibeacon融合定位的結(jié)果，從圖中可以看出采用了融合定位的方法，定位軌跡和建筑物走向重合度明顯提高。

圖8(a)-(b)給出，采用超寬帶傳感器基站與ibeacon傳感器基站同時(shí)布置的情況。如圖8(a)所示，m4、m5、m6軌跡分別表示時(shí)間依次變長(zhǎng)的慣性定位的軌跡，可以看出，隨著時(shí)間的變長(zhǎng)，慣性定位的誤差隨時(shí)間累積越來(lái)越大，m5的軌跡甚至出現(xiàn)了穿過墻體的情況，m6的軌跡甚至出現(xiàn)了在錯(cuò)誤的房間的定位軌跡，完全偏離了真實(shí)軌跡。根據(jù)圖8(b)所示，本發(fā)明的方法中同時(shí)布置超寬帶傳感器基站和ibeacon傳感器基站，相同的時(shí)間過程中，未出現(xiàn)誤差累積的過程，始終處于真實(shí)軌跡，未出現(xiàn)明顯的累積誤差，定位精度較高。

同時(shí)融合超寬帶傳感器基站和ibeacon傳感器基站就精度而言相比較與單個(gè)基站相融合在精度上沒有較大的區(qū)別，但是由于超寬帶傳感器基站的布置成本較高，ibeacon傳感器成本低，因此對(duì)于傳感器的布置要求降低，使靈活性更高。在實(shí)際中，可根據(jù)實(shí)際成本的要求靈活選擇如何布置基站及布置哪種類型的基站。

需要說明的是，本說明書中的各個(gè)實(shí)施例均采用遞進(jìn)的方式描述，每個(gè)實(shí)施例重點(diǎn)說明的都是與其他實(shí)施例的不同之處，各個(gè)實(shí)施例之間相同相似的部分互相參見即可。

以上對(duì)本發(fā)明的實(shí)施例進(jìn)行了詳細(xì)說明，但所述內(nèi)容僅為本發(fā)明的較佳實(shí)施例，不能被認(rèn)為用于限定本發(fā)明的實(shí)施范圍。凡依本發(fā)明范圍所作的均等變化與改進(jìn)等，均應(yīng)仍歸屬于本專利涵蓋范圍之內(nèi)。