多目魚眼相機(jī)系統(tǒng)的視覺里程計(jì)解決方案

劉一龍，孟維亮，張曉鵬

(1.中國(guó)科學(xué)院自動(dòng)化研究所 模式識(shí)別國(guó)家重點(diǎn)實(shí)驗(yàn)室，北京 100190;2.中國(guó)科學(xué)院軟件研究所 計(jì)算機(jī)科學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，北京 100190;3.之江實(shí)驗(yàn)室，杭州 311121;4.中國(guó)科學(xué)院大學(xué)，北京 100049)

0 引言

在自動(dòng)駕駛領(lǐng)域，里程計(jì)算法是指通過接收每幀的各車載傳感器輸入，不斷估計(jì)車輛本身位置和姿態(tài)的增量，從而確定車身當(dāng)前所處的位置和姿態(tài)的算法。里程計(jì)算法的本質(zhì)是一個(gè)同時(shí)建圖定位算法(Simultaneous localization and mapping, SLAM)，它工作在沒有高精度地圖先驗(yàn)的場(chǎng)景下，從原始輸入中構(gòu)建場(chǎng)景結(jié)構(gòu)特征并解算車身位姿，其結(jié)果可以作為進(jìn)一步高精度地圖建圖或定位的輸入信息。因此，它是整個(gè)自動(dòng)駕駛系統(tǒng)的最為基礎(chǔ)的模塊之一。

自動(dòng)駕駛產(chǎn)品中，常用的里程計(jì)解決方案依據(jù)其使用的外觀性傳感器類型的不同，通?？煞譃槭褂眉す饫走_(dá)的方案、使用相機(jī)圖像的方案以及僅使用內(nèi)觀性傳感器(輪速或 IMU 等)的方案等。在本文中我們主要討論使用相機(jī)圖像的方案。特別地，在自動(dòng)泊車等慢速行車且周圍環(huán)境較復(fù)雜、行車線路較靈活的場(chǎng)景中，多目魚眼相機(jī)由于其視野較大，可觀測(cè)到的信息較多，因此具備一定的優(yōu)勢(shì)。在本文中，我們主要設(shè)計(jì)了多目魚眼相機(jī)系統(tǒng)的里程計(jì)方案。

1 相關(guān)工作

1.1 基于激光雷達(dá)的里程計(jì)算法

由于激光雷達(dá)可以對(duì)周圍環(huán)境掃描獲得高精度的三維點(diǎn)云信息，使得自動(dòng)駕駛中的定位和感知具有很高的可靠性。因此，在完全無(wú)人駕駛的解決方案中受到青睞。使用激光雷達(dá)信息構(gòu)建里程計(jì)算法，最簡(jiǎn)單的方案是對(duì)每幀的點(diǎn)云使用ICP[1-2]對(duì)齊算法，但這種方案通常容易受到點(diǎn)云密度不均勻性、噪聲和復(fù)雜物體表面點(diǎn)云的影響。后續(xù)的一些工作[3-4]使用了平面或線特征等幾何特征，得到了更加魯棒的結(jié)果。LOAM[5]提出了一個(gè)容易使用的從掃描幀提取線、面特征的方法，并實(shí)現(xiàn)了一個(gè)高效、準(zhǔn)確的實(shí)時(shí)里程計(jì)方案。在此基礎(chǔ)上，后人提出 LeGO-LOAM[6]，LIO-SAM[7]等不同的改進(jìn)方案。LIC-Fusion[8-9]，LINS[10]，F(xiàn)AST-LIO[11]等工作聯(lián)合使用激光雷達(dá)特征與慣性傳感器數(shù)據(jù)，構(gòu)建基于濾波框架的里程計(jì)。PSF-LO[12]則使用參數(shù)化的語(yǔ)義特征構(gòu)建里程計(jì)，取得了很高的精度?？傮w而言，激光雷達(dá)里程計(jì)可以達(dá)到很高的準(zhǔn)確度，但由于激光雷達(dá)的成本比較昂貴，短時(shí)間內(nèi)難以大規(guī)模投入量產(chǎn)。而基于視覺的里程計(jì)算法，則因其低廉成本和可靠的成像質(zhì)量，成為量產(chǎn)方案的首選。

1.2 基于視覺的里程計(jì)算法

基于視覺的里程計(jì)算法研究較多，經(jīng)過工作前后大量的探索，逐漸形成了一些被廣泛采用的方法和設(shè)計(jì)。其中，PTAM[13]提出了使用 Tracking 和 Mapping 兩個(gè)線程對(duì)關(guān)鍵幀進(jìn)行優(yōu)化的方案。ORB-SLAM[14]使用 ORB 作為特征點(diǎn)的描述，構(gòu)建了一個(gè)經(jīng)典的單目 SLAM 解決方案，并以此為基礎(chǔ)在后續(xù)工作[15-16]中增加雙目和 IMU 等支持。SVO[17]、DSO[18]等工作不提取特征點(diǎn)的描述子，而是使用圖像光度信息進(jìn)行跟蹤，形成了一系列直接法里程計(jì)的解決方案。VINS-MONO[19]使用 Lucas-Kanade 光流[20]跟蹤替代描述子匹配，并使用IMU 信息實(shí)現(xiàn)了一個(gè)高效的視覺-慣性里程計(jì)，并在后續(xù)工作 VINS-Fusion[21]中增加了雙目的支持。近年來，還有一些工作[22-23]探索使用深度學(xué)習(xí)的方法構(gòu)造視覺里程計(jì)，也取得了不錯(cuò)的成果。這些工作為里程計(jì)算法提供了很好的經(jīng)驗(yàn)，但它們都沒有針對(duì)多目魚眼相機(jī)系統(tǒng)進(jìn)行特別的設(shè)計(jì)，而魚眼相機(jī)圖像所具有的較大畸變扭曲等特性，通常會(huì)使得這些方法無(wú)法適配或效果下降。同時(shí)，魚眼相機(jī)間的數(shù)據(jù)關(guān)聯(lián)也比較困難，從而限制了整個(gè)系統(tǒng)的精度。

1.3 數(shù)據(jù)關(guān)聯(lián)算法

在包括里程計(jì)算法在內(nèi)的 SLAM 算法中，通常需要在幀間或相機(jī)間的進(jìn)行相同特征的關(guān)聯(lián)，關(guān)聯(lián)的特征將形成優(yōu)化問題的約束，對(duì)問題的解決非常重要。對(duì)于使用直接法的算法來講，通過在不同幀之間使用鄰域的圖像光度信息的差異進(jìn)行匹配。這種方法由于光度匹配在單幀上容易出現(xiàn)錯(cuò)配，通常需要多幀連續(xù)匹配，使用深度濾波器等方法不斷更新結(jié)果。因此，在相機(jī)間的單幀匹配上則不易發(fā)揮作用。對(duì)于使用特征點(diǎn)法的算法來講，通常使用不同的特征點(diǎn)描述子進(jìn)行特征點(diǎn)的匹配。FAST[24]特征點(diǎn)是實(shí)時(shí)系統(tǒng)中最常用的角點(diǎn)檢測(cè)算法之一。而 BRIEF[25]描述子則提供了可以快速計(jì)算的二進(jìn)制描述子。它們不區(qū)分角點(diǎn)特征的方向，也不能很好地處理圖像信息尺寸的不同，但極低的計(jì)算力要求和良好的實(shí)時(shí)性仍然是很多場(chǎng)合下不錯(cuò)的選擇。SIFT[26]和 SURF[27]提供了具有旋轉(zhuǎn)不變性和一定的尺度不變性的浮點(diǎn)描述子，可以在特征匹配中達(dá)到非常好的效果，但它們的提取和生成需要耗費(fèi)較多的算力和時(shí)間。ORB[28]通過在 FAST角點(diǎn)和 BRIEF 描述子的基礎(chǔ)上增加方向信息和圖像金字塔，以更高的計(jì)算效率達(dá)到了與 SIFT 相當(dāng)?shù)男Ч?。然而，盡管這些特征對(duì)于特征點(diǎn)旋轉(zhuǎn)方向的補(bǔ)償，僅能適用于圖像平面上的旋轉(zhuǎn)，對(duì)于魚眼相機(jī)系統(tǒng)中相機(jī)間距離較遠(yuǎn)形成的視角變化，以及畸變帶來的扭曲變化，都不能達(dá)到滿意的效果。近年來隨著神經(jīng)網(wǎng)絡(luò)算法的發(fā)展，研究者們提出了許多基于卷積神經(jīng)網(wǎng)絡(luò)等模型提取特征點(diǎn)和描述子的方法[29-33]，以及基于深度學(xué)習(xí)模型的匹配方法[34-35]。相比于傳統(tǒng)的特征及匹配方法，這些方法在魯棒性、準(zhǔn)確度、匹配數(shù)量等方面均有明顯的優(yōu)勢(shì)。但這些方法通常需要消耗較多的算力，甚至需要GPU等硬件支持才能高效運(yùn)行。

2 里程計(jì)方案設(shè)計(jì)

2.1 算法架構(gòu)設(shè)計(jì)

我們的里程計(jì)算法是一套基于優(yōu)化框架的 SLAM 算法，在架構(gòu)上采用業(yè)界廣泛應(yīng)用的前端-后端架構(gòu)方案，整體架構(gòu)如圖1 所示。算法輸入多路相機(jī)圖像信息，以及可選的輪速脈沖或 IMU 等信號(hào)，持續(xù)估計(jì)每時(shí)刻車體位姿的相對(duì)變化。從而輸出每幀車體位姿。其中，算法前端負(fù)責(zé)關(guān)鍵幀管理、特征點(diǎn)的提取與跟蹤、地圖點(diǎn)的生成與維護(hù)等功能，而后端維護(hù)一個(gè)關(guān)鍵幀的滑動(dòng)窗口，并在窗口內(nèi)進(jìn)行地圖點(diǎn)與車體位姿的聯(lián)合優(yōu)化，從而提高算法精度。此外，我們提出了兩種數(shù)據(jù)關(guān)聯(lián)的模塊，可以用于初始化時(shí)的尺度矯正和為后端聯(lián)合優(yōu)化提供額外約束，以提高整個(gè)系統(tǒng)的輸出位姿的精度。

圖1 本文里程計(jì)方案系統(tǒng)架構(gòu)總覽

2.2 算法初始化

當(dāng)某個(gè)相機(jī)滿足以下所有條件時(shí)，將被認(rèn)為初始化就緒狀態(tài)：

(1)當(dāng)前跟蹤中的特征點(diǎn)數(shù)超過閾值;

(2)特征點(diǎn)連續(xù)跟蹤幀數(shù)超過閾值;

值得注意的是，其中條件 3 是為了保證相機(jī)位置在兩幀間形成的基線足夠長(zhǎng)，從而可以得到比較準(zhǔn)確的三角化結(jié)果。但該條件無(wú)法排除相機(jī)運(yùn)動(dòng)接近純旋轉(zhuǎn)的情況，而這種情況下并不能達(dá)到長(zhǎng)基線的預(yù)期。由于在本文所應(yīng)用的車載系統(tǒng)的使用場(chǎng)景下，幾乎不會(huì)發(fā)生各個(gè)相機(jī)的純旋轉(zhuǎn)，因此使用此條件是合理的。

當(dāng)系統(tǒng)中有超過半數(shù)的相機(jī)進(jìn)入初始化就緒狀態(tài)后，可以收集各相機(jī)已三角化的地圖點(diǎn)信息，通過構(gòu)建和求解如公式(2)所示的優(yōu)化問題，完成里程計(jì)系統(tǒng)的初始化。

2.3 前端管理

前端的管理主要涉及地圖點(diǎn)的管理、關(guān)鍵幀的管理以及對(duì)每幀輸入進(jìn)行的特征點(diǎn)跟蹤與簡(jiǎn)單的位姿優(yōu)化。

在地圖點(diǎn)的管理上，對(duì)于已三角化的地圖點(diǎn)，可根據(jù)其在兩幀之前的輪速或IMU 推算得到的本幀位姿下的投影位置作為光流初值的預(yù)測(cè)，利用2.2節(jié)所述的雙向光流匹配策略，得到其在本幀的觀測(cè)位置，加入到約束中。同時(shí)，應(yīng)加入特征點(diǎn)和地圖點(diǎn)的補(bǔ)充：將整個(gè)圖像區(qū)域劃分為m×n的格子，當(dāng)此幀上某一格子內(nèi)跟蹤的特征點(diǎn)數(shù)少于閾值時(shí)，則在該格子中補(bǔ)充 FAST 特征點(diǎn)。對(duì)于存在兩個(gè)以上觀測(cè)的地圖點(diǎn)，可以通過三角化得到地圖點(diǎn)的三維位置。值得注意的是，隨著車輛的行駛，會(huì)產(chǎn)生原來在圖像上分布較分散的特征點(diǎn)逐漸聚集到一起，此時(shí)這些點(diǎn)消耗的算力不能為優(yōu)化系統(tǒng)提供更多的信息，反而以較高的權(quán)重影響了優(yōu)化效果。比如正常向前行駛時(shí)的后視相機(jī)或者倒車行駛時(shí)的前視相機(jī)，特征點(diǎn)將逐漸向圖像中心聚集。為減少這種影響，我們?cè)谠O(shè)定區(qū)域劃分格子，每個(gè)格子中僅保留可觀測(cè)幀數(shù)最多的一個(gè)地圖點(diǎn)。

對(duì)于每個(gè)相機(jī)，如果其滿足如下的兩個(gè)條件之一，則被認(rèn)為需要設(shè)定關(guān)鍵幀。

(1)距離上個(gè)關(guān)鍵幀已有2幀或以上，同時(shí)滿足以下條件之一：

? 該幀的平均光流距離超過閾值;

? 現(xiàn)存的地圖點(diǎn)數(shù)量少于閾值;

? 該幀上發(fā)生了聚集特征點(diǎn)刪除。

(2)距離上個(gè)關(guān)鍵幀已有4幀或以上，同時(shí)本幀觀測(cè)到的特征點(diǎn)數(shù)小于閾值。

當(dāng)存在任一相機(jī)需要設(shè)定關(guān)鍵幀時(shí)，該幀將被標(biāo)記為關(guān)鍵幀，進(jìn)入后端的優(yōu)化。此外，還可設(shè)定一個(gè)強(qiáng)制插入關(guān)鍵幀的距離參數(shù)，當(dāng)估計(jì)的行駛距離超過該設(shè)定參數(shù)時(shí)，即使未滿足前述條件，也會(huì)將當(dāng)前幀設(shè)定為關(guān)鍵幀。

當(dāng)接收到每幀的輸入后，保持各相機(jī)中當(dāng)前各地圖點(diǎn)位置，通過求解優(yōu)化問題估計(jì)當(dāng)前幀位姿：

(1)

式中，π(·)為投影過程，Cc和Tc分別為相機(jī)c的內(nèi)參和外參；xp為地圖點(diǎn)的三維位置，pp,i,c為地圖點(diǎn)xp在第i幀、相機(jī)c圖像上的二維觀測(cè)位置；ρ(·)為魯棒核函數(shù)，在本文中使用 Huber 函數(shù)。在前端的優(yōu)化中，待優(yōu)化的變量?jī)H為當(dāng)前幀(第i幀)的位姿Pi。構(gòu)建約束的是當(dāng)前幀所有相機(jī)的所有觀測(cè)，其中所有地圖點(diǎn)的位置xp均為固定值而不被優(yōu)化。這使得前端優(yōu)化可以以很小的計(jì)算量快速得到每幀的位姿估計(jì)。

2.4 后端優(yōu)化

為了得到更準(zhǔn)確的三維地圖點(diǎn)位置和每幀位姿，在后端維護(hù)一個(gè)固定長(zhǎng)度為W幀的關(guān)鍵幀滑窗，并對(duì)于滑窗內(nèi)各關(guān)鍵幀位姿及觀測(cè)到的地圖點(diǎn)位置進(jìn)行聯(lián)合優(yōu)化。具體而言，優(yōu)化問題表示為：

(2)

式中，i的取值范圍為滑窗內(nèi)各幀ID；π(·)為投影過程，Cc和Tc分別為相機(jī)c的內(nèi)參和外參；xp為地圖點(diǎn)的三維位置，pp,i,c為地圖點(diǎn)xp在第i幀、相機(jī)c圖像上的二維觀測(cè)位置；ρ(·)為魯棒核函數(shù)，在本文中使用 Huber 函數(shù)。與公式(1)相比，公式(2)所表示的后端優(yōu)化有兩處不同：一是滑窗內(nèi)的所有關(guān)鍵幀都參與了優(yōu)化，這些幀的位姿Pi都是待優(yōu)化變量；二是觀測(cè)的所有地圖點(diǎn)三維位置xp也被同時(shí)優(yōu)化。后端的這種優(yōu)化通常被稱為光束平差優(yōu)化(Bundle Adjustment)，可以進(jìn)一步得到更加準(zhǔn)確的位姿和地圖點(diǎn)位置。

3 魚眼相機(jī)間數(shù)據(jù)關(guān)聯(lián)

相機(jī)間數(shù)據(jù)關(guān)聯(lián)的目標(biāo)是在兩個(gè)相機(jī)圖像上內(nèi)容重疊區(qū)域確定若干對(duì)以像素坐標(biāo)表示的匹配點(diǎn)，確保每對(duì)匹配點(diǎn)對(duì)應(yīng)三維世界中相同的三維點(diǎn)。正確的相機(jī)間數(shù)據(jù)關(guān)聯(lián)是視覺里程計(jì)算法能夠充分利用多目視覺信息、獲得與物理尺度相一致的優(yōu)化結(jié)果的必要前提。在相機(jī)間數(shù)據(jù)關(guān)聯(lián)缺失時(shí)，來自不同相機(jī)的觀測(cè)僅在每個(gè)相機(jī)本身構(gòu)建優(yōu)化問題，不同相機(jī)的約束僅通過相對(duì)車體的相機(jī)外參聯(lián)系起來，雖然絕對(duì)尺度也是可觀的，但是約束并不強(qiáng)。實(shí)驗(yàn)表明這種約束下尺度仍會(huì)具有 1%～2% 水平的誤差，從而影響系統(tǒng)結(jié)果的精度。不失一般性，我們可以將多目相機(jī)系統(tǒng)的相機(jī)間數(shù)據(jù)關(guān)聯(lián)問題簡(jiǎn)化為雙目相機(jī)圖像上的匹配問題。

現(xiàn)有的雙目匹配技術(shù)主要分為兩類:基于描述子匹配的方法和基于圖像灰度信息的直接法。這些方法通常適用于相機(jī)中心距離較近(短基線)場(chǎng)景，在長(zhǎng)基線的魚眼相機(jī)的配置下，圖像內(nèi)容重疊區(qū)域通常在圖像邊緣畸變明顯處占據(jù)很小面積，同時(shí)圖像特征差異較大，且多存在遮擋等極端條件，現(xiàn)有方法均不能保證匹配點(diǎn)的數(shù)目和質(zhì)量。本節(jié)將探討適用于長(zhǎng)基線魚眼相機(jī)配置下的雙目數(shù)據(jù)關(guān)聯(lián)問題。

3.1 基于光度的數(shù)據(jù)關(guān)聯(lián)

基于光度的方法主要是在給定三維點(diǎn)深度下計(jì)算其投影在兩個(gè)相機(jī)圖像位置上局部區(qū)域像素光度值的相關(guān)性，從而在對(duì)極線上進(jìn)行搜索，以確定正確的深度值。但由于長(zhǎng)基線魚眼相機(jī)配置情況下，可能出現(xiàn)兩相機(jī)曝光情況不同和存在嚴(yán)重畸變或遮擋等，對(duì)于單一點(diǎn)的光度匹配搜索往往會(huì)被噪音或局部極值影響而得到錯(cuò)誤匹配?？梢杂^察到，在缺少相機(jī)間數(shù)據(jù)關(guān)聯(lián)的多目系統(tǒng)里，雖然其尺度可能存在誤差，但在里程計(jì)初始化結(jié)束后，已經(jīng)得到了針對(duì)每個(gè)單獨(dú)相機(jī)(如相機(jī)1)的二維特征點(diǎn)和三維地圖點(diǎn)，利用這些信息可以為匹配提供可靠的線索。這些地圖點(diǎn)具有如下特性：

? 如果向相機(jī)1投影，則每個(gè)三維點(diǎn)可以投影到其相應(yīng)的二維特征點(diǎn);

? 各地圖點(diǎn)深度不準(zhǔn)確，但相對(duì)的深度關(guān)系較準(zhǔn)確。即通過一個(gè)統(tǒng)一的尺度修正，可以同時(shí)變成準(zhǔn)確的三維位置;

? 各點(diǎn)沿相機(jī)1的投影方向進(jìn)行尺度改變，不影響其仍然投影到原始二維特征點(diǎn)。

基于這些特性，只需要估計(jì)一個(gè)最佳尺度值，使得在其作用下，這些三維點(diǎn)能夠達(dá)到其真實(shí)位置，此時(shí)投影到相機(jī)2也可以到達(dá)正確的二維位置。整體步驟如圖2所示。

圖2 通過光度數(shù)據(jù)關(guān)聯(lián)修正系統(tǒng)尺度流程

具體來講，可以通過以下步驟得到整個(gè)系統(tǒng)的尺度修正值。

(1)相機(jī)1的圖像通過當(dāng)前里程計(jì)初始化得到二維特征點(diǎn)和三維地圖點(diǎn)；

(2)初始化尺度為最大可能值，然后在其達(dá)到最小值之前循環(huán)以下步驟：

a.將當(dāng)前尺度應(yīng)用于所有原始三維地圖點(diǎn)，得到更新后的地圖點(diǎn)三維位置；

(3)

i.假設(shè)圖像上局部小區(qū)域內(nèi)深度相同，將每個(gè)地圖點(diǎn)對(duì)應(yīng)的二維特征點(diǎn)及其周圍n×n方塊像素點(diǎn)均按更新后的深度值投影到相機(jī)2，得到在相機(jī)2圖像上的n×n個(gè)像素位置;

ii.根據(jù)公式(3)計(jì)算 NCC 值;

iii.將所有三維點(diǎn)的 NCC 值進(jìn)行平均并記錄。

c.沿對(duì)極線方向尺度步進(jìn)更新：

i.尋找到所有三維點(diǎn)中深度最小(離相機(jī)1最近)的點(diǎn);

ii.將其尺度減小一個(gè)極小量，將新的點(diǎn)投影到相機(jī)2得到其在相機(jī)2上投影的改變方向，即對(duì)極線的切線方向;

iii.沿該方向步進(jìn)一個(gè)像素的距離，此時(shí)深度與原始深度的比值作為新的尺度。

(3)循環(huán)結(jié)束后，將具有最大 NCC 值的匹配和相應(yīng)的尺度值輸出。

由于該方法綜合考慮了每幀上所有的地圖點(diǎn)，因此，對(duì)于曝光差異、畸變和遮擋的魯棒性較好。圖3 中展示了一些極端情況下仍可得到正確的匹配和尺度值的場(chǎng)景。在圖3(a)中，來自兩個(gè)魚眼相機(jī)的圖像，其內(nèi)容視角相差較大，但得到了正確匹配；圖3(b)中展示了由于遮擋造成圖像內(nèi)容被掩蓋，曝光情況也發(fā)生了差異，右圖中可見的遠(yuǎn)處的面包車在左圖中完全被遮擋，但匹配點(diǎn)仍然出現(xiàn)在了相應(yīng)位置，得到的尺度值也是正確的。

圖3 極端場(chǎng)景下光度數(shù)據(jù)關(guān)聯(lián)可以得到正確尺度值與匹配結(jié)果

得到正確的尺度值后，可以將新的尺度應(yīng)用到當(dāng)前系統(tǒng)，從而修復(fù)系統(tǒng)的尺度偏差。該方法在里程計(jì)系統(tǒng)中輪速計(jì)、IMU 等具備尺度的觀測(cè)傳感器不可用時(shí)，用于在初始化后進(jìn)行尺度的修正，可以有效地減少尺度偏差，從而提高系統(tǒng)整體精度。另一方面，在尺度誤差較小的前提下，直接將 NCC 誤差構(gòu)建入后端的非線性優(yōu)化問題中，作為優(yōu)化約束之一參與每幀優(yōu)化也是可行的。但由于NCC 求導(dǎo)數(shù)比較復(fù)雜，每一迭代中的搜索匹配也需要消耗大量計(jì)算，因此，通常并不將其加入每幀優(yōu)化中，而是在此時(shí)使用基于描述子的數(shù)據(jù)關(guān)聯(lián)。

3.2 基于描述子的數(shù)據(jù)關(guān)聯(lián)

使用 ORB、SIFT 等具有一定的旋轉(zhuǎn)、尺度不變性的描述子，在魚眼相機(jī)長(zhǎng)基線、大畸變的情況下，匹配結(jié)果相對(duì)較好，但其描述子的提取計(jì)算量較大，對(duì)車載實(shí)時(shí)系統(tǒng)算力造成不小壓力。

業(yè)界通常使用PolyN模型表述魚眼相機(jī)，可以認(rèn)為是在針孔相機(jī)成像的基礎(chǔ)上增加了一個(gè)徑向角度的映射變化。如圖4(a)所示，在相機(jī)坐標(biāo)系中的一個(gè)點(diǎn)P=(xc,yc,zc)T，其投影到圖像的入射光線的角度為:

圖4 本文使用的兩種相機(jī)模型

(4)

在 PolyN 模型中，認(rèn)為入射角度為θ的光線將被向內(nèi)偏折，使得最終投影點(diǎn)發(fā)生在距主點(diǎn)為r的位置p：

(5)

(6)

式中，階數(shù)N可以選擇不同的數(shù)量，從而在復(fù)雜程度與精度之間做出權(quán)衡，相應(yīng)的模型即可被具體地稱為 Poly1/Poly3/Poly5 等。在我們的實(shí)際使用中，通常使用 Poly5 的模型。

為了提升相機(jī)間描述子的一致性，需要構(gòu)建另一種假想的相機(jī)模型——柱面相機(jī)模型，如圖4(b)所示。在相機(jī)坐標(biāo)系中的一個(gè)點(diǎn)P=(xc,yc,zc)T，其投影點(diǎn)的圖像坐標(biāo)如下確定：

(7)

將魚眼圖像轉(zhuǎn)換為柱面圖像，保證了圖像內(nèi)容的方向具備了一定的一致性，同時(shí)yaw角上均勻分布的角分辨率也大大提升了簡(jiǎn)單描述子的一致性。因此，我們對(duì)于多目魚眼相機(jī)，使用柱面圖像和基本的 FAST 角點(diǎn)加BRIEF描述子，可以得到不錯(cuò)的匹配效果。

通過這種方法得到每幀相機(jī)間的數(shù)據(jù)關(guān)聯(lián)匹配后，其在匹配幀上的重投影誤差也可以與原單相機(jī)約束相同的方式加入到公式(2)所示的原優(yōu)化問題中，從而提高系統(tǒng)的精度。

圖5 中對(duì)比了開啟數(shù)據(jù)關(guān)聯(lián)尺度矯正及優(yōu)化配置下不同的軌跡估計(jì)結(jié)果。測(cè)試數(shù)據(jù)在地下停車場(chǎng)中采集，從給定車位出發(fā)，經(jīng)過較長(zhǎng)距離后回到原車位并泊入。這使得從軌跡形狀上很容易分辨起始和終點(diǎn)的位移偏差。由圖5可見，未開啟數(shù)據(jù)關(guān)聯(lián)時(shí)，軌跡存在一定的尺度偏差與漂移，整體誤差累積也較高；而加入了數(shù)據(jù)關(guān)聯(lián)尺度矯正及優(yōu)化后，誤差的大小及累積速度都得到了較好的控制。

圖5 開啟數(shù)據(jù)關(guān)聯(lián)尺度優(yōu)化前后的軌跡結(jié)果

4 結(jié)果與討論

本節(jié)將對(duì)本文所述里程計(jì)方案的結(jié)果進(jìn)行展示，并進(jìn)行定量的評(píng)估。

4.1 公開數(shù)據(jù)集量化評(píng)測(cè)

使用公開數(shù)據(jù)集 EuRoC[36]對(duì)本文里程計(jì)算法的結(jié)果進(jìn)行評(píng)測(cè)，以驗(yàn)證我們方案的正確性與可用性。EuRoC 數(shù)據(jù)主要是短距離的無(wú)人機(jī)拍攝數(shù)據(jù)，在各個(gè)方向上均有不同模式的運(yùn)動(dòng)。采用雙目視覺、無(wú) IMU 的配置，并將結(jié)果與開源算法 VINS-Fusion[21]進(jìn)行對(duì)比。兩者的 APE誤差統(tǒng)計(jì)如表 1 所示。

同時(shí)，在圖6中展示了本文方法得到的軌跡在各個(gè)測(cè)試數(shù)據(jù)上與真值軌跡相比的APE誤差分布情況。從表1和圖6軌跡結(jié)果中可見，在 EuRoC 數(shù)據(jù)集上，本文的方法比 VINS-Fusion 得到的結(jié)果誤差更小。由于該評(píng)測(cè)數(shù)據(jù)集并非本文算法研發(fā)時(shí)主要考慮的場(chǎng)景，這證明了本文算法具有良好的普適性。

圖6 本文方法在EuRoC 數(shù)據(jù)集上的軌跡結(jié)果與誤差分布

表1 與VINS-Fusion 在 EuRoC 數(shù)據(jù)集APE對(duì)比

4.2 自采實(shí)車數(shù)據(jù)量化評(píng)測(cè)

為了證明我們的算法在主要應(yīng)用場(chǎng)景，即車輛行駛場(chǎng)景下的精度，我們使用自有的數(shù)據(jù)采集車，在GPS/RTK信號(hào)良好的城區(qū)和高速道路采集了若干段長(zhǎng)距離的實(shí)車數(shù)據(jù)。其中真值軌跡使用RTK軌跡，并進(jìn)行了正確的外參補(bǔ)償。由于里程計(jì)不包含全局約束，誤差會(huì)隨距離累積。對(duì)于大范圍長(zhǎng)距離數(shù)據(jù)而言，在未修正全局漂移時(shí)，APE數(shù)值的參考價(jià)值不大，故省略，僅評(píng)測(cè)RPE。同時(shí)，我們還基于 MSCKF[37]實(shí)現(xiàn)了一個(gè)濾波版本的里程計(jì)作為對(duì)比。3個(gè)數(shù)據(jù)集下誤差情況統(tǒng)計(jì)如表2所示。

表2 自采實(shí)車數(shù)據(jù)集下 RPE 精度結(jié)果

從結(jié)果可以看出，本文里程計(jì)達(dá)到了 200 m 平均相對(duì)誤差 < 5‰ 的較高水平，且與常用的濾波框架下的里程計(jì)精度相當(dāng)，在位移精度上占優(yōu)，具備較好的可用性。

為了進(jìn)一步證明本文結(jié)果在自動(dòng)駕駛場(chǎng)景下可以滿足下游模塊的精度要求，我們將得到的軌跡位姿接入GPS信息，通過簡(jiǎn)單的位姿圖優(yōu)化將其對(duì)齊到真實(shí)地理坐標(biāo)上，在城市地圖上將相關(guān)測(cè)試軌跡與RTK真值軌跡的200 m相對(duì)誤差可視化，顯示在圖上。如圖7所示，絕大多數(shù)區(qū)域的相對(duì)誤差均在 6‰ 以下，可以很好地滿足下游進(jìn)一步進(jìn)行高精度建圖或定位的精度要求。

圖7 本文方法在實(shí)車數(shù)據(jù)上的結(jié)果(GPS對(duì)齊后)200m相對(duì)誤差分布

5 總結(jié)

本文提出了一個(gè)適用于自動(dòng)駕駛場(chǎng)景的多目相機(jī)系統(tǒng)，它具備高效的前后端架構(gòu)，同時(shí)適用于包括魚眼相機(jī)在內(nèi)的多目相機(jī)間的數(shù)據(jù)關(guān)聯(lián)模塊，可以達(dá)到較高的輸出位姿準(zhǔn)確度，具有很高的實(shí)用性。