## 3 Feasible Trajectory Planning Algorithm  
Let the two nodes \(\mathcal{G}_{S}\) and \(\mathcal{G}_{F}\) represent the starting point \(S\) and end point \(F\) of a trajectory. A straightforward choice to find (if it exists) the shortest trajectory between them is using the classical \(A^{*}\) algorithm, whose output is a sequence of segments connecting \(S\) and \(F\). Such segments are, according to the planning procedure which is used in this work, traversed at a constant forward velocity \(\bar{V}\). The \(A^{*}\) algorithm employs a heuristic function to evaluate the trajectory linking the initial node \(\mathcal{G}_{S}\) to an arbitrary node \(\mathcal{G}_{A}\). This trajectory, denoted by \(cSA\), comprises the sequence of segments connecting \(S\) to \(A\) at a predefined speed. The length of this trajectory is represented as \(l(cSA)\). Using the notation introduced earlier, the heuristic function is defined as follows:  
\[f(c_{SA},T_{AB},\mathcal{G}_{B},\mathcal{G}_{F})=l(c_{SA})+l_{AB}+\|B-F\| \tag{24}\]  
where \(|\cdot|\) represents the Euclidean norm. This function estimates a lower bound on the total length of the path from \(S\) to the final node \(F\), incorporating the segments \(c_{SA}\) and \(T_{AB}\). Based on (24), the solution to the feasible trajectory planning problem is implemented as a recursive algorithm. The process, starting from the initial node \(\mathcal{G}_{S}\) and its associated tracking error \(S_{S}\), is summarized as follows:  
1. Identify and evaluate the adjacent node \(\mathcal{G}_{A}\) with the most favorable heuristic value relative to \(\mathcal{G}_{S}\);
2. Compute the projection of \(S_{S}\) to the end point of the trajectory segment \(T_{SA}\);
3. If condition (22) is satisfied and \(A\) is not the destination node, calculate \(S_{A}\bigoplus\Pi_{SA}\) and repeat the process. The resulting algorithm is described by the following pseudocode (Algorithm 1). ```
1:\(c_{opt}=\varnothing\);
2:\(c_{exp}=\varnothing\);
3:\(dup=false\)
4:\(\mathcal{G}_{A}=\mathcal{G}_{S}\);
5:function trajectoryPlanner(\(\mathcal{G},S_{A},\mathcal{G}_{A},\mathcal{G}_{F},c_{opt},c_{exp},dup\))
6:\(S_{A}\leftarrow\) initial tracking error as defined in (20);
7:\(\mathcal{G}_{A}\leftarrow\) current node;
8:\(c_{opt}\leftarrow\) candidate optimal trajectory;
9:\(c_{exp}\leftarrow\) trajectory being explored;
10:for all arcs \(T_{AB}\) whose origin is \(\mathcal{G}_{A}\), if \(\
eg(\mathcal{G}_{B}\in c_{exp}\wedgedup)\)do
11:if\(f(c_{exp},T_{AB},\mathcal{G}_{B},\mathcal{G}_{F})\) as specified in (24) \(<l(c_{opt})\)then
12: Given \(S_{A}\) find \(S_{n_{AB}}\) according to Appendix A
13:if\(S_{n_{AB}}\bigoplus\Pi\subseteq\Gamma_{0}\)then
14: append \(T_{AB}\) to \(c_{exp}\)
15:if\(\mathcal{G}_{B}\) is the destination node then
16:\(c_{opt}=c_{exp}\)
17:else
18:if\(\mathcal{G}_{B}\in c_{exp}\)then
19:\(dup=true\)
20:endif
21:\(\textsc{trajectoryPlanner}(\mathcal{G},S_{n_{AB}},\mathcal{G}_{B},\mathcal{G}_{F},c_{opt},c_{exp},dup)\)
22:endif
23:endif
24:endif
25:endforreturn\(c_{opt}\)
26:endfunction
```  
**Algorithm 1** The pseudocode for the planning algorithm. It should be noted that only one duplicate node per trajectory is allowed through the \(dup\) flag so that some trajectories, including a loop--which can be useful in practical scenarios--are possible. While the classical \(A^{*}\) algorithm provides an effective foundation for trajectory planning, its heuristic function does not take into account kinodynamic constraints. Its computational complexity in time is, however, in its worst case exponential in the length \(l\) (number of nodes) of the shortest path: \(O(b^{l})\), with \(b\) being the branching factor (i.e., the average number of arcs per node). The heuristics can lower the _practical_ branching factor (i.e., the number of nodes that are actually explored), thus reducing the complexity [60]. Algorithms derived from \(A^{*}\) usually try enhancing the heuristic;among them, the _hybrid_\(A^{*}\) adds kinematics-related penalty factors.