光源編碼+PDR組合的室內(nèi)行人定位方法

閆 偉，牛小驥，曠 儉

(武漢大學(xué)衛(wèi)星導(dǎo)航定位技術(shù)研究中心，湖北 武漢 430079)

在室外環(huán)境中，全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system，GNSS)能夠提供分米級甚至厘米級精度的定位；但在室內(nèi)環(huán)境中，GNSS及相關(guān)技術(shù)無法提供高精度的定位服務(wù)[1]。針對室內(nèi)高精度定位的需求，國內(nèi)外研究機(jī)構(gòu)主要利用視覺、藍(lán)牙天線陣列、超寬帶、音頻、光源[1-2]等方法實(shí)現(xiàn)定位。其中，隨著基礎(chǔ)照明設(shè)施的普及，光源定位技術(shù)在室內(nèi)迅速發(fā)展[3]。

目前，光源定位技術(shù)主要包括基于LED燈信號特征提取的定位技術(shù)[4]、基于光強(qiáng)模型的定位技術(shù)[5]、基于紅外光視覺捕捉的定位技術(shù)[6]、基于特殊光源的定位技術(shù)[7]等。光源編碼定位是一種基于特殊光源的定位技術(shù)：光源基站進(jìn)行編碼設(shè)計(jì)，使地面不同位置具有不同的光信號；接收端對光信號進(jìn)行解碼后，準(zhǔn)確計(jì)算自身位置，其定位精度可以達(dá)到分米級甚至厘米級[2-3]。但單獨(dú)利用光源編碼定位可能會遇到以下問題：①在非視距(non line of sight，NLOS)環(huán)境中，接收端被障礙物或行人部分遮擋，接收到的光強(qiáng)降低，噪聲增大，影響位置精度；②接收端將反射光信號誤判為直射光信號，造成定位粗差；③接收端因某些原因，若被完全遮擋，導(dǎo)致定位失敗。這些問題影響了光源編碼定位在室內(nèi)場景中的發(fā)展。

近年來，微機(jī)電系統(tǒng)(micro-electro-mechanical system，MEMS)快速發(fā)展，智能手機(jī)都帶有MEMS傳感器[8]?；谥悄苁謾C(jī)的行人航跡推算(pedestrian dead reckoning，PDR)技術(shù)利用MEMS慣性測量單元(inertial measurement unit，IMU)，獨(dú)立、連續(xù)地推算行人位置，短期精度高。但MEMS傳感器誤差較大，且導(dǎo)航算法中存在積分運(yùn)算，故PDR的位置誤差會隨時(shí)間迅速累積[9]。

針對上述問題，本文基于智能手機(jī)平臺，運(yùn)用擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)將光源編碼位置與PDR位置相融合。光源編碼+PDR組合既能利用光源編碼定位結(jié)果實(shí)時(shí)修正IMU誤差參數(shù)，控制PDR結(jié)果的發(fā)散；又能在光源編碼定位無效時(shí)，利用PDR的短期結(jié)果提供較高精度的導(dǎo)航信息，從而保證定位結(jié)果的連續(xù)性和準(zhǔn)確性。

光源編碼+PDR組合算法的完整框圖如圖1所示。

1 光源編碼定位系統(tǒng)

光源編碼定位系統(tǒng)由光源基站、接收端與相應(yīng)軟件組成。系統(tǒng)的編碼體現(xiàn)在外殼的編碼設(shè)計(jì)：將半球形外殼沿經(jīng)緯度方向均勻劃分成多塊，每一塊分別進(jìn)行開孔、閉孔的設(shè)計(jì)。開孔表示編碼位1，即某部分外殼被去除，可使光線通過；閉孔表示編碼位0，即某部分外殼未被去除，阻止光線通過。外殼沿緯度方向進(jìn)行不同的開孔、閉孔順序設(shè)計(jì)。

圖2為光源編碼定位原理示意圖。光源基站懸掛在天花板上，工作時(shí)內(nèi)部電機(jī)帶動編碼外殼旋轉(zhuǎn)，LED光源通過開孔形成光束，使地面產(chǎn)生明暗變化。接收端記錄光強(qiáng)的時(shí)間序列測量值，根據(jù)光源基站坐標(biāo)、外殼編碼參數(shù)和光信號解碼信息，判斷自身的粗略位置；之后通過小波變化降噪、優(yōu)化的編碼策略等，精確測量自身位置，達(dá)到厘米級的定位精度。具體的光源編碼基站設(shè)計(jì)和定位算法參考文獻(xiàn)[3]。

2 基于捷聯(lián)慣導(dǎo)的PDR算法

基于捷聯(lián)慣導(dǎo)的PDR算法框架如圖1所示。利用IMU數(shù)據(jù)進(jìn)行慣性導(dǎo)航系統(tǒng)(inertial navigation system，INS)機(jī)械編排，得到用戶實(shí)時(shí)的導(dǎo)航信息(三維位置、速度和姿態(tài))。為了減緩捷聯(lián)慣導(dǎo)的誤差累計(jì)速度，算法實(shí)時(shí)檢測行人運(yùn)動狀態(tài)：當(dāng)檢測到行人向前邁出一個(gè)腳步時(shí)，利用行進(jìn)的距離構(gòu)造速度觀測值進(jìn)行EKF速度更新[9]。

2.1 INS機(jī)械編排

已知物體上一時(shí)刻的導(dǎo)航信息，利用INS機(jī)械編排，對IMU數(shù)據(jù)進(jìn)行積分，可以得到當(dāng)前時(shí)刻的導(dǎo)航信息。由于智能手機(jī)IMU的傳感器誤差較大，積分后位置誤差會迅速增大，因此：①進(jìn)行機(jī)械編排時(shí)可以忽略影響極小的誤差項(xiàng)(如地球自轉(zhuǎn)引入的誤差)而不影響算法精度[9]；②需要引入其他觀測量，利用EKF進(jìn)行數(shù)據(jù)融合以提高精度。

導(dǎo)航坐標(biāo)系(n系)下INS機(jī)械編排方程的離散化形式[10]如下

(1)

2.2 EKF模型

EKF狀態(tài)方程、量測方程的離散化形式見式(2)，狀態(tài)向量δxk、狀態(tài)轉(zhuǎn)移矩陣Φk/k-1見式(3)、式(4)[11]。

(2)

(3)

(4)

式中，δpn、δvn、φ分別為位置、速度、姿態(tài)的誤差。

根據(jù)EKF狀態(tài)方程和量測方程，利用式(5)進(jìn)行狀態(tài)更新和量測更新，得到行人的導(dǎo)航信息[1]。

(5)

2.3 行走速度更新

本文針對行人運(yùn)動特征，進(jìn)行如下合理假設(shè)：多數(shù)時(shí)候行人面朝的方向就是行走的方向[12]；短時(shí)間內(nèi)行人的速度是一個(gè)常值[13]。即在一步周期內(nèi)，只存在恒定的前向速度，側(cè)向和垂向的速度幾乎為0?；谠摷僭O(shè)計(jì)算行走速度，行人坐標(biāo)系(l系)下的速度觀測值[13]如下

(6)

式中，vl為l系下的速度真值；nvl為觀測噪聲；SL為步長；Δt為走一步的時(shí)間。

n系下速度真值vn與vl存在以下關(guān)系

(7)

(8)

(9)

利用EKF融合行走速度，將l系速度預(yù)測值與速度觀測值之差作為量測信息，量測方程如下

(10)

3 光源編碼+PDR組合算法

3.1 光源編碼位置更新

光源編碼定位系統(tǒng)可以提供二維位置信息，將PDR位置與光源編碼位置的差值作為量測信息，得到量測方程如式(11)—式(13)。

δzk,P-l=Hk,P-lδxk+vk

(11)

(12)

(13)

3.2 抗差處理

在實(shí)際應(yīng)用中，光源編碼定位偶爾會出現(xiàn)遠(yuǎn)超限定誤差的情況。光源編碼+PDR組合算法利用PDR推算結(jié)果短期精度高的特點(diǎn)，使用PDR位置預(yù)測結(jié)果探測光源編碼定位結(jié)果中的粗差，進(jìn)行剔除或降權(quán)處理以起到抗差的目的[14]。

(14)

(15)

(16)

4 試驗(yàn)與結(jié)果分析

4.1 算法參數(shù)與試驗(yàn)設(shè)計(jì)

為了驗(yàn)證光源編碼+PDR組合算法的有效性，本文進(jìn)行了4組試驗(yàn)。測試手機(jī)為華為P10 Plus，該手機(jī)IMU芯片型號為InvenSense公司的ICM-20690。光源編碼定位系統(tǒng)由中國科學(xué)院深圳先進(jìn)技術(shù)研究院提供[3]。設(shè)置光源編碼定位頻率為1 Hz，即每1 s從光源編碼定位系統(tǒng)獲取一次位置與標(biāo)志位(1表示位置數(shù)據(jù)有效，0表示無效)；設(shè)置IMU采樣率為50 Hz。本文利用光源編碼+PDR組合反向平滑[10]的位置結(jié)果作為軌跡的參考真值。

試驗(yàn)場地為武漢大學(xué)詩琳通地球空間信息技術(shù)協(xié)同創(chuàng)新中心，光源編碼基站布置在場地側(cè)上方，可以完整覆蓋試驗(yàn)場地，圖3為基站實(shí)物圖。試驗(yàn)路徑為5.4 m×5.4 m的矩形，基站、參考軌跡與行走方向如圖4所示，(0,0)為試驗(yàn)起點(diǎn)。4組試驗(yàn)分別沿預(yù)定軌跡行走5圈、3圈、3圈、3圈，測試人員端平手機(jī)向前行走。試驗(yàn)軌跡包含了正對、側(cè)對、背對光源基站的軌跡，可以有效考察LOS與NLOS環(huán)境中光源編碼+PDR組合算法的有效性。

4.2 光源編碼定位結(jié)果分析

4組試驗(yàn)光源編碼定位結(jié)果的標(biāo)志位如圖5所示。每組試驗(yàn)都存在標(biāo)志位為0的數(shù)據(jù)，即在試驗(yàn)過程中，由于行人本身的遮擋或其他原因，光源編碼定位系統(tǒng)會出現(xiàn)定位失敗的情況，特別是第3組試驗(yàn)，有超過20個(gè)點(diǎn)定位失敗。

圖6給出了標(biāo)志位為1的光源編碼定位結(jié)果。第2、3、4組試驗(yàn)在背對光源基站的區(qū)域，都出現(xiàn)了定位誤差大于2 m的粗差點(diǎn)。但對于正常定位點(diǎn)，光源編碼定位的精度在1 dm以內(nèi)，與參考軌跡基本完全符合。

表1統(tǒng)計(jì)了4組試驗(yàn)的光源編碼定位結(jié)果。3組試驗(yàn)的正常定位點(diǎn)占80%以上，但存在少數(shù)定位誤差大的點(diǎn)；第3組試驗(yàn)有超過1/3的點(diǎn)標(biāo)志位為0，正常定位結(jié)果僅占60%。

表1 光源編碼定位結(jié)果統(tǒng)計(jì)

經(jīng)過以上分析，在實(shí)際應(yīng)用中，單獨(dú)依靠光源編碼定位系統(tǒng)進(jìn)行室內(nèi)行人定位，將無法獲得穩(wěn)定可靠的定位結(jié)果。

4.3 光源編碼+PDR組合定位結(jié)果分析

圖7為光源編碼+PDR組合的定位結(jié)果，組合結(jié)果與參考軌跡基本完全符合，可以反映出行人真實(shí)的導(dǎo)航信息。在光源編碼定位結(jié)果良好時(shí)，光源編碼+PDR組合的定位精度與光源編碼定位精度相近，且定位結(jié)果連續(xù)穩(wěn)定；當(dāng)光源編碼定位出現(xiàn)粗差點(diǎn)時(shí)，如第2、3、4組試驗(yàn)，光源編碼+PDR組合的定位結(jié)果仍然良好，未出現(xiàn)異常結(jié)果；當(dāng)光源編碼正常定位點(diǎn)明顯減少、定位結(jié)果不連續(xù)性時(shí)，如圖6中參考軌跡西南角的光源編碼定位點(diǎn)比較稀疏，光源編碼+PDR組合可以提供連續(xù)的導(dǎo)航信息，保證系統(tǒng)的可靠性。

表2給出了光源編碼+PDR組合定位的誤差統(tǒng)計(jì)。光源編碼+PDR組合定位的水平誤差均方根值在1 dm以內(nèi)，均值在8 cm以內(nèi)，最大值在3 dm左右。EKF在沒有外部信息輔助的情況下，解算結(jié)果必然是逐漸發(fā)散的；結(jié)合圖5(c)，即便光源編碼定位系統(tǒng)在5 s內(nèi)沒有提供可用的位置結(jié)果，光源編碼+PDR組合算法也僅僅產(chǎn)生了3 dm左右的誤差，這也進(jìn)一步體現(xiàn)了本文算法的有效性。

表2 光源編碼+PDR組合定位誤差統(tǒng)計(jì) cm

經(jīng)過以上分析，在理想情況下，光源編碼+PDR組合和光源編碼定位都能達(dá)到分米級乃至厘米級的定位精度，定位結(jié)果連續(xù)可靠，能反映行人真實(shí)的位置情況；當(dāng)光源編碼定位系統(tǒng)定位不連續(xù)或定位性能下降時(shí)，單獨(dú)依靠光源編碼定位不能很好地滿足實(shí)際需要，但光源編碼+PDR組合仍能保持1 dm的定位精度，提供連續(xù)的導(dǎo)航信息。整體而言，光源編碼+PDR組合更適合高精度的室內(nèi)行人定位。

5 結(jié) 語

本文針對光源編碼定位不連續(xù)、易受遮擋導(dǎo)致定位性能下降等問題，以基于捷聯(lián)慣導(dǎo)的PDR算法為基礎(chǔ)，實(shí)現(xiàn)了基于EKF的光源編碼+PDR組合算法，并運(yùn)用新息濾波進(jìn)行抗差處理。試驗(yàn)結(jié)果表明，光源編碼+PDR組合在室內(nèi)環(huán)境中能實(shí)現(xiàn)高精度的連續(xù)定位，具有更高的穩(wěn)定性和可靠性，適用于高精度室內(nèi)行人定位。