# Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants
## 4 Optimization and Constraints
### Constraints
It can be derived as follows  
\[V_{\zeta s}=\sqrt{g\varphi_{\mu}cos(\beta)/K} \tag{82}\]  
where \(g=\) gravity acceleration constant, \(\varphi_{\mu}=\) lateral adhesion coefficient. If maximum tire-road adhesion is \(\varphi_{max}\) and the adhesion in the longitudinal direction requires a potential \(\varphi_{\zeta}\), the remaining lateral adhesion potential is  
\[\varphi_{\mu}=\varphi_{max}\sqrt{1-\big{(}\varphi_{\zeta}/\varphi_{max}\big{)} ^{2}} \tag{83}\]The longitudinal \(\varphi_{\zeta}\) may be found through the equation of longitudinal dynamics in the dimensionless form  
\[\varphi_{\zeta}=a_{\zeta}/g+f_{d}+f_{r} \tag{84}\]  
where \(f_{r}=\) coefficient of total rolling resistance, and \(f_{d}=\) specific drag force. The longitudinal speed \(V_{\zeta}\) must not exceed a threshold according to the condition \(V_{\zeta}<V_{\zeta s}\). \(V_{\zeta s}\) may change in a range [\(V_{\zeta smin},\infty\)). Thus, a rapidly saturating function may be applied, for instance, \(\chi(\cdot)=tanh(\cdot)\). Then, the condition similar to Equation (73) is  
\[\int_{0}^{f_{f}}\chi\big{(}V_{\zeta s}\big{)}dt=\int_{0}^{f_{f}}\big{|}\chi \big{(}V_{\zeta s}\big{)}-\chi\big{(}V_{\zeta}\big{)}\big{|}dt+\int_{0}^{t_{f} }\chi\big{(}V_{\zeta}\big{)}dt \tag{85}\]  
Using Equations (74)-(80), it is possible to impose physical restrictions on the AV speed. Then  
\[\mathbf{\psi}_{p}=V_{\zeta},\ \ \ \mathbf{\psi}_{pU}=V_{\zeta s},\ \ \ \mathbf{\psi}_{kL}=0,\ \ \ \mathbf{\varepsilon}_{p}=c_{V\zeta} \tag{86}\]  
A special case concerns the vehicle's traction potential. The vehicle's maximum acceleration strictly depends on design features and decreases with growing speed. Thus, if the vehicle's speed-acceleration characteristic is predefined, the following condition must be satisfied  
\[\int_{V_{\zeta(i)min}}^{V_{\zeta(i)max}}\big{(}a_{\zeta U}-a_{\zeta L}\big{)} dV_{\zeta}=\int_{V_{\zeta(i)min}}^{V_{\zeta(i)max}}\big{|}a_{\zeta U}-a_{ \zeta}\big{|}dV_{\zeta}+\int_{V_{\zeta(i)min}}^{V_{\zeta(i)max}}\big{|}a_{ \zeta}-a_{\zeta L}\big{|}dV_{\zeta} \tag{87}\]  
where \(a_{\zeta U}\), \(a_{\zeta L}=\) upper and lower limit values of acceleration achievable by the vehicle. The general scheme for obtaining integrals is the same as Equations (74)-(80). Then, the dynamic constraint has the form  
\[\mathbf{\psi}_{d}=a_{\zeta},\ \ \ \mathbf{\psi}_{dU}=a_{\zeta U},\ \ \ \mathbf{\psi}_{dL}=a_{\zeta L},\ \ \ \mathbf{c}_{d}=c_{a_{\zeta}^{c}} \tag{88}\]  
**Boundary constraints**. Another type of constraint determines boundary conditions of kinematic parameters. Since the initial (0) values defined in Equation (10) are known or may be evaluated directly, one can require (but not necessarily), for example, that the predicted final (f) values of acceleration and jerk would correspond to desirable values \(A_{\zeta f}\) and \(J_{\zeta f}\). That is,  
\[\mathbf{\psi}_{b}= \begin{pmatrix}a_{\zeta f}\\ j_{\zeta f}\end{pmatrix} ,\ \ \ \mathbf{\psi}_{bf}= \begin{pmatrix}A_{\zeta f}\\ J_{\zeta f}\end{pmatrix} ,\ \ \ \mathbf{c}_{b}=\mathbf{\psi}_{b}-\mathbf{\psi}_{bf}=\mathbf{0} \tag{89}\]  
**Complete Set of Constraints**. Combining all the parameters defined in the kinematic, physical, dynamic, and boundary constraints, the vectors of parameters, limits, and nonlinear constraints are as follows  
\[\mathbf{\psi}= \begin{pmatrix}\mathbf{\psi}_{k}\\ \mathbf{\psi}_{p}\\ \mathbf{\psi}_{d}\\ \mathbf{\psi}_{b}\end{pmatrix} ,\ \ \ \mathbf{\psi}_{U}= \begin{pmatrix}\mathbf{\psi}_{kU}\\ \mathbf{\psi}_{pU}\\ \mathbf{\psi}_{dU}\\ \mathbf{\psi}_{b0}\end{pmatrix} ,\ \ \ \mathbf{\psi}_{L}= \begin{pmatrix}\mathbf{\psi}_{kL}\\ \mathbf{\psi}_{pL}\\ \mathbf{\psi}_{dl}\\ \mathbf{\psi}_{bf}\end{pmatrix} ,\ \ \mathbf{c}_{cq}= \begin{pmatrix}c_{k}\\ c_{p}\\ c_{d}\\ c_{b}\end{pmatrix}  \tag{90}\]  
**Initial conditions**. If the sought-for parameters from the previous cycle are known, they are automatically accepted as the initial ones for the next planning.