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

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

基于 Frenet 坐標(biāo)系的動(dòng)作規(guī)劃方法由于是由 BMW 的 Moritz Werling 提出的，為了簡(jiǎn)便，我們?cè)诤笪闹幸矔?huì)使用 Werling 方法簡(jiǎn)稱。在討論基于Frenet 坐標(biāo)系的動(dòng)作規(guī)劃方法之前，我們首先得定義什么是最優(yōu)的動(dòng)作序列：對(duì)于橫向控制而言，假定由于車輛因?yàn)橹岸惚苷系K物或者變道或者其他制動(dòng)原因而偏離了期望的車道線，那么此時(shí)最優(yōu)的動(dòng)作序列（或者說(shuō)軌跡）是在車輛制動(dòng)能力的限制下，相對(duì)最安全，舒適，簡(jiǎn)單和高效的軌跡。

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

▌為什么使用 Frenet 坐標(biāo)系

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

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

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

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

▌Jerk 最小化和 5 次軌跡多項(xiàng)式求解

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

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

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

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

我們通過(guò)令t0=0來(lái)簡(jiǎn)化這個(gè)六元方程組的求解，可直接求得，和為：

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

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

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

我們可以取如下目標(biāo)配置集合來(lái)計(jì)算出一組備選的多項(xiàng)式集合：

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

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

該損失函數(shù)包含三個(gè)懲罰項(xiàng)：

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

：制動(dòng)應(yīng)當(dāng)迅速，時(shí)間短；?

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

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

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

討論完橫向的軌跡優(yōu)化問(wèn)題，我們?cè)賮?lái)看看縱向的軌跡優(yōu)化，在不同的場(chǎng)景下縱向軌跡的優(yōu)化的損失函數(shù)也各不相同，Werling方法中將縱向軌跡的優(yōu)化場(chǎng)景大致分成如下三類：

跟車

匯流和停車

車速保持

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

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

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

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

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

▌事故避免（Collision Avoiding）

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

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

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

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

軌跡的曲率是否超過(guò)最大曲率

軌跡是否會(huì)引起碰撞（事故）

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

▌基于 Frenet 優(yōu)化軌跡的無(wú)人車動(dòng)作規(guī)劃實(shí)例

由于 planner 的代碼篇幅過(guò)長(zhǎng)，本實(shí)例完整代碼請(qǐng)見文末鏈接，在此僅講解算法核心代碼內(nèi)容。和之前一樣，我們?nèi)匀皇褂?Python 來(lái)實(shí)現(xiàn)該動(dòng)作規(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)

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

其中紅線就是我們的全局路徑，藍(lán)點(diǎn)為障礙物。定義一些參數(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ù)測(cè)時(shí)間[s]MINT=4.0#最小預(yù)測(cè)時(shí)間[s]TARGET_SPEED=30.0/3.6#目標(biāo)速度（即縱向的速度保持）[m/s]D_T_S=5.0/3.6#目標(biāo)速度采樣間隔[m/s]N_S_SAMPLE=1#目標(biāo)速度的采樣數(shù)量ROBOT_RADIUS=2.0#robotradius[m]#損失函數(shù)權(quán)重KJ=0.1KT=0.1KD=1.0KLAT=1.0KLON=1.0

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

defcalc_frenet_paths(c_speed,c_d,c_d_d,c_d_dd,s0):frenet_paths=[]#采樣，并對(duì)每一個(gè)目標(biāo)配置生成軌跡fordiinnp.arange(-MAX_ROAD_WIDTH,MAX_ROAD_WIDTH,D_ROAD_W):#橫向動(dòng)作規(guī)劃forTiinnp.arange(MINT,MAXT,DT):fp=Frenet_path()#計(jì)算出關(guān)于目標(biāo)配置di，Ti的橫向多項(xiàng)式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ù)乘對(duì)應(yīng)的系數(shù)相加tfp.cf=KLAT*tfp.cd+KLON*tfp.cvfrenet_paths.append(tfp)returnfrenet_paths

其中，一個(gè)重要的類是五次多項(xiàng)式類，其定義如下：

classquintic_polynomial:def__init__(self,xs,vxs,axs,xe,vxe,axe,T):#計(jì)算五次多項(xiàng)式系數(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

這里的五次多項(xiàng)式的系數(shù)的求解過(guò)程和我們前面的理論講解是一樣的，只不過(guò)我們使用Numpy中的np.linalg.solve(A, b)方法將矩陣解了出來(lái)。最后，我們來(lái)看一下障礙物規(guī)避是如何實(shí)現(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ī)避問(wèn)題都簡(jiǎn)化為靜態(tài)了，所以在這里我們只簡(jiǎn)單地計(jì)算了所有規(guī)劃點(diǎn)到障礙物的距離，一句距離預(yù)計(jì)是否會(huì)發(fā)生碰撞，來(lái)看看完整的優(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ù)，軌跡檢查還會(huì)過(guò)濾掉一些軌跡，所以使用基于 Frenet 的優(yōu)化軌跡來(lái)做無(wú)人車的動(dòng)作規(guī)劃，通常能夠找到有限集的最優(yōu)解，當(dāng)最優(yōu)解無(wú)法通過(guò)檢查是，自會(huì)采用“次優(yōu)解”甚至更加“次優(yōu)的”解。

最后我們來(lái)看一下完整的動(dòng)作規(guī)劃效果：