Dynamic Obstacle Avoidance for Mobile Robots Based on 2D Differential Euclidean Signed Distance Field Maps in Park Environment

Abstract

In this paper, a novel and complete navigation system is proposed for mobile robots in a park environment, which can achieve safe and stable navigation as well as robust dynamic obstacle avoidance. The navigation system includes a global planning layer and a local planning layer. The global planner plans a series of way-points using the A* algorithm based on an offline stored occupancy grid map and sends them to the local planner. The local planner incorporates a dynamic obstacle avoidance mechanism. In contrast to existing dynamic obstacle avoidance algorithms based on trajectory tracking, we innovatively construct a two-dimensional Difference ESDF (Euclidean Signed Distance Field) map to represent obstacle motion information. The local planner outputs control actions by scoring candidate paths. A series of simulation experiments and real-world tests are conducted to verify that the navigation system can safely and robustly accomplish navigation tasks. The safety distance of the simulation experiment group with the dynamic obstacle avoidance scoring item added increased by 1.223 compared to the group without the dynamic obstacle avoidance scoring item.

1. Introduction

The autonomous navigation of mobile robots has been extensively studied and developed in recent decades [1]. The use of mobile robots in manufacturing, warehousing, and construction is on the rise. Alongside traffic scenes, the application of mobile robots in park scenes has become more important. Real-world scenarios pose challenges when robots and humans share the same environment. Robust, stable, and flexible dynamic obstacle avoidance has become a fundamental requirement for mobile robots and many navigation systems have been successfully developed [2]. However, due to complex dynamic cases, and a changing environment, constructing an effective and adaptable navigation system is still necessary.

At present, researchers have explored a range of approaches incorporating various sensor types, including vision-based [3,4] and LiDAR-based method [5,6]. Vision-based methods can provide rich environmental information with cameras but are vulnerable to changes in light conditions, and the computational cost is large. The method based on LiDAR is more robust and suitable for navigation tasks in the park environment.

Dynamic obstacle avoidance has always been a serious problem in robot applications, such as navigation and motion planning. Compared with static obstacles, the obstacle avoidance of dynamic obstacles is more challenging for the following reasons: Firstly, the environment of the moving obstacle is uncertain. Secondly, accurately predicting the motion trajectory of the obstacle is often difficult in a dynamic environment because the motion of the obstacle is unknown. Thirdly, interactions between moving obstacles and autonomous mobile robots affect the trajectory of the moving obstacles. When robots encounter obstacles during their movement within their field of view, they must promptly take proactive actions to avoid the obstacles. Traditional dynamic obstacle avoidance algorithms mainly include artificial potential field (APF) [7] and nonlinear model predictive control (NMPC) [8,9]. They work by setting up safe sets or tracking safe paths to avoid dynamic obstacles. Such algorithms typically have high computational resource requirements and are not suitable for use in embedded devices for mobile robots.

The Euclidean Signed Distance Field (ESDF) map has the advantage of evaluating the distance and gradient information against obstacles. In this paper, we propose a novel dynamic obstacle avoidance system based on a 2D Difference ESDF Map. The mapping module and local path planning module constitute the entire system. The mapping module represents dynamic information in the local environment by constructing a 2D Difference ESDF map, while the local path planning module avoids dynamic obstacles by scoring candidate paths. The main contributions of our study are presented as follows:

• Different from traditional dynamic obstacle detection algorithms, we characterize dynamic information in the environment by calculating the rate of change of ESDF values between grid cells in two frames.
• Adding the differential ESDF values to candidate trajectory evaluation achieves a more stable and robust dynamic obstacle avoidance effect.
• Simulation experiments and real-world experiments are performed to evaluate the proposed system.

The remainder of this paper is organized as follows: Section 2 reviews related work. Section 3 presents the system framework including a 2D Differential ESDF map construction module and a planning module. Section 4 introduces simulation and real-world experiments. Finally, Section 5 concludes this work.

2. Related Work

This section consists of two components: an investigation into the algorithms for constructing ESDF maps, followed by a study on dynamic path planning algorithms for mobile robots.

2.1. ESDF Map

Maps are crucial for the autonomous navigation of mobile robots. The basic requirement of a mapping system is to balance the accuracy of fusing depth measurements and the overhead of storing a fine representation of the environment. ESDF maps provide fast queries for free/occupied status, as well as the ability to determine the distance to the nearest obstacle from the current position. Therefore, they are widely used in the field of path planning for mobile robots, for example, [10,11,12].

For a mobile robot that relies on distance information for real-time path planning, efficiency and accuracy in maintaining and updating the ESDF map are necessary. In this field, researchers have explored a range of methodologies.

Felzenszwalb et al. [13] proposed a linear-time algorithm that utilizes parabolas as the distance transform to compute the ESDF. However, the method necessitates the computation of a distance transform. However, the high density of obstacle grids in the global map results in low computational efficiency, rendering them unsuitable for the real-time path-planning requirements of mobile robots.

Oleynikova et al. [14] proposed Voxblox, an incremental framework for constructing an ESDF map. This framework initially consolidates sensor data into a Truncated Signed Distance Field (TSDF) map, followed by expanding to all voxels using Breath-First Search (BFS) and ultimately deriving the ESDF map through direct computation. While capable of real-time dynamic map updates, the ESDF map produced by the framework may exhibit notable inaccuracies.

Han et al. [15] proposed the FIESTA framework, which devises a specialized data structure to store voxels with changing occupancy states, and subsequently employs BFS to update the ESDF of all voxels. This framework operates independently of the TSDF map and demonstrates superior efficiency in comparison to Voxblox [14].

Current research on mobile robot path planning based on ESDF maps [10,11,12] mainly leverages the capability of ESDF maps to rapidly query distance information from the current position to the nearest obstacle. Such planning methods are only suitable for the path planning of mobile robots in static environments. When the obstacle positions change, paths planned based on ESDF distance values may face collision risks.

2.2. Path Planning

In an environment where pedestrians and mobile robots coexist, the uncertainty stemming from pedestrian motion adds a layer of complexity to path planning for mobile robots in dynamic settings. Researchers have accumulated rich expertise in this field [16,17,18,19]. Currently, path-planning algorithms for mobile robots are primarily categorized into traditional planning algorithms and learning-based path-planning algorithms.

Traditional path-planning algorithms can be divided into graph-based algorithms and sampling-based algorithms. Classical graph search algorithms include the Dijkstra algorithm [20] and the A* algorithm [21], along with its variations such as the D* algorithm [22] and Hybrid A* algorithm [23]. Sampling-based algorithms include the Rapidly-exploring Random Tree (RRT) algorithm [24] and its variant, the RRT* algorithm [25]. Traditional path-planning algorithms are contingent upon map generation, with the precision of the map playing a pivotal role in determining the efficacy of the resultant path. As artificial intelligence progresses, learning-based path-planning algorithms are increasingly emerging as a focal point of research interest [26,27,28,29]. Learning-based path-planning algorithms necessitate a considerable volume of annotated training datasets for the training of intelligent agents, with interpretability serving as a limiting factor in their practical implementation.

Existing mobile robot path planning algorithms currently lack consideration for real-time dynamic obstacle avoidance. Traditional planning algorithms such as A* and RRT operate in static maps. While learning-based methods have achieved some promising results in dynamic obstacle avoidance for mobile robots, they rely on extensive high-quality datasets and substantial computational resources, making them challenging to deploy on embedded platforms for mobile robots.

Our navigation system integrates the ESDF map construction method with planning algorithms. Our method is compared with related approaches as shown in

Table 1. Comparison with related approaches.

Scheme	Classification	Dynamic Obstacle Avoidance
A* [21]	model-based	✗
D* [22]	model-based	✗
TSN [30]	learning-based	✓
Hybrid-RL [31]	learning-based	✓
Ours	model-based	✓

. Our method is model-based, but unlike existing methods, our navigation system is a multi-level system with dynamic obstacle avoidance capability. The Differential ESDF map construction algorithm, vehicle model, and multi-level path planning framework are outlined, leading to the realization of a LiDAR-only ground mobile robot’s stable, safe, and robust autonomous navigation in a park environment.

3. Proposed System

Our navigation system is a hierarchical system, primarily divided into a 2D Differential ESDF map construction module, a planning module, and an input module.

3.1. System Framework

As shown in Figure 1, our system comprises three primary components. The input module integrates laser point cloud data and pose data, synchronized through timestamp alignment. The map module, pivotal in the system, consists of two key aspects: the first involves the establishment of a static global occupancy grid map by eliminating dynamic elements and extracting drivable regions [32]; the second encompasses the construction of a 2D Differential ESDF map, creating a local occupancy grid map from laser point cloud and pose data, followed by the application of the ESDF update algorithm to generate the 2D Differential ESDF Map. The planning module is segregated into a global planner and a local planner. The global planner employs the A* algorithm for global path planning on the global occupancy grid map, with resultant path points serving as targets for the local planner. The local planner determines the traversal path by evaluating the ESDF values and differential ESDF values of grids along candidate paths, ultimately translating the chosen path into control signals for the mobile robot.

3.2. The Mapping Module

In this section, we outline the process of real-time updating of a 2D Differential ESDF map, which involves three components: Firstly, we obtain a local occupancy grid map from the input data. Secondly, we perform ESDF updates on the acquired map, and finally, we calculate the Differential ESDF map through interpolation.

3.2.1. Occupancy Integration

Upon receiving each pose estimation and point estimation, a local occupancy grid map is generated. At the t-th data reception instance, the laser point cloud data are denoted by $P_t=\left\{p_t^1,p_t^2,···,p_t^M\right\}$, where M represents the total number of laser points within the range, and the robot’s pose is expressed by $R_t\in SO\left(3\right)$ and $t_t\in R^3$. To meet the navigation requirements of mobile robots and establish a 2D occupancy grid map for ESDF value updates, point cloud data filtering and occupancy status integration in the plane are imperative. Initially, employing the Equation (1):

$$\overline{p_t^i}=R_t\ast p_t^i+t_t$$

(1)

Points are transformed into the robot’s base coordinate system, eliminating points with heights below $hmin$ and filtering out ground laser points. Points exceeding the height threshold $hmax$, which impedes the robot’s movement, are also excluded. Subsequently, the space voxels corresponding to the filtered points are marked as occupied, and the occupancy information is integrated into the plane by designating plane grid cells with occupancy information within the specified height range as occupied, reducing the number of nodes in the subsequent ESDF update process and improving update efficiency. This intricate process is elucidated in Figure 2, with the resultant local occupancy grid map utilized for ESDF updates.

3.2.2. ESDF Updating

Our ESDF update algorithm is an improvement based on [15]. Our algorithm updates the ESDF value of a grid by calculating the shortest Euclidean distance between this grid and the closest obstacles among its neighbors. The ESDF value always represents an “Euclidean” distance. Our navigation system is applied to the autonomous navigation of ground mobile robots in park environments. The park environment has the following characteristics: the driving surface is relatively flat, static obstacles are scattered, and dynamic obstacles mainly consist of pedestrians and vehicles. Under these conditions, it is possible to map the mobile robot and obstacles onto a two-dimensional plane ($X-Y$) and perform ESDF updates on the obtained occupancy grid map.

The ESDF update algorithm defines the data structure shown in

Table 2. Data structures used in ESDF updating algorithm.

Name	Member	Abbreviation
grid	Postion ($x,y$)	pos
	Occupancy	occ
	ESDF	ESDF
	Closest Obstacle Grid	cog
	Observed	obs
	Double Linked List	dll
insertQueue	grid	InQ
deleteQueue	grid	DeQ
updateQueue	grid	UpQ

to store information. We have defined a structure called grid that includes multiple member variables. The position variable represents the x and y coordinates of the grid, the occupancy variable indicates the occupancy status of the grid, the ESDF variable stores the Euclidean distance from the current grid to its nearest obstacle grid, the Closest Obstacle Grid variable represents the x and y coordinates of the nearest obstacle grid to the current grid, the Observed variable is used to mark whether the grid has been observed, and the Doubly Linked List is used to store all grids for which the current grid is the nearest obstacle grid. When the current grid is not occupied, the corresponding Doubly Linked List is empty. We have defined three queues to store grids: the insertQueue stores grids that change from free to occupied status, the deleteQueue stores grids that change from occupied to free status, and after initialization, we merge the insertQueue and deleteQueue into the updateQueue.

The ESDF updating algorithm mainly consists of two parts: firstly, merging the insertQueue and deleteQueue into the updateQueue, and then updating the ESDF values of each grid in the updateQueue using a breadth-first search approach. The complete algorithm is shown in Algorithm 1.

Algorithm 1 ESDF Updating Algorithm

Input: InQ, DeQ Output: local ESDF map 1: while InQ is not empty do 2:     grid ← InQ.FRONT() 3:     InQ.POP() 4:     DELETE grid FROM grid.coc.dll 5:     grid.coc ← grid 6:     grid.ESDF ← 0 7:     PUSHBACK grid INTO UpQ 8: end while 9:   10: while DeQ is not empty do 11:     grid ← DeQ.FRONT() 12:     DeQ.POP() 13:     for each $\mathit{mem}\in grid.\mathit{dll}$ do 14:         DELETE mem FROM grid.dll 15:         mem.coc ← NULL 16:         mem.ESDF ←∞ 17:         for each $nbr\in mem.nbrs$ do 18:            if nbr.coc existing and DIST$(nbr.coc,mem)<mem.ESDF$ then 19:                mem.ESDF ← DIST(nbr.coc, mem) 20:                mem.coc ← nbr.coc 21:            end if 22:         end for 23:         PUSHBACK mem INTO UpQ 24:     end for 25: end while 26:   27: while UpQ is not empty do 28:     grid ← UpQ.FRONT() 29:     UpQ.POP() 30:     for each $nbr\in grid,nbrs$ do 31:         if then 32:            nbr.ESDF ← DIST(grid.coc, nbr) 33:            DELETE nbr FROM nbr.coc.dll 34:            $nbr.coc\leftarrow grid$ 35:            INSERT $nbr$ INTO grid.$dll$ 36:            PUSHBACK $nbr$ INTO $UpQ$ 37:         end if 38:     end for 39: end while

The system takes insertQueue and deleteQueue as inputs and outputs a local ESDF map. For each grid in the insertQueue, the algorithm removes it from the doubly linked list (dll) of the nearest obstacle grid, then sets the grid’s nearest obstacle grid to itself, sets the ESDF value to 0, and finally adds this grid to the updateQueue. For each grid in the deleteQueue, all grids in its dll will be iterated. For each grid in its dll, it is first removed from the dll, then its nearest obstacle grid is set to NULL, and its ESDF value is set to a large value. Considering its neighboring grids (set as 8-connected in our algorithm), if there is a neighboring grid with the nearest obstacle, the algorithm compares its ESDF value with the distance value of the nearest obstacle of the grid to its neighbor. If the ESDF value is greater than the distance value, the grid’s ESDF value is reset to this distance value, and the grid is added to the dll of the nearest obstacle of its neighbor. After traversing all neighboring grids, the grid is added to the updateQueue, with the specific process explained in Figure 3a. The ESDF update is carried out through a breadth-first search, as illustrated in Figure 3b. For each grid in the updateQueue, its neighboring grids are iterated over to compare its ESDF value with the distance to the nearest obstacle from the neighbor. When the ESDF value is greater than the distance value, updates are made to the grid’s nearest obstacle and ESDF value, followed by the addition of its neighbors to the updateQueue for traversal updates.

By utilizing the ESDF update algorithm, we can obtain a local two-dimensional ESDF map. Typically, in a park-like environment, moving obstacles are sparse and sparsely distributed. Thus, during each ESDF update process, the number of grid nodes added to the updateQueue is minimal. Our ESDF update algorithm can rapidly and stably construct a local ESDF map, maintaining the ESDF values of each grid effectively.

3.2.3. Differential ESDF Map

The primary objective of our algorithm is to generate a two-dimensional differential ESDF map. Traditional ESDF maps provide static information about the proximity of obstacles to grid cells and are typically used for distance gradient-based path planning algorithms [10] in static environments for mobile robots. While these algorithms are effective in ensuring safe and reliable planning in static settings, challenges arise when dynamic obstacles are present. The movement paths of dynamic obstacles may intersect with planned routes, leading to planning failures. In such scenarios, a manual definition of obstacle avoidance rules is often required to enhance path safety. However, this process demands substantial expertise and may consume significant computational resources for rule-based decision-making. In contrast, our proposed approach using differential ESDF maps eliminates the need for explicit rule setting. By incorporating the differential ESDF values as scoring criteria in the selection of candidate paths, our method enables effective and automatic obstacle avoidance for dynamic obstacles, enhancing the overall safety and efficiency of path planning algorithms.

The construction of a differential ESDF map primarily involves computing the differences in values. Consider an ESDF map with a grid size of $L\times L$, where grid cell $g_i$ represents a cell in the map, and i is the index of the grid cell. During the $t$-th update, the ESDF value of grid cell $g_i$ is denoted as $ESD{F}_i^t$, and during the $(t+1)$-th update, the ESDF value of grid cell $g_i$ is denoted as $ESD{F}_i^{t+1}$. $△T$ represents the time interval for receiving ESDF messages, and we can calculate the differential ESDF value of a grid cell using Formula (2):

$$△ESD{F}_i^{t+1}={\displaystyle \frac{ESD{F}_i^{t+1}-ESD{F}_i^t}{△T}}$$

(2)

The complete differential ESDF map construction algorithm is shown in Algorithm 2. In a 2D differential ESDF map, the differential ESDF values of grid cells represent dynamic information in the environment. When the differential ESDF value of a grid cell $g_i$ is greater than 0, it indicates that the distance from this grid cell to the nearest obstacle is increasing. Conversely, when the differential ESDF value of a grid cell $g_i$ is less than 0, it indicates that the obstacle is approaching this grid cell. Additionally, the magnitude of the absolute value of the differential ESDF value of a grid cell $g_i$ reflects the speed at which the obstacle is moving: a larger absolute value indicates a faster speed, while a smaller absolute value indicates a slower speed. Through analysis, we can conclude that the differential ESDF values of grid cells in the region where a moving obstacle is heading towards are less than 0, while the values in the region where the obstacle is moving away from are greater than 0. The movement speed of dynamic objects will affect the construction of the differential ESDF map. The faster the movement speed of dynamic objects, the greater the rate of change in ESDF values between two frames. As indicated by Formula (2), the absolute value of the differential ESDF value of a grid will also be larger.

Algorithm 2 2D differential ESDF map construction algorithm

Input: local ESDF map Output: 2D differential ESDF map 1: while local ESDF map is not empty do 2:     grid ← map.FRONT() 3:     map.POP() 4:     $ESD{F}_i^t$← the ESDF values of the grid at frame t 5:     $ESD{F}_i^{t+1}$← the ESDF values of the grid at frame $t+1$ 6:     $△ESD{F}_i^{t+1}$←$△ESD{F}_i^{t+1}={\displaystyle \frac{ESD{F}_i^{t+1}-ESD{F}_i^t}{△T}}$ 7: end while

3.3. The Planning Module

As an important part of the navigation system, path planning uses the existing map’s prior information and real-time sensor data to plan a safe, short and smooth path for the mobile robot to reach the goal area or goal point. As shown in Figure 4, our planning module is divided into two sub-modules, namely the global planner and the local planner.

The global planner generates way-points by planning on the global occupancy grid map, while the local planner uses these way-points as local target points for path selection and movement. The planning system we propose is designed for ground-autonomous mobile robots with an Ackermann chassis structure. The kinematic model for the robot’s motion is formulated as shown in Formula (3):

$$\left(\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right)=V\left(\begin{array}{c}cos\left(\theta \right)\\ sin\left(\theta \right)\\ {\displaystyle \frac{tan\left(\delta \right)}{L}}\end{array}\right)$$

(3)

where V represents the vehicle’s velocity, $\delta$ denotes the front wheel steering angle, $\theta$ signifies the vehicle’s yaw angle, and L stands for the length of the vehicle. The path selected by the local planner is mapped to velocity and steering signals for the robot’s movement control.

3.3.1. Global Planner

The Global planner sub-module plans a feasible path by receiving the target position on the global occupancy grid map. It combines the global occupancy grid map and occupancy grid-based planning algorithms. The occupancy grid map used for the Global planner is stored offline. We utilize the A* algorithm, a classic graph search algorithm that uses heuristic information to find an optimal path. When the planning module receives a final target point, the Global planner uses the A* algorithm to plan a series of way-points and outputs them to the Local planner sub-module, with these intermediate way-points serving as the target points for the Local planner.

3.3.2. Local Planner

The Local planner subscribes to the way-points published by the Global planner and selects the intermediate way-point closest to the moving robot as the target point for the Local planner. The Local planner scores candidate paths to choose the final driving path, with all candidate paths mapped to control actions for the robot and stored offline. The Local planner publishes the control actions corresponding to the final selected path. The Local planner is mainly divided into three parts.

Candidate Paths: Candidate paths are obtained through forward simulation to generate a series of discrete way-point trajectories that satisfy the robot’s kinematic constraints, representing possible future movements of the mobile robot. As shown in Figure 5, we obtained 729 candidate paths through data filtering and normalization.

Scoring Formula: Before scoring the candidate paths, we first eliminate paths that pass through obstacles. The discrete way-points of the candidate paths are mapped to an occupancy grid map. If a way-point projected from a candidate path falls into an occupied grid cell, the path is marked. For a marked path, we mainly use three scoring criteria for path selection, which is formulated as follows:

$$L_i={\omega }_1{S}_1+{\omega }_2{S}_2+{\omega }_3{S}_3\forall i=1,2,···,729$$

(4)

where $L_i$ is the total score of the $i$-th free path, ${\omega }_i,i=1,2,3$ are the weight coefficients, and $S_i,i=1,2,3$ are the $i$-th scoring criteria. The first scoring criterion $S_1$ tends to select paths that have endpoints closest to the target point, which is formulated as follows:

$$S_1=1-\sqrt{\sqrt{0.005\times {\theta }_i}}$$

(5)

$${\theta }_i=arccos{\displaystyle \frac{(p_{target}-p_{robot})·(p_{end,i}-p_{robot})}{∥p_{target}-p_{robot}∥∥p_{end,i}-p_{robot}∥}}$$

(6)

where ${\theta }_i$ is the angular difference between the robot’s current $p_{robot}$ to the endpoint $p_{end,i}$ of the $i$-th path and the target position $p_{target}$. A larger $S_1$ value indicates that the endpoint direction of the path is closer to the direction of the current target point, ensuring the shortest path for the robot to travel. The second scoring criterion $S_2$ ensures that the robot effectively avoids static obstacles, which is formulated as follows:

$$S_2={\displaystyle \frac{1}{n}}\sum _{j=1}^{n}ESD{F}_j$$

(7)

where n is the number of discrete road points in the $i$-th free path, and $ESD{F}_j$ is the ESDF value of the grid projected by the $j$-th way-point. The third scoring criterion $S_3$ ensures that the robot effectively avoids dynamic obstacles, which is formulated as follows:

$$S_3={\displaystyle \frac{1}{n}}\sum _{j=1}^n▵ESD{F}_j$$

(8)

where $▵ESD{F}_j$ is the differential ESDF value of the grid projected by the $j$-th road point. The final path of the mobile robot is the candidate path with the highest score, and the scoring formula will determine the selection of the final path. By adjusting the weight $\omega$, we can emphasize the importance of different scoring criteria. The first scoring criterion, $S_1$, tends to choose a path that is closest to the target point at the end. The second scoring criterion, $S_2$, ensures the effectiveness of the robot in avoiding static obstacles. The third scoring criterion, $S_3$, ensures the effectiveness of the robot in avoiding dynamic obstacles. We adjust the weights to make the scoring lean more towards the corresponding scoring criteria, thereby ensuring that the robot has a safe and stable obstacle avoidance effect for both static and dynamic obstacles. The complete algorithm is shown in Algorithm 3.

Algorithm 3 local planner

Input: the current target point Output: v, ω 1: while $dis(robot,targetpoint)<distance$ do 2:     for each $pat{h}_i$∈ candidate paths do 3:         Initialize $L_i$, set $S_1$, $S_2$ and $S_3$ to zero 4:         $S_1=1-\sqrt{\sqrt{0.005\times {\theta }_i}}$ 5:         for each $poin{t}_j$∈$pat{h}_i$ do 6:            $gri{d}_j$←$poin{t}_j$ position 7:            $S_2$ += $gri{d}_j.ESDF$ 8:            $S_3$ += $gri{d}_j.△ESDF$ 9:         end for 10:         $L_i={\omega }_1{S}_1+{\omega }_2{S}_2+{\omega }_3{S}_3$ 11:     end for 12:     $L_{max}$ is the maximum value in $L_i$ 13:     v←$L_{max}.v$ 14:     $\omega$←$L_{max}.\omega$ 15: end while

Replanning: Since our navigation system lacks feedback information on control actions, a replanning procedure is set up to ensure that the mobile robot does not deviate from the intended position due to open-loop control. The replanning procedure operates at a certain frequency and determines whether to trigger replanning based on the current position of the robot and the position of the target point. When the distance between the current position and the target point exceeds a set threshold, the replanning procedure is executed. The current position is taken as the starting point, and the closest intermediate way-point to the starting point is selected as the target point. Subsequently, candidate path scoring and path selection are performed again.

4. Experiment

4.1. 2D ESDF Map

As described in Section 3.2, the ESDF map construction algorithm we proposed can build local two-dimensional ESDF maps in real-time. First, we receive laser data with timestamps and robot pose data and synchronize the two input data by aligning the timestamps. Then, we set the voxels where the laser data are located in the world coordinate system as obstacles to updating and maintaining the subsequent two-dimensional ESDF map. To test our algorithm, we collected experimental data with a mobile robot in a park environment and tested our algorithm on some public datasets. We use the cow and lady dataset to test our mapping algorithm. The cow and lady dataset is collected using an RGB-D camera and mainly includes depth information and pose information. It is encapsulated into an ROS bag file for easy access and utilization. The experimental results of the dataset “cow and lady” are shown in Figure 6. Figure 6a shows the display of the laser point cloud in the scene, where different colors represent different heights of the laser point cloud. Figure 6b displays the results of the generated two-dimensional ESDF map, with grid colors ranging from red to green indicating increasing ESDF values for the corresponding grids. The results indicate that our algorithm can efficiently and stably build two-dimensional ESDF maps, and update and maintain ESDF values in the current area in real-time, and the planning module can utilize the stored ESDF information for planning purposes.

The “cow and lady” dataset is designed for indoor scenes with a large number of point clouds, showing excellent mapping results. To test our algorithm, we collected data in a park environment. We record our bag file using ROS by controlling a mobile robot. Our mobile robot is equipped with a 16-beam LiDAR sensor and an RTK (Real-Time Kinematic) system. The RTK is used to provide real-time pose information. The experimental results in the park environment are shown in Figure 7. The results demonstrate that our algorithm can quickly and efficiently build a local ESDF map. The established local ESDF values will be published to the local planner for subsequent path selection.

4.2. 2D Differential ESDF Map

We tested our algorithm for constructing 2D differential ESDF maps in a simulated environment. The constructed simulation environment mainly consists of a pedestrian model moving at a constant speed and a simulated 16-beam Lidar model to test the influence of moving obstacles on the rate of change of ESDF values in local scene grids. To represent the impact of obstacle movement on the ESDF values in the scene, we only consider the positive and negative differential ESDF values of the grids. The grid size also affects the local differential ESDF map. When the grid size is too small, the movement of obstacles will increase the number of occupied grids projected downward, leading to confusion in the local differential ESDF values. In our experiment, we set the grid size to be $0.3\times 0.3$, and the experimental results are shown in Figure 8.

4.3. Simulated Dynamic Obstacle Avoidance Experiment

We have built a simulated experimental environment in Gazebo 9, as shown in Figure 9. The environment includes some static obstacles and a pedestrian model moving at a constant speed. We have also built a mobile car model with an Ackermann chassis, equipped with a 16-beam Lidar sensor for real-time Lidar data input. The ROSp3d plugin is embedded in the model to publish the real-time pose data of the mobile car for pose input to the system. The simulation experiments are conducted on a laptop equipped with an AMD R7-5800H 3.0 GHz processor and 32 GB of memory.

We achieve autonomous navigation of the simulated mobile robot by publishing a final target point in the simulation scene. The weight coefficients in the path scoring Formula (4) affect the final decision of the local planner. As analyzed in Section 3.3.2, the scoring item $S_1$ prioritizes the shortest path from the current position to the local target point, $S_2$’s scoring item incorporates dynamic obstacle avoidance capability, and $S_3$’s scoring item pushes the selected path away from static obstacles, compensating for the trajectory’s neglect of static obstacles due to excessive consideration of dynamic obstacles. For our proposed system, we demonstrated the effectiveness of our method through a simulated comparative experiment. We set the weight coefficients ${\omega }_2$ and ${\omega }_3$ to 0 and ${\omega }_1$ to 10 in one set of experiments; in another set, we set ${\omega }_1$ to 10, ${\omega }_2$ to 1, and ${\omega }_3$ to 1. As shown in Figure 10, the experiment group without the differential ESDF scoring item may encounter dangerous situations such as collisions, while the group with the differential ESDF has a good dynamic obstacle avoidance effect.

The scoring item $S_2$ deviates the selected path from the direction of dynamic obstacles, and when its value is too large, it increases the travel cost of the mobile robot and may even increase the risk of collision with static obstacles in dense environments. To obtain a safe and robust navigation system, we need to balance these weight coefficients. We conducted a simulation experiment to determine the most stable set of weight coefficients. The weight coefficients for all experimental groups are shown in

Table 3. The weight coefficients used in the three control experiments.

	${\mathit{\omega }}_1$	${\mathit{\omega }}_2$	${\mathit{\omega }}_3$
group1	10	1	1
group2	10	0.5	1
group3	10	1	0.5

, and the experimental results are shown in Figure 11. By selecting the most robust weight coefficients, we conducted actual navigation experiments.

We characterize the effect of dynamic obstacle avoidance by the maximum distance of the moving routes of mobile robot and pedestrian models. The experimental results are shown in the

Table 4. The maximum safety distance in the three control experiments.

	${\mathit{\omega }}_1$	${\mathit{\omega }}_2$	${\mathit{\omega }}_3$	${\mathit{L}}_{\mathbf{max}}$
group1	10	0	0	0.467
group2	10	1	1	1.692
group3	10	0.5	1	1.690
group4	10	1	0.5	1.396

. The maximum distance value after adding the differential ESDF scoring item is 1.69, which is an increase of 1.223 in safety distance compared to the system without dynamic obstacle avoidance.

The experiment indicated that when parameters ${\omega }_1=10$, ${\omega }_2=0.5$, and ${\omega }_3=1$, the mobile robot demonstrated the most robust navigation performance. At this setting, the robot’s trajectory deviates from static obstacles in the environment, while exhibiting good avoidance behavior towards dynamic obstacles. We will further test our navigation system’s obstacle avoidance performance in real-world scenarios using this parameter configuration.

4.4. Navigation Experiment in Real-World Scenarios

To test the autonomous navigation performance of our navigation system in a real park environment, we will deploy our algorithm on a real mobile robot model for testing. The mobile robot model, as shown in Figure 12, features an Ackermann drive chassis, equipped with a 16-line 3D LiDAR sensor and a high-precision vehicle-mounted positioning module. The central processing unit is a laptop computer with an AMD R7-5800H 3.0 GHz processor, used for communication with the chassis. We tested our navigation algorithm in a campus park environment, where static obstacles such as flower beds and dynamic obstacles, mainly represented by pedestrians and vehicles, are present.

As shown in Figure 13, when the mobile robot equipped with our navigation algorithm travels on park roads, it will choose to drive in areas far from the road edges. During the driving process, the navigation algorithm considers the edges of the road as static obstacles, and the candidate scores will select paths that deviate from these static obstacles. This ensures the safety and stability of park navigation in typical environments.

To test the dynamic obstacle avoidance performance of the navigation algorithm in real scenarios, we implement an interaction between an actual pedestrian model and the mobile robot. During the experimental tests, we ensured that the direction of movement of pedestrians intersected with the direction of movement of the robot. The movement speeds of pedestrians and the robot were maintained at normal values. Specifically, in the tests, the walking speed of pedestrians was set at the normal adult walking speed of 0.8 m/s, while the movement speed of the mobile robot was set at 1 m/s. As shown in Figure 14a, the pedestrian’s movement direction will block the robot’s original movement direction, leading to collision events. In Figure 14b, by adding the ESDF value and differential ESDF value to the candidate scoring formula, the mobile robot will ultimately choose a path deviating from the direction of pedestrian movement, thereby achieving a more robust dynamic obstacle avoidance capability.

As shown in Figure 15, our navigation algorithm maintains good performance when facing narrow road situations. The local path planner eliminates candidate paths that may lead to collisions when selecting a driving path. Additionally, due to the inclusion of the ESDF term in the scoring mechanism, the selected path tends to deviate from static obstacles, allowing for good navigability even in narrow road conditions.

5. Conclusions

We have proposed a multi-layer planning mobile robot navigation system for park environments, which can achieve safe and stable navigation as well as robust dynamic obstacle avoidance. The upper-level planner plans a series of way-points using the A* algorithm based on an offline stored occupancy grid map and sends them to the lower-level planner. The lower-level planner receives real-time pose inputs and laser scanner inputs and uses an ESDF mapping algorithm to construct a 2D ESDF map and a 2D differential ESDF map as the map module. It selects the closest way-point from the upper level as the local target point and completes local planning through scoring candidate paths. We conducted a series of simulation experiments and real-world experiments, and the results show that our navigation system can safely and robustly complete navigation tasks.

In future work, the navigation system will rely on the establishment of ESDF maps and candidate path clusters. Developing a safe and robust campus navigation framework requires a fast and accurate ESDF map construction algorithm. Future research can focus on the development of more efficient and accurate mapping algorithms, with a particular emphasis on ESDF map construction algorithms. Additionally, research efforts can be directed toward enabling the rapid querying of ESDF values at the current location.

Candidate path clusters impact the final navigation performance of mobile robots, and the quality of navigation is closely related to the number of candidate path clusters. Candidate path clusters represent the future travel paths of mobile robots. Excessive addition of candidate path clusters can increase the computational burden. Alternatively, optimization-based methods can optimize and fit a single path numerically, with the differential ESDF serving as the optimization objective. This represents a potential future research direction for improving navigation efficiency.