(4) [Track Optimization] Method 1: Based on the Frenet lane line coordinate system, use decoupled sampling quintic polynomial fitting for local planning

Series Article Directory

Tip: Here you can add the table of contents of all the articles in the series, and the table of contents needs to be manually added by yourself
TODO: finishing after writing

---

foreword

I have limited cognition, I hope everyone will bear with me, and I hope to communicate with you more if you have any questions, and grow together!

This article first makes a brief introduction to trajectory optimization based on the Frenet coordinate system, lane line decoupling sampling, and fifth-degree polynomial fitting . The specific content will be updated later. Other modules can refer to my other articles.

---

Tip: The following is the content of the text of this article

A. Chapter 1: Introduction of Frenet Lane Line Coordinate System and Coordinate Transformation

1. Introduction to the frenet coordinate system

1. Definition of Frenet coordinates

The Frenet coordinate system describes the position of the car relative to the road. S represents the distance along the road, also known as the ordinate, L represents the displacement from the longitudinal line, also known as the abscissa, the ordinate represents the distance traveled in the road, and the abscissa represents the distance the car deviates from the center line

Obstacles and trajectories can be projected onto the S vertical axis and the L horizontal axis through Frenet coordinates. The goal is to decompose the three-dimensional trajectory (longitudinal dimension, lateral dimension, and time dimension) into two separate two-dimensional problems, that is, by separating the longitudinal and lateral components of the trajectory. To solve, one two-dimensional trajectory refers to the longitudinal trajectory (ST trajectory) with a time stamp, and the other two-dimensional trajectory is laterally offset relative to the longitudinal trajectory (SL trajectory).

The frenet coordinate system is compared to the scene of the lane (reference line) line [the s-axis is parallel to the reference line, and the L-axis is perpendicular to the reference line], which is specially used in the lane line scene
.

2. ST diagram (longitudinal speed planning)

The ST diagram can help us design and select the speed curve, "s" represents the longitudinal displacement of the vehicle, and "t" represents the time. The curve on the ST diagram is a description of the vehicle's movement because it illustrates the position of the vehicle at different times. Since the speed is the rate of position change, the speed can be inferred from the ST diagram through the slope of the curve. The steeper the slope, the greater the movement in a short period of time corresponds to the faster speed .

Obstacles can be drawn within the ST diagram as rectangles that block sections of the road for a specific time period, and the speed curve must not intersect the rectangle

The termination state of the ST trajectory: According to the situation, the state can be divided into three groups: cruise, follow and stop.

.

3. SL diagram (lateral displacement planning)

The SL diagram shows the lateral displacement of the vehicle on different vertical coordinates, which is reflected in different vertical directions.

If in a high-speed scene

.

4. Definition of horizontal and vertical errors of frenet coordinate system

.

Two, the method of mutual conversion of coordinate systems

For planning, it is necessary to convert from Cartesian coordinate system to frenet coordinate system
For control, map, positioning and other modules, it is necessary to convert from frenet coordinate system to Cartesian coordinate system

1. Convert Cartesian coordinates to frenet coordinate system

(1) Project the trajectory to the st map of the frenet coordinate system

(2) Project the obstacle to the st map of the frenet coordinate system

(3) Convert the xy coordinates in the world coordinate system into the sl coordinate system of the car coordinate system

The conversion process from sl coordinate system to xy coordinate system is unique, and from xy coordinate system to sl coordinate system is not unique, but it can be unique when there are constraints

.
.

2. Convert Frenet coordinate system to Cartesian coordinate system

Once you have both ST and SL trajectories, you need to reconvert them to Cartesian coordinates, and then combine them to build a 3D trajectory consisting of 2D waypoints and 1D time stamps. The ST trajectory is the longitudinal displacement over time, and the lateral offset of each point on the SL longitudinal trajectory. Both trajectories have an ordinate S, and the trajectories can be merged by matching the S values

3. Program realization of coordinate transformation

Refer to Baidu's demo

Summarize

Both em planner and lattice planner use the frenet coordinate system, so it is necessary to understand

---

B. Chapter 2: Horizontal and vertical decoupling sampling generation path + Jerk trajectory optimization algorithm in Frenet Frame

[Core idea] The function of generating and optimizing the trajectory of unmanned vehicles based on the Frenet coordinate system

Function
Given the starting point and target point on the map, through the interpolation optimization problem of mini_jerk or mini_snap, the trajectory between the two points is solved and the trajectory between the two points is guaranteed to be optimal
.

The difference between the path generated based on the Frenet coordinate system method and the path generated directly by three-five splines (three-five polynomials)

The path generated directly by cubic quintic spline (three or five degree polynomial) does not consider the actual scene, and there is no cost loss function, so there is no function such as obstacle avoidance. For
the method of trajectory generation by cubic spline interpolation, refer to my other blog
https://blog.csdn.net/qq_35635374/article/details/121772237

The path generated based on the Frenet coordinate system method inherits the three-fifth spline method to generate the path method. By trying to give the target points in the horizontal and vertical directions of sd, a bunch of candidate paths are generated using the three-fifth spline method, and then the optimal trajectory is selected by solving the mini_jerk optimization problem and designing different cost loss function items, clipping, etc.

.
.

1. Definition of optimal trajectory sequence for unmanned driving

The Frenet coordinate system is divided into horizontal and vertical coordinate axes, because the discussion of the optimal trajectory sequence should be divided into the optimal trajectory sequence for horizontal control and the optimal trajectory sequence for vertical control

1. Optimal Trajectory Sequence for Lateral Control

For lateral control, assuming that the vehicle deviates from the desired lane line due to avoiding obstacles or changing lanes or other braking reasons, the optimal action sequence (or trajectory) at this time is the relatively safest, comfortable, simple and efficient trajectory under the limitation of the vehicle's braking ability.
.
.

2. Longitudinal Control Optimal Trajectory Sequence

The optimal longitudinal trajectory can also be defined in this way: if the vehicle is too fast at this time, or too close to the vehicle in front, then it must be decelerated, so what is "comfortable and simple" deceleration? We can use the physical quantity of Jerk to describe it. Jerk is the rate of change of acceleration, that is, jerk. Generally speaking, too high jerk will cause discomfort to the occupant. Therefore, in terms of ride comfort, the quantity of Jerk should be optimized. At the same time, the braking period T of the trajectory is introduced, which is the operating time of a brake:

.
.

3.Frenet coordinate system definition

In the Frenet coordinate system, we use the center line of the road as the reference line, and use the tangent vector tt and the normal vector nn of the reference line to establish a coordinate system. As shown in the right figure below, this coordinate system is the Frenet coordinate system. It takes the vehicle itself as the origin, and the coordinate axes are perpendicular to each other. The Frenet coordinate system obviously simplifies the problem, because when driving on the road, we can always easily find the reference line of the road (that is, the center line of the road), then the representation of the position based on the reference line can be simply described by the longitudinal distance (that is, the distance along the road direction) and the lateral distance (that is, the distance away from the reference line). Similarly, the calculation of the speed in two directions (s˙s˙ and d˙d˙) is relatively simple.

So now the configuration space in our action planning problem has a total of three dimensions: (s,d,t), t is the time point of each action we plan, and the essential difference between trajectory and path is that the trajectory considers the dimension of time .

.
.

4. Why use the Frenet coordinate system for trajectory optimization

Answer : Divide the high-dimensional optimization problem of action planning into two independent optimization problems in the horizontal and vertical directions. If the

task layer in the planning layer issues a lane-changing task and requires the current vehicle to cross the dotted line at t8t8 to complete a lane change, that is, the vehicle needs to complete a ΔdΔd in the horizontal direction and a ΔsΔs in the vertical direction. Then s and d can be expressed as functions of t: s(t) and d(t) (the right figure in the above figure), then d, sd ,s Which one should be chosen as the optimal trajectory with respect to time tt? Through this transformation, the original action planning problem is divided into two independent optimization problems. For the horizontal and vertical trajectory optimization, we select the loss function C, and the trajectory that makes C the smallest is the final planned action sequence.

Simply put, it is to optimize a trajectory (x, y) with two-dimensional information, and convert it into two independent one-dimensional trajectory s(t) and d(t)
.
.

2. Process theory based on Frenet coordinate system trajectory optimization

The definition of the loss function in the Werling method uses the acceleration Jerk function. Jerk is the rate of change of acceleration . Generally speaking, a jerk whose absolute value is too high will cause discomfort to the rider. Therefore, from the perspective of ride comfort, the amount of Jerk should be optimized

1. Step 1: Design Jerk optimization problem

Since we divide the trajectory optimization problem into two directions of s and d, the Jerk minimization can be carried out from the horizontal and vertical directions respectively. Let p be the configuration we consider (namely s or d). The jerk Jt expression for the jerk accumulated in the configuration p during the time period t1−t0 is:

Now our task is to find the p(t) that can minimize Jt(p(t))
How to understand the above optimization problem? The expression of the above optimal problem is the integral of the acceleration in a short period of time. If the acceleration curve oscillates, the integral of the acceleration will be larger in a shorter period of
time. Our purpose is to make the jerk not oscillate by optimizing the position p(t)
.

2. Step 2: Solve the Jerk optimization problem (that is, solve the 5 coefficients of a polynomial of degree 5)

Takahashi’s article—Local path planning and motion control for AGV in positioning has proved that the solution to any Jerk optimization problem can be represented by a polynomial of degree 5: To solve this system of equations, some initial configuration and target configuration are required. Taking lateral path planning as an example, the initial configuration is D0=[d0,d0˙,d0¨], that is, the lateral offset of the vehicle at time t0t0, and the lateral velocity and lateral acceleration are d0,d0˙,d0 ¨, we can get the system of equations

:

Similarly, according to the horizontal target configuration D1=[d1,d1˙,d1¨], the equations can be obtained:

we can simplify the solution of this six-element equation by setting t0=0, and can directly obtain αd0, αd1 and αd2 as: let T=t1−t0, and the remaining three coefficients αd3, αd4, αd5 can be obtained by solving the following matrix equation: the solution of this equation can be obtained through np.linalg in Python's Numpy .solve simply

finds

.

So far, given any initial configuration D0=[d0,d0˙,d0¨], target configuration D1=[d1,d1˙,d1¨] and braking time T, we can find the coefficient of the corresponding quintic polynomial in the dd direction with respect to time t

In order to distinguish between horizontal and vertical, we use αdi and αsi to denote the polynomial coefficients in the d and s directions respectively. Similarly, the same method can be used to solve the quintic polynomial coefficients in the vertical direction (that is, the s direction).

.
.

3. Step 3: Determine the optimal trajectory

Step 1: Obtain a candidate set of trajectories through a set of target configurations

Alternative set of lateral trajectories

我们仍然以 d 方向的优化轨迹为例讲解：
我们可以取如下目标配置集合来计算出一组备选的多项式集合

对于优化问题而言，我们实际上希望车辆最终沿着参考线（道路中心线）平行的方向行驶，所以我们令 di˙=di¨=0di˙=di¨=0 ，那么目标配置只涉及 didi 和 TjTj 两个变量的组合，而这两个变量在无人驾驶的应用场景中实际上是受限的，我们可以通过定义 (dmin,dmax)(dmin,dmax) 和 (Tmin,Tmax)(Tmin,Tmax) 来约束目标配置的取值范围，通过 ΔdΔd 和 ΔTΔT 来限制采样密度，从而在每一个制动周期获得一个有限的备选轨迹集合

.

Alternative set of longitudinal trajectories

In different scenarios, the optimization loss function of the vertical trajectory is also different. In the Werling method, the optimization scenarios of the vertical trajectory are roughly divided into the following three categories:

• follow the car
• Merge and Parking
• keep speed

在本文中我们详细了解车速保持场景下的纵向轨迹优化，在高速公路等应用场景中，目标配置中并不需要考虑目标位置（即 s1s1 ），所以在该场景下，目标配置仍然是 (s0,s0˙,s0¨)，目标配置变成了 (s1˙,s1¨)，损失函数为：

其中 sc˙ 是我们想要保持的纵向速度，第三个惩罚项的引入实际上是为了让目标配置中的纵向速度尽可能接近设定速度，该情景下的目标配置集为：

即优化过程中的可变参数为 Δsi˙ 和 Tj ，同样，也可以通过设置 ΔT 和 ΔΔsi˙ 来设置轨迹采样的密度，从而获得一个有限的纵向轨迹集合：

.
.

Step 2: Based on the principle of Jerk minimization in the candidate set, design a cost loss function to select the optimal trajectory

To select the optimal trajectory in the candidate set (that is, the green trajectory in the above figure), we need to design a loss function, which is different for different scenarios

Loss function design for lateral trajectory

In the case of higher speeds, the loss function for the lateral trajectory is:

This loss function contains three penalty terms:

• kjJt(d(t)): Penalize the alternative trajectory with a large Jerk;
• ktT: braking should be fast and the time should be short;
• kdd : the target state should not deviate too far from the centerline of the road

Among them, kj, kt and kd are the coefficients of these three penalty items. Their ratio determines which aspect of optimization our loss function pays more attention to. From this, we can calculate the loss of all candidate trajectories, and take the candidate trajectory with the smallest loss as our final horizontal trajectory.

It is worth noting that the above loss function is only applicable to relatively high-speed scenarios. In extreme low-speed situations, the braking ability of the vehicle is incomplete. We no longer express d as a quintic polynomial with respect to time t, and the loss function will also be slightly different. However, the method of searching for the optimal trajectory by optimizing the loss function based on limited sampling trajectories is still the same, and will not be repeated here.

Loss function design for horizontal trajectory + vertical trajectory

. Above we discussed the horizontal and vertical optimal trajectory search methods respectively. In the application, we combined the loss functions of the two directions into one, that is, in this way, we can obtain the optimized trajectory set by minimizing Ctotal (we can not get the "optimal" trajectory polynomial parameters, but also get "suboptimal", "suboptimal" trajectory, etc.)

.
.
.

Step 3: Rejecting trajectories by clipping (accident avoidance)

Our trajectory optimization loss function above does not include the relevant penalties for obstacle avoidance, and our loss function does not include braking constraints such as maximum speed, maximum acceleration, and maximum curvature. That is to say, our optimized trajectory set does not consider obstacle avoidance and braking constraints. An important reason for not adding obstacle avoidance to the loss function is that the introduction of collision penalty items will substitute a large number of parameters that need to be manually adjusted (ie weights). Yes, the design of the loss function becomes complicated. to be carried out later. Specifically, we will perform a trajectory inspection after completing the loss calculation of all alternative trajectories, and filter out trajectories that do not meet the braking limit and may collide with obstacles. The inspection content includes:

Whether the speed in the s direction exceeds the set maximum speed limit
Whether the acceleration in the s direction exceeds the set maximum acceleration
Whether the curvature of the trajectory exceeds the maximum curvature
Whether the trajectory will cause a collision (accident)

Generally speaking, obstacle avoidance is related to target behavior prediction. Even though it is a complex topic, advanced automatic driving systems usually have the ability to predict target behavior, so as to determine whether an accident will occur on the trajectory. In this section, we focus on the motion planning of unmanned vehicles, so the following examples only involve static obstacle avoidance and motion planning.
.
.

3. Realization of process code based on trajectory optimization of Frenet coordinate system

Complete code link:
https://download.csdn.net/download/adamshan/10494062

1. First, we generate reference lines to track and static obstacles

# 路线
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)

Generate the following reference paths and obstacles, where the red line is our global path, and the blue dots are obstacles

2. Define some parameters

# 参数
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

3. Use the Frenet-based optimized trajectory method to generate a series of horizontal and vertical trajectories, and calculate the loss corresponding to each trajectory

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

.
.

4. Quintic polynomial (quintic spline curve solution) class

The solution process of the coefficients of the quintic polynomial here is the same as our previous theoretical explanation, except that we use the np.linalg.solve(A, b) method in Numpy to solve the matrix.

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

.
.

4. Implementation of obstacle avoidance

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]

It can be seen from this that the selection of the final optimized trajectory is not based solely on the minimum loss function. The trajectory inspection will also filter out some trajectories. Therefore, using the Frenet-based optimized trajectory for unmanned vehicle action planning can usually find a finite set of optimal solutions.

---

Summarize

1. The function of generating and optimizing the trajectory of unmanned vehicles based on the Frenet coordinate system:
given the starting point and target point on the map, through the interpolation optimization problem of mini_jerk or mini_snap, the trajectory between the two points is solved, and the trajectory between the two points is guaranteed to be optimal.

.
2、基于Frenet坐标系的无人车轨迹生成并优化的使用方法
可以把轨迹从笛卡尔坐标系转换到frenet坐标系，完成基于Frenet坐标系的无人车轨迹优化后，把轨迹再转换到笛卡尔坐标系
.
3、基于Frenet坐标系的无人车轨迹生成并优化是继承五次样条（五次多项式曲线）生成路径的方法的
.
.
4、基于Frenet坐标系计算五次样条的方法和基于笛卡尔坐标系计算五次样条的方法异同
基于笛卡尔坐标系计算五次样条的方法，可以看我这篇博客
https://blog.csdn.net/qq_35635374/article/details/121772237
https://blog.csdn.net/qq_35635374/article/details/120732771

The trajectory of the method of calculating the quintic spline based on the Cartesian coordinate system is two-dimensional, that is, the horizontal and vertical coordinates are xy-- y(x) The trajectory of the method of calculating the quintic spline based on the Frenet coordinate system is one-dimensional (
decoupling the two-dimensional trajectory to two one-dimensional trajectories), namely s(t), d(t)

参考资料

https://blog.csdn.net/AdamShan/article/details/80779615

理论可以参考我下面这篇博客
http://t.csdn.cn/hU2Wl

.