POI-Guided Heuristic Mapping for UAV Motion Planning with Bounded Distance Updates

Highlights

What are the main findings?

• A sequential motion-planning framework is proposed for UAV obstacle avoidance that exploits a compact set of trajectory-relevant obstacles, enabling safe trajectory refinement beyond the explicitly updated distance band under bounded distance-field updates.
• A heuristic mapping mechanism guided by Points of Interest (POIs), defined as trajectory-relevant obstacles, is developed to tightly couple long-term occupancy mapping, POI-based short-term distance approximation, and sequential trajectory optimization, thereby improving obstacle clearance without requiring global distance updates.

What are the implications of the main findings?

• The study shows that trajectory-relevant obstacles can be exploited as a compact and effective representation for local map refinement, improving the safety of gradient-based planning under limited onboard resources.
• The proposed framework provides a practical pipeline toward real-time, safety-oriented UAV navigation in unknown and cluttered environments, with potential value for onboard deployment in resource-constrained platforms.

Abstract

Safety-oriented UAV motion planning relies on distance-to-obstacle fields and their gradients, yet onboard mapping is typically limited to bounded local distance updates. Consequently, optimization may stall outside the updated band due to missing gradients, while enlarging the update range substantially increases computational cost. Our key insight is that motion-planning locality implies only a small subset of obstacles governs local trajectory refinement. We term this subset points of interest (POIs). Motivated by this observation, we develop a locality-aware sequential motion planning framework with a POI-driven feedback mechanism that continuously identifies and augments these trajectory-relevant obstacles during search and optimization. The mechanism tightly couples mapping, search, and optimization and enables safe trajectory refinement without requiring global distance updates. The framework adopts a heuristic mapping strategy that combines a long-term occupancy grid with bounded incremental distance updates and a POI-based short-term k-d tree, enabling efficient nearest-neighbor queries and gradient proxies beyond the update band. The search process generates a dynamically feasible initial trajectory in the long-term map while collecting POIs, which are then used to construct the short-term component. The trajectory is subsequently refined through iterative optimization loops, where newly exposed closest obstacles are incorporated into the POI set and the short-term map is updated until convergence. Safety is enforced through conservative collision checking against the inflated long-term occupancy map. Simulations in building and forest environments show that 99.7% of trials converge within two refinements in sparse scenes and none exceed four overall. Compared with FastPlanner and EgoPlanner, the proposed method achieves consistently larger obstacle clearances. Onboard experiments further validate its practicality under real sensing and computational constraints.

1. Introduction

The increasing autonomy of unmanned aerial vehicles (UAVs) has substantially improved operational efficiency in applications such as geographic surveying [1,2], agricultural production [3,4], and disaster rescue [5,6]. In these missions, motion planning is a key capability that generates collision-free trajectories from sequential sensor observations in unknown, obstacle-rich environments [7,8]. Despite significant progress, safety-oriented planning in large-scale exploration remains challenging [9], largely because the planner’s effectiveness depends on how efficiently distance-to-obstacle information can be produced and queried under onboard sensing and computational constraints.

A large class of planners steers trajectories away from obstacles by leveraging distance values and their gradients, which typically requires precomputing spatial distance information via grid-based distance fields or point-cloud representations [10,11,12,13]. However, in practical onboard systems, depth and pose measurements only support local and incremental map updates, and the update range must be bounded to meet real-time requirements. This leads to a fundamental dilemma: regions outside the updated distance band provide no reliable distance gradients, causing gradient-based safety optimization to stall once gradients vanish; meanwhile, expanding the distance-update range to cover a larger workspace increases computational complexity dramatically and can overwhelm embedded resources, undermining real-time performance [14,15]. This paper, therefore, investigates the planning–mapping interplay and asks how to maintain safety margins without paying the full cost of global distance updates.

Motivated by the locality property of motion planning, we propose a heuristic mapping strategy that enhances trajectory safety while minimizing explicit distance-field updates. Locality suggests that once an initial feasible trajectory is obtained, subsequent refinements typically remain in a neighborhood of that trajectory [16], and only a limited number of obstacles play a dominant role in shaping the final path. Based on this observation, we introduce Points of Interest (POIs) as a compact representation of trajectory-relevant obstacles. The key idea is to exploit POIs discovered during search and optimization to provide actionable distance/gradient cues beyond the long-term map’s explicit update band, thereby enabling safe refinement even where the long-term map does not maintain full distance gradients.

Building on this idea, we develop a sequential motion-planning framework that leverages heuristic maps for safe and flexible navigation. The framework contains two tightly coupled components: heuristic mapping and sequential planning. For heuristic mapping, a long-term map is maintained as an occupancy grid with incremental updates, storing occupancy, closest occupied voxel indices, and distance information within a bounded update range [17]. A short-term map is then constructed from POIs, organized by a k-d tree, to approximate distance and gradient information outside the long-term update band. For sequential planning, an improved hybrid A* generates an initial dynamically feasible trajectory while recording POIs encountered by motion primitives. The trajectory is subsequently optimized in the combined heuristic map. If the refined trajectory reveals new closest obstacles, POIs are augmented and the short-term map is rebuilt; the optimization continues until no additional POIs are detected. This POI-driven refinement establishes an explicit information exchange among mapping, search, and optimization, enabling the planner to improve clearance even in regions lacking explicit distance gradients.

The main contributions are summarized as follows:

1.

We develop a POI model that tracks trajectory-relevant obstacles and establishes an information exchange mechanism among mapping, search, and optimization, exploiting motion-planning locality to improve safety.

2.

We propose a heuristic mapping strategy that integrates a long-term occupancy/distance-update band with a POI-based short-term distance proxy, providing efficient distance and gradient estimation beyond the explicit update range with minimal additional cost.

3.

We design a sequential planning method that iteratively refines both the trajectory and the heuristic map until convergence, yielding improved clearance under limited distance-update resources, as validated in both simulated and onboard experiments.

The remainder of the paper is organized as follows. Section 2 reviews related work. Section 3 presents the system architecture. Section 4 reports simulation and onboard results. Section 5 concludes the paper.

2. Related Work

Two primary strategies have emerged to enhance motion planning safety in recent years. The first focuses on improving mapping efficiency, particularly in updating distance information. The maps for motion planning are dense, which enables rapid queries for occupancy and distance data across the workspace. After constructing occupancy grid maps, batch-type techniques use parabola sample functions to perform distance transformation, achieving linear-time complexity [14,15,18]. However, these methods linearly depend on the number of voxels, and as map coverage expands, the number of voxels (n) grows cubically, leading to a significant time-consuming bottleneck. To tackle this, incremental-type algorithms motivated by locality compute only the neighborhoods of voxels where the occupancy probability has inverted [13,19,20,21]. However, these algorithms still face challenges, including instability and too much runtime under initial or other extreme conditions. Additionally, the k-d tree data structure is also utilized in mapping [22,23,24]. The challenges of the k-d tree include dynamically acquiring all obstacle points and sensitivity to depth noise. Hence, we adopt occupancy grid maps and incremental-type algorithms for long-term mapping. In practice, a maximum update distance threshold is used to balance performance and functionality.

The second strategy aims to address these issues by avoiding the direct construction of distance fields. The flight corridor is introduced to make the trajectory sampling points stay inside for collision avoidance [25,26]. Spherical or polyhedral flight corridors are extracted in occupancy grid maps directly [27]. However, these methods often struggle to accurately control trajectory locations due to the absence of real distance measurements, which can lead to paths being too close to obstacles. Recently, end-to-end learning methods have utilized reinforcement learning to plan directly on depth images [28,29,30]. These approaches require extensive simulated training scenarios, which may pose challenges in terms of low cost, rapid deployment, and adaptability. The EgoPlanner [31,32] has introduced a distance field approximation strategy that pairs trajectory points (s) with obstacle points (v), using the gradient from s to v to replace the real distance, showing robust performance. However, if the initial path does not touch the occupied voxels, there are no distance estimations. EgoPlanner risks having trajectory points too close to obstacle edges and separates mapping from optimization, missing potential synergies. In contrast, this paper introduces a POI model that integrates feedback from the search and optimization into the mapping, fostering a seamless integration between mapping and planning.

3. Algorithms and the Pipeline

3.1. Overview of the Proposed Methods

An overview of the proposed framework is depicted in Figure 1, which comprises three tightly coupled components: heuristic mapping, path search, and path optimization. First, the heuristic mapping component fuses depth measurements and pose estimates into a long-term occupancy grid map, which maintains occupancy states, closest occupied voxel indices, and bounded incremental distance updates around obstacles. Based on trajectory-relevant obstacles identified during planning, a short-term component is further constructed from POIs organized in a k-d tree. Together, these two representations form the heuristic map, where the long-term map provides reliable collision information and locally accurate distance cues, while the short-term component supplies efficient nearest-neighbor queries and gradient proxies beyond the explicit distance-update band. Next, the path search component employs an improved hybrid A* to search within the long-term map and generate a dynamically feasible initial trajectory from the start state to the goal state. During this process, obstacle voxels approached by the motion primitives are recorded as POIs, which are then used to initialize the short-term component of the heuristic map. Finally, the path optimization component refines the initial trajectory in the combined heuristic map through gradient-based optimization. During optimization, newly exposed closest obstacles along the updated trajectory are appended to the POI set, and the short-term component is rebuilt accordingly. This establishes a closed feedback loop among heuristic mapping, path search, and path optimization: the search stage initializes the POI set, the short-term component supplements missing distance cues outside the long-term update band, and the optimizer further augments the POI set when additional trajectory-relevant obstacles are discovered. The refinement proceeds iteratively until no new POIs are detected or the trajectory converges. To ensure safety, all candidate trajectories are conservatively validated against the inflated long-term occupancy map rather than relying solely on the POI-based proxy. Once convergence is reached, the optimized trajectory is passed to the path tracking controller for execution on the UAV.

The overall pipeline of the method is depicted in the Algorithm 1. Heuristic mapping is implemented by LongTermMapBuilding() and ShortTermMapBuilding() discussed in Section 3.2. Map refining is also implemented by ShortTermMapBuilding() discussed in Section 3.3.2. PathSearching() and PathOptimizing() are responsible for Path Search and Path Optimization, discussed in Section 3.3.1 and Section 3.3.3.

Algorithm 1: Sequential Motion Planning Leveraging Heuristic Maps.

3.2. Heuristic Mapping

3.2.1. Heuristic Mapping Model

The proposed heuristic map is composed of a persistent long-term map and an adaptive short-term map, aiming to provide efficient occupancy queries and actionable distance/gradient information for trajectory optimization in large-scale, partially observed environments.

Let the workspace be discretized into voxels with resolution r. Here, $\mathbf{x}\in {\mathbb{R}}^3$ denotes a continuous query position in the workspace. We denote by $O\subset {\mathbb{R}}^3$ the set of coordinates of occupied voxel centers maintained by the long-term map. In practice, only a small subset of these occupied voxels exerts dominant influence on the locally optimized trajectory; we denote this subset as the set of points of interest (POIs) $I\subseteq O$, where each $\mathbf{i}\in I$ is the coordinate of a POI voxel center. The short-term map organizes POIs in a k-d tree to support fast nearest-neighbor queries beyond the long-term update band.

Long-Term Map

Given point-cloud and pose measurements, the long-term map maintains (i) occupancy via raycasting and Bayesian fusion [33], and (ii) local distance and closest occupied voxel index $coc\left(\mathbf{x}\right)$ via incremental updates [17]. To control runtime, distance updates are restricted to a finite update band around obstacles. We denote by $d_L\left(\mathbf{x}\right)$ the distance value provided by the long-term map when $\mathbf{x}$ lies inside this band (i.e., $d_L\left(\mathbf{x}\right)\le d_{map}$), and treat $d_L\left(\mathbf{x}\right)$ as unavailable otherwise.

Short-Term Map

For states outside the long-term update band, we define a proxy distance field using POIs:

$$d_S\left(\mathbf{x}\right)=\underset{\mathbf{i}\in I}{min}\parallel \mathbf{x}-\mathbf{i}\parallel .$$

(1)

POIs are collected during both search and optimization: during search, motion primitives that approach obstacles (as indicated by $d_L(·)$ and $coc(·)$) contribute obstacle voxels to I; during optimization, newly exposed closest obstacles along the refined trajectory are appended to I, triggering a short-term map rebuild.

Heuristic Distance Fusion

The distance used by the optimizer is computed by combining the accurate local distance from the long-term map and the POI-based proxy from the short-term map:

$$\widehat{d}\left(\mathbf{x}\right)=\left\{\begin{array}{cc}{d}_L\left(\mathbf{x}\right),\hfill & d_L\left(\mathbf{x}\right)\le d_{map},\hfill \\ d_S\left(\mathbf{x}\right),\hfill & \mathrm{otherwise}.\hfill \end{array}\right.$$

(2)

The gradient $\nabla \widehat{d}\left(\mathbf{x}\right)$ is obtained via finite differences. Since $\widehat{d}\left(\mathbf{x}\right)$ is piecewise-defined, its gradient may be non-smooth near the interface. In implementation, we employ a narrow blending band of width w around $d_{map}$ to obtain a continuous surrogate,

$$\widehat{d}\left(\mathbf{x}\right)=\alpha \left(\mathbf{x}\right)d_L\left(\mathbf{x}\right)+\left(1-\alpha \left(\mathbf{x}\right)\right)d_S\left(\mathbf{x}\right),\alpha \left(\mathbf{x}\right)=\mathrm{clip}\left({\displaystyle \frac{d_{map}-d_L\left(\mathbf{x}\right)}{w}},0,1\right),$$

(3)

which empirically improves optimizer stability while preserving the locality advantage.

Safety Enforcement

It is important to note that $d_S\left(\mathbf{x}\right)$ is a heuristic proxy constructed from a subset of obstacles, and therefore, it is not, by itself, a conservative certificate of clearance in unobserved regions. To ensure safety, all candidate trajectories are additionally validated against the long-term occupancy (with inflation) within the current sensing/update horizon; whenever the refined trajectory enters regions where the long-term map provides new obstacle evidence, the corresponding obstacles are added to I and the heuristic map is refined accordingly.

3.2.2. Heuristic Mapping Refinement and Termination

The refinement process is driven by the monotonic growth of the POI set. Let the true Euclidean distance to the full occupied set be

$$d_O\left(\mathbf{x}\right)=\underset{\mathbf{o}\in O}{min}\parallel \mathbf{x}-\mathbf{o}\parallel .$$

(4)

Because $I\subseteq O$, the POI-based proxy satisfies

$$d_S\left(\mathbf{x}\right)=\underset{\mathbf{i}\in I}{min}\parallel \mathbf{x}-\mathbf{i}\parallel \ge \underset{\mathbf{o}\in O}{min}\parallel \mathbf{x}-\mathbf{o}\parallel =d_O\left(\mathbf{x}\right),$$

(5)

i.e., $d_S\left(\mathbf{x}\right)$ may overestimate clearance when some obstacles are missing from I. Importantly, as new POIs are appended, $d_S\left(\mathbf{x}\right)$ decreases monotonically and converges to $d_O\left(\mathbf{x}\right)$ in the limiting case $I=O$. Therefore, the heuristic map becomes progressively more accurate in the neighborhood explored by the planner.

Termination is ensured in practice by motion-planning locality. The initial trajectory produced by the search phase already discovers most POIs that shape the corridor of feasible solutions. Subsequent trajectory optimizations are local updates around this corridor, and thus typically expose only a small number of additional POIs. Each refinement iteration strictly increases $\left|I\right|$ unless convergence is reached (no new POIs), yielding a bounded number of short-term map rebuilds, which is consistent with our empirical results.

3.3. Sequential Motion Planning

In this paper, motion planning is modeled as a nonlinear optimization problem with inequality constraints, following the general formulation in [14]. Under the differential-flatness assumption for quadrotors, with the yaw angle aligned with the trajectory tangent, the optimization variable is a polynomial trajectory $\mathbf{x}\left(t\right)\in {\mathbb{R}}^3$ for $t\in [0,1]$. The objective function combines collision and smoothness costs, while the inequality constraints enforce dynamic feasibility through bounds on the UAV velocity and acceleration [25]. The specific optimization model is formulated as follows:

$$\begin{array}{c}\hfill \begin{array}{cc}min\hfill & F={\lambda }_s{f}_s+{\lambda }_o{f}_o\hfill \\ \hfill & ={\lambda }_s{\int }_0^1{\Vert {\mathbf{x}}^{\left(3\right)}\left(t\right)\Vert }^{2}dt+{\lambda }_o{\int }_0^1\varphi \left(d\left(\mathbf{x}\left(t\right)\right)\right)dt\hfill \\ \mathrm{s}.\mathrm{t}.\hfill & \Vert \mathbf{v}\Vert <v_{max}\hfill \\ \hfill & \Vert \mathbf{a}\Vert <a_{max}\hfill \end{array}\end{array}$$

(6)

where the parameters ${\lambda }_s$ and ${\lambda }_o$ represent the weights for smoothness and safety, respectively. The smoothness term $f_s$ describes the energy consumed by the trajectory, while the safety term $f_o$ imposes a penalty based on the distance between the trajectory and obstacles. In general, the penalty function can be defined as $\varphi \left(d\right)=\left\{\begin{array}{cc}{\left(d-d_{opt}\right)}^2& d\le d_{opt}\\ 0& d>d_{opt}\end{array}\right.$. And $d_{opt}$ is the optimization minimum distance parameter.

We will discuss the initial trajectory generation and trajectory optimization separately.

3.3.1. Path Search Using an Improved Hybrid A*

The proposed improved hybrid A* algorithm extends the kinodynamic A* approach in [14]. In addition to generating a feasible initial trajectory that satisfies the UAV’s dynamic constraints, the enhanced search process explicitly records the POIs encountered during the search, as illustrated in Figure 2 and detailed in Algorithm 2. The primary objective of the improved hybrid A* is to produce a sequence of motion primitives that connects the start state to the goal state while minimizing the overall cost. The core of the algorithm relies on two key components: state expansion and cost evaluation.

The state expansion phase is governed by the system state transition equation. Here, the state of the system is denoted as $\mathbf{s}={\left(\mathbf{p},\mathbf{v},\mathbf{a}\right)}^T$, where $\mathbf{p}$, $\mathbf{v}$, and $\mathbf{a}$ represent the position, velocity, and acceleration, respectively. The control input is symbolized as $\mathbf{u}$, which is supposed to be the jerk $\mathbf{j}$. The system equation is given by the following:

$$\dot{\mathbf{s}}=f\left(\mathbf{s},\mathbf{u}\right)={\left(\mathbf{v},\mathbf{a},\mathbf{j}\right)}^T.$$

(7)

A motion primitive is defined as a control input pair $(\mathbf{u},T)$, where T is the duration, representing a short trajectory segment in the state space. The $Expand\left(\right)$ function generates a set of motion primitives based on various control inputs and utilizes Equation (7) to predict the end states. To accelerate the search process, the $Prune\left(\right)$ function only keeps one of the motion primitives that terminate in the same voxel.

Algorithm 2: PathSearching via improved hybrid A*.

The cost evaluation phase encompasses both actual and heuristic costs, following [34]. The actual cost, denoted as g, quantifies the cumulative energy consumption along the motion primitive sequence from time 0 to $T_c$. This cost is calculated as the integral of the square of the control input $\mathbf{u}\left(t\right)$ over the entire trajectory duration, normalized by $T_c$:

$$g={\displaystyle \frac{1}{T_c}}{\int }_0^{T_c}{\parallel \mathbf{u}\left(t\right)\parallel }^{2}dt.$$

(8)

The heuristic cost h estimates the potential cost associated with a motion primitive from the current time $T_c$ to the goal. It serves as a guide for the trajectory toward the goal state. Assuming the current state is ${\mathbf{s}}_c={\left({\mathbf{p}}_c,{\mathbf{v}}_c,{\mathbf{a}}_c\right)}^T$ and the goal state is ${\mathbf{s}}_g={\left({\mathbf{p}}_g,{\mathbf{v}}_g,{\mathbf{a}}_g\right)}^T$. For motion along the x-axis, we employ Pontryagin’s Minimum Principle [34] and introduce parameters $\left(\alpha ,\beta ,\gamma \right)$. The resulting optimal control trajectory of the x-axis is formulated as follows:

$$u_x^{*}\left(t\right)={\displaystyle \frac{1}{2}}\alpha t^2+\beta t+\gamma .$$

(9)

Suppose that the goal state ${\mathbf{s}}_g$ is reached after an additional time T. There is $s_x^{*}\left(T\right)={\left(p_{gx},v_{gx},a_{gx}\right)}^T$, substituting into Equation (9) and obtaining the following parameters:

$$\left[\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right]={\displaystyle \frac{1}{T^5}}\left[\begin{array}{ccc}720& -360T& 60T^2\\ -360T& 168T^2& -24T^3\\ 60T^2& -24T^3& 3T^4\end{array}\right]\left[\begin{array}{c}{p}_{gx}-p_{cx}\\ v_{gx}-v_{cx}\\ a_{gx}-a_{cx}\end{array}\right].$$

(10)

By substituting the above coefficients into the cost functional and following the derivation in [14], the potential future energy consumption along the x-axis can be expressed as follows:

$$\begin{array}{cc}\hfill h_x\left(t\right)& ={\displaystyle \frac{1}{T}}{\int }_0^T{\parallel u_x\left(t\right)\parallel }^{2}dt\hfill \\ \hfill & ={\displaystyle \frac{1}{20}}{\alpha }^2{T}^5+{\displaystyle \frac{1}{4}}\alpha \beta T^4+{\displaystyle \frac{1}{3}}\left({\beta }^2+2\gamma \right)T^3+\beta \gamma T^2+{\gamma }^{2}T.\hfill \end{array}$$

(11)

To find the minimum heuristic cost $h_x$ along the x-axis, we can set the partial derivative of Equation (11) for T to zero, ${\displaystyle \frac{d}{dT}}{F}_x\left(t\right)=0$. Similar procedures are then applied to the y-axis and z-axis to obtain $h_y$ and $h_z$, respectively. The total heuristic cost, calculated by the PontryaginHeuristic() function, is given by the sum of these individual costs:

$$h=h_x+h_y+h_z.$$

(12)

The key enhancement resides in the implementation of recording the closest obstacles $coc\left(\mathbf{x}\right)$, as outlined in lines 11–13 of the Algorithm 2. Figure 2 demonstrates the 2D searching process, highlighting that only a few voxels significantly shape the trajectory, marked by blue dots. These POIs constitute redundant data that can be leveraged to refine the mapping. Notably, CheckFeasible() is determined by evaluating whether the distance from the current position is less than the search minimum distance parameter $d_{sear}$. When $d_{sear}$ is set to 0.0 m, the algorithm degrades to a straightforward search directly within occupied voxels.

3.3.2. Map Refining Based on the POI Model

The POI model meticulously identifies a pivotal set of obstacles that have a profound impact on shaping the trajectory’s contours. This model encapsulates obstacle point clouds, which serve as landmarks traversed by both the searcher and the optimizer. As the searcher’s path nodes expand, each motion primitive undergoes rigorous evaluation to identify potential collision hazards with encountered obstacles, diligently recording the precise coordinates of these obstacles.

Subsequently, leveraging these coordinates of POIs, a k-d tree is efficiently constructed, offering an initial estimation of the distance field. In parallel, within the optimizer’s realm, the collision risks associated with the refined trajectory are meticulously assessed. Should any novel obstacle coordinates be uncovered, they are seamlessly integrated into the existing k-d tree, thereby improving the accuracy of the distance field approximation. This iterative process of optimization continues until no further POIs emerge, ensuring comprehensive coverage. Furthermore, our study introduces a novel direct assessment of collision risks within a grid-based framework, which explicitly defines the distance field. When the distance metric associated with a query point falls below a preset threshold, the closest obstacle to this query point is immediately designated as a POI. The experimental outcomes underscore the remarkable efficiency of this approach, as optimization swiftly converges, necessitating merely a modest number of iterations, demonstrating its practicality and effectiveness.

3.3.3. Path Optimization in the Heuristic Map

The optimization variable $\mathbf{x}\left(t\right)$ is discretized and represented by a series of sampling points, denoted as $q_i,i=0,\dots ,n$. Employing the Lagrange multiplier method, we derive an unconstrained optimization model under discrete conditions, as expressed below:

$$minF=\sum _{i=0}^n\left[{\lambda }_s{∥\mathbf{j}\left(q_i\right)∥}^2+{\lambda }_o\varphi \left(d\left(q_i\right)\right)+{\lambda }_f\left({\left({∥\mathbf{v}\left(q_i\right)∥}^2-v_{max}^2\right)}^2+{\left({∥\mathbf{a}\left(q_i\right)∥}^2-a_{max}^2\right)}^2\right)\right]$$

(13)

where ${\lambda }_f$ represents the feasibility weight.

In this study, we utilize the L-BFGS optimizer from the nlopt library (https://nlopt.readthedocs.io/ (accessed on 1 April 2026)) to solve this optimization problem. The initial $q_i$ values are procured by sampling the trajectory generated by the improved hybrid A*. The distance value $d\left(q_i\right)$ and its corresponding gradient are computed within the heuristic maps, adhering to Equation (2). For the detailed computations of function values and gradients, we refer the reader to [32].

As specified in lines 4–8 of Algorithm 1, our sequential optimization process incorporates a verification step to confirm whether the optimized trajectory encompasses any novel POIs. Upon detecting the presence of new POIs, we subsequently adjust the short-term map to augment the accuracy of the heuristic map. An additional iteration of the optimization procedure follows this adjustment.

4. Experiments

In this section, the safety and time performance of the proposed method are validated through simulation and onboard experiments.

4.1. Simulation Experiments

In the simulation experiments, we established two kinds of scenarios—buildings and forests—to evaluate the performance of the proposed mapping, path search and optimization algorithms. All scenes are represented with point clouds at a resolution of 0.1 m, as shown in Figure 3. The buildings consist of randomly placed planes with varying thicknesses, while the forests are made up of randomly distributed cylindrical and annular obstacles. The forest scenes are generated at three density levels—low, medium and high. In these scenarios, the numbers of cylindrical and annular obstacles are 40 and 30 in the low-density case, 80 and 60 in the medium-density case, and 120 and 90 in the high-density case, respectively. First, the heuristic mapping algorithm is validated both qualitatively and quantitatively against popular mapping techniques in different scenes, demonstrating its superior performance. Next, we evaluate the performance of path searching, with particular emphasis on its influence on heuristic mapping. Then, the completeness of the proposed method is further verified by analyzing variations in POIs. Finally, through comparison with several motion planning methods, the proposed approach’s flexibility and safety are highlighted. All experiments were conducted on a single-threaded computer equipped with an Intel Core i7-9700K CPU running at 3.60 GHz.

4.1.1. Performance of Heuristic Mapping

Qualitative Analysis in the Buildings

In this subsection, we qualitatively compare the heuristic mapping algorithm against the popular Euclidean signed distance field (ESDF) in the buildings scene, as illustrated in Figure 4. The UAV is required to traverse four randomly selected waypoints, with a target minimum distance ($d_{opt}$) of 1 m. The classic ESDF algorithm used for comparison is taken from [35]. The long-term map is based on our prior work [17]. Both path search algorithms use the proposed hybrid A*, ignoring z-axis extensions because the buildings can be treated as two-dimensional. Path optimization uses the same objective function, which is solved via the L-BFGS optimizer. To meet the target $d_{opt}=1$ m, the map size covers all obstacles, and the update range $d_{map}$ of classic ESDF must be set to 1 m, yielding the results shown in Figure 4a. As the zoomed-in view shows, the path can at most be pushed to the ESDF’s boundary. In contrast, the heuristic mapping achieves the target 1 m with a long-term update range of only 0.5 m, as shown in Figure 4b. This occurs because the POIs found during search approximate local distances and gradients along the path. The results demonstrate that, unlike traditional planners, which can only reshape trajectories in regions with explicit gradient information, our approach creates a feedback loop between planning and mapping that significantly improves both flexibility and safety.

Quantitative Analysis in the Forest Scenes at Different Densities

This subsection compares three mapping algorithms to further analyze the runtime performance with different target minimum distance $d_{opt}$. Safety is approximately quantified by the average distance from the path to obstacles. The point-based map estimates distances across the entire workspace and is widely used in many motion-planning methods [10,36]. In the experiment, the point-based map is implemented using a k-d tree [23]. To meet the target $d_{opt}$, the update ranges $d_{map}$ of heuristic map and ESDF are $d_{opt}/2$ and $d_{opt}$, respectively. The map size is 40 m × 40 m × 5 m, covering all obstacles. The resolution of all maps is 0.1 m. The path search and optimization algorithms are the same as those in the previous experiment. Results are shown in Figure 5.

4.1.2. Performance of Improved Hybrid A*

In this subsection, we evaluate the influence of the A*, JPS, and improved hybrid A* path search methods on heuristic mapping in a medium density forest scene. The A* is a widely used path search approach [37]. JPS is a variant of the A* algorithm, with the advantage of being able to find jump points in grid maps [38]. Our hybrid A* can generate a smooth trajectory by applying a kinematic model. The implementations of A* and hybrid A* are derived from [14,31], and the JPS code is adapted from [25]. The $d_{sear}$ is set to 0.2 m. The $d_{opt}$ is set to 1.0 m, and the weights ${\lambda }_s$, ${\lambda }_f$, and ${\lambda }_o$. The maximum velocity and maximum acceleration are 3 m/s and 2 m/s². The long-term map update range is $d_{map}$ = 0.5 m and resolution is 0.1 m. The experiment was repeated independently 30 times. Results are summarized in

Table 1. Performance comparison of search methods under medium forest density.

Search Method	Mapping Time (ms)	Searching					Optimizing
		Search Time (ms)	POIs	Err. (m)	Safety (m)		Opt Time (ms)	POIs	Err. (m)	Safety (m)
					Min	Avg				Min	Avg
A*	35.85 ± 1.37	9.18 ± 0.18	372	0.30	0.22	0.87	25.94 ± 0.20	2	0.22	0.68	1.16
JPS	35.28 ± 0.46	3.83 ± 3.52	135	0.21	0.31	0.73	20.97 ± 0.52	0	0.03	0.68	1.25
Hybrid A*	35.72 ± 2.00	0.61 ± 0.03	13	0.38	0.34	1.05	15.89 ± 0.27	7	0.30	0.69	1.23

and Figure 6.

The heuristic maps obtained with different search algorithms are shown in Figure 6. The search trajectory is typically close to obstacles, while the optimizer forces the trajectory away from them, as depicted in Figure 6a,d,e. The errors of distance and gradient are quantitatively represented by heatmaps, as depicted in Figure 6b–e,h,i. Let $\mathbf{x}\in {\mathbb{R}}^3$ denote a continuous query position in the workspace. Since distance is a scalar field, we calculate the absolute error as follows:

$$e_d\left(\mathbf{x}\right)=\left|\widehat{d}\left(\mathbf{x}\right)-d\left(\mathbf{x}\right)\right|,$$

(14)

where $\widehat{d}\left(\mathbf{x}\right)$ is the estimated distance and $d\left(\mathbf{x}\right)$ is the true distance. The true values $d\left(\mathbf{x}\right)$ are calculated by finding the minimum distance from $\mathbf{x}$ to all obstacles. Since the gradient is a vector field, we use the cosine similarity offset to describe the directional consistency between the estimated gradient and the true gradient:

$$e_g\left(\mathbf{x}\right)=1-{\displaystyle \frac{<\widehat{g}\left(\mathbf{x}\right),g\left(\mathbf{x}\right)>}{|\widehat{g}\left(\mathbf{x}\right)\left|\right|g\left(\mathbf{x}\right)|}}$$

(15)

where $\widehat{g}\left(\mathbf{x}\right)$ is the estimated gradient and $g\left(\mathbf{x}\right)$ is the truth.

In practice, the distance gradient is the key factor that truly affects motion planning safety. When $e_g\left(\mathbf{x}\right)\in (0,1)$, it indicates an acute angle between the estimated and actual gradient directions, so the estimated gradient points away from obstacles. As the third column of Figure 6 shows, the gradients are sufficiently accurate near the trajectory, which ensures the correctness of the method.

The improved hybrid A* algorithm achieves the fastest search speed while maintaining sufficient accuracy, revealing an inherent trade-off between heuristic information and short-term map fidelity. The results show that A* and JPS perform better in terms of short-term map accuracy but have a significantly longer runtime. Although JPS achieves the highest map accuracy, many of the POIs are far from the trajectory, so the actual impact is limited. Similarly, A* discovers a large number of stacked POIs, but the computational overhead exceeds the benefits of improved accuracy. In practice, only obstacles near the trajectory have a significant impact on the trajectory, which is consistent with the locality discussed earlier. The hybrid A* has a stronger heuristic and explores fewer nodes. These POIs are relevant to the trajectory and provide the appropriate information needed to ensure safety. Additionally, from Table 1, we can conclude that most POIs are discovered through the search process. The first discovery of POIs ensures fast convergence in the sequential motion planning process. Therefore, the improved hybrid A* algorithm has the best performance.

4.1.3. The Completeness of the Proposed Methods

This subsection validates the completeness by counting optimization loops across multiple independent trials in different scenarios, as shown in Figure 7. The proposed method iteratively rebuilds the short-term map when new POIs are detected during search and optimization. The motion planning converges without a short-term map [14]. Therefore, we discuss the distribution of the number of reconstruction loops to determine whether the method converges. In each trial, the planner generates a collision-free path from the starting point to the destination. The starting point and destination are randomly sampled and are separated by 10 m. The $d_{map}$ of the long-term map is set to 0.5 m, with a resolution of 0.1 m and a map size of 80 m × 80 m × 5 m. The $d_{sear}$ of hybrid A* is set to 0.2 m. The $d_{opt}$ is 1 m, and the weights ${\lambda }_s$, ${\lambda }_f$, and ${\lambda }_o$ in the objective function are set to $2.0$, $0.01$, and $1.0$, respectively. The maximum velocity and maximum acceleration are 3 m/s and 2 m/s². We performed 1000 independent trials. Figure 7a depicts a representative result in the low-density scene, and Figure 7b shows the distribution of refinement loop counts.

The results show that, in the low-, medium-, and high-density forest scenes, 99.7%, 95.7%, and 93.9% of the 1000 trials, respectively, converged within two refinement loops. Here, “within two refinement loops” means that the trajectory optimization and the associated short-term map rebuilding triggered by newly detected POIs were required no more than twice. The maximum loop count did not exceed four, and in the most complex environment, only four trials required four loops. Moreover, the advantage of the proposed method becomes more pronounced in sparser scenes: by combining a small update range $d_{map}$ with the POI model, the algorithm can flexibly adjust trajectory shapes and thereby achieve better safety.

4.1.4. The Safety of the Proposed Methods

Comparison of Different Planning Methods

This subsection compares different motion planning methods under the same safety setting specified by $d_{opt}$, specifically FastPlanner [14,15] and EgoPlanner [31,32]. FastPlanner is a widely used gradient-based motion planning approach and uses the ESDF to optimize the trajectory. EgoPlanner does not require the distance gradient explicitly. Instead, it constructs point-to-point pairs $(s,v)$ between obstacles and trajectory points using A* search. The map size covers the starting point and destination with a resolution of 0.1 m. The $d_{map}$ is 0.5 m. The optimization objective function is the same as that in Equation (13). The $d_{opt}$ is 1.5 m and the weights ${\lambda }_s$, ${\lambda }_f$, and ${\lambda }_o$ in the objective function are set to $2.0$, $0.01$, and $1.0$, respectively. The visual comparison results are presented in Figure 8.

Table 2. Optimization performance metrics across different scenes and planning methods.

Scenes	Methods	Mapping Time (ms)	Planning Time (ms)	Path Length (m)	Min Distance (m)	Accum. Distance (m)
forest	Ours	9.09 ± 0.14	8.26 ± 0.86	14.54 ± 0.15	1.10 ± 0.02	120.39 ± 1.91
	EgoPlanner	8.86 ± 0.21	6.81 ± 0.01	13.86 ± 0.01	0.39 ± 0.01	113.42 ± 0.01
	FastPlanner	9.09 ± 0.14	3.76 ± 0.08	13.10 ± 0.01	0.51 ± 0.01	94.61 ± 0.01
buildings	Ours	20.86 ± 0.70	12.46 ± 2.50	12.32 ± 0.01	0.69 ± 0.01	78.12 ± 0.56
	EgoPlanner	19.75 ± 0.12	5.89 ± 0.14	11.42 ± 0.01	0.39 ± 0.01	72.60 ± 0.01
	FastPlanner	20.86 ± 0.70	3.83 ± 0.40	11.45 ± 0.01	0.54 ± 0.01	67.22 ± 0.01

provides the safety and runtime statistics of 30 independent trials.

In the forest scene, our method significantly outperforms FastPlanner, achieving a remarkable 115.69% improvement in minimum distance and a 27.25% enhancement in accumulated distance, although at the cost of a 35.02% increase in total computation time (mapping plus planning time). Compared with EgoPlanner, our method incurs a 10.72% increase in total computation time while improving the minimum distance by 182.05% and the accumulated distance by 6.15%. Similarly, in the buildings scene, our method shows a 34.95% increase in total computation time over FastPlanner, accompanied by gains of 27.78% and 16.22% in minimum and accumulated distances, respectively. Against EgoPlanner, it exhibits a 29.95% increase in total computation time, coupled with improvements of 76.92% in the minimum distance and 7.60% in the accumulated distance. The sparser obstacle distribution in the forest scenario leads to more pronounced improvements in both minimum and accumulated distances.

The proposed method significantly enhances the safety of the generated trajectories. As shown in Figure 8, FastPlanner can only optimize trajectories in regions with distance gradients. When the map update range $d_{map}$ is small, this can result in trajectories that are dangerously close to obstacles. EgoPlanner’s minimum distance from obstacles is determined by $d_{sear}$, and if the initial trajectory does not intersect with obstacles, $(s,v)$ pairs cannot be constructed, leading to trajectories that are too close to obstacles. This poses a significant safety risk for UAVs. In contrast, the minimum distance in our method is determined by $d_{map}$. As long as the trajectory intersects with the long-term map, POIs can be identified, enabling the short-term map to push the trajectory further from obstacles.

The Safety Under Different $d_{opt}$

This subsection further compares the performance of our method to that of FastPlanner at different $d_{opt}$. Due to the small $d_{map}$ in the previous subsection, although FastPlanner consumed the least amount of time, its trajectory safety was significantly lower than that of our method. Here, we consider the runtime of both methods under the same actual level of safety. Apart from the settings of $d_{map}$ and $d_{opt}$, all other parameters remain the same as in the previous section. We conducted 30 independent repeat trials for each $d_{opt}$, and the results are shown in Figure 9. We set $d_{map}$ to be the same as $d_{opt}$ to enable FastPlanner to work effectively. As the optimization distance threshold increases, the proposed algorithm demonstrates notable competitiveness in terms of runtime, achieving further improvements in trajectory safety within limited resources.

4.2. Onboard Experiments

We further evaluate the practical feasibility of the proposed framework for fully autonomous obstacle avoidance in two real-world environments with different depth and localization system. As shown in Figure 10, the first scene contains nine cylinders, each 20 cm in diameter. For this scenario, we employ an F450 quadrotor equipped with a DJI Manifold 2-C onboard computer with an Intel Core i7-8550U CPU running at 3.60 GHz (https://www.dji.com/cn/manifold-2 (accessed on 1 April 2026)). Depth and localization data are provided, respectively, by an Intel RealSense D455 depth camera (https://www.intel.com/content/www/us/en/products/sku/205847/intel-realsense-depth-camera-d455/specifications.html (accessed on 1 April 2026)) and the GNSS module (https://www.blicube.com/ (accessed on 1 April 2026)). Another SU17 quadrotor (https://docs.amovlab.com/su17/#/ (accessed on 1 April 2026)) is used in the second scene, where several trees are randomly distributed. Depth measurements are acquired with a Livox MID-360 LiDAR (https://www.livoxtech.com/mid-360 (accessed on 1 April 2026)), and vehicle poses are estimated by the FAST-LIO algorithm [23]. The parameter settings of both are as follows: the map resolution is 0.1 m, map size is 12 m × 12 m × 6 m, $d_{map}$ = 0.5 m, $d_{sear}$ = 0.2 m, $d_{opt}$ = 1 m, and the planning horizon is 5 m, maximum velocity and acceleration are 2 m/s and 1 m/s². The flight results are presented in Figure 11.

In two scenes, the proposed planner successfully achieves autonomous flight with a limited distance field. The F450 accomplished obstacle avoidance flight in 19.9 s with a 30.48 m path. The SU17 accomplished obstacle avoidance flight in 12.65 s with a 15.69 m path shown in Figure 11a,b. Although the max distance is 0.5 m, the quadrotors can achieve the target $d_{opt}$ = 1 m. The experimental results demonstrate that the proposed method can further adjust the path shape in regions lacking explicit distance gradients.

5. Conclusions

In this paper, we propose a sequential motion-planning framework that leverages heuristic maps to improve the safety and efficiency of UAV navigation in unknown, obstacle-rich environments under bounded distance-field updates. The heuristic map combines a grid-based long-term map, which maintains reliable occupancy and locally updated distance information, with a POI-guided short-term component that provides efficient clearance and gradient proxies outside the explicit update band. Motivated by motion-planning locality, the POI model extracts trajectory-relevant obstacles during both search and optimization, establishing an explicit feedback loop among mapping, path search, and trajectory refinement. The proposed pipeline first generates a dynamically feasible initial trajectory using an improved hybrid A* in the long-term map, and then performs trajectory optimization in the combined heuristic map with iterative short-term refinement whenever new POIs are exposed. It is important to note that the POI-based proxy is not treated as a conservative safety certificate in unobserved space; instead, safety is enforced through conservative collision checking against the inflated long-term occupancy map within the current sensing/update horizon, while POIs are progressively augmented as new obstacle evidence becomes available. Simulation results in building and forest environments with varying obstacle densities demonstrate that our method consistently achieves larger clearance and competitive runtime compared with representative baselines, while requiring substantially smaller explicit distance-update ranges. In sparse environments, 99.7% of the 1000 trials converged within two short-term refinements, meaning that the trajectory optimization and short-term map rebuilding were required no more than twice; across all densities, no trial exceeded four refinement loops. In simulated forests, our approach improved the minimum distance to obstacles by 115.69% and 182.05% over FastPlanner and EgoPlanner, respectively, and increased the accumulated clearance by 27.25% and 6.15%. In simulated building environments, the minimum-distance improvements were 27.78% and 76.92%, with accumulated-clearance gains of 16.22% and 7.60%. Onboard experiments further validated the practical applicability of the proposed framework on two UAV platforms with different sensing and localization setups.

Future work will focus on more informative and compact short-term representations, on search strategies that further exploit POI distributions to reduce refinement iterations, and on improving robustness against trajectory-tracking errors caused by environmental disturbances such as wind, which may otherwise compromise trajectory safety.