基于雙目視覺的工程車輛定位與行駛速度檢測

同志學 趙 濤 賀利樂 王消為

西安建筑科技大學機電工程學院，西安，710055

0 引言

工程車輛作業時的實際行駛速度及相對于作業環境的位置信息對車輛控制至關重要，如何快速檢測這些運動參數是值得研究的。傳統的車速測量一般是通過檢測驅動輪的轉速及車輪半徑而獲得的，但當出現滑轉時所測的速度已不是實際車速[1]。目前對實際車速的測量通常采用五輪儀測速、線圈測速、雷達測速等方法[2]，但這些方法也有諸多不足，例如：五輪儀的低速性能及路面適應性較差；線圈測速位置固定，使用不靈活；雷達測速易受干擾且不適合低速測量。工程車輛的行駛速度較低，工作環境多樣，滑轉率較高，上述方法難以滿足其實際速度測量的精度要求。近年來，視覺測量技術由于具有非接觸性和被動性的優點[3]而在運動參數測量方面得到了廣泛應用。目前，車載視覺測速系統一般是在車輛上安裝單目視覺傳感器，通過采集車輛行駛時的地面序列圖像，根據序列圖像的匹配進而計算出車輛的行駛速度[4]，但該方法無法確定車輛相對于環境的位置。

雙目立體視覺可以有效地確定物體的位置信息，因此，能通過雙目序列圖像同時獲取工程車輛的相對位置與實際速度。本文將雙目視覺系統側向安裝在工程車輛上，連續采集車輛行駛時的周圍環境圖像；并根據各幀左右圖像的立體匹配結果，計算出環境特征點的景深，從而確定車輛相對于工作環境特征點的位置；同時對相鄰兩幀圖像進行特征跟蹤匹配，根據匹配特征點的三維坐標估算車輛的運動參數，再利用L-M (Levenberg-Marquardt)法進行優化，最終計算出車輛的行駛速度。

1 基于雙目視覺的測距定位與速度估計原理

光軸平行的雙目相機模型求解過程較為簡單，在實際應用中甚廣，因此，本文也采用此類相機。攝像機坐標系以其瞬時成像點為原點，XC軸與車輛瞬時運動方向相同，YC軸垂直于地面；ZC軸指向環境特征點。

雙目視覺測距的原理就是通過圖像匹配與三角測量原理計算物體在左右圖像上的成像偏差，以此獲取物體景深。首先對各幀雙目圖像進行立體匹配以獲取視差，再結合雙目視覺系統的標定參數從而計算出環境特征點在左攝像機坐標系下的三維坐標，其中Z坐標值就是車輛到這些環境特征點的距離，從而確定了車輛在環境中的相對位置。

基于視覺的速度估計一般都是通過序列圖像的特征跟蹤匹配進行的，圖1為本文所用自車速度估計模型的示意圖。車輛在行駛過程中可以視為剛體運動，剛體的運動參數可由旋轉矩陣R和平移矢量T描述；而攝像機固定在車輛上，隨車一起運動，可以認為攝像機坐標系與車體坐標系重合，則可用相鄰兩幀間攝像機坐標系的變換參數來描述車輛的運動參數。假設在車輛運動前后的相鄰兩幀圖像中，對應的特征匹配點對集合Q={(iPtk、iPtk+1)，i=1,2,…,N}，其中，iPtk、iPtk+1分別代表在tk時刻和tk+1時刻，第i組匹配點對在左攝像機坐標系下的三維坐標，則它們有如下關系：

iPtk+1=R(iPtk)T+T

(1)

式中，R、T為相鄰兩幀之間左攝像機坐標系變換的正交旋轉矩陣和平移矢量，表示車輛在tk+1時刻相對于tk時刻的位姿變化。

圖1 車輛自車速度估計模型示意圖Fig.1 Schematic diagram of ego-velocity estimation for vehicle

根據行駛路面的實際情況，車輛在相鄰兩幀之間的運動可以進一步視為平面剛體運動。因此，旋轉矩陣R和平移矢量T可簡化為

(2)

T=(tX，0，tZ)T

(3)

式中，θ為車輛的偏轉角度；tX、tZ為平移分量。

求解出旋轉矩陣R和平移矢量T后，就可以計算出車輛從tk時刻到tk+1時刻的平移速度v和偏轉角速度ω：

v=‖T‖/Δt

(4)

ω=|θ|/Δt

(5)

由于圖像采集時間間隔Δt非常小，故計算出的速度可以當作車輛的瞬時速度。根據上述原理，基于雙目視覺的車輛測距定位與自車速度估計的詳細流程如圖2所示。

圖2 基于雙目視覺的車輛定位與自車速度估計方法流程圖Fig.2 Flow chart of localization and ego-velocity estimation for vehicle based on binocular vision

2 立體匹配與特征跟蹤匹配

在進行圖像匹配之前，首先對圖像進行校正、濾波等預處理。校正的目的是使圖像的外極線與掃描線重合，以便立體匹配時減小搜索范圍。對校正后的圖像再進行高斯濾波，以去除圖像中的高斯噪聲。

車輛在運動過程中，攝像機采集到的圖像會發生旋轉、縮放、光照強度等方面的變化，而SURF(speed up robust features)算法與SIFT(scale invariant feature transform)算法一樣，對圖像旋轉、縮放和光照強度等具有魯棒性，且它的速度比SIFT算法快很多[5-7]，所以可采用SURF算法來提取特征。

提取SURF特征后，以歐氏距離作為匹配相似性度量函數，采用最近鄰/次近鄰比值法，分別以左右圖像為基準進行雙向匹配，滿足左右一致性才能判斷為正確匹配點。經過極線校正后的對應特征點基本處于同一行，那么每個特征點的實際搜索范圍可限制在以同一行為中心的上下3行。同時,為降低誤匹配率，采用簡化的RANSAC (random sample consensus)算法[8]對得到的匹配點對進行過濾。完成立體匹配后，獲取視差圖并對其進行濾波處理，最后結合攝像機標定參數即可計算出這些匹配特征點的三維坐標。

特征跟蹤匹配的目的是建立車輛運動前后左圖像特征點間的對應關系。當車輛運動到tk+1時刻獲取雙目圖像后，完成立體匹配的同時將左圖像與運動前tk時刻的左圖像進行特征跟蹤匹配。由于本文為估算車輛的實時速度，圖像采樣間隔較小，前后兩幀之間有較大重合度，故可以采用前述立體匹配方法進行特征跟蹤匹配。為了減少誤匹配，采用以下方法。

(6)

3 車輛運動參數估計

經過圖像立體匹配和特征跟蹤匹配之后，得到車輛運動前后N組匹配特征點對的坐標集合Q，通過這些坐標就可計算出式(1)中的正交旋轉矩陣R和平移矢量T。考慮到距離車輛較遠的特征點對車輛的偏轉運動是敏感的，車輛小角度的偏轉將使遠距特征點在圖像中的位置發生較大的變化；距離車輛較近的特征點對車輛的平移是敏感的，車輛的平移運動會使近距特征點的位置發生較大變化[9]。那么利用遠距特征點計算旋轉矩陣，近距特征點計算平移矢量，在一定程度上就降低了車輛運動估計的誤差。在前后兩幀圖像匹配的特征點對集合Q中，選擇Nf組景深(Z坐標值)較大的匹配點對構成遠距點對集合Qf，Nn組景深較小的匹配點對構成近距點對集合Qn，Nf、Nn的值不能太小，否則運動參數估計誤差較大，但一般也不要大于N/2，要根據實際場景選擇合適的值。

3.1 運動參數初步估計

對于旋轉矩陣R的求解方法有奇異值分解(SVD)法、正交分解(OD)法和單位四元數分解法[10]。其中，單位四元數分解法穩定性好，能夠得到很好的數值結果，旋轉矩陣R與單位四元q=[q0,q1,q2,q3]之間的關系為

R=

(7)

從Q中隨機選取4組非共線的匹配點對，依據文獻[11]中的Horn單位四元數法估算出旋轉矩陣的初值Re，并代入式(1)求出Τe。由于誤匹配點對的存在，計算出的旋轉矩陣誤差較大，一般采用RANSAC法進行精確求解，步驟如下：

(1)利用單位四元數法估算M={Re、Te}，并將Q中剩余的匹配點對中代入式(1)，設置一個誤差限e，若各匹配點對的誤差滿足：

ei=‖iptk+1-(Re(iptk)T+Te)‖2≤e

(8)

則認為這對匹配點對為內點，否則為外點。

(2)多次重復步驟(1)，并選擇內點數最多的一組重新計算M。

可以看出RANSAC法的計算量較大。本文采用遠距匹配點對和近距匹配點對分別估計旋轉矩陣和平移矢量，在一定程度上已經降低了估計誤差；再考慮到本文模型中旋轉矩陣R和平移矢量T的特點：①detR接近1；②T中的第2個元素t2接近于0，且滿足‖T‖≤vmaxΔt，最大車速vmax與Δt都已知。那么根據先驗信息①對R進行篩選，就可以避免利用錯誤的R去統計正確匹配點對，減少了整體計算量。具體步驟如下。

(a)從遠距點對集合Qf中隨機選取4組非共線的匹配點對，利用單位四元數法估算Re，設置閾值σ，若滿足σ≤detRe，則認為Re是候選旋轉矩陣。

(b)多次重復步驟(a)，可得到多組候選旋轉矩陣Re(j)。

(c)利用候選旋轉矩陣將近距點對集合Qn中的匹配點對代入式(1)，每個Re(j)都會得到Nn個的平移矢量Te，根據先驗信息②對這Nn個Te進行排除，得到k個滿足條件的Te。則每個Re(j)對應的最佳平移矢量為

(9)

(d)將每組對應的Re(j)、Te(j)代回原匹配點對集合Q中，根據式(1)計算正確匹配點對的數目，選擇正確匹配點對數目最多的一組{Re(initial),Te(initial)}作為初步估計的最終值。

3.2 運動參數優化估計

用單位四元數表示旋轉矩陣R如式(7)所示，其中，q0+q1+q2+q3=1，則R共有3個獨立參數(q1,q2,q3)，加上平移矢量T三個參數，則運動參數的待優化量

α=[q1q2q3txtytz]T

(10)

然后利用以下目標函數求解滿足其最小值的α：

(11)

令h(α)=R(iptk)T+T，則式(11)可以表示為

f(α)=(iptk+1-h(α))T·(iptk+1-h(α))

(12)

利用Levenberg-Marquardt(L-M)算法求解式(12)，迭代計算過程如下[12]：

(1)以上文初步估計得到的{Re(initial),Te(initial)}作為優化初值α；

(3)?α=(JTJ+λI)-1JTr，則αnew=α+?α；

(4)若f(αnew)

參數λ、β、ε為經驗值，應根據實際場景和精度要求選取。本文中λ、β、ε的取值分別為0.01，10，0.000 001。在獲得優化后的旋轉矩陣和平移矢量后，利用式(2)和式(3)就可求得θ、tX、tZ。

4 試驗

為了驗證所提方法的有效性，進行模擬試驗。圖3所示為試驗裝置，AGV小車上安裝有Bumblebee2雙目視覺系統，相機分辨率為648像素×488像素，幀頻為每秒48幀。試驗環境為室外水泥路面，光照強度適中(多云天氣)；試驗中小車沿“直線-圓弧-直線”路線行駛，各階段以給定速度大小(0.4 m/s、0.2 m/s、0.4 m/s)勻速前進；攝像機每隔0.25 s采集一幀道路旁的場景雙目圖像。

圖3 試驗裝置Fig.3 Test equipment

首先對采集到的各幀雙目圖像進行極線校正、高斯濾波處理。然后對預處理后的左右圖像進行立體匹配，并根據匹配結果計算環境特征點的三維坐標。圖4所示為某時刻的立體匹配結果，該時刻立體匹配后獲得的部分環境特征點三維坐標見表1，其中，Z坐標值就是小車到該環境特征點的距離。

圖4 某時刻立體匹配結果Fig.4 Image matching results at a certain moment

特征點編號tk時刻特征點坐標(X,Y,Z)1(22.226,-30.932,766.384)2(12.908,-25.373,683.549)3(31.918,28.246,671.893)4(11.790,12.766,769.812)5(15.595,-26.373,781.284)6(19.429,18.277,656.984)

對相鄰兩幀左圖像進行特征跟蹤匹配，并獲取這些匹配特征點對的三維坐標，用以估計小車運動參數。圖5所示為某相鄰兩幀間的特征跟蹤匹配結果。

圖5 某相鄰兩幀左圖像特征跟蹤匹配結果Fig.5 The results of image matching and tracking for two adjacent left frames

為了驗證本文所用基于遠距點和近距點的運動參數估計算法的效率和準確率，利用小車直線行駛階段相鄰兩幀匹配點對的三維坐標，分別使用RANSAC法和本文方法估計旋轉矩陣和平移矢量。試驗所用計算機配置為AMD E-475 2.3GHz CPU和4.0GB內存；用于運動估計的匹配點對總數一致，共進行10次計算，每次重復采樣100次。統計每次計算所用時間，以及X、Z方向的運動估計誤差的平方和。結果如圖6所示，可以看出RANSAC法用時約25 ms，本文方法用時約15 ms，相比縮短約40%；與RANSAC法相比，本文方法具有更高的精度，且誤差波動較小，在一定程度上提高了算法的魯棒性。

(a)計算用時

(b)誤差圖6 兩種方法計算用時及誤差Fig.6 Calculation time and error of two methods

計算出各時刻運動參數的預估值后，利用L-M法進行優化求解；最后根據優化值，利用式(4)和式(5)估計出小車在各時刻的平移速度和偏轉角速度，并與設定的理論速度大小進行比較，結果如圖7所示。可以看出本文方法具有較高的估計精度，平均誤差均在6%以內。

(a)平移速度曲線

(b)偏轉角速度曲線圖7 車輛速度曲線Fig.7 Vehicle velocity profile

5 結論

本文提出了一種基于雙目視覺的車輛定位與自車速度檢測方法，同步獲取車輛相對于周圍環境的位置與速度信息。模擬試驗結果表明，該方法具有可行性，能夠在一定環境條件下有效地獲取運動車輛的相對位置與速度，并且根據特征點分類策略的運動參數估計方法有效地減少了計算量。今后可以進行不同環境條件下的模擬試驗，進一步驗證該方法的穩定性。

[1] 金守峰，胡永彪，田明銳. 基于序列圖像的車載測速方法與實驗[J]. 合肥工業大學學報(自然科學版)，2013，36(3): 271-272.

JIN Shoufeng，HU Yongbiao，TIAN Mingrui. Vehicle-borne Speed Measurement Method and Experiment Based on Image Sequences [J].Journal of Hefei University of Technology，2013，36(3): 271-272.

[2] 金守峰. 基于機器視覺的工程機械行走速度測量系統研究[D]. 西安：長安大學，2015:8-13.

JIN Shoufeng. A Research on Engineering Machine Speed Detection System Based on Machine Vision [D]. Xi’an：Chang’an University，2015:8-13.

[3] YAMAGUCHI K, KATO T, NINOMIYA Y. Vehicle Ego-motion Estimation and Moving Object Detection Using a Monocular Camera [C]// Proceedings of the International Conference on Pattern Recognition.IEEE，2006:610-613.

[4] 劉紅亮，陳維義，許中勝. 基于序列圖像匹配的車載自主測速方法[J]. 系統工程與電子技術，2015，37(4): 964-965.

LIU Hongliang， CHEN Weiyi，XU Zhongsheng. Research on Vehicle-borne Autonomous Velocity Measurement Method Based on Sequence Image Matching [J]. Systems Engineer and Electronics，2015，37(4): 964-965.

[5] 紀利娥，楊風暴，王志社，等. 基于邊緣與SURF算子的SAR與可見光圖像配準方法[J]. 應用光學，2013，34(5): 811-812.

JI Li’e，YANG Fengbao，WANG Zhishe，et al. SAR and Visible Image Registration Method Based on Edge and SURF Algorithm [J]. Journal of Applied Optics，2013，34(5): 811-812.

[6] BAY H， TUYTELAARS T， VAN GOOL L. SURF: Speeded Up Robust Features [C]//Proceedings of European Conference on Computer Vision. Graz,2006: 404-407.

[7] 化春鍵，陳瑩. 高精度尺度不變特征點匹配方法及其應用[J]. 中國機械工程，2014，25(12): 1655-1656.

HUA Chunjian，CHEN Ying. Precise Scale Invariant Feature Matching and Its Application[J]. China Mechanical Engineering，2014，25(12): 1655-1656.

[8] 趙燁，蔣建國，洪日昌. 基于RANSAC 的SIFT 匹配優化[J]. 光電工程，2014，41(8): 60-61.

ZHAO Ye，JIANG Jianguo，HONG Richang. An Optimized SIFT Matching Based on RANSAC [J]. Opto- Electronic Engineering，2014，41(8): 60-61.

[9] 尹俊，董利達，遲天陽. 基于特征點分類策略的移動機器人運動估計[J].計算機應用，2015，35(2): 590-594.

YIN Jun，DONG Lida，CHI Tianyang. Mobile Robot Motion Estimation Based on Classified Feature Points [J].Journal of Computer Applications，2015，35(2): 590-594.

[10] 代俁西，沈秋，王惠南，等. 雙目視覺中基于對偶四元數的三維運動估計[J]. 光學技術，2015，41(2):132-135.

DAI Yuxi，SHEN Qiu,WANG Huinan,et al. 3D Motion Estimation of Binocular Vision Based on Dual Quaternion [J]. Optical Technique，2015，41(2):132-135.

[11] 曹鳳萍，王榮本. 基于立體視覺的月球車運動估計算法[J]. 吉林大學學報(工學版)，2011，41(6): 1593-1595.

CAO Fengping，WANG Rongben. Stereovision Based Ego-motion Estimation Algorithm for Lunar Rover [J]. Journal of Jilin University (Engineering and Technology Edition) ，2011，41(6): 1593-1595.

[12] 許允喜，蔣云良，陳方. 基于2D-3D雙目運動估計的立體視覺定位算法[J].光電工程，2011，38(9): 106-107.

XUN Yunxi，JIANG Yunliang，CHEN Fang. Stereo Visual Localization Algorithm Based on 2D-3D Binocular Motion Estimation [J]. Opto-Electronic Engineering，2011，38(9): 106-107.