Sage Journals: Discover world-class research

Abstract

Motion planners for autonomous driving improve traffic safety through collision-free motion generation along the path. However, conventional motion planners render passengers uncomfortable as a result of jerky motion. To overcome this, we propose a model predictive control (MPC) based motion planner that not only ensures safety but also improves driving comfort. The proposed planner generates path tracking and collision-free maneuvers to ensure safety, and improve driving comfort by minimizing acceleration and jerk. Collision-free maneuvers include vehicle following and overtaking. The target speed is determined by comprehensively considering path tracking performance improvement and whether overtaking is possible. The speed of the vehicle is controlled by considering longitudinal acceleration and jerk minimization. The steering command is determined by considering both path tracking error reduction, and lateral acceleration and jerk minimization. In cases where vehicle overtaking is required and during high-speed driving conditions, the consideration of lateral acceleration and jerk minimization increases to improve driving comfort. The proposed planner is formulated as a convex optimization problem. The effectiveness of the proposed planner was evaluated in path tracking and collision avoidance simulations. The simulation results confirm that the proposed planner ensures vehicle safety through lane keeping and collision avoidance, and improves driving comfort.

Keywords

Autonomous vehicles model predictive control motion planning collision avoidance path tracking driving comfort

Introduction

Motion planning algorithms are essential elements in the field of autonomous driving, as well as the fields of mobile robots and manipulators. In particular, autonomous driving has been the subject of attention in the past decade as a key to solving human error,¹ which accounts for 94% of all traffic accidents. Autonomous vehicles drive with collision-free motion along a path generated by the motion planning algorithm. The most popular motion planning algorithms in the field of autonomous driving are adaptive cruise control (ACC)² and lane keeping assist system (LKAS).³ ACC is a function that maintains distance and speed to prevent collisions between a driving vehicle and a preceding vehicle and has reduced traffic accident fatalities by approximately 9%.² LKAS is a system that assists steering wheel control such that the vehicle does not deviate from the driving lane and is expected to prevent lane departure accidents by up to 66.5%.³ The motion planning algorithm applied to autonomous driving has improved overall traffic safety, but unfortunately it has provided discomfort to some drivers and passengers.

The discomfort of the driver or passenger is caused by jerky motion, such as a sudden stop for autonomous vehicles to avoid collisions and a rapid acceleration for speed tracking. Some passengers of Tesla vehicles have complained about the discomfort caused by accelerating and decelerating motion while using Autopilot.⁴ Additionally, sudden changes in steering are also factors that cause motion sickness and discomfort in passengers.^5,6 The frequency of vehicle jerk increases because of a congested road environment or the aggressive movement of surrounding vehicles.^7,8 Therefore, in order for a vehicle to minimize jerk, a smooth driving motion must be generated, considering various constraints that occur while driving.

Model predictive control (MPC) is a representative motion planning algorithm that generates optimal motion while considering various constraints. MPC determines the optimal control signal based on the control system’s future behavior prediction and constraints.⁹ MPC-based motion plannings are divided into hierarchical approach and direct approach. The hierarchical approach creates a collision-free local path and determines the desired control input for path tracking. Local paths are generated based on path planning algorithms such as A*,¹⁰ dynamic programing,¹¹ parametric curves,^12–14 artificial potential field.^15,16 Here, since path planning algorithms do not consider the motion constraints of the vehicle, an impossible path may be created. In the hierarchical approach, an impossible path leads to the generation of the desired motion with degraded tracking performance. On the other hand, the direct approach determines the desired motion without creating a collision-free local path. Therefore, the direct approach has advantages in the process of determining the desired motion, compared with the hierarchical approach.

In this paper, we propose an MPC-based motion planning algorithm that minimizes jerk without collision by utilizing a direct approach. The proposed algorithm plans motions using convex optimization to quickly find feasible control commands. Through this process, we create optimal maneuvers that include both vehicle following and overtaking. The generated maneuver allows the vehicle to be provided with minimal jerk and collision freedom. Therefore, the proposed algorithm ensures driving safety through collision-free motion and improves the driving comfort of passengers through jerk minimization. This paper is organized as follows: Section 2 presents a review of the existing MPC-based motion planning algorithms. Our vehicle motion model is introduced in Section 3. In Section 4, we discuss the constraints for creating control commands. In Section 5, we design a cost function for motion planning. The calculation of the target speed required for cost function formulation is described in Section 6. The proposed algorithm is evaluated in Section 7. Finally, Section 8 presents the conclusions of the study.

Related works

To ensure safety in complex driving environments, several driving strategies for collision avoidance have been used, including vehicle following,¹⁷ overtaking,¹⁸ emergency braking,¹⁹ and evasive steering.²⁰ However, driving strategies that involve rapid motion changes significantly increase acceleration and jerk, making passengers uncomfortable. In addition, a sudden change in motion can cause accidents between a driving vehicle and adjacent vehicles.²¹ Therefore, a new driving strategy is required to improve driving comfort while ensuring driving safety. A representative approach to consider driving safety and comfort is to use vehicle following or overtaking.

ACC is a representative technology for vehicle following. The latest ACC-based approaches are MPC-based motion planners that consider driving comfort as well. Gabalawy et al.¹⁷ limited the longitudinal acceleration and jerk ranges to [−3 m/s², 5 m/s²] and [−5 m/s³, 5 m/s³], respectively, to ensure driving comfort. The proposed motion planner converges to the desired distance and speed without exceeding the maximum acceleration by setting the acceleration and jerk boundary constraints. However, the proposed method has the disadvantage of using constant cost weights. Cost weights are parameters that determine the extent to which the motion planner considers driving safety and driving comfort. If driving comfort has a much greater weight than driving safety, the motion planner generates motion that stops gradually to maintain driving comfort in a situation where a sudden stop is required. In this case, the vehicle cannot stop, resulting in an accident. To address this problem, methods to adjust the cost weights between driving comfort and driving safety according to driving conditions in real time have been proposed.^22,23 The proposed methods reset the cost weights based on the distance to the leading vehicle and the relative speed. If the leading vehicle is closer than the desired distance and the ego vehicle is faster, the distance and speed control weights are increased. Conversely, if the leading vehicle is farther than the desired distance, the acceleration control weight is increased. As a result, it is possible to converge more quickly to the desired distance and the target speed in a scenario where the leading vehicle that the ego vehicle follows suddenly changes. Furthermore, the driving comfort was improved by up to 63.26%. However, the actual road environment requires not only acceleration, but also various motions that require steering control such as left turns, right turns, and lane changes. Since the motion planner that considers only the vehicle following strategy generates only longitudinal motion without considering steering, there is a limit to its use in a real road environment.

An overtaking strategy wherein steering is considered generates a swerving maneuver that passes the leading vehicle in the same lane. Wang et al.²⁴ proposed an MPC-based motion planner that avoids obstacles near the global path with only steering control at the current speed. However, the proposed motion planner sets the driving speed of the ego vehicle as a constant. Therefore, as the steering angle increases, the lateral acceleration and jerk increase significantly, reducing the driving comfort. Gutjahr et al.²⁵ used a speed profile rather than a constant speed in the calculation of steering control commands to improve driving comfort. In addition, the range of vehicle curvature is limited based on the vehicle’s maximum physical steering angle and the conditions to maintain wheel traction at a given speed profile. However, the proposed method is dependent on a predetermined speed profile. Thus, if an incorrect speed profile is provided, the range of acceptable steering angles is reduced and the path tracking performance is degraded. Werling and Liccardo²⁶ proposed a motion planner that determines the speed and steering angle together. The proposed motion planner sets the acceleration constraint such that the vector sum of the longitudinal and lateral accelerations enters the traction circle representing the range at which the tire grip is maintained. However, because of the nonlinear vehicle model, the MPC formulation becomes non-convex, and the calculation of the feasible solution is generally slow compared with the planner in the form of a convex optimization problem.²⁷ Dixit et al.¹⁸ and Taherian et al.²⁸ proposed a method of formulating the overtaking problem with convex optimization. The proposed planners use a linearized vehicle model and set the collision-free space as a convex region. Here, a collision-free space is set using a ramp barrier to guide the vehicle to the adjacent lane. However, ramp barriers limit the maximum longitudinal distance that a vehicle can move, causing unnecessary deceleration while moving into an adjacent lane. On the other hand, Ammour et al.²⁹ used a sigmoid barrier instead of a ramp barrier. Through the sigmoid barrier, the width of the entire road gradually decreases by one lane when a leading vehicle is detected. As a result, unnecessary deceleration caused by the ramp barrier-based approach was improved. However, motion planners using the overtaking strategy require a space where overtaking motion is possible, and there is a limit that does not consider vehicle following.

To overcome the limitations of using a single strategy, motion planners using both vehicle following and overtaking strategies have been proposed. Karlsson et al.³⁰ applied two different MPCs to perform both vehicle following and overtaking. After that, for each trajectory, the cost is calculated for driving comfort, path tracking error, and avoiding frequent changes in driving strategy, and only the trajectory with lowest overall cost is executed. However, the proposed method has a disadvantage of increasing the computation load because two MPCs need to be operated every iteration. Liu et al.³¹ proposed a planner that can perform both single-lane change and vehicle tracking using one MPC framework. However, because of the vehicle model and collision-free constraints, the proposed planner is formulated as a non-convex optimization. According to the paper,³¹ it is possible to fail to find a feasible solution. Franco and Santos³² used a behavior planning flowchart to predetermine driving strategies. Subsequently, the constraints corresponding to the determined driving strategy were formulated. Through this process, the proposed planner allows both collision avoidance strategies while maintaining convex optimization. However, the proposed method does not consider acceleration and jerk, and it may create uncomfortable driving motions. Lattarulo and Pérez Rastelli³³ limited the range of longitudinal acceleration and jerk, and lateral acceleration to improve driving comfort. However, the proposed method only considers the minimization of path tracking error and speed error when planning motion. Thus, the proposed method plans the motion with given maximum acceleration and jerk to quickly reduce the two errors. Therefore, as the range of acceptable acceleration and jerk increases, it may cause motion with greater acceleration. However, acceleration and jerk affect driving comfort. Therefore, a safe and comfortable motion planning is required, considering not only collision avoidance and path tracking error reduction, but also acceleration and jerk reduction of the vehicle.

We propose a new motion planner that ensures safety and considers acceleration and jerk. The proposed motion planner integrates the target speed determination process and MPC. The target speed is updated in real time in consideration of the improvement of the path tracking performance in the curved section and whether overtaking is possible. This target speed determination allows the proposed motion planner to be formulated as a convex optimization problem. Once the target speed is determined, the proposed planner generates control commands to ensure safety and improve driving comfort in the motion optimization phase. A speed control command is generated to slowly converge to the target speed using cost terms that minimize longitudinal acceleration and jerk. Additionally, the steering control command is generated with consideration of minimizing path tracking error and the lateral acceleration and jerk. Here, in order to consider that the lateral acceleration and jerk increase as the speed increases during steering action, we used lateral motion cost weights that change according to the speed. In addition, we propose a cost function that creates a smooth steering control command in maneuver generation for obstacle overtaking.

Vehicle modeling

We aim to plan the collision-free motions that can actually be operated on vehicles. For this, it is necessary to consider the vehicle motion characteristics when planning the motions. To ensure the kinematic feasibility, we used a kinematics vehicle model in this work.

Kinematic vehicle model

To drive safely without colliding with road infrastructure in a structured environment, it is desirable to follow a given reference path (or global path) as possible. We model the vehicle movement along a given reference path. Considering that the tracking error consists of lateral error and orientation error, the lateral deviation, vehicle orientation, and path orientation are modeled as state variables to calculate these two tracking error components.

Moreover, to improve driving comfort, it is also necessary to consider minimizing acceleration and its derivative (jerk) when planning motion. The acceleration applied to non-slip vehicle can be expressed as the sum of the longitudinal acceleration and the centripetal acceleration, which is proportional to the curvature and velocity. Therefore, the curvature, speed, and longitudinal acceleration are also modeled as state variables. Referring to the paper,²⁵ the geometric relations between the vehicle and a given reference path can be expressed as shown in Figure 1, where $(x, y)$ is the rear axle center position of the vehicle, $d_{n}$ is the normal displacement from the reference path, $θ$ and $θ_{r}$ are the orientations of the vehicle and the reference path, $κ$ and $κ_{r}$ are the curvature of the vehicle and the reference path, and $υ$ and $a$ are the vehicle’s longitudinal speed and acceleration. Assuming that the reference path to follow is continuous and differentiable, the states for the vehicle and the reference path are modeled as a nonlinear system as in (1). Here, to plan the longitudinal motions as well, we extended the vehicle model of paper²⁵ to include longitudinal motion terms as (1f) and (1g).

{\overset{\cdot}{d}}_{n} = υ \sin (θ - θ_{r})

(1a)

{\overset{\cdot}{θ}}_{r} = \frac{\cos (θ - θ_{r})}{1 - d_{n} κ_{r}} κ_{r} υ

(1b)

{\overset{\cdot}{κ}}_{r} = z

(1c)

\overset{\cdot}{θ} = κ υ

(1d)

\overset{\cdot}{κ} = u_{1}

(1e)

\overset{\cdot}{υ} = a

(1f)

\overset{\cdot}{a} = u_{2}

(1g)

Figure 1.

Kinematic vehicle model with respect to the reference path.

where $u_{1}$ and $u_{2}$ are the vehicle control inputs, and $z$ is a given external signal representing the curvature change rate of the reference path.

Linearization

Due to the trigonometric terms and fractional form in (1), the vehicle model is nonlinear. This can increase the complexity of calculating the future vehicle states. To simplify the model, linearization is performed on the vehicle model around the operating point. In this work, the operating point is selected based on the situation wherein a vehicle with current speed $υ_{curr}$ follows the given reference path well. This situation can be expressed as in (2).

\begin{matrix} d_{n} & = 0 \\ θ & = θ_{r} \\ κ & = κ_{r} \\ z & = u_{1} \\ υ & = υ_{curr} \end{matrix}

(2)

If the vehicle drives near the operating point, the system can be linearized as in (3).

\overset{\cdot}{x} (t) = A_{C} x (t) + B_{C} u (t) + E_{C} z (t)

(3)

where $x = {[\begin{matrix} d_{n} & θ_{r} & κ_{r} & θ & κ & υ_{x} & a_{x} \end{matrix}]}^{T}$ and $u = {[\begin{matrix} u_{1} & u_{2} \end{matrix}]}^{T}$ are the state vector and the control input vector, respectively, and $A_{C}$ , $B_{C}$ , and $E_{C}$ are the system matrices that are linearized at the operating point. These linearized system matrices can be calculated as follows.

\begin{matrix} A_{C} & = [\begin{matrix} 0 & - υ_{curr} & 0 & υ_{curr} & 0 & 0 & 0 \\ κ_{r}^{2} υ_{curr} & 0 & υ_{curr} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & υ_{curr} & κ_{r} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{matrix}] \\ B_{C} & = {[\begin{matrix} 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}]}^{T} \\ E_{C} & = {[\begin{matrix} 0 & 0 & 1 & 0 & 0 & 0 & 0 \end{matrix}]}^{T} \end{matrix}

Discrete-time linearized vehicle model

Calculating future vehicle states over several seconds in the continuous-time domain can result in computational burden. To reduce the computational load, the continuous-time linear system is approximated by the discrete-time linear system. In this process, we assume that the system matrix $A_{C}$ , control input $u (t)$ , and the external signal $z (t)$ are constant during the discretization interval $T_{s}$ . Under these assumptions, the system state transition matrix $A$ , as well as other discretized matrices $B$ and $E$ can be calculated as in (4).

\begin{matrix} A & = e^{A_{C} T_{s}} \\ B & = \int_{0}^{T_{s}} e^{A_{C} τ} B_{C} d τ \\ E & = \int_{0}^{T_{s}} e^{A_{C} τ} E_{C} d τ \end{matrix}

(4)

Finally, the discrete-time linearized vehicle model can be described as in (5) for $k \leq N$ by using matrices in (4), where $N$ is the total prediction horizon.

x [k + 1] = Ax [k] + Bu [k] + E z [k]

(5)

Considering that the external signal $z (t)$ is the rate of curvature change as in (1c), we approximate the external signal of each time step $z [k]$ as follows.

z [k] = \frac{κ_{o} [k + 1] - κ_{o} [k]}{T_{s}}

System constraints

For safety, it is essential to drive without collisions with the surrounding obstacles. Depending on the presence of obstacles, it is sometimes necessary to plan motion such that the vehicle moves within a confined space. Furthermore, it is preferred to plan motion with consideration of the range in which the vehicle can actually operate. In this section, we formulate various constraints for each time step to avoid obstacles with feasible motions.

Representation of lateral displacement for collision avoidance

To define the constraints for collision avoidance, several lateral displacements are defined in this section. If unexpected obstacles exist near the global path, a maneuver to avoid collision with them is required for safety. Assuming multiple lanes exist, one way to create a collision avoidance maneuver in such situations is to induce the vehicle to move toward a collision-free lane by temporarily limiting lateral movement. To express this lateral range restriction, a displacement in the normal direction between the reference path and several points on the ego vehicle can be considered as outputs (see Figure 2). In this work, the deviations of the rear axle center, midpoint, and front axle center are formulated by (6).

\begin{matrix} d_{1} = d_{n} \\ d_{2} = d_{n} + \frac{l}{2} \sin (θ - θ_{r}) \\ d_{3} = d_{n} + l \sin (θ - θ_{r}) \end{matrix}

(6)

Figure 2.

Visualization of lateral displacements with respect to the reference path.

where $l$ is the wheelbase of the vehicle, and $(θ - θ_{r})$ is the orientation error. If the vehicle drives near the operating point (2), the lateral displacements can be linearized as in (7).

\begin{matrix} d_{1} = d_{n} \\ d_{2} = d_{n} + \frac{l}{2} (θ - θ_{r}) \\ d_{3} = d_{n} + l (θ - θ_{r}) \end{matrix}

(7)

Lateral deviation constraints

Due to static and dynamic obstacles in the driving environment, the lateral position of the vehicle is limited. This can be expressed as a range wherein the circles that approximate the ego vehicle body can move in the lateral direction without colliding with the road boundaries and obstacles, as in the paper.²⁵ Each radius of collision-free circle is user-defined value, and we set the radius to cover the ego vehicle body with a little margin in this work. Figure 3 illustrates the lateral deviation constraints, which depend on the surrounding obstacles. The lateral deviation constraints can be formulated by (8), where $d_{i, min} [k]$ and $d_{i, max} [k]$ are determined by the road boundaries and the predicted positions of the obstacle at time step $k$ .

d_{i, min} [k] \leq d_{i} [k] \leq d_{i, max} [k]

(8)

Figure 3.

Visualization of the lateral deviation constraints. The red and green arrows indicate the deviation limits that the circles approximating the ego vehicle can move to the left and right from the reference path, respectively.

The process of determining the upper and lower limits of lateral deviation for each time step is as follows. For simplicity, we assume that all detected traffic vehicles drive at their current speeds constantly, and maintain their current lanes for all prediction horizon $N$ . First, using the distances between the reference path and each road boundary, the range that can be moved left and right is determined. Second, by using the predicted positions of the ego vehicle and each obstacle at the $k$ th time step ( $1 \leq k \leq N$ ), a collision-free range at the $k$ th time step is calculated. Here, if a traffic vehicle exists only in the middle lane and the left/right lanes are empty, two collision-free ranges are possible. In this case, only one lateral deviation range is selected according to which side the ego vehicle is currently located from the reference path. If the ego vehicle is located on the left-side of the reference path, only the left-side space is treated as a feasible lateral deviation range. Finally, the final lateral deviation range is determined by comparing each upper and lower bounds limited by the road boundary and the obstacles. For example, if the upper bound by the road boundary is closer to the reference path than by the obstacle, the upper bound of the lateral deviation range is set by the road boundary. Through this process, lateral deviation constraints are generated for each time step.

In some cases, all lanes may be blocked by several leading vehicles. In this case, $d_{i, min} [k]$ and $d_{i, max} [k]$ are set to sufficiently small values (e.g. −0.1 and 0.1 m, respectively) because there is no space to move in any lateral direction. In addition, since there is a risk of collision in the longitudinal direction in a situation where all lanes are blocked, it is not enough to limit only the lateral deviation. This problem can be addressed through speed control and will be covered in Sections 5 and 6.

Motion limits

Most vehicles have a limited range of steering angle and actuator input power. Thus, the control inputs and the vehicle curvature are bounded for all time steps. In addition, the vehicle must not exceed the maximum speed if there is a speed limit on the road. These constraints can be formulated as in (9).

\begin{matrix} κ_{min} \leq & κ [k] \leq κ_{\max} \\ υ_{min} \leq & υ [k] \leq υ_{\max} \\ a_{min} \leq & a [k] \leq a_{\max} \\ u_{1, min} \leq & u_{1} [k] \leq u_{1, max} \\ u_{2, min} \leq & u_{2} [k] \leq u_{2, max} \end{matrix}

(9)

Cost function

Lane departure prevention is essential to ensure safety. Therefore, it is important to follow the given path as well as possible. Conversely, if the vehicle’s acceleration and jerk increase, the vehicle may not only lose stability, but also become far from comfortable driving. Thus, it is also necessary to consider motion smoothness. In this section, the cost function is designed to determine the optimal motion sequence considering both the path tracking performance and smoothness.

Cost function design

Our goal is to generate a vehicle control input sequence that follows the reference path but has a comfortable motion. First, to consider reducing the tracking error, we used the following three terms for cost calculation.

\begin{matrix} lateral error penalization : d_{n} [k]^{2} \\ orientation error penalization : (θ [k] - θ_{r} [k {])}^{2} \\ speed error penalization : (υ [k] - υ_{tar})^{2} \end{matrix}

where $(θ - θ_{r})$ is the orientation error within a range of $[- π / 2, π / 2]$ , and $υ_{tar}$ is the temporary target speed within a range of $[υ_{\min}, υ_{\max}]$ . Note that for the orientation error range, we excluded $[- π, - π / 2]$ and $[π / 2, π]$ , which correspond to wrong-way driving. In addition, the target speed $υ_{tar}$ calculation process will be discussed in Section 6.

On the other hand, a smooth and comfortable maneuver can be obtained by minimizing the force applied to the vehicle and its changes. This is equivalent to reducing the vehicle’s acceleration and jerk. The acceleration applied to the vehicle can be decomposed into longitudinal and centripetal accelerations if the vehicle does not slip. Considering that the centripetal acceleration is proportional to the curvature, a comfortable motion can be obtained by minimizing the curvature and longitudinal acceleration of the vehicle and their derivatives (i.e. control inputs). For this reason, these state values and control inputs are used in the cost calculation to consider driving comfort as follows.

\begin{matrix} curvature penalization : κ [k]^{2} \\ acceleration penalization : a_{x} [k]^{2} \\ control input penalization : u_{1} [k]^{2}, u_{2} [k]^{2} \end{matrix}

(10)

Intuitively, there is a trade-off between path tracking performance and comfortable driving. Based on this trade-off relationship, we designed the cost function in a quadratic form using all the cost terms mentioned above.

\begin{matrix} J [k] = w_{d} d_{n} [k]^{2} + w_{θ} (θ [k] - θ_{r} [k {])}^{2} + w_{υ} (υ_{x} [k] - υ_{tar})^{2} \\ + w_{κ} κ [k]^{2} + w_{a} a_{x} [k]^{2} + w_{u_{1}} u_{1} [k]^{2} + w_{u_{2}} u_{2} [k]^{2} \end{matrix}

(11)

where $w_{d}, w_{θ}, w_{v}, w_{κ}, w_{a}, w_{u_{1}}, w_{u_{2}} > 0$ are the cost weights. One thing to consider when assigning weights is that each cost term has a different range. If this range difference is not considered, each cost term is not considered equally, even if the same weights are assigned. For example, the lateral error $d_{n}$ can have a value greater than 3 m, but the curvature of the vehicle $κ$ is substantially less than 1. That is, the lateral error cost $d_{n} [k]^{2}$ is usually larger than the curvature penalization cost $κ [k]^{2}$ . If we simply assign the same weights to these two cost terms, the cost function may focus more on reducing the lateral error because the lateral error cost has a larger value. To address this issue, each weight is calculated as follows.

\begin{matrix} w_{d} & = \frac{1}{\max {d_{n}^{2}}} \times w_{TrkErr} \\ w_{θ} & = \frac{1}{\max {(θ - θ_{r})^{2}}} \times w_{TrkErr} \\ w_{υ} & = \frac{1}{\max {(υ - υ_{tar})^{2}}} \times w_{SpdErr} \\ w_{κ} & = \frac{1}{\max {κ^{2}}} \times w_{LatAct} \\ w_{a} & = \frac{1}{\max {a^{2}}} \times w_{LongAct} \\ w_{u_{1}} & = \frac{1}{\max {u_{1}^{2}}} \times w_{LatAct} \\ w_{u_{2}} & = \frac{1}{\max {u_{2}^{2}}} \times w_{LongAct} \end{matrix}

(12)

where $w_{TrkErr}, w_{SpdErr}, w_{LatAct}, w_{LongAct} > 0$ are the user-defined weights for tracking error, speed error, lateral action, and longitudinal action, respectively. The fraction terms in (12) make each cost to have the range [0, 1]. Since the minimum value in the quadratic value is 0, the fraction terms are designed using only the maximum value. Each weight on the right-hand side scales the normalized weight.

Variable weights for smooth lateral motion

The conventional method for setting the cost weights is to render all weights constant. If all weights are set to a constant, motions with the same driving style are planned in any driving situation. However, there is a limit to achieving both comfortable motion and path tracking performance improvement with a single driving style. In detail, it is always necessary to consider reducing the path tracking error such that lane departure does not occur for safety. Conversely, when the vehicle runs at a high speed, the importance of comfortable motion cannot be ignored because the driving comfort decreases as the centrifugal force increases. To address this problem, $w_{LatAct}$ is designed as a function of the current speed $υ_{curr}$ . The weight $w_{LatAct}$ becomes larger than other weights as the speed increases. Here, we used a linear function in this study because it is the simplest function that can increase the importance of comfortable motion in proportion to speed. Additionally, to prevent the cost weight from being negative, we set 0.01 as the lower limit for the weight $w_{LatAct}$ .

\begin{matrix} w_{LatAct} & = c_{LatAct} \times (υ_{curr} - υ_{thres}) + w_{TrkErr} \end{matrix}

(13)

where $c_{LatAct} > 1$ is a hyperparameter that determines the increment speed of the weight $w_{LatAct}$ , and $υ_{thres}$ is the speed threshold at which the cost weights $w_{TrkErr}$ and $w_{LatAct}$ become equal. If $υ_{curr} > υ_{thres}$ , since the cost weight $w_{LatAct}$ becomes larger than $w_{TrkErr}$ , the cost function is more focused on reducing curvature penalization cost and lateral control input cost. Conversely, if $υ_{curr} < υ_{thres}$ , by a similar logic, the cost function is more focused on reducing the path tracking error. Through this, it is expected to plan a comfortable motion at high speed by reducing the centripetal acceleration and its rate, and to plan a motion that improves the path tracking performance at low speed.

Lateral error cost weight for smooth overtaking

When overtaking a leading vehicle on the reference path, the ego vehicle needs to move temporarily toward the adjacent lane. For comfortable driving, it is desirable to plan a motion that gradually changes lanes. However, it is difficult to generate comfortable motion in the form of the cost function (11) because lateral error cost becomes dominant in the overtaking scenario. Figure 4 shows the cost calculation and the expected vehicle trajectory in an overtaking situation when using the cost function (11) as it is.

Figure 4.

Cost for each time step and expected ego vehicle trajectory in overtaking scenario. The ego vehicle and leading vehicle are indicated by blue box and gray box, respectively.

As shown in Figure 4, when creating an overtaking maneuver, the steering action is only needed while moving the vehicle into the next lane, so the curvature and lateral control input costs are only increased while changing the steering. On the other hand, since the lateral error persists after moving to the next lane, the lateral error cost is accumulated until the last time step. That is, most of the total cost is the lateral error cost. Thus, to reduce the overall cost, a motion sequence that minimizes the lateral error cost is planned. Such a motion sequence forms the ego vehicle’s trajectory wherein the vehicle makes a sudden lane change just before colliding with the leading vehicle. Clearly, a sudden steering action is far from comfortable motion. To address this problem, we reduce the cumulative effect of lateral error cost by dividing the lateral error cost weight $w_{d}$ by the total time horizon $N$ when overtaking obstacles on the reference path.

{\tilde{w}}_{d} = \frac{1}{N} \times w_{d}

(14)

Note that the reduced lateral error cost weight ${\tilde{w}}_{d}$ is only used while overtaking the leading vehicle on the reference path. Other than that, the lateral error cost weight $w_{d}$ is used to improve the path tracking performance.

Target speed calculation

As mentioned in Section 4.2, it is sometimes possible that all lanes are temporarily blocked by several leading vehicles. In this case, deceleration is more effective for avoiding collisions than trying to overtake. Also, to render the predicted movement of the kinematic vehicle model more accurate in a curved section, sufficient deceleration is required to reduce the dynamic effect, which can help to improve the path tracking performance in a high-curvature section. To achieve these two goals simultaneously, we calculate two target speed candidates for each purpose, and determine the final target speed by selecting one of them.

Non-side slip condition

The kinematic vehicle model describes its motion without considering the effect of the force. If the lateral motion resulting from the dynamic effect increases, the difference between the predicted motion calculated by the kinematic model and the actual driving maneuver might increase. Therefore, to reduce the difference between them, it is desirable to reduce the additional movement caused by side slip as much as possible. According to Park et al.,³⁴ the non-side slip condition is given by the relationship between the curvature and speed of the vehicle as in (15).

υ \leq \sqrt{\frac{g (i + f)}{κ}}

(15)

where $g$ denotes gravitational acceleration, $i$ is a super-elevation of the road, and $f$ is a side-friction factor. The first target speed candidate $υ_{tar, 1}$ is determined using the curvature of the reference path and (15). Figure 5 shows the overall scheme for calculating the target speed candidate $υ_{tar, 1}$ using the reference path.

Figure 5.

Calculation of target speed $υ_{tar, 1}$ based on the curvature of the reference path. The orange line represents the expected overall driving trajectory along the reference path.

As the first process of calculating $υ_{tar, 1}$ , a path segment is extracted from the reference path that the vehicle can travel at the current speed $υ_{curr}$ during the total prediction horizon $N$ . Next, the highest curvature $κ_{r, \max}$ is obtained from the extracted path. To improve the tracking performance over the entire extracted path including the highest curvature $κ_{r, \max}$ section, we set the target speed $υ_{tar, 1}$ using the nonslip condition described in (15).

υ_{tar, 1} = \sqrt{\frac{g (i + f)}{κ_{r, \max}}}

(16)

If the calculation result of (16) exceeds the road limit speed $υ_{\max}$ , then $υ_{tar, 1}$ is set to $υ_{\max}$ .

Vehicle following

If one or more obstacles are detected in each lane, including both forward and backward, driving at the speed of the leading vehicle until a free lane is created is more appropriate for safety, rather than trying to change the lane excessively. When performing vehicle following, it is necessary to drive at a sufficient distance to prevent collision, even if the speed of the leading vehicle changes. To accomplish this, we calculate the second target speed candidate by considering both the relative speed of the leading vehicle and the desired time gap.

First, the desired relative speed for vehicle following is 0, which is equivalent to setting the target speed of the ego vehicle as the leading vehicle speed. Through this, the speed error of the ego vehicle $υ_{err, 1}$ for vehicle following can be calculated as in (17).

υ_{err, 1} = υ_{lead} - υ_{ego}

(17)

where $υ_{ego}$ and $υ_{lead}$ are the current speed of the ego vehicle and the leading vehicle, respectively.

Next, the speed at which the ego vehicle travels to have a predefined time gap $t_{gap}$ at the current distance from the leading vehicle $l_{dist}$ can be calculated as follows.

υ_{gap} = \frac{l_{dist}}{t_{gap}}

Through this, the speed error of the ego vehicle $υ_{err, 2}$ to have the desired time gap can be calculated as in (18).

υ_{err, 2} = υ_{gap} - υ_{ego}

(18)

Finally, to reduce both speed errors, we set a second target speed candidate $υ_{tar, 2}$ by adding two weighted speed error terms to the current ego vehicle speed as in (19), where $w_{er r_{1}}, w_{er r_{2}} > 0$ are the predefined weights for each speed error that satisfy $w_{er r_{1}} + w_{er r_{2}} = 1$ .

υ_{tar, 2} = υ_{ego} + w_{er r_{1}} υ_{err, 1} + w_{er r_{2}} υ_{err, 2}

(19)

Meanwhile, it is not necessary to generate a vehicle following maneuver if the leading vehicles are too far (for instance, out of the sensor detection range), or if a free lane exists. In this case, $υ_{tar, 2}$ is set as the road limit speed $υ_{\max}$ .

Target speed determination

In this step, the target speed is finally determined such that both the non-side slip condition and vehicle following are satisfied. Each target speed candidate corresponds to the “maximum” speed that satisfies the given condition within the range of the road speed limit. In $υ_{tar, 1} < υ_{tar, 2}$ case, driving at a speed of $υ_{tar, 1}$ does not cause a collision with the leading vehicle. Conversely, in $υ_{tar, 1} > υ_{tar, 2}$ case, driving at a speed of $υ_{tar, 2}$ still satisfies the non-side slip condition. Therefore, if the final target speed $υ_{tar}$ is selected as the lowest value among the two candidates, both conditions can be satisfied.

υ_{tar} = min {υ_{tar, 1}, υ_{tar, 2}}

(20)

This target speed $υ_{tar}$ for the cost function is determined at runtime whenever an optimal control problem is formulated, and the speed of the ego vehicle is controlled by changing the target speed.

Experiments

This section presents the simulation results for the proposed method. The performance was evaluated in terms of path tracking performance, acceleration, jerk in various curvatures, and collision avoidance ability.

Simulation setup

Our proposed planner was implemented in MATLAB, and the Gurobi Optimizer was used to solve the quadratic programing. Additionally, we used IPG CarMaker³⁵ as a vehicle simulator to evaluate the performance of the proposed controller. Since CarMaker uses gas/brake pedal positions and steering wheel angle as vehicle control inputs, the control inputs generated by our proposed MPC need to be converted. For gas/brake pedal position command generation, we used CarMaker’s built-in logic that converts the desired acceleration to pedal positions. In the case of steering control, the input was converted through the following three steps. First, the desired curvature was calculated from the changing rate of the curvature of the vehicle using Euler’s integral method. Second, the desired steering angle was calculated using the relationship between the curvature and the steering angle based on the kinematic bicycle model. Finally, the desired steering wheel angle was calculated using the steering ratio. Through these processes, the ego vehicle was controlled with the proposed MPC in CarMaker. All simulations were conducted on a computer with Intel Core i9-10900K CPU and 64 GB RAM. In this setup, the proposed MPC took an average of 37.50 ms and a maximum of 72.24 ms per iteration. The parameter values used in the simulations are listed in Table 1.

Table 1.

Parameter values for simulation.

Parameters	Notation	Value
Time step size	$T_{s}$	0.2 [s]
Prediction horizon	$N$	20
Maximum steering angle	$κ_{max}$	0.18 [1/m]
Minimum steering angle	$κ_{min}$	–0.18 [1/m]
Maximum vehicle speed	$v_{\max}$	60 [km/h]
Minimum vehicle speed	$v_{\min}$	0 [km/h]
Longitudinal acceleration limit	$a_{max}$	3.0 [m/s²]
Longitudinal deceleration limit	$a_{min}$	–3.0 [m/s²]
Maximum steering input	$u_{1, max}$	0.5 [1/m · s]
Minimum steering input	$u_{1, min}$	–0.5 [1/m · s]
Maximum longitudinal jerk	$u_{2, max}$	4.0 [m/s³]
Minimum longitudinal jerk	$u_{2, min}$	–4.0 [m/s³]
Side friction factor	$f$	0.1
Gravitational acceleration	$g$	9.81

Also, there are several weight parameters in the cost function and target speed calculation process. The weights used in this simulation are listed in Table 2. For $υ_{thres}$ , it was set based on our experience of driving at a speed of about 30 km/h when making a sharp turns at intersections or roundabouts. All other weights were experimentally tuned. With the weight parameter settings in Table 2, the lateral action cost weight $w_{LatAct}$ becomes greater than 5 when the vehicle is faster than 30 km/h. The weight $w_{LatAct}$ used in this simulation is visualized in Figure 6. In addition, the speed error cost weight $w_{SpdErr}$ was set higher than the longitudinal action cost weight $w_{LongAct}$ to reach the target speed $υ_{tar}$ quickly.

Table 2.

Weight values used for simulation.

Parameters	$w_{TrkErr}$	$w_{SpdErr}$	$w_{LongAct}$	$c_{LatAct}$
Values	5	7.5	2.5	1.2
Parameters	$υ_{thres}$	$w_{er r_{1}}$	$w_{er r_{2}}$
Values	30/3.6	0.7	0.3

Figure 6.

Visualization of lateral action cost weight.

Figure 7 shows the overall shape of the test track used in the simulations. The maximum speed of the test track is limited to 60 km/h, and the super-elevation is 0 throughout the entire section. Also, it consists of a total of three lanes, and the width of each lane is 3.5 m. To verify the effect of the proposed speed control method according to the curvature and considering acceleration and jerk, the results are compared with the MPC-based planner to which they are not applied.

Figure 7.

A closed-loop test track. The black line indicates the reference path. The magenta and the blue lines represent left and right boundary, respectively. (a) A closed-loop test track. (b) Simulation snapshot. (c) Curvature of the road according to the distance.

Path tracking performance

To check the speed control effect of the proposed method, the path tracking performance was compared with other MPC-based planner that does not plan the speed. In this test, we used a planner proposed by Gutjahr et al.²⁵ as a representative planner for comparison because it also uses a linearized kinematic vehicle model. Since the MPC²⁵ uses a predefined speed profile, we used the speed profile generated by CarMaker IPG Driver model as shown in Figure 8 for it. On the other hand, our proposed planner does not require such speed profiles, we set the maximum speed $υ_{\max}$ for the entire track as default. All weights for the cost function in the MPC²⁵ were set to the same value.

Figure 8.

Speed profile generated by IPG Driver along the reference path.

Figure 9 shows the overall lateral error when driving the entire test track for each MPC-based planner. As shown in the figure, the proposed method has a smaller lateral error in most sections. This means that the proposed method has better path tracking performance than the case of using MPC.²⁵ The statistics on the lateral error measured while driving the entire track are shown in Table 3. As can be seen in the table, the lateral error of the proposed method was reduced by 46.4% on average compared with the MPC.²⁵

Figure 9.

Comparison of path tracking performance for each MPC-based planner.

Table 3.

Path tracking error comparison.

Lateral error statistics [m]	MPC²⁵	Proposed
Mean	0.084	0.045
Standard deviation	0.10	0.057
Maximum value	0.69	0.44

The path tracking performance of the MPC²⁵ was highly dependent on a given speed profile. First of all, when the MPC²⁵ used the constant speed profile at 60 km/h in all sections, speed control was not performed even in the high-curvature track segment. As a result, side slip occurred significantly in the high-curvature track segment, and it failed to track the path. Therefore, the path tracking performance could not be compared in this case. Next, in the case of using the MPC²⁵ with the speed profile generated by IPG Driver, path tracking was performed while adjusting the speed. However, since the IPG Driver generated the speed profile without considering the non-slip condition, the vehicle did not satisfy the non-slip condition in most track segments in this case as shown in Figure 10. Thus, the difference between the kinematic predicted motion and the actual vehicle movement became large. On the other hand, since the proposed method uses a target speed that satisfies the non-slip condition according to the reference path curvature, the ego vehicle was sufficiently slowed down in the corner section. Therefore, the motion prediction error between the kinematic model and the actual vehicle model was reduced, even in the high-curvature path, and as a result, the average lateral error became smaller than that of the MPC.²⁵ Figure 11 shows the tire side slip angle while the vehicle traveled using each MPC. As shown in the figure, the proposed MPC has a smaller overall side slip angle. This implies that the difference between the movement predicted by the kinematic model and the actual motion is smaller.

Figure 10.

Comparison of speed profiles.

Figure 11.

Comparison of sideslip angle.

Note that the proposed MPC does not have a smaller lateral error in the entire track. In particular, in the track segment of [250 m, 335 m] (corresponding to region 1 in Figure 7(a)), the proposed MPC temporarily had a larger lateral error in the negative direction, even though the side slip angle was smaller than the case of using the MPC.²⁵ This is because of the effect of the weights used in this experiment. Referring to Figure 10, the driving speed in this region was over 30 km/h. In this case, since the weights $w_{κ}$ and $w_{u_{1}}$ have greater values than both $w_{d}$ and $w_{θ}$ , the cost function was designed to penalize the curvature change more than the path tracking error. Therefore, in the [250 m, 335 m] section, the lateral error temporarily increased as a result of planning the motion with an “out-in-out” trajectory in the sharp curve section to minimize the curvature change along the curved track segment. Most of the remaining curved track segments had smaller lateral errors compared to MPC.²⁵ This is because the proposed planner decelerated sufficiently down to 30 km/h before entering the curved track segment. As the vehicle travels at less than 30 km/h, the path tracking error cost weights become relatively high. Thus, a trajectory that is more focused on reducing the path tracking error was generated, instead of the out-in-out trajectory.

Driving comfort quality

We compared the driving comfort quality between MPC²⁵ and the proposed planner by comparing the acceleration and jerk. Additionally, we analyzed the effect of acceleration and jerk penalizing cost term (10) on driving comfort. Figures 12 and 13 show the acceleration and jerk applied to the vehicle while it follows the reference path using each algorithm, and Table 4 shows the statistics of these acceleration and jerk values. Note that the acceleration and jerk in Table 4 represent the magnitude of the vector where the longitudinal and lateral components are summed. As a result, acceleration was reduced by 30.2% compared to MPC²⁵when cost term (10) was not used, and was reduced by 41.5% when (10) was used. Additionally, the jerk was reduced by 18.7% compared to MPC²⁵ before using the cost term (10), whereas it was reduced by 71.0% after using the cost term (10). The experimental results imply that the proposed planner had better driving comfort.

Figure 12.

Acceleration profile.

Figure 13.

Jerk profile.

Table 4.

Comparison of acceleration and jerk.

Acceleration statistics, m/s²	MPC²⁵	Proposed without (10)	Proposed	Jerk statistics, m/s³	MPC²⁵	Proposed without (10)	Proposed
Mean	1.06	0.74	0.62	Mean	1.07	0.87	0.31
Standard deviation	1.23	0.51	0.40	Standard deviation	1.76	0.92	0.33
Max	7.33	2.71	2.95	Max	16.2	5.92	2.08

Since the MPC²⁵ does not use longitudinal acceleration and jerk constraints, it is possible to create a motion that changes the speed as quickly as possible. On the other hand, the proposed planner without (10) prevents unlimited longitudinal acceleration and jerk increase through its range constraints. Therefore, the average acceleration and jerk were reduced. However, proposed planner without (10) calculates the cost using only the path tracking error and speed error. As a result, the longitudinal acceleration and jerk sometimes reached the limit value to quickly reduce the speed error. Also, it generated a steering input that corrected immediately even if a slight path tracking error occurred, which led to an increase in the lateral jerk. On the other hand, the proposed planner additionally uses terms, including the curvature of the vehicle trajectory, longitudinal acceleration, and control inputs, that affect the overall acceleration and jerk increase for cost calculation. This led to a motion plan that also considers reducing the acceleration and jerk to minimize the overall cost. Thus, the proposed planner showed a smaller acceleration and jerk than the planner without (10) overall as shown in Figures 12 and 13. Through these two effects, the proposed planner increased the driving comfort quality.

Collision avoidance

Next, the collision avoidance ability of the proposed algorithm was verified. To check whether the proposed planner is able to plan a collision-free motion through vehicle following and overtaking, we prepared a scenario with the following four phases as shown in Figure 14. The prepared scenario starts with a situation in which a reference path is created along the center lane, only the center lane is empty, and the left and right lanes are blocked by preceding vehicles. First, a vehicle in the front-left performs a lane change to the center lane. Second, all lanes remain blocked by preceding vehicles. Third, the right-front vehicle moves away from the ego vehicle, but another vehicle approaching at high speed is detected from the rear-right. Finally, the approached vehicle moves away from the ego vehicle and the right lane becomes free. The default driving speed of ego vehicle is set as $υ_{\max}$ , and all traffic vehicles in the left and center lanes were driven at a speed lower than $υ_{\max}$ constantly (35 km/h for the left vehicles and 40 km/h for the center vehicles). The vehicles on the right drove at 40 km/h in the phases 1 and 2 and more than $υ_{\max}$ in the phases 3 and 4. We assumed that all leading vehicles were accurately detected, tracked, and maintained their own lanes. Note that this test was performed on a road with zero curvature. Since the proposed algorithm has a logic to change the target speed according to the curvature, it is difficult to check the validity of the target speed setting logic for vehicle following when there is a high-curvature section. Therefore, in order to exclude the effect of speed control due to curvature, we used a the straight road in this test. The straight road used in this test has a total length of 3 km, and consists of three lanes with a width of 3.5 m for each lane.

Figure 14.

Overall scenario for collision avoidance ability test.

Figure 15 shows the planned trajectories that correspond to the obtained motion sequences and the vehicle movements in bird’s eye view, and Figure 16 is the overall time gap and relative speed between the ego vehicle and the leading vehicle in the same scenario. In this figure, the section with “current time gap << 0” indicates that target speed candidate $υ_{tar, 2}$ for vehicle following was not calculated because leading vehicles had not yet been detected by the sensor or a free lane exists. In addition, in the relative speed profile graph, the black line section with value of 0 indicates an area where there is no need to calculate the relative speed with the leading vehicle, which is the same as the condition of “current time gap << 0.” In the initial part of the simulation ( $t < 18.2$ ), since the leading vehicles were outside the detection range of the sensor on the ego vehicle, the target speed candidate $υ_{tar, 2}$ for vehicle following was not yet calculated. After $t \geq 18.2$ , the front vehicles began to be detected. However, since the center lane was not blocked, the ego vehicle maintained the default driving speed until $t = 21.0$ , even if traffic vehicles were detected. Once the left-front vehicle completely changed to the center lane at the time $t = 21.0$ , all lanes were blocked by the preceding vehicles. Since there was no free lane at this time, a decelerating motion was employed to perform vehicle following. After that, the speed was adjusted in consideration of the time gap and the relative speed, and finally, the vehicle was driven at almost the same speed as the leading vehicle in the distance with the desired time gap (see $t = 100$ ). This shows that the target speed calculation method proposed in Section 6.2 for following the leading vehicle is valid. In the middle of the entire simulation, the right-front vehicle accelerated, and the right lane became free at $t = 118.8$ . Since there is an area to overtake the preceding vehicle in this step, the ego vehicle started to accelerate to the maximum speed and planned a maneuver that attempts to avoid obstacles by overtaking. Note that when generating an entire trajectory for the ego vehicle, the predicted position of the leading vehicles for each time step was also considered. Thus, as shown at $t = 127.6$ in Figure 15(b), the point where the ego vehicle starts to move to the next lane is behind the current leading vehicle position. However, while the ego vehicle was accelerating, another vehicle rapidly approaching from the rear-right was detected, so the right lane was again occupied by a traffic vehicle. Thus, at the time $t = 127.8$ , the ego vehicle was decelerated again to follow the leading vehicle. This is the reason that the time gap is not 0 in section $127.8 < t < 142.4$ in Figure 16. Once the rear-right vehicle was far enough away from the ego vehicle at $t > 142.4$ , the right lane became free again. Accordingly, the ego vehicle attempted to overtake the leading vehicle once again. In this time, since there is no vehicle in the right lane, the ego vehicle actually took an overtaking motion. Once the ego vehicle moved to the right of the leading vehicle ( $t = 156.2$ ), a trajectory was generated that returns to the reference path with sufficient distance from the current leading vehicle position. Finally, after the ego vehicle passed the leading vehicle, overtaking was successfully performed by returning to the reference path as shown in the last snapshot image in Figure 15. Through this simulation result, we can see that the proposed method can plan the motions for vehicle following and obstacle overtaking.

Figure 15.

Simulation snapshots of vehicle following and overtaking motions. The red and black boxes represent the ego vehicle and traffic vehicles detected within the sensing range, respectively. The predicted trajectories of the traffic vehicles are plotted with cyan lines, and the trajectory corresponding to the planned optimal motion sequence is indicated by the red lines. (a) Vehicle following motions in phases 1 and 2. (b) Vehicle following motions after canceling overtaking in phase 3. (c) Overtaking motions in phase 4.

Figure 16.

Time gap and relative speed in simulation shown in Figure 15.

In the overtaking scenario, to check whether the reduced lateral error cost weight ${\tilde{w}}_{d}$ as in (14) improves driving smoothness, we compared the acceleration and jerk when using $w_{d}$ in (12) and ${\tilde{w}}_{d}$ in (14) as lateral error cost weights, respectively. In both cases, the aforementioned scenario for collision avoidance was used again, and the same weights were used.

Figure 17 shows the acceleration and jerk profile when using $w_{d}$ and ${\tilde{w}}_{d}$ , respectively. In the case where $w_{d}$ was used, the lateral acceleration and jerk data does not exist after $t = 158.2$ . This is because overtaking failed due to the extreme steering changes. As shown in Figure 18, rapid steering changes increase the side slip, which significantly increases the difference between the predicted and actual movements. The motion with increased dynamic effect caused the ego vehicle to enter the bounding box of the adjacent vehicle before the next control input was generated, which violates the lateral deviation constraints. Therefore, data collection was stopped. When using $w_{d}$ , since the lateral error cost increased in all areas where the drivable space does not include the reference path, the overall lateral error cost has a relatively large value compared with other costs. To minimize the total lateral error cost, the ego vehicle showed a tendency to maintain the current lane as much as possible until just before colliding with the leading vehicle. When lane keeping was no longer possible because of the lateral deviation constraints, the ego vehicle moved rapidly toward the free lane, and thus the lateral acceleration and jerk were increased significantly. Conversely, when using ${\tilde{w}}_{d}$ , since the lateral error cost for each time step was reduced to 1/N, the importance of reducing the costs for curvature of the vehicle trajectory and lateral control input penalization were increased. The motion was planned to sufficiently consider reducing the steering action. Therefore, the lateral acceleration and jerk were reduced by more than half compared with the case before reducing the lateral error cost.

Figure 17.

Comparison of driving comfort quality during high-speed overtaking for each lateral error cost weight. (a) Lateral acceleration. (b) Lateral jerk.

Figure 18.

Comparison of side slip angle during high-speed overtaking. (a) Front tire side slip angle. (b) Rear tire side slip angle.

Conclusions

We propose an MPC-based motion planner that can improve path tracking performance and perform collision avoidance while considering acceleration and jerk minimization. The proposed MPC is capable of avoiding collisions with obstacles through vehicle following and overtaking. For comfortable motion generation, the cost function is designed including longitudinal acceleration and jerk penalization. In addition, cost weights for vehicle curvature and curvature change were made as a function of speed, such that the importance of minimizing lateral acceleration and jerk increases as the speed increases. In a situation where overtaking is required, the lateral error cost weight for each time step is divided by the prediction horizon length. This process prevents the lateral error cost from significantly increasing as a result of the continued lateral deviation, and enables smooth overtaking. We also determine the target speed by comprehensively considering path tracking on curved roads and whether it is possible to overtake obstacles to ensure vehicle safety. For this, we calculate two target speed candidates. The first candidate is determined using the road curvature and non-slip condition. This candidate is to prevent lane departure due to unexpected motion by reducing the difference between the kinematic vehicle model and actual movement. The second candidate is determined by using the time gap and relative speed between the ego vehicle and the leading vehicle, such that the ego vehicle travels at the same speed as the leading vehicle with a sufficient distance when overtaking is impossible. The effectiveness of the proposed planner was evaluated through road driving scenarios with various curvatures and collision avoidance scenarios in simulations. In the track driving scenario, including various curved roads, not only the path tracking performance but also the driving comfort were improved. Compared with the case where all cost weights were set to the same constant and road curvature was not considered, the average path tracking error was reduced by 46.4%, and the average acceleration and jerk were decreased by 41.5% and 71.0%, respectively. Furthermore, in the collision avoidance scenario, the ego vehicle succeeded in driving without collision through vehicle following and overtaking. In particular, in the overtaking scenario, when the reduced lateral error cost weight was used, both lateral acceleration and jerk were reduced by more than 50% compared with the case where the lateral error cost weight was not reduced, and driving comfort was greatly improved. The simulation tests confirmed that the proposed planner can generate motions that ensure vehicle safety and improve driving comfort. Future work will be devoted to evaluating the driving comfort, path tracking, and collision avoidance ability of the proposed planner in various scenarios.

Footnotes

Handling Editor: Chenhui Liang

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the National Research Foundation of Korea (NRF) grant funded by the Korea government (MSIT) (No. 2019R1F1A1059496) and the DGIST R&D Program of the Ministry of Science and ICT (22-CoE-IT-01).

ORCID iD

Ji Hwan Seo

References

Singh

Critical reasons for crashes investigated in the national motor vehicle crash causation survey. Technical Report, 2015.

Jeong

Evaluating the effectiveness of active vehicle safety systems. Accid Anal Prev 2017; 100: 85–96.

Penmetsa

Hudnall

Nambisan

Potential safety benefits of lane departure prevention technology. IATSS Res 2019; 43: 21–26.

Dikmen

Burns

CM.

Autonomous driving in the real world: Experiences with tesla autopilot and summon. In: International conference on automotive user interfaces and interactive vehicular applications (AutomotiveUI’16), Ann Arbor, MI, USA, 24–26 October 2016, pp.225–228. New York, NY: ACM.

Oliveira

Proctor

Burns

, et al. Driving style: How should an automated vehicle behave? Information 2019; 10: 219.

Dillen

Ilievski

Law

, et al. Keep calm and ride along: Passenger comfort and anxiety as physiological responses to autonomous driving styles. In: Proceedings of the 2020 CHI conference on human factors in computing systems (CHI’20), Honolulu, HI, USA, 25–30 April 2020, pp.1–13. New York, NY: ACM.

Mousavi

Osman

Lord

, et al. Investigating the safety and operational benefits of mixed traffic environments with different automated vehicle market penetration rates in the proximity of a driveway on an urban arterial. Accid Anal Prev 2021; 152: 105982.

Zhai

A new car-following model considering driver’s characteristics and Traffic Jerk. Nonlinear Dyn 2018; 93: 2185–2199.

Bemporad

Morari

Robust model predictive control: A survey. In: Garulli

Tesi

(eds) Robustness in identification and control. London: Springer, 1999, pp.207–226.

10.

Wang

Liu

Path planning and path tracking for collision avoidance of autonomous ground vehicles. IEEE Syst J 2021; 1–10.

11.

Liniger

Domahidi

Morari

Optimization-based autonomous racing of 1:43 scale RC cars. Optim Control Appl Methods 2015; 36: 628–647.

12.

Qian

Navarro

de La Fortelle

, et al. Motion planning for urban autonomous driving using bézier curves and MPC. In: Proceedings of the IEEE international conference on intelligent transportation systems (ITSC), Rio de Janeiro, Brazil, 1–4 November 2016, pp. 826–833. New York, NY: IEEE.

13.

Cao

Zhao

Song

, et al. An optimal hierarchical framework of the trajectory following by convex optimisation for highly automated driving vehicles. Veh Syst Dyn 2019; 57: 1287–1317.

14.

Chen

Pei

Okuda

, et al. A hierarchical hybrid system of integrated longitudinal and lateral control for intelligent vehicles. ISA Trans 2020; 106: 200–212.

15.

Khajepour

Melek

, et al. Path planning and tracking for vehicle collision avoidance based on model predictive control with multiconstraints. IEEE Trans Vehicular Technol 2017; 66: 952–964.

16.

Cao

Wang

, et al. An integrated mpc approach for fwia autonomous ground vehicles with emergency collision avoidance. In: International conference on intelligent transportation systems (ITSC), Maui, HI, USA, 4–7 November 2018, pp.2432–2437. New York, NY: IEEE.

17.

Al-Gabalawy

Hosny

Aborisha

AHS

. Model predictive control for a basic adaptive cruise control. Int J Dyn Control 2021; 9: 1132–1143.

18.

Dixit

Montanaro

Fallah

, et al. Trajectory planning for autonomous high-speed overtaking using mpc with terminal set constraints. In: Proceedings of the IEEE international conference on intelligent transportation systems (ITSC), Maui, HI, USA, 4–7 November 2018, pp.1061–1068. New York, NY: IEEE.

19.

Cheng

Guo

, et al. Longitudinal collision avoidance and lateral stability adaptive control system based on mpc of autonomous vehicles. IEEE Trans Intell Transp Syst 2020; 21: 2376–2385.

20.

Cui

Ding

, et al. A new strategy for rear-end collision avoidance via autonomous steering and differential braking in highway driving. Veh Syst Dyn 2020; 58: 955–986.

21.

Kopetz

Poledna

Autonomous emergency braking: A system-of-systems perspective. In: Proceedings of the 2013 43rd annual IEEE/IFIP conference on dependable systems and networks workshop (DSN-W), Budapest, Hungary, 24–27 June 2013, pp.1–7. New York, NY: IEEE.

22.

Zhao

Wong

Xie

, et al. Real-time weighted multi-objective model predictive controller for adaptive cruise control systems. Int J Autom Technol 2017; 18: 279–292.

23.

Takahama

Akasaka

Model predictive control approach to design practical adaptive cruise control for traffic jam. Int J Automot Eng 2018; 9: 99–104.

24.

Wang

Jiang

, et al. Collision-free navigation of autonomous vehicles using convex quadratic programming-based model predictive control. IEEE/ASME Trans Mechatron 2018; 23: 1103–1113.

25.

Gutjahr

Gröll

Werling

Lateral vehicle trajectory optimization using constrained linear time-varying MPC. IEEE Trans Intell Transp Syst 2016; 18: 1–10.

26.

Werling

Liccardo

Automatic collision avoidance using model-predictive online optimization. In: Proceedings of the IEEE conference on decision and control (CDC), Maui, HI, USA, 10–13 December 2012, pp.6309–6314. New York, NY: IEEE.

27.

Mao

Dueri

Szmuk

, et al. Convexification and real-time optimization for MPC with aerospace applications. In: Raković

Levine

(eds) Handbook of model predictive control. Cham: Springer, 2019, pp.335–358.

28.

Taherian

Halder

Dixit

, et al. Autonomous collision avoidance using MPC with LQR-based weight transformation. Sensors 2021; 21: 4296.

29.

Ammour

Orjuela

Basset

Collision avoidance for autonomous vehicle using MPC and time varying sigmoid safety constraints. In: IFAC conference on engine powertrain control, simulation and modeling (E-COSM), Tokyo, Japan, 23–25 August 2021, pp.39–44. Amsterdam: Elsevier.

30.

Karlsson

Murgovski

Sjoberg

Optimal trajectory planning and decision making in lane change maneuvers near a highway exit. In: European control conference (ECC), Naples, Italy, 25–28 June 2019, pp.3254–3260. New York, NY: IEEE.

31.

Liu

Lee

Varnhagen

, et al. Path planning for autonomous vehicles using model predictive control. In: IEEE intelligent vehicles symposium (IV), Los Angeles, CA, USA, 11–14 June 2017, pp.174–179. New York, NY: IEEE.

32.

Franco

Santos

Short-term path planning with multiple moving obstacle avoidance based on adaptive MPC. In: Proceedings of the 2019 IEEE international conference on autonomous robot systems and competitions (ICARSC), Porto, Portugal, 24–26 April 2019, pp.1–7. New York, NY: IEEE.

33.

Lattarulo

Pérez Rastelli

A hybrid planning approach based on MPC and parametric curves for overtaking maneuvers. Sensors 2021; 21: 595.

34.

Park

Lee

Han

. Development of lateral control system for autonomous vehicle based on adaptive pure pursuit algorithm. In: International Conference on Control, Automation and Systems (ICCAS), Gyeonggi-do, Korea, 2225 October 2014, pp. 1443–1447. New York, NY: IEEE.

35.

IPG Automotive GmbH. CarMaker, https://ipg-automotive.com/en/products-solutions/software/carmaker/ (2022, accessed 9 January).

A discrete-time linear model predictive control for motion planning of an autonomous vehicle with adaptive cruise control and obstacle overtaking

Abstract

Keywords

Introduction

Related works

Vehicle modeling

Kinematic vehicle model

Linearization

Discrete-time linearized vehicle model

System constraints

Representation of lateral displacement for collision avoidance

Lateral deviation constraints

Motion limits

Cost function

Cost function design

Variable weights for smooth lateral motion

Lateral error cost weight for smooth overtaking

Target speed calculation

Non-side slip condition

Vehicle following

Target speed determination

Experiments

Simulation setup

Path tracking performance

Driving comfort quality

Collision avoidance

Conclusions

Footnotes

Declaration of conflicting interests

Funding

ORCID iD

References