Safe Vehicle Trajectory Planning in an Autonomous Decision Support Framework for Emergency Situations

Abstract

For a decade, researchers have focused on the development and deployment of road automated mobility. In the development of autonomous driving embedded systems, several stages are required. The first one deals with the perception layers. The second one is dedicated to the risk assessment, the decision and strategy layers and the optimal trajectory planning. The last stage addresses the vehicle control/command. This paper proposes an efficient solution to the second stage and improves a virtual Cooperative Pilot (Co-Pilot) already proposed in 2012. This paper thus introduces a trajectory planning algorithm for automated vehicles (AV), specifically designed for emergency situations and based on the Autonomous Decision-Support Framework (ADSF) of the EU project Trustonomy. This algorithm is an extended version of Elastic Band (EB) with no fixed final position. A set of trajectory nodes is iteratively deduced from obstacles and constraints, thus providing flexibility, fast computation, and physical realism. After introducing the project framework for risk management and the general concept of ADSF, the emergency algorithm is presented and tested under Matlab software. Finally, the Decision-Support framework is implemented under RTMaps software and demonstrated within Pro-SiVIC, a realistic 3D simulation environment. Both the previous virtual Co-Pilot and the new emergency algorithm are combined and used in a near-accident situation and shown in different risky scenarios.

1. Introduction

In the last 20 years, the transportation paradigm has evolved in order to improve road safety, minimize energy consumption and pollutants emissions, and optimize traffic conditions [1,2]. One of the key solutions is the development and deployment of automated mobility (level 3 to 5 of SAE (Society of Automotive Engineers) standard). With the expected functionalities of such automated systems, the mobility concepts will strongly and radically evolve. Technologies are a crucial issue in the development of this new generation of mobility means. Technology issues are not only focused on new powertrains, new hardware architecture, or new types of efficient embedded sensors for Autonomous Driving Systems (ADS) (i.e., radar [3], lidar, IR-neuromorphic-UHD-Polarized-Stereo-Fisheye cameras, GPS [4], etc.) but also on new methods, approaches and algorithms in order to process the data coming from information sources (sensors, HD Maps, communication). Artificial Intelligence (AI) takes an increasingly important role in the “onboard intelligence” [5]. Thus, sensors provide information to the perception layer which allows to assess the attributes of the road key components (obstacles, road, ego-vehicle, environment, and driver). This perception information allows building dynamic local perception maps, which can then be extended by sharing data using specific Vehicle to Vehicle or Vehicle to Infrastructure communication protocols. From these maps, decision module and path planning methods can be implemented in order to drive vehicles automatically and safely without any human active operation (see Figure 1).

In a fully autonomous driving system, while perception modules are compared to human eyes and ears, decision-making and planning are the “brains” of autonomous driving, control/command stages being the feet and hands. In this architecture, the decision-making and planning modules represent the system’s intelligence (reasoning layer). Like for human operating, after the brain receives various stimuli and perception information, the decision layer analyzes them to understand the current environment and then apply strategies to move according to constraints (safety, energy, comfort, etc.). Then, the decision-making module generates instructions (path, trajectory, orders, maneuvers, or advice) to the control module. The level of complexity that a decision-making planning module can handle is a core indicator for measuring and evaluating autonomous driving capabilities [6]. Another indicator is its ability to respond to unexpected events by rapidly re-planning its trajectory.

1.1. Problem Description

In a very general setting, path planning refers to finding a continuous mapping $\sigma :[0,1]\to X$ (a path), in a given environment with known boundaries and obstacles embedded in a state-space X. Constraints on this mapping typically include:

• Constraints on the initial state $\sigma \left(0\right)$ (position, orientation, etc.) and the final state $\sigma \left(1\right)$ (location of the target point);
• Staying within the boundaries and avoiding collisions with obstacles $\sigma \left(\alpha \right)\in X_{free},\forall \alpha \in [0,1]$;
• Geometric constraints on the path, typically a maximum curvature.

However, when moving obstacles are taken into account, a simple path planning algorithm (i.e., generation of a geometric path between the origin and the destination) cannot be used alone, and the time dimension must be added. In this case, the planning problem can not be solved in polynomial time, and becomes an NP-hard problem [7]. Correspondingly, the constraints change to:

• Constraints on the initial state and the sub-goal state, considering the time dimension.
• Constraints on staying within the boundaries and avoiding moving obstacles;
• Differential constraints on the trajectory, typically upper bounds for speed, acceleration, steering, and steering rate.

This problem is now called trajectory planning. It can be broken down by using a “path-speed” decomposition approach, where the geometric path and the speed profile are computed separately, but this is not always possible or optimal. These real-time planning algorithms typically run continuously with a receding horizon, and must be robust enough to handle sudden changes in their environment. In simulation, these emergency situations can be used to test the algorithm’s limits.

1.2. Literature Review

1.2.1. Decision Making

Different decision-support architectures have been applied to autonomous driving. In a controlled environment (such as in the 2007 DARPA Urban Challenge [8]), rule-based [9,10] and knowledge-based [11,12,13] methods have been successfully applied. However, these methods assume full knowledge of the traffic environment, including the other road users intent. Considering the uncertainty in the environment, the tactical decision task is usually modelled as a Partially Observable Markov Decision Process [14], which has been applied to different driving scenarios [15,16,17]. The core of the planning-based method is trajectory planning, i.e., generating a trajectory for a given scenario [18,19,20] based on the trajectories of other vehicles, but its applicability is still limited in a highly interactive environment. The rapid development of machine learning allowed using learning-based methods for autonomous driving decision-making in highly interactive environments, such as the imitation learning method [21,22], or more recently a combination between traditional methods and reinforcement learning methods [23].

1.2.2. Path and Trajectory Planning Algorithm

Solving the lower dimensional planning problem through the search space is a common approach for path planning. The basic idea is to discretize the state space into a graph and then use a search algorithm such as Dijkstra [24] or A* [25] to find feasible solutions or optimal solutions. The A* algorithm is an extension of Dijkstra, which assigns weights to nodes based on a heuristic-estimated cost, then performs a fast graph search to the target node. Many different variations based on this algorithm have been proposed for path planning: Hybrid A* [26], Theta* [27], Filed D* [28], D* Lite [29], etc. Besides, several planning approaches [30,31] have been proposed to increase the system reliability, considering the localization uncertainty problem.

However, when it comes to the trajectory planning problem in a dynamic and continuous environment, the search space dimensions will increase, leading to significant computational time growth. In this case, the planning problem can be approximated to a discrete sequence of optimization problems by sampling the continuous state space. The representative algorithms are Probabilistic Road Map [32], and Rapidly-Exploring Random Tree (RRT) [33] algorithms, which have a good performance in high-dimensional non-convex state spaces. The latter method has been extensively adopted in the trajectory planning for autonomous driving [34,35], with several improvements to the RRT algorithm being proposed over the years, either to reduce the computation cost (anytime RRT [36], RRT-Connect [37], and Dynamic RRT [38], etc.), or to optimize the generated path (RRT* [39,40]).

When faced with the trajectory planning problem in a dynamic and continuous driving environment, Artificial Potential Field (APF) methods [41] use a continuous equation, similar to the electrostatic potential field. APF methods can easily be implemented and executed in real-time for control and navigation [42]. In these algorithms, the moving object tries to reach a target and to avoid obstacles on the trajectory by using virtual forces that either attracts it or pushes it away. One benefit of this framework is that different constraints can be taken into account by simply adding specific forces, and several new potentials have been proposed, based on the road structure (with lane and boundary potentials) or on vehicle physics (guided potential), in order to use APFs in different scenarios of autonomous driving (highway [43], intersection [44]). Several improvements were proposed to solve different limitations [45] of the traditional APF algorithm for autonomous driving. An improved APF model [46] was designed to avoid the local minimum problem, which can also be tackled by using a virtual obstacle [47], or a virtual target point [48]. By modifying the calculation of APF using fuzzy logic, the nearby obstacle problem can be dealt with. In [49], an additional artificial friction force was introduced to limit oscillations. Although APF algorithms can provide adequate trajectories as the results, the final configuration of the vehicle is unpredictable, which may lead to dangerous situations. Another general problem with this method is that the kinematic constraints of the vehicle are very difficult to take into account, thus the mechanical feasibility of the trajectories cannot be guaranteed.

Elastic bands (EB) methods [50] also come from a physical analogy. In these methods, the planned path of the object is simulated through a series of springs that can be deformed to react to changes in the environment. Refs. [51,52] applied EB theory to the emergency path planning problem. Song [53,54] proposed specific forces to respect different constraints, such as the lane constraint, to make the vehicle drive on a given lane, and the guide-potential constraint, to avoid collisions between following vehicles. The internal forces of the EB provide a constraint to neighbouring trajectory nodes, but the method still struggles with precise kinematic constraints. Choosing the trajectory’s final point (during re-planning) is also an difficult issue in emergency situations.

1.2.3. Vehicle Control

The role of vehicle control in the autonomous driving architecture is to track the theoretical trajectory computed by the planning algorithm. Models of vehicle kinematics, like the bicycle model [55], are used to translate this trajectory $\left(x\right(t),y(t\left)\right)$ into a series of commands, usually steering angle and acceleration. Different controllers have been used for autonomous driving at relatively low speed: Proportional Integral Derivative (PID) controllers [56], pure pursuit controllers [57], Stanley controllers [58], among others. Dynamic model-based control methods have better performance at high vehicle speed or with a large curvature change rate. Nonlinear control [59], Model Predictive Control (MPC) implementations [60,61,62] and feedback-feedforward control [63] can increase the stability of the vehicle at high speeds.

1.3. Contribution

This paper looks to improve a planning algorithm developed at Eiffel University [64] by testing its limits in emergency situations. The main contributions of this paper, developed for the EU H2020 Trustonomy project [65], are the following.

• A general decision-support framework for autonomous driving is introduced, which uses a combination of two different algorithms for trajectory planning (“autonomous mode” and “emergency mode” algorithms). This expanded framework keeps the efficiency of the previous algorithm but improves its robustness in emergency situation.
• An emergency trajectory planning algorithm was developed, based on elastic bands. The following improvements are presented:

  –

  A new internal force is proposed, to ensure the steering and acceleration of the vehicle stay bounded (kinematic constraints);

  –

  The last point of the trajectory is not fixed, but instead is subject to specific internal and external forces, to give more flexibility;

  –

  The final trajectory is a linear combination of the solution at different iterations, and is therefore guaranteed to be within the (static) boundaries of the road.

• The new framework was implemented and validated using Matlab and Pro-SiVIC simulations in different emergency scenarios, using both algorithms. The corresponding performance results are evaluated.

1.4. Organization

The structure of this paper follows these three contributions. Section 2 presents first the decision-support previously developed at Eiffel University, then the expanded framework and its components. Section 3 presents the emergency trajectory planning algorithm and its implementation. Section 4 shows the simulation results of our two trajectory planning algorithms in different near-accident scenarios, while Section 5 analyzes the results in details and discusses future research directions.

2. Autonomous Decision Support Framework

2.1. Autonomous Driving and Virtual Co-Pilot

The Decision-support framework presented in this paper is based on the concept of “virtual Co-Pilot” developed over the past few years at Gustave Eiffel University (see Appendix A). As part of a European project (HAVEit [64]) and a French project (ABV [66]), some components needed for the design of a virtual Co-Pilot framework involving the perception, decision, path planning, and control modules have been developed at Gustave Eiffel University [64] as a highly automated driving delegation application shared with a human driver for Highway situations. This optimal path planning application (minimization of a set of criteria including risk) tackled a multi-constrained problem. In terms of general architecture, the functions developed for the virtual Co-Pilot are very similar to human operations and behaviours (see Figure 2).

The virtual Co-Pilot can guarantee a high level of legal safety regarding the road environment. To do that, the Co-Pilot uses human and traffic rules to guarantee safety and efficiency in mixed traffic (with other non-equipped vehicles and users).

Its application domain is defined as an operating space guaranteeing the safety of users when all road users respect the traffic rules. In this Operating Driving Domain (ODD), all the necessary strategies are implemented to avoid collisions or, in the worst-case scenario, to mitigate them, using emergency strategies. In autonomous mode, the Co-Pilot considers several constraints, such as the target speed and the target lane of traffic.

Finally, the human can be put in the loop, using specific mechanisms to allow the human driver to share the driving task with the virtual Co-Pilot. A few years ago, this active and/or informative human/machine interaction was a great challenge. The sharing of the driving task simultaneously by the ADS and the human driver will not be addressed in this paper but can be found in [64].

Some of the key ideas and algorithms of the virtual Co-Pilot are presented in Appendix A. The next section will present an expanded decision-support framework, built on this Co-Pilot.

2.2. New Framework Design

One of the limitations of the Co-Pilot presented above is its assumption that all road users follow its set of human and traffic rules. As a result, it is possible to reach an emergency situation where the Co-Pilot cannot find a collision-free trajectory in specific extreme scenarios (examples of these are shown in Section 4.2.3).

As part of the European Trustonomy project [65], the Co-Pilot was integrated into a more general Automated Decision-Support Framework (ADSF) to avoid these emergency situations if possible and propose a safe answer otherwise. The main innovation in this framework, which is illustrated in Figure 3, is its use of two independent trajectory planning algorithms (the “autonomous” and “emergency” algorithms), together with a specific module to switch between modes depending on the situation. The autonomous mode uses the planning algorithm developed for the Co-Pilot, while the emergency mode uses a new and specific algorithm for emergency situations, detailed in Section 3. The early warning module was added to try to prevent emergency situations whenever possible, especially in “human driving” mode (when the Co-Pilot is not used), and is detailed in [68]. In the Trustonomy project, the Decision-support module also interacts with the HMI (Human-Machine Interface) and DIPA (Driver Intervention Performance Assessment) modules, but these interactions are not detailed in this paper.

2.3. Component Overview

As shown in Figure 3, this framework is divided into three major components:

Early warning:

The role of this component is to monitor and forecast different risk levels and indicators. Those risks are relative to the main road key components (obstacles, road attributes, ego-vehicle, environment, driver). Each calculated risk depends not only on the current state of the component itself but also on a forecast of its evolution. Thus, each risk has two thresholds (warning and emergency) and three driving states (regular, cautious/degraded, and emergency/critical). When a risk is higher than the warning threshold, the associated warning is sent to the driver via the HMI, whereas the emergency threshold is used directly by the Mode manager. This component is presented in details in [68].

Mode manager:

This component decides which driving mode should be adopted, depending on the current and short time predicted situations, the driver’s wish (input from HMI), and the combination of the different risk levels (input from Early warning). The ODD establishes conditions under which the vehicle may operate in autonomous mode. The Mode manager is also responsible for starting the emergency mode and issuing a request to intervene to the human driver.

Trajectory planning:

This module generates the trajectories, which the vehicle tracks in autonomous and emergency modes. It uses inputs from the perception module to predict the trajectories of the different objects/obstacles in its environment. It then generates, evaluates and selects the best trajectory for the ego-vehicle. This trajectory must respect a set of constraints (traffic, system, driver rules) relative to safety. Depending on the mode, it uses either the classical Co-Pilot algorithm (see Appendix A) or the new emergency algorithm (see Section 3).

2.4. Mode Transitions

The ADSF can send warnings to the driver (in non-autonomous mode), request to intervene (in autonomous mode) or start the emergency mode (at any time). In non-autonomous mode, the human driver controls the vehicle, the trajectory planning module is turned off, and the Early Warning is monitoring the driving risks. In autonomous mode, the ADSF controls the car, and the three components are active simultaneously. In emergency mode, the early warning component is turned off, and the trajectory planning component takes inputs from the vehicle status and environment perception data. The transitions between those different modes are operated as shown in Figure 4.

3. Emergency Trajectory Planning Algorithm

In order to build an efficient emergency trajectory allowing to take into account the level of criticality of the current situation, the emergency function of the automated decision support is made of two blocks. One is the emergency trajectory planning which rapidly produces candidates for emergency trajectories and the other one is an “ethical” based decision module. According to an IEEE study on ethics [69]: “Society has not established universal standards or guidelines for embedding human norms and values into Autonomous and Intelligent Systems today. However, as these systems are instilled with increasing autonomy in making decisions and manipulating their environment, it is essential they be designed to adapt, learn, and follow the norms and values of the community they serve.” Thus, the idea of Trustonomy project is to implement human norms and test them, as a first stone on the way of ethically designed autonomous decision systems.

3.1. New Emergency Mode

Although the model used in the virtual Co-Pilot (referred to as “the Vanholme’s model” thereafter [64]) is able to provide efficient strategies for different autonomous driving scenarios and different autopilot styles, its decision-making abilities are limited in certain specific situations:

• In this model, the only option proposed in case of emergencies (no collision-free solution found) is an emergency braking to mitigate the collision. Although this answer can work in certain low-speed driving situations, we cannot expect the vehicle to have sufficient braking distance to avoid a collision when entering emergency mode even at medium speed. Meanwhile, a different decision, such as steering rapidly to avoid an obstacle instead of braking, may allow to avoid collisions altogether. It follows that this single option (safe braking) makes the Vanholme’s model less adaptable in emergency situations.
• The Vanholme’s model was designed to react to emergency situations while driving in autonomous mode. The ego vehicle takes the corresponding actions based on certain rules, and all the decisions are made in a fully autonomous driving environment. However, when the emergency mode is force switched from manual mode because of unsafe behavior or visual constraints of the driver (e.g., following with high speed and close distance, invisible obstacles), preconditions in emergency mode will be uncertain for the vehicle and namely lead to Vanholme’s model going to an unknown state, then fails in reducing the risk of collision.

In our new decision-support framework, a new trajectory planning algorithm was proposed specifically for these emergency situations, when the Vanholme’s model cannot find a collision-free trajectory. This new algorithm is based on Elastic Bands theory. Having as inputs the road environment and the obstacles state together with the normally planned trajectory (from the previous section), the emergency planner adapts the initially planned trajectory to avoid any collision with the detected obstacles while keeping the vehicle on a safe road. This module then outputs potential candidates to be activated in a decision module. The basic principle of this relies on the EBs which seek minimal deformation of the original trajectory. This algorithm has been improved in 2003 by the team of Hilgert et al. [51]. The module proposed in this ADS is mainly based on the works from Brandt et al. in 2005 [70] (see Figure 5). Our new algorithm in this paper combines an improved internal force based on the classic EB method and external force provided by the common artificial potential method. When the vehicle goes into emergency mode, the trajectory can be computed based on the physically acting of these two types of forces, which results in the mode being able to provide multiple solutions based on the different obstacles and road information. In addition, the previous state(speed and orientation) of the vehicle will be also considered in the computation of internal forces in order to simulate a more realistic dynamic process of collision avoidance. Based on these improvements, whether the emergency mode is switched from autonomous mode or manual mode, the model can quickly generate an optimal trajectory with mechanical feasibility to react to current situations.

Thus, it can be seen that the decision-making model combing Vanholme’s model for autonomous mode and the new improved trajectory planning for emergency mode is able to provide more efficient and feasible solutions based on the different scenarios of autonomous driving (even combining with manual driving), especially for some uncertain emergency situations.

3.2. Internal Force

The general Elastic Bands method simulates a trajectory through a series of nodes $r_t$ representing the positions of the vehicle, at different times t in the immediate future. Usually, each pair of consecutive trajectory nodes is linked by a spring so that they stay at a relatively constant distance from each other (see Figure 5). The total contraction force $F_t^{cont}$ applied to node t is computed as shown in Equations (1) and (2):

$$F_t^{cont}=F_{t-\Delta t,t}^{cont}+F_{t,t+\Delta t}^{cont}$$

(1)

$$F_{t,t+\Delta t}^{cont}=k_{contraction}·\left(\right||r_{t+\Delta t}-r_t\left|\right|-l_0)·\frac{r_{t+\Delta t}-r_t}{\left|\right|r_{t+\Delta t}-r_t\left|\right|}$$

(2)

where $F_{t-\Delta t,t}^{cont}$ (resp. $F_{t,t+\Delta t}^{cont}$) is the contraction force from the previous (resp. next) trajectory point. The spring original length $l_0$ and coefficient $k_{contraction}$ are free parameters. Since the internal force of the trajectory node is only calculated based on the neighbouring nodes in the classic algorithm, the state of the previous time step does not have any influence on the current time step, which is not able to satisfy a continuous and dynamic process in the trajectory planning. Considering this limitation of the classic algorithm, we redefine the internal force as shown in Equation (3):

$$F_t^{internal}=k_{internal}·(r_t^{Equilibrium}-r_t)$$

(3)

where $F_t^{internal}$ is the internal force on the current trajectory point, $r_t$ is the node position of current time step. and the coefficient $k_{internal}$ is free parameter. $r_t^{Equilibrium}$ is the equilibrium position of current node, which is computed based on the positions of previous two nodes and next nodes. When these three points are collinear, the equilibrium position can be simply calculated as $r_{t-\Delta t}+\frac{(r_{t+\Delta t}-r_{t-2·\Delta t})}{3}$. On the contrary, current equilibrium position is calculated based on the arc established by previous two nodes and next nodes, which satisfies that ${\theta }_3-{\theta }_2={\theta }_2-{\theta }_1$ (see Figure 6).

In classical EB, the first trajectory node (current position) and the last one (target) are fixed in advance, then forces apply to the nodes in the middle. Therefore, the result is largely related to the position of the last point on the band, which may cause the trajectory of the current situation is not optimal. For example, when the fixed last point is near to the obstacle, the vehicle may lose an early deceleration opportunity, namely the risk of collision has been increased. Besides, the attraction of the last node may lead to some mechanically unrealistic trajectories, when the vehicle is pushed by strong external forces. This problem is manifested in the sudden changes in the speed and direction of the last few trajectory points. Based on these problems, our approach defined a movable last point depending on the previous two trajectory nodes (see Equation (4)), accordingly provided a more optimized solution in some emergency situations.

$$F_{t_{last}}^{internal}=k_{internal}·(2·r_{t-\Delta t}-r_{t-2·\Delta t}-r_t)$$

(4)

where $F_{t_{last}}^{internal}$ is the internal force on the last trajectory point, which is different to the fixed last trajectory point of the classical EB method.

As mentioned in the last section, compared to the classical EB method, the new internal force also considered the state of the previous two nodes. The reasons are due to the potential problems specifically linked to emergency situations, which are often ignored in the literature. The first problem related to emergency situations is that if the obstacle is very close, a simple contraction/repulsion algorithm might end up contracting all the nodes towards the current position of the car, ignoring the current speed of the car (see in Figure 7), which is lack of mechanical feasibility. In the above emergency situations, the classic method also ignored the previous orientation of the car, which is the second problem (see in Figure 8). To avoid these abrupt changes in the vehicle orientation and velocity, especially between the first and second nodes, the internal force is applied to each node depending on the relative orientation and distance of the previous two nodes and the next node, which ensures that steering and speed changes of the vehicle are in a gradual process (see in Figure 7 and Figure 8).

3.3. External Force

As is common in the literature, the potential of the road prevents the vehicle from leaving the boundary and keeping its current lane on the road. When the vehicle drives out of the boundary, the value of the potential $U_{boundary}^t\left(x\right)$ should become infinite. In the meantime, the potential will also give a relatively weak potential $U_{lane}^t\left(x\right)$ to the vehicle when it drives on a certain lane (as in Figure 9). Thus, a repulsive force is employed according to Equation (5):

$$F_{external,road}^t=-\nabla [U_{boundary}^t\left(x\right)+U_{lane}^t\left(x\right)]={\overrightarrow{u}}_{road}·(k_{lane}·(x-sign\left(x\right)·\frac{w}{2})+k_{boundary}·(\frac{1}{p+x}-\frac{1}{p-x}))$$

(5)

where $F_{external,road}^t$ is the external force provided by the road potential. ${\overrightarrow{u}}_{road}$ is the direction vector (left side to the right side) of the x-axis in the relative coordinate system, and x is the current position in this coordinate system. The boundary force acting on the vehicle is in the area $p-w$, where w is the width of the lane, the length of p is slightly bigger than w. Coefficient $k_{lane}$ and $k_{boundary}$ are free parameters.

The external force also aims to keep the current vehicle a safe distance d0 away from other dynamic or static obstacles by building an obstacle potential $U_{obstacle}^t\left(x\right)$ that rises to infinite strength when any other vehicles approach any part of them. In some emergency situations, steering to avoid is encouraged instead of stop, which means a shaped obstacle potential is desired. Thus, we choose an obstacle force as shown in Equation (6):

$$F_{external,obs}^t=-\nabla \left[U_{obstacle}^t\left(x\right)\right]={\overrightarrow{u}}_{obs}·k_{obs}·(\frac{1}{d}-\frac{1}{d0})·\left(\frac{1}{d^3}\right)$$

(6)

where $F_{external,obs}^t$ is the external force provided by the obstacle potential, ${\overrightarrow{u}}_{obs}$ is the direction vector from the center of the obstacle to the current point. d is the distance to the boundary of the obstacle, and $k_{obs}$ is a positive constant. In order to take into account moving obstacles, each node at time step t has its own environment and therefore its own potentials $U^t$, and the external force acting on this trajectory node is the sum of the forces provided by these potentials.

3.4. Dynamic Process

This proposed algorithm starts with an initial collision-free exploring trajectory of n nodes, representing the position of the vehicle at different times in the future. The total force $F_{total}\left(i\right)$ applied to a node at iteration i is given in:

$$F_{total}\left(i\right)=F_{internal}\left(i\right)+F_{external}\left(i\right)$$

(7)

where $F_{internal}\left(i\right),F_{external}\left(i\right)$ represent respectively the internal force, external force on the current node at iteration i. During the planning step, the trajectory nodes move under the action of total force applied to them for several iterations, until an equilibrium state is reached where the total force on each node is approaching zero. This step must be fast enough to ensure real-time operations. For this reason, second-order computation is employed to compute displacements between trajectory points at each iteration as in Equation (8), which can provide a fast convergence compared to directly applying the forces on the trajectory nodes.

$$\mathit{q}(i+1)=-\frac{{\mathit{M}}_{total}\left(i\right)}{{\mathit{F}}_{total}\left(i\right)}+\mathit{q}\left(i\right)$$

(8)

where $\mathit{q}(i+1)$ is the positions of trajectory at iteration $i+1$. ${\mathit{M}}_{total}\left(i\right)$ represents the total matrix derivative ($2n*2n$ Hessian matrix) of the total applied forces, namely the sum of the matrix derivative of each force at iteration i. ${\mathit{F}}_{total}\left(i\right)$ is the forces acting on the trajectory at iteration i, and $\mathit{q}\left(i\right)$ is the positions of trajectory at iteration i (see Equation (9))

$$\begin{array}{cc}\hfill {\mathit{F}}_{total}\left(i\right)& =[F_{total}^1\left(i\right),F_{total}^2\left(i\right),...,F_{total}^n\left(i\right)]\hfill \\ \hfill \mathit{q}\left(i\right)& =[r^1\left(i\right),r^2\left(i\right),...,r^n\left(i\right)]\hfill \\ \hfill {\mathit{M}}_{total}(j,k)\left(i\right)& =\frac{\mathrm{d}{\mathit{F}}_{total}\left(j\right)\left(i\right)}{\mathrm{d}\mathit{q}\left(k\right)\left(i\right)}\hfill \end{array}$$

(9)

One problem occurs when second-order computation is employed, the trajectory node may go into the non-driving area (inside the obstacle, out of boundary) due to a large displacement. In order to solve this problem, we added a new computation (see Equation (10)) at each iteration in this area, as shown in the Figure 10. When the trajectory node is detected at the non-driving area at iterations $i+1$, the possible biggest $\mathit{k}$ is chosen to ensure that the new position is in the driving area, and it is determined $\mathit{k}$ with the value of 0 can still work in the worst case. In addition, $\mathit{k}$ is 1 for the nodes that are already in the driving area.

$$\begin{array}{cc}\hfill {\mathit{q}}_{new}(i+1)& =\mathit{k}·\mathit{q}(i+1)+(1-\mathit{k})·\mathit{q}\left(i\right)\hfill \end{array}$$

(10)

where ${\mathit{q}}_{new}(i+1)$ is the new positions of trajectory in driving area at iterations $i+1$. $\mathit{k}$ is a array of positive coefficients in the range of 0 to 1.

The final trajectory is then selected and sent to the vehicle controller. At the next iteration, the exploring trajectory starts at the new position of the vehicle, and uses the last $n-1$ points of the final trajectory; one last point is added assuming constant speed and steering. The environment data is updated (obstacle positions and speeds, road, etc). The emergency algorithm stops when the vehicle has stopped (or crashed), or if either the driver or the autonomous mode is ready to take over.

4. Results

4.1. Results in Normal Driving Conditions

In order to test, evaluate and validate the performance and capacities of this Co-Pilot, a set of use cases have been implemented in both real and virtual environments. The purpose is to confirm that the Co-Pilot is able to guarantee reliable, robust and, above all, safe operating and driving behavior in relation to different rule categories. The different scenarios used and tested were “approaching a speed limit”, “approaching a vehicle or a ghost zone”, “following a target”, and “overtaking a vehicle”.

It is important to note that this virtual Co-Pilot (ADS) was prototyped and developed using the Pro-SiVIC platform (ESI group), coupled with the RTMaps platform (Intempora). Pro-SiVIC (presented in Figure 11) is a 3D simulation software designed for autonomous vehicle prototyping, test, evaluation, and validation: it features complex environments, realistic modelling of the vehicles, and of all the embedded sensors (cameras, radars, lidars). RTMaps is a data processing platform that controls the car and does all the trajectory computations, either in simulation (when connected to Pro-SiVIC) or in the field (when implemented on a real vehicle).

In order to be able to concentrate solely on the development of the virtual Co-Pilot, the perception section was generated by the Pro-SiVIC “observers”. These “observers” were “ground truths” sensors that provided data and by extension, perfect perception. In order to validate the reliability and robustness of the suggested method, we ran scenarios with 5 and 10 vehicles, each with its own Co-Pilot configured for one of the three driving modes (“comfortable”, “normal”, “sporting”). On the HMI presented in Figure 11, we can observe the use of an instruction matrix for possible maneuvers and reachable zones (size 3 × 3). The first cell represents a left lane change and acceleration. The second cell represents a simple acceleration. The fifth cell represents a constant state (constant position and speed). The 9th cell represents a right lane change and deceleration. As we can see, the cells in red represent impossible maneuvers (trajectories, speed profiles) after the evaluation stage.

Once the results obtained in the simulation were considered to be of sufficiently good quality, the Co-Pilot application was embedded into one of Eiffel University’s prototype vehicles dedicated to automated driving systems (presented in Figure 12) [64]. For these real tests on test tracks, the dynamic local perception map was built from a set of modules and functions allowing assessing the state of all relevant road key components (obstacles, road, ego-vehicle). The obstacle detection and tracking were made with a cooperative fusion approach. This cooperative fusion approach used either a dense stereo vision system and a lidar [71], or a mono camera system and a lidar [72]. For the dynamic assessment of the road objects, a robust and efficient tracking stage based on the belief theory has been applied in [73]. For the road markings and lanes detection and tracking, the work developed in [74,75] was integrated in the perception architecture. This road-centered part allowed managing multiple lane configurations and lane changes without discontinuities and interruptions. About the localization and ego-vehicle state assessment, an Interacting Multiple Model (IMM) approach adapted for unusual vehicle state and maneuver has been proposed [76]. The fusion of data coming from lateral cameras, HD Maps, and the IMM approach provides a very accurate lateral positioning [77]. It is essential for lane changing and overtaking maneuvers. This full perception architecture is presented in Figure 12.

Figure 13, Figure 14, Figure 15 and Figure 16 present the results obtained for the first three use cases with the embedded Co-Pilot in a real prototype, used on the Versailles Satory’s test tracks. In Figure 13, we can clearly see the adaptation of the vehicle speed to a constraint (new speed limit). Figure 14 shows the reaction of the Co-Pilot recognizing a “ghost” vehicle after having detected an obstacle. Figure 15 shows a target tracking scenario, with the target making gear changes and one lane change. Finally, Figure 16 shows an overtaking scenario involving both longitudinal and lateral maneuvers.

4.2. Results in Emergency Driving Mode

The previous section presented four simulations in different scenarios, for which the Co-Pilot was initially designed and can handle easily. However, when put in more difficult situations, like the two presented in Section 4.2.3, the Co-Pilot alone could not find a safe trajectory and collided with the obstacle. The simulations presented below will showcase the abilities of our new framework and of the emergency algorithm used in combination with the Co-Pilot.

4.2.1. Parameters Values

In

Table 1. Parameters adopted in the simulations.

Notations	Description	Value
Indices
$\Delta t$	interval of time step	0.25 (s)
w	width of the road	4 (m)
l	length of the vehicle	4 (m)
h	width of the vehicle	2.5 (m)
$v_{initial}$	initial velocity of the vehicle	10 (m/s)
${\theta }_{initial}$	initial orientation of the vehicle	0 (°)
$k_{internal}$	strength of internal force	1
$Force\_limit$	condition of equilibrium	1 × 10−3
Problem parameters
p	identify the acting area of boundary force	5 (m)
$k_{lane}$	strength of external force from the lane	0.02
$k_{boundary}$	strength of external force from the boundary	0.002
$k_{obs}$	strength of external force from the obstacles	5
N	number of the nodes in exploring trajectory	10
$I{t}_{max}$	maximal number of iterations	20
State variables
${\mathbf{k}}_i$	the array of the parameters of the forces in the non-driving area at iteration i	[0,1]

, the adopted parameters are enumerated for the simulations in Matlab and Pro-SiVIC. In this paper, we use the same parameters both in Matlab and Pro-SiVIC, in order to make a comparison and validation of the simulation results. It is worth mentioning that the strength of the internal force is fixed to constant 1, other problem parameters are tested based on this value and other indices. In addition, the state variables $\mathit{k}$ is an array with all ones initially since the initial exploring trajectory is collision-free. Once some nodes of the new exploring trajectory locate in the non-driving area, the corresponding element in $\mathit{k}$ will change according to the current and the previous position of the node to ensure that the trajectory nodes are always in the driving area.

4.2.2. Simulation in Matlab

The Emergency Trajectory Planning Algorithm was first implemented and validated in Matlab, in order to calibrate the different parameters of the model. For these preliminary tests only the emergency algorithm was used, and not the full framework. The results are presented below.

In these simulations, the ego vehicle was crossing an intersection from south to north and has priority (e.g., green light), while another vehicle crossed from east to west without stopping or slowing down. The simulation started when the other vehicle was detected and the ego vehicle switched to emergency mode to quickly adapt its trajectory and avoid the collision if possible. Figure 17 shows the external potential corresponding to both the roadside and the other moving vehicle.

In the first scenario (Figure 18a), the ego vehicle was able to avoid the collision by accelerating and steering slightly to the left, in order to cross the intersection in front of the other vehicle. After the intersection was passed, it went back to the middle of the road and to its normal speed. In the second scenario (Figure 18b), the ego vehicle slowed down and let the other vehicle pass first to avoid the collision. In the third scenario (Figure 18c, the ego vehicle met a long, slow-moving vehicle (such as a truck) at the intersection. In this situation, it had to brake hard, almost coming to a complete stop, before passing the intersection right behind the other vehicle. Finally, in the fourth scenario (Figure 18d), the intersection was completely blocked and the ego vehicle had to do an emergency stop.

4.2.3. Simulation in Pro-SiVIC

The next phase of testing the Automated Decision-Support Framework was again in the simulation, this time using two interconnected softwares Pro-SiVIC and RTMaps (Intempora).

In the two scenarios presented here, the ego-vehicle (the red car on Figure 19) started in Autonomous mode (using the virtual Co-Pilot presented in Section 3), relatively close to another vehicle (the white car), which did an emergency braking at 1 second, coming to a complete stop about 2 s later. The ego vehicle switched to Emergency mode (using the emergency trajectory planning algorithm presented in Section 4) and tried to avoid the collision. These scenarios were designed to show the limits of the Co-Pilot, and demonstrate how the emergency mode could help in these critical situations.

Figure 20a,b show the trajectory plot for all cars in both scenarios. In the first one (Figure 20a), the left lane was available. Autonomous mode was engaged and the advice was given to change lane and keep the same speed in order to overtake the white vehicle. Unfortunately, the white vehicle applied emergency braking. The only way for the virtual Co-Pilot to avoid the collision was to change lanes and apply a braking maneuver. The two vehicles were too close and a collision seemed to be unavoidable. The emergency mode was activated and an abrupt steering wheel order was given in order to avoid the collision. In the advice matrix, the right column was fully inaccessible. Now only the left lane was green. In the second one (Figure 20b), the autonomous mode was engaged. The advice was given to stay in the same lane at the same speed. The left lane was unavailable due to the presence of a third vehicle (the black vehicle) moving in the opposite way. Unfortunately, the white vehicle applied emergency braking. The red vehicle tried to overtake the white vehicle but could not apply this maneuver due to the black vehicle. The only way was to switch to the emergency mode. The emergency mode advice was to use the emergency lane (or sidewalk) in order to avoid collision with both the black and the white vehicles.

Figure 21 and Figure 22 show the distance between the two cars, the speed of the vehicles 1 and 2, and the steering angle of the ego-vehicle, as a function of time during the simulation, for both scenarios. In all plots (as well as the trajectory plots above), the ego-vehicle mode is indicated by the color: green for Autonomous mode (Co-Pilot), and red for Emergency mode. The first scenario shows the limits of the lateral maneuvers of the Co-Pilot: the steering angle stayed cautious and rarely exceeded 7 degrees in Autonomous mode (see Figure 21), which was not enough given the urgency. In Emergency mode, the steering angle reached up to 20 degrees. Lane changing was normally a maneuver that the Co-Pilot was capable of doing, but not in such a short time interval. As soon as the obstacle was safely passed, the ego-vehicle switched back to Autonomous mode and converged toward a cautious and soft driving behavior. The second scenario aims to demonstrate the emergency algorithm’s flexibility, its ability to ignore traffic rules and laws and come up with creative options that the general Co-Pilot would never consider. Since the two legal lanes were occupied, the Co-Pilot simply could not find a trajectory with no collisions. However, the Emergency mode was able to use the space between the white car and the sidewalk.

In an emergency situation like that, the ability to ignore the lane structure and only focus on the position of the obstacles was an advantage compared to the traditional lane choice and speed profile selection used in the Co-Pilot.

In both of these scenarios, slightly different initial conditions (speed, initial distance, deceleration of the leader vehicle) were tried in order to verify the algorithm’s robustness.

5. Discussion and Conclusions

This paper introduced the Autonomous Decision-Support Framework, developed by Eiffel University for the EU project Trustonomy, and focused on the problem of trajectory planning in emergency situations. Some results of the virtual Co-Pilot were given in a simulation environment (Pro-SiVIC and RTMaps) and embedded in a real prototype. A complementary algorithm specifically designed for emergencies has been proposed and developed in order to propose unusual maneuvers allowing taking into account critical situations. At this moment, this “emergency” system has been developed and tested in simulations, using Matlab and Pro-SiVIC. Its main characteristics are the following.

• It is based on EB’s trajectory generation, for fast computation.
• Unlike in classical methods, the last point of the trajectory is not fixed, but simply attracted to the general direction of motion, to give more flexibility in different emergency situations.
• The current orientation and speed of the vehicle are taken into account using additional ad hoc forces during the trajectory generation, to ensure mechanical feasibility even when obstacles are very close.

This algorithm was used in the Trustonomy framework for risk management, where the decision-support module can switch between the normal Autonomous mode and this new Emergency mode depending on the risk level. Simulation results in a realistic 3D environment showcased the benefits of this hybrid approach in near-accident situations.

However, the simulations presented here have several limitations which will have to be tackled in future works. First, this model has many free parameters, so the calibration step to balance the different forces requires a special attention, to ensure the highest level of safety in any situation. The behavior of the vehicle when the collision is unavoidable (or when the trajectory given returned the algorithm is not feasible) is not clearly defined at the moment. The prediction of the movement of obstacles in dynamic settings is very basic at the moment and could be improved. Finally, interactions and driving sharing with the human driver of the ego-vehicle was not taken into account in the emergency part, although it has been used with Vanholme’s model before, this paper was only concerned with how the vehicle would react in an emergency situation, and not the pair vehicle and driver.

As the Trustonomy project continues in 2021, the next steps of developing an autonomous Decision-Support framework are the following.

• Calibration and testing of the Emergency trajectory planning algorithm will be continued in different scenarios (urban, scenarios with multiple vehicles, pedestrians, etc).
• The current algorithms will be included in the general ADSF framework, which includes a Driver State Monitoring system and a Driver Intervention Performance Assessment (DIPA) module to monitor mode transitions and driver-related risk management at all times.
• The general framework will be tested on a new real vehicle in 2021 in France, possibly using virtual obstacles generated from Pro-SiVIC platform (Vehicle in the Loop architecture with communication means).
• Different perception architectures and perception data will be used in order to assess the capability of the full ADS in adverse and degraded conditions involving possible sensor failures.