無人駕駛汽車系統入門（二十一）——基於Frenet優化軌跡的無人車動作規劃方法

動作規劃動作在無人車規劃模塊的最底層，它負責根據當前配置和目標配置生成一序列的動作，我們前面討論的三次樣條插值實際上只是一個簡單的路徑，而非我們最終能夠執行的軌跡，本文介紹一種基於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$ 時刻車輛的橫向偏移，橫向速度和橫向加速度爲 d0,d0˙,d0¨$d_0,\dot{d_0},\ddot{d_0}$ ，即可得方程組：

d(t0)=αd0+αd1t0+αd2t20+αd3t30+αd4t40+αd5t50$$d(t_0)={\alpha }_{d0}+{\alpha }_{d1}{t}_0+{\alpha }_{d2}{t}_0^2+{\alpha }_{d3}{t}_0^3+{\alpha }_{d4}{t}_0^4+{\alpha }_{d5}{t}_0^5$$

d˙(t0)=αd1+2αd2t0+3αd3t20+4αd4t30+5αd5t40$$\dot{d}(t_0)={\alpha }_{d1}+2{\alpha }_{d2}{t}_0+3{\alpha }_{d3}{t}_0^2+4{\alpha }_{d4}{t}_0^3+5{\alpha }_{d5}{t}_0^4$$

d¨(t0)=2αd2+6αd3t0+12αd4t20+20αd5t30$$\ddot{d}(t_0)=2{\alpha }_{d2}+6{\alpha }_{d3}{t}_0+12{\alpha }_{d4}{t}_0^2+20{\alpha }_{d5}{t}_0^3$$

爲了區分橫向和縱向，我們使用 αdi${\alpha }_{di}$ 和 αsi${\alpha }_{si}$ 來分別表示d和s方向的多項式係數，同理，根據橫向的目標配置 D1=[d1,d1˙,d1¨]$D_1=[d_1,\dot{d_1},\ddot{d_1}]$ 可得方程組：

d(t1)=αd0+αd1t1+αd2t21+αd3t31+αd4t41+αd5t51$$d(t_1)={\alpha }_{d0}+{\alpha }_{d1}{t}_1+{\alpha }_{d2}{t}_1^2+{\alpha }_{d3}{t}_1^3+{\alpha }_{d4}{t}_1^4+{\alpha }_{d5}{t}_1^5$$

d˙(t1)=αd1+2αd2t1+3αd3t21+4αd4t31+5αd5t41$$\dot{d}(t_1)={\alpha }_{d1}+2{\alpha }_{d2}{t}_1+3{\alpha }_{d3}{t}_1^2+4{\alpha }_{d4}{t}_1^3+5{\alpha }_{d5}{t}_1^4$$

d¨(t1)=2αd2+6αd3t1+12αd4t21+20αd5t31$$\ddot{d}(t_1)=2{\alpha }_{d2}+6{\alpha }_{d3}{t}_1+12{\alpha }_{d4}{t}_1^2+20{\alpha }_{d5}{t}_1^3$$

我們通過令 t0=0$t_0=0$ 來簡化這個六元方程組的求解，可直接求得 αd0${\alpha }_{d0}$ ，αd1${\alpha }_{d1}$ 和 αd2${\alpha }_{d2}$ 爲：

αd0=d(t0)$${\alpha }_{d0}=d(t_0)$$

αd1=d˙(t0)$${\alpha }_{d1}=\dot{d}(t_0)$$

αd2=d¨(t0)2$${\alpha }_{d2}=\frac{\ddot{d}(t_0)}{2}$$

令 T=t1−t0$T=t_1-t_0$ ，剩餘的三個係數 αd3,αd4,αd5${\alpha }_{d3},{\alpha }_{d4},{\alpha }_{d5}$ ，可通過解如下矩陣方程得到：

⎡⎣⎢⎢⎢T33T26TT44T312T2T55T420T3⎤⎦⎥⎥⎥×⎡⎣⎢⎢αd3αd4αd5⎤⎦⎥⎥=⎡⎣⎢⎢⎢d(t1)−(d(t0)+d˙(t0)T+12d¨(t0)T2)d˙(t1)−(d˙(t0)+d¨(t0)T)d¨(t1)−d¨(t0)⎤⎦⎥⎥⎥$$\left[\begin{array}{ccc}{T}^3& T^4& T^5\\ 3T^2& 4T^3& 5T^4\\ 6T& 12T^2& 20T^3\end{array}\right]\times \left[\begin{array}{c}{\alpha }_{d3}\\ {\alpha }_{d4}\\ {\alpha }_{d5}\end{array}\right]=\left[\begin{array}{c}d(t_1)-(d(t_0)+\dot{d}(t_0)T+\frac{1}{2}\ddot{d}(t_0)T^2)\\ \dot{d}(t_1)-(\dot{d}(t_0)+\ddot{d}(t_0)T)\\ \ddot{d}(t_1)-\ddot{d}(t_0)\end{array}\right]$$

該方程的解可以通過Python的Numpy中的 np.linalg.solve 簡單求得。至此，我們在給定任意的初始配置 D0=[d0,d0˙,d0¨]$D_0=[d_0,\dot{d_0},\ddot{d_0}]$ ，目標配置 D1=[d1,d1˙,d1¨]$D_1=[d_1,\dot{d_1},\ddot{d_1}]$ 以及制動時間 T$T$ 的情況下，可以求的對應的 d$d$ 方向關於時間 t$t$ 的五次多項式的係數，同理，可以使用相同的方法來求解縱向（即 s$s$ 方向）的五次多項式係數。那麼問題來了，我們如何去確定最優的軌跡呢？ Werling方法的思路是通過一組目標配置來求得軌跡的備選集合，然後在備選集合中基於Jerk最小化的原則選擇最優軌跡 ，我們仍然以 d$d$ 方向的優化軌跡爲例講解：

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

[d1,d1˙,d1¨,T]ij=[di,0,0,Tj]$$[d_1,\dot{d_1},\ddot{d_1},T{]}_{ij}=[d_i,0,0,T_j]$$

對於優化問題而言，我們實際上希望車輛最終沿着參考線（道路中心線）平行的方向行駛，所以我們令 di˙=di¨=0$\dot{d_i}=\ddot{d_i}=0$ ，那麼目標配置只涉及 di$d_i$ 和 Tj$T_j$ 兩個變量的組合，而這兩個變量在無人駕駛的應用場景中實際上是受限的，我們可以通過定義 (dmin,dmax)$(d_{min},d_{max})$ 和 (Tmin,Tmax)$(T_{min},T_{max})$ 來約束目標配置的取值範圍，通過 Δd$\mathrm{\Delta }d$ 和 ΔT$\mathrm{\Delta }T$ 來限制採樣密度，從而在每一個制動週期獲得一個有限的備選軌跡集合，如下圖所示：

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

Cd=kjJt(d(t))+ktT+kdd21$$C_d=k_j{J}_t(d(t))+k_{t}T+k_d{d}_1^2$$

該損失函數包含三個懲罰項：
* kjJt(d(t))$k_j{J}_t(d(t))$ ：懲罰Jerk大的備選軌跡；
* ktT$k_{t}T$ ：制動應當迅速，時間短；
* kdd21$k_d{d}_1^2$ ：目標狀態不應偏離道路中心線太遠

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

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

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

在本文中我們詳細瞭解車速保持場景下的縱向軌跡優化，在高速公路等應用場景中，目標配置中並不需要考慮目標位置（即 s1$s_1$ ），所以在該場景下，目標配置仍然是 (s0,s0˙,s0¨)$(s_0,\dot{s_0},\ddot{s_0})$ ，目標配置變成了 (s1˙,s1¨)$(\dot{s_1},\ddot{s_1})$ ，損失函數爲：

Cs=kjJt(s(t))+ktT+ks˙(s1˙−sc˙)2$$C_s=k_j{J}_t(s(t))+k_{t}T+k_{\dot{s}}(\dot{s_1}-\dot{s_c}{)}^2$$

其中 sc˙$\dot{s_c}$ 是我們想要保持的縱向速度，第三個懲罰項的引入實際上是爲了讓目標配置中的縱向速度儘可能接近設定速度，該情景下的目標配置集爲：

[s1˙,s1¨,T]ij=[[sc˙+Δsi˙],0,Tj]$$[\dot{s_1},\ddot{s_1},T{]}_{ij}=[[\dot{s_c}+\mathrm{\Delta }\dot{s_i}],0,T_j]$$

即優化過程中的可變參數爲 Δsi˙$\mathrm{\Delta }\dot{s_i}$ 和 Tj$T_j$ ，同樣，也可以通過設置 ΔT$\mathrm{\Delta }T$ 和 ΔΔsi˙$\mathrm{\Delta }\mathrm{\Delta }\dot{s_i}$ 來設置軌跡採樣的密度，從而獲得一個有限的縱向軌跡集合：

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

Ctotal=klatCd+klonCs$$C_{total}=k_{lat}{C}_d+k_{lon}{C}_s$$

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

事故避免（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  # Delta T [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  # robot radius [m]

# 損失函數權重
KJ = 0.1
KT = 0.1
KD = 1.0
KLAT = 1.0
KLON = 1.0

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

def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
    frenet_paths = []

    # 採樣，並對每一個目標配置生成軌跡
    for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):

        # 橫向動作規劃
        for Ti in np.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 = [t for t in np.arange(0.0, Ti, DT)]
            fp.d = [lat_qp.calc_point(t) for t in fp.t]
            fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
            fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
            fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]

            # 縱向速度規劃 (速度保持)
            for tv in np.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) for t in fp.t]
                tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
                tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
                tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]

                Jp = sum(np.power(tfp.d_ddd, 2))  # square of jerk
                Js = sum(np.power(tfp.s_ddd, 2))  # square of jerk

                # square of diff from target speed
                ds = (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.cv

                frenet_paths.append(tfp)

    return frenet_paths

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

class quintic_polynomial:
    def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
        # 計算五次多項式係數
        self.xs = xs
        self.vxs = vxs
        self.axs = axs
        self.xe = xe
        self.vxe = vxe
        self.axe = axe

        self.a0 = xs
        self.a1 = vxs
        self.a2 = axs / 2.0

        A = 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]

    def calc_point(self, t):
        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
             self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5

        return xt

    def calc_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 ** 4

        return xt

    def calc_second_derivative(self, t):
        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3

        return xt

    def calc_third_derivative(self, t):
        xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2

        return xt

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

def check_collision(fp, ob):
    for i in range(len(ob[:, 0])):
        d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
             for (ix, iy) in zip(fp.x, fp.y)]

        collision = any([di <= ROBOT_RADIUS ** 2 for di in d])

        if collision:
            return False

    return True

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

def check_paths(fplist, ob):
    okind = []
    for i in range(len(fplist)):
        if any([v > MAX_SPEED for v in fplist[i].s_d]):  # 最大速度檢查
            continue
        elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]):  # 最大加速度檢查
            continue
        elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]):  # 最大麴率檢查
            continue
        elif not check_collision(fplist[i], ob):
            continue

        okind.append(i)

    return [fplist[i] for i in okind]

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

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

完整代碼鏈接：https://download.csdn.net/download/adamshan/10494062