## 3 Emergency Trajectory Planning Algorithm
### 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}(i)\) applied to a node at iteration \(i\) is given in :  
\[F_{total}(i)=F_{internal}(i)+F_{external}(i) \tag{7}\]  
where \(F_{internal}(i),F_{external}(i)\) 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.  
\[\mathbf{q}(i+1)=-\frac{M_{total}(i)}{F_{total}(i)}+\mathbf{q}(i) \tag{8}\]  
where \(\mathbf{q}(i+1)\) is the positions of trajectory at iteration \(i+1\). \(M_{total}(i)\) 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\). \(F_{total}(i)\) is the forces acting on the trajectory at iteration \(i\), and \(\mathbf{q}(i)\) is the positions of trajectory at iteration \(i\) (see Equation (9))  
\[\begin{split}\mathbf{F}_{total}(i)&=[F^{1}_{total} (i),F^{2}_{total}(i),...,F^{n}_{total}(i)]\\ \mathbf{q}(i)&=[r^{1}(i),r^{2}(i),...,r^{n}(i)]\\ \mathbf{M}_{total}(j,k)(i)&=\frac{\mathrm{d}F_{total}( j)(i)}{\mathrm{d}\mathbf{q}(k)(i)}\end{split} \tag{9}\]  
Figure 9: Potential of boundary and lane.  
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 \(\mathbf{k}\) is chosen to ensure that the new position is in the driving area, and it is determined \(\mathbf{k}\) with the value of 0 can still work in the worst case. In addition, \(\mathbf{k}\) is 1 for the nodes that are already in the driving area.  
\[\mathbf{q}_{new}(i+1)=\mathbf{k}\cdot\mathbf{q}(i+1)+(1-\mathbf{k})\cdot\mathbf{q}(i) \tag{10}\]  
where \(\mathbf{q}_{new}(i+1)\) is the new positions of trajectory in driving area at iterations \(i+1\). \(\mathbf{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.