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

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

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

▌為什么使用 Frenet 坐標系

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

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

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

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

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

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

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

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

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

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

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

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

那么問題來了，我們如何去確定最優的軌跡呢？Werling 方法的思路是通過一組目標配置來求得軌跡的備選集合，然后在備選集合中基于 Jerk 最小化的原則選擇最優軌跡，我們仍然以d方向的優化軌跡為例講解：

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

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

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

該損失函數包含三個懲罰項：

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

：制動應當迅速，時間短；?

：目標狀態不應偏離道路中心線太遠

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

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

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

跟車

匯流和停車

車速保持

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

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

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

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

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

▌事故避免（Collision Avoiding）

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

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

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

s 方向的加速度是否超過設定的最大加速度

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

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

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

▌基于 Frenet 優化軌跡的無人車動作規劃實例

由于 planner 的代碼篇幅過長，本實例完整代碼請見文末鏈接，在此僅講解算法核心代碼內容。和之前一樣，我們仍然使用 Python 來實現該動作規劃算法。

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

#路線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)

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

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

#參數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#最大預測時間[s]MINT=4.0#最小預測時間[s]TARGET_SPEED=30.0/3.6#目標速度（即縱向的速度保持）[m/s]D_T_S=5.0/3.6#目標速度采樣間隔[m/s]N_S_SAMPLE=1#目標速度的采樣數量ROBOT_RADIUS=2.0#robotradius[m]#損失函數權重KJ=0.1KT=0.1KD=1.0KLAT=1.0KLON=1.0

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

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):#橫向動作規劃forTiinnp.arange(MINT,MAXT,DT):fp=Frenet_path()#計算出關于目標配置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]#縱向速度規劃(速度保持)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#橫向的損失函數tfp.cd=KJ*Jp+KT*Ti+KD*tfp.d[-1]**2#縱向的損失函數tfp.cv=KJ*Js+KT*Ti+KD*ds#總的損失函數為d和s方向的損失函數乘對應的系數相加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):#計算五次多項式系數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

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

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

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

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]

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

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