朱建良, 王立雅, 薄煜明

(南京理工大學(xué) 自動化學(xué)院, 江蘇 南京 210094)

0 引言

隨著半導(dǎo)體設(shè)計和制造技術(shù)的飛速發(fā)展,采用微機電系統(tǒng)技術(shù)的傳感器的體積和功耗獲得了顯著下降。目前,智能手機嵌入了各種傳感器,如加速度計、陀螺儀、視覺攝像頭、磁傳感器、Wi-Fi、藍牙、全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)接收機等[1-2]。為了滿足行人和單兵等室內(nèi)外定位、導(dǎo)航需求,基于智能手機的多傳感器信息構(gòu)建無縫導(dǎo)航和定位系統(tǒng)已經(jīng)成為研究熱點[3-4]。在這些傳感器中,GNSS接收機能夠在戶外信號良好的場景下為用戶提供精確的定位和導(dǎo)航信息[5-6]。在開闊的天空環(huán)境下,視野內(nèi)擁有足夠多的GNSS衛(wèi)星可以計算出高精度位置信息。然而,在信號具有挑戰(zhàn)性的環(huán)境中,GNSS信號可能會因環(huán)境而惡化,例如隧道中的GNSS信號阻塞、城市地區(qū)的多路徑信號和非視距(NLOS)信號等[7-8]。信號阻塞導(dǎo)致GNSS接收機無法定位,多路徑信號和NLOS信號會導(dǎo)致GNSS定位精度下降。因此,在構(gòu)建基于智能手機的行人/單兵無縫定位系統(tǒng)時,基于GNSS/慣性導(dǎo)航系統(tǒng)(INS)的組合導(dǎo)航系統(tǒng)是提供更可靠導(dǎo)航解決方案的基石[9-10]。

智能手機中嵌入了帶有三軸加速度計和陀螺儀的慣性測量單元(IMU)來測量智能手機的姿態(tài)[10-12]?？紤]到體積、成本和功耗,嵌入智能手機的IMU通常采用MEMS技術(shù)制造。智能手機的MEMS IMU測量信息質(zhì)量低,經(jīng)典的捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)算法不適用于基于智能手機的行人導(dǎo)航[10-12]?；谛腥瞬叫胁綉B(tài)的行人航位推算(PDR)算法是一種更可靠的行人定位和導(dǎo)航方法[12]。PDR通過處理傳感器量測信息來更新行人位置,而無需接收來自外部的信號,在室內(nèi)外環(huán)境下都可以提供定位信息;換言之,PDR是一個獨立的定位系統(tǒng)。然而,由于IMU量測信息復(fù)雜的噪聲和航向角誤差,其位置精度隨著行人行走距離的增加而降低。因此,將互補的GNSS和PDR組合在一起是生成更加可靠的行人導(dǎo)航和定位信息的可行方法[12-14]。為提高GNSS/PDR組合導(dǎo)航系統(tǒng)性能,Ye等[15]提出一種GNSS/PDR/Beacon組合導(dǎo)航方法,當(dāng)GNSS信號較差時,利用Beacon信號和PDR構(gòu)建組合導(dǎo)航系統(tǒng),抑制PDR誤差。該方法可以有效地提升無GNSS下導(dǎo)航定位精度。Hsu等[16]提出一種3D地圖輔助GNSS/PDR組合導(dǎo)航方法,利用3D地圖輔助GNSS和PDR組合提高系統(tǒng)在城市復(fù)雜環(huán)境下的導(dǎo)航和定位精度。該方法可以有效地減輕NLOS信號等對GNSS/PDR組合導(dǎo)航系統(tǒng)精度的影響,通過3D地圖輔助可以識別NLOS信號等,提高整個系統(tǒng)在復(fù)雜城市環(huán)境的定位精度。另外,Rehman等[17]提出一種GNSS/PDR航向聯(lián)合估計方法,從提高系統(tǒng)航向角精度方面提高整個系統(tǒng)的導(dǎo)航和定位精度。航向角估計是PDR算法的核心,但是該方法并不能解決智能手機和人之間的固定航向偏差帶來的定位誤差問題?？傊?目前GNSS/PDR研究主要是集中在提高步長估計精度、提高步態(tài)檢測正確率、提高航向角估計精度和改進數(shù)據(jù)融合方面[17]。目前GNSS/PDR組合主要建立在以卡爾曼濾波為數(shù)據(jù)融合算法的基礎(chǔ)上。在GNSS/PDR組合導(dǎo)航系統(tǒng)中行人軌跡是連續(xù)的,充分利用歷史信息可以有效地提高行人位置估計精度。

為進一步提高GNSS/PDR組合導(dǎo)航系統(tǒng)性能,本文從數(shù)據(jù)融合的角度出發(fā),提出一種基于因子圖優(yōu)化(FGO)GNSS/PDR組合導(dǎo)航方法。本文主要研究內(nèi)容如下:

1) 針對行人導(dǎo)航GNSS或者PDR信息缺失時導(dǎo)航誤差快速發(fā)散的問題,提出用圖論法表示行人位置、GNSS量測信息等之間的相互約束關(guān)系的解決方法,基于圖的框架還能夠支持“即插即用”,為GNSS/PDR融合提供了一個通用并且可靈活配置的框架。

2) 用圖優(yōu)化算法代替常規(guī)的卡爾曼濾波算法完成GNSS和PDR的信息融合并估計定位信息。在FGO-GNSS/PDR中,所有過去的狀態(tài)都作為未知量,所有來自GNSS的位置信息都作為狀態(tài)的約束,通過最小化整體代價函數(shù)獲取狀態(tài)的最優(yōu)解。相對卡爾曼濾波,FGO-GNSS/PDR方法可以充分利用歷史軌跡信息和狀態(tài)之間的相互聯(lián)系,提高位置估計精度。

最后本文利用智能手機采集實際場景數(shù)據(jù),驗證和分析了本文提出算法的有效性。

1 PDR算法

基于智能手機的PDR一種相對定位算法,通過處理手機傳感器數(shù)據(jù)估計行人步長和行走方向更新行人位置[18]。圖1為智能手機PDR原理圖。圖1中,N表示北向,E表示東向,pk和pk+1表示k和k+1時刻的位置向量。假設(shè)k時刻行人位置為pk=(xk,yk),航向為θk,從k到k+1時刻步長為SLk,k+1,則k+1時刻位置pk+1=(xk+1,yk+1),更新公式如下:

xk+1=xk+SLk,k+1·cosθk

(1)

yk+1=yk+SLk,k+1·sinθk

(2)

圖1 航位推算位置更新原理Fig.1 Principle diagram of position updating

智能手機傳感器如加速度計、陀螺儀、磁力計等數(shù)據(jù)經(jīng)過處理之后,用來識別行人是否行走,然后估計行人行走的步長。同時利用磁力計和陀螺儀等傳感器數(shù)據(jù)聯(lián)合估計行人的行走方向,通過上述公式更新位置。圖2為基于智能手機的PDR數(shù)據(jù)處理和原理框圖。

圖2 智能手機航位推算原理圖Fig.2 Principle diagram of pedestrian positioning based on smartphone function

在智能手機PDR中,通過三軸加速度計的輸出檢測步長。加速度計輸出為時序數(shù)據(jù),可以通過檢測加速度計峰值和谷值來檢測步長。這里采用三軸加速度計的矢量和ak作為輸入,計算公式如式(3)所示:

(3)

利用加速度輸出估計行人步長計算公式如下:

(4)

式中:α為常值參數(shù),通常取值在0～1之間。

2 GNSS/PDR組合導(dǎo)航

由于傳感器噪聲和行人航向誤差等,PDR結(jié)果通常隨著時間發(fā)散。智能手機GNSS在信號良好的情況下可以提供高精度定位信息,可以構(gòu)建GNSS和PDR組合導(dǎo)航系統(tǒng)。當(dāng)GNSS信號良好的情況下通過與PDR組合可以輸出穩(wěn)定的定位信息;當(dāng)GNSS無法工作時,PDR可以提供定位信息。

2.1 基于卡爾曼濾波PDR/GNSS組合導(dǎo)航方法

PDR/GNSS組合導(dǎo)航方法狀態(tài)方程以及量測方程如下:

pk+1=Φk,k+1·pk+wk+1

(5)

(6)

卡爾曼濾波更新過程如下:

(7)

(8)

(9)

(10)

(11)

2.2 基于因子圖PDR/GNSS組合導(dǎo)航方法

因子圖屬于概率圖的一種,本質(zhì)上用圖來表示各種變量或者狀態(tài)之間相互依存關(guān)系。因子圖最開始用在通信技術(shù)中的信道編碼估計,經(jīng)過不斷發(fā)展,為解決一些復(fù)雜的估計問題提供了新的方法和思路。通常,在因子圖中會將全局函數(shù)經(jīng)過分解成多個局部函數(shù)的乘積,這樣可以明顯地減少全局函數(shù)求解的復(fù)雜度。在導(dǎo)航系統(tǒng)的應(yīng)用中,因子圖本質(zhì)上表示并可視化了導(dǎo)航系統(tǒng)中的狀態(tài)和量測信息之間的聯(lián)系。

因子圖由變量節(jié)點和狀態(tài)節(jié)點組成,并通過無向邊連接。假設(shè)函數(shù)g(X1,X2,…,Xn)可以分解為如下形式:

(12)

式中:f(Sj)表示為局部函數(shù),Sj為狀態(tài)變量X的集合。為方便表示,假設(shè)g(X1,X2,…,Xn)有4個變量,式(12)可以表示為如下乘積關(guān)系:

g(X1,X2,X3,X4)=f(X1)f(X2,X4)f(X1,X3)f(X1,X3,X4)

(13)

以上乘積關(guān)系可以用圖3所示因子圖表示,其中黑色方框表示因子節(jié)點,圓形代表變量節(jié)點。上述過程表示了經(jīng)過因子圖模型將復(fù)雜函數(shù)分解為 4個局部函數(shù)乘積。

圖3 局部函數(shù)分解因子圖Fig.3 Factor graph of local function decomposition

假設(shè)一組變量(x1,…,xn)的聯(lián)合概率密度函數(shù)為f(x1,…,xn),變量之間概率關(guān)系可以寫成如下形式:

(14)

為方便描述,假設(shè)n=4,則式(14)可以寫成如下形式:

f(x1,…,x4)=f(x1)f(x2|x1)f(x3|x1,x2)f(x4|x1,x2,x3)

(15)

圖4 變量依存關(guān)系因子圖Fig.4 Factor graph of variable dependency relation

進一步地,以上關(guān)系可以用圖4所示因子圖表示。 通過2.1節(jié)中卡爾曼濾波狀態(tài)估計和更新公式可以看出,卡爾曼濾波采用遞推的方式估計和更新狀態(tài)估計。通過狀態(tài)轉(zhuǎn)移模型和量測模型可以知道,狀態(tài)之間通過狀態(tài)轉(zhuǎn)移方程相互聯(lián)系,狀態(tài)和量測信息之間通過量測觀測方程相互聯(lián)系。結(jié)合上面的因子圖描述和GNSS/PDR組合導(dǎo)航狀態(tài)和量測模型,可以利用因子圖表示GNSS/PDR組合導(dǎo)航的狀態(tài)轉(zhuǎn)移和量測信息之間的概率關(guān)系。根據(jù)貝葉斯概率準則,式(5)和式(6)中的狀態(tài)和量測信息的之間概率依存關(guān)系如下公式:

(16)

式中:變量p0∶k全局條件概率密度函數(shù)與等式右邊的似然概率密度和狀態(tài)轉(zhuǎn)移先驗概率的乘積呈正比。具體如下:

(17)

因此,狀態(tài)的最優(yōu)估計可以轉(zhuǎn)換為求取狀態(tài)變量的最大后驗概率(MAP),可以表示為

(18)

假設(shè)噪聲服從高斯分布,概率密度函數(shù)P(pi|zi)可以寫為

(19)

要獲得最大后驗概率估計,需要將下面公式最小化:

[zi-hi(pi)]TR-1[zi-hi(pi)]

(20)

根據(jù)式(16)描述的概率依存關(guān)系,采用因子圖描述和表示GNSS/PDR組合導(dǎo)航中的狀態(tài)和量測信息之間的關(guān)系。GNSS/PDR組合導(dǎo)航因子圖如圖5所示,GNSS因子和PDR因子共同作為狀態(tài)的約束信息。結(jié)合式(21)和GNSS/PDR組合導(dǎo)航模型,狀態(tài)的最優(yōu)估計可以轉(zhuǎn)化成為求取下面代價函數(shù)的最小值。

圖5 FGO-GNSS/PDR組合導(dǎo)航方法Fig.5 FGO-GNSS/PDR integrated navigation method

狀態(tài)代價函數(shù)如下:

(21)

量測代價函數(shù)如下:

(22)

總的代價函數(shù)如下:

(23)

在卡爾曼濾波中,估計pk+1時默認pk是已知的,事實上pk是存在誤差的并且會對狀態(tài)估計產(chǎn)生影響。根據(jù)以上分析,狀態(tài)之間是相互聯(lián)系的。將過去所有的狀態(tài)都作為未知量,并結(jié)合根據(jù)式(16)和式(17),可以得到新的代價函數(shù)如下:

(24)

可以通過列克夸爾文算法或者高斯牛頓法等求取式(24)代價函數(shù)最小值,并獲得狀態(tài)的最優(yōu)估計。

2.3 因子圖求解

構(gòu)建代價函數(shù)之后,狀態(tài)的最優(yōu)估計可以從求取代價函數(shù)的最小值獲取。結(jié)合式(18)～式(20),式(21)和式(22)[19-21]可以寫為

(25)

(26)

整體代價函數(shù)可以寫為

(27)

為方便描述式(27)中代價函數(shù)最小值的求取過程,代價函數(shù)[19-21]可以寫成如下通用形式:

(28)

式中:ei(X)為誤差代價函數(shù);Ω為對應(yīng)誤差方差矩陣的逆矩陣。

(29)

式(27)可以展開為

(30)

綜上,可得以下公式:

(31)

將上述公式對ΔX進行求導(dǎo),并令求導(dǎo)后的公式值為0,則可得以下公式:

aΔX=-b

(32)

為穩(wěn)定的求得ΔX,通常引入?yún)?shù)λ,新的公式如下:

(a+λI)ΔX=-b

(33)

(34)

將上述過程進行不停循環(huán),直至達到設(shè)定的循環(huán)次數(shù),或者誤差小于設(shè)置的值。圖6為求解過程流程圖。

圖6 求解過程流程圖Fig.6 Flow chart of the solving process

3 實驗結(jié)果與分析

為驗證本文提出算法的性能,采集智能手機實際場景數(shù)據(jù)進行實驗。通過處理實際場景數(shù)據(jù),更有效地測試和評價本文提出的算法。實驗采用華為Mate 40 Pro手機和安卓APP采集手機傳感器數(shù)據(jù)進行處理。采集的數(shù)據(jù)包括帶時標的GNSS位置、三軸加速度計、三軸陀螺儀、三軸磁傳感器數(shù)據(jù)等。

為進一步進行數(shù)據(jù)對比,實驗中采用GNSS RTK數(shù)據(jù)作為軌跡基準計算位置誤差,實驗場景如圖7所示。GNSS RTK高精度定位系統(tǒng)是一種基于衛(wèi)星導(dǎo)航定位地基增強系統(tǒng)的高性能GNSS RTK接收機,包含GNSS接收機、GNSS雙天線、鋰電池電源等,通過4G通道利用千尋服務(wù)作為差分修正數(shù)據(jù)的輸入源。GNSS RTK系統(tǒng)定位精度可達厘米級,可作為行人導(dǎo)航的位置基準。

圖7 數(shù)據(jù)采集裝置Fig.7 Dataset collecting equipment

實驗中PDR使用的航向角為手機加速度計、陀螺儀和磁傳感器融合后的結(jié)果。采用兩種不同的方式攜帶手機采集實驗數(shù)據(jù):第1種是模擬行人行走看手機的情形,因此將手機持于胸前;第2種模擬行人手機放置在背包中,將手機固定在測試背帶上。

3.1 手持實驗

首先進行手持智能手機導(dǎo)航實驗,將手機持于胸前模仿行人邊走路邊看手機的場景。行人邊走邊采集數(shù)據(jù),同時背包中的GNSS RTK輸出的數(shù)據(jù)被筆記本電腦采集作為后續(xù)的手機定位誤差計算基準。手機加速度計能夠感受到行人行走帶來的加速度計測量值的變化。實驗數(shù)據(jù)處理時,首先處理加速度數(shù)據(jù),檢測行人步態(tài)并估計行人步長,然后根據(jù)對應(yīng)的加速度時標和GNSS信息時間同步,獲取對應(yīng)的GNSS位置信息進行融合。

圖8 手持實驗1水平位置誤差和分布Fig.8 Hand-held experimental 1 horizontal position errors and distribution

圖8為實驗中水平位置誤差結(jié)果,圖8(a)為水平定位誤差與步數(shù)的關(guān)系,圖8(b)為水平位置誤差統(tǒng)計分布。從圖8中可以看出,FGO-GNSS/PDR具有更好的定位精度。表1給出了兩種方法的水平誤差統(tǒng)計結(jié)果對比。由表1可知,與KF-GNSS/PDR相比,FGO-GNSS/PDR水平位置誤差平均值下降了43.9%,誤差均方根值下降了35.1%。另一組實驗結(jié)果如圖9所示,類似地,FGO-GNSS/PDR獲得了更好的定位結(jié)果。表2為對應(yīng)的統(tǒng)計結(jié)果分析,水平位置誤差均值下降了43.8%,誤差均方根下降了48.3%。

表1 手持實驗1水平位置誤差統(tǒng)計結(jié)果Table 1 Hand-held experiment 1 statistical results of horizontal position errors

圖9 手持實驗2水平位置誤差和分布Fig.9 Hand-held experiment 2 horizontal position errors and distribution

表2 手持實驗2水平位置誤差統(tǒng)計結(jié)果

3.2 背包實驗

除手持實驗之外,進行兩組將手機放置在背包中的實驗,手機水平放置,以保證手機的航向和行人航向盡可能一致。同樣對水平定位結(jié)果進行分析,兩組實驗水平位置誤差分別如圖10和圖11所示。圖10(a)和圖11(a)分別為水平定位誤差和步數(shù)之間的關(guān)系;圖10(b)和圖11(b)分別為對應(yīng)的水平定位誤差的統(tǒng)計結(jié)果和分布圖。與KF-GNSS/PDR方法相比,FGO-GNSS/PDR方法同樣地獲得了更好的定位精度。表3和表4給出了進一步對應(yīng)的統(tǒng)計分析結(jié)果:第1組數(shù)據(jù)中,水平位置平均誤差減少了40.5%,誤差均方根降低了36.9%;第2組數(shù)據(jù)中,水平位置平均誤差減少了56.0%,誤差均方根降低了48.9%。

圖10 背包實驗1水平位置誤差和分布Fig.10 Knapsack experiment 1 horizontal position errors and distribution

圖11 背包實驗2水平位置誤差和分布Fig.11 Knapsack experiment 2 horizontal position errors and distribution

3.3 結(jié)果分析、討論和展望

手持和背包實驗中通過與卡爾曼濾波算法的對比,誤差統(tǒng)計分析結(jié)果表明相對于卡爾曼濾波,本文提出的FGO-GNSS/PDR方法能夠有效地提升GNSS/PDR定位精度,GNSS/PDR方法平均定位誤差和定位誤差均方根值都得到了不同的降低。實驗中盡量保持了手機航向角和行人行走方向的一致性,以排除手機航向角和行人之間的誤差對結(jié)果的影響。另外,實驗中KF-GNSS/PDR和FGO-GNSS/PDR方法的噪聲協(xié)方差矩陣設(shè)置值保持一致,以保證對比的公平性。實驗結(jié)果可以很好地證明基于歷史軌跡的整體優(yōu)化方法能夠有效地降低定位誤差。

表3 背包實驗1水平位置誤差統(tǒng)計結(jié)果Table 3 Knapsack experiment 1 statistical results of horizontal position errors

表4 背包實驗2水平位置誤差統(tǒng)計結(jié)果Table 4 Knapsack experiment 2 statistical results of horizontal position errors

雖然本文基于因子圖GNSS/PDR融合算法能夠在同樣條件下比卡爾曼濾波獲得更好的定位精度,以下4點仍然值得繼續(xù)深入研究:

1) 本文中沒有考慮GNSS提供的速度,實際速度可以用來修正航向角,可以進一步考慮基于位置和速度建立新的模型。

2) GNSS位置信息在城市峽谷等環(huán)境存在一些較大的誤差,例如NLOS信號誤差、多路徑信號誤差等,基于因子圖研究完好性檢測算法有助于提升系統(tǒng)的魯棒性和整體性能。

3) 基于手機的PDR的難點在于手機的放置方式隨意性和多變性,有時并不能獲取穩(wěn)定的航向角;例如手機豎直放置在口袋中,但是此時加速度數(shù)據(jù)仍然可以用來檢測步數(shù)用來輔助GNSS位置估計。

4) 本文基于因子圖的GNSS/PDR組合導(dǎo)航可以進一步拓展成多源信息融合導(dǎo)航系統(tǒng),進一步開發(fā)室內(nèi)外無縫導(dǎo)航系統(tǒng)。

4 結(jié)論

本文提出一種基于FGO-GNSS/PDR組合導(dǎo)航方法,采用因子圖算法代替卡爾曼濾波進行融合,并通過實際場景實驗中證明了本文方法能夠提高系統(tǒng)的定位精度。得出主要結(jié)論如下:

1) 與卡爾曼濾波算法相比,因子圖算法可以進一步降低系統(tǒng)平均定位誤差超過40%。因為使用了較多的歷史數(shù)據(jù),計算量偏大,但更高的定位精度使之可應(yīng)用于行人及單兵導(dǎo)航裝置。

2) 與卡爾曼濾波相比,因子圖算法能夠充分利用歷史信息,通過整體優(yōu)化能夠有效地提高GNSS/PDR組合導(dǎo)航系統(tǒng)的定位精度。

3) 圖優(yōu)化算法中,求解代價函數(shù)的迭代過程有助于提高非線性模型下的估計精度。