結合車道線檢測的智能車輛位姿估計方法

李琳輝，張溪桐，連 靜，周雅夫

(大連理工大學運載工程與力學學部汽車工程學院，大連 116024；大連理工大學工業裝備結構分析國家重點實驗室，大連 116024)

在智能駕駛領域，對智能車輛的實時定位是實現自主導航的重要前提，受到中外研究者的廣泛關注[1]。根據傳感器的不同，傳統的車輛定位方法可分為基于激光[2]、全球定位系統(GPS)[3]和輪式里程計[4]等定位方法。在GPS信號弱和道路狀況較差等復雜環境下，上述傳感器極易失效，導致定位精度下降。

近年來，Nister等[5]提出了視覺里程計這一概念。基于視覺里程計的定位方法通過安裝在車輛上的視覺傳感器采集環境圖像，利用這些圖像特征的運動約束求解相機位姿，實現對智能車輛的定位。由于視覺傳感器具有成本低和獲取環境信息豐富等優點，基于視覺里程計的定位方法逐漸成為智能車輛實時定位領域的研究熱點。

基于視覺傳感器的位姿計算方法可分為以SVO(semi-direct monocular visual odometry)[6]和LSD(large-scale direct monocular)[7]為代表的直接法，以及以ORB(object request broker)[8]和SOFT[9]為代表的特征點法。其中，基于特征點法的位姿計算方法受光照條件較差、相機大尺度旋轉和位移的影響較小，魯棒性強于直接法。基于特征點法的位姿估算法利用SURF(speeded-up robust features)[10]、SIFT(scale invariant feature transform)[11]和ORB[8]等算法對相機拍攝的圖像進行特征點提取，計算其描述子；然后根據前后兩幀中特征點的描述子距離進行匹配；最后基于這些匹配點對利用ICP(iterative closest point)、對極約束或PnP等算法求解兩幀之間的相對位姿。其中，ORB特征提取法具有良好的旋轉不變性，對噪聲不敏感，并且運算速度較快，適用于復雜的大型場景。

基于特征點的位姿計算依賴于對場景中特征點的提取，某些較遠處的特征點會因不在立體匹配有效范圍內而被排除，導致參與位姿計算的有效特征點數量不足，降低計算結果的準確性。對此，前人利用線特征[12]和角特征[13]與特征點共同參與位姿估計，但這些方法只適用于一些結構特征顯著的特殊場景，并且會使整體算法的實時性降低。

針對上述問題，提出了一種結合車道線檢測的位姿計算方法。通過檢測車道線對該區域內的像素點進行幀間匹配，增加參與位姿計算的有效特征點數量，從而提高位姿計算的準確性。通過在Kitti數據上的場景實驗證明所提出的位姿計算改進算法在近景特征不足的場景中是有效的，能夠實現對智能車輛的準確定位。

1 位姿估計方法

1.1 算法框架

總體算法框架如圖1所示。

圖1 算法整體框架

(1)對第k幀圖像左視圖和第k+1幀圖像左視圖進行ORB特征點檢測與匹配，得到前后幀匹配點對。

(2)對前后兩幀進行車道線檢測，并對車道線區域內的點進行幀間匹配，得到車道線匹配點對。

(3)對第k幀圖像的左右視圖進行立體匹配得到視差圖，利用視差圖獲取V視差圖。再根據V視差圖求解出地平面的視差方程，通過映射關系得到準確的地面視差，并更新車道線匹配點對的視差值。

(4)將ORB特征檢測得到的前后幀匹配點對與車道線檢測得到的匹配點對進行融合，利用PnP方法求解得到第k+1幀相機的位姿。

1.2 車道線檢測及匹配

提出的車道線檢測算法流程如圖2所示。

圖2 車道線檢測算法流程

(1)首先對輸入的原始圖像進行Canny邊緣檢測和自適應ROI區域選擇的預處理。ROI區域基于上一幀車道線檢測結果確定。

(2)對經過預處理的圖像進行霍夫變換，得到車道線候選線。

(3)根據車道線的幾何結構特性對候選線進行左右車道線分類和單側車道線分割，得到車道線檢測結果。

(4)對前后兩幀圖像車道線區域的點進行匹配，將匹配得到的結果與原匹配結果融合。

(5)根據匹配點對利用PnP方法求解出兩幀之間的位姿。

1.2.1 消失點檢測

對圖片進行預處理，包括二值化和邊緣檢測，結果如圖3(a)所示。對圖像進行霍夫變換，檢測出消失點以及車道線的候選線，結果如圖3(b)所示。

圖3 候選線檢測

車道線候選線中包含道路的結構信息，其匯聚點即為消失點。利用結構特征剔除掉其中接近平行的線，計算剩余候選線之間的交點；篩選出與其他交點平均歐式距離最短的點，將其作為這一幀的消失點結果，用于車道線篩選。

1.2.2 自適應ROI區域選擇

車載相機所采集到的圖像包含豐富的道路及場景信息，但是在車道線檢測任務中，只關注道路部分圖像的特征。考慮相機幀率，車輛前后兩幀行駛的距離較短，車道線在圖像上的位置變化不明顯，所以按照前一幀車道線的檢測結果，繪制平行四邊形區域作為下一幀的ROI，以減小其他信息對車道線檢測的干擾，如圖4所示。確定ROI區域后，只保留在該區域內的候選線，為后續篩選減少干擾。

圖4 ROI區域選擇

1.2.3 車道線分割及匹配

通過車道線在路面上的幾何結構性質篩選候選線，從而獲取準確的車道線方程。由于透視效應，該特征在車載相機所拍攝到的道路圖像中有所失真。因此，候選線在進行篩選前需要經過透視變換投影到俯瞰圖中。車道線篩選條件如下，其中條件(1)篩選是在原圖中進行的，條件(2)～條件(5)篩選是在俯瞰圖中進行的，示意圖如圖5所示。

圖5 俯瞰圖中車道線候選線篩選條件

(1)與消失點距離：車道線必然經過消失點，考慮檢測誤差，消失點與車道線距離在閾值范圍內。

(2)同側車道線角度差：左右側車道線均包含內外兩側的線，同一側車道線的內外側線相互平行，兩者角度差在閾值范圍內。

(3)異側車道線角度差：左右兩側的車道線互相平行，兩者角度差在閾值范圍內。

(4)同側車道線線寬：每條車道線線寬基本固定，即鳥瞰圖上同一側車道線的內外兩側線之間的距離在閾值范圍內。

(5)異側車道線間距：每條車道寬度基本固定，即鳥瞰圖上左右兩側車道線之間的距離在閾值范圍內。

通過以上條件篩選，可以獲得滿足條件的當前左右兩側車道線內外側線，共4條，如圖6所示。其中，綠色的兩條為左側車道線內外側線；藍色的兩條為右側車道線內外側線。在檢測出前后兩幀的車道線之后，針對車道線上的特征點，計算其描述子并進行匹配，匹配結果用于后續部分的位姿計算，匹配結果如圖7所示。

圖6 車道線檢測結果

圖7 車道線區域點匹配結果

1.3 位姿計算

1.3.1 地面深度值求解

設某點在世界坐標系下的坐標為Pw=[xw,yw,zw]T,在像素坐標系下的坐標為Pp=[u,v]T，其中u為行坐標，v為列坐標。在世界坐標系中，某一平面的數學模型表達式為yP=h,其中h表示水平高度。則該平面上所有點在雙目圖像中的視差值滿足式(1)：

(1)

式(1)中:ddisp為某點視差值;f為相機焦距;b為雙目相機基線長度;θ為道路平面與相機主光軸之間的夾角。

由此可見，屬于地面的像素點的視差值與其在圖像中的縱坐標成線性關系。根據這一關系，可以求出地面上像素縱坐標已知的任意一點的準確視差值。

對雙目視圖進行極限矯正，立體匹配后得到該幀圖像的視差圖，統計同一行中具有相同視差值點的個數得到V視差[13]。V視差圖中顯示的圖像信息包含三個維度：①x軸：表示0～255的不同視差值；②y軸：表示原視差圖的行坐標；③強度軸(圖像中表現為灰度值)：表示視差圖中該行中有相同視差值的像素點數量。圖8為某幀視差圖和其V視差圖獲取的結果。

圖8 通過V視差圖獲取地面點視差

經上述分析可知，圖像中的道路平在V視差圖中表現為一條斜率為負的直線。為求出該直線方程，對V視差圖進行霍夫變換，選擇長度最大的線(圖8中的紅色線)，擬合出地面視差方程ddisp=kv+b，其中k為斜率，b為截距。通過其映射關系可以得到地面上像素點較為準確的視差值，進而通過式(2)求出該點深度ddepth：

(2)

1.3.2 PnP位姿求解

將原始匹配點對與車道線上的匹配點對融合，用于位姿求解。選擇PnP[14]方法中的3D-2D模型求解相機位姿，該模型的求解需要獲取n對控制點，根據這些點的3D世界坐標和其在待求幀上的像素坐標求解出其映射關系，即為該幀的位姿。

相機成像過程一共涉及4個坐標系。根據其相互之間的轉換關系，利用已知的控制點在第k幀圖像中的像素坐標以及相機內參求解出其在相機坐標系中的坐標，再根據已知的第k幀相機位姿(根據第k-1幀和第k幀求得)求解出其在世界坐標系中的位姿，過程如下。

(1)像素坐標系與相機坐標系。設該幀中共有n個控制點，

Pp=[ui,vi]T分別表示第i個控制點在世界坐標系、相機坐標系、圖像坐標系以及像素坐標系中的坐標。從像素坐標系到世界坐標系的轉換關系如式(3)所示：

(3)

式(3)中：fx、fy、cx、cy為相機內參；(ui,vi)為圖像坐標；(xi,yi,zi)為相機坐標系坐標；zi為該點深度ddepth。若該點在地面上，則利用1.3.1節中的方法求解得到z；若該點處于其他平面，則直接從視差圖中讀取其視差值再利用式(2)求解得到zi。

(2)相機坐標系與世界坐標系。相機坐標系到世界坐標系的轉換關系如式(4)所示：

(4)

(5)

根據上述推導，可以計算出所有控制點的3D世界坐標，結合其第k+1幀的2D像素坐標，就可以利用PnP方法求解出當前幀的位姿。

2 實驗

2.1 數據集

選擇Kitti_odometry作為位姿計算的數據集。該數據集基于雙目相機拍攝，包含22個不同交通場景的數據。其中0～10序列的數據集配套提供了相機位姿的Groundtruth，便于實驗部分位姿計算結果的評估。

2.2 算法結果比較

圖9為提出的改進算法與原始ORB特征點檢測與匹配在Kitti_odometry數據集01序列場景中的位姿計算結果。該數據集于高速公路拍攝，車道線清晰，并且包含近景特征不足的場景，便于驗證提出改進算法的有效性。

圖9 相機軌跡計算結果比較

原始算法在計算過程中從某一幀開始出現了較大偏差，之后的軌跡算結果由于累計誤差的存在逐漸偏離真實軌跡，算法準確度較低。而提出的結合車道線的位姿計算方法在原始算法出現較大偏差的路段表現良好，與真實軌跡重合度較高。

使用絕對軌跡誤差和相對位姿誤差這兩種誤差作為 SLAM算法性能的評估標準。其中，絕對軌跡誤差為SLAM系統的估計值與相機位姿的真實值之間的誤差；相對位姿誤差(ATE)為相同時間間隔內估計位姿變化量與真實位姿變化量之間的誤差。分別計算這兩種誤差在250連續幀圖像中的誤差最大值、誤差平均值、誤差中值、誤差最小值和均方根誤差值以比較改進前后SLAM算法的性能，結果如表1、表2所示。對表中誤差數據分析可知，提出的算法都具有遠小于原始算法的計算誤差，軌跡計算平均誤差為5.65 cm，平均相對位姿誤差為0.003。結果證明在有效近景特征不足的情況下，提出的結合車道線檢測的相機位姿計算方法具有更高的有效性。

表1 絕對軌跡誤差

表2 相對位姿誤差

3 結論

設計了一套在有效近景特征不足的場景中智能車輛位姿估計的算法。提出了一種包含自適應感興趣區域選擇和基于車道幾何結構篩選候選線的車道線檢測方法。準確地檢測到了車輛當前所在車道左右兩側車道線的內外側線，為自動駕駛的車道保持任務提供更加精確的交通環境信息。同時，根據車道線的檢測結果，對車道線區域內的點進行幀間匹配，增加了參與PnP位姿計算的控制點數量。利用V視差圖求解出地面的視差方程，得到了地面上車道線像素點的準確深度。經實驗證明，提出的改進算法在近景特征不足的場景下能夠更加準確地估計出相機的位姿，具有較好的環境適應性，對智能車輛行駛過程中的定位問題具有重要意義。