## 3 Initial Waypoint Identification  
Generally speaking, robots have a limited sensing range. Thus, planning a trajectory out of such a sensing range would be counterproductive. Hence, local trajectory planning and refinement when a robot moves is the optimal choice. With the help of sensing capabilities within the robots' sensing range, the robot's surrounded environment can be constructed as the intersection of three separate disjoint sets: free-known (\(C_{free}\)), occupied (\(C_{obs}\)), and unknown (\(C_{unknown}\)). Once \(C_{free}\cup C_{unknown}\) is identified, a set of intermediate waypoints is needed to navigate the robot along the trajectory from the start position to the desired position. There are various techniques for finding a set of intermediate waypoints: sampling-based techniques (e.g., RRT*, probabilistic road map (PRM)) and path-searching techniques (e.g., A*, D*, JPS), where waypoint poses are given in the UTM (universal transverse mercator) coordinate system and then converted into the local coordinate system. Moreover, kinodynamic properties are incorporated into preceding intermediate waypoints finding techniques to ensure the dynamic feasibility of the robot. One of the first kinodynamic-based path planning approaches was proposed in [66], in which a variant of the A* method alongside with kinodynamic properties was applied to ensure the dynamic feasibility. Subsequently, several different methods were proposed for enhancing path planning, ensuring the dynamic feasibility by kinodynamic properties, including motion-primitive-based approaches. Motion-primitive-based approaches [42; 67; 68] can be utilized for finding intermediate waypoints and for trxajcjectory generation. [PERSON] et al. [69] proposed a set of motion primitives for connecting edges of the graph that was constructed from A*. In this method, motion primitives were used to defining state vector \(\mathbf{x}(t)\) and control input \(\mathbf{u}(t)\) as a linear time invariant (LTI) system as follows:  
\[ \begin{split}\mathbf{x}_{i}(t)=[p_{i}(t)^{\top},p_{i}(t)^{\top},\ldots,p_{i}^{(k_{r}-1)}(t)^{\top}]^{\top}\in\mathbf{x}_{i}(t)\subset \mathbb{R}^{3\times k_{r}},\\ p_{i}(t)=[p_{x}(t),p_{y}(t),p_{z}(t)]^{T},\mathbf{u}_{i}(t)=p^{(k_ {r})}(t),\end{split}  \tag{6}\]where \(\mathbf{p}_{h}(t)=\sum_{j=0}^{d}\lambda_{j}t^{j},\ \mu\in\{x,y,z\}\), which is formulated similar to (16), while \(k_{r}\) and \(d\) are the order of the derivative and the order of the polynomial, respectively. \[\dot{\mathbf{x}}_{i}(t)=A\mathbf{x}_{i}(t)+B\mathbf{u}_{i}(t),\] \[A= \begin{bmatrix}0&I_{3}&0&\cdots&0\\ 0&0&I_{3}&\cdots&0\\ \vdots&\vdots&\vdots&\ddots&\vdots\\ 0&\ldots&\cdots&0&I_{3}\\ 0&\ldots&\cdots&0&0\end{bmatrix} ,\quad B= \begin{bmatrix}0\\ 0\\ \vdots\\ 0\\ I_{3}\end{bmatrix} . \tag{7}\]  
Hence, given control policy \(\mathbf{u}_{i}(t)\) and initial state \(\mathbf{x}(0)\), a sequence of succeeding states for a given time duration is determined by:  
\[\mathbf{x}_{i}(t)=e^{At}\mathbf{x}(0)+\int_{0}^{t}e^{A(t-\gamma)}B\mathbf{u}( \gamma)d\gamma, \tag{8}\]  
where \(\gamma\) is the time duration that control policy is applied. In [69], to define the actual and heuristic cost of A*, the researchers used motion primitives, which are defined (as shown) in (8), and calculated initial waypoints set. Another interesting approach to finding a set of initial intermediate waypoints is by using fast marching methods. In general, fast marching methods [70] are applied to track the propagation of a convoluted interface such as wavefront, especially in image processing. Let \(\varphi\) be a close curve in a plane \(\in\mathbb{R}^{3}\) that propagates orthogonally to the plane with a speed \(v(\mathbf{p})\), assume \(v>0\). Given \(\bigtriangledown T\) time period, propagation of the plane can be described by \(|\bigtriangledown T(x)|=\frac{1}{v(\mathbf{p})}\) based on Eikonal partial differential Equation [71], where \(\mathbf{p}\) is the position in \(\mathbb{R}^{3}\) and the arrival time is formulated by \(T(x)\).