Exploiting a Variable-Sized Map and Vicinity-Based Memory for Dynamic Real-Time Planning of Autonomous Robots

Abstract

This paper presents a complete system for autonomous navigation in GPS-denied environments using a minimal sensor suite that operates onboard a robotic vehicle. Our system utilizes a single camera and, given a target destination without prior knowledge of the environment, replans in real time to generate a collision-free trajectory that avoids static and dynamic obstacles. To achieve this, we introduce, for the first time, a local Euclidean Signed Distance Field (ESDF) map with variable size and resolution, which scales as a function of the vehicle’s velocity. The map is updated at a high rate, requiring minimal computational power. Additionally, a short-term vicinity-based memory is maintained for previously observed areas to facilitate smooth trajectory generation, addressing the limited field-of-view provided by the RGB-D camera. System validation is carried out by deploying our algorithm on a differential drive vehicle in both simulation and real-world experiments involving static and dynamic obstacles. We benchmark our robotic system against state-of-the-art autonomous navigation frameworks, successfully navigating to designated target locations while avoiding obstacles in both static and dynamic scenarios, all without introducing additional computational overhead. Our approach consistently achieves the target goals even in complex settings where current state-of-the-art methods may fall short.

1. Introduction

The way a robot perceives its environment is crucial for autonomous navigation, exploration, and task completion. The increasing use of autonomous robotic vehicles in industrial applications requires safe operation in unstructured, dynamic, and human-populated environments, often without prior knowledge. In such scenarios, conventional localization systems like GPS can be unreliable for providing accurate navigational information [1]. This need guides research endeavors to develop real-time motion planning algorithms that can handle complex and dynamic environments without prior knowledge of the map while executing computational tasks onboard. Developing such a system involves integrating four core components: localization, obstacle detection, trajectory generation, and trajectory tracking. The need for onboard, real-time processing necessitates lightweight frameworks that efficiently discard abundant information.

The advantages of Euclidean Signed Distance Fields (ESDFs) for local trajectory planning are widely recognized [2,3], as their volumetric maps offer dynamic updating and accurate representation of complex environments, enabling real-time mapping and localization in dynamic environments [4,5,6]. However, these methods add computational overhead to large-scale navigation scenarios. To tackle these challenges, certain methods resort to the use of distance information from a Truncated Signed Distance Field (TSDF) map [7], gradient information that does not require the generation of an ESDF map [8], occupancy grid maps [9], or simpler data structures like a 3D circular buffer [10], leading to suboptimal or incomplete paths and inefficiency in complex or dynamic environments. To address these limitations, adaptive-size data structures have also been employed to represent the robot’s environment across multiple layers with different voxel sizes [11,12,13,14]. This approach aims to achieve detailed representations without excessive computational cost. However, these methods are typically optimized based on the characteristics of the environment and the sensors used, rather than the requirements of a moving robotic system. Consequently, they cannot be seamlessly integrated with path planning methods.

In addition, discretizing the environment into a grid often leads to search-based solutions on path planning [8,15,16,17,18]. However, these methods may become inefficient in large or cluttered environments and often struggle with dynamic settings. Optimization-based algorithms, such as [10,19], aim to minimize a cost function defined by constraints and preferences and can offer adaptability to different scenarios and the generation of smooth and continuous paths, but this comes at high computational costs, particularly in real-time applications.

This paper aims to present a complete system for autonomous navigation in GPS-denied environments, able to generate collision-free trajectories in real time in both static and dynamic environments (Figure 1). It employs a minimal sensor suite, relying on a stereo camera for all of the robot’s sensing needs [20]. In addition, it is completely autonomous, since all required computational tasks are executed onboard. To achieve this, we create a local map containing the necessary information for path planning and discard unused information to minimize computational power. We summarize our key contributions as follows:

• A unified autonomous navigation system that dynamically adapts to complex environments with robust obstacle avoidance. Its low computational overhead enables onboard processing for visual odometry, local mapping, and trajectory planning without requiring prior knowledge of the environment or reliance on complex sensors.
• An innovative, adaptive local ESDF map that dynamically adjusts its size and resolution based on vehicle velocity, enabling high-frequency updates for efficient and responsive path planning.
• A novel mechanism combining adaptive offset map positioning based on angular velocity and short-term map memory to retain information from previously observed areas at the vicinity of the robot, minimizing redundant calculations and preventing unnecessary maneuvers.

The work is organized as follows: Section 2 discusses the mapping and path planning algorithms used to navigate the robot. Section 3 describes the robotic vehicle used for real-world experiments. Section 4 presents experimental results in simulation and real-world environments. Finally, Section 5 offers conclusions and discussions for future directions.

2. Mapping and Path Planning Algorithm

2.1. Local Mapping

This work builds upon the approach described in [6], but we focus on maximizing the update rate of the generated map. To achieve this, we limit the creation of a global ESDF map to a submap of the robot’s surroundings, effectively reducing the computational overhead by excluding redundant information that is not required for the replanning process. The map’s high update rate enables real-time replanning in dynamic environments without relying on predictive or velocity-based models. To ensure that no valuable spatial data are lost, a trade-off mechanism between the represented area’s size and the map’s resolution is introduced. Unlike existing ESDF-based approaches that maintain static grid resolutions, this mechanism adopts a variable sized map approach where the map’s covering area is dynamically scaled based on the vehicle’s velocity. Specifically, the dimensions of the map along the x- and y-axes scale proportionally to the velocity, while the resolution is adapted accordingly to maintain a high update rate. As a result, the map’s resolution increases when velocity is low to accommodate precise path planning in spaces with narrow passages and increases at higher-velocities to effectively facilitate path planning.

Map Size: To adjust the size of the map, we follow the assumption that the trajectory replanning algorithm has a number of N control points with a fixed time interval $dt$ where the robot would need occupancy information in a horizon of $x_{map}=N\times dt\times V_x$, where $V_x$ is the velocity of the robot, considering a constant velocity. Setting a safety factor S to ascertain that the mapped area maintains sufficient information of the surroundings, the length of the map along the x-axis is as follows:

$$x_{planned}=S\times N\times dt\times V_x$$

(1)

where $xϵ[x_{min},x_{max}],x_{min}\ne 0$ to avoid a null map at zero velocities.

To replan around obstacles when moving at higher velocities, the map must depict the space where the robot can safely execute maneuvers. To accomplish this, the map’s size along the y-axis is also adjusted based on the vehicle’s linear velocity. However, blind spots are encountered during trajectories with a change in curvature often leading to missing out useful trajectory information. To avoid those blind spots, we introduce a lateral offset to the map. The amount of the offset provided is determined by the robot’s angular velocity (see Equation (2)). This adaptive behavior allows the robot to maintain, for a longer duration, that part of the visual information on the map. This could be compared to the left or right adaptive headlights offered in modern automobiles, which detect the steering angle and light up accordingly to illuminate the trajectory of the car only during the turning maneuver. In our algorithm, the y-coordinate of the map center can be calculated based on the size of the map and the angular velocity as

$$y_c={\displaystyle \frac{\omega }{{\omega }_{max}}}{y}_{map}$$

(2)

Resolution: The generated map divides the indicated 3D space into voxels to determine occupancy. The number of these voxels $N_v$ is

$$N_v=x_{map}\times y_{map}\times z_{map}/re{s}^3$$

(3)

The number of voxels regulates the map update time, and, therefore, to maintain the map update frequency high, the map’s resolution decreases according to its size. With this approach, the map offers a finer representation of the robot’s surroundings for low-velocity motions, allowing path planning through narrow spaces. In higher velocities, the vehicle provides a coarse, but extensive, representation of the environment to avoid collisions with static and dynamic obstacles.

Given that the camera generates point clouds within a conical sector due to its field of view, the updated area of the map is limited. To maintain simplicity and cost-effectiveness, we avoid using additional cameras or expensive 3D LiDAR sensors. Instead, we adopt a short-term vicinity-based memory area to extend the information on the map without compromising the ability to handle dynamic environments through constant updates. The previously observed information is not updated within this area to minimize unnecessary maneuvers and non-smooth trajectories. Additionally, the updated area is approximated using three rectangular boxes (two constant-sized and one variable-sized), as shown in Figure 2 to approximate the near conical shape of the point cloud close to the camera (see Figure 3).

The occupancy update algorithm of Algorithm 1 consists of two loops. The first loop ensures that voxels that are no longer in range, but were previously observed, are still marked as occupied. The second loop updates the map based on new sensor data, managing voxels that need to be added or eliminated. The ESDF update algorithm (Algorithm 2) handles inserting and deleting voxels in the map using two independent queues while managing distances to the nearest obstacle. Figure 3 shows a map created from our system during our experiments with an unmanned ground vehicle (UGV) (see Section 3).

Algorithm 1 Occupancy update algorithm

for each  $vox$  in  $memory\_range$  do   $idx\leftarrow VoxToIdx\left(vox\right)$   if $Exist\left(idx\right)$ and not $InRange\left(idx\right)$ then     $occupancy\left(idx\right)\leftarrow 1$     $distance\left(idx\right)\leftarrow 0$   else     $occupancy\left(idx\right)\leftarrow undefined$     $distance\left(idx\right)\leftarrow Inf$   end if end for for each  $point$  in  $new\_pointcloud\_msg$  do   $idx\leftarrow PntToIdx\left(point\right)$   $occupied\leftarrow Exist\left(idx\right)$   if ($InRange\left(idx\right)$) then     $occupancy\left(idx\right)\leftarrow 1$     $distance\left(idx\right)\leftarrow 0$   end if   $vox\leftarrow IdxToVox\left(idx\right)$   if ($Exist\left(idx\right)$ and not $occupied$) then     $in\_queue.$push($vox$)   else if (not $Exist\left(idx\right)$ and $occupied$) then     $out\_queue.$push($vox$)   end if end for

Algorithm 2 ESDF update algorithm

for each  $vox$  in  $insert\_queue$  do   $idx\leftarrow VoxToIdx\left(vox\right)$   $closest\_obst\left(idx\right)\leftarrow vox$   $distance\left(idx\right)\leftarrow 0$   $update\_queue.$push($vox$) end for for each  $vox$  in  $delete\_queue$  do   $idx\leftarrow VoxToIdx\left(vox\right)$   $distance\left(idx\right)\leftarrow Inf$   for each $neighbour$ in $neighbours\left(idx\right)$ do     $n\_idx\leftarrow VoxToIdx\left(neighbour\right)$     $tmp\leftarrow EuclDist(n\_idx,idx)+distance(n\_idx)$     if ($tmp<distance\left(idx\right)$) then       $distance\left(idx\right)\leftarrow tmp$       $closest\_obst\left(idx\right)\leftarrow closest\_obst(n\_idx)$     end if   end for end for

2.2. Path Planning

Our approach uses an optimization-based local trajectory replanning algorithm [10], which, given a global path, will lead to obstacle avoidance utilizing gradient information from the ESDF map with minimum deviation from the given path through the minimization of the cost function L of Equation (4). To reduce the computation time of the optimization framework, we simplify the cost function by incorporating only the essential terms required to ensure a feasible, collision-free path while avoiding unnecessary maneuvers. The points generated by this optimization process serve as the control points of a B-Spline trajectory, which reduces the optimization time by minimizing the number of constraints needed in the optimization process. Additionally, the B-Splines’ inherent ability to generate smooth trajectories and their advantageous property of local control are essential for real-time replanning in dynamic environments [21].

$$L=L_c+L_{ep}+L_s$$

(4)

The cost function consists of a collision term (Equation (5)) that penalizes the paths that lead to a collision, an endpoint error term (Equation (6)) used to maintain the replanned trajectory as close to the initial one as possible, and a soft limit (Equation (7)) used to maintain the velocity, acceleration and jerk values bounded and that allows the use of an unconstrained optimization algorithm.

$$L_c={\lambda }_c{\int }_{t_i}^{t_f}{\left(diff\left(t\right)\right)}^{2}dt$$

(5)

where:

$$diff\left(t\right)=\left\{\begin{array}{cc}0\hfill & \mathrm{if}d\left(t\right)-\tau >0\hfill \\ d\left(t\right)-\tau \hfill & \mathrm{if}d\left(t\right)-\tau \le 0\hfill \end{array}\right.$$

${\lambda }_c$ is a weight parameter, $d\left(t\right)$ is the distance to the nearest obstacle calculated using trilinear interpolation from the incrementally built ESDF map, and $\tau$ is a user-defined threshold.

$$L_{ep}={\lambda }_p\left|\right|p_{glob}\left(t_f\right)-p\left(t_f\right){\left|\right|}^2+{\lambda }_v\left|\right|v_{glob}\left(t_f\right)-v\left(t_f\right){\left|\right|}^2$$

(6)

${\lambda }_i$ are weight parameters for the position and velocity terms; $p_{glob}\left(t_f\right)$ and $v_{glob}\left(t_f\right)$ are the desired position and velocity, respectively, at the end of the time segment; and $p\left(t_f\right)$ and $v\left(t_f\right)$ are the position and velocity, respectively, at the end of the segment of the B-Spline.

$$L_s=\sum _{n=2}^{n=4}{\int }_{t_i}^{t_f}b(t,i)dt$$

(7)

where

$$b(t,i)=\left\{\begin{array}{cc}0\hfill & \mathrm{if}{p}^{\left(i\right)}\left(t\right)\le p_{max}^{\left(i\right)}\hfill \\ exp({\left(p^{\left(i\right)}\left(t\right)\right)}^2-{\left(p_{max}^{\left(i\right)}\right)}^2)\hfill & \mathrm{if}{p}^{\left(i\right)}\left(t\right)>p_{max}^{\left(i\right)}\hfill \end{array}\right.$$

$p^{\left(i\right)}\left(t\right)$ is the i-th derivative of the position at time t and $p_{max}^{\left(i\right)}$ is the limit value.

For the optimization problem, an open-source library for nonlinear optimization, the NLopt [22], is adopted.

3. The Developed UGV

3.1. Hardware

The UGV used for the experiments is the differential drive vehicle in Figure 4. The robot’s chassis consists of the power management cabinet containing the batteries with the power management board and the electronics cabinet with the onboard computer, the motor drivers, and the camera. The onboard computer is a Jetson Xavier with a 6-core NVIDIA Carmel ARM CPU (Nvidia, Santa Clara, CA, USA). At the front of the vehicle, a ZEDX mini camera [23] (ZEDX, Bellefonte, PA, USA) with a 2.2 mm focal length is integrated for visual odometry and point cloud generation with a depth range of 0.1–8 m & an update rate of up to 30 Hz.

The front wheels are independently controlled, while the rear are passive caster wheels. The active front wheels (ODrive BotWheels [24]) are a combination of brushless wheel hub motors with hall sensors and a high-resolution (3200 CPR) integrated reflecting encoder. The motors can continuously produce a torque of 5 Nm. A CAN controller operating at a 250 kbps bitrate is used to send velocity commands to the motor drivers (ODrive S1 [25]) from the onboard computer to control the wheels.

The vehicle dimensions are 430 mm × 400 mm × 300 mm and the maximum velocity is 0.985 m/s. Two 4S Li-ion batteries (14.8 V, 18 Ah) connected in series power the UGV.

3.2. Control

The optimization process for local path replanning produces an output of control points that define the robot’s trajectory. Although the number of the computed control points is defined and maintained throughout the vehicle’s motion, their coordinates are computed every time the vehicle reaches the next control point in the sequence until the robot reaches its final destination. To ascertain that the robot can accurately traverse all the control points generating a smooth trajectory, a tracking PID controller is adopted [26]. This approach offers a tuneable PID control loop, where the robot’s base link tracks a projected point (PP) on the generated trajectory at a fixed distance l ahead of the robot. The PID gains are empirically tuned through an iterative process to ensure a smooth trajectory, minimizing overshoot and achieving zero steady-state error. This PP traverses the path between two control points at a constant velocity, with longitudinal and angular errors used as control feedback to compute the vehicle’s velocity. As the robot approaches the next control point within a distance threshold, the subsequent control point is incorporated into the desired trajectory. By keeping l sufficiently smaller than the threshold distance, the vehicle is able to follow a smooth trajectory. The controller continuously updates the path with each new control point until the robot reaches its final destination. For our application, PP is set at $l=$ 0.1 m ahead of the vehicle and moves at a velocity of 0.3 m/s.

The differential drive dynamics of the vehicle are incorporated into the controller, and the controller gains are adjusted through an iterative tuning process to further improve the vehicle’s performance during path tracking. The PI gains of the ODrive controller that controls the wheels of the robotic platform are similarly tuned to minimize overshoot and to ensure the precise tracking of velocity commands.

4. Experimental Results

4.1. Simulation Experiments

To test the stability of the proposed method, the vehicle is required to navigate avoiding obstacles—static or dynamic—randomly placed in simulated environments generated using Gazebo-11.12, run on a 2.6 GHz Intel i7 CPU with 16 GB RAM (Intel, Santa Clara, CA, USA). The virtual robotic vehicle model employed (Figure 5a) is a differential drive UGV that implements the controller approach described in Section 3.2, and its mass properties are derived from a computer-aided design of the vehicle in Section 3.1 and cross-referenced using the fabricated prototype. The point cloud data are generated by [27] and are parameterized to match the ZED X mini camera. Pedestrian models crossing the robot’s planned path are added using the pedsim_ros library [28], and odometry data are provided by [29]. Simulation experiments are carried out in an indoor environment with random static and dynamic obstacles creating narrow passages, such as the one presented in Figure 5a and in a large-scale dynamic environment.

To benchmark our approach, we compare it with three state-of-the-art open-source approaches for autonomous navigation, restricting them to perform path planning exclusively in 2D: FAST-Planner [30], EWOK [10], and FAEL [31]. The first uses kinodynamic path searching with a B-spline-based trajectory optimized over the Euclidean Distance Fields of a voxel map. The second is an optimization-based path planning method used to replan in cluttered environments with static obstacles, and the latter uses a probabilistic 3D grid map with a search-based approach for planning and is optimized for UGVs. We compare the frameworks on the optimization time, map update time, path length, and success ratio. While FAEL is optimized for large-scale exploration, we adapt it for direct goal-reaching tasks. Similarly, FAST-Planner, although designed for UAV navigation, shares ESDF-based optimization principles and is evaluated in a 2D setting. EWOK, which assumes static environments, provides a baseline for testing dynamic obstacle handling. To evaluate the map update and optimization time, we use Scalopus [32], a tracing framework, to obtain traces for each scope of interest (Figure 5b). The utilized profiling tool visualizes the time spent in annotated sections of the code or function call frames. It uses a browser-based trace viewer, displaying performance data in a way that is easy to interpret.

We initially compare our approach to all methods regarding the duration of the optimization process before each path replanning. As shown in Figure 6, FAST-Planner demonstrates the fastest optimization times, with our proposed method following closely, showing a difference of approximately 20 ms. The longer duration values observed in FAEL can be attributed to its design for large-scale environmental exploration, which requires tracking unexplored areas.

The assumption of EWOK of a static environment, which results in inaccurate maps in dynamic scenarios, led us to exclude it from comparing the map update time. The results in Figure 7 show minor differences in the update times of the maps between the methods, reasonable considering the variations in their map generation approaches. For comparison methods that require a predefined resolution and known map size, these parameters are set to the median of the minimum and maximum values used in our approach: resolution = 0.1 m, map_size = 5.5 m × 5.5 m × 4 m.

As FAEL is primarily designed for autonomous exploration rather than guiding a robot to a specific goal and EWOK cannot handle dynamic obstacles during map updates, we excluded them and focused our evaluation on comparing our approach with FAST-Planner, a state-of-the-art method for autonomous navigation in dynamic environments.

Table 1. A comparison of the mean normalized path lengths.

Method	4S	7S	4S-1D	4S-2D
Proposed	1.100	1.113	1.266	1.153
FAST Planner	1.095	1.128	1.268	1.154

presents the normalized path lengths generated by our approach and FAST for successful indoor experiments (S: static; D: dynamic). The results demonstrate that the path lengths are highly comparable and close to the initial path, indicating that unnecessary maneuvers are avoided and near-optimal paths are produced.

FAST-Planner employs a hybrid method, creating an optimized replanned path over an A*-generated initial guess, which minimizes the duration of the optimization time. However, this approach has certain disadvantages in specific scenarios. For example, dense and narrow corridors (Figure 8a) and winding paths with dead ends (Figure 8b) can make A* less efficient, as it can explore numerous paths before finding the correct one. Furthermore, sparse obstacles that require long detours (Figure 8c) can significantly slow the process, potentially leading to failures to reach the target location. In all the experimental setups of Figure 8, using the same parameters for map generation, the FAST-Planner approach always fails to reach the goal point, while our approach shows success with scores greater than 0.8 for all scenarios (as shown in

Table 2. Success ratio.

Method	Narrow	Winding	Sparse
Proposed	0.83	1.0	0.8
FAST-Planner	0.0	0.0	0.0

). This is largely due to the incorporation of constraints regarding vehicle dynamics directly into the optimization process in our method.

4.2. Real-World Experiments

We evaluate our system on the UGV prototype (Section 3) in several indoor experiments. To keep our system computationally light and avoid unnecessary onboard processing, we restrict from using an open-source pose estimator and use the positional tracking module of the ZEDX mini stereo camera [33].

In these experiments, the vehicle operates without prior knowledge of the map and is assigned a target destination. Using only a stereo camera for localization, mapping, and path following, the vehicle successfully navigates towards the goal, avoiding static objects and randomly walking pedestrians encountered along its path. All computations are executed onboard using an embedded computer system (Jetson Xavier, NVidia), allowing the robotic vehicle to consistently avoid obstacles and reach the goal position in all experiments. Throughout the real-world experiments, various configurations involving both static and dynamic obstacles within an indoor corridor are employed (Figure 9).

The mean optimization time was recorded at 116.9 ms, with the map update time averaging 92.3 ms. These values are higher compared to simulation results, primarily because the embedded computer offers lower computational power relative to the system used to perform the simulations. Subsequently, a densely cluttered environment featuring dead ends (similar to the one in Figure 8b) was constructed to evaluate the performance of our approach in comparison to FAST-Planner. As shown in Figure 10, our method successfully guided the robot to the target, while FAST-Planner failed to complete the path.

5. Discussion

We introduced a novel autonomous navigation approach capable of handling unstructured and dynamic environments. Using distance information from a newly developed variable-sized ESDF map, the robot can reach its target destination by generating a smooth, B-spline-based trajectory through optimization. Unlike fixed ESDF methods, our approach maintains computational efficiency while adapting to the vehicle’s motion state. The system was validated in simulated environments and on a custom mobile robotic platform equipped with a minimum sensor suite in various indoor scenarios, specifically using only a stereo camera for all sensing needs. Our approach is benchmarked against state-of-the-art autonomous navigation methods in complex static and dynamic environments. Although our system incurs slightly higher computational overhead compared to FAST-Planner, it consistently demonstrates robustness and efficiency in reaching the target position, even in highly challenging scenarios where existing algorithms fail to complete the course.

In the future, we plan to deploy our system in a heterogeneous robotic swarm consisting of both ground and aerial vehicles. This will allow us to validate the transferability of the parameterized software across various types of robotic platforms, enabling robust task execution in even more complex and dynamic scenarios.