屈盼讓++李林++任曉琨++荊立雄

摘要：路徑規(guī)劃是無人車運(yùn)動(dòng)控制的重要研究內(nèi)容之一，路徑規(guī)劃算法的性能優(yōu)劣直接影響無人車能否在復(fù)雜交通場景中自主行駛?？紤]到車輛運(yùn)動(dòng)學(xué)模型對軌跡平滑性的要求，本文提出了基于B樣條曲線解決無人車路徑規(guī)劃問題的方法，建立了完整的分步規(guī)劃模型，即將路徑規(guī)劃的過程分為路徑簇生成、最大曲率約束、碰撞檢測以及最優(yōu)評(píng)價(jià)四個(gè)步驟串行進(jìn)行，最終得到最優(yōu)路徑。該方法實(shí)現(xiàn)過程簡單，運(yùn)行效率較高，仿真結(jié)果表明了本文算法的有效性。

關(guān)鍵詞： B樣條曲線； 無人車； 路徑規(guī)劃； 碰撞檢測； 最大曲率約束； 最優(yōu)路徑

中圖分類號(hào)：TP181 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1009-3044（2016）26-0235-03

B-spline Curve based Trajectory Planning for Autonomous Vehicles

QU Pan-rang，LI Lin ， REN Xiao-kun ，JING Li-xiong

（Institute of Aeronautics Computing Technique Research， Xian 710065， China）

Abstract：Path planning is an important research topic in the field of the unmanned vehicle motion planning， and it directly affects the performance of unmanned vehicles in a complex traffic environment. Taking the requirement for smoothness into account， this paper proposed a method based on B-spline curve and built a planning model which can be divided into four steps， including path clusters， constraint of maximal curvature， collision detection and optimal path. This method works efficiently and simulation results show efficiency of the method.

Key words：B-spline curve； autonomous vehicle； path planning； collide detection； constraint of maximal curvature

1 引言

近年來，無人駕駛技術(shù)備受關(guān)注，各大研究機(jī)構(gòu)和企業(yè)爭相推出各自的無人駕駛平臺(tái)。無人車作為未來智能交通的主要主體也逐漸融入到我們的日常生活中，比如自主巡航[1]和自動(dòng)泊車等等。然而，為了使其更好地服務(wù)于我們，需要進(jìn)一步提高其智能化水平，而路徑規(guī)劃作為連接環(huán)境感知和運(yùn)動(dòng)規(guī)劃的橋梁，是無人車智能化水平的重要體現(xiàn)[2]。

由于受到自身動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)模型的約束，車輛的路徑規(guī)劃問題除過要嚴(yán)格滿足端點(diǎn)狀態(tài)約束之外，還要求其中間狀態(tài)滿足運(yùn)動(dòng)系統(tǒng)的微分約束。由于實(shí)現(xiàn)簡單，并且高階多項(xiàng)式曲線能夠很好地滿足運(yùn)動(dòng)系統(tǒng)的微分約束，生成高階平滑的路徑，所以很多路徑規(guī)劃系統(tǒng)選擇使用基于多項(xiàng)式曲線的方法生成路徑。B樣條曲線是一種典型的多項(xiàng)式曲線，且因?yàn)槠渌械闹虚g狀態(tài)均是由控制點(diǎn)加權(quán)生成，所以其能夠完全滿足端點(diǎn)狀態(tài)約束。綜合考慮無人車路徑規(guī)劃的要求和實(shí)現(xiàn)復(fù)雜度，在僅已知初始位姿和目標(biāo)位姿的情況下，本文選擇B樣條曲線生成路徑，重點(diǎn)講述分步規(guī)劃模型，即路徑簇生成、最大曲率約束、碰撞檢測以及最優(yōu)評(píng)價(jià)四個(gè)過程，并通過Matlab仿真對本文方法進(jìn)行了驗(yàn)證。

2 問題描述

本節(jié)分別描述了無人車路徑規(guī)劃問題和B樣條曲線。

2.1 路徑規(guī)劃問題描述

路徑規(guī)劃得到的是一條從初始位置到目標(biāo)位置的路徑，即二維平面內(nèi)一條從初始位置點(diǎn)到目標(biāo)位置點(diǎn)的曲線，曲線上的每一個(gè)點(diǎn)表示車在行駛過程中的一個(gè)狀態(tài)?？紤]到實(shí)現(xiàn)方便，本文將路徑描述成離散點(diǎn)序列[Sstart，S1，???，Sn，Sgoal]，如圖1所示，序列中每一個(gè)點(diǎn)[Si（xi，yi，θi）]表示車的一個(gè)狀態(tài)，其中[（xi，yi）]表示此時(shí)刻車輛的位置，[θi]表示車輛的航向，[Sstart]和[Sgoal]分別表示車輛的初始狀態(tài)和目標(biāo)狀態(tài)。圖1中的圓[（xobs1，yobs1，robs1）]表示環(huán)境中的障礙物，[（xobs1，yobs1）]表示障礙物的位置信息，[robs1]表示障礙物的半徑。

2.2 B樣條曲線

如果給定[m+n+1]個(gè)控制點(diǎn)[Pi（i=0，1，???，m+n）]，就可以構(gòu)造[m+1]段[n]次B樣條曲線，其可以表示為公式1：

[Pi，n（t）=k=0nPi+k?Fk，n（t） ，t∈[0，1]Fk，n（t）=1n！j=0n-k（-1）j?Cjn+1?（t+n-k-j）n ， t∈[0，1] ， k∈0，1，???，n] （1）

其中，[Pi，n（t）]表示第[i]個(gè)[n]次B樣條曲線片段，[n]表示B樣條曲線的次數(shù)，[t]為控制參數(shù)，其取值范圍為[[0，1]]，[Pi+k]為控制點(diǎn)，[Fk，n（t）]為B樣條基。依次連接全部[n]階B樣條曲線段就組成了整條B樣條曲線。

3 本文算法

本節(jié)重點(diǎn)講述基于B樣條曲線的路徑規(guī)劃方法和基于該方法生成路徑的過程。

3.1 基于B樣條曲線的路徑規(guī)劃方法

選擇三階B樣條曲線生成幾何路徑，即每四個(gè)控制點(diǎn)生成一個(gè)路徑片段，然后通過片段的拼接就可以實(shí)現(xiàn)從初始狀態(tài)到目標(biāo)狀態(tài)的路徑規(guī)劃，下面著重講述基于六控制點(diǎn)的三階B樣條曲線生成滿足車輛端點(diǎn)位姿約束路徑的方法，如圖2所示。

l 依據(jù)初始狀態(tài)選擇控制點(diǎn)[P0，P1，P2]。當(dāng)[P0，P1，P2]三個(gè)控制點(diǎn)共線，并且[P1]為線段[P0P2]的中點(diǎn)時(shí)，生成的B樣條曲線與線段[P0P2]相切于點(diǎn)[P1]。所以選擇無人車的初始位置為控制點(diǎn)[P1]，將控制點(diǎn)[P0]和[P2]選在初始航向角[θstart]所在直線上，并關(guān)于控制點(diǎn)[P1]對稱。如是，即能滿足車輛的初始位姿約束；

l 依據(jù)目標(biāo)狀態(tài)選擇控制點(diǎn)[P3，P4，P5]。當(dāng)[P3，P4，P5]三個(gè)控制點(diǎn)共線，并且[P4]為線段[P3P5]的中點(diǎn)時(shí)，生成的B樣條曲線與線段[P3P5]相切與點(diǎn)[P4]。所以選擇無人車的目標(biāo)位置為控制點(diǎn)[P3]，將控制點(diǎn)[P3]和[P5]選在目標(biāo)航向角[θgoal]所在的直線上，并關(guān)于控制點(diǎn)[P3]對稱。如是，即能滿足車輛的目標(biāo)位姿約束。

（a） 路徑簇

（b） 最大曲率約束

（c） 碰撞檢測

3.2 分步路徑規(guī)劃

本小節(jié)將以圖3所給定的場景為例，講述最優(yōu)路徑生成的過程。

3.2.1 路徑簇生成

在選定控制點(diǎn)[P1]和[P4]之后，通過選擇不同的控制點(diǎn)[P2]和[P3]，從而得到多組控制點(diǎn)，進(jìn)而得到多條路徑。將控制點(diǎn)選擇的極限定為線段[P1P2]、[P3P4]與[P1P4]相等，但是[P1P2]和[P3P4]不能出現(xiàn)交叉。將這個(gè)范圍等間隔量化，各取四個(gè)點(diǎn)，共組成16組控制點(diǎn)，得到16條路徑，如圖3（a）中的藍(lán)色曲線所示。

3.2.3 最大曲率約束

理論上，車輛的最小轉(zhuǎn)彎半徑[Rmin=Lsin（θmax）]是其本身屬性[3]，只取決于車輛的軸距[L]和最大前輪轉(zhuǎn)角[θmax]。那么，車輛可行駛路徑的最大曲率[κmax=1Rmin]也是固定的，假設(shè)無人車可行駛路徑的最大曲率[κmax=16]，以此為約束條件，在路徑簇中選擇滿足最大曲率約束的路徑，如圖3（b）所示，圖中綠色曲線表示不滿足最大曲率約束的路徑。

3.2.4 碰撞檢測

碰撞檢測的目的是保證車輛在規(guī)劃路徑上行駛而不與障礙物發(fā)生碰撞。采取的碰撞檢測的方法很簡單，因?yàn)榄h(huán)境中的障礙物采用圓來描述，所以只要判斷路徑上每一點(diǎn)到圓心的距離與障礙物半徑的關(guān)系，就能確定其是否發(fā)生碰撞。由兩點(diǎn)間距離公式：

[d=（x1-x2）2+（y1-y2）2] （2）

如果[d]大于障礙物半徑，則不發(fā)生碰撞；如果[d]小于障礙物半徑，則發(fā)生碰撞。圖3（c）中的藍(lán)色曲線表示既滿足最大曲率約束，又不與障礙物碰撞的路徑。

3.2.2 最優(yōu)路徑

路徑要求的側(cè)重點(diǎn)不同，優(yōu)化的目標(biāo)函數(shù)也可以有多種選擇，常用的目標(biāo)函數(shù)有最短和最平滑等。其中，路徑最短可以抽象成優(yōu)化問題：

[traoptimal=arg mintraids] （3）

路徑最平滑可以抽象成優(yōu)化問題：

[traoptimal=arg mintraiκ2] （4）

式中，[traoptimal]為最優(yōu)路徑，[traids]為第[i]條路徑的長度，[traiκ2]為第[i]條路徑上所有點(diǎn)處的曲率平方之和。圖3（d）中的紅色曲線即為得到的最短可行駛路徑。

如是，就能得到滿足車輛運(yùn)動(dòng)學(xué)約束，并且無碰撞的最優(yōu)路徑。

4 結(jié)論

本文選擇使用B樣條曲線解決無人車路徑規(guī)劃問題，并建立了基于B樣條曲線的分步規(guī)劃模型。仿真結(jié)果表明，使用基于B樣條曲線的路徑規(guī)劃方法能夠很好地解決簡單障礙物場景中無人車的路徑規(guī)劃問題，并且因?yàn)槁窂缴蛇^程簡單，所以該方法常常表現(xiàn)得十分高效，能夠完全滿足無人車路徑規(guī)劃系統(tǒng)對算法實(shí)時(shí)性的要求。

參考文獻(xiàn)：

[1] Vahidi A， Eskandarian A. Research advances in intelligent collision avoidance and adaptive cruise control [J]. IEEE Transactions on Intelligent Transportation Systems， 2003， 4（3）：143-153.

[2] Siegwart R， Nourbakhsh I R， Scaramuzza D. Introduction to autonomous mobile robots [M]. US： MIT press， 2011.

[3] Snider J M. Automatic steering methods for autonomous automobile path tracking [J]. Robotics Institute， Pittsburgh， 2009.