基于抗差LM的視覺慣性里程計(jì)與偽衛(wèi)星混合高精度室內(nèi)定位

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

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

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

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

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

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

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

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

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

由于在室內(nèi)無法接收到GNSS信號(hào)，因此在室內(nèi)就無法利用GNSS消除VIO的累積誤差。偽衛(wèi)星又稱“地面衛(wèi)星”，是從地面某特定地點(diǎn)發(fā)射類似于GNSS的導(dǎo)航信號(hào)，采用的電文格式與GNSS基本一致。偽衛(wèi)星系統(tǒng)作為GNSS定位系統(tǒng)的輔助手段和工具，即可以用來輔助增強(qiáng)GNSS在某些惡劣環(huán)境下的定位性能，也可以單獨(dú)使用構(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)星鐘差采用的是原子鐘，精度較高，因此，在一般精度定位中可以忽略不計(jì)，但是原子鐘價(jià)格昂貴，室內(nèi)偽衛(wèi)星采用不合適。由于偽衛(wèi)星鐘差的影響，單接收機(jī)無法完成定位，必須采用站間和星間雙差技術(shù)，這在實(shí)際應(yīng)用中非常不便。為了解決偽衛(wèi)星鐘差的問題，目前很多研究機(jī)構(gòu)都是采用陣列天線來解決這個(gè)問題。陣列天線中所有的偽衛(wèi)星鐘差都是一樣的，因此通過星間單差技術(shù)可以同時(shí)消去接收機(jī)鐘差和偽衛(wèi)星鐘差[14-16]。

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

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

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

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

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

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

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

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

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

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

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

(1)

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

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

由于偽衛(wèi)星不能求出旋轉(zhuǎn)向量，因此在優(yōu)化的時(shí)候只能利用坐標(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分別是陀螺儀和加速度計(jì)偏置；i表示滑動(dòng)窗口中幀數(shù)；C、S和G代表視覺世界、偽衛(wèi)星和融合后全局坐標(biāo)系標(biāo)志。

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

1.2.1VIO約束

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

(4)

(5)

(6)

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

(7)

1.2.2 偽衛(wèi)星約束

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

(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)系相對(duì)全局坐標(biāo)系下的旋轉(zhuǎn)矩陣，至少需要提供4個(gè)歷元的位置約束。假設(shè)滑窗寬度為10，首先利用擴(kuò)展卡爾曼濾波初始化全局坐標(biāo)。

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

(11)

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

(12)

當(dāng)i=1時(shí)

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

1.3 有效性檢核

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

已知第i+1幀相對(duì)視覺世界坐標(biāo)系(視覺初始化時(shí)的參考坐標(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時(shí)刻的坐標(biāo)為pi+1,S，方差為σ2，可以建立檢核方程為

(20)

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

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

(Hk+μkI)Δxk=gk

(21)

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

(22)

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

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

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

(23)

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

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

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

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

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

(24)

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

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

(25)

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

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

3 試驗(yàn)驗(yàn)證及分析

3.1 試驗(yàn)驗(yàn)證

為了驗(yàn)證本文提出的多傳感器融合算法，本文利用筆者團(tuán)隊(duì)搭建的組合設(shè)備在某所C7試驗(yàn)場(chǎng)進(jìn)行測(cè)試。圖2(a)多傳感器融合實(shí)時(shí)測(cè)試設(shè)備；圖2(b)是試驗(yàn)場(chǎng)景圖，圖2(a)中的箭頭所指的區(qū)域?yàn)閭涡l(wèi)星信號(hào)發(fā)射系統(tǒng)，試驗(yàn)場(chǎng)地一共有8顆偽衛(wèi)星，為了消除偽衛(wèi)星鐘差，使用了陣列天線，由圖2可以看出，偽衛(wèi)星結(jié)構(gòu)分布較差，且都分布在房屋頂部；圖2(b)中的箭頭所指的區(qū)域?yàn)楣怆姴蹲较到y(tǒng)設(shè)備，試驗(yàn)場(chǎng)所二樓一周都布滿了該設(shè)備，長和寬大約都是20 m。本研究利用光電捕捉設(shè)備測(cè)量的數(shù)據(jù)作為真值進(jìn)行參考，視覺定位部分采用小覓雙目相機(jī)標(biāo)準(zhǔn)版。為了驗(yàn)證本文提出的融合算法，使用搭建的測(cè)試設(shè)備分別采集兩組場(chǎng)景數(shù)據(jù)(長度分別為41 m和271 m)進(jìn)行對(duì)比分析。絕對(duì)軌跡誤差(ATE)直接計(jì)算相機(jī)位姿的真實(shí)值與系統(tǒng)的估計(jì)值之間的差，首先根據(jù)位姿的時(shí)間戳將真實(shí)值和估計(jì)值進(jìn)行對(duì)齊，然后計(jì)算每對(duì)位姿之間的差值，該標(biāo)準(zhǔn)適合于估計(jì)系統(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)試驗(yàn)場(chǎng)地偽衛(wèi)星PDOP分布?？梢钥闯觯酵闹躊DOP越差，中間區(qū)域結(jié)構(gòu)最好。圖4(a)和4(b)是場(chǎng)景1偽衛(wèi)星定位軌跡對(duì)比圖和定位殘差圖；圖5(a)和5(b)是場(chǎng)景2偽衛(wèi)星定位軌跡對(duì)比圖和定位殘差圖，真值為光電捕捉數(shù)據(jù)。偽衛(wèi)星實(shí)時(shí)坐標(biāo)解算采用星間單差的模式，由于在偽衛(wèi)星發(fā)射系統(tǒng)中采用了陣列天線，因此，星間單差能夠同時(shí)消除偽衛(wèi)星鐘差和接收機(jī)鐘差。由圖4可以看出，盡管偽衛(wèi)星數(shù)據(jù)不平滑，噪聲較大，但殘差較為均勻，基本都在0.5 m之內(nèi)。由圖4和圖5對(duì)比可知，由于場(chǎng)景2測(cè)試范圍較大，從圖3可知PDOP因子較差，因此場(chǎng)景2的偽衛(wèi)星定位精度比場(chǎng)景1定位精度差。

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

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

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

3.2.2 視覺定位性能分析

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

圖6 VIO試驗(yàn)場(chǎng)景Fig.6 VIO experimental scene diagram

圖7 場(chǎng)景1中VIO定位效果Fig.7 VIO positioning result in scene 1

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

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

圖8 場(chǎng)景2中VIO定位效果Fig.8 VIO positioning result in scene 2

3.2.3 組合定位性能分析

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

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

(26)

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

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

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

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

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

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

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

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

3.2.4 遮擋下組合定位分析

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

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

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

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

圖17分別是不同頻率(5、1和0.2 Hz)的偽衛(wèi)星輸出結(jié)果與視覺慣性里程計(jì)組合后的定位軌跡和定位殘差圖，平均定位殘差分別是0.254、0.278、0.312 m。經(jīng)過分析可知，偽衛(wèi)星的頻率越高，內(nèi)插精度也就越高，因此能更精確地修正陀螺儀和加速度計(jì)的偏差，組合后的精度也就更高。由于在室內(nèi)多徑現(xiàn)象比較嚴(yán)重，如果采集頻率過高，信號(hào)可能會(huì)跟蹤失敗，而且5 Hz組合后的結(jié)果相對(duì)于1 Hz和0.2 Hz提高并不多，因此在實(shí)際組合定位中可以選擇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)化中。首先，分別測(cè)試了視覺VIO和偽衛(wèi)星定位的精度。然后，把本文改進(jìn)的LM算法與經(jīng)典的LM算法進(jìn)行對(duì)比。通過實(shí)例數(shù)據(jù)分析，場(chǎng)景1中利用本文改進(jìn)的抗差LM算法融合后相對(duì)于偽衛(wèi)星和視覺VIO定位精度分別提高了35.5%和59.0%；場(chǎng)景2中利用本文改進(jìn)的抗差LM算法融合后相對(duì)于偽衛(wèi)星和視覺VIO定位精度分別提高了27.8%和77.5%。

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