## 基于Frenet坐标优化轨迹的无人车动作规划方法

1. Frenet坐标系下规划的优势
2. Jerk最小化和五次轨迹多项式求解

$T = t_1-t_0 \tag{1}$

## Jerk最小化和五次轨迹多项式求解

$$\\ J_t(p(t)) = \intop\nolimits_{t_0}^{t_1} p(\tau)^2d\tau \tag{2}$$

$$\\ p(t) =\alpha_0 + \alpha_1t + \alpha_2t^2 + \alpha_3t^3 + \alpha_4t^4 + \alpha_5t^5 \tag{3}$$

$$\\ 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 \tag{4}$$ $$\\ \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 \tag{5}$$ $$\\ \ddot{d}(t_0) =2\alpha_{d2} + 6\alpha_{d3}t_0 + 12\alpha_{d4}t_0^2 + 20\alpha_{d5}t_0^3 \tag{6}$$

$\alpha_{si}$来分别表示 d 和 s 方向的多项式系数，同理，根据横向的目标配置$D_1 =[d_1, \dot{d_1}, \ddot{d_1}]$可得方程组：

$$\\ 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 \tag{7}$$ $$\\ \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 \tag{8}$$ $$\\ \ddot{d}(t_1) =2\alpha_{d2} + 6\alpha_{d3}t_1 + 12\alpha_{d4}t_1^2 + 20\alpha_{d5}t_1^3 \tag{9}$$

$t_0=0$

$\alpha_{d1}$ 和

$\alpha_{d2}$ 为：

$$\\ \alpha_{d0} = d(t0) \tag{10}$$ $$\\ \alpha_{d1} = \dot{d}(t_0) \tag{11}$$ $$\\ \alpha_{d2} = \ddot{d}(t_0) \tag{12}$$

$T=t_1 − t_0$

$\alpha_{d4}$,

$\alpha_{d5}$ ，可通过解如下矩阵方程得到：

$$\\ \begin{bmatrix}T^3&T^4&T^5\\3T^2&4T^3&5T^4\\6T&12T^2&20T^3\end{bmatrix} \times \begin{bmatrix}\alpha_{d3}\\\alpha_{d4}\\\alpha_{d5}\end{bmatrix} = \begin{bmatrix}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{bmatrix} \tag{13}$$

$$\\ [d_1, \dot{d_1}, \ddot{d_1}, T]_{ij} = [d_i, 0, 0, T_j] \tag{14}$$

$($d_{min}$,$d_{max}$)$

$($T_{min}, T_{max}$)$

$$\\ C_d = k_jJ_t(d(t)) + k_tT + k_dd_1^2 \tag{15}$$

• $k_jJ_t(d(t))$ ：惩罚Jerk大的备选轨迹；
• $k_tT$ ：制动应当迅速，时间短；
• $k_dd_1^2$ ：目标状态不应偏离道路中心线太远

• 跟车
• 汇流和停车
• 车速保持

$$\\ C_s = k_jJ_t(s(t)) + k_tT + k_{\dot{s}}(\dot{s_1} - \dot{s_c})^2 \tag{16}$$

$$\\ [\dot{s_1}, \ddot{s_1}, T]_{ij} = [[\dot{s_c} + \Delta\dot{s_i}], 0, T_j] \tag{17}$$

$$\\ C_{total} = k_{lat}C_d + k_{lon}C_s \tag{18}$$

• s方向上的速度是否超过设定的最大限速
• s方向的加速度是否超过设定的最大加速度
• 轨迹的曲率是否超过最大曲率
• 轨迹是否会引起碰撞（事故）

Appendix :Paper ‘Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame’

• The paper discusses some topics like:

• Cost Functions.
• Differences between high speed and low speed trajectory generation.
• Implementation of specific maneuvers relevant to highway driving like following, merging, and velocity keeping.
• How to combining lateral and longitudinal trajectories.
• A derivation of the transformation from Frenet coordinates to global coordinates (in the appendix).
• Abstract

• semi-reactive trajectory generation method
• be tightly integrated into the behavioral layer
• realize long-term objectives (such as velocity keeping, merging, following, stopping)
• combine with a reactive collision avoidance
• Frenét-Frame
• Related work

• [11], [19], [2], [4]: fail to model the inherent unpredictability of other traffic, and the resulting uncertainty, given that they rely on precise prediction of other traffic participant’s motions over a long time period.
• [16], [1], [7]: The trajectories are represented parametrically. A finite set of trajectories is computed, typically by forward integration of the differential equations that describe vehicle dynamics.While this reduces the solution space and allows for fast planning, it may introduce suboptimality.
• [9]: a tree of trajectories is sampled by simulating the closed loop system using the rapidly exploring random tree algorithm [10].
• [17]: in a similar spirit to our method but only considers the free problem that is not constrained by obstacle.
• We propose a local method, which is capable of realizing high-level decisions made by an upstream, behavioral layer (long-term objectives) and also performs (reactive) emergency obstacle avoidance in unexpected critical situations.
• Optimal control approach

• system inputs or curvature to be polynomials.
• cost functional is compliance with Bellman’s principle of optimality.
• making the best compromise between the jerk and the time.
• not limited to a certain function class, the problem becomes highly complicated and can be solved numerically at best.

A quintic polynomial through the same points and the same time interval will always lead to a smaller cost.

• Generation of lateral movement
• High speed trajectories
• at high speed, d(t) and s(t) can be chosen independently.
• cost function: g(T)=T, h(d1)=d1^2.
• process:
1. calculate its coefficients and T minimizing.
2. check it against collision.
3. if not, check and find the second best and collision-free trajectory.
• Low speed trajectories
• at low speed, we must consider the non-holonomic property (invalid curvatures) of the car.
• cost function: see in the paper.
• Generation of longitudianal movement
• Following
• safe distance (constant time gap law)
• Merging
• Stopping
• Velocity keeping
• Combining lateral and longitudinal trajectories
• check aginst outsized acceleration values first.
• derive higher order infomation (heading, curvature, velocity, acceleration)
• calculate the conjoint cost: Cost_total = w_lat Cost_lat + w_lon Cost_lon
• for collison dectection: we add a certain safety distance to the size of our car on each side.