趙倬祺，張引，張琨，王嚴(yán)

（華晨汽車工程研究院，遼寧 沈陽 110141）

前言

汽車自動駕駛研究中，應(yīng)用了大量自動控制及機(jī)器人控制算法，但由于應(yīng)用場景的差異以及對動態(tài)性的高度需求，出現(xiàn)了許多適用于特定場景的路徑生成算法。例如泊車工況下，要求車輛可以倒退；非結(jié)構(gòu)化道路下，僅有終點狀態(tài)和障礙物約束，不限制中間運動狀態(tài)；而結(jié)構(gòu)化道路要求車輛以相對平穩(wěn)的狀態(tài)在車道內(nèi)行駛或換道。

非結(jié)構(gòu)化場景工況常用的算法包括啟發(fā)式搜索、維諾圖、快速擴(kuò)展隨機(jī)樹[5]、人工勢場[7]等。其中 A*[6]和快速擴(kuò)展隨機(jī)樹，以及其優(yōu)化算法和得到了廣泛應(yīng)用，并在其原模型基礎(chǔ)上加入了阿克曼轉(zhuǎn)向約束。結(jié)構(gòu)化道路受到約束較少，主要以離散化車輛可能行駛路徑再進(jìn)行篩選的方式生成路徑；也有一些路徑生成算法使用了迭代優(yōu)化的方式。

在車輛可能行駛路徑離散化過程中，受道路曲率或曲線表達(dá)式的影響，為保證離散過程中的魯棒性，算法需要許多特殊處理。本文使用的路徑生成算法使用離散化車輛相對參考路徑的偏移量的方式，提高算法魯棒性。同時將人工勢場加入路徑的代價函數(shù)計算中，實現(xiàn)避障。

1 路徑生成

1.1 候選路徑離散化

汽車行駛過程中，實際候選路徑的集合是一個平面，其邊緣受到車輛轉(zhuǎn)向約束，因此需要對候選路徑進(jìn)行離散化。本文中的候選路徑離散化方法主要考慮結(jié)構(gòu)化道路對車輛的約束，通常可使用道路中心線作為參考路徑，同時利用車輛自身轉(zhuǎn)向約束，過濾部分候選路徑，以減小代價函數(shù)計算量。需要離散化的變量包括：路徑終點與參考路徑的橫向偏差，和達(dá)到路徑終點時經(jīng)過的參考路徑弧長，如圖1所示。到達(dá)路徑終點車輛的航向也是候選路徑的重要因素，但考慮運算量和車輛實際行駛的情況，本文在建模中保留了車輛終點的所有航向，在運算時僅取與參考路徑切線方向相同的一個目標(biāo)方向。

1.2 候選路徑生成

決定候選路徑的參數(shù)共四個，即起點(x0,y0)和終點(x1,y1)的位置及方向。為了使曲線更平滑，限定終點處的二階導(dǎo)數(shù)為 0，起點處曲線二階導(dǎo)數(shù)值為當(dāng)前車輛前輪轉(zhuǎn)角形成的車身角變化量。以上六個參數(shù)可確定一條關(guān)于x、y的五次多項式曲線。

設(shè)由未知量構(gòu)成的向量：

假設(shè)有矩陣A由多項式系數(shù)以及該多項式的一、二階導(dǎo)函數(shù)系數(shù)構(gòu)造而成，則：

則可解得矩陣A，從而得到五次多項式的系統(tǒng)向量：

式中：x、y為平面坐標(biāo)下的橫、縱坐標(biāo)。

1.3 坐標(biāo)轉(zhuǎn)換

在算法實現(xiàn)過程中，參考路徑點集{Pref}內(nèi)的橫縱坐標(biāo)可能存在一對多關(guān)系，例如U型彎，使用曲線表達(dá)式表示路徑會使程序魯棒性嚴(yán)重降低，因此此處多項式公式僅表示候選路徑各點與參考路徑的距離[1]，再通過弗萊納標(biāo)架將候選該距離轉(zhuǎn)換到參考路徑坐標(biāo)下，生成候選路徑。

在弗萊納標(biāo)架下單位切向量：

式中：r為參考路徑弧長， 為法向量方向的單位向量，d為候選路徑與參考路徑的距離。

通過該坐標(biāo)轉(zhuǎn)換，可以將橫向位移形成的路徑轉(zhuǎn)換到實際坐標(biāo)系下，如圖2所示。

1.4 起始點映射

路徑生成依據(jù)的參考路徑是離散的路網(wǎng)點，理想狀況下路網(wǎng)點能較好的表現(xiàn)道路曲率，但限于路網(wǎng)數(shù)據(jù)存儲量和傳輸速度，路網(wǎng)點密度不能達(dá)到理想狀態(tài)，從而表現(xiàn)出折線形式，需要對其進(jìn)行平滑處理。通常的方法是使用樣條線[2]、貝塞爾曲線[4]等，對路網(wǎng)點進(jìn)行擬合并插值，最終逼近曲線。

坐標(biāo)轉(zhuǎn)換需要將圖1中（0,0）點映射到路網(wǎng)坐標(biāo)系下，即車輛在參考路徑上的位置。文獻(xiàn)[3]使用牛頓法計算參考路徑曲線上距離車輛最近的點，認(rèn)為該點為車輛到曲線的垂足，即與該點的距離能表征橫向位移。因此可以把該點作為路徑起始點。

圖1 車輛橫向運動分量規(guī)劃

圖2 橫向運動分量轉(zhuǎn)換坐標(biāo)得到的參考路徑

2 代價函數(shù)

路徑規(guī)劃中，實際可能形成的路徑有很多，例如但不限于圖3所示，因此需要對路徑進(jìn)行評價。影響規(guī)劃的因素表示為候選路徑的代價。通常影響因素包括候選路徑相對參考路徑的偏移，路徑彎曲程度，路徑長度及障礙物。其中：

候選路徑偏移使用距離平方的積分：

路徑彎曲程度考慮曲線三階導(dǎo)函數(shù)平方的積分：

式中：xr為平面坐標(biāo)系下的橫坐標(biāo)。

路徑長度使用弧長的積分：

式中：roption為候選路徑弧長。

障礙物使用分段函數(shù)：

為統(tǒng)一計算模型，將障礙物代價表示為：

式中：db為路徑距離某一障礙物的距離；dc為發(fā)生碰撞的最大距離；ds為障礙物影響距離，可以使用駕駛員主觀感受的障礙物安全距離；F為障礙物人工勢場對路徑的斥力。

所以完整的路徑代價為：

圖3 候選路徑

3 人工勢場

人工勢場是機(jī)器人路徑規(guī)劃領(lǐng)域較成熟的算法，主要思想是通過為障礙物添加虛擬力場，得到目標(biāo)在虛擬力場作用下的運動路徑，作為規(guī)劃算法生成的目標(biāo)路徑。本文僅使用斥力場計算障礙物對候選路徑的代價值。

由斥力勢能公式：

式中：pobs為障礙物位置。

4 結(jié)論

4.1 試驗結(jié)論

實驗程序使用在Ubuntu環(huán)境下使用Python實現(xiàn)，主要驗證該算法下車輛行駛路徑的表現(xiàn)以及代價函數(shù)的參數(shù)對路徑選取的影響形式。

如圖4所示，在結(jié)構(gòu)化道路上，當(dāng)車輛距離參考路徑有一定距離，且行駛方向偏離車道時，受距離項影響，車輛快速到達(dá)參考路徑上。

在圖4相同的起始條件下，增大路徑平順系數(shù)以及長度系數(shù)，車輛以較平緩的方式接近參考路徑，如圖 5；繼續(xù)增大以上兩個系數(shù)，由于距離項系數(shù)占比重減小，車輛平穩(wěn)的沿參考路徑平行的路徑行駛，而不必須回到參考路徑上，如圖6。

在路徑前方有障礙的情況下，如圖7中藍(lán)色區(qū)域所示，區(qū)域的中心為障礙物，深色范圍為考慮車身尺寸，可能發(fā)生碰撞的距離范圍，淺色區(qū)域為考慮誤差及駕駛員感受，障礙物可能影響的范圍，在圖7 中車輛繞過障礙物繼續(xù)沿路徑方向行駛，增大障礙物代價系數(shù)，路徑受到淺藍(lán)色區(qū)域勢場力影響，如圖8所示，車輛以更遠(yuǎn)的距離繞過障礙物。

4.2 后續(xù)研究

通常路徑生成算法需要配合速度規(guī)劃，很多相關(guān)研究采用了路徑和速度相互獨立的方式，也有部分將速度、加速度及導(dǎo)數(shù)和車輛位置與姿態(tài)整合成為完整的狀態(tài)空間[1]，此方法需要行為決策模塊給出更明確的狀態(tài)信息。可以在弗萊納坐標(biāo)下對速度進(jìn)行整合。

圖4 車輛快速靠近參考路徑

圖5 車輛緩慢靠近參考路徑

圖6 車輛快速到達(dá)參考路徑

圖7 有障礙物時的路徑規(guī)劃

圖8 車輛盡量遠(yuǎn)離障礙物時的路徑規(guī)劃

在工程化方面，需要調(diào)整路徑選擇的優(yōu)化算法，目前通過選取適當(dāng)?shù)那耙暰嚯x以及貪心算法，使車輛選取的路徑在當(dāng)前狀態(tài)下選擇局部最優(yōu)，而對于更復(fù)雜的路況，則可以使用動態(tài)規(guī)劃算法對其進(jìn)行優(yōu)化，以完成向非結(jié)構(gòu)化道路算法的過渡。

[1] Moritz, Werling. Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenét Frame[C]. Anchorage, AK, USA：2010 IEEE International Conference on Robotics and Automation, 2010. 987-993.

[2] K, Chu. Local Path Planning for Off-Road Autonomous Driving With Avoidance of Static Obstacles[J].IEEE Transactions on Intellig-ent Transportation Systems, 2012, 13(4)： 1599-1616.

[3] H. Wang, J. Kearney, and K. Atkinson, “Robust and efficient comput-ation of the closest point on a spline curve,” in Proc. 5th Int. Conf.Curves Surf.,2002, pp. 397-406.

[4] L.Han, H.Yashiro, H.T.N.Nejad, D.Q.Huy, S.Mita," Bezier Curve Based Path Planning for Autonomous Vehicle in Urban Environ-ment," IEEE Intelligence Vehicle symposium, San Diego, June 21-24, 2010.1036-1042.

[5] J. a. S. M. L. James J.Kuffner, "RRT-Connect： An Effficient Approach to Single-Query Path Planning,"Proceedings -IEEE International Conference on Robotics and Automation： Inst. Of Electrical and Ele-ctronics Engineers, New York, vol. 2, pp. 995-1001,2000.

[6] Likhachev, M.; Ferguson, D.; Gordon, G.; Stentz, A.; and Thrun,S.2005. Anytime Dynamic A*： The Proofs. Technical Report CMURI-TR-05-12, Carnegie Mellon School of Computer Science.

[7] J. Barraquand, B. Langlois, and J.C.Latombe. Numerical potential fie-ld techniques for robot planning.IEEE Transactions on Man and Cybernetics, 22(2)：224-241, Mar/Apr 1992.