楊高朝，王 慶，蔚保國，劉鵬飛，李 爽

1. 東南大學(xué)儀器科學(xué)與工程學(xué)院，江蘇 南京 210096； 2. 東南大學(xué)智慧城市研究院，江蘇 南京 210096； 3. 衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國家重點實驗室，河北 石家莊 050050

視覺導(dǎo)航定位是通過視覺系統(tǒng)在相機(jī)運動過程中提取不同的圖像，通過檢測這些不同圖像的變化，提取并且匹配相同的特征點，判斷特征點的運動變化來估計相機(jī)的運動情況[1-3]。視覺里程計(VO)由于在面對單純的旋轉(zhuǎn)時無法很好地跟蹤，因此在實際應(yīng)用中通常會與低廉的IMU等慣性傳感器進(jìn)行組合，在組合后視覺慣性里程計(VIO)能夠穩(wěn)健地應(yīng)用[4-5]。VIO雖然在光照條件、圖像質(zhì)量良好的情況下具有很高的定位精度，但在沒有其他信息的情況下由于缺少全局的位置信息參考，其實質(zhì)是一種局部定位算法，存在累積誤差的問題[6]。因此，目前有許多研究通過設(shè)計全局的路標(biāo)或者利用其他全局的信息予以補(bǔ)充，提高視覺定位在長距離范圍的適用性。

VIO無法避免累積誤差。GNSS是一種全局觀測的傳感器，每一次觀測都是獨立的，因此誤差并不會累積，但是導(dǎo)航型GNSS定位輸出頻率和定位精度較低，通常無法滿足用戶更高的需求。從理論上看，將一個局部精度很高但存在累積誤差的VIO和一個局部精度無法保證到不存在累積誤差的GNSS相結(jié)合，可以互相彌補(bǔ)各自的不足[7]。

由于視覺、IMU等傳感器無法獲得全局信息，因此被定義為局部傳感器。而對于GNSS、磁力計、UWB等能夠感知全局信息的傳感器，被定義為全局傳感器。對于局部傳感器，首先進(jìn)行局部位姿估計，即傳統(tǒng)意義上的VO或者VIO。得到局部的位姿估計之后，再與GNSS等全局傳感器進(jìn)行對齊。對齊的方式是建立一個位姿圖，每個GNSS時刻建立一個位姿節(jié)點，連續(xù)的兩個節(jié)點之間將局部位姿估計得到的相對位姿作為約束。每個節(jié)點還與GNSS等全局位置建立約束[8]。

文獻(xiàn)[9—10]提出基于濾波的多傳感器松耦合方法。主要思想是把IMU作為主傳感器，通過積分得到6自由度的位姿。VO/VIO作為相對位姿的估計器，GNSS等作為全局位姿估計器，與IMU積分得到的結(jié)果進(jìn)行EKF，得到更加準(zhǔn)確的位置估計。IMU積分受到偏差和噪聲的影響，會很快地發(fā)散。而VO可以認(rèn)為在局部范圍內(nèi)誤差很小，因此可以修正IMU的偏差。而GNSS不存在累積誤差的問題，又可以修正IMU和VO/VIO的累積誤差。GNSS的觀測方程比較簡單，即將待估計的位姿通過外參轉(zhuǎn)化到GNSS坐標(biāo)系的位姿，因為GNSS無法測量旋轉(zhuǎn)，所以直接取三維位置的差作為觀測誤差。由于VO的尺度漂移是不穩(wěn)定的，因此直接把VO的位姿放在全局坐標(biāo)系下建立觀測方程去估計尺度是不夠準(zhǔn)確的，因此，筆者提出了把VO的pose作為兩幀之間相對的位姿去建立觀測方程。

文獻(xiàn)[11—12]提出基于優(yōu)化的方式，支持雙目配置、多軌跡融合以及GNSS等全局傳感器與VO進(jìn)行聯(lián)合優(yōu)化。

文獻(xiàn)[11]主要思想是，首先進(jìn)行局部位姿估計，即傳統(tǒng)意義上的VO或者VIO，得到局部的位姿估計之后，再與GNSS等全局傳感器進(jìn)行對齊。對齊的方式是建立一個位姿圖，每個GNSS時刻建立一個位姿節(jié)點，連續(xù)的兩個節(jié)點之間將局部位姿估計得到的相對位姿作為約束。每個節(jié)點還與GNSS等全局位置建立約束。

文獻(xiàn)[12]總體思想與文獻(xiàn)[11]類似。但細(xì)節(jié)和試驗上更加完備一些。筆者認(rèn)為，GNSS無法提供旋轉(zhuǎn)的約束，因此基于這種結(jié)構(gòu)優(yōu)化后的旋轉(zhuǎn)的精度會變差。因此提出了第二種因子圖結(jié)構(gòu)。其中GNSS部分的約束保持不變。但相對位姿只利用VO/VIO的平移部分建立節(jié)點和節(jié)點之間的約束。而旋轉(zhuǎn)則通過建立一個虛擬的局部坐標(biāo)系，通過計算局部坐標(biāo)系和全局坐標(biāo)系(GNSS坐標(biāo)系)之間的變換，建局部坐標(biāo)系下的旋轉(zhuǎn)轉(zhuǎn)換成全局坐標(biāo)系下的旋轉(zhuǎn)，然后建立全局的旋轉(zhuǎn)約束。在初始化階段，通過直接對齊VO/VIO位姿和GNSS，解SVD來求解。在進(jìn)行優(yōu)化之前，筆者把VO/VIO的位姿與IMU進(jìn)行一次松耦合的濾波。盡量減少與GNSS之間的時延帶來的誤差。

由于在室內(nèi)無法接收到GNSS信號，因此在室內(nèi)就無法利用GNSS消除VIO的累積誤差。偽衛(wèi)星又稱“地面衛(wèi)星”，是從地面某特定地點發(fā)射類似于GNSS的導(dǎo)航信號，采用的電文格式與GNSS基本一致。偽衛(wèi)星系統(tǒng)作為GNSS定位系統(tǒng)的輔助手段和工具，即可以用來輔助增強(qiáng)GNSS在某些惡劣環(huán)境下的定位性能，也可以單獨使用構(gòu)建偽衛(wèi)星定位系統(tǒng)。因此，在室內(nèi)，可以利用偽衛(wèi)星與視覺VIO進(jìn)行組合消除VIO的誤差累積[13]。但是偽衛(wèi)星在室內(nèi)應(yīng)用中也面臨著兩大挑戰(zhàn)。

偽衛(wèi)星鐘差的影響。由于GNSS中衛(wèi)星鐘差采用的是原子鐘，精度較高，因此，在一般精度定位中可以忽略不計，但是原子鐘價格昂貴，室內(nèi)偽衛(wèi)星采用不合適。由于偽衛(wèi)星鐘差的影響，單接收機(jī)無法完成定位，必須采用站間和星間雙差技術(shù)，這在實際應(yīng)用中非常不便。為了解決偽衛(wèi)星鐘差的問題，目前很多研究機(jī)構(gòu)都是采用陣列天線來解決這個問題。陣列天線中所有的偽衛(wèi)星鐘差都是一樣的，因此通過星間單差技術(shù)可以同時消去接收機(jī)鐘差和偽衛(wèi)星鐘差[14-16]。

室內(nèi)多徑的影響。GNSS一般應(yīng)用于室外定位，多徑一般都忽略不計。但是室內(nèi)結(jié)構(gòu)復(fù)雜，多徑對偽距的影響往往能達(dá)到幾十米甚至幾百米。而偽衛(wèi)星通常是靜止于室內(nèi)的，無法通過多歷元平滑來減弱多徑。目前偽衛(wèi)星多徑解決方法主要分為3類：基帶解調(diào)端、接收機(jī)端及數(shù)據(jù)處理端。因此，在實際室內(nèi)應(yīng)用中只利用載波觀測值。由于只利用載波觀測值會導(dǎo)致秩虧問題(N顆衛(wèi)星至少需要解算(N+3)個參數(shù))，為了解決這個問題，在動態(tài)應(yīng)用前一般需要在已知位置上進(jìn)行初始化一分鐘[17-19]。

多傳感器融合算法一般包括基于濾波和基于非線性的融合方式?；跒V波的算法有卡爾曼濾波(包括一些延伸的算法)、無跡卡爾曼濾波(UKF)及粒子濾波(PF)等，基于非線性優(yōu)化的算法有最速下降法、高斯-牛頓及LM(Levenberg-Marquard)算法等[20-24]。很多文獻(xiàn)表明，由于基于非線性優(yōu)化的算法可以同時優(yōu)化多個時間段的數(shù)據(jù)，因此基于非線性優(yōu)化的算法要優(yōu)于基于濾波的算法。本文主要研究基于非線性優(yōu)化的融合算法。最速下降法無法確定收斂步長，步長較小會導(dǎo)致收斂較慢，步長較大會導(dǎo)致算法發(fā)散。高斯牛頓法采用二階泰勒展開來近似，只有在展開點附近才會有比較好的近似效果，如果Hessian矩陣是非奇異矩陣，可能會導(dǎo)致算法發(fā)散。LM算法能夠很好地克服上面的缺點，已經(jīng)廣泛應(yīng)用于很多非線性優(yōu)化實例中，但是LM算法還有自身的缺陷。盡管LM算法能夠克服Hessian矩陣病態(tài)解的影響，由于LM算法采用最小二乘估計作為收斂條件從而不能抵御傳感器數(shù)據(jù)中心粗差的干擾，特別是室內(nèi)偽衛(wèi)星定位時，由于室內(nèi)多徑比較嚴(yán)重，很難保證偽衛(wèi)星定位的穩(wěn)定性和可靠性。若直接使用LM算法進(jìn)行優(yōu)化，則會使得求解出的融合解偏離真實值。鑒于VIO和偽衛(wèi)星單獨定位的缺陷，本文主要研究應(yīng)用抗差LM算法進(jìn)行VIO和偽衛(wèi)星融合的問題。

1 多源數(shù)據(jù)融合約束方程的建立

1.1 數(shù)據(jù)內(nèi)插

多傳感器的融合時間戳對齊是非常關(guān)鍵的一步。時間戳無法對齊或?qū)R錯誤最后可能得出一個錯誤的軌跡，很可能使優(yōu)化算法失效。為了保證融合算法的穩(wěn)健性，視覺傳感器的時間戳與偽衛(wèi)星的時間戳相差不超過10 ms。

一般偽衛(wèi)星數(shù)據(jù)的輸出頻率為1～5 Hz，而視覺VIO數(shù)據(jù)一般可以達(dá)到10～20 Hz，為了更方便進(jìn)行后端融合，本文首先根據(jù)視覺輸出的頻率及相應(yīng)的時間戳對偽衛(wèi)星輸出數(shù)據(jù)進(jìn)行內(nèi)插。由于兩個偽衛(wèi)星輸出數(shù)據(jù)間隔一般只有3～4個視覺輸出數(shù)據(jù)，考慮到內(nèi)插平滑的精度，本文利用三次樣條曲線內(nèi)插[25]，假設(shè)已知的n個坐標(biāo)點(S(ti),ti)，…，(S(ti+n),ti+n)，其中S(ti)為ti時間戳對應(yīng)的三維坐標(biāo)值x(ti)，y(ti)和z(ti)，本文可以列出樣條曲線的微分式。如果函數(shù)S(ti)滿足以下3個條件(本文為了方便計算，把xi,yi,zi根據(jù)ti分開內(nèi)插求解)：

(1)S(ti)=x(ti),i=(1,2,…,n)。

(2)S(ti)在每個區(qū)間上[ti,ti+1](1,2,…,n-1)是一個三次多項式。

(3)S(ti)在整個區(qū)間上[t1,tn]有連續(xù)的一階及二階導(dǎo)數(shù)。

則稱S(ti)為過n個點的三次樣條函數(shù)。三次樣條曲線內(nèi)插是通過一系列形值點的一條光滑曲線，數(shù)學(xué)上通過求解三彎矩方程組得出曲線函數(shù)組的過程。實際計算時還需要引入邊界條件才能完成計算。假設(shè)方程為

Si(t)=ai+bi(t-ti)+ci(t-ti)2+di(t-ti)3
i=1,2,…,n-1

(1)

1.2 多傳感器約束方程的建立

為了實時進(jìn)行偽衛(wèi)星和VIO的融合，本文選擇滑窗融合模式，窗口長度為10。偽衛(wèi)星坐標(biāo)系采用室內(nèi)局部坐標(biāo)系。為防止秩虧現(xiàn)象發(fā)生，首先把融合后的地圖坐標(biāo)系第一幀與偽衛(wèi)星坐標(biāo)系對齊。

由于偽衛(wèi)星不能求出旋轉(zhuǎn)向量，因此在優(yōu)化的時候只能利用坐標(biāo)向量和速度向量?？偟娜诤霞s束方程可以寫為

(2)

χ=[pi,Gqi,GVi,Gbi,gbi,api,CGqi,CG]

(3)

式中，pi,G、Vi,G和qi,G是優(yōu)化后的全局平移、速度和旋轉(zhuǎn)參數(shù)；bi,g和bi,a分別是陀螺儀和加速度計偏置；i表示滑動窗口中幀數(shù)；C、S和G代表視覺世界、偽衛(wèi)星和融合后全局坐標(biāo)系標(biāo)志。

由于pi,CG、qi,CG和視覺VIO密切相關(guān)，而視覺VIO會隨著時間有誤差累積，因此,pi,CG和qi,CG也會有誤差累積。為了使優(yōu)化后的位姿更加平滑，本次研究選擇實時優(yōu)化這兩個向量。

1.2.1VIO約束

由于VIO的尺度漂移是不穩(wěn)定的，直接把VIO的位姿放在全局坐標(biāo)系下建立觀測方程去估計尺度是不夠準(zhǔn)確的，因此，把VIO的兩幀之間相對的位姿去建立觀測方程

(4)

(5)

(6)

視覺目標(biāo)函數(shù)可寫為

(7)

1.2.2 偽衛(wèi)星約束

偽衛(wèi)星約束計算公式為

(8)

(9)

δVi,S關(guān)于pi,G、θi,G、Vi,G、bi,g和bi,a的雅克比矩陣為

(10)

1.2.3 基于擴(kuò)展卡爾曼濾波的全局坐標(biāo)初始化

pk,G和qk,G為全局坐標(biāo)系下的平移和旋轉(zhuǎn)矩陣。由于偽衛(wèi)星只能提供全局位置和速度約束，因此若想求得當(dāng)前坐標(biāo)系相對全局坐標(biāo)系下的旋轉(zhuǎn)矩陣，至少需要提供4個歷元的位置約束。假設(shè)滑窗寬度為10，首先利用擴(kuò)展卡爾曼濾波初始化全局坐標(biāo)。

狀態(tài)預(yù)測方程為

(11)

狀態(tài)更新方程為

(12)

當(dāng)i=1時

p1,G=p1,S

(13)

(14)

V1,G=V1,S

(15)

假設(shè)i是當(dāng)前幀，根據(jù)pi,C、qi,C、pi,G及qi,G，可以求得視覺世界坐標(biāo)系到全局坐標(biāo)系的旋轉(zhuǎn)和平移矩陣pi,CG和qi,CG為

(16)

(17)

(18)

由于VIO隨著時間誤差會累積，因此，為了更加準(zhǔn)確地求取視覺世界坐標(biāo)系到全局坐標(biāo)系的轉(zhuǎn)換，在每次優(yōu)化完成后重新求取pi,CG和qi,CG。

1.3 有效性檢核

為增加定位的穩(wěn)健性，減少粗差給融合定位帶來影響，本文使用下列方案檢驗數(shù)據(jù)的有效性。

已知第i+1幀相對視覺世界坐標(biāo)系(視覺初始化時的參考坐標(biāo)系)下的平移量pi+1,C，又已知第i幀視覺世界坐標(biāo)系向全局坐標(biāo)系的旋轉(zhuǎn)矩陣qi,CG和平移矩陣pi,CG，可以得到第i+1幀全局坐標(biāo)系下的坐標(biāo)估值為

(19)

假設(shè)偽衛(wèi)星在i+1時刻的坐標(biāo)為pi+1,S，方差為σ2，可以建立檢核方程為

(20)

2 改進(jìn)的抗差LM非線性優(yōu)化的融合策略

已知LM非線性優(yōu)化模型為

(Hk+μkI)Δxk=gk

(21)

采用拉格朗日乘子將上述問題轉(zhuǎn)化為一個無約束問題

(22)

因此，LM算法可以看作給變化量Δx添加一個信賴區(qū)域來限制Δx的大小，并認(rèn)為在信賴區(qū)域里面近似是有效的，否則近似不準(zhǔn)確。

確定信賴區(qū)域一個好的辦法是通過比較近似模型和實際模型的差異來確定，如果差異小，就增大范圍；如果差異太大，就縮小范圍?？紤]實際模型和近似模型變化量的比值

ρ=(f(x+Δx)-f(x))/(J(x)Δx)

(23)

式(23)可以通過ρ的值來判斷泰勒近似的好壞，其中分子是實際模型的變化量，分母是近似模型的變化量。當(dāng)ρ接近1的時候，表明近似模型較好；如果ρ較小，則實際模型的變化量小于近似模型的變化量，則認(rèn)為近似模型較差，需要縮小近似范圍。反之，當(dāng)ρ較大時，說明實際模型變化量更大，需要放大近似范圍。經(jīng)典LM算法可以表示如下：

(1) 給定初始迭代值x0(坐標(biāo)向量初值可由偽衛(wèi)星給出，旋轉(zhuǎn)向量初值由視覺VIO提供)及調(diào)節(jié)因子μ0：μ0=max([hii])，其中hii是H0的對角線元素。

(2) 對于第k次迭代，求解優(yōu)化問題。

(3) 計算ρ。如果ρ>0，則認(rèn)為近似可行，令xk+1=xk+Δxk；若ρ>4/5,則μk+1=2μk；若ρ>1/5，則μk+1=0.5μk。

(4) 判斷算法是否收斂。如果不收斂，跳回步驟(2)，否則結(jié)束。

當(dāng)μk較小時，說明Hk占主導(dǎo)地位，說明二次近似在該范圍內(nèi)是比較好的，LM方法更接近于高斯牛頓法；另一方面，當(dāng)μk較大時，LM算法更接近一階梯度下降算法，這說明二次近似不夠好。LM算法的求解方式，可以避免線性方程組的矩陣非奇異和病態(tài)等問題，提供更穩(wěn)定、更準(zhǔn)確的解法。

(Hk+μk·diag[Hk])Δxk=gk

(24)

diag[Hk]指只保留Hk的對角線元素。帶上矩陣diag[Hk]后約束區(qū)域就是一個橢球，可以約束參數(shù)中不同維度的變化。

盡管LM能夠克服Hessian矩陣病態(tài)解的影響，由于LM算法采用最小二乘估計作為收斂條件從而不能抵御觀測數(shù)據(jù)中心粗差的干擾，特別是室內(nèi)偽衛(wèi)星定位時，由于室內(nèi)多徑比較嚴(yán)重，即使偽衛(wèi)星原始觀測數(shù)據(jù)能夠通過上文的有效性檢核，仍舊很難保證偽衛(wèi)星定位的穩(wěn)定性和可靠性。若直接使用LM算法進(jìn)行優(yōu)化，則會使得求解出的融合解偏離真實值。

(25)

本次研究的多傳感器融合具體流程如圖1所示，本文主要研究基于抗差LM優(yōu)化的VIO與偽衛(wèi)星后端松耦合技術(shù)。下面主要通過試驗對比來分析融合的優(yōu)勢。

圖1 多傳感器融合流程Fig.1 Flowchart of multi-sensor fusion

3 試驗驗證及分析

3.1 試驗驗證

為了驗證本文提出的多傳感器融合算法，本文利用筆者團(tuán)隊搭建的組合設(shè)備在某所C7試驗場進(jìn)行測試。圖2(a)多傳感器融合實時測試設(shè)備；圖2(b)是試驗場景圖，圖2(a)中的箭頭所指的區(qū)域為偽衛(wèi)星信號發(fā)射系統(tǒng)，試驗場地一共有8顆偽衛(wèi)星，為了消除偽衛(wèi)星鐘差，使用了陣列天線，由圖2可以看出，偽衛(wèi)星結(jié)構(gòu)分布較差，且都分布在房屋頂部；圖2(b)中的箭頭所指的區(qū)域為光電捕捉系統(tǒng)設(shè)備，試驗場所二樓一周都布滿了該設(shè)備，長和寬大約都是20 m。本研究利用光電捕捉設(shè)備測量的數(shù)據(jù)作為真值進(jìn)行參考，視覺定位部分采用小覓雙目相機(jī)標(biāo)準(zhǔn)版。為了驗證本文提出的融合算法，使用搭建的測試設(shè)備分別采集兩組場景數(shù)據(jù)(長度分別為41 m和271 m)進(jìn)行對比分析。絕對軌跡誤差(ATE)直接計算相機(jī)位姿的真實值與系統(tǒng)的估計值之間的差，首先根據(jù)位姿的時間戳將真實值和估計值進(jìn)行對齊，然后計算每對位姿之間的差值，該標(biāo)準(zhǔn)適合于估計系統(tǒng)的漂移。

圖2 多傳感器融合設(shè)備和偽衛(wèi)星分布Fig.2 Multi-sensor fusion equipment and pseudosatellite map

3.2 結(jié)果分析

3.2.1 偽衛(wèi)星定位性能分析

PDOP(position ditution of precision)大小能夠反映偽衛(wèi)星結(jié)構(gòu)的好壞，PDOP越小，結(jié)構(gòu)越好。圖3是室內(nèi)試驗場地偽衛(wèi)星PDOP分布?？梢钥闯觯酵闹躊DOP越差，中間區(qū)域結(jié)構(gòu)最好。圖4(a)和4(b)是場景1偽衛(wèi)星定位軌跡對比圖和定位殘差圖；圖5(a)和5(b)是場景2偽衛(wèi)星定位軌跡對比圖和定位殘差圖，真值為光電捕捉數(shù)據(jù)。偽衛(wèi)星實時坐標(biāo)解算采用星間單差的模式，由于在偽衛(wèi)星發(fā)射系統(tǒng)中采用了陣列天線，因此，星間單差能夠同時消除偽衛(wèi)星鐘差和接收機(jī)鐘差。由圖4可以看出，盡管偽衛(wèi)星數(shù)據(jù)不平滑，噪聲較大，但殘差較為均勻，基本都在0.5 m之內(nèi)。由圖4和圖5對比可知，由于場景2測試范圍較大，從圖3可知PDOP因子較差，因此場景2的偽衛(wèi)星定位精度比場景1定位精度差。

圖3 室內(nèi)偽衛(wèi)星PDOP分布圖Fig.3 PDOP distribution map of indoor pseudosatellite

圖4 場景1中偽衛(wèi)星定位效果Fig.4 Pseudosatellite positioning result in scene 1

圖5 場景2中偽衛(wèi)星定位效果Fig.5 Pseudosatellite positioning result in scene 2

3.2.2 視覺定位性能分析

圖6是視覺VIO試驗場景圖。圖7(a)和7(b)分別是場景1的VIO定位軌跡對比圖和VIO定位殘差圖；圖8(a)和8(b)分別是場景2的VIO定位軌跡對比圖和VIO定位殘差圖。由圖7和圖8以看出，雖然VIO數(shù)據(jù)較為平滑，但RMS不均勻，波動較大，特別是場景2，最大誤差達(dá)到2.5 m。

圖6 VIO試驗場景Fig.6 VIO experimental scene diagram

圖7 場景1中VIO定位效果Fig.7 VIO positioning result in scene 1

視覺里程計定位性能受環(huán)境因素和運動狀態(tài)影響比較大。在強(qiáng)光環(huán)境下或者運動狀態(tài)劇烈的時候會導(dǎo)致誤差累積越來越大，甚至?xí)苯邮估锍逃嫆斓簟?/p>

由圖4—圖8可知，盡管偽衛(wèi)星實時定位噪聲較大，但是誤差不會像視覺里程計那樣有累積，絕對誤差的波動幅度會保持在一個相對大小的值。視覺誤差盡管會隨著距離的增加誤差會漸漸累積，但是在短時間內(nèi)會保持在一個極小的值。因此，二者可以實現(xiàn)優(yōu)勢互補(bǔ)。下文就二者的組合定位性能進(jìn)行分析。

圖8 場景2中VIO定位效果Fig.8 VIO positioning result in scene 2

3.2.3 組合定位性能分析

本文主要研究應(yīng)用抗差LM算法進(jìn)行VIO和偽衛(wèi)星融合的問題。為了對比本文算法的性能，下文主要利用經(jīng)典的LM算法優(yōu)化后的效果與本文算法優(yōu)化后的效果及進(jìn)行對比分析。

圖9和圖10是視覺世界坐標(biāo)系與全局坐標(biāo)系的旋轉(zhuǎn)和平移圖，可以通過式(26)求解

(26)

式中，pi,G和qi,G分別是優(yōu)化后全局坐標(biāo)系下的平移和旋轉(zhuǎn)矩陣；pi,C和qi,C是利用視覺慣性里程計輸出的在視覺世界坐標(biāo)系下的平移和旋轉(zhuǎn)矩陣；i指當(dāng)前最新幀。求解qi,CG和pi,CG的目的是為i+1幀全局坐標(biāo)系旋轉(zhuǎn)和平移矩陣提供一個初值。

由于兩個坐標(biāo)系都屬于參考坐標(biāo)系，原則上兩個坐標(biāo)系的平移和旋轉(zhuǎn)矩陣應(yīng)該是固定的。但由于VIO存在誤差累積的因素，不同時刻兩個坐標(biāo)系的旋轉(zhuǎn)和平移是發(fā)生變化的，每個時間段的視覺世界坐標(biāo)系和全局坐標(biāo)系之間的旋轉(zhuǎn)和平移矩陣也會發(fā)生變化，因此視覺世界坐標(biāo)系和全局坐標(biāo)系之間的平移向量的波動較大并不完全等同于系統(tǒng)融合后的定位殘差。由于在測試場地非常整平，因此平移轉(zhuǎn)換時暫不考慮高程的變化。由圖9和圖10可知，全局與視覺坐標(biāo)系之間平移向量X和Y方向的波動幾乎在2 m左右。圖9和圖10下半部分是3個旋轉(zhuǎn)歐拉角變化圖。由圖中可知，除了yaw波動較大外，pitch角和roll角相對都比較穩(wěn)定。這是由于pitch和roll可以通過校準(zhǔn)后的重力方向進(jìn)行糾正，方向是可觀的，而yaw方向無法糾正，因此，yaw角會隨著時間誤差累積越來越大。

圖9 場景1中視覺世界坐標(biāo)系與全局坐標(biāo)系的平移和旋轉(zhuǎn)Fig.9 The translation and rotation of the visual world coordinate system and the global coordinate system in scene 1

圖10 場景2中視覺世界坐標(biāo)系與全局坐標(biāo)系的平移和旋轉(zhuǎn)Fig.10 The translation and rotation of the visual world coordinate system and the global coordinate system in scene 2

圖11是場景1中經(jīng)典LM 融合定位效果，圖12是場景1中抗差LM 融合后定位效果，圖13是場景2中經(jīng)典LM 融合后定位效果，圖14是場景2中抗差LM 融合后定位效果?！癛MS-xy”代表平面的絕對誤差大小。由圖11和圖12可知，經(jīng)典的LM算法融合后，X和Y殘差的最大值都達(dá)到了0.2 m，而利用改進(jìn)的LM算法融合后最大只有0.15 m。表1是場景1和場景2關(guān)于4種定位方案的RMS對比，表中最后兩列分別表示經(jīng)過LM融合后平面方向精度相對于偽衛(wèi)星和VIO提高的百分比(計算方式為：百分比=(A-B)/A，其中，A為改進(jìn)前的RMSE，B為改進(jìn)后的RMSE)。由表1可知利用改進(jìn)的LM算法融合后場景1相對于偽衛(wèi)星和VIO定位精度分別提高了35.5%和59.0%；場景2相對于偽衛(wèi)星和VIO定位精度分別提高了27.8%和77.5%。通過對比場景1和場景2相對于VIO精度的增長率可知：軌跡越長，VIO的誤差累積越大，組合系統(tǒng)相對于單系統(tǒng)VIO的優(yōu)勢越明顯。組合系統(tǒng)能有效消除VIO的誤差累積。

圖11 場景1中經(jīng)典LM融合定位效果Fig.11 Positioning result after classic LM fusion in scene 1

圖12 場景1中抗差LM融合后定位效果Fig.12 Positioning result after classic LM fusion in scene 1

圖13 場景2中經(jīng)典LM融合后定位效果Fig.13 Positioning result after classic LM fusion in scene 2

圖14 場景2中抗差LM融合后定位效果Fig.14 Positioning result after robust LM fusion in scene 2

表1 不同傳感器組合方案平均定位精度對比Tab.1 Comparison of average positioning accuracy of different sensor combination schemes

3.2.4 遮擋下組合定位分析

由于VIO是VO與IMU采用緊組合建模，視覺在短時間內(nèi)具備較高的相對定位精度，因此，IMU的加速度計偏差和陀螺儀偏差在短時間內(nèi)能夠通過VO進(jìn)行修正，因此，VIO在短時間內(nèi)能夠得到較為平滑的高精度定位數(shù)據(jù)。為了驗證偽衛(wèi)星在遮擋下組合系統(tǒng)中定位系統(tǒng)定位的性能，本試驗場景1在12～17 s及29～34 s兩個時段給偽衛(wèi)星設(shè)置全遮擋；場景2在35～65 s及85～135 s兩個時段給偽衛(wèi)星設(shè)置全遮擋；在135～185 s時間段給偽衛(wèi)星設(shè)置半遮擋(即在試驗中有1～3顆衛(wèi)星被遮擋。由于偽衛(wèi)星發(fā)射器利用了陣列天線，利用星間單差就可以基本消除偽衛(wèi)星鐘差和接收機(jī)鐘差，因此4顆衛(wèi)星就可以完成定位)。

圖15(a)和圖16(a)是偽衛(wèi)星存在短時間遮擋下與視覺VIO組合定位軌跡圖，圖15(b)是場景1定位殘差圖，16(b)是場景2可用偽衛(wèi)星數(shù)及定位殘差圖。由圖15(b)可以看出，場景1的最大殘差不超過0.2 m，除了在30 s和50 s處有較小的波動外，其余部分都非常穩(wěn)定；場景2的殘差波動較大，但整體都在1 m之內(nèi)。場景2的遮擋分為全遮擋和半遮擋。由圖中可以看出，當(dāng)可用偽衛(wèi)星個數(shù)滿足定位要求時，組合定位性能基本不受影響。結(jié)合圖2和圖3通過分析可知，由于偽衛(wèi)星都分布在試驗場頂部，而且偽衛(wèi)星都是靜止不動的，PDOP較差，因此在滿足定位要求后，偽衛(wèi)星的個數(shù)對定位結(jié)果影響較小?？芍词箓涡l(wèi)星在短暫遮擋下，基本不影響組合系統(tǒng)定位性能，組合系統(tǒng)依然能夠輸出較高精度定位數(shù)據(jù)。

圖15 場景1中VIO遮擋下定位效果圖Fig.15 Positioning result under VIO occlusion in scene 1

圖16 場景2中VIO遮擋下定位效果圖Fig.16 Positioning result under VIO occlusion in scene 2

圖17分別是不同頻率(5、1和0.2 Hz)的偽衛(wèi)星輸出結(jié)果與視覺慣性里程計組合后的定位軌跡和定位殘差圖，平均定位殘差分別是0.254、0.278、0.312 m。經(jīng)過分析可知，偽衛(wèi)星的頻率越高，內(nèi)插精度也就越高，因此能更精確地修正陀螺儀和加速度計的偏差，組合后的精度也就更高。由于在室內(nèi)多徑現(xiàn)象比較嚴(yán)重，如果采集頻率過高，信號可能會跟蹤失敗，而且5 Hz組合后的結(jié)果相對于1 Hz和0.2 Hz提高并不多，因此在實際組合定位中可以選擇1 Hz或者更低的頻率。

圖17 不同偽衛(wèi)星頻率組合后定位效果圖Fig.17 Positioning result after combining different pseudosatellite frequencies

4 結(jié) 論

本文提出了改進(jìn)的LM算法，并把改進(jìn)的LM算法用于視覺VIO和偽衛(wèi)星融合模型優(yōu)化中。首先，分別測試了視覺VIO和偽衛(wèi)星定位的精度。然后，把本文改進(jìn)的LM算法與經(jīng)典的LM算法進(jìn)行對比。通過實例數(shù)據(jù)分析，場景1中利用本文改進(jìn)的抗差LM算法融合后相對于偽衛(wèi)星和視覺VIO定位精度分別提高了35.5%和59.0%；場景2中利用本文改進(jìn)的抗差LM算法融合后相對于偽衛(wèi)星和視覺VIO定位精度分別提高了27.8%和77.5%。

本文測試了偽衛(wèi)星在短暫遮擋下組合系統(tǒng)定位性能，通過試驗可以得出，即使偽衛(wèi)星在遮擋情況下，組合系統(tǒng)定位性能基本不受影響，場景1和場景2的平均定位殘差都在0.3 m以內(nèi)，依然能保持定位的可靠性和連續(xù)性。最后，測試了不同頻率(5、1和0.2 Hz)的偽衛(wèi)星輸出結(jié)果與視覺慣性里程計組合后的定位性能。通過分析可知，盡管頻率越高，組合后定位精度越高，但是精度提高幅度較小，因此在實際組合定位中為了避免信號跟蹤失敗可以選擇1 Hz或者更低的頻率。