基于LiDAR輔助的GNSS/INS組合導(dǎo)航設(shè)計

申建華，譚志強，楊思佳

(1.中國電子科技集團公司第五十四研究所，河北石家莊 050081；2.電子信息系統(tǒng)復(fù)雜電磁環(huán)境效應(yīng)國家重點實驗室，河南洛陽 471001；3.北京衛(wèi)星導(dǎo)航中心，北京 100094)

隨著自動駕駛、智慧城市等研究領(lǐng)域的飛速發(fā)展，基于定位服務(wù)(location based services，LBS)的行業(yè)標(biāo)準(zhǔn)及要求越來越高。全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system,GNSS) 近幾年得到飛速發(fā)展，主要包括多頻多星座的系統(tǒng)拓展，如中國北斗的全球服務(wù)應(yīng)用、美國GPS系統(tǒng)現(xiàn)代化等，同時相關(guān)增強系統(tǒng)(包括星級增強(SBAS),地基增強(GBAS)等)的發(fā)展大大提高了GNSS系統(tǒng)在開闊場景下的服務(wù)能力，滿足了大部分應(yīng)用場景下的作業(yè)要求。

但對于挑戰(zhàn)環(huán)境下的GNSS導(dǎo)航技術(shù)仍有待突破，特別是在茂密的森林、高樓林立的城市或室內(nèi)環(huán)境下，GNSS信號因信號衰減、多路徑、干擾等因素?zé)o法正常到達接收機終端，嚴重影響定位精度，甚至無法完成定位服務(wù)[1-2]。為了增強GNSS拒止場景下的導(dǎo)航定位服務(wù)能力，目前主要應(yīng)用的強化技術(shù)包括GNSS反干擾天線、手機通信、Wi-Fi定位、航位推算、天文導(dǎo)航、重力梯度儀、量子導(dǎo)航、偽衛(wèi)星等技術(shù)。基于機會信號(電視信號、手機信號、偽衛(wèi)星等)的定位技術(shù)需要基礎(chǔ)設(shè)施支撐，往往需要較大的成本，同時也需要保證機會信號的完好性，另一方面，基于量子、重力梯度等前沿技術(shù)的導(dǎo)航技術(shù)，因成本、技術(shù)成熟度等問題短時間內(nèi)無法落地實現(xiàn)。

在目前常規(guī)應(yīng)用中，仍以自主性較強的航位推算算法為主，以解決GNSS拒止場景下的精度、可靠性問題。其中慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)是應(yīng)用較廣的自主導(dǎo)航方案，其可以通過自主加速度計、陀螺量測的積分操作來實現(xiàn)短期的航位推算，提供位移、姿態(tài)變化信息。由于加速度計、陀螺儀零偏的存在，導(dǎo)致位置、速度、姿態(tài)的估計誤差會隨時間指數(shù)級增長，一方面，可以通過高精度慣性量測單元(IMU)來實現(xiàn)長期的高精度航位推算，但通常類似高精度IMU(如導(dǎo)航級)的成本過高，將無法應(yīng)用于常規(guī)應(yīng)用場景中。另一方面，INS可以通過GNSS不定期的組合糾正誤差來抑制誤差的積累，從而實現(xiàn)長期可靠的導(dǎo)航。典型的GNSS/INS組合導(dǎo)航系統(tǒng)包括松組合、緊組合、超緊耦合系統(tǒng)[3-8]，考慮到目前可見星數(shù)，一般采用松組合系統(tǒng)即可滿足常規(guī)的定位導(dǎo)航服務(wù)。

在室內(nèi)自主導(dǎo)航定位應(yīng)用中，可以通過激光雷達(LiDAR)或視覺系統(tǒng)來記錄周圍環(huán)境信息，推算自己的行進軌跡，達到定位目的[9]。SLAM(simultaneous localization and mapping)起源于1986年的ICRA(international conference on robotics and automation)會議，PETER，JIM，DURRANT-WHYTE等幾位學(xué)者討論了定位與制圖的估計理論與方法。 SMITH和DURRANT-WHYTE等建立了基于相對地標(biāo)的卡爾曼濾波模型。1966年，DURRANT-WHYTE等系統(tǒng)性地定義了SLAM的結(jié)構(gòu)、收斂結(jié)果以及縮寫。國外對SLAM技術(shù)的研究要比國內(nèi)早很多，已經(jīng)有一批相當(dāng)不錯的成果，比如Google的Cartographer背包系統(tǒng)[10]、CSIRO的Zebedee系統(tǒng)，這些系統(tǒng)包括了2D，3D激光和視覺SLAM[11]，可以進行高精度的環(huán)境建模，現(xiàn)已應(yīng)用到各個領(lǐng)域,并結(jié)合其他傳感器進行了融合定位導(dǎo)航[12-13]。同時國內(nèi)的SLAM也有了一定的發(fā)展[14-15]，但主要是應(yīng)用研究，受限于LiDAR成本的約束，并沒有得到廣泛的推廣應(yīng)用。

本文在分析了GNSS定位與激光SLAM技術(shù)各自的優(yōu)缺點之后，對GNSS/INS/LiDAR組合的可行性進行了探究，期望構(gòu)建出一個能全天候運行、適應(yīng)各種環(huán)境的導(dǎo)航定位算法，并在實驗車平臺中實現(xiàn)算法的初步驗證。

1 GNSS/INS組合系統(tǒng)

全球衛(wèi)星導(dǎo)航系統(tǒng)和慣性導(dǎo)航系統(tǒng)具有很強的互補特性，GNSS/INS組合系統(tǒng)充分利用了GNSS長期精度高和INS短期精度高、抗干擾能力強的優(yōu)點，取長補短，獲得了比單獨使用任一系統(tǒng)時更高的導(dǎo)航性能，也因此在軍民領(lǐng)域都得到了越來越廣泛的研究和應(yīng)用。GNSS/INS組合導(dǎo)航系統(tǒng)如圖1所示。

圖1 GNSS/INS組合導(dǎo)航系統(tǒng)Fig.1 GNSS/INS integration system

GNSS與INS的組合策略通常有3種：松組合、緊組合和深組合[4-5]。其中，松/緊組合系統(tǒng)是系統(tǒng)級組合，GNSS與INS保持相對的獨立性，該技術(shù)迄今為止已經(jīng)相對比較成熟。GNSS可見情況下，GNSS/INS組合導(dǎo)航系統(tǒng)是常規(guī)的定位、定姿解決方案。在低成本GNSS/INS組合導(dǎo)航系統(tǒng)中，主要有初始化問題。初始姿態(tài)需要一定的精度，否則導(dǎo)航系統(tǒng)會慢慢發(fā)散或者存有粗差。即便在精準(zhǔn)的初始化前提下，由于航向參數(shù)的弱觀測性，航向估測也會慢慢漂移，在這種情況下，需要其他傳感器，如電子羅盤等來對系統(tǒng)航向估測保持長時間的穩(wěn)定可靠。

如圖2所示，IMU組件將通過自身的航位推算來計算位置、速度(主要包括速度更新、位置更新、姿態(tài)更新3個步驟)[4]。在本方案中，主要采用了如式(1)和式(2)所示的“單子樣+前一周期”算法來補償不可交換誤差。

(1)

(2)

φ(T)為當(dāng)前時刻的等效旋轉(zhuǎn)矢量。在速度更新中，考慮到IMU精度及航位推算時間長度，在速度更新中并沒有加入旋轉(zhuǎn)效應(yīng)及劃槳效應(yīng)補償。基于航位推算的位置、速度精度主要取決于IMU器件本身及累積時間，所以不能依靠長時間積累的位置、速度信息。

圖2 INS航位推算Fig.2 Dead-reckoning of INS

在GNSS拒止或不可見情況下，GNSS/INS組合系統(tǒng)直接切換為圖2所示的純INS導(dǎo)航系統(tǒng)，由于初始狀態(tài)零偏、慣性器件零偏等誤差因素的影響，在中低精度的IMU傳感器情況下，導(dǎo)航的精度會在短時間內(nèi)發(fā)散，無法保障導(dǎo)航系統(tǒng)的聯(lián)系服務(wù)能力。如圖3所示，在RTK/INS組合系統(tǒng)測試中，在組合系統(tǒng)穩(wěn)定之后，通過不同周期的純INS定位結(jié)果來估計INS短時精度，具體精度主要取決于初始零偏、初始姿態(tài)等狀態(tài)信息精度，在圖3跑車實驗中，可以得出在30 s以上，MEMS慣組的純DR精度將大打折扣。

圖3 基于SBG Ellipse-A-GEA2-B1慣組的純導(dǎo)航 測試，30 s偏離(5.2 m)40 s 偏移(11.87 m)Fig.3 INS DR test using a MEMS-IMU(SBG Ellipse- A-GEA2-B1),positioning error in 30 s (is 5.2 m), positioning error in 40 s (is 11.87 m)

2 LiDAR輔助的GNSS/INS導(dǎo)航系統(tǒng)

常規(guī)LiDAR-SLAM與視覺SLAM的工作流程基本都可以概括為3個環(huán)節(jié)：1)前端利用連續(xù)的點云/影像配準(zhǔn)，粗略估計機器人的相對運動；2)將當(dāng)前接收到的環(huán)境信息與之前存儲的局部地圖進行frame-to-map匹配(或者局部地圖與全局地圖匹配等)，修正位姿；3)后端建立位姿圖進行整體優(yōu)化，或濾波求解。

如圖4所示，主要采用了LOAM框架[16]，將整個LOAM系統(tǒng)分成4個主要部分：1)點云注冊(point cloud registration)；2)里程計(LiDAR odometry)；3)建圖(LiDAR mapping)；4)位姿整合(transform integration)。

圖4 LOAM系統(tǒng)架構(gòu)Fig.4 Structure of LOAM algorithm

工作流程概括：首先點云注冊部分對接收到的LiDAR點云數(shù)據(jù)進行預(yù)處理，包括消除運動畸變、特征點提取等；在里程計部分，通過相鄰幀的點云匹配技術(shù)，輸出一個高頻(10 Hz)的位姿；接著建圖部分利用新接收的點云幀與建好的局部地圖再配準(zhǔn)優(yōu)化高頻位姿，輸出一個低頻位姿(1 Hz)并更新地圖；最后位姿整合部分將低頻位姿融合到高頻位姿并輸出。LOAM相比其他激光SLAM算法的優(yōu)點是它充分發(fā)揮了幀到幀(frame-to-frame)和幀到地圖(frame-to-map)點云匹配算法的優(yōu)勢，并能針對計算量大小為它們安排合適的任務(wù)，LOAM算法能實現(xiàn)的精度可以達到總行駛距離的3%以內(nèi)[16]。

點云匹配算法可分為幀到幀(frame-to-frame)、幀到地圖(frame-to-map)、子圖到地圖(map-to-map)。其中，frame-to-frame用到的點云數(shù)據(jù)最少，所以計算量小、精度低。 frame-to-map適中，map-to-map匹配得到的位姿最準(zhǔn)確但計算量太大，不適合用在實時環(huán)境中。LOAM把定位和建圖分開，在定位時，使用frame-to-frame時，因為其計算量小，所以高頻輸出，達到實時定位的目的；建圖時則使用frame-to-map，使得生成的地圖和位姿更準(zhǔn)確，并以較低的頻率執(zhí)行保證不占用過多的CPU資源[17]；最后再將這2個位姿信息融合。總之就是高頻里程計低頻建圖，這樣的方案既保證了里程計信息和建圖的準(zhǔn)確性，又兼具了實時性。

LiDAR數(shù)據(jù)掃描過程如圖5所示。針對機械式LiDAR，點云數(shù)據(jù)的運動補償是關(guān)鍵步驟。

圖5 LiDAR數(shù)據(jù)掃描過程Fig.5 Scanning process of a LIDAR

(3)

(4)

考慮到LiDAR-SLAM與傳統(tǒng)GNSS/INS組合系統(tǒng)的優(yōu)缺點，提出了如圖6所示的LiDAR輔助的GNSS/INS組合系統(tǒng)。

圖6 LiDAR輔助的GNSS/INS組合系統(tǒng)Fig.6 LiDAR aided GNSS/INS integration system

GNSS/INS組合系統(tǒng)主要的原理是利用LiDAR-SLAM前端里程計短時精度，去彌補短時GNSS拒止情況下的導(dǎo)航定位服務(wù),同時也考慮了激光雷達的完好性分析[16]。

3 實驗驗證

為了驗證LiDAR輔助的GNSS/INS組合系統(tǒng)架構(gòu)的正確性，在實驗車的平臺上搭建了驗證系統(tǒng)，如圖7所示。

圖7 實驗驗證平臺Fig.7 Experiment platform

軟件：VMware Workstation 14 Player虛擬機，Ubuntu16.04，ROS Kinetic，Opencv-3.3.0，PCL-1.8.1。硬件：1)GNSS/INS使用u-blox的GPS芯片，SBG慣組；2)LiDAR使用速騰聚創(chuàng)的Robosense16線激光雷達。

3.1 室內(nèi)實驗(純LiDAR)

首先在室內(nèi)做了LiDAR里程計算法的測試，室內(nèi)場景實驗結(jié)果如圖8所示。可以明顯看到紅圈后面的定位結(jié)構(gòu)完全與真實方向存在明顯的90°偏離，這是由于在特征匹配問題上造成的。實驗中由于在手持LiDAR時，繞該方向的抖動導(dǎo)致LiDAR在x軸的位姿角陡增，而后無法修正的原因。

圖8 室內(nèi)場景實驗結(jié)果Fig.8 Experiment result in indoor scenario

其次在室外停車場做了3組典型實驗，結(jié)果如圖9所示：1)是沿著停車場直線行走的結(jié)果；2)和3)是在轉(zhuǎn)彎處，快速轉(zhuǎn)彎導(dǎo)致特征匹配失效，發(fā)生了不同程度的偏離。

圖9 停車場實驗結(jié)果Fig.9 Experiment results in a parking spot

3.2 室外組合實驗

在室外組合實驗中，如圖10所示，點1到點2，點3到點4，點5到點6段，手動將GNSS輸入關(guān)閉(拒止場景)，通過圖6所示的方法，保證了導(dǎo)航定位的連續(xù)性服務(wù)。

圖10 GNSS拒止場景下的組合導(dǎo)航軌跡圖Fig.10 Integrated navigation trajectory diagram in GNSS-denied scenario

圖11 LiDAR/INS定位與GNSS參考結(jié)果對比Fig.11 Comparison between positioning results of LiDAR/INS odometry and GNSS reference

如圖11所示，在模擬的4段GNSS拒止場景下(前3段各自為25 s，第4段為35 s)，LiDAR/INS里程計很好地保持了GNSS/INS導(dǎo)航精度，其軌跡與GNSS參考軌跡基本一致，累積距離誤差控制在0.5 m以內(nèi)。LiDAR/INS短期定位誤差如表1所示。

表1 LiDAR/INS短期定位誤差(以GNSS為標(biāo)準(zhǔn))Tab.1 Position error statics of LiDAR/INS in short periods (take GNSS as the standard)

實驗結(jié)果表明，在對設(shè)備和算法進行改善后，完全可以勝任為GNSS信號弱的區(qū)域提供短時間相對定位任務(wù)的。

4 結(jié) 語

首先在分析了GNSS定位的局限性后，探討了一種基于SLAM技術(shù)相對定位的補償方式，并將這兩種技術(shù)結(jié)合在一起期望達到更好的定位效果，并在小車平臺上進行驗證。從本文實驗驗證中，可以得出以下結(jié)論。

1)純LiDAR定位、定姿存在一定的漂移，特別在中間出現(xiàn)特征匹配丟失的情況下，直接會引入較大的姿態(tài)、位置估計誤差。

2)在GNSS/INS定位導(dǎo)航系統(tǒng)工作期間，GNSS的丟失可以依靠LiDAR里程計去彌補并保證整個定位導(dǎo)航系統(tǒng)服務(wù)的連續(xù)性和可靠性。

從GNSS/INS/LiDAR組合方案及對應(yīng)的測試結(jié)果可以初步證實GNSS挑戰(zhàn)環(huán)境下的多源異構(gòu)組合導(dǎo)航系統(tǒng)的優(yōu)越性。但針對多源融合方式本身的算法結(jié)構(gòu)需要進一步優(yōu)化和探討，主要包括多源異構(gòu)傳感之間的深耦合組合及完好性分析，這些將有助于實現(xiàn)高精度、高魯棒性的多源融合導(dǎo)航系統(tǒng)。