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

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

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

關鍵詞： B樣條曲線； 無人車； 路徑規劃； 碰撞檢測； 最大曲率約束； 最優路徑

中圖分類號：TP181 文獻標識碼：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 引言

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

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

2 問題描述

本節分別描述了無人車路徑規劃問題和B樣條曲線。

2.1 路徑規劃問題描述

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

2.2 B樣條曲線

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

3 本文算法

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

3.1 基于B樣條曲線的路徑規劃方法

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

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

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

（a） 路徑簇

（b） 最大曲率約束

（c） 碰撞檢測

3.2 分步路徑規劃

本小節將以圖3所給定的場景為例，講述最優路徑生成的過程。

3.2.1 路徑簇生成

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

3.2.3 最大曲率約束

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

3.2.4 碰撞檢測

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

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

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

3.2.2 最優路徑

路徑要求的側重點不同，優化的目標函數也可以有多種選擇，常用的目標函數有最短和最平滑等。其中，路徑最短可以抽象成優化問題：

[traoptimal=arg mintraids] （3）

路徑最平滑可以抽象成優化問題：

[traoptimal=arg mintraiκ2] （4）

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

如是，就能得到滿足車輛運動學約束，并且無碰撞的最優路徑。

4 結論

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

參考文獻：

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