高 云，盧 虎，賀磊南

(空軍工程大學(xué)信息與導(dǎo)航學(xué)院，陜西 西安 710077)

0 引言

近年來，以智能終端為基礎(chǔ)的位置服務(wù)(Location-Based Services，LBS)呈現(xiàn)出迅猛發(fā)展態(tài)勢，已廣泛用于小型商業(yè)、應(yīng)急救援、交通、醫(yī)療等多個領(lǐng)域之中[1]。與此同時，用戶對于終端設(shè)備導(dǎo)航定位的可靠性和定位精度的要求也越來越高，而當(dāng)前主用全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)信息的智能終端定位技術(shù)在場景適應(yīng)范圍和定位精度方面都受到了限制，為保證不同場景下終端導(dǎo)航定位的適用性和高精度，研究人員利用智能終端中普遍包含的通用嵌入式傳感器，如微機械陀螺儀、加速度計、磁強計、光強計、紅外傳感器、藍(lán)牙芯片、無線局域網(wǎng)(Wireless Local Area Network，WLAN)芯片等[2]，在導(dǎo)航定位領(lǐng)域開展了大量的研究[3-6]。

目前，通過采用卡爾曼濾波、粒子濾波等算法融合衛(wèi)星、Wi-Fi、地磁、微慣導(dǎo)等多源導(dǎo)航信息，可實現(xiàn)高精度、低成本的導(dǎo)航定位，并能夠適用于不同的場景，如半遮掩環(huán)境和室內(nèi)環(huán)境等。當(dāng)若干傳感器不能使用或受到干擾時，總有適合的傳感器可完成定位解算，從而提高了導(dǎo)航定位信息的冗余度和可用性。然而，智能終端在具體定位時場景是多變的，而場景的切換勢必會帶來舊傳感器的失效和新傳感器的出現(xiàn)，而現(xiàn)有的經(jīng)典組合導(dǎo)航方法——聯(lián)邦濾波囿于不可擴展的融合模型結(jié)構(gòu)，不能得到最佳的狀態(tài)估計，甚至可能出現(xiàn)不可用場景[7-9]。

針對經(jīng)典方法的不足，本文提出了一種基于因子圖的智能終端多源導(dǎo)航信息融合算法。通過利用因子圖描述多傳感器融合的極大后驗估計的問題，將其轉(zhuǎn)化為一個非線性最小二乘的狀態(tài)估計，建立了因子圖描述的慣性測量裝置(Inertial Measurement Unit，IMU)、GNSS和WLAN三類因子節(jié)點和先驗節(jié)點，并利用因子圖方法進(jìn)行多源導(dǎo)航傳感器的即插即用融合。

1 邊緣函數(shù)與因子圖

令{x1，x2，…,xn}表示變量的集合，設(shè)xi的定義域為Bi，令g(x1，x2，…,xn)表示該變量集合的一個值域為實數(shù)域R的函數(shù)。假設(shè)已經(jīng)定義了一個R中的函數(shù)g(x1，x2，…,xn)，其存在n個邊緣函數(shù)gi(xi)。對于b∈Bi，gi(b)的值由所有滿足xi=b的函數(shù)g(x1，x2，…,xn)的全部構(gòu)造求和得到，因為不是對指定的參量求和，而是對除去該參量外的其他變量求和，所以因子圖中將這種類型的和稱為“補和”。例如：h是{x1，x2，x3}的函數(shù)，對于x1的補和為：

(1)

按照該定義，其邊緣函數(shù)gi(xi)為：

(2)

通?？梢酝ㄟ^以下兩個方式快速高效地計算邊緣概率函數(shù)，一是使用分布式規(guī)則簡化全局和函數(shù)的計算；二是重復(fù)使用中間值，即局部和，可用因子圖表示。

假設(shè)g(x1，x2，x3，x4，x5)為一個含義5個變量的全局函數(shù)，可以表示為如下的局部函數(shù)乘積的形式：

g(x1,x2,x3,x4,x5)=fA(x1)fB(x2)fC(x1,x2,x3)·fD(x3,x4)fE(x3,x5)

(3)

式(3)中，g=(A,B,C,D,E)，fA={x1}，fB={x2}，fC={x1,x2,x3}，fD={x3,x4}，fE={x3,x5}，可用因子圖表示，見圖1。

圖1 函數(shù)乘積的因子圖Fig.1 Factor diagram of the product of a function

2 智能終端傳感器節(jié)點的因子圖描述

簡明起見，本文所研究的智能終端定位算法僅利用IMU，GNSS，WLAN三種異構(gòu)傳感器的測量信息，下面對上述三類傳感器的測量及定位模型分別進(jìn)行因子圖的描述。

2.1 先驗節(jié)點

智能終端導(dǎo)航狀態(tài)信息包含位置和姿態(tài)兩部分，可用先驗節(jié)點表示，用于對定位的初始位置進(jìn)行初始化，并定義為：

(4)

式(4)中，mx是導(dǎo)航狀態(tài)的均值，σm是協(xié)方差矩陣。顯然，位置和姿態(tài)信息相互獨立。

2.2 IMU節(jié)點

IMU解算得到的導(dǎo)航狀態(tài)x隨時間變化，根據(jù)概念其變化可以描述為非線性差分方程：

(5)

式(5)中，fb和ωb分別表示慣性傳感器測量得到的載體的比力和角加速度。IMU的校準(zhǔn)參量用c表示，根據(jù)IMU誤差模型來對測量的fb和ωb進(jìn)行修正。IMU的誤差模型通常與導(dǎo)航狀態(tài)一起聯(lián)合估計，對上式的線性化將產(chǎn)生雅克比行列式表示的狀態(tài)空間和零均值高斯分布的過程噪聲。一般來說，c隨時間的變化可以由自身的非線性傳播模型描述為：

(6)

一個給定的IMU量測zk,IMU{fb，ωb}與兩個連續(xù)時間序列tk和tk+1時刻的導(dǎo)航狀態(tài)相關(guān)。由量測的積分和上一時刻導(dǎo)航狀態(tài)即可得到當(dāng)前時刻的導(dǎo)航狀態(tài)，可知狀態(tài)推算的離散方程為：

(7)

式(7)中，每一個等式都定義了一個因子圖與慣性導(dǎo)航相關(guān)的因子節(jié)點，一個是IMU因子節(jié)點，連接導(dǎo)航狀態(tài)xk、xk+1和偏移變量節(jié)點ck；另一個是偏移因子節(jié)點，連接偏移變量節(jié)點ck和ck+1。對于給定的IMU量測zk,IMU，IMU因子節(jié)點定義如下，IMU的量測zk,IMU和當(dāng)前時刻的xk，ck用來計算下一時刻狀態(tài)xk+1的預(yù)測值。這一預(yù)測值與當(dāng)前估計xk+1的差值即為因子節(jié)點代表的誤差函數(shù)：

fIMU(xk+1,xk,ck)?c(xk+1-h(xk,ck,zk,IMU))

(8)

同樣，根據(jù)IMU誤差的計算模型，偏移因子節(jié)點定義為：

fbias(ck+1,ck)?c(ck+1-g(ck))

(9)

圖2 IMU因子節(jié)點Fig.2 IMU factor node

圖2表示的是連續(xù)三個IMU量測的因子節(jié)點。按照IMU更新速率向圖中添加導(dǎo)航狀態(tài)xi和IMU的校準(zhǔn)ci，假設(shè)IMU的更新速率為1/Δt，圖2中導(dǎo)航狀態(tài)對應(yīng)的時刻分別為t1，t1+Δt，t1+2Δt，t1+3Δt。在實際應(yīng)用中，如果按照IMU的更新速率向圖模型中添加新的導(dǎo)航狀態(tài)，會使得全局融合均以IMU速率更新，則會造成巨大的計算負(fù)擔(dān)，從而降低了融合效能。因此，需要對連續(xù)的IMU測量進(jìn)行等價稀疏處理，即用等價IMU因子節(jié)點替代傳統(tǒng)的IMU因子節(jié)點，如圖3所示。與該等價節(jié)點相連的兩個連續(xù)的導(dǎo)航狀態(tài)，在時空上與傳統(tǒng)的導(dǎo)航狀態(tài)相比距離較遠(yuǎn)，從而避免了以IMU的速率添加新的導(dǎo)航狀態(tài)。

圖3 IMU等價因子節(jié)點Fig.3 IMU equivalence factor node

2.3 GNSS節(jié)點

GNSS定位的原理是接收機通過接收多顆衛(wèi)星的星歷參數(shù)和偽隨機碼，與接收機本地復(fù)制的偽隨機碼進(jìn)行相關(guān)運算，如果衛(wèi)星與接收機的時間同步，即可計算出兩者之間的距離[10-11]。然而，一般情況下，接收機和衛(wèi)星之間并不嚴(yán)格同步，二者的時鐘之間存在鐘差Δt，碼測距得到的距離ρ并不是真實的距離，因此將其稱之為偽距。在地心地固直角坐標(biāo)系(Earth Centered Earth Fixed，ECEF)下，假設(shè)第i顆衛(wèi)星的坐標(biāo)為(lxi，lyi，lzi)，載體坐標(biāo)為(lx，ly，lz)，則定位方程為：

(10)

當(dāng)接收機觀測到四顆以上的衛(wèi)星時，通過最小二乘等方法即可求解得到載體的位置坐標(biāo)。由于GNSS的測量與導(dǎo)航狀態(tài)存在一定延遲，因此當(dāng)前時刻GNSS的測量是對之前某一時刻的狀態(tài)的觀測，上述的量測定位方程可寫為：

zk,GNSS=hGNSS(xi)+n

(11)

式(11)中，hGNSS是GNSS傳感器的測量函數(shù)，表示導(dǎo)航狀態(tài)與測量的函數(shù)關(guān)系；n是測量噪聲。通常tk≥ti。由于GNSS估計狀態(tài)的時刻可能與IMU推算的導(dǎo)航狀態(tài)的時刻存在時間差，因此將ti取為最接近GNSS估計時刻的IMU推算時刻。根據(jù)GNSS測量方程，定義GNSS傳感器的因子節(jié)點為：

fGNSS?c(zk,GNSS-h(xi))

(12)

如圖4所示，GNSS因子節(jié)點只與代表ti時刻狀態(tài)的變量節(jié)點xi相連接，當(dāng)?shù)玫紾NSS傳感器的測量時，在圖4中加入GNSS因子節(jié)點，對導(dǎo)航狀態(tài)的估計結(jié)果進(jìn)行優(yōu)化。

圖4 GNSS測量的因子圖Fig.4 Factor diagram of GNSS measurement

2.4 WLAN節(jié)點

首先濾波處理WLAN傳感器得到的原始距離，然后對原始距離進(jìn)行預(yù)處理得到有效距離測量，記為zk,wlan，最后將建立最優(yōu)二次規(guī)劃，按照最小誤差平方準(zhǔn)則，定義目標(biāo)函數(shù)為智能手機到錨節(jié)點的測距誤差平方和，定位方程為：

(13)

fwlan?c(xk-hwlan(zk,wlan))

(14)

WLAN因子節(jié)點只與變量節(jié)點xk相連，當(dāng)WLAN傳感器探測到信道狀態(tài)更新時，輸出當(dāng)前時刻的位置估計，將其加入圖5中對導(dǎo)航狀態(tài)的估計結(jié)果進(jìn)行優(yōu)化。

3 基于因子圖的智能終端即插即用信息融合模型

由第2章分析可知，通過因子圖描述傳感器的變量節(jié)點和導(dǎo)航狀態(tài)的極大后驗估計，將任一時刻可用的導(dǎo)航傳感器的狀態(tài)變量由導(dǎo)航解的貝葉斯后驗概率的最大后驗估計測度聯(lián)系起來，當(dāng)有新的傳感器信息出現(xiàn)時，只需要在因子圖模型中對應(yīng)的變量節(jié)點增加一個與之相連的因子節(jié)點。同樣地，當(dāng)有舊的傳感器信息失效時，也只需要在圖模型中減掉一個因子節(jié)點即可，這種信息的即插即用融合能力還體現(xiàn)在對于異步信息的融合，當(dāng)任意時刻有某個傳感器的信息出現(xiàn)時，因子圖融合模型可在圖中增加相應(yīng)時刻的因子節(jié)點和狀態(tài)節(jié)點，進(jìn)行全局優(yōu)化估計，而不是傳統(tǒng)聯(lián)邦濾波方法按照固定速率更新的估計方法。

圖5 多傳感器融合即插即用因子圖模型Fig.5 Plug and play factor diagram fusion model of three sensor fusion

圖5為本文提出的因子圖融合模型。終端軌跡推算使用的IMU傳感器是基本的傳感器單元，狀態(tài)輸出貫穿整個導(dǎo)航定位階段。GNSS傳感器在x1～x4的導(dǎo)航狀態(tài)有效，更新速率為IMU的1/2，在信息更新時將其添加至圖5中，對導(dǎo)航狀態(tài)進(jìn)行融合估計，則x1～x4的狀態(tài)估計分別為：

(15)

WLAN傳感器在x4～x7的導(dǎo)航狀態(tài)有效，更新速率為IMU的1/3，GNSS自x5開始不可用，x4的導(dǎo)航狀態(tài)如式(15)，x5～x7的狀態(tài)估計分別為：

(16)

基于因子圖的多源傳感器信息融合估計體現(xiàn)了因式乘積的最優(yōu)化，每個因式代表傳感器量測的耗散函數(shù)，融合時根據(jù)當(dāng)前時刻的有效傳感器信息，將其量測的耗散函數(shù)相乘，當(dāng)傳感器信息發(fā)生變化，即乘上或除去變化的傳感器信息，而不需要做其他復(fù)雜的處理，實現(xiàn)了多源導(dǎo)航信息融合的即插即用能力。

4 實驗及數(shù)據(jù)分析

本文實驗區(qū)域是實驗樓和實驗樓四周的道路，如圖6所示。其中，實驗樓西側(cè)與南側(cè)的道路較為開闊，GNSS信號不受遮擋，在東側(cè)與北側(cè)GNSS信號受到樹蔭遮擋。WLAN定位的實驗區(qū)域為實驗樓東側(cè)走廊，走廊兩側(cè)部署8個位置已知的錨節(jié)點，部署位置為圖6中白色區(qū)域的圓形點。在實驗數(shù)據(jù)采集的行進(jìn)過程中，行人始終穩(wěn)定持握智能手機并沿著預(yù)定軌跡行走(圖6中箭頭軌跡)以獲得連續(xù)的測量數(shù)據(jù)，三角形為起始點，行人攜帶智能手機由起始點出發(fā)，向東走至實驗樓正門口進(jìn)入實驗樓，在實驗樓內(nèi)部沿走廊行至側(cè)門，出門后沿道路一周回到起點位置。

圖6 實驗場地及路線Fig.6 Experimental site and route

數(shù)據(jù)采集的智能設(shè)備為華為P9手機，搭載Andriod操作系統(tǒng)，嵌入了多種導(dǎo)航傳感器，包括LSM-330型3軸加速度/陀螺儀傳感器模塊、BCM-4355-XKUB型Wi-Fi控制器模塊和GPS模塊等等，能夠滿足實驗的應(yīng)用需求。

數(shù)據(jù)采集頻率為100 Hz，采集的歷元數(shù)據(jù)個數(shù)為28 851個。每一個歷元的數(shù)據(jù)包括三軸陀螺儀、三軸加速度和歷元的時間戳。實驗中IMU的更新速率為100 Hz。多傳感器融合時，為了避免整個融合算法以該速率更新會帶來的計算負(fù)擔(dān)，本文采取了等價稀疏處理的方法。考慮到實驗時是行人攜帶手機進(jìn)行數(shù)據(jù)采集，因此取為500 ms，即等價稀疏后的IMU的更新速率為2 Hz，歷元數(shù)據(jù)個數(shù)為576個，WLAN傳感器的更新速率為0.75 Hz，GNSS傳感器的速率為1 Hz。

4.1 融合性能分析

圖7是定位實驗的結(jié)果，包含了行人的真實軌跡和IMU推算定位、EKF融合定位、基于本文方法的定位三種不同定位方法的軌跡?？梢钥闯觯琍DR定位在室內(nèi)外環(huán)境下都能夠?qū)崿F(xiàn)對行人運動軌跡的連續(xù)跟蹤，但由于IMU推算固有的易受累積誤差影響的特點，在沒有絕對位置信息校正情況下，定位會越來越偏離真實軌跡，且轉(zhuǎn)彎后定位軌跡偏離較為顯著。EKF融合定位利用GNSS和WLAN的定位結(jié)果對IMU推算定位進(jìn)行校正，定位精度有所提高，但因非線性和模型選取的問題，軌跡與真實軌跡仍然相差較大。本文提出的基于因子圖的融合定位能夠利用IMU、GNSS和WLAN傳感器的測量信息融合輸出定位結(jié)果，定位軌跡準(zhǔn)確，實現(xiàn)了可靠的對行人的無縫定位。

圖7 定位實驗的結(jié)果Fig.7 The result of location experiment

由于實驗是在二維平面內(nèi)開展，因此多源融合定位的姿態(tài)性能分析主要是對航向的估計結(jié)果進(jìn)行分析，如圖8所示，對比了本文多源融合定位和IMU輸出的航向角的誤差?？梢钥闯?，基于因子圖的多源融合的航向角平均誤差為1.4°，在轉(zhuǎn)彎時誤差有所增大，最大誤差5.7°，但隨后的定位結(jié)果又對航向進(jìn)行了實時修正，以降低其誤差，表明加入絕對的定位源能夠消除IMU的累積誤差，得到較為準(zhǔn)確的航向估計。

圖8 航向角誤差Fig.8 Heading angle error

圖9是EKF融合定位和本文方法定位的定位精度，使用均方根誤差分別在X軸向和Y軸向?qū)ζ溥M(jìn)行表示。由圖可以看出，在X軸向，本文方法定位的RMSE平均值為0.7 m，EKF定位的RMSE的均值為1.2 m，在Y軸向，本文所研究的因子圖方法定位的RMSE平均值為1.1 m，EKF定位的RMSE的均值為1.9 m，定位誤差減小了42%。表明本文所提方法的定位精度優(yōu)于EKF，有效地提高了位置估計的準(zhǔn)確度。

圖9 均方誤差Fig.9 Mean square error

表1顯示了因子圖和EKF的計算速度。需要說明的是在計算速度方面，因子圖融合算法是一種圖優(yōu)化的算法，估計時對圖模型的所有節(jié)點都進(jìn)行估計，計算量隨著狀態(tài)增多而不斷累計增大，因此相對于EKF算法，因子圖融合算法的快速性要差一些。

表1 算法的快速性

4.2 即插即用的融合定位

在定位試驗中，因定位環(huán)境變化，GNSS和WLAN傳感器會出現(xiàn)上線和阻斷的情況，可用傳感器的類型在三到四個之間變化。圖10是根據(jù)傳感器的數(shù)目，融合不同傳感器的定位結(jié)果，虛線代表融合了三種類型的傳感器的定位結(jié)果，點劃線代表融合了兩種傳感器的定位結(jié)果。圖11是圖10中標(biāo)注的“1”、“2”兩處的細(xì)節(jié)圖，對應(yīng)實際的定位環(huán)境，“1”處為由室外進(jìn)入室內(nèi)，“2”處為由室內(nèi)到室外?？梢钥闯觯?種傳感器融合的估計軌跡比3種傳感器融合的更加接近行人的真實軌跡。表明本文提出的因子圖融合模型能夠靈活的融合多種傳感器測量信息，在有新的可用傳感器出現(xiàn)時，即將其加入融合模型中，得到更優(yōu)的定位結(jié)果，具有良好的動態(tài)適應(yīng)性，實現(xiàn)了傳感器即插即用的融合定位。

圖10 不同傳感器融合的定位結(jié)果Fig.10 Positioning results of different sensor fusion

圖11 兩處傳感器數(shù)目變化時的定位結(jié)果Fig.11 Results of changes in the number of sensors at two locations

5 結(jié)論

本文提出了基于因子圖的智能終端多源導(dǎo)航信息異步融合算法。該算法利用概率關(guān)聯(lián)算法將多源導(dǎo)航信息由貝葉斯后驗概率的最大后驗估計測度聯(lián)系起來，給出了IMU、GNSS和WLAN三類典型傳感器的因子模型和異步融合結(jié)構(gòu)。

實驗結(jié)果表明所提方法相較于經(jīng)典EKF融合方法，定位誤差減小了42%，能有效地提高智能終端位置估計的準(zhǔn)確度，且在傳感器信息變化時能夠靈活的融合多種傳感器測量信息，具有良好的動態(tài)適應(yīng)性，可以實現(xiàn)多傳感器的即插即用融合定位。相關(guān)研究結(jié)果，有助于促進(jìn)傳統(tǒng)智能終端的導(dǎo)航定位技術(shù)從單項主用傳感技術(shù)(如IMU、衛(wèi)星)向協(xié)同配合、聯(lián)合求解的方向轉(zhuǎn)變，并在個人應(yīng)用、應(yīng)急救援、大型建筑內(nèi)定位等方面有著廣泛的應(yīng)用前景。