Fast and efficient indoor navigation: a hybrid pathfinding approach using rapidly-exploring random tree (RRT)-connect and Dijkstra’s algorithm

Dijikstra’s algorithm

Dijkstra’s algorithm is one of the most well-known and widely used algorithms for finding the shortest path between two points on a graph. It guarantees finding the optimal solution by exhaustively exploring every possible path until the shortest one is determined. It has been applied extensively in 2D environments, such as road networks and basic indoor navigation.

However, its exhaustive nature becomes a limitation when applied to more complex, high-dimensional environments. In multistory buildings or environments with dynamic obstacles, Dijkstra’s algorithm suffers from excessive computation time and memory consumption. Furthermore, since the algorithm explores all nodes, it struggles with real-time efficiency in scenarios where fast responses are critical.

A* algorithm

A* builds upon Dijkstra’s algorithm by introducing a heuristic function to guide the search process more efficiently. It reduces computational overhead by focusing on the most promising nodes, balancing exploration and path cost. A* is faster than Dijkstra in practice for most applications and has been applied in many navigation systems, particularly where obstacles are sparse and the environment is mostly static.

However, like Dijkstra’s algorithm, A* faces challenges in environments with multiple levels and dynamic obstacles, particularly when the heuristic used is not well-suited to the problem space. Moreover, A* can struggle with memory efficiency in large environments due to its reliance on storing the open and closed lists.

Rapidly-exploring random trees (Algorithm 1)

RRT is a more recent algorithm designed to tackle the shortcomings of traditional pathfinding methods in high-dimensional and dynamic spaces like robots or autonomous driving path findings. RRT works by incrementally building a search tree by randomly sampling the environment, expanding the tree toward each sample. Unlike Dijkstra and A*, RRT does not attempt to explore the entire graph exhaustively but focuses on expanding the tree based on random exploration. Furthermore, In Dijkstra’s algorithm, the graph is predefined and fully explored, with edges representing the known connections between nodes. Meanwhile, In RRT the graph is built incrementally by adding random samples as new nodes and connecting them to the nearest existing nodes in the tree. As shown in Fig. 1. It is exploratory and adapts based on the configuration space without needing the entire graph structure beforehand (Martinez, Jacinto & Montiel, 2023).

The basic steps are as follows:

1. It constructs a random tree at the starting point $X_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}$ of the two-dimensional state space as the root node;

2. A random sampling point $X_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ is generated in the free search space and used to guide the expansion of the random tree;

3. After traversing the nodes that have been generated in the whole random tree, the tree node that is closest to the random point $X_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ is found, selected, and defined as $X_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$;

4. From the node $X_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$, along the extension direction of the node $X_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ as the extension direction, the appropriate step size is expanded, and the appropriate step size is set as the branch length to generate a new node $X_{\mathrm{n}\mathrm{e}\mathrm{w}}$ as the new tree node;

5. If an obstacle is encountered in the expansion process, the expansion is canceled, and sampling is performed again. The path repeats the above iterative process until the target node exceeds the specified number of iterations and finally forms a fast-expanding random tree path, ending the search.

Wang et al. (2023), Brad & Dolha (2021).

RRT has become a popular choice in robotics, automated navigation, and multilevel environments due to its ability to quickly generate paths in complex spaces. However, the basic RRT algorithm does not guarantee an optimal solution and can suffer from slow convergence in certain scenarios.

RRT-Connect and bi-directional RRT

RRT-Connect is an extension of the RRT algorithm that attempts to improve the convergence speed by growing two trees simultaneously: one from the start point and one from the goal. The trees explore the environment independently but attempt to connect to each other as they grow, significantly reducing the time needed to find a feasible path. Bi-directional RRT works similarly, expanding the search from both the start and goal nodes simultaneously but is more efficient in larger environments where growing a single tree takes too long (Fan et al., 2024).

As shown in Fig. 2. The RRT-Connect algorithm grows two trees: one from the start position $q_{start}$ and the other from the goal position $q_{goal}$. The goal is to connect the two trees to find a collision-free path.

The RRT algorithm attempts to find a path by sampling random configurations in the space and then extending the tree to include these samples. The tree grows from an initial point $q_{start}$, and in each iteration, a random configuration $q_{rand}$ is generated. The algorithm then extends the tree by moving a step from the nearest node $q_{near}$ towards $q_{rand}$, but stopping if a collision occurs.

The extension function is given as:

(1) $$q_{\mathrm{n}\mathrm{e}\mathrm{w}}=q_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}+\epsilon \frac{(q_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}-q_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}})}{\Vert q_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}-q_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\Vert }$$where:

• $q_{\mathrm{n}\mathrm{e}\mathrm{w}}$ is the new node,

• $\epsilon$ is the step size,

• $q_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ is the random configuration, and

• $q_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ is the nearest node in the tree to $q_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$.

RRT-Connect extends this by simultaneously growing two trees: one from the start and one from the goal. The trees will attempt to connect to each other as they grow. In each iteration, the algorithm grows tree $T_a$ from $q_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ towards the random configuration $q_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$, and then grows tree $T_b$ from $q_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}$ towards $q_{\mathrm{n}\mathrm{e}\mathrm{w}}$, the new node in tree $T_a$.

The extension of tree $T_b$ towards tree $T_a$ is done using the same extension function as Eq. (1), but with the trees reversed:

(2) $$q_{\mathrm{n}\mathrm{e}\mathrm{w}}^{\mathrm{\prime }}=q_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}^{\mathrm{\prime }}+\epsilon \frac{(q_{\mathrm{n}\mathrm{e}\mathrm{w}}-q_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}^{\mathrm{\prime }})}{\Vert q_{\mathrm{n}\mathrm{e}\mathrm{w}}-q_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}^{\mathrm{\prime }}\Vert }$$where:

• $q_{\mathrm{n}\mathrm{e}\mathrm{w}}^{\mathrm{\prime }}$ is the new node added to tree $T_b$,

• $q_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}^{\mathrm{\prime }}$ is the nearest node in tree $T_b$ to $q_{\mathrm{n}\mathrm{e}\mathrm{w}}$, and

• $q_{\mathrm{n}\mathrm{e}\mathrm{w}}$ is the new node generated in tree $T_a$.

If $q_{\mathrm{n}\mathrm{e}\mathrm{w}}$ from tree $T_a$ and $q_{\mathrm{n}\mathrm{e}\mathrm{w}}^{\mathrm{\prime }}$ from tree $T_b$ are close enough, the trees are connected, and a feasible path has been found.

In mathematical terms, the two trees are connected when:

(3) $$\Vert q_{\mathrm{n}\mathrm{e}\mathrm{w}}-q_{\mathrm{n}\mathrm{e}\mathrm{w}}^{\mathrm{\prime }}\Vert <\delta$$where $\delta$ is a predefined threshold for connection.

The computational complexity of RRT-Connect depends on the number of nodes generated in both trees, which is often improved by utilizing parallel processing techniques such as CPU parallelization (Hidalgo-Paniagua et al., 2018).

These algorithms are well-suited for real-time applications in dynamic environments, where traditional algorithms like Dijkstra and A* are too slow. Furthermore, by leveraging parallel processing such as CPU parallelization the performance of these algorithms can be significantly improved, enabling their application in real-time scenarios like complex indoor navigation.

Finally, overall overview of pathfinding algorithms is summed up, as shown in Fig. 3.

Excessive computation time

Dijkstra and A* tend to explore nodes exhaustively, which becomes computationally expensive in large graphs.

Inability to scale

As the number of nodes and levels increase (such as in multistory buildings), these algorithms struggle with efficiency and memory usage.

Lack of adaptability to dynamic environments

When dealing with environments that may change frequently (such as obstacles appearing or disappearing), traditional algorithms face challenges in re-computing paths efficiently.

CPU parallelization

In environments where real-time performance is critical, the RRT-Connect algorithm can be parallelized to distribute the task of node expansion across multiple CPU cores. Each core handles the expansion of a section of the tree, performing operations such as:

• Sampling a random node

• Checking for obstacles

• Extending the tree toward the node

By distributing these tasks across multiple CPU cores, we can significantly reduce the time taken for the algorithm to find a valid path.

This allows for rapid node exploration and tree growth, particularly in environments where fast response times are required, such as real-time navigation in crowded spaces.

Node storage

Each node in the spatial database is represented by its coordinates (X, Y, Z for 3D spaces, or just X, Y for 2D environments). In a multilevel building, for example, nodes could represent:

• Rooms on different floors

• Staircases or elevators connecting floors

• Hallways or corridors

Path storage

Paths between nodes represent the edges in the graph, with each path having an associated distance and other attributes, such as:

• The time it takes to traverse the path

• The conditions of the path (e.g., obstacles, blocked routes)

The database is structured to allow for fast querying and updating of nodes and paths, particularly in environments where the conditions may change dynamically.

Database query optimization

To ensure efficient querying, the spatial database employs advanced indexing techniques such as:

• R-trees: Used to index multi-dimensional data, allowing for efficient spatial queries.

• Quadtree structures: Help partition the space into manageable sections, enabling faster lookup of nodes and paths.

By optimizing the database for fast lookups and updates, the system can handle dynamic changes in the environment, such as new obstacles appearing, in real-time without compromising on performance.

Obstacle handling

The system detects obstacles (e.g., blocked hallways, staircases, or elevators) and updates the database to mark these paths as unavailable. The pathfinding algorithm then recalculates the optimal route based on the updated environment.

Real-time path updates

When a dynamic change is detected, such as a new obstacle in the path, the algorithm uses the information stored in the database to recalculate the best path in real-time, ensuring that users are always provided with the most efficient route.

Backend system

The backend system is the core component of the pathfinding operations, integrating the algorithm and parallel processing to navigate complex indoor environments.

Frontend system

The frontend system is built with NextJs to provide a seamless, interactive user interface for real-time navigation updates.

API design

RESTful APIs. The API layer is built using Django (or Flask/Node.js alternatives) to handle requests from the frontend and communicate with the backend for pathfinding operations. These APIs manage user requests, retrieve optimal paths, and send updates to the frontend in real time.

Pathfinding request and response structure. The API processes requests from the NextJs frontend, which may include user-selected start and end points or dynamic updates from the environment (e.g., newly detected obstacles). The API then communicates with the backend to trigger the RRT-Connect and Dijkstra’s algorithms, sending the optimal path data back to the frontend for visualization.

CPU parallelization

By employing multithreading, the algorithm can concurrently process multiple nodes, expanding the search space and discovering new paths more rapidly. This speeds up path exploration, reducing overall computational time. Tasks such as node sampling, obstacle checking, and tree expansion are distributed across multiple CPU cores. This parallel processing ensures efficient utilization of hardware resources, accelerating the pathfinding process and allowing the system to handle more complex environments (Zhang et al., 2023; Pérez-Higueras et al., 2019).

Real-time recalculation

Once environmental changes are detected, recalculations of paths are parallelized. This allows for rapid path adjustments using the updated data from the spatial database, ensuring that real-time navigation is responsive and efficient. The results are shown in Fig. 11.

Node and path storage

In the implementation, nodes representing key elements such as rooms, elevators, and stairs are stored in a structured spatial database with their 3D coordinates (X, Y, Z). These nodes are linked to paths, which represent the traversable routes between locations. This ensures that the system efficiently manages navigation data in complex, multi-story buildings (Mohanty & Parhi, 2013).

To optimize the performance of querying and storing data, the spatial database leverages R-tree indexing. This ensures that node and path retrievals are fast, which is essential for real-time operations. The R-trees help manage spatial data more effectively, particularly when dealing with large and complex environments like multi-level structures (Mohanty & Parhi, 2013). The Entity Relationship Diagram of spatial database is shown in Fig. 12.

Dynamic environment handling

The database is designed to dynamically handle changes in the environment. For example, when an obstacle blocks a path, the system updates the node and path data in real-time (Zhang et al., 2023). This capability ensures that the navigation algorithm always has access to the latest information, allowing it to adjust the routes dynamically and avoid obstacles. The implementation supports rapid querying and efficient updates to the node and path data.

Path management

The interface allows administrators to efficiently generate, save, and validate paths within the system. Using the RRT-Connect algorithm, administrators can dynamically create predefined paths between nodes, which are then stored in the database for future use. This reduces computational overhead by allowing pre-generated paths to be reused instead of recomputing them for every query. The screen view is shown in Fig. 13.

• Generate and save paths: Paths can be generated between selected nodes using the RRT-Connect algorithm and stored persistently.

• Load predefined paths: Administrators can retrieve and use previously stored paths to optimize the system’s real-time performance.

• Validation and consistency checks: Ensures that generated paths are traversable, free from obstacles, and comply with optimal navigation standards.

Drawing and editing tools

To further enhance usability, the Administrator Interface includes several drawing and customization tools for fine-tuning the indoor map representation.

• Drawing tools: Includes pen, line, rectangle, and eraser tools for marking obstacles, open paths, and restricted zones.

• Brush customization: Allows administrators to adjust brush size and color selection for precision in marking critical zones and boundaries.

• Undo/redo and clear functionality: Enables easy modifications and corrections to the navigation layout without the need for manual reconfiguration.

Path validation and Dijkstra’s algorithm execution

Once paths are generated, the system offers path validation tools to ensure accuracy and reliability. Administrators can execute Dijkstra’s algorithm on selected node pairs to compute the shortest path for further optimization.

• Path validation: The system checks generated paths for continuity, traversability, and collision avoidance.

• Dijkstra’s algorithm: If multiple paths exist between two points, the administrator can compute the shortest and most efficient path.

Grid and node selection

A grid-based overlay is included in the Administrator Interface to support systematic selection of nodes and structured navigation planning.

• Grid overlay for node selection: The interface allows structured selection of start and goal nodes by overlaying a grid on the map.

• Node naming and identification: Administrators can assign meaningful labels to nodes (e.g., “Exit A”, “Staircase 1”, “Room 205”) to enhance usability.

• Saving path configurations: Named nodes and generated paths can be stored, reducing the need for repeated manual configurations.

RRT-connect (rapidly-exploring random tree-connect)

RRT-Connect extends the standard RRT by growing two trees simultaneously from the start and goal, aiming to reduce exploration time.

• Strengths: Faster than standard RRT, suitable for high-dimensional environments, finds feasible paths quickly.

• Weaknesses: Does not guarantee optimal paths, can generate inefficient routes, computationally expensive with obstacles.

Lazy Theta*

Lazy Theta* is a variant of A* that optimizes node expansions by incorporating line-of-sight checks for direct connections. The result is shown in Fig. 16.

• Strengths: More efficient than A*, reduces node exploration, produces smoother paths.

• Weaknesses: Slower in constrained environments, struggles with narrow passages, high memory usage.

Probabilistic RoadMap

Probabilistic RoadMap (PRM) constructs a roadmap by sampling the free space and then applying a shortest-path algorithm. The result is shown in Fig. 17.

• Strengths: Efficient in high-dimensional spaces, fast query times, suitable for precomputed maps.

• Weaknesses: Requires many samples, unsuitable for dynamic environments, lacks adaptability.

Particle swarm optimization for path planning

Particle swarm optimization (PSO) models paths as particles moving in space and optimizes based on local and global solutions. The result is shown in Fig. 18.

• Strengths: Avoids local minima, effective for complex optimization problems, suitable for global planning.

• Weaknesses: Slow for real-time pathfinding, converges slowly, struggles with dynamic obstacles.

Hybrid (RRT-Connect + Dijkstra)

This hybrid approach leverages RRT-Connect for rapid exploration and Dijkstra’s algorithm for refining optimal paths. An admin panel allows easy environmental modifications.

• Strengths: Balances exploration and optimality, adaptable to dynamic environments, efficient with parallel processing.

• Weaknesses: More computationally intensive in initial iterations, paths may require smoothing.