無人駕駛車通過高速公路閘口區(qū)的方法研究

熊 瑩，毛雪松

（1.武漢科技大學(xué)計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，武漢 430065； 2.武漢科技大學(xué)，智能信息處理與實(shí)時(shí)工業(yè)系統(tǒng)湖北省重點(diǎn)實(shí)驗(yàn)室，武漢 430065； 3.武漢科技大學(xué)信息科學(xué)與技術(shù)學(xué)院／人工智能學(xué)院，武漢 430081）

前言

靜態(tài)環(huán)境下，面向無人駕駛、移動(dòng)機(jī)器人、無人機(jī)等各種應(yīng)用的路徑規(guī)劃問題已經(jīng)被廣泛研究，存在大量經(jīng)典算法和很好的應(yīng)用實(shí)例［1-3］，因此靜態(tài)環(huán)境中的無人駕駛已經(jīng)得到很好的解決。然而，實(shí)際道路場景中必然存在大量的動(dòng)態(tài)目標(biāo)，包括行人和車輛，相對速度范圍0～100 m／s左右，且動(dòng)態(tài)目標(biāo)速度方向具有很大的隨機(jī)性、瞬時(shí)變化性，無法準(zhǔn)確預(yù)測。雖然在動(dòng)態(tài)環(huán)境下，也出現(xiàn)很多文獻(xiàn)研究車輛的一些簡單操作，如結(jié)構(gòu)化道路上的車道線變更、合并車流、十字路口的直行、左轉(zhuǎn)、右轉(zhuǎn)［4-6］等，但是隨著道路環(huán)境的復(fù)雜性進(jìn)一步增加，這些路徑規(guī)劃方法在動(dòng)態(tài)環(huán)境下很容易達(dá)到性能極限。例如在寬闊非結(jié)構(gòu)化區(qū)域，車輛的行駛具有很大的隨機(jī)性，因而不能歸類到某一種標(biāo)準(zhǔn)操作中。本文中研究高速公路閘口區(qū)，即收費(fèi)窗口到匝道之間的無人駕駛路徑規(guī)劃與跟隨問題。該區(qū)域內(nèi)沒有車道線且存在多個(gè)收費(fèi)窗口，具有典型的寬闊、非結(jié)構(gòu)化特性。收費(fèi)窗口包括人工收費(fèi)窗口和ETC收費(fèi)窗口，因此車輛的初始速度可以為零，也可以為20 km／h左右。來自任一收費(fèi)窗口的車輛可以駛向任一匝道，而且無法預(yù)測駛向哪一條匝道。隨著車輛數(shù)量的增加，道路復(fù)雜性明顯增大，傳統(tǒng)路徑規(guī)劃算法無法實(shí)現(xiàn)無人駕駛車輛的安全行駛。

傳統(tǒng)路徑規(guī)劃方法大致分為基于地圖的方法、基于圖論的方法和基于障礙物自身信息的方法等類型。其中基于地圖的方法假設(shè)已經(jīng)掌握地圖信息，將地圖分割為小的柵格并在柵格中搜索最佳路徑，最著名的搜索算法有A*算法［7］、D*算法［8］、任意時(shí)刻動(dòng)態(tài)A*啟發(fā)式搜索算法［9］等，因?yàn)檫@些算法簡單、且能在覆蓋地圖的無障礙物區(qū)域內(nèi)找到最短的路徑。然而，這些算法在開始搜索前需要建立柵格地圖，柵格大小的選擇受環(huán)境變化的影響，這使得動(dòng)態(tài)環(huán)境下的路徑規(guī)劃問題非常復(fù)雜。而且，通過這些算法得到的路徑為1階連續(xù)曲線，車輛并不能實(shí)際跟隨這些路徑，在執(zhí)行路徑跟隨前需要對這些曲線平滑使其成為可跟隨路徑。平滑后的路徑通常偏離平滑前的路徑，使得平滑得到的可跟隨路徑可能成為非安全路徑。

第2類實(shí)現(xiàn)無人駕駛車輛路徑規(guī)劃的方法為基于圖論的方法，包括狀態(tài)柵格［10］、可視圖［11］、地圖知識(shí)［12］等。這些方法首先沿著公路創(chuàng)建節(jié)點(diǎn)，然后連接這些節(jié)點(diǎn)得到拓?fù)鋱D。隨著道路上障礙物數(shù)量的增加，節(jié)點(diǎn)的創(chuàng)建會(huì)變得非常困難。其中可視圖的方法在動(dòng)態(tài)環(huán)境下實(shí)際上是不可實(shí)現(xiàn)的，因?yàn)檎系K物對視線的遮擋，無法實(shí)現(xiàn)節(jié)點(diǎn)之間的連接。另外，狀態(tài)柵格需要綜合考慮車輛的方位、轉(zhuǎn)向、位置，將這些分量組合為一個(gè)狀態(tài)，因而對于寬闊非結(jié)構(gòu)化區(qū)域，節(jié)點(diǎn)的數(shù)量會(huì)變得異常龐大。

基于障礙物自身信息的典型路徑規(guī)劃方法包括最大邊界法［13］和基于人工勢場法［14］。這兩種方法都基于道路邊界和道路中障礙物的位置獲取最佳路徑。最大邊界法首先將障礙物分為左右兩類，然后使用支持向量機(jī)生成一條無碰撞路徑。人工勢場法為每一種障礙物定義一個(gè)勢場函數(shù)，然后沿著合成勢場梯度下降的方向計(jì)算出一條路徑。在寬闊非結(jié)構(gòu)化道路上，如果周圍沒有車輛等障礙物，則支持向量或勢場函數(shù)只能來自于道路邊界，這將使得連接道路起點(diǎn)和終點(diǎn)的所有路徑都收斂到道路的中心線。顯然，這些路徑不一定滿足長度最優(yōu)或曲率最優(yōu)原則，而且也不符合人類的駕駛習(xí)慣。

為解決在寬闊、非結(jié)構(gòu)化區(qū)域的無人駕駛問題，本文中提出決策樹模型，控制車輛安全行駛速度，并以高速公路閘口區(qū)為場景驗(yàn)證方法的有效性。首先使用多項(xiàng)式曲線為每個(gè)收費(fèi)窗口和每條匝道之間定義一條曲率最優(yōu)路徑，然后使用純追蹤法使車輛沿預(yù)定義路徑行駛，在車輛行駛過程中構(gòu)建決策樹，控制車輛的行駛速度，使車輛在無碰撞的情況下盡可能快地到達(dá)匝道。為了構(gòu)建決策樹，需要預(yù)測無人駕駛車輛與周圍車輛可能發(fā)生碰撞的位置，為此假定周圍車輛的位置、速度在任意時(shí)刻均已知。

1 道路地圖、路徑優(yōu)化與車輛模型

本文中選擇沈海高速上海段新橋收費(fèi)站到匝道之間的區(qū)域，研究無人駕駛的路徑規(guī)劃與跟隨，如圖1所示。由圖可見，共有6個(gè)收費(fèi)窗口，兩條匝道，其中一條匝道引導(dǎo)車輛駛向浦東機(jī)場，另一條匝道引導(dǎo)車輛駛向張家港。從6個(gè)收費(fèi)窗口中任意一個(gè)窗口駛出的車輛可以駛向任意一條匝道，整個(gè)收費(fèi)窗口區(qū)的寬度達(dá)到30 m左右，且收費(fèi)窗口到匝道之間沒有車道線引導(dǎo)，因此該區(qū)域可以看作寬闊非結(jié)構(gòu)化道路，在該部分道路中的無人駕駛非常復(fù)雜。由于車輛駛?cè)朐训篮蠹催M(jìn)入高速公路的結(jié)構(gòu)化部分，本文中僅研究從收費(fèi)窗口到匝道之間無人駕駛的路徑規(guī)劃與跟隨問題。

圖1 沈海高速新橋收費(fèi)站衛(wèi)星圖

為了對車輛與障礙物定位，定義一個(gè)全局坐標(biāo)系，坐標(biāo)原點(diǎn)固定于收費(fèi)窗口中間，如圖2所示。

圖2 駕駛場景與坐標(biāo)系統(tǒng)

車輛的位姿包括位置、方向和轉(zhuǎn)向角，因此定義車輛的狀態(tài)矢量為

式中：x和y為位置坐標(biāo)；d y／d x為車輛的方向；d2y／d x2為與轉(zhuǎn)向角有關(guān)的參量。圖中y軸上短實(shí)線分隔6個(gè)區(qū)域分別表示6個(gè)收費(fèi)窗口。假設(shè)車輛在收費(fèi)窗口和到達(dá)匝道處的方位和轉(zhuǎn)向角均為零，即

式中x0和xf分別為收費(fèi)窗口和匝道處的x軸坐標(biāo)。在這一約束下，可以得到一個(gè)5階多項(xiàng)式連接收費(fèi)窗口與匝道，作為無人駕駛車輛的參考路徑。

為優(yōu)化路徑（曲率最優(yōu)或長度最優(yōu)），可增加一個(gè)自由度，選擇6階多項(xiàng)式

將式（2）和式（3）代入式（4），則前6個(gè)系數(shù)a0～a5可用a6表示為

其中：

通常，高階系數(shù)a6是一個(gè)很小的量，因?yàn)榍€的主要形狀由低階系數(shù)來確定。按照路徑長度或曲率最優(yōu)來對參考路徑優(yōu)化，結(jié)果顯示參考路徑的差別不大，對車輛駕駛來說可以忽略。圖2給出了兩條連接收費(fèi)窗口到匝道的路徑，可作為使用純追蹤法仿真車輛行駛的參考路徑。

為完成車輛的方程，選擇自行車模型對車輛建模。

式中：θ為以全局坐標(biāo)系為參考系的車體方位；δ為以車體為參考系的轉(zhuǎn)向角；L為車輛前后輪之間的軸距。

式（6）中最后一個(gè)方程是冗余的，需要使用純追蹤法來更新車輛的轉(zhuǎn)向角，即根據(jù)當(dāng)前車輛的方位與轉(zhuǎn)向角計(jì)算下一時(shí)刻的轉(zhuǎn)向角。

為在動(dòng)態(tài)環(huán)境下得到一條車輛可跟隨的路徑，需要在無碰撞約束下求解動(dòng)態(tài)方程：

式中ut為t時(shí)刻車輛的狀態(tài)變量。文獻(xiàn)［15］中給出通過式（6）建模的車輛可以較準(zhǔn)確地跟隨由多項(xiàng)式表示的路徑。然而文獻(xiàn)中給出的防碰撞標(biāo)準(zhǔn)要求路徑不僅要避開障礙物當(dāng)前位置，還要避開將來所有可能有障礙物的位置。在這一標(biāo)準(zhǔn)下，無人駕駛車輛將總是等待周圍車輛全部通過后才重新出發(fā)，因此不能快速到達(dá)匝道。高速閘口區(qū)車輛行駛速度一般要求從0（人工收費(fèi)窗口）或者20（ETC收費(fèi)窗口）加速到60 km／h，過低的速度亦違反交通法規(guī)。

2 用于防碰撞的決策樹模型

根據(jù)人類的駕駛經(jīng)驗(yàn)，在車輛行駛速度低于60 km／h的情況下，車輛之間的安全距離要求近似等于車輛速度減去15，轉(zhuǎn)換為時(shí)間，則要求兩車通過同一點(diǎn)之間的時(shí)間間隔不低于3 s［16］。本文中根據(jù)這一人類駕駛經(jīng)驗(yàn)建立決策樹，實(shí)現(xiàn)車輛在高速公路閘口區(qū)的無碰撞駕駛。

2.1 問題描述

無人駕駛車輛沿著預(yù)先定義的路徑行駛，路徑數(shù)據(jù)可包含于地圖中。假設(shè)周圍只有一輛車，如圖3所示，無人駕駛車輛通過控制自身速度很容易防止碰撞的發(fā)生。

圖3 周圍僅有一輛車的情景

首先，根據(jù)自身速度與位置和障礙物車輛的速度與位置計(jì)算出可能發(fā)生碰撞的點(diǎn)，在圖中用三角形表示。計(jì)算出兩車到達(dá)可能發(fā)生碰撞點(diǎn)所需要的時(shí)間t1和t2，并比較t1和t2的大小。如果時(shí)間差大于3 s，則保持原狀態(tài)行駛；如果0＜t1-t2＜3 s，則減速讓行；如果0＜t2-t1＜3 s，則加速通過。注意，由于無法預(yù)測周圍車輛的行駛路徑，可能發(fā)生碰撞點(diǎn)不是車輛行駛軌跡的交點(diǎn)。

當(dāng)周圍車輛的數(shù)量增加到2，速度控制的復(fù)雜性明顯增加。如圖4所示，在無人駕駛車輛的行駛軌跡上出現(xiàn)兩個(gè)可能發(fā)生碰撞的位置，在圖中用三角形表示。不僅需要考慮本車到兩個(gè)可能發(fā)生碰撞點(diǎn)的距離，還需要考慮兩個(gè)點(diǎn)之間的間隔。因此，本車需要處理以下情形：（1）本車加速可以通過所有可能碰撞點(diǎn)；（2）本車加速通過第1個(gè)可能碰撞點(diǎn)，但必須減速讓行第2個(gè)可能碰撞點(diǎn)；（3）本車可以加速通過第2個(gè)可能碰撞點(diǎn)，但必須讓行第1個(gè)可能碰撞點(diǎn)；（4）本車必須讓行所有可能碰撞點(diǎn)。相比于只有1個(gè)可能碰撞點(diǎn)的情況，無人駕駛車輛決策系統(tǒng)明顯要復(fù)雜得多。

圖4 周圍有兩輛車的情景

在高速公路的閘口區(qū)，周圍車輛的數(shù)量是不可預(yù)測的，隨著可能發(fā)生碰撞點(diǎn)數(shù)量的增加，復(fù)雜性會(huì)進(jìn)一步增加，無法通過switch-case編程技術(shù)解決這一問題。

2.2 決策樹模型

本文中考慮無人駕駛車輛的行駛軌跡上存在任意數(shù)量可能發(fā)生碰撞點(diǎn)的情況下，通過控制車身速度實(shí)現(xiàn)安全行駛問題。為了降低編程復(fù)雜度，提出決策樹模型，引導(dǎo)車輛從收費(fèi)窗口安全到達(dá)匝道。假定所有車輛的位置和速度均已知，因此可計(jì)算出所有的可能發(fā)生碰撞位置。另外，還可計(jì)算出本車和所有車輛到達(dá)可能碰撞點(diǎn)所需要的時(shí)間。為降低計(jì)算量，設(shè)置一個(gè)隊(duì)列，將到達(dá)可能碰撞點(diǎn)所需時(shí)間小于5 s的車輛加入隊(duì)列中，其余車輛不予考慮。在每一時(shí)間步長上根據(jù)預(yù)測的可能碰撞點(diǎn)更新列表，刪除消失點(diǎn)并加入新出現(xiàn)的點(diǎn)。然后，對列表中所有的點(diǎn)按照到無人駕駛車輛的遠(yuǎn)近排序。每一個(gè)可能碰撞點(diǎn)被分配兩個(gè)狀態(tài)Acc和Dec，分別表示加速和減速。每個(gè)狀態(tài)有兩個(gè)可能的值0和1，分別表示是否可通過加速／減速，在可能碰撞點(diǎn)處避免碰撞。如果Acc和Dec同時(shí)為0，則在該點(diǎn)處碰撞將不可避免，這一狀態(tài)在模擬仿真中用于算法的性能評(píng)估。另一方面，如果隊(duì)列中所有點(diǎn)的Acc值均為1，則無需考慮Dec的值，可加速通過所有可能碰撞點(diǎn)。

對于一般的情形，找出隊(duì)列中第一個(gè)Acc為0的可能碰撞點(diǎn)，表示本車不能通過加速通過，必須減速給周圍車輛讓行。然后，將該點(diǎn)之前的所有可能碰撞點(diǎn)組建成一顆樹，在本文中成為決策樹，如圖5所示，在第一個(gè)Acc＝0的點(diǎn)之后的所有點(diǎn)均被忽略。樹中的節(jié)點(diǎn)表示可能發(fā)生碰撞的位置。對應(yīng)于每個(gè)可能碰撞點(diǎn)的兩個(gè)狀態(tài)變量，每個(gè)節(jié)點(diǎn)有兩個(gè)樹枝，Acc和Dec，子樹在Acc變量一側(cè)連接到它們的父節(jié)點(diǎn)。樹中Acc＝0的點(diǎn)表示不能加速通過的、距離本車最近的可能碰撞點(diǎn)。下一步，從Acc＝0的點(diǎn)回溯搜索第一個(gè)Dec＝1的節(jié)點(diǎn)，并把該點(diǎn)稱為決策點(diǎn)。無人駕駛車輛根據(jù)該決策點(diǎn)之前所有節(jié)點(diǎn)處計(jì)算出的最大加速度值設(shè)置車輛的加速度，這一策略可使車輛安全通過所有可能碰撞點(diǎn)。

圖5 用于控制無人駕駛車輛速度的決策樹模型

圖6給出算法實(shí)現(xiàn)的流程。在創(chuàng)建駕駛場景和計(jì)算出鏈接收費(fèi)窗口與匝道間的參考路徑后，初始化過程設(shè)置車體參數(shù)。下一步設(shè)置仿真參數(shù)，其中θ0和steer0表示起始位置車的方位和轉(zhuǎn)向角，相應(yīng)地，θf和steerf表示仿真結(jié)束位置處車的方位和轉(zhuǎn)向角。然后，定義一個(gè)隊(duì)列用于存儲(chǔ)可能發(fā)生碰撞位置處的各種狀態(tài)變量。

圖6 算法流程圖

當(dāng)進(jìn)入循環(huán)后，預(yù)測無人駕駛車輛行駛軌跡上可能發(fā)生碰撞的位置，并根據(jù)距離遠(yuǎn)近選擇必要點(diǎn)加入到隊(duì)列中。同時(shí)計(jì)算并存儲(chǔ)周圍車輛到達(dá)可能碰撞點(diǎn)所需的時(shí)間。然后，根據(jù)是否能加速通過或減速讓行，分配各個(gè)節(jié)點(diǎn)狀態(tài)變量Acc和Dec的值。為保證駕駛的舒適性和車輛的可執(zhí)行性，必須在程序中指定允許的最大加／減速度值。最后，根據(jù)存儲(chǔ)的決策樹計(jì)算出無人駕駛車輛為避障所需要的加／減速度值。根據(jù)所得到的值，利用純追蹤法仿真車輛的行駛，當(dāng)車輛當(dāng)前x坐標(biāo)到達(dá)仿真終點(diǎn)xf時(shí)，結(jié)束仿真運(yùn)行。

3 仿真結(jié)果

為驗(yàn)證所提出的方法，本文在兩種駕駛場景下進(jìn)行模擬仿真。駕駛場景、車體和仿真參數(shù)如表1、表2和表3所示。為簡便起見，假設(shè)所有車輛具有相同的尺寸。程序中，無人駕駛車輛可起始于任意一個(gè)收費(fèi)窗口，到達(dá)任意一條匝道。

表1 道路場景參數(shù)

表2 車體參數(shù)

表3 仿真參數(shù)

第1個(gè)示例給出周圍有一輛車并入本車行駛路徑時(shí)減速讓行的情況。本車由4號(hào)收費(fèi)窗口駛向2號(hào)匝道，周圍車輛從其它收費(fèi)窗口隨機(jī)駛出，駛向任一匝道。示例中起點(diǎn)與終點(diǎn)分配為：1→2，2→1，3→1，4→2，5→1，6→2（收費(fèi)窗口編號(hào)→匝道編號(hào)）。1到6號(hào)收費(fèi)窗口車輛出發(fā)時(shí)間分別為0.37、3.95、3.22、4.06、1.89和2.66 s。本車初始速度5.56 m／s，為通過ETC法定允許速度。仿真結(jié)果如圖7所示。圖7（a）給出行駛路徑，圖中給出幾個(gè)樣本時(shí)刻本車與周圍車輛的位置，并標(biāo)注了在行駛過程中可能發(fā)生碰撞的位置。當(dāng)本車檢測到可能碰撞點(diǎn)，并判別無法加速通過后，主動(dòng)減速讓行。圖7（b）給出由道路平面和時(shí)間軸構(gòu)成的3D空間中本車與周圍車輛的行駛軌跡，示例中沒有交叉點(diǎn)，因此行駛過程中沒有發(fā)生碰撞。圖7（c）給出本車行駛過程中的速度、加速度曲線。可以看出，本車從初始速度以1 m／s2加速，當(dāng)檢測到可能碰撞點(diǎn)，主動(dòng)減速給周圍車輛讓行。速度和加速度都在車輛的可執(zhí)行范圍內(nèi)，且滿足駕駛舒適性的要求。

圖7 減速讓行周圍車輛

第2個(gè)場景示例為檢測到可能碰撞點(diǎn)后加速通過。本車從1號(hào)收費(fèi)窗口駛出，到2號(hào)匝道。起點(diǎn)與終點(diǎn)分配為：1→2，2→1，3→2，4→1，5→2，6→1。各窗口處車輛的出發(fā)時(shí)間分別為2.33、3.85、0.42、5.06、1.92和6.96 s，本車初始速度與第一個(gè)示例相同，仿真結(jié)果如圖8所示。圖8（a）給出行駛路徑，本車檢測到的可能碰撞點(diǎn)，判斷可以通過加速安全通過，并執(zhí)行加速操作。圖8（b）給出由道路平面和時(shí)間軸構(gòu)成的3D空間中本車與周圍車輛的行駛軌跡，示例中沒有交叉點(diǎn)，因此行駛過程中沒有發(fā)生碰撞。圖8（c）給出本車行駛過程中的速度、加速度曲線?？梢钥闯?，與上一示例一樣，速度和加速度都在車輛的可執(zhí)行范圍內(nèi)，且滿足駕駛舒適性的要求。

圖8 加速通過可能碰撞點(diǎn)

除上述兩種示例情況外，假定所有車輛的始發(fā)時(shí)間為隨機(jī)，對所編制的程序執(zhí)行超過100次仿真，結(jié)果給出，在周圍車輛不違反交通規(guī)則的前提下，無人駕駛車輛可安全到達(dá)匝道。算法不能解決的問題有因讓行某一車輛停止等待時(shí)，其它車輛的主動(dòng)碰撞。

4 結(jié)論

本文中提出一種決策樹模型用于控制車輛速度，實(shí)現(xiàn)無人駕駛車輛從高速公路收費(fèi)窗口到匝道之間的安全行駛，該方法對于實(shí)現(xiàn)高速全程無人駕駛具有重要意義。利用6階多項(xiàng)式優(yōu)化從收費(fèi)窗口到匝道之間參考路徑，對于更復(fù)雜的道路結(jié)構(gòu)，可以通過采集路點(diǎn)使用連接低階貝塞爾曲線的方法獲得參考路徑。

針對決策樹模型，設(shè)置駕駛速度范圍在0到60 km／h之間，無人駕駛車輛與周圍車輛的駕駛行為完全隨機(jī)，執(zhí)行超過100次仿真。汽車行駛仿真結(jié)合了汽車運(yùn)動(dòng)學(xué)模型，并采用純追蹤法調(diào)整轉(zhuǎn)向角。仿真結(jié)果表明，本文中所提出的方法在周圍車輛位置、速度任意時(shí)刻都已知的條件下，能夠?qū)崿F(xiàn)無人駕駛車輛從高速公路收費(fèi)窗口到匝道之間的安全駕駛。