GNSS/VO組合導航研究現狀及發展趨勢

曾慶喜，邱文旗，馮玉朋，劉德輝，賈豐源

(1.南京航空航天大學 自動化學院，南京 210016；2.汽車仿真與控制國家重點實驗室，長春 130012；3.奇瑞汽車股份有限公司 前瞻技術研究院，安徽 蕪湖 241009)

0 引言

精確的導航技術是無人駕駛車輛的核心技術之一。為滿足車輛厘米級的定位要求，多傳感器融合技術與單一傳感器相比具有時空覆蓋范圍廣、導航信息精確度和可靠性高等優點。目前常用組合導航系統是全球衛星導航系統(global navigation satellite system，GNSS)及慣性導航系統(inertial navigation system，INS)的組合導航系統，但是要達到無人車的定位精度，需要使用高成本的INS；文獻[1]采用輪式里程計取代INS與GNSS組合，但是通常會由于車輪打滑、抱死等原因導致系統誤差迅速增大；目前無人車都裝有攝像頭，且攝像頭價格便宜，能夠提供高分辨率的定位信息，操作要求較低，易與目標跟蹤、地圖構建、避障等功能進行融合：所以近些年出現了采用視覺里程計取代INS與GNSS組合的導航方法。

1 全球導航衛星系統

全球衛星導航系統(global navigation satellite system，GNSS)包括美國的全球定位系統(global positioning system，GPS)、俄羅斯的格洛納斯衛星導航系統(global navigation satellite system，GLONASS)、歐洲的伽利略衛星導航系統(Galileo navigation satellite system，Galileo)，以及中國的北斗衛星導航系統(BeiDou navigation satellite system，BDS)，是能在地球表面或近地空間的任何地點為用戶提供全天候的3維坐標和速度以及時間信息的空基無線電導航定位系統[2]。GNSS系統由衛星星座、地面監測系統、GNSS接收機3部分組成。地面GNSS接收機通過接收和解算衛星播發的無線電導航定位信號就可以獲得導航定位信息。由于GNSS系統屬于無線電導航系統；即使現代的GNSS接收機也只有在對衛星直接可見(line of sight，LOS)的條件下才可以實現高精度定位[3]。潮濕的樹葉、森林、高山或者建筑物都可能導致導航信息暫時丟失[4]。又由于水面、山的側面、墻體等造成多路徑接收；用戶接收機同樣也無法識別，并可能導致相當大的誤差。裝有GNSS的智能車輛大部分是在密集的城市環境中使用，在這種情況下，無線電導航信號可能與幾個障礙物相互作用后再傳播到達GNSS接收機。所以GNSS接收機接收到的信號可能是直接信號和幾個非視距(non line of sight，NLOS)延遲信號的總和，從而導致定位準確性嚴重下降，甚至會使衛星信號短暫消失；因而單獨的GNSS接收機并不能滿足智能車在復雜的城市環境中的定位要求。

2 視覺里程計

視覺里程計(visual odometry，VO)是將單個或多個攝像頭在運動過程中采集的圖像作為輸入，并基于相機模型與視覺幾何學模型等來估計車體旋轉運動和平移運動的定位方法。按照VO所需的攝像機的個數，可將VO分為單目VO和立體VO。單目VO的缺點是缺乏尺度信息[5-6]，需要其他傳感器的輔助[7-8]；雙目VO的缺點是受雙目VO基線的限制，無法運用于像微型無人機等微型機器的導航[9]。與輪式里程計很類似，輪式里程計通過累記輪子的轉動來推算載體的位置方位，視覺里程計則通過累積估計相鄰2幀圖像間攝像機的運動來估計載體的位姿。與輪式里程計相比，它能夠適應車輪打滑、車輪側向移動、非平地運動等輪式里程計無法適應的場景。

經典的視覺里程計系統大多基于2幀計算框架，其模型如圖1所示，雙目視覺里程計需要在能夠獲取清晰圖像的前提下，保證相鄰2幀圖像中具有足夠的特征重疊區域，用來確保對載體運動估測的準確性。其工作流程如圖2所示：一般包括特征的選擇與關聯，以及基于所獲得的特征關聯集合的幀間位姿估計；采用多幀圖像信息的局部優化是視覺里程計系統的可選模塊。視覺里程計所采用的攝像頭不僅能夠實現對載體的運動進行估計，還可以用于很多其他的應用，如目標跟蹤、距離測定、地圖構建[10-13]、避障等場景[14]。

圖1 VO 2幀計算框架模型

圖2 VO工作流程

3 GNSS/VO組合系統

3.1 GNSS/VO組合方式

參考GNSS/INS信號融合分類方法，現有基于GNSS的多傳感器組合導航的信號組合方式可以分為3類，分別是松組合、緊組合、超緊組合。各種組合系統的優缺點如表1所示。根據目前相關文獻檢索，GNSS/VO現有的組合方式有松組合和緊組合。GNSS/VO松組合系統是使用獨立分開的GNSS和VO模塊，首先以不同的運算速率對載體的6個自由度信息(包括3個自由度載體旋轉運動、3個自由度載體平移運動)，進行估計；然后再將各自輸出的載體運動信息進行融合。GNSS/VO松組合系統的優點是沒有改變GNSS和VO的系統結構，同時保持了 GNSS系統和VO系統各自的獨立性，有利于對各個系統的獨立優化；缺陷是當GNSS系統衛星數目低于4顆的情況下，組合系統失效。GNSS/VO緊組合系統繼承了GNSS/VO松組合系統的優點，同樣保持GNSS和VO系統的獨立性，不同點在于GNSS/VO緊組合系統的輸入數據不再是載體的位姿信息，而是較為原始的偽距、偽距率、載波相位數據；所以GNSS/VO緊組合系統能夠在衛星數目低于4顆的情況下參與組合系統的定位。不論是GNSS/VO松組合系統還是GNSS/VO緊組合系統，絕大部分系統所使用的融合算法工具都是擴展卡爾曼濾波器(extended Kalman filter，EKF)。

表1 基于GNSS的組合導航系統信號融合方式及性能對比

3.2 GNSS/VO松組合系統

GNSS/VO松組合系統的結構和GNSS/INS松組合結構相似，是將GNSS輸出的位姿信息與VO輸出的位姿信息通過擴展卡爾曼濾波器進行組合。文獻[15]于2006最早提出低成本GNSS/立體VO松組合系統，該系統充分利用了VO系統與低成本的GPS在估計誤差方面具有互補的特點：即VO能夠提供精確的局部一致的位姿信息，但是隨著時間推移，位姿信息會發散；低成本的GPS接收機在良好的工作環境下，在北、南方向上的標準差在3 m左右，但在短期內會受到小的跳躍和漂移的影響。考慮到低成本GPS接收機對高度測量的不可靠性，將GPS接收機輸出的南、北位置信息，以及航向信息與VO系統通過卡爾曼濾波器進行融合，用來校正視覺里程計的位置估計值。在該文中，位置信息用于校正位置狀態，速度信息獨立用于校正航向。當低成本GPS接收機至少有一個3維位置的定位時，使用位置信息校正系統位置狀態；為了限制GPS關于速度測量的噪聲對航向估計的影響，當車輛行駛速度不低于0.5 m/s時，速度信息才獨立用于校正航向。此外，該系統又單獨采用INS系統對載體的運動進行估計，以保證在VO系統失效的情況下，系統仍然能夠對載體的運動狀況進行估計，增加系統的可靠性。該文將GNSS/VO松組合搭載到如圖3所示的平臺小車上進行實驗，采用環路閉合誤差(起點和終點的距離與總的環性長度的比值)來評定各個系統的性能。實驗表明，立體VO的性能優于輪式里程計的性能，GNSS/VO松組合系統的性能優于純立體VO系統的性能。

圖3 GNSS/VO組合系統搭載平臺

文獻[16]于2010年借鑒GNSS/INS組合導航系統模型，提出一種類似于GPS/INS松組合方式的GPS/VO組合導航系統，如圖4所示。GNSS/VO松組合與GNSS/INS松組合結構相似，都是利用卡爾曼濾波器將GPS/VO系統進行組合，具有相同的狀態量和量測量。文獻[16]提到的系統不需要如平臺的動態、地圖匹配、特征跟蹤、可視環路閉合、重力矢量或諸如慣性測量單元(inertial measurement unit，IMU)、磁羅盤等附加傳感器這樣的額外限制或假設。該文通過構造觀測矩陣，對該觀測矩陣的秩進行分析，從理論上證實了當加速度矢量不平行于速度矢量時，系統姿態完全可觀測。該文利用基于控制變量法和數值模擬法對系統的可觀測性進行實驗驗證，證明了當加速度矢量垂直于速度矢量時，系統姿態是完全可觀測的。文獻[17]又在文獻[16]的基礎上對尺度信息未知的單目視覺里程計系統進行了擴展研究，通過小型飛機采集的真實數據驗證了系統誤差可觀測性分析的可靠性。文獻[17]通過實驗數據驗證了在直線飛行和水平飛行的情況下，俯仰和偏航會立即向真正的值收斂；滾動很難估計，但在操作過程中會收斂。這種行為與GNSS/INS組合系統是互補的，在GNSS/INS組合導航系統中，滾動和俯仰是由重力向量所限制的，但是偏航必須通過操縱來觀察。

圖4 GPS/INS松組合系統與GPS/VO松組合系統對比

文獻[18]旨在解決在GNSS/VO松組合系統中，由于相鄰2幀圖像中重疊區域的特征點較少導致視覺里程的位姿解算誤差偏大，以及由于遮擋等原因導致的衛星只能間歇性給出定位的問題。在解決相鄰2幀圖像中重疊區域的特征點較少導致視覺里程的位姿解算誤差偏大問題的時候，首先假設該文提出的像素噪聲模型是準確的；其次假設運動中的偏差可以近似于該文中最初估計的偏差；最后建立改進的雙目視覺里程計位姿估計模型。在解決衛星只能間歇性給出定位問題的時候，由于間歇性的GPS信息可能會延遲整個狀態的可觀測性，將局部測量量和全局測量量進行統一表示，把GNSS/VO松組合轉化為非線性最小二乘問題來對載體的位姿信息作最優估計。該結構能夠用來推斷全局定位，并能從不準確的初始估計中恢復過來。該文通過小船在水面上的實驗，驗證了在相鄰2幀圖像中重疊區域的特征點較少情況下，改進的雙目視覺里程計的精度是傳統雙目視覺里程計的5倍。該文通過小車在環形路面上行駛2 km范圍內的實驗，驗證了該文中設計的GNSS/VO松組合系統僅僅只有6個衛星數據就能完成整個路段的全局定位；使用改進的視覺里程在平均位置誤差方面有2倍的改進。

文獻[19]采用的視覺里程計定位方法與以往的相對定位方法不同，該雙目視覺里程計利用已知的簡易基站點作為絕對坐標信息來完成智能車的絕對定位。其算法流程為：第一步完成左、右2臺攝像機的標定；第二步通過如圖5所示的基于尺度不變特征變換(scale invariant feature transformation，SIFT)的特征匹配，提取基站在左、右相機中的坐標；第三步求取相機相對基站的坐標。該文中指出，基站并不需要專門搭建，只要是具有可辨認的特征以及絕對坐標已知的物體，都可以作為基站。在該文中提到，單獨的視覺里程計絕對定位是采用2個相同的分別率為960 個像素×720個像素的Logitech攝像頭，安裝基線的距離為1.2 m，攝像機的同步采樣頻率為1 Hz，測得8個點的絕對坐標值，得出平均誤差在4 m左右。為了進一步提高智能車的定位精度，該文采用GNSS/VO松組合的方法。該GNSS/VO松組合方法與經典的將系統誤差作為狀態量的方法相比，相同點在于都是通過卡爾曼濾波器將2個獨立系統融合，不同點在于GNSS/VO松組合系統的狀態量是水平位置、水平角度、載體的行駛速度信息，觀測量是VO輸出的水平位置、左攝像頭與道路邊緣的距離、汽車行駛速度、相對于地圖的偏航角信息、GNSS輸出的水平位置信息。

圖5 SIFT匹配算法流程

3.3 GNSS/VO緊組合系統

GNSS/VO緊組合系統采用載波相位或者偽距作為系統數據融合量，能夠解決衛星數目低于4顆時導航衛星系統不能參與定位的問題。文獻[9]旨在證明在城市、室內等GPS信號不太好的環境下將GPS與VO進行緊組合的可行性。當GPS衛星數目低于4顆的情況下，導航衛星系統不能給出定位結果，但仍可以用于輔助VO系統。盡管立體視覺里程計可以給出尺度信息，但是受到雙攝像頭間的基線長度約束；無法在諸如微型無人機等小平臺上使用：所以該文中采用GPS的厘米級載波相位信息輔助單目視覺里程計獲取尺度信息。單目VO系統在獲取尺度信息之后，可以在衛星完全失效的情況下獨立工作。如圖6所示，在該文獻介紹的GPS與VO互相融合的方法中，融合系統的輸入量包括：已經獲取尺度信息的單目VO輸出的6個自由度信息、GPS給出的多普勒頻移和載體的速度，以及INS輸出的方向余弦矩陣(direction consine matrix，DCM)。由于系統的DCM隨時間變化，使得GPS/VO組合系統為非線性系統。為了獲得線性化的GPS/VO組合系統，該文獻采用INS輔助獲取DCM。該文獻采用線性最小均方法(least mean square，LMS)對系統的位置改變向量、累計的時鐘漂移進行估計。當接收機時鐘已經被初始化并且相機的方向已知的情況下，組合導航系統完成定位解算只需要1顆衛星；對于校準的時鐘和未知的方位，需要2顆衛星；如果時鐘和方位都未知的情況下，就需要3顆衛星。該文將該系統在平臺小車上進行試驗，試驗參數為：1)小車以2 m/s做直線運動；2)GPS的載波相位測量誤差為5 mm；3)低成本INS的陀螺漂移為0.1 rad/s；4)視覺里程計的特征測量噪聲為1個像素；5)系統的數據更新速率為1 Hz。試驗結果表明該GPS/VO組合系統能夠成功得到平臺位置厘米級的變化量、平臺的3 rad左右的航向角、單目視覺里程計的厘米級的尺度信息。

圖6 衛星載波相位與視覺組合導航系統

文獻[20]采用的GNSS/VO組合方法和GPS/INS緊組合系統相似，都是將位置誤差、速度誤差、姿態誤差、接收機時鐘誤差、接收機時鐘漂移作為狀態量，GNSS與VO的偽距差、偽距率差作為觀測值，對系統進行建模，并利用卡爾曼濾波器對狀態量進行估計。該文的系統結構只包括低成本的GPS接收機和雙目VO。該系統采用雙層容錯結構，對VO數據和GNSS數據進行檢測，以增加系統的穩定性；但是一旦系統檢測偽距出現故障，整個可見星的偽距都不可用。該方法解決了在松組合系統中衛星數目低于4顆不能參與定位的問題。該融合系統不僅能夠兼容2個系統的數據更新率，而且能夠兼容不同數據的有效時間。該文實現了數據更新的速率可達到200 Hz。實測數據在定位精度方面超過了型號為OxTS RT3003的INS與GPS組合系統。

4 結束語

為確保視覺里程計對載體運動估測的準確性，需要在能夠獲取清晰圖像的前提下，保證相鄰2幀圖像中具有足夠的特征重疊區域。單目視覺里程計存在尺度模糊的問題，需要具有厘米級精度的額外傳感器對其進行輔助。雙目視覺里程計由于受到基線長度的約束，不能運用于小型設備的定位導航。單獨的衛星導航系統只有在衛星數目達到4顆或以上時才可以完成定位，不利于在城市中獨立運行。GNSS/VO松組合系統在GNSS不能完成定位的情況下，VO系統可以獨立運行，增加了定位系統的可靠性。GNSS/VO緊組合系統解決了當衛星數目低于4顆時不能參與定位的問題。但是目前的GNSS/VO組合方案基本上都是采用卡爾曼濾波器，并沒有考慮到模型誤差等對系統的影響；對GNSS故障性的檢測機制中，一旦檢測出偽距出現故障，整個衛星系統將不參與GNSS/VO組合系統的定位；沒有考慮GNSS/VO組合系統的運算量以及運算速度問題。

參考GNSS/INS組合的發展特點以及GNSS/VO組合自身的特點，分析得出GNSS/VO組合系統未來會向如下幾個方向發展：1)GNSS/VO組合導航系統向著超緊組合方向發展，將組合系統的輸出數據反饋到接收機的跟蹤環路，以提高GNSS信號的有效信噪比，改善GNSS接收機的性能；2)開展GNSS/VO組合導航系統的故障檢測與隔離機制研究，以增加系統的可靠性；3)開展GNSS/VO組合系統在各個組合階段的卡爾曼濾波算法改進研究，以改善組合系統的精度和穩定性；4)GNSS/VO組合系統采用并行的算法結構，以加快系統的運算速度。

[1] 甘雨，隋立芬，張清華，等.GNSS/里程計組合定位抗差估計法[J].測繪科學技術學報，2015，16(3)：252-255.

[2] 寧津生，姚宜斌，張小紅.全球導航衛星系統發展綜述[J].導航定位學報，2013，1(1)：3-8.

[3] RABAOUI A，VIANDIER N，DUFLOS E，et al.Dirichlet process mixtures for density estimation in dynamic nonlinear modeling：application to GPS positioning in urban canyons[J].IEEE Transactions on Signal Processing，2012，60(4)：1638-1655.

[4] 馬云峰.MSINS/GPS組合導航系統及其數據融合技術研究[D].南京：東南大學，2006：4-10.

[5] AQEL M O A，MARHABAN M H，SARIPAN M I，et al.Adaptive-search template matching technique based on vehicle acceleration for monocular visual odometry system[J].IEEE Transactions on Electrical and Electronic Engineering，2016，11(6)：739-752.

[6] BERAN L，CHMELAR P，REJFEK L.Navigation of robotics platform using monocular visual odometry[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 25th International Conference on Radioelectronica.Pardubice：IEEE，2015：213-216.

[7] FU H，MA H，XIAO H.Real-time accurate crowd counting based on RGB-D information[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 19th International Conference on Image Processing.Florida：IEEE，2012：2685-2688.

[8] MING Y，RUAN Q，HAUPTMANN A G.Activity recognition from RGB-D camera with 3D local spatio-temporal features[C]// The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 19th International Conference on Multimedia and Expo.Melbourne：IEEE，2012：344-349.

[9] SOLOVIEV A，VENABLE D.Integration of GPS and vision measurements for navigation in GPS challenged environments[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 2010 Position Location and Navigation Symposium(PLANS).Indian Wells：IEEE，2010：826-833.

[10] TAO Z，BONNIFAIT P.Sequential data fusion of GNSS pseudoranges and dopplers with map-based vision systems[J].IEEE Transactions on Intelligent Vehicles，2017，1(3)：254-265.

[11] TAO Z，BONNIFAIT P，IBANEZ-GUZMAN J.Road-centered map-aided localization for driverless cars using single-frequency GNSS receivers[J].Journal of Field Robotics，2017，34(5)：1010-1033.

[12] TAO Z，BONNIFAIT P，FREMONT V，et al.Lane marking aided vehicle localization[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 16th International IEEE Conference on Intelligent Transportation Systems.The Hague Netherlands：IEEE，2013：1509-1515.

[13] TAO Z，BONNIFAIT P.Tightly coupling GPS with lane markings for autonomous vehicle navigation[C]// The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 17th International IEEE Conference on Intelligent Transportation Systems.Qingdao，China：IEEE，2014：439-444.

[14] 江燕華，熊光明，姜巖，等.智能車輛視覺里程計算法研究進展[J].兵工學報，2012，33(2)：214-220.

[15] AGRAWAL M，KONOLIGE K.Real-time localization in outdoor environments using stereo vision and inexpensive GPS[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 18th International Conference on Pattern Recognition.Hong Kong,China：IEEE，2006：1063-1068.

[16] DUSHA D，MEJIAS L.Attitude observability of a loosely-coupled GPS/visual odometry integrated navigation filter[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of 2010 International Conference on Robotics and Automation.Australia：IEEE，2010.

[17] DUSHA D，MEJIAS L.Error analysis and attitude observability of a monocular GPS/visual odometry integrated navigation filter[J].International Journal of Robotics Research，2012，31(6)：714-737.

[18] REHDER J，GUPTA K，NUSKE S，et al.Global pose estimation with limited GPS and long range visual odometry[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 17th International Conference on Methods & Models in Automation & Robotics(MMAR).Minnesota：IEEE，2012：627-633.

[19] 周莉.基于GNSS與視覺的道路檢測與避障技術[D].成都：電子科技大學，2015：24-30.

[20] SCHREIBER M，K?NIGSHOF H，HELLMUND A M，et al.Vehicle localization with tightly coupled GNSS and visual odometry[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 2016 IEEE Intelligent Vehicles Symposium.Gothenburg：IEEE，2016：858-863.