## 2 Trajectory Planning Problem
### System Model  
For trajectory planning purposes, a suitable mathematical description of the dynamics of the vehicle must be used. The modeling solutions that can be found in the literature lie in a trade-off between complexity and descriptivity, ranging from simple kinematic models (which show a good level of accuracy for low-speed cases [17]) to increasingly complex ones, which embed the description of a greater share of physical phenomena [57]. For the purposes of this work, the model presented in [58] was used. The model relies on a few simplifying assumptions, namely, the following:  
* The longitudinal forces affecting the tires are due to brakes and engine torque only;
* The considered vehicle uses front-wheel drive, with an equal distribution of torque between front tires, and the braking torque is, instead, evenly distributed among all four tires;
* Tire aligning torques are neglected;
* Dissipative force is made up of drag, rolling friction, and gravity force. Further assumptions are made in this work:  
* The slope is considered as an uncertain parameter;
* The vehicle forward velocity is constant. Figure 1: Trajectory segments switch. Let us introduce the kinematic description of the trajectory tracking. Through some straightforward geometric consideration, the subsequent kinematic model can be formulated to represent the motion of the vehicle:  
\[ \begin{bmatrix}x_{E}\\ y_{E}\\ \theta_{E}\end{bmatrix} = \begin{bmatrix}\cos(\theta_{E})&0\\ \sin(\theta_{E})&0\\ 0&1\end{bmatrix}\begin{bmatrix}v_{xE}\\ \omega_{E}\end{bmatrix}  \tag{5}\]  
where \(x_{E}\), \(y_{E}\), and \(\theta_{E}\) define the pose of the vehicle in the inertial reference frame (\(E\)), \(v_{xE}\) is the longitudinal forward velocity, and the yaw rate \(\omega_{E}\) is considered positive if it is counterclockwise. For trajectory tracking, Equation (5) is expressed in a local reference frame (\(L\)) centered in \(\{\bar{x}_{E},\bar{y}_{E}\}\) and oriented with angle \(\bar{\theta}_{E}\) see Figure 2.  
Thus, let us define the position of the vehicle in \(L\) through the following rotation:  
\[ \begin{bmatrix}x_{L}\\ y_{L}\\ \theta_{L}\end{bmatrix} = \begin{bmatrix}\cos(\bar{\theta}_{E})&\sin(\bar{\theta} _{E})&0\\ -\sin(\bar{\theta}_{E})&\cos(\bar{\theta}_{E})&0\\ 0&0&1\end{bmatrix} \cdot \begin{bmatrix}x_{E}-\bar{x}_{E}\\ y_{E}-\bar{y}_{E}\\ \theta_{E}-\bar{\theta}_{E}\end{bmatrix}  \tag{6}\]  
where \(x_{L}\), \(y_{L}\), and \(\theta_{L}\) are the pose of center of mass of the vehicle in the local reference frame. Let the desidered pose be \( \begin{bmatrix}x_{L}^{D}&y_{L}^{D}&\theta_{L}^{D}\end{bmatrix} ^{T}\), and the let \(V_{D}\) be the nominal forward velocity associated with a trajectory segment; the tracking error in \(L\), centered in \(\{\bar{x}_{E},\bar{y}_{E}\}\) and oriented according to the angle \(\bar{\theta}_{E}\), becomes the following:  
\[ \begin{bmatrix}\epsilon_{x}\\ \dot{\epsilon}_{y}\\ \dot{\epsilon}_{\theta}\end{bmatrix} = \begin{bmatrix}\cos(\epsilon_{\theta})& 0\\ \sin(\epsilon_{\theta})&0\\ 0&1\end{bmatrix}\begin{bmatrix}v_{x}\\ \omega\end{bmatrix} - \begin{bmatrix}V_{D}\\ 0\\ 0\end{bmatrix}  \tag{7}\]  
where \(e_{x}=x_{L}-x_{L}^{D}\), \(e_{y}=y_{L}-y_{L}^{D}\), and \(e_{\theta}=\theta_{L}-\theta_{L}^{D}\). In [58], the dynamics of the vehicle are described in terms of a nonlinear system whose state vector is the following:  
\[z= \begin{bmatrix}v_{x}&v_{y}&\omega&x&y&\theta\end{bmatrix} ^{T} \tag{8}\]  
and whose input variables are  
\[u= \begin{bmatrix}T&P&\beta\end{bmatrix} ^{T} \tag{9}\]  
Figure 2: Inertial (\(E\)) and local (\(L\)) reference systems. i.e., the torque provided by the motor \(T\), the braking force \(P\), and the steering angle \(\beta\), and the road-tire friction coefficient \(\mu\) is considered as an unknown but bounded external input:  
\[\dot{z}(t)=\tilde{A}(z(t),u(t))z(t)+\tilde{B}(z(t),u(t))u(t)+\tilde{B}_{d}(t) \mu(t) \tag{10}\]  
with \(\tilde{A}\), \(\tilde{B}\), and \(\tilde{B}_{d}\) being matrices of proper dimensions.