[首發(fā)于智駕最前沿微信公眾號(hào)]對(duì)于自動(dòng)駕駛車輛而言，不僅需要感知周圍的障礙物，更需要時(shí)刻明確自己的精確位置，以及相對(duì)于道路邊界的細(xì)微偏差。然而，在真實(shí)的城市街道中，實(shí)現(xiàn)厘米級(jí)、高頻率且具備極高魯棒性的定位是一項(xiàng)極其復(fù)雜的系統(tǒng)工程。

衛(wèi)星導(dǎo)航定位技術(shù)（GNSS）與慣性導(dǎo)航技術(shù)（IMU）雖然構(gòu)成了定位系統(tǒng)的基礎(chǔ)架構(gòu)，但它們?cè)诿鎸?duì)摩天大樓林立的“城市峽谷”、深邃的隧道以及交錯(cuò)的立交橋時(shí)，會(huì)表現(xiàn)出顯著的性能衰減。那如何解決城市復(fù)雜環(huán)境下定位失準(zhǔn)的問題？

城市環(huán)境對(duì)基礎(chǔ)定位傳感器的挑戰(zhàn)與局限分析

全球衛(wèi)星導(dǎo)航系統(tǒng)（GNSS）在提供絕對(duì)地理位置參考方面具有天然的優(yōu)勢(shì)，其工作原理主要基于衛(wèi)星到接收機(jī)之間的距離測(cè)量。但在城市中心區(qū)域，這種基于電磁波傳播的測(cè)量方式極易受到物理環(huán)境的干擾。其問題主要源于多路徑效應(yīng)（Multipath）和非視距接收（NLOS）。

當(dāng)衛(wèi)星信號(hào)在到達(dá)車載接收機(jī)之前，經(jīng)過高大建筑物的玻璃幕墻、地面或其他反射面的多次折射或反射后，接收機(jī)捕獲到的信號(hào)路徑會(huì)長(zhǎng)于真實(shí)的直線距離。這種現(xiàn)象會(huì)導(dǎo)致偽距觀測(cè)產(chǎn)生數(shù)米甚至數(shù)十米的誤差。

若當(dāng)直接視距被建筑完全遮擋，接收機(jī)僅收到反射信號(hào)，此時(shí)定位結(jié)果會(huì)出現(xiàn)劇烈的跳變，甚至導(dǎo)致定位解算完全失效。在深窄的城市街道中，可見衛(wèi)星數(shù)量可能在短時(shí)間內(nèi)從十余顆銳減至三顆以下，無法滿足標(biāo)準(zhǔn)定位所需的最小觀測(cè)條件。

為了彌補(bǔ)衛(wèi)星信號(hào)的波動(dòng)，慣性測(cè)量單元（IMU）被引入系統(tǒng)，其利用加速度計(jì)和陀螺儀感知車輛的即時(shí)運(yùn)動(dòng)狀態(tài)。IMU的優(yōu)勢(shì)在于其完全自主性，不需要外部信號(hào)，且輸出頻率極高。

然而，IMU本質(zhì)上是一個(gè)積分系統(tǒng)，它通過對(duì)加速度和角速度進(jìn)行二次積分來推算位置和姿態(tài)，這種機(jī)制導(dǎo)致其存在誤差累積。IMU傳感器存在復(fù)雜的誤差分量，其中包括偏置不穩(wěn)定性、軸偏斜和隨時(shí)間增長(zhǎng)的隨機(jī)游走。

偏置不穩(wěn)定性意味著即使車輛靜止，IMU輸出的微小偏差也會(huì)隨著時(shí)間不斷積分，導(dǎo)致位置估算迅速漂移。對(duì)于常見的微機(jī)電系統(tǒng)（MEMS）級(jí)別的IMU，如果沒有外部參考信號(hào)進(jìn)行校正，其推算的位置誤差在一分鐘內(nèi)就可能達(dá)到數(shù)十米甚至更多，這對(duì)于厘米級(jí)定位要求的自動(dòng)駕駛而言是不可接受的。

圖片源自：網(wǎng)絡(luò)

城市環(huán)境的復(fù)雜性還體現(xiàn)在其多變的氣候與光照條件下。在隧道或高架橋下，衛(wèi)星信號(hào)會(huì)被完全切斷，定位系統(tǒng)必須完全依賴車輛的“內(nèi)感知”能力。

而在暴雨、大霧或降雪等惡劣天氣中，空氣中懸浮的微小顆粒會(huì)嚴(yán)重散射激光雷達(dá)的紅外脈沖，導(dǎo)致點(diǎn)云數(shù)據(jù)中出現(xiàn)大量噪聲和假陽(yáng)性障礙物。

視覺傳感器則在強(qiáng)光直射、夜晚低照度或面對(duì)單調(diào)的白墻時(shí)，難以提取到有效的紋理特征，從而導(dǎo)致特征跟蹤失敗。

SLAM算法的介入與多源信息融合的技術(shù)演進(jìn)

面對(duì)上述單一傳感器的局限性，融合激光雷達(dá)SLAM（LIO）及視覺SLAM（VIO）算法成為了提升系統(tǒng)魯棒性的路徑之一。

SLAM算法的本質(zhì)是通過感知環(huán)境特征來反推自身的位姿變化。激光雷達(dá)通過發(fā)射激光束并接收反射回波，能夠?qū)崟r(shí)生成車輛周圍三維環(huán)境的“點(diǎn)云地圖”。在SLAM過程中，算法會(huì)將當(dāng)前幀的點(diǎn)云與之前的地圖進(jìn)行幾何匹配（如常用的ICP或NDT算法），從而推算出車輛的相對(duì)位移。

在衛(wèi)星信號(hào)失效的隧道或地下停車場(chǎng)，激光雷達(dá)可以憑借周圍墻壁、支柱等固定特征，提供非常穩(wěn)定的運(yùn)動(dòng)約束，極大地減緩了IMU的漂移速度。視覺SLAM則利用圖像序列中的特征點(diǎn)（如路牌、建筑輪廓）進(jìn)行運(yùn)動(dòng)估計(jì)，其在幾何特征不明顯的平坦道路上具有較好的補(bǔ)充作用。

圖片源自：網(wǎng)絡(luò)

在技術(shù)實(shí)現(xiàn)方案上，多傳感器融合正經(jīng)歷著從松耦合到緊耦合的跨越。

松耦合方案將GNSS給出的絕對(duì)坐標(biāo)、SLAM給出的相對(duì)位移以及IMU的推算結(jié)果分別作為獨(dú)立的輸入，在卡爾曼濾波器中進(jìn)行結(jié)果級(jí)的融合。

雖然這種方式邏輯清晰、計(jì)算量小，但在極具挑戰(zhàn)性的城市環(huán)境中，它的表現(xiàn)依舊不盡如人意。如當(dāng)可見衛(wèi)星不足四顆時(shí)，GNSS解算器可能直接報(bào)錯(cuò)退出，此時(shí)松耦合系統(tǒng)就無法利用剩余的三顆衛(wèi)星所包含的信息。

緊耦合方案則完全打破了傳感器之間的邊界，它直接處理每個(gè)傳感器的原始觀測(cè)數(shù)據(jù)，如GNSS的偽距和多普勒頻率、激光雷達(dá)的原始反射點(diǎn)坐標(biāo)以及IMU的高頻采樣信號(hào)等。

圖片源自：網(wǎng)絡(luò)

這些原始數(shù)據(jù)會(huì)被放置在同一個(gè)數(shù)學(xué)優(yōu)化框架中進(jìn)行聯(lián)合求解。在這種模式下，即使衛(wèi)星數(shù)量極少，哪怕只有兩三顆，系統(tǒng)依然可以結(jié)合激光雷達(dá)提供的地面約束和IMU提供的運(yùn)動(dòng)先驗(yàn)，對(duì)車輛位姿進(jìn)行有效約束，從而在極大程度上縮短信號(hào)盲區(qū)的“斷檔期”。

更進(jìn)一步的“深耦合”（Deep Coupling）技術(shù)甚至深入到GNSS接收機(jī)的信號(hào)跟蹤環(huán)路，利用IMU感知到的瞬時(shí)速度變化去輔助載波跟蹤環(huán)路，從而讓車輛在復(fù)雜環(huán)境下能夠鎖定衛(wèi)星信號(hào)，有效對(duì)抗由于震動(dòng)或快速運(yùn)動(dòng)導(dǎo)致的失鎖問題。

因子圖優(yōu)化，構(gòu)建抗干擾的魯棒估計(jì)框架

在融合定位的數(shù)學(xué)求解中，因子圖優(yōu)化（Factor Graph Optimization, FGO）正在逐漸取代傳統(tǒng)的擴(kuò)展卡爾曼濾波（EKF），成為支撐自動(dòng)駕駛定位系統(tǒng)的核心框架。

EKF作為一種基于時(shí)間遞歸的方法，其最大的弱點(diǎn)在于“健忘”，它在每個(gè)時(shí)刻只保留前一刻的狀態(tài)，并根據(jù)當(dāng)前觀測(cè)進(jìn)行一次更新，無法利用歷史數(shù)據(jù)進(jìn)行回溯修正。

在城市環(huán)境中，如果接收機(jī)誤將一個(gè)被建筑反射的強(qiáng)信號(hào)當(dāng)作真值，EKF會(huì)由于單次權(quán)重的傾斜而瞬間產(chǎn)生巨大的定位誤差，且很難在之后的步驟中自我修正。

圖片源自：網(wǎng)絡(luò)

因子圖優(yōu)化則將定位問題建模為一個(gè)龐大的數(shù)學(xué)圖表。在這個(gè)圖中，節(jié)點(diǎn)代表車輛在不同歷史時(shí)刻的位姿和運(yùn)動(dòng)參數(shù)，而連接節(jié)點(diǎn)的邊（即因子）則代表各種傳感器提供的觀測(cè)約束。

簡(jiǎn)單理解下，IMU因子連接著相鄰的時(shí)間節(jié)點(diǎn)，代表運(yùn)動(dòng)的連續(xù)性；GNSS因子為特定節(jié)點(diǎn)提供全球坐標(biāo)約束；SLAM因子則通過匹配環(huán)境特征建立起不同時(shí)刻節(jié)點(diǎn)間的相對(duì)位置聯(lián)系。

FGO的工作過程就像是解一個(gè)多約束的最優(yōu)方案，它會(huì)在一個(gè)滑動(dòng)窗口內(nèi)，同時(shí)對(duì)過去幾秒鐘甚至更長(zhǎng)時(shí)間內(nèi)的所有位姿進(jìn)行迭代優(yōu)化，尋找一個(gè)能讓所有傳感器觀測(cè)“矛盾最小”的最優(yōu)路徑。

這種“滑動(dòng)窗口”加“多次迭代”的機(jī)制賦予了系統(tǒng)極強(qiáng)的魯棒性。當(dāng)系統(tǒng)在窗口內(nèi)發(fā)現(xiàn)某一幀GNSS觀測(cè)與IMU及雷達(dá)的推算結(jié)果嚴(yán)重不符時(shí)，它并不會(huì)盲目跟隨這個(gè)錯(cuò)誤觀測(cè)，而是可以通過魯棒核函數(shù)（Robust Kernels）有效地降低這個(gè)“離群點(diǎn)”的權(quán)重，甚至將其徹底剔除，從而確保整體軌跡的平穩(wěn)。

圖片源自：網(wǎng)絡(luò)

為了實(shí)現(xiàn)高效的因子圖優(yōu)化，有技術(shù)引入了“預(yù)積分”（Pre-integration）技術(shù)。由于IMU的數(shù)據(jù)頻率極高（通常在100Hz至400Hz之間），如果將每一幀IMU數(shù)據(jù)都作為因子加入圖中，計(jì)算量將呈指數(shù)級(jí)增長(zhǎng)。

預(yù)積分技術(shù)巧妙地將兩個(gè)關(guān)鍵幀之間的大量IMU原始觀測(cè)合并成一個(gè)相對(duì)位移和旋轉(zhuǎn)增量，并計(jì)算出相應(yīng)的誤差傳遞矩陣。

這不僅保證了數(shù)學(xué)上的嚴(yán)謹(jǐn)性，還讓高頻率的慣性數(shù)據(jù)能夠與低頻率的GNSS（1Hz至10Hz）和SLAM（10Hz至20Hz）觀測(cè)完美契合，使得系統(tǒng)能夠?qū)崟r(shí)輸出厘米級(jí)的位姿信息。

場(chǎng)景適應(yīng)性與城市環(huán)境下的穩(wěn)定性評(píng)估

高精定位系統(tǒng)的穩(wěn)定性不僅取決于算法本身，更取決于對(duì)特定城市工況的深度適配。在長(zhǎng)達(dá)數(shù)公里的海底隧道或地下環(huán)路中，GNSS信號(hào)會(huì)完全“靜默”，此時(shí)定位系統(tǒng)將進(jìn)入純里程計(jì)模式。

為了防止在此期間產(chǎn)生不可控的漂移，有些技術(shù)方案中引入了高精度地圖（HD Map）匹配技術(shù)。高精度地圖不僅僅是導(dǎo)航坐標(biāo)的集合，更包含了車道線的曲率、紅綠燈的精確位置、甚至是路緣石的三維輪廓等城市環(huán)境的精細(xì)語(yǔ)義信息。

車輛通過激光雷達(dá)實(shí)時(shí)掃描到的周圍輪廓與地圖中的先驗(yàn)信息進(jìn)行“二次對(duì)齊”，這就像是在黑暗中摸索時(shí)找到了固定的扶手，能夠瞬間消除掉IMU累積的誤差，實(shí)現(xiàn)長(zhǎng)時(shí)間、長(zhǎng)距離的零漂移定位。

在應(yīng)對(duì)動(dòng)態(tài)交通流方面，城市環(huán)境同樣充滿了不確定性。當(dāng)車輛周圍被多輛大型公交車或卡車環(huán)繞時(shí)，SLAM算法提取到的很多特征點(diǎn)實(shí)際上是移動(dòng)的。如果系統(tǒng)誤將這些動(dòng)態(tài)點(diǎn)當(dāng)作靜止參考，定位就會(huì)隨之偏離。

圖片源自：網(wǎng)絡(luò)

因此，先進(jìn)的融合系統(tǒng)會(huì)結(jié)合深度學(xué)習(xí)模型進(jìn)行語(yǔ)義過濾，在算法層面自動(dòng)屏蔽掉圖像或點(diǎn)云中屬于行人、車輛等動(dòng)態(tài)物體的部分，只鎖定路牌、建筑和電線桿等永恒的地理特征進(jìn)行解算。

評(píng)估系統(tǒng)在城市環(huán)境中的魯棒性，需要通過一系列嚴(yán)苛的指標(biāo)，其中包括均方根誤差、系統(tǒng)可用性以及完整性。

均方根誤差反映了定位結(jié)果在長(zhǎng)時(shí)間序列下的平均精準(zhǔn)度；可用性則衡量在99.9%甚至更高的運(yùn)行時(shí)間內(nèi)，定位精度能否始終維持在安全閾值（如車道線居中要求的20厘米）以內(nèi)。

在實(shí)際的城市路測(cè)中，一個(gè)設(shè)計(jì)良好的緊耦合融合系統(tǒng)在面臨隧道出入口的劇烈光照變化、高架橋下的衛(wèi)星遮擋以及繁忙路口的多路徑干擾時(shí)，仍能保持極為平滑的軌跡輸出。這種穩(wěn)定性不僅降低了后續(xù)感知與規(guī)劃模塊的負(fù)擔(dān)，更為自動(dòng)駕駛的決策提供了最高級(jí)別的心理“安全墊”。

最后的話

GNSS/IMU組合定位在城市環(huán)境中的魯棒性問題，實(shí)質(zhì)上是不同傳感物理特性與復(fù)雜環(huán)境特征之間的博弈。通過引入激光雷達(dá)與視覺SLAM作為運(yùn)動(dòng)補(bǔ)償，并利用緊耦合的因子圖優(yōu)化框架進(jìn)行多維約束，已經(jīng)能夠讓自動(dòng)駕駛車輛在大多數(shù)城市工況下表現(xiàn)得像資深老司機(jī)一樣淡定自如。

技術(shù)的演進(jìn)從未停步，未來的城市定位系統(tǒng)或?qū)⑸疃葥肀?a href="http://m.makelele.cn/tags/ai/" target="_blank">AI，通過神經(jīng)網(wǎng)絡(luò)自適應(yīng)地調(diào)整不同傳感器的權(quán)重，甚至在惡劣天氣下模擬人類的視覺記憶進(jìn)行定位。

審核編輯 黃宇