本文内容來源于《測繪學報》2022年第1期（審圖号GS（2022）104号）

　　基于抗差LM的視覺慣性裡程計與僞衛星混合高精度室内定位

　　楊高朝1,2

　　, 王慶1,2

　　, 蔚保國3, 劉鵬飛1,2,李爽3

　　1. 東南大學儀器科學與工程學院, 江蘇 南京 210096;

　　2. 東南大學智慧城市研究院, 江蘇 南京 210096;

　　3. 衛星導航系統與裝備技術國家重點實驗室, 河北 石家莊 050050

　　基金項目：國家重點研發計劃（2020YFD110011-01）

　　摘要：視覺慣性裡程計（VIO）和僞衛星已經廣泛應用于室内環境定位中，但在實際應用中，二者各自都有明顯的缺陷。視覺裡程計依賴于實際場景，在景深變化明顯和光照不均勻的環境下會産生粗差，而且誤差會不可避免地随着時間累積，但是在相鄰幀間能保證相對高精度的位姿測量。由于受到室内多徑的影響，僞衛星室内定位的精度和可靠性很難保證。為增加室内定位的可靠性和穩定性，基于抗差LM非線性優化理論，本文主要研究利用視覺慣性裡程計的相鄰幀間高精度位姿測量和僞衛星融合的室内高精度定位技術。該算法不僅可以抵抗粗差，而且可以減弱不同傳感器間權重設置不合理帶來的影響。最後使用在室内環境下搭建的高精度動态捕捉設備對組合定位方法進行實驗驗證。試驗結果表明，該方法不依賴回環即可消除視覺慣性裡程計的累積誤差，有效提高室内定位精度及可靠性。利用改進的LM算法融合後場景1和場景2，相對于VIO單獨定位精度分别提高了59.0%和77.5%。

　　關鍵詞：視覺慣性裡程計 僞衛星 LM 抗差估計 室内定位

　　引文格式：楊高朝, 王慶, 蔚保國, 等. 基于抗差LM的視覺慣性裡程計與僞衛星混合高精度室内定位[J]. 測繪學報，2022，51(1)：18-30. DOI: 10.11947/j.AGCS.2022.20200251

　　YANG Gaochao, WANG Qing, YU Baoguo, et al. High-precision indoor positioning based on robust LM visual inertial odometer and pseudosatellite[J]. Acta Geodaetica et Cartographica Sinica, 2022, 51(1): 18-30. DOI: 10.11947/j.AGCS.2022.20200251

　　閱讀全文：http://xb.sinomaps.com/article/2022/1001-1595/2022-1-18.htm

　　引 言

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

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

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

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

　　文獻[11-12]提出基于優化的方式，支持雙目配置、多軌迹融合以及GNSS等全局傳感器與VO進行聯合優化。

　　文獻[11]主要思想是，首先進行局部位姿估計，即傳統意義上的VO或者VIO，得到局部的位姿估計之後，再與GNSS等全局傳感器進行對齊。對齊的方式是建立一個位姿圖，每個GNSS時刻建立一個位姿節點，連續的兩個節點之間将局部位姿估計得到的相對位姿作為約束。每個節點還與GNSS等全局位置建立約束。

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

　　由于在室内無法接收到GNSS信号，因此在室内就無法利用GNSS消除VIO的累積誤差。僞衛星又稱“地面衛星”，是從地面某特定地點發射類似于GNSS的導航信号，采用的電文格式與GNSS基本一緻。僞衛星系統作為GNSS定位系統的輔助手段和工具，即可以用來輔助增強GNSS在某些惡劣環境下的定位性能，也可以單獨使用構建僞衛星定位系統。因此，在室内，可以利用僞衛星與視覺VIO進行組合消除VIO的誤差累積[13]。但是僞衛星在室内應用中也面臨着兩大挑戰。

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

　　室内多徑的影響。GNSS一般應用于室外定位，多徑一般都忽略不計。但是室内結構複雜，多徑對僞距的影響往往能達到幾十米甚至幾百米。而僞衛星通常是靜止于室内的，無法通過多曆元平滑來減弱多徑。目前僞衛星多徑解決方法主要分為3類：基帶解調端、接收機端及數據處理端。因此，在實際室内應用中隻利用載波觀測值。由于隻利用載波觀測值會導緻秩虧問題(N顆衛星至少需要解算(N 3)個參數)，為了解決這個問題，在動态應用前一般需要在已知位置上進行初始化一分鐘[17-19]。

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

　　1 多源數據融合約束方程的建

　　立

　　1.1 數據内插

　　多傳感器的融合時間戳對齊是非常關鍵的一步。時間戳無法對齊或對齊錯誤最後可能得出一個錯誤的軌迹，很可能使優化算法失效。為了保證融合算法的穩健性，視覺傳感器的時間戳與僞衛星的時間戳相差不超過10ms。

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

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

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

　　(3) S(ti)在整個區間上t1,tn有連續的一階及二階導數。

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

　　(1)

　　式中，ai、bi、ci、di待定。根據二階微分連續性，可得

　　S″i(ti 1)=S″i 1(ti 1)。其中，ti為僞衛星時間戳。S′i(t)和S″i(t)分别是Si(t)的一階和二階導數。

　　由文獻[25-26]可知，可列出(n-1)個方程，但是有(n 1)個參數，因此求解該方程組，還需添加其他約束。根據文獻[27]，本文選擇“自由邊界”約束，即：

　　ai、bi、ci和di，關于y(ti)和z(ti)的内插同x(ti)。解算出這些系數後，可根據視覺對應的時間戳内插出該時間階段任意時刻的三維坐标值。

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

　　為了實時進行僞衛星和VIO的融合，本文選擇滑窗融合模式，窗口長度為10。僞衛星坐标系采用室内局部坐标系。為防止秩虧現象發生，首先把融合後的地圖坐标系第一幀與僞衛星坐标系對齊。

　　由于僞衛星不能求出旋轉向量，因此在優化的時候隻能利用坐标向量和速度向量。總的融合約束方程可以寫為

　　(2)

　　式中, rC和rS分别是VIO和僞衛星測量殘差；pi,Ci 1和qi,Ci 1分别是VIO的平移和旋轉測量值；pi,S是僞衛星測量值；χ是需要優化的參數。

　　(3)

　　式中，pi,G、Vi,G和qi,G是優化後的全局平移、速度和旋轉參數；bi,g和bi,a分别是陀螺儀和加速度計偏置；i表示滑動窗口中幀數；C、S和G代表視覺世界、僞衛星和融合後全局坐标系标志。

　　由于pi,CG、qi,CG和視覺VIO密切相關，而視覺VIO會随着時間有誤差累積，因此,pi,CG和qi,CG也會有誤差累積。為了使優化後的位姿更加平滑，本次研究選擇實時優化這兩個向量。

　　1.2.1 VIO約束

　　由于VIO的尺度漂移是不穩定的，直接把VIO的位姿放在全局坐标系下建立觀測方程去估計尺度是不夠準确的，因此，把VIO的兩幀之間相對的位姿去建立觀測方程

　　(4)

　　(5)

　　(6)

　　視覺目标函數可寫為

　　(7)

　　根據經驗值選取權值初值PLi,Cq=0.1、PLi,Cp=0.01和PLi,CV=0.01[9]；θ是旋轉矩陣的歐拉角；i是滑窗内數據索引(i=1, 2，…10)；pi,C、Vi,C和qi,C是VIO位置、速度和旋轉向量；pi,C、pi,s、qi,C和qi,s為後端輸出數據；pi,Ci 1、Vi,Ci 1和qi,Ci 1分别為VIO相鄰兩幀間的平移、速度和旋轉變化量。VIO相對于各參數雅克比向量見參考文獻[11]。

　　1.2.2 僞衛星約束

　　僞衛星約束計算公式為

　　(8)

　　式中，PLi,Sp和PLi,SV為僞衛星的定位和測速的精度因子；ωGCC為陀螺儀測量出的視覺相對于僞衛星全局坐标系的角速率；LCGC為僞衛星接收機天線相對于載體坐标系的平移向量；(·)×表示反對稱矩陣；Vi,S可以根據瞬時差分多普勒求得。因為僞衛星無法求出旋轉角度，因此隻利用位置和速度信息進行約束。δpi,S關于pi,G，θi,G，Vi,G，bi,g和bi,a的雅克比矩陣為

　　(9)

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

　　(10)

　　1.2.3 基于擴展卡爾曼濾波的全局坐标初始化

　　pk,G和qk,G為全局坐标系下的平移和旋轉矩陣。由于僞衛星隻能提供全局位置和速度約束，因此若想求得當前坐标系相對全局坐标系下的旋轉矩陣，至少需要提供4個曆元的位置約束。假設滑窗寬度為10，首先利用擴展卡爾曼濾波初始化全局坐标。

　　狀态預測方程為

　　(11)

　　狀态更新方程為

　　(12)

　　當i=1時

　　(13)

　　(14)

　　(15)

　　假設i是當前幀，根據pi,C、qi,C、pi,G及qi,G，可以求得視覺世界坐标系到全局坐标系的旋轉和平移矩陣pi,CG和qi,CG為

　　(16)

　　(17)

　　(18)

　　由于VIO随着時間誤差會累積，因此，為了更加準确地求取視覺世界坐标系到全局坐标系的轉換，在每次優化完成後重新求取pi,CG和qi,CG。

　　1.3 有效性檢核

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

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

　　(19)

　　假設僞衛星在i 1時刻的坐标為pi 1,S，方差為σ2，可以建立檢核方程為

　　(20)

　　2 改進的抗差LM非線性優化的融合策略

　　已知LM非線性優化模型為

　　(21)

　　式中，

　　(22)

　　因此，LM算法可以看作給變化量Δx添加一個信賴區域來限制Δx的大小，并認為在信賴區域裡面近似是有效的，否則近似不準确。

　　确定信賴區域一個好的辦法是通過比較近似模型和實際模型的差異來确定，如果差異小，就增大範圍；如果差異太大，就縮小範圍。考慮實際模型和近似模型變化量的比值

　　(23)

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

　　(1) 給定初始叠代值x0(坐标向量初值可由僞衛星給出，旋轉向量初值由視覺VIO提供)及調節因子μ0：μ0=max([hii])，其中hii是H0的對角線元素。

　　(2) 對于第k次叠代，求解優化問題。

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

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

　　當μk較小時，說明Hk占主導地位，說明二次近似在該範圍内是比較好的，LM方法更接近于高斯牛頓法；另一方面，當μk較大時，LM算法更接近一階梯度下降算法，這說明二次近似不夠好。LM算法的求解方式，可以避免線性方程組的矩陣非奇異和病态等問題，提供更穩定、更準确的解法。

　　在經典LM算法中，從信賴域法角度分析，μk可以看作是每次叠代約束區域的半徑，其實是一個球形的區域，該約束認為隻有在球内才是有效的。

　　Jf為f的一階雅克比矩陣，這就默認了各參數分量的約束是等權的。H近似等于Hessian矩陣，為了更加高效地約束Δxk的變化，變形如下

　　(24)

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

　　盡管LM能夠克服Hessian矩陣病态解的影響，由于LM算法采用最小二乘估計作為收斂條件從而不能抵禦觀測數據中心粗差的幹擾，特别是室内僞衛星定位時，由于室内多徑比較嚴重，即使僞衛星原始觀測數據能夠通過上文的有效性檢核，仍舊很難保證僞衛星定位的穩定性和可靠性。若直接使用LM算法進行優化，則會使得求解出的融合解偏離真實值。

　　為了抵消觀測值粗差對模型解的影響，可以采用抗差估計來減弱粗差對估值的影響，得到LM算法的抗差解。把式(7)和式(8)中的權P換為等價權P，再計算

　　x*為求得的最優解。

　　采用IGGⅢ等價權函數計算P，令

　　υ為觀測值的改正數，σ為改正數的中誤差。計算P的模型為[27-28]

　　(25)

　　式中，p是P的一個元素；

　　d≤1；k0可取值1.0~1.2；k1可取值2.0~4.0。

　　本次研究的多傳感器融合具體流程如圖 1所示，本文主要研究基于抗差LM優化的VIO與僞衛星後端松耦合技術。下面主要通過試驗對比來分析融合的優勢。

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

　　圖選項

　　3 試驗驗證及分析

　　3.1 試驗驗證

　　為了驗證本文提出的多傳感器融合算法，本文利用筆者團隊搭建的組合設備在某所C7試驗場進行測試。圖 2(a)多傳感器融合實時測試設備；圖 2(b)是試驗場景圖，圖 2(a)中的箭頭所指的區域為僞衛星信号發射系統，試驗場地一共有8顆僞衛星，為了消除僞衛星鐘差，使用了陣列天線，由圖 2可以看出，僞衛星結構分布較差，且都分布在房屋頂部；圖 2(b)中的箭頭所指的區域為光電捕捉系統設備，試驗場所二樓一周都布滿了該設備，長和寬大約都是20m。本研究利用光電捕捉設備測量的數據作為真值進行參考，視覺定位部分采用小覓雙目相機标準版。為了驗證本文提出的融合算法，使用搭建的測試設備分别采集兩組場景數據(長度分别為41m和271m)進行對比分析。絕對軌迹誤差(ATE)直接計算相機位姿的真實值與系統的估計值之間的差，首先根據位姿的時間戳将真實值和估計值進行對齊，然後計算每對位姿之間的差值，該标準适合于估計系統的漂移。

　　圖 2 多傳感器融合設備和僞衛星分布 Fig. 2 Multi-sensor fusion equipment and pseudosatellite map

　　圖選項

　　3.2 結果分析

　　3.2.1 僞衛星定位性能分析

　　PDOP(position ditution of precision)大小能夠反映僞衛星結構的好壞，PDOP越小，結構越好。圖 3是室内試驗場地僞衛星PDOP分布。可以看出，越往四周PDOP越差，中間區域結構最好。圖 4(a)和4(b)是場景1僞衛星定位軌迹對比圖和定位殘差圖；圖 5(a)和5(b)是場景2僞衛星定位軌迹對比圖和定位殘差圖，真值為光電捕捉數據。僞衛星實時坐标解算采用星間單差的模式，由于在僞衛星發射系統中采用了陣列天線，因此，星間單差能夠同時消除僞衛星鐘差和接收機鐘差。由圖 4可以看出，盡管僞衛星數據不平滑，噪聲較大，但殘差較為均勻，基本都在0.5m之内。由圖 4和圖 5對比可知，由于場景2測試範圍較大，從圖 3可知PDOP因子較差，因此場景2的僞衛星定位精度比場景1定位精度差。

　　圖 3 室内僞衛星PDOP分布圖 Fig. 3 PDOP distribution map of indoor pseudosatellite

　　圖選項

　　圖 4 場景1中僞衛星定位效果 Fig. 4 Pseudosatellite positioning result in scene 1

　　圖選項

　　圖 5 場景2中僞衛星定位效果 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數據較為平滑，但RMS不均勻，波動較大，特别是場景2，最大誤差達到2.5m。

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

　　圖選項

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

　　圖選項

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

　　圖選項

　　視覺裡程計定位性能受環境因素和運動狀态影響比較大。在強光環境下或者運動狀态劇烈的時候會導緻誤差累積越來越大，甚至會直接使裡程計挂掉。

　　由圖 4-圖 8可知，盡管僞衛星實時定位噪聲較大，但是誤差不會像視覺裡程計那樣有累積，絕對誤差的波動幅度會保持在一個相對大小的值。視覺誤差盡管會随着距離的增加誤差會漸漸累積，但是在短時間内會保持在一個極小的值。因此，二者可以實現優勢互補。下文就二者的組合定位性能進行分析。

　　3.2.3 組合定位性能分析

　　本文主要研究應用抗差LM算法進行VIO和僞衛星融合的問題。為了對比本文算法的性能，下文主要利用經典的LM算法優化後的效果與本文算法優化後的效果及進行對比分析。

　　圖 9和圖 10是視覺世界坐标系與全局坐标系的旋轉和平移圖，可以通過式(26)求解

　　圖 9 場景1中視覺世界坐标系與全局坐标系的平移和旋轉 Fig. 9 The translation and rotation of the visual world coordinate system and the global coordinate system in scene 1

　　圖選項

　　圖 10 場景2中視覺世界坐标系與全局坐标系的平移和旋轉 Fig. 10 The translation and rotation of the visual world coordinate system and the global coordinate system in scene 2

　　圖選項

　　(26)

　　式中，pi,G和qi,G分别是優化後全局坐标系下的平移和旋轉矩陣；pi,C和qi,C是利用視覺慣性裡程計輸出的在視覺世界坐标系下的平移和旋轉矩陣；i指當前最新幀。求解qi,CG和pi,CG的目的是為i 1幀全局坐标系旋轉和平移矩陣提供一個初值。

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

　　圖 11是場景1中經典LM融合定位效果，圖 12是場景1中抗差LM融合後定位效果，圖 13是場景2中經典LM融合後定位效果，圖 14是場景2中抗差LM融合後定位效果。“RMS-xy”代表平面的絕對誤差大小。由圖 11和圖 12可知，經典的LM算法融合後，X和Y殘差的最大值都達到了0.2m，而利用改進的LM算法融合後最大隻有0.15m。表 1是場景1和場景2關于4種定位方案的RMS對比，表中最後兩列分别表示經過LM融合後平面方向精度相對于僞衛星和VIO提高的百分比(計算方式為：百分比=(A-B)/A，其中，A為改進前的RMSE，B為改進後的RMSE)。由表 1可知利用改進的LM算法融合後場景1相對于僞衛星和VIO定位精度分别提高了35.5%和59.0%；場景2相對于僞衛星和VIO定位精度分别提高了27.8%和77.5%。通過對比場景1和場景2相對于VIO精度的增長率可知：軌迹越長，VIO的誤差累積越大，組合系統相對于單系統VIO的優勢越明顯。組合系統能有效消除VIO的誤差累積。

　　圖 11 場景1中經典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中經典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采用緊組合建模，視覺在短時間内具備較高的相對定位精度，因此，IMU的加速度計偏差和陀螺儀偏差在短時間内能夠通過VO進行修正，因此，VIO在短時間内能夠得到較為平滑的高精度定位數據。為了驗證僞衛星在遮擋下組合系統中定位系統定位的性能，本試驗場景1在12~17s及29~34s兩個時段給僞衛星設置全遮擋；場景2在35~65s及85~135s兩個時段給僞衛星設置全遮擋；在135~185s時間段給僞衛星設置半遮擋(即在試驗中有1~3顆衛星被遮擋。由于僞衛星發射器利用了陣列天線，利用星間單差就可以基本消除僞衛星鐘差和接收機鐘差，因此4顆衛星就可以完成定位)。

　　圖 15(a)和圖 16(a)是僞衛星存在短時間遮擋下與視覺VIO組合定位軌迹圖，圖 15(b)是場景1定位殘差圖，16(b)是場景2可用僞衛星數及定位殘差圖。由圖 15(b)可以看出，場景1的最大殘差不超過0.2m，除了在30s和50s處有較小的波動外，其餘部分都非常穩定；場景2的殘差波動較大，但整體都在1m之内。場景2的遮擋分為全遮擋和半遮擋。由圖中可以看出，當可用僞衛星個數滿足定位要求時，組合定位性能基本不受影響。結合圖 2和圖 3通過分析可知，由于僞衛星都分布在試驗場頂部，而且僞衛星都是靜止不動的，PDOP較差，因此在滿足定位要求後，僞衛星的個數對定位結果影響較小。可知，即使僞衛星在短暫遮擋下，基本不影響組合系統定位性能，組合系統依然能夠輸出較高精度定位數據。

　　圖 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.2Hz)的僞衛星輸出結果與視覺慣性裡程計組合後的定位軌迹和定位殘差圖，平均定位殘差分别是0.254、0.278、0.312m。經過分析可知，僞衛星的頻率越高，内插精度也就越高，因此能更精确地修正陀螺儀和加速度計的偏差，組合後的精度也就更高。由于在室内多徑現象比較嚴重，如果采集頻率過高，信号可能會跟蹤失敗，而且5Hz組合後的結果相對于1Hz和0.2Hz提高并不多，因此在實際組合定位中可以選擇1Hz或者更低的頻率。

　　圖 17 不同僞衛星頻率組合後定位效果圖 Fig. 17 Positioning result after combining different pseudosatellite frequencies

　　圖選項

　　4 結 論

　　本文提出了改進的LM算法，并把改進的LM算法用于視覺VIO和僞衛星融合模型優化中。首先，分别測試了視覺VIO和僞衛星定位的精度。然後，把本文改進的LM算法與經典的LM算法進行對比。通過實例數據分析，場景1中利用本文改進的抗差LM算法融合後相對于僞衛星和視覺VIO定位精度分别提高了35.5%和59.0%；場景2中利用本文改進的抗差LM算法融合後相對于僞衛星和視覺VIO定位精度分别提高了27.8%和77.5%。

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

　　作者簡介

　　第一作者簡介：楊高朝(1987—), 男, 博士生, 研究方向為多傳感器信息融合導航定位。E-mail: [email protected]

　　通信作者：王慶, E-mail：[email protected]

　　初審：張豔玲

　　複審：宋啟凡

　　終審：金 君

　　往期推薦

　　資訊

　　○ 中國大學MOOC 地理信息系統（GIS）實驗第六期課程即将開課！

　　○ 測繪遙感國家重點實驗室馬盈盈副研究員利用遙感氣象要素助力冬奧

　　○ 山東省水利勘測設計院有限公司2022年公開招聘！含測繪、地信、遙感相關專業

　　○ 自然資源部：這11家導航電子地圖制作甲級測繪資質單位複審換證通過

　　會議

　　○ Geoinformatics 2022 暨CPGIS成立30周年年會（二号通知）

　　○ 會議通知 | 第三屆IEEE地質、測繪與遙感國際學術會議

　　○ 關于召開大地測量與導航2022年綜合學術年會的通知（第一輪）

　　○ 誠邀莅臨丨“新測繪 新融合 新跨越”南方測繪2022用戶大會

　　《測繪學報》

　　○測繪學報 | 劉萬科：非完整約束的OD/SINS自适應組合導航方法

　　○ 測繪學報 | 李德仁：論萬物互聯時代的地球空間信息學

　　○ 高被引論文 | 藍朝桢：異源遙感影像特征匹配的深度學習算法

　　○ 高被引論文 | 張永軍：天空地多源遙感數據的廣義攝影測量學

　　《測繪通報》

　　○《測繪通報》2021年第12期目錄

　　○ 《測繪通報》2022年第1期目錄

　　○ 地市級實景三維城市建設及應用

　　○ 圖殘差神經網絡支持下的建築物群組模式分類

　　《北京測繪》

　　○《北京測繪》2022年第1期摘要推薦

　　○《北京測繪》2021年第12期摘要推薦

　　○《北京測繪》2021年第11期摘要推薦

　　○《北京測繪》2021年第10期摘要推薦

　　《測繪科學技術學報》

　　○ 摘要 |《測繪科學技術學報》2021年第2期摘要推薦

　　○ 摘要 |《測繪科學技術學報》2021年第4期摘要推薦

　　○ 摘要 |《測繪科學技術學報》2021年第5期摘要推薦

　　○ 摘要 |《測繪科學技術學報》2021年第6期摘要推薦

　　《地球信息科學學報》

　　○ 《地球信息科學學報》2022年第1期佳文推介

　　○ 龔健雅院士：全球位置信息疊加協議與位置服務網技術研究進展與展望

　　○ 佳文推介 | 大數據城市通勤交通模型的構建與模拟應用

　　○ 專刊征稿：社會感知與地理大數據挖掘（征稿中）

　　《測繪工程》

　　○ 摘要 |《測繪工程》2021年第5期摘要推薦

　　○ 摘要 |《測繪工程》2021年第4期摘要推薦

　　○ 摘要 |《測繪工程》2021年第6期摘要推薦

　　○ 佳文推介 | 單目視覺技術在室内定位中的應用研究

　　《中國空間科學技術》

　　○ 火衛二地形地貌探測綜述

　　○《中國空間科學技術》2022年第1期摘要

　　○《中國空間科學技術》2021年第6期摘要

　　○《中國空間科學技術》2021年第5期摘要推薦

　　《衛星應用》

　　○《衛星應用》2021年第12期摘要○《衛星應用》2021年第11期摘要○《衛星應用》2021年第10期摘要○ 摘要 |《衛星應用》2021年第9期摘要推薦 《Journal of Geodesy and Geoinformation Science》

　　○《測繪學報（英文版）》專刊征稿 | 地圖學與地球空間信息教育：理論與實踐

　　○ 《測繪學報（英文版）》專刊征稿 | 用于三維地理信息的攝影測量和計算機視覺

　　○ Kexian WANG et al. | 《測繪學報（英文版）》（JGGS）精選論文

　　○ Huan XU et al. | 《測繪學報（英文版）》（JGGS）精選論文

　　《Satellite Navigation》

　　○ 徐元博士：面向有色測量噪聲下UWB/INS組合行人導航的分布式卡爾曼濾波| SANA佳文速遞

　　○ 楊飛博士：GNSS天頂對流層精化模型的構建與分析| SANA佳文速遞

　　○ 姚铮教授：北鬥PPP服務信号調制複用方案設計與質量評估| SANA佳文速遞

　　○ 2022征文| SatNav“普适定位、室内導航與基于位置服務”專題

　　《自然資源遙感》

　　○ 《自然資源遙感》入駐“智繪科服”融媒體平台！

　　○ 《自然資源遙感》征稿：“海岸帶空間資源及生态健康遙感監測”專欄

　　○ 摘要 |《自然資源遙感》2021年第3期摘要推薦

　　○ 摘要 |《自然資源遙感》2021年第4期摘要推薦

　　《Journal of Geovisualization and Spatial Analysis》

　　○《Journal of Geovisualization and Spatial Analysis》入駐“智繪科服”融媒體平台！

　　○ JGSA國際期刊2021年第5卷第2期論文摘要

　　○ 高被引論文推薦 | Journal of Geovisualization and Spatial Analysis

　　,