動作規劃動作在無人車規劃模組的最底層，它負責根據當前配置和目標配置生成一序列的動作，我們前面討論的三次樣條插值實際上只是一個簡單的路徑，而非我們最終能夠執行的軌跡，本文介紹一種基於Frenet座標系的優化軌跡動作規劃方法，該方法在高速情況下的高階車道保持和無人駕駛都具有很強的實用性，是目前普遍採用的一種動作規劃演算法。

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

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

T=tend−tstart$$T=t_{end}-t_{start}$$

為什麼使用Frenet座標系

在Frenet座標系中，我們使用道路的中心線作為參考線，使用參考線的切線向量 t$t$ 和法線向量 n$n$ 建立一個座標系，如下圖的右圖所示，這個座標系即為Frenet座標系，它以車輛自身為原點，座標軸相互垂直，分為

s$s$ 方向（即沿著參考線的方向，通常被稱為縱向，Longitudinal）和 d$d$ 方向（即參考線當前的法向，被稱為橫向，Lateral），相比於笛卡爾座標系（下圖的作圖），Frenet座標系明顯地簡化了問題，因為在公路行駛中，我們總是能夠簡單的找到道路的參考線（即道路的中心線），那麼基於參考線的位置的表示就可以簡單的使用縱向距離（即沿著道路方向的距離）和橫向距離（即偏離參考線的距離）來描述，同樣的，兩個方向的速度（s˙$\dot{s}$ 和 d˙$\dot{d}$）的計算也相對簡單。

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

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

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

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

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

Jt(p(t))=∫t1t0p(τ)2dτ$$J_t(p(t))={\int }_{t_0}^{t_1}p(\tau {)}^{2}d\tau$$

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

p(t)=α0+α1t+α2t2+α3t3+α4t4+α5t5$$p(t)={\alpha }_0+{\alpha }_{1}t+{\alpha }_2{t}^2+{\alpha }_3{t}^3+{\alpha }_4{t}^4+{\alpha }_5{t}^5$$

要解這個方程組需要一些初始配置和目標配置，以橫向路徑規劃為例，初始配置為 D0=[d0,d0˙,d0¨]$D_0=[d_0,\dot{d_0},\ddot{d_0}]$ ，即 t0$t_0$ 時刻車輛的橫向偏移，橫向速度和橫向加速度為