## 2 Problem Formulation and Modeling
### Path Following  
For a controlled nonlinear system of the form below,  
\[\dot{\mathbf{x}}(t)=f(\mathbf{x}(t),\mathbf{u}(t)),\ \ \mathbf{x}(t_{0})= \mathbf{x}_{0} \tag{1}\]  
where \(\mathbf{x}\in\mathcal{X}\subseteq\mathbb{R}^{n}\) and \(\mathbf{u}\in\mathcal{U}\subseteq\mathbb{R}^{m}\) define state and input constraints, the objective of path following is to design a controller, such that the system follows a parametrized reference path [2]. The reference path can be given by Equation (2), where only the movement on the plane is considered.  
\[\mathcal{P}=\{\mathbf{p}\in\mathbb{R}^{2}|\mathbf{p}=\mathbf{p}_{T}(\omega), \forall\omega\in\mathbb{R}^{+}\} \tag{2}\]  
For any given \(\omega\), a local tangential path reference frame \(\{T\}\) centered at \(\mathbf{p}_{T}(\omega)\) can be defined, which is indicated by the subscript \(T\). The relative angle \(\sigma\) between the fixed ground frame \(\{O\}\) and the local reference frame \(\{T\}\) can be calculated with Equation (3).  
\[\sigma_{T}(s)=\text{atan2}(y^{{}^{\prime}}_{T}(\omega),\chi^{{}^{\prime}}_{T}( \omega)) \tag{3}\]  
where the function atan2 is the four-quadrant version of arctan that returns the angle between the positive x-axis and a point \([x_{T},y_{T}]^{T}\) in the Cartesian plane and is positive counterclockwise. It is also evidently seen that the parametrized reference path must be continuously differentiable.  
Given the vehicle's posture \([x(t),y(t),\phi(t)]^{T}\) at time \(t\), the path-following error is given by Equation (4), which is a cross product between two vectors. The control objective is to guarantee that the path-following error converges such that \(\lim\limits_{t\to\infty}e_{p}(t)=0\).  
\[e_{p}(t)=d_{y}\hat{t}_{x}-d_{x}\hat{t}_{y} \tag{4}\]  
where \(\mathbf{d}=(d_{x},d_{y})\) is the tracking error vector and \(\hat{\mathbf{t}}=(\hat{t}_{x},\hat{t}_{y})\) is the unit tangent vector to the reference path at \(\omega(t)\) as defined in Equations (5) and (6), respectively.  
\[\mathbf{d}=(x(t),y(t))-(x_{T}(\omega(t)),y_{T}(\omega(t))) \tag{5}\]  
\[\hat{\mathbf{t}}=\frac{(x^{{}^{\prime}}_{T}(\omega(t)),y^{{}^{\prime}}_{T}( \omega(t)))}{\left\|(x^{{}^{\prime}}_{T}(\omega(t)),y^{{}^{\prime}}_{T}( \omega(t)))\right\|} \tag{6}\]The relative orientation \(\psi(t)\) between the vehicle and the path at time \(t\) is given by Equation (7). It indicates whether the vehicle is moving towards the direction of the path or away from it. In practice, we employ the normalization technique to restrict the value of \(\psi(t)\) within the range of \(-\pi\) to \(\pi\). Normalizing the angle difference is a common technique. Although the trigonometric operations remain the same, constraining the range within this specific interval helps reduce the observation space.  
\[\psi(t) =\phi(t)-\sigma_{T}(\omega(t)) \tag{7}\] \[\psi(t) =\text{atan2}(\sin(\psi(t)),\cos(\psi(t)))\]  
To find the point along the reference path from which to calculate the cross-track error, the point that is nearest to the vehicle is selected [9; 19; 20]. This gives rise to an optimization problem of finding the parameter \(\omega\) that minimizes the distance between the vehicle position and the reference path. We prefer the squared Euclidean distance that is based on the equivalence it holds with the original optimization problem and its computational convenience. The optimization problem can be expressed as follows:  
\[\omega(t)=\operatorname*{arg\,min}_{\omega}\|(x(t),y(t))-(x_{T}(\omega),y_{T} (\omega))\|^{2} \tag{8}\]  
One natural approach for updating the path variable \(\omega\) is to iteratively compute the value that yields the nearest distance between the vehicle and the reference path using [PERSON]'s method [19]. The feature that [PERSON]'s method only guarantees a local optimum helps prevent sudden jumps in the path variable and promotes stability in the optimization process by using the previous path variable value as the initial guess. One can refer to [21] for details.