基于立體視覺的交叉路口對向車輛運動狀態估計*

舒方林 張海波 曹文冠

（上海工程技術大學機械與汽車工程學院 上海 201600）

1 引言

高級輔助駕駛系統目的是在復雜的架勢環境中為駕駛員提供精確的路面信息，從而預先讓駕駛員能夠在最短時間內察覺到潛在的危險。快速、準確地檢測出車輛前方運動目標位置以及它們相對于自身車輛的運動狀態，是此類系統的重要基礎之一。以往許多有關于此類系統中車輛的檢測與跟蹤大多采用毫米波雷達、激光雷達等傳感器，但鑒于立體視覺傳感器具有非接觸性、抗干擾以及被動性的優點，立體視覺傳感器逐漸被應用到高級輔助駕駛系統中，例如障礙物檢測、車道線識別和交通標志識別［1～5］等。國內外也有許多學者提出了許多基于立體視覺的運動目標運動狀態估計方法。文獻［6］是最早解決這個問題的著作之一，文獻中Young 等利用運動模型的擴展卡爾曼濾波器通過對一組點的三維位置的噪聲觀測，估計出這些點的運動參數。文獻［7］中研究了利用標定的立體視覺裝置計算剛體運動的三維路徑問題，在立體視圖中對運動目標的特征點進行檢測與跟蹤，并利用相似三角形原理計算特征點的三維位置。文獻［8］通過立體視覺估計迎面而來車輛的運動，使用擴展卡爾曼濾波器跟蹤車輛特征點云，進而估計出目標車輛的運動狀態，包括車輛的速度、加速度以及偏航率。文獻［9］提出了一種利用立體視覺估計月球車運動狀態的算法，該算法提取圖像的SIFT 特征點并建立相應幀間的對應關系，由此估計月球車的運動參數。文獻［10］對車載雙目視覺傳感器采集的雙目序列圖像進行基于SURF 特征的跟蹤匹配實現實時測距及自車速度的估計。以上利用立體視覺對目標運動參數的估計方法都依賴于目標的特征點，但隨著立體視覺傳感器視角的改變以及目標的自身遮擋，目標特征點的穩定性顯著降低，當目標在三維空間中經歷較大的旋轉和平移時，特征點跟蹤的精度就會受到較大影響。因此，本研究針對此問題基于非結構化道路交叉口交通場景，提出一種動態參考點方法估計目標車輛的速度，并提出基于車輛運動軌跡的航向角估計方法。

2 車輛姿態估計

2.1 參數表示

目標車輛采用特征點云以及外包長方體表示，如圖1 所示。在立體視覺系統坐標系Oxyz下，t時刻目標車輛參數可以表示為Bt=(Xt，vt，Dt)，其中Xt=(xreft，yreft，zreft，φt) 為 目 標 車 輛 位 姿，(xreft，yreft，zreft)表示參考點坐標，φt為目標車輛的航向角即其運動方向與Z軸的夾角；vt=(vxt，vyt，vzt)表示目標車輛分別沿x、y、z軸的速度，假設車輛沿高度軸方向沒有運動，則vt=(vxt，0，vzt)；Dt=(lt，wt，ht)為目標車輛外包長方體模型，其中lt、wt和ht分別為t時刻測量的外包長方體的長、寬、高。

圖1 目標車輛及參數表示示意圖

2.2 參數估計

2.2.1 基于動態參考點的目標車輛速度估計

動態參考點模型基本原理［11～12］：特征點云分布特性以致于無法利用外包幾何體中心作為參考點，但只要目標車輛在自身車輛可觀測范圍內，都可以找到一個外包長方體體角點作為參考點，并且隨著目標車輛位置的改變該參考點可以動態更新。

將非結構化道路交叉口對向車輛運動模式分為三種：直行、左轉以及右轉，基于此原理針對對向車輛每種運動模式選取不同的參考點。如圖2 和圖3 所示，當對向目標車輛直行通過交叉口時，由于車輛自遮擋導致側部以及尾部數據丟失，立體視覺系統只能準確獲取車輛外包長方體的寬度和高度參數；當對向目標車輛左轉或者右轉通過交叉口時，由于車輛側部的數據部分丟失立體視覺系統只能準確獲取車輛外包長方體的高度參數，而不能精確獲取長度和寬度參數；且目標車輛無論是右轉還是左轉，觀測車輛都存在傳感器視場盲區，此種情況下立體視覺系統智能獲得目標車輛的尾部數據。因此，動態參考點選擇遵循以下原則。

圖2 動態參考點選擇示意圖

圖3 動態參考點選擇鳥瞰圖

1）初始化參考點：當目標車輛直行通過交叉口時，選擇車輛外包長方體上角點作為參考點(xreft，yreft，zreft)，即圖2 中的A3角點；當t時刻目標車輛左轉或右轉通過交叉口時，選擇目標車輛外包長方體與自車最鄰近上角點作為參考點(xreft，yreft，zreft)，分別如圖2中的A9、A10角點。

2）參考點保持：當目標車輛不改變方向直行通過交叉口時，則參考點(xreft，yreft，zreft)仍為上一時刻車輛外包長方體角點。

3）參考點更新：當目標車輛先直行再左轉進入左視場盲區通過交叉口時，參考點先由A3切換至A9，進入左視場盲區后再從A9切換至A11；同理當目標車輛先直行再右轉進入右視場盲區通過交叉口時，參考點先由A3切換至A10，進入右視場盲區后再從A10切換至A12。

因此，對參考點前后時刻位置進行差分可求解目標車輛的速度：

式中T為前后時間間隔。

2.2.2 航向角估計

在復雜的道路交通環境中，車輛經常出現換道、轉向等行為，這要求智能車必須能夠準確、快速地識別其行駛航向角。基于車輛的軸對稱性，目標車輛的航向角可以表示為其行駛方向與立體視覺系統坐標系Z軸的夾角，如圖4 所示。

圖4 目標車輛運動軌跡及航向角示意圖

在短時間內（幾個采用周期或幾十個采樣周期），對傳感器測量數據進行逐段擬合，目標車輛的運動采用如下二次曲線表示：

二次曲線的參數a、b、c可由最小二乘估計法［13］求出，通過求導原則可以得到二次曲線的斜率：

曲線的斜率表示其切線方向，也即目標車輛的瞬時運動方向或相反方向，對曲線斜率求反正切便可以得到曲線切線方向與Z軸的夾角，即目標車輛相對于觀測車輛的航向角：

3 車輛跟蹤

應用于車輛跟蹤的高級運動模型包括恒定速度模型、恒定加速度模型、恒定轉率和速度模型以及恒定轉率和加速度模型等。恒定速度模型和恒定加速度模型均為線性運動模型，實際的車輛跟蹤顯然不能使用簡單的線性模型建模。基于交叉口車輛運動特性，本研究采用恒定轉率和速度模型［14］作為基本研究模型，以恒定轉率和速度模型構造擴展卡爾曼濾波對目標車輛運動狀態進行估計。

在恒定轉率和速度模型中，目標車輛的狀態向量表示為

其中，(z，x)表示目標車輛在車輛坐標系下的位置，v，φ，ω分別表示目標車輛的速度、偏航角以及偏航角速度。

當ω≠0 時，恒定轉率和速度模型的狀態轉移函數為

當ω=0 時，此時目標車輛變為直線行駛，因此，狀態轉移函數為

擴展卡爾曼濾波［15］是利用泰勒級數展開方法將非線性濾波問題轉化成近似的線性濾波問題，利用線性濾波的理論求解非線性濾波問題的次優濾波算法。該算法的原理是根據前一時刻對狀態的估計值和當前時刻的觀測值來獲得當前時刻的濾波值，因此，整個環路濾波過程可以分為狀態預測和狀態更新兩部分。

1）預測

在以恒定轉率和速度模型構造的擴展卡爾曼濾波系統中，系統的一步狀態預測方程可寫為

其中，g(xk，u)表示恒定轉率和速度模型的狀態轉移函數，u表示處理噪聲。

一步狀態預測協方差為

其中，JA是對狀態轉移函數中每個元素求偏導得到的雅可比矩陣，Q為處理噪聲的協方差矩陣。

2）更新

濾波增益：

式中，JH為傳感器測量矩陣的雅可比矩陣，R為傳感器測量噪聲協方差矩陣。

狀態估計方程：

式中，Zk為傳感器測量值，h(xk)為非線性映射函數。

狀態估計協方差：

式中，I為單位矩陣。

4 實驗結果與分析

我們在典型的鄉村交叉口交通場景對本研究提出的方法進行了試驗。主要實驗設備包括一臺目標車輛和一臺觀測車輛，其中觀測車主要搭載了一套立體視覺系統。立體相機的主要參數為基線16mm，視場角40°，圖像幀率30fps。目標車輛作為觀測車識別對象，實驗主要通過對目標車輛的速度以及航向角計算來驗證本研究所提出的方法。所有的實驗都是在Intel i7-8700、3.20GHz、12GB 內存的核心處理器上進行的。

1）速度估計實驗分析

為了更好地將目標車輛的真實運動速度與實驗估計速度進行對比，實驗目標車輛的運動模式為：目標車輛在距離交叉口12m 遠處以25km/h 的初始速度減速至15km/h，并以此速度左轉通過交叉路口。圖5 為實驗視頻序列中截取幀，如圖所示。在第20 幀中，目標車輛在距離觀測車28m 處被檢測到，此時車輛還處于直線運動階段；在第65幀中，雖然目標車輛的運動方向沒有明顯改變，但預測的車輛運動軌跡此時已經開始彎曲；在第82幀中，目標車輛正在左轉彎通過交叉口。

圖5 交叉口目標車輛跟蹤截取幀

圖6 為目標車輛實驗估計速度與真實速度曲線圖，將速度估計結果與目標車輛真實速度進行比較可以看出，估計出的速度與目標車輛的真實速度非常接近，誤差控制在4%以內。值得注意的是，在第100 幀時，目標車輛進入半視場盲區，此時參考點由A4角點切換至A8角點，但目標車輛的速度仍非常平穩，并未發生大的突變。

圖6 目標車輛速度估計結果

由以上實驗結果分析可知，針對非結構化交叉口道路，提出的動態參考點模型結合擴展卡爾曼濾波算法的速度估計方法在準確性和平穩性方面均能取得比較滿意的結果，可以較好地反映目標車輛的實際運動速度。

2）航向角估計實驗分析

航向角估計實驗目標車輛采用與速度估計實驗相同的運動模式。圖7 為預測的目標車輛運動軌跡鳥瞰圖，圖8為目標車航向角估計結果。

圖7 目標車輛運動軌跡鳥瞰圖

由圖8可知，目標車輛在第20幀被檢測到至第65 幀開始左轉，航向角估計值在15°上下微小波動（實驗中設定左轉航向角為正，右轉為負），目標車輛從第65 幀開始至左轉通過交叉路口，航向角估計值從15°逐漸增加至90°左轉通過交叉口，整個過程航向角變化與實驗設置的目標車輛運動過程基本相符。由上述分析可知，本研究提出的基于目標車輛運動軌跡的航向角估計方法能夠獲得比較理想的結果，可以較好地應用于交叉口車輛航向角估計。

圖8 目標車輛航向角估計結果

5 結語

本文主要研究了在非結構化交叉路口下，采用立體視覺估計對向車輛的運動狀態，包括車輛的速度和航向角。在速度估計中，針對采用車輛特征點作為參考點計算車輛速度方法容易受車輛自身遮擋以及觀測點位置的影響，提出了一種動態參考點模型估計目標車輛的速度；還提出一種基于車輛運動軌跡的方法計算目標車輛的航向角。實驗結果表明，目標車輛的速度估計誤差在±4%范圍以內，航向角估計也與目標車輛實際運動情況基本相符。多車輛相互遮擋情況下的運動狀態估計，將是我們未來的工作之一。