[Paper Notes] Optimal trajectory generation for dynamic street scenarios in a Frenét Frame

A method of horizontal and vertical decoupling optimization planning of the classic Frenet coordinate system.
Due to the limitations of personal abilities, there may be some mistakes. Please criticize and correct me. I hope to learn from everyone together.

Abstract

• A semi-reactive trajectory generation algorithm is proposed , which can be well integrated into the decision-making layer. This method combines passive collision with cruise control, merging, car following, etc. based on the optimal control strategy in the Frenet coordinate system.
• Find reasonable approximations in the set of optimal solutions to unconstrained optimization problems to solve constrained optimization problems and select the best solution to satisfy constraints .
• High speeds are decoupled horizontally and vertically from each other; low speeds are different.

I. INTRODUCTION

A. Motivation

Traditional algorithms have some limitations and are difficult to apply to dynamic and complex environments and high-speed scenarios. The method proposed by the author can:

1. It is well adapted to highway scenarios and can generate driving actions that keep the speed constant (meaning that operations such as lane changing and merging are independent of driving speed) and transfer speed and distance control to the planning level.
2. Reactive (passive?) obstacle avoidance is achieved by combining steering, braking/acceleration.

B. Related work

• [11], [19], [2], [4]: ​​Use a global trajectory connecting the starting point and a distant possible target state, but this type of method does not consider various uncertainties in the driving process of autonomous vehicles . Relies on the accurate prediction of the future behavior trajectories of other traffic participants .
• [16], [1], [7]: The discrete optimization method is used: the finite trajectory library (discretization of the control space) is obtained through forward integration of the vehicle dynamics differential equation . Use the trajectory library to evaluate and score using the cost function for local path planning. The types of trajectories can take many forms. However, this approach may be suboptimal. And this method may produce overshoot or even instability , as shown in the figure below.
  
  This figure shows that trajectories with high replanning frequency can avoid or reduce the overshoot and even instability problems encountered by trajectories with low replanning frequency.

PS1: Similar papers Maximum Likelihood Path Planning for Fast Aerial Maneuvers and Collision Avoidance Ji Zhang, Chen Hu, Rushat Gupta Chadha, and Sanjiv Singh
PS2: Notes from previous mobile robot motion planning courses: https://blog.csdn.net/ sinat_52032317/article/details/132125557

• [9]: Sampling the trajectory tree through RRT simulation closed-loop system [10].
• [17]: Similar idea to this paper, but does not consider obstacles. The concept of Galerkin base was mentioned.

PS: Galerkin base : Galerkin base is a basis function that can describe the dynamic behavior of a system in function space. In optimal control, the Galerkin basis is used to represent the state and control space of the system in order to find the optimal control trajectory. Simply put, the Galerkin basis is a mathematical method used to express the dynamic behavior of a system as a linear combination of a set of basis functions, thereby simplifying the modeling and control of the system.

PS: Bellman's optimality principle is one of the basic principles of dynamic programming. This principle states that in any case, the optimal sequence of decisions contains the optimal sequence of decisions for the current decision. That is, if we find the optimal decision sequence in a problem, then every decision in this sequence is the optimal decision for this problem , regardless of the previous decision sequence. This principle is the basis of the dynamic programming algorithm, which gradually decomposes complex problems into simple problems and solves them step by step.

II. OPTIMAL CONTROL APPROACH

The author uses the idea of ​​introducing optimal control in trajectory generation to ensure the temporal consistency of the trajectory.

---

Q: Generally speaking, the cost function method may not comply with the Bellman optimality principle, and the planned trajectory will be different during the next planning. So how did this article do it?

A: The method in this article conforms to the Bellman optimality principle because during the trajectory generation process, the trajectory tracking problem is optimized into an optimal control problem and a cost function is selected that conforms to the optimality principle. The principle of optimality states that when an optimal solution is found, it will be retained until some aspect of the problem changes. In our approach, once the optimal solution is found, the vehicle follows the remaining portion of the precomputed trajectory at each planning step, thus ensuring the temporal consistency of the trajectory. The temporal consistency of the trajectory is visually demonstrated in the following figures.

---

• In terms of optimal control, Bellman's optimization principle is mainly followed to select the cost
  function. On the basis of minimizing the cost, the trajectory can also be as close as possible to the ideal driving form of the vehicle.

  Ideal self-driving vehicle kinematic behavior : good balance of comfort (with $jerk$ measure) and the time to return to the desired position ($t_{end}-t_{start}$)。

• When solving general restricted optimization problems, since the problem is not limited to a certain function class, the problem becomes very complex and can only be solved numerically at best. Therefore, the method proposed by the author is to find reasonable approximations in the optimal solution set of the unconstrained (free) optimization problem to solve the constrained optimization problem and select the best solution to satisfy the constraints. This means that once the optimal solution is found to be valid (the constraints are no longer active), the temporal consistency of non-reactive trajectories is guaranteed.

  Simply put, it is to use the optimal solution sequence obtained under the unconstrained optimization problem to approximate the constrained optimization problem.

III. MOTION PLANNING IN THE FREN ́ET FRAME

A better method in trajectory tracking control is the Frenet coordinate system method, which can

• $SE\left(2\right):=SO(2)\times {\mathbb{R}}^{2.}$ The special Euclidean subgroup has good tracking performance (invariant tracking
  performance)

  PS: The special Euclidean subgroup SO(2) refers to the two-dimensional rotation group, which is composed of all two-dimensional rotation matrices. The form is: SO (2) = { ( $SO(2)=\left\{\left(\begin{array}{cc}\cos \left(\theta \right)& -\sin \left(\theta \right)\\ \sin (\theta & \cos (\theta \end{array}\right)|i\in [0,2\pi )\right\}$ where$\theta$ represents the rotation angle.
  The special Euclidean subgroup SE(2) refers to the two-dimensional Euclidean group, which consists of all two-dimensional rigid body motion matrices, including translation and rotation, in the form: SE (2) = { ( cos$SE\left(2\right)=\left\{\left(\begin{array}{ccc}\cos \left(\theta \right)& -\sin \left(\theta \right)& x\\ \sin \left(\theta \right)& \cos \left(\theta \right)& y\\ 0& 0& 1\end{array}\right)|i\in [0,2p),x,y\in \mathbb{R}\right\}$
  Among them $\theta$ represents the rotation angle,$x$和$y$ represents the translation amount. The two-dimensional Euclidean group is very important because it is a common motion group in fields such as robotics and computer vision, and can be used to describe the motion of a robot on a plane.

• In planning problems under the Frenet coordinate system, reference lines are very important. The reference line can be the road centerline or the rough trajectory generated by path planning under unstructured roads.
  One-dimensional trajectories in the horizontal and vertical directions are generated based on the dynamic reference system. The relative relationship is as follows: $$\vec{x}(s(t),d(t))=\vec{r}(s(t))+d(t){\vec{n}}_r(s(t))$$$\vec{r}$is the reference point on the center line; $d\left(t\right)$ is the lateral deviation;$s\left(t\right)$ refers to
  the arc length passing through a period of time along the direction of the center line;${\vec{t}}_x,{\vec{n}}_x$are the tangential and normal vectors of the trajectory respectively.

This article uses a fifth-degree polynomial as the trajectory, which can achieve the effect of minimum jerk and construct the classic BVP/OBVP problem. Previous related notes: https://blog.csdn.net/sinat_52032317/article/details/132125557

$$J_t(p(t)):={\int }_{t_0}^{t_1}{\ddot{p}}^2\left(\tau \right)d\tau .Starting$$
state:$P_0=[p_0,{\dot{p}}_0,{\ddot{p}}_0]$ , starting time$t_0$;
End state: $P_1=[{\dot{p}}_1,{\ddot{p}}_1]$ , end time$t_1=t_0+T$;

The cost function is a fifth degree polynomial:
$$C=k_j{J}_t+k_{t}g(T)+k_{p}h(p_1)$$
wheregg$g$ and$h$ is any function,$k_j,k_t,k_p$>0
Take a look at the proof process of the paper (prove cost function $C$ is also a fifth-degree polynomial):
Proof by contradiction:
If not, then$J_t$will always be smaller than the form of the fifth degree polynomial.

IV. GENERATION OF LATERAL MOVEMENT

PS: In the field of optimal control, "target manifold" is a term that refers to the set of state variables that the control input needs to achieve in the control system. In an optimal control problem, the objective manifold is the part of the optimization problem that describes the constraints that the system must obey. In the paper, the author puts ${\dot{d}}_1=d_1=0$ as the target manifold so that the car moves parallel along the center line.

A. High Speed Trajectories

At high speed, horizontal and vertical decoupling and independent calculation are adopted.

The starting state is calculated from the previous trajectory to ensure continuity: $D_0=[d_0,{\dot{d}}_0,{\ddot{d}}_0]$ . In order to make the trajectory parallel to the center line, the final state${\dot{d}}_1,=0{\ddot{d}}_1=0$ . Then$g(T)=T,h(d_1)=d_1^2$(The former is to accelerate convergence, and the latter is to reduce the deviation from the center line), the final cost function is obtained:
$$C_d=k_j{J}_t(d(t))+k_{t}T+k_d{d}_1^2,$$

The process of trajectory generation is explained with reference to the figure below.

The first step is not to directly calculate the trajectory and then adjust the relevant parameters. The author first generated a trajectory library containing the final state $d_i$and the experienced time $T_j$:
$$[d_1,{\dot{d}}_1,{\ddot{d}}_1,T{]}_{ij}=[d_i,0,0,T_j]$$

Step 2: Select the lowest cost effective trajectory.

The trajectory planned in each cycle will continue to be planned along the optimal trajectory planned in the previous cycle. In this way, it can comply with Bellman's optimality principle and achieve time consistency.

B. Low Speed Trajectories

For low-speed scenarios, the horizontal and longitudinal decoupling method ignores the non-integrity constraints of the vehicle, so the trajectory may not be effective. The relative relationship at low speed is as follows:
$$\vec{x}(s(t),d(t))=\vec{r}(s(t))+d(s(t)){\vec{n}}_r\left(s\right(t)).$$ Why not do this in high-speed scenarios?

For low-speed scenes, you can set a speed threshold in the behavioral layer. When the speed is lower than this threshold, a horizontal trajectory is generated based on longitudinal motion.

Cost function in low-speed scenarios.
The generation of the trajectory library is similar to the high-speed situation, but attention should be paid to $d^{{}^{\prime }}$ is for$s$求导
$$[d_1,{\dot{d}}_1,{\ddot{d}}_1,T{]}_{ij}=[d_i,0,0,T_j]$$

V. GENERATION OF LONGITUDINAL MOVEMENT

Horizontal planning mainly uses time/distance as the main evaluation criteria. The evaluation annotations here are mainly comfort and safety, so vertical jerk is introduced as a consideration for optimization problems.

Use diagrams to understand the process of vertical planning.

For scenarios such as car following, merging, and parking, a target state $s_{target}\left(t\right)$ serves as a guide, and in each cycle, the end constraints are updated (there will be different$\Delta s{\_}_i$and $T_j$)。
$S_0=[s_0,{\dot{s}}_0,{\ddot{s}}_0]$
$$[s_1,{\dot{s}}_1,{\ddot{s}}_1,T{]}_{ij}=[[s_{\text{target}}(T_j)+\Delta s{\_}_i],{\dot{s}}_{\text{target}}(T_j),{\ddot{s}}_{\text{target}}\left(T_j\right),T_j]$$
The cost function is:$C_t=k_j{J}_t+k_{t}T+k_s[s_1-s_d{]}^2.$

Following

mentioned constant time gap law

PS: " constant time gap law " refers to the international traffic rules that require a vehicle to maintain a safe distance from the vehicle in front of it at a certain time interval. This distance will change with the change of speed. This time gap is considered to be a constant, hence the name "constant time gap law". The purpose of this rule is to ensure a safe distance between vehicles traveling on highways and other high-speed roads to ensure driving safety.

$$s_{target}(t):=s_{lv}\left(t\right)-[D_0+t{\dot{s}}_{lv}(t)]$$

$D_0,\tau$ is fixed;$s_{lv},{\dot{s}}_{lv}$are the position and speed of the vehicle in front respectively.
Some data of the vehicle in front need to be obtained through perception prediction, where we assume that the acceleration of the vehicle in front remains unchanged, ${\ddot{s}}_{lv}\left(t\right)={\ddot{s}}_{lv}\left(t_0\right)=const.$ Integrating over time gives:

$$\begin{array}{rl}{\dot{s}}_{lv}(t)& ={\dot{s}}_{lv}(t_0)+{\ddot{s}}_{lv}(t_0)[t-t_0]\\ s_{lv}(t)& =s_{lv}\left(t_0\right)+{\dot{s}}_{lv}\left(t_0\right)[t-t_0]+\frac{1}{2}{\ddot{s}}_{lv}\left(t_0\right)[t-t_0{]}^2,\end{array}$$

Then get the remaining variables of the target state.
$$\begin{array}{rl}{\dot{s}}_{\mathrm{target}}(t)& ={\dot{s}}_{lv}\left(t\right)-t{\ddot{s}}_{lv}(t),\\ {\ddot{s}}_{target}(t)& ={\ddot{s}}_{lv}(t_1)-t{\ddot{s}}_{lv}\left(t\right)={\ddot{s}}_{lv}(t_1).\end{array}$$

Merging and Stopping

$$s_{target}\left(t\right)=\frac{1}{2}[s_a(t)+s_b(t)]$$
The target point of the merge is set between the two vehicles.
The parking scene is as follows:$s_{\mathrm{target}}=s_{\mathrm{stop}},{\dot{s}}_{target}\equiv 0,{\ddot{s}}_{target}\equiv 0.$

Velocity Keeping

For the situation where there is no car ahead, the cost function is as follows: $C_v=k_j{J}_t(s(t))+k_{t}T+k_{\dot{s}}[{\dot{s}}_1-{\dot{s}}_d{]}^{2By}$
using$D\dot{s}，T_j$To change the constraints of the final state, to generate the trajectory library of quartic polynomial: $[{\dot{s}}_1,{\ddot{s}}_1,T{]}_{ij}=[[{\dot{s}}_d+D{\dot{s}}_i],0,T_j]$

VI. COMBINING LATERAL AND LONGITUDINAL TRAJECTORIES

• First perform an acceleration limiter check on the trajectories and then merge them.

• Normally, the reference line is not
  in close form, but is represented by a pre-sampled point set (heading angle $i_r\left(s\right)$ , curvature$K_r\left(s\right)$ , derivative of curvature${\dot{K}}_r\left(s\right)$ ), higher-order information cannot be obtained by numerical solution.

  PS: close form is a mathematical concept, which refers to a mathematical expression that can be expressed by analytical expressions obtained by limited basic arithmetic operations and functional operations, as well as finite-order derivatives and integrals and other basic mathematical operations.

• The joint cost of the final trajectory is obtained by adding different weight coefficients to the horizontal and vertical directions: $C_{to}=k_{lat}{C}_{lat}+k_{lon}{C}_{lat}$

• Measures to reduce unnecessary side-passes: the scope of collision checking expands a little over time.

• When encountering a situation where the reference line needs to be changed, the terminal end points $(x,i_x,K_x,v_x,a_x)$ is projected onto the starting point of the new reference line$[s_0,{\dot{s}}_0,{\ddot{s}}_0,d_0,{\dot{d}}_0,{\ddot{d}}_0]$.$s_0$Exception, since no specific reference line shape is constrained. But it can be solved by constructing an optimization problem. $s=\underset{p}{\mathrm{argmin}}\Vert x-r\left(\sigma \right)\Vert$

The change in the color of the trajectory in the figure represents the change in cost.

VIII. EXPERIMENTS

Fig. 7. Simulated highway scenario: The center line is given by the middle lane and the desired speed is significantly higher than the traffic flow. Two completely reactive passing maneuvers are shown between t = 0.0 s and 7.75 s, as well as between t = 29.07 s and 50.0 s with predicted obstacle movements on Thor ≤ 3.0 s.

TRANSFORMATIONS FROM FREN ́ET COORDINATES TO GLOBAL COORDINATES

The appendix of this paper is also very useful, introducing the transformation from Frenet to global coordinates.

For further information, please refer to this blog. Frenet coordinate system and its conversion to Cartesian coordinates (1) principle

code

[1] https://atsushisakai.github.io/PythonRobotics/modules/path_planning/path_planning.html
[2] https://github.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/blob/master/Optimal%20Trajectory%20in%20a%20Frenet%20Frame/cubic_spline_planner.py
[3] https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning
[4] https://github.com/ndrplz/self-driving-car/blob/master/project_11_path_planning/src/coords_transform.h
[5] https://github.com/caokaifa/Matlab-planning
[6] https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
[7] https://www.cnblogs.com/kin-zhang/p/15006838.html