本發(fā)明涉及導(dǎo)航定位裝置和方法，特別是涉及一種基于mems傳感器和vlc定位融合的雙粒子濾波導(dǎo)航裝置和方法。

背景技術(shù)：

隨著室內(nèi)定位技術(shù)的發(fā)展和應(yīng)用，基于可見(jiàn)光通信的室內(nèi)定位技術(shù)也在迅速興起并得到廣泛關(guān)注。在充足室內(nèi)光源的環(huán)境下，通過(guò)光傳感器等設(shè)備檢測(cè)得到經(jīng)過(guò)復(fù)用協(xié)議調(diào)制的光信號(hào)，通過(guò)信號(hào)解調(diào)可以將不同光源信號(hào)數(shù)據(jù)分離，從而結(jié)合環(huán)境參數(shù)可以計(jì)算出定位目標(biāo)相對(duì)每個(gè)光源的距離或角度信息，最后通過(guò)定位算法如三邊定位可以完成目標(biāo)定位。

然而，由于目標(biāo)接收設(shè)備的姿態(tài)會(huì)隨著目標(biāo)移動(dòng)產(chǎn)生抖動(dòng)，將對(duì)vlc定位結(jié)果帶來(lái)較大的影響。另一方面，實(shí)際場(chǎng)景中光信號(hào)容易被遮擋，將會(huì)導(dǎo)致定位不連續(xù)。針對(duì)前一個(gè)問(wèn)題，目前的方案主要是通過(guò)多傳感器組合共同定位。針對(duì)后一個(gè)問(wèn)題，主流的解決方案是通過(guò)卡爾曼濾波或者粒子濾波來(lái)估計(jì)目標(biāo)位置。但這些方案存在一些問(wèn)題：1)相比于單傳感器定位，多傳感器組合定位算法復(fù)雜且成本較高；2)目前定位方案中提出的濾波器融合是以探測(cè)器姿態(tài)平穩(wěn)為前提，在實(shí)際場(chǎng)景中穩(wěn)定性較差。3)在信號(hào)被遮擋的情況頻繁發(fā)生的場(chǎng)景中，僅通過(guò)vlc數(shù)據(jù)加濾波器的定位系統(tǒng)推測(cè)的目標(biāo)位置仍然與實(shí)際位置有較大偏差。

另外，在慣導(dǎo)定位中一般采用卡爾曼濾波進(jìn)行噪聲處理和定位，卡爾曼濾波對(duì)于線性系統(tǒng)效果較好，而在實(shí)際環(huán)境中，很多系統(tǒng)是非線性的。擴(kuò)展卡爾曼濾波是卡爾曼濾波的一種新的形式，它通過(guò)泰勒展開(kāi)后忽略二階以上的分量從而將非線性系統(tǒng)轉(zhuǎn)化為線性系統(tǒng)。粒子濾波算法則能夠處理完全非線性的系統(tǒng)，效果比卡爾曼濾波更好。此外，在慣導(dǎo)定位技術(shù)中，多采用單個(gè)濾波器來(lái)進(jìn)行噪聲處理和定位，然而這樣通過(guò)單一濾波器同時(shí)定位和測(cè)姿會(huì)造成姿態(tài)和位置相互影響。

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

發(fā)明目的：為解決現(xiàn)有技術(shù)的不足，提供一種基于mems傳感器和vlc定位融合的雙粒子濾波導(dǎo)航方法和裝置。

技術(shù)方案：基于mems傳感器和vlc定位融合的雙粒子濾波導(dǎo)航裝置，包括mems傳感器、ins模塊、pdr定位模塊、vlc定位模塊、測(cè)姿擴(kuò)展卡爾曼濾波器和定位擴(kuò)展卡爾曼濾波器；所述mems傳感器包括加速度計(jì)、陀螺儀以及磁力計(jì)；

測(cè)姿擴(kuò)展卡爾曼濾波器的輸入包括：陀螺儀采集接收器在xyz方向的角速度信息經(jīng)過(guò)ins模塊的機(jī)械編排算法處理得到的ins姿態(tài)信息、加速度計(jì)采集的接收器在xyz方向的加速度信息以及磁力計(jì)采集的接收器相對(duì)東、南、西、北方向的角度信息；其中，加速度信息和角速度信息按時(shí)間軸順序輸入至測(cè)姿擴(kuò)展卡爾曼濾波器；輸出包括傳遞給prd定位模塊和vlc定位模塊的姿態(tài)誤差向量和反饋給陀螺儀用以噪聲補(bǔ)償?shù)耐勇萜钕蛄浚?/p>

定位擴(kuò)展卡爾曼濾波器的輸入包括pdr定位模塊輸出的pdr位置信息和vlc定位模塊輸出的位置信息，pdr定位模塊的輸入包括采集的接收器在xyz方向的加速度信息和所述測(cè)姿擴(kuò)展卡爾曼濾波器輸出的姿態(tài)信息；定位擴(kuò)展卡爾曼濾波器輸出接收器的定位信息。

一種基于所述導(dǎo)航裝置的導(dǎo)航方法，包括以下步驟：

(1)估計(jì)接收器姿態(tài)信息

(11)建立a-pf的狀態(tài)向量；

(12)建立a-pf的系統(tǒng)模型；

(13)建立a-pf的觀測(cè)模型；

(14)a-pf濾波輸出姿態(tài)信息；

(2)估計(jì)接收器位置信息

(21)建立l-pf的狀態(tài)向量；

(22)建立l-pf的系統(tǒng)模型；

(23)建立l-pf的觀測(cè)模型；

(24)l-pf濾波輸出定位信息。

進(jìn)一步的，所述步驟(11)中a-pf的狀態(tài)向量定義為：

x＝[ψbg]^t

其中，ψ為姿態(tài)誤差向量；bg為陀螺偏差向量。

進(jìn)一步的，所述步驟(12)中先對(duì)姿態(tài)矩陣進(jìn)行坐標(biāo)系變換，其坐標(biāo)轉(zhuǎn)移方程為：

其中，是從載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的方向余弦矩陣，即姿態(tài)矩陣；和分別是角速度矢量和的斜對(duì)稱矩陣；而和表示載體坐標(biāo)系相對(duì)于慣性坐標(biāo)的轉(zhuǎn)動(dòng)角速度和導(dǎo)航坐標(biāo)系相對(duì)于慣性坐標(biāo)的轉(zhuǎn)動(dòng)角速度；

a-pf的系統(tǒng)模型為具體展開(kāi)公式如下：

其中，和分別表示地球自傳的角速度和導(dǎo)航坐標(biāo)系相對(duì)于地心地固坐標(biāo)系的轉(zhuǎn)動(dòng)角速度；wg是傳感器噪聲；τbg代表慣導(dǎo)噪聲的相關(guān)時(shí)間；wbg是驅(qū)動(dòng)噪聲；符號(hào)“×”表示叉乘。

進(jìn)一步的，所述步驟(13)中a-pf的觀測(cè)模型包括加速度計(jì)觀測(cè)模型和磁力計(jì)觀測(cè)模型，觀測(cè)模型的表達(dá)式為z＝hx+v其中，

加速度計(jì)觀測(cè)模型具體為：

其中，是加速度計(jì)讀數(shù)矢量，fⁿ＝-gⁿ＝[00-g]^t是導(dǎo)航坐標(biāo)系中的特定力矢量，f^b是載體坐標(biāo)系中的特定力矢量，g是局部重力值，ψ是姿態(tài)誤差,i是單位矩陣，n2是噪聲，[ψ×]表示ψ矢量的斜對(duì)稱矩陣，[gⁿ×]表示gⁿ矢量的斜對(duì)稱矩陣，此時(shí)，h＝-[gⁿ×]，

磁力計(jì)觀測(cè)模型具體為：

其中，是磁力計(jì)讀數(shù)矢量，mⁿ是校準(zhǔn)的lmf矢量，n3是噪聲，h＝[mⁿ×]，

進(jìn)一步的，所述步驟(14)包括：

(a)將輸入量和zk輸入a-pf

其中，為k-1時(shí)刻的粒子，表示k-1時(shí)刻的第i個(gè)粒子的狀態(tài)；表示k-1時(shí)刻第i個(gè)粒子的權(quán)重，zk為k時(shí)刻的觀測(cè)值；其中粒子狀態(tài)量x＝[ψbg]^t；

(b)濾波過(guò)程

①對(duì)于每一個(gè)k-1時(shí)刻第i個(gè)粒子的狀態(tài)通過(guò)s-pf系統(tǒng)方程模型生成k時(shí)刻第i個(gè)粒子的狀態(tài)

②對(duì)于每個(gè)新粒子利用s-pf觀測(cè)模型來(lái)計(jì)算每個(gè)粒子的權(quán)重

其中，表示k時(shí)刻觀測(cè)量zk對(duì)于粒子的條件概率；

③歸一化權(quán)重

其中，ns為粒子個(gè)數(shù)；

④重采樣過(guò)程

(c)濾波輸出

輸出k時(shí)刻的粒子輸出k時(shí)刻的vlc接收器的姿態(tài)信息給vlc定位模塊，輸出k時(shí)刻的pdr接收器的姿態(tài)信息給pdr定位模塊，以矯正姿態(tài)的影響，輸出k時(shí)刻的陀螺偏差矢量反饋給陀螺儀用以噪聲補(bǔ)償。

進(jìn)一步的，所述步驟(21)中l(wèi)-pf的狀態(tài)向量定義為：

其中，和λ分別為接收器緯度信息和經(jīng)度信息。

進(jìn)一步的，所述步驟(22)中使用pdr定位模塊處理加速度計(jì)讀數(shù)獲得步數(shù)和步長(zhǎng)信息，并從a-pf獲得方位角信息，然后pdr通過(guò)上一時(shí)刻的位置接收器方位角和接收器步長(zhǎng)來(lái)計(jì)算當(dāng)前時(shí)刻新的位置其系統(tǒng)模型為：

其中和s是方位角和步長(zhǎng)，下標(biāo)k-1和k代表步數(shù)，rm和rn是地球的主垂直線中的子午線和曲率的曲率半徑，h是高度。

進(jìn)一步的，所述步驟(23)中l(wèi)-pf的觀測(cè)方程直接使用vlc輸出的位置信息建立，觀測(cè)方程如下：

其中，和分別為pdr預(yù)測(cè)的緯度和經(jīng)度，和分別為vlc輸出的緯度和經(jīng)度，和nλ是測(cè)量噪聲。

進(jìn)一步的，所述步驟(24)包括：

(a)將輸入量和zk輸入l-pf

其中，為k-1時(shí)刻的粒子，表示k-1時(shí)刻的第i個(gè)粒子的狀態(tài)；表示k-1時(shí)刻第i個(gè)粒子的權(quán)重，zk為k時(shí)刻的觀測(cè)值，即vlc定位系統(tǒng)得到位置信息；其中粒子狀態(tài)向量

(b)濾波過(guò)程

①對(duì)于每一個(gè)k-1時(shí)刻的粒子通過(guò)將pdr位置信息作為s-pf系統(tǒng)方程模型生成新粒子

②對(duì)于每個(gè)新粒子利用s-pf觀測(cè)模型來(lái)計(jì)算每個(gè)粒子的權(quán)重

其中，表示k時(shí)刻觀測(cè)量zk對(duì)于粒子的條件概率；

③歸一化權(quán)重

其中，ns為粒子個(gè)數(shù)；

④重采樣過(guò)程

(c)濾波輸出

輸出k時(shí)刻的粒子輸出接收器k時(shí)刻的定位信息。

有益效果：與傳統(tǒng)的vlc定位相比，本發(fā)明的雙粒子濾波器(particlefilter，pf)將vlc信號(hào)和mems傳感器融合在一起，具有如下優(yōu)點(diǎn)：1)該融合濾波器首次在vlc定位領(lǐng)域使用融合測(cè)姿準(zhǔn)確估計(jì)vlc接收器的姿態(tài)信息，并消除姿態(tài)對(duì)vlc定位的影響；2)該融合濾波器設(shè)計(jì)可彌補(bǔ)vlc定位結(jié)果不連續(xù)、不平滑的缺點(diǎn)；3)該融合濾波器設(shè)計(jì)可在vlc信號(hào)被遮擋的情況下使用mems傳感器信息提供定位結(jié)果。

附圖說(shuō)明

圖1是測(cè)姿定位雙粒子濾波器導(dǎo)航裝置結(jié)構(gòu)示意圖；

圖2是俯仰角0°的仿真定位結(jié)果cdf曲線；

圖3是俯仰角5°的仿真定位結(jié)果cdf曲線；

圖4是俯仰角8°的仿真定位結(jié)果cdf曲線；

圖5是接收設(shè)備平放、傾斜5°、傾斜8°時(shí)的定位誤差分析結(jié)果。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明的技術(shù)方案進(jìn)行詳細(xì)的描述。

如圖1所示，基于mems傳感器和vlc定位融合的雙粒子濾波器導(dǎo)航裝置，其特征在于：包括mems傳感器、慣性導(dǎo)航系統(tǒng)(inertialnavigationsystem，ins)模塊、行人航位推算(pedestriandeadreckoning，pdr)定位模塊、可見(jiàn)光通信(visiblelightcommunication，vlc)定位模塊、測(cè)姿擴(kuò)展卡爾曼濾波器(attitudeextendedkalmanfilter，a-ekf)和定位擴(kuò)展卡爾曼濾波器(locationextendedkalmanfilter，l-ekf)；所述mems傳感器包括加速度計(jì)、陀螺儀以及磁力計(jì)。

陀螺儀測(cè)得的接收器在xyz方向的角速度信息傳遞給ins模塊，通過(guò)陀螺機(jī)械編排算法處理數(shù)據(jù)得到ins姿態(tài)信息。加速度計(jì)測(cè)得的接收器在xyz方向的加速度信息傳遞給pdr定位模塊進(jìn)行位置估計(jì)。另一方面，加速度計(jì)測(cè)得的接收器在xyz方向的加速度信息、磁力計(jì)測(cè)得的接收器相對(duì)東南西北方向的角度信息與ins姿態(tài)信息三者作為測(cè)姿擴(kuò)展卡爾曼濾波器的輸入量。測(cè)姿擴(kuò)展卡爾曼濾波器輸出包含姿態(tài)誤差向量和陀螺偏差向量。陀螺偏差向量反饋給陀螺儀進(jìn)行噪聲補(bǔ)償，而姿態(tài)誤差向量傳遞給vlc定位模塊和pdr定位模塊幫助定位。vlc定位模塊和pdr定位模塊通過(guò)各自的定位算法分別輸出接收器位置信息給定位擴(kuò)展卡爾曼濾波器。定位擴(kuò)展卡爾曼濾波器會(huì)持續(xù)更新?tīng)顟B(tài)量并輸出接收器的定位信息(位置、速度)。

系統(tǒng)中有兩點(diǎn)需要注意：對(duì)于a-pf，磁力計(jì)和加速計(jì)的讀數(shù)會(huì)按時(shí)間軸順序參與到測(cè)姿擴(kuò)展卡爾曼濾波器的觀測(cè)方程中，并非同時(shí)參與濾波；對(duì)于l-pf，從pdr定位模塊獲得的位置信息作為定位擴(kuò)展卡爾曼濾波器預(yù)測(cè)和更新的狀態(tài)量，而從vlc定位模塊獲得的位置信息作為觀測(cè)量參與到定位擴(kuò)展卡爾曼濾波器中。

測(cè)姿擴(kuò)展卡爾曼濾波器的輸入包括：陀螺儀采集接收器在xyz方向的角速度信息經(jīng)過(guò)ins模塊的機(jī)械編排算法處理得到的ins姿態(tài)信息、加速度計(jì)采集的接收器在xyz方向的加速度信息以及磁力計(jì)采集的接收器相對(duì)東、南、西、北方向的角度信息；輸出包括傳遞給prd定位模塊和vlc定位模塊的姿態(tài)誤差向量和反饋給陀螺儀用以噪聲補(bǔ)償?shù)耐勇萜钕蛄俊?/p>

定位擴(kuò)展卡爾曼濾波器的輸入包括pdr定位模塊輸出的pdr位置信息和vlc定位模塊輸出的位置信息，pdr定位模塊的輸入包括采集的接收器在xyz方向的加速度信息和所述測(cè)姿擴(kuò)展卡爾曼濾波器輸出的姿態(tài)信息；定位擴(kuò)展卡爾曼濾波器輸出接收器的定位信息。

本發(fā)明基于ins慣導(dǎo)機(jī)械編排的誤差方程作為融合濾波器的系統(tǒng)方程，觀測(cè)方程包括vlc定位信息更新、pdr定位信息更新和磁力計(jì)觀測(cè)量更新。融合濾波器輸出vlc接收器的姿態(tài)信息給vlc定位模塊，輸出pdr接收器的姿態(tài)信息給pdr定位模塊以校正姿態(tài)的影響。

一種基于所述導(dǎo)航裝置的導(dǎo)航方法，包括以下步驟：

步驟1：估計(jì)接收器姿態(tài)信息:

a-pf主要用來(lái)估計(jì)接收器的姿態(tài)信息(即：橫滾角、俯仰角和方位角)。

(1)建立a-pf的狀態(tài)向量

a-pf的狀態(tài)向量定義為：

x＝[ψbg]^t(1)

其中：ψ為姿態(tài)誤差向量；bg為陀螺偏差向量。

(2)建立a-pf的系統(tǒng)模型

先對(duì)姿態(tài)矩陣進(jìn)行坐標(biāo)系變換，其坐標(biāo)轉(zhuǎn)移方程為：

其中，是從載體坐標(biāo)系(b-frame)到導(dǎo)航坐標(biāo)系(n-frame)的方向余弦矩陣；和分別是角速度矢量和的斜對(duì)稱矩陣；而和表示載體坐標(biāo)系(b-frame)相對(duì)于慣性坐標(biāo)(i-frame)的轉(zhuǎn)動(dòng)角速度和導(dǎo)航坐標(biāo)系(n-frame)相對(duì)于慣性坐標(biāo)(i-frame)的轉(zhuǎn)動(dòng)角速度。

a-pf的系統(tǒng)模型為具體展開(kāi)公式如下：

其中，和表示地球自傳的角速度和導(dǎo)航坐標(biāo)系(n-frame)相對(duì)于地心地固坐標(biāo)系(ecef坐標(biāo)系，e-frame)的轉(zhuǎn)動(dòng)角速度；wg是傳感器噪聲；τbg代表慣導(dǎo)噪聲的相關(guān)時(shí)間；wbg是驅(qū)動(dòng)噪聲；符號(hào)“×”表示叉乘。

(3)建立a-pf的觀測(cè)模型

a-pf的觀測(cè)方程直接使用加速度計(jì)和磁力計(jì)讀數(shù)建立。本發(fā)明通過(guò)使用加速度計(jì)讀數(shù)來(lái)構(gòu)建觀測(cè)模型，而不是應(yīng)用加速度計(jì)導(dǎo)出的橫滾角和俯仰角。這個(gè)選擇對(duì)于具有任意接收器姿態(tài)的行人應(yīng)用是重要的，因?yàn)樗苊饬水?dāng)俯仰角達(dá)到±90°時(shí)的奇點(diǎn)問(wèn)題。

a-pf的觀測(cè)模型包括加速度計(jì)觀測(cè)模型和磁力計(jì)觀測(cè)模型，觀測(cè)模型的表達(dá)式為z＝hx+v其中：

加速度計(jì)觀測(cè)模型具體為：

其中，是加速度計(jì)讀數(shù)矢量，fⁿ＝-gⁿ＝[00-g]^t是導(dǎo)航坐標(biāo)系中的特定力矢量，f^b是載體坐標(biāo)系中的特定力矢量，g是局部重力值，ψ是姿態(tài)誤差,i是單位矩陣，n2是噪聲，[ψ×]表示ψ矢量的斜對(duì)稱矩陣，[gⁿ×]表示gⁿ矢量的斜對(duì)稱矩陣，此時(shí)h＝-[gⁿ×]，

a-pf使用磁力計(jì)讀數(shù)建立觀測(cè)模型的主要挑戰(zhàn)在于存在頻繁的磁擾動(dòng)。典型類型的磁擾動(dòng)是局部磁場(chǎng)(localmagneticfield,lmf)的方向和強(qiáng)度都改變，但是該改變?cè)谟邢薜目臻g(或時(shí)間段)內(nèi)是穩(wěn)定的。lmf穩(wěn)定的周期可以稱為準(zhǔn)靜態(tài)磁場(chǎng)(quasi-staticmagneticfield,qsmf)周期，并且可以通過(guò)使用磁力計(jì)讀數(shù)的大小來(lái)檢測(cè)。本發(fā)明所使用的算法將使用磁力計(jì)在qsmf期間的觀測(cè)值，假設(shè)完全不知道lmf參數(shù)，并通過(guò)算法在每個(gè)qsmf周期的開(kāi)始階段校準(zhǔn)lmf。第k個(gè)qsmf周期期間的lmf向量下列公式計(jì)算：

其中，是在第k個(gè)qsmf周期的第一個(gè)時(shí)期的開(kāi)始處的磁力計(jì)讀數(shù)，然后將計(jì)算的用作第k個(gè)qsmf周期期間的參考。本發(fā)明直接使用磁力計(jì)讀數(shù)建立觀測(cè)模型，該策略避免了調(diào)平步驟(即：使用加速度計(jì)讀數(shù)來(lái)計(jì)算橫滾角和俯仰角)。因此，下面列出的磁力計(jì)測(cè)量模型與加速度計(jì)測(cè)量值無(wú)關(guān)。

磁力計(jì)觀測(cè)模型具體為：

其中，是磁力計(jì)讀數(shù)矢量，mⁿ是校準(zhǔn)的lmf矢量，n3是噪聲，此時(shí)，h＝[mⁿ×]，

其中，由于加速度計(jì)和磁力計(jì)從測(cè)量到返回讀數(shù)不一定同步，且和二者的讀數(shù)頻率有關(guān)，所以二者作為輸入量在時(shí)間上是錯(cuò)開(kāi)的。同時(shí)a-ekf濾波器在一個(gè)時(shí)刻只能接收一個(gè)觀測(cè)方程，即兩個(gè)觀測(cè)方程根據(jù)時(shí)間軸順序參與到算法中。

(4)a-pf濾波輸出姿態(tài)信息；

(a)將輸入量和zk輸入a-pf

其中，為k-1時(shí)刻的粒子，表示k-1時(shí)刻的第i個(gè)粒子的狀態(tài)；表示k-1時(shí)刻第i個(gè)粒子的權(quán)重，zk為k時(shí)刻的觀測(cè)值；其中粒子狀態(tài)量x＝[ψbg]^t；

(b)濾波過(guò)程

①對(duì)于每一個(gè)k-1時(shí)刻第i個(gè)粒子的狀態(tài)通過(guò)s-pf系統(tǒng)方程模型生成k時(shí)刻第i個(gè)粒子的狀態(tài)

②對(duì)于每個(gè)新粒子利用s-pf觀測(cè)模型來(lái)計(jì)算每個(gè)粒子的權(quán)重

其中，表示k時(shí)刻觀測(cè)量zk對(duì)于粒子的條件概率；

③歸一化權(quán)重

其中，ns為粒子個(gè)數(shù)；

④重采樣過(guò)程

由于在濾波過(guò)程中，部分狀態(tài)量相近的粒子的權(quán)重會(huì)逐漸增大并趨于相同，其余大多數(shù)粒子權(quán)重會(huì)逐漸減小，導(dǎo)致粒子多樣性缺失，會(huì)使?fàn)顟B(tài)估計(jì)產(chǎn)生較大偏差，這就是粒子退化問(wèn)題。為了解決該問(wèn)題，一般在粒子濾波中引入重采樣過(guò)程。重采樣過(guò)程有重要性重采樣、殘差重采樣、分層重采樣和優(yōu)化組合重采樣等，任選其一即可。如重要性重采樣為例，該過(guò)程將權(quán)重大的粒子按其權(quán)值復(fù)制出相應(yīng)數(shù)量的副本，而淘汰權(quán)重小的粒子，但總體數(shù)量保持不變。

(c)濾波輸出

輸出k時(shí)刻的粒子輸出k時(shí)刻的vlc接收器的姿態(tài)信息給vlc定位模塊，輸出k時(shí)刻的pdr接收器的姿態(tài)信息給pdr定位模塊，以矯正姿態(tài)的影響。輸出k時(shí)刻的陀螺偏差矢量反饋給陀螺儀用以噪聲補(bǔ)償。

步驟2：估計(jì)接收器位置信息

l-pf主要用來(lái)估計(jì)接收器的二維位置信息(即：緯度和經(jīng)度)。圖1顯示l-pf的輸入為1)pdr得出的位置信息和2)vlc得出的位置信息；輸出為接收器的位置信息。

(1)建立l-pf的狀態(tài)向量

l-pf的狀態(tài)向量定義為：

其中，和λ分別為接收器緯度信息和經(jīng)度信息。

(2)建立l-pf的系統(tǒng)模型

使用行人航位推算(pdr)定位模塊處理加速度計(jì)讀數(shù)獲得步數(shù)和步長(zhǎng)信息，并從l-pf獲得方位角信息，然后pdr通過(guò)上一時(shí)刻的位置接收器方位角和接收器步長(zhǎng)來(lái)計(jì)算當(dāng)前時(shí)刻新的位置其系統(tǒng)模型為：

其中和是k-1時(shí)刻的方位角，sk是k時(shí)刻的步長(zhǎng)，下標(biāo)k-1和k代表步數(shù)，rm和rn分別是是地球的主垂直線中的子午線和曲率的曲率半徑，h是高度。

(3)建立l-pf的觀測(cè)模型

l-pf的觀測(cè)方程直接使用vlc輸出的位置信息建立，觀測(cè)方程如下：

其中，和分別為pdr預(yù)測(cè)的緯度和經(jīng)度，和分別為vlc輸出的緯度和經(jīng)度，和nλ是測(cè)量噪聲。

(4)l-pf濾波輸出定位信息

(a)將輸入量和zk輸入l-pf

其中，為k-1時(shí)刻的粒子，表示k-1時(shí)刻的第i個(gè)粒子的狀態(tài)；表示k-1時(shí)刻第i個(gè)粒子的權(quán)重，zk為k時(shí)刻的觀測(cè)值，即vlc定位系統(tǒng)得到位置信息；其中粒子狀態(tài)向量

(b)濾波過(guò)程

①對(duì)于每一個(gè)k-1時(shí)刻的粒子通過(guò)將pdr位置信息作為s-pf系統(tǒng)方程模型生成新粒子

②對(duì)于每個(gè)新粒子利用s-pf觀測(cè)模型來(lái)計(jì)算每個(gè)粒子的權(quán)重

其中，表示k時(shí)刻觀測(cè)量zk對(duì)于粒子的條件概率；

③歸一化權(quán)重

其中，ns為粒子個(gè)數(shù)；

④重采樣過(guò)程

由于在濾波過(guò)程中，部分狀態(tài)量相近的粒子的權(quán)重會(huì)逐漸增大并趨于相同，其余大多數(shù)粒子權(quán)重會(huì)逐漸減小，導(dǎo)致粒子多樣性缺失，會(huì)使?fàn)顟B(tài)估計(jì)產(chǎn)生較大偏差，這就是粒子退化問(wèn)題。為了解決該問(wèn)題，一般在粒子濾波中引入重采樣過(guò)程。重采樣過(guò)程有重要性重采樣、殘差重采樣、分層重采樣和優(yōu)化組合重采樣等，任選其一即可。如重要性重采樣為例，該過(guò)程將權(quán)重大的粒子按其權(quán)值復(fù)制出相應(yīng)數(shù)量的副本，而淘汰權(quán)重小的粒子，但總體數(shù)量保持不變。

(c)濾波輸出

輸出k時(shí)刻的粒子輸出接收器k時(shí)刻的定位信息(位置信息和速度信息)。

技術(shù)原理：

在vlc定位模塊中，定位算法一般采用三邊定位算法。其原理是利用室內(nèi)頂端的多盞燈(一般至少有三盞led燈)作為發(fā)射源，通過(guò)接收器實(shí)時(shí)測(cè)得的光信號(hào)強(qiáng)度代入光信號(hào)傳播模型估計(jì)出目標(biāo)點(diǎn)到每盞燈的距離，最后通過(guò)聯(lián)立方程估算出定位位置。然而，當(dāng)光信號(hào)被遮擋時(shí)，探測(cè)器無(wú)法獲取全部信號(hào)，求解出的位置會(huì)嚴(yán)重超出誤差范圍，這種情況會(huì)通過(guò)閾值進(jìn)行排除，所以系統(tǒng)在該情況下不輸出位置信息，導(dǎo)致定位結(jié)果不連續(xù)。另外，考慮到vlc系統(tǒng)往往存在散粒噪聲和熱噪聲等隨機(jī)噪聲，距離的估計(jì)往往存在一定誤差，因此估得的位置形成的運(yùn)動(dòng)軌跡會(huì)存在陡峭變化等特征，這往往不符合實(shí)際目標(biāo)的運(yùn)動(dòng)特征。由于在目標(biāo)跟蹤時(shí)，先驗(yàn)知識(shí)應(yīng)當(dāng)表示定位軌跡平滑，目標(biāo)當(dāng)前時(shí)刻的狀態(tài)與上一時(shí)刻的狀態(tài)相關(guān)，而濾波方法可以將先驗(yàn)值考慮進(jìn)來(lái)，從而使得定位軌跡更加平滑。

一般的vlc定位往往假設(shè)設(shè)備是平放姿態(tài)，從而簡(jiǎn)化了定位算法。然而在實(shí)際情況中設(shè)備會(huì)產(chǎn)生抖動(dòng)、傾斜。設(shè)備的隨機(jī)傾斜會(huì)改變光信號(hào)的入射角，從而導(dǎo)致測(cè)得的光信號(hào)強(qiáng)度發(fā)生變化。如果無(wú)法準(zhǔn)確估計(jì)設(shè)備的姿態(tài)，這部分的光信號(hào)強(qiáng)度變化將成為系統(tǒng)誤差的一部分，導(dǎo)致定位誤差變大。為了說(shuō)明姿態(tài)誤差對(duì)于vlc定位精度的影響程度，我們通過(guò)對(duì)比仿真了不同俯仰角的情況下vlc定位的精度。該仿真實(shí)驗(yàn)對(duì)接收器發(fā)生傾斜，但定位算法不校正的情況進(jìn)行了模擬，對(duì)所有接收器仿真了兩種不同的傾斜情況，定位結(jié)果cdf曲線分別如圖3和圖4所示(圖2表示設(shè)備未發(fā)生傾斜的定位結(jié)果)。實(shí)驗(yàn)結(jié)果顯示接收器俯仰角為5°和8°時(shí)最大定位誤差分別約為0.13m和0.21m。對(duì)比接收器傾斜和平放時(shí)的定位結(jié)果(如圖5所示)，可以看出當(dāng)接收器發(fā)生了傾斜，如果不對(duì)接收器姿態(tài)進(jìn)行修正，定位誤差會(huì)顯著的增大。該仿真結(jié)果證明準(zhǔn)確估計(jì)接收器的姿態(tài)對(duì)提高可見(jiàn)光定位系統(tǒng)的性能具有重要意義。因此，引入mems姿態(tài)估計(jì)模塊，將準(zhǔn)確的姿態(tài)信息傳遞給vlc定位模塊進(jìn)行姿態(tài)校準(zhǔn)，能夠顯著的減小姿態(tài)對(duì)vlc定位結(jié)果的影響。

由于光信號(hào)在被遮擋的情況下vlc定位無(wú)法進(jìn)行，因此引入pdr模塊進(jìn)行定位。pdr定位可以完全利用mems傳感器信號(hào)來(lái)計(jì)算步長(zhǎng)和方向，利用行人上一時(shí)刻的位置推測(cè)出行人當(dāng)前的位置。通過(guò)反復(fù)迭代，即可得到行人的運(yùn)動(dòng)軌跡。因此，當(dāng)vlc定位發(fā)生信號(hào)遮擋時(shí)(無(wú)法輸出位置結(jié)果)，融合系統(tǒng)會(huì)采用pdr模塊的定位結(jié)果進(jìn)行彌補(bǔ)。

單濾波器結(jié)構(gòu)姿態(tài)和位置誤差間相互影響：由于濾波器中的系統(tǒng)模型利用了慣導(dǎo)定位的位置預(yù)測(cè)公式。而在慣導(dǎo)中，速度誤差矢量的狀態(tài)轉(zhuǎn)移包含姿態(tài)變量，位置誤差矢量的狀態(tài)轉(zhuǎn)移又和速度相關(guān)。因此，當(dāng)采用單濾波器的架構(gòu)時(shí)，姿態(tài)和位置信息同時(shí)預(yù)測(cè)估計(jì)，必然會(huì)造成姿態(tài)的預(yù)測(cè)估計(jì)誤差影響位置信息的預(yù)測(cè)。所以本發(fā)明通過(guò)采用雙濾波器架構(gòu)將姿態(tài)和位置的預(yù)測(cè)估計(jì)過(guò)程解耦，避免兩者之間相互影響。