基于GNSS/SLAM組合高精度室內(nèi)動(dòng)態(tài)目標(biāo)定位法

張曉娜，張淑芳

(大連海事大學(xué)信息科學(xué)技術(shù)學(xué)院，遼寧大連116026)

1 引言

在大多數(shù)應(yīng)用場(chǎng)景，GPS定位系統(tǒng)雖然在室外條件下有著較高的定位精度，但復(fù)雜的室內(nèi)環(huán)境產(chǎn)生的信號(hào)易受建筑物等障礙遮擋、多徑傳播等效應(yīng)使得GPS難以滿足在室內(nèi)的定位需求[1-3]。因此，發(fā)展導(dǎo)航及目標(biāo)定位信息的相關(guān)技術(shù)，對(duì)于如何實(shí)時(shí)、穩(wěn)定、高效的提供精確室內(nèi)的位置信息具有重要意義[4]。

石琴琴[5]等人提出基于RSSI測(cè)距的室內(nèi)動(dòng)態(tài)目標(biāo)定位方法，通過信標(biāo)間的真實(shí)距離和RSSI衰減值對(duì)RSSI測(cè)距模型參數(shù)進(jìn)行反向?qū)崟r(shí)修正，通過牛頓迭代法獲取室內(nèi)動(dòng)態(tài)目標(biāo)的定位，該方法沒有對(duì)位置信息周跳值進(jìn)行探測(cè)與修復(fù)，導(dǎo)致位置誤差較大。周牧[6]等人提出基于信號(hào)分布混合假設(shè)檢驗(yàn)的室內(nèi)動(dòng)態(tài)目標(biāo)定位方法，通過混合Mann-Whitney U檢驗(yàn)/T檢驗(yàn)方法構(gòu)造匹配參考點(diǎn)集合，定位目標(biāo)區(qū)域，該方法不能對(duì)各個(gè)頻率上的小周跳進(jìn)行高精度修復(fù)，導(dǎo)致姿態(tài)誤差較為嚴(yán)重。王安然[7]等人提出基于卡爾曼濾波預(yù)估的室內(nèi)動(dòng)態(tài)目標(biāo)定位方法，采用卡爾曼濾波方法對(duì)動(dòng)態(tài)目標(biāo)坐標(biāo)進(jìn)行估計(jì)，利用分區(qū)定位方式進(jìn)行定位，該方法在探測(cè)與修復(fù)中噪聲波動(dòng)范圍較大，導(dǎo)致速度不穩(wěn)定，即定位精準(zhǔn)度較低。

為了解決存在的問題，提出基于GNSS/SLAM組合高精度室內(nèi)動(dòng)態(tài)目標(biāo)定位方法，通過對(duì)室內(nèi)動(dòng)態(tài)觀測(cè)數(shù)據(jù)進(jìn)行預(yù)處理，探測(cè)出不同頻率的周跳值，對(duì)大周跳、小周跳均進(jìn)行修復(fù)，獲得高質(zhì)量數(shù)據(jù)及準(zhǔn)確定位，GNSS/SLAM組合導(dǎo)航具有定位精度高，穩(wěn)定性強(qiáng)等優(yōu)勢(shì)，對(duì)室內(nèi)定位及無(wú)線通信技術(shù)的發(fā)展具有廣闊的應(yīng)用前景。

2 室內(nèi)動(dòng)態(tài)目標(biāo)定位預(yù)處理

2.1 室內(nèi)動(dòng)態(tài)目標(biāo)定位去噪處理

信號(hào)處理目前已成為現(xiàn)代科學(xué)技術(shù)中不可或缺的一部分，而在信號(hào)處理的許多應(yīng)用中，信號(hào)傳輸過程中經(jīng)常受到環(huán)境的影響，導(dǎo)致初始信號(hào)變成含噪信號(hào)，因此，對(duì)取得的信號(hào)進(jìn)行去噪是信號(hào)處理的關(guān)鍵[8]?；贕NSS/SLAM組合高精度室內(nèi)動(dòng)態(tài)目標(biāo)定位方法，通過EMD-閾值處理組合進(jìn)行去噪，具體步驟如下：

假設(shè)原信號(hào)為x(t)，高斯白噪聲為s(t)，得到含噪信號(hào)為y(t)，通過對(duì)含噪信號(hào)進(jìn)行去噪處理獲得距離原始信號(hào)最為接近的去噪信號(hào)x(t)，即

(1)

基于EMD對(duì)含噪信號(hào)進(jìn)行分解，得到的分量由高到低進(jìn)行排列，一般情況下，噪聲能量大部分存在于高頻分量中，低頻分量中較少，因此低通濾波去噪方法需舍棄高頻分量階數(shù)，重構(gòu)低頻分量，實(shí)現(xiàn)去噪目的[9]。從分解后的分量中提取噪聲能量和有用信號(hào)作為主要模態(tài)分界點(diǎn)。運(yùn)用連續(xù)均方根誤差準(zhǔn)則對(duì)IMF分量噪聲能量密度進(jìn)行計(jì)算，獲得有用信號(hào)分量分界點(diǎn)與噪聲分量，IMFk表示為有用信號(hào)分量分界點(diǎn)，具體步驟如下

(2)

式中，n為分解階數(shù)，N為信號(hào)長(zhǎng)度。IMF分量噪聲能量密度的變化曲線可通過連續(xù)均方誤差準(zhǔn)則獲得，在選取對(duì)于噪聲信號(hào)與期望信號(hào)均能起主導(dǎo)作用的位置時(shí)，應(yīng)選擇曲線局極小值位置作為分界點(diǎn)，并找到對(duì)應(yīng)的噪聲能量分布突變每個(gè)分解點(diǎn)的IMF分量，分解點(diǎn)k計(jì)算公式如下

(3)

當(dāng)有用信號(hào)能量起主導(dǎo)作用時(shí)，此時(shí)分界點(diǎn)的噪聲能量成為全局最小值前的局部極小值。當(dāng)在全局極小值前存在局部極小值時(shí)，k的表達(dá)式如下

(4)

(5)

為了有效保留部分有用信號(hào)，在閾值處理及選取閾值函數(shù)時(shí)，假設(shè)特定閾值為λ。據(jù)小波閾值去噪方法可知，設(shè)置閾值的規(guī)則主要包括極值閾值準(zhǔn)則、啟發(fā)式閾值準(zhǔn)則、固定閾值原則、無(wú)偏風(fēng)險(xiǎn)估計(jì)閾值準(zhǔn)則。固定閾值相比之下比較簡(jiǎn)單，通常會(huì)被優(yōu)先選用，其計(jì)算公式為

(6)

式中，N為信號(hào)長(zhǎng)度，λi為固定閾值，σe為噪聲標(biāo)準(zhǔn)差，Wi，j為信號(hào)分解后的分量系數(shù)，噪聲標(biāo)準(zhǔn)差可采用魯棒估計(jì)進(jìn)行計(jì)算。

小波去噪閾值處理一般會(huì)使用硬閾值函數(shù)與軟閾值函數(shù)，通常軟閾值函數(shù)效果更好一些[11]。軟閾值去噪時(shí)，對(duì)于小于閾值系數(shù)的分量進(jìn)行歸零處理，對(duì)于大于閾值λ的系數(shù)Wλ則選取最初系數(shù)與閾值的差額進(jìn)行計(jì)算，經(jīng)過軟閾值處理后，整體效果平展性與持續(xù)性較好，表達(dá)式如下

(7)

(8)

基于GNSS/SLAM組合高精度室內(nèi)動(dòng)態(tài)目標(biāo)定位方法，將EMD-閾值處理組合進(jìn)行去噪，可高效的剔除大部分存在于高頻分量的噪聲能量，并使分解中的小部分噪聲也隨之清除，使去噪效果更佳，去噪目的更準(zhǔn)確。

2.2 周跳探測(cè)與修復(fù)

周跳探測(cè)與修復(fù)是基于GNSS/SLAM組合高精度室內(nèi)動(dòng)態(tài)目標(biāo)定位方法觀測(cè)數(shù)據(jù)預(yù)處理的核心[12]。具體步驟如下：

1)φ(0，1，-1)組合觀測(cè)值周跳的探測(cè)與修復(fù)

設(shè)φ(0，1，-1)為無(wú)周跳的載波相位組合觀測(cè)值，利用式(9)、(10)可探測(cè)與修復(fù)φ(0，1，-1)組合觀測(cè)值的周跳，表達(dá)式如下

(9)

2)φ(1，-3，2)組合觀測(cè)值周跳的探測(cè)與修復(fù)

當(dāng)φ(0，1，-1)載波相位組合觀測(cè)值周跳實(shí)現(xiàn)探測(cè)與修復(fù)后，根據(jù)φ(1，-3，2)組合觀測(cè)值和無(wú)周跳φ(0，1，-1)載波相位組合觀測(cè)值獲得幾何無(wú)關(guān)觀測(cè)值，通過下式對(duì)φ(1，-3，2)相應(yīng)的周跳進(jìn)行探測(cè)與修復(fù)

S(1，-3，2)λ(1，-3，1)=I1+Δφ(0，1，-1)λ(0，1，-1)-Δφ(1，-3，2)λ(1，-3，2)

(10)

式中，Δ描述的是歷元差分，S(1，-3，2)為組合觀測(cè)值φ(1，-3，2)的周跳，I1為歷元之間對(duì)應(yīng)幾何無(wú)關(guān)組合觀測(cè)值的變化程度。

在數(shù)據(jù)處理時(shí)，線性擬合與高次差對(duì)φ(0，1，-1)組合觀測(cè)值的周跳進(jìn)行探測(cè)與修復(fù)。

3)φ(-3，1，3)組合觀測(cè)值周跳的探測(cè)與修復(fù)

由1)、2)步獲得無(wú)周跳的φ(0，1，-1)與φ(1，-3，2)組合觀測(cè)值，可形成無(wú)周跳且觀測(cè)值噪聲較小的φ(1，0，-1)組合觀測(cè)值，即

φ(1，0，-1)=3φ(0，1，-1)+φ(1，-3，2)

(11)

利用該組合與φ(-3，1，3)組合觀測(cè)值構(gòu)建對(duì)應(yīng)的組合觀測(cè)值，通過上述步驟2)的方法對(duì)φ(-3，1，3)組合觀測(cè)值的周跳進(jìn)行探測(cè)與修復(fù)，公式如下

S(-3，1，3)λ(-3，1，3)=I2+Δφ(1，0，-1)λ(1，0，-1)-Δφ(-3，1，3)λ(-3，1，3)

(12)

4)初始載波觀測(cè)值的周跳

上述過程結(jié)束后，如前期組合觀測(cè)值均無(wú)周跳發(fā)生，那么原始載波相應(yīng)的觀測(cè)值也無(wú)周跳發(fā)生。但如有組合觀測(cè)值發(fā)生周跳，那么可通過下式進(jìn)行計(jì)算周調(diào)值[13]。

(13)

周跳探測(cè)與修復(fù)在高精度數(shù)據(jù)處理中一直都是比較重要的環(huán)節(jié)，對(duì)于高采樣率的非差相位觀測(cè)數(shù)據(jù)能夠快速和準(zhǔn)確地探測(cè)及修復(fù)周跳，為高精度室內(nèi)定位提供了較好的數(shù)據(jù)質(zhì)量控制。

3 高精度室內(nèi)動(dòng)態(tài)目標(biāo)定位方法

SLAM為同步定位與制圖技術(shù)，GNSS為優(yōu)越的導(dǎo)航技術(shù)，普遍被各個(gè)領(lǐng)域所應(yīng)用。在部分場(chǎng)景中，由于傳播環(huán)境復(fù)雜，傳播通道受到干擾，GNSS信號(hào)無(wú)法被接收，其導(dǎo)航功能失效，因此GNSS/SLAM組合可以獲取高精度的環(huán)境特征，提供的準(zhǔn)確的定位信息[14]。在GNSS SLAM組合算法中，對(duì)于參考坐標(biāo)系下激光雷達(dá)坐標(biāo)系的姿態(tài)角、傳感器及已觀測(cè)到的全部地標(biāo)點(diǎn)的位置向量，均為k時(shí)刻的系統(tǒng)狀態(tài)向量，即表示為：

(14)

N(Xk/k，Pk/k)為系統(tǒng)狀態(tài)向量的后驗(yàn)概率密度。期望均值與協(xié)方差分別表示為

(15)

如GNSS/SLAM組合觀測(cè)到新的地標(biāo)點(diǎn)，則將新地標(biāo)點(diǎn)的位置向量增加到系統(tǒng)狀態(tài)向量中，詳細(xì)流程如下

首先進(jìn)行時(shí)間更新，具體方程如下

(16)

式中

(17)

式中，Q為過程噪聲矩陣。

其次進(jìn)行數(shù)據(jù)關(guān)聯(lián)，將新觀測(cè)的地標(biāo)點(diǎn)與初始地標(biāo)點(diǎn)進(jìn)行匹配，通過已存在的地標(biāo)觀測(cè)值進(jìn)行計(jì)算

k|k=k|k-1+Kkμk

Pk|k=(I-KkHk)Pk|k-1

(18)

其中

(19)

式中，Ra為已存在地標(biāo)的觀測(cè)噪聲矩陣，za，k為已存在的地標(biāo)點(diǎn)，zb，k為新觀測(cè)到的地標(biāo)點(diǎn)。系統(tǒng)狀態(tài)增廣為

(20)

式中，mb為新觀測(cè)到地標(biāo)點(diǎn)距離。將新觀測(cè)到的地標(biāo)點(diǎn)位置向量增加到系統(tǒng)狀態(tài)向量中。

從上述過程中看出，GNSS SLAM組合相對(duì)于傳統(tǒng)的GNSS增加了數(shù)據(jù)關(guān)聯(lián)和狀態(tài)增廣的過程，數(shù)據(jù)關(guān)聯(lián)是GNSS SLAM組合中重要一步，另外特征提取也是GNSS SLAM組合的前提條件[15]。

4 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證基于GNSS SLAM組合高精度室內(nèi)動(dòng)態(tài)目標(biāo)定位方法的整體有效性，進(jìn)行相關(guān)實(shí)驗(yàn)。本次實(shí)驗(yàn)在開發(fā)環(huán)境(Graphic User Interface Design Environment GUIDE)下完成開發(fā)。UWB 測(cè)距性能可能受到外部環(huán)境和系統(tǒng)硬件等因素影響，因此在進(jìn)行目標(biāo)定位之前首先需要對(duì) UWB 測(cè)距進(jìn)行誤差修正。

圖1 實(shí)驗(yàn)的硬件平臺(tái)

采用基于GNSS/SLAM組合高精度室內(nèi)動(dòng)態(tài)目標(biāo)定位方法(方法1)、基于RSSI測(cè)距的室內(nèi)動(dòng)態(tài)目標(biāo)定位方法(方法2)、基于信號(hào)分布混合假設(shè)檢驗(yàn)的室內(nèi)動(dòng)態(tài)目標(biāo)定位方法(方法3)進(jìn)行測(cè)試。

測(cè)試位于一間實(shí)驗(yàn)室與一間辦公室內(nèi)，內(nèi)有桌椅，設(shè)備等，其室內(nèi)布局圖如圖下圖2所示。

圖2 實(shí)驗(yàn)室環(huán)境示意圖

假設(shè)測(cè)量得到的第k個(gè)動(dòng)態(tài)目標(biāo)坐標(biāo)為Mk(xk，yk)，真實(shí)軌跡所有點(diǎn)坐標(biāo)的集合為Mr(xr，yr)，則定位誤差de表示為：

de=min|Mk-Mr|

(21)

具體測(cè)試結(jié)果如圖3所示。

圖3 定位誤差

由圖3可知，在位置誤差測(cè)試中方法1的誤差更接近于0，證明此方法的定位更接近動(dòng)態(tài)目標(biāo)，位置更精準(zhǔn)。方法2與方法3的由于緯度誤差、經(jīng)度誤差、高度誤差浮動(dòng)較大，因此位置誤差較大，即定位精準(zhǔn)度較低。因?yàn)榉椒?將觀測(cè)數(shù)據(jù)進(jìn)行周跳探測(cè)與修復(fù)，不管對(duì)于大周跳還是小周跳，均能探測(cè)出各個(gè)頻率上發(fā)生的周跳，能夠以一定的精度進(jìn)行修復(fù)，獲得高質(zhì)量數(shù)據(jù)，使得位置誤差更接近于0，即準(zhǔn)確的實(shí)現(xiàn)了室內(nèi)目標(biāo)定位。

在實(shí)驗(yàn)中，一個(gè)測(cè)試人員沿著一個(gè)長(zhǎng)方形的軌跡勻速移動(dòng)。動(dòng)態(tài)圖目標(biāo)跟蹤測(cè)量結(jié)果如圖4所示。

圖4 動(dòng)態(tài)圖目標(biāo)跟蹤測(cè)量結(jié)果

動(dòng)態(tài)目標(biāo)測(cè)量的誤差較小。主要原因在于本文方法對(duì)觀測(cè)數(shù)據(jù)進(jìn)行周跳探測(cè)與修復(fù)，探測(cè)與修復(fù)各個(gè)頻率上發(fā)生的周跳，準(zhǔn)確的實(shí)現(xiàn)室內(nèi)目標(biāo)定位。

5 結(jié)束語(yǔ)

動(dòng)態(tài)背景下的運(yùn)動(dòng)目標(biāo)檢測(cè)與定位，當(dāng)前方法在周跳探測(cè)與修復(fù)中對(duì)不同頻率區(qū)域無(wú)法進(jìn)行高精度修復(fù)，難以獲取高質(zhì)量數(shù)據(jù)，導(dǎo)致位置及姿態(tài)誤差較大、速度不穩(wěn)定等現(xiàn)象。即室內(nèi)動(dòng)態(tài)目標(biāo)定位的精準(zhǔn)度較低，對(duì)此問題提出了基于GNSS/SLAM組合高精度室內(nèi)動(dòng)態(tài)目標(biāo)定位方法，對(duì)室內(nèi)動(dòng)態(tài)觀測(cè)數(shù)據(jù)進(jìn)行預(yù)處理，探測(cè)出各個(gè)頻率上發(fā)生的周跳，高精度進(jìn)行修復(fù)，獲得高質(zhì)量數(shù)據(jù)，精準(zhǔn)定位室內(nèi)動(dòng)態(tài)目標(biāo)，該方法有效地解決了當(dāng)前方法中存在的問題，為高精度室內(nèi)動(dòng)態(tài)目標(biāo)定位的實(shí)現(xiàn)提供基礎(chǔ)。