Adaptive path planning algorithm for closed denial environment

Abstract

The invention discloses a closed denial environment adaptive path planning algorithm, and belongs to the technical field of mobile robot navigation. According to the algorithm, aiming at the characteristics of dense obstacles and many dynamic interferences in a closed denial environment, an artificial potential field method is innovatively combined with a sampling-based RRT algorithm and a dynamic window method, so that organic fusion of global and local path planning is realized. According to the algorithm, an artificial potential field gravitational field is introduced into an RRT sampling process, so that new node generation always deviates to a target direction, and adaptive change is realized by dynamically adjusting a sampling sector angle; in local path planning, a global artificial potential field gravitational field is utilized to optimize a DWA evaluation algorithm, and a better dynamic obstacle avoidance effect is achieved. The algorithm is suitable for path planning requirements of unmanned aerial vehicles, specialized robots and the like in a closed denial environment.

Description

Technical Field

[0001] The present invention belongs to the field of mobile robot navigation and relates to an adaptive path planning algorithm for a closed and denied environment. Background Art

[0002] In the field of mobile robot navigation, confined and restricted environments place higher demands on path planning algorithms. Global paths must converge quickly in densely populated areas, and local obstacle avoidance must cope with the dynamic interference of multiple moving targets. Path planning technology is the core component of mobile robots' missions. Together with sensor information fusion, motion control, and autonomous positioning technologies, it forms the basis of a mobile robot's autonomous navigation system.

[0003] Traditional path planning methods can be categorized as global and local. Global path planning algorithms, such as the A* algorithm and the RRT algorithm, can generate global paths but fail in environments with dynamic obstacles. Local path planning algorithms, such as the artificial potential field method and the dynamic window method, can be used to avoid obstacles, but the resulting paths are long and of low quality, making them only suitable for local environments. With the advancement of computer technology, numerous emerging intelligent algorithms have been applied to path planning, such as particle swarm optimization, ant colony optimization, genetic algorithms, and deep reinforcement learning algorithms.

[0004] The RRT* algorithm is a sampling-based path planning algorithm commonly used to find continuous, feasible paths in large, high-dimensional spaces. It works by rapidly constructing a random tree structure, randomly sampling nodes in space and then expanding the tree toward the target point. However, its random search efficiency is low in confined spaces, making it ineffective in addressing the challenges of confined, denied environments.

[0005] The Dynamic Window Approach (DWA) is a local path planning technique that uses the vehicle's current position as the center of the window. Using a mathematical model, it collects velocity samples from the robot and predicts and simulates the resulting motion trajectories over a period of time at the sampled speeds. These trajectories are then evaluated using a standard to select the optimal set of trajectories. However, traditional DWA is prone to local oscillation in certain scenarios and suffers from insufficient obstacle avoidance capabilities when multiple obstacles are moving simultaneously.

[0006] The Artificial Potential Field (APF) method is a path planning algorithm based on a virtual force field. Its core concept is to model the environment as a potential field: the target point generates attraction, obstacles generate repulsion, and the robot finds a collision-free path by descending the potential gradient. However, the APF method is prone to falling into local minima at non-target equilibrium points, such as U-shaped obstacles.

[0007] Since various path planning algorithms themselves have certain limitations, relying solely on global or local path planning algorithms is difficult to effectively deal with path planning problems in complex dynamic scenarios. Only by overcoming these limitations can mobile robots operate stably in a global dynamic environment. Summary of the Invention

[0008] This paper aims to address the shortcomings of existing technologies and proposes an adaptive path planning algorithm for confined and denied environments. By combining global and local algorithms, this algorithm innovatively introduces an artificial potential field (APF) into the sampling process and proposes a hierarchical optimization scheme to better adapt to the path planning requirements of confined and denied environments.

[0009] The technical solutions of the present invention are as follows:

[0010] The adaptive path planning algorithm for a closed and denied environment includes the following steps:

[0011] 1) Initialize the grid map in the closed denial environment, convert the map grid data into obstacle coordinate information, and expand the obstacles. The expanded obstacle coordinates are input into the fusion algorithm;

[0012] 2) Set sampling strategy parameters, including random sampling probability, target point selection probability during artificial potential field (APF) sampling, leaf node selection probability, and random node selection probability;

[0013] 3) Initialize the adaptive angle constraint parameters, including the initial sampling angle, maximum sampling angle, minimum sampling angle, and angle adjustment step size;

[0014] 4) In each iteration, random sampling or APF hybrid sampling is chosen with probability. For random sampling, sampling is performed within an angle range based on the APF force direction of the current node. For APF hybrid sampling, an expansion node is selected based on a set probability and a new node is generated in combination with the APF force.

[0015] 5) Perform collision detection on the newly generated nodes and adjust the sampling angle range adaptively according to the detection results; when a collision occurs, the angle range is increased, and the adjustment formula is: θ=min(θ+θ step ,θ max ); When there is no collision, the angle range is reduced, and the adjustment formula is: θ=max(θ-θ step ,θ min ); where θ is the sampling angle, θ step Adjust the step size for the sampling angle, θ max is the maximum sampling angle, θ min is the minimum sampling angle;

[0016] 6) Add the new node that passes the collision detection to the RRT tree and repeat steps 4) and 5) until a feasible path to the target point is found or the maximum number of iterations is reached;

[0017] 7) Optimize the found path using the line-of-sight accessibility principle. Starting from the starting point, use ray detection to verify whether direct connections to subsequent nodes are feasible. When the farthest directly reachable point is found, it is set as the new detection starting point. Repeat this process until the target point, significantly simplifying the path nodes and obtaining the global path.

[0018] 8) Based on the global path obtained in step 7), the dynamic window required by the local path planning algorithm is constructed. The velocity space is generated according to the robot kinematic formula and the corresponding constraints, and the velocity space sampling is obtained by taking the intersection with the obstacle constraint;

[0019] 9) Perform uniform sampling in the speed space according to the set resolution, remove speed combinations that do not meet the conditions, and obtain speed combinations that meet the conditions;

[0020] 10) Using the speed combination sampled in step 9) and the kinematic formula of the mobile robot, a path simulation is performed to generate a local trajectory in the short term in the future;

[0021] 11) Calculate the attraction, repulsion and resultant force of the mobile robot at its current position based on the relevant parameters of the APF;

[0022] The calculation formula for the attraction of the target point in the APF algorithm is:

[0023]

[0024] The calculation formula of obstacle repulsion force in the APF algorithm is:

[0025]

[0026] Among them, F att For attraction, U att is the attractive potential energy, F rep is the repulsive force, q is the current position of the robot, q goal is the target point position, q obs is the obstacle position, ρ0 is the influence radius of the obstacle repulsive force potential energy, ρ(q) is the distance from the robot to the nearest obstacle, is the influence radius of the obstacle repulsive force potential energy, k att is the attraction gain parameter, k rep is the repulsive force gain coefficient.

[0027] 12) The sampled velocity combinations are scored by angle, speed, and distance. The angle score is based on the cosine similarity between the APF resultant force direction and the average heading angle of the current trajectory. The speed score uses an exponential decay function to obtain the current velocity score.

[0028] 13) The simulation trajectory with the highest score is selected by weighted summation, and the speed combination with the highest score is executed. Steps 8) to 13) are repeated until the mobile robot position reaches the target point or times out.

[0029] The step 7) specifically includes:

[0030] 7-1) Starting from the starting point, use ray detection to verify whether direct connection with subsequent nodes is feasible;

[0031] 7-2) When the farthest directly reachable point is found, it is set as the new detection starting point;

[0032] 7-3) Repeat steps 7-1) and 7-2) until the target point is reached, thus achieving a significant simplification of the path nodes.

[0033] The more specific calculation method of step 12) is:

[0034] For angle scoring, the cosine similarity between the APF resultant force direction and the average heading angle of the current trajectory is calculated as the scoring basis:

[0035] cos(θ F -θ traj )

[0036] Among them, θ F is the direction of the resultant force of APF, θ traj Is the average heading angle of the current trajectory. The average heading angle is directly obtained by taking the direction angle of the trajectory end point relative to the starting point:

[0037]

[0038] Among them, x end is the horizontal coordinate of the end point of the current trajectory, x start is the horizontal coordinate of the starting point; y end is the ordinate of the end point of the current trajectory, y start is the vertical coordinate of the starting point.

[0039] The speed score uses an exponential decay function to obtain the current speed score:

[0040]

[0041] Where k is the control sensitivity constant, v is the linear velocity, ω is the angular velocity, and v idealIt is the ideal speed obtained by mapping the APF resultant force to the speed:

[0042]

[0043] Among them, F norm is the maximum force that the robot can receive, v max is the maximum linear velocity of the robot, F total is the resultant force of APF.

[0044] The beneficial effects of the present invention are:

[0045] 1. To address the low efficiency of random searches in confined spaces caused by the RRT* algorithm, we innovatively introduced an artificial potential gravitational field into the sampling process, ensuring that new nodes are always generated in the direction of the target. Dynamically adjusting the sampling sector angle achieves adaptive change, reducing the expansion of useless nodes while maintaining the algorithm's ability to escape. In testing, this improved algorithm achieved an 8x improvement in path search convergence time compared to the original algorithm.

[0046] 2. To address the flaw of the traditional dynamic window method (DWA), which is prone to local oscillation in certain scenarios, a global artificial potential field (gravitational field) is used to optimize the evaluation algorithm in the DWA algorithm process, achieving better dynamic obstacle avoidance. Even when faced with multiple moving obstacles, the algorithm will not pass through dynamic obstacles due to sampling resolution issues. The introduction of the APF in the evaluation algorithm for selecting the simulated path ensures that the final trajectory not only meets the kinematic constraints but also stays away from the threat zone of moving obstacles. Field tests show that in 100 dynamic obstacle avoidance tests, the improved algorithm increased the obstacle avoidance success rate from 64% of the original DWA algorithm to 77%.

[0047] 3. The fusion algorithm proposed in this invention organically combines global and local planning, which not only retains the high efficiency and path optimization capability of the RRT* algorithm's global path planning, but also enhances the real-time obstacle avoidance capability in dynamic environments, and realizes reliable navigation in confined and denied environments.

[0048] 4. This invention is applicable to the path planning requirements of drones, special robots, etc. in closed and denied environments.

[0049] The present invention has the following characteristics:

[0050] (1) Efficient sampling strategy: By introducing an adaptive sampling mechanism guided by the APF force direction, the search efficiency of the RRT* algorithm in confined obstacle environments is greatly improved;

[0051] (2) Powerful obstacle avoidance capability: By integrating the APF force into the DWA evaluation function, the obstacle avoidance capability for multiple dynamic obstacles is significantly improved;

[0052] (3) Strong adaptability: The system can dynamically adjust the sampling angle range and evaluation parameters according to the complexity of the environment to adapt to different navigation scenarios;

[0053] (4) High practicality: The algorithm performs well in both simulation tests and actual projects, and has been successfully applied to a variety of closed and denied environment scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 Schematic diagram of the improved RRT* algorithm;

[0055] Figure 2 This is a schematic diagram of the principle of adaptive angle sampling;

[0056] Figure 3 This is a schematic diagram of the line of sight accessibility path optimization principle;

[0057] Figure 4 It is the overall flow chart of the present invention;

[0058] Figure 5 This is a specific experimental map of the present invention;

[0059] Figure 6 Schematic diagram of specific experimental results of the present invention. DETAILED DESCRIPTION

[0060] The specific implementation of the present invention is further described below in conjunction with the accompanying drawings and technical solutions.

[0061] The present invention designs a closed denial environment adaptive path planning algorithm, such as Figure 4 As shown, the specific operation of this path planning algorithm is divided into two parts. The first part is the grid map initialization and global path planning stage, and the second part is the local path planning stage. These two parts are combined. After the global path is planned, sub-target points are selected in the planned global path and the sub-target points are used as the target points for local path planning. The specific steps of the present invention are as follows:

[0062] 1) When designing the global path planning module, the path search is performed based on the improved RRT* algorithm. Here, the artificial potential field method is innovatively introduced to guide the sampling process. The schematic diagram of the improved algorithm is shown in the following figure. Figure 1 As shown in the figure, the APF resultant force direction is added to the randomly sampled paths of the RRT* algorithm, giving the search direction a certain degree of directionality. The algorithm sets the random sampling probability to 0.7, the APF attraction coefficient to 0.7, and the repulsion coefficient to 0.8. The introduction of the APF resultant force direction ensures that the sampling points are always biased towards the target direction, effectively reducing useless searches caused by excessive randomness.

[0063] 2) Adaptive angle sampling, as shown in the diagram Figure 2As shown in the figure, during the adaptive angle sampling phase, if there is an obstacle in the sampling direction, the sampling angle is increased by a step size. If there is no obstacle in the sampling direction, the sampling angle is decreased by a step size, making random sampling more targeted. To address the low efficiency of traditional RRT* random sampling, the present invention proposes an adaptive angle sampling strategy based on the APF resultant force direction:

[0064] 2-1) With the APF resultant force direction of the current node as the center, the sampling points are restricted to be within a certain angle range θ, forming a fan-shaped search area. The initial angle is set to a moderate value, which not only ensures the goal orientation of the search, but also maintains a certain random exploration capability.

[0065] 2-2) Perform collision detection on the newly generated nodes. If a collision with an obstacle is detected, the angle range is increased; if no collision is detected, the angle range is decreased. This allows for dynamic adjustment of the sampling space, expanding the search range in areas with dense obstacles and concentrating the search in open areas, thereby improving algorithm efficiency.

[0066] 2-3) Add the new node that passes the collision detection to the RRT tree, and use the line of sight accessibility principle to optimize the found path, such as Figure 3 As shown in the figure, the planned path is optimized. The specific method is to start from the starting point, check the direct reachability to the subsequent nodes, find the farthest directly reachable point, use it as the end point of the optimized path, and use it as the new starting point for the next path optimization. Repeat this process to the target point, significantly eliminating redundant nodes in the path and obtaining a shorter global path.

[0067] 3) Design of a local path planning module, based on an improved DWA algorithm for dynamic obstacle avoidance. The algorithm first generates a velocity sampling space based on the robot's kinematic constraints and obstacle constraints. It then uniformly samples within this space to simulate short-term trajectories under different speed combinations. The innovation lies in integrating the APF resultant force into the evaluation function, using the cosine similarity between the resultant force direction and the trajectory heading angle as the angle score. The speed score is calculated as the difference between the ideal speed mapped to the resultant force magnitude and the actual speed, enabling safer and more efficient path selection in multi-obstacle environments.

[0068] 4) Integrate the algorithms of step 2) and step 3) to build a complete adaptive path planning system.

[0069] According to the specific embodiment of the present invention, Figure 5 The specific implementation is carried out in a typical closed and denied environment shown in the figure. This environment is a typical tunnel environment in a closed and denied environment, with narrow and long passages and dense obstacles. This environment can well test the specific effect of the present invention. The specific implementation results are as follows: Figure 6, it can be seen from the specific results that the present invention can perform path planning very well in a tunnel environment, the planned path is close to the optimal path, all static obstacles in the scene are avoided, and the entire path does not have too much redundancy. Through specific experiments, it can be seen that the fusion algorithm proposed by the present invention decomposes the global path into a series of local navigation target points, and uses the improved DWA algorithm to plan the local path in real time and avoid dynamic obstacles. When the local environment changes too much or deviates too far from the global path, the global path re-planning mechanism is triggered. Through this hierarchical optimization fusion strategy, efficient navigation in a closed and denied environment is achieved, which not only ensures the approximate optimality of the global path, but also has the real-time obstacle avoidance capability for dynamic obstacles, making the final generated motion trajectory safer, smoother and more efficient.

Claims

1. Adaptive path planning algorithm for closed denial environment, characterized by: The following steps are involved: 1) Initialize the grid map in the closed denial environment, convert the map grid data into obstacle coordinate information, and expand the obstacles. The expanded obstacle coordinates are input into the fusion algorithm; 2) Set sampling strategy parameters, including random sampling probability, target point selection probability during artificial potential field (APF) sampling, leaf node selection probability, and random node selection probability; 3) Initialize the adaptive angle constraint parameters, including the initial sampling angle, maximum sampling angle, minimum sampling angle, and angle adjustment step size; 4) In each iteration, random sampling or APF hybrid sampling is chosen with probability. For random sampling, sampling is performed within an angle range based on the APF force direction of the current node. For APF hybrid sampling, an expansion node is selected based on a set probability and a new node is generated in combination with the APF force. 5) Perform collision detection on the newly generated nodes and adjust the sampling angle range adaptively according to the detection results; when a collision occurs, the angle range is increased, and the adjustment formula is: θ=min(θ+θ step ,θ max ); When there is no collision, the angle range is reduced, and the adjustment formula is: θ=max(θ-θ step ,θ min ); where θ is the sampling angle, θ step Adjust the step size for the sampling angle, θ max is the maximum sampling angle, θ min is the minimum sampling angle; 6) Add the new node that passes the collision detection to the RRT tree and repeat steps 4) and 5) until a feasible path to the target point is found or the maximum number of iterations is reached; 7) Optimize the found path using the line-of-sight accessibility principle. Starting from the starting point, use ray detection to verify whether direct connections to subsequent nodes are feasible. When the farthest directly reachable point is found, it is set as the new detection starting point. Repeat this process until the target point, significantly simplifying the path nodes and obtaining the global path. 8) Based on the global path obtained in step 7), the dynamic window required by the local path planning algorithm is constructed. The velocity space is generated according to the robot kinematic formula and the corresponding constraints, and the velocity space sampling is obtained by taking the intersection with the obstacle constraint; 9) Perform uniform sampling in the speed space according to the set resolution, remove speed combinations that do not meet the conditions, and obtain speed combinations that meet the conditions; 10) Using the speed combination sampled in step 9) and the kinematic formula of the mobile robot, a path simulation is performed to generate a local trajectory in the short term in the future; 11) Calculate the attraction, repulsion and resultant force of the mobile robot's current position based on the relevant parameters of the APF algorithm; the calculation formula for the attraction of the target point in the APF algorithm is: The calculation formula of obstacle repulsion force in the APF algorithm is: Among them, F att For attraction, U att is the attractive potential energy, F rep is the repulsive force, q is the current position of the robot, q goal is the target point position, q obs is the obstacle position, ρ0 is the influence radius of the obstacle repulsive force potential energy, ρ(q) is the distance from the robot to the nearest obstacle, is the influence radius of the obstacle repulsive force potential energy, k att is the attraction gain parameter, k rep is the repulsive force gain coefficient; 12) The sampled speed combinations are scored by angle, speed, and distance. The angle score is based on the cosine similarity between the APF resultant force direction and the average heading angle of the current trajectory. The speed score uses an exponential decay function to obtain the current speed score. 13) The simulation trajectory with the highest score is selected by weighted summation, and the speed combination with the highest score is executed. Steps 8) to 13) are repeated until the mobile robot position reaches the target point or times out.

2. The closed denial environment adaptive path planning algorithm according to claim 1 is characterized in that: The step 7) specifically includes: 7-1) Starting from the starting point, use ray detection to verify whether direct connection with subsequent nodes is feasible; 7-2) When the farthest directly reachable point is found, it is set as the new detection starting point; 7-3) Repeat steps 7-1) and 7-2) until the target point is reached, thus achieving a significant simplification of the path nodes.

3. The closed denial environment adaptive path planning algorithm according to claim 1 is characterized in that: The more specific calculation method of step 12) is: For angle scoring, the cosine similarity between the APF resultant force direction and the average heading angle of the current trajectory is calculated as the scoring basis: cos(θ F -θ traj ) Among them, θ F is the direction of the resultant force of APF, θ traj Is the average heading angle of the current trajectory. The average heading angle is directly obtained by taking the direction angle of the trajectory end point relative to the starting point: Among them, x end is the horizontal coordinate of the end point of the current trajectory, x start is the horizontal coordinate of the starting point; y end is the ordinate of the end point of the current trajectory, y start is the vertical coordinate of the starting point; The speed score uses an exponential decay function to obtain the current speed score: Where k is the control sensitivity constant, v is the linear velocity, ω is the angular velocity, and v ideal It is the ideal speed obtained by mapping the APF resultant force to the speed: Among them, F norm is the maximum force that the robot can receive, v max is the maximum linear velocity of the robot, F total is the resultant force of APF.

Images