基于B樣條曲線的無人車路徑規(guī)劃算法

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

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

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

中圖分類號：TP181 文獻標(biāo)識碼：A 文章編號：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)注，各大研究機構(gòu)和企業(yè)爭相推出各自的無人駕駛平臺。無人車作為未來智能交通的主要主體也逐漸融入到我們的日常生活中，比如自主巡航[1]和自動泊車等等。然而，為了使其更好地服務(wù)于我們，需要進一步提高其智能化水平，而路徑規(guī)劃作為連接環(huán)境感知和運動規(guī)劃的橋梁，是無人車智能化水平的重要體現(xiàn)[2]。

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

2 問題描述

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

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

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

2.2 B樣條曲線

如果給定[m+n+1]個控制點[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]個[n]次B樣條曲線片段，[n]表示B樣條曲線的次數(shù)，[t]為控制參數(shù)，其取值范圍為[[0，1]]，[Pi+k]為控制點，[Fk，n（t）]為B樣條基。依次連接全部[n]階B樣條曲線段就組成了整條B樣條曲線。

3 本文算法

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

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

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

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

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

（a） 路徑簇

（b） 最大曲率約束

（c） 碰撞檢測

3.2 分步路徑規(guī)劃

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

3.2.1 路徑簇生成

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

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ā)生碰撞。采取的碰撞檢測的方法很簡單，因為環(huán)境中的障礙物采用圓來描述，所以只要判斷路徑上每一點到圓心的距離與障礙物半徑的關(guān)系，就能確定其是否發(fā)生碰撞。由兩點間距離公式：

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

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

3.2.2 最優(yōu)路徑

路徑要求的側(cè)重點不同，優(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]條路徑上所有點處的曲率平方之和。圖3（d）中的紅色曲線即為得到的最短可行駛路徑。

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

4 結(jié)論

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

參考文獻：

[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.