# Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants
## 4 Optimization and Constraints  
### Optimization Criteria  
Note that the criteria for finding an optimal solution can be conditionally divided into dynamic and static. The first ones are generally time (speed) functions. Their common impact is aimed at ensuring trajectory smoothness and maneuver rapidity. The static criteria, in turn, are tied to parameters of the vehicle's final position, being responsible for the AV's disposition on the lane and the course direction. Let us consider the criteria in more detail. The _longitudinal speed deviation_ relative to a preset upper-level \(V_{\zeta U}\) may be the main \"moving\" factor of the AV model. The integral of the squared velocity deviation has the form  
\[ \begin{array}{l}I_{V}=\int_{0}^{t_{f}}\big{(}V_{\zeta U}-V_{\zeta}\big{)}^{2} dt=\int_{0}^{s}\big{(}V_{\zeta U}-V_{\zeta}\big{)}^{2}\frac{ds}{\
u_{x}(x)}=D \int_{0}^{1}\big{(}V_{\zeta U}-V_{\zeta}\big{)}^{2}\frac{d\xi}{\
u_{x}(x)}\\ \end{array}  \tag{58}\]  
The integrand \(z_{V}(\xi)\) is expressed through the speed nodal parameters \(\mathbf{Q}_{sp}\)  
\[z_{V}(\xi)=\Big{(}V_{\zeta U}-V_{\zeta}\Big{(}\mathbf{Q}_{sp},D,\xi\Big{)}\Big{)}^{ 2}/V_{x}\Big{(}\mathbf{Q}_{sp},D,\xi\Big{)},\ \ \ z_{V}=z_{V}\Big{(}\mathbf{Q}_{sp},D,\xi\Big{(}\mathbf{\vartheta}^{T}\Big{)}\Big{)} \tag{59}\]  
Then,  
\[I_{V}\approx\frac{D}{2}\mathbf{\Delta}\xi z_{V}\mathbf{w} \tag{60}\]  
The _longitudinal and lateral jerk_ criteria [22] include all the kinematic parameters containing curvature and its derivatives. Thus, these criteria reflect the transient dynamics in two directions. The longitudinal direction is to a greater extent associated with the vehicle traction process, and the lateral direction is generally related to the steering control. The common approach is as follows  
\[I_{jp}=\int_{0}^{t_{f}}j_{p}^{2}dt=\int_{0}^{s}j_{p}^{2}\frac{ds}{V}=\int_{0}^{ D}j_{p}^{2}\frac{dx}{V_{x}(x)}=D\int_{0}^{1}j_{p}^{2}\frac{d\xi}{V_{x}(D_{ \xi}^{x})}=D\int_{0}^{1}z_{jp}(\xi)d\xi,\ \ \ p\in[\zeta,\mu] \tag{61}\]  
The integrand \(z_{jp}(\xi)\) is determined according to Equations (14)-(16) and depends on both trajectory \(\mathbf{Q}_{tr}\) and speed \(\mathbf{Q}_{sp}\) parameters, then,  
\[z_{jp}(\xi)=j_{p}^{2}\Big{(}\mathbf{Q}_{tr},\mathbf{Q}_{sp},D,\xi\Big{)}/V_{x}\Big{(} \mathbf{Q}_{sp},D,\xi\Big{)},\ \ \ \mathbf{z}_{jp}=z_{jp}\Big{(}\mathbf{Q}_{tr},\mathbf{Q}_{sp},D,\xi\Big{(}\mathbf{\vartheta}^{ T}\Big{)}\Big{)} \tag{62}\]  
Then,  
\[I_{jp}\approx\frac{D}{2}\mathbf{\Delta}\xi z_{jp}\mathbf{w} \tag{63}\]  
_Motion time_\(T\) reflects final time costs and may be calculated as follows  
\[I_{t}=\int_{0}^{t_{f}}dt=\int_{0}^{s}\frac{ds}{V(s)}=\int_{0}^{D}\frac{dx}{V _{x}(x)}=D\int_{0}^{1}\frac{d\xi}{V_{x}(D\xi)}=D\int_{0}^{1}z_{T}(\xi)d\xi \tag{64}\]  
According to Equations (10) and (12), the integrand \(z_{T}(\xi)\) and \(T\) value  
\[z_{T}(\xi)=\frac{1}{V_{x}\Big{(}\mathbf{Q}_{sp},D,\xi\Big{)}},\ \ \ z_{T}=z_{T}\Big{(}\mathbf{Q}_{tr},\mathbf{Q}_{sp},D,\xi\Big{(}\mathbf{ \vartheta}^{T}\Big{)}\Big{)},\ \ \ T=I_{T}\approx\frac{D}{2}\mathbf{\Delta}\xi z_{T}\mathbf{w}, \tag{65}\]  
_The final lateral position_\(L_{f}\) should best match the desired lateral displacement \(L_{d}\) to satisfy the safe completion of maneuver and to control distances to the lane boundaries. Then,  
\[I_{L}=\Big{(}L_{f}-L_{d}\Big{)}^{2} \tag{66}\]  
_The final tangent angle_\(\alpha_{f}\) characterizes the vehicle control stability at the end of the maneuver, directly affecting the safety of subsequent motion. The desired value \(\alpha_{d}\) must be defined by the road curve's tangent at the vehicle's final position. Then  
\[I_{\alpha}=\Big{(}\alpha_{f}-\alpha_{d}\Big{)}^{2} \tag{67}\]  
The position \(L_{d}\) and angle \(\alpha_{f}\) depend on the lane to which a maneuver is planned. Distantes to moving obstaclesLet us assume that the distance between AV and a moving obstacle changes continuously.