一種基于Frenet坐標系的優(yōu)化軌跡動作規(guī)劃方法

動作規(guī)劃動作在無人車規(guī)劃模塊的最底層，它負責(zé)根據(jù)當前配置和目標配置生成一序列的動作。本文介紹一種基于Frenet坐標系的優(yōu)化軌跡動作規(guī)劃方法，該方法在高速情況下的高級車道保持和無人駕駛都具有很強的實用性，是目前普遍采用的一種動作規(guī)劃算法。

基于 Frenet 坐標系的動作規(guī)劃方法由于是由 BMW 的 Moritz Werling 提出的，為了簡便，我們在后文中也會使用 Werling 方法簡稱。在討論基于Frenet 坐標系的動作規(guī)劃方法之前，我們首先得定義什么是最優(yōu)的動作序列：對于橫向控制而言，假定由于車輛因為之前躲避障礙物或者變道或者其他制動原因而偏離了期望的車道線，那么此時最優(yōu)的動作序列（或者說軌跡）是在車輛制動能力的限制下，相對最安全，舒適，簡單和高效的軌跡。

同樣的，縱向的最優(yōu)軌跡也可以這么定義：如果車輛此時過快，或者太接近前方車輛，那么就必須做減速，具體什么是“舒適而又簡單的”減速呢？我們可以使用Jerk這個物理量來描述，Jerk 即加速度的變化率，也即加速度，通常來說，過高的加速度會引起乘坐者的不適，所以，從乘坐舒適性而言，應(yīng)當優(yōu)化 Jerk 這個量，同時，引入軌跡的制動周期T, 即一個制動的操作時間：

▌為什么使用 Frenet 坐標系

在 Frenet 坐標系中，我們使用道路的中心線作為參考線，使用參考線的切線向量t和法線向量n建立一個坐標系，如下圖的右圖所示，這個坐標系即為Frenet 坐標系，它以車輛自身為原點，坐標軸相互垂直，分為s方向（即沿著參考線的方向，通常被稱為縱向，Longitudinal）和d方向（即參考線當前的法向，被稱為橫向，Lateral），相比于笛卡爾坐標系（下圖的作圖），F(xiàn)renet 坐標系明顯地簡化了問題，因為在公路行駛中，我們總是能夠簡單的找到道路的參考線（即道路的中心線），那么基于參考線的位置的表示就可以簡單的使用縱向距離（即沿著道路方向的距離）和橫向距離（即偏離參考線的距離）來描述，同樣的，兩個方向的速度（?和?）的計算也相對簡單。

那么現(xiàn)在我們的動作規(guī)劃問題中的配置空間就一共有三個維度：(s,d,t),t是我們規(guī)劃出來的每一個動作的時間點，軌跡和路徑的本質(zhì)區(qū)別就是軌跡考慮了時間這一維度。

Werling 的動作規(guī)劃方法一個很關(guān)鍵的理念就是將動作規(guī)劃這一高維度的優(yōu)化問題分割成橫向和縱向兩個方向上的彼此獨立的優(yōu)化問題，具體來看下面的圖：

假設(shè)我們的上層（行為規(guī)劃層）要求當前車輛在t8越過虛線完成一次變道，即車輛在橫向上需要完成一個Δd以及縱向上完成一個Δs的移動，則可以將s和d分別表示為關(guān)于t的函數(shù)：s(t)和d(t)（上圖右圖），那么 d,s關(guān)于時間tt的最優(yōu)軌跡應(yīng)該選擇哪一條呢？通過這種轉(zhuǎn)換原來的動作規(guī)劃問題被分割成了兩個獨立的優(yōu)化問題，對于橫向和縱向的軌跡優(yōu)化，我們選取損失函數(shù)C，將使得C最小的軌跡作為最終規(guī)劃的動作序列。而Werling方法中損失函數(shù)的定義，則與我們前面提到的加速度 Jerk 相關(guān)。

▌Jerk 最小化和 5 次軌跡多項式求解

由于我們將軌跡優(yōu)化問題分割成了s和d兩個方向，所以 Jerk 最小化可以分別從橫向和縱向進行，令p為我們考量的配置（即s或d），加速度Jt關(guān)于配置p在時間段t1?t0內(nèi)累計的 Jerk 的表達式為：

現(xiàn)在我們的任務(wù)是找出能夠使得Jt(p(t))最小的p(t)，Takahashi的文章——Local path planning and motion control for AGV in positioning中已經(jīng)證明，任何 Jerk 最優(yōu)化問題中的解都可以使用一個 5 次多項式來表示：

要解這個方程組需要一些初始配置和目標配置，以橫向路徑規(guī)劃為例，初始配置為，即?t0?時刻車輛的橫向偏移，橫向速度和橫向加速度為，即可得方程組：

為了區(qū)分橫向和縱向，我們使用和?來分別表示 d 和 s 方向的多項式系數(shù)，同理，根據(jù)橫向的目標配置可得方程組：

我們通過令t0=0來簡化這個六元方程組的求解，可直接求得，和為：

令T=t1?t0，剩余的三個系數(shù)，可通過解如下矩陣方程得到：

該方程的解可以通過 Python 的 Numpy 中的np.linalg.solve簡單求得。至此，我們在給定任意的初始配置，目標配置以及制動時間?T?的情況下，可以求的對應(yīng)的?d?方向關(guān)于時間?t?的五次多項式的系數(shù)，同理，可以使用相同的方法來求解縱向（即?s?方向）的五次多項式系數(shù)。

那么問題來了，我們?nèi)绾稳ゴ_定最優(yōu)的軌跡呢？Werling 方法的思路是通過一組目標配置來求得軌跡的備選集合，然后在備選集合中基于 Jerk 最小化的原則選擇最優(yōu)軌跡，我們?nèi)匀灰詃方向的優(yōu)化軌跡為例講解：

我們可以取如下目標配置集合來計算出一組備選的多項式集合：

對于優(yōu)化問題而言，我們實際上希望車輛最終沿著參考線（道路中心線）平行的方向行駛，所以我們令，那么目標配置只涉及?didi?和?TjTj?兩個變量的組合，而這兩個變量在無人駕駛的應(yīng)用場景中實際上是受限的，我們可以通過定義(dmin,dmax)?和?(Tmin,Tmax)?來約束目標配置的取值范圍，通過?Δd?和?ΔT?來限制采樣密度，從而在每一個制動周期獲得一個有限的備選軌跡集合，如下圖所示：

要在備選集合中選擇最優(yōu)軌跡（即上圖中的綠色軌跡），我們需要設(shè)計損失函數(shù)，對于不同的場景，損失函數(shù)也不相同，以橫向軌跡為例，在較高速度的情況下，損失函數(shù)為：

該損失函數(shù)包含三個懲罰項：

：懲罰Jerk大的備選軌跡；?

：制動應(yīng)當迅速，時間短；?

：目標狀態(tài)不應(yīng)偏離道路中心線太遠

其中kj,kt和kd是這三個懲罰項的系數(shù)，它們的比值大小決定了我們的損失函數(shù)更加注重哪一個方面的優(yōu)化，由此我們可以算出所有備選軌跡的損失，取損失最小的備選軌跡作為我們最終的橫向軌跡。

值得注意的是，以上的損失函數(shù)僅適用于相對高速度的場景，在極端低速的情況下，車輛的制動能力是不完整的，我們不再將d表示為關(guān)于時間t的五次多項式，損失函數(shù)也會略有不同，但是這種基于有限采樣軌跡，通過優(yōu)化損失函數(shù)搜索最優(yōu)軌跡的方法仍然是一樣的，在此不再贅述。

討論完橫向的軌跡優(yōu)化問題，我們再來看看縱向的軌跡優(yōu)化，在不同的場景下縱向軌跡的優(yōu)化的損失函數(shù)也各不相同，Werling方法中將縱向軌跡的優(yōu)化場景大致分成如下三類：

跟車

匯流和停車

車速保持

在本文中我們詳細了解車速保持場景下的縱向軌跡優(yōu)化，在高速公路等應(yīng)用場景中，目標配置中并不需要考慮目標位置（即s1），所以在該場景下，目標配置仍然是，目標配置變成了，損失函數(shù)為：

其中是我們想要保持的縱向速度，第三個懲罰項的引入實際上是為了讓目標配置中的縱向速度盡可能接近設(shè)定速度，該情景下的目標配置集為：

即優(yōu)化過程中的可變參數(shù)為，同樣，也可以通過設(shè)置來設(shè)置軌跡采樣的密度，從而獲得一個有限的縱向軌跡集合：

其中，綠線即為縱向最優(yōu)軌跡。以上我們分別討論了橫向和縱向的最優(yōu)軌跡搜索方法，在應(yīng)用中，我們將兩個方向的損失函數(shù)合并為一個，即：

這樣，我們就可以通過最小化得到優(yōu)化軌跡集合（我們不能得到“最優(yōu)”的軌跡多項式參數(shù)，還可以得到“次優(yōu)”，“次次優(yōu)”軌跡等等）。

▌事故避免（Collision Avoiding）

顯然，我們上面的軌跡優(yōu)化損失函數(shù)中并沒有包含關(guān)于障礙物躲避的相關(guān)懲罰，并且我們的損失函數(shù)中也沒有包含最大速度，最大加速度和最大曲率等制動限制，也就是說我們的優(yōu)化軌跡集合并沒有考慮障礙物規(guī)避和制動限制因素，不將障礙物避免加入到損失函數(shù)中的一個重要的原因在于碰撞懲罰項的引入將代入大量需要人工調(diào)整的參數(shù)（即權(quán)重），是的損失函數(shù)的設(shè)計變得復(fù)雜，Werling 方法將這些因素的考量獨立出來，在完成優(yōu)化軌跡以后進行。

具體來說，我們會在完成所有備選軌跡的損失計算以后進行一次軌跡檢查，過濾掉不符合制動限制的，可能碰撞障礙物的軌跡，檢查內(nèi)容包括：

s 方向上的速度是否超過設(shè)定的最大限速

s 方向的加速度是否超過設(shè)定的最大加速度

軌跡的曲率是否超過最大曲率

軌跡是否會引起碰撞（事故）

通常來說，障礙物規(guī)避又和目標行為預(yù)測等有關(guān)聯(lián)，本身即使一個復(fù)雜的課題，高級自動駕駛系統(tǒng)通常具備對目標行為的預(yù)測能力，從而確定軌跡是否會發(fā)生事故。在本節(jié)中，我們關(guān)注的重點是無人車的動作規(guī)劃，故后面的實例僅涉及靜態(tài)障礙物的規(guī)避和動作規(guī)劃。

▌基于 Frenet 優(yōu)化軌跡的無人車動作規(guī)劃實例

由于 planner 的代碼篇幅過長，本實例完整代碼請見文末鏈接，在此僅講解算法核心代碼內(nèi)容。和之前一樣，我們?nèi)匀皇褂?Python 來實現(xiàn)該動作規(guī)劃算法。

首先，我們生成要追蹤的參考線以及靜態(tài)障礙物，參考線的生成只要使用了我們上一節(jié)提到的立方樣條插值，代碼如下：

#路線wx=[0.0,10.0,20.5,30.0,40.5,50.0,60.0]wy=[0.0,-4.0,1.0,6.5,8.0,10.0,6.0]#障礙物列表ob=np.array([[20.0,10.0],[30.0,6.0],[30.0,5.0],[35.0,7.0],[50.0,12.0]])tx,ty,tyaw,tc,csp=generate_target_course(wx,wy)

生成如下參考路徑以及障礙物：

其中紅線就是我們的全局路徑，藍點為障礙物。定義一些參數(shù)：

#參數(shù)MAX_SPEED=50.0/3.6#最大速度[m/s]MAX_ACCEL=2.0#最大加速度[m/ss]MAX_CURVATURE=1.0#最大曲率[1/m]MAX_ROAD_WIDTH=7.0#最大道路寬度[m]D_ROAD_W=1.0#道路寬度采樣間隔[m]DT=0.2#DeltaT[s]MAXT=5.0#最大預(yù)測時間[s]MINT=4.0#最小預(yù)測時間[s]TARGET_SPEED=30.0/3.6#目標速度（即縱向的速度保持）[m/s]D_T_S=5.0/3.6#目標速度采樣間隔[m/s]N_S_SAMPLE=1#目標速度的采樣數(shù)量ROBOT_RADIUS=2.0#robotradius[m]#損失函數(shù)權(quán)重KJ=0.1KT=0.1KD=1.0KLAT=1.0KLON=1.0

使用基于 Frenet 的優(yōu)化軌跡方法生成一系列橫向和縱向的軌跡，并且計算每條軌跡對應(yīng)的損失：

defcalc_frenet_paths(c_speed,c_d,c_d_d,c_d_dd,s0):frenet_paths=[]#采樣，并對每一個目標配置生成軌跡fordiinnp.arange(-MAX_ROAD_WIDTH,MAX_ROAD_WIDTH,D_ROAD_W):#橫向動作規(guī)劃forTiinnp.arange(MINT,MAXT,DT):fp=Frenet_path()#計算出關(guān)于目標配置di，Ti的橫向多項式lat_qp=quintic_polynomial(c_d,c_d_d,c_d_dd,di,0.0,0.0,Ti)fp.t=[tfortinnp.arange(0.0,Ti,DT)]fp.d=[lat_qp.calc_point(t)fortinfp.t]fp.d_d=[lat_qp.calc_first_derivative(t)fortinfp.t]fp.d_dd=[lat_qp.calc_second_derivative(t)fortinfp.t]fp.d_ddd=[lat_qp.calc_third_derivative(t)fortinfp.t]#縱向速度規(guī)劃(速度保持)fortvinnp.arange(TARGET_SPEED-D_T_S*N_S_SAMPLE,TARGET_SPEED+D_T_S*N_S_SAMPLE,D_T_S):tfp=copy.deepcopy(fp)lon_qp=quartic_polynomial(s0,c_speed,0.0,tv,0.0,Ti)tfp.s=[lon_qp.calc_point(t)fortinfp.t]tfp.s_d=[lon_qp.calc_first_derivative(t)fortinfp.t]tfp.s_dd=[lon_qp.calc_second_derivative(t)fortinfp.t]tfp.s_ddd=[lon_qp.calc_third_derivative(t)fortinfp.t]Jp=sum(np.power(tfp.d_ddd,2))#squareofjerkJs=sum(np.power(tfp.s_ddd,2))#squareofjerk#squareofdifffromtargetspeedds=(TARGET_SPEED-tfp.s_d[-1])**2#橫向的損失函數(shù)tfp.cd=KJ*Jp+KT*Ti+KD*tfp.d[-1]**2#縱向的損失函數(shù)tfp.cv=KJ*Js+KT*Ti+KD*ds#總的損失函數(shù)為d和s方向的損失函數(shù)乘對應(yīng)的系數(shù)相加tfp.cf=KLAT*tfp.cd+KLON*tfp.cvfrenet_paths.append(tfp)returnfrenet_paths

其中，一個重要的類是五次多項式類，其定義如下：

classquintic_polynomial:def__init__(self,xs,vxs,axs,xe,vxe,axe,T):#計算五次多項式系數(shù)self.xs=xsself.vxs=vxsself.axs=axsself.xe=xeself.vxe=vxeself.axe=axeself.a0=xsself.a1=vxsself.a2=axs/2.0A=np.array([[T**3,T**4,T**5],[3*T**2,4*T**3,5*T**4],[6*T,12*T**2,20*T**3]])b=np.array([xe-self.a0-self.a1*T-self.a2*T**2,vxe-self.a1-2*self.a2*T,axe-2*self.a2])x=np.linalg.solve(A,b)self.a3=x[0]self.a4=x[1]self.a5=x[2]defcalc_point(self,t):xt=self.a0+self.a1*t+self.a2*t**2+\self.a3*t**3+self.a4*t**4+self.a5*t**5returnxtdefcalc_first_derivative(self,t):xt=self.a1+2*self.a2*t+\3*self.a3*t**2+4*self.a4*t**3+5*self.a5*t**4returnxtdefcalc_second_derivative(self,t):xt=2*self.a2+6*self.a3*t+12*self.a4*t**2+20*self.a5*t**3returnxtdefcalc_third_derivative(self,t):xt=6*self.a3+24*self.a4*t+60*self.a5*t**2returnxt

這里的五次多項式的系數(shù)的求解過程和我們前面的理論講解是一樣的，只不過我們使用Numpy中的np.linalg.solve(A, b)方法將矩陣解了出來。最后，我們來看一下障礙物規(guī)避是如何實現(xiàn)的：

defcheck_collision(fp,ob):foriinrange(len(ob[:,0])):d=[((ix-ob[i,0])**2+(iy-ob[i,1])**2)for(ix,iy)inzip(fp.x,fp.y)]collision=any([di<=?ROBOT_RADIUS?**?2?for?di?in?d])????????if?collision:????????????return?False????return?True

由于我們將障礙物規(guī)避問題都簡化為靜態(tài)了，所以在這里我們只簡單地計算了所有規(guī)劃點到障礙物的距離，一句距離預(yù)計是否會發(fā)生碰撞，來看看完整的優(yōu)化軌跡檢查函數(shù)：

defcheck_paths(fplist,ob):okind=[]foriinrange(len(fplist)):ifany([v>MAX_SPEEDforvinfplist[i].s_d]):#最大速度檢查continueelifany([abs(a)>MAX_ACCELforainfplist[i].s_dd]):#最大加速度檢查continueelifany([abs(c)>MAX_CURVATUREforcinfplist[i].c]):#最大曲率檢查continueelifnotcheck_collision(fplist[i],ob):continueokind.append(i)return[fplist[i]foriinokind]

由此可以看出，最終的優(yōu)化軌跡的選擇并不單純基于最小損失函數(shù)，軌跡檢查還會過濾掉一些軌跡，所以使用基于 Frenet 的優(yōu)化軌跡來做無人車的動作規(guī)劃，通常能夠找到有限集的最優(yōu)解，當最優(yōu)解無法通過檢查是，自會采用“次優(yōu)解”甚至更加“次優(yōu)的”解。

最后我們來看一下完整的動作規(guī)劃效果：