Trajectory Planning and Control of Autonomous Vehicles for Static Vehicle Avoidance in Dynamic Traffic Environments

This paper presents a trajectory planning and control algorithm of autonomous vehicles for static traffic agent avoidance in multi vehicle urban environments. In urban autonomous driving, the subject vehicle encounters diverse traffic scenes including lane changing, intersection driving, and illegally parked static vehicle avoidance. Among these, dealing with illegally parked static target vehicle is a major challenge to urban autonomous driving due to large velocity difference between ego and target vehicles and interactions with surrounding vehicles. In order to tackle this problem, we introduce a decision making and motion planning framework for static vehicle avoidance considering both the preceding static vehicles and surrounding vehicles. Among the surrounding vehicles, the set of objects with potential collision risk is selected based on the lane boundaries and road geometry. Then, the driving status of the selected target vehicles are classified as normal driving vehicles or parked vehicles based on their longitudinal speed, lateral position and lateral space occupancy. For the preceding parked vehicles, the motion planner generates lateral and longitudinal evasive motion, by taking side lane traffic flow and risk into account. The desired motion is executed by applying optimized control inputs computed by lateral and longitudinal model predictive controllers. The performance validation of the proposed algorithm has been conducted with actual autonomous test vehicles. The test results confirmed that the proposed algorithm can successfully perform evasive maneuvers on urban roads to ensure safety and mitigate collision risk with the surrounding traffic agents.


I. INTRODUCTION
Advances in autonomous vehicle systems significantly reduced road accidents caused by diverse human driver errors [1]. Automobile safety standards and technologies have been expanded from passive safety to active safety, and technologies are being compulsorily applied to production vehicles [2]. Moreover, driver safety and assistance functions have been actively studied over the past decade for highway The associate editor coordinating the review of this manuscript and approving it for publication was Emre Koyuncu . driving situations [3]. Advanced Driver Assistance Systems (ADAS) for highway semi-autonomous driving provides lane keeping control and adaptive cruise control functions. These functions allow drivers to travel most of the distance without the intervention of throttle/brake pedals and steering. ADAS has been proven to effectively prevent accidents and reduce damage in case of accidents while increasing driver convenience [4].
The difficulties of urban autonomous driving compared to highway autonomous driving are mainly due to irregular motion of surrounding vehicles, which includes stationary motion within the lane due to roadside parking [5]. Various studies have addressed the motion planning algorithm of autonomous vehicles for avoiding collision with the surrounding traffic agents using various techniques, including artificial potential field, waypoint sampling, and predictive control approaches. In [6] and [7], an artificial potential field and predictive control based path planner is introduced for obstacle avoidance. The heuristic potential functions are designed to prevent road departure and collision with crossable and non-crossable obstacles. Similar approaches were adopted in [8], where side lane vehicles are also considered for the constrained optimization problem. Although these studies consider the behavior of surrounding vehicles, the results were provided in simulation environments under the assumption that the true states of the target vehicles are known in advance. However, accurate surrounding vehicle information including bounding boxes needs to be provided form the perception algorithm for stable operation of actual on road autonomous vehicles.
Sampling based spatio-temporal motion planning in consideration of surrounding target vehicles is an efficient approach for mid-range vehicle behavior planning [9], [10]. Waypoint nodes are defined as the center points of the current and adjacent driving lanes with a predefined longitudinal spacing. The selection of each node for a given horizon determines whether the ego vehicle performs a lane changing motion. By defining feasible sample trajectories of the ego vehicle considering surrounding vehicles' predicted motion, the optimal trajectory is selected based on the travel cost and constraints. Although this method has been shown to effectively optimize the ego vehicle motion in presence of multiple target vehicles, limitations are present when applied to partially occupying in-lane obstacles. Due to the nature of discretized node sampling, avoidance of static in-lane obstacles is achieved by lane changing motion. In order to perform in-lane evasive lateral motion regarding the same obstacle, it is necessary to add multiple node candidates with different lateral deviations in each lane and longitudinal position. Methods regarding this approach were introduced in [11], where a search based multiple node expansion and path optimization methods are elaborated. In [12], a sampling and optimization based path and velocity profile generation method was introduced. Geometric and dynamic constraints have been incorporated for the optimization problem, and the method has been validated through path tracking and obstacle avoidance maneuvers of an actual test vehicle.
Previous researches based on model predictive control (MPC) approaches for risk mitigation are elaborated in [7], [13], [14], and [15]. Cesari et al. [16] presented a scenario model predictive control based lane change assistance function to consider uncertainties in traffic conditions. Based on the defined environmental constraints and the dynamics model, the MPC controller ensures the stability of the system and tracks the desired target states [17]. In [18] and [19], a safe driving envelope-based obstacle evasive motion planning framework is introduced. The safe environmental envelope that prevents collision with the surrounding obstacles on the road is defined as a tube, and the desired vehicle states are planned by applying the tube as a constraint. Also, a stable handling envelope is defined and applied as a state constraint in order to secure lateral stability. Similar approaches are adopted in [20], where the ego vehicle's lateral error boundaries defined for obstacle collision avoidance vary linearly along the path centerline station. These studies mainly deal with the avoidance behavior of obstacles within a single lane, and it is assumed that the state and intention of the obstacles are known in advance.
Inferring the intention of the stationary front obstacle has been addressed in several studies regarding urban autonomous driving. In [21], the term 'parked vehicles' was defined and the relevant features for parked vehicle classification were presented. The parked status of the vehicle was predicted using a support vector machine (SVM) based classifier. Similarly, a deep neural network based static vehicle overtaking decision algorithm was introduced in [22]. The methods presented in both papers predicted the parked status of a stationary vehicle with high accuracy, but the avoidance motion planning algorithm regarding the stopped vehicle is not provided. Even if a lane change decision is made, in dynamic traffic environments, the ego vehicle must continuously assess the collision risk and plan its motion accordingly. Typical examples of these motion include lane change motion abort, stop and wait in lane depending on the behavior of the side lane vehicles.
A trajectory planning and control algorithm for urban autonomous driving is proposed, which decides the required avoidance behavior according to the states of the preceding obstacle and plans the subsequent motion in consideration of the collision risk with the surrounding traffic agents. The proposed framework selects the obstacle of interest and determines its static states at the initial stage. Based on the obstacle velocity and lane occupancy, the decision maker outputs the desired driving mode for obstacle avoidance. Next, the motion planner generates the reference motion to implement the risk mitigation strategy for the obstacle. While executing the desired motion, the collision risk of the side lane vehicle as well as the front target vehicle is assessed and the subsequent vehicle response is updated. Lastly, the planned motion is tracked using a model predictive control based lateral and longitudinal controllers. The control inputs, steering wheel angle and longitudinal acceleration, are optimized under state and actuation constraints and are applied to the autonomous vehicle system. The proposed framework was tested in urban traffic environments with a full size actual autonomous vehicle.
The contributions of our work are summarized as follows: 1) A decision making and motion planning framework is proposed, which enables different strategies depending on the status of the preceding static vehicle. 2) The collision risk is mitigated in consideration of the surrounding target vehicles. 3) The proposed algorithm was validated through actual vehicle tests in urban driving environments.
The remainder of the paper is organized as follows: Section II provides the structure of the autonomous driving vehicle system and describes the behavior of the traffic agents under general urban driving situations; Section III illustrates the target vehicle management and decision making processes; Section IV formulates the motion planning and control algorithm; Section V discusses the performance of the algorithm through actual vehicle test results; Finally, Section VI concludes the paper.

A. OVERALL ARCHITECTURE OF AUTONOMOUS DRIVING SYSTEM AND TEST VEHICLE
The overall autonomous driving algorithm constructed for this study is described in Fig. 1, which is implemented in industrial PC and algorithm processor modules. The implemented algorithms have been developed based on Robot Operating System (ROS). The structure of the entire system, including the proposed motion planning and control algorithm, can be divided into four main modules: localization, perception, motion planning, and control. The localization and perception module provides ego vehicle pose and surrounding environment information to the motion planning module. Equipped with a differential GPS device and frontal view monitor cameras, the global position and heading of the ego vehicle is estimated from the localization module. The states of the ego vehicle are estimated from the vehicle chassis sensor measurements using extended Kalman filtering [23]. The raw measurement information obtained from the environment sensor set consisting of 32 channel 360-degree LiDAR sensors and front vision camera with 78 degrees of Field of View (FOV) are transferred to the perception module. The perception algorithm then generates the current state information of surrounding vehicles and the static obstacle occupancy map [24]. The configuration and specifications of the autonomous driving test vehicle are summarized in Fig. 2.
Based on the outputs processed from the localization and perception modules and the high definition map (HD map) Urban roads with multiple lanes in one way are operated in a similar manner. Overtaking must be performed using the left lane of the driving lane, and vehicles are encouraged to use the inner lane in the order of desired travel speed. Along with these characteristics, there are lanes for designated uses such as straight driving, left turn, right turn, and U-turn. However, there are vehicles that demonstrate irregular behavior in the corresponding lane. One typical example is illegally parked vehicle, which often occupies the rightmost lane for passenger loading and unloading. In order to respond to these vehicles, surrounding vehicles perform a lane change motion or an avoidance maneuver depending on the traffic situation.

III. DECISION MAKING IN CONSIDERATION OF STATIC TRAFFIC AGENTS A. SAFE DRIVABLE ENVELOPE-BASED ENVIRONMENT REPRESENTATION
The developed autonomous vehicle system for our work acquires the necessary surrounding environment information using LiDAR and vision sensors. Among these, the point cloud data from the LiDAR sensor are mainly used for precise detection of surrounding objects. The perception module provides the states of the detected vehicle tracks including position, velocity and acceleration [26]. In the case of basic lane keeping and clearance control maneuvers, the corresponding functions have been successfully performed only with the aforementioned target state information [27]. However, in order to plan the avoidance behavior which will be presented in section IV, it is necessary to accurately measure how much the obstacle occupies the lateral space of the driving lane as well as the states of the LiDAR track. Therefore, in this study, a static obstacle map is constructed based on the detected raw point cloud data [24]. Represented as a grid map with a longitudinal and lateral resolution of 0.1m defined in the ego vehicle local frame, the static obstacle map contains the probability of each grid being occupied by a static obstacle. The grid points with static probability of 95% and above are considered as being occupied by a static obstacle.
Among the constructed static obstacle map grid points, the ones having collision risk with the ego vehicle are filtered based on the HD map lane information. That is, the static obstacles within the left and right boundary of the current driving lane of the ego vehicle are selected to construct a safe drivable envelope, which defines the range of collision free lateral displacement as a function of longitudinal position. A similar approach regarding the lateral obstacle avoidance has been verified in [28]. The safe drivable envelope is constructed in the following order. Initially, the maximum and minimum drivable range at a particular longitudinal position are each defined as the lateral position of left and right lane boundaries expressed in the vehicle local frame. The vehicle coordinate system is adopted according to the description provided in Fig. 3. If obstacle points exist within the lane boundary, these are divided into left and right obstacles based on their relative lateral position with respect to the lane centerline. Next, the initially computed envelope is narrowed down to the lateral position of the innermost obstacle among the ones which exist within half the spacing of the envelope in longitudinal direction from each decision point. The envelope decision point, longitudinal region of interest (ROI) and the envelope modification law are defined as follows: where τ ROI is the time gap for envelope ROI decision, N env denotes the number of envelope decision points. ϵ is the lateral buffer for safety margin, which is set to 0.2m in order to consider the possible uncertainties of perception module measurements. v x and v x,min each denotes the ego vehicle longitudinal speed and its lower bound saturation value. The only vehicle parameter, w veh , is the width of the ego vehicle. The longitudinal range of the ROI is defined proportional to the speed of the ego vehicle, where τ ROI is the factor multiplied to the vehicle speed. The discretized envelope points x env,i are separated with equal longitudinal distance of ds, where ds can be obtained by dividing the ROI length with the number of envelope decision points. The velocity-proportional ROI length model is adopted since the evasive maneuver at higher speed requires longer preview distance in order to secure ride comfort and safety [29].
On the other hand, the minimum speed for ROI calculation is defined for preventing extremely short preview distance at low speeds. The envelope defined in (1) provides valid lateral boundaries for the ego vehicle if the obstacle does not cross the centerline of the driving lane. However, although the obstacles do not fully occupy the driving lane width, they might exist across the lane center. Although it is practically possible to avoid these obstacles, the defined envelope narrows the drivable area to the extent that the ego vehicle cannot pass through it. This is because the innermost obstacles on the left and right are selected as obstacles located at the center of the driving lane. To overcome these VOLUME 11, 2023 shortcomings, the left and right spaces of each obstacle are proposed in this work. Based on the left and right spaces, the possibility of obstacle avoidance behavior is redetermined when the envelope lateral space is insufficient for passing. The left and right spaces are defined as follows: where y obs denotes the lateral positions of the set of obstacles within the lane boundary. The geometric representation of the left and right spaces is provided in Fig. 4. If the presented safe drivable envelope and the free spaces are calculated in parallel, these information are fed to the target management module in order to predict the intentions and driving lane occupancy of the preceding obstacles. Subsequently, the ego vehicle can establish a different behavior plan according to each situation. More details are described in section III-B.  lane, left lane and right lane [30]. The state variables of the j-th surrounding vehicles are provided from the perception module as follows: where x, y, ψ denotes the local position and heading of the target measured in the ego vehicle reference frame, v x is the longitudinal velocity. The collision risk of side lane vehicle is determined by the relative longitudinal distance and the target safety distance. If the distance between the ego vehicle and the side lane target vehicle is greater than the desired safety distance, the target vehicle is not considered as a risky target.
On the other hand, if the target vehicle exists within the safety distance, it is considered as a risky target. The front and rear safety distance of the side lane target vehicle are defined as follows: where L veh is the vehicle length, v x denotes the ego vehicle longitudinal velocity, τ rel and τ LC,front/rear are the time gap for relative velocity and absolute velocity terms. C safe,front and C safe,rear each denote the minimum longitudinal clearance for front and rear target vehicles. The parameters designed for the safety distances are summarized in Table 1.  3) Class 3: Fully occupying obstacles that require lane changing motion for avoidance The presence of Class 1 obstacle does not alter the driving lane of the ego vehicle, but it is necessary for the ego vehicle to replan the path within the current driving lane boundaries to secure a safe lateral margin from the obstacle. Class 2 obstacles are similar to the ones in Class 1, but these obstacles occupy greater lateral lane space so that the available lateral space within the current driving lane becomes less than the width of the ego vehicle. Therefore, the ego vehicle should cross the driving lane boundary while executing the evasive motion. Also, another condition is added regarding the left side free space of the obstacle in order to determine the parking intention of the obstacle [21]. In our work, obstacles having left side free space greater than a threshold distance are classified as Class 2. This implies that the obstacle is biased toward the right side, willing to allow the following vehicles to overtake and pass by. Finally, obstacles that fully occupy the lateral lane space are classified into Class 3. Although these obstacles may be avoided with either cross-lane evasive motion and lane changing motion, performing a lane changing motion is appropriate if the ego vehicle should invade a major portion of the side lane space while executing the cross-lane evasive motion. The conditions to distinguish the class of the static obstacles are described in Fig. 5.

C. DRIVING MODE DECISION BASED ON OBSTACLE STATES AND TRAFFIC RISK
As introduced in section III-B, the main driving modes of an autonomous vehicle are lane centering mode and lane changing mode. In this study, two additional driving modes (mild shift, severe shift) were defined and added as possible actions for the ego vehicle to take in order to avoid the preceding static obstacles. The overall decision-making process for obstacle avoidance is depicted in Fig. 6 in the form of a state flow. Detailed motion planning algorithm which takes the resulting decision as the input will be discussed in the upcoming sections.  Fig. 6, the ego vehicle escapes the evasive maneuver mode and re-enters the lane centering mode when the preceding obstacle is passed.

A. MPC BASED LONGITUDINAL AND LATERAL CONTROL ARCHITECTURE
For each driving mode determined by the decision maker, the motion planning algorithm plans the desired longitudinal VOLUME 11, 2023 and lateral motion based on the surrounding target vehicle information and the safe drivable envelope. The motion planner calculates the desired reference states of the ego vehicle at the future prediction time for a total of 2 seconds at time intervals of 0.1 seconds. A model predictive control (MPC) based longitudinal and lateral controllers then calculate the desired acceleration and steering angle input to follow the desired reference motion.
The reference longitudinal motion of the ego vehicle is determined by the desired longitudinal states at each prediction step. In our work, the longitudinal states are defined as the longitudinal position, velocity, and acceleration of the ego vehicle. The control input is defined as the desired longitudinal acceleration. The longitudinal prediction model is based on a kinematic model with a first order acceleration input delay in order to consider the desired acceleration tracking characteristics of the actual vehicle [31]. The continuous time state space representation of the proposed longitudinal vehicle model is as follows: where p x , v x , a x are the longitudinal position, velocity, acceleration of the ego vehicle expressed in the local coordinate, τ lon is the time constant of the first order acceleration delay model, a x,des is the desired longitudinal acceleration. The prediction model is then discretized under a constant time interval as below: where z lon (k) is the longitudinal state vector at the k-th prediction step, t is the discretization time interval, A lon,d and B lon,d are the discretized system matrices. The initial state vector is defined as the measured current vehicle state. An MPC problem is formulated in order to calculate the cost optimal control input under state and input constraints. The cost function and constraints for the MPC are defined as follows: subject to z lon (k + 1) = A lon,d z lon (k) where z lon and z lon,ref are the predicted ego vehicle longitudinal state vector and the desired reference longitudinal state vector at each prediction time step, u lon is the control input. z lon,min and z lon,max correspond to the predicted longitudinal state bounds, Q lon and R lon each denotes the longitudinal state and input weight matrices. The feasible input range and the input slew rate are bounded as expressed in (7c) and (7d). Based on this objective function which is quadratic in state error and input, the MPC controller yields an optimal longitudinal acceleration input which minimizes the reference state tracking error and control effort within the prediction horizon. For the longitudinal MPC controller, the state boundaries of (7e) are defined as the longitudinal position of the nearest in-lane target vehicle and following vehicle [32]. The maximum and minimum longitudinal acceleration inputs are constrained to 1.5m/s 2 and -5m/s 2 . The maximum acceleration slew rate is set as 4m/s 2 . The values of these parameters have been set to ensure ride comfort and safety of the passenger while the autonomous vehicle utilizes the longitudinal acceleration range similar to that of a human driver [27], [33], [34]. The lateral motion planner generates the desired reference lateral states of the ego vehicle. Utilizing the dynamic bicycle model and path tracking error dynamics [35], the lateral states are defined as the side slip angle, yaw rate, heading error, and lateral error. Graphical representations for the lateral states are provided in Fig. 7. The control input is the desired steering angle of the vehicle. The dynamics of the lateral vehicle model in continuous time domain are defined as follows: where κ road is the curvature of the road, m is the vehicle mass, l f and l r are the distances from the front and rear wheel axles to the mass center, I z is the yaw inertia, C f and C r are the cornering stiffness of the front and rear tires. Similar to the longitudinal vehicle model, the lateral vehicle model (8) is also discretized into the following form: where z lat (k) is the lateral state vector at the k-th prediction step, A lat,d , B lat,d , F lat,d are the discretized system matrices.
The lateral MPC problem is then formulated as follows, which generates an optimal steering angle that regulates the predicted lateral state error and control effort under constraints: where z lat and z lat,ref are the predicted and reference state vector at each prediction time step, z lat,min and z lat,max are the lateral state bounds, Q lat and R lat denote the lateral state and input weight matrices. The maximum steering wheel angle magnitude is set to 450deg, which corresponds to the full right and left turn steering angles. The steering wheel angle rate is limited to 400deg/s, in order to limit rapid control inputs and generate smooth steering wheel motion. For lateral stability, the desired side slip angle and heading error within the prediction horizon is set to zero. The reference yaw rate of the vehicle is defined based on the geometry of the target path and the speed of the vehicle. Assuming that the vehicle follows the center of the target path, the desired yaw rate of the vehicle at each prediction step can be expressed as the product of the path curvature and longitudinal velocity. The sign of the road curvature is defined as positive for a left handed curve, and negative for right handed curve. The counter clockwise direction of yaw rate is therefore defined as positive, which matches the sign convention defined in Fig. 7.

B. LANE CENTERING AND CLEARANCE CONTROL
The control object of the ego vehicle during lane centering mode is to regulate the lateral and yaw error with respect to the reference path. Therefore, the reference lateral state for lane centering is defined as follows: The longitudinal motion is planned to keep a safe distance with the preceding vehicle while following the desired speed profile, which is determined by the road limit speed and path curvature. Similar to the safety distances of the side lane vehicles which are defined in (4), the safety distance of the preceding target vehicle is defined as follows, in consideration of the velocities of ego and target vehicle: where v x,prc denotes the target vehicle longitudinal velocity, τ LK is the time gap for absolute velocity term, C safe,LK denotes the minimum longitudinal clearance for preceding in-lane target vehicle. The preceding target vehicle safety distance parameters are provided in Table 2.
Depending on the relative position of the preceding target vehicle, the reference longitudinal position and velocity are VOLUME 11, 2023 planned as follows: where the subscripts prc and ego stand for the preceding target and ego vehicle, t is the prediction time interval. Variables with parentheses correspond to the values evaluated at the prediction step number denoted in the parentheses. The formulated reference state profiles are then fed to the lateral and longitudinal MPC.

C. PARTIALLY OCCUPYING OBSTACLE AVOIDANCE VIA MILD EVASIVE MOTION
In case when a static obstacle of Class 1 is present within the driving lane, the motion planner should generate a lateral motion which can drive the ego vehicle to avoid collision without crossing the lane boundaries. The desired lateral offset is determined according to the geometry and status of the safety envelope introduced in section III-A. If the left and right envelope boundaries do not intersect each other, the ego vehicle can travel without collision. In this case, the desired lateral position is set as the mean value of the left and right envelope in order to follow the center of the defined envelope boundary, and the corresponding desired offset can be expressed as follows: (14) where y center,i is the y-coordinate of the driving lane evaluated at the longitudinal envelope position x env,i . The lateral motion planner then calculates the lateral target position for the MPC controller. It mitigates the collision risk of the ego vehicle by applying the prioritized value among the obtained lateral offset from the entire range of the envelope. For example, if there is no obstacle within the control horizon but there is an obstacle that requires avoidance at a point on the target path at a greater distance, the corresponding target lateral offset is applied as the desired lateral offset. The longitudinal motion is planned in the same manner as proposed for the case of lane centering and clearance control. Since the Class 1 obstacle does not require the ego vehicle to decelerate, only the preceding in-lane target vehicle affects the desired position and velocity of the ego vehicle. Therefore, the reference longitudinal states are determined according to (13).

D. PARTIALLY OCCUPYING OBSTACLE AVOIDANCE VIA SEVERE EVASIVE MOTION 1) RISK FREE EVASIVE MOTION FOR CLASS 2 OBSTACLES
As described in the decision making process, the ego vehicle can respond to Class 2 obstacles in either two ways: taking evasive motion across the lane boundary when no risk is present, or waiting behind the obstacle until the risk is resolved. The evasive vehicle motion when there is no side lane risk is depicted in Fig. 8 (a).
Since the safety envelope boundaries and the corresponding envelope midpoints are valid for in-lane evasive motion, the cross lane evasive motion necessary for Class 2 obstacle is formulated based on the free spaces which are defined as (2). The value of desired lateral offset which ensures side to side space margin between the ego vehicle and the obstacle is obtained as follows: where ϵ lat is the lateral margin which is set to 1.4m, w veh denotes the width of the ego vehicle. If the calculated target lateral motion is performed to avoid obstacles without collision, the ego vehicle does not need to reduce its speed. However, human drivers feel uncomfortable when driving next to an obstacle at their maximum allowable speed, so they apply deceleration when taking evasive motion and return back to the original target speed after passing the obstacle. In order to quantify this tendency, a set of manual  driving data is collected to observe the change in driving speed according to the lateral offset of a human driver. The experiment was conducted under traffic free environment with single target vehicle present as an obstacle, which is the case described in Fig. 8 (a). The cumulative driving results are shown in Fig. 9. A total 4181 data points of lateral position and longitudinal velocity information have been obtained from the experiment, where the ego vehicle was driven by a test driver with multiple years of urban and highway driving experience.
The green point denotes the start of the evasive motion, and the blue markers indicate the points where the ego vehicle reaches the maximum lateral offset during the maneuver. The normalized longitudinal velocity is obtained by dividing the velocity by the initial vehicle speed at the beginning of each avoidance behavior. Similarly, the initial lateral deviation is subtracted for each trial in order to eliminate the effect of lateral error bias generated by human driver during the experiment. Therefore, the initial normalized velocity and lateral deviation are set to 1 and 0, respectively. It can be inferred that the human driver generates steep deceleration rate during the start of the avoidance behavior and reaches a constant velocity when sufficient lateral offset is reached. In order to model the normalized velocity as a function of lateral displacement while reflecting the previously observed tendency, the following expression is proposed: where η and η min are the velocity reduction factor and its lower bound, e nom is the nominal lateral offset where velocity saturation takes place. The model parameters η min and e nom are set to 0.7 and 0.6m, respectively. The dashed line in Fig. 9 represents the normalized velocity value calculated from the approximated function model.

2) WAITING MOTION FOR RISK RESOLVE
When side lane risk is present, the ego vehicle cannot perform the aforementioned evasive motion. As described in Fig. 8 (b), the ego vehicle stops behind the obstacle with a desired longitudinal clearance until the risk is resolved. While it is impossible to cross the lane boundary immediately, the ego vehicle can convey its lane crossing intention to the side lane traffic agents by applying lateral traffic pressure within the current driving lane [36]. This induces the yielding of the side rear target vehicle, which can be beneficial in terms of ego vehicle longitudinal motion progress. Also, it has the advantage of moving less lateral distance during evasive motion after the risk is resolved compared to the case of stopping behind the obstacle at the center of the driving lane. The desired lateral offset for applying traffic pressure while waiting behind the obstacle is defined as follows: where ϵ TP is the lateral margin for traffic pressure, which denotes the minimum distance between the left lane boundary and the left side edge of the ego vehicle. The longitudinal desired motion is straightforward, by setting the reference position as the position of the obstacle subtracted by a constant safety distance value. Similar to the reference shaping defined in (13a) which is the case of lane centering and clearance control, the reference position for waiting motion is defined as follows: (18) where p x,prc is the relative longitudinal position of the obstacle, C pv is the desired safety clearance between the ego vehicle and the obstacle. The parameters ϵ TP and C pv are set to 0.3m and 5.0m each.

E. FULLY OCCUPYING OBSTACLE AVOIDANCE
In order to respond to Class 3 obstacles, the ego vehicle can execute two types of maneuvers: avoidance by lane VOLUME 11, 2023  In lane change mode, a geometric reference trajectory is planned to realize the lane change maneuver considering the collision avoidance and driver acceptance. A sigmoid function, which is continuous and differentiable, is used to design the lane change trajectory in the spatial domain. In fact, the sigmoid function has been widely applied to autonomous lane change tasks and validated by many previous studies [37], [38]. To follow the lane geometry efficiently, the lane change trajectory is designed on the curvilinear coordinate system with the base frame constructed based on the lane centerline [39]. The formula of the trajectory is described as below: d(s) = ± 1 1 + e k(s−l lc /2)/l lc · w road (19) where s and d are the longitudinal and lateral coordinate, respectively; k is the coefficient related to the trajectory where l lc,free = τ lc · v x + l lc,min , l lc,avoid = 2(x obs − l margin ) (20b) where l lc,free is the trajectory length considering the free traffic situation where no preceding static obstacle exists; v x is the longitudinal speed of the ego vehicle; τ lc and l lc,min are the coefficients related to linear modeling of l lc,free ; l lc,avoid is the trajectory length considering the collision avoidance situation where Class 3 obstacles exist; x obs is the distance from the ego vehicle to the Class 3 obstacle; l margin is the margin distance from the ego vehicle to the Class 3 obstacle to secure the safety. As shown in Fig. 11, l lc,avoid is designed in the sense that the ego vehicle maintains the distance gap of l margin with the Class 3 obstacle at the lane crossing point (s, d) = (l lc /2, ±w road /2). τ lc and l lc,min are determined as 9.0s and 10m considering the driver acceptance, respectively. The shaping coefficient k is applied as the value of 7 for l lc > 20, and 4 for l lc < 20 [40]. Therefore, the lateral motion controller follows the sigmoid reference trajectory if lane change mode is activated responding to the Class 3 obstacles. Similar to (13) in Section IV-B, the longitudinal motion for the lane change maneuver is planned to maintain a safe distance with the front vehicle in the side lane and  follow the desired speed profile. The reference position and velocity for lane change maneuver are determined as follows: where the subscripts front and ego stand for the front target in the side lane and ego vehicle, respectively; SD front is the safety distance of the front target in the side lane described in (4a). Therefore, the longitudinal MPC follows the formulated reference states in lane change mode.

V. VEHICLE TEST RESULTS
The proposed motion planning and control algorithm was verified and tested in an urban public road using a full size actual autonomous test vehicle. The experiment was conducted on urban roads in South Korea. Non-autonomous driving target vehicles and autonomous driving ego vehicle were driven together on the road. Unlike general road conditions, illegally parked vehicles in the lane frequently exist at the test site. Therefore, the ego vehicle mainly responded to the situation of avoiding these parked vehicles.
The experiment was conducted on two-lane and three-lane roads, where the parked vehicles exist in the rightmost lane.

A. MILD EVASIVE MOTION FOR PARTIALLY OCCUPYING OBSTACLE AVOIDANCE
The vehicle test results including the on-board front camera image snapshots for mild evasive motion are shown in this section. Each of the three photos from Fig. 12 depicts the scene when the vehicle detects an obstacle, travels alongside the target vehicle, and starts returning to the center of the lane. Fig. 13 shows the history of the ego vehicle states and the relative position of the obstacle. The lateral offset of the static obstacle, plotted in magenta, is defined as the measured lateral position of the detected obstacle with respect to the center of the driving lane at each time step. The ego vehicle initially drives in lane centering mode, following the center of the desired path. As the ego vehicle approaches the Class 1 obstacle, the envelope is modified in advance according to the constructed static obstacle map. As can be seen in the lateral offset plot depicted in Fig. 13 (a), the lateral avoidance motion starts at time t = 16 sec and remains until passing the obstacle. The shaded plot in Fig. 13 (a) denotes the lateral displacement swept by the area between the left and right edges of the ego vehicle. The subject vehicle generates smooth proactive behavior without using abrupt steering and acceleration control inputs, which is shown in Fig. 13 Table 3. The ego vehicle returns to the center of the driving lane when all the obstacles are passed. During the evasive motion, it can be inferred that a minimum lateral margin of 0.8m is secured at the moment when the ego vehicle passes the obstacle. As shown in Fig. 13 (b), the ego vehicle maintains its initial longitudinal speed since the obstacle is not selected as an in-lane vehicle that requires longitudinal clearance control. Therefore, it can be seen that the ego vehicle does not perform unnecessary deceleration and can execute the desired evasive motion without disturbing the traffic flow of the driving lane.

B. SEVERE EVASIVE MOTION FOR PARTIALLY OCCUPYING OBSTACLE AVOIDANCE
Vehicle test results for severe evasive motion are summarized in Figs. 15 -17, in a similar manner as provided in   section V-A. In this experiment, the ego vehicle performs two consecutive evasive motions. The first evasive motion takes place at time t = 17 sec to t = 25 sec, where side lane collision risk is not present. As can be seen in Fig. 16 (a), the ego vehicle passes the first set of obstacles without collision. The on board camera images of this maneuver are illustrated in Fig. 15 (a) -(c). The ego vehicle however encounters the second set of obstacles when the left side lane risk is present. The time region in which the side lane risk was detected is indicated by red shaded areas in Fig. 16 (b) and (d). Since the Class 2 obstacles were detected while the side lane risk was present, the ego vehicle initially plans to stop behind the obstacle without executing lateral evasive motion. The side lane target and in lane obstacles are visualized in Fig. 15 (d). Therefore, negative acceleration input is applied at time t = 25 sec to t = 29 sec, reducing the longitudinal velocity. At time t = 30 sec, the risk is resolved as the side lane vehicle completely passes the ego vehicle. The ego vehicle then initiates the lateral motion and passes the obstacle, where the process is depicted in Fig. 15 (e), (f). The lateral positions of the static obstacle and the edges of the ego vehicle illustrated in Fig. 16 (a) suggest that for both evasive maneuvers, minimum lateral margin of 0.8m is secured without colliding with the obstacles.
As can be seen from the longitudinal and lateral acceleration distribution shown in Fig. 17, a wider range of acceleration was generated compared with the case of mild evasive motion. The magnitudes of longitudinal and lateral accelerations reached a maximum value of 1.55m/s 2 and 1.99m/s 2 each, which does not exceed the acceleration range experienced in general lane change driving motion [32]. It can be seen that the positive value of the bar with the highest frequency in the longitudinal acceleration histogram is near 0, which reflects the characteristics of maintaining less longitudinal input magnitude when the ego vehicle is accelerating. The statistics of the acceleration distributions are summarized in Table 4.   The longitudinal and lateral acceleration distributions during the lane changing motion are depicted in Fig. 21. Since the ego vehicle tries to reach its target speed while it starts the lane changing motion at rest, positive longitudinal acceleration is observed at high frequency. Table 5 provides the statistics of the acceleration distribution during the maneuver.

VI. CONCLUSION
A trajectory planning and control algorithm under dynamic traffic environments for avoidance of static traffic agent has been developed and verified by actual vehicle experiments.
The test vehicle equipped with the proposed algorithm effectively avoided the static obstacles parked in the driving lane. Safe drivable envelope and free spaces were introduced to discriminate the drivable area under urban driving conditions and to efficiently respond to on-road obstacles. The motion planner generates the reference longitudinal and lateral states to be tracked by the ego vehicle. The longitudinal reference states were defined as the target longitudinal position and velocity to drive while maintaining a safe clearance from the preceding target vehicle. The lateral target states are defined as the desired yaw rate according to the curvature of the road and the desired lateral offset with respect to the center of the path.
In the case of in-lane obstacle avoidance motion, the lateral motion planner determines the target lateral offset to drive along the midpoints of the constructed envelopes. If the free space of the lane is narrow for in-lane evasive maneuver, the ego vehicle performs an evasive maneuver which invades the side lane under risk free conditions or stops behind the obstacle if the side lane collision risk is present. On the other hand, when the obstacle is fully occupying the lane, the ego vehicle decelerates to maintain a safe distance from the target or attempts to overtake by changing lanes according to the presence of side lane risk. In order to proactively respond to obstacles outside the predicted horizon, priority was given to target lateral offset values, and through this, the motion planner effectively responded to obstacles that required severe lateral motion or had a high risk of collision. The control inputs for following the reference states with safety guarantees are obtained by solving longitudinal and lateral MPC problems. Actual vehicle test results confirmed that the proposed motion planning and control architecture is directly applicable for urban autonomous driving.
Future work aims at extending the proposed framework to various urban driving scenarios, including signalized and unsignalized intersections. In the situation of driving at an intersection, it is necessary to proactively respond not only to the obstacles recognized at the current time but also to the vehicles that are about to merge into the current driving lane. In addition, it is expected that if the intention of the preceding vehicle to stop within the driving lane is inferred, the avoidance motion of the ego vehicle can be performed in advance, which is much closer to a human-like behavior.
CHANGHEE KIM received the B.S. degree in mechanical engineering from Seoul National University, South Korea, in 2019, where he is currently pursuing the Ph.D. degree in mechanical engineering. His research interests include motion planning in urban environments, autonomous vehicle control, and optimal trajectory planning for autonomous race cars.
YOUNGMIN YOON received the B.S. degree in mechanical engineering from Seoul National University, South Korea, in 2018, where he is currently pursuing the Ph.D. degree in mechanical engineering. His research interests include motion prediction of surrounding vehicles, predictive motion planning, and automated driving vehicle control.
SANGYOON KIM received the B.S. degree in mechanical engineering from Seoul National University, South Korea, in 2021, where he is currently pursuing the Ph.D. degree in mechanical engineering. His research interests include optimal control and motion planning of autonomous vehicles.
MICHAEL JINSOO YOO received the B.S. and M.Eng. degrees in biological and environmental engineering from Cornell University. He is currently pursuing the Ph.D. degree in mechanical engineering with Seoul National University, Seoul, South Korea. His research interests include autonomous driving vehicle motion planning and control in urban environments.
KYONGSU YI (Member, IEEE) received the B.S. and M.S. degrees in mechanical engineering from Seoul National University, South Korea, in 1985 and 1987, respectively, and the Ph.D. degree in mechanical engineering from the University of California, Berkeley, in 1992. He is currently a Professor with the School of Mechanical Engineering, Seoul National University. His research interests include control systems, driver assistant systems active safety systems, and automated driving of ground vehicles.