Algorithm 3 Sampling-based Sparse A* Search Algorithm.

Input: The starting position, the destination position, the search leg length, and the termination condition Output: Coordinates of the waypoints Begin: 1:Empty the open set and the close set; 2:Set the starting waypoint as the first node ${\mathrm{Q}}_1$, add ${\mathrm{Q}}_1$ to the open set ($\mathcal{J}\left({\mathrm{Q}}_1\right)=0$), and initialize the number of the explored node $N_j=1$; 3:repeat 4:   Select the best node ${\mathrm{Q}}_m$ from the open set, move ${\mathrm{Q}}_m$ from the open set to the close set; 5:   if the termination condition is satisfied at ${\mathrm{Q}}_m$ then 6:      Terminate the iteration, then trace back the path up the search tree from ${\mathrm{Q}}_m$ until the starting waypoint is reached (return the traced path); 7:   end if 8:   Expand the search tree from ${\mathrm{Q}}_m$, and obtain the children node set $\left\{{\mathrm{Q}}_m^c\right\}$; 9:   for each child node in $\left\{{\mathrm{Q}}_m^c\right\}$ do 10:      Calculate its distances from the other reached nodes ${\left\{{\mathrm{Q}}_j\right\}}_{j=1,2,\dots ,N_j}$, and select the nearest node ${\mathrm{Q}}_n$ from $\left\{{\mathrm{Q}}_j\right\}$ with minimum distance $r_{min}$; 11:      if $r_{min}<r_c$ then 12:            This is a node that has been reached before; 13:            if $\mathcal{J}\left({\mathrm{Q}}_m^c\right)<\mathcal{J}\left({\mathrm{Q}}_n\right)$ then 14:               Replace ${\mathrm{Q}}_n$ by ${\mathrm{Q}}_m^c$ ; 15:            end if 16:      else 17:            A new node is obtained, $N_j=N_j+1$, and add ${\mathrm{Q}}_m^c$ to the open set as ${\mathrm{Q}}_{N_j}$; 18:      end if 19:   end for 20:until the open set is empty (return failure)