許智理,閆倬豪,李星星,申志恒,周宇軒,吳宗洲,李 昕

(武漢大學(xué)測繪學(xué)院,武漢 430079)

0 引言

人工駕駛的傳統(tǒng)交通方式面臨著交通事故高發(fā)[1-2]、通勤時間過長[3-4]等諸多問題。以自動駕駛[5-6]為代表的智能駕駛技術(shù)在增強(qiáng)交通安全性、緩解擁堵等方面具有巨大應(yīng)用潛力,能夠滿足人們對于高效率、低風(fēng)險交通服務(wù)的需求,對于交通運輸系統(tǒng)具有重大革新意義。

智能駕駛指能夠智能地感知、綜合、判斷、推理、決斷和記憶并能實施主動控制以及人機(jī)交互與協(xié)同的技術(shù)[7]。高精度位置信息是智能駕駛感知環(huán)境和決策控制的基礎(chǔ)與核心。面向智能駕駛?cè)找鎻?fù)雜的應(yīng)用場景,連續(xù)、可靠、無縫的高精度位置服務(wù)已經(jīng)成為產(chǎn)業(yè)應(yīng)用的迫切需求,同時也對現(xiàn)有的導(dǎo)航定位手段提出了更高要求。目前,車載高精度定位的實現(xiàn)依賴各種傳感器技術(shù),常用技術(shù)包括全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system,GNSS)[8]、慣性導(dǎo)航系統(tǒng) (inertial navigation system,INS)[9]、視覺傳感器[10]、激光雷達(dá)(light detection and ranging, LiDAR)[11]、高精度地圖(high definition map,HD Map)[12]以及輪速計(wheel odometer)[13]。然而,在復(fù)雜多變的城市環(huán)境中,使用單一或者兩種傳感器組合的定位系統(tǒng)已經(jīng)逐漸不能滿足高可靠、高精度的定位需求,例如GNSS的信號容易受到信號衰減、反射或阻塞的影響,INS精度會受到誤差累積的影響,視覺和激光雷達(dá)等傳感器也存在受重復(fù)結(jié)構(gòu)、紋理影響等缺陷。充分利用不同傳感器互補特性的多源融合技術(shù)對于提升車載導(dǎo)航定位在城市復(fù)雜環(huán)境中的準(zhǔn)確和連續(xù)性有著重要的實際意義與應(yīng)用價值,被視為解決定位系統(tǒng)局限性和脆弱性的有效方案。

一套完備的多源融合定位方案,需要建立整體的數(shù)據(jù)處理流程,包括多源傳感器標(biāo)定、數(shù)據(jù)預(yù)處理、融合算法與質(zhì)量控制等環(huán)節(jié),以確保高精度定位服務(wù)的可靠性。其中多源融合算法直接影響到定位結(jié)果的質(zhì)量。當(dāng)下主流的多源融合算法以卡爾曼濾波(Kalman filter,KF)與因子圖優(yōu)化(factor graph optimization,FGO)兩種方式為代表[14]。在智能駕駛應(yīng)用中依據(jù)算力支持、精度需求與搭載傳感器種類對兩種方案作出合理選擇與改進(jìn)能夠?qū)崿F(xiàn)穩(wěn)健并泛在的高精度定位。

針對智能駕駛的高精度定位應(yīng)用需求,本文重點關(guān)注智能駕駛系統(tǒng)中的導(dǎo)航定位方法,從不同傳感器技術(shù)出發(fā),闡述包括GNSS、視覺、激光雷達(dá)、慣性測量單元等傳感器特性,給出主流的基于濾波和因子圖優(yōu)化的多源融合框架,并對代表性算法進(jìn)行了整理。最后總結(jié)當(dāng)前的技術(shù)研究與應(yīng)用現(xiàn)狀,并對未來智能駕駛導(dǎo)航定位技術(shù)的發(fā)展作出展望。

1 傳感器技術(shù)

1.1 全球衛(wèi)星導(dǎo)航系統(tǒng)

得益于全球覆蓋的多頻多系統(tǒng)衛(wèi)星信號在室外提供的全天候高精度定位服務(wù),GNSS是智能駕駛導(dǎo)航定位中獲取絕對位置的首選技術(shù)方案,全球定位系統(tǒng)(global positioning system,GPS)在1991年便被首次應(yīng)用于車載導(dǎo)航定位當(dāng)中[15]。GNSS信號基礎(chǔ)的偽距與載波相位觀測方程如下

(1)

(2)

隨著我國北斗三號全球衛(wèi)星導(dǎo)航系統(tǒng)(BeiDou-3 navigation satellite system,BDS-3)的正式開通,GPS、伽利略衛(wèi)星導(dǎo)航系統(tǒng)(Galileo satellite navi-gation system, Galileo)、格洛納斯衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system, GLONASS)等全球?qū)Ш叫l(wèi)星系統(tǒng)的現(xiàn)代化以及準(zhǔn)天頂衛(wèi)星系統(tǒng)(quasi-zenith satellite system, QZSS)、印度區(qū)域衛(wèi)星導(dǎo)航系統(tǒng)(Indian regional navigation satellite system, IRNSS)等區(qū)域?qū)Ш叫l(wèi)星系統(tǒng)的進(jìn)一步發(fā)展,如今已經(jīng)發(fā)展出了多頻多系統(tǒng)的衛(wèi)星導(dǎo)航定位模式,各衛(wèi)星導(dǎo)航定位系統(tǒng)的基本信息如表1所示。

表1 多頻多星座GNSS衛(wèi)星星座概況Tab.1 General situation of multi-constellation and multi-frequency signals of GNSS

當(dāng)下,為滿足智能駕駛精度需求,以RTK(real-time kinematic)、PPP(precise point positioning)、PPP-RTK為代表的基于GNSS相位測量的定位技術(shù)得到了廣泛研究[16-21]。其中,利用差分定位的RTK以及網(wǎng)絡(luò)RTK(network real-time kinematic,NRTK)[21-22]技術(shù)可以通過模糊度固定(ambiguity resolution,AR)技術(shù)實現(xiàn)瞬時毫米/厘米級精度的定位[24],已經(jīng)成為目前GNSS車載定位的主流技術(shù)方案;PPP[25]技術(shù)克服了RTK以及網(wǎng)絡(luò)RTK技術(shù)需要大量布設(shè)基站的弊端,僅利用高精度的載波相位觀測值與偽距觀測值以及精密GNSS產(chǎn)品,即可完成單臺接收機(jī)的高精度定位,用戶能夠在確保自身位置隱私不泄露的前提下在全球范圍內(nèi)實現(xiàn)厘米/分米級定位;結(jié)合了RTK與PPP技術(shù)優(yōu)點的PPP-RTK[25-26]技術(shù)可以在室外開闊環(huán)境下達(dá)到穩(wěn)定的厘米級定位精度,并且具備服務(wù)范圍廣、隱私性好等優(yōu)勢,近年來成為智能駕駛領(lǐng)域的理想技術(shù)方案,部分全球以及區(qū)域衛(wèi)星導(dǎo)航系統(tǒng)以及商業(yè)公司已經(jīng)開始提供相關(guān)服務(wù)[27]。不同GNSS技術(shù)方案的差異如表2所示。

表2 GNSS定位技術(shù)對比Tab.2 Comparations of different GNSS positioning technologies

1.2 視覺傳感器

對于智能駕駛載體來說,相機(jī)/視覺傳感器是獲取環(huán)境紋理信息的主要傳感器,依據(jù)其工作方式的不同可以分為單目相機(jī)、雙目相機(jī)、深度相機(jī)以及事件相機(jī)等。相較于其他傳感器,相機(jī)成本較低,體積較小,能夠獲得外部環(huán)境的語義特征,因此常被用于多源融合的智能駕駛導(dǎo)航定位技術(shù)當(dāng)中。利用視覺特征的視覺同時定位與建圖(visual simultaneous localization and mapping,VSLAM)[10]技術(shù),以相機(jī)作為傳感器獲取外部信息,通過對環(huán)境特征的連續(xù)跟蹤來同時實現(xiàn)自身的位姿估計以及環(huán)境結(jié)構(gòu)的重建。

通常來說,完整的VSLAM流程由前端、后端、回環(huán)檢測與建圖組成。圖1展示了一般算法的系統(tǒng)流程圖。如果側(cè)重于自身的位姿估計而不考慮一致性的建圖,則通常稱為視覺里程計(visual odometry,VO)。

圖1 VSLAM流程Fig.1 Process of VSLAM

根據(jù)視覺信息的利用方式,VO的算法主要分為兩個大類:特征點法[28]和直接法[29]。特征點法首先提取圖像關(guān)鍵點(key-point),通過特征匹配或光流跟蹤等方法實現(xiàn)不同圖像幀中特征點的數(shù)據(jù)關(guān)聯(lián),之后利用對極幾何、三角化(triangulation)、PnP(perspective-n-point)及光束法平差(bundle adjustment,BA)等方法估計路標(biāo)點位置和相機(jī)運動。直接法不提取具體的特征點,而是通過直接優(yōu)化不同圖像幀像素之間的光度誤差來同時估計像素深度和相機(jī)運動,可以充分利用圖像中具有梯度的區(qū)域信息。兩種方法對比如表3所示。

表3 直接法與間接法對比Tab.3 Comparison of direct method and indirect method

近年來,深度學(xué)習(xí)被廣泛應(yīng)用于車載導(dǎo)航的視覺里程計和回環(huán)檢測部分[37]。一些端到端的視覺里程計方法[38-40]通過神經(jīng)網(wǎng)絡(luò)可以直接輸出載體位姿。在回環(huán)檢測方面,一些在線的深度學(xué)習(xí)方法可以通過在閉環(huán)之前向系統(tǒng)添加訓(xùn)練數(shù)據(jù),從而在回環(huán)檢測期間識別車輛周圍環(huán)境對象和場景,判斷是否發(fā)生回環(huán)[41]。此外,深度學(xué)習(xí)也應(yīng)用于圖像數(shù)據(jù)處理和地圖構(gòu)建,以獲得高精度和快速的車輛位姿估計。

1.3 激光雷達(dá)

激光雷達(dá)是智能駕駛中常用的傳感器之一。與視覺傳感器相比,激光雷達(dá)傳感器的顯著優(yōu)點是測距相對準(zhǔn)確,誤差模型簡單,對光照變化不敏感,采集的點云中直接包含空間信息。因此,基于激光雷達(dá)的SLAM(LiDAR simultaneous localization and mapping,LiDAR SLAM)技術(shù)不存在尺度漂移的問題,可以獲得精確的位移估計。此外,激光雷達(dá)也常被用于障礙物檢測以及匹配定位技術(shù)當(dāng)中。近年來,隨著傳感硬件的發(fā)展,激光雷達(dá)的體積、重量、成本都有所減小,越來越多地被用于車載定位。相機(jī)和激光雷達(dá)在傳感器特性上的區(qū)別如表4所示。

表4 相機(jī)與激光雷達(dá)的互補特性Tab.4 Complementarity characteristics of camera and LiDAR

為了適應(yīng)激光雷達(dá)的技術(shù)特點,在LiDAR SLAM前端,算法需要對激光雷達(dá)點云數(shù)據(jù)進(jìn)行統(tǒng)一的預(yù)處理,其流程如圖2所示。

圖2 激光雷達(dá)預(yù)處理流程Fig.2 Data preprocessing of LiDAR

在實際應(yīng)用中,需要按照激光雷達(dá)的種類選擇合適的算法。按照獲取點云信息維度上的區(qū)別,激光雷達(dá)可以分為2D LiDAR與3D LiDAR,純激光SLAM方案也可以分為2D和3D的方案,其代表性算法如表5所示。

表5 LiDAR SLAM代表算法Tab.5 Representative algorithm of LiDAR SLAM

相較于2D LiDAR SLAM,3D LiDAR SLAM更適用于室外大尺度的復(fù)雜場景,但是其數(shù)據(jù)量和計算量更大,空間特征匹配也更復(fù)雜。由于激光點云無序、稀疏、紋理信息較弱等特點,激光SLAM需要對點云特征進(jìn)行有效的提取,從而獲取高精度的幾何約束信息。在這一點上,現(xiàn)有的LiDAR SLAM系統(tǒng)仍有很大的改進(jìn)空間。此外,智能駕駛場景中存在大量的動態(tài)物體(車輛、行人等),這對于傳統(tǒng)LiDAR SLAM的精度和穩(wěn)定性會產(chǎn)生一定的影響。近年來,基于深度學(xué)習(xí)的物體檢測技術(shù)和多目標(biāo)跟蹤技術(shù)被用于LiDAR SLAM,對周圍運動物體和自身車輛進(jìn)行聯(lián)合估計,以獲得高精度和高魯棒性的位姿估計結(jié)果[50]。

目前常用的激光雷達(dá)傳感器依照結(jié)構(gòu)上的區(qū)別可分為機(jī)械激光雷達(dá)和固態(tài)激光雷達(dá)。相比于機(jī)械雷達(dá),固態(tài)激光雷達(dá)具有可靠性高、壽命長、成本低、體積小、功耗低等優(yōu)勢,這使得固態(tài)激光雷達(dá)成為未來自動駕駛系統(tǒng)中的關(guān)鍵傳感器之一。大疆、Ouster等公司發(fā)布了多款高效商用固態(tài)激光雷達(dá),一些學(xué)者提出了相應(yīng)的固態(tài)激光雷達(dá)SLAM算法,如LiLi-OM[51]、LOAM-Livox[52]。

1.4 慣性導(dǎo)航系統(tǒng)

慣性導(dǎo)航系統(tǒng)由慣性測量單元(inertial measure-ment unit,IMU)與慣性導(dǎo)航算法兩部分組成。IMU能夠提供高頻的載體運動信息,有效填補GNSS、視覺、LiDAR等傳感器量測空缺,目前被廣泛應(yīng)用于機(jī)器人與車載導(dǎo)航等領(lǐng)域當(dāng)中。IMU能夠輸出內(nèi)置的三軸陀螺儀與加速度計量測信息,通過機(jī)械編排[9,53-55]在已知載體初始狀態(tài)的前提下進(jìn)行位姿的遞推。目前常用的慣導(dǎo)系統(tǒng)包括平臺式慣導(dǎo)(gimbaled INS,GINS)與捷聯(lián)式慣導(dǎo)(strap-down INS,SINS)。捷聯(lián)式慣導(dǎo)在體積和成本上更具優(yōu)勢,因此消費級車載導(dǎo)航系統(tǒng)更多使用捷聯(lián)式慣導(dǎo)。本文中慣導(dǎo)代指捷聯(lián)式慣導(dǎo)。

慣性器件存在不同種類的誤差源,主要包括零偏偏差、比例因子誤差、交叉耦合誤差以及隨機(jī)噪聲等。其中零偏誤差是慣導(dǎo)的關(guān)鍵指標(biāo),不同級別慣導(dǎo)零偏差別極大,穩(wěn)定性也各不相同。按照慣導(dǎo)的零偏的水平,可以將其按精度由高到低分為戰(zhàn)略級(strategic grade)、導(dǎo)航級(navigation grade)、戰(zhàn)術(shù)級(tactical gra-de)以及微機(jī)械級(micro-electro-mechanical system, MEMS)4類。慣導(dǎo)精度等級越高,其在純慣導(dǎo)遞推時抵抗位置發(fā)散的能力越強(qiáng),其成本也越高昂。因此戰(zhàn)略級與導(dǎo)航級慣導(dǎo)一般用于軍事、航海航空等高精度定位領(lǐng)域,而精度相對較低的戰(zhàn)術(shù)級與微機(jī)械級慣導(dǎo)則更多地用于車載定位中。

由于上電啟動時慣導(dǎo)坐標(biāo)系三軸指向沒有確定方位,因此需要進(jìn)行初始對準(zhǔn)以確定坐標(biāo)系指向。智能駕駛中常用的戰(zhàn)術(shù)級與微機(jī)械級慣導(dǎo)的初始對準(zhǔn)主要包括靜基座與動基座兩種方法[53-54]。

在車載定位系統(tǒng)中,由于遞推誤差的快速積累,使用純慣性導(dǎo)航技術(shù)難以完成導(dǎo)航定位工作。然而,在多源融合算法中,慣性導(dǎo)航系統(tǒng)往往被視為算法的核心。這是因為IMU可以提供連續(xù)高頻的量測信息。慣性導(dǎo)航系統(tǒng)可以與GNSS、輪速計等傳感器結(jié)合組成POS系統(tǒng)(positioning and orientation system),從而提供更為可靠的位置、方向和速度信息,降低在短時期內(nèi)面臨 GNSS 信號挑戰(zhàn)時的風(fēng)險。即使面臨頻繁的短時信號中斷,POS系統(tǒng)仍然可以提供高精度的位置定位結(jié)果。此外,與視覺、LiDAR、高精度地圖等具有環(huán)境感知功能的傳感器結(jié)合,可以利用感知技術(shù)獲取的特征、結(jié)構(gòu)以及語義信息實現(xiàn)感知增強(qiáng)定位,避免長時間處于拒止環(huán)境中導(dǎo)致定位結(jié)果退化,并能夠輔助恢復(fù)視覺尺度和補償點云運動畸變。

1.5 高精度地圖

高精度地圖一般是指由靜態(tài)圖層、語義圖層、定位圖層以及動態(tài)圖層所組合成的復(fù)合圖層地圖。在隧道與林蔭等場景下,SLAM技術(shù)不可避免地存在誤差累積的情況,系統(tǒng)的定位精度不足以滿足在城市環(huán)境中的高精度定位需要,此時可以利用離線構(gòu)建的高精度地圖,通過檢測相關(guān)環(huán)境信息,如交通燈和交通標(biāo)志,對定位結(jié)果進(jìn)行增強(qiáng),實現(xiàn)定位和建圖的解耦[58]。高精度地圖與傳統(tǒng)地圖的區(qū)別如表6所示。

表6 高精度地圖與傳統(tǒng)地圖區(qū)別Tab.6 Differences between high definition maps and traditional maps

結(jié)合基于視覺與LiDAR傳感器提取的特征制作的先驗高精度地圖,能夠在利用視覺、LiDAR方法進(jìn)行匹配定位時削弱光照變化與極端天氣等不利因素的影響[60-62]。利用高精度地圖提供的環(huán)境信息,使用感知類型傳感器以及基于特征(feature-based)或外觀(appearance-based)方法進(jìn)行定位的大致流程如圖3所示[12]。其中,數(shù)據(jù)關(guān)聯(lián)指將傳感器獲取的數(shù)據(jù)與地圖數(shù)據(jù)相關(guān)聯(lián),位置估計指利用濾波或因子圖優(yōu)化方法進(jìn)行解算。

圖3 高精地圖相對定位流程Fig.3 Relative positioning process with HD map

1.6 輪速計與運動約束

對于汽車等輪式機(jī)器的導(dǎo)航定位來說,輪速計是一種能夠提供連續(xù)速度信息的理想輔助傳感器[63]。通過獲取單位時間內(nèi)車輪轉(zhuǎn)速,在已知車輪半徑的前提下,輪速計可以得到車輪前進(jìn)方向的速度,從而在定位算法中提供量測進(jìn)行約束。

車載場景下,除了直接利用輪速計提供的量測進(jìn)行約束以外,通過判斷車體運動模式,進(jìn)行車體運動約束(vehicle motion constraint,VMC)的方法也能夠有效抑制誤差漂移[64]。例如,當(dāng)車體處于靜止?fàn)顟B(tài)時,由于速度、角速度以及加速度等與載體狀態(tài)變化相關(guān)的速度量在理想狀態(tài)下均為零,可以采用零速修正(zero velocity update,ZUPT)與零角速度修正(zero angular rate update,ZAUPT)[13]的方式進(jìn)行約束。當(dāng)處于運動狀態(tài)時,由于大多數(shù)情況下車體進(jìn)行直線行駛,車體坐標(biāo)系下其側(cè)向與法向速度可視為零,非完整性約束(non-holonomic constraint,NHC)[65]能夠采用上述特性對車體二維速度提供約束。

2 多源數(shù)據(jù)融合

考慮到各種傳感器的不同特點以及互補特性,對多源傳感器數(shù)據(jù)進(jìn)行融合可以有效地克服單一傳感器的局限性,提升導(dǎo)航定位效果。本章將重點關(guān)注多源數(shù)據(jù)融合方法,首先提出基于不同融合方法的多源融合定位模型,在此基礎(chǔ)上對相關(guān)傳感器融合代表算法以及各類算法特點進(jìn)行總結(jié)。

2.1 狀態(tài)估計

多源融合算法決定了不同傳感器間的信息利用和融合方式,直接影響到融合定位的結(jié)果質(zhì)量。當(dāng)前主流融合算法估計器以貝葉斯濾波與因子圖優(yōu)化為代表,下文將對兩種方法展開介紹。

2.1.1 濾波

在傳感器融合中,由于位置、速度、姿態(tài)、零偏的估計以及特征檢測并不滿足線性條件,目前在多源融合濾波中使用最多的算法是非線性的擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)[66]、無跡卡爾曼濾波[67](unscented Kalman filter,UKF)、粒子濾波(particle filter,PF)[68]等貝葉斯濾波形式。其中,EKF采用線性化的方式將非線性系統(tǒng)近似為線性系統(tǒng)。UKF與PF不存在線性化的過程,而是選擇采用一組隨機(jī)樣本(粒子),利用樣本均值代替積分運算的方式來處理非線性問題。相較EKF而言,UKF與PF能夠達(dá)到更高的精度,但考慮采樣過程的UKF與PF具有更大的運算成本[69]。當(dāng)傳感器噪聲滿足或近似于高斯分布且傳感器模型非線性程度不強(qiáng)時,采用易于使用的EKF的方法能夠在確保計算效率的情況下滿足智能駕駛服務(wù)的精度需求。

以EKF方法為例,基于式(3)所示的狀態(tài)空間模型與量測模型,其濾波過程由時間更新與量測更新兩部分組成,如圖4所示。其中,X為狀態(tài)量,Z為觀測量,φ為狀態(tài)轉(zhuǎn)移矩陣,W為系統(tǒng)噪聲,V為量測噪聲,Γ為噪聲分配矩陣,P為協(xié)方差陣,Q為過程噪聲方差陣,R為量測噪聲方差陣,k為增益矩陣,H為觀測系數(shù)陣。

圖4 多源融合EKF流程Fig.4 EKF process of multi-sensor fusion

(3)

基于濾波的多源融合方法一般以慣導(dǎo)作為主濾波器,狀態(tài)向量包括三維的姿態(tài)、速度、位置以及加速度計與陀螺零偏,如式(4)所示,其中δθ為失準(zhǔn)角向量、δv為速度誤差向量、δp為位置誤差向量、δba為加速度計零偏誤差向量、δbg為陀螺儀零偏誤差向量。其他傳感器相關(guān)狀態(tài)向量需要合并至慣導(dǎo)狀態(tài)向量中進(jìn)行時間更新,獲取的原始觀測數(shù)據(jù)通過量測方程的形式加入量測更新過程當(dāng)中,如式(5)、式(6)所示。

XINS=[δθδvδpδbaδbg]T

(4)

X=[XINSXGNSSXVisionXLiDAR…]T

(5)

Z=[ZINSZGNSSZVision
ZLiDARZConstraint…]T

(6)

現(xiàn)有的多源融合定位濾波模型大多建立在GNSS/INS組合導(dǎo)航的基礎(chǔ)上,通過加入相機(jī)、激光雷達(dá)等環(huán)境感知傳感器進(jìn)行增強(qiáng)。感知傳感器技術(shù)為高精度定位賦予了環(huán)境信息,對于城市峽谷、隧道以及林蔭道等GNSS信號遮蔽嚴(yán)重的場景,通過環(huán)境感知提供的特征、結(jié)構(gòu)以及語義信息可以增強(qiáng)定位,避免長時間處于拒止環(huán)境中導(dǎo)致定位結(jié)果退化。由此所建立的多源融合濾波基本框架如圖5所示。

圖5 基于濾波、以IMU為中心的多傳感器融合框架Fig.5 Filtering based and IMU-centric multi-sensor fusion framework

2.1.2 因子圖優(yōu)化

因子圖優(yōu)化方法在近年來表現(xiàn)出巨大潛力[70-72],該方法采用由因子節(jié)點(factor)與變量節(jié)點(variable)以及相連接的邊所組成的因子圖表達(dá)優(yōu)化問題,以聯(lián)合概率密度最大化來解決優(yōu)化問題。在多源融合定位中,為了將相機(jī)與激光雷達(dá)等傳感器量測幀間的IMU數(shù)據(jù)整合,需要通過IMU的預(yù)積分使位姿解耦,避免重復(fù)積分,提高優(yōu)化效率[73]。GNSS、相機(jī)、雷達(dá)等傳感器作為觀測因子加入系統(tǒng)中進(jìn)行聯(lián)合優(yōu)化,如式(7)。

(7)

得益于多次迭代與迭代過程中的線性化以及對歷史信息的充分利用,基于因子圖優(yōu)化的方法理論精度比濾波方法更優(yōu),但是其對于內(nèi)存的使用和計算量也相應(yīng)地增加。在一些應(yīng)用中,為了平衡算法的精度與效率,可以選擇采用基于滑動窗口的邊緣化方法。通過消去舊的變量,維護(hù)新的變量,在合適的窗口尺度下將算法的計算負(fù)擔(dān)控制在一定的范圍內(nèi)[74]。采用滑動窗口的多源融合因子圖優(yōu)化模型如圖6所示。

圖6 基于滑窗的因子圖優(yōu)化多源融合示意圖Fig.6 Diagram of multi-sensor fusion based on factor graph optimization utilizing sliding window

相比于傳統(tǒng)的EKF方案,緊組合的因子圖優(yōu)化方案在一定條件下經(jīng)驗證具有更優(yōu)的解算精度與魯棒性[14]。圖7展示了經(jīng)典的GNSS/IMU/Camera/LiDAR等多源融合因子圖優(yōu)化框架。

圖7 多源融合因子圖優(yōu)化框架Fig.7 Multi-sensor fusion framework based on factor graph optimization

雖然因子圖優(yōu)化在理論上相對于濾波方法具有更高的精度優(yōu)勢,但在實時車載導(dǎo)航定位中應(yīng)用并不廣泛。主要原因是其受限于大量的歷史信息與多次迭代帶來的計算負(fù)擔(dān),需要充足的硬件算力支持,在實時應(yīng)用中具有一定的困難。尤其是在進(jìn)行多源融合時,多種傳感器測量信息的處理進(jìn)一步加重了算力負(fù)擔(dān)。然而,精度和擴(kuò)展性上的優(yōu)勢決定了它在智能駕駛領(lǐng)域具有廣闊的應(yīng)用前景。近年來,在智能駕駛領(lǐng)域,以百度Apollo為代表的無人車研究項目已經(jīng)開始嘗試采用基于因子圖優(yōu)化的框架[75]。

2.2 多源融合算法

智能駕駛中的多源融合定位建立在組合導(dǎo)航的基礎(chǔ)上,在智能駕駛導(dǎo)航定位組合當(dāng)中,結(jié)合異構(gòu)傳感器的不同特性有助于提升整體系統(tǒng)對于不同環(huán)境的適應(yīng)能力。在傳統(tǒng)應(yīng)用中,由于GNSS的絕對定位以及INS的位姿推算能力,基于GNSS/INS的POS系統(tǒng)是應(yīng)用最為廣泛的二者組合方案,能夠在室外開闊環(huán)境下滿足智能駕駛的車道級服務(wù)需求,進(jìn)行高精度的定位定姿,并且能夠抵抗短時間的GNSS信號中斷[67-68]。除了基本的固定單天線GNSS/INS配置外,基于多天線乃至移動天線的POS系統(tǒng)也得到廣泛研究,并被驗證能夠達(dá)到更加精確的定位定姿效果[78-80]。Li等人利用輪速計進(jìn)行量測并輔助車體運動約束,進(jìn)一步提出了PPP-RTK/INS/Odometer緊組合濾波模型,并在多場景下驗證了其定位定姿精度與穩(wěn)定性的提升[13]。

對于GNSS長時間信號失鎖的環(huán)境,智能駕駛可以通過搭載的傳感器感知環(huán)境特征來估計自身的位姿信息,以實現(xiàn)對單一定位方式的補充,避免定位結(jié)果退化。感知融合定位中常用的傳感器包括相機(jī)、LiDAR等。GNSS/INS/Vision(GIV)、GNSS/INS/LiDAR(GIL)、GNSS/INS/Vision/LiDAR(GIVL)也成為智能駕駛中最常用的算法。下文將從以上算法出發(fā)介紹多源異構(gòu)信息融合(multi-source and heterogeneous information fusion,MSHIF)算法在智能駕駛中的發(fā)展。

2.2.1 GNSS/INS/Vision融合算法

得益于成本相對較低以及能夠獲取語義信息的優(yōu)勢,視覺通常是進(jìn)行感知融合定位的首選方案。在GIV融合算法發(fā)展初期,利用松組合的濾波與因子圖優(yōu)化算法占據(jù)主流。Chu等人利用濾波方法,結(jié)合GPS差分偽距觀測值恢復(fù)了相機(jī)幀間尺度信息,完成了GIV三者的松組合,但該方法在GNSS拒止環(huán)境下可用性不強(qiáng)[81]。利用矩陣的稀疏性,Indelman等人利用因子圖優(yōu)化方法,實現(xiàn)了不同傳感器的即插即用[82]。松組合算法實現(xiàn)簡單,可以從結(jié)果層面上實現(xiàn)抵抗單一傳感器退化的目標(biāo)。但由于松組合對信息利用并不充分,沒有深入挖掘異構(gòu)傳感器間原始信息的耦合關(guān)系,因此整個系統(tǒng)魯棒性較弱;在單一傳感器位置解算失敗時,系統(tǒng)會退化到二者組合甚至單一傳感器定位方案。在松組合GIV算法的許多研究中,GNSS主要用來提供絕對坐標(biāo),削弱INS遞推發(fā)散以及恢復(fù)相機(jī)幀間尺度信息,但在進(jìn)行定位時僅應(yīng)用了偽距定位的結(jié)果,沒有充分利用更高精度載波相位觀測。由于視覺特征點法更適合全局匹配與回環(huán)檢測,采用直接法進(jìn)行多源融合的算法方案較少。Wang等人在GIV因子圖優(yōu)化算法中采用了直接法的視覺方案,并輔以磁力計量測信息,效果比VINS(visual-inertial navigation system)算法更優(yōu)[83]。

隨著緊組合研究的不斷開展,如何進(jìn)行原始觀測值層面的耦合成為研究重點。在緊組合算法中,GNSS由于具有多種觀測信息與定位模式,融合RTK/INS/Vision、PPP/INS/Vision、PPP-RTK/INS/Vision的GIV緊組合算法也先后被提出。利用半緊組合,Wang等人在帶滑窗的因子圖優(yōu)化框架下利用半緊組合結(jié)合多系統(tǒng)GNSS的PPP方法與S-VINS(stereo VINS)方法,提升了系統(tǒng)在GNSS拒止環(huán)境下的表現(xiàn)[84]。Liao等人利用偽距、相位、多普勒多種GNSS觀測實現(xiàn)了緊組合的GIV濾波,將VIO模型從局部框架轉(zhuǎn)換到全球框架,利用差分GNSS在拒止環(huán)境下實現(xiàn)了米級定位[85]。Zhu等人提出利用先驗特征圖的方式輔助GIV緊組合的算法,實現(xiàn)了城市環(huán)境下厘米級的定位[86]。Cao等人采用緊組合的方式,將GNSS偽距、多普勒與視覺約束、慣性約束結(jié)合到緊組合的因子圖優(yōu)化框架中,僅使用一顆衛(wèi)星量測信息也能對系統(tǒng)提供增益[71]。

由于相位整周模糊度具有整數(shù)特性,考慮相位模糊度固定的GNSS定位方案能夠提供更高的精度。文獻(xiàn)[87]與[88]在考慮模糊度固定的情況下實現(xiàn)了RTK、PPP、PPP-RTK方法的GIV三者濾波緊組合,并在GNSS挑戰(zhàn)環(huán)境中得到了效果驗證。圖8與圖9分別展示了RTK/PPP與PPP-RTK結(jié)合INS與視覺的融合方案測試結(jié)果。圖8(a)為設(shè)備圖,圖8(b)為GNSS挑戰(zhàn)環(huán)境場景,圖8(c)表示GNSS挑戰(zhàn)場景下的GIV融合定位方案結(jié)果,其中VINS-Fusion(RTK)與VINS-Fusion(PPP)代表VINS松耦合RTK或者PPP的結(jié)果?？梢钥闯?由于嚴(yán)重的信號中斷,基于RTK與PPP的VINS-Fusion都偏離了參考值。相比之下,采用RTK或PPP模型的GIV三者緊組合系統(tǒng)可以有效抑制位置發(fā)散,相應(yīng)軌跡能夠得到更好的約束并且更加平滑。圖9(a)為設(shè)備圖,圖9(b)為典型場景,圖9(c)為PPP-RTK/INS/Vision融合定位方案結(jié)果。PPP-RTK可以在衛(wèi)星觀測充足的情況下實現(xiàn)厘米級定位。其他傳感器的加入進(jìn)一步提高了定位性能,減少了定位序列中由于模糊度固定失敗產(chǎn)生的異常值。在圖9(c)中陰影部分所代表的城市地區(qū)衛(wèi)星數(shù)急劇下降的典型情況下,PPP-RTK定位誤差發(fā)散可達(dá)數(shù)米。在戰(zhàn)術(shù)級IMU輔助下,定位仍能達(dá)到厘米級精度。然而,MEMS-IMU由于高噪聲與零偏不穩(wěn)定性致使改進(jìn)有限。但通過將視覺信息加入PPP-RTK/MEMS系統(tǒng)后,在準(zhǔn)確性與可靠性方面都達(dá)到了PPP-RTK與戰(zhàn)術(shù)級IMU組合相同的水平。

圖8 RTK與PPP方案下融合定位誤差序列[87]Fig.8 Position error of fusion scheme integrated with RTK and PPP[87]

圖9 PPP-RTK方案下融合方案不同尺度定位誤差序列[88]Fig.9 Position error of fusion scheme integrated with PPP-RTK[88]

基于GIV融合代表性成果如表7所示。在表中,僅在GNSS、INS、視覺均采用原始觀測值進(jìn)行融合時,才將該方法視為緊組合方法。其中P指偽距,L指相位,D指多普勒觀測值,AR指模糊度固定,LC,TC,STC分別指松組合、緊組合與半緊組合。由于松組合算法只利用了GNSS單獨解算的位置、速度信息,表中對應(yīng)部分以“/”代表GNSS單獨解算結(jié)果。

表7 GNSS/INS/Vision融合算法代表性成果Tab.7 Representative results of GNSS/INS/Vision fusion algorithm

2.2.2 GNSS/INS/LiDAR融合算法

由于LiDAR的精確測距能力,其能夠獲得較高精度的位姿推算以及環(huán)境三維重建結(jié)果,對于智能車輛應(yīng)用來說具有較強(qiáng)的實用性。在大尺度場景下,結(jié)合了GNSS/INS/LiDAR三種傳感器的GIL算法得到廣泛研究。受LiDAR硬件發(fā)展的影響,GIL算法早期以融合2D LiDAR為主,Soloviev在2008年較早地提出了GIL的緊組合濾波方案,在城市環(huán)境中實現(xiàn)了厘米級的軌跡重建[89]。利用松組合形式,Gao等人采用創(chuàng)新的掃描匹配算法提升了GPS/INS/LiDAR組合的精度與穩(wěn)定性[90]。隨著3D LiDAR的普及應(yīng)用,融合3D LiDAR的GIL算法成為主流。Chiang等人利用3D LiDAR,提出了一種INS/GNSS/Refresh-SLAM算法,實現(xiàn)了車道級定位,但只涉及了平面速度[91]。Shan等人提出了基于因子圖優(yōu)化的LIO-SAM(LiDAR inertial odometry with smoothing and mapping)框架,通過可選的GPS因子糾正INS/LiDAR組合結(jié)果的發(fā)散[70]。在GIL的質(zhì)量控制方面,Wang等人利用3D-LiDAR輔助GNSS/INS組合,在故障檢測、定位性能與完好性評估均得到了提升[92]。

由于LiDAR數(shù)據(jù)量巨大,在GIL緊組合的應(yīng)用中,雖然融合算法精度進(jìn)一步提升[93],但數(shù)據(jù)處理壓力對整個定位系統(tǒng)的實時性帶來了挑戰(zhàn)。文獻(xiàn)[94]使用GNSS的RTK方法與INS與LiDAR進(jìn)行融合,提高了GNSS挑戰(zhàn)環(huán)境下RTK固定率與穩(wěn)定性,但由于LiDAR數(shù)據(jù)量大的影響,RTK結(jié)果輸出延遲。Li等人提出了一種PPP/INS/PF(plane feature)-LiDAR算法,首次將GNSS偽距以及載波相位量測、MEMS-IMU數(shù)據(jù)以及LiDAR平面特征緊組合在濾波框架當(dāng)中并實現(xiàn)了實時應(yīng)用,增強(qiáng)了GIL算法的應(yīng)用能力[77],文獻(xiàn)中對比了PF-LiDAR與PLAP(point to line and plane)-LiDAR等方法在融合中的表現(xiàn),其中GNSS挑戰(zhàn)環(huán)境下的測試結(jié)果如圖10所示,其中圖10(a)為設(shè)備圖,圖10(b)為典型場景,圖10(c)、圖10(d)和圖10(e)分別為定位、測速、定姿結(jié)果。由定位誤差序列圖可以發(fā)現(xiàn),所有方案垂直精度都低于水平精度,主要原因是GNSS與LiDAR在垂直方向的可觀測性較弱。與PPP/INS/PF-LiDAR松組合類似,PPP/INS緊組合積分存在明顯的位置發(fā)散。通過與LiDAR進(jìn)行緊組合,定位結(jié)果更加平滑,整體精度也提升了一個量級,其中PPP/INS/PF-LiDAR相比PPP/INS/PLAP-LiDAR精度進(jìn)一步提高。不同方法激光雷達(dá)部分的平均運行時長如表8所示。基于GIL的融合算法代表性成果總結(jié)如表9所示。

圖10 GNSS挑戰(zhàn)環(huán)境下GIV緊組合算法測試結(jié)果[77]Fig.10 Test results of GIV tight integration algorithm under GNSS challenge environment[77]

表8 激光雷達(dá)部分平均處理時間[77]Tab.8 Average processing time in LiDAR part[77]

表9 GNSS/INS/LiDAR融合算法代表性成果Tab.9 Representative results of GNSS/INS/LiDAR fusion algorithm

2.2.3 GNSS/INS/Vision/LiDAR融合算法

GNSS/INS/Vision/LiDAR四者融合基本上建立在上述三者融合方案之上,屬于較為綜合的組合導(dǎo)航方案。由于涉及傳感器種類多,系統(tǒng)設(shè)計相對復(fù)雜,目前文獻(xiàn)中關(guān)于四者融合的算法研究較少,已有的代表性成果如表10所示。

表10 GNSS/INS/Vision/LiDAR融合算法代表性成果Tab.10 Representative results of GNSS/INS/Vision/LiDAR fusion algorithm

對于因子圖優(yōu)化的方法來說,四者融合的計算負(fù)擔(dān)較重。利用GNSS單獨解算提供的絕對位置進(jìn)行約束是較為可行的方法。Wang等人在基于因子圖的緊組合雙目視覺/慣性/激光雷達(dá)SLAM的基礎(chǔ)上,利用GNSS(PPP/RTK)提供的絕對位置信息,實現(xiàn)了一種解耦合的多傳感器融合定位算法[95]。Lee等人基于MSCKF提出了MINS方法,實現(xiàn)了GIVL四者組合,并通過了實時測試[96]。Li等人通過濾波方法在原始觀測值層面上對GNSS/INS/Vision/LiDAR進(jìn)行了緊組合,充分利用了異構(gòu)傳感器信息,對傳感器退化具有自然的魯棒性[97]。文獻(xiàn)[97]分析了四者緊組合相較三者與二者組合在多場景下的定位性能提升,其定位效果如圖11所示,圖11(a)為設(shè)備圖,圖11(b)為典型場景,圖11(c)、圖11(d)和圖11(e)分別為定位、測速、定姿結(jié)果。可以看出四者融合的方法在GNSS拒止環(huán)境下具有最優(yōu)的綜合定位、測速與定姿性能,這些提升主要來自魯棒的視覺與激光雷達(dá)提供的3D環(huán)境特征,尤其是在長隧道的環(huán)境下表現(xiàn)出了更強(qiáng)的抑制定位結(jié)果發(fā)散的能力,如圖12。圖12(a)為長隧道典型場景,圖12(b)為不同算法在長隧道場景下的定位結(jié)果。然而該方法帶來了巨大的運算資源負(fù)擔(dān)。如何實現(xiàn)GIVL緊組合算法的實時應(yīng)用,將是未來的研究重點。

圖11 GNSS挑戰(zhàn)環(huán)境下GIVL緊組合算法測試結(jié)果[97]Fig.11 Test results of GIVL tight integration algorithm under GNSS challenged environment[97]

圖12 長隧道環(huán)境下不同算法魯棒性對比[97]Fig.12 Robustness comparison of different algorithms in long tunnel environment[97]

2.3 挑戰(zhàn)與機(jī)遇

2.3.1 挑戰(zhàn)

近年來,關(guān)于多源融合定位的系統(tǒng)實現(xiàn)以及性能評估已經(jīng)有了較多的研究,其結(jié)果表明,對于智能駕駛而言,多源融合技術(shù)大幅提高了定位系統(tǒng)的精度與魯棒性。但對于進(jìn)一步實現(xiàn)實時與泛在的智能駕駛而言,多源融合技術(shù)仍面臨著如下挑戰(zhàn):

1)在半開闊以及GNSS拒止等極端場景下,現(xiàn)有的多源融合算法定位性能仍有所欠缺。由于極端環(huán)境下往往面臨嚴(yán)重的多徑效應(yīng)、NLOS以及特征退化等影響,單一傳感器定位性能退化嚴(yán)重,相互之間增益有限。例如文獻(xiàn)[97]在長隧道中,僅能實現(xiàn)米級定位。對于車位級甚至未來更高精度的定位需求而言,當(dāng)前的多源融合算法難以滿足需要。

2)融合傳感器數(shù)目以及種類的增加對定位模塊帶來沉重負(fù)擔(dān)。對于多源融合算法而言,盡管傳感器冗余能夠提供更高的定位精度,但卻增加了算力負(fù)擔(dān)。對于算力有限的智能駕駛平臺而言,定位模塊的算力壓力不僅影響到定位服務(wù)性能,而且可能為評估、規(guī)劃與決策等其他模塊帶來風(fēng)險。

3)高精度傳感器價格昂貴,制作魯棒的高精度消費級多源融合定位平臺具有挑戰(zhàn)性。高精度傳感器具有更高的精度與穩(wěn)定性,但卻價格高昂。除了硬件本身的成本,還需要考慮相應(yīng)定位服務(wù)的成本,例如RTK技術(shù)需要相應(yīng)廠商的基站配合提供高精度定位服務(wù)。

2.3.2 機(jī)遇

近年來,隨著導(dǎo)航定位應(yīng)用領(lǐng)域的進(jìn)一步拓寬以及制造業(yè)技術(shù)的進(jìn)一步發(fā)展,定位算法與傳感器硬件都開始展現(xiàn)出新的發(fā)展趨勢,為智能駕駛的定位性能提供了新的機(jī)遇:

1)多車協(xié)同與多源同構(gòu)等算法得到廣泛關(guān)注[88-89]。多車協(xié)同定位算法優(yōu)化了集群智能體的定位性能。隨著智能網(wǎng)聯(lián)車的應(yīng)用,多車協(xié)同的定位算法得到推廣,對于未來基于V2V(vehicle to vehicle)的集群智能駕駛應(yīng)用來說具有重要意義。多源同構(gòu)進(jìn)一步提升了多源融合定位的精度與魯棒性。除了本文所詳細(xì)討論的多源異構(gòu)定位之外,多源同構(gòu)定位方法也得到了廣泛研究。多天線、多IMU定位等多源同構(gòu)定位方案在現(xiàn)有的研究中展現(xiàn)出巨大的應(yīng)用前景。

2)傳感器技術(shù)的快速進(jìn)步。高精度定位需求的上升推動了傳感器需求爆發(fā)式增長,傳感器制造能力與應(yīng)用也迎來了技術(shù)性突破。例如,多頻多星座系統(tǒng)與低軌衛(wèi)星增強(qiáng)、MEMS IMU性能進(jìn)一步提升、事件相機(jī)與4D雷達(dá)技術(shù)的發(fā)展以及相關(guān)傳感器成本的下降,都為高精度智能駕駛提供了更大的發(fā)展空間。

3 結(jié)論

本文介紹了智能駕駛中常用的傳感器技術(shù),分別闡述了目前采用GNSS、視覺、LiDAR、INS等技術(shù)的定位方法與發(fā)展現(xiàn)狀。由于沒有任何一種單一的傳感器可以滿足城市多種復(fù)雜環(huán)境下的高精度定位,本文從彌補單一技術(shù)缺陷的角度出發(fā),進(jìn)一步總結(jié)了基于濾波與因子圖優(yōu)化的多源融合定位框架,并介紹了GIV、GIL與GIVL融合方式的代表性成果。結(jié)果表明,多源融合的定位方法大幅提升了智能駕駛定位模塊的精度與魯棒性。

然而,目前智能駕駛的多源融合定位方法仍面臨一些挑戰(zhàn),例如在極端場景下的定位性能退化、多源數(shù)據(jù)嚴(yán)重影響定位模塊的實時性、多源傳感器平臺搭建成本高昂。同時,導(dǎo)航定位應(yīng)用領(lǐng)域的進(jìn)一步拓寬及硬件制造能力的提升為智能駕駛的多源融合方案提供了新的機(jī)遇。多機(jī)協(xié)同、多源同構(gòu)等方向的研究將會進(jìn)一步提升智能駕駛的定位性能,傳感器的革新會加速上述技術(shù)的應(yīng)用。