Keywords

1 Introduction

Motion planning algorithms ensure the safety of Autonomous Vehicles (AVs) in the presence of Obstacle Vehicles (OVs) and also ensure that AVs reach their target destinations. Obstacle avoidance is an important aspect of motion planning. One of the earliest implementation was based on Artificial Potential Fields (APF), where repulsive forces are used to avoid obstacles [1]. However, a limitation of APF-based algorithms is their poor performance in the presence of highly nonlinear systems and the oscillations that occur when operating at low frequencies. To overcome this issue, a model-dependent Control Barrier Function (CBF) [2] was used to handle obstacles in more complex non-linear optimization algorithms.

To fulfill the second fundamental goal of a motion planning algorithm, which is goal reaching, approaches based on way-point tracking or Control Lyapunov Functions (CLFs) [3] have been widely adopted in literature. Research work has shown that it is possible to combine CBFs and CLFs and other essential constraints through a Quadratic Programming (QP) to form an OCP [4]. The OCP can then be propagated along a predefined receding time horizon resulting in an MPC framework and is one of the current state-of-the-art techniques [5, 6] for AV motion planning in addition to data-driven techniques. Data-driven approaches are becoming increasingly popular due to their ability to efficiently optimize in diverse environments [7]. However, these approaches have the disadvantage of not providing security guarantees defined through a mathematical formulation of constraints. For this reason, in safety-critical tasks and when proper modeling of the physical problem is available, model-based approaches are still a viable solution for motion planning. This is confirmed by the extensive literature available on automated control of vehicles, such as in [8,9,10] where MPC is used for obstacle avoidance control, automatic highway overtaking and urban low speed autonomous driving respectively.

The most common solution to handle multiple driving scenarios in a model-based motion planner is a hierarchical approach. Each scenario is characterized by a specific problem formulation, and a high-level algorithm identifies the situation and selects the appropriate algorithm. To develop a single algorithm capable of handling multiple scenarios, this paper proposes a rule-based motion planner for an AV using a waypoint-assisted MPC formulation subjected to time-varying CBF constraints. The formulated motion planner can overtake, follow, and fully stop behind OV depending on the autonomous driving scenario sensed by the AV. This adaptability eliminates the need for transition planners for each scenario, making the algorithm versatile. This is achieved by making the MPC adapt the weights based on predefined driving rules corresponding to the sensed scenario. The first section of the paper focuses on the mathematical modeling of the vehicle and the formulation of the motion planner. The second section deals with simulation methodology while the third section presents the simulation results.

2 Mathematical Modelling

2.1 Vehicle Model

The AV to be controlled has been mathematically approximated by using a Single Track Model (STM) [11] and the kinematic expressions that describe the motion of STM along the constrained XY space are expressed in (1)

$$\begin{aligned} \left\{ \begin{aligned} \dot{x} &= v \sin (\theta + \beta ) \\ \dot{y} &= v \cos (\theta + \beta ) \\ \dot{\theta } &= \frac{v \sin (\beta )}{\text {L}_\text {r}} \\ \end{aligned} \right. \end{aligned}$$
(1)

where

$$\begin{aligned} \beta = \text {atan} \left( \frac{\text {L}_\text {r} \tan (\delta )}{\text {L}}\right) \end{aligned}$$

The parameters \(\text {L}\) and \(\text {L}_\text {r}\) denote the total length of the vehicle and length of the vehicle from rear axle to the vehicle’s Center of Gravity (CoG) point respectively. The variables v, \(\delta \) and \(\theta \) represent velocity of the vehicle, steering angle imposed at front wheel and orientation of the vehicle with respect to the global XY plane respectively. Integrating (1) yields x, y and \(\theta \) where the quantities x and y denote the position of the vehicle along global XY frame.

The variables v and \(\delta \) are the control variables to vehicle. To have a smoother control, the control actions are placed on acceleration \(u_\text {acc}\) and steering rate \(\psi \) and are defined in (2).

$$\begin{aligned} \left\{ \begin{aligned} \dot{v} &=u_\text {acc}\\ \dot{\delta } &=\psi \\ \end{aligned} \right. \end{aligned}$$
(2)

The expressions (1) and (2) are collectively taken as \(\underline{X}\) and \(\underline{U}\) respectively and are combined to define the total non-linear dynamic Eq. (3) of STM and is fed as a dynamic equality constraint to the optimization algorithm.

$$\begin{aligned} \dot{\underline{X}}=f(\underline{X},\underline{U}) \end{aligned}$$
(3)

2.2 Development of the Motion Planning Algorithm

The specific problem formulation developed is analyzed as follows. In particular, the CBF formulation is described in detail and then the overall problem formulation is reported.

2.2.1 CBF Constraint Formulation

The OV in this work is formulated as a circular entity centered at its CoG point and this circular entity is described mathematically in (4)

$$\begin{aligned} B(\underline{X}_{\text {AV}}, \underline{X}_{\text {OV}}) = (\underline{X}_{\text {AV}} - \underline{X}_{\text {OV}})^\top (\underline{X}_{\text {AV}} - \underline{X}_{\text {OV}}) - \text {D}_\text {OV}^2 \end{aligned}$$
(4)

where \(\underline{X}_{\text {AV}}\) is a vector containing position states of the AV (x and y), while \(\underline{X}_{\text {OV}}\) corresponds to the OV’s positional state vector (\(x_\text {OV}\) and \(y_\text {OV}\)) and finally, \(\text {D}_{\text {OV}}\) defines the region of influence around the OV. The first-order time derivative of (4) is taken and is described in (5)

$$\begin{aligned} \dot{B}(\underline{X}_{\text {AV}}, \underline{X}_{\text {OV}}, \dot{\underline{X}}_{\text {AV}}, \dot{\underline{X}}_{\text {OV}}) = 2 (\underline{X}_{\text {AV}} - \underline{X}_{\text {OV}})^\top (\dot{\underline{X}}_{\text {AV}} - \dot{\underline{X}}_{\text {OV}}) \end{aligned}$$
(5)

where \(\dot{\underline{X}}_{\text {AV}}\) points to the AV’s velocity vector (\(\dot{x}\) and \(\dot{y}\)), while \(\dot{\underline{X}}_{\text {OV}}\) corresponds to the OV’s velocity vector (\(\dot{x}_\text {OV}\) and \(\dot{y}_\text {OV}\)) with \(\dot{x}_\text {OV}\) = 0. The generalized expression for CBF constraint in continuous form [12] after substituting the B and its derivative \(\dot{B}\) is defined in (6)

$$\begin{aligned} \dot{B}(\underline{X}_{\text {AV}},\underline{X}_{\text {OV}},\dot{\underline{X}}_{\text {AV}},\dot{\underline{X}}_{\text {OV}})+ \gamma B({\underline{X}_{\text {AV}}},\underline{X}_{\text {OV}}) \ge 0 \end{aligned}$$
(6)

where \(\gamma \) defines the rate at which the AV is guided away from the region confined to the OV. A high value of gamma makes the AV overtake OV closer to it, while a small value makes the AV overtake OV by going very wide of it. The setting of the parameters \(\text {D}_{\text {OV}}\) and \(\gamma \) values are done by the rules defined in the motion planner and are optimally tuned for each specific driving scenario. Under the OV overtaking rule, \(\text {D}_{\text {OV}}\) and \(\gamma \) are set at 2.5 m and 100 to allow the AV to overtake the OV within the available lane width. In the scenario of OV vehicle following, \(\text {D}_{\text {OV}}\) is extended to 15 m while \(\gamma \) is reduced to 1 to ensure the AV maintains a safe following distance. Lastly, for the fully stopping rule, \(\text {D}_{\text {OV}}\) is further increased to 20 m while \(\gamma \) is brought to around 0.35 to prevent the AV from overtaking the OV. The constraint Eq. (6) when satisfied creates a forward invariant safe set \(\mathcal {S}\) that assists the AV to keep it away from the space confined to that of the OV.

2.2.2 Definition of Way-Points Assisted MPC Based OCP

From the fundamentals of optimal control theory, a problem formulation based on a quadratic cost function can be defined. This cost function is then subjected to the constraints arising from the vehicle dynamics (3), OV induced CBFs (6) and from vehicle state and control limits. This OCP is then propagated in time along a predefined receding time horizon T leading to a MPC based formulation as defined in (7).

$$\begin{aligned} \left\{ \begin{aligned} \min _{\underline{U}} \quad J(\underline{X}, \underline{U}) = \frac{1}{2} \sum _{k=0}^{N-1} \left[ \Vert \underline{X}_k - \underline{X}_{\text {ref},k} \Vert _{\textbf{Q}}^2 + \Vert \underline{U}_k - \underline{U}_{\text {ref},k} \Vert _{\textbf{R}}^2 \right] + \frac{1}{2} \Vert \underline{X}_N - \underline{X}_{\text {ref},N} \Vert _{\textbf{P}}^2 & \\ \text {s.t.} \left\{ \begin{aligned} \underline{\dot{X}}_\text {k}- f(\underline{X}_\text {k},\underline{U}_\text {k})= 0 & \quad \text {Dynamic Constraints} \\ \dot{B}(\underline{X}_{\text {AV}},\underline{X}_{\text {OV}},\dot{\underline{X}}_{\text {AV}},\dot{\underline{X}}_{\text {OV}})+ \gamma B({\underline{X}_{\text {AV}}},\underline{X}_{\text {OV}}) \ge 0 & \quad \text {CBF Constraint} \\ {\underline{U}}_{\text {min}} \le \underline{U} \le {\underline{U}}_{\text {max}} & \quad \text {Control Constraints} \\ {\underline{X}}_{\text {min}} \le \underline{X} \le {\underline{X}}_{\text {max}} & \quad \text {State Constraints} \end{aligned} \right. \end{aligned} \right. \end{aligned}$$
(7)

where N indicates the number of discretization time steps distributed along the MPC time horizon and is fixed as 50 steps while T is set at 2.5 s leading to a \(\varDelta \text {T}\) of 0.05 s for the MPC.

\(\underline{X}_{N}\) stands for terminal states of AV, while \({\textbf {P}}\), \({\textbf {Q}}\) and \({\textbf {R}}\) correspond to the weights for the MPC. The way-points that define the path along which AV needs to travel have been set in \(\underline{X}_{\text {ref},k}\) in the problem definition (7). In this paper, the MPC is optimized based on the hypothesis that the distance between AV and OV is known and AV knows whether it is travelling on a road consisting of a single lane or a double lane. The MPC is numerically solved thanks to an open source acados solver based on sqp-rti [13]. The formulation is developed to properly handle 3 common driving scenarios: overtaking, following and full stopping by defining them as rules set by weights to the MPC. In particular, the default driving rule is OV overtake, but if the speed of OV relative to AV is greater than a threshold defined in this paper, vehicle following rule is applied. In situations when AV is travelling along a single lane road and there is an OV stopped along that lane vehicle stopping rule is applied to the MPC. The mathematical relationship between OV velocities for different AV velocities that causes change of rule between OV overtaking and vehicle following scenario is shown in Fig. 1a.

The MPC is able to automatically toggle between these 3 rules depending upon the information about OV velocity, AV velocity and lane width datum by changing the weight matrices \({\textbf {P}}\), \({\textbf {Q}}\) and \({\textbf {R}}\) and parameters \(\gamma \) and \(\text {D}_\text {OV}\) that are defined and set for each rule. For the overtaking maneuver, less emphasis is placed on the weights associated with lateral and longitudinal position tracking error (\(\delta x\) and \(\delta y\)) compared to the weights assigned to velocity tracking error (\(\delta v\)). This approach allows the CBF constraint to push the AV away from region occupied by OV while trying to maintaining the reference velocity \(v_\text {ref,AV}\). Regarding the vehicle following and fully stopping maneuvers, the weights for both scenarios focus primarily on \(\delta x\), with near-zero weights on \(\delta y\) and \(\delta v\). This configuration ensures that the AV strictly adheres to the reference lane with minimal lateral movement and aligns its driving behavior based on the parameters \(\text {D}_{\text {OV}}\) and \(\gamma \). In the vehicle following scenario, as discussed in Sect. 2.2.1, \(\text {D}_{\text {OV}}\) is set to 15 m and \(\gamma \) is reduced to 1. This enables the AV to decelerate and match the velocity of the preceding OV while maintaining a safe following distance. For the fully stopping scenario, as outlined in Sect. 2.2.1, the parameter \(\text {D}_{\text {OV}}\) is increased to 20 m, while \(\gamma \) is adjusted to 0.35. This ensures that the AV comes to a complete stop behind the stationary OV while maintaining a safe stopping distance with minimal lateral movement.

3 Methodology

Extensive virtual simulations were conducted to test the adaptiveness of the MPC-based motion planner and an integrated scenario comprising of an ego vehicle and four OVs is reported in this work. The ego vehicle encounters the four OVs one after the another and performs different manoeuvres based on the velocity of the obstacle vehicle which defines the rule set for the MPC weight adaptation. The algorithm is expected to seamlessly transition between the overtaking, following or safety-critical fully stopping manoeuvres. Further details are included in Sect. 4. The simulations were performed using Simulink platform of MATLAB environment in combination with high-fidelity simulation environment of IPG CarMaker. At each time-step, the environment and the multi-body vehicle model of the simulator take as input the control outputs from the motion-planner. The details of the information exchanged between the Simulink and IPG CarMaker is schematically represented in Fig. 1b. Since ego-localization and obstacle tracking is out of scope of this research, the initial states of the AV and the OV are provided to the motion planning algorithm which in real life would be given by localization and perception modules.

Fig. 1.
figure 1

(a) Plot between the AV reference velocity and the OV velocity that defines the threshold velocity limit that separates vehicle following maneuver from the overtaking maneuver by the AV. (b) Layout of feedback loop established between the developed motion planner and the high-fidelity simulation software.

Full size image

The control algorithm outputs the optimized acceleration (\({u}_\text {acc}\)) and steering rate (\(\psi \)) of the AV, latter of which is integrated to obtain the steering angle (\(\delta \)) at the front wheels from the given initial steering angle. \({u}_\text {acc}\) is used to derive brake pedal angle and gas pedal angle of IPG CarMaker vehicle by utilizing the in-built PID control block of IPG CarMaker. \(\delta \) is used to obtain steering angle of the steering wheel of IPG CarMaker vehicle using the steering system kinematics. The obtained states of the simulator model are fed to the algorithm, which acts as a feedback to the optimizer.

Table 1. Obstacle Vehicle characteristics for Integrated Simulation
Full size table

4 Simulation Results

The simulated scenario could be divided into four functional sub-scenarios occurring sequentially as follows:

  • The AV, driving at a reference cruise velocity overtakes a stationary OV.

  • The AV, driving at a reference cruise velocity overtakes a slow-moving OV.

  • The AV, driving at a reference cruise velocity decelerates and follows an OV moving at a considerably high velocity which then takes a right turn at the road junction allowing the AV to accelerate back to its reference speed.

  • The AV accelerates back to its stated reference velocity and then stops while detecting a stationary OV when overtaking option is not possible because of a divider in between the 2 lanes.

The scenario consists of 4 OVs along the path of the AV as seen in Fig. 2. The OV characteristics are stated in Table 1.

Fig. 2.
figure 2

The path and positions of the AV and the OVs in the integrated scenario simulation. The red car indicates the stationary \(\text {OV}_\text {1}\), while green and yellow cars represent \(\text {OV}_\text {2}\) and \(\text {OV}_\text {3}\) respectively. The dark orange line denotes the barrier that divides the two lanes. Finally the blue car denotes \(\text {OV}_\text {4}\).

Full size image
Fig. 3.
figure 3

Results from the integrated scenario simulation

Full size image

All the sub-scenarios proceed continuously on a two-lane straight road, with each lane having a width of 3.5 m. Before the junction, the lanes are separated by lane markings, and after the junction, they are separated by a barricade. The initial and the reference velocity for the AV are set to 17.5 m/s and the simulation results are shown in Fig. 3. The \(\text {OV}_\text {1}\) gets overtaken by the AV since it is stationary (shown in green region of Fig. 3). The AV then overtakes \(\text {OV}_\text {2}\) as it is travelling within the vehicle following threshold limit of 13.5 m/s (shown in yellow region of Fig. 3). The AV then approaches \(\text {OV}_\text {3}\) and decelerates to follow the OV since \(\text {OV}_\text {3}\) is travelling above the set threshold limit. The AV keeps following \(\text {OV}_\text {3}\) (shown in pink region of Fig. 3) till a junction located at about 1100 m from the starting position of the AV (shown in Fig. 3), where \(\text {OV}_\text {3}\) shifts to another path different from that of the AV. The AV now is free to accelerate to its stated reference velocity \(v_\text {ref,AV}\) of 17.5 m/s and this could be inferred from orange region of Fig. 3. Once the AV crosses the junction and approaches \(\text {OV}_\text {4}\), the presence of barricades between the lanes prevents the AV from performing any further overtaking maneuver. Since there is no adjacent lane for AV now to overtake \(\text {OV}_\text {4}\), even though \(\text {OV}_\text {4}\) is stationary AV decelerates and stops behind \(\text {OV}_\text {4}\) (shown in indigo region of Fig. 3).

The logarithm of the CBF value during the simulation run over time is plotted in Fig. 3 as the black line and it could be inferred that constraint values are always greater than zero with a minimum value of 0.22 confirming that the constraints are always satisfied. As previously mentioned, the weights assigned to the MPC cost function are instantaneously adjusted according to the OV handling rules. This adjustment can be observed from the magenta line in Fig. 3, which defines the rule activated based on the scenario observed by the AV. A value of 1 indicates that the vehicle overtaking rule is followed, while values of 2 and 3 indicate that vehicle following and vehicle stoppage rules are followed by the MPC. Notwithstanding the fact that the MPC weights are shifted instantaneously, the AV does not exhibit any significant transient behaviours which could be observed from AV path in Fig. 2, which in-turn signifies that sudden change of the weights does not lead to instability of the AV. This behaviour was possible by setting the weights to control inputs on the \(\psi \) relatively higher compared to all other weights for all the scenarios in addition to the rule specific weight setting on tracking errors discussed earlier. A twofold advantage of such setting is that AV does not show high fluctuation in acceleration and steering rate values thereby resulting in smoother velocity profile which can be observed from the velocity plot in Fig. 3 (red line).

The normalized \(\delta x\) error cost is shown in Fig. 3 as the blue line. It can be inferred that this cost increases when the AV steers away from its reference line to perform an overtaking maneuver around the OV. The motion planner then works to bring the AV back to the reference line while satisfying the CBF constraints to reduce this cost. A similar pattern is observed for the \(\delta y\) error cost, also normalized and depicted as the dark green line in Fig. 3, during the overtaking maneuver. During the vehicle following and fully stopping scenarios, it could be inferred that the \(\delta x\) error cost is almost equal to 1 with near-zero cost values for \(\delta y\) and \(\delta v\) (also normalized and shown as brown line) error. This occurs because the AV tries to follow the \(\text {OV}_\text {3}\) while maintaining a safe following distance with minimal lateral deviation from the reference line, achieved by heavily penalizing the \(\delta x\) error for the vehicle following maneuver. Similar behavior is observed in the vehicle stopping scenario where the AV decelerates from its stated reference velocity \(v_\text {ref,AV}\) and completely stops behind \(\text {OV}_\text {4}\) with minimum lateral deviation from its reference by sacrificing the cost on \(\delta y\) and \(\delta v\) errors. Additionally, when \(\text {OV}_\text {3}\) steers right from the junction, the \(\delta y\) error cost is initially higher and gradually declines. This is because the AV is now free to track the way-points set for \(v_\text {ref,AV}\), which was not possible earlier due to the vehicle-following rule enforced because of \(\text {OV}_\text {3}\). The AV reaches these way-points by accelerating through the road junction. The \(\delta v\) error cost is higher during the overtaking maneuvers around OVs because a reduction in the AV velocity is necessary to safely overtake the OVs as observed in the red line. The normalized \(\delta v\) error cost is also higher when the AV accelerates along the road junction to reach \(v_\text {ref,AV}\) as \(\text {OV}_\text {3}\) moves right at the junction.

The maximum solving time for the combined setup (Simulink + IPG CarMaker) during the entire simulation time range is found to be 4.2 milliseconds which is less than \(\varDelta \text {T}\) of 0.05 s (20 hz) required by MPC thereby demonstrating the real-time integration capability of the developed motion planner. This 20 Hz control frequency ensures the system can dynamically react to unexpected situations within a suitable reaction time, as highlighted in the research work of [10].

5 Conclusion

In this paper, a rule-based adaptive motion planner for an AV capable of autonomously overtaking, following, and stopping behind an OV is developed using an adaptive MPC framework. Based on the lane width available and relative speed between the AV and the OV, rules are defined that enables the motion planner to consider the proper driving scenario. The simulation results for these rule-defined scenarios are presented which confirms that the motion planner is able successfully adapt to different driving scenarios without colliding with any of the OV by seamlessly transitioning between weights assigned for different driving conditions.