A dynamic vine map-based safety-constrained path planning method and device

Abstract

This invention provides a method and apparatus for safety-constrained path planning based on dynamic Vinographs, which can solve the problems of large computational load in dynamic environments, insufficient safety distance constraints during path search, and the possibility of smoothed paths approaching obstacles. The method involves generating an initial Euclidean distance map and extracting an initial Vinograph skeleton based on a static grid map; updating the occupied grid map based on a ray model, and performing local incremental updates on the Euclidean distance map and the Vinograph skeleton; performing an A-star search within the constraint space of the dynamic Vinograph skeleton under safety distance constraints to obtain the original paths distributed along the dynamic Vinograph skeleton; supplementing path points on the original paths; extracting the contours of the safety regions and generating safe candidate points that satisfy the safety distance interval constraints within the effective safety regions; constructing a path search graph and searching for optimized path point sequences that satisfy the safety distance constraints; and performing offset corrections until a smooth path satisfying the safety distance constraints is obtained.

Description

Technical Field

[0001] This invention relates to the field of robot path planning, specifically a method and apparatus for safety constraint path planning based on dynamic Venn diagrams. Background Technology

[0002] With the rapid development of robotics and artificial intelligence, mobile robots are increasingly being used in warehousing and logistics, industrial inspection, and complex environments. Global path planning, as one of the key technologies for mobile robots to achieve autonomous navigation, aims to plan a traversable path from the starting position to the target position on a known map, while meeting the requirements of robot navigation safety and operational efficiency.

[0003] Existing global path planning techniques mainly include grid map-based search methods, sampling-based planning methods, and topology-based path planning methods. Among these, grid map-based search methods are prone to producing paths that are too close to obstacles in complex environments, posing a significant collision risk during navigation. Sampling-based planning methods have slow convergence speeds and unstable path quality in large-scale complex environments. Topology-based planning methods can structurally guide paths away from obstacles, but they typically rely on pre-built static maps. When obstacle positions change in the environment, map construction and topology extraction processes need to be re-executed, resulting in high computational costs and difficulty meeting the real-time requirements of dynamic environments.

[0004] Among existing mobile robot path planning methods, patent CN115657655A discloses a high-safety-margin global path planning method based on optimized Veno diagrams. This method generates an initial Veno road network based on a grid map, constructs a sparse environment topology map by identifying adjacent nodes, and performs multi-layer Bézier curve smoothing. However, this method does not effectively constrain the actual passable area corresponding to the robot's body size during path planning, and multiple Bézier curve smoothings can cause some path segments to approach the edge of obstacles, resulting in a reduced safety margin.

[0005] Patent CN120846311A discloses a method and system for extracting the skeleton of an environmental map. The method first shrinks the accessible area of ​​the environmental map based on the minimum passable width of the mobile robot and extracts the corresponding accessible area boundary contours. Then, it approximates the boundary contours with polygons and decomposes the accessible area boundary contours. Subsequently, it constructs a generalized Vinograph and extracts the environmental boundaries based on the generalized Vinograph, thereby generating the skeleton of the environmental map. Based on this skeleton, Dijkstra's algorithm can be used to search for the shortest path to obtain the planned path from the starting position to the target position. However, this method relies on the static processing results of the grid map, and the constructed environmental skeleton structure is difficult to update in real time according to environmental changes after construction. When the environment changes, the global calculation processes such as accessible area shrinkage, contour extraction, and Vinograph construction need to be re-executed, resulting in high computational costs and failing to meet the real-time requirements of robot path planning in dynamic scenarios. Summary of the Invention

[0006] To address the aforementioned problems, this invention provides a method and apparatus for safe constraint path planning based on dynamic Venographs, which can solve the problems of large computational load for updating in dynamic environments, insufficient safe distance constraints during path search, and the possibility of the path getting too close to obstacles after curve smoothing in existing Venograph path planning methods.

[0007] The technical solution is as follows: a safety-constrained path planning method based on dynamic Venn diagrams, characterized by comprising: Step S1: Based on the pre-built static raster map, generate an initial Euclidean distance map and extract the initial Vino skeleton; Step S2: Acquire two-dimensional LiDAR point cloud data during robot movement, update the occupied grid map based on the ray model, and perform local incremental updates on the Euclidean distance map and the Vino skeleton according to the grid state change area to obtain the dynamic Euclidean distance map and the dynamic Vino skeleton. Step S3: Connect the starting position and the ending position to the skeleton points in the dynamic Vino skeleton respectively, and perform A-star search in the constraint space of the dynamic Vino skeleton under the safety distance constraint to obtain the original path distributed along the dynamic Vino skeleton. Step S4: Perform path point supplementation processing on the original path to ensure that the distance between adjacent path points is not greater than a preset path point spacing threshold. Step S5: Extract the outline of the safe area based on the dynamic Euclidean distance map, determine the effective safe area related to the original path, generate safe candidate points that satisfy the safe distance interval constraints within the effective safe area, and form a set of safe candidate points; Step S6: Using the path points obtained in Step S4 and the safe candidate points obtained in Step S5 as graph nodes, construct a path search graph, and use Dijkstra's algorithm to search the path search graph to obtain an optimized path point sequence that satisfies the safe distance constraint; Step S7: Construct a third-order Bézier curve based on the optimized path point sequence, and perform safety distance detection on the sampling points of the third-order Bézier curve according to the dynamic Euclidean distance map; when there are path sampling points that do not meet the safety distance constraints, construct a normal vector based on the curve tangent vector, and offset and correct the corresponding Bézier curve control points along the normal vector direction until a smooth path that meets the safety distance constraints is obtained.

[0008] Furthermore, step S1 specifically includes: A two-dimensional grid map is constructed using two-dimensional laser SLAM or three-dimensional laser SLAM; the two-dimensional grid map is then processed into a binary grid map according to a preset probability threshold, wherein grids without obstacles are in an idle state, and grids with obstacles are in an occupied state. Based on the binary grid map, the Euclidean distance from each grid cell to the nearest obstacle is calculated to form an initial Euclidean distance map. The initial Vino skeleton is then extracted based on the source relationship of the nearest obstacles corresponding to adjacent grid cells.

[0009] Furthermore, in step S2, updating the occupied grid map based on the ray model includes: S21: Denoise and downsample the original laser point cloud data, and transform the laser point cloud to the robot coordinate system according to the robot's current pose; S22: Traverse the grids traversed by the ray from the robot's position to the laser endpoint, construct an updated grid set and record the occupancy status before the update, and update the occupancy probability of the grids in the updated grid set; S23: Determine the state of the grids in the updated grid set according to the preset occupancy threshold, identify the grids that change from idle state to occupied state as newly added obstacle grids, establish a set of newly added obstacle grids, identify the grids that change from occupied state to idle state as removed obstacle grids, and establish a set of removed obstacle grids. S24: Initialize the distance value of the newly added grid of the obstacle to 0 and add it to the update queue as a new distance propagation source; set the distance value corresponding to the removed grid of the obstacle to an invalid value and add it to the update queue after marking it as a state to be propagated. S25: Traverse the grids in the update queue and propagate the nearest obstacle source and Euclidean distance value to the neighboring grids; S26: When the nearest obstacle sources corresponding to adjacent grids are different and the preset stability judgment conditions are met, the corresponding grid is marked as a candidate Vinio grid. When the update queue is empty, the current round of propagation ends and the local incremental update of the Euclidean distance map and the Vinio map is completed. S27: Based on the preset neighborhood connectivity rules, candidate Wino grids are filtered to remove redundant branches and grids that do not meet the topological constraints, resulting in a simplified dynamic Wino skeleton.

[0010] Furthermore, in S22, the graticules in the updated graticule set are updated using log probability, specifically as follows: the log probability ratio of graticule c at time t satisfies: ; in For grid The probability of being occupied. For grid The logarithmic probability ratio at time t; For each grid cell in the updated grid set, its updated probability value is: ; in This represents the log-probability ratio of the grid at the previous time step. It can be represented as: ; in, The preset occupation update value, Update the preset idle occupancy value.

[0011] Furthermore, in S25, the propagation of the nearest obstacle source and Euclidean distance value to the neighboring grid is specifically performed as follows: For each grid cell's neighboring grid cells, calculate the Euclidean distance from the neighboring grid cell to the nearest obstacle source corresponding to the current grid cell. When the Euclidean distance is less than the original distance value of the corresponding neighboring grid cell or the nearest obstacle of the corresponding neighboring grid cell is invalid, update the nearest obstacle source and Euclidean distance value of the corresponding neighboring grid cell, and add the corresponding neighboring grid cell to the update queue.

[0012] Furthermore, step S3 specifically includes: S31: Determine whether the starting position and the ending position are located on the Vino skeleton. If the starting position or the ending position is not located on the Vino skeleton, search for the connection path from the starting position and the ending position to the nearest Vino skeleton node respectively under the condition of satisfying the safety distance constraint. S32: Within the dynamic Vino skeleton, the A* algorithm is used for path search based on the total cost function. A safety distance and a distance penalty function are introduced into the total cost function of the path search, where the safety distance is defined as: ; in, For the robot radius, For map resolution, The number of grid cells to expand; The safety threshold is defined based on the safety distance and is expressed as follows: ; in, As a safety threshold, For adjustment amount; The distance penalty function is defined based on a safety threshold and is expressed as follows: ; in, Distance expressed in terms of grid cells; The total cost function of path search is expressed as: ; in, The cost is the path length of the movement. For heuristic functions, For grid points Euclidean distance to the nearest obstacle For adjustment amount; S33: Combine the starting point to skeleton connection path, the search path inside the skeleton, and the skeleton to end point connection path to form the original path.

[0013] Furthermore, step S4 specifically includes: adding the start point and the end point to the original path; detecting the Euclidean distance between adjacent path points; when the Euclidean distance between adjacent path points is greater than a preset path point spacing threshold, inserting a new path point at the midpoint of the corresponding path segment, until the Euclidean distance between adjacent path points is not greater than the preset path point spacing threshold.

[0014] Furthermore, step S5 specifically includes: S51: Perform contour detection on the dynamic Euclidean distance map to form multiple safe region contours. Use the Douglas-Peucker algorithm to perform polygon approximation on the multiple safe region contours to obtain safe region polygons and form a set of safe region polygons. S52: Select a safe region polygon containing the path points obtained in step S4 from the set of safe region polygons as an effective safe region polygon; select grid points in the dynamic Euclidean distance graph that simultaneously satisfy the conditions of being located inside the effective safe region polygon and satisfying the safe distance interval constraint as safe candidate points: wherein the safe distance interval constraint is: ; in, For grid points Euclidean distance to the nearest obstacle To preset the minimum safe distance threshold, This is the preset maximum safe distance threshold.

[0015] Furthermore, step S6, which involves constructing the path search, specifically includes: S61: Use the path points obtained in step S4 and the safe candidate points obtained in step S5 as vertices of the path search graph; for any two graph nodes Candidate connection edges are constructed when the spacing between graph nodes meets a preset distance threshold constraint, as shown below: ; in For the preset distance threshold, The node spacing in a graph is determined by the Euclidean distance between nodes. It is represented and defined as: ; in For the vertices of the graph coordinates For the vertices of the graph The coordinates; S62: Perform straight-line reachability detection on the candidate connecting edges, construct straight-line segments through interpolation, and calculate the distance values ​​from each sampling point on the straight-line segment to the nearest obstacle. The distance values ​​of all sampling points must satisfy the following: ; in The first line segment on the line segment One sampling point, for Distance to the nearest obstacle To establish a preset safe distance; S63: Based on the constructed graph structure, Dijkstra's algorithm is used to search for the shortest path from the starting point to the ending point, and the resulting node sequence is used as the optimized path point sequence.

[0016] Furthermore, step S7 specifically includes: S71: Perform fixed-step resampling on the obtained optimized path point sequence to obtain a path point sequence with uniform point spacing. ,in ; S72: For adjacent path points and The expression for a third-order Bézier curve is as follows: ; Control points and Defined as: ; ; in These are preset parameters used to control the smoothness of the path. and respectively path points and waypoints The tangent vector is represented as: ; ; in path point The next path point; S73: Discretely sample each segment of the Bézier curve to obtain smoothed path sampling points. The condition is that the distance from all path sampling points to the nearest obstacle is greater than the safe distance. When the current smooth path satisfies the safety constraints, output the corresponding path. S74: When a situation that does not meet the safety constraints is detected, determine the Bézier curve segment where the sampling point is located, construct the corresponding normal vector based on the tangent vector direction of the Bézier curve, and adjust the control point corresponding to the Bézier curve segment along the normal vector direction. S75: Determine the optimal adjustment direction by comparing the change in distance from the control point to the nearest obstacle before and after adjustment, offset the normal vector direction of the control point and verify the safe distance, and when the safety constraint is met, re-smooth the third-order Bézier curve based on the control point after offset correction.

[0017] This invention updates the occupied grid map based on real-time laser point cloud information, and only incrementally updates the Euclidean distance map and Vino skeleton in local areas where the grid state changes, avoiding global recalculation of the Vino map and distance field. This can significantly reduce the computational complexity in dynamic environments and improve the real-time performance of path planning, while ensuring the accuracy of global path planning. This invention, based on the original path generated by the Vino skeleton, extracts the outline of the safe area using a Euclidean distance map and samples it to generate safe candidate points, constructs a path search graph containing the original path points and safe candidate points, and then performs secondary optimization of the path using the Dijkstra algorithm, so that the final path is distributed along the Vino skeleton in the topological structure while further moving away from obstacles, thereby improving the safety margin of the path.

[0018] This invention, in the path smoothing process, incorporates a safety distance by calculating the tangent vector of the Bézier curve and constructing the corresponding normal vector. It then performs directional offset correction on control points in sections that do not meet safety constraints until the preset safety distance requirement is met. This reduces the risk of the path locally approaching obstacles, which is common with traditional curve smoothing, while simultaneously improving path continuity and smoothness. Attached Figure Description

[0019] Figure 1 This is a schematic diagram illustrating the steps of the safety constraint path planning method based on dynamic Venn diagrams provided in an embodiment of the present invention. Figure 2 This is a schematic diagram of a raster map generated based on the SLAM algorithm in an embodiment of the present invention; Figure 3 This is a schematic diagram of the Wino skeleton generated based on a grid map in an embodiment of the present invention; Figure 4 This is a schematic diagram of the original path result generated based on the Wino skeleton in an embodiment of the present invention; Figure 5 This is a schematic diagram of safe candidate points generated based on Euclidean distance graphs and safe path results generated based on Dijkstra's algorithm in an embodiment of the present invention. Figure 6 This is a schematic diagram of the final path result after smoothing and dynamic adjustment based on a third-order Bézier curve in an embodiment of the present invention; Figure 7 This is an internal structural diagram of a computer device in one embodiment. Detailed Implementation

[0020] See Figure 1 The present invention provides a safety-constrained path planning method based on dynamic Venn diagrams, comprising: Step S1: Based on the pre-built static raster map, generate an initial Euclidean distance map and extract the initial Vino skeleton; Step S2: Acquire two-dimensional LiDAR point cloud data during robot movement, update the occupied grid map based on the ray model, and perform local incremental updates on the Euclidean distance map and Vino skeleton according to the grid state change area to obtain the dynamic Euclidean distance map and dynamic Vino skeleton. Step S3: Connect the starting position and the ending position to the skeleton points in the dynamic Vino skeleton respectively. Under the safety distance constraint, perform A* search in the constraint space of the dynamic Vino skeleton to obtain the original path distributed along the dynamic Vino skeleton. Step S4: Perform path point supplementation processing on the original path to ensure that the distance between adjacent path points is not greater than the preset path point spacing threshold. Step S5: Extract the outline of the safe area based on the dynamic Euclidean distance map, determine the effective safe area related to the original path, generate safe candidate points that satisfy the safe distance interval constraints within the effective safe area, and form a set of safe candidate points; Step S6: Using the path points obtained in Step S4 and the safe candidate points obtained in Step S5 as graph nodes, construct a path search graph, and use Dijkstra's algorithm to search for an optimized path point sequence that satisfies the safe distance constraint in the path search graph. Step S7: Construct a third-order Bézier curve based on the optimized path point sequence, and perform safety distance detection on the sampling points of the third-order Bézier curve according to the dynamic Euclidean distance map; when there are path sampling points that do not meet the safety distance constraints, construct a normal vector based on the curve tangent vector, and offset and correct the corresponding Bézier curve control points along the normal vector direction until a smooth path that meets the safety distance constraints is obtained.

[0021] In one embodiment of the present invention, in step S1, a two-dimensional grid map is constructed using two-dimensional laser SLAM or three-dimensional laser SLAM, such as... Figure 2 As shown; The two-dimensional grid map is processed into a binary grid map according to a preset probability threshold. The grid without obstacles is in an idle state, and the grid with obstacles is in an occupied state, which represents the obstacle distribution information of the current environment. Based on the binarized raster map, the Euclidean distance from each raster to the nearest obstacle is calculated to form an initial Euclidean distance map. An initial Vino skeleton is then extracted based on the source relationships of the nearest obstacles corresponding to adjacent raster cells. Figure 3 As shown. In the Euclidean distance map, each grid cell stores its distance information to the nearest obstacle; the Vino skeleton is a set of skeleton grid cells that are equidistant from the boundaries of at least two obstacles, used to represent the topological center structure of the passable area.

[0022] In step S2 of the embodiment, two-dimensional LiDAR point cloud data is acquired in real time during the robot's movement, and the occupied grid map is updated using a ray model. The updating of the occupied grid map based on the ray model includes: S21: Denoise and downsample the original laser point cloud data, and transform the laser point cloud to the robot coordinate system according to the robot's current pose; S22: Traverse the grids traversed by the ray from the robot's position to the laser endpoint, construct an updated grid set and record the occupancy status before the update, and update the occupancy probability of the grids in the updated grid set. Specifically, the logarithmic probability update of the grids in the updated grid set is performed as follows: The logarithmic probability ratio of grid c at time t satisfies: ; in For grid The probability of being occupied. For grid The logarithmic probability ratio at time t; For each grid cell in the updated grid set, its updated probability value is: ; in This represents the log-probability ratio of the grid at the previous time step. It can be represented as: ; in, The preset occupation update value, Update the value based on the preset idle occupancy. It is usually taken as a positive value to increase the probability of the ray endpoint grid being occupied; It is usually taken as a negative value to reduce the probability of the ray occupies the grid. S23: Based on the preset occupancy threshold, determine the status of the grids in the updated grid set, identify the grids that change from idle state to occupied state as newly added obstacle grids, establish a set of newly added obstacle grids, identify the grids that change from occupied state to idle state as removed obstacle grids, and establish a set of removed obstacle grids. S24: Initialize the distance value of the newly added grid of the obstacle to 0 and add it to the update queue as a new distance propagation source; set the distance value corresponding to the removed grid of the obstacle to an invalid value and add it to the update queue after marking it as a state to be propagated. S25: Traverse the grid in the update queue and propagate the nearest obstacle source and Euclidean distance value to neighboring grids. The specific execution of propagating the nearest obstacle source and Euclidean distance value to neighboring grids is as follows: For each grid cell's neighboring grid cells, calculate the Euclidean distance from the neighboring grid cell to the nearest obstacle source corresponding to the current grid cell. When the Euclidean distance is less than the original distance value of the corresponding neighboring grid cell or the nearest obstacle of the corresponding neighboring grid cell is invalid, update the nearest obstacle source and Euclidean distance value of the corresponding neighboring grid cell, and add the corresponding neighboring grid cell to the update queue. S26: When the nearest obstacle sources corresponding to adjacent grids are different and the preset stability determination conditions are met, the corresponding grid is marked as a candidate Vino grid. When the update queue is empty, the current round of propagation ends and the local incremental update of the Euclidean distance map and the Vino map is completed. In the embodiment, the stability determination conditions include that the distance between the two nearest obstacle sources is greater than a map resolution and the distance difference between the current grid and the two obstacle sources is less than a preset difference. S27: Based on the preset neighborhood connectivity rules, candidate Vino rasters are screened to remove redundant branches and rasters that do not meet the topological constraints, resulting in a simplified dynamic Vino skeleton. The neighborhood connectivity rules can adopt the 8-neighborhood connectivity rule, retaining rasters that maintain the skeleton connectivity and deleting short end branches that do not affect connectivity.

[0023] Through the above steps S21 to S27, this embodiment only performs the propagation and updating of the Euclidean distance map and the Vinio map in the local area where the grid state changes, without having to recalculate the entire map globally. This can significantly reduce computational overhead and improve real-time performance in dynamic environments.

[0024] In step S3 of the embodiment, based on the dynamic Euclidean distance map and dynamic Vino skeleton obtained in step S2, the starting position and the ending position are connected to the skeleton points in the dynamic Vino skeleton, respectively. The A-star search algorithm constrained by the safe distance is used to search for an original path that connects the starting point and the ending point and is distributed along the Vino skeleton in the constrained space of the Vino skeleton. Figure 4 The image shows the original path result generated in this embodiment.

[0025] In this embodiment, step S3 is specifically performed as follows: S31: Determine whether the starting position and the ending position are located on the Wino skeleton. If the starting position or the ending position is not located on the Wino skeleton, search for the connection path from the starting position and the ending position to the nearest Wino skeleton node respectively, under the condition of satisfying the safety distance constraint. S32: Within the dynamic Vino skeleton, the A* algorithm is used for path search based on the total cost function. The safe distance and distance penalty function are introduced into the total cost function of the path search, where the safe distance is defined as: ; in, For the robot radius, For map resolution, The number of grid cells to expand; The safety threshold is defined based on the safety distance and is expressed as follows: ; in, As a safety threshold, For adjustment amount; The distance penalty function is defined based on a safety threshold and is expressed as follows: ; in, Distance expressed in terms of grid cells; The total cost function of path search is expressed as: ; in, The cost is the path length of the movement. For heuristic functions, For grid points Euclidean distance to the nearest obstacle To adjust the cost, a distance penalty term is introduced into the total cost function, which significantly increases the cost when a node is close to an obstacle, thus guiding the A-star search process to favor skeleton paths that are far from obstacles. S33: Combine the starting point to skeleton connection path, the search path inside the skeleton, and the skeleton to end point connection path to form the original path.

[0026] In step S4 of this embodiment, the original path undergoes path point supplementation processing to ensure that the distance between adjacent path points does not exceed a preset path point spacing threshold: the Euclidean distance between adjacent path points is detected; when the Euclidean distance between adjacent path points exceeds the preset path point spacing threshold, a new path point is inserted at the midpoint of the corresponding path segment until the Euclidean distance between adjacent path points does not exceed the preset path point spacing threshold. In this embodiment, the preset distance threshold can be set according to the robot size and environmental complexity.

[0027] In step S5 of the embodiment, the outline of the safe region is extracted based on the dynamic Euclidean distance map, the effective safe region related to the original path is determined, and safe candidate points that satisfy the safe distance interval constraints are generated within the effective safe region to form a set of safe candidate points; such as Figure 5 The image shows a schematic of the safe candidate points generated in this embodiment. The specific implementation steps in this embodiment are as follows: S51: Perform contour detection on the dynamic Euclidean distance map to form multiple safe region contours. Use the Douglas-Peucker algorithm to perform polygon approximation on the multiple safe region contours to obtain safe region polygons and form a set of safe region polygons. S52: Select the safe region polygon containing the path points obtained in step S4 from the safe region polygon set as the effective safe region polygon; select grid points in the dynamic Euclidean distance map that simultaneously satisfy the conditions of being located inside the effective safe region polygon and satisfying the safe distance interval constraint as safe candidate points: where the safe distance interval constraint is: ; in, For grid points Euclidean distance to the nearest obstacle To preset the minimum safe distance threshold, This is the preset maximum safe distance threshold.

[0028] In this embodiment, the preset minimum safe distance threshold in step S52 is 0.01 meters, but it can be set to other values ​​in other instances. The preset maximum safe distance threshold in step S52 is 0.05 meters, but it can also be set to other values ​​in other instances.

[0029] In the implementation, in step S6, the path points obtained in step S4 and the safe candidate points obtained in step S5 are used as graph nodes to construct a path search graph. The Dijkstra algorithm is used to search for an optimized path point sequence that satisfies the safe distance constraint in the path search graph. Furthermore, step S6, which involves constructing the path search, specifically includes: S61: Use the path points obtained in step S4 and the safe candidate points obtained in step S5 as vertices of the path search graph; for any two graph nodes Candidate connection edges are constructed when the spacing between graph nodes meets a preset distance threshold constraint, as shown below: ; in For the preset distance threshold, The node spacing in a graph is determined by the Euclidean distance between nodes. It is represented and defined as: ; in For the vertices of the graph coordinates For the vertices of the graph The coordinates; S62: Perform straight-line reachability detection on candidate connecting edges, construct straight-line segments through interpolation, and calculate the distance values ​​from each sampling point on the straight-line segment to the nearest obstacle. The distance values ​​of all sampling points must satisfy the following: ; in The first line segment on the line segment One sampling point, for Distance to the nearest obstacle A preset safety distance is used; this allows for relaxed constraints during the candidate point sampling stage to ensure sufficient candidate nodes, while strict constraints during the path connection stage ensure a safe path interval.

[0030] S63: Based on the constructed graph structure, Dijkstra's algorithm is used to search for the shortest path from the starting point to the ending point, and the resulting node sequence is used as the optimized path point sequence. This process combines the relatively lenient candidate point selection in step S5 with the strict edge safety check in step S6, ensuring that the output path stays away from obstacles while guaranteeing sufficient search space.

[0031] In step S7 of the embodiment, a third-order Bézier curve is constructed based on the optimized path point sequence. The sampling points of the third-order Bézier curve are then subjected to safety distance detection based on a dynamic Euclidean distance map. When there are path sampling points that do not meet the safety distance constraints, a normal vector is constructed based on the curve tangent vector. The corresponding Bézier curve control points are offset and corrected along the normal vector direction until a smooth path that meets the safety distance constraints is obtained. Figure 6 The image shows the final smooth path generated in this embodiment. The specific implementation steps are as follows: S71: Perform fixed-step resampling on the obtained optimized path point sequence to obtain a path point sequence with uniform point spacing. ,in ; S72: For adjacent path points and The expression for a third-order Bézier curve is as follows: ; Control points and Defined as: ; ; in These are preset parameters used to control the smoothness of the path. and respectively path points and waypoints The tangent vector is represented as: ; ; in path point The next path point; S73: Discretely sample each segment of the Bézier curve to obtain smoothed path sampling points. The condition is that the distance from all path sampling points to the nearest obstacle is greater than the safe distance. When the current smooth path satisfies the safety constraints, output the corresponding path. S74: When a situation that does not meet the safety constraints is detected, determine the Bézier curve segment where the sampling point is located, construct the corresponding normal vector based on the tangent vector direction of the Bézier curve, and adjust the control point corresponding to the Bézier curve segment along the normal vector direction. S75: Determine the optimal adjustment direction by comparing the change in distance from the control point to the nearest obstacle before and after adjustment, offset the normal vector direction of the control point and verify the safe distance, and when the safety constraint is met, re-smooth the third-order Bézier curve based on the control point after offset correction.

[0032] Through the above steps S71 to S75, this embodiment ensures the overall smoothness of the path while using the geometric mechanism constructed by the tangent vector and the normal vector to perform targeted offset correction of the control points. This avoids the problem of some path segments getting close to the edge of obstacles due to improper selection of control points in the traditional Bézier curve smoothing process, and can obtain a final global path that combines smoothness and safety.

[0033] In an embodiment of the present invention, a computer device is also provided, including a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the safety constraint path planning method based on dynamic Venn diagrams as described above.

[0034] The computer device can be a terminal, and its internal structure diagram can be as follows: Figure 7 As shown, the computer device includes a processor, memory, network interface, display screen, and input devices connected via a bus. The processor provides computing and control capabilities. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs stored in the non-volatile storage medium. The network interface is used to communicate with external terminals via a network connection. When executed by the processor, the computer program implements a safety constraint path planning method based on dynamic Venn diagrams. The display screen can be an LCD screen or an e-ink screen. The input devices can be a touch layer covering the display screen, buttons, a trackball, or a touchpad mounted on the computer device casing, or an external keyboard, touchpad, or mouse.

[0035] Memory can be, but is not limited to, Random Access Memory (RAM), Read Only Memory (ROM), Programmable Read-Only Memory (PROM), Erasable Programmable Read-Only Memory (EPROM), and Electrically Erasable Programmable Read-Only Memory (EEPROM). Memory stores programs, and the processor executes these programs after receiving execution instructions.

[0036] A processor can be an integrated circuit chip with signal processing capabilities. The aforementioned processor can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc. The processor can also be other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable Gate Arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this application. A general-purpose processor can be a microprocessor or any conventional processor.

[0037] Those skilled in the art will understand that Figure 7 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. A specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0038] In an embodiment of the present invention, a computer-readable storage medium is also provided, on which a program is stored, which, when executed by a processor, implements the safety constraint path planning method based on dynamic Venn diagrams as described above.

[0039] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, computer devices, or computer program products. Therefore, embodiments of the present invention can take the form of entirely hardware embodiments, entirely software embodiments, or embodiments combining software and hardware aspects. Furthermore, embodiments of the present invention can take the form of computer program products implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0040] The embodiments of the present invention are described with reference to flowchart illustrations and / or block diagrams of methods, computer apparatuses, or computer program products according to embodiments of the invention. These computer program instructions may be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing terminal device to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing terminal device, create means for implementing the functions specified in the flowchart illustrations and / or block diagrams.

[0041] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing terminal device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means that implement the functions specified in the flowchart.

[0042] In an embodiment of the present invention, a computer program product is also provided, including a computer program / instructions that, when executed by a processor, implement the steps of the above-described method.

[0043] In practical applications, the aforementioned computer program products include, but are not limited to: unloading robots, drones, smartphones, desktop computers, laptops, tablets, host computers, and server platforms, etc., without specific limitations.

[0044] The above provides a detailed description of the application of the safety constraint path planning method, computer device, computer-readable storage medium, and computer program product based on dynamic Venn diagrams provided by this invention. Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A dynamic vineyard-based safety-constrained path planning method, characterized in that, include: Step S1: Based on the pre-built static raster map, generate an initial Euclidean distance map and extract the initial Vino skeleton; Step S2: Acquire two-dimensional LiDAR point cloud data during robot movement, update the occupied grid map based on the ray model, and perform local incremental updates on the Euclidean distance map and the Vino skeleton according to the grid state change area to obtain the dynamic Euclidean distance map and the dynamic Vino skeleton. Step S3: Connect the starting position and the ending position to the skeleton points in the dynamic Vino skeleton respectively, and perform A-star search in the constraint space of the dynamic Vino skeleton under the safety distance constraint to obtain the original path distributed along the dynamic Vino skeleton. Step S4: Perform path point supplementation processing on the original path to ensure that the distance between adjacent path points is not greater than a preset path point spacing threshold. Step S5: Extract the outline of the safe area based on the dynamic Euclidean distance map, determine the effective safe area related to the original path, generate safe candidate points that satisfy the safe distance interval constraints within the effective safe area, and form a set of safe candidate points; Step S6: Using the path points obtained in Step S4 and the safe candidate points obtained in Step S5 as graph nodes, construct a path search graph, and use Dijkstra's algorithm to search the path search graph to obtain an optimized path point sequence that satisfies the safe distance constraint; Step S7: Construct a third-order Bézier curve based on the optimized path point sequence, and perform safe distance detection on the sampling points of the third-order Bézier curve according to the dynamic Euclidean distance map; When there are path sampling points that do not meet the safety distance constraint, a normal vector is constructed based on the curve tangent vector, and the corresponding Bézier curve control points are offset and corrected along the direction of the normal vector until a smooth path that meets the safety distance constraint is obtained.

2. The method of claim 1, wherein, Step S1 specifically includes: A two-dimensional grid map is constructed using two-dimensional laser SLAM or three-dimensional laser SLAM; the two-dimensional grid map is then processed into a binary grid map according to a preset probability threshold, wherein grids without obstacles are in an idle state, and grids with obstacles are in an occupied state. Based on the binary grid map, the Euclidean distance from each grid cell to the nearest obstacle is calculated to form an initial Euclidean distance map. The initial Vino skeleton is then extracted based on the source relationship of the nearest obstacles corresponding to adjacent grid cells.

3. The method of claim 1, wherein, In step S2, the occupied grid map is updated based on the ray model, including: S21: Denoise and downsample the original laser point cloud data, and transform the laser point cloud to the robot coordinate system according to the robot's current pose; S22: Traverse the grids traversed by the ray from the robot's position to the laser endpoint, construct an updated grid set and record the occupancy status before the update, and update the occupancy probability of the grids in the updated grid set; S23: Determine the state of the grids in the updated grid set according to the preset occupancy threshold, identify the grids that change from idle state to occupied state as newly added obstacle grids, establish a set of newly added obstacle grids, identify the grids that change from occupied state to idle state as removed obstacle grids, and establish a set of removed obstacle grids. S24: Initialize the distance value of the newly added grid of the obstacle to 0 and add it to the update queue as a new distance propagation source; set the distance value corresponding to the removed grid of the obstacle to an invalid value and add it to the update queue after marking it as a state to be propagated. S25: Traverse the grids in the update queue and propagate the nearest obstacle source and Euclidean distance value to the neighboring grids; S26: When the nearest obstacle sources corresponding to adjacent grids are different and the preset stability judgment conditions are met, the corresponding grid is marked as a candidate Vinio grid. When the update queue is empty, the current round of propagation ends and the local incremental update of the Euclidean distance map and the Vinio map is completed. S27: Based on the preset neighborhood connectivity rules, candidate Wino grids are filtered to remove redundant branches and grids that do not meet the topological constraints, resulting in a simplified dynamic Wino skeleton.

4. The safety constraint path planning method based on dynamic Venn diagrams according to claim 3, characterized in that, In S22, the rasters in the updated raster set are updated using log probability, specifically as follows: the log probability ratio of raster c at time t satisfies: ； in For grid The probability of being occupied. For grid The logarithmic probability ratio at time t; For each grid cell in the updated grid set, its updated probability value is: ； in This represents the log-probability ratio of the grid at the previous time step. It can be represented as: ； in, The preset occupation update value, Update the preset idle occupancy value.

5. The safety constraint path planning method based on dynamic Venn diagrams according to claim 3, characterized in that, In S25, the propagation of the nearest obstacle source and Euclidean distance value to the neighboring grid is specifically performed as follows: For each grid cell's neighboring grid cells, calculate the Euclidean distance from the neighboring grid cell to the nearest obstacle source corresponding to the current grid cell. When the Euclidean distance is less than the original distance value of the corresponding neighboring grid cell or the nearest obstacle of the corresponding neighboring grid cell is invalid, update the nearest obstacle source and Euclidean distance value of the corresponding neighboring grid cell, and add the corresponding neighboring grid cell to the update queue.

6. The safety constraint path planning method based on dynamic Venn diagrams according to claim 1, characterized in that, Step S3 specifically includes: S31: Determine whether the starting position and the ending position are located on the Vino skeleton. If the starting position or the ending position is not located on the Vino skeleton, search for the connection path from the starting position and the ending position to the nearest Vino skeleton node respectively under the condition of satisfying the safety distance constraint. S32: Within the dynamic Vino skeleton, the A* algorithm is used for path search based on the total cost function. A safety distance and a distance penalty function are introduced into the total cost function of the path search, where the safety distance is defined as: ； in, For the robot radius, For map resolution, The number of grid cells to expand; The safety threshold is defined based on the safety distance and is expressed as follows: ； in, As a safety threshold, For adjustment amount; The distance penalty function is defined based on a safety threshold and is expressed as follows: ； in, Distance expressed in terms of grid cells; The total cost function of path search is expressed as: ； in, The cost is the path length of the movement. For heuristic functions, For grid points Euclidean distance to the nearest obstacle For adjustment amount; S33: Combine the starting point to skeleton connection path, the search path inside the skeleton, and the skeleton to end point connection path to form the original path.

7. The safety constraint path planning method based on dynamic Venn diagrams according to claim 1, characterized in that, Step S4 specifically includes: adding the start point and the end point to the original path; detecting the Euclidean distance between adjacent path points; when the Euclidean distance between adjacent path points is greater than a preset path point spacing threshold, inserting a new path point at the midpoint of the corresponding path segment, until the Euclidean distance between adjacent path points is not greater than the preset path point spacing threshold.

8. The safety constraint path planning method based on dynamic Venn diagrams according to claim 1, characterized in that, Step S5 specifically includes: S51: Perform contour detection on the dynamic Euclidean distance map to form multiple safe region contours. Use the Douglas-Peucker algorithm to perform polygon approximation on the multiple safe region contours to obtain safe region polygons and form a set of safe region polygons. S52: Select a safe region polygon containing the path points obtained in step S4 from the set of safe region polygons as an effective safe region polygon; select grid points in the dynamic Euclidean distance graph that simultaneously satisfy the conditions of being located inside the effective safe region polygon and satisfying the safe distance interval constraint as safe candidate points: wherein the safe distance interval constraint is: ； in, For grid points Euclidean distance to the nearest obstacle To preset the minimum safe distance threshold, This is the preset maximum safe distance threshold.

9. The safety constraint path planning method based on dynamic Venn diagrams according to claim 1, characterized in that, Step S6, which involves constructing the path search, specifically includes: S61: Use the path points obtained in step S4 and the safe candidate points obtained in step S5 as vertices of the path search graph; for any two graph nodes Candidate connection edges are constructed when the spacing between graph nodes meets a preset distance threshold constraint, as shown below: ； in For the preset distance threshold, The node spacing in a graph is determined by the Euclidean distance between nodes. It is represented and defined as: ； in For the vertices of the graph coordinates For the vertices of the graph The coordinates; S62: Perform straight-line reachability detection on the candidate connecting edges, construct straight-line segments through interpolation, and calculate the distance values ​​from each sampling point on the straight-line segment to the nearest obstacle. The distance values ​​of all sampling points must satisfy the following: ； in The first line segment on the line segment One sampling point, for Distance to the nearest obstacle To establish a preset safe distance; S63: Based on the constructed graph structure, Dijkstra's algorithm is used to search for the shortest path from the starting point to the ending point, and the resulting node sequence is used as the optimized path point sequence.

10. The safety-constrained path planning method based on dynamic Venn diagrams according to claim 1, characterized in that, Step S7 specifically includes: S71: Perform fixed-step resampling on the obtained optimized path point sequence to obtain a path point sequence with uniform point spacing. ,in ; S72: For adjacent path points and The expression for a third-order Bézier curve is as follows: ； Control points and Defined as: ； ； in These are preset parameters used to control the smoothness of the path. and respectively path points and waypoints The tangent vector is represented as: ； ； in path point The next path point; S73: Discretely sample each segment of the Bézier curve to obtain smoothed path sampling points. The condition is that the distance from all path sampling points to the nearest obstacle is greater than the safe distance. When the current smooth path satisfies the safety constraints, output the corresponding path. S74: When a situation that does not meet the safety constraints is detected, determine the Bézier curve segment where the sampling point is located, construct the corresponding normal vector based on the tangent vector direction of the Bézier curve, and adjust the control point corresponding to the Bézier curve segment along the normal vector direction. S75: Determine the optimal adjustment direction by comparing the change in distance from the control point to the nearest obstacle before and after adjustment, offset the normal vector direction of the control point and verify the safe distance, and when the safety constraint is met, re-smooth the third-order Bézier curve based on the control point after offset correction.

11. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that: When the processor executes the computer program, it implements the safety constraint path planning method based on dynamic Venn diagrams as described in any one of claims 1 to 10.

12. A computer-readable storage medium having a program stored thereon, characterized in that: When the program is executed by the processor, it implements the safety constraint path planning method based on dynamic Venn diagrams as described in any one of claims 1 to 10.

13. A computer program product comprising a computer program / instructions, characterized in that, When the computer program / instructions are executed by the processor, they implement the steps of the method described in any one of claims 1 to 10.

Images