## 3 Emergency Trajectory Planning Algorithm
### 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} \tag{1}\]  
\[F_{t,t+\Delta t}^{cont}=k_{contraction}\cdot\left(||r_{t+\Delta t}-r_{t}||-l_{0} \right)\cdot\frac{r_{t+\Delta t}-r_{t}}{||r_{t+\Delta t}-r_{t}||} \tag{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}\cdot\left(r_{t}^{Equilibrium}-r_{t}\right) \tag{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{\left(r_{t+\Delta t}-r_{t-\Delta t}\right)}{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  
Figure 5: Classic Elastic Band. 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_{last}^{internal}=k_{internal}\cdot(2\cdot r_{t-\Delta t}-r_{t-2\cdot\Delta t }-r_{t}) \tag{4}\]  
where \(F_{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.