運動規划是無人車導航的和核心技術之一，在不同場景會有不同的應對算法。本文主要介紹在高速公路場景下的軌跡規划算法。
一、Frenet坐標系
無人車在運行中會交替使用笛卡爾坐標系和 Frenet坐標系，在進行運動路徑規划時，普遍采用Frenet坐標系。 如圖1 左圖所示，以道路的中心線作為參考線，沿着參考線的方向為S軸，垂直參考線向外的為D軸，相比於右邊的笛卡爾坐標系，Frenet 坐標系可以降低汽車的運動狀態描述難度，因為參考線一直是道路的中心線，根據道路中線可以根據縱向距離s，橫向距離d來表示汽車的當前位置。s和d，再加上運行的時間 t，就是汽車在軌跡規划時的參考依據。
圖1. 無人車坐標系
二、Jerk最小化
Jerk最小化體現了關於物體運動過程中的顛簸程度，顛簸程度越小，乘車體驗越好。 Jerk最小化這需要在運動過程中累計的加速度的變化總和盡可能小。以縱向距離s為例，如圖2所示汽車要從A位置到B位置，已知兩個時刻汽車的位置\(s\)，速度\(\dot{s}\)，加速度\(\ddot{s}\)和所運行的時間\(\Delta t\)，我們需要刻畫\(\Delta t\)時間內的軌跡形式過程\(s(t)\)。
圖2. 汽車運動狀態參數
因為顛簸程度是與加速度的變化\(\dddot{s}\)相關，所以沿着這條軌跡行駛過程中累計的總顛簸量\(Jerk_{sum}=\int_{0}^{t_f}\dddot{s}(t)^2dt\)。如果要最小化這個\(Jerk_{sum}\)，就必須將\(s(t)\)的所有時間導數為0，即
\(\frac{d^ms}{dt^m}=0, \forall m\geqslant 6\)
通過這個性質，我們可以把\(s(t)\)表示為含有6個系數的5次多項式，即
\(s(t) = \alpha _0 + \alpha _1t+ \alpha _2t^2 + \alpha _3t^3+\alpha_4t^4+\alpha_5t^5\)
這只是縱向移動的s，同理橫向移動的d也適用。
三、多項式求解
為了使\(Jerk_{sum}\)最小化，我們獲得了一個含有6個系數的一維Jerk軌跡。關於求解這6個參數，我們可以借助已知的汽車狀態，即在兩個時刻的位置\(s\)，速度\(\dot{s}\)和加速度\(\ddot{s}\)。通過已知的\([s_i,\dot{s_i},\ddot{s_i},s_f,\dot{s_f},\ddot{s_f}, \Delta t]\)，可求解方程組
\(s(t) = \alpha _0 + \alpha _1t+ \alpha _2t^2 + \alpha _3t^3+\alpha_4t^4+\alpha_5t^5\)
\(\dot{s(t)} = \alpha _1+ 2\alpha _2t + 3\alpha _3t^2+ 4\alpha_4t^3+ 5\alpha_5t^4\)
\(\ddot{s(t)} = 2\alpha _2 + 6\alpha _3t+ 12\alpha_4t^2+20\alpha_5t^3\)
令\(t=0\)，可以直接得到\([\alpha _0, \alpha _1,\alpha _2]=[s_i,\dot{s_i},\frac{1}{2}\ddot{s_i}]\)，將\(\alpha _0\), \(\alpha _1\),\(\alpha _2\)的值帶入后，得
\(s(t) = s_i + \dot{s_i}t+ \frac{1}{2}\ddot{s_i}t^2 + \alpha _3t^3+\alpha_4t^4+\alpha_5t^5\)
\(\dot{s(t)} = \dot{s_i}+ \ddot{s_i}t + 3\alpha _3t^2+ 4\alpha_4t^3+ 5\alpha_5t^4\)
\(\ddot{s(t)} = \ddot{s_i} + 6\alpha _3t+ 12\alpha_4t^2+20\alpha_5t^3\)
接着再將\([s_f,\dot{s_f},\ddot{s_f}, \Delta t]\)帶入，即
最終獲得該Jerk軌跡的具體參數。
