行人GNSS/PDR組合導航優化估計方法

朱建良, 王立雅, 薄煜明

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

0 引言

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

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

為進一步提高GNSS/PDR組合導航系統性能,本文從數據融合的角度出發,提出一種基于因子圖優化(FGO)GNSS/PDR組合導航方法。本文主要研究內容如下:

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

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

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

1 PDR算法

基于智能手機的PDR一種相對定位算法,通過處理手機傳感器數據估計行人步長和行走方向更新行人位置[18]。圖1為智能手機PDR原理圖。圖1中,N表示北向,E表示東向,pk和pk+1表示k和k+1時刻的位置向量。假設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

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

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

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

(3)

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

(4)

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

2 GNSS/PDR組合導航

由于傳感器噪聲和行人航向誤差等,PDR結果通常隨著時間發散。智能手機GNSS在信號良好的情況下可以提供高精度定位信息,可以構建GNSS和PDR組合導航系統。當GNSS信號良好的情況下通過與PDR組合可以輸出穩定的定位信息;當GNSS無法工作時,PDR可以提供定位信息。

2.1 基于卡爾曼濾波PDR/GNSS組合導航方法

PDR/GNSS組合導航方法狀態方程以及量測方程如下:

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

(5)

(6)

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

(7)

(8)

(9)

(10)

(11)

2.2 基于因子圖PDR/GNSS組合導航方法

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

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

(12)

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

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

(13)

以上乘積關系可以用圖3所示因子圖表示,其中黑色方框表示因子節點,圓形代表變量節點。上述過程表示了經過因子圖模型將復雜函數分解為 4個局部函數乘積。

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

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

(14)

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

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

(15)

圖4 變量依存關系因子圖Fig.4 Factor graph of variable dependency relation

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

(16)

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

(17)

因此,狀態的最優估計可以轉換為求取狀態變量的最大后驗概率(MAP),可以表示為

(18)

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

(19)

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

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

(20)

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

圖5 FGO-GNSS/PDR組合導航方法Fig.5 FGO-GNSS/PDR integrated navigation method

狀態代價函數如下:

(21)

量測代價函數如下:

(22)

總的代價函數如下:

(23)

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

(24)

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

2.3 因子圖求解

構建代價函數之后,狀態的最優估計可以從求取代價函數的最小值獲取。結合式(18)～式(20),式(21)和式(22)[19-21]可以寫為

(25)

(26)

整體代價函數可以寫為

(27)

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

(28)

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

(29)

式(27)可以展開為

(30)

綜上,可得以下公式:

(31)

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

aΔX=-b

(32)

為穩定的求得ΔX,通常引入參數λ,新的公式如下:

(a+λI)ΔX=-b

(33)

(34)

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

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

3 實驗結果與分析

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

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

圖7 數據采集裝置Fig.7 Dataset collecting equipment

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

3.1 手持實驗

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

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

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

表1 手持實驗1水平位置誤差統計結果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水平位置誤差統計結果

3.2 背包實驗

除手持實驗之外,進行兩組將手機放置在背包中的實驗,手機水平放置,以保證手機的航向和行人航向盡可能一致。同樣對水平定位結果進行分析,兩組實驗水平位置誤差分別如圖10和圖11所示。圖10(a)和圖11(a)分別為水平定位誤差和步數之間的關系;圖10(b)和圖11(b)分別為對應的水平定位誤差的統計結果和分布圖。與KF-GNSS/PDR方法相比,FGO-GNSS/PDR方法同樣地獲得了更好的定位精度。表3和表4給出了進一步對應的統計分析結果:第1組數據中,水平位置平均誤差減少了40.5%,誤差均方根降低了36.9%;第2組數據中,水平位置平均誤差減少了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 結果分析、討論和展望

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

表3 背包實驗1水平位置誤差統計結果Table 3 Knapsack experiment 1 statistical results of horizontal position errors

表4 背包實驗2水平位置誤差統計結果Table 4 Knapsack experiment 2 statistical results of horizontal position errors

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

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

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

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

4) 本文基于因子圖的GNSS/PDR組合導航可以進一步拓展成多源信息融合導航系統,進一步開發室內外無縫導航系統。

4 結論

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

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

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

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