A Holistic Safe Planner for Automated Driving Considering Interaction With Human Drivers

This article advances state-of-the-art automated driving systems with a comprehensive framework that encompasses decision making, maneuver planning, and trajectory tracking considering safety, computational efficiency, and passenger comfort. In face of the co-existence of automated vehicles (AVs) and human-driven vehicles (HDVs), a decision making framework of AVs is proposed for safe lane keeping or changing. The decision making is based on the HDVs' future motion predicted by a learning-based Long Short-Term Memory model. To quantify the uncertainties in prediction, an error ellipse is used to capture the model deviations from the ground truth to ensure driving safety. This article develops a novel method that leverages lower-order parametric curves to efficiently generate feasible, safe, and comfortable lateral movements for AVs. The planner is complemented by maneuver replanning that can guide the AV back to the original lane when confronted with unexpected blockages from surrounding vehicles. Based on real-world datasets, simulation results show that the proposed method achieves curvature compatibility, shorter trajectory length in lateral maneuvers, accurate trajectory tracking, and effective collision avoidance in lane changing.

effectively reduce congestion, enhance passenger comfort and improve energy efficiency [2] [3].One of the major challenges in automated driving is to generate a smooth, feasible, and safe trajectory.Meanwhile, this generated trajectory must consider road and vehicle constraints, and traffic rules.This work focuses on AVs' forced lane change (LC) in dynamic environments because LC happens when overtaking slowmoving traffic, avoiding obstacles, or the current lane ends.
The first step in AVs' forced LC is to generate a feasible trajectory, for which a few methods have been reported [4].This work considers the Geometric trajectory planning method because it eases real-time implementation.Geometric forms of trajectory planning include clothoid [5], basic splines [6], polynomial curves [7] [8], and Bézier [9] [10].Clothoid curves are generated using forward integration, so the computational burden is increased.The trajectory complexity of spline curves is higher than other geometrical methods [11].The use of the quintic polynomial curve saw great success due to its smooth curvature and efficiency [12] [7].The primary concern for generating higher-order polynomial curves is their requirements for many input parameters [8], while the lower-order polynomial compromises the curvature continuity.
This work uses the parametric Bézier method because of its advantages in complexity reduction, computational efficiency, and continuity [11] [13].The efficiency of the lower-order Bézier curve is evident in [14] [10] [15].Both piecewise Bézier [14] [10] and single Bézier curves [15] have been used in their works to generate trajectories for various driving environments.However, these works often fail to acknowledge the curvature continuity of the generated curves.Moreover, the method in [10] uses an optimization method to generate the curves, posing a challenge for real-time implementation.The fifth or higher-order Bézier curves can generate continuous curvature paths.For example, the fifth-order Bézier curve is used in [16] [17] for LC and obstacle avoidance.The main drawback of [16] [17] is the computational expense in iteratively generating multiple curves to find a collision-free path.An efficient trajectory generation method proposed in [18] uses a piecewise quartic Bézier curve for path smoothing, lane changing, and lane replanning.The curve maintains curvature continuity in the planned trajectory but relies on an offline database containing all potential turning angles and curve lengths, which is a drawback.This work uses a cubic piecewise Bézier curve for trajectory planning because a lower-order Bézier curve can reduce control points, thus lowering the computational burden [19].Earlier applications of the cubic piecewise Bézier curve saw great success in path-smoothing applications, where two cubic curves are joined when there is an angle change in the traveling path [20] [21].During LC maneuver the traveling path undergoes two angle changes, requiring four curves to generate the LC path.The LC curve used in this work can reduce the curve numbers to two, thereby reduces complexity while maintaining continuity.
Predicting the HDV future trajectory can be divided into physics-based, maneuver-based, and interaction-based models [22].Physics-based models predict the future trajectory based on dynamic or kinematic models governed by the laws of physics.The kinematic models are simple and easy to implement, while the dynamic model captures the vehicle interaction more precisely with the expense of computational power.Assumptions like constant acceleration, constant velocity, or constant turn rate and velocity are commonly used in kinematic models [23].Kalman filtering-based method [24] is a typical physics-based model that considers uncertainties or measurement noises in prediction.However, physics-based models are only suitable for short-term prediction of around 1 s [22].The maneuver-based models predict the trajectory based on the assumption that the vehicle's future states are independent of the surrounding vehicles and can be categorized into trajectory matching and intention prediction methods [22].The trajectory matching method often employs a clusterbased or probabilistic method trained on an offline dataset for trajectory prediction.Here, the future trajectory is learned by a model conditioned on either the identified cluster they belong to or the trajectory history.For example, a k-means clustering method is used in [25] to categorize the trajectories of the same nature, and a Gaussian model is trained on each cluster to predict the future trajectory.For the intention prediction method, the vehicle's intentions are initially identified using learning models [26], heuristics, or probabilistic models [27].The future trajectory is then generated using the geometrical trajectory generation method based on the identified intention [27].The limitation of maneuver-based prediction is the ignorance of vehicle dependencies on surrounding vehicles, leading to wrong predictions [22].This work aims to predict HDV's future trajectory by capturing the HDV's interaction with its surrounding vehicles.Hence, an interaction-based model is used in this work as it accurately captures the vehicle's interaction with its surrounding vehicles.Markov model variants are among the popular choices for probabilistic interaction aware prediction.Results from Hidden Markov Models [28] [29] and Partially Observable Markov Models [30] [31] showed promising results in trajectory prediction considering HDV interactions.However, high computational demand is a major bottleneck for implementing these methods in real-time.One solution with low computational cost is using a sequential learning model to capture the complex influence of surrounding vehicles on the vehicle considered.
This work uses a sequential learning model called Long Short-Term Memory (LSTM) for trajectory prediction.The LSTM model developed in [32]- [34] shows prominent results in trajectory prediction but fails to integrate with AV decision making.Only a few works like [35] combined the LSTM prediction into the AV decision making framework.However, they did not consider the interaction with surrounding ve-hicles and possible uncertainties from the LSTM model.It is essential to recognise that prediction from learned models is not 100% accurate.Traditional methods often quantify uncertainties in training and then integrate uncertainties to make predictions [36] [37].Nonetheless, these approaches often demand complex and laborious training procedures.In contrast, this work aims to simplify the training process while effectively capturing model uncertainties.This approach quantifies uncertainties during the model validation stage, incorporates them during real-time predictions, and provides efficient and reliable predictions.
After obtaining a feasible reference trajectory for the AV navigation, an efficient controller is needed to track the reference.The nonlinear model predictive controller (NMPC) is adopted as it can generate optimal control policy for constrained nonlinear systems.The prediction model, constraints, and cost function can be specified in nonlinear forms, offering better stability and reliability over the linear MPC [38].NMPC is suitable for the proposed work as it adopts a nonlinear bicycle model traveling on a constrained road.
The rest of the paper is organized as follows.Section II introduces the overall architecture and the vehicle model.Section III details the target vehicle position prediction and uncertainty quantification, Section IV presents the Bézier curve based trajectory planning and velocity generation, Section V explains the decision making process for AV navigation, Section VI details the reference tracking using NMPC and Section VII provides the simulation results.Section VIII draws the conclusions.The main contributions of this paper are: • A complete architecture is proposed for AV safe navigation, which consists of a predictor to predict human driver trajectory, a trajectory and velocity planner for the AV, a safety assessor to ensure the feasibility of generated trajectory, and a controller to track the final planned trajectory and velocity.• A novel computationally efficient control point placement is proposed for cubic piecewise Bézier curve suitable for real-time application.A trajectroy replanning using Bézier curve is also proposed to deal with abnormal behaviors of other traffic participants.• A path modification strategy is proposed with a low computational load, which is critical when the AV approaches the barrier during forced LC. • An efficient way to contain the uncertainties from the learning model without complicating the model training process is proposed for safe AV navigation.

II. PROBLEM DESCRIPTION AND DESIGN OVERVIEW
This section outlines the driving environment and the safety distances that AV must maintain while performing the LC.Additionally, details of a realistic vehicle model that captures the real-world vehicle's dynamics are introduced.Also, the detailed architecture for safe AV navigation is described.

A. Problem Description
Figure 1 shows the forced LC scenario, where the ego vehicle is controlled to merge between the target and lead vehicles in the merging lane as a barrier in the traveling lane is encountered.The ego vehicle is an AV, while the other vehicles in the scenario are HDVs.The safe distance to be maintained between the AV and the lead vehicle is defined as: where v ego and L ego are the longitudinal velocity and vehicle length, respectively.The term 1.5L ego is the constant safe distance that the ego vehicle must maintain from the lead vehicle, even when the ego vehicle's longitudinal velocity is zero.For a safe normal LC, the value of T h is chosen as 1.5 s [39].However, achieving a normal safe space in real-time may not always be possible.Therefore, T h can be reduced till 0.7 s to allow for a more aggressive LC [39].Consequently, the width of the safe merging area is obtained by substituting different values of T h from 1.5 to 0.7 s.This work employs the front-steered dynamic bicycle model [40] to represent the AV dynamics, as shown in Fig. 2. The model center of gravity is marked as CoG.The model uses a linear tire model based on the small slip angle assumption, where the lateral tire forces will be proportional to the slip angles.The dynamics of the vehicle are governed by where v x and v y are the longitudinal and lateral velocities, respectively.ψ represents the yaw angle.M is the vehicle mass.I z is the moment of inertia along the z axis.L f and L r represent the distance from the front and rear wheels to the CoG, respectively.x and y represent the global longitudinal and lateral positions of the vehicle in the inertial coordinates, respectively.a is the vehicle longitudinal acceleration.δ f is the steering angle.F yf and F yr represent the lateral tire forces in the front and rear wheels that are given by where C f and C r are the front and rear cornering stiffness.α f and α r are the front and rear slip angles defined by The AV model used in NMPC assumes linear tire forces as mentioned in (3), while the ego vehicle in Fig. 1 takes into account nonlinear tire forces [41].Thus, the lateral tire forces in (3) are modified to where α f,op and α r,op are the optimal slip angles for the front and rear tires, respectively.The maximum lateral forces in the front (F yf,max ) and rear (F yr,max ) tires are

B. Overall Architecture for AV Safe Navigation
The overall architecture of the proposed AV safe navigation system design is outlined in Fig. 3.The system is initiated by collecting real-world data, then the data is pre-processed for training and validating a sequential learning model.The learned model will be utilized for predicting the future location of the target vehicle in real-time.trajectory of the target vehicle in a decision making module to estimate the feasibility of LC.If the LC cannot be performed, the AV reverts the decision to lane keeping.After the final path and velocity are obtained from the decision making module, they are tracked by the AV using the NMPC controller.

III. TARGET VEHICLE PREDICTION
This section introduces the network structure, the training process of the HDV prediction model, and the mitigation to tackle the uncertainties in making reliable predictions.

A. LSTM for Target Vehicle Position Prediction
An Encoder-Decoder LSTM architecture for the target vehicle position prediction is shown in Fig. 4.An LSTM cell in the encode-decoder architecture captures the dependencies between the past and current information using different gating operations [33].The LSTM mechanism is defined as where the subscripts t and t − 1 represent the current and past time steps, respectively.σ and tanh are the nonlinear activation functions.F t , I t , and O t represent the forget, input, and output gates.C t is the cell state.H t is the hidden state.The encoder summarizes the input sequence from X h1 to X hi to fixed size context vector represented as the weighted sum of the final hidden state H hi of the encoder LSTM.The context vector is then provided as an input to the decoder at each time step to generate the output sequence.Also, the decoder's cell and hidden states are initiated with the encoder's last cell state (C hi ) and hidden state (H hi ).
To show the influence of surrounding vehicles on the target vehicle, the input for each time step will be the surrounding vehicles' states relative to the target vehicle and the states of the target vehicle.In Fig. 4, the interaction period varies from h 1 to h i , and the prediction horizon varies from h p1 to h p .The time step between the inputs of the interaction horizon and the outputs of the prediction horizon is 0.1 s.The encoder inputs are: Input states X vehicle,n where vehicle ∈ [target, ego, lead, lag] are the vehicles' position and velocity in the x − y direction and the yaw angle ψ.
Therefore, there will be 20 inputs at each time step.The time step between each input is 0.1 s.The outputs from the decoder are: is the position of target vehicle in the x − y coordinates.All the input and output data are normalized before training the network due to considerable differences in the scale of each state using where Data max and Data min are the maximum and minimum values of Data, respectively.In scenarios where the lead vehicle is absent, +1 is assigned to the x − y positions, while other input states are set to zero, indicating the minimal influence of the lead vehicle on the target vehicle.Similarly, -1 is assigned when the lag vehicle is absent.The interaction period h i was substituted with 1 s, 2 s, and 3 s, respectively, to choose the best result.
Table I shows the range of network parameters used to optimize the network.The number of neurons in the network refers to the LSTM memory cells stacked at each time step to capture sequential dependencies.The batch size divides the training dataset into a fixed-length input fed to the network to update its weights and biases.Each batch will be fed to the network to make the prediction, and then these predictions will be compared to the ground truth to update the weights and biases.This work uses an Adam optimizer with a constant  learning rate of 0.001 in updating these parameters.Epoch refers to the number of times the neural network has processed the entire dataset to update the network parameters.
The model has been trained, tested, and validated by investigating a forced LC scenario in China from the INTERACTION dataset [42].The target vehicle's interaction period with the surrounding vehicles and the output prediction period have been heuristically separated to train the prediction model, similar to [34].The interaction horizon ends when the ego vehicle driven by a human shows a lateral deviation for LC or attempts the LC. Figure 5 shows an example test case, illustrating the heuristic selection of the interaction horizon endpoint.A total of 895 interaction is collected from the dataset to train and test the model.Out of which, 75% is used for training and the rest for testing.The dataset contains around 80% successful and 20% unsuccessful lane changes.

B. Handling Uncertainties using Error Ellipse
Uncertainties in the LSTM prediction must be critically considered.A bivariate variance-covariance error vector is used to construct an error ellipse that captures the model errors in both x and y directions.After that, a linear transformation is performed to obtain the eigenvalues and eigenvectors to determine the orientation of the error ellipse, as the x − y positions are highly correlated.The transformed matrix is to find the new ellipse equation and the angle at which the ellipse is aligned.The square root of the largest and smallest eigenvalues are the length of the ellipse semi-major axis and semi-minor axis, respectively.The ellipse equation after linear transformation is where EIG refers to eigenvalue transformation, σ x 2 and σ y 2 are the variance along x and y axis, respectively; σ xy is the covariance between x − y coordinates; λ 1 and λ 2 are the largest and smallest eigenvalues, respectively; s is the chisquare value.This work uses a chi-square value of 5.991, corresponding to the 95% confidence factor [43].The ellipse orientation angle is where ν 1 (1) and ν 1 (2) are the first and second components of the largest eigenvector, respectively.An error ellipse after linear transformation on an example dataset with different confidence factors is shown in Fig. 6.The ellipse size increases and covers more error data points as the confidence factor value increases.As the expanded ellipse would occupy more navigation space, the confidence factor value is set to 95%.

IV. EGO VEHICLE PLANNING
This work focuses on local trajectory and velocity planning for an LC scenario with the following assumptions: precise waypoints representing the midpoints of both merging and traveling lanes are obtained before starting the journey.The scenario also contains a smooth trajectory connecting these waypoints called the "global path".The vehicle tracks the global path before and after a maneuver is performed.The LC trajectory always starts from the global path in the merging lane.Initial acceleration at the start of LC is zero.

A. Lane Change Trajectory Planning
To reduce the complexity of higher-order parametric curves without compromising the curvature continuity, a cubic Bézier curve is used for trajectory planning with the general form of where P i represents the control points in the Cartesian coordinates.The curve starts at u = 0 and ends at u = 1.The curvature of the curve in Cartesian coordinates is where ẋ and ẍ are the first and second derivates of the curve in the x direction, respectively, and ẏ and ÿ are the first and second derivates of the curve in the y direction, respectively.A piecewise curve is used, where P 0 to P 3 refers to the control points of the first Bézier curve B1, and P 4 to P 7 the control points of refers to the second Bézier curve B2.For a smooth curve transition from B1 to B2, the curves must obey the C0, C1, and C2 continuity criteria.Hence, the control points, the first and second derivatives at the end of B1 and the beginning of B2 should be equal, as described below.
C0 continuity, B1(1) = B2(0) = Ḃ2(0) This work proposes a waypoint-based planning suitable for both straight and curved paths.The procedures and requirements to generate a LC curve are: • The first control point P 0 will be at the center of gravity of the ego vehicle.• The second control point P 1 will be on the tangent from the first control point P 0 and the immediate waypoint after P 0 in the direction travel, displaced at distance d from P 0 .• P 3 will be half the length of the total curve in the x direction and half the height of points P 1 and P 7 in the y direction.Therefore, P 3 will be at the center of the road when the lanes have equal dimensions and are parallel.Also, P 3 will be the starting point of B2, taken as P 4 .• P 6 lies on the intersection from extending the tangent from P 1 to P 4 and the tangent in the direction of P 7 to the waypoint before P 7 in the Merging Lane.Hence, P 6 will be the same distance d for symmetric B1 and B2, occuring when lanes are in parallel with the same width.• The control point P 2 will be in the direction of tangent from P 1 to P 3 and at a half distance between P 1 and P 3 .Similarly, P 5 will be at the center of P 4 and P 6 .
Figure 7 shows the trajectory generation steps using waypoints, detailed in Algorithm 1.The parameter d is estimated by iteration that aims to minimizes the lateral acceleration as in Algorithm 1.The total linear length L, depends on the lead vehicle position after t seconds.W is the linear width between control points P 0 and P 7 .If the lead vehicle is absent, L is the length that bounds the maximum lateral acceleration (|a y,max |) considered to optimize passenger comfort .Algorithm 1 shows two cases of curve generation: L is known and needs to be determined.a y is the lateral acceleration, ∆ is a small increment value, and P lead,t refers to the final lead vehicle position at the end of AV LC duration.t 1 refers to the unit tangent vector from the start point P 0 to the immediate next waypoint in the traveling lane, t 2 refers to the unit tangent vector from the endpoint P 7 to the previous waypoint in the merging lane, and t 3 refers to the unit tangent vector from P 1 to P 3 .D 1 refers to the distance between P 1 and P 3 , and D 2 refers to the distance between P 4 and P 6 .For case 2, the initial guess for the Bézier curve length is taken as where L 0 is the initial curve length.Note that control points P 1 , P 2 , P 3 , P 5 and P 6 lie on the same tangent, thus C0 to C2 continuity are satisfied as mentioned in (13).Curves B1 and B2 will be symmetric if the lead vehicle is absent.If an obstacle is detected in the traveling lane, P 3 of the B1 curve can be placed to leave a safe distance between the obstacle and the AV.According to the Bézier curve property, the control points will have precise control over the curve and can be adjusted to avoid this obstacle.Finally, B2 curve can be generated depending on the final position of the lead vehicle as described in Algorithm 1, or can be symmetric to the B1 curve if the merging lane is empty.

B. Trajectory Replanning
A trajectroy replanning method is proposed to modify the LC decision if required.Trajectroy replanning will consider uncertainties from the target vehicle, including delays from the target vehicle prediction system or unexpected aggressive acceleration to block the ego vehicle planned LC.Algorithm 1 is modified with the following steps for trajectory replanning: • The second control point will be tangent to the vehicle's current heading direction at a distance d.Distance d is obtained via iteration that gives a minimum of maximum lateral accelerations obtained with different d values.• In the presence of a lead vehicle in the traveling lane, the Bézier curve length L depends on the final position of the lead vehicle after t seconds.If the lead vehicle is absent, the algorithm finds a length that contains |a y,max | to the considered maximum limit.The other control points remain similar to the LC trajectory.Figure 8 shows the steps involved in replanning, where the green curve refers to the initial B1 planned for the LC, P 0 refers to the start point of trajectroy replanning where the AV decides to abandon LC, and t 1 refers to the unit tangent vector from P 0 to the current heading direction.Assuming the target vehicle travels along the center lane, the maximum limit of d, marked as a red cross, is obtained by subtracting the target and ego vehicles' half-width from merging lane center lane to ensure safety between the vehicles.It is not always valid to assume that the target vehicle follows the center of the lane.To this extent, the maximum limit of d can be modified to the upper border of the traveling lane, ensuring the trajectroy replanning is contained within the traveling lane.

C. Global Path Modification
The AV must consider the scenarios when the LC maneuver cannot be performed before the barrier.In this case, the AV must consider the approaching barrier, stop at a safe distance from the barrier and be ready to merge at the next LC opportunity.This is done differently from the works of [44]- [46], where the controller solves an optimization problem to ensure a safe distance from the barrier in addition to the trajectory tracking.The modified global path aims to ease the computational load of the controller while obtaining a trajectory that avoids collision into a barrier.Meanwhile, the workload on the control system is reduced to track the reference trajectory and velocity for lane keeping.This work uses a cubic Bézier curve at the end of the traveling lane to leave a safe distance from the barrier.Figure 9 shows the modified global path for two different barrier shapes in the traveling lane.The dashed-dotted line passing through the center of the ego vehicle shows the initial global path, and the red trajectory highlighted shows the modification added.P 3,global , marked as a red cross in Fig. 9, is the last control point of the modified global path.Initial estimation for P 3,global to avoid collision is chosen as the difference between barrier edge and L ego .However, if the initial estimation results in collision, P 3,global is moved backward.The process is repeated until the AV obtains a collision-free trajectory that leaves a safe distance of S t from the barrier.Algorithm 2 shows the pseudo-code to generate a modified global path B global .Collision check is carried out by ensuring the AV edge D shown in Fig. 9 leaves a safe distance (L ego ) from the barrier.Point D is selected for the collision check as this will be the first point to contact the barrier.The collision check process uses a translation and rotation matrix described in line 9 of Algorithm 2 to ease the computation.
In Algorithm 2, points D r , D, O orgin , and Seg x,y ∈ R 2 are in Cartesian coordinates.D r refers to the final point after translation and rotation of the AV edge D. O orgin is the origin considered for the rotation.The points A, B, and C of the AV marked in Fig. 9 will be transformed similarly to point D for projecting the AV shape to B global .The length of the curve (L global ) is 2.5 times the AV length (L ego ), as the AV will be cruising to a stop.Once the AV enters the modified global path, the ego vehicle follows the trajectory till the end of the curve.Therefore, only the B2 curve must be generated, and the length of the B2 depends on the lead vehicle position.During LC, if P 3 of B1 obtained via Algorithm 1 moves beyond P 3,global , P 3 will be taken as P 3,global for safety.
This work also finds a point in the modified global path marked as a black cross in Fig. 9, where the AV comes to rest and is ready to change the lane.Before LC, it is necessary to ensure that the AV stays in the traveling lane.This requirement

D. Velocity Planning
1) Lane changing: Velocity planning for the planned trajectory depends on velocity limits at each point, which depends on maximum allowable lateral acceleration and curvature.The relation between these quantities can be expressed as where v limit is the velocity limit along the planned trajectory and K is the curvature at the point.A trapezoidal acceleration profile [47] is used in this work, considering the maximum acceleration possible and the first derivative of acceleration (jerk).A general case scenario as in Fig. 10 is considered, where the profile includes 7 phases.Phases 1 to 3 are the acceleration zone, phase 4 is the constant acceleration zone, and phases 4 to 7 are the deceleration zone.The acceleration, velocity, and traveled distance at time step t are where ∆t = t i − t i−1 and the time intervals for phases 1 to 3 are calculated using the velocities at each interval are and the total traveling distance for phases 1 to 3 is given by where v 0 and v f refer to the initial and final velocities, respectively, a max is the maximum longitudinal acceleration, and j max is the maximum allowable jerk.The jerk value for phase 1 is +J max , phase 2 is 0, and phase 3 is −J max .Phase 2 may be neglected if the traveling distance is short.If the distance required to reach v f from v 0 is greater than the planned LC length, the AV accelerates after completing the LC and performs lane keeping in the merging lane until the desired velocity is achieved.Detailed explanations and special cases of velocity profiles are given in [47].Phases 5 to 7 are the reverse case of phases 1 to 3, where the velocity profile is calculated by swapping the jerk values in phases 1 and 3.The initial velocity will be v max , and the final velocity be v 0 .Phase 4 depends on the total curvilinear length of the Bézier (s total ), i.e., if s total > s 1,2,3 + s 5,6,7 , then ∆t 4 is defined by (20).
2) Lane keeping: While performing lane keeping in a forced LC scenario, the velocity planner must ensure that AV decelerates from the traveling velocity to zero as it reaches the resting point mentioned in Section IV-C.To this end, the ego vehicle calculates the distance required to decelerate from the current velocity to zero using (19) considering the maximum deceleration rate and comfort.The velocity profile for lane keeping will be the deceleration phases from 5 to 7 in Fig. 10.

V. DECISION MAKING
The data collected for the study contained LC scenarios with final target velocity reaching up to 11 m/s.Approximately 79% of LC velocity occurs between 2 to 6 m/s.Hence, the average LC duration is 4.5 s.The target vehicle position prediction is limited to 3 s as a longer horizon leads to a larger position error.Moreover, a larger position error increases the error ellipse size, so more navigation space is excluded.While limiting the target vehicle prediction horizon to 3 s, AV will cover around 66% of its total LC trajectory, leading to a reliable decision.It is preferred to leave a safe gap between the target and ego vehicles, similar to (1), throughout the LC process.However, while considering this safe gap, AV LC's success rate is drastically reduced compared to human drivers.Hence for this work, during the prediction horizon of 3 s, the minimum safety threshold to be kept between the target vehicle and the AV is taken as the length of the target vehicle.Thus, a safety threshold equivalent to the length of the target vehicle is added with the outputs from the encoder-decoder LSTM to guarantee safe navigation.This padded output will be added to the ground truth and used to construct the error ellipse to avoid further calculations.
Figure 11 shows the flow chart for the feasibility check.The initial value of τ with 1.5 s will be substituted in (1) to find the last control point of LC trajectory P 7 .Once the LC trajectory is generated using Algorithm 1, the corresponding velocity is obtained, as mentioned in Section IV-D .Once the trajectory and velocity are planned, the AV position after 3 s is projected using rotation and translation matrix as described in Algorithm 2, into the LC trajectory to check for collision with the error ellipse.Since the error ellipse does not consider the width of the target vehicle while checking for collision, the target vehicle width is added with the ego vehicle width for a more comprehensive collision assessment.If there is no collision with the error ellipse, the trajectory is selected as the final LC trajectory and velocity.If there is a collision, the value of τ is reduced, and the process collision check is repeated.The process continues till τ reaches 0.7 s.If collisions happen to all the LC trajectories the LC decision is abandoned, and the AV continues to drive in the traveling lane and selects the lag vehicle as the next target vehicle.

VI. TRAJECTORY AND VELOCITY TRACKING
An NMPC is used for trajectory and velocity tracking.The following discretized prediction model derived from (2), using the Euler method with sample time T s , is used to obtain the control sequence for the NMPC.
The outputs of the controller are δ f and a.The planning module gives the reference lateral position, longitudinal position and velocity.The controller has to bring the error between state and reference to zero, i.e.
where X s represents the states of the prediction model (lateral position, longitudinal position, and velocity) and X r refers to the corresponding reference state from the planning module.
The cost function to be minimized for the NMPC problem can be formulated as The NMPC constraints are where the prediction and control horizons are denoted by nc and np, respectively.X s (K + i + 1) defines the current states as a discretized function (F dis ) of previous states (X s (K + i)) and inputs (U (K + i)) as in (21).X s0 represents the system's initial states.X e is the tracking error as in (22).U ∈ R m×1 , where m represents the number of control inputs.Q ∈ R n×n is the weight matrix for state error, and R ∈ R m×m is the weight matrix to penalize the rate of change of control inputs.−a max and a max represent the maximum deceleration and maximum acceleration, respectively.δ f min and δ f max represent the minimum and maximum steering angle, respectively.

VII. SIMULATION RESULTS
Simulations have been carried out using MATLAB on a laptop with Intel Core i7-10850H CPU @ 2.70 GHz and 16 GB s, a max = +2.5 m/s 2 , ∆a max = +1 m/s 3 , and J max = ∆a max .The NMPC optimization problem is solved using the MATLAB nonlinear optimization function f mincon.
The simulation results reported in Section VII-A show the proposed trajectory planner's details and advantages compared to similar methods.Section VII-B covers the details of the velocity planner efficiency and altering the velocity profile according to the scenarios considered.Sections VII-C, VII-D, and VII-E cover the results obtained from the NMPC controller, the trajectroy replanning, and the modified global path, respectively.Section VII-F details the results obtained from the target vehicle prediction and AV planner integration using the INTERACTION dataset.

A. Performance of Trajectory Planning
Figure 12 shows two planned LC trajectory for the AV and its corresponding curvatures at a constant velocity of 20 m/s.The first trajectory represented with the solid lane, the vehicle must quickly steer to avoid the static vehicle.Here, the efficiency of the proposed method on precise curve control can  Achieved by multiple curve generation via iteration be observed.The control point P 3 is placed at a distance of 1.5L ego from the rear of the static lead vehicle and the curve is contained within the control points.The dotted lines in Fig. 12 show the second case where the lead vehicle in the traveling lane is moving.The curve here is generated to limit |a y,max | to 1 m/s 2 according to Algorithm 1.
It should be noted that the curves are C1 continuous at the beginning and end since they start from a non-zero value.However, these values are close to zero.Consider the solid curve in Fig. 12, which gives a curvature value of ≈ 0.0027 m −1 at the beginning and end.For this, if front steering angle approximation by Ackerman is considered: the steering angle value is within the comfort steering angle change (∆δ) that the controller can handle.Thus, the steering angle change from zero to the specific curvature value at the beginning and end can be carried out without affecting performance or passenger comfort.The same claim is mentioned and supported in [8] for trajectory planning using a cubic spline.While generating an LC Bézier curve using Algorithm 1 or a modified global path using Algorithm 2, any curve that requires the initial steering angle computed using (25) to be greater than the desired ∆δ is neglected.In such cases, the curve length is increased, which reduces the curvature thus, keeping the ∆δ within the comfort limit.
The results are compared with a quintic Bézier curve [17] and a quadratic [9] piecewise Bézier curve.Table II summarizes the results of a LC scenario that ignores the obstacles.The traveling velocity is taken as 20 m/s.Quintic and piecewise cubic curves are generated to limit |a y,max | to 1 m/s 2 .The quadratic piecewise Bézier curve is generated based on the traveling velocity [9] rather than limiting |a y,max |.The method used by [9] gives the least lateral acceleration with |a y,max | = 0.82 m/s 2 .However, considering the same velocity, the curve length is significantly longer than other curves.
Table II lists the computational time when the trajectory length L is unknown.The proposed method consumes slightly longer computation time because both L and the parameter d must be determined.Nonetheless, the computational time for generating three curves is substantially low and can be easily implemented for real-time application.It should be noted that if L is known, the computation time for the cubic piecewise Bézier curve can be further reduced.Figure 13 shows the cubic, quadratic, and quintic Bézier curves and their corresponding control points.The quintic Bézier curve uses a longer length to achieve the same lateral acceleration compared to the proposed method.
Table III compares the three methods during an obstacle avoidance scenario.For the quintic curve, as there are no control points in the middle, the curve cannot be precisely directed.Hence, obstacle avoidance is achieved by iteration, where multiple curves are generated until a collision-free curve is obtained.The computation time for the quintic Bézier curve increases during obstacle avoidance, as it may require generating multiple curves to find the optimal one.In contrast, the computational time reduces during obstacle avoidance for cubic piecewise curves as it can be treated as the case where L is known.Since the precise location of the obstacle and safe distance to be kept from the obstacle are known, L is determined before the curve generation.In any case, if L has to be determined, unlike the quintic Bézier curve, the computational time remains constant at 16.7 ms.For quadratic Bézier curve [9], as the curve generation depends on the velocity, obstacle avoidance is difficult and not covered in their study.

B. Performance of Velocity Planning
Assume that the lead vehicle in the traveling lane is moving for the static obstacle avoidance LC with a fixed curve length.is also 20 m/s. Figure 14 shows the planned trajectory, velocity profile, acceleration profile, and corresponding velocity limits.The AV should decelerate from the current velocity to V 1,min , while phases 1 to 3 of the velocity planner are used.Similarly, the AV should accelerate from V 2,min to 20 m/s.Phases 5 to 7 are used for acceleration.For region V 1,min to V 2,min , general velocity planning is used to maximize the travel time.For this region, the AV cannot achieve the a max since the distance required to travel from V 1,min to V 2,min is short.Hence, velocity is planned with a lower value of a max for this region.The traveling distance from V 1,min to 20 m/s is calculated using (19), which is around 48 m.However, with the initial trajectory marked in red, the V 1,min point comes around 9 m.Hence, the AV should perform lane keeping around 39 m to achieve V 1,min .The same calculation applies for the acceleration case V 2,min to 20 m/s.Lane keeping regions are marked as green line in Fig. 14.V 1,min and V 2,min appear at an equal distance since the road shape is symmetric.

C. Performance of Trajectory and Velocity Tracking
Figure 15 shows the results of trajectory and velocity tracking in the dynamic environment mentioned in Fig. 14.The Root mean square error came out to be 0.0059 m for trajectory reference tracking and around 0.016 m/s for velocity tracking.The time required to complete the tracking from start to end is around 2.21 s.From the trajectory and velocity obtained from the planner, the time required to complete the LC scenario mentioned in Fig. 14 is around 8.3 s.The NMPC can provide the control inputs to complete the LC trajectory and velocity tracking within a quarter of the total LC duration.Thus, the NMPC used for trajectory tracking can be efficiently implemented in real-time applications.

D. Performance of Trajectroy Replanning
Consider an unlikely scenario with the proposed decision making framework where a considerable delay was observed in the target vehicle trajectory prediction.The AV initially planned an LC trajectory and started the LC process in this case.However, the prediction obtained from the encoderdecoder LSTM shows that the target vehicle would block the LC, and there is no feasible trajectory for LC.Therefore the AV changes the LC decision initiated to lane keeping.Figure 16 shows resulting trajectory obtained after changing the LC decision to lane keeping.The green line shows the initially planned LC trajectory, assuming that the target vehicle yields for the ego vehicle.Instead, the target vehicle blocks the LC.Consider the traveling velocity of 20 m/s and a lead vehicle in the traveling lane with constant velocity.Since the LC occurs at a higher velocity, the LC duration is taken as 6 s [9].In Fig. 16, the filled plot shows the vehicles in the initial position, and the other box shows the position after 6 s.The replanned trajectory length is obtained by subtracting the final  position of the lead vehicle with S df in (1).A constant velocity replanning is considered here.For this scenario |a y,max | came around 0.9 m/s 2 .If the obtained |a y,max | is higher than the considered |a y,max |, either the replanning duration is increased, or a variable velocity planning is considered.

E. Analysis on Forced LC
Figure 17 shows the modified global path in red and LC trajectories from the traveling lane to the merging lane.Different LC length indicates merging behind the lead vehicle at different positions.Consider the ego vehicle in the traveling lane travels at 10 m/s.Using (19), the distance required to decelerate with 1.5 m/s 2 from 10 to 0 m/s comes around 40.83 m.The deceleration distance lies between the green and red cross in Fig. 17.For this work, the deceleration limit for lane keeping is considered lower than −a max to consider the possibility of an LC.Hence, AV does not have to start from −a max to begin the LC maneuver effectively.Table IV shows the effect of different interaction horizons (h i ) on the prediction horizon (h p ), where x e and y e show the RMSE error in the x and y positions.For shorter predictions, the worst performance emerges when h i = 1 s, and the performance of h i = 2 s outperforms that of h i = 3 s.However, the average x e is much larger when h i = 2 s and h p = 3 s, even though the average y e is slightly better.Thus, when h p = 3 s, the interaction horizon is set to 3 s.It is important to note that the data should be normally distributed to construct  21 and 20 show the results two successful LC cases when τ = 1.5 s, including their corresponding velocity and acceleration profiles from the test samples.In both cases, acceleration phases 1 to 3 are used for velocity planning, as the curve satisfies the considered |a y,max | due to the low-velocity.Figure 21 shows the results of the case where LC happens before the modified global path.In this case, the AV follows the global path till the end and plans the LC trajectory considering the lead vehicle position.Figure 20 shows the results of the LC case where the initial P 3 for the LC trajectory goes beyond P 3,global .In this instance, P 3 is modified to P 3,global to ensure a collision-free trajectory, as mentioned in Section IV-C.
Figure 22 shows the results of the case where the LC trajectory with τ = 1.5 s collides with the error ellipse.The collision is not directly observable in the plot.Nevertheless, when the target vehicle width gets added to the AV, LC trajectory intercepts with the error ellipse.However, a safe LC trajectory is achieved when τ = 0.9 s, enabling the AV to  execute the LC successfully.The velocity profile for the LC trajectory when τ = 0.9 s is also displayed in Fig. 22.
Figure 23 shows the case where the target vehicle blocks the LC of the AV.Here, two LC paths are depicted, i.e., when τ = 1.5 s and when τ = 0.7 s.AV collides with the ellipse in both cases and performing an LC is unsafe.
Upon expanding the proposed decision making framework on the test set, the AV was able to detect all the blocking behaviors of the target vehicle during the LC.On the other hand, about 2 % of the successful LC were misidentified as a blocking scenario.This may be due to the assumptions used in the study, like starting the LC from the global path, or because

VIII. CONCLUSIONS
The paper proposes a holistic approach for automated driving, combining an efficient lateral maneuver planner using piecewise cubic Bézier curves with a safe decision making framework.Compared to the quintic Bézier curve, the piecewise curve shows better controllability, shorter trajectory length, and compatible curvature characteristics.The coupled velocity planner generates reference velocities considering passenger comfort, and an NMPC ensures accurate tracking of the planned trajectory and velocity with negligible errors.The decision making framework incorporates an LSTM-based encoder-decoder architecture for predicting the future position of the target vehicle, ensuring collision-free courses by bounding uncertainties with error ellipses.Although initial lateral position deviation by the AV affects the accuracy of successful lane changes, the framework successfully detects blocking behaviors of all vehicles.Future work involves determining the lead vehicle position in addition to the target vehicle position for determining the Bézier curve length.

Fig. 1 .
Fig. 1.Dynamic lane change scenario considered in this paper.

Fig. 3 .
Fig. 3. AV decision making framework including the target vehicle position prediction and uncertainties.
and W HC are the weight matrices and b F , b I , b O , and b C are the bias vectors.A ⊙ B represents the Hadamard product between quantities A and B.

Fig. 5 .
Fig. 5. Scenario from INTERACTION dataset used in the study.

Algorithm 2 : 4 P 1 , 5 B6 10 if D r > S t then 11 Collision = 1 12P 3 ,
Modified global path 1 Input: Collision = 1, L ego , L global , O origin , P 3,global , ∆, S t = 0.5 L global , and AV corners (A, B, C, and D) 2 while Collision= 1 do 3 P 0,global = P 3,global -L global global , P 2,global = generated similar to P 1 and P 2 global = generate the curve similar to Algorithm 1 Seg x,y =linspace(B global ,15) 7 θ s = atan( Ṡeg x / Ṡeg y ) 8 for i=1:length (Seg x,y ) do 9 D r = cos(θ s (i)) − sin(θ s (i)) sin(θ s (i)) cos(θ s (i)) * (O origin -D) + O origin + (Seg x,y (i) − O origin ) global = P 3,global − ∆ when the AV edge C does not cross the boundary of the traveling lane.Restricting the AV within the traveling lane can be done by modifying lines 10 to 15 in Algorithm 2. The AV will be projected and cross-checked to each point in B global , to determine if C crosses the traveling lane boundary.The black cross in Fig. 9 is taken as the point before the edge C intersects with the traveling lane boundary.
Considering a constant velocity of 20 m/s, |a y,max | comes around 1.7 m/s 2 .In this case, limiting |a y,max | to the specified range can be achieved by the velocity planner.Consider the maximum limits for the vehicles as: |a y,max | = 1 m/s 2 , |a max | = 1.25 m/s 2 and |J max | = 1 m/s 3 .The final velocity considered

Fig. 17 .
Fig. 17.Modified global path, velocity profile for lane keeping, and LC trajectories from modified global path with different lead vehicle position.

Fig. 20 .
Fig. 20.AV Lane change scenario from modified global path and corresponding velocity profile.

VI. Trajectory and Velocity Tracking NMPC controller V. Decision Making
To handle uncertainties in the learned model, a bivariate error ellipse defines a boundary that captures prediction errors.The ego vehicle planning module utilizes a Bézier curve to generate an LC trajectory and a velocity profile considering passenger comfort.The planned LC trajectory is then compared with the predicted future

IV. Ego Vehicle Planning III. Target Vehicle Prediction Ego vehicle
X hi ] where Xh2Yhp2 Fig.4.Encoder-Decoder LSTM for position prediction.

TABLE I RANGE
OF NETWORK PARAMETERS CONSIDERED

TABLE II B
ÉZIER CURVE PROPERTIES COMPARISON

TABLE IV EFFECT
With a large test sample size containing 223 LC scenarios, with each LC scenario recorded at 0.1 s time step satisfies this condition and makes the error data large enough at each second to construct the ellipse.If the test samples were fewer, one ellipse could be plotted with all the error data accumulated over the prediction horizon.Figures 20 to 23 show some of the selected decision making scenarios in the test set.Figure OF h i ON hp h i = 1 s h i = 2 s h i = 3 s