## 2 Motion-Planning Graph-Based Algorithm  
In this section, the generation of the graph, which is used during the motion-planning process, is presented. Since guaranteeing the comfort requirements is a crucial part of the graph generation, the lateral acceleration must be limited during the trajectory planning. The maximum of the lateral acceleration is determined by the comfort requirements and the dynamical limits of the controlled vehicle. The motion-planning algorithm is based upon a graph-based approach, in which the nodes represent the possible positions of the vehicle. The nodes are interconnected with edges. The goal is to compute the feasible trajectories, and furthermore to find the optimal one. Since the trajectories are represented with the nodes and edges of the graph, the selected positions of the nodes are crucial. The whole graph is projected on the given road segment, and two nodes are connected with an edge if the vehicle can get from one node to another, and an example is presented in Figure 1.  
Figure 1: Trajectories between three points. Using the computed feasible trajectory segments the maximum value of the lateral acceleration can be computed, which characterizes the given segment. A segment is described by an edge between two nodes of the graph. In the following, the calculation of the reference positions is presented in both lateral and longitudinal directions. The longitudinal positions of the nodes are computed as: \(x_{i}=\frac{i\cdot x_{length}}{N},\ i=1\ldots N\) where \(i\) is the \(i^{th}\) layer of the graph and \(x_{length}\) is the length of the prediction horizon and \(N\) gives the number of the layers, which indicates the number of parts the prediction horizon is divided into. This means, in the longitudinal direction, the graph is equidistantly generated. The lateral positions of the nodes are computed for the given longitudinal positions. The calculation of the feasible trajectory is made by a predictive optimization method (see Appendix A). The dynamic lateral bicycle model is used during the trajectory planning. Furthermore, the kinematic longitudinal and lateral model is used in the motion prediction of the surrounding vehicles (see: Appendix A.1). During the computation of the feasible trajectories an edge is defined by the following six parameters: \([x_{initial,j},y_{initial,j},\psi_{initial,j},x_{end,j},\psi_{end,j},\psi_{end,j}]\), where \(x_{initial,j},y_{initial,j}\) are the coordinates of the start point of the \(j\)-th edge, and \(\psi_{initial,j}\) gives the yaw angle of the vehicle. The end point of the given edge is defined by the following triad: \(x_{end,j},y_{end,j},\psi_{end,j}\). This means that the length of the segment in longitudinal direction is \(x_{l,j}=x_{end,j}-x_{initial,j}\) and the length in lateral direction is \(y_{l,j}=y_{end,j}-y_{initial,j}\). The resulted candidate trajectories must take into account the limitations to the lateral distances, which means that the trajectories must satisfy the relation  
\[|R(x_{initial,j},y_{initial,j},\psi_{initial,j},x_{end,j},y_{end,j},\psi_{end,j})- y_{MP}|\leq y_{b}\quad\forall j, \tag{1}\]  
where \(R\) gives the reference vector, which can be computed using the initial and end conditions. In this case, the reference vector for the model predictive optimization is equivalent to the coordinates of the edges. \(y_{b}\) is the bound, which is a design parameter. Constraints can be defined during the quadratic optimization as it is described in Appendix A. In Figure 1 an example can be seen. In this case, three nodes of the graph is presented, and also a feasible trajectory, which is built up by two segments. In order to guarantee the continuity of the trajectory two requirements must be fulfilled:  
1. The longitudinal and lateral position of the trajectory must be the same, at the end point, as the coordinates of the given node: \(x_{initial,j}=x_{end,j-1}\) and \(y_{initial,j}=y_{end,j-1}\) using this, the initial yaw angle can be computed as: \(\psi_{start,j-1}=tan^{-1}(y_{l,j-1}/x_{l,j-1})\).