Research on Obstacle Avoidance Trajectory Planning for Autonomous Vehicles on Structured Roads

Abstract

This paper focuses on the obstacle avoidance trajectory planning problem for autonomous vehicles on structured roads. The objective is to design a trajectory planning algorithm that can ensure vehicle safety and comfort and satisfy the rationality of traffic regulations. This paper proposes a path and speed decoupled planning method for non-split vehicle trajectory planning on structured roads. Firstly, the path planning layer adopts the improved artificial potential field method. The obstacle-repulsive potential field, gravitational potential field, and fitting method of the traditional artificial potential field are improved. Secondly, the speed planning aspect is performed in the Frenet coordinate system. Speed planning is accomplished based on S-T graph construction and solving convex optimization problems. Finally, simulation and experimental verification are performed. The results show that the method proposed in this paper can significantly improve the safety and comfortable riding of the vehicle.

1. Introduction

The rapid development of autonomous driving technology can improve human quality of life and transportation safety. Path planning, as one of the core components, is crucial for realizing safe and efficient autonomous driving. With the increasing complexity of urban traffic environments, traditional path planning algorithms face various challenges, such as real-time obstacle avoidance in the face of dynamic obstacles, adaptation to complex road conditions, and multi-objective optimization. Therefore, studying more intelligent and flexible path planning algorithms has become a research hotspot in autonomous driving. Traditional planning methods are classified into sampling-based, search-based, and optimization-based methods [1]. Sampling-based methods are usually used for global planning, and Dijkstra’s algorithm [2], proposed by Dutch computer scientists in 1959, is used to find the shortest path in a network or node. A team of researchers at Stanford University introduced the A* [3] algorithm in 1968, which guides the search process in the direction most likely to reach the goal by incorporating heuristic information related to the goal location. Representative search-based algorithms include the rapidly exploring random tree (RRT) [4,5] and probabilistic map (PRM) [6,7] algorithms for fast path planning by randomly sampling the space and expanding the tree structure along these sampling points. Numerical optimization-based planning methods focus on the optimization of the objective function. The cost is maximized or minimized according to different constraints. For example, the artificial potential field method, first proposed by Khatib [8] in 1986, is a typical numerical optimization algorithm usually applied to perform local path planning. The core idea of this method is to map the natural environment into a virtual potential field environment in which the target location generates an attractive force and the obstacle generates a repulsive force. A combined potential field is formed by synthesizing the vectors of these two forces. The self-driving vehicle plans its traveling path based on this synthesized potential field to avoid obstacles and move towards the target.

Therefore, the artificial potential field method, which does not rely on complex maps or environment models, is suitable for various scenarios and environments. Its advantages include simplicity, intuitiveness, strong real-time performance, effective obstacle avoidance, robust local optimization capabilities, and broad applicability. As a result, it has garnered widespread attention and research. Duan et al. [9] overcame the local minimum problem by introducing a second virtual target attraction potential field. Park G et al. [10] proposed an artificial potential field-based algorithm to compute the desired vehicle longitudinal speed and vehicle traverse angle in real-time, which includes both repulsive fields for avoiding the road boundary ahead and nearby obstacles and attractive fields for tracking the appropriate lanes. Yuan et al. [11] proposed a longitudinal safe distance model based on the analysis of the braking process, as well as under the constraint of the side deflection angle. A lane change safety spacing model with the shortest lane change time was proposed to solve the problem of the near-neighbor obstacles not reaching the target. Fan X. et al. [12] proposed the positive hexagonal bootstrap method to improve the local minimum problem. Meanwhile, a relative velocity method on moving object detection and avoidance was proposed for dynamic environments. Tian et al. [13] proposed a local minima jumping strategy based on smaller steering angles, which enables an intelligent vehicle to jump out of the local minima by finding a smaller steering angle and setting a suitable jump-out step size.

The main objective of this paper is to solve the problem of trajectory planning for static and dynamic obstacle avoidance on structured roads. This trajectory satisfies the safety and comfort requirements of an autonomous vehicle. This paper proposes a hierarchical planning method that decomposes the trajectory into path and speed [14]. The path planning layer improves the traditional artificial potential field method: the obstacle potential field at the road boundary is established by using Gaussian and power functions; the centerline of the road is taken as the reference path and discretized into equidistant global target points; the exclusion potential field model is designed concerning a two-dimensional normal distribution. Ellipses and deformed ellipses are introduced as the range of action of the obstacle-repulsive potential field. In the speed planning layer, the convex optimization problem of speed planning [15] is constructed and solved based on the S-T graph, which opens the convex space for coarse planning through dynamic planning to form a decision sequence. Finally, it optimizes it through the second planning to create a smooth speed profile. The following section consists of four main parts. (1) Improving the traditional artificial potential field method to complete the path to avoid obstacles. (2) Avoiding dynamic obstacles and ensuring comfortable riding to match the corresponding speed for the planned paths. Speed planning is performed in the Frenet coordinate system. Constructing and solving dynamic and quadratic planning based on the S-T graph completes the speed planning. (3) Constructing a Panosim simulation environment for simulation validation, using the maximum lateral and longitudinal safety distances, the path curvature and rate of change, the yaw angle and rate of change, and the velocity as metrics to assess the merits of the planned trajectory. (4) Deploying the algorithm into an ROS intelligent microvehicle for experimental validation in a road sandbox. The planning results of the algorithm proposed in this paper, as well as the improved A-star and improved RRT algorithm, are compared. It should be noted that the trajectory planning algorithm proposed in this paper can autonomously plan a trajectory for obstacle avoidance without human intervention. Therefore, it can be applied to Level 4–5 autonomous driving technology in the autonomous driving classification mentioned in the literature [16].

2. Improvement of the Artificial Potential Field Method

Autonomous vehicles on structured roads need high-precision navigation to provide global paths. At the same time, sensors are used to obtain real-time scenarios of the road and to build road boundaries, obstacle repulsion potential fields, and gravitational potential fields at the target point. This paper uses the global path and real-time scenario as known conditions for constructing an artificial potential field for path planning.

2.1. Road Boundary Repulsive Force Field

Unlike robots that move arbitrarily in space, vehicles must ensure they travel along the center line of the road they are on as much as possible when traveling on structured roads. When the front meets a moving vehicle, the ego vehicle can change lanes to overtake or follow the vehicle. Based on the restrictions of traffic regulations, it can only change lanes at the dashed lane line, and lane changing is prohibited at the solid line or double yellow line. The total repulsive potential field of the structured road proposed in this paper is obtained by superimposing multiple repulsive potential fields. It is divided into the following two main categories: the dashed lane line potential field, and the potential field of the solid lane line and the road boundary. Figure 1 is a schematic graph of the structured road, where the width of a single lane $N_{lane,i}$ (i = 1, 2, 3…) denotes the road number.

The dashed lane line potential field $U_{road}$ is used to prevent the vehicle from changing lanes, guide the vehicle along the center line of the road, and ensure that the potential field can be overcome when encountering obstacles. Therefore, the potential field must be small enough. In this paper, a Gaussian function is used to construct the dashed lane line potential field, as shown below:

$$U_{lane}={\displaystyle \sum _i^{N_{lane}}{U}_{lane,i}}$$

(1)

$$U_{lane,i}=A_{lane}\exp \left(-\frac{{\left(dis{t}_i\right)}^2}{2{\sigma }^2}\right)$$

(2)

where $A_{lane}$ is the peak value of the potential field the dashed lane line; $dis{t}_i$ is the distance to the ith lane line, measured in m; $\sigma$ is the rate of change in the potential field, which is proportional to the width of the lane; $N_{lane}$ is the number of imaginary lines.

The road boundary potential field $U_{road}$, which is not allowed to be crossed by vehicles, is defined to take an infinite value at the edge of the road and at the solid lane line and is defined as follows for the road boundary potential field:

$$U_{road}={\displaystyle \sum _j^{N_{road}}{U}_{road,j}}$$

(3)

$$U_{road,j}=\frac{1}{2}\eta {\left(\frac{1}{{\mathrm{dist}}_j}\right)}^2$$

(4)

where $\eta$ is the boundary road potential field factor; $dis{t}_j$ is the distance to the jth road boundary, measured in m; $N_{road}$ is the sum of the number of road boundaries and the number of solid lines.

These two potential fields have specific roles in the vehicle traveling process. Moreover, the two potential fields are added together to obtain the total potential field of the road, as shown in Figure 2.

$$U=U_{lane}+U_{road}$$

(5)

2.2. Gravitational Potential Field

The gravitational potential field to which the vehicle is subjected during motion is defined using a power function with the following expression:

$$U_{att}\left(p\right)=\frac{1}{2}{d}_p{p}_0^m$$

(6)

where $d_p$ is the gravitational gain coefficient; $p_0$ is the relative distance between the vehicle’s position and the position of the target point, measured in m, and the gravitational direction is pointing from the vehicle’s position to the position of the target point; $m$ is the exponent of the power function of the gravitational potential field.

The gravitational potential field of the traditional artificial potential field method is generated by the target point, which is a single-point gravitational field. Excessive gravitational force is generated when the vehicle is far from the target. This reduces the strength of the obstacle-repulsive potential field and increases the risk of collision. Under steering conditions, such as right and left turns and curved roads, the direction of gravity is not consistent with the direction of road travel. The single-point gravitational field leads to vehicle planning failures, resulting in an unsafe factor. As shown in Figure 3, a single-point gravitational field cannot provide effective guidance for path planning in long-range target and steering conditions.

Improvement of the single-point gravitational field to the global gravitational field. Using the perceived environmental information, the road centerline is used as the reference path, which is discretized into equidistant global path points, and a global gravitational field is established using each point. When planning the path, as shown in Figure 4, the vehicle is only affected by the gravitational points in the influence area, and the influence area rolls along with the vehicle as it moves forward.

The strength of the global gravitational field is consistent with the single-point gravitational field model and is not affected by the gravitational potential field at the target point. In addition, the global gravitational field also considers the road traveling direction problem during the construction process. Its distribution is related to the positive direction of the road to guide the vehicle to travel along the positive direction of the road. At the same time, in the reference trajectory, the change in the gravitational field is smaller than the longitudinal direction in the horizontal direction, which is conducive to the vehicle traveling along the reference trajectory in a better way and the construction of the global gravitational field is as follows:

$$E_p=\left[{\displaystyle \sum _{i=1}^{n}k\left(\begin{array}{l}\left(\cos \left({\alpha }_i\right)\left(x-x_i\right)+\sin \left({\alpha }_i\right)\left(y-y_i\right)\right)\\ +\left(\cos \left({\alpha }_i\right)\left(y-y_i\right)-\sin \left({\alpha }_i\right)\left(x-x_i\right)\right)\end{array}\right)}\right]$$

(7)

where $i$ is the path point number, the first number in the area range is 1, and the end point is n; $x_i$ and $y_i$ are the path points numbered $i$; ${\alpha }_i$ is the positive direction angle of the road where the path points numbered $i$ is located, measured in degrees; $k$ is the gravitational potential field gain coefficient.

Figure 5 shows the planning results under the action of the global gravitational potential field. Comparing Figure 5 with Figure 3, it is found that the path planned under the action of the global gravitational potential field is the centerline of the road. It avoids falling into a local optimum when encountering obstacles and also conforms to the traffic rules.

2.3. Obstacle-Repulsive Potential Field

2.3.1. Potential Field Model

The obstacle-repulsive potential field model is designed with reference to a two-dimensional normal distribution as follows:

$$P\left(x,y\right)=H\exp \left[-\frac{1}{2}\left(\frac{{\left(x-{\mu }_x\right)}^2}{{\sigma }_{xg}^2}+\frac{{\left(y-{\mu }_y\right)}^2}{{\sigma }_{yg}^2}\right)\right]$$

(8)

where $u_x$ and $u_y$ are the Cartesian coordinates of the obstacle, respectively; ${\sigma }_{xg}$ and ${\sigma }_{yg}$ are the distribution factors of the repulsive potential field along the x-axis and axial direction, respectively; $H$ is the repulsive gain coefficient.

According to the nature of the normal distribution function, the shape and distribution of the entire potential field can be adjusted by changing ${\sigma }_{xg}$, ${\sigma }_{yg}$, and $H$ to obtain the ideal repulsive potential field model accurately. Figure 6 shows the repulsive potential field map under different parameters. The results show that, by changing the distribution factor, the repulsive gain coefficient can change the influence range of the repulsive potential field and the repulsive potential field value, which can specifically depict the distribution of the repulsive potential field of the obstacle.

2.3.2. Range of Repulsive Force Field

The range of the obstacle-repulsive potential field is an essential factor affecting the planning results, and the range proposed in the literature [17] is circular. In the process of structured road travel, the hazard coefficient in the longitudinal direction is higher than that in the transverse direction. The transverse safety distance of the circular range is too considerable to meet the vehicle travel characteristics. This paper introduces the safety ellipse as the range of the obstacle-repulsive potential field. It makes the influence of obstacles on vehicles more in line with the characteristics of vehicle motion and ensures that the planning path is the shortest and smoother at the same time.

Figure 7 shows the long and short semiaxes of the safety ellipse as the main factors affecting the range of the repulsive potential field, defining the expression of the long semiaxis as the following:

$$A=\frac{L}{2}+v_{o}t+\frac{m{v}^2}{2F}$$

(9)

where $L$ is the length of the vehicle; m is the mass of the vehicle, measured in $s$; $v_0$ is the relative speed between the ego vehicle and the obstacle, $v_0=v$ for static obstacles, measured in $m/s$; $v$ is the ego vehicle speed, measured in $m/s$; $t$ is the reaction time before braking, measured in $s$; $F$ is the maximum braking force, measured in $N$.

The long half-axle is made up of the length of the vehicle and the distance traveled in response to the driver’s braking. The expression for the short half-axle $B$ is as follows:

$$B=\frac{C\mathrm{W}}{2}$$

(10)

where $C$ is the expansion factor, measured in $m$; $W$ is the vehicle width, measured in $m$.

The final repulsive potential field is modeled as follows:

$$P\left(x,y\right)=\left\{\begin{array}{l}A\exp \left[-\frac{1}{2}\left(\frac{{\left(x-{\mu }_x\right)}^2}{{\sigma }_{xg}^2}+\frac{{\left(y-{\mu }_y\right)}^2}{{\sigma }_{yg}^2}\right)\right]\frac{x^2}{A}+\frac{y^2}{B}\le 1\\ 0\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\frac{x^2}{A}+\frac{y^2}{B}>1\end{array}\right.$$

(11)

In Figure 8, the gray rectangles represent static obstacles and the dotted lines indicate the collision range. The main content of the figure is to plan a path from the starting point (blue point) to avoid obstacles and to reach the destination (green point). The comparison reveals that the safety ellipse increases the longitudinal safety distance, avoiding obstacles in advance to increase the safety of the traveling path. It also decreases the curvature of the path and reduces redundant paths. However, due to the geometric characteristics of the ellipse of obstacle repulsion, the vehicle will fall into the local optimal solution under steering conditions. The vehicle cannot choose the ideal path to avoid obstacles optimally.

To address this problem, the safety ellipse is deformed. It is transformed into an elliptical slot, as shown in Figure 9. And the whole elliptical slot is defined as a closed figure consisting of arcs 1, 2, 3, and 4, where the difference between the radius of arc 1 and the radius of arc 3 is $2\times A$, the two arcs are concentric arcs, the radii of arcs 2 and 4 are A, and the maximal arc of the whole elliptical slot is $2\times A$, as shown by the dotted line in the figure.

2.4. Bezier Curves Fitting

The path planned by the artificial potential field exists where there are bending points. For the folding point problem, n-order Bezier curves [18] are used to fit the path so as to obtain a smooth path curve that can be tracked. The path points planned by the improved artificial potential field method are used as the control points of the Bezier curve, and the n-order Bezier curve functional equation is as follows:

$$P(t)={\displaystyle \sum _{i=0}^n{B}_{i,n}\left(t\right)}{P}_i,t\in \left[0,1\right]$$

(12)

where $t$ is an independent variable; $P_i$ is the control point of the motion of the Bezier curve, such as $P(0)=P_0$ and $P(1)=P_1$; $B_{i,n}\left(t\right)$ is an nth order Bernstein polynomial with the expression as shown below:

$$B_{i,n}\left(t\right)=C_n^i{t}^i{\left(1-t\right)}^{n-i}=\frac{n!}{\left(n-1\right)!i!}{t}^i{\left(1-t\right)}^{n-i},i=0,1,2,\cdot \cdot \cdot ,n$$

(13)

Figure 10 shows the comparison of the paths before and after using Bezier fitting, and the comparison shows that the paths are smoother after Bezier fitting and there is no sudden change in the large curvature, which meets the requirements of vehicle travel.

3. Speed Planning Based on the S-T Graph

In speed planning, dynamic and quadratic planning are performed based on the S-T graph, and the Cartesian coordinate system is converted to the Frenet coordinate system [19] for solving. The paths are matched for speed, and dynamic obstacle avoidance is accomplished, i.e., accelerated overtaking or decelerated avoidance is performed.

The S-T graph is a two-dimensional relationship graph with the longitudinal distance $s$ as the vertical axis and time $t$ as the horizontal axis. Based on a known reference curve, dynamic obstacles are mapped onto the S-T graph to obtain their projection. The principle of projection is that, when a dynamic obstacle intersects with the trajectory of the ego vehicle, it implies a collision with the ego vehicle. The time and path distance of the dynamic obstacle entering and leaving the trajectory of the ego vehicle are used as the projection range. The projected area represents the region of collision, which is the infeasible area. As shown in Figure 11, the blue vehicle represents the ego vehicle, while the red vehicle represents the dynamic obstacle; $s_0$ and $s_1$ are the longitudinal distances on both sides of the host vehicle and the dynamic obstacle, respectively; $t_{in}$ and $t_{out}$ are when dynamic obstacles drive into and out of the ego vehicle trajectory, respectively. The shaded area represents the region of collision, and the upper and lower lines represent different decision behaviors.

3.1. Dynamic Planning

In speed planning, the dynamic planning module mainly plays the role of decision making, starting from the initial state, through the intermediate stages of decision-making options, and finally reaching the end state. The decisions form a sequence of decisions to determine the optimal route. It provides the determination of whether the vehicle is accelerating to overtake or decelerating to avoid the vehicle during traveling, thus laying the foundation for more accurate speed planning later. Dynamic planning is coarse in speed planning and de-determines the asymptotically optimal feasible space.

As shown in Figure 12, the S-T two-dimensional graph is discretized into equidistant points whose horizontal coordinates are time and whose vertical coordinates are longitudinal displacements. In the search process, starting from the calculation of the cost of each discrete point at the moment of $t_1$, the calculation of the optimal path of each node is carried out sequentially in chronological order, and the optimal speed profile is finally obtained. The endpoint of the speed profile can be any node in the last row or column, and there is no need to take ($t_n$,$s_n$) as the endpoint. The blue line in Figure 12 shows the planned time versus longitudinal displacement below the collision range, i.e., the decision to slow down and avoid is made. Similarly, when the curve is above the shading, it represents that the vehicle moves out of the collision range earlier and decides to accelerate to overtake.

In order to describe the advantages and disadvantages of planning speed profiles, a cost function is introduced for evaluation, and the evaluation metrics are the following:

(1)

Obstacle Distance Cost Function

The shading is the range of collision. When the vehicle is far from the obstacle, its cost is defined as 0. When the vehicle is closer to the obstacle, the cost of the obstacle gradually increases. When the distance between the vehicle and the obstacle reaches a critical value, its value reaches the maximum value, which indicates that the degree of danger at that point is very high, and the cost function is as follows:

$$C_{obs}=\left\{\begin{array}{l}10000\hspace{1em} \hspace{0.33em}\left|d_s\right|<\left|d_{safe}\right|\\ k/d_s\hspace{1em}\hspace{1em} \left|d_{safe}\right|<\left|d_s\right|<\left|d_a\right|\\ 0\hspace{1em}\hspace{1em}\hspace{1em} \left|d_s\right|>\left|d_a\right|\end{array}\right.$$

(14)

where $d_s$ is the distance between the vehicle and the obstacle, measured in $m$; $d_{safe}$ is the shortest allowable distance with the obstacle, measured in $m$; $d_a$ is the initial distance at which the obstacle has an impact on the vehicle, measured in $m$; $k$ is the gain coefficient at the cost of distance from the obstacle.

(2)

Recommended Speed Cost Function

In order to limit the difference between the planning speed and the reference speed from being too large, the cost of the deviation between the planning speed and the reference speed is designed, where the reference speed is generated from the road speed limit and the current speed, and its cost function is as follows:

$$C_v=w_v{\left(v-v_{ref}\right)}^2$$

(15)

where $v_{ref}$ is the recommended vehicle speed, measured in $m/s$; $v$ is the actual vehicle speed, measured in $m/s$; $w_v$ is the recommended speed cost weight.

(3)

Acceleration Cost Function

In order to ensure the comfort of vehicle travel and to avoid excessive acceleration, its cost function is designed as follows:

$$C_a=w_a{\left(\frac{v_i-v_{i-1}}{T_i-T_{i-1}}\right)}^2$$

(16)

where $w_a$ is the acceleration cost weight; $v_i$ and $v_{i-1}$ are the speed of the vehicle at the moment of $T_i$ and $T_{i-1}$.

(4)

The Jerk Cost Function

The jerk is also crucial for safety and comfort, with the following cost function:

$$C_j=w_j{\left(\frac{v_i-2v_{i-1}+v_{i-2}}{T_i-T_{i-1}}\right)}^2$$

(17)

where $w_j$ is the jerk cost weight coefficient; $v_i$, $v_{i-1}$, and $v_{i-2}$ are the vehicle speeds at $T_i$, $T_{i-1}$, and $T_{i-2}$ moments.

In summary, for dynamic planning in speed planning, the cost function is shown below:

$$C_t=C_{obs}+C_v+C_a+C_j$$

(18)

3.2. Quadratic Planning

There are fluctuations in the speed curve calculated by dynamic planning. In order to improve the ride comfort and reduce the difficulty of trajectory tracking, the quadratic planning problem is constructed based on the convex space opened up by dynamic planning, and the final speed planning is completed by solving the quadratic planning problem.

The speed profile is a mapping relationship between position and time $t$. There are two ways to optimize the speed profile: one is to optimize the longitudinal displacement corresponding to each time point based on the time t, the other is to optimize the time corresponding to each longitudinal displacement based on the longitudinal displacements. In this paper, the first optimization method is chosen to facilitate the calculation of the vehicle velocity, acceleration, and other parameters. The speed optimization problem is transformed into a quadratic planning problem, where the optimization variable is the vehicle position corresponding to a fixed time series.

3.2.1. Establishing the Cost Function

To describe the advantages and disadvantages of the speed optimization curve, three evaluation metrics are introduced to form a cost function for evaluation, as follows:

(1)

The Cost of Speed

The speed cost is a cost function to ensure that the vehicle travels at a specified speed without the influence of the collision area, limiting the speed to the following certain range:

$$C_v={\omega }_v{\displaystyle \sum _{i=1}^n{({\dot{s}}_i-v_{ref})}^2}$$

(19)

where ${\omega }_v$ is the speed cost weighting factor; ${\dot{s}}_i$ is the speed at the ith moment, measured in $m/s$; $v_{ref}$ is the desired vehicle speed, measured in $m/s$.

(2)

The Cost of Acceleration

Acceleration denotes the rate of change in speed and directly affects a vehicle’s acceleration, braking, and maneuverability. In order to ensure passenger comfort, the vehicle should maintain a constant speed, hence the introduction of the following acceleration cost:

$$C_a={\omega }_a{\displaystyle \sum _{i=1}^{n-1}{(\frac{{\dot{s}}_{i+1}-{\dot{s}}_i}{\Delta t})}^2}$$

(20)

where ${\omega }_a$ is the acceleration cost weighting factor.

(3)

The Cost of Jerk

The first-order derivative of acceleration–jerk significantly impacts on the vehicle driving, which is related to the ride comfort and dynamic performance, including smooth acceleration, cornering stability, and emergency braking response. It is now used to improve the driving experience and safety by controlling the acceleration, and its cost function is as follows:

$$C_j={\omega }_j{\displaystyle \sum _{i=1}^{n-2}{(\frac{{\dot{s}}_{i+2}-2{\dot{s}}_{i+1}+{\dot{s}}_i}{\Delta t^2})}^2}$$

(21)

where ${\omega }_j$ is the jerk cost weight factor.

The longitudinal displacement $s(m)$ of the vehicle is related to the time $t(s)$, as follows:

$${\dot{s}}_i=\frac{s_{i+1}-s_i}{\Delta t}$$

(22)

$${\ddot{s}}_i=\frac{s_{i+2}-2s_{i+1}+s_i}{\Delta t^2}$$

(23)

$${\stackrel{⃛}{s}}_i=\frac{s_{i+3}-3s_{i+2}+3s_{i+1}-s_i}{\Delta t^3}$$

(24)

where $\ddot{s}$ is the acceleration at the ith moment, measured in $m/s^2$; $\stackrel{⃛}{s}$ is the first-order derivative of the acceleration at the ith moment, measured in $m/s^3$.

In summary, the total cost function used for quadratic planning is shown below:

$$C=C_v+C_a+C_{jerk}$$

(25)

The total cost function is converted to the standard form of quadratic planning as an equation. Equations (26)–(28) are the final quadratic planning expressions for the velocity, acceleration, and jerk cost functions, respectively.

$$C_v={\omega }_v{\displaystyle \sum _{i=1}^n{({\dot{s}}_i-v_{ref})}^2}={\omega }_v\left(x^T{A}_1^T{A}_{1}x-2h^{T}x\right)$$

(26)

$$C_a={\omega }_a{\displaystyle \sum _{i=1}^{n-1}{(\frac{{\dot{s}}_{i+1}-{\dot{s}}_i}{\Delta t})}^2}=\frac{{\omega }_a}{\Delta t^2}\left(x^T{A}_2^T{A}_{2}x\right)$$

(27)

$$C_j={\omega }_j{\displaystyle \sum _{i=1}^{n-2}{(\frac{{\dot{s}}_{i+2}-2{\dot{s}}_{i+1}+{\dot{s}}_i}{\Delta t^2})}^2}=\frac{{\omega }_j}{\Delta t^4}\left(x^T{A}_3^T{A}_{3}x\right)$$

(28)

where $A_1$ is a unit matrix of order n; $A_2$ is an $(n-1)\times n$ matrix; $A_3$ is a matrix of order $(n-2)\times n$; $x$ is shown in Equation (29); $h$ is shown in Equation (30).

$$x={\left\{\begin{array}{cccc}{\dot{s}}_1& {\dot{s}}_2& \cdot \cdot \cdot & {\dot{s}}_n\end{array}\right\}}_{1\times n}^T$$

(29)

$$h={\left\{\begin{array}{cccc}{v}_{ref}& v_{ref}& \cdot \cdot \cdot & v_{ref}\end{array}\right\}}_{1\times n}^T$$

(30)

Ultimately, the standard type expression for the quadratic planning cost function is shown below:

$$C=x^T\frac{H}{2}x-f^{T}x$$

(31)

where $H$ is shown in Equation (32); $f^T$ is shown in Equation (33).

$$H=2\left({\omega }_v{A}_1^T{A}_1+\frac{{\omega }_a}{\Delta t^2}{A}_2^T{A}_2+\frac{{\omega }_j}{\Delta t^4}{A}_3^T{A}_3\right)$$

(32)

$$C=x^T\frac{H}{2}x-f^{T}x$$

(33)

3.2.2. Constraint Condition

The restrictions on the vehicle position are based on dynamic programming. Without considering the reversing behavior of the vehicle, a convex space is opened in which the upper and lower limits on the vehicle position are obtained, i.e., the longitudinal displacement at the next moment is greater than at the current moment. The constraint of the traveling speed is considered as the speed limit of the lane itself, which is a constant more significant than or equal to zero. The acceleration is based on the constraints of the physical structure of the vehicle, i.e., the maximum acceleration that can be achieved by the vehicle itself. Jerk constraints are constraints on the comfort of the occupants, reducing the magnitude of the acceleration change, and the following are the expressions for each constraint:

$$s_{\min }\le s_i\le s_{\max }$$

(34)

$$0\le \dot{s}\le {\left(\frac{a_{y\max }}{kappa}\right)}^{1/2}$$

(35)

$${\ddot{s}}_{\min }\le {\ddot{s}}_i\le {\ddot{s}}_{\max }$$

(36)

$${\stackrel{⃛}{s}}_{\min }\le {\stackrel{⃛}{s}}_i\le {\stackrel{⃛}{s}}_{\max }$$

(37)

To ensure the continuity of the speed profile, the variables need to be constrained, and the continuity of the variables is constrained using a Taylor expansion and by ignoring the higher-order infinitesimals of the longitudinal position and its first-order derivatives.

$$s_{i+1}=s_i+{\dot{s}}_{i}dt+\frac{1}{2}{\ddot{s}}_{i}d{t}^2+\frac{1}{6}\left(\frac{{\ddot{s}}_{i+1}-{\ddot{s}}_i}{dt}\right)d{t}^3$$

(38)

$${\dot{s}}_{i+1}={\dot{s}}_i+{\ddot{s}}_{i}dt+\frac{1}{2}\left(\frac{{\ddot{s}}_{i+1}-{\ddot{s}}_i}{dt}\right)d{t}^2$$

(39)

A simplification of Equations (38) and (39) are obtained as follows:

$$\left(\begin{array}{llllll}1\hfill & dt\hfill & \frac{1}{3}d{t}^2\hfill & -1\hfill & 0\hfill & \frac{1}{6}d{t}^2\hfill \\ 0\hfill & 1\hfill & \frac{1}{2}dt\hfill & 0\hfill & -1\hfill & \frac{1}{2}dt\hfill \end{array}\right)\left(\begin{array}{l}{s}_i\hfill \\ {\dot{s}}_i\hfill \\ {\ddot{s}}_i\hfill \\ s_{i+1}\hfill \\ {\dot{s}}_{i+1}\hfill \\ {\ddot{s}}_{i+1}\hfill \end{array}\right)=\left(\begin{array}{c}0\\ 0\end{array}\right)$$

(40)

Now, we optimize $s_1,{\dot{s}}_1,{\ddot{s}}_1\cdots \cdots s_n,{\dot{s}}_n,{\ddot{s}}_n$; then, the equation is constrained as follows:

$${\left(\begin{array}{cccccccccc}1& dt& \frac{1}{3}d{t}^2& -1& 0& \frac{1}{6}d{t}^2& 0& 0& 0& \cdots \\ 0& 1& \frac{1}{2}dt& 0& -1& \frac{1}{2}dt& 0& 0& 0& \dots \\ 0& 0& 0& 1& dt& \frac{1}{3}d{t}^2& -1& 0& \frac{1}{6}d{t}^2& \dots \\ 0& 0& 0& 0& 1& \frac{1}{2}dt& 0& -1& \frac{1}{2}dt& \dots \\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& \ddots \end{array}\right)}_{\left(2n-2\right)\times 3n}{\left(\begin{array}{c}{s}_1\\ {\dot{s}}_1\\ {\ddot{s}}_1\\ s_2\\ {\dot{s}}_2\\ {\ddot{s}}_2\\ ⋮\\ s_n\\ {\dot{s}}_n\\ {\ddot{s}}_n\end{array}\right)}_{3n\times 1}=\left(\begin{array}{c}0\\ 0\\ ⋮\\ 0\end{array}\right)$$

(41)

The planning starting point constraint is to ensure that the initial position, velocity, and acceleration of the vehicle remain constant after the speed optimization is performed, and is expressed as follows:

$$\left\{\begin{array}{l}{s}_0=s_{start}\\ {\dot{s}}_0=v_{start}\\ {\ddot{s}}_0=a_{start}\end{array}\right.$$

(42)

4. Simulation Verification

To verify the feasibility of the trajectory planning algorithm proposed in this paper, the simulation environment is constructed based on the Panosim platform. Moreover, its built-in real sensors are utilized to obtain information on the roads, obstacles, vehicle speed, etc. Python was used to write the model for simulation verification, and the simulation flow is shown in Figure 13.

For path planning, (1) uses a circle (C-APF) [17], an ellipse (E-APF), and a deformed ellipse (DE-APF) as the repulsive potential field range for the obstacles, respectively. The results before and after the improvement of the artificial potential field method are compared under global gravity potential field conditions. (2) Simulation comparisons are made between the improved A-star algorithm of the literature [20], the improved RRT algorithm of the literature [21], and the DE-APF algorithm under the same working conditions. Speed planning is formed based on the planning paths. The results of the dynamic planning and quadratic planning are then compared. Finally, the effectiveness of the algorithm proposed in this paper in dynamic and static obstacle avoidance is verified by the above simulation comparison. The values of the relevant parameters used for the simulation are shown in

Table 1. Parameters related to the simulation.

Parameters	Value	Parameters	Value
${\mathrm{A}}_{lane}$	10	$W$ · (m)	2
$\sigma$	0.35	$L$ · (m)	3.3
$\eta$	0.5	$k_d$	1000
$k$	3	$d_{safe}$ · (m)	0.5
$H$	2	$d_a$ · (m)	1.5
${\sigma }_{xg}$	1.5	${\omega }_v$	235
${\sigma }_{x\mathrm{y}}$	1.5	${\omega }_a$	10
$C$	4.9	${\omega }_j$	500

.

4.1. Description of the Simulation

Urban roads have many intersections, which are formed by intersecting multiple structured roads. At the intersections, roadway boundaries need to be supplemented according to the driving intentions of the autonomous vehicles. This paper simulates and validates the dynamic and static obstacle avoidance capabilities of the algorithm under the uncontrolled intersection human left-turn condition. As shown in Figure 14, the initial position of ego vehicle A is (22, −8), with an initial speed of 5.5 m/s, and is located in the inside lane when making a left turn; the position of obstacle vehicle B is (2, 30), with a speed of 5.5 m/s, and is located in the outside lane, traveling straight ahead at a constant speed; the position of obstacle vehicle C is (21, 4.5), and is at a stationary state.

For the evaluation of the artificial potential field method before and after improvement, this paper uses as the evaluation indexes of the planning path the following: whether or not to fall into the local optimum, as well as the maximum transverse and longitudinal safety distance, the mean curvature, and the maximum curvature. Among them, the maximum transverse and longitudinal safety distance is the maximum radiation distance of the obstacle-repulsive potential field in the transverse and longitudinal directions. Furthermore, to evaluate the improved artificial potential field method with other planning algorithms, this paper uses the curvature, curvature change rate, heading angle, and heading angle change rate as evaluation indexes.

4.2. Path Planning

Figure 15 shows the planning situations under different ranges of the obstacle-repulsive potential field, respectively: the range of the repulsive potential field of the C-APF spans three lanes and completes the planning; the range of the repulsive potential field of the E-APF is not uniformly distributed within the driving lanes and falls into the local optimum, which prevents them from completing the planning; the range of the repulsive potential field of the DE-APF is uniformly distributed within the driving lanes, which avoids the local minima and completes the path planning.

The simulation results are shown in

Table 2. Path planning simulation validation results.

	C-APF	E-APF	DE-APF
Fall into local optimality *	N	Y	N
Maximum longitudinal safety distance/m	6.1211	\	8.4459
Maximum lateral safety distance/m	4.9507	\	4.4117
Average curvature	0.0709	\	0.0652
Maximum curvature	0.2020	\	0.1630

* This evaluation metric of ‘Y’ indicates that the algorithm is in local optimality and ‘N’ indicates that it is not in local optimality. If the algorithm falls into local optimality, then all other metrics are denoted by ‘\’.

. Under the left-turn condition, the E-APF algorithm falls into the local optimum and cannot complete the planning without reaching the target point. Compared with the C-APF algorithm, the DE-APF algorithm increases the maximum longitudinal safety distance by 2.3428 m (the distance between the center of mass of the vehicle and the center of mass of the obstacle) and decreases the maximum transverse safety distance by 0.593 m, while the mean curvature decreases by 0.0057 and the maximum curvature of the path decreases by 0.039. In conclusion, the DE-APF algorithm trajectory planning on structured roads is more in line with the characteristics of vehicle travel: the longitudinal safety distance is increased, the lateral safety distance is appropriately reduced, and path smoothing can be used for tracking control.

In Figure 16, the paths planned by the A-star, RRT, and DE-APF algorithms all reach their destinations. Among them, the path planned by A-star is too close to the obstacle, with the closest lateral only 2.12 m (the distance between the center of mass of the vehicle and the center of mass of the obstacle). Moreover, due to this algorithm’s characteristic of finding the shortest distance, it shows the tendency to splice multiple straight lines. The maximum longitudinal range between the path planned by the RRT algorithm and the obstacle is only 2.34 m, a potential safety hazard for collision. Moreover, the timing of the lane change after avoiding the obstacles could be more reasonable for these two algorithms.

The path variations planned by the three algorithms are revealed in Figure 17. The average curvature of the paths planned by DE-APF, A-star, and RRT are 0.0062, 0.0202, and 0.0487, respectively. The average rate of change is 0.0002, 0.0010, and 0.0020, respectively. It can be seen that the paths planned by the DE-APF algorithm proposed in this paper are flatter. Due to the limitation of the working conditions, the yaw angle of the path needs to be changed from 0 degrees to 90 degrees. From graph b in Figure 17, it can be seen that the path planned by DE-APF changes smoothly, and the rate of change in the yaw angle of the path is nearly constant in the half section. In summary, the path planned by DE-APF can significantly improve the safety and comfort of autonomous vehicle travel.

4.3. Speed Planning

Figure 18 shows the dynamic and static obstacle avoidance process under this working condition. By combining the DE-APF algorithm with speed planning, the ego vehicle A starts from the initial point to avoid static obstacle C and dynamic obstacle B to reach the target point in 12.5 s. The ego vehicle A avoids the dynamic obstacle B by decelerating to give way to the dynamic obstacle B. It starts to decelerate from 0.0 s based on 5.5 m/s until 10.5 s, and, finally, it starts to accelerate to reach the target point.

Figure 19 shows the results of dynamic planning and quadratic planning. Graph a in Figure 19 shows the S-T graph of the velocity planning, where the planned longitudinal displacements versus time curves are all located below the green collision range. It indicates that the primary vehicle decides to slow down and give way, eventually avoiding the dynamic obstacle. The graphs b–d in Figure 19 show the planning results for velocity, acceleration, and jerk. The data show that the variation in each curve after the quadratic planning optimization is significantly reduced and smoother. The ego vehicle decelerates from 0 s to 4.2 s, then accelerates to 9.8 s, and finally maintains a uniform speed; the acceleration fluctuates in the range of −0.9~0.7 $\mathrm{m}/{\mathrm{s}}^2$, and the rate of change in the acceleration is in the range of −0.3~0.4 $\mathrm{m}/{\mathrm{s}}^3$. In conclusion, the speed planning algorithm proposed in this paper can reasonably avoid dynamic obstacles and provide the ego vehicle with high safety and comfort.

5. Experimental Verification

This paper verifies the feasibility of the proposed algorithm in the real world based on the ROS intelligent microvehicle. It is compared and analyzed with the improved A-star algorithm proposed in the literature [20] and the improved RRT algorithm proposed in the literature [21]. Evaluation metrics include the curvature, heading angle, rate of change, and speed. The advantages and disadvantages of the proposed algorithms are evaluated using these metrics.

5.1. Experimental Condition

The experimental scene and working conditions are shown in Figure 20. The structured road sandbox can genuinely reflect the road’s lane lines and obstacle information. Static obstacles, dynamic obstacles, and destinations are set in the structured road of the sandbox. The dynamic obstacle accelerates from 0 m/s to 0.2 m/s at the location in the figure and then travels in the direction of the red arrow at a constant speed. The ego vehicle departs from the location in the figure at a standstill. It evades the static obstacle and performs speed planning to evade the dynamic obstacle and reach the destination.

The experimental microvehicle shown in Figure 21 is equipped with a single line LiDAR (RPLIDARA1360) for detecting static obstacle position information, dynamic obstacle position and velocity information. The depth camera (IMX219-160) performs target detection and provides category and depth information. The STM32 microcontroller embedded with an Inertial Measurement Unit (MPU9250) provides linear and angular acceleration information in three directions. Real-time vehicle speed and position can be calculated by listening to the acceleration information. The computing platform (Jetson Nano) executes complex perception, decision-making, and control algorithms, extracting information from sensor data for target recognition, path planning, and motion control. The high-performance computing power and support for the Jetson Nano deep learning enable the intelligent microvehicle to respond faster and more accurately to environmental changes.

5.2. Experimental Results

The algorithms were written in Python and deployed into the intelligent microvehicle. Utilizing the ROS’s topic publishing and receiving mechanism, real-time communication between environment sensing, path planning, chassis control, and feedback was accomplished. Figure 22 shows the paths planned by different algorithms in this scenario. The depth camera provides the lane line information. The green, purple, and blue paths are the planning results of DE-APF, A-star, and RRT algorithms, respectively. All three algorithms accomplished the task of avoiding obstacles and reaching the destination.

The path variations planned by the three algorithms are revealed in Figure 23. The average curvature of the paths planned by DE-APF, A-star, and RRT are 0.1148, 0.1249, and 0.1423, respectively. The average rate of change is 0.0306, 0.0356, and 0.0478, respectively. It can be seen that the paths planned by the DE-APF algorithm proposed in this paper are flatter. The trend of the yaw angle changes is roughly the same, with the DE-APF algorithm planning the smoothest path changes. Although there are significant angle changes, the frequency of changes is the lowest.

Figure 24 shows the state changes in the intelligent microvehicle when avoiding static and dynamic obstacles. It can be seen that the ego vehicle chooses the deceleration–yielding strategy when facing dynamic obstacles. The overall change in speed, acceleration, and jerk is relatively smooth.

5.3. Analysis of Results

Compared to simulation validation, actual vehicle experiments contain some uncertainties. These uncertainties mainly stem from how the vehicle senses its surroundings and state through various sensors. These sensors include the LIDAR, cameras, inertial measurement unit (IMU), etc. Due to their physical structure and other factors, these sensors generate errors affecting this paper’s algorithms. The main errors are measurement errors and localization errors. Measurement errors are caused by the sensor’s inherent uncertainty or by environmental noise. They can cause discrepancies between sensor measurements and actual values, thus affecting the accuracy of vehicle status information and surrounding perception information. The localization uses the integration of the acceleration collected by the IMU, which accumulates the localization error over time. Considering the sensor errors mentioned above, the impact of sensor errors on the performance of the algorithms is fully considered when performing real-vehicle validation, and corresponding measurements are taken to minimize the impact of these errors. The sensor data are filtered and corrected when performing the actual vehicle-deployment algorithm. The effect of measurement errors is reduced by using an extended Kalman filter. This improves the accuracy of the sensed data. To eliminate positioning errors, instead of relying on information from a single positioning system, a multi-sensor fusion approach was used to synthesize the positioning information. The effective compensation of positioning errors was carried out by combining the positioning data from multiple sensors, such as the IMU and LIDAR. This improved the accurate estimation of the vehicle position. Finally, it was used as the input of the positioning information of the algorithm in this paper.

In the real-vehicle validation of the algorithm in this paper, both the environment sensing algorithm and the trajectory tracking control algorithm had to be deployed. The environment sensing algorithm provides environmental information such as obstacles. The trajectory tracking control algorithm controls the vehicle horizontally and vertically so that the vehicle reaches the desired position. Therefore, the accuracy of the environment sensing and trajectory tracking control algorithms also affects the results of the algorithm proposed in this paper. In addition, due to the limitations of the experimental conditions, it is impossible to verify the effects of environmental changes, such as the sudden appearance of pedestrians and other exceptional cases. In the future, the work in this area will be improved continuously.

The simulation was conducted under ideal conditions without sensor noise interference, resulting in smooth results. However, despite preprocessing the sensor-collected data, experimental measurements were affected by sensor noise, leading to some jitter. Despite this jitter, the overall results closely resembled the simulated results. This suggests that the outputs of the actual vehicle and the simulation model align closely and share similar environmental input information. This effectively demonstrates the real-world feasibility of the algorithm proposed in this paper. With the curvature of the planned trajectory and its rate of change, the heading angle and its rate of change, and the metrics of speed and acceleration, the algorithm proposed in this paper dramatically improves the safety and comfortable riding of the passengers in autonomous vehicle trips.

6. Conclusions

This paper proposes a method to decouple path and speed to solve the trajectory planning problem of static obstacle avoidance and dynamic obstacle avoidance for autonomous vehicles on structured roads. The research features and innovations of this article include the following: (1) designing an obstacle-repulsive potential field model concerning a two-dimensional normal distribution, and introducing the ellipse and deformed ellipse as the repulsive potential field range; (2) constructing a global gravitational field and designing a rolling updating mechanism for the range of influence; (3) constructing and solving a convex optimization problem based on the S-T graph for speed planning in the Frenet coordinate system. Simulation and experimental validation results show that the algorithm proposed in this paper can achieve trajectory planning for static and dynamic obstacle avoidance on structured roads. Comparison with the unimproved artificial potential field method, A-star, and RRT algorithms shows that the algorithm planning proposed in this paper has apparent advantages in terms of the obstacle avoidance effect and trajectory flatness. It is proved that the trajectory planned by the algorithm in this paper can meet the requirements of vehicle safety and comfort.

It should be noted that the trajectory planning model designed in this paper does not apply to split vehicles, such as trailers, because split vehicles usually have different driving characteristics and dynamics that do not match the assumptions on which the model in this paper is based, which may lead to poor application of the model to split vehicles.