城市環境下視覺慣性里程計輔助PPP定位

張守建，李欣然，王逸石，徐 博

城市環境下視覺慣性里程計輔助PPP定位

張守建，李欣然，王逸石，徐 博

（武漢大學 測繪學院，武漢 430079）

針對復雜城市環境下，衛星信號容易受到遮蔽，精密單點定位（PPP）技術無法實現高精度連續定位，而視覺慣性里程計（VIO）可以提供連續的相對位置和速度，但其誤差隨時間累積，無法長時間獨立提供高精度導航等問題，提出半緊組合VIO輔助PPP（VIO-PPP）的定位方法：設計了VIO和PPP 2個濾波器；在VIO濾波器中，使用多狀態約束下的卡爾曼濾波器來處理獲取的視覺圖像和慣性測量單元（IMU）觀測數據，以預測位置和速度，并用PPP濾波器來修正預測值；同時根據定位精度因子和衛星的數量，在不同的環境中對PPP和VIO采取不同的加權策略；然后搭建全球衛星導航系統（GNSS）、慣性和視覺多源融合數據采集平臺，對各傳感器完成參數標定和時間同步，形成一套完整的城市環境下的高精度定位系統；最后在戶外復雜環境中進行多組實驗，比較VIO、PPP和VIO-PPP不同定位模式的定位結果。實驗結果表明，多源融合導航系統可以顯著提高導航性能，與PPP相比，VIO-PPP模型的精度能夠提高40%以上，平面精度和高程精度分別可達到0.2和0.3 m。

多源融合平臺；參數標定；車載導航；半緊組合；精密單點定位（PPP）；視覺慣性里程計（VIO）

0 引言

當前，高精度車道級導航已經成為自動駕駛中的關鍵技術。而在城市中，由于環境復雜，任何單一技術都無法長時間提供連續可靠的導航定位服務，因此集成多種傳感器的多源融合導航成為未來導航領域的發展趨勢[1-7]。

高精度全球衛星導航系統（global navigation satellite system，GNSS）定位模式主要有實時動態（real-time kinematic，RTK）載波相位差分定位技術和精密單點定位（precise point position，PPP）技術。與需要密集的地面基站的RTK技術相比，精密單點定位技術僅使用單一GNSS接收機就可以提供全球高精度定位服務[8-10]，因而得到了國內外的廣泛關注。然而，在城市地區，衛星信號經常被阻斷，導致PPP出現頻繁初始化的問題，其定位精度和效率受到嚴重影響。文獻[11]在復雜城市環境中，使用PPP-RTK技術的定位精度僅為10 m左右。同樣，文獻[12]在城市環境下基于單頻PPP的三維均方根（root mean square，RMS）精度為1.17 m。僅靠單一GNSS難以在城市環境中提供穩定可靠的車道級導航服務。

慣性導航系統（inertial navigation system，INS）不依賴于外部信號，具備自主工作能力。然而，其定位誤差會隨著時間的推移而累積，因此在一段時間后很難提供準確的位置和速度。

慣性測量單元（inertial measurement unit, IMU）通?？筛鶕湫阅芊譃?個級別，分別是戰略級、導航級、戰術級和微機械級。目前市面上的IMU設備定位精度從每小時數十公里到數米不等。性能優異的IMU誤差發散較慢、定位精度高，但受限于高昂的價格而難以推廣?？茖W技術的發展催生了低成本的微機電系統（micro-electro-mechanical system，MEMS）IMU。目前MEMS IMU得到了廣泛的應用。文獻[12-17]通過將其與GNSS、視覺等導航技術結合，彌補了單一MEMS IMU誤差飄移快、定位精度低的缺陷，為高精度的連續導航提供了新的方案。

為了在復雜城市環境下提供連續可靠的定位服務，搭建包含慣性、視覺和GNSS等多源傳感器的移動觀測平臺，設計開發包括視覺慣性里程計（visual inertial odometry，VIO）濾波器和PPP濾波器在內的多頻多系統PPP與VIO的半緊組合系統；為了保證定位結果的連續性，穩健的定權策略至關重要，因此還提出一種根據定位精度衰減因子（position dilution of precision，PDOP）和衛星數量的綜合加權策略；最后，利用自主搭建的多傳感器平臺在城市環境下采集多源觀測數據，對系統的精度進行全面評估。

1 VIO-PPP組合模型

如圖1所示為半緊組合VIO-PPP系統的結構。為了簡化算法并提高系統的穩健性，將系統分為 2個濾波器，即GNSS濾波器和VIO濾波器。VIO濾波器使用多狀態約束下的卡爾曼濾波器（multi-state constraint Kalman filter，MSCKF）處理視覺圖像和IMU觀測數據。首先通過慣性導航系統機械編排整合狀態變量，然后從視覺圖像中提取并跟蹤特征點，使用緊組合的方法對慣性與視覺觀測數據進行融合。由于VIO的定位誤差會隨著時間的推移而累積，因此當最新的狀態變量的時間戳與當前的GNSS歷元同步時，PPP的結果可用來更新濾波器中的狀態變量以限制誤差累積。

圖1 半緊組合VIO輔助PPP系統設計路線

在GNSS濾波器中，本文使用多頻率和多系統的GNSS偽距和載波相位觀測值和精密產品進行PPP解算，以獲得定位結果。在計算過程中，VIO的預測定位結果被用于輔助PPP濾波器計算衛星到接收機的幾何距離，同時參與進行半緊組合濾波，以提高PPP解算精度。

1.1 PPP觀測模型

將衛星軌道、鐘差和偽距碼偏差代入到精密單點定位模型中，可以得到用戶端多頻多系統PPP的線性化觀測方程為

PPP模型的詳細公式可參考文獻[10]。

1.2 視覺慣性里程計組合模型

誤差狀態可以表示為

將整個狀態的協方差表示為

完整的誤差傳遞方程可以表示為

當收到新的圖像時，應增廣狀態協方差矩陣為

通過式（8）、式（9）和式（15），預測和更新步驟以擴展卡爾曼濾波的形式進行，完整公式可參見文獻[18]。

1.3 半緊組合VIO輔助PPP模型

PPP的解算結果包含系（地心地固坐標系，即以地球為中心且與地球固聯的坐標系，Earth-centered Earth-fixed，ECEF）中的緯度、經度和高程坐標。而VIO的解算結果是載體相對于起始系（世界坐標系，與重力方向一致的起始VIO框架）的狀態，其中有4個不可估計的狀態，即全球位置坐標和航向。為了整合2個不同坐標框架下的結果，以第一個歷元的坐標為起點，將PPP解的結果轉換為到系（當地導航坐標系，即以載體為中心，指向東-北-天方向的坐標系，east-north-up，ENU）下，且隨后的PPP定位結果均被轉換到第一個系下。在VIO的初始化過程中，位置積分的原點被設置為全局初始位置，因此系和系之間的轉換只是關于基于PPP軌跡計算的航向角的旋轉排列。通過上述初始化方法，即可以在系下融合PPP和VIO觀測值。

在短時間內，VIO的定位精度高，因此將其作為初始值，取代PPP處理中標準單點定位的結果；同時，為了將VIO的預測位置融合到PPP濾波器中，我們在PPP觀測模型中加入位置約束

位置約束的協方差由文獻[19]中的方法確定。

2 實驗與結果分析

車載實驗在武漢大學校內進行，路線場景包括林蔭小道和高樓建筑等復雜環境。本文所使用的多源融合導航硬件平臺（如圖2所示）中搭載了北京米文動力科技有限公司的Apex Xavier II產品作為控制單元，其內部集成了英偉達智能控制平臺Jetson AGX Xavier。GNSS接收機和IMU采用的是Inertial Labs（慣性實驗室）公司的INS-B型號GNSS/INS組合接收機，IMU的采樣頻率為100 Hz，其主要性能參數如表1所示。相機模塊采用森云智能SG-AR0143型號攝像頭，圖像數據采樣頻率為10 Hz。同時，基準站設置于武漢大學測繪學院樓頂，用于實驗后使用Inertial Explorer（慣性瀏覽器）軟件獲得RTK/INS的雙向平滑緊組合定位結果。該結果的整體定位精度為厘米級，并在接下來的評估中作為參考值使用。

圖2 GNSS/INS/視覺多源融合導航數據采集平臺

表1 IMU主要性能參數

本次實驗采集道路環境如圖3所示。

圖3 車載實驗采集環境

圖4 實驗期間衛星數與PDOP值

2.1 多傳感器參數標定與時間同步

本文的多傳感器數據采集平臺安裝了包括GNSS接收機、IMU、視覺傳感器等在內的多種傳感器，因此需要事先標定出IMU與GNSS天線、視覺傳感器的空間幾何關系，以便后續解算，保證導航定位精度。關于視覺傳感器的使用，其內外參數需要標定。相機的內參包括焦距、主點位置、成像畸變系數等。此外，由于本文使用的相機鏡頭為魚眼鏡頭，其內參數還包括一個鏡頭彎曲系數。相機的外參是相機相對IMU的相對位姿。實驗過程中，視覺傳感器的內參數及其與IMU之間的外參均采用開源標定工具包Kalibr進行標定[20]。

此外，多傳感器時間同步會很大程度地影響算法實現。GNSS絕對時間通?？蛇_到納秒級，可靠性強。我們可通過硬件時間同步和軟件時間同步等將GNSS絕對時間傳遞給其他傳感器。本文設計的多源融合導航平臺中，IMU與GNSS已進行時間同步，但視覺傳感器采集圖像過程相對獨立，采集到的圖像數據并無時標信息，因此需要使用GNSS絕對時間傳遞給圖像數據并打上時間戳，以便在融合解算過程中判斷圖像數據的采集時間。本文采取的時間同步策略為，首先由GNSS接收機發送秒脈沖及美國國家海洋電子協會（National Marine Electronics Association，NMEA）協議信號給控制單元，控制單元結合內部高穩晶振完成外部協調世界時（coordinated universal time，UTC）授時，建立自身高精度的時間基準，并通過發送脈沖信號來觸發相機采樣，同時設備會記錄該脈沖的產生時間。傳感器完成采樣后，設備會將記錄的時間與本次相機傳回的圖像做關聯，作為該圖像的時間戳，完成同步。

2.2 結果分析

為了驗證半緊組合的VIO-PPP算法的有效性和可靠性，本文設計了3種不同的方案，包括VIO、PPP和VIO-PPP，來處理實驗觀測數據。其中PPP結果為GPS+BDS+Galileo三系統的雙頻非組合浮點解，其中GPS系統選用的信號頻率為L1和L2，BDS系統選用的信號頻率為B1I和B3I，GAL系統使用的信號頻率為E1和E5a。并且實驗使用Inertial Explorer軟件解算得到的RTK/INS的雙向平滑緊組合定位結果作為參考值。

2.2.1 INS/視覺里程計緊組合定位性能分析

在全程一共約25 min的觀測數據中，選擇其中視覺數據紋理清晰、路上車輛和行人干擾較少的約700 s時長數據進行INS/視覺里程計緊組合解算，同時該段時間內車輛的運動狀態豐富，有利于進行實驗驗證。

選取GPS周內秒25200～25900 s時間段的觀測數據，解算得到INS/視覺緊組合模式下的姿態誤差序列和定位誤差序列，如圖5所示。由圖可知，INS/視覺緊組合解算模式下，車輛俯仰角（pitch）和橫滾角（roll）在700 s內仍能保持較好的穩定性，而最能明顯反映城市環境下車輛行駛狀態的航向角（yaw）出現了明顯隨時間發散的情況，行駛700 s后航向角誤差已超過10°，同時INS/視覺里程計緊組合解算定位誤差累積在N、E 2個方向上均達到50 m以上，U方向精度略高，但也超過20 s?？梢妴渭兓诤轿煌扑隳Ｊ降腎NS/視覺里程計組合定位方式難以在城市環境中提供長時間穩定、可靠的導航服務，因此利用GNSS技術提供的絕對位置來限制INS/視覺里程計系統的誤差累積是很有必要的。

圖5 INS/視覺推算定姿、定位誤差

如表2所示為INS/視覺緊組合模式下的位置誤差和姿態誤差精度定量統計情況，分別列出了位置和姿態誤差的RMS值以及最大發散誤差。

總結上述視覺慣性里程計推算結果可知，在視覺傳感器輔助INS的情況下，定位定姿誤差會隨時間積累而逐漸增大，呈現出發散性，難以滿足車道級別導航的要求。

表2 INS/視覺定位定姿誤差統計

2.2.2 PPP/INS/視覺半緊組合定位性能分析

利用雙頻數據進行GPS+BDS+Galileo三系統動態PPP和PPP/INS/視覺半緊組合2種模式下的定位解算。

如圖6所示分別為單PPP和VIO-PPP組合2種模式下的定位誤差序列。由于實驗過程的環境十分復雜，單純GNSS的定位結果較差，盡管引入了高精度的相位觀測值，但是誤差序列依然呈現出顯著的發散，遮擋嚴重的環境下，粗差和多路徑效應導致多次出現收斂后再次發散的情況，PPP動態浮點解的條件下，N、E、U 3個方向的位置誤差均存在1～2 m的浮動。而在VIO-PPP半緊組合的融合定位模式下，整個時間段內的系統差和定位精度得到了大幅改善，N、E、U 3個方向的位置誤差均控制在1 m以內，且變化更為平滑，沒有大量跳躍現象。該組實驗說明，在復雜環境中，慣性數據和視覺信息的引入對動態PPP定位的精度有著顯著的提升，對單一GNSS定位方式受周圍環境影響產生的多路徑效應有明顯的改善。

如表3所示為單獨PPP浮點解和VIO-PPP組合2種模式下的N、E、U方向定位精度統計情況?？梢钥闯?，VIO-PPP組合條件下在各方向上的精度均相比單純PPP模式有顯著提升。由于信號被遮擋，PPP定位的結果在N、U方向上僅能達到米級的精度，E方向上的定位精度能達到0.5 m以內；而在利用慣性和視覺手段輔助PPP進行粗差探測、多路徑誤差建模等預處理環節的情況下，可以顯著改進定位精度而達到分米級。結合表2可知，借助GNSS測量信息的誤差不隨時間累積的特性，可以成功抑制VIO解算結果精度隨時間發散的情況。同圖5中的VIO解算結果相比，VIO-PPP半緊組合的位姿精度和穩定性均有明顯提升。

3 結束語

本文搭建了多源融合數據采集平臺，對使用的傳感器進行參數標定與時間同步；開發了半緊組合VIO-PPP模型，包含一個PPP濾波器和一個VIO濾波器。GNSS數據和VIO數據被集成到MSCKF模型中，該模型使用VIO觀測數據預測位置和速度，并用PPP濾波器修正預測值，為PDOP和衛星數量設定了閾值，以便對PPP和VIO采取不同的加權策略。然后在戶外復雜環境環境中采集多傳感器數據，利用半緊組合VIO-PPP組合導航系統進行解算，驗證其導航性能，并分別與VIO和PPP解算結果進行比較。其中單一VIO模式的精度在約700 s的時間段內航向角誤差累積超過10°，定位誤差也發散至數十米，難以滿足車輛導航的精度要求。而在復雜城市環境中，動態PPP的三維定位有效值也僅有1.98 m，無法滿足車道級導航的要求。同時集成了VIO和GNSS的半緊組合VIO-PPP模式可以在城市環境中提供連續準確的位置，東、北、天3個方向的精度分別為0.22、0.12和0.18 m，與PPP相比，分別提高了85%、76%和85%。這表明，使用多傳感器系統可以極大程度上彌補單一導航手段帶來的缺陷，有效限制INS/視覺傳感器的誤差累積，提升衛星信號被嚴重遮擋情況下的定位精度與穩定性，實現高精度連續定位，并且目前的精度已經可以初步滿足車輛在復雜城市環境中的車道級導航，為自駕車輛等技術的發展提供參考。同時為了使系統更加可靠，提高定位精度，未來我們可能會考慮其他傳感器，如激光雷達（light detection and ranging，LiDAR）。此外，預計使用GNSS PPP固定解，性能將得到進一步提高。

[1] LIAO J C, LI X X, WANG X B, et al. Enhancing navigation performance through visual-inertial odometry in GNSS-degraded environment[J]. GPS Solutions, 2021, 25(2): 1-18.

[2] LI X, LI X X, HUANG J X, et al. Improving PPP-RTK in urban environment by tightly coupled integration of gnss and ins[J]. Journal of Geodesy, 2021, 95(12):1-18.

[3] 朱鋒. GNSS/SINS/視覺多傳感器融合的精密定位定姿方法與關鍵技術[D]. 武漢大學, 2019.

[4] 黃聲享, 李冠青, 張文, 等. 港珠澳大橋沉管隧道施工控制網布設研究[J/OL]. 武漢大學學報?信息科學版: 1-9[2022-10-28]. https://doi.org/10.13203/j.whugis20170196.

[5] REID T, HOUTS S E, CAMMARATA R, et al. Localization requirements for autonomous vehicles[J]. SAE International Journal of Connected and Automated Vehicles, 2019, 2(3): 173-190.

[6] 張樂添, 趙冬青, 賈曉雪, 等. 面向動態環境的雙目/慣性SLAM系統[J/OL]. 導航定位學報: 1-7[2022-11-03]. http://kns.cnki.net/kcms/detail/10.1096.p.20220902.1438.002.html.

[7] BOCK Y, MELGAR D. Physical applications of GPS geodesy: A review[J]. Reports on Progress in Physics, 2016, 79(10): 106801.

[8] BLEWITT G. An automatic editing algorithm for GPS data[J]. Geophysical Research Letters, 1990, 17(3): 199-202. DOI:10.1029/GL017i003p00199.

[9] KOUBA J, HéROUX P. Precise point positioning using IGS orbit and clock products[J]. GPS Solutions, 2001, 5: 12-28.

[10] 簡濠駿, 王逸石, 張元泰, 等. 北斗衛星廣播星歷精度評估與單點定位優化模型[J]. 大地測量與地球動力學, 2022, 42(3): 291-297. DOI:10.14075/j.jgg.2022.03.014.

[11] LI X, LI X X, LI S Y, et al. Centimeter-Accurate vehicle navigation in urban environments with a tightly integrated PPP-RTK/MEMS/vision system[J]. GPS Solution, 2022, 26:124.

[12] GU S F, DAI C Q, FANG W T, et al. Multi-GNSS PPP/INS tightly coupled integration with atmospheric augmentation and its application in urban vehicle navigation[J]. Journal of Geodesy, 2021, 95(6): 1-15.

[13] RABBOU M A, EL-RABBANY A. Tightly coupled integration of GPS precise point positioning and MEMS-based inertial systems[J]. GPS Solution, 2017, 19(4): 601-609.

[14] LI M Y, MOURIKIS A I. High-Precision,consistent EKF-based visual-inertial odometry[J]. Int J Robot Res, 2013, 32: 690-711.

[15] BLOESCH M, OMARI S, HUTTER M, et al. Robust visual inertial odometry using a direct EKF-based approach[C]// IEEE/RSJ International Conference on Intelligent Robots and Systems. Hamburg, Germany, 2015. DOI: 10.1109/IROS.2015.7353389.

[16] LEUTENEGGER S, LYNEN S, BOSSE M, et al. Keyframe based visual-inertial odometry using nonlinear optimization[J]. Int J Robot Res, 2015, 34: 314-334.

[17] GAO Z Z, ZHANG H P, GE M R, et al. Tightly coupled integration of multi-GNSS PPP and MEMS inertial measurement unit data[J]. GPS Solutions, 2017, 21(2): 377-391.

[18] SUN K, MOHTA K, PFROMMER B, et al. Robust stereo visual inertial odometry for fast autonomous flight[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 965-972.

[19] LI X X, WANG X B, LIAO J C, et al. Semi-Tightly coupled integration of multi-GNSS PPP and S-VINS for precise positioning in GNSS-challenged environments[J]. Satellite Navigation, 2021, 2(1): 1-14.

[20] 蘇景嵐. 車載視覺/INS/GNSS多傳感器融合定位定姿算法研究[D]. 武漢大學, 2019.

Visual inertial odometry-aided PPP positioning in urban environments

ZHANG Shoujian, LI Xinran, WANG Yishi, XU Bo

（School of Geodesy and Geomatics, Wuhan University, Wuhan 430079, China）

Aiming at the problems that in the complex urban environment, precise point positioning (PPP) cannot achieve high precision continuous positioning because satellite signal is easily blocked by trees, buildings and tunnels, meanwhile visual inertial odometry (VIO) can provide continuous relative position and velocity, but its time-increasing errors make it difficult to provide highly accurate positioning results independently after a long time, the paper proposed a positioning method of semi-tightly coupled VIO aided PPP (VIO-PPP) system: two Kalman filters including VIO filter and PPP filter were designed; in the VIO filter, the multi-state constraint Kalman filter (MSCKF) model was used to process the acquired visual images and inertial measurement unit (IMU) observations to predict the position and velocity, and the predicted values were corrected with the PPP filter; then based on the position dilution of precision (PDOP) and the number of satellites, different weighting strategies for the PPP and the VIO were adopted in different environments; in addition, a multi-sensor data acquisition platform was built with global navigation satellite system (GNSS), inertial sensors and cameras, and the parameter calibration and the time synchronization were performed for each sensor, for forming a complete set of high-precision positioning system in the urban environment; finally, multiple sets of experiments were conducted in a complex outdoor environment to compare the positioning results of different positioning modes of VIO, PPP and VIO-PPP. Results showed that a multi-source fusion navigation system integrating multiple sensors could significantly improve the performance of navigation and positioning, and the VIO-PPP model would bring more than 40% accuracy improvement compared with PPP, with 0.2 m accuracy in plane and 0.3 m accuracy in elevation.

multi-source fusion platform platform; parameter calibration; in-vehicle navigation; semi-coupled integration; precise point positioning (PPP); visual inertial odometry (VIO)

張守建, 李欣然, 王逸石, 等. 城市環境下視覺慣性里程計輔助PPP定位[J]. 導航定位學報, 2023, 11(6): 34-41.（ZHANG Shoujian, LI Xinran, WANG Yishi, et al. Visual inertial odometry-aided PPP positioning in urban environments[J]. Journal of Navigation and Positioning, 2023, 11(6): 34-41.）DOI:10.16547/j.cnki.10-1096.20230605.

P228

A

2095-4999(2023)06-0034-08

2023-11-08

國家重點研發計劃（2021YFB2501100）

張守建（1980—），男，山東臨沂人，博士，副教授，研究方向為多源融合導航、低軌衛星精密定軌等。