## 2 Trajectory Planning Problem  
Suppose a two-dimensional operational scenario \(\Delta\subseteq\mathcal{R}^{2}\) that can be discretized by a grid of safe positions of finite size \(\Delta_{D}\). Let us assume the undirected weighted graph \(\mathcal{G}\), whose nodes represent the points in \(\Delta_{D}\).  
Let us consider two generic points \(M=(x_{1},y_{1})\) and \(N=(x_{2},y_{2})\) in \(\Delta_{D}\) that are closer than \(\bar{L}\) and whose connecting segment is fully contained in \(\Delta\). Thus, if \(\mathcal{G}_{M}\) and \(\mathcal{G}_{N}\) are the nodes of \(\mathcal{G}\) associated, respectively, to points \(M\) and \(N\), then \(T_{MN}\) is defined as the arc of the graph \(\mathcal{G}\) that links \(\mathcal{G}_{M}\) and \(\mathcal{G}_{N}\). Moreover, \(T_{MN}\) represents the segment of the trajectory \(MN\) of length \(l_{MN}\) that is traversed at a constant speed \(\bar{V}\). Consider \(n_{MN}=\left\lceil\frac{l_{MN}}{\bar{V}\,T_{S}}\right\rceil\), i.e., the rounded number of time steps associated with the \(T_{MN}\) trajectory segment, and let \(l_{MN}\) be the weight of the arc \(T_{MN}\). Hence, we have the following definition:  
\[\mathcal{G}=\big{(}\{\mathcal{G}_{A},\ldots,\mathcal{G}_{N},N\in\Delta_{D}\}, \big{\{}T_{AB},T_{AC},\ldots,T_{MN},\overline{MN}\in\Delta\big{\}}\big{)} \tag{1}\]  
Let the nodes \(\mathcal{G}_{A},\mathcal{G}_{B},\mathcal{G}_{C}\) be associated with the points \(A,B,C\in\Delta_{D}\). And assume that the two arcs \(T_{AB}\) and \(T_{BC}\) (associated with segments \(\overline{AB}\) and \(\overline{BC}\)) both exist and are part of a trajectory, as shown in Figure 1.  
Consider the intervals \([t_{A},t_{A}+n_{AB}\cdot T_{s}]\) and \((t_{B},t_{B}+n_{BC}\cdot T_{s}]\), which represent the time frames during which the vehicle traverses the trajectory segments \(AB\) and \(BC\), respectively. Since these segments are contiguous, the following temporal relationship holds:  
\[t_{B}=t_{A}+n_{AB}\cdot T_{S} \tag{2}\]  
At the moment \(t_{B}\), the vehicle's position is \((x(t_{B}),y(t_{B}))\), and its orientation is \(\theta(t_{B})\). These quantities are expressed in a local reference frame centered at point \(A\), where segment \(AB\) is aligned with the \(x\) axis. At \(t_{B}\), a new local reference frame is established at point \(B\)which is oriented according to \(\theta_{BC}\). Given that the segment \(BC\) with orientation \(\theta_{BC}\) must be traversed at velocity \(\bar{V}\), the state of (19) at \(t_{B}\) undergoes the following instantaneous update:  
\[\Pi=[x_{D}(t_{B})-x_{B},\,0,\,-\theta_{BC}]^{T} \tag{3}\]  
where \(x_{D}(t_{B})\) denotes the desired position along segment \(AB\) at time \(t_{B}\). Refer to Figure 1 for additional details.  
Let us thus introduce the following condition:  
\[\delta_{z}(t_{B})+\Pi\in\Gamma_{0} \tag{4}\]  
with \(\delta z(t_{B})\) being the state of the system at the time instant \(t_{B}\) and \(\Gamma_{0}\) being the robust positively invariant region [56] associated with its dynamics. If (4) is held, the sequence of trajectory segments corresponding to arcs \(T_{AB}\) and \(T_{BC}\) is deemed admissible, enabling the evaluation of another candidate trajectory segment.