本發(fā)明屬于綜合導(dǎo)航技術(shù)，特別是涉及一種基于慣性器件導(dǎo)航，同時(shí)構(gòu)建wi-fi(無(wú)線網(wǎng)絡(luò)信號(hào))指紋數(shù)據(jù)庫(kù)輔助導(dǎo)航的室內(nèi)導(dǎo)航定位算法。
背景技術(shù)：
在室內(nèi)環(huán)境下，由于衛(wèi)星信號(hào)的大幅衰減，基于衛(wèi)星定位的方法無(wú)法實(shí)施，成為了導(dǎo)航定位“最后一公里”的問(wèn)題。由于近年來(lái)人們對(duì)于無(wú)線上網(wǎng)的需求日益增大，越來(lái)越多的wi-fi無(wú)線接入點(diǎn)被架設(shè)在樓宇內(nèi)以滿足人們無(wú)縫接入互聯(lián)網(wǎng)的需求，進(jìn)而為基于wi-fi信號(hào)的室內(nèi)導(dǎo)航技術(shù)提供了空間。但傳統(tǒng)的基于wi-fi位置指紋信息的定位技術(shù)誤差大，易受干擾，且指紋庫(kù)的建立與更新效率較低。與此同時(shí)，wi-fi接入點(diǎn)的移動(dòng)或增加，也會(huì)大幅影響指紋庫(kù)的性能，從而影響導(dǎo)航效果。
技術(shù)實(shí)現(xiàn)要素：
本發(fā)明所要解決的技術(shù)問(wèn)題是：提供一種適用于可穿戴設(shè)備的室內(nèi)導(dǎo)航方案。
本發(fā)明的技術(shù)方案是：解決思路主要分為三個(gè)步驟：(1)通過(guò)以慣性傳感器為主的多傳感器融合技術(shù)、步速探測(cè)、零位校正等技術(shù)實(shí)現(xiàn)室內(nèi)導(dǎo)航；(2)同時(shí)構(gòu)建并實(shí)時(shí)更新wi-fi指紋數(shù)據(jù)庫(kù)；(3)通過(guò)wi-fi指紋識(shí)別的定位結(jié)果輔助導(dǎo)航，提升導(dǎo)航精度。該方案所使用的器件可封裝成為便攜式模塊，滿足可穿戴設(shè)備使用。一種適用于可穿戴設(shè)備的室內(nèi)導(dǎo)航算法，具體方案如下：以下步驟：
步驟1：初始化階段
在產(chǎn)品初始化階段，需要導(dǎo)入室內(nèi)地圖并指定初始位置，用戶基于提前內(nèi)置的室內(nèi)地圖集選擇當(dāng)下室內(nèi)地圖，并手動(dòng)指定目前所在位置；如沒(méi)有該地圖，可通過(guò)網(wǎng)絡(luò)數(shù)據(jù)庫(kù)進(jìn)行下載；
步驟2：基于多傳感器的航跡推算
a)預(yù)處理陀螺、加計(jì)數(shù)據(jù)
將三軸陀螺信號(hào)分別記為nx、ny、nz，對(duì)其進(jìn)行低通濾波，濾除外界高頻噪聲
gx＝b0·nx+b1·nx_1+b2·nx_2+b3·nx_3+b4·nx_4+b5·nx_5+...
b6·nx_6+b7·nx_7-(a1·gx_1+a2·gx_2+a3·gx_3+a4·gx_4+...
a5·gx_5+a6·gx_6+a7·gx_7)
gy＝b0·ny+b1·ny_1+b2·ny_2+b3·ny_3+b4·ny_4+b5·ny_5+...
b6·ny_6+b7·ny_7-(a1·gy_1+a2·gy_2+a3·gy_3+a4·gy_4+...
a5·gy_5+a6·gy_6+a7·gy_7)
gz＝b0·nz+b1·nz_1+b2·nz_2+b3·nz_3+b4·nz_4+b5·nz_5+...
b6·nz_6+b7·nz_7-(a1·gz_1+a2·gz_2+a3·gz_3+a4·gz_4+...
a5·gz_5+a6·gz_6+a7·gz_7)
其中，每個(gè)采樣時(shí)刻采集到的數(shù)據(jù)為nx，濾波后的輸出值為gx，
nx_1為上次采樣值，nx_2為上上次采樣值，其它類(lèi)同；
gx_1為上次濾波值，gx_2為上上次濾波值，其它類(lèi)同；
a1-a7，b0-b7為濾波器參數(shù)，需根據(jù)陀螺加計(jì)信號(hào)進(jìn)行適當(dāng)選擇；
加速度計(jì)同理，得到陀螺輸出gx、gy、gz和加計(jì)輸出ax、ay、az；
b)磁力計(jì)確定航向
靜止時(shí)只進(jìn)行定位，移動(dòng)時(shí)初始時(shí)刻以磁航向作為初始航向，不進(jìn)行初始對(duì)準(zhǔn)；
航向角ψ＝磁力計(jì)航向角
俯仰角θ＝0
橫滾角γ＝0
c)捷聯(lián)慣性解算
計(jì)算
c11＝cos(γ)·cos(ψ)+sin(γ)·sin(θ)·sin(ψ)
c12＝cos(θ)·sin(ψ)
c13＝sin(γ)·cos(ψ)-cos(γ)·sin(θ)·sin(ψ)
c21＝-cos(γ)·sin(ψ)+sin(γ)·sin(θ)·cos(ψ)
c22＝cos(θ)·cos(ψ)
c23＝-sin(γ)·sin(ψ)-cos(γ)·sin(θ)·cos(ψ)
c31＝-cosθ·sinγ
c32＝sinθ
c33＝cosθ·cosγ
計(jì)算
d)用陀螺數(shù)據(jù)進(jìn)行姿態(tài)角零速校正
平臺(tái)偏角修正
e)用加計(jì)數(shù)據(jù)進(jìn)行位置零速校正
λ(+)＝λ(-)-δλ
λ(-)＝λ(+)
速度修正
ve(+)＝ve(-)-δve
vn(+)＝vn(-)-δvn
ve(-)＝ve(+)
vn(-)＝vn(+)
f)氣壓計(jì)阻尼高度
vu＝vu+k1(hb-h)
vu(-)＝vu
其中：k1＝0.0005
若滿足高度阻尼計(jì)算條件，則按照下式進(jìn)行阻尼計(jì)算：
h＝h+k2·(hb-h)
h(-)＝h
其中：k2＝0.01
基于以上四步判斷步速并進(jìn)行基本航跡推算；
步驟3：wi-fi指紋數(shù)據(jù)庫(kù)的生成與更新
在本階段，wi-fi無(wú)線網(wǎng)卡實(shí)時(shí)接收來(lái)自附近無(wú)線接入點(diǎn)(ap,accesspoint)的烽火信號(hào)(beaconsignal)，其信號(hào)中包含了每個(gè)ap的mac地址信息和信號(hào)強(qiáng)度(rssi)信息；wi-fi指紋數(shù)據(jù)庫(kù)即包含了在室內(nèi)環(huán)境下任意某點(diǎn)位置能夠接收到來(lái)自附近ap發(fā)出的烽火信號(hào)的數(shù)據(jù)強(qiáng)度值的集合；
假設(shè)每個(gè)位置都可以用來(lái)自不同無(wú)線接入點(diǎn)的rssi向量表示，那么某位置a可以表示為：
其中xi和yi分別代表采樣點(diǎn)的坐標(biāo)位置，m表示采樣點(diǎn)的數(shù)量；在本方案中，使用了一種快速實(shí)時(shí)生成并更新數(shù)據(jù)庫(kù)的方式，即采用了基于高斯過(guò)程的機(jī)器學(xué)習(xí)方法；假設(shè)每個(gè)采樣點(diǎn)可以被寫(xiě)為：
d＝{(xi,yi)|i＝1,2,...,n}
其中x表示輸入向量(即坐標(biāo))，同時(shí)y表示測(cè)量值；假設(shè)這些數(shù)據(jù)取自于一個(gè)加噪的過(guò)程：
yi＝f(xi)+ε
其中誤差ε服從均值為零，方差為
實(shí)現(xiàn)高斯過(guò)程回歸；利用這種辦法可以通過(guò)有限的采樣點(diǎn)快速生成整個(gè)環(huán)境下的數(shù)據(jù)庫(kù)；刷新時(shí)間為1秒/次，每刷新一次更新一次wi-fi指紋數(shù)據(jù)庫(kù)；
步驟4：多信息源融合導(dǎo)航算法
a)根據(jù)步驟3的融合wifi信號(hào)比對(duì)數(shù)據(jù)，圈定室內(nèi)位置，融合慣導(dǎo)阻尼
氣壓計(jì)數(shù)據(jù)并wifi信號(hào)，圈定樓層
δφx＝(c11·ωe'+c21·ωn'+c31·ωu')·ts
δφy＝(c12·ωe'+c22·ωn'+c32·ωu')·ts
δφz＝(c13·ωe'+c23·ωn'+c33·ωu')·ts
其中：
du1＝(ω8-ω7)×(vn-k1·vrn)-ω7·u1
du2＝(ω5-ω6)×(vn-k1·vrn+u1)-ω6·u2
du3＝(ω4-ω3)×(ve-k1·vre)-ω3·u3
du4＝(ω1-ω2)×(ve-k1·vre+u3)-ω2·u4
du5＝c×b×(vn-k1·vrn+u1+u2)-ω9·u5
du6＝ω10·u5-ω10·u6
u1＝u1+du1×ts
u2＝u2+du2×ts
u3＝u3+du3×ts
u4＝u4+du4×ts
u5＝u5+du5×ts
u6＝u6+du6×ts
u1、u2、u3、u4、u5、u6初值均為0
b)設(shè)計(jì)擴(kuò)展卡爾曼算法實(shí)現(xiàn)室內(nèi)導(dǎo)航定位
xk/k-1＝φk/k-1·xk-1
pk-1＝pk/k-1
xk-1＝xk/k-1
xk＝xk/k-1+kk·(zk-hk·xk/k-1)
pk＝(i-kk·hk)·pk/k-1
pk-1＝pk
當(dāng)滿足零速校正條件時(shí)：
δλ＝x′k(2)
否則：
δλ＝0
δdn、δdu保持不變；
●量測(cè)矩陣
其中：
零速校正時(shí)量測(cè)矩陣：
p＝diag(6e-4，6e-4，1，1，6e-6，6e-6，3.0e-4，1e-12，1e-12，1e-12，2.5e-5，2.5e-5，2.5e-5，0，0，0，0，0，0，0,0,0)
q＝diag(0，0，5.e-5，5.e-5，5.e-13，5.e-13，5.e-13，0，0，0，0，0，0,0,0,0,0,0，0，0,0,0)
r＝diag(2.6e-10，2.6e-10)
航跡推算過(guò)程中更換wifi接入點(diǎn)時(shí)，要對(duì)p1,1、p2,2進(jìn)行重置：
p1,1＝6e-4
p2,2＝6e-4
●零速校正濾波參數(shù)
q＝diag(0，0，0，0，5.e-13，5.e-13，5.e-13，0，0，0，0，0，0,0,0,0,0,0，0，0，0，0)
r＝diag(2.6e-10，2.6e-10，9，9)
步驟5：導(dǎo)航結(jié)果顯示
導(dǎo)航結(jié)果將會(huì)通過(guò)無(wú)線網(wǎng)絡(luò)傳輸至終端顯示器進(jìn)行顯示(手機(jī)屏幕、手持設(shè)備屏幕等)，從而完成室內(nèi)導(dǎo)航的任務(wù)。
根據(jù)多敏感組件進(jìn)行慣性導(dǎo)航，并利用wifi信號(hào)進(jìn)行實(shí)時(shí)，實(shí)現(xiàn)室內(nèi)導(dǎo)航。
本發(fā)明的有益效果是：本方案設(shè)計(jì)的一種基于慣性器件和wi-fi指紋識(shí)別的室內(nèi)導(dǎo)航算法，基于低成本的mems慣性傳感器、氣壓計(jì)、磁力計(jì)等器件，輔助以wi-fi定位算法，能夠快速的生成并更新wi-fi指紋數(shù)據(jù)庫(kù)輔助航跡推算，進(jìn)而大幅提升室內(nèi)導(dǎo)航能力。該方案適用于可穿戴設(shè)備上，能夠有效的完成室內(nèi)導(dǎo)航定位的工作。
附圖說(shuō)明
圖1為本發(fā)明的處理流程圖。
具體實(shí)施方式
如圖1所示，本發(fā)明的技術(shù)方案是：一種適用于可穿戴設(shè)備的室內(nèi)導(dǎo)航算法，其特點(diǎn)為如下所述：
1.1)初始化地圖信息及位置信息
1.2)基于mems陀螺加計(jì)的慣性導(dǎo)航算法
1.3)對(duì)陀螺加計(jì)信號(hào)進(jìn)行分析，找出步速的計(jì)算點(diǎn)，實(shí)行步速計(jì)算
1.4)當(dāng)靜止不動(dòng)時(shí)，通過(guò)速度進(jìn)行零位修正
1.5)有g(shù)ps信號(hào)時(shí)進(jìn)行實(shí)時(shí)修正
1.6)利用基于高斯過(guò)程回歸的機(jī)器學(xué)習(xí)算法實(shí)時(shí)生成并更新wi-fi指紋數(shù)據(jù)庫(kù)
1.7)wi-fi指紋識(shí)別技術(shù)定位
1.8)采用擴(kuò)展卡爾曼濾波器，對(duì)信息源進(jìn)行融合計(jì)算，從而計(jì)算出導(dǎo)航結(jié)果
1.9)導(dǎo)航結(jié)果顯示
結(jié)果表明，采用本發(fā)明的方法，基于可穿戴設(shè)備的室內(nèi)導(dǎo)航算法可以實(shí)現(xiàn)偏差為1米(rms)的室內(nèi)導(dǎo)航精度。
本發(fā)明說(shuō)明書(shū)中未作詳細(xì)描述的內(nèi)容屬于本領(lǐng)域?qū)I(yè)技術(shù)人員公知的現(xiàn)有技術(shù)。
最后所應(yīng)說(shuō)明的是：以上實(shí)施實(shí)例僅用以說(shuō)明而非限制本發(fā)明的技術(shù)方案，所有的不脫離本發(fā)明的精神和范圍的修改或局部替換，均應(yīng)涵蓋在本發(fā)明的權(quán)利要求范圍當(dāng)中。