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

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

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

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

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

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

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

針對上述問題，提出了一種結(jié)合車道線檢測的位姿計算方法。通過檢測車道線對該區(qū)域內(nèi)的像素點進行幀間匹配，增加參與位姿計算的有效特征點數(shù)量，從而提高位姿計算的準(zhǔn)確性。通過在Kitti數(shù)據(jù)上的場景實驗證明所提出的位姿計算改進算法在近景特征不足的場景中是有效的，能夠?qū)崿F(xiàn)對智能車輛的準(zhǔn)確定位。

1 位姿估計方法

1.1 算法框架

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

圖1 算法整體框架

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

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

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

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

1.2 車道線檢測及匹配

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

圖2 車道線檢測算法流程

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

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

(3)根據(jù)車道線的幾何結(jié)構(gòu)特性對候選線進行左右車道線分類和單側(cè)車道線分割，得到車道線檢測結(jié)果。

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

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

1.2.1 消失點檢測

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

圖3 候選線檢測

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

1.2.2 自適應(yīng)ROI區(qū)域選擇

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

圖4 ROI區(qū)域選擇

1.2.3 車道線分割及匹配

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

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

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

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

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

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

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

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

圖6 車道線檢測結(jié)果

圖7 車道線區(qū)域點匹配結(jié)果

1.3 位姿計算

1.3.1 地面深度值求解

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

(1)

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

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

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

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

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

(2)

1.3.2 PnP位姿求解

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

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

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

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

(3)

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

(2)相機坐標(biāo)系與世界坐標(biāo)系。相機坐標(biāo)系到世界坐標(biāo)系的轉(zhuǎn)換關(guān)系如式(4)所示：

(4)

(5)

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

2 實驗

2.1 數(shù)據(jù)集

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

2.2 算法結(jié)果比較

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

圖9 相機軌跡計算結(jié)果比較

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

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

表1 絕對軌跡誤差

表2 相對位姿誤差

3 結(jié)論

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