Autonomous navigation path planning method and system for complex environment

Abstract

The application provides a complex environment-oriented autonomous navigation path planning method and system, and relates to the technical field of path planning. The method comprises the following steps: acquiring multi-sensor fusion data, constructing a space-time cost map containing static obstacles and dynamic obstacles; searching a global reference path from a starting point to a target point based on the space-time cost map; generating a local motion trajectory based on the global reference path and dynamic obstacle avoidance; and performing trajectory optimization on the local motion trajectory to generate an executable trajectory. The application can solve the technical problem that the autonomous navigation system is difficult to generate a continuous motion trajectory in a complex environment due to the lack of quantitative prediction and integrated analysis of the future occupation space of dynamic obstacles in the prior art, achieve the technical goal of autonomous navigation path planning and trajectory optimization based on multi-sensor fusion data and space-time cost analysis, and achieve the technical effect of generating a continuous, smooth and safe executable trajectory in a complex dynamic environment.

Description

Technical Field

[0001] This application relates to the field of path planning technology, and in particular to autonomous navigation path planning methods and systems for complex environments. Background Technology

[0002] As mobile robots are widely used in complex environments, the requirements for autonomous navigation capabilities are increasing, including achieving smooth, safe, and efficient path planning and real-time obstacle avoidance in environments with dense dynamic obstacles.

[0003] Currently, most existing autonomous navigation methods rely on static environmental maps or path planning based solely on local sensor information. They lack effective prediction of the movement patterns of dynamic obstacles and spatial occupancy analysis. They typically respond to moving obstacles through fixed rules or simple obstacle avoidance strategies. In environments where obstacles change rapidly or are densely distributed, this approach can easily lead to frequent path corrections, discontinuous trajectories, or obstacle avoidance failures. As a result, mobile robots operate inefficiently, pose potential collision risks, and affect the overall quality of task completion.

[0004] In summary, existing technologies suffer from a lack of quantitative prediction and integrated analysis of the space occupied by dynamic obstacles in the future. This makes it difficult for autonomous navigation systems to generate continuous, safe, and efficient motion trajectories in complex environments, further affecting the stable passage capability and task execution reliability of mobile robots in dynamic scenarios. Summary of the Invention

[0005] The purpose of this application is to provide an autonomous navigation path planning method and system for complex environments, in order to solve the technical problem in the prior art that the lack of quantitative prediction and integrated analysis of the space occupied by dynamic obstacles in the future makes it difficult for autonomous navigation systems to generate continuous, safe and efficient motion trajectories in complex environments, which further affects the stable passage capability and task execution reliability of mobile robots in dynamic scenarios.

[0006] In view of the above problems, this application provides an autonomous navigation path planning method and system for complex environments.

[0007] Firstly, this application provides an autonomous navigation path planning method for complex environments, implemented through an autonomous navigation path planning system for complex environments, comprising: acquiring multi-sensor fusion data and constructing a spatiotemporal cost map containing static and dynamic obstacles; searching a global reference path from the starting point to the target point based on the spatiotemporal cost map; performing dynamic obstacle avoidance based on the global reference path to generate a local motion trajectory; and optimizing the local motion trajectory to generate an executable trajectory.

[0008] Preferably, the autonomous navigation path planning method for complex environments further includes: performing fusion processing on the multi-sensor fusion data to obtain environmental element information; constructing a basic grid map based on the environmental element information, and assigning cost values ​​according to obstacle distance and terrain slope to generate a static cost map; and using a time-series prediction algorithm to analyze the historical movement trajectory of dynamic obstacles, and superimposing the occupied space as a dynamic time-varying cost onto the static cost map to generate the spatiotemporal cost map.

[0009] Preferably, the autonomous navigation path planning method for complex environments further includes: using an asymptotic optimization algorithm to take the cost value in the spatiotemporal cost map as a path cost evaluation parameter; and using the path cost evaluation parameter to search from the starting point to the target point to obtain the global reference path, with the goal of balancing path length and safe passage cost.

[0010] Preferably, the autonomous navigation path planning method for complex environments further includes: extracting local target points within a preset distance in front of the robot, guided by the global reference path; generating candidate local trajectories by sampling in the velocity space based on the local target points; evaluating the candidate local trajectories using a dynamic window through a multi-objective evaluation function, and optimally selecting the local motion trajectory.

[0011] Preferably, the autonomous navigation path planning method for complex environments further includes: the multi-objective evaluation function is constructed from obstacle safety distance, target direction proximity, and movement speed.

[0012] Preferably, the autonomous navigation path planning method for complex environments further includes: constructing a safe corridor based on the distribution of surrounding obstacles along the local motion trajectory; establishing a quadratic programming optimization model with the objectives of minimizing trajectory smoothness, execution time, and acceleration under the constraints of the safe corridor; solving the quadratic programming model and outputting the executable trajectory.

[0013] Preferably, the autonomous navigation path planning method for complex environments further includes: the safety corridor is composed of multiple interconnected free space bubbles.

[0014] Secondly, this application also provides an autonomous navigation path planning system for complex environments, used to execute the autonomous navigation path planning method for complex environments as described in the first aspect, comprising: a spatiotemporal cost map construction module, which acquires multi-sensor fusion data and constructs a spatiotemporal cost map containing static and dynamic obstacles; a global reference path search module, which searches for a global reference path from the starting point to the target point based on the spatiotemporal cost map; a local motion trajectory generation module, which performs dynamic obstacle avoidance based on the global reference path and generates a local motion trajectory; and an executable trajectory generation module, which optimizes the local motion trajectory to generate an executable trajectory.

[0015] The technical solution provided in this application has at least the following technical effects or advantages: by achieving the technical goal of autonomous navigation path planning and trajectory optimization based on multi-sensor fusion data and spatiotemporal cost analysis, it can generate continuous, smooth and safe executable trajectories in complex dynamic environments, thereby improving the mobility efficiency and task completion reliability of mobile robots.

[0016] The above description is merely an overview of the technical solution of this application. To enable a clearer understanding of the technical means of this application and to facilitate its implementation according to the description, and to make the above and other objects, features, and advantages of this application more apparent, specific embodiments of this application are described below. It should be understood that the content described in this section is not intended to identify key or important features of the embodiments of this application, nor is it intended to limit the scope of this application. Other features of this application will become readily apparent through the following description. Attached Figure Description

[0017] To more clearly illustrate the technical solutions in this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are merely exemplary. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0018] Figure 1 This is a flowchart illustrating the autonomous navigation path planning method for complex environments proposed in this application.

[0019] Figure 2 This is a schematic diagram of the structure of the autonomous navigation path planning system for complex environments proposed in this application.

[0020] Figure labeling: Spatiotemporal cost map construction module 1, global reference path search module 2, local motion trajectory generation module 3, executable trajectory generation module 4. Detailed Implementation

[0021] This application provides an autonomous navigation path planning method and system for complex environments, addressing the technical problem in existing technologies where the lack of quantitative prediction and integrated analysis of the future space occupied by dynamic obstacles makes it difficult for autonomous navigation systems to generate continuous, safe, and efficient motion trajectories in complex environments, further affecting the stable passage capability and task execution reliability of mobile robots in dynamic scenarios. It achieves the technical goal of autonomous navigation path planning and trajectory optimization based on multi-sensor fusion data and spatiotemporal cost analysis, enabling the generation of continuous, smooth, and safe executable trajectories in complex dynamic environments, thereby improving the passage efficiency and task completion reliability of mobile robots.

[0022] The technical solutions of this application will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them. It should be understood that this application is not limited to the exemplary embodiments described herein. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application. It should also be noted that, for ease of description, only the parts related to this application are shown in the accompanying drawings, not all of them.

[0023] Example 1, please refer to the appendix. Figure 1 This application provides an autonomous navigation path planning method for complex environments, which is applied to an autonomous navigation path planning system for complex environments. The method includes the following steps:

[0024] S1: Acquire multi-sensor fusion data and construct a spatiotemporal cost map containing both static and dynamic obstacles.

[0025] Furthermore, this application also includes: performing fusion processing on the multi-sensor fusion data to obtain environmental element information; constructing a basic grid map based on the environmental element information, and assigning cost values ​​according to obstacle distance and terrain slope to generate a static cost map; using a time-series prediction algorithm to analyze the historical movement trajectory of dynamic obstacles, and superimposing the occupied space as a dynamic time-varying cost onto the static cost map to generate the spatiotemporal cost map.

[0026] Specifically, by using various types of sensors installed on a mobile platform, including but not limited to lidar, millimeter-wave radar, cameras, inertial measurement units, and ultrasonic sensors, environmental information is collected and uniformly formatted to obtain multi-sensor fusion data that reflects the surrounding environmental structure, object position, and motion state.

[0027] By fusing multi-sensor data, including time synchronization, spatial registration, and data filtering, redundant information and measurement errors between different sensors can be eliminated, thereby accurately extracting environmental element information, such as the location, shape, and height of obstacles, as well as the boundary features of passable areas.

[0028] By discretizing the environmental space into grid cells of fixed size and assigning corresponding costs based on the distance to obstacles and the slope of the terrain within each grid cell to represent the passage cost of that location, a static cost map is generated. The static cost map is used to describe environmental features and potential passage difficulties that do not change over time.

[0029] By analyzing the historical movement trajectories of dynamic obstacles using time-series prediction algorithms and establishing a predictive model of obstacle position changes over time, it is possible to infer the space that obstacles may occupy in the future and superimpose the cost of the occupied space onto a static cost map, thereby forming a spatiotemporal cost map. This spatiotemporal cost map can simultaneously reflect the spatial structural characteristics of the environment and the time-varying passage cost of dynamic obstacles, providing a quantifiable cost reference for subsequent path planning.

[0030] S2: Based on the spatiotemporal cost map, search for a global reference path from the starting point to the target point.

[0031] Furthermore, this application also includes: employing an asymptotic optimization algorithm to use the cost value in the spatiotemporal cost map as a path cost evaluation parameter; and, with the goal of balancing path length and safe passage cost, searching from the starting point to the target point using the path cost evaluation parameter to obtain the global reference path.

[0032] Specifically, an asymptotic optimization algorithm is used to iteratively generate and expand the set of sampling points to gradually approximate the optimal path from the starting point to the target point. The cumulative cost of the grid cells traversed by each path segment is used as a path cost evaluation parameter, thereby quantifying the overall cost of the path.

[0033] By searching from the starting point to the target point using path cost evaluation parameters, with the goal of balancing path length and safe passage cost, it means that during the path generation process, the total path length and the passage safety at each position along the path are considered simultaneously. By comparing and selecting the total cost of different candidate paths, a global reference path that achieves a balance between length and safety is obtained. This path provides guidance for subsequent local obstacle avoidance and trajectory optimization.

[0034] S3: Perform dynamic obstacle avoidance based on the global reference path to generate a local motion trajectory.

[0035] Furthermore, this application also includes: extracting local target points within a preset distance in front of the robot, guided by the global reference path; generating candidate local trajectories by sampling in the velocity space based on the local target points; evaluating the candidate local trajectories using a dynamic window through a multi-objective evaluation function, and optimally selecting the local motion trajectory.

[0036] Furthermore, this application also includes: the multi-objective evaluation function is constructed from the obstacle safety distance, the target direction approach degree, and the movement speed.

[0037] Specifically, guided by the global reference path, local target points within a preset distance in front of the robot are extracted. This includes setting a fixed detection distance in front of the mobile platform along the global reference path, obtaining the target point positions in the local space through discrete sampling, and using them as reference nodes for local motion planning to guide subsequent trajectory generation and obstacle avoidance strategy implementation.

[0038] Under the constraints of the current velocity, acceleration, and motion of the mobile platform, several possible trajectory candidate schemes are generated by discretely sampling the velocity vector space. Each candidate trajectory starts from the current position, takes the local target point as the reference endpoint, satisfies the kinematic and dynamic constraints, and generates multiple path selections to cope with dynamic environmental changes.

[0039] A multi-objective evaluation function is used to evaluate candidate local trajectories using a dynamic window, thereby constructing a comprehensive evaluation index. Under constraints such as obstacle safety distance, target point directional proximity, and movement speed, the comprehensive evaluation index quantifies and scores each candidate trajectory within the dynamic window. By comparing the scores, the trajectory with the highest score is selected as the optimal trajectory, ultimately outputting the local motion trajectory. This ensures that the mobile platform achieves smooth, executable, and safe local navigation behavior in complex environments.

[0040] Furthermore, the multi-objective evaluation function is constructed from obstacle safety distance, target directional approach, and movement speed. Specifically, a mathematical model is established to quantify the minimum safe distance between the mobile platform and surrounding obstacles as a safety distance index, evaluating the feasibility of the trajectory in obstacle avoidance. Simultaneously, the directional deviation between the local target point and the current position of the mobile platform is quantified as a target directional approach index, measuring whether the trajectory is moving towards the target along the global reference path. The speed change of the mobile platform along the candidate trajectory is quantified as a movement speed index, evaluating the trajectory's performance in satisfying dynamic constraints and execution efficiency. By weighting and combining these three indices to form a comprehensive evaluation function, the candidate trajectory achieves a balance in terms of safety, target directional approach, and speed control, thus providing a quantitative basis for the optimal selection of local motion trajectories.

[0041] S4: Optimize the local motion trajectory to generate an executable trajectory.

[0042] Furthermore, this application also includes: constructing a safe corridor based on the distribution of surrounding obstacles along the local motion trajectory; establishing a quadratic programming optimization model with the objectives of minimizing trajectory smoothness, execution time, and acceleration under the constraints of the safe corridor; solving the quadratic programming model and outputting the executable trajectory.

[0043] Furthermore, this application also includes: the security corridor is composed of multiple interconnected free space bubbles.

[0044] Specifically, a safety corridor is constructed along the local movement trajectory based on the distribution of surrounding obstacles. This includes determining the boundaries of the free passage area by performing spatial analysis on the current position of the mobile platform and the obstacles around the trajectory, and generating multiple continuous and connected free space areas along the trajectory. These free space areas are combined to form a safety channel that restricts the movement of the mobile platform, in order to ensure that a necessary safe distance is maintained from obstacles during trajectory execution.

[0045] The trajectory optimization problem is formalized as a constrained optimization problem, where the constraints consist of the safety corridor boundary and the dynamic constraints of the moving platform. The objective function comprehensively considers the trajectory curvature change to improve smoothness, the total time required for trajectory execution, and the acceleration value along the trajectory. The performance of the trajectory in terms of smoothness, time efficiency, and dynamic stability is quantified through mathematical methods.

[0046] Solving the quadratic programming model and outputting an executable trajectory involves iteratively finding the optimal solution under the established quadratic programming problem constraints and objective function using a numerical optimization algorithm. This generates a trajectory sequence that satisfies safety, smoothness, and dynamic constraints, serving as a motion path that the mobile platform can directly execute. This achieves a closed-loop connection from local motion planning to actual trajectory control.

[0047] Furthermore, the safety corridor consists of multiple interconnected free-space bubbles. Along the local motion trajectory, by analyzing the positions and spatial distribution of surrounding obstacles, a continuous free area accessible to the mobile platform is determined, and each free area is represented as a free-space bubble. Each bubble has a center position and radius, used to define the safe passage range. By sequentially connecting multiple free-space bubbles to form a continuous safety corridor, the minimum safe distance between the local motion trajectory and obstacles is maintained during execution. This also provides constraints for trajectory optimization and motion control, achieving smooth, safe, and executable motion path planning.

[0048] In summary, the autonomous navigation path planning method for complex environments provided in this application has the following technical effects: by achieving the technical goal of autonomous navigation path planning and trajectory optimization based on multi-sensor fusion data and spatiotemporal cost analysis, it can generate continuous, smooth, and safe executable trajectories in complex dynamic environments, thereby improving the mobility efficiency and task completion reliability of mobile robots.

[0049] Example 2: Based on the same inventive concept as the autonomous navigation path planning method for complex environments described in the foregoing examples, this application also provides an autonomous navigation path planning system for complex environments. Please refer to the appendix. Figure 2 The system includes: a spatiotemporal cost map construction module 1, which acquires multi-sensor fusion data and constructs a spatiotemporal cost map containing static and dynamic obstacles; a global reference path search module 2, which searches for a global reference path from the starting point to the target point based on the spatiotemporal cost map; a local motion trajectory generation module 3, which performs dynamic obstacle avoidance based on the global reference path and generates a local motion trajectory; and an executable trajectory generation module 4, which optimizes the local motion trajectory to generate an executable trajectory.

[0050] Furthermore, the autonomous navigation path planning system for complex environments is also used to: perform fusion processing on the multi-sensor fusion data to obtain environmental element information; construct a basic grid map based on the environmental element information, and assign cost values ​​according to obstacle distance and terrain slope to generate a static cost map; analyze the historical movement trajectory of dynamic obstacles using a time-series prediction algorithm, and superimpose the occupied space as a dynamic time-varying cost onto the static cost map to generate the spatiotemporal cost map.

[0051] Furthermore, the autonomous navigation path planning system for complex environments is also used to: employ an asymptotic optimization algorithm to use the cost value in the spatiotemporal cost map as a path cost evaluation parameter; and, with the goal of balancing path length and safe passage cost, search from the starting point to the target point using the path cost evaluation parameter to obtain the global reference path.

[0052] Furthermore, the autonomous navigation path planning system for complex environments is also used to: extract local target points within a preset distance in front of the robot, guided by the global reference path; generate candidate local trajectories by sampling in the velocity space based on the local target points; evaluate the candidate local trajectories using a dynamic window through a multi-objective evaluation function, and optimally select the local motion trajectory.

[0053] Furthermore, the autonomous navigation path planning system for complex environments is also used in the following way: the multi-objective evaluation function is constructed from the obstacle safety distance, the target direction proximity, and the movement speed.

[0054] Furthermore, the autonomous navigation path planning system for complex environments is also used to: construct a safe corridor based on the distribution of surrounding obstacles along the local motion trajectory; establish a quadratic programming optimization model with the goal of minimizing trajectory smoothness, execution time, and acceleration under the constraints of the safe corridor; solve the quadratic programming model and output the executable trajectory.

[0055] Furthermore, the autonomous navigation path planning system for complex environments is also used in the following: the safety corridor is composed of multiple interconnected free-space bubbles.

[0056] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The autonomous navigation path planning method and specific examples for complex environments in the foregoing Embodiment 1 are also applicable to the autonomous navigation path planning system for complex environments in this embodiment. Through the foregoing detailed description of the autonomous navigation path planning method for complex environments, those skilled in the art can clearly understand the autonomous navigation path planning system for complex environments in this embodiment. Therefore, for the sake of brevity, it will not be described in detail here.

[0057] The above description of the disclosed embodiments enables those skilled in the art to make or use this application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of this application. Therefore, this application is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0058] Obviously, those skilled in the art can make various modifications and variations to this application without departing from the spirit and scope of this application. Therefore, if such modifications and variations fall within the scope of this application and its equivalents, this application also intends to include such modifications and variations.

Claims

1. An autonomous navigation path planning method for complex environments, characterized in that, include: Acquire multi-sensor fusion data to construct a spatiotemporal cost map that includes both static and dynamic obstacles; Based on the spatiotemporal cost map, a global reference path is searched from the starting point to the target point; Dynamic obstacle avoidance is performed based on the global reference path to generate a local motion trajectory; The local motion trajectory is optimized to generate an executable trajectory.

2. The autonomous navigation path planning method for complex environments as described in claim 1, characterized in that, Construct a spatiotemporal cost map that includes both static and dynamic obstacles, including: The multi-sensor fusion data is fused to obtain environmental element information; A basic grid map is constructed based on the environmental element information, and a cost value is assigned according to the obstacle distance and terrain slope to generate a static cost map. The historical movement trajectory of dynamic obstacles is analyzed using a time-series prediction algorithm, and the occupied space is superimposed on the static cost map as a dynamic time-varying cost to generate the spatiotemporal cost map.

3. The autonomous navigation path planning method for complex environments as described in claim 1, characterized in that, Based on the aforementioned spatiotemporal cost map, a global reference path is searched from the starting point to the target point, including: An asymptotic optimization algorithm is used to take the cost value in the spatiotemporal cost map as a path cost evaluation parameter; With the goal of balancing path length and safe passage cost, the global reference path is obtained by searching from the starting point to the target point using the path cost evaluation parameters.

4. The autonomous navigation path planning method for complex environments as described in claim 1, characterized in that, Dynamic obstacle avoidance is performed based on the global reference path, generating local motion trajectories, including: Guided by the global reference path, extract local target points within a preset distance in front of the robot; Based on the local target points, candidate local trajectories are generated by sampling within the velocity space; The candidate local trajectories are evaluated using a dynamic window through a multi-objective evaluation function, and the optimal local motion trajectory is obtained by selection.

5. The autonomous navigation path planning method for complex environments as described in claim 4, characterized in that, The multi-objective evaluation function is constructed from the obstacle safety distance, the target direction approach, and the movement speed.

6. The autonomous navigation path planning method for complex environments as described in claim 1, characterized in that, The local motion trajectory is optimized to generate an executable trajectory, including: A safe corridor is constructed based on the distribution of surrounding obstacles along the local motion trajectory. Under the constraints of the safety corridor, a quadratic programming optimization model is established with the objectives of minimizing trajectory smoothness, execution time, and acceleration. Solve the quadratic programming model and output the executable trajectory.

7. The autonomous navigation path planning method for complex environments as described in claim 6, characterized in that, The safety corridor is composed of multiple interconnected free-space bubbles.

8. An autonomous navigation path planning system for complex environments, characterized in that: The steps for implementing the autonomous navigation path planning method for complex environments according to any one of claims 1 to 7 include: A spatiotemporal cost map construction module is used to acquire multi-sensor fusion data and construct a spatiotemporal cost map that includes static and dynamic obstacles. A global reference path search module is used to search for a global reference path from the starting point to the target point based on the spatiotemporal cost map. A local motion trajectory generation module is used to generate a local motion trajectory by performing dynamic obstacle avoidance based on the global reference path. An executable trajectory generation module is used to optimize the local motion trajectory and generate an executable trajectory.

Images