基于時(shí)域差動(dòng)成像的車輛精細(xì)運(yùn)動(dòng)軌跡重構(gòu)方法

張輝 陳靜萍 林軍記 王浩遠(yuǎn) 龔文森

（中山大學(xué) 智能交通研究中心，廣東省智能交通系統(tǒng)重點(diǎn)實(shí)驗(yàn)室，廣州 510006）

主題詞：差動(dòng)成像 汽車軌跡 速度積分 汽車運(yùn)動(dòng)參數(shù) 圖像匹配

1 前言

在智能車運(yùn)動(dòng)控制、智能網(wǎng)聯(lián)汽車協(xié)同運(yùn)行、事故現(xiàn)場(chǎng)復(fù)現(xiàn)、車輛微觀駕駛狀態(tài)觀測(cè)等領(lǐng)域，準(zhǔn)確感知車輛運(yùn)動(dòng)軌跡的細(xì)節(jié)都具有重要的作用。目前，一般采用衛(wèi)星定位技術(shù)、蜂窩無(wú)線定位技術(shù)，通過解算序列化的定位信息復(fù)現(xiàn)車輛的運(yùn)動(dòng)軌跡，但往往由于序列定位點(diǎn)的精度較差，無(wú)法準(zhǔn)確感知車輛變道、泊車等運(yùn)動(dòng)過程的精細(xì)軌跡。為了解決這一問題，在無(wú)人車領(lǐng)域常采用高精度差分衛(wèi)星定位系統(tǒng)，通過厘米級(jí)精度的序列定位點(diǎn)來(lái)重構(gòu)車輛的精細(xì)運(yùn)動(dòng)軌跡，但采用這類系統(tǒng)成本一般較高，且往往受到環(huán)境條件的影響[1-4]。另一類獲取車輛精細(xì)運(yùn)動(dòng)軌跡的方法是慣性導(dǎo)航技術(shù)，這類技術(shù)主要使用慣性傳感器檢測(cè)汽車在慣性參考系中的慣性加速度和歐拉角速度等慣性參數(shù)，利用牛頓運(yùn)動(dòng)定律和動(dòng)坐標(biāo)系角速度轉(zhuǎn)換進(jìn)行逐點(diǎn)推算，通過積分計(jì)算得出汽車運(yùn)動(dòng)位置。但是，其軌跡感知性能在很大程度上依賴于慣性傳感器的性能以及車輛本身的固有特性，載荷的動(dòng)態(tài)變化、道路條件的差異等各種因素都可能導(dǎo)致軌跡感知的巨大誤差積累[5-8]。

有學(xué)者提出采用汽車圖像匹配技術(shù)對(duì)環(huán)境進(jìn)行初步檢測(cè)，主要是基于兩幀計(jì)算框架，選取兩幀圖像中的特征點(diǎn)，基于所獲得的特征關(guān)聯(lián)集合進(jìn)行幀間位姿估計(jì)，可以實(shí)現(xiàn)車輛的定位和運(yùn)動(dòng)估計(jì)。然后結(jié)合導(dǎo)航衛(wèi)星、傳感器或者蜂窩無(wú)線技術(shù)檢測(cè)汽車自身的運(yùn)動(dòng)參數(shù)，進(jìn)而實(shí)現(xiàn)汽車運(yùn)動(dòng)軌跡的感知。由于此類方法多采用前視圖像，感知的車輛軌跡仍然不夠精細(xì)[9-10]。

為此，本文提出了一種高精度、低成本，不依賴輔助設(shè)施的車輛精細(xì)運(yùn)動(dòng)軌跡感知方法。通過時(shí)域差動(dòng)成像的方法，首先使用光軸垂直于地面的攝像頭獲取車輛所在路面的細(xì)節(jié)圖像，利用對(duì)路面圖像細(xì)節(jié)在差動(dòng)時(shí)間內(nèi)偏移量的識(shí)別與匹配，計(jì)算車輛相對(duì)于路面的運(yùn)動(dòng)速度、方向、橫擺角，進(jìn)而根據(jù)速度積分法實(shí)現(xiàn)汽車精細(xì)軌跡重構(gòu)與感知。

2 車輛運(yùn)動(dòng)軌跡差動(dòng)成像重構(gòu)原理

車輛的運(yùn)動(dòng)軌跡可以通過車輛精確位置坐標(biāo)時(shí)間序列獲取，也可以通過車輛運(yùn)動(dòng)參數(shù)依據(jù)動(dòng)力學(xué)原理進(jìn)行推算，本文研究的方法在不依賴車輛的精確位置坐標(biāo)的條件下實(shí)現(xiàn)車輛運(yùn)動(dòng)軌跡的感知，因此采用常用的動(dòng)力學(xué)推算方法——速度積分法重構(gòu)車輛的精細(xì)運(yùn)動(dòng)軌跡，同時(shí)討論獲取速度積分所需參數(shù)的方法。

2.1 速度積分法

速度積分法[11]重構(gòu)車輛運(yùn)動(dòng)軌跡的原理如圖1所示。首先，定義車輛二維坐標(biāo)系，原點(diǎn)為車輛中心，車輛運(yùn)動(dòng)方向?yàn)闄M軸正方向，橫軸逆時(shí)針旋轉(zhuǎn)90°為縱軸方向。圖中坐標(biāo)系XOY為大地坐標(biāo)系，且與車輛初始狀態(tài)的車輛坐標(biāo)系重合；坐標(biāo)系Xk-1Ok-1Yk-1、XkOkYk分別為(k-1)和k時(shí)刻的地面坐標(biāo)系，且分別與相應(yīng)時(shí)刻的車輛坐標(biāo)系重合；vk和uk分別為k時(shí)刻車輛在坐標(biāo)系XkOkYk中的縱向速度和橫向速度；ψk為k時(shí)刻車輛坐標(biāo)系橫軸與坐標(biāo)系XOY橫軸的夾角，即橫擺角；φk為車輛坐標(biāo)系橫軸與大地坐標(biāo)系XOY橫軸的夾角在(k-1)至k時(shí)刻期間的增量，在此定義為“瞬態(tài)橫擺角”。

圖1 速度積分法原理示意

根據(jù)速度積分原理，在離散情況下車輛位置的軌跡序列為(xn,yn),n=0,1,2,…，可以表示為：

式中，T為兩次采樣的間隔，即(k-1)和k時(shí)刻的時(shí)間差。

T足夠小時(shí)，所計(jì)算的軌跡序列無(wú)限逼近真實(shí)軌跡，且車輛坐標(biāo)系橫軸與大地坐標(biāo)系XOY橫軸夾角的累計(jì)值為：

其中，定義φ0=0、x0=0、y0=0，則只要感知和檢測(cè)到車輛在采樣時(shí)刻k的縱向速度vk、橫向速度uk和瞬態(tài)橫擺角φk，即可重構(gòu)出車輛的運(yùn)動(dòng)軌跡。

2.2 差動(dòng)成像車輛運(yùn)動(dòng)參數(shù)檢測(cè)原理

傳統(tǒng)的速度積分法利用陀螺儀測(cè)量出汽車的角速度在汽車坐標(biāo)系上的分量，利用五輪儀或加速度計(jì)測(cè)量出汽車的速度和加速度在汽車坐標(biāo)系上的分量，經(jīng)過積分與變換重構(gòu)出汽車的質(zhì)心軌跡。本文通過車載設(shè)備的差動(dòng)成像提取車輛的縱向速度、橫向速度和瞬態(tài)橫擺角。

在此，差動(dòng)成像是指間隔一定時(shí)間，具有一定視場(chǎng)重合度的兩次成像工作模式，利用差動(dòng)成像提取車輛運(yùn)動(dòng)參數(shù)的原理如圖2所示。在車輛運(yùn)動(dòng)過程中，通過車載設(shè)備差動(dòng)成像對(duì)路面進(jìn)行拍攝，則第k次差動(dòng)成像會(huì)在Δ時(shí)間間隔連續(xù)獲取2幀圖像，即A幀圖像和B幀圖像，如圖2a所示；此時(shí)，路面特征點(diǎn)（如圖2中Ⅰ、Ⅱ、Ⅲ）將分別呈現(xiàn)在2幀圖像上，但其坐標(biāo)有所區(qū)別，如圖2b、圖2c所示。特征點(diǎn)的坐標(biāo)差別可以反映出車輛相對(duì)于路面的位移和橫擺運(yùn)動(dòng)，從而獲取車速和橫擺角速度。

圖2 差動(dòng)成像運(yùn)動(dòng)參數(shù)檢測(cè)原理

式中，D為兩坐標(biāo)系之間的平移距離向量[dxdy]T；dx、dy分別為橫向和縱向位移量；PA和PB分別為特征點(diǎn)在和中的坐標(biāo)矩陣為旋轉(zhuǎn)矩陣；β為兩幀圖像的相對(duì)旋轉(zhuǎn)角。

利用圖像識(shí)別技術(shù)獲取各特征點(diǎn)在兩幀圖像中的坐標(biāo)，求解式（3）中的D和β，經(jīng)過圖像距離與地面距離的尺度增益，可得k時(shí)刻的車輛運(yùn)動(dòng)參數(shù)：

式中，α=H/f為尺度增益；H為差動(dòng)成像設(shè)備與路面間的高度差；f為成像系統(tǒng)特性參數(shù)（可通過標(biāo)定試驗(yàn)獲得）；Δ為間隔時(shí)間。

3 差動(dòng)成像軌跡重構(gòu)的實(shí)現(xiàn)

3.1 時(shí)域差動(dòng)成像設(shè)計(jì)

時(shí)域差動(dòng)成像的核心是間隔時(shí)間Δ完成連續(xù)2幀圖像的采集，最簡(jiǎn)單的實(shí)現(xiàn)方法是采用單個(gè)攝像頭的連續(xù)2幀圖像，此時(shí)T=Δ。但車速較快時(shí)，由于單個(gè)攝像頭的幀率較低（25幀/s）或視場(chǎng)較小，導(dǎo)致相鄰2幀圖像間無(wú)重疊部分，無(wú)法通過該方法進(jìn)行檢測(cè)，而高幀率攝像頭的成本較高，且無(wú)法適應(yīng)高速情況或者速度變化較大的情況[12]。為了解決這一問題，本文設(shè)計(jì)了采用雙攝像頭的差動(dòng)成像方案，其基本原理如圖3所示。

圖3 雙攝像頭時(shí)域差動(dòng)成像原理

由圖3可以看出，攝像頭A和攝像頭B幀率雖均很低，一般T=40 ms，但可以通過控制2個(gè)攝像頭的啟動(dòng)時(shí)刻，使曝光幀F(xiàn)A,k和FB,k的時(shí)間間隔為Δ。

為此，設(shè)計(jì)時(shí)域差動(dòng)控制和光路結(jié)構(gòu)分別如圖4、圖5所示。圖4中，將2個(gè)攝像頭的獨(dú)立時(shí)鐘改為共用時(shí)鐘，以保證兩者的時(shí)鐘同步，同時(shí)采用單片機(jī)控制其啟動(dòng)時(shí)刻，實(shí)現(xiàn)兩者的差動(dòng)成像，差動(dòng)時(shí)間1～40 ms可調(diào)。圖5所示的雙攝像頭光路結(jié)構(gòu)，使用立方體分光棱鏡確保2個(gè)攝像頭的視場(chǎng)保持一致，避免檢測(cè)區(qū)域減小。

圖4 差動(dòng)控制示意

圖5 立方體分光棱鏡

3.2 圖像匹配方法

本文選用加速魯棒性特征（Speeded Up Robust Features，SURF）匹配算法[13-14]進(jìn)行圖像匹配。Hessian矩陣是SURF匹配算法的核心，圖像中的某一點(diǎn)是否為圖像的極值點(diǎn)可以使用Hessian矩陣及其判別式來(lái)判定，從而檢測(cè)出圖像中的特征點(diǎn)及其位置與尺度[16]。假設(shè)圖像中的某一點(diǎn)為I(U,V)，圖像尺度為σ，則該點(diǎn)的Hessian矩陣為：

則該點(diǎn)的Hessian矩陣的判別式為：

當(dāng)Hessian矩陣的判別式取得局部極大值或極小值時(shí)，判定當(dāng)前點(diǎn)較周圍鄰域內(nèi)其他點(diǎn)更亮或更暗，由此定位關(guān)鍵點(diǎn)，可以檢測(cè)出圖像在尺度σ下特征點(diǎn)的響應(yīng)值，遍歷所有像素，即可檢測(cè)出該尺度下所有的特征點(diǎn)。SURF匹配算法采用不斷增大濾波器模板尺寸求取Hessian矩陣響應(yīng)值，求取不同尺度下的特征點(diǎn)。

在檢測(cè)出特征點(diǎn)后，對(duì)其進(jìn)行特征向量描述。首先求得特征點(diǎn)的主方向，以y表示沿主方向，x表示垂直于主方向。為了描述特征點(diǎn)，首先以該點(diǎn)為中心，取20s×20s（s為該點(diǎn)所在圖像的尺度值）的圖像，將該圖像劃分為4×4共16個(gè)子塊，每個(gè)子塊內(nèi)有5s×5s個(gè)像素。對(duì)每個(gè)子塊進(jìn)行Haar小波響應(yīng)值計(jì)算，分別得到dy、dx及對(duì)應(yīng)的絕對(duì)值|dy|和|dx|。

對(duì)需要匹配的圖像進(jìn)行特征向量匹配，以特征向量間的歐幾里得度量作為相似性度量的結(jié)果，采用基于歐幾里得度量的最鄰比較法來(lái)實(shí)現(xiàn)。將圖像A中任意一個(gè)特征點(diǎn)與圖像B中歐幾里得度量最小的2個(gè)特征點(diǎn)進(jìn)行比較，當(dāng)度量最小的點(diǎn)小于度量次小的點(diǎn)一定比例時(shí)匹配成功。匹配得到的結(jié)果如圖6所示。

3.3 系統(tǒng)誤差校正

有2類系統(tǒng)誤差對(duì)軌跡的重構(gòu)產(chǎn)生較大影響，分別是成像光路帶來(lái)的圖像畸變和車輛承載狀態(tài)帶來(lái)的成像高度差變化。系統(tǒng)應(yīng)用中必須對(duì)其進(jìn)行校正。

圖6 SURF匹配結(jié)果

3.3.1 圖像畸變校正

圖像畸變校正的方法已經(jīng)非常成熟，本文采用網(wǎng)格模板法[15]進(jìn)行畸變校正。通過對(duì)標(biāo)準(zhǔn)網(wǎng)格模板進(jìn)行拍攝，獲取理想像素坐標(biāo)與相應(yīng)畸變像素坐標(biāo)的對(duì)應(yīng)關(guān)系：

式中，aij、bij為待定系數(shù)；n為多項(xiàng)式的次數(shù)。

利用曲面擬合方法擬合選取的控制點(diǎn)對(duì)，然后用最小二乘法求出待定系數(shù)aij與bij，就可以得到理想像素坐標(biāo)與畸變像素坐標(biāo)之間的映射關(guān)系，完成圖像畸變校正。

3.3.2 側(cè)傾與高度誤差校正

由于汽車自身的振動(dòng)、輪胎的壓力變化、路面的起伏變化等原因，運(yùn)動(dòng)過程中安裝在汽車上的差動(dòng)圖像試驗(yàn)平臺(tái)與地面的相對(duì)位置會(huì)發(fā)生變化，一般情況下其影響不大，但是車輛重載時(shí)誤差不可忽略。針對(duì)該問題，本文在試驗(yàn)平臺(tái)中加入激光發(fā)射器模塊，利用激光光斑與路面的圖像分析出激光發(fā)射器、差動(dòng)攝像頭、路面三者的相對(duì)位置關(guān)系，從而實(shí)現(xiàn)汽車側(cè)傾角和汽車垂直高度的實(shí)時(shí)檢測(cè)，其具體實(shí)現(xiàn)方法見文獻(xiàn)[16]。

4 試驗(yàn)與結(jié)果分析

為了檢驗(yàn)基于差動(dòng)圖像的汽車精細(xì)軌跡重構(gòu)方法在實(shí)車條件下的性能，本文設(shè)計(jì)了實(shí)車連續(xù)軌跡重構(gòu)試驗(yàn)。將差動(dòng)圖像試驗(yàn)平臺(tái)固定在汽車外部可以觀測(cè)到路面的位置，記錄汽車的起始位置和終點(diǎn)位置作為軌跡重構(gòu)的參考位置。試驗(yàn)過程中，差動(dòng)圖像試驗(yàn)平臺(tái)拍攝得到連續(xù)的路面圖像，同時(shí)使用GPS與慣性傳感器進(jìn)行測(cè)量，利用測(cè)得的數(shù)據(jù)進(jìn)行軌跡重構(gòu)，得到的試驗(yàn)結(jié)果如圖7所示。

從圖7可以看出，汽車進(jìn)行前進(jìn)、轉(zhuǎn)彎、倒車等運(yùn)動(dòng)時(shí)，差動(dòng)圖像試驗(yàn)平臺(tái)能夠比較精確地匹配運(yùn)動(dòng)軌跡起點(diǎn)與終點(diǎn)的實(shí)際位置，誤差較小。本方法試驗(yàn)平臺(tái)重構(gòu)的軌跡與GPS和慣性傳感器重構(gòu)的軌跡走勢(shì)一致，雖然無(wú)法獲取真實(shí)的運(yùn)動(dòng)軌跡，但本文提出的方法終點(diǎn)位置誤差小于GPS與慣性傳感器軌跡重構(gòu)的終點(diǎn)位置誤差，如表1所示。這一結(jié)果表明，基于差動(dòng)圖像的汽車精細(xì)軌跡重構(gòu)方法具有較高的精度。其中，3種方法的理論值存在差異的原因是試驗(yàn)時(shí)3種測(cè)量裝置在車輛上的安裝位置存在前后差異。GPS的數(shù)據(jù)誤差是由于試驗(yàn)現(xiàn)場(chǎng)的差分基站信號(hào)不穩(wěn)定造成的，因此其誤差遠(yuǎn)大于系統(tǒng)的標(biāo)稱誤差。

圖7 汽車連續(xù)軌跡重構(gòu)結(jié)果

表1 軌跡終點(diǎn)檢測(cè)結(jié)果 m

使用基于差動(dòng)圖像的汽車精細(xì)軌跡重構(gòu)方法一方面能夠重構(gòu)汽車的運(yùn)動(dòng)軌跡，另一方面根據(jù)速度積分法的特性，在計(jì)算過程中能夠得到各時(shí)刻相對(duì)于大地坐標(biāo)系的橫擺角，因此本方法能夠?qū)⑵囘\(yùn)動(dòng)過程中車身姿態(tài)的精細(xì)變化進(jìn)行重現(xiàn)。試驗(yàn)結(jié)果如圖8所示，這一結(jié)果可以有效反映車輛運(yùn)動(dòng)中所占用的空間，對(duì)于車輛的運(yùn)動(dòng)控制和軌跡規(guī)劃起到重要的反饋?zhàn)饔谩?/p>

圖8 汽車精細(xì)軌跡重構(gòu)結(jié)果

5 結(jié)束語(yǔ)

本文提出的基于時(shí)域差動(dòng)成像的汽車精細(xì)軌跡重構(gòu)方法，通過雙攝像頭的可控延時(shí)啟動(dòng)實(shí)現(xiàn)了時(shí)域差動(dòng)成像，解決了低成本下的毫秒級(jí)差動(dòng)控制問題，克服了車輛運(yùn)動(dòng)帶來(lái)的視場(chǎng)不足現(xiàn)象，并通過圖像匹配技術(shù)計(jì)算出汽車運(yùn)動(dòng)參數(shù)，使用速度積分法實(shí)現(xiàn)了汽車精細(xì)運(yùn)動(dòng)軌跡的重構(gòu)，所得軌跡顯著優(yōu)于GPS和慣導(dǎo)系統(tǒng)。同時(shí)，所研究的軌跡重構(gòu)方法還能有效檢測(cè)到運(yùn)動(dòng)過程中車體的姿態(tài)和空間占用情況，為多種車輛運(yùn)動(dòng)控制提供狀態(tài)反饋。