# Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants
## 4 Optimization and Constraints
### Constraints  
**General approach**. Kinematic, dynamic, and physical parameters of vehicle motion may be restricted. Usually, parameters' values are constrained in the control points. Nevertheless, this does not guarantee the non-violation of limits between these points by a considered function. If it is supposed that all the parameters are represented by smooth functions, the integral approach to constraints may be implemented based on the same Gaussian scheme applied for the cost function calculation. Let us consider the general technique for some parameter \(\psi\), which must not exceed the upper \(\psi_{U}\) and lower \(\psi_{L}\) bounds. Then, the sum of the areas between the upper limit and the \(\psi\)-function and between the lower limit and the \(\psi\)-function must be strictly equal to the area within limits. That is, if the \(\psi\)-function is determined relative to the time  
\[\int_{0}^{t_{f}}\left(\psi u-\psi_{L}\right)dt=\int_{0}^{t_{f}}|\psi u-\psi|dt+ \int_{0}^{t_{f}}|\psi-\psi_{L}|dt \tag{73}\]Each integral can be considered concerning the spatial coordinate by replacing the time differential. Integral between upper and lower bounds (\(ul\)):  
\[\int_{0}^{t_{f}}\left(\psi_{U}-\psi_{L}\right)dt=\int_{0}^{s_{f}}\left(\psi_{U}- \psi_{L}\right)\frac{ds}{V(s)}=D\int_{0}^{1}\frac{\left(\psi_{U}-\psi_{L} \right)}{V_{x}(D\xi)}d\xi=D\int_{0}^{1}z_{\psi}^{(ul)}(\xi)d\xi \tag{74}\]  
Similar to Equations (16)-(18),  
\[z_{\psi}^{(ul)}(\xi)=\frac{\left(\psi_{U}-\psi_{L}\right)}{V_{x}(D\xi)},\ \ \ z_{\psi}^{(ul)}=z_{\psi}^{(ul)}\Big{(}\xi\Big{(}\mathbf{\vartheta}^{T}\Big{)} \Big{)},\ \ \ I_{\psi}^{(ul)}\approx\frac{D}{2}\mathbf{\Delta}\xi z_{\psi j}^{(ul)}\mathbf{w}, \tag{75}\]  
Integral between the upper bound and \(\psi\)-function (\(uf\)):  
\[\int_{0}^{t_{f}}\left|\psi_{U}-\psi\right|dt=\int_{0}^{s_{f}}\left|\psi_{U}- \psi\right|\frac{ds}{V(s)}=D\int_{0}^{1}\frac{\left|\psi_{U}-\psi\right|}{V_{x }(D\xi)}d\xi=D\int_{0}^{1}z_{\psi}^{(uf)}(\xi)d\xi \tag{76}\]  
Thus,  
\[z_{\psi}^{(uf)}(\xi)=\frac{\left|\psi_{U}-\psi\right|}{V_{x}(D\xi)},\ \ \ z_{\psi}^{(uf)}=z_{\psi}^{(uf)}\Big{(}\xi\Big{(}\mathbf{ \vartheta}^{T}\Big{)}\Big{)},\ \ \ I_{\psi}^{(uf)}\approx\frac{D}{2}\mathbf{\Delta}\xi z_{\psi}^{(uf)}\mathbf{w}, \tag{77}\]  
Integral between the \(\psi\)-function and lower bound (\(fl\)):  
\[\int_{0}^{t_{f}}\left|\psi-\psi_{L}\right|dt=\int_{0}^{s_{f}}\left|\psi-\psi_{ L}\right|\frac{ds}{V(s)}=D\int_{0}^{1}\frac{\left|\psi-\psi_{L}\right|}{V_{x}(D \xi)}d\xi=D\int_{0}^{1}z_{\psi}^{(fl)}(\xi)d\xi \tag{78}\]  
Thus,  
\[z_{\psi}^{(fl)}(\xi)=\frac{\left|\psi-\psi_{L}\right|}{V_{x}(D\xi)},\ \ \ z_{\psi}^{(fl)}=z_{\psi}^{(fl)}\Big{(}\xi\Big{(}\mathbf{ \vartheta}^{T}\Big{)}\Big{)},\ \ \ I_{\psi}^{(fl)}\approx\frac{D}{2}\mathbf{\Delta}\xi z_{\psi}^{(fl)}\mathbf{w}, \tag{79}\]  
The requirement of nonlinear equality constraint within the segment \(D\) is expressed as follows  
\[c_{\psi}=\mathbf{I}_{\psi}^{(ul)}-\mathbf{I}_{\psi}^{(uf)}-\mathbf{I}_{\psi}^{(fl)}=0 \tag{80}\]  
**Kinematic constraints**. Using the scheme above, we may compose a vector \(c_{k}\) of nonlinear integral constraints for a set of kinematic parameters \(\mathbf{\psi}_{k}\), considering vectors of upper \(\mathbf{\psi}_{kl}\) and lower \(\mathbf{\psi}_{kl}\) limits. \[\mathbf{\psi}_{k}= \begin{pmatrix}V_{\zeta}\\ \omega\\ \varepsilon\\ j_{\zeta}\end{pmatrix} ,\ \ \ \mathbf{\psi}_{kl}= \begin{pmatrix}V_{\zeta U}\\ \omega_{U}\\ \epsilon_{U}\\ j_{\zeta U}\end{pmatrix} ,\ \ \ \mathbf{\psi}_{kl}= \begin{pmatrix}V_{\zeta L}\\ \omega_{L}\\ \epsilon_{L}\\ j_{\zeta L}\end{pmatrix} ,\ \ \ \epsilon_{k}= \begin{pmatrix}c_{V\zeta}\\ c_{\omega}\\ c_{\varepsilon}\\ c_{\zeta\zeta}\end{pmatrix}  \tag{81}\]  
**Physical constraints**. Subject to a relatively equal distribution of longitudinal and transverse forces between the AV wheels, the critical speed \(V_{\zeta s}\) criterion may be used stipulated by limiting the stable motion depending on the trajectory's curvature [21] and tire-road adhesion.