A Motion Planning Method for Automated Vehicles in Dynamic Trafﬁc Scenarios

: We propose a motion planning method for automated vehicles (AVs) to complete driving tasks in dynamic trafﬁc scenes. The proposed method aims to generate motion trajectories for an AV after obtaining the surrounding dynamic information and making a preliminary driving decision. The method generates a reference line by interpolating the original waypoints and generates optional trajectories with costs in a prediction interval containing three dimensions (lateral distance, time, and velocity) in the Frenet frame, and ﬁlters the optimal trajectory by a series of threshold checks. When calculating the feasibility of optional trajectories, the cost of all optional trajectories after removing obstacle interference shows obvious axisymmetric regularity concerning the reference line. Based on this regularity, we apply the constrained Simulated Annealing Algorithm (SAA) to improve the process of searching for the optimal trajectories. Experiments in three different simulated driving scenarios (speed maintaining, lane changing, and car following) show that the proposed method can efﬁciently generate safe and comfortable motion trajectories for AVs in dynamic environments. Compared with the method of traversing sampling points in discrete space, the improved motion planning method saves 70.23% of the computation time, and overcomes the limitation of the spatial sampling interval.


Introduction
As an important aspect of profiling the evolution of human civilization, transportation modalities are rapidly moving towards automation and interconnection. It is widely accepted that transportation systems will become much more efficient and safer when the task of vehicle driving shifts from a manual process to an automatic process. Over the past few decades, researchers have devoted a great deal of effort to realize the goal of autopiloting. The current process of achieving autonomous driving consists of environment perception and localization, behavior decisions, motion planning, and trajectory tracking. The purpose of motion planning is to generate a trajectory that satisfies the constraints of vehicle dynamics, driving safety, comfort, and efficiency, after the AV makes the next initial driving decision based on dynamic environment information and its state. Motion planning is a key part of the autonomous driving technology process and a critical factor in drivers' experience and safety.

Related Work
The methods of motion planning for AVs originally evolved from mobile robot technology [1][2][3][4]. As autopilot technology has gradually developed, numerous motion planning methods have become available for AVs [5]. Originally applied from the field of mobile optimal trajectory from the sampling space. In addition, increasing the speed by decreasing the density of sampling points may lead to a decrease in the accuracy of the motion planning method, due to the fact that reducing the sampling points may cause the optimal trajectory to be missed. Achieving a balance between the number of optional trajectories in the sampling space and the efficiency of the method has rarely been mentioned in previous studies.

Contribution
To overcome these limitations, this paper proposes a safe and efficient motion planning method for dynamic traffic scenes by combining the sampling-based method in the Frenet frame and the optimal trajectory searching method improved by SAA. The motion planning is divided into two parts: trajectory generation and optimal trajectory searching. The lateral and longitudinal motions of the AV are generated by the sampling-based method in the Frenet frame [22,23], which ensures the continuous rate of change in the vehicle's acceleration. The lateral motions are modeled by a quintic polynomial, and the longitudinal motions are modeled by a quintic or quartic polynomial. Through the combination of lateral and longitudinal motions, the AV can handle different dynamic traffic scenarios. The cost function and the trajectory checking function are designed in accordance with traffic rules and drivers' habits. The proposed trajectory generation strategy satisfies the constraints of kinematics, dynamics, and the physical shape of the road. To efficiently search for the optimal trajectory, the constrained SAA is applied to search for the minimum cost from the non-convex cost distribution space. Based on the axisymmetric property of the cost distribution space, the initial value is reserved in the searching process of the optimal trajectory to ensure efficiency. Because the selection probability of all trajectories in the sampling space is equal, the number of optional trajectories in the sampling space is guaranteed. In addition, as the searching process is improved from ergodic to probabilistic, the execution time of the method no longer depends on the setting of the sampling space, but on the parameter settings of the SAA. This searching strategy balances the number of optional trajectories in the sampling space with the efficiency of the motion planning method. Driven by the cost and constraint, this motion planning method can actively adapt to different dynamic traffic scenarios. Under different driving strategies, the performance of the AV using the proposed motion planning method in simulated dynamic traffic scenarios was evaluated.
The remainder of the paper is organized as follows. Section 2 introduces the methodological framework, including coordinates transformation, reference line generation, generation of optional trajectories, searching for optimal trajectories, and final path selection. Section 3 presents an improved method for the optimal trajectories searching process. Section 4 conducts numerical experiments on the proposed methodology with a simulated urban road. Finally, Section 5 concludes the paper.

Problem Description and Basic Assumptions
Consider the driving scenario shown in Figure 1, where the vehicle is expected to drive along the center line of the current lane (i.e., the reference line in Figure 1). A human driver will adjust the distance between the vehicle and the reference line during the driving process with his perception and experience, and an AV will be expected to control the vehicle in the same manner as a human driver when performing a same driving task. A Cartesian coordinate system can be established with reference to the orientation of the road to specify the vehicle's position (X 0 , Y 0 ). However, it is complicated to evaluate the deviation of the vehicle from the reference line in the Cartesian coordinate system, and it is not well adapted for curved roads.
Because the reference line tends to be generated along the centerline of the road, the reference line is differentiable in most roads. Then, the tangential and normal directions of any point on the reference line can be calculated, and a Frenet frame as shown in Figure 2 Symmetry 2022, 14, 208 4 of 22 can be constructed. For each planning period, there will be a Frenet coordinate origin, and there will be vehicle coordinates and obstacle coordinates in the Frenet frame corresponding to the Cartesian coordinate system. Taking the scenario in Figure 2 as an example, the Frenet coordinates of the obstacle are (s 5 , d 2 ) at this point. When the vehicle moves along the green trajectory in Figure 2, a set of Frenet coordinates consisting of an arc length s and a normal offset length d corresponding to their Cartesian coordinates exists at each moment. These two coordinate systems will be used to describe the motion state of the AV in this paper. metry 2022, 14, x FOR PEER REVIEW Cartesian coordinate system can be established with reference to the road to specify the vehicle's position ( , ). However, it is complicat deviation of the vehicle from the reference line in the Cartesian coordin is not well adapted for curved roads. Because the reference line tends to be generated along the centerlin reference line is differentiable in most roads. Then, the tangential and of any point on the reference line can be calculated, and a Frenet frame a 2 can be constructed. For each planning period, there will be a Frenet and there will be vehicle coordinates and obstacle coordinates in the F sponding to the Cartesian coordinate system. Taking the scenario in Fig  ple, the Frenet coordinates of the obstacle are ( , ) at this point. moves along the green trajectory in Figure 2, a set of Frenet coordinate arc length and a normal offset length corresponding to their Cart exists at each moment. These two coordinate systems will be used to de state of the AV in this paper.  Because the reference line tends to be generated along the centerline of the road reference line is differentiable in most roads. Then, the tangential and normal direc of any point on the reference line can be calculated, and a Frenet frame as shown in Fi 2 can be constructed. For each planning period, there will be a Frenet coordinate or and there will be vehicle coordinates and obstacle coordinates in the Frenet frame c sponding to the Cartesian coordinate system. Taking the scenario in Figure 2 as an ex ple, the Frenet coordinates of the obstacle are ( , ) at this point. When the ve moves along the green trajectory in Figure 2, a set of Frenet coordinates consisting o arc length and a normal offset length corresponding to their Cartesian coordin exists at each moment. These two coordinate systems will be used to describe the mo state of the AV in this paper. Assuming that the road edge is continuously smooth, and its centerline is acc ingly smooth, AVs are expected to follow the reference line in most cases. It is neces to temporarily deviate from the reference line for the AV when there are obstacles or o vehicles on the reference line. The task of the motion planning is to find the safest most comfortable trajectory for the AV within a planning period. Assuming that the road edge is continuously smooth, and its centerline is accordingly smooth, AVs are expected to follow the reference line in most cases. It is necessary to temporarily deviate from the reference line for the AV when there are obstacles or other vehicles on the reference line. The task of the motion planning is to find the safest and most comfortable trajectory for the AV within a planning period.

Coordinates Transformation and the Reference Line Generation
The trajectory of an AV can more easily be represented in the Frenet frame due to the irregular road structure, whereas the inputs and outputs of the motion planning need to be transformed into Cartesian coordinates. Moreover, the process of calculating the distance between two points on the trajectory is indispensable during the trajectory checking. For each set of coordinates in the Frenet frame, it is necessary to define the transformation between it and the corresponding coordinates in the Cartesian coordinate system. As shown in Figure 3, assuming that the Cartesian coordinates (X t , Y t ) of a point at moment t on the real trajectory of the AV are known, there exists a reference point (X r , Y r ) on the s-axis of the Frenet frame that minimizes the distance from the point (X r , Y r ) to the point (X t , Y t ). Then, the curve length from the point (X r , Y r ) to the coordinate origin in the Frenet frame is the s-coordinate of the AV at time t, and the distance length from the point (X r , Y r ) to the point (X t , Y t ) is the l-coordinate of the AV at time t. Therefore, the Frenet coordinates of the vehicle at moment t are (s t , l t ). The value of l t can be calculated from Equations (1) and (2).
where → X and → R are vectors of the coordinate origin pointing to the vehicle position and the reference point, respectively; sign represents a symbolic function, which can only take the value of 1 or −1; ρ l is the parameter to determine the positive or negative of the l coordinate, which can be calculated from Equation (3).
where θ r is the heading angle of the reference point at moment t.

Coordinates Transformation and the Reference Line Generation
The trajectory of an AV can more easily be represented in the Frenet frame due to the irregular road structure, whereas the inputs and outputs of the motion planning need to be transformed into Cartesian coordinates. Moreover, the process of calculating the distance between two points on the trajectory is indispensable during the trajectory checking. For each set of coordinates in the Frenet frame, it is necessary to define the transformation between it and the corresponding coordinates in the Cartesian coordinate system. As shown in Figure 3, assuming that the Cartesian coordinates ( , ) of a point at moment on the real trajectory of the AV are known, there exists a reference point ( , ) on the s-axis of the Frenet frame that minimizes the distance from the point ( , ) to the point ( , ). Then, the curve length from the point ( , ) to the coordinate origin in the Frenet frame is the s-coordinate of the AV at time , and the distance length from the point ( , ) to the point ( , ) is the l-coordinate of the AV at time . Therefore, the Frenet coordinates of the vehicle at moment are ( , ). The value of can be calculated from Equations (1) and (2).
where ⃗ and ⃗ are vectors of the coordinate origin pointing to the vehicle position and the reference point, respectively; represents a symbolic function, which can only take the value of 1 or −1; is the parameter to determine the positive or negative of the coordinate, which can be calculated from Equation (3).
where is the heading angle of the reference point at moment . When the Frenet coordinates ( , ) of the AV at moment are known, the Cartesian coordinates of the AV at that time can be found with the help of the heading angle of the reference line. As shown in Figure 3, the unit tangent vector ⃗ and the unit normal vector ⃗ can be found at all points on the reference line. The angle between the ⃗ and the parallel line of the x-axis in the Cartesian coordinate system is defined as the heading angle of that point, and ⃗ is in the same direction as the d-axis of the Frenet frame at this moment. The coordinates in the Cartesian coordinate system of the AV at moment can be calculated by Equation (4). When the Frenet coordinates (s t , d t ) of the AV at moment t are known, the Cartesian coordinates of the AV at that time can be found with the help of the heading angle of the reference line. As shown in Figure 3, the unit tangent vector → t r and the unit normal vector → n r can be found at all points on the reference line. The angle θ r between the → t r and the parallel line of the x-axis in the Cartesian coordinate system is defined as the heading angle of that point, and → n r is in the same direction as the d-axis of the Frenet frame at this moment. The coordinates in the Cartesian coordinate system of the AV at moment t can be calculated by Equation (4).
The premise of the above transformation process is that the points on the s-axis in the Frenet frame are known. AVs usually undertake global path planning based on the HD map according to their origin and destination. Then the lane center lines at each stage is spliced to obtain the reference line by referring to the road information given by the global path planning. From the perspective of vehicle dynamics and comfort, the reference line must not have any curvature interruption. The discrete original waypoints obtained on the HD map are interpolated by applying the cubic spline curve to obtain the reference line that matches the human driver's driving habits. The reference line is usually used as the s-axis of the Frenet frame in the motion planning process.
The s-coordinates in the Frenet frame are expected to be mapped to the Cartesian coordinate system. The path consisting of the original waypoints is sampled uniformly, and its cumulative length is expressed as S i . The reference line is parameterized with S i as the independent variable. The coordinates (X ri , Y ri ) of the reference line are transformed into functions of S i , i.e., X ri = f x (S i ), and Y ri = f y (S i ), respectively. Taking the process of interpolation of (S i , X ri ) as an example, the process of cubic spline interpolation is shown in Equations (5)- (9).
where h i = S i+1 − S i represents step size; m i is a quadratic differential value that satisfies the condition h i m i The value of m i can be solved by applying the Not-A-Knot boundary condition to these equations.
After the cubic spline interpolations, the information of all the sampling points on the pre-selected driving centerline can be obtained in the Frenet frame and the Cartesian coordinate system. A usable reference line can be obtained. For the convenience of presentation, the longitudinal and lateral positions mentioned in this paper represent the coordinates in the Frenet frame. The vehicle positions in the Cartesian coordinate system will be expressed in x-and y-coordinates.

Trajectories Generation
In this section, the optional trajectories of the AV are described by mathematical equations. Safety and comfort are especially important during the motion of an AV. The degree of jerking is used to evaluate the comfort level when generating a trajectory, i.e., the derivative of the vehicle acceleration. The polynomial can generate smooth curves while imposing a series of constraints on the curves, which is very suitable for solving the problem of trajectory generation. The trajectories in a time-varying and two-dimensional space need to satisfy multiple constraints from the lateral and longitudinal motion in the Frenet frame. Because the accuracy and speed of the solution can be affected when the order of the polynomial is too high, the lateral and longitudinal motions of the vehicle are described as polynomials of the vehicle state concerning time.
For d t1 represent the starting lateral acceleration and the ending lateral acceleration of the vehicle, respectively. The motion planning problem can be transformed into a constrained functional extremum solution problem as shown in Equation (10).
where f lateral (t) is the mathematical function concerning the lateral distance of the motion of the AV with respect to moment t; dt is the integral of the square of the lateral motion jerk from t0 to t1.
Obviously f lateral (t) must be a continuous bounded function in [t0, t1], otherwise ... f lateral would appear to be infinite and make the problem unsolvable. The quintic polynomial shown in Equation (11) is used to represent f lateral (t).
where a d0 , a d1 , a d2 , a d3 , a d4 , and a d5 are the parameters of the quintic polynomial, which can be solved by substituting the constraints to obtain the values of each parameter, as shown in Equations (12)- (17). For convenience, assume that t1 − t0 = τ.
It is important to note that f lateral (t) is not a standard planning process because the AV cannot move only in the lateral direction with moment t. Therefore f lateral (t) can only guarantee the smoothness and comfort of the trajectories in the lateral direction. It is also necessary to describe and constrain the longitudinal motion.
Similar to the lateral motion planning, the longitudinal motion planning problem can be transformed into f ind f longitudinal (t) that minimizes ..
dt. It is feasible to describe longitudinal motion in the same manner, but the driving strategies sometimes affect the vehicle's motion constraints. In the usual case, the AV is expected to arrive at a specified location within time τ to execute the order of the upper system. The constraints of longitudinal motion in this case are like those on lateral motion, and a quintic polynomial as shown in Equation (18) is used to represent f longitudinal (t) in this case.
where s t0 and s t1 represent the starting longitudinal position and the ending longitudinal position of the vehicle motion, respectively; . s t0 and . s t1 represent the starting longitudinal speed and the ending longitudinal speed of the vehicle, respectively; .. s t0 and .. s t1 represent the starting longitudinal acceleration and the ending longitudinal acceleration of the AV, respectively; and a s0 , a s1 , a s2 , a s3 , a s4 , and a s5 are the parameters of the quintic polynomial, which can be solved by substituting the constraints to obtain the values of each parameter, as shown in Equations (19)-(24).
In addition, there are cases when we want the AV to maintain a constant speed while ensuring safety and smoothness of trajectories, and its ending position within a planning period is not considered. Then, the number of constraints is 5 (i.e., starting longitudinal position s t0 , starting longitudinal speed s t1 ). A quadratic polynomial, as shown in Equation (25), is used to represent the f longitudinal (t) for the velocity maintenance case.
, and b s4 are the parameters of the quintic polynomial, which can be solved by substituting the constraints to obtain the values of each parameter, as shown in Equations (26)- (30).
After determining f lateral (t) and f longitudinal (t), a series of smooth trajectories can be obtained between the starting position and all optional ending positions of the AV in a motion planning period.

Optimal Trajectories Searching Based on the Cost Function
In a motion planning period, optional trajectories are obtained after selecting an optional ending position. The performance of the trajectories is evaluated by defining the cost function; a series of costs and their weights are defined as shown in Table 1. In consideration of human comfort, we consider lateral and longitudinal jerking, and the jerking is used to evaluate the comfort level of trajectories when the AV is moving, i.e., the derivative of the AV's acceleration. In consideration of efficiency, we also penalize longer trajectories that take more time to assess. In addition, shorter trajectories can sometimes lead to higher mobility. In general, we expect the AV to follow the reference line, so we penalize deviations from the reference line. For safety, we introduce the reciprocal of the cumulative distance between the trajectory and the obstacles to avoid risky behaviors. The weights of the cost function can be adjusted according to different scenarios. Equal weights are used in this study, which means each indicator is considered to be equally important.

Cost
Weight As shown in Table 1, C J d and C J s represent the jerk cost of lateral motion and the jerk cost of longitudinal motion of the AV. C o f f set represents the degree of offset of the AV from the reference line. C velocity represents the deviation between the planning speed and the desired speed. distance x−obstacle represents the minimum distance between the AV and the obstacles. C collision represents the cost of a collision between the AV and the static or dynamic obstacles.
In an automated vehicle system, these costs are calculated in the form of discrete sampling points. Then, all the integral operations are replaced by cumulative summation. Supposing that there are n sampling moments in the planning period [t0, t1], the cost function of a trajectory is expressed as Equation (31).
where C total represents the total cost of an optional trajectory. Therefore, the trajectory with the lowest cost can be found by calculating the cost of all trajectories in turn.

Final Path Selection
After the optimal trajectory searching process is completed, all trajectories are sorted in order of cost from minimum to maximum, and pass through the trajectory checking process in this order.
As shown in Figure 4, assuming that there are m sets of different combinations of lateral ending position d t1 , ending time t1, and ending speed . s t1 in the sampling space of motion planning at moment t0, there are m optional ending positions in the sampling space of the AV in this motion planning period, which means that there are m optional trajectories. The cost of m trajectories is calculated according to Equation (31). These trajectories are sorted in order of cost from minimum to maximum. From the perspective of safety and comfort, these trajectories need to be checked by the maximum velocity, maximum acceleration, maximum curvature, and collision to obtain a preliminary total of m trajectories satisfying those physical properties. The first trajectory that passes the check in this sorted set of trajectories is the final path selection for the current period. The AV then moves along this path until the start of the next planning period.

Final Path Selection
After the optimal trajectory searching process is completed, all trajectories are sorted in order of cost from minimum to maximum, and pass through the trajectory checking process in this order.
As shown in Figure 4, assuming that there are sets of different combinations of lateral ending position , ending time 1, and ending speed in the sampling space of motion planning at moment 0, there are optional ending positions in the sampling space of the AV in this motion planning period, which means that there are optional trajectories. The cost of trajectories is calculated according to Equation (31). These trajectories are sorted in order of cost from minimum to maximum. From the perspective of safety and comfort, these trajectories need to be checked by the maximum velocity, maximum acceleration, maximum curvature, and collision to obtain a preliminary total of ′ trajectories satisfying those physical properties. The first trajectory that passes the check in this sorted set of trajectories is the final path selection for the current period. The AV then moves along this path until the start of the next planning period.
When the next planning period starts, the AV's motion state is considered as the initial state of the AV for the next period and the above process is repeated. The AV obtains the complete path through continuous loop iteration.

Improved Optimal Trajectory Searching Process Based on SAA
After determining the sampling space, it is common practice to compute all the trajectories and to examine the trajectories [22,23,31]. The costs of all trajectories in a typical sampling space are shown in Figure 5, where different trajectories are distinguished by the lateral and longitudinal coordinates of the Frenet frame at the ending position of the When the next planning period starts, the AV's motion state is considered as the initial state of the AV for the next period and the above process is repeated. The AV obtains the complete path through continuous loop iteration.

Improved Optimal Trajectory Searching Process Based on SAA
After determining the sampling space, it is common practice to compute all the trajectories and to examine the trajectories [22,23,31]. The costs of all trajectories in a typical sampling space are shown in Figure 5, where different trajectories are distinguished by the lateral and longitudinal coordinates of the Frenet frame at the ending position of the AV during the motion planning period. Assuming that the initial position of the AV is located on the reference line, the costs of different trajectories without obstacles are shown in Figure 5a, where the x-axis represents the longitudinal positions of the ending points of the trajectories, the y-axis represents the transverse positions of the ending points of the trajectories, and the z-axis represents the costs. Figure 5b shows the contour projection based on Figure 5a. It can be observed that the costs show an obvious axisymmetric regularity about the reference line. High-cost trajectories end off of the reference line, whereas low-cost trajectories tend to end on the reference line. This conclusion can also be verified in theory through the cost calculation function in Equation (31), because we penalize the degree of reference line offset and the lateral jerk. In addition, the other penalty terms also show the same symmetry regularity along the reference line. Based on this regularity, it can be speculated that when there are obstacles in the sampling space, the cost distribution of the trajectories close to the obstacles will produce obvious fluctuations.
low-cost trajectories tend to end on the reference line. This conclusion can also be verified in theory through the cost calculation function in Equation (31), because we penalize the degree of reference line offset and the lateral jerk. In addition, the other penalty terms also show the same symmetry regularity along the reference line. Based on this regularity, it can be speculated that when there are obstacles in the sampling space, the cost distribution of the trajectories close to the obstacles will produce obvious fluctuations. To verify this speculation, two scenarios with the existence of static obstacles were performed. It is assumed that the starting position of the AV is located on the reference line, and the AV drives at 40 /ℎ along the reference line direction. The costs of different trajectories with a static obstacle placed at 35 along the reference line direction directly in front of the AV is shown in Figure 6a in front of the adjacent left lane. In this case, the costs are similar to the previous static obstacle case. However, the fluctuation is not obvious for longer trajectories, which is because the arc of long trajectories circumvents the obstacles closer to the starting point. Based on the above verification, it can be determined that there is a certain regularity in the costs of the trajectories. The costs show an axisymmetric regularity when the starting position of the AV is located on the reference line, and the existence of obstacles will lead to an increase in the costs of the nearby trajectories. To verify this speculation, two scenarios with the existence of static obstacles were performed. It is assumed that the starting position of the AV is located on the reference line, and the AV drives at 40 km/h along the reference line direction. The costs of different trajectories with a static obstacle placed at 35 m along the reference line direction directly in front of the AV is shown in Figure 6a, while the x-axis indicates the longitudinal positions of the ending points of the trajectory, the y-axis indicates the lateral positions of the ending points of the trajectory, and the z-axis indicates the costs. Figure 6b shows the contour projection of the costs. It can be observed that when there is a static obstacle on the reference line, the costs of all trajectories increase significantly along the reference line, i.e., the lateral ending position at 0 m. Figure 6c,d show the costs of different trajectories when the static obstacle is placed 35 m in front of the adjacent left lane. In this case, the costs are similar to the previous static obstacle case. However, the fluctuation is not obvious for longer trajectories, which is because the arc of long trajectories circumvents the obstacles closer to the starting point. Based on the above verification, it can be determined that there is a certain regularity in the costs of the trajectories. The costs show an axisymmetric regularity when the starting position of the AV is located on the reference line, and the existence of obstacles will lead to an increase in the costs of the nearby trajectories.
Combining this symmetry regularity, we modify the cost function of the AV to ensure that it better follows the driving habits of human drivers and has better safety and comfort. Due to traffic rules and driving habits, human drivers in China tend to pass or avoid other vehicles from the left side. Therefore, we hope that the trajectory on the left side will be less costly when there are feasible trajectories on both sides of the reference line. The cost is modified as shown in Equation (32).
where C f inal represents the modified cost; ε represents a very small non-zero positive number; sign represents the sign function; d t1 represents the distance between the ending position of the trajectory and the reference line.  Combining this symmetry regularity, we modify the cost function of the AV to ensure that it better follows the driving habits of human drivers and has better safety and comfort. Due to traffic rules and driving habits, human drivers in China tend to pass or avoid other vehicles from the left side. Therefore, we hope that the trajectory on the left side will be less costly when there are feasible trajectories on both sides of the reference line. The cost is modified as shown in Equation (32). Therefore, the cost of all trajectories in the sampling space of a planning period forms a nonconvex space. The global optimal solution is often found by calculating the costs of all trajectories after sampling to prevent finding a locally optimal solution. We use the SAA to replace the process of calculating the cost, which refines the sampling interval with a fixed number of samples. The process of finding the optimal trajectory by SAA is contained in the dashed box in Figure 7. The variables in the optional solution are the lateral ending position of the trajectory (di), the planning time during the period (ti), and the longitudinal ending speed of the trajectory (tv). The optimal combination of variables is obtained by the searching process, including the inner isothermal layer and the probability adjustment of the outer layer with constant cooling. In addition, the corresponding trajectory is checked immediately after each set of solutions and generated to ensure comfort and safety. A new set of solutions is generated if the check does not pass. The computational effort of trajectory generation and cost calculation will be greatly reduced by setting a reasonable iteration number and temperature coefficients. The key part of SAA is the Metropolis acceptance rule. We avoid falling into a local optimum by utilizing the property of accepting poorer solutions with a certain probability described mathematically, as shown in Equation (33).
where P represents the probability of accepting the new solution; Cost now and Cost next represent the costs of the trajectories corresponding to the current solution and the new solution, respectively; k is the temperature dropping rate, and the value is between 0 and 1; T represents the system temperature.
Therefore, the cost of all trajectories in the sampling space of a planning period forms a nonconvex space. The global optimal solution is often found by calculating the costs of all trajectories after sampling to prevent finding a locally optimal solution. We use the SAA to replace the process of calculating the cost, which refines the sampling interval with a fixed number of samples. The process of finding the optimal trajectory by SAA is contained in the dashed box in Figure 7. The variables in the optional solution are the lateral ending position of the trajectory ( ), the planning time during the period ( ), and the longitudinal ending speed of the trajectory ( ). The optimal combination of variables is obtained by the searching process, including the inner isothermal layer and the probability adjustment of the outer layer with constant cooling. In addition, the corresponding trajectory is checked immediately after each set of solutions and generated to ensure comfort and safety. A new set of solutions is generated if the check does not pass. The computational effort of trajectory generation and cost calculation will be greatly reduced by setting a reasonable iteration number and temperature coefficients. The key part of SAA is the Metropolis acceptance rule. We avoid falling into a local optimum by utilizing the property of accepting poorer solutions with a certain probability described mathematically, as shown in Equation (33).
where represents the probability of accepting the new solution; and represent the costs of the trajectories corresponding to the current solution and the new solution, respectively; is the temperature dropping rate, and the value is between 0 and 1; represents the system temperature.

Numerical Experiments
In this section, the performance of the proposed motion planning method is analyzed to demonstrate the effectiveness in several typical traffic scenarios on a three-lane road, including maintaining speed, lane changing, and car following. It was assumed that decisions of the AV were provided from the behavior planning layer, and all traffic participants were going in the same direction. In these experiments, the lateral and longitudinal motions of the AV were given by the quadratic or quintic polynomials described in Equations (11), (18), and (25). By sampling the road environment ahead, a series of optional motion trajectories consisting of different lateral and longitudinal motions are obtained.

Numerical Experiments
In this section, the performance of the proposed motion planning method is analyzed to demonstrate the effectiveness in several typical traffic scenarios on a three-lane road, including maintaining speed, lane changing, and car following. It was assumed that decisions of the AV were provided from the behavior planning layer, and all traffic participants were going in the same direction. In these experiments, the lateral and longitudinal motions of the AV were given by the quadratic or quintic polynomials described in Equations (11), (18) and (25). By sampling the road environment ahead, a series of optional motion trajectories consisting of different lateral and longitudinal motions are obtained. The traversal method and the SAA-based method proposed in this study were applied to search for the optimal trajectory among these optional trajectories, respectively. In each planning period, the AV followed the optimal path that was searched for and checked. The state of the AV at the end of a planning period was inherited as the initial state of the next period, and we obtained the global driving performance of the AV by cycling through this process.
We compared the traversal method [22,23,31] and the improved method based on SAA under the same environment and parameter settings, and analyzed the computational effort, motion planning performance, and driving comfort.

Scenario and Parameter Settings
The AV was placed in a three-lane urban road scenario. The whole road was connected by 10 original waypoints according to the cubic spline curve interpolation method in this paper. The rough length of the road was 500 m, but there were curves within the first 200 m, and the width of each lane was 3.6 m. Three vehicles driven by human drivers at different speeds drove around the AV at the beginning of the simulation, and parameters of all vehicles were set as shown in Table 2.  For convenience, we named the method that calculates the trajectory cost based on the exhaustive method, Method A, and the method based on SAA, Method B. To compare the two methods, the sampling space needed to be set with consistent settings, and the parameters of the sampling space for the AV in each motion planning period were set as shown in Table 3. For Method A, the number of generated trajectories and costs that needed to be calculated increases exponentially along with the increase in the number of sampling points, which could not satisfy the requirement of trajectory computation. Therefore, we limited the number of lateral sampling points in Method A to improve the calculation performance.
For Method B, the parameters of SAA were set as shown in Table 4. Table 4. Parameters of SAA.

Parameters of SAA Values
Initial temperature ( • C) 100 Length of Markov chain 5 Rate of temperature decrease 0.9 The temperature at which the algorithm stops ( • C) 3

Performance of the AV with Methods A and B
All vehicles departed at the same time in the preset dynamic scenario. The AV followed the maintain speed command for the first 15 s, received the command to change to the left lane after 15 s, and performed the following command after the longitudinal distance with the left obstructing vehicle was less than 25 m. The trajectory tracking module of the AV was considered to be performing under ideal operating conditions. We tested the methods on a computer with an AMD Ryzen 5 3600 CPU (6-core, 3.9 GHz) and 16 GB RAM. The two experiments were run on the same computer with the same configuration, and the computing elapsed times were extremely long, and affected by plotting and data storage. The program can be simplified to meet real-time requirements in practical applications. The performance of the AV when applying Methods A and B is shown in Figures 8 and 9, respectively. The blue rectangle represents the AV, and the black rectangles represent the obstructing vehicles driven by human drivers. The green and red trajectory lines represent the optional and nonoptional trajectories planned by the AV at the current moment, respectively. The series of blue trajectory points extending in front of the AV represent its selected optimal trajectory at the current moment, and similarly, the black trajectory points in front of the obstructing vehicles represent the predicted points of their motion trajectories in the future period. The traces left by each vehicle represent its real trajectory in the past.

Performance of the AV with Methods A and B
All vehicles departed at the same time in the preset dynamic scenario. The AV followed the maintain speed command for the first 15 s, received the command to change to the left lane after 15 s, and performed the following command after the longitudinal distance with the left obstructing vehicle was less than 25 . The trajectory tracking module of the AV was considered to be performing under ideal operating conditions. We tested the methods on a computer with an AMD Ryzen 5 3600 CPU (6-core, 3.9 GHz) and 16 GB RAM. The two experiments were run on the same computer with the same configuration, and the computing elapsed times were extremely long, and affected by plotting and data storage. The program can be simplified to meet real-time requirements in practical applications. The performance of the AV when applying Methods A and B is shown in Figures  8 and 9, respectively. The blue rectangle represents the AV, and the black rectangles represent the obstructing vehicles driven by human drivers. The green and red trajectory lines represent the optional and nonoptional trajectories planned by the AV at the current moment, respectively. The series of blue trajectory points extending in front of the AV represent its selected optimal trajectory at the current moment, and similarly, the black trajectory points in front of the obstructing vehicles represent the predicted points of their motion trajectories in the future period. The traces left by each vehicle represent its real trajectory in the past.       Figure 8f, when the distance between the AV and the obstructing vehicle in the left lane was less than 25 m, the decision command was changed to follow the vehicle, and the AV started to adjust its speed to follow the vehicle ahead.
As the searching process of Method A traversed all the sampled points, the AV calculated the costs of all the trajectories at each moment. This is evidenced by the red and green trajectories shown in Figure 8. Figure 9 shows the performance of the AV when applying the motion planning Method B. The whole calculation process took 1538.82 s, and all the settings in this test were the same as those for the test of Method A, except for the different searching method for optimal trajectories. The motion planning performance of the AV under each decision command when applying Method B was basically the same as that when applying Method A; both methods completely avoided the moving obstructing vehicles. It is worth noting that the trajectories searched for by Method B shown in Figure 9 are significantly sparser at each moment compared to those of Method A. This means that Method B may achieve the same motion planning results as Method A with less computational effort.
The driving path of the AV in two numerical experiments over time and its twodimensional projection are shown in Figure 10. The reference line is always defined as the centerline of the middle lane, and the two offsets that occur for the AV relative to the reference line correspond to the overtaking and lane changing processes in the experiments, respectively. The 3D path generated by Method A and Method B almost overlap, which indicates that the optimal trajectories selected by the AV at each moment are approximately the same when applying Method A and Method B. In addition to the safety and efficiency, the comfort of the AV needs We extracted the velocity, acceleration, and jerking of the AV in two expe alize the comfort. Figure 11a,b shows the velocity and acceleration chan In addition to the safety and efficiency, the comfort of the AV needs to be ensured. We extracted the velocity, acceleration, and jerking of the AV in two experiments to visualize the comfort. Figure 11a,b shows the velocity and acceleration changing process of the AV in the two experiments, respectively; the velocity changing based on Method A and Method B is almost the same. The averages of the absolute values of velocity error under Method A and Method B were 0.2580 m/s and 0.2450 m/s, respectively. Method B based on SAA showed better velocity performance due to more dense sampling points. The large velocity deviations of both methods occurred during the lane changing process and at the beginning of the following process. The large changes in acceleration and jerking of the AV also occurred in these two processes. The smaller the acceleration changing rate, namely the degree of jerking, the greater comfort. Figure 11c  In addition to the safety and efficiency, the comfort of the AV needs to be ensured. We extracted the velocity, acceleration, and jerking of the AV in two experiments to visualize the comfort. Figure 11a,b shows the velocity and acceleration changing process of the AV in the two experiments, respectively; the velocity changing based on Method A and Method B is almost the same. The averages of the absolute values of velocity error under Method A and Method B were 0.2580 / and 0.2450 / , respectively. Method B based on SAA showed better velocity performance due to more dense sampling points. The large velocity deviations of both methods occurred during the lane changing process and at the beginning of the following process. The large changes in acceleration and jerking of the AV also occurred in these two processes. The smaller the acceleration changing rate, namely the degree of jerking, the greater comfort. Figure 11c

Comparison of the Efficiency of Methods A and B
Because motion planning is only one part of the process of automating an AV, it is essential to conserve the computational effort of the on-board computer. After determining that the two methods can produce nearly consistent motion planning performance for an AV in dynamic environments, we focused on the differences between the two methods

Comparison of the Efficiency of Methods A and B
Because motion planning is only one part of the process of automating an AV, it is essential to conserve the computational effort of the on-board computer. After determining that the two methods can produce nearly consistent motion planning performance for an AV in dynamic environments, we focused on the differences between the two methods in terms of computational efficiency. Figure 12a,b shows the process of searching for the optimal trajectories of the AV in the first planning period at the beginning of the simulation when applying Method A and Method B, respectively. Method A traversed the sampling space at a fixed interval and computed the costs of 561 trajectories at the moment t = 0, taking 15.38 s. As shown for the searching process in Figure 12a, Method A was more detailed in searching the sampling space, but at the same time searched some unnecessary and costly trajectories. Method B computed the costs of 165 trajectories at the moment t = 0, taking 5.87 s. As shown in Figure 12b, Method B is more focused on searching for less costly trajectories with the effect of SAA, and the AV performed well in dynamic traffic environments. Method B saved 62% of the planning time compared to Method A in this planning period, which proves that method B improves the efficiency of the motion planning at the moment t = 0.

Comparison of the Efficiency of Methods A and B
Because motion planning is only one part of the process of automating an AV, it is essential to conserve the computational effort of the on-board computer. After determining that the two methods can produce nearly consistent motion planning performance for an AV in dynamic environments, we focused on the differences between the two methods in terms of computational efficiency. Figure 12a,b shows the process of searching for the optimal trajectories of the AV in the first planning period at the beginning of the simulation when applying Method A and Method B, respectively. Method A traversed the sampling space at a fixed interval and computed the costs of 561 trajectories at the moment = 0, taking 15.38 . As shown for the searching process in Figure 12a, Method A was more detailed in searching the sampling space, but at the same time searched some unnecessary and costly trajectories. Method B computed the costs of 165 trajectories at the moment = 0, taking 5.87 . As shown in Figure 12b, Method B is more focused on searching for less costly trajectories with the effect of SAA, and the AV performed well in dynamic traffic environments. Method B saved 62% of the planning time compared to Method A in this planning period, which proves that method B improves the efficiency of the motion planning at the moment = 0.
(a) (b) Figure 12. The process of searching for optimal trajectories: (a) the process of searching for optimal trajectories in the first planning period at t = 0 with Method A; (b) the process of searching for optimal trajectories in the first planning period at t = 0 with Method B.
In addition, the difference between the searching time of Method A and Method B was not constant. Figure 13 shows the comparison of the searching time between the two experiments within 25 s. It can be observed that the searching time of Method A was roughly divided into three intervals, which were the speed maintaining process from 0 to 15 s, the lane changing process from 15 to 20 s, and the following process from 20 to 25 s. This was because the change in the decision commands of the AV affected the sampling space. The speed maintaining decision command corresponded to the largest sampling space, covering three lanes, with the longest searching time. When the AV received the lane changing command, the sampling space was reduced to half of the original size, and the searching time was also reduced by nearly half. The sampling space of the AV was the smallest when it was performing the following command [35]. It is worth noting that the searching time of Method B was relatively stable and short because the searching method of SAA does not depend on the sampling space settings, but rather on the temperature settings to determine the searching process. The simulation process took 5168.84 s when Method A was applied, and took 1538.82 s when Method B was applied. Method B accelerated the whole motion planning process by 70.23% compared to Method A. was the smallest when it was performing the following command [35]. It is worth noting that the searching time of Method B was relatively stable and short because the searching method of SAA does not depend on the sampling space settings, but rather on the temperature settings to determine the searching process. The simulation process took 5168.84 when Method A was applied, and took 1538.82 when Method B was applied. Method B accelerated the whole motion planning process by 70.23% compared to Method A.

Conclusions
Combined with the analysis of the experimental results, the following conclusions can be drawn.
1. We propose a motion planning method applicable to AVs in dynamic traffic scenarios. The trajectories are solved by a polynomial in the Frenet frame, and the costs of the optional trajectories are quantified as a cost function that includes cost terms for safety, comfort, efficiency, etc. An improved optimal trajectory searching method applying SAA is proposed. The experimental results in the simulated dynamic traffic scenarios show that the method proposed in this paper is feasible and efficient. 2. The process of searching for the least costly trajectory is visualized, and the axisymmetric regularity of the costs about the reference line is summarized. Based on this, the cost function is modified to make the performance of the AV more consistent with the driving behaviors of human drivers.

Conclusions
Combined with the analysis of the experimental results, the following conclusions can be drawn.

1.
We propose a motion planning method applicable to AVs in dynamic traffic scenarios. The trajectories are solved by a polynomial in the Frenet frame, and the costs of the optional trajectories are quantified as a cost function that includes cost terms for safety, comfort, efficiency, etc. An improved optimal trajectory searching method applying SAA is proposed. The experimental results in the simulated dynamic traffic scenarios show that the method proposed in this paper is feasible and efficient.

2.
The process of searching for the least costly trajectory is visualized, and the axisymmetric regularity of the costs about the reference line is summarized. Based on this, the cost function is modified to make the performance of the AV more consistent with the driving behaviors of human drivers.

3.
Compared with the optimal trajectories searching method that traverses the sampling space, the proposed method in this paper saves 70.23% of the searching time without affecting the performance of the AV. In addition, the searching time of the proposed method shows good robustness to variations in the sampling space. This is conducive to the improvement of the motion planning adaptability of AVs in a variety of road scenarios.
In this study, we assumed that decisions of the AV were provided by the behavior planning layer. However, decision making is another challenging problem for AVs in urban scenarios. In the future, we will focus on human-like decision making and test the performance of the method in a more realistic environment.