Space-time safe path planning method based on risk field reweighting

Abstract

The invention relates to the technical field of mobile robot path planning, and particularly discloses a space-time safe path planning method based on risk field reweighting, comprising the following steps: S1, constructing a truncated reciprocal risk field; s2, constructing a dynamic obstacle space-time diagram; s3, calculating a safety interval of the risk constraint; s4, executing the RTR-SIPP path search; and S5, performing dynamic smoothing on the searched discrete space-time path. The space-time safe path planning method based on risk field reweighting aims to solve the problems of insufficient safety margin, low dynamic obstacle avoidance efficiency, unsmooth path and the like in the prior art, can automatically plan a safe path far away from an obstacle, efficiently utilize a dynamic obstacle gap and generate a smooth trackable trajectory, and has the advantages of high reliability, high reliability, high reliability and the like. And the navigation safety and reliability of the robot in a complex dynamic environment are obviously improved.

Description

Technical Field

[0001] This invention relates to the field of mobile robot path planning technology, and in particular to a spatiotemporal safe path planning method based on risk field reweighting. Background Technology

[0002] With the increasing application of mobile robots and autonomous driving technologies in complex and enclosed environments such as warehousing and logistics, smart factories, and ship interiors, path planning algorithms are facing severe challenges. These scenarios are typically characterized by narrow spaces, frequent appearances of dynamic obstacles (such as people and forklifts), and uncertain movement trajectories.

[0003] Currently, the mainstream path planning technologies can be mainly divided into the following three categories: (1) Grid-based search algorithms (such as...) Dijkstra): Principle: Discretize the environment into a grid and use a heuristic function to search for the shortest path from the starting point to the ending point.

[0004] Algorithms typically treat obstacles as binary (0 or 1), and the planned paths often hug the edges of obstacles ("walking along the wall"). In practical engineering, due to control tracking errors or sensor positioning noise, such paths can easily cause the robot to scrape against obstacles.

[0005] The algorithm only plans on a static map and cannot handle moving obstacles.

[0006] (2) Sampling-based algorithms (such as RRT, RRT*): Principle: Paths are found by randomly sampling within the state space and constructing a connected tree.

[0007] Disadvantages: Slow convergence in narrow passages: In complex and confined environments, random sampling is unlikely to fall into narrow passages, resulting in excessively long planning times or even failure; Poor path quality: The generated paths are usually tortuous and jittery, containing a large number of unnecessary turns, which is not conducive to smooth robot control; Low efficiency of dynamic obstacle avoidance: In dynamic environments, frequent replanning is required, resulting in high computational overhead and difficulty in ensuring real-time performance.

[0008] (3) Spatiotemporal planning algorithms (such as SIPP-Safe Interval Path Planning): Principle: Search in the location-time (Configuration-Time) space, using "SafeInterval" to represent the passable window of a node on the time axis.

[0009] Defects: Lack of risk perception: Standard SIPP only pursues the shortest time, ignoring the spatial distribution of risks. It may plan a path that, although it does not collide, is in a high-risk area (such as being adjacent to moving obstacles) for a long time; Poor trajectory smoothness: The output path consists of discrete spatiotemporal nodes, lacking dynamic constraints, making it difficult for robots to track directly. Summary of the Invention

[0010] The purpose of this invention is to provide a spatiotemporal safe path planning method based on risk field reweighting, which aims to solve the problems of low safety margin, poor dynamic obstacle avoidance efficiency, and non-smooth path in complex dynamic environments.

[0011] To achieve the above objectives, this invention provides a spatiotemporal safe path planning method based on risk field reweighting, comprising the following steps: S1. Construct a truncated reciprocal risk field; S2. Construct a dynamic obstacle spacetime map; S3. Calculate the safe interval of risk constraints; S4. Perform RTR-SIPP path search; S5. Perform geometric and dynamic smoothing on the searched discrete spatiotemporal paths.

[0012] Preferably, in S1, the process of constructing the truncated reciprocal risk field is as follows: Input a binary raster map G, and calculate the free grid cells on map G using Euclidean distance transform (EDT). x Euclidean distance to the nearest obstacle ; Risk value is calculated using a truncated reciprocal model. :

[0013] in, ρ ( x ) represents a grid x The risk value, Rcut The cutoff radius is the safety impact range, and λ is the risk intensity coefficient.

[0014] Preferably, in S2, the process of constructing the dynamic obstacle spatiotemporal map is as follows: Input a list of dynamic obstacles containing their current position, velocity, and predicted trajectory. O i Divide future time into discrete time layers ; At each time level, based on dynamic obstacles At any moment Predicted location and shape radius Mark dynamic obstacles on the corresponding time-layer raster map. The spatiotemporal occupancy information of the occupied area, forming dynamic obstacles.

[0015] Preferably, in S3, the calculation process for the safe interval of the risk constraint is as follows: For each grid c The list of its safe intervals is as follows: This indicates that the grid is collision-free and the risk is manageable during these time periods; First, perform hard collision detection. Based on the dynamic obstacle spatiotemporal map of S2, remove the time periods occupied by dynamic obstacles and determine the initial safe time period set that is not occupied by dynamic obstacles. Then perform soft risk filtering and check the grid. static risk value : like Then determine the grid. As a soft obstacle, This indicates a high-risk threshold, and the grid cell is removed from its initial set of safe time periods. c The final list of risk-constrained safe intervals is obtained by taking all time periods or reducing their safe time periods according to preset rules.

[0016] Preferably, S4 is as follows: The state during the search is defined as follows: ; in,( x,y ) represents raster coordinates. interval_id This is the index of the safe zone corresponding to this grid. When expanding from the initial state, calculate from the current state. s Reaching its neighbor status n Cost function: ; in, For time increments, β For risk weighting coefficients, This represents the risk value for the next state.

[0017] Preferably, S4 specifically includes the following steps: S41. Initialize the priority queue OpenList and add it to the starting point; S42. Iteratively extract the minimum cost node. ; S43, Extended Neighbor Status n Calculate from s arriven earliest feasible arrival time The time must fall within the neighbor state. n Within a certain safe zone; S44. Calculate the overall cost: ; Update the parent state and add it to the OpenList; S45 w The weight of the heuristic function is set to 1 to quickly obtain a feasible solution. Then, the weight of the heuristic function is gradually reduced to 1 to optimize the path quality and approach the optimal solution.

[0018] Preferably, S5 is as follows: S51. Use the RDP algorithm to downsample the original path and extract key turning points; S52. Use B-splines to generate continuous curves; S53. Perform dense sampling on the generated curve and check each sampling point. At its corresponding time t Whether there is a collision with a dynamic obstacle. If a collision is detected, revert to the previous level of smoothing parameters or retain the original polyline segment.

[0019] Therefore, the present invention employs the above-mentioned spatiotemporal safe path planning method based on risk field reweighting, and the beneficial effects are as follows: (1) Significantly improves the safety of the path: By introducing a truncated reciprocal risk field, this invention can automatically plan a "central" path away from static obstacles, effectively dealing with control errors; The algorithm of this invention improves the mean clearance by more than 35%.

[0020] (2) Achieves efficient and optimal dynamic obstacle avoidance: This invention utilizes the spatiotemporal characteristics of SIPP, eliminating the need for frequent replanning like D*-Lite, and instead enabling the one-time planning of trajectories utilizing dynamic obstacle gaps (such as weaving and following). In dynamic scene testing, the planning success rate reached 98%, far exceeding RRT*'s 75%.

[0021] (3) It takes into account both planning real-time performance and path quality: The Anytime strategy adopted in this invention enables the algorithm to output the first feasible solution within 20ms, which meets the real-time response requirements of high-speed robots; at the same time, it can converge to the theoretical optimal solution when there is sufficient computation time (such as 100ms).

[0022] (4) The generated trajectory has good traceability: The spline smoothing module integrated in this invention not only smooths the path geometry, but also ensures that the smoothed trajectory is still safe through dynamic collision detection, which solves the problem that traditional post-processing methods are prone to collisions.

[0023] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0024] Figure 1 This is an overall flowchart of an embodiment of a spatiotemporal safe path planning method based on risk field reweighting according to the present invention; Figure 2 This is a schematic diagram of the truncated reciprocal risk field constructed according to an embodiment of the spatiotemporal safe path planning method based on risk field reweighting of the present invention; Figure 3 This is a dynamic obstacle spatiotemporal map constructed according to an embodiment of the spatiotemporal safe path planning method based on risk field reweighting of the present invention; Figure 4 This is a schematic diagram comparing the combined path search results of multiple algorithms in an embodiment of the spatiotemporal safe path planning method based on risk field reweighting according to the present invention; Figure 5 This is a schematic diagram of the spline-based dynamic smoothing result of an embodiment of the spatiotemporal safe path planning method based on risk field reweighting according to the present invention. Detailed Implementation

[0025] The technical solution of the present invention will be further described below with reference to the accompanying drawings and embodiments.

[0026] Unless otherwise defined, the technical or scientific terms used in this invention shall have the ordinary meaning understood by one of ordinary skill in the art to which this invention pertains. The terms "first," "second," and similar terms used in this invention do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Terms such as "comprising" or "including" mean that the element or object preceding the word encompasses the elements or objects listed following the word and their equivalents, without excluding other elements or objects.

[0027] like Figure 1 As shown, a spatiotemporal safe path planning method based on risk field reweighting includes the following steps: S1. Construct a truncated reciprocal risk field to calculate the static risk distribution of the environment.

[0028] To quantify environmental risks and guide the robot away from obstacles, this scheme constructs a nonlinear risk potential field, the specific process of which is as follows: Input a binary raster map G, and calculate the free grid cells on map G using Euclidean distance transform (EDT). x Euclidean distance to the nearest obstacle .

[0029] Risk value is calculated using a truncated reciprocal model. : ; in, Represents grid x The risk value, Rcut The cutoff radius is the safe impact range. This represents the risk intensity coefficient.

[0030] like Figure 2 As shown, the truncated reciprocal risk field model constructed in this invention forms a high-risk zone near obstacles, and the risk rapidly decays to 0 with increasing distance. This ensures both the repulsive force at close range and avoids invalid calculations at distant locations, making it more efficient than the exponential model.

[0031] S2. To handle dynamic environments, the predicted trajectories of dynamic obstacles need to be mapped to the planning space. This is achieved by constructing a Dynamic Obstacle Spatio-Temporal Map, which maps dynamic obstacles to a spatio-temporal occupancy grid. The specific process is as follows: Input a list of dynamic obstacles containing their current position, velocity, and predicted trajectory. First, discretize the time layers: divide future time into discrete time layers. Then, occupation marking is performed: at each time layer, based on dynamic obstacles. At any moment Predicted location and shape radius Mark dynamic obstacles on the corresponding time-layer raster map. The spatiotemporal occupancy information of the occupied area, forming dynamic obstacles.

[0032] like Figure 3 As shown, dynamic obstacles appear as tilted columnar or pipe structures in the spatiotemporal map, clearly indicating their spatial occupancy at different times.

[0033] To address obstacle avoidance issues in dynamic environments, this embodiment demonstrates the process of constructing a dynamic obstacle spatiotemporal map. Figure 3 Two typical dynamic obstacles are set up: longitudinally moving obstacles, such as... Figure 3 As shown in the red circular area, the obstacle is moving along the negative Y-axis. Its position is clearly marked in the diagram. =0s、 =5s and The predicted positions at discrete times such as =10s show that it gradually approaches the central region from above.

[0034] Laterally move obstacles, such as Figure 3 As shown in the blue circular area, the obstacle is moving along the positive X-axis. Its predicted positions at t=0s, t=5s, and t=10s are also marked in the figure, showing it traversing the map from left to right.

[0035] Overlay of static and dynamic elements: In the figure, gray rectangles represent static obstacles. By mapping the predicted trajectories of dynamic obstacles onto discrete time layers and marking the grid areas they occupy at corresponding times, the algorithm successfully constructs a grid map containing spatiotemporal occupancy information.

[0036] This allows the path planner to know precisely which grid cells will be occupied by dynamic obstacles at any future time (e.g., t=5s), thus providing accurate spatiotemporal constraint data for subsequent calculations of the "safe zone".

[0037] S3. Calculate the risk-constrained safe intervals.

[0038] This is the core preprocessing step of the SIPP algorithm. This invention introduces risk constraints here, and the calculation process for the safe interval of the risk constraints is as follows: For each grid c Its safe range list:

[0039] This indicates that the grid is collision-free and the risk is manageable during these time periods.

[0040] During preprocessing: First, perform hard collision detection. Based on the dynamic obstacle spatiotemporal map of S2, remove the time periods occupied by dynamic obstacles and determine the initial safe time period set that is not occupied by dynamic obstacles.

[0041] Then perform soft risk filtering and check the grid. static risk value : like Then determine the grid. As a soft obstacle, This indicates a high-risk threshold, prohibiting passage during specific high-risk periods, thus further segmenting or reducing the safe zone. Specifically, all time periods of the grid are removed from its initial set of safe time periods, or its safe time periods are reduced according to preset rules, thereby generating the final list of risk-constrained safe zones.

[0042] S4. Perform path search for the combined risk-time cost of RTR-SIPP (Risk-Time ReweightedSIPP), specifically as follows: like Figure 4 As shown, to verify the comprehensive performance of the algorithm of this invention in complex environments, a comparative simulation experiment was conducted on a grid map containing multiple static obstacles (black tiles in the figure). The experiment tested the traditional A algorithm, the RRT algorithm, and the RTR-SIPP method proposed in this invention, starting from [starting point]. Figure 4 The green dot in the bottom left corner is the target point. Figure 4 The red dot in the upper right corner clearly shows the path trajectory in the image: 1. Traditional The algorithm (solid green line in the diagram), while able to plan the theoretical shortest path, exhibits a typical "wall-hugging" characteristic, moving close to the edges of obstacles. In a real physical environment, this path, lacking safety margins, greatly increases the risk of the robot colliding with obstacles.

[0043] 2. The algorithm (pink dotted line in the figure) can avoid obstacles, but due to its random sampling nature, the generated path is extremely irregular, containing a lot of unnecessary twists and turns and jitters. The path consistency is poor, which is not conducive to the smooth tracking control of the robot.

[0044] 3. The RTR-SIPP method of this invention (orange solid line in the figure) has the best overall performance. This is due to the introduction of the truncated reciprocal risk field. The path can automatically identify the "safe corridor" between obstacles. While maintaining a moderate safe distance from obstacles, i.e. driving "in the center", it generates a smooth, continuous and intuitive preferred trajectory, effectively balancing the safety of navigation and traffic efficiency.

[0045] The state during the search is defined as follows: ; in, For raster coordinates, This is the index of the safe interval corresponding to this raster.

[0046] When expanding from the initial state, calculate from the current state. s Reaching its neighbor status n Cost function: ; in, For time increments, β For risk weighting coefficients, This represents the risk value for the next state.

[0047] Path searching specifically includes the following steps: S41. Initialize the priority queue OpenList and add it to the starting point.

[0048] S42. Iteratively extract the minimum cost node. .

[0049] S43, Extended Neighbor Status Calculate from arrive earliest feasible arrival time The time must fall within the neighbor state. n Within a certain safe range.

[0050] S44. Calculate the overall cost: ; Update the parent state and add it to the OpenList.

[0051] S45 Larger heuristic weights w The weight of the heuristic function is set to 1 to quickly obtain a feasible solution. Then, the weight of the heuristic function is gradually reduced to 1 to optimize the path quality and approach the optimal solution.

[0052] S5. Perform geometric and dynamic smoothing (Spline-Based Smoothing) on ​​the searched discrete spatiotemporal path, that is, transform the grid path into a smooth trajectory suitable for robot tracking, specifically: S51. Use the RDP (Ramer-Douglas-Peucker) algorithm to downsample the original path and extract key turning points.

[0053] S52, Spline Fitting: Use B-splines to generate continuous curves.

[0054] S53. Dynamic Collision Detection: Perform dense sampling on the generated curve and check each sampling point. At its corresponding time t Whether there is a collision with a dynamic obstacle. If a collision is detected, revert to the previous level of smoothing parameters or retain the original polyline segment.

[0055] like Figure 5 The diagram shows a comparison of the results before and after geometric and dynamic smoothing of the discrete spatiotemporal path searched in step S4. The gray dashed line in the figure represents the original grid path output by step S4. It can be seen that although the path avoids obstacles, due to the discrete nature of the grid map, the overall path presents as a series of broken lines connecting the grid centers, including abrupt right-angle turns. This jagged path does not meet the continuity requirements of robot kinematics.

[0056] This invention extracts key turning points using the RDP algorithm and combines it with B-spline fitting technology. The algorithm successfully eliminates the jagged edges and sharp corners of the original path, generating a smooth curve with continuous curvature. Figure 5 The solid green line represents the final smoothed trajectory (Spline Smoothing) after step S5.

[0057] Specifically, the "gap" indicated by the red arrow in the figure visually verifies the effectiveness of the dynamic collision detection mechanism in this invention. When smoothing the path corners, the algorithm does not blindly optimize, but rather samples points on the curve. Real-time detection was performed. Results show that the generated smooth trajectory, while optimizing the geometry, strictly maintains a safe distance from obstacles (gray circular areas in the image), avoiding the risk of cutting into obstacles due to excessive smoothing, thus ensuring the feasibility and safety of the final trajectory in dynamic environments. The smoothed trajectory eliminates polygonal corners and strictly satisfies the collision-free constraints in dynamic environments.

[0058] In summary, this invention constructs a risk field based on a truncated reciprocal model by utilizing Euclidean distance transformation to calculate environmental risk and employing a truncation mechanism to limit the calculation range, thereby optimizing computational efficiency. When generating the spatiotemporal safe interval for risk constraints, environmental risk values ​​are used as filtering conditions to preprocess the SIPP safe interval, eliminating high-risk periods.

[0059] When performing a risk-time combined cost SIPP search, the cost function J It includes both time consumption and risk accumulation items, and is weighted by coefficients. β To balance the two, a time-stamp-based dynamic collision detection step is added after spline fitting when implementing trajectory smoothing and verification mechanisms under dynamic constraints, ensuring the feasibility of smoothed trajectories in dynamic environments.

[0060] Therefore, this invention employs a risk-field-weighted spatiotemporal safe path planning method, which can efficiently plan a time-efficient, low-risk, smooth, and trackable trajectory while ensuring robot movement safety. This significantly improves the autonomous navigation performance and reliability of mobile robots in narrow, crowded, and dynamically changing scenarios.

[0061] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A risk field reweighting based spatiotemporal safe path planning method, characterized in that, The method comprises the following steps: S1, constructing a truncated reciprocal risk field; S2, constructing a dynamic obstacle space-time graph; S3, calculating a risk-constrained safety interval; S4, performing RTR-SIPP path search; S5, performing geometric and dynamic smoothing on the searched discrete space-time path.

2. The risk field reweighting based spatiotemporal safe path planning method according to claim 1, wherein, In S1, the process of constructing the truncated reciprocal risk field is specifically as follows: An input binary grid map G, the Euclidean distance transform EDT is computed for each free grid cell on the map G x to the nearest obstacle ; Using a truncated inverse model to calculate risk values : ； wherein, represents a grid x of risk values, Rcut is a cut-off radius, i.e. a safety impact range, is a risk intensity coefficient.

3. The risk field reweighting based spatiotemporal safe path planning method according to claim 1, wherein, In S2, the process of constructing the dynamic obstacle space-time graph is specifically as follows: Input: dynamic obstacle list containing current position, velocity and predicted trajectory of dynamic obstacles O i Divide future time into discrete time layers ; At each time level, based on dynamic obstacles At any moment Predicted location and shape radius Mark dynamic obstacles on the corresponding time-layer raster map. The spatiotemporal occupancy information of the occupied area, forming dynamic obstacles.

4. The risk field reweighting based spatiotemporal safe path planning method according to claim 3, wherein, In S3, the calculation process of the risk-constrained safety interval is as follows: For each grid c whose list of safe intervals is: indicates that the grid is collision-free and risk-manageable for the time period; First, hard collision detection is performed, and according to the dynamic obstacle space-time graph of S2, the time period occupied by the dynamic obstacle is removed to determine the initial safety time period set not occupied by the dynamic obstacle; Soft risk filtering is then performed, checking the static risk value of the grid :​ If , then determine that the grid is a soft obstacle, , which represents a high-risk threshold, all time periods of the grid c are removed from its initial set of safe time periods, or its safe time periods are reduced according to a preset rule, to obtain a final list of risk-constrained safe intervals.

5. The risk field reweighting based spatiotemporal safe path planning method according to claim 4, wherein, S4 is specifically as follows: The state in the search is defined as: ； wherein, is a grid coordinate, is a safety interval index corresponding to the grid. When expanding from a start state, compute the cost function from the current state s to its neighbor states ​ ； wherein, is a time increment, β is a risk weight coefficient, is a risk value for the next state.

6. The risk field reweighting based spatiotemporal safe path planning method according to claim 5, wherein, S4 specifically comprises the following steps: S41, initializing a priority queue OpenList and adding a starting point; S42, cycle to extract the minimum cost node ; S43, extend neighbor state n , compute earliest feasible arrival time s from n to , which must fall within some safety interval of neighbor state n ; S44, calculating a comprehensive cost: ； updating a parent state and adding the parent state to the OpenList; S45、 w >1 to quickly obtain a feasible solution, and then gradually reduce the weight of the heuristic function to 1 to optimize the path quality and approach the optimal solution.

7. The risk field reweighting based spatiotemporal safe path planning method according to claim 6, wherein, S5 is specifically as follows: S51, using an RDP algorithm to down-sample the original path and extracting key turning points; S52, using a B-spline to generate a continuous curve; S53, densely sample the generated curve, check each sampling point at its corresponding time t whether it collides with dynamic obstacles, if collision is detected, fall back to the previous smoothing parameter or keep the original polyline segment.

Images