Improved Two-Stage Theta* Algorithm for Path Planning with Uncertain Obstacles in Unstructured Rescuing Environments

Abstract

Path planning aims to find a safe and efficient path from a starting point to an end point, and it has been well developed in fields such as robot navigation, autonomous driving, and intelligent decision systems. However, traditional path planning faces challenges in an uncertain rescuing environment due to limited sensing range and a lack of accurate obstacle information. In order to address this issue, this paper proposes an improved two-stage Theta* algorithm for handling multi-probability obstacle scenarios in unstructured rescue environments. First, a global probability raster map is constructed by integrating historical maps and expert prediction maps with probability weights quantifying the uncertainty in the spatial and temporal distribution of obstacles. Second, a probability-sensitive heuristic function (PSHF) is designed, and a Sigmoid function is used to map the probability field of obstacles, thereby enabling limited penetration in low-risk areas and enforced avoidance in high-risk areas. Furthermore, a multi-stage line-of-sight detection optimization mechanism is proposed, which combines probability soft threshold penetration and backtracking verification to improve the noise robustness. Finally, a hierarchical planning architecture is constructed to separate global probabilistic guidance from local strict obstacle avoidance, ensuring both the global optimality and local adaptability of the path. Extensive simulation results in mine rescue scenarios demonstrate that the proposed method achieves lower path cost and fewer path nodes compared to traditional A*, Dijkstra, and Theta* algorithms, while significantly reducing local replanning overhead and maintaining stable performance across multiple uncertain environments.

1. Introduction

Robotic technology has experienced rapid development in recent years, with broad applications in industrial automation, logistics, medical assistance, and disaster response. Especially in disaster scenarios [1], rescue robots have gradually taken on key tasks such as rubble search and rescue, dangerous goods disposal, and disaster mapping by relying on their risk-resistant capabilities and precise operation characteristics, becoming the “second hands” for humans to deal with extreme environments, disaster response, and search and rescue tasks [2].

Path planning [3] is a fundamental capability for rescue robots, as it directly affects mission success and operational safety. The goal of rescue robotic path planning is to find a safe and efficient moving path from the starting point to the target point, which not only requires a reasonable cost of the path, but also takes into account planning efficiency and execution stability. By optimizing and adjusting the path nodes, unnecessary turning can be further reduced, planning efficiency can be improved, and reliable operation of the robot in a complex environment can be ensured. The advent of various path planning algorithms has been driven by the need for efficient and fast route-solving in rescue robotics. Notwithstanding these advancements, their application to uncertain and unstructured environments presents significant challenges in terms of computational speed and planning efficiency, with a non-negligible risk of planning failure [4]. Consequently, the inability to achieve both rapid and highly efficient path planning remains a critical hindrance to the more extensive application of rescue robots. Improving the adaptability and robustness of traditional path planning algorithms for complex and uncertain environments has become the key to solving the problem of autonomous navigation for rescue robots.

After decades of development, path planning methodologies for rescue robots can be categorized into three main technical approaches: search-based, sampling-based, and intelligent bionic learning algorithms, as illustrated in Figure 1. Among these three types in Figure 1, the sample-based path planning algorithm builds feasible paths by randomly generating and connecting nodes in the configuration space, and is especially good at dealing with high-dimensional and complex constrained environments. The Probabilistic Roadmap Method (PRM) algorithm, discussed in [5], generated a large number of random nodes and initially connected them with a local planner to form a probabilistic roadmap, and then it employed a graph search to obtain the shortest path. An improved version of the PRM* [6] introduced asymptotic optimality, guaranteeing that paths converged to the global optimum as the number of samples increased. The Rapidly Exploring Random Tree (RRT) algorithm constitutes another prominent sampling-based search method for robot path planning. Ref. [7] explored unknown space through random tree growth, which was highly efficient, but path quality was significantly affected by randomness. Subsequent improvements were widely developed based on RRT algorithms, such as RRT-Connect [8], employing bidirectional search to accelerate convergence; Informed RRT* [9] sampling optimized paths in an elliptic domain; and FMT* (Fast Marching Trees) [10] extending balance efficiency and optimality through delayed collision detection and cost driving. Despite their probabilistic completeness, sampling-based algorithms face significant challenges in uncertain, unstructured rescue environments. Their random sampling is inefficient for mapping complex clutter and narrow passages, while their typical assumption of a static world makes them brittle against dynamic obstacles. Moreover, the slow convergence to optimality often produces impractically long and jerky paths, which are difficult to execute reliably with real-world robots under efficient online adaptation.

Learning algorithms based on intelligent bionics are another type of optimization method that optimizes the planning path of rescue robots by simulating natural behavior or cognitive mechanisms. These approaches encompass both swarm intelligence optimization methods, such as Genetic Algorithm (GA) [11], Ant Colony Optimization (ACO) [12], and Particle Swarm Optimization (PSO) [13], as well as methods based on neural networks [14] and deep reinforcement learning methods [15]. However, swarm intelligence algorithms, such as GA and PSO, often suffer from high computational cost and slow convergence, making them impractical for time-sensitive rescue operations. Regarding reinforcement learning methods, it has become an active and influential research trend in recent years, which has been applied to emergency rescue vehicles on congested urban arterial roads [16], maritime search and rescue [17,18,19], urban pluvial flood rescue [20,21], and mine search and rescue [22]. However, applying DRL to safety-critical, uncertain, and highly non-structured environments, such as post-disaster search and rescue, still faces several inherent challenges:

(1)

Sample inefficiency and sim-to-real gap: DRL typically requires vast amounts of training data/interactions. Realistic simulation of all possible rubble configurations and stochastic collapses is extremely difficult, and the policy learned in a simplified simulator may not transfer reliably to the real, complex disaster site.

(2)

Lack of formal safety guarantees: DRL policies are often “black boxes,” making it hard to provide probabilistic completeness or any worst-case performance guarantees, which are paramount when human lives or expensive equipment are at risk.

(3)

Online adaptation to new maps: A trained RL policy is usually tied to a specific environment distribution. In a novel, unseen disaster scene (a new “map”), it may fail or require costly re-training/adaptation, whereas classical planners can immediately reason about a newly provided map.

Graph search-based algorithms constitute an important class of methods in robotic path planning for deterministic environments, due to their computational efficiency and optimality guarantees. This type of algorithm is also the focus of our paper. The evolution of graph search-based algorithms for robotic path planning begins with the Dijkstra algorithm [23], which serves as the fundamental benchmark. As a global and non-heuristic method, Dijkstra systematically explores all possible directions from the start node, guaranteeing the shortest path to any reachable goal in a static, known environment. Subsequent studies improved its efficiency through data structure optimization (such as Fibonacci heap [24]) and heuristic weight updating [25]. However, its lack of directional bias results in significant computational inefficiency, as it expends resources exploring areas irrelevant to the goal. A major leap forward was achieved with the introduction of the A* algorithm [26], which ingeniously combines the actual cost from the start node, g(n), with a heuristic estimate to the goal, h(n). This heuristic guidance, encapsulated in the cost function f(n) = g(n) + h(n), directs the search towards the goal, dramatically narrowing the search scope and improving efficiency. Provided the heuristic is admissible—meaning it never overestimates the true cost—A* maintains the guarantee of finding an optimal path. Consequently, A*, as well as bidirectional A* [27], has established itself as the cornerstone of heuristic search for static global path planning. Nevertheless, conventional graph search algorithms, including the widely used A*, produce paths that are confined to grid nodes. This confinement introduces redundant inflection points, causing navigational inefficiencies and escalated energy consumption for rescue robots. Based on the A* algorithm, the Theta* algorithm [28] addresses a different limitation inherent to grid-based planners like A*: the sub-optimal, jagged “grid-path” effect. The algorithm, with its line-of-sight detection mechanism [29], effectively addresses this drawback by facilitating direct connections across grid boundaries, thereby markedly improving path smoothness. This attribute is especially critical in rescue contexts, where continuous operation and energy limitations are paramount [30]. Thus, the smoothness and concomitant advantages provided by Theta* constitute the compelling reasons for its selection as a foundational algorithm in our research.

In recent years, Theta* algorithms have been widely employed in unmanned aerial vehicles [31], unmanned surface vehicles [32], and deep-sea mining vehicles [33]. However, the absence of a deterministic environmental map is a common challenge in rescue scenarios, resulting from limited sensor capabilities and missing information. Generally, the uncertain environment is often characterized by multiple subjective probabilistic environmental maps, generated from pre-disaster data and prior knowledge through sampling and scene reduction technology. Undertaking path planning within this framework of environmental uncertainty thus becomes a task of critical significance. Although the Theta* algorithm excels in generating optimal, any-angle paths within perfectly known environments, its core reliance on precise line-of-sight checks becomes a critical weakness in uncertain settings. The algorithm is highly vulnerable to even minor map inaccuracies, as a single unmapped obstacle can invalidate an entire planned segment, leading to catastrophic failure. Furthermore, it lacks any mechanism to reason about probabilistic information or multiple map hypotheses, often producing overconfident and risky paths with insufficient safety margins. Consequently, for applications like rescue robotics where world models are often incomplete or dynamic, Theta*’s geometrically optimal paths can be practically unreliable and unsafe.

Therefore, in scenarios where there is a clear foundation for map modeling but environmental information is uncertain, how to retain the certainty and controllability of traditional path planning algorithms while improving their adaptability to complex environments remains a problem worthy of in-depth study. To enhance the Theta* algorithm for uncertain, multi-map rescue environments, this paper makes the following key contributions:

(1) First, an improved Theta* algorithm framework is proposed to meet the needs of multi-map adaptability, risk quantification, and local obstacle avoidance in path planning under a dynamic, uncertain environment. By weighted superposition of obstacle information from different sub-maps, a global probabilistic raster map is constructed to simulate the spatial and temporal uncertainty of obstacles in the actual scene. At the same time, the Sigmoid response mechanism is introduced to design a nonlinear heuristic function, dynamically adjust the influence weight of obstacle probability on path cost, and balance the efficiency of submaps and the overall efficiency.

(2) Then, a two-stage hierarchical optimization architecture is adopted to solve the optimal path planning problem with multiple environment maps using the improved Theta* algorithm. In the global planning stage, the initial path is generated based on the probability map, and the line-of-sight detection mechanism with obstacle avoidance threshold is used to reduce redundant inflection points and improve path smoothness. In the local correction stage, by dynamically removing obstacle nodes and re-planning adjacent nodes, a safe path for different sub-map scenarios is quickly generated to ensure online adaptation performance.

(3) Finally, combined with the path optimization of post-processing, the motion energy consumption is further reduced. This framework breaks through the limitations of the traditional Theta* algorithm in multi-source information fusion and dynamic risk response, and provides an efficient, safe, and scalable path planning solution for complex scenarios such as rescue robots and automatic driving, with both theoretical innovation and engineering application value.

The structure of this paper is organized as follows. The brief overview and the core technology, as well as the flow chart of the Theta* global planning algorithm, are first described in Section 2. The whole problem description, the solution procedure based on the proposed two-stage hierarchical optimization architecture, and the improved Theta* algorithm for multi-map rescue robot path planning are introduced in Section 3. An experimental study, which includes feasibility checking, validity verification, and stability verification, is reported in Section 4. The summary conclusions, research limitations, and future research directions are summarized in Section 5, along with the potential practical application value of this study.

2. Theta* Global Planning Algorithm

2.1. Brief Overview

In grid-based maps, the path generated by A* is constrained by the grid structure, where node expansions are limited to eight discrete directions (i.e., horizontal, vertical, and diagonal), resulting in paths composed of segments with angles that are multiples of 45°. Consequently, the generated paths often follow grid edges and may contain redundant turns, commonly referred to as zigzag paths. Although post-processing techniques, such as path smoothing, can alleviate this issue to some extent, global path optimality cannot be guaranteed under such constraints.

It should be noted that A* itself is a general graph search algorithm and is not inherently restricted to grid-based environments. In principle, A* can be applied to any planning problem that can be formulated as a graph with defined states (nodes) and transitions (edges). Grid maps represent one of the most common and practical graph constructions in robotic path planning, while in continuous environments, A* can also be applied by discretizing the space into smaller units or by constructing graphs using sampling-based techniques, at the cost of increased modeling and computational complexity.

Based on an A* frame, the algorithm determines whether there is a straight path without obstacles between two points by dynamically detecting line-of-sight (LOS) between nodes, and allows for the path to move in a straight Line between non-adjacent vertices to bypass the Angle limit of the grid. The node expansion comparison between A* and Theta* is illustrated in Figure 2. From this figure, the two path choices each time you expand the vertex are simply compared. Path 1(A*) in Figure 2 stands for the path of A*, which is a straight path from the parent of the current node n(parent) to the current node n(current) and then to the neighborhood node n(neighbour). While path 2(Theta*) is the path of Theta*, which is the straight path from the parent of the current node n(parent) to the neighbor node n(neighbour). If path 2 has a line of sight and is shorter, it is updated n(neighbour) as a child n(child) of the parent node n(parent), skipping the middle node directly. Therefore, for visual purposes, A* can only be continuously expanded in multiples of 45°, and Theta* is not constrained by angles and can be expanded at “any” Angle. As a result, Theta* is able to generate shorter and smoother paths than A* under the same grid resolution.

Additionally, a comparative path planning experiment between A* and Theta* is also implemented on the same map. The comparison results between these two algorithms are also illustrated in Figure 3, showing differences in path nodes, search ranges, and other aspects. As can be seen from this figure, Theta* shows significant advantages over the A* algorithm in both path quality and computational efficiency. It significantly reduces the number of path nodes, inflection points, and extension nodes in the search process, resulting in a smoother, shorter near-optimal path.

2.2. Bresenham Algorithm

The Bresenham algorithm, a classic computer graphics method, serves as the core of Theta*’s line-of-sight detection. Its implementation relies on calculating an incremental value at each step and deciding whether to move along the x or y direction based on this value. By approximating straight-line slopes through integer arithmetic rather than floating-point calculations, the algorithm efficiently identifies the grid cells intersected by a straight path between two points (x₁, y₁) and (x₂, y₂). As a result, it is widely used in two-dimensional grid line detection and path planning tasks. The standard Bresenham algorithm procedure is as follows:

(1)

Calculate the linear increment and determine the main direction of the step

Given the starting point P₁(x₁, y₁) and end point P₂(x₂, y₂), calculate the increments Δx = x₂ − x₁ and Δy = y₂ − y₁ of the straight line, and determine the step direction of the line according to the increment (vector). If |Δx| > |Δy|, indicating a horizontal or near-horizontal line, step along the x direction and gradually update the y coordinates. If |Δx| < |Δy|, indicating a vertical or near-vertical line, step along the y direction and gradually update the x coordinates; if equal, indicate along the diagonal direction.

(2)

Update the error variable with the integer increment

Determine the degree of deviation between the current point and the ideal line through the error variable d, and dynamically adjust the stepping direction. For example, |Δx| > |Δy|, the direction x is the main direction at this time, and the initialization error variable is d = 2Δy − Δx. If d > 0, step along the secondary direction y, update d = d − 2Δx; If d < 0, step along the main direction x, update d = d + 2Δy; If d = 0, step in the diagonal direction, update. d = d + 2(Δy − Δx).

(3)

Keep iterating until the target point

Start at the starting point P₁(x₁, y₁) and gradually iterate to the end point P₂(x₂, y₂), finding all the grids that the straight path between the two points passes through.

2.3. Flow Chart of Theta* Algorithm

The main change in the traditional Theta* algorithm relative to A* is the addition of line-of-sight detection in the main loop. The detailed steps are as follows, and their illustration is also shown in Figure 4.

(1)

Initialization

The starting point is set to the initial node while other nodes are not explored. The open list (priority queue) contains only the starting point, and the closed list is empty. The initial path cost g is 0, while the initial heuristic value h is the Euclidean distance from the starting point to the target point, and so the initial total cost f = g + h = h.

(2)

Main cycle

Select the current node: If the open list is empty, there is no solution, and the algorithm terminates; otherwise, take the node with the smallest f = g + h in the open list as the current node, and determine whether the current node is the end point. If it is the end point, prepare to retrace the path and the loop ends; otherwise, go to the expanding process.

Expand nodes and update open lists: Go through all the non-obstructing neighbor nodes of the current node, if the line of sight of the current node’s parent → neighbor node path 1 exists, and it is shorter than the path 2 of the parent → current node → neighbor node, then directly update the neighbor’s parent node to the current node’s parent node; otherwise follow path 2. Calculate the g and f of the neighbor nodes, adding the neighbor node that is not in the open list to the open list, and adding the neighbor node to the open list after the neighbor node in the open list is updated.

(3)

Generating Path

Step by step, trace back from the end point along the chain of parent nodes to the beginning point to generate the final path.

3. Multi-Map Path Planning Based on Improved Theta*

3.1. Problem Description

The Theta* based path planning problem is an optimization problem. Under the premise of meeting the collision and topological constraints, the optimal path between the starting point and the end point is found by balancing the comprehensive cost and safety of the path, and heuristic search, line-of-sight detection, and other techniques are used to optimize the search efficiency and path smoothness. However, the traditional Theta* algorithm still has several limitations. To overcome the inherent limitations of the traditional Theta* algorithm, this paper introduces key improvements. These enhancements, together with the limitations they address, are summarized in

Table 1. Innovation points of this paper.

Limitations of Traditional Theta* Algorithm	Disadvantages and Challenges	Innovative Improvements
Use a single binary obstacle map, ignoring spatiotemporal uncertainty	Multiple source information is fragmented, and probabilistic associations between historical maps and real-time perceptual data are not fused	Probabilistic Fusion of multiple maps
Heuristic functions compute only geometric distances (such as Euclidean or Manhattan distances)	Risk is poorly quantified, and the heuristic function relies only on distance, ignoring the nonlinear effects of the probability of obstacles	Probability-sensitive Heuristic function (PSHF)
Line-of-sight detection uses a binary decision, is easily disturbed by sensor noise, and relies only on a single forward search verification	The noise is too sensitive, and the obstacles are strictly determined directly, without setting the threshold of line-of-sight detection and obstacle avoidance. Lack of backtracking verification after path generation, which may leave redundancy or potential collision risk	Line-of-sight- detection optimization mechanism
There is no local re-planning for the sub-map, and the overall path adaptability is poor	After the global path is generated, it cannot be quickly adjusted for the sub-map, and in case of local congestion, it cannot be re-planned to obtain a complete obstacle avoidance path	Two-stage (hierarchical) planning architecture

.

The optimized and improved initial model elements are as follows:

① Input: Starting point s, end point g, N sub-map set $\{M_1,M_2,...,M_N\}$, occurrence probabilities of each sub-map $\{{\omega }_1,{\omega }_2,...,{\omega }_N\}$.

② Output: The global initial path $P_{global}$ generated based on the global probability map $M_{\mathrm{global}}$, the local correction path $\{P_1,P_2,...,P_N\}$ generated based on the complete obstacle avoidance of the sub-map.

③ Decision variables: Define a global path composed of k nodes $P_{\mathrm{global}}=\{n_1,n_2,...,n_k\}$, where $n_1$ is the starting point s, $n_k$ is the end point g, the middle node $n_i$ is the key node passed on the path.

④ Objective function: Minimize the comprehensive cost, comprehensive consideration of global path length, planning efficiency, smoothness, and security.

$$\underset{P}{\min }\sum _{i=1}^{k-1}f\left({‖n_{i+1}-n_i‖}_2\right), i=1,2,\dots ,k-1$$

(1)

where ${‖n_{i+1}-n_i‖}_2$ is the Euclidean distance, $f\left(\cdot \right)$ is the transformation function of the Euclidean distance.

$${‖n_{i+1}-n_i‖}_2=\sqrt{{\left(x_{n_{i+1}}-x_{n_i}\right)}^2+{\left(y_{n_{i+1}}-y_{n_i}\right)}^2}$$

(2)

⑤ Constraints:

The size of the N submaps is equal, the length is L, the width is W, and each submap $M_i$ is a binary grid matrix:

$$M_i={\left\{\mathrm{0,1}\right\}}^{L\times W}, i=1,2,\dots ,N$$

(3)

$$M_i\left(x,y\right)=\left\{\begin{array}{l}1, \mathrm{P}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{o}\mathrm{n} (\mathrm{x}, \mathrm{y}) \mathrm{h}\mathrm{a}\mathrm{s} \mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}\mathrm{s}\\ 0, \mathrm{P}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{o}\mathrm{n} (\mathrm{x},\mathrm{y}) \mathrm{h}\mathrm{a}\mathrm{s} \mathrm{n}\mathrm{o} \mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}\mathrm{s}\end{array}\right.$$

(4)

The starting point s and end point g of all submaps are the same, and the coordinates are fixed:

$$\left\{\begin{array}{l}s=\left(x_s,y_s\right),g=\left(x_g,y_g\right)\\ x_s,y_s,x_g,y_g\in Z^{+}\\ 1\le x_s,y_s\le L, 1\le x_g,y_g\le W\end{array}\right.$$

(5)

The submaps represent all possible scenarios, so the sum of probabilities is 1:

$$\sum _{i=1}^N{\omega }_i=1, {\omega }_i\in \left[\mathrm{0,1}\right], i=1,2,\dots ,N$$

(6)

In addition, there are obstacle avoidance constraints (beyond the obstacle avoidance threshold $p_b$ to be judged not to pass the line of sight), topological constraints (the final path node of the submap must fall within the grid allowed to pass), and kinematic feasibility and other physical constraints.

3.2. Probabilistic Fusion of Multiple Maps

In a real environment, the obstacle distribution may change over time or depending on the situation. Take disaster relief as an example. The pre-disaster environment information is known, but there are many unknown factors in the post-disaster scenario. For example, a road is unimpeded in one time period, but may be partially blocked in another time period. Due to the limited detection range of sensors, only local area information can be obtained, and static algorithms cannot exhaust all possible scenarios (too much computation). Therefore, in view of the spatial and temporal uncertainty of obstacle distribution in the environment, experts cluster and reduce the possible obstacle distribution into several typical scenes based on prior knowledge and experience summary, and construct corresponding sub-maps. Each sub-map represents a possible distribution of obstacles, which is composed of a binary grid (0 means no obstacles, 1 means obstacles).

In this paper, by assigning weights to each submap (the sum of all submaps’ weights is 1), a probability-weighted superposition of the grid values is performed to generate a global probability map. This method fuses the sub-map information from multiple sources and uses the grid value to represent the probability of random obstacles, providing a unified environment representation for path planning, which is more in line with the statistical characteristics of the real environment. It not only retains the uncertainty characteristics of the environment but also makes path planning more intelligent, reduces the risk of path failure, and provides a forward-looking decision basis for subsequent path planning. The specific realization formula is as follows:

$$\left\{\begin{array}{l}{\displaystyle \sum _{i=1}^N}{\omega }_i=1, {\omega }_i\in \left[\mathrm{0,1}\right]\\ M_i\left(x,y\right)\in \left\{\mathrm{0,1}\right\}\\ i=1,2,\dots ,N\end{array}\right. \Rightarrow { M}_{\mathrm{global}}\left(x,y\right)=\sum _{i=1}^N{\omega }_i\cdot M_i\left(x,y\right)\in \left[\mathrm{0,1}\right]$$

(7)

In the above formula, N represents the number of sub-maps, $M_i\left(x,y\right)$ represents the grid value corresponding to the position $\left(x,y\right)$ in the i-th sub-map, and ${\omega }_i$ represents the probability (weight) of the sub-map. M_global(x, y) represents the global raster value after the weighted superposition corresponding to the position (x, y); that is, it indicates the probability that there is an obstacle in the global probability ground M_global of this raster, and it can be any probability value between [0, 1]. For example, 0 means no obstacle, 1 means a complete obstacle, and 0.35 means that the probability of an obstacle appearing in the grid is 35%.

3.3. Probability-Sensitive Heuristic Function

The heuristic function of the traditional Theta* algorithm relies only on geometric distance and has insufficient risk-perception granularity in dynamic or uncertain environments. The improved Theta* algorithm incorporates the influence of obstacle Probability into Heuristic cost calculation, and proposes a probability-sensitive heuristic function(PSHF) that integrates obstacle probability field gradient and adaptive weight. The formula for the heuristic cost of a node is as follows:

$$\left\{\begin{array}{l}h\left(n\right)=h_0\left(n\right)\cdot \sigma \left(n\right)\\ h_0(n)=\sqrt{(x_n-x_g{)}^2-(y_n-y_g{)}^2}\\ \sigma (n)=\alpha \cdot \left({\displaystyle \frac{1}{1+e^{-\beta \cdot \left(p\left(n\right)-p_b\right)}}}\right)\end{array} \right. \Rightarrow h\left(n\right)={\displaystyle \frac{\alpha \cdot {‖n-g‖}_2}{1+e^{-\beta \cdot \left(p\left(n\right)-p_b\right)}}}$$

(8)

where h₀(n) represents the Euclidean distance from the node n to the end point g, σ(n) is the weight function, obtained by the probability of obstacles p(n) of the node through the Sigmoid function nonlinear mapping. α and β are adjustable parameters, which are, respectively, used to control the intensity of the risk weight and adjust the slope of the Sigmoid function illustrated in Figure 5, and p_b is the obstacle avoidance threshold of line-of-sight detection. The practical adjustment of the parameters α and β and their influence on path planning behavior are systematically investigated through sensitivity analysis in Section 4.4.2, items ② and ③.

This design has a nonlinear perception of risk, imposes weak penalties ($p\left(n\right)<p_b$) on low-probability obstacles ($\sigma \left(n\right)\approx 0$) and significantly increased strong penalties ($p\left(n\right)>p_b$) on high-probability obstacles ($\sigma \left(n\right)\approx \alpha$), guides the path to avoid high-risk areas, and allows for flexible trade-offs between path length and safety to accommodate different task requirements. Meanwhile, the continuous differentiability of the Sigmoid function enables the heuristic cost to smoothly transition near the threshold p_b. By setting the magnitude of the function’s slope coefficient β, the transition trend of the curve can be adjusted, avoiding path mutation caused by minor fluctuations in probability values and significantly enhancing the algorithm’s robustness.

3.4. Optimization Mechanism of Line-of-Sight Detection

In view of the low reliability of the traditional Theta* algorithm, the following optimization strategy is proposed to achieve the dual improvement of noise robustness and path safety through probabilistic relaxation and posterior validation:

① Probabilistic soft threshold anti-noise mechanism: In a real environment, there may be noise in map data. If all obstacles are strictly judged directly, the path planning may be overly sensitive, resulting in a large detour of the path. If the robot is allowed to travel in the low-probability obstacle area, the abnormal path fluctuation caused by noise can be effectively reduced, and the stability and feasibility of the algorithm can be improved. In order to prevent the noise from being too sensitive, the obstacle avoidance probability threshold p_b is set in the search stage of sight detection, and the area where p(x, y) > p_b is marked as a high-risk area. Within this soft mechanism, a sight obstruction is confirmed only when the obstacle probability exceeds the specified threshold.

$$\mathrm{LOS}\left(a,b\right)=\left\{\begin{array}{l}True, \forall n\in \mathrm{P}\mathrm{a}\mathrm{t}\mathrm{h}(a,b), p\left(n\right)\le p_b\\ False, \mathrm{o}\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{w}\mathrm{i}\mathrm{s}\mathrm{e}\end{array}\right.$$

(9)

where LOS(a, b) represents the line-of-sight accessibility from node a to node b, and Path(a, b) represents the set of grids that the straight path from node a to node b passes through.

② Backtracking check mechanism: After the initial generation of the path, the algorithm will conduct a backtracking check on the path, and re-detect the line of sight between each key node to form a verification closed-loop. This process can eliminate redundant nodes and identify and eliminate potential collision risks that may remain, so as to further optimize the smoothness and security of the path.

3.5. Two-Stage (Tiered) Planning Framework

In the global planning stage, a probabilistic model is used to model the uncertainty of environmental obstacles, and a global probability M_global map is obtained. Through nonlinear heuristic function design, probabilistic soft threshold anti-noise, and backtracking check improvements, an initial total path P_global that takes into account global optimality and security is obtained.

$$\begin{array}{cc}\hfill P_{\mathrm{global}}=& \left\{n_1,n_2,\dots ,n_{k-1},n_k\right\}=\left\{s,n_2,\dots ,n_{k-1},g\right\}\hfill \\ \hfill =& \left\{\left(x_s,y_s\right),\left(x_2,y_2\right),\dots ,\left(x_{k-1},y_{k-1}\right),\left(x_g,y_g\right)\right\}\hfill \end{array}$$

(10)

$$\forall n_i\in \mathrm{Path}\left(n_i,n_{i+1}\right), p\left(n_i\right)\le p_b, i=1,2,\dots ,k-1$$

(11)

However, the global initial path is planned on the basis of integrating the probabilistic information of multiple maps, which meets the threshold requirements of the line-of-sight detection optimization mechanism, but may not completely avoid the clear obstacles in a specific submap. In order to meet the complete obstacle avoidance requirements of specific submaps, local re-planning adjustment will be carried out for different submaps.

Since each submap is a binary (0 and 1) raster map and independent of the other in the actual environment, probability sensitivity and information fusion processing are no longer required, and the original Theta* algorithm (before improvement) can be directly invoked to correct the path on the actual submap. This step can quickly and accurately generate the subpath that meets the requirements.

$$\left\{\begin{array}{l}{P}_i=\left\{\left(x_1^{\left(i\right)},y_1^{\left(i\right)}\right),\left(x_2^{\left(i\right)},y_2^{\left(i\right)}\right),...,\left(x_{m_i}^{\left(i\right)},y_{m_i}^{\left(i\right)}\right)\right\}\\ \forall \left(x^{\left(i\right)},y^{\left(i\right)}\right)\in \mathrm{P}\mathrm{a}\mathrm{t}\mathrm{h}\left(\left(x_j^{\left(i\right)},y_j^{\left(i\right)}\right),\left(x_{j+1}^{\left(i\right)},y_{j+1}^{\left(i\right)}\right)\right), p\left(x^{\left(i\right)},y^{\left(i\right)}\right)=0\\ i=1,2,\dots ,N ,j=1,2,\dots ,m_i-1\end{array}\right.$$

(12)

where m_i is the number of nodes in the final path P_i of the submap M_i after correction, (x⁽ⁱ⁾, y⁽ⁱ⁾) is the node on the path P_i, and the final path of the submap meets the condition of complete obstacle avoidance, that is, all the grids through which P_i passes are idle grids.

This two-stage (hierarchical) planning architecture, illustrated in Figure 6, effectively copes with the problem of inconsistent global and local objectives in complex environments through the progressive process of probability fusion → global optimization → local correction. It not only makes use of the advantages of global probability maps in capturing the representation of generalized environmental uncertainty, but also exerts the high efficiency of the original Theta* in strict obstacle avoidance on binary maps. Thus, providing a comprehensive and feasible solution.

3.6. Solution Process

3.6.1. Global Path Planning Stage

The global planning process Pseudo-code is shown in Algorithm 1, while the specific steps of global path planning based on the improved Theta* algorithm are as follows:

(1)

Probabilistic fusion and initialization of multiple maps

First, multiple sub-maps are read, and probability-weighted superposition is performed on each grid according to its probability of occurrence to generate a global probability map. The value of each grid in the map represents the probability of obstacles appearing in this grid. Next, the path planning initialization is carried out: the starting point and end point coordinates are defined; add the start point to the open list (store nodes to be explored) and the close list (store nodes that have been explored) as empty; set the actual cost of the starting point g to 0, and the heuristic cost h to be the Euclidean distance from the starting point to the ending point, with the parent node pointing to itself.

(2)

Main loop—node selection and expansion

First, conduct node selection. If the open list is not empty, select the node with the lowest comprehensive cost f(n) from the open list as the current node. If the selected current node is the end point, the loop ends; otherwise, move the current node from the open list to the closed list and mark it as an extended node.

Then, perform a neighborhood expansion. Traverse the eight neighborhood nodes of the current node (including up and down, left and right, and diagonal directions). If the neighborhood node is out of the map range or the corresponding grid value exceeds the preset obstacle threshold, then skip the node; skip if the neighborhood node already exists in the closed list. Process each neighborhood node one by one, calculating and recording the actual path cumulative cost g(n), probability-sensitive heuristic estimated cost h(n), and synthetic cost f(n) for each domain node.

The formula for the synthetic cost function f(n) is as follows:

$$f\left(n\right)=g\left(n\right)+h\left(n\right), n=n\left(neighbor\right)$$

(13)

where g(n) represents the total path cost traveled from the starting point to the domain node, h(n) is a probability-sensitive heuristic function after adding the influence of the probability of obstacles (refer to Section 3.3). For the domain node of the current node, g(n) represents the total cost from the start point to the current node plus the Euclidean distance from the current node to the node, while the total cost from the start point to the current node has been updated since the last loop.

$$g\left(n\right)=\sum _{i=2}^{current}{‖n_i-n_{i-1}‖}_2+{‖n_{neighbor}-n_{current}‖}_2=\sum _{i=2}^{neighbor}{‖n_i-n_{i-1}‖}_2$$

(14)

Finally, line-of-sight detection is performed with the parent node update. Look back to the parent node of the current node. If there is a parent node, then call the optimized version of the sight detection function to check whether there is a straight path from the parent node to the selected next extended neighborhood node that does not exceed the obstacle avoidance threshold of p_b. If the line of sight exists, the actual cost g(n) and the parent of the selected neighborhood node are updated directly through the parent node. If no parent exists, it is still updated through the current node. After the update, the neighborhood node is added to the open list, waiting for further exploration into the next loop.

Repeat the loop process until the end point is found or the open list is empty.

(3)

Post-processing verifies and outputs the result

From the end point, trace back step by step along the parent node pointer to the starting point, and use line-of-sight detection to optimize the generated path after processing. If the gap nodes in the path are still redundant, the intermediate nodes are deleted to further reduce the cost of the path. Finally, the optimized path coordinates, the actual path moving cost, and the path generation time from the starting point to the end point are output.

Algorithm 1: Global planning process pseudocode
1	StartTimer()
2	Initialize OPEN ← {start}, CLOSED ← ∅, EXPAND ← ∅
3	Set g(start) ← 0, h(start) ← EuclideanDistance(start, goal)
4	Set parent(start) ← start
5	while OPEN is not empty do
6	Select cur_node ∈ OPEN with minimum f(n) = g(n) + h(n)
7	Remove cur_node from OPEN and add to CLOSED
8	if cur_node == goal then
9	break
10	end if
11	for each node_neigh in 8-neighborhood of cur_node do
12	if node_neigh is out of bounds or map(node_neigh) > p_b then
13	continue
14	end if
15	if node_neigh ∈ CLOSED then
16	continue
17	end if
18	Compute obstacle-weighted heuristic h(node_neigh)
19	h(node_neigh) ← EuclideanDistance(node_neigh, goal) × α/(1 + exp(−β(map(node_neigh) − p_b)))
20	if parent(cur_node) exists and LineOfSight(parent(cur_node), node_neigh) then
21	UpdateVertex(parent(cur_node), node_neigh)
22	else
23	UpdateVertex(cur_node, node_neigh)
24	end if
25	if node_neigh ∉ OPEN then
26	Add node_neigh to OPEN
27	end if
28	end for
29	end while
30	StopTimer()
31	runtime T ← GetElapsedTime()
32	Extract initial path P from CLOSED by backtracking
33	Optimize P by removing intermediate nodes with valid line-of-sight
34	Compute total path cost C
35	return P, C, T

3.6.2. Local Path Re-Planning Phase

After obtaining the initial path generated by the global path planning, when the actual scene corresponds to a sub-map, the global path needs to be locally corrected. At this time, the local re-planning process must be implemented, whose pseudocode is shown in Algorithm 2. In the local re-planning process, the nodes located in the obstacle area will be eliminated, and the original Theta* algorithm is called on for re-planning to ensure that the sub-map path completely avoids obstacles. The specific steps are as follows.

(1)

Dynamically remove obstacle nodes

Traverse all nodes in the global path and check their raster properties in the current sub-map. If the grid of a node is completely obstructed, it is determined that the node is in an impassable area. All nodes located on the obstacle grid are dynamically removed to generate a temporary corrected path. If the temporary path is empty or the number of nodes is less than or equal to 2, it is determined that the path is completely unfeasible or broken, and the exception processing mechanism is triggered to re-plan the path directly from the start point to the target point on the sub-map.

(2)

Local path re-planning between adjacent nodes

The original Theta* algorithm is invoked to perform line-of-sight detection on the adjacent node pairs traversing the corrected temporary path in turn. If there are no obstacles between the nodes of the pair, the traversal continues to the next pair of nodes; if there are obstacles, the node pair is taken as the starting point and the target point, respectively, to conduct local obstacle avoidance and generate a complete obstacle avoidance subpath. The first and last nodes of each subpath are spliced into a complete path, and the sequence of path nodes and the cumulative path cost are updated.

(3)

Path post-processing and optimization

The path after splicing is de-duplicated, the redundant nodes are deleted, and the final path is obtained. The optimized path, its total path cost, and path generation time are used as the output of the subsequent path performance evaluation.

Algorithm 2: Local re-planning process pseudocode
1	StartTimer()
2	Initialize final_path ← ∅, final_cost ← 0
3	Input initial path P and grid map M (0: free, 1: obstacle)
4	for each node n in P do
5	if M(n) == 1 then
6	Remove n from P
7	end if
8	end for
9	for each consecutive node pair (n_i, n_{i+1}) in P do
10	Call Theta*(n_i, n_{i+1}, M) to generate local path P_sub
11	if P_sub is not found then
12	Terminate local re-planning and report failure
13	end if
14	Append P_sub to final_path
15	Update final_cost
16	end for
17	Remove redundant nodes from final_path while preserving order
18	StopTimer()
19	local_runtime ← GetElapsedTime()
20	return final_path, final_cost, local_runtime

4. Simulation

4.1. Experimental Environment and Data Set

This experiment was based on MATLAB, version 2021b (The MathWorks, Inc., Natick, MA, USA) to simulate a highly structured, uncertain mine rescue environment. The environment comprehensively considers fixed obstacles and random dynamic elements, which not only retain the basic layout of the mine but also improve the complexity of the environment and the authenticity of the simulation through the random distribution of resources, building materials, and gravel.

Three 50 × 50 raster maps (composed of a binary matrix, where 0 represents the passable area and 1 represents the complete obstacle) shown in Figure 7 are used in the experiment. In this figure, the starting point is marked in red while the end point is marked in blue color (All remaining figures are to be set up accordingly). It is noted from this figure that the starting point, end point, and fixed obstacle distribution of the three sub-maps are the same, but the dynamic obstacle distribution is different, so as to simulate the rescue scene under different circumstances. Fixed obstacles (accounting for about 35%) are used to simulate stable structures in mines, such as walls, support columns, etc. Dynamic obstacles (accounting for about 5–7%) are used to simulate sudden obstacles that may occur during rescue, such as collapsed rubble or equipment. The total proportion of obstacles is about 40% to ensure the background consistency and practical challenge of the scene.

4.2. Parameter Setting

This experiment involves several key parameter settings to optimize the performance of path planning and improve the adaptability of the algorithm in uncertain environments. The main parameter descriptions are detailed in

Table 2. Parameters and notations of the proposed algorithm.

Category	Parameter	Notation	Description
Input	Probabilistic maps	$\left\{ma{p}_i\right\}$	Set of probabilistic grid maps representing different possible environments
	Map weights	$\left\{w_i\right\}$	Weight coefficients corresponding to each probabilistic map
	Number of maps	$k$	Total number of probabilistic sub-maps used for map fusion
	Start node	$start$	Initial node of the planned path
	Goal node	$goal$	Target node of the planned path
	Obstacle threshold	$p_b$	Probability threshold for determining obstacle grids and blocking line-of-sight
Algorithm	Path cost	$g(n)$	Accumulated cost from the start node to node n
	Heuristic cost	$h(n)$	Estimated cost from node n to the goal node
	Linear heuristic weight	$l$	Weight coefficient used in the linear probability-aware heuristic function
	Risk weight coefficient	$\alpha$	Parameter controlling the penalty intensity for high-probability obstacles
	Sigmoid slope parameter	$\beta$	Parameter controlling the sensitivity of the nonlinear heuristic to obstacle probability
Output	Optimized path	$P$	Final planned optimal path
	Path cost	$C$	Total cost of the optimized path
	Runtime	$T$	Average computational time required for path planning (computed over 20 repeated runs under identical settings, where minor runtime variations are caused by system-level factors)

.

It should be noted here that the parameters $\alpha$ and $\beta$ directly affect the trade-off between path optimality and risk avoidance. Rather than being arbitrarily selected, their influence on planning behavior is systematically investigated through dedicated sensitivity experiments, which are presented in Section 4.4.

① Obstacle avoidance threshold: Set as an obstacle avoidance threshold p_b = 0.3, for line-of-sight detection. When the obstacle probability of a grid exceeds this threshold, the line of sight is determined to be blocked.

② Heuristic parameters: Set α = 10 and β = 10 to control the weight calculation of the heuristic cost during path planning, and the probability-sensitive heuristic function is $\sigma (n)=\left({\displaystyle \frac{1}{1+e^{-\beta \cdot \left(p\right(n)-p_b)}}}\right)\cdot \alpha ={\displaystyle \frac{10}{1+e^{-10\cdot \left(p\right(n)-0.3)}}}$.

③ Visual global probability map: In the experiment, the occurrence probability of the three sub-maps is set to 0.3, 0.3, and 0.4, respectively, which is used to simulate the occurrence probability of different environments in the actual scene. In the multi-map fusion stage shown in Figure 8, the grid color depth reflects the probability of obstacles in the grid; the darker the color indicates the higher the probability of obstacles, so that the processing can intuitively present the uncertainty of the environment, and can assist decision-making and debugging, improving the interpretability of the results.

4.3. Result Presentation (Feasibility Analysis)

(1)

Experimental design

First, the global probability graph is used to generate initial paths for the test group through a probability-sensitive heuristic function, which are then compared with fully obstacle-avoidance paths produced by traditional algorithms such as A*, Dijkstra, and Theta*. The comparison evaluates path length, node count, and time consumption. Second, local obstacle-avoidance corrections are applied to each subgraph to ensure complete obstacle avoidance. The algorithm calculates both the total and extended node counts, and computes the combined cost and deviation between global and local paths to quantify its performance.

(2)

Experimental results and analysis

The complete obstacle avoidance results of traditional A*, Dijkstra and Theta* are shown in Figure 9 (in this figure and all following ones, the red dashed line represents the routing path while the gree dots represent corresponding path nodes), while the global initial path of the experimental group is shown in Figure 10. The comparisons on path cost, number of path nodes, and runtime among the traditional A*, Dijkstra, and Theta* with the experimental group are shown in

Table 3. Comparison of traditional A*, Theta*, and experimental group paths.

	Traditional A*	Traditional Dijkstra	Traditional Theta*	Experimental Group Global Path
Path Cost	70.3553	69.522	65.6734	64.633
Number of path nodes	61	61	14	9
Number of expand nodes	205	1168	539	1036
Runtime(s)	0.6549	10.0227	2.9587	8.3132

.

The algorithm performance is quantified by considering the total cost, the number of path nodes, and the deviation degree between global and local paths.

As illustrated in Figure 9, both A* and Dijkstra algorithms strictly adhere to grid-based expansion patterns. While A* employs a heuristic function for high-search efficiency, its path exploration and obstacle avoidance processes often adhere closely to obstacle edges, resulting in redundant node layouts. Dijkstra, as a blind breadth-first search, consumes more computational resources and exhibits longer runtime. In contrast, Theta* utilizes line-of-sight detection technology to maintain greater distance from densely obstructed areas, significantly reducing node counts while markedly improving path smoothness.

Furthermore, the enhanced Theta* algorithm implemented in the experimental group demonstrated significant improvements in path planning performance compared to the traditional Theta* algorithm.

Table 4. Experimental group and control group.

Groups	Method Description	Purpose of Comparison
Experimental group	Generate the initial path based on the probability map, make local obstacle avoidance correction on each submap, and calculate the comprehensive cost.	Verify the advantages of the combination of global probability planning and local correction.
Control Group 1	Only the setting of the obstacle avoidance threshold in the experimental group model was retained, and the heuristic function was pure Euclidean distance without adding probability sensitivity. The global paths generated under different obstacle avoidance thresholds are directly compared.	The significance of incorporating probabilistic information into cost calculation is discussed, and the appropriate obstacle avoidance threshold is determined.
Control Group 2	Set the linear heuristic function hnew, change the regulating factors in the nonlinear and linear heuristic cost functions, respectively, adjust the sensitivity, and observe the influence of different heuristic functions on the global path planning results.	The influence of linear and nonlinear heuristic design on path planning performance is compared to provide a reference for parameter optimization.
Control Group 3	The initial path was generated on the sub-map with the highest probability of occurrence (Map 3, 40%), and then partially corrected on other maps to calculate the comprehensive cost.	The difference in path length and efficiency between the probabilistic fusion strategy and the single high-probability map planning strategy is compared.
Control Group 4	The linear function h (hnew in Control Group 2) was used to generate the global initial path, and then the local obstacle avoidance correction was made on each submap to calculate the comprehensive cost.	The influence of linear and nonlinear heuristic design on the comprehensive results was compared.

further reveals that the improved Theta* algorithm, by integrating probability-sensitive heuristic functions and line-of-sight detection, exhibits greater caution during path expansion and node selection. Although this increases computational time, it ensures high safety. The global initial path generated on the probability map has a cost of 64.633, 9 path nodes. This global path only passes through a small number of low-probability obstacle areas, effectively avoiding high-risk zones. With streamlined node counts and a smooth, fluid path, it achieves outstanding performance. Since the global map is weighted through sub-map probabilities, this result indicates that the initial path not only optimizes path length but also ensures high safety in most sub-map scenarios.

Next, it is time to see the real performance of local re-planning for the proposed method, and its correction paths on different submaps; its path cost and other performances are shown in

Table 5. Different path cost and other performances of the experimental group.

	Global Initial Path	Map One	Map Two	Map Three	Comprehensive Path
Path cost	64.633	65.7989	64.9913	64.8793	65.18878
Offset degree Δ	-	1.79%	0.55%	0.38%	0.86%
Number of path nodes	9	12	11	9	10.5
Offset degree Δ	-	33.33%	22.22%	0	16.67%
Runtime(s)	8.3132	+0.7074	+0.6009	+0.5414	-

Offset degree Δ refers to the change relative to the global initial path and can reflect the adjustment efficiency; “+” represents the incremental runtime introduced by local replanning, excluding the initial global planning cost.

.

After partial obstacle avoidance, complete obstacle avoidance is achieved on the sub-map. Due to the different distribution of obstacles in different maps, the path may be partially offset in some areas, but the overall structure is still stable. It is clearly seen from Figure 11 and Table 5 that the path costs corresponding to the three submaps are 65.7989, 64.9913, and 64.8793, respectively, and the final weighted comprehensive path cost is 65.18878. As for the path node, as seen in the figure, the number of path nodes is 12, 11, and 9, and the number of comprehensive path nodes obtained by the final weighted calculation is 10.5. As shown in the last row of Table 5, once the initial global path is obtained, the additional runtime required for local obstacle avoidance on different sub-maps is very small.

From the analysis of the above table, it can be seen that Map 3 has a high consistency with the global map, and the adjustment is subtle. Individual obstacles in Maps 1 and 2 forced the path to insert several inflection points, resulting in detour deviation, but the local adjustment did not significantly increase the cost of the path. The results show that a hierarchical framework can efficiently adapt the global path to local environmental variations with minimal computational overhead. This experiment verifies the feasibility of the improved algorithm from multiple dimensions and provides a reliable solution for path planning in complex environments.

4.4. Comparative Experiment (Validity Verification)

In order to verify the effectiveness of the improved algorithm proposed in this paper in terms of multi-map adaptability, risk quantification, and path security, and further analyze the impact of key parameters on the algorithm performance, the following controlled experiments shown in Table 4 were designed:

4.4.1. Control Group 1

(1)

Experimental design

In this control group, obstacle avoidance is performed on the global probability map and the line-of-sight detection optimization mechanism is retained, but the traditional Theta* heuristic function is adopted, that is, the probability-sensitive heuristic function is disabled and pure Euclidean distance (σ(n) = 1, h(n) = h₀(n) = ‖n − g‖₂) is adopted. The obstacle avoidance threshold p_b is changed, and the influence of different obstacle avoidance thresholds on global path planning is observed to determine the appropriate threshold.

(2)

Experimental results and analysis

In this experiment, only grid cells with obstacle probability not exceeding the obstacle avoidance threshold p_b are allowed to pass the line-of-sight (LOS) check and be directly connected. The global paths under different obstacle avoidance thresholds are illustrated in Figure 12, and the corresponding quantitative comparisons are summarized in

Table 6. Performance comparison of global paths with different obstacle avoidance thresholds.

Obstacle Avoidance Threshold pb	Global Path Cost	Number of Global Path Nodes	Number of Expand Nodes	Runtime(s)	Path Characteristics
−0.1	69.5269	61	160	0.5658	The “direct connection” function of line-of-sight detection is turned off, and the generated path is similar to the traditional A* total obstacle avoidance result, with continuous path nodes, too conservative, but still bypassing dense obstacle areas
0.2	65.6734	14	357	1.3772	For this global probability map (minimum obstacle probability 0.3 > 0.2), the generated path is consistent with the traditional Theta* total obstacle avoidance results
0.3	64.6404	10	1036	8.0831	Balanced mode, cost, and security balance the best
0.4	60.2745	6	1107	9.8121	The path cost decreases slowly, but the collision risk increases significantly
0.7	60.1025	4	1172	11.5442	The penetration pattern is aggressive, and the path cost changes slightly
1	53.8015	2	1188	12.8917	All lines of sight can pass through, full penetration mode, a straight line to connect the end point, no obstacle avoidance

.

As shown in Table 6, increasing the obstacle avoidance threshold generally leads to a reduction in both the global path cost and the number of path nodes, since a larger pb allows for more direct LOS connections and reduces intermediate waypoints. Meanwhile, the number of expanded nodes and the runtime increase moderately, reflecting a more permissive but computationally heavier search process. When p_b = 0.3, a favorable trade-off between path safety and planning efficiency is achieved, resulting in a global path cost of 64.6404 with only 10 path nodes and a moderate runtime of 8.0831 s.

Furthermore, under the same obstacle avoidance threshold, the experimental group employing the probability-sensitive heuristic function based on Sigmoid mapping further improves path quality, achieving a slightly lower global path cost (64.633) and fewer path nodes (9) with comparable runtime. This comparison demonstrates that incorporating obstacle probability into the heuristic function yields better planning performance than using a purely Euclidean distance heuristic.

4.4.2. Control Group 2

(1)

Experimental design

To investigate the influence of the parameters, a systematic single-parameter sensitivity analysis is conducted. Specifically, one parameter is varied while the others are kept fixed. Each parameter is first explored over a relatively wide range to observe its overall impact on path behavior, followed by a finer-grained adjustment within a narrowed interval based on quantitative metrics and visual comparisons of the resulting paths. This process allows for both the identification of suitable parameter values and a clear understanding of how the parameters affect the trade-off between path safety and length.

In this experiment, the obstacle avoidance threshold p_b is fixed to 0.3 for the control group. Additionally, a linear probability-sensitive heuristic function in the control group is employed for comparison with the experimental group, which employs a nonlinear probability-sensitive heuristic function. The linear heuristic function is $h_{new}=l\times p\left(n\right)\times h_0\left(n\right)$, where $l$ is the regulatory factor, $p$(n) is the obstacle probability, and $h_0\left(n\right)$ is the Euclidean distance.

The experimental procedure is as follows: Change the tuning factors l, α, and β in the nonlinear and linear heuristic cost functions, respectively, to adjust the sensitivity, then observe and compare the differences in sensitivity to parameter adjustments between linear and nonlinear functions, as well as the impact of different heuristic functions on global path planning results.

(2)

Experimental results and analysis

① Change the parameter l of the linear heuristic function

The global paths generated using the linear probability-aware heuristic with different values of l are illustrated in Figure 13, and the corresponding quantitative comparisons are summarized in

Table 7. Comparison of global paths corresponding to different l.

l	1	2	3	5	10	20
Global path length	64.6404	67.2832	67.6	68.9198	69.004	69.1975
Number of global path nodes	10	14	15	19	19	20
Number of expand nodes	529	159	168	161	158	158
Runtime(s)	2.766853	0.4266	0.5317	0.4110	0.3871	0.4058

.

As shown in Table 7, increasing l leads to a noticeable increase in both global path length and the number of path nodes, indicating that the planner becomes increasingly conservative as the influence of obstacle probability grows. When l exceeds a small range (e.g., l ≥ 5), the path quality degrades and quickly reaches saturation, suggesting that further increasing the linear weight does not yield meaningful improvement.

Meanwhile, the number of expanded nodes and the runtime decrease significantly as l increases. This phenomenon indicates that an overly strong linear probability term dominates the heuristic function, restricting the search space prematurely and forcing the planner to favor locally preferred regions. Although this behavior reduces computational cost, it leads to suboptimal path quality and limits the planner’s ability to balance safety and efficiency.

These results demonstrate that the linear heuristic function is highly sensitive to parameter tuning and has a very limited effective adjustment range. Small changes in l can significantly alter search behavior, making the planner prone to overly conservative decisions. This limitation motivates the introduction of a nonlinear probability mapping, which can provide smoother and more robust control over the influence of obstacle probability.

② Change the parameter α of the nonlinear heuristic function

To analyze the effect of α, a single-parameter sensitivity analysis was conducted by varying α from wide to narrow ranges while keeping other parameters constant. Here, the experimental results under several typical parameters are presented and compared.

As shown in Figure 14 and

Table 8. β = 10, comparison of different α-corresponding global paths.

α(β = 10)	1	10	20	30	50	100
Global path length	64.633	64.633	64.6404	65.5138	67.2832	68.9198
Number of global path nodes	9	9	10	10	14	19
Number of expand nodes	1152	1036	620	238	158	161
Runtime(s)	12.7818	8.4117	5.3911	0.8858	0.6054	0.4173

, when β = 10, the path length and global path node count remain largely unchanged as α increases from 1 to 30, while the number of extended nodes and runtime are significantly reduced. This indicates that the nonlinear probability-sensitive heuristic progressively strengthens its guidance ability without degrading path quality, enabling the search process to converge more efficiently.

When α exceeds 30, the influence of the probability term becomes dominant, leading to a noticeable increase in path length and path nodes, while the number of expanded nodes and runtime further decrease. This suggests that excessive emphasis on obstacle probability restricts the search space prematurely, resulting in conservative path selection.

Overall, the results demonstrate that the nonlinear heuristic function exhibits a wide effective adjustment range for α, within which both planning efficiency and path quality are well balanced. Compared with the linear heuristic, the nonlinear formulation significantly reduces parameter sensitivity and provides more robust control over the trade-off between safety and efficiency.

③ Change the parameter β of the nonlinear heuristic function

Similarly, the parameter β is adjusted independently to investigate its effect on the smoothness of risk perception and the resulting path behavior.

The obtained global paths corresponding to a linear heuristic function with various β with α = 10 are illustrated in Figure 15, while their comparisons on path length, path nodes, and runtime are shown in

Table 9. α = 10, comparison of global paths corresponding to different β.

β(α = 10)	0.1	1	3	5	10	50
Global path length	68.9198	68.5767	67.6	67.2832	64.633	64.633
Number of global path nodes	19	18	15	14	9	9
Number of expand nodes	160	162	164	166	1036	1169
Runtime(s)	0.3818	0.6538	0.5316	0.4691	8.4117	10.4058

. It is found, as demonstrated in Figure 15 and Table 9, that β determines how fast the algorithm responds to changes in probability. As it increases, the Sigmoid function transitions from a gentle gradient to a steep jump. For the global probability map, before saturation, the path is gradually optimized from the state close to A* with the increase in β, and reaches the optimal state near 10. After 10, the saturation interval is reached, and the increase in β no longer affects the change in the value of the whole heuristic function. Before 10, the gap between path expansion node count and runtime was minimal, but since 10, both expanded nodes and runtime surged dramatically.

The experiment for Control Group 2 shows that the nonlinear heuristic function realizes the regionalized risk response through Sigmoid mapping, its parameter sensitivity is low, and the adjustment robustness is high, which is significantly better than the global scaling mechanism of the linear model, and verifies the core value of nonlinear probability-sensitive design in path planning.

4.4.3. Control Group 3

(1)

Experimental design

This experiment is employed to verify the effectiveness of the experimental group in generating global initial paths on comprehensive probability maps for improving path planning efficiency. In this experiment, the control group adopts the same two-stage approach as the experimental group. The improved Theta* is used to generate the initial path on the sub-map with the highest probability of occurrence (Map 3, 40%), and this path is selected as the global path. Then, the traditional Theta* is invoked on other submaps (Maps 1 and 2) for local correction, and the complete obstacle avoidance path was generated. Finally, the comprehensive path cost, the number of nodes, and the runtime are calculated according to the weight of the sub-map.

(2)

Experimental results and analysis

Figure 16 and Figure 17 illustrate the global initial path of the control group on Map 3, as well as the locally adjusted paths after obstacle avoidance on Maps 1 and 2, respectively. The corresponding path cost and performance metrics are summarized in

Table 10. Comparison of path cost and number of nodes for control group 3.

	Global Initial Path (Map 3)	Map 1	Map 2	Comprehensive Path
Path cost	62.8524	66.7809	63.9876	64.37151
Offset degree Δ	-	6.25%	1.81%	2.42%
Number of path nodes	9	16	12	12
Offset degree Δ	-	77.78%	33.33%	33.33%
Runtime(s)	11.8292	+1.1879	+0.7635	-

. The comparison results indicate that the control group achieves a slightly better overall path cost than the experimental group, but with a significantly higher runtime (the experimental group results are shown in Figure 10 and Figure 11 and Table 5).

This indicates that in certain scenarios, when the high-probability sub-map has a relatively simple structure, generating the initial path based on a single high-probability map may yield better performance. However, when the high-probability sub-map becomes more complex, the complexity of the generated initial path increases accordingly. Due to the lack of global probabilistic guidance, the local obstacle avoidance stage of this method requires substantial modifications to the initial path, leading to frequent adjustments and reconstructions. As a result, it performs poorly in terms of the number of path nodes, path deviation, and runtime, which negatively affects path stability and generation efficiency.

In contrast, the experimental group integrates global probabilistic information from multiple sub-maps to predict potential obstacle distributions under different scenarios, enabling the generation of a more general and stable global initial path while reducing the computational cost of subsequent local replanning. These results demonstrate that the hierarchical planning method based on a global probabilistic map can achieve a favorable balance between efficiency and stability, thereby significantly enhancing adaptability across multiple scenarios.

4.4.4. Control Group 4

(1)

Experimental design

In the design of Control Group 4, the initial path is generated based on the global probability map, while the linear heuristic function $h_{new}=l\times p\left(n\right)\times h_0\left(n\right)(l=1)$ of Control Group 2 is selected, and the remaining parameters, steps, and models are consistent with the experimental group. By comparing the cost, the number of nodes, the runtime, and the degree of deviation of the comprehensive path, the performance difference in linear and nonlinear design in path smoothness and efficiency is analyzed.

(2)

Experimental results and analysis

Figure 18 and Figure 19 illustrate the global initial paths and the corresponding obstacle-avoided paths under different map conditions, while

Table 11. Comparison of path cost and number of nodes for control group 4.

	Global Initial Path	Map 1	Map 2	Map 3	Comprehensive Path
Path cost	64.6404	65.8765	65.0802	64.6404	65.1432
Offset degree Δ	-	1.91%	0.68%	0	0.78%
Number of path nodes	10	13	12	10	11.5
Offset degree Δ	-	30%	20%	0	15%
Runtime(s)	3.0872	+0.4145	+0.2936	+0.1757	-

compares the path cost and other performance metrics. From both the visual results and the quantitative comparisons, it can be clearly observed that both the control group and the experimental group (with the experimental group results shown in Figure 10 and Figure 11 and Table 5) achieve good performance. This indicates that both linear and nonlinear probability-sensitive heuristic functions can effectively improve planning efficiency and reduce the extent of local adjustments applied to the global initial path across different sub-maps.

Compared with the experimental group, the control group exhibits slightly better performance in terms of node deviation and runtime. However, the experimental group consistently outperforms the control group in overall path cost, number of path nodes, and path cost deviation, resulting in superior overall path quality. The experimental results further confirm that the nonlinear probability-sensitive heuristic function provides more effective optimization in terms of path cost, node reduction, and path smoothness than the global scaling strategy adopted by the linear model, making it a more suitable choice for multi-objective path planning.

4.5. Extended Experiment (Stability Verification)

To further verify the advantages of the two-stage improved algorithm in large-scale and complex scenarios, the following extended experiments are added. Based on a 100 × 100 raster map (including four sub-maps with occurrence probabilities of 0.15, 0.4, 0.25, and 0.2, respectively), the analysis is deepened from two dimensions: parameter robustness and strategy comparison.

4.5.1. Expansion Group 1

(1)

Experimental design

In the context of a larger-scale map, set $\alpha =10, \beta =10 \mathrm{a}\mathrm{n}\mathrm{d} p_b=0.3$ to generate a global probability map. Use a probability-sensitive heuristic function to generate the initial path, and then make local corrections on each sub-map to ensure complete obstacle avoidance. Calculate the comprehensive cost, the number of nodes, and the offset degree of the global path and the local path, and quantify the performance of the algorithm.

(2)

Experimental results and analysis

The global initial path and the map obstacle avoidance path correction are shown in Figure 20 and Figure 21, respectively, while the comparisons on path cost and other performances are shown in

Table 12. Comparison of path cost and number of nodes for expansion group 1.

	Global Initial Path	Map 1	Map 2	Map 3	Map 4	Comprehensive Path
Path cost	131.3982	132.5209	131.6759	131.8916	131.9647	131.9143
Offset degree Δ	-	0.85%	0.21%	0.38%	0.43%	0.39%
Number of path nodes	9	12	11	11	11	11.15
Offset degree Δ	-	33.33%	22.22%	22.22%	22.22%	23.89%

. An analysis of the above figures and table shows that in large-scale application scenarios, the improved algorithm still performs well. After local correction, the comprehensive path cost only increases by 0.39% compared to the global initial path, and the comprehensive offset of the number of nodes is 23.89%. Both the comprehensive path cost and the number of nodes are within a reasonable and stable range, and the local adjustment amplitude is small, indicating that the algorithm can effectively balance risk and efficiency.

4.5.2. Expansion Group 2

(1)

Experimental design

This expansion group still adopts a two-stage approach. On the sub-map with the highest occurrence probability (Map 2, 40%), the initial path is generated using the improved Theta*, and this path is selected as the global path. Next, on other sub-maps (Map 1, Map 3, Map 4), the traditional Theta* is called for local correction to generate a complete obstacle avoidance path. Finally, the comprehensive path cost and the number of nodes are calculated based on the weights of the sub-maps.

(2)

Experimental results and analysis

The global initial path on Map 2 and the map obstacle avoidance path corrections for Maps 1, 3, and 4 are shown in Figure 22 and Figure 23, respectively, while the comparisons on path cost and other performances are shown in

Table 13. Comparison of path cost and number of nodes for expansion group 2.

	Global Initial Path (Map 2)	Map 1	Map 3	Map 4	Comprehensive Path
Path cost	128.1119	131.9459	131.4376	131.6263	64.37151
Offset degree Δ	-	2.99%	2.60%	2.74%	2.42%
Number of path nodes	12	23	19	22	12
Offset degree Δ	-	91.67%	58.33%	83.33%	33.33%

. It is found from the above maps and table that, although the initial path cost and the locally corrected offset cost are relatively low, on other sub-maps, the adaptability of the initial path has obvious limitations. A large number of local adjustments are required for complete obstacle avoidance of the path, and the number of nodes doubles, resulting in a significant decrease in path smoothness and efficiency. This indicates that this strategy fails to integrate multi-map information and only considers obstacle avoidance in the case with the highest occurrence probability. As a result, it is unable to avoid the densely populated areas of non-high-probability sub-maps, and there is a local optimization defect.

4.5.3. Comparison and Summary of the Expansion Group

The comparative analysis between the two expansion groups clearly indicates that the local correction amplitude in Group 1 is substantially lower than in Group 2. This result validates that global planning with multi-map fusion effectively integrates obstacle distributions from multiple scenarios, which reduces the need for local adjustments and results in a more stable overall path. The improved algorithm hierarchical planning architecture based on multi-map probability fusion shows significant advantages in complex, large-scale scenarios. Its global prediction ability and parameter robustness provide reliable technical support for actual engineering deployment.

4.6. Experiment Summary

In this section, the proposed improved algorithm is benchmarked against conventional algorithms using various heuristic functions and planning strategies. Its advantages are quantified through metrics including path cost, number of nodes, runtime, and deviation degree. Furthermore, the innovative contributions of the multi-map probabilistic fusion framework, the nonlinear probability-sensitive heuristic function, and the hierarchical planning architecture are validated. The main findings are summarized as follows:

(1)

Multi-map probability fusion improves the global adaptability of paths

The initial path generated by the experimental group based on the global probability map (cost—64.633, number of nodes—9, runtime—8.3132 s) shows stable performance in most submaps. After local correction, the cost of the comprehensive path only increases by 0.86%, the number of nodes shift by 16.67%, and the runtime local increment is small (+0.7074 s, +0.6009 s, +0.5414 s), which is significantly better than the single high probability map strategy of Control Group 3 (comprehensive path offset—2.42% and number of nodes—12, node number offset—33.33%, initial runtime—8.3132 s, local adjustments: +1.1879 s, +0.7635 s). The results show that a probabilistic fusion strategy can predict multi-source obstacle distribution and reduce local adjustment cost.

(2)

Parameter sensitivity analysis guides practical application

Through the experiments of Control Group 1 and Control Group 2, the obstacle avoidance threshold p_b = 0.3 is determined as the optimal equilibrium point, and the robustness of Sigmoid function parameters (α = 10, β = 10) is verified, which provides a reference for engineering parameters adjustment in actual scenarios.

(3)

The nonlinear heuristic function is more suitable for the actual-risk model

Experimental comparisons demonstrate that the nonlinear probability-sensitive heuristic function (PSHF) outperforms linear models in both parameter tuning robustness (Control Group 2, parameter tuning experiment) and path optimization effectiveness (Experiment Group vs. Control Group 4 results). The sigmoid function’s nonlinear mapping mechanism effectively balances path length and risk aversion requirements while avoiding the sensitivity issues inherent in linear functions.

(4)

Stability verification in large-scale scenarios

The extended experiments demonstrate that the proposed hierarchical planning framework maintains stable path structures under larger-scale and more complex multi-map environments. The global probability fusion effectively reduces sensitivity to environmental variations, requiring only limited local adjustments and ensuring consistent planning performance across different scenarios.

(5)

Hierarchical planning architecture balances efficiency and security

In the two-stage design (global optimization + local correction), the comprehensive path cost (65.19) is reduced by 0.73% and the number of nodes is reduced by 25% compared with the traditional Theta* full obstacle avoidance path (65.67) under the premise of ensuring complete obstacle avoidance. In the local correction stage, only a minimal number of nodes and runtime costs (average deviation of 1.79%, with runtime increases of +0.7074 s, +0.6009 s, and +0.5414 s) were inserted to satisfy the obstacle avoidance requirements of the submap, which verifies the practicability and stability of the layered architecture.

5. Conclusions

This paper proposes a hierarchical path planning framework for uncertain rescue environments based on multi-map probabilistic fusion and a nonlinear probability-sensitive heuristic function. By integrating probabilistic information from multiple sub-maps, a global initial path with strong generality is generated, followed by efficient local corrections to ensure complete obstacle avoidance under different scenarios. Extensive simulation experiments, including comparative and extended studies, demonstrate that the proposed method achieves a favorable balance between path safety, smoothness, and computational efficiency. The nonlinear heuristic design effectively improves robustness to parameter tuning and avoids the excessive sensitivity observed in linear probability-scaling strategies. Moreover, the hierarchical planning architecture significantly reduces local adjustment costs while maintaining stable path quality across multiple environments. Overall, the proposed approach significantly improves adaptability across multiple uncertain scenarios, providing a reliable and robust solution for path planning in unstructured, non-deterministic environments.

Despite its effectiveness, this work is limited to grid-based map representations, which may introduce a large number of nodes and increase computational complexity in large-scale environments. Extending the proposed probabilistic fusion and planning framework to continuous map representations with explicit obstacle geometries remains an important direction for future research. In addition, while this study adopts a model-based graph search framework to ensure interpretability and controllability, learning-based methods, such as reinforcement learning, have shown strong potential in handling highly dynamic and unknown environments. Exploring hybrid approaches that combine probabilistic modeling with data-driven learning strategies is a promising avenue for further investigation. Furthermore, real-world deployment and validation on physical robotic platforms will be considered to evaluate robustness under sensing noise and real-time constraints. These extensions are expected to further improve the generality and applicability of the proposed method.