基于因子圖的自動(dòng)駕駛?cè)诤隙ㄎ谎芯?/h1>
2019-04-07 11:23:24袁子葉尹慧琳伍淑莉
汽車工程學(xué)報(bào) 2019年4期
袁子葉，尹慧琳，伍淑莉
（同濟(jì)大學(xué) 中德學(xué)院 電子信息系，上海 201804）
自動(dòng)駕駛技術(shù)在降低交通事故發(fā)生率、緩解交通擁堵、實(shí)時(shí)提供有效最短路徑等方面起到了明顯的作用[1]，其中的關(guān)鍵技術(shù)“即時(shí)定位與地圖構(gòu)建”（Simultaneous Localization and Mapping，SLAM）也隨之備受關(guān)注。在SLAM 中，能夠根據(jù)多個(gè)傳感器信息，對(duì)車輛進(jìn)行精確定位尤為重要，所以提高即時(shí)定位的精確度一直是研究的熱點(diǎn)[2]。
對(duì)于車輛定位有許多不同的方法，其中利用全球定位系統(tǒng)（Global Positioning System，GPS）與慣性測(cè)量元件（Inertial Measurement Unit ，IMU）進(jìn)行數(shù)據(jù)融合從而對(duì)車輛定位，是最常用的方法之一[3]。GPS 的定位精確度高，且時(shí)間以及地域限制對(duì)其精度影響較小，但當(dāng)車輛在有地形遮蔽處駕駛時(shí)，GPS信號(hào)有可能中斷，如隧道、深山等[4]。IMU 能以較高的頻率提供多種高精度的導(dǎo)航參數(shù)，如速度、加速度、角速度、角加速度等，但由于其數(shù)據(jù)本身存在噪聲干擾，導(dǎo)致位置誤差會(huì)隨時(shí)間累計(jì)而產(chǎn)生較大偏差，不適合長(zhǎng)時(shí)間高精度實(shí)時(shí)定位[5]。然而，由于GPS 數(shù)據(jù)與IMU 數(shù)據(jù)具有互補(bǔ)性，所以可通過數(shù)據(jù)融合實(shí)現(xiàn)高精度的實(shí)時(shí)定位[6]。同GPS 和IMU 數(shù)據(jù)融合類似的，還有視覺里程計(jì)（Visual Odometry，VO）與IMU 的數(shù)據(jù)融合。VO 主要利用車載相機(jī)連續(xù)拍攝圖片，通過圖片中多個(gè)明顯的特征點(diǎn)與相機(jī)的距離對(duì)相機(jī)進(jìn)行定位，進(jìn)而確定車輛位置[7]，但在使用VO 定位時(shí)，需要提前標(biāo)定行車地點(diǎn)周圍的路標(biāo)，即照片中的特征點(diǎn)。所以，相對(duì)于GPS 定位，VO 需要做大量的前期工作才能確保定位的可靠性[8]。也正是由于VO 需要多個(gè)路標(biāo)點(diǎn)，在對(duì)相機(jī)定位的同時(shí)，可以對(duì)陸標(biāo)點(diǎn)位置進(jìn)行優(yōu)化，所以常用于SLAM 中的地圖構(gòu)建。
在處理數(shù)據(jù)融合問題時(shí)，主要使用的方法有加權(quán)平均法、卡爾曼濾波法、貝葉斯網(wǎng)絡(luò)法等[9]。其中，加權(quán)平均法模型比較簡(jiǎn)單，但對(duì)于數(shù)據(jù)類型不同、更新頻率不同、噪聲模型也不同的多組傳感器來(lái)說，融合效果并不理想?？柭鼮V波是最常用的濾波算法，現(xiàn)在許多優(yōu)化問題都選擇采用卡爾曼濾波法解決[10]?？柭鼮V波是一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法?？柭鼮V波非常適合于連續(xù)系統(tǒng)，只需要保存當(dāng)前狀態(tài)與前一步狀態(tài)即可。并且，卡爾曼濾波占用內(nèi)存較小，處理速度快，適合應(yīng)用于實(shí)時(shí)系統(tǒng)[11]。但卡爾曼濾波必須已知系統(tǒng)狀態(tài)方程以及測(cè)量方程，并且要求狀態(tài)方程與測(cè)量方程為線性方程，如果方程為非線性，則要先對(duì)其進(jìn)行線性化，才可以使用卡爾曼濾波，但線性化過程本身就導(dǎo)致了誤差的產(chǎn)生[12]。貝葉斯網(wǎng)絡(luò)法是新興的優(yōu)化方法，其主要思想是由先驗(yàn)概率以及樣本信息得到后驗(yàn)概率，最后由最大后驗(yàn)估計(jì)方法對(duì)數(shù)據(jù)進(jìn)行優(yōu)化[13]。貝葉斯網(wǎng)絡(luò)能以有向無(wú)環(huán)圖的形式直觀地反映出多組傳感器測(cè)量值與車輛位置之間的因果關(guān)系，在不同的傳感器組測(cè)量坐標(biāo)一致時(shí)，可以直接對(duì)傳感器數(shù)據(jù)進(jìn)行融合[14]，但在計(jì)算過程中，發(fā)現(xiàn)很多狀態(tài)之間的關(guān)系與所求狀態(tài)無(wú)關(guān)，在狀態(tài)數(shù)量過多的情況下采用貝葉斯網(wǎng)絡(luò)計(jì)算將會(huì)產(chǎn)生龐大的計(jì)算量。同時(shí)，當(dāng)在貝葉斯網(wǎng)絡(luò)中新增節(jié)點(diǎn)時(shí)，往往需要對(duì)全圖進(jìn)行更新，也增加了優(yōu)化過程的工作量，不適合用于實(shí)時(shí)系統(tǒng)。
針對(duì)上述問題，考慮到實(shí)際應(yīng)用的準(zhǔn)確性和實(shí)時(shí)性，本文提出采用因子圖進(jìn)行數(shù)據(jù)融合的方法。因子圖同貝葉斯網(wǎng)絡(luò)一樣，也是概率圖的一種，它是由KSCHISCHANG 等[15]在2001 年提出的一種二部無(wú)向圖。因子圖可以將一個(gè)具有多變量的全局函數(shù)分解為幾個(gè)局部函數(shù)的乘積，即在計(jì)算過程中可以分離出與當(dāng)前關(guān)系無(wú)關(guān)的變量[16]。同時(shí)，由于因子圖與SLAM 底層的優(yōu)化有更緊密的聯(lián)系，所以選擇用因子圖模型進(jìn)行SLAM 的優(yōu)化。相比于卡爾曼濾波法和貝葉斯網(wǎng)絡(luò)法，因子圖更直觀地表現(xiàn)了不同節(jié)點(diǎn)之間的關(guān)系。在需要添加狀態(tài)量時(shí)，因子圖可以直接在原有圖的基礎(chǔ)上增添因子，同樣，如果測(cè)量值可信度較低，或信號(hào)丟失，只需要在原圖的基礎(chǔ)上簡(jiǎn)單地減少因子即可，不需要特別地編程或者修改模型。因子圖的這一特點(diǎn)極大地減少了處理SLAM 問題時(shí)的計(jì)算量[17]。
1 建立因子圖模型
因子圖是概率圖的一種，其中有兩類節(jié)點(diǎn)，分別是變量節(jié)點(diǎn)與因子節(jié)點(diǎn)。本文把車輛位姿、路標(biāo)位置、傳感器測(cè)量值等作為變量節(jié)點(diǎn)，測(cè)量值與位姿之間的概率關(guān)系等作為因子節(jié)點(diǎn)，建立因子圖模型。
1.1 因子圖模型介紹
假設(shè)在運(yùn)行的車輛上裝備有多種傳感器，包括IMU、單目或立體照相機(jī)、激光雷達(dá)、GPS 等。首先，構(gòu)建因子圖中的變量節(jié)點(diǎn)，用xi表示ti時(shí)刻的車輛位姿，包括位置、速度、方向，用ci表示ti時(shí)刻的標(biāo)準(zhǔn)參數(shù)，包括加速度計(jì)和陀螺儀的偏差、照相機(jī)角度偏差和位置偏差等。此后，可以定義從t1時(shí)刻到當(dāng)前時(shí)刻tk所產(chǎn)生的所有狀態(tài)為：
在有觀測(cè)地標(biāo)時(shí)，定義第j個(gè)觀測(cè)地標(biāo)為lj。類似式（1），可以定義從t1時(shí)刻到當(dāng)前時(shí)刻tk產(chǎn)生的所有觀測(cè)地標(biāo)為lk，則有：
從t1到tk時(shí)刻，所有的狀態(tài)變量可以表示為：
同時(shí)，將從t1時(shí)刻開始接收到的所有的傳感器數(shù)據(jù)記為Zk，則相應(yīng)的有：
式中：zi表示在ti時(shí)刻接收到的測(cè)量值。
構(gòu)建完所有變量節(jié)點(diǎn)之后，開始構(gòu)建因子節(jié)點(diǎn)，因子節(jié)點(diǎn)主要描述了變量之間的概率關(guān)系。由上述定義得到，當(dāng)前時(shí)刻tk的位姿狀態(tài)Vk與前一時(shí)刻tk-1的狀態(tài)Vk-1有關(guān)，也與tk時(shí)刻的標(biāo)準(zhǔn)參數(shù)ck有關(guān)，同時(shí)ck也由前一時(shí)刻的ck-1決定，由遞推關(guān)系以及全概率公式，可以把由觀測(cè)值Zk得到實(shí)際位姿Vk的聯(lián)合概率函數(shù)表示為：
式中：p為聯(lián)合概率函數(shù)。
由式（5）可以看出，聯(lián)合概率密度函數(shù)是非常復(fù)雜的，但在實(shí)際情況中，當(dāng)前時(shí)刻tk的位姿狀態(tài)Vk與前一時(shí)刻tk-1的狀態(tài)Vk-1、當(dāng)前時(shí)刻tk的測(cè)量值Zk以及標(biāo)準(zhǔn)參數(shù)ck有關(guān)，即有：
式中：g為由前一時(shí)刻tk-1的標(biāo)準(zhǔn)參數(shù)ck-1到當(dāng)前時(shí)刻tk的標(biāo)準(zhǔn)參數(shù)ck的狀態(tài)轉(zhuǎn)移方程。
式中：fi為第i組數(shù)據(jù)的誤差方程，在因子圖中，fi為變量節(jié)點(diǎn)之間的因子節(jié)點(diǎn)。
當(dāng)噪聲為高斯白噪聲時(shí)，有：
式（7）可簡(jiǎn)單表示為：
在構(gòu)建完所有的變量節(jié)點(diǎn)Vi與因子節(jié)點(diǎn)fi之后，建立完整的因子圖模型如圖1 所示。
圖1 行車路經(jīng)的因子圖模型
在圖1 中，紅色節(jié)點(diǎn)代表由IMU 測(cè)量值表示的變量節(jié)點(diǎn)，該變量節(jié)點(diǎn)構(gòu)成了鏈狀骨架的主體，兩側(cè)的藍(lán)色節(jié)點(diǎn)代表由路標(biāo)位置表示的變量節(jié)點(diǎn)，該變量節(jié)點(diǎn)是骨架兩側(cè)的藍(lán)色線段所代表的二元似然因子，即因子節(jié)點(diǎn)，與紅色節(jié)點(diǎn)相連[18]。
1.2 建立 GPS 及IMU 融合因子圖模型
IMU 與GPS 的數(shù)據(jù)更新頻率不同，IMU 相對(duì)更新頻率較高，具體的數(shù)據(jù)更新頻率由不同的IMU傳感器型號(hào)所決定。由于IMU 更新較為頻繁，考慮以IMU 代表的變量因子作為骨架的主體，構(gòu)建因子圖如圖2 所示。
圖2 IMU 因子圖模型
如圖2 所示，xi表示第i時(shí)刻車輛的狀態(tài)，ci表示第i時(shí)刻的標(biāo)準(zhǔn)參數(shù)。由于IMU 數(shù)據(jù)會(huì)隨時(shí)間產(chǎn)生誤差累積，該誤差產(chǎn)生的原因除了測(cè)量噪聲以外，還有IMU 傳感器自身產(chǎn)生的漂移誤差，所以需要添加變量節(jié)點(diǎn)ci對(duì)IMU 傳感器的測(cè)量值進(jìn)行矯正。fPrior是一元因子，該因子由式（5）中的p(V0)確定，只和第一步狀態(tài)x1有關(guān)。狀態(tài)xi+1由xi和ci以及當(dāng)前時(shí)刻測(cè)量值z(mì)i所確定，因子節(jié)點(diǎn)如式（10）所示，其中的為由狀態(tài)xi和ci到下一狀態(tài)的狀態(tài)轉(zhuǎn)移方程[14]。與此類似，因子節(jié)點(diǎn)fbias由式（8）確定，為ci到ci+1的轉(zhuǎn)移方程。在實(shí)際應(yīng)用中，可以適當(dāng)降低標(biāo)準(zhǔn)參數(shù)的更新頻率，如圖3 所示。
骨架構(gòu)建完成后，為了提高定位的精確度，將GPS 數(shù)據(jù)與IMU 數(shù)據(jù)進(jìn)行融合。在每過一定步長(zhǎng)n時(shí)，添加因子節(jié)點(diǎn)fGPS。其中間隔的步長(zhǎng)n由車輛實(shí)際裝備的IMU 和GPS 的更新頻率決定。添加因子節(jié)點(diǎn)后的因子圖如圖4 所示。
如圖4 所示，在每個(gè)連接了因子節(jié)點(diǎn)fGPS的變量節(jié)點(diǎn)xi之間，存在多個(gè)僅由IMU 數(shù)據(jù)得到的車輛位姿?？紤]將圖4 中間隔的步長(zhǎng)中的變量節(jié)點(diǎn)合并，對(duì)因子圖化簡(jiǎn)的結(jié)果如圖5 所示。
圖3 減少參數(shù)后的IMU 因子圖模型
圖4 GPS 及IMU 融合因子圖模型
圖5 化簡(jiǎn)后的GPS 及IMU 融合因子圖模型
在圖5 中，狀態(tài)xi+1和xi之間實(shí)際間隔了nΔt，圖中的二元因子節(jié)點(diǎn)fEquiv與fIMU類似，但是其中的為第ti時(shí)刻到ti+n時(shí)刻的狀態(tài)轉(zhuǎn)移方程。
1.3 建立VO 與IMU 融合因子圖模型
隨著圖像識(shí)別技術(shù)的發(fā)展，利用照片或視頻對(duì)行車附近的特征點(diǎn)進(jìn)行標(biāo)識(shí)的技術(shù)也日益成熟。反之，如果知道被標(biāo)識(shí)的特征點(diǎn)的具體位置，則可以利用圖像得到車輛與特征點(diǎn)間的距離，估計(jì)出車輛的具體位置。VO 定位的方式與GPS 不同，GPS 主要通過衛(wèi)星進(jìn)行定位，得到的是車輛的全球坐標(biāo)。利用衛(wèi)星定位，導(dǎo)致GPS 信號(hào)受到空間磁場(chǎng)干擾、建筑物遮蔽等因素的影響，最終導(dǎo)致在某些特定場(chǎng)所或特定時(shí)間定位的偏差較大。針對(duì)上述問題，不妨考慮用VO 代替GPS。
相比于GPS 信號(hào)，利用VO 定位需要有明顯的路標(biāo)作為圖像特征，通過對(duì)圖像與圖像或圖像與地圖之間的特征描述進(jìn)行匹配，進(jìn)而確定車輛的確切位置。當(dāng)然，由于圖像特征的局部特性，誤匹配的情況還是廣泛存在的。根據(jù)上文所述，在圖2 的基礎(chǔ)上，添加路標(biāo)節(jié)點(diǎn)作為變量節(jié)點(diǎn)，建立的因子圖如圖6 所示。
圖6 中添加了變量節(jié)點(diǎn)li，表示第i個(gè)路標(biāo)。連接路標(biāo)lj與車輛位姿狀態(tài)xi之間的因子節(jié)點(diǎn)fvision與因子節(jié)點(diǎn)fIMU類似，但zk變?yōu)檐囕v位置與特征點(diǎn)距離的測(cè)量值。由圖6 可知，利用VO 也可以在定位的同時(shí)對(duì)路標(biāo)位置進(jìn)行優(yōu)化，從而構(gòu)建車輛駕駛區(qū)域的地圖。
圖6 VO 及IMU 融合因子圖模型
由于本文主要討論在SLAM 中的融合定位問題，不考慮構(gòu)建駕駛區(qū)域地圖，所以弱化路標(biāo)在因子圖中的作用，僅考慮由路標(biāo)得到的車輛當(dāng)前位姿信息，即添加因子節(jié)點(diǎn)fVO。又由于VO 與IMU 的數(shù)據(jù)更新頻率也會(huì)隨具體車載裝備而不同，一般情況下，VO 的數(shù)據(jù)更新頻率低于IMU 數(shù)據(jù)更新頻率。根據(jù)以上描述，可更新因子圖如圖7 所示。
與圖5 相同，由于因子節(jié)點(diǎn)fVO所跨越的所有變量節(jié)點(diǎn)均只由IMU 數(shù)據(jù)優(yōu)化得到，所以考慮將圖7中間的因子節(jié)點(diǎn)和變量節(jié)點(diǎn)相融合，可得到的因子圖如圖8 所示。
同樣，狀態(tài)xi+1和xi之間實(shí)際間隔了nΔt，圖8中的二元因子節(jié)點(diǎn)fEquiv中的為第ti時(shí)刻到ti+n時(shí)刻的狀態(tài)轉(zhuǎn)移方程。
圖7 弱化路標(biāo)節(jié)點(diǎn)后的融合因子圖模型
圖8 化簡(jiǎn)后的VO 及IMU 融合因子圖模型
2 試驗(yàn)結(jié)果及分析
本部分試驗(yàn)測(cè)試的硬件環(huán)境為內(nèi)存8 GB，Intel Core i7-8750 處理器，主頻2.2 GHz，軟件環(huán)境為Matlab。試驗(yàn)數(shù)據(jù)采用KITTI 數(shù)據(jù)集，KITTI 數(shù)據(jù)集由德國(guó)卡爾斯魯厄理工學(xué)院和豐田美國(guó)技術(shù)研究院聯(lián)合創(chuàng)辦，是目前國(guó)際上最大的自動(dòng)駕駛場(chǎng)景下的計(jì)算機(jī)視覺算法測(cè)評(píng)數(shù)據(jù)集。試驗(yàn)主要采用右側(cè)的彩色攝像機(jī)和GPS 導(dǎo)航系統(tǒng)OXTS 的數(shù)據(jù)，OXTS 數(shù)據(jù)同時(shí)包括GPS 定位信息和IMU 數(shù)據(jù)信息。攝像機(jī)位置和OXTS 位置如圖9 所示。
圖9 傳感器位置及坐標(biāo)系
圖9 中，紅色所標(biāo)位置為攝像機(jī)位置，其中x軸方向?yàn)檐囕v正右方，y軸方向?yàn)檐囕v正下方，z軸方向?yàn)檐囕v正前方。藍(lán)色所標(biāo)位置為GPS 導(dǎo)航系統(tǒng)OXTS 位置，其中x軸方向?yàn)檐囕v正前方，y軸方向?yàn)檐囕v正左方，z軸方向?yàn)檐囕v正上方。
2.1 GPS 及IMU 定位
由于GPS 數(shù)據(jù)與IMU 數(shù)據(jù)均由GPS 導(dǎo)航系統(tǒng)獲取，所以本試驗(yàn)的GPS 信號(hào)與IMU 信號(hào)有相同的時(shí)間頻率。首先，將GPS 數(shù)據(jù)與IMU 數(shù)據(jù)從所獲取的OXTS 數(shù)據(jù)中分離。其次，對(duì)GPS 數(shù)據(jù)進(jìn)行處理，將GPS 數(shù)據(jù)由全球經(jīng)緯坐標(biāo)，變?yōu)榈孛孀鴺?biāo)。最后，將處理后的GPS 數(shù)據(jù)和IMU 數(shù)據(jù)帶入圖5 所示的因子圖模型中，得到行車路徑的優(yōu)化結(jié)果。流程如圖10 所示。
圖10 GPS 及IMU 定位
本試驗(yàn)主要根據(jù)數(shù)據(jù)的數(shù)量、復(fù)雜度等因素，選取了4 組不同的KITTI 數(shù)據(jù)集進(jìn)行測(cè)試，4 組數(shù)據(jù)集的數(shù)據(jù)量分別為76 幀、108 幀、269 幀和313 幀。測(cè)試順序按照數(shù)據(jù)集中數(shù)據(jù)信息的數(shù)量，由少至多依次排列，測(cè)試結(jié)果如圖11 所示。
圖11 GPS 及IMU 定位后的車輛位姿
圖11 分別展示了由4 組數(shù)據(jù)得到的紅色GPS定位路徑、綠色的因子圖融合后的定位路徑以及藍(lán)色的實(shí)際行車路經(jīng)。由圖可知，經(jīng)因子圖融合后的定位路徑曲線相較于原本的GPS 定位路徑曲線，精度更高。在4 組數(shù)據(jù)中，由圖11c 可以觀察出第3 組數(shù)據(jù)相較于其它數(shù)據(jù)有所不同。在第3 組數(shù)據(jù)中，車輛在數(shù)據(jù)采集的初始階段進(jìn)行了長(zhǎng)時(shí)間的轉(zhuǎn)向操作。然而，GPS 并沒有明確對(duì)當(dāng)時(shí)的車輛位姿進(jìn)行判斷，僅給出了大致定位。但采用因子圖對(duì)數(shù)據(jù)融合之后，得到了車輛駕駛過程中明確的車輛位姿。
由第3 組數(shù)據(jù)中前100 組數(shù)據(jù)得到的GPS 定位路徑與因子圖融合后的路徑如圖12 所示，其中GPS 定位結(jié)果在點(diǎn)（0，0）附近 ±0.5 m 的位置 ，并未做明確的移動(dòng)路徑標(biāo)識(shí)。但是由因子圖得到的定位結(jié)果，明確地表現(xiàn)出車輛經(jīng)歷的位姿。
圖12 車輛移動(dòng)幅度較小時(shí)的位姿（預(yù)估軌跡）
GPS 路徑與由因子圖融合后的定位路徑的具體誤差情況如圖13 所示。
根據(jù)圖13 所示的車輛定位誤差信息，得到定位誤差的均值與均方差值見表1。
圖13 車輛定位誤差
表1 車輛定位誤差
由圖13 和表1 可知，在第1 組數(shù)據(jù)中，GPS定位的誤差由于數(shù)據(jù)較少，誤差的均值和均方差相對(duì)較小。隨著數(shù)據(jù)量的增大，GPS 定位誤差均值上升趨勢(shì)減緩，基本保持在4 m左右，沒有明顯的上升。經(jīng)過因子圖融合后，定位誤差相較于GPS 定位誤差有明顯減少，誤差均值基本可以保持在1 m 左右。綜上所述，由因子圖對(duì)數(shù)據(jù)進(jìn)行融合之后得到的車輛位姿更接近真實(shí)情況。
2.2 VO 及IMU 定位
VO 及IMU 數(shù)據(jù)融合的因子圖結(jié)構(gòu)與GPS 和IMU 數(shù)據(jù)融合的因子圖結(jié)構(gòu)在骨架部分是相似的，但不同之處在于，VO 數(shù)據(jù)受到前一時(shí)刻和當(dāng)前時(shí)刻車輛位姿的共同影響，GPS 數(shù)據(jù)僅受到當(dāng)前時(shí)刻測(cè)量位姿的影響。
具體流程如圖14 所示，首先由彩色攝像機(jī)拍攝得到的圖像，提取車輛的位姿信息。其次，統(tǒng)一VO 數(shù)據(jù)和IMU 數(shù)據(jù)的時(shí)間戳。最后，將得到的IMU 數(shù)據(jù)以及VO 數(shù)據(jù)帶入圖8 所示的因子圖模型中，得到融合后的車輛位姿。本次試驗(yàn)選取了3組不同的數(shù)據(jù)進(jìn)行測(cè)試，按照流程圖得到的測(cè)試結(jié)果如圖15 所示。
圖14 VO 及IMU 定位
圖15 VO 及IMU 定位后的車輛位姿
3 結(jié)論
為了更精確地實(shí)現(xiàn)SLAM 中的定位，本文提出了建立因子圖模型的方法，解決了準(zhǔn)確判別車輛位姿的問題，包括GPS 與IMU 數(shù)據(jù)的融合，以及VO 與IMU 數(shù)據(jù)的融合，這兩部分分別利用不同的數(shù)據(jù)對(duì)車輛位姿進(jìn)行判斷。其中，第一部分得到了較為理想的車輛定位結(jié)果，第二部分車輛定位結(jié)果不太理想，主要原因在于單目VO 對(duì)于距離缺乏準(zhǔn)確判斷，并且隨著時(shí)間增加漂移誤差累積較大。即由于VO 數(shù)據(jù)的準(zhǔn)確度偏低導(dǎo)致定位路徑偏差較大，但仍可看出數(shù)據(jù)融合對(duì)車輛定位路徑進(jìn)行了一定的矯正。目前還需要提高VO 數(shù)據(jù)的準(zhǔn)確度，在這方面可以選擇雙目或多目VO 數(shù)據(jù)，以期待得到理想的效果。
猜你喜歡
冬“傲”時(shí)刻環(huán)球人物(2022年4期)2022-02-22 22:05:06 捕獵時(shí)刻小資CHIC！ELEGANCE(2021年32期)2021-09-18 06:17:14 《導(dǎo)航定位與授時(shí)》征稿簡(jiǎn)則導(dǎo)航定位與授時(shí)(2020年5期)2020-09-23 03:05:00 Smartrail4.0定位和控制鐵道通信信號(hào)(2020年9期)2020-02-06 09:16:06 找準(zhǔn)定位 砥礪前行知識(shí)經(jīng)濟(jì)·中國(guó)直銷(2018年3期)2018-04-12 06:43:37 基于共面直線迭代加權(quán)最小二乘的相機(jī)位姿估計(jì)光學(xué)精密工程(2016年5期)2016-11-07 09:05:55 基于CAD模型的單目六自由度位姿測(cè)量光學(xué)精密工程(2016年4期)2016-11-07 09:05:11 小型四旋翼飛行器位姿建模及其仿真湖北工業(yè)大學(xué)學(xué)報(bào)(2016年5期)2016-02-27 13:14:48 青年擇業(yè)要有準(zhǔn)確定位學(xué)習(xí)月刊(2015年1期)2015-07-11 01:51:12 街拍的歡樂時(shí)刻到來(lái)了爆笑ｓｈｏｗ(2015年4期)2015-06-24 01:55:12
袁子葉，尹慧琳，伍淑莉
（同濟(jì)大學(xué) 中德學(xué)院 電子信息系，上海 201804）
自動(dòng)駕駛技術(shù)在降低交通事故發(fā)生率、緩解交通擁堵、實(shí)時(shí)提供有效最短路徑等方面起到了明顯的作用[1]，其中的關(guān)鍵技術(shù)“即時(shí)定位與地圖構(gòu)建”（Simultaneous Localization and Mapping，SLAM）也隨之備受關(guān)注。在SLAM 中，能夠根據(jù)多個(gè)傳感器信息，對(duì)車輛進(jìn)行精確定位尤為重要，所以提高即時(shí)定位的精確度一直是研究的熱點(diǎn)[2]。
對(duì)于車輛定位有許多不同的方法，其中利用全球定位系統(tǒng)（Global Positioning System，GPS）與慣性測(cè)量元件（Inertial Measurement Unit ，IMU）進(jìn)行數(shù)據(jù)融合從而對(duì)車輛定位，是最常用的方法之一[3]。GPS 的定位精確度高，且時(shí)間以及地域限制對(duì)其精度影響較小，但當(dāng)車輛在有地形遮蔽處駕駛時(shí)，GPS信號(hào)有可能中斷，如隧道、深山等[4]。IMU 能以較高的頻率提供多種高精度的導(dǎo)航參數(shù)，如速度、加速度、角速度、角加速度等，但由于其數(shù)據(jù)本身存在噪聲干擾，導(dǎo)致位置誤差會(huì)隨時(shí)間累計(jì)而產(chǎn)生較大偏差，不適合長(zhǎng)時(shí)間高精度實(shí)時(shí)定位[5]。然而，由于GPS 數(shù)據(jù)與IMU 數(shù)據(jù)具有互補(bǔ)性，所以可通過數(shù)據(jù)融合實(shí)現(xiàn)高精度的實(shí)時(shí)定位[6]。同GPS 和IMU 數(shù)據(jù)融合類似的，還有視覺里程計(jì)（Visual Odometry，VO）與IMU 的數(shù)據(jù)融合。VO 主要利用車載相機(jī)連續(xù)拍攝圖片，通過圖片中多個(gè)明顯的特征點(diǎn)與相機(jī)的距離對(duì)相機(jī)進(jìn)行定位，進(jìn)而確定車輛位置[7]，但在使用VO 定位時(shí)，需要提前標(biāo)定行車地點(diǎn)周圍的路標(biāo)，即照片中的特征點(diǎn)。所以，相對(duì)于GPS 定位，VO 需要做大量的前期工作才能確保定位的可靠性[8]。也正是由于VO 需要多個(gè)路標(biāo)點(diǎn)，在對(duì)相機(jī)定位的同時(shí)，可以對(duì)陸標(biāo)點(diǎn)位置進(jìn)行優(yōu)化，所以常用于SLAM 中的地圖構(gòu)建。
在處理數(shù)據(jù)融合問題時(shí)，主要使用的方法有加權(quán)平均法、卡爾曼濾波法、貝葉斯網(wǎng)絡(luò)法等[9]。其中，加權(quán)平均法模型比較簡(jiǎn)單，但對(duì)于數(shù)據(jù)類型不同、更新頻率不同、噪聲模型也不同的多組傳感器來(lái)說，融合效果并不理想?？柭鼮V波是最常用的濾波算法，現(xiàn)在許多優(yōu)化問題都選擇采用卡爾曼濾波法解決[10]?？柭鼮V波是一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法?？柭鼮V波非常適合于連續(xù)系統(tǒng)，只需要保存當(dāng)前狀態(tài)與前一步狀態(tài)即可。并且，卡爾曼濾波占用內(nèi)存較小，處理速度快，適合應(yīng)用于實(shí)時(shí)系統(tǒng)[11]。但卡爾曼濾波必須已知系統(tǒng)狀態(tài)方程以及測(cè)量方程，并且要求狀態(tài)方程與測(cè)量方程為線性方程，如果方程為非線性，則要先對(duì)其進(jìn)行線性化，才可以使用卡爾曼濾波，但線性化過程本身就導(dǎo)致了誤差的產(chǎn)生[12]。貝葉斯網(wǎng)絡(luò)法是新興的優(yōu)化方法，其主要思想是由先驗(yàn)概率以及樣本信息得到后驗(yàn)概率，最后由最大后驗(yàn)估計(jì)方法對(duì)數(shù)據(jù)進(jìn)行優(yōu)化[13]。貝葉斯網(wǎng)絡(luò)能以有向無(wú)環(huán)圖的形式直觀地反映出多組傳感器測(cè)量值與車輛位置之間的因果關(guān)系，在不同的傳感器組測(cè)量坐標(biāo)一致時(shí)，可以直接對(duì)傳感器數(shù)據(jù)進(jìn)行融合[14]，但在計(jì)算過程中，發(fā)現(xiàn)很多狀態(tài)之間的關(guān)系與所求狀態(tài)無(wú)關(guān)，在狀態(tài)數(shù)量過多的情況下采用貝葉斯網(wǎng)絡(luò)計(jì)算將會(huì)產(chǎn)生龐大的計(jì)算量。同時(shí)，當(dāng)在貝葉斯網(wǎng)絡(luò)中新增節(jié)點(diǎn)時(shí)，往往需要對(duì)全圖進(jìn)行更新，也增加了優(yōu)化過程的工作量，不適合用于實(shí)時(shí)系統(tǒng)。
針對(duì)上述問題，考慮到實(shí)際應(yīng)用的準(zhǔn)確性和實(shí)時(shí)性，本文提出采用因子圖進(jìn)行數(shù)據(jù)融合的方法。因子圖同貝葉斯網(wǎng)絡(luò)一樣，也是概率圖的一種，它是由KSCHISCHANG 等[15]在2001 年提出的一種二部無(wú)向圖。因子圖可以將一個(gè)具有多變量的全局函數(shù)分解為幾個(gè)局部函數(shù)的乘積，即在計(jì)算過程中可以分離出與當(dāng)前關(guān)系無(wú)關(guān)的變量[16]。同時(shí)，由于因子圖與SLAM 底層的優(yōu)化有更緊密的聯(lián)系，所以選擇用因子圖模型進(jìn)行SLAM 的優(yōu)化。相比于卡爾曼濾波法和貝葉斯網(wǎng)絡(luò)法，因子圖更直觀地表現(xiàn)了不同節(jié)點(diǎn)之間的關(guān)系。在需要添加狀態(tài)量時(shí)，因子圖可以直接在原有圖的基礎(chǔ)上增添因子，同樣，如果測(cè)量值可信度較低，或信號(hào)丟失，只需要在原圖的基礎(chǔ)上簡(jiǎn)單地減少因子即可，不需要特別地編程或者修改模型。因子圖的這一特點(diǎn)極大地減少了處理SLAM 問題時(shí)的計(jì)算量[17]。
1 建立因子圖模型
因子圖是概率圖的一種，其中有兩類節(jié)點(diǎn)，分別是變量節(jié)點(diǎn)與因子節(jié)點(diǎn)。本文把車輛位姿、路標(biāo)位置、傳感器測(cè)量值等作為變量節(jié)點(diǎn)，測(cè)量值與位姿之間的概率關(guān)系等作為因子節(jié)點(diǎn)，建立因子圖模型。
1.1 因子圖模型介紹
假設(shè)在運(yùn)行的車輛上裝備有多種傳感器，包括IMU、單目或立體照相機(jī)、激光雷達(dá)、GPS 等。首先，構(gòu)建因子圖中的變量節(jié)點(diǎn)，用xi表示ti時(shí)刻的車輛位姿，包括位置、速度、方向，用ci表示ti時(shí)刻的標(biāo)準(zhǔn)參數(shù)，包括加速度計(jì)和陀螺儀的偏差、照相機(jī)角度偏差和位置偏差等。此后，可以定義從t1時(shí)刻到當(dāng)前時(shí)刻tk所產(chǎn)生的所有狀態(tài)為：
在有觀測(cè)地標(biāo)時(shí)，定義第j個(gè)觀測(cè)地標(biāo)為lj。類似式（1），可以定義從t1時(shí)刻到當(dāng)前時(shí)刻tk產(chǎn)生的所有觀測(cè)地標(biāo)為lk，則有：
從t1到tk時(shí)刻，所有的狀態(tài)變量可以表示為：
同時(shí)，將從t1時(shí)刻開始接收到的所有的傳感器數(shù)據(jù)記為Zk，則相應(yīng)的有：
式中：zi表示在ti時(shí)刻接收到的測(cè)量值。
構(gòu)建完所有變量節(jié)點(diǎn)之后，開始構(gòu)建因子節(jié)點(diǎn)，因子節(jié)點(diǎn)主要描述了變量之間的概率關(guān)系。由上述定義得到，當(dāng)前時(shí)刻tk的位姿狀態(tài)Vk與前一時(shí)刻tk-1的狀態(tài)Vk-1有關(guān)，也與tk時(shí)刻的標(biāo)準(zhǔn)參數(shù)ck有關(guān)，同時(shí)ck也由前一時(shí)刻的ck-1決定，由遞推關(guān)系以及全概率公式，可以把由觀測(cè)值Zk得到實(shí)際位姿Vk的聯(lián)合概率函數(shù)表示為：
式中：p為聯(lián)合概率函數(shù)。
由式（5）可以看出，聯(lián)合概率密度函數(shù)是非常復(fù)雜的，但在實(shí)際情況中，當(dāng)前時(shí)刻tk的位姿狀態(tài)Vk與前一時(shí)刻tk-1的狀態(tài)Vk-1、當(dāng)前時(shí)刻tk的測(cè)量值Zk以及標(biāo)準(zhǔn)參數(shù)ck有關(guān)，即有：
式中：g為由前一時(shí)刻tk-1的標(biāo)準(zhǔn)參數(shù)ck-1到當(dāng)前時(shí)刻tk的標(biāo)準(zhǔn)參數(shù)ck的狀態(tài)轉(zhuǎn)移方程。
式中：fi為第i組數(shù)據(jù)的誤差方程，在因子圖中，fi為變量節(jié)點(diǎn)之間的因子節(jié)點(diǎn)。
當(dāng)噪聲為高斯白噪聲時(shí)，有：
式（7）可簡(jiǎn)單表示為：
在構(gòu)建完所有的變量節(jié)點(diǎn)Vi與因子節(jié)點(diǎn)fi之后，建立完整的因子圖模型如圖1 所示。
圖1 行車路經(jīng)的因子圖模型
在圖1 中，紅色節(jié)點(diǎn)代表由IMU 測(cè)量值表示的變量節(jié)點(diǎn)，該變量節(jié)點(diǎn)構(gòu)成了鏈狀骨架的主體，兩側(cè)的藍(lán)色節(jié)點(diǎn)代表由路標(biāo)位置表示的變量節(jié)點(diǎn)，該變量節(jié)點(diǎn)是骨架兩側(cè)的藍(lán)色線段所代表的二元似然因子，即因子節(jié)點(diǎn)，與紅色節(jié)點(diǎn)相連[18]。
1.2 建立 GPS 及IMU 融合因子圖模型
IMU 與GPS 的數(shù)據(jù)更新頻率不同，IMU 相對(duì)更新頻率較高，具體的數(shù)據(jù)更新頻率由不同的IMU傳感器型號(hào)所決定。由于IMU 更新較為頻繁，考慮以IMU 代表的變量因子作為骨架的主體，構(gòu)建因子圖如圖2 所示。
圖2 IMU 因子圖模型
如圖2 所示，xi表示第i時(shí)刻車輛的狀態(tài)，ci表示第i時(shí)刻的標(biāo)準(zhǔn)參數(shù)。由于IMU 數(shù)據(jù)會(huì)隨時(shí)間產(chǎn)生誤差累積，該誤差產(chǎn)生的原因除了測(cè)量噪聲以外，還有IMU 傳感器自身產(chǎn)生的漂移誤差，所以需要添加變量節(jié)點(diǎn)ci對(duì)IMU 傳感器的測(cè)量值進(jìn)行矯正。fPrior是一元因子，該因子由式（5）中的p(V0)確定，只和第一步狀態(tài)x1有關(guān)。狀態(tài)xi+1由xi和ci以及當(dāng)前時(shí)刻測(cè)量值z(mì)i所確定，因子節(jié)點(diǎn)如式（10）所示，其中的為由狀態(tài)xi和ci到下一狀態(tài)的狀態(tài)轉(zhuǎn)移方程[14]。與此類似，因子節(jié)點(diǎn)fbias由式（8）確定，為ci到ci+1的轉(zhuǎn)移方程。在實(shí)際應(yīng)用中，可以適當(dāng)降低標(biāo)準(zhǔn)參數(shù)的更新頻率，如圖3 所示。
骨架構(gòu)建完成后，為了提高定位的精確度，將GPS 數(shù)據(jù)與IMU 數(shù)據(jù)進(jìn)行融合。在每過一定步長(zhǎng)n時(shí)，添加因子節(jié)點(diǎn)fGPS。其中間隔的步長(zhǎng)n由車輛實(shí)際裝備的IMU 和GPS 的更新頻率決定。添加因子節(jié)點(diǎn)后的因子圖如圖4 所示。
如圖4 所示，在每個(gè)連接了因子節(jié)點(diǎn)fGPS的變量節(jié)點(diǎn)xi之間，存在多個(gè)僅由IMU 數(shù)據(jù)得到的車輛位姿?？紤]將圖4 中間隔的步長(zhǎng)中的變量節(jié)點(diǎn)合并，對(duì)因子圖化簡(jiǎn)的結(jié)果如圖5 所示。
圖3 減少參數(shù)后的IMU 因子圖模型
圖4 GPS 及IMU 融合因子圖模型
圖5 化簡(jiǎn)后的GPS 及IMU 融合因子圖模型
在圖5 中，狀態(tài)xi+1和xi之間實(shí)際間隔了nΔt，圖中的二元因子節(jié)點(diǎn)fEquiv與fIMU類似，但是其中的為第ti時(shí)刻到ti+n時(shí)刻的狀態(tài)轉(zhuǎn)移方程。
1.3 建立VO 與IMU 融合因子圖模型
隨著圖像識(shí)別技術(shù)的發(fā)展，利用照片或視頻對(duì)行車附近的特征點(diǎn)進(jìn)行標(biāo)識(shí)的技術(shù)也日益成熟。反之，如果知道被標(biāo)識(shí)的特征點(diǎn)的具體位置，則可以利用圖像得到車輛與特征點(diǎn)間的距離，估計(jì)出車輛的具體位置。VO 定位的方式與GPS 不同，GPS 主要通過衛(wèi)星進(jìn)行定位，得到的是車輛的全球坐標(biāo)。利用衛(wèi)星定位，導(dǎo)致GPS 信號(hào)受到空間磁場(chǎng)干擾、建筑物遮蔽等因素的影響，最終導(dǎo)致在某些特定場(chǎng)所或特定時(shí)間定位的偏差較大。針對(duì)上述問題，不妨考慮用VO 代替GPS。
相比于GPS 信號(hào)，利用VO 定位需要有明顯的路標(biāo)作為圖像特征，通過對(duì)圖像與圖像或圖像與地圖之間的特征描述進(jìn)行匹配，進(jìn)而確定車輛的確切位置。當(dāng)然，由于圖像特征的局部特性，誤匹配的情況還是廣泛存在的。根據(jù)上文所述，在圖2 的基礎(chǔ)上，添加路標(biāo)節(jié)點(diǎn)作為變量節(jié)點(diǎn)，建立的因子圖如圖6 所示。
圖6 中添加了變量節(jié)點(diǎn)li，表示第i個(gè)路標(biāo)。連接路標(biāo)lj與車輛位姿狀態(tài)xi之間的因子節(jié)點(diǎn)fvision與因子節(jié)點(diǎn)fIMU類似，但zk變?yōu)檐囕v位置與特征點(diǎn)距離的測(cè)量值。由圖6 可知，利用VO 也可以在定位的同時(shí)對(duì)路標(biāo)位置進(jìn)行優(yōu)化，從而構(gòu)建車輛駕駛區(qū)域的地圖。
圖6 VO 及IMU 融合因子圖模型
由于本文主要討論在SLAM 中的融合定位問題，不考慮構(gòu)建駕駛區(qū)域地圖，所以弱化路標(biāo)在因子圖中的作用，僅考慮由路標(biāo)得到的車輛當(dāng)前位姿信息，即添加因子節(jié)點(diǎn)fVO。又由于VO 與IMU 的數(shù)據(jù)更新頻率也會(huì)隨具體車載裝備而不同，一般情況下，VO 的數(shù)據(jù)更新頻率低于IMU 數(shù)據(jù)更新頻率。根據(jù)以上描述，可更新因子圖如圖7 所示。
與圖5 相同，由于因子節(jié)點(diǎn)fVO所跨越的所有變量節(jié)點(diǎn)均只由IMU 數(shù)據(jù)優(yōu)化得到，所以考慮將圖7中間的因子節(jié)點(diǎn)和變量節(jié)點(diǎn)相融合，可得到的因子圖如圖8 所示。
同樣，狀態(tài)xi+1和xi之間實(shí)際間隔了nΔt，圖8中的二元因子節(jié)點(diǎn)fEquiv中的為第ti時(shí)刻到ti+n時(shí)刻的狀態(tài)轉(zhuǎn)移方程。
圖7 弱化路標(biāo)節(jié)點(diǎn)后的融合因子圖模型
圖8 化簡(jiǎn)后的VO 及IMU 融合因子圖模型
2 試驗(yàn)結(jié)果及分析
本部分試驗(yàn)測(cè)試的硬件環(huán)境為內(nèi)存8 GB，Intel Core i7-8750 處理器，主頻2.2 GHz，軟件環(huán)境為Matlab。試驗(yàn)數(shù)據(jù)采用KITTI 數(shù)據(jù)集，KITTI 數(shù)據(jù)集由德國(guó)卡爾斯魯厄理工學(xué)院和豐田美國(guó)技術(shù)研究院聯(lián)合創(chuàng)辦，是目前國(guó)際上最大的自動(dòng)駕駛場(chǎng)景下的計(jì)算機(jī)視覺算法測(cè)評(píng)數(shù)據(jù)集。試驗(yàn)主要采用右側(cè)的彩色攝像機(jī)和GPS 導(dǎo)航系統(tǒng)OXTS 的數(shù)據(jù)，OXTS 數(shù)據(jù)同時(shí)包括GPS 定位信息和IMU 數(shù)據(jù)信息。攝像機(jī)位置和OXTS 位置如圖9 所示。
圖9 傳感器位置及坐標(biāo)系
圖9 中，紅色所標(biāo)位置為攝像機(jī)位置，其中x軸方向?yàn)檐囕v正右方，y軸方向?yàn)檐囕v正下方，z軸方向?yàn)檐囕v正前方。藍(lán)色所標(biāo)位置為GPS 導(dǎo)航系統(tǒng)OXTS 位置，其中x軸方向?yàn)檐囕v正前方，y軸方向?yàn)檐囕v正左方，z軸方向?yàn)檐囕v正上方。
2.1 GPS 及IMU 定位
由于GPS 數(shù)據(jù)與IMU 數(shù)據(jù)均由GPS 導(dǎo)航系統(tǒng)獲取，所以本試驗(yàn)的GPS 信號(hào)與IMU 信號(hào)有相同的時(shí)間頻率。首先，將GPS 數(shù)據(jù)與IMU 數(shù)據(jù)從所獲取的OXTS 數(shù)據(jù)中分離。其次，對(duì)GPS 數(shù)據(jù)進(jìn)行處理，將GPS 數(shù)據(jù)由全球經(jīng)緯坐標(biāo)，變?yōu)榈孛孀鴺?biāo)。最后，將處理后的GPS 數(shù)據(jù)和IMU 數(shù)據(jù)帶入圖5 所示的因子圖模型中，得到行車路徑的優(yōu)化結(jié)果。流程如圖10 所示。
圖10 GPS 及IMU 定位
本試驗(yàn)主要根據(jù)數(shù)據(jù)的數(shù)量、復(fù)雜度等因素，選取了4 組不同的KITTI 數(shù)據(jù)集進(jìn)行測(cè)試，4 組數(shù)據(jù)集的數(shù)據(jù)量分別為76 幀、108 幀、269 幀和313 幀。測(cè)試順序按照數(shù)據(jù)集中數(shù)據(jù)信息的數(shù)量，由少至多依次排列，測(cè)試結(jié)果如圖11 所示。
圖11 GPS 及IMU 定位后的車輛位姿
圖11 分別展示了由4 組數(shù)據(jù)得到的紅色GPS定位路徑、綠色的因子圖融合后的定位路徑以及藍(lán)色的實(shí)際行車路經(jīng)。由圖可知，經(jīng)因子圖融合后的定位路徑曲線相較于原本的GPS 定位路徑曲線，精度更高。在4 組數(shù)據(jù)中，由圖11c 可以觀察出第3 組數(shù)據(jù)相較于其它數(shù)據(jù)有所不同。在第3 組數(shù)據(jù)中，車輛在數(shù)據(jù)采集的初始階段進(jìn)行了長(zhǎng)時(shí)間的轉(zhuǎn)向操作。然而，GPS 并沒有明確對(duì)當(dāng)時(shí)的車輛位姿進(jìn)行判斷，僅給出了大致定位。但采用因子圖對(duì)數(shù)據(jù)融合之后，得到了車輛駕駛過程中明確的車輛位姿。
由第3 組數(shù)據(jù)中前100 組數(shù)據(jù)得到的GPS 定位路徑與因子圖融合后的路徑如圖12 所示，其中GPS 定位結(jié)果在點(diǎn)（0，0）附近 ±0.5 m 的位置 ，并未做明確的移動(dòng)路徑標(biāo)識(shí)。但是由因子圖得到的定位結(jié)果，明確地表現(xiàn)出車輛經(jīng)歷的位姿。
圖12 車輛移動(dòng)幅度較小時(shí)的位姿（預(yù)估軌跡）
GPS 路徑與由因子圖融合后的定位路徑的具體誤差情況如圖13 所示。
根據(jù)圖13 所示的車輛定位誤差信息，得到定位誤差的均值與均方差值見表1。
圖13 車輛定位誤差
表1 車輛定位誤差
由圖13 和表1 可知，在第1 組數(shù)據(jù)中，GPS定位的誤差由于數(shù)據(jù)較少，誤差的均值和均方差相對(duì)較小。隨著數(shù)據(jù)量的增大，GPS 定位誤差均值上升趨勢(shì)減緩，基本保持在4 m左右，沒有明顯的上升。經(jīng)過因子圖融合后，定位誤差相較于GPS 定位誤差有明顯減少，誤差均值基本可以保持在1 m 左右。綜上所述，由因子圖對(duì)數(shù)據(jù)進(jìn)行融合之后得到的車輛位姿更接近真實(shí)情況。
2.2 VO 及IMU 定位
VO 及IMU 數(shù)據(jù)融合的因子圖結(jié)構(gòu)與GPS 和IMU 數(shù)據(jù)融合的因子圖結(jié)構(gòu)在骨架部分是相似的，但不同之處在于，VO 數(shù)據(jù)受到前一時(shí)刻和當(dāng)前時(shí)刻車輛位姿的共同影響，GPS 數(shù)據(jù)僅受到當(dāng)前時(shí)刻測(cè)量位姿的影響。
具體流程如圖14 所示，首先由彩色攝像機(jī)拍攝得到的圖像，提取車輛的位姿信息。其次，統(tǒng)一VO 數(shù)據(jù)和IMU 數(shù)據(jù)的時(shí)間戳。最后，將得到的IMU 數(shù)據(jù)以及VO 數(shù)據(jù)帶入圖8 所示的因子圖模型中，得到融合后的車輛位姿。本次試驗(yàn)選取了3組不同的數(shù)據(jù)進(jìn)行測(cè)試，按照流程圖得到的測(cè)試結(jié)果如圖15 所示。
圖14 VO 及IMU 定位
圖15 VO 及IMU 定位后的車輛位姿
3 結(jié)論
為了更精確地實(shí)現(xiàn)SLAM 中的定位，本文提出了建立因子圖模型的方法，解決了準(zhǔn)確判別車輛位姿的問題，包括GPS 與IMU 數(shù)據(jù)的融合，以及VO 與IMU 數(shù)據(jù)的融合，這兩部分分別利用不同的數(shù)據(jù)對(duì)車輛位姿進(jìn)行判斷。其中，第一部分得到了較為理想的車輛定位結(jié)果，第二部分車輛定位結(jié)果不太理想，主要原因在于單目VO 對(duì)于距離缺乏準(zhǔn)確判斷，并且隨著時(shí)間增加漂移誤差累積較大。即由于VO 數(shù)據(jù)的準(zhǔn)確度偏低導(dǎo)致定位路徑偏差較大，但仍可看出數(shù)據(jù)融合對(duì)車輛定位路徑進(jìn)行了一定的矯正。目前還需要提高VO 數(shù)據(jù)的準(zhǔn)確度，在這方面可以選擇雙目或多目VO 數(shù)據(jù)，以期待得到理想的效果。
猜你喜歡
環(huán)球人物(2022年4期)2022-02-22 22:05:06
小資CHIC！ELEGANCE(2021年32期)2021-09-18 06:17:14
導(dǎo)航定位與授時(shí)(2020年5期)2020-09-23 03:05:00
鐵道通信信號(hào)(2020年9期)2020-02-06 09:16:06
知識(shí)經(jīng)濟(jì)·中國(guó)直銷(2018年3期)2018-04-12 06:43:37
光學(xué)精密工程(2016年5期)2016-11-07 09:05:55
光學(xué)精密工程(2016年4期)2016-11-07 09:05:11
湖北工業(yè)大學(xué)學(xué)報(bào)(2016年5期)2016-02-27 13:14:48
學(xué)習(xí)月刊(2015年1期)2015-07-11 01:51:12
爆笑ｓｈｏｗ(2015年4期)2015-06-24 01:55:12
