# Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants
## 2 Motion-Planning Concept
Assume that some integrand \(z(x)\) is considered within a segment \([x_{i-1},x_{i}]\), then, expressing \(x=\xi D\)  
\[\int_{0}^{D}z(x)dx=D\!\int_{0}^{1}\!z(\mathbf{Q},D,\xi)d\xi\approx D\!\sum_{k=1}^{ N}w_{k}z(\xi(\theta_{k}))det(J(\theta_{k})) \tag{13}\]  
where \(w_{k}=\) integration weight in the \(k\)-th point, \(\theta_{k}\)--\(k\)-th point in the master-element coordinate system, \(J=\) Jacobian, \(k\in[1,N]\), and \(N=\) number of integration points. For one-dimensional FE  
\[\xi(\theta_{k})=\frac{(\xi_{1}-\xi_{0})}{2}(\theta_{k}+1)+\xi_{0}=\frac{1}{2} (\theta_{k}+1),\ \ det(J)=\frac{\xi_{1}-\xi_{0}}{2}=\frac{1}{2} \tag{14}\]  
Denoting the vectors,  
\[\mathbf{\vartheta}=\left( \begin{array}{c}\theta_{1}\\ \vdots\\ \dot{\theta}_{N}\end{array} \right)\!,\ \ \mathbf{w}=\left( \begin{array}{c}w_{1}\\ \vdots\\ w_{N}\end{array} \right)\!,\ \mathbf{\xi}=\left( \begin{array}{c}\xi_{1}\\ \vdots\\ \xi_{N}\end{array} \right)^{T}\!,\ \ \mathbf{z}=\left( \begin{array}{c}z_{1}\\ \vdots\\ z_{N}\end{array} \right)^{T} \tag{15}\]  
The short expression for calculating the integral becomes  
\[D\!\int_{0}^{1}\!z(\mathbf{Q},D,\xi)d\xi\approx\frac{D}{2}\mathbf{z}\mathbf{w} \tag{16}\]  
where \(D=\) section length and \(\mathbf{z}=\) vector of integrands of \(1\times N\) size. In most cases, we will need not only final integral values but also intermediate ones corresponding to the points \(x_{i}\) (or \(\xi_{i}\)). That is, the integration must also be conducted on the sections \([x_{i-1},x_{i}]\). \[ \begin{array}{c}\int_{x_{i-1}}^{x_{i}}z(x)dx=D\int_{\xi_{i-1}}^{\xi_{i}}z(D \xi)d\xi=\\ D\Delta\xi_{i}\!\int_{0}^{1}\!z(D\xi(\theta))d\theta=\\ \frac{D}{2}\Delta\xi_{i}\!\sum_{k=1}^{N}z(D\xi_{i}(\theta_{k}))w_{k}=\\ \frac{D}{2}\Delta\xi_{i}\!\sum_{k=1}^{N}w_{k}z(D,\xi_{i,k})=\frac{D}{2}\Delta \xi_{i}z^{T}_{i}\mathbf{w}\end{array}  \tag{17}\]  
If we combine all increments \(\Delta\xi_{i}\) in a row vector \(\Delta\xi\), then the sum of Equation (13) can be obtained as follows  
\[\int_{0}^{D}z(x)dx=\frac{D}{2}\Delta\xi diag\left(\mathbf{z}^{T}\right)\mathbf{w} \tag{18}\]  
**Planning Strategy in the Presence of Obstacles**. Let us consider the AV motion in the presence of other vehicles (Figure 3). We assume, for instance, that AV is moving along the middle (2) lane and may perform three maneuver variants [21]: lane changes to the first (1) and third (3) lanes and following the second lane. In terms of geometric sense, such a rearrangement may be considered the best, which ensures keeping the greatest distance from an obstacle. At the moment of starting the motion planning (1)-(4), AV fixes the midline points of the current lane in such a way to form smooth conjugations (splines) reflecting the road curvature. Then, considering the number of lanes, the longitudinal \(s_{r}\) and transverse \(L\) coordinates, it is possible to build a curvilinear grid of the road segment space relative to which the motion is planned. By using radars, AV can obtain distances to objects and angles of measuring beams. Then, the coordinates of vehicles' initial positions may be calculated relative to the AV's _xy_ coordinate system. To do this, we use the vehicle safety zones (contours) represented by conditional circles. Thus, for a passenger vehicle, three round areas of radius \(r\) relative to the front, middle, and rear vehicle parts are enough to form the safety contour. The positions of the front and back parts' centers are calculated as follows  
\[x_{cp}=x_{c}+h_{p}cos(\phi),\ \ \ y_{cp}=y_{c}+h_{p}sin(\phi),\ \ \ p\in[f,r] \tag{19}\]  
where \(x_{c}\), \(y_{c}\) = coordinates of the vehicle's mass center, \(h_{f}\), \(h_{r}\) = distances to the conditional centers of the vehicle's front and rear parts, respectively, \(\phi\) = yaw angle. Thus, considering the radar measurements, the positions of other vehicles' centers can be estimated, and based on them and the predetermined road segment curvilinear grid, the lateral displacements \(L_{i,0}\) can be found. This helps to determine the corresponding traffic lane for each moving obstacle.