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