Path planning obstacle avoidance system of industrial robot

Abstract

The invention relates to the technical field of industrial robot path planning, and discloses a path planning obstacle avoidance system of an industrial robot. According to the system, a space grid model containing obstacles is constructed through an environment modeling module. The obstacle processing module identifies an obstacle profile and generates an affected area thereof. And the feasible space analysis module calculates and divides a passable area of the robot according to the result. The trajectory initialization module generates an initial reference trajectory in a designated area and calculates a quantized avoidance priority parameter for each waypoint. And the path optimization and output module identifies and adjusts high-risk key points in the trajectory according to the priority parameters, performs iterative optimization in a feasible space, and finally outputs a safe obstacle avoidance path. According to the technology, differentiated evaluation and priority-driven optimization are carried out on the risk of the path points, so that the safety and the overall efficiency of path planning in a complex environment are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of industrial robot path planning, in particular to a path planning obstacle avoidance system of an industrial robot. BACKGROUND

[0002] The existing industrial robot path planning obstacle avoidance technology usually generates a motion trajectory directly after completing environment modeling by using global or local path search algorithms. The conventional method regards path planning as a whole search or optimization problem, and applies uniform safety rules and optimization strategies to the whole trajectory or all path points in the trajectory. Whether it is a deterministic algorithm based on graph search or a probabilistic algorithm based on random sampling, the core logic is to find a continuous sequence from the starting point to the ending point that satisfies the collision-free constraint, and there is a lack of differentiated assessment of the immediate obstacle avoidance risk faced by each node in the sequence.

[0003] Such technical solutions have limitations. Since all points on the trajectory are treated indiscriminately, the system cannot effectively identify and distinguish the safety urgency of different position points in the path. This leads to the optimization process either making equal and possibly unnecessary adjustments to all points, causing redundant bending of the path in the safety area, or failing to effectively correct truly high-risk points due to insufficient optimization effort. At the same time, the conventional one-time planning combined with fixed rule post-smoothing processing has a static and one-way optimization process, lacking a dynamic and targeted repeated adjustment mechanism according to the real-time risk state of the path points. This makes it difficult for the generated final path to simultaneously consider high safety and overall optimality in complex scenarios.

[0004] The current path planning obstacle avoidance technology needs a method that can finely quantify the individual obstacle avoidance needs of path points and drive the trajectory for directional and iterative self-optimization accordingly, to solve the problems of extensive path point processing and rigid optimization process. SUMMARY

[0005] The purpose of the present application is to provide a path planning obstacle avoidance system for an industrial robot to solve the problems raised in the background art.

[0006] To achieve the above purpose, the present application provides a path planning obstacle avoidance system for an industrial robot, which comprises: An environment modeling module for processing environment perception data, constructing and maintaining a spatial grid model containing obstacle occupancy information and spatial grid cell state; An obstacle processing module for analyzing the spatial grid model, identifying obstacle entity contours, calculating obstacle circumscribed boundary ranges, and generating obstacle influence areas; A feasible space analysis module is configured to calculate a robot feasible space based on the obstacle influence area, and perform connectivity analysis on the robot feasible space to identify different passable areas; A trajectory initialization module is configured to plan an initial reference trajectory composed of a sequence of waypoints based on a task start point and a target point in a designated passable area, and calculate an avoidance priority parameter for each waypoint; A path optimization and output module is configured to adjust key points in the initial reference trajectory according to the avoidance priority parameter, and perform iterative optimization in the robot feasible space until a safety-requirement-satisfied obstacle avoidance planning path is generated.

[0007] Preferably, the obstacle entity contour is defined by a set of coordinate sets of boundary grid cells, the obstacle influence area is determined by an expansion distance parameter and an original contour, the robot feasible space is a set of grid cells that are not marked by the obstacle influence area, the passable area is a subset of the robot feasible space defined by adjacency relationship, and the avoidance priority parameter is a numerical indicator negatively related to a distance value.

[0008] Preferably, the process of establishing the spatial grid model is as follows: Point cloud data and image data collected by an environment perception device are acquired, and the point cloud data and the image data are fused to identify the three-dimensional positions and geometric dimensions of all static objects in the environment; A three-dimensional coordinate space covering the entire working scene is defined, and the three-dimensional coordinate space is discretized into cubic cells of equal volume according to a preset resolution; The three-dimensional positions and geometric dimensions of each static object are mapped to the three-dimensional coordinate space, and it is determined whether each cubic cell is occupied by an object. The occupied cells are marked as obstacle occupied cells; The marking information of all obstacle occupied cells is integrated to form a spatial grid model containing spatial occupancy state information.

[0009] Preferably, the process of generating the obstacle influence area by the obstacle processing module is as follows: For each obstacle entity contour formed by adjacent blocked cells, a minimum cuboid space completely wrapping the obstacle entity contour is calculated, and the minimum cuboid space is recorded as an obstacle circumscribed boundary range; According to a preset robot body safety radius, the obstacle circumscribed boundary range is expanded by an equal distance in each spatial dimension, and the expansion amount is the sum of the safety radius and a margin coefficient; The spatial range obtained after expansion is remapped to the spatial grid model, and all grid cells covered by the spatial range are marked as obstacle influence area cells regardless of their original state.

[0010] Preferably, the process of identifying the passable regions by the feasible space analysis module is as follows: In the space grid model, all grids marked as obstacle influence region cells are set as unavailable, and the remaining grids constitute a basic cell set of the robot feasible space; In the basic cell set of the robot feasible space, an unprocessed available grid cell is selected as a seed point, and a region growing method is used to find all available grid cells connected to the seed point through adjacency, and the available grid cells form a connected sub-region; The connected sub-region is recorded as an independent passable region, and the passable region is assigned a unique identifier; The steps of selecting a seed point and region growing are repeated until all available grid cells in the robot feasible space are classified into a passable region, and a set of mutually disconnected passable regions is finally obtained.

[0011] Preferably, the process of planning an initial reference trajectory and calculating an avoidance priority parameter in the trajectory initialization module is as follows: According to the task instruction, the coordinates of the motion starting position of the industrial robot in the space grid model and the coordinates of the task target position in the space grid model are obtained; In the set of passable regions, a passable region containing both the starting position coordinates and the target position coordinates is found, and if found, the passable region is determined as a planning base region; In the planning base region, a path search method is used to find a continuous cell sequence with the least number of grid cells between the starting position and the target position, and the center points of the cell sequence are connected to form a path point sequence of the initial reference trajectory; For each path point on the initial reference trajectory, the boundary point of the obstacle influence region cell closest to the path point is searched in the space grid model, and the Euclidean distance from the path point to the boundary point is calculated; A mapping relationship table of distance and priority parameter is established, and the avoidance priority parameter corresponding to each path point is assigned by querying the mapping relationship table according to the calculated Euclidean distance, and the closer the distance, the higher the priority parameter value.

[0012] Preferably, the process of the path optimization and output module finally outputting an obstacle avoidance planning path is as follows: All path points on the initial reference trajectory are traversed, and path points with avoidance priority parameters exceeding a preset activation threshold are selected and marked as key points to be adjusted; For each of the key points to be adjusted, a set of alternative location points within a preset radius is searched within the planning base area, with the point as the center. The points in the set of alternative location points must be located within the robot's feasible space. From the set of candidate locations, select a point that minimizes the change in smoothness between adjacent trajectory segments before and after the selected point, and replace the original key point to be adjusted. Using all the replaced new path points, reconnect the starting position and the target position to form a new trajectory, and calculate the minimum distance from all points on the new trajectory to the boundary of the nearest obstacle's influence area; Determine whether the minimum distance meets the safety interval requirement. If it does, use the new trajectory as the final output obstacle avoidance planning path. If it does not, use the new trajectory as a new initial reference trajectory and repeat the steps starting from screening key points to be adjusted until the safety interval requirement is met. Preferably, the step of using a region growing method to find all available grid cells connected to the seed point through adjacency includes: The adjacency relationship between mesh cells is defined as a six-connected neighborhood, meaning that each mesh cell is adjacent to mesh cells in the six directions of up, down, left, right, front, and back in three-dimensional space; Initialize an empty set to record the grid cells of the current passage area, and add the seed point to the queue to be processed; Take a grid cell from the queue to be processed, mark it as a member of the current passage area, and check all its adjacent grid cells; For each adjacent grid cell, if the adjacent grid cell is in an available state and has not been marked to any passable area, then it is added to the queue to be processed; Repeat the process of retrieving grid cells and checking adjacent cells until the queue to be processed is empty. At this point, all connected available grid cells have been found and recorded in the current passage area.

[0013] Preferably, within the planned basic area, finding a continuous sequence of cells between the starting position and the target position that traverses the fewest grid cells using a path search method includes: Each available grid cell within the planned basic area is considered as a node in a graph, and connecting edges are established between nodes, wherein there are undirected edges between nodes corresponding to adjacent available grid cells; Assign a uniform weight value to each edge, and use Dijkstra's algorithm to search from the node corresponding to the starting position to calculate the shortest path to the node at the target position. During the search process, a priority queue is maintained to store nodes to be visited, where nodes are sorted according to the weight accumulated from the starting position; When the target location node is visited, the path record is traced back, and a continuous unit sequence from the starting position to the target position is extracted to ensure that the number of grid cells traversed by the continuous unit sequence is minimized. The center points of the continuous unit sequence are connected sequentially to form a path point sequence of the initial reference trajectory.

[0014] Preferably, the system further includes a dynamic replanning module, used to update the spatial grid model in real time according to environmental changes during the execution of the obstacle avoidance planning path, and to correct local segments in the path that conflict with new obstacles, thereby generating an updated obstacle avoidance planning path.

[0015] Compared with the prior art, the beneficial effects of the present invention are: By calculating obstacle avoidance priority parameters for waypoints, the system can dynamically assign a quantified priority value to each waypoint based on factors such as its distance from the nearest obstacle's influence area and the degree of congestion at its location. This allows the algorithm to accurately identify the highest-risk critical points along the entire initial trajectory. Unlike traditional methods that perform uniform optimization or simple obstacle avoidance expansion across the entire line, this technology achieves differentiated processing of waypoints. Optimization resources can be concentrated on high-priority points that most urgently need adjustment, while low-priority points are subject to minor modifications or even left unchanged. This improves the targeting and efficiency of path optimization, ensuring that high-risk points are handled safely while preserving the optimal shape of the initial trajectory in open areas, avoiding unnecessary path distortion and growth.

[0016] Adjusting key points based on obstacle avoidance priority parameters and performing iterative optimization means that path correction is a continuous feedback process. The system first prioritizes points, locates and moves the highest-risk path points into safer, feasible areas, and then reassesses the priority status of each point on the entire trajectory in the new environment. This priority-driven iteration can be repeated until the safety indicators of all path points meet preset thresholds. This process differs from one-time searches or fixed-number smoothing post-processing; it allows the path to evolve gradually under safety constraints. This generates a more globally coordinated path. Moving high-priority points may free up space for adjacent low-priority points or provide better passageways, ultimately optimizing the overall length and smoothness of the path while meeting strict obstacle avoidance safety requirements, enhancing the system's adaptability in complex and dynamic environments. Attached Figure Description

[0017] Figure 1 This is a schematic diagram illustrating the working principle of the path planning and obstacle avoidance system for the industrial robot described in this invention. Figure 2 Flowchart for creating a spatial grid model; Figure 3 A flowchart for identifying the access area; Figure 4 A scatter plot related to path planning for industrial robots; Figure 5 This is a schematic diagram of the spatial distribution of obstacles in industrial robot path planning. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0019] Please see Figure 1 This invention provides a path planning and obstacle avoidance system for industrial robots. The system includes: an environment modeling module, an obstacle handling module, a feasible space analysis module, a trajectory initialization module, and a path optimization and output module. The environment modeling module processes environmental perception data, constructs and maintains a spatial grid model containing obstacle occupancy information and spatial grid cell states. The obstacle handling module analyzes the spatial grid model, identifies obstacle entity outlines, calculates the obstacle's outer boundary range, and generates the obstacle's influence area. The feasible space analysis module calculates the robot's feasible space based on the obstacle's influence area and performs connectivity analysis on the robot's feasible space, identifying different traversable areas. Within the specified traversable area, the trajectory initialization module plans an initial reference trajectory composed of a sequence of path points based on the task's starting point and target point, and calculates avoidance priority parameters for the path points. The path optimization and output module adjusts the key points in the initial reference trajectory according to the avoidance priority parameters, iteratively optimizing within the robot's feasible space until a safety-compliant obstacle avoidance planning path is generated.

[0020] Example 1: In this system, the outline of an obstacle entity is defined by a set of coordinates of boundary mesh cells. The obstacle's influence area is determined by the extended distance parameter and the original outline. The robot's feasible space is a set of mesh cells not marked by the obstacle's influence area. The traversable area is a subset of the robot's feasible space defined by adjacency relationships. The avoidance priority parameter is a numerical index negatively correlated with the distance value.

[0021] In practical implementation, the outline of an obstacle entity is defined by a set of coordinates of boundary grid cells. This set of coordinates constitutes a digital description of the obstacle's geometry in a discrete spatial grid model. For example, for a cubic obstacle, its outline is not represented by all the grid cells it occupies, but by the set of coordinates of the layer of grid cells that constitute its outer surface. The obstacle's influence area is determined by the expansion distance parameter and the original outline. The expansion distance parameter is a constant value preset according to the robot's physical dimensions and safety strategy. The new three-dimensional spatial range formed by expanding the original outline outward by this distance is the obstacle's influence area. The robot's feasible space is the set of grid cells not marked by the obstacle's influence area. It represents the set of all spatial locations that the robot can theoretically occupy without obstacles after considering safety buffers. The traversable area is a subset within the robot's feasible space defined by adjacency relationships. By checking the adjacency connectivity between grid cells, continuous feasible space cells are clustered into independent, internally connected spatial subsets. Each such subset is a traversable area, and the robot path is constrained to be planned within a single traversable area.

[0022] In some embodiments, the avoidance priority parameter is a numerical metric negatively correlated with distance values. This parameter assigns a quantified weight to each point on the trajectory to guide subsequent optimization adjustments. The calculation of the avoidance priority parameter depends on the Euclidean distance from a pathpoint to the boundary point of the nearest obstacle's influence zone. It can be understood that there exists a mathematical relationship to formally express this negative correlation. For example, the formula for calculating the avoidance priority parameter is defined as follows:

[0023] in: Indicates the first The avoidance priority parameter for each path point Indicates the first The Euclidean distance from each path point to the boundary point of the nearest obstacle's influence area. It is a normalization coefficient. It is a decay coefficient used to control the rate at which priority decreases with increasing distance. In this formula, due to the exponential function... As distance The priority parameter increases and then decreases monotonically, therefore it avoids priority parameters. With distance The two points show a negative correlation; the closer the points are, the higher their avoidance priority parameter value, indicating that they need to be adjusted first during the optimization process.

[0024] In some embodiments, by comparing the output of the avoidance priority parameter under different distance input values, the negative correlation characteristic can be clearly observed. For example, when the path point is 0.1 meters away from the boundary of the obstacle's influence area, the avoidance priority parameter value calculated by substituting into the formula is 5.6; when the distance increases to 0.5 meters, the calculated avoidance priority parameter value decreases to 1.2; when the distance further increases to 1.0 meter, the calculated avoidance priority parameter value is only 0.3. This series of numerical changes intuitively verifies the definition of the avoidance priority parameter as a numerical indicator negatively correlated with distance values. It can be understood that in the path optimization module, the system will determine which points on the trajectory will be prioritized and their positions adjusted based on the level of this avoidance priority parameter. Optionally, the coefficient... and The specific values ​​can be configured and adjusted according to the movement agility of different robots and the risk level of the working environment, but the core mathematical relationship between the avoidance priority parameter and the distance value remains unchanged.

[0025] Example 2: See Figure 2 The process of establishing the spatial mesh model involves acquiring point cloud data and image data collected by environmental sensing devices, fusing the point cloud data and image data, and identifying the 3D position and geometric dimensions of all static objects in the environment. A 3D coordinate space covering the entire working scene is defined and discretized into cubic units of equal volume according to a preset resolution. The 3D position and geometric dimensions of each static object are mapped to this 3D coordinate space, and it is determined whether each cubic unit is occupied by an object. Occupied units are marked as obstacle-occupied units. The marking information of all obstacle-occupied units is integrated to form a spatial mesh model containing spatial occupancy status information. The obstacle processing module generates the obstacle influence area by calculating the minimum cuboid space that can completely enclose the outline of each obstacle entity formed by connecting adjacent blocking units for each obstacle entity outline. This minimum cuboid space is recorded as the obstacle's outer boundary range. Based on the preset robot body safety radius, the obstacle's outer boundary range is equidistantly extended in each spatial dimension by an extension amount equal to the sum of the safety radius and a margin coefficient. The expanded spatial extent is remapped into the spatial grid model, and all grid cells covered by the spatial extent, regardless of their original state, are marked as obstacle-affected area cells.

[0026] In practical implementation, the establishment of the spatial mesh model begins with the acquisition and fusion of environmental perception data. This process involves acquiring point cloud data and image data collected by environmental perception devices, fusing the point cloud and image data, and identifying the 3D positions and geometric dimensions of all static objects in the environment. For example, point cloud data acquired by a 3D LiDAR provides precise 3D coordinates of objects, while image data acquired by a visual camera assists in object recognition and contour refinement. After fusion processing, a static environment list containing the positions, dimensions, and boundary information of multiple objects is generated. A 3D coordinate space covering the entire work scene is defined, and this 3D coordinate space is discretized into uniformly sized cubic units according to a preset resolution. The preset resolution determines the fineness of the spatial mesh model; for example, setting the side length of each cubic unit to 0.05 meters, a work scene 10 meters long, 5 meters wide, and 3 meters high is divided into a mesh space composed of 400×100×60 cubic units. The 3D position and geometric dimensions of each static object are mapped to this 3D coordinate space. It is then determined whether each cube unit is occupied by an object, and the occupied units are marked as obstacle-occupied units. The marking information of all obstacle-occupied units is integrated to form a spatial mesh model containing spatial occupancy status information. This spatial mesh model is represented in computer memory as a 3D Boolean array, where each element represents the occupancy status of the cube unit at the corresponding coordinates.

[0027] In practical implementation, the obstacle handling module generates the obstacle influence area based on a spatial mesh model through geometric analysis and spatial expansion. For each obstacle entity contour formed by connecting adjacent blocking units, the module calculates the minimum cuboid space that can completely enclose the obstacle entity contour, recording this minimum cuboid space as the obstacle's circumscribed boundary. For example, for an approximately spherical obstacle composed of 125 occupying units in the spatial mesh model, the system iterates through these units to find their minimum and maximum coordinate values ​​in the X, Y, and Z axes, thus determining a cuboid boundary that exactly encloses all units. Based on a preset robot body safety radius, the obstacle's circumscribed boundary is expanded equidistantly in each spatial dimension, with the expansion amount being the sum of the safety radius and a margin coefficient. The purpose of this expansion is to create a safe buffer zone for the robot body away from the original obstacle. The calculation of the expansion amount Δ can be explicitly expressed by a formula:

[0028] in: Indicates the expansion quantity. The preset safe radius of the robot body is a constant related to the physical envelope size of the robot; This represents the margin factor, an additional safety constant used to address systematic errors and uncertainties. For example, the safety radius. Set to 0.3 meters, margin factor If the circumscribed cuboid boundary is set to 0.05 meters, the expansion amount Δ is 0.35 meters. This means that the circumscribed cuboid boundary of the original obstacle will expand outward by 0.35 meters on each of its six faces, forming a new, larger cuboid space. In some embodiments, the expanded space is remapped into a spatial mesh model, and all mesh cells covered by the space, regardless of their original state, are marked as obstacle influence area cells. Through this marking, a "no-entry zone" is formed around the original obstacle, and any subsequent planning must prevent the robot's pathpoints from falling into these marked cells. Optionally, for non-convex obstacles, calculating their convex hull before calculating and expanding the circumscribed cuboid allows for a tighter and safer definition of the obstacle influence area.

[0029] Example 3: See Figure 3 The process of identifying passable areas in the feasible space analysis module involves setting all grid cells marked as obstacle-affected areas to an unusable state in the spatial grid model. The remaining grid cells constitute the basic set of cells for the robot's feasible space. Within this basic set, an unprocessed usable grid cell is randomly selected as a seed point. Using a region growing method, all usable grid cells connected to the seed point through adjacency are identified, forming a connected sub-region. This connected sub-region is recorded as an independent passable area and assigned a unique identifier. The steps of selecting the seed point and region growing are repeated until all usable grid cells in the robot's feasible space are classified into a single passable area, ultimately resulting in a set of unconnected passable areas. The region growing method identifies all usable grid cells connected to the seed point through adjacency, including defining the adjacency relationship between grid cells as a six-connected neighborhood, meaning each grid cell is adjacent to grid cells in the six directions (up, down, left, right, front, and back) in three-dimensional space. Initialize an empty set to record the grid cells of the current passable region, and add the seed point to the processing queue. Retrieve a grid cell from this queue, mark it as a member of the current passable region, and check all its adjacent grid cells. For each adjacent grid cell, if it is available and not marked as belonging to any passable region, add it to the processing queue. Repeat the process of retrieving grid cells and checking adjacent cells until the processing queue is empty. At this point, all connected available grid cells have been identified and recorded in the current passable region.

[0030] In practical implementation, the process of identifying traversable areas in the feasible space analysis module begins with filtering state markers in the spatial grid model. In the spatial grid model, all grids marked as obstacle-affected areas are set to an unusable state, and the remaining grids constitute the basic set of cells in the robot's feasible space. It can be understood that obstacle-affected areas are impassable; therefore, based on the usability judgment of grid cells, the basic set of cells in the robot's feasible space is a discrete set of points composed of "usable" state cells in the grid model. Within this basic set of cells, an unprocessed usable grid cell is randomly selected as a seed point. Using a region growing method, all usable grid cells connected to the seed point through adjacency are identified, forming a connected sub-region. This connected sub-region is recorded as an independent traversable area and assigned a unique identifier. The steps of selecting the seed point and region growing are repeated until all usable grid cells in the robot's feasible space are classified into a single traversable area, ultimately resulting in a set of unconnected traversable areas. Each traversable area represents a spatial range within which the robot can move continuously without crossing obstacle-affected areas.

[0031] In some embodiments, a region growing method is used to find all available grid cells connected to the seed point through adjacency relationships. Specifically, this involves defining the adjacency relationship between grid cells as a six-connected neighborhood, meaning each grid cell is adjacent to grid cells in the six directions (up, down, left, right, front, and back) in three-dimensional space. Determining adjacency requires a clear mathematical standard. The grid cell coordinates are defined as integer triples (x, y, z), representing their index in the discrete grid space. The condition for two grid cells to be adjacent can be expressed using a Manhattan distance formula:

[0032] Where: symbol and These represent the coordinate indices of two grid cells, with the symbols... This represents the Manhattan distance, which is the sum of the absolute values ​​of the coordinate differences. Two grid cells are considered adjacent in a six-connected sense when their Manhattan distance is equal to 1. An empty set is initialized to record the grid cells of the current traversable region, and the seed point is added to the processing queue. A grid cell is retrieved from the processing queue, marked as a member of the current traversable region, and all its neighboring grid cells are checked. For each neighboring grid cell, if it is available and not marked as belonging to any traversable region, it is added to the processing queue. The process of retrieving grid cells and checking neighboring cells is repeated until the processing queue is empty. At this point, all connected available grid cells have been found and recorded in the current traversable region.

[0033] In some embodiments, the effectiveness of the operation can be clearly demonstrated by comparing the data states before and after region growth. For example, in a mesh model containing multiple isolated obstacles, the initial set of basic cells in the robot's feasible space is scattered. The system selects an available mesh cell at coordinates (5,10,3) as a seed point to start region growth. Through calculation, it is found that several of its neighboring cells (5,10,4), (5,10,2), (5,9,3), (5,11,3), (4,10,3), and (6,10,3) meet the condition of being available but unmarked. These cells are added to the processing queue and processed one by one, eventually growing a connected region containing 235 mesh cells, which is assigned the identifier "Region_1". Subsequently, the system selects a new seed point in the remaining unmarked available cells, such as at coordinates (20,8,2), to start the next region growth, generating another "Region_2" containing 78 mesh cells. Taking an industrial robot's working scenario as an example, the spatial mesh model has a resolution of 0.05 meters, and the basic unit set of the robot's feasible space contains 1200 usable mesh units. A seed point with coordinates (8,15,4) is selected. Following the six-connected neighborhood rule, satisfying a Manhattan distance of 1, the processing queue is initialized, and this seed point is added. First, the seed point (8,15,4) is retrieved and marked as a member of "Region_3". Its neighboring units (8,15,3), (8,15,5), (8,14,4), (8,16,4), (7,15,4), and (9,15,4) are checked. Five of these units are usable and unmarked, and are added to the processing queue. The above operation is repeated, retrieving units from the queue sequentially. After three iterations, the processing queue has processed a total of 89 units, ultimately generating a connected sub-region containing 156 usable mesh units, and assigning it the unique identifier "Region_3". From the remaining unprocessed available cells, the seed point (30,22,4) is selected to perform region growing again, generating "Region_4" containing 98 cells, until all 1200 available cells are classified into 6 independent traversable regions. This process is repeated until all available cells have obtained region identification, and the robot's feasible space is clearly divided into several independent traversable regions.

[0034] Example 4: The process of planning the initial reference trajectory and calculating the avoidance priority parameters in the trajectory initialization module is as follows: Based on the task instructions, the coordinates of the industrial robot's starting position and the target position in the spatial grid model are obtained. In the set of travel areas, a travel area containing both the starting and target position coordinates is searched. If found, this travel area is designated as the planning base area. Within this planning base area, a path search method is used to find a continuous sequence of grid cells between the starting and target positions that passes through the fewest grid cells. The center points of this sequence are connected to form the path point sequence of the initial reference trajectory. For each path point on the initial reference trajectory, the boundary point of the nearest obstacle influence area cell in the spatial grid model is searched, and the Euclidean distance from the path point to this boundary point is calculated. A mapping table between distance and priority parameters is established. Based on the calculated Euclidean distance, this mapping table is consulted to assign a corresponding avoidance priority parameter to each path point; the closer the distance, the higher the priority parameter value. Within the planned basic area, a path search method is used to find a continuous sequence of cells between the starting and target positions that traverses the shortest number of grid cells. This involves treating each available grid cell within the planned basic area as a node in a graph and establishing connecting edges between nodes, where undirected edges exist between nodes corresponding to adjacent available grid cells. A uniform weight is assigned to each edge, and Dijkstra's algorithm is used to search from the node corresponding to the starting position to calculate the shortest path to the target node. During the search, a priority queue is maintained to store nodes to be visited, sorted by their accumulated weights from the starting position. When the target node is visited, the path record is backtracked to extract the continuous sequence of cells from the starting position to the target position, ensuring that this continuous sequence traverses the fewest possible grid cells. The center points of this continuous sequence are then connected sequentially to form a path point sequence for the initial reference trajectory.

[0035] In practical implementation, the initial reference trajectory planning and avoidance priority parameter calculation process in the trajectory initialization module, based on the task instructions, obtains the coordinates of the industrial robot's starting position and the target position in the spatial grid model. Within the set of travel regions, a travel region containing both the starting and target position coordinates is searched. If found, this travel region is designated as the planning base region. For example, if the starting position coordinates are grid (10,20,5) and the target position coordinates are grid (80,60,5), the system traverses the boundary ranges of all travel regions and finds that the travel region labeled "Region_A" contains both coordinate points. Therefore, "Region_A" is selected as the planning base region for this path planning. Within the planning base region, a path search method is used to find a continuous sequence of grid cells between the starting and target positions that passes through the fewest grid cells. The center points of this sequence are connected to form the path point sequence of the initial reference trajectory. For each path point on the initial reference trajectory, search for the boundary point of the nearest obstacle influence zone cell in the spatial grid model, and calculate the Euclidean distance from the path point to the boundary point. Establish a mapping table between distance and priority parameters. Based on the calculated Euclidean distance, look up the mapping table and assign a corresponding avoidance priority parameter to each path point; the closer the distance, the higher the priority parameter value.

[0036] In some embodiments, within a planning base area, a path search method is used to find a continuous sequence of grid cells between the starting position and the target position that traverses the shortest number of grid cells. Specifically, each available grid cell within the planning base area is treated as a node in a graph, and undirected edges are established between nodes corresponding to adjacent available grid cells. A uniform weight value is assigned to each edge, and Dijkstra's algorithm is used to search from the node corresponding to the starting position to calculate the shortest path to the node at the target position. Specifically, Dijkstra's algorithm first defines each available grid cell within the planning base area as a node in a graph, establishes undirected edges between nodes corresponding to adjacent grid cells, and assigns a uniform weight value to each edge. The algorithm initializes the search process from the node corresponding to the starting position, maintaining a priority queue to store nodes to be visited, where nodes are sorted in ascending order according to their accumulated weight values ​​from the starting position. The search iteratively retrieves the node with the smallest current accumulated weight from the priority queue, checks all its neighboring nodes, and updates the accumulated weight values ​​of neighboring nodes by comparing path weights. If a shorter path is found, the order of nodes in the queue is adjusted. During the search process, a priority queue is maintained to store nodes to be visited, where nodes are sorted according to their accumulated weight from the starting position. When the target node is visited, the path record is backtracked, and a continuous sequence of cells from the starting position to the target position is extracted, ensuring that the number of grid cells traversed in the continuous sequence is minimized. The center points of this continuous sequence of cells are connected sequentially to form the path point sequence of the initial reference trajectory. For example, after Dijkstra's algorithm completes the search, a sequence of 45 grid cells is obtained by backtracking. Connecting the center coordinates of these cells in order generates an initial reference trajectory containing 45 path points. Assume that there are 800 available grid cells (nodes) in the planning base region "Region_A", and the uniform weight of undirected edges between adjacent nodes is 1. The coordinates of the node corresponding to the starting position are (10,20,5), and the coordinates of the node corresponding to the target position are (80,60,5). During the initialization of Dijkstra's algorithm, the accumulated weight of the starting node is set to 0, and the accumulated weights of the remaining nodes are set to infinity. The priority queue only contains the starting node. In the first iteration, the starting node (10,20,5) is removed from the queue, its 6 neighboring nodes' cumulative weights are updated to 1, and it is added to the queue. In the second iteration, the neighboring node with the smallest cumulative weight (e.g., (10,20,6)) is removed, its unvisited neighboring nodes' cumulative weights are updated to 2, and this process is repeated. When the algorithm iterates to the 52nd iteration, the target node (80,60,5) is visited, and its cumulative weight is 44.By backtracking the path record, a sequence of 45 consecutive grid cells is extracted (node ​​coordinates are (10,20,5)→(11,20,5)→……→(80,60,5)). The center points of each cell (e.g., (10.025,20.025,5.025), (11.025,20.025,5.025)……) are connected sequentially to form an initial reference trajectory containing 45 path points, ensuring that the number of grid cells traversed is minimized.

[0037] In practice, the mapping relationship between distance and priority parameters is implemented through a predefined function or lookup table mechanism. The mapping table defines the conversion rules from Euclidean distance values ​​to avoidance priority parameter values. Distance values With the avoidance priority parameter value The functional relationship between them can be expressed as:

[0038] Where: symbol This represents the calculated obstacle avoidance priority parameter value, with the symbol... Represents the Euclidean distance from a path point to the boundary point of the nearest obstacle's influence area, denoted by [symbol]. It is a scaling factor used to adjust the numerical range of the priority parameter, sign... It is a small normal smoothing factor used to prevent distance As the value approaches zero, the calculated result approaches infinity. According to this formula, the distance... The smaller the value, the higher the avoidance priority parameter. The larger the value, the more it reflects the definition of a negative correlation. This calculation process can be performed directly, or the results can be pre-calculated and presented in a table for querying. See Table 1, which shows a fragment of the mapping relationship.

[0039] Table 1: Mapping Relationship between Distance and Priority Parameters ; In practice, after calculating the Euclidean distance of each path point, the corresponding obstacle avoidance priority parameter can be obtained by calculating using the above formula or by looking up a table. For example, for path point P5, its distance to the boundary of the nearest obstacle's influence area is calculated to be 0.20 meters. This distance can be determined by referring to Table 1 or substituting into the formula. , The avoidance priority parameter Q is calculated to be 5.00 for path point P5, which is 0.80 meters away. Therefore, the corresponding avoidance priority parameter Q is 1.39. It can be understood that path point P5, with a distance of 0.20 meters, has a higher avoidance priority parameter value than path point P12, which is 0.80 meters away, indicating that P5 needs to be prioritized for adjustment in subsequent optimizations. Optionally, the specific values ​​of the scaling factor γ and the smoothing factor ε can be configured according to the robot's motion accuracy and environmental safety level. Optionally, the mapping relationship can also be defined using a piecewise function or other nonlinear decay function, as long as the avoidance priority parameter and the distance value are negatively correlated. In some embodiments, the edge weight values ​​used in the path search process do not have to be 1. For example, different weights can be set according to the approximate distance between the center point of the grid cell and the obstacle, but the core objective remains to find the shortest feasible path in the grid space.

[0040] See Figure 4 This is a scatter plot related to industrial robot path planning. Color is negatively correlated with distance: lighter colors (e.g., yellow, green) correspond to smaller Euclidean distances (mostly concentrated between 0.1-0.3 meters), indicating these path points are closer to obstacles and have a higher avoidance priority. Distance distribution characteristics: the distances of path points fluctuate between 0.1-1.2 meters, with most points concentrated between 0.4-1.0 meters, and a few points extremely close. This plot is used in the trajectory initialization phase of industrial robot path planning. By visualizing the distance and priority distribution of path points, it helps engineers identify high-risk path points that need priority adjustment, providing a basis for subsequent path optimization.

[0041] Example 5: The process of the path optimization and output module finally outputting the obstacle avoidance planning path involves traversing all path points on the initial reference trajectory, filtering out path points whose avoidance priority parameters exceed a preset activation threshold, and marking these path points as key points to be adjusted. For each key point to be adjusted, a set of candidate location points within a preset radius is searched within the planning base area, centered on that point. Points in this set of candidate location points must be located within the robot's feasible space. From this set of candidate location points, a point that minimizes the change in smoothness between adjacent trajectory segments is selected and replaced with the original key point to be adjusted. Using all the replaced new path points, the starting position and the target position are reconnected to form a new trajectory. The minimum distance from all points on the new trajectory to the boundary of the nearest obstacle's influence area is calculated. It is determined whether this minimum distance meets the safety interval requirement. If it does, the new trajectory is used as the final output obstacle avoidance planning path; otherwise, the new trajectory is used as a new initial reference trajectory, and the steps starting from filtering key points to be adjusted are repeated until the safety interval requirement is met. The system also includes a dynamic replanning module, which updates the spatial grid model in real time according to environmental changes during the execution of obstacle avoidance planning path, corrects local segments in the path that conflict with new obstacles, and generates an updated obstacle avoidance planning path.

[0042] In practical implementation, the process of the path optimization and output module finally outputting the obstacle avoidance planning path begins with the evaluation and screening of path points on the initial reference trajectory. All path points on the initial reference trajectory are traversed, and path points whose avoidance priority parameters exceed a preset activation threshold are selected and marked as key points to be adjusted. For example, with a preset activation threshold of 3.0, in an initial reference trajectory containing 20 path points, 5 path points have avoidance priority parameter values ​​of 5.0, 4.2, 3.5, 3.1, and 6.0 respectively. These 5 points are marked as key points to be adjusted because their parameter values ​​exceed 3.0. For each key point to be adjusted, a set of candidate location points within a preset radius is searched within the planning base area, centered on that key point. Points in the candidate location point set must be located within the robot's feasible space. For example, if the preset radius is set to 0.5 meters, the system will search the spatial grid model for all grid cell center points with an Euclidean distance within 0.5 meters and a status of "available," using the coordinates of the key point to be adjusted as the center, to form the candidate location point set. From the set of candidate points, select the point that minimizes the change in smoothness between adjacent trajectory segments and replace the original key point to be adjusted. The change in smoothness can be quantified by calculating the change in the angle between the new point and the vectors formed by the preceding and following path points. Selecting the candidate point whose angle is closest to 180 degrees means that the replacement has the least impact on the original trajectory. Using all the replaced new path points, reconnect the starting and target positions to form a new trajectory. Calculate the minimum distance from all points on the new trajectory to the boundary of the nearest obstacle's influence area.

[0043] In practice, determining whether the minimum distance meets the safety interval requirement requires a clear criterion, with the safety interval requirement set as a fixed numerical threshold. The minimum value in the set of minimum distances corresponding to all points on the new trajectory is denoted as . The judgment condition is:

[0044] Where: symbol This represents the minimum distance among all path points on the new trajectory to the boundary of the nearest obstacle's influence area, denoted by [symbol]. This indicates the preset safety interval requirement. If met... If the condition is met, the new trajectory is used as the final obstacle avoidance planning path. If not, the new trajectory is used as a new initial reference trajectory, and the steps starting from selecting key points to be adjusted are repeated until the safety distance requirement is met. This can be understood as an iterative optimization process, gradually increasing the safety distance between the entire trajectory and the obstacle by continuously adjusting the positions of key points close to the obstacle. In some embodiments, the optimization effect can be demonstrated by comparing data before and after the iteration. Assuming the new trajectory obtained after the first iteration... The required safe distance is 0.08 meters. The distance is 0.15 meters, which does not meet the condition. The system uses this new trajectory as the new initial reference trajectory, recalculates the avoidance priority parameters of each point, selects new key points to be adjusted, and replaces the key points again. After the second round of optimization, the new trajectory is obtained. The height was increased to 0.18 meters, meeting the requirements. When the required distance is reached, the iteration terminates, and this trajectory is output as the final obstacle avoidance planning path.

[0045] In practical implementation, the dynamic replanning module is used to update the spatial mesh model in real time according to environmental changes during the execution of obstacle avoidance planning paths, and to correct local segments in the path that conflict with new obstacles, generating an updated obstacle avoidance planning path. For example, when the robot moves along the planned path, the environmental perception system detects a newly appearing obstacle at time t and marks the corresponding mesh cell as occupied. The dynamic replanning module first updates the spatial mesh model in real time based on the new perception data, marking the cell containing the new obstacle and its expanded area according to the safety radius as the obstacle influence area. Subsequently, the module checks whether any path points in the current planned path fall into the newly marked obstacle influence area. If a conflict exists, the dynamic replanning module takes the last safe position before the conflict point as the new local starting point, and the original target point or the next necessary point as the local target point. Within the latest feasible space, it repeats the trajectory initialization and optimization process to generate a corrected path segment from the new local starting point to the local target point, avoiding the new obstacle. This corrected path segment replaces the conflicting part in the original path, thereby generating an updated and safe obstacle avoidance planning path. Optionally, a safety interval requirement is specified. The value can be greater than or equal to the extension amount Δ used in the obstacle handling module to provide additional safety redundancy.

[0046] See Figure 5 This is a schematic diagram of obstacle spatial distribution in industrial robot path planning. Multiple obstacle entities (red) are concentrated in the upper, middle, and lower parts of the scene, with some obstacle influence areas (light yellow) overlapping. The areas not covered by light yellow represent the robot's feasible space, which needs to be further subdivided into traversable areas based on connectivity analysis. This diagram is used in the environmental modeling and obstacle handling stages of industrial robot path planning. By visualizing the spatial extent of obstacle entities and their influence areas, it provides basic data for subsequent feasible space analysis and path search, helping the system avoid obstacle influence areas and plan safe paths.

[0047] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0048] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A path planning and obstacle avoidance system for an industrial robot, characterized in that, The system includes: The environmental modeling module is used to process environmental perception data and build and maintain a spatial grid model that includes obstacle occupancy information and spatial grid cell status. The obstacle processing module is used to analyze the spatial mesh model, identify the outline of the obstacle entity, calculate the range of the obstacle's outer boundary, and generate the obstacle's influence area. The feasible space analysis module is used to calculate the robot's feasible space based on the area affected by the obstacle, and to perform connectivity analysis on the robot's feasible space to identify different passage areas; The trajectory initialization module is used to plan an initial reference trajectory consisting of a sequence of path points within a specified passage area, based on the task start point and the target point, and to calculate the avoidance priority parameters for the path points. The path optimization and output module is used to adjust the key points in the initial reference trajectory according to the obstacle avoidance priority parameters, and to perform iterative optimization within the robot's feasible space until an obstacle avoidance planning path that meets safety requirements is generated.

2. The path planning and obstacle avoidance system for industrial robots according to claim 1, characterized in that, The obstacle entity outline is defined by a set of coordinates of a set of boundary grid cells. The obstacle influence area is determined by the extended distance parameter and the original outline. The robot's feasible space is a set of grid cells not marked by the obstacle influence area. The passable area is a subset of the robot's feasible space defined by adjacency relationships. The avoidance priority parameter is a numerical index negatively correlated with the distance value.

3. The path planning and obstacle avoidance system for industrial robots according to claim 1, characterized in that, The process of establishing the spatial grid model is as follows: The system acquires point cloud data and image data collected by environmental sensing devices, fuses the point cloud data and image data, and identifies the three-dimensional position and geometric dimensions of all static objects in the environment. Define a three-dimensional coordinate space that covers the entire work scene, and discretize the three-dimensional coordinate space into cubic units of equal volume according to a preset resolution; Map the three-dimensional position and geometric dimensions of each static object to the three-dimensional coordinate space, determine whether each cube unit is occupied by an object, and mark the occupied unit as an obstacle-occupied unit. By integrating the marking information of all obstacle-occupied units, a spatial grid model containing spatial occupancy status information is formed.

4. The path planning and obstacle avoidance system for industrial robots according to claim 3, characterized in that, The process by which the obstacle handling module generates the obstacle influence area is as follows: For each obstacle entity outline formed by connecting adjacent blocking units, calculate the minimum cuboid space that can completely enclose the obstacle entity outline, and record the minimum cuboid space as the outer boundary range of the obstacle. Based on the preset safe radius of the robot body, the outer boundary of the obstacle is extended equidistantly in each spatial dimension, and the extension amount is the sum of the safe radius and a margin coefficient. The expanded spatial extent is remapped into the spatial grid model, and all grid cells covered by the spatial extent, regardless of their original state, are marked as obstacle influence area cells.

5. The path planning and obstacle avoidance system for industrial robots according to claim 4, characterized in that, The process by which the feasible space analysis module identifies the passable area is as follows: In the spatial grid model, all grids marked as obstacle-affected areas are set to an unusable state, and the remaining grids constitute the basic set of cells for the robot's feasible space. In the set of basic cells in the robot's feasible space, one unprocessed available grid cell is randomly selected as a seed point. Using a region growing method, all available grid cells connected to the seed point through adjacency are found, and the available grid cells form a connected sub-region. The connected sub-regions are recorded as independent access areas, and a unique identifier is assigned to each access area; Repeat the steps of selecting seed points and growing regions until all available mesh cells in the robot's feasible space are classified into a certain travel region, ultimately resulting in a set of unconnected travel regions.

6. The path planning and obstacle avoidance system for industrial robots according to claim 5, characterized in that, The calculation process for the planning and avoidance priority parameters of the initial reference trajectory in the trajectory initialization module is as follows: Based on the task instructions, obtain the coordinates of the industrial robot's starting position in the spatial grid model, and the coordinates of the task target position in the spatial grid model; In the set of travel areas, find a travel area that contains both the starting position coordinates and the target position coordinates. If found, the travel area is determined as the planning base area. Within the planned basic area, using the path search method, a continuous sequence of cells with the fewest grid cells is found between the starting position and the target position. The center points of the cell sequence are connected to form a path point sequence of the initial reference trajectory. For each path point on the initial reference trajectory, search for the boundary point of the obstacle influence area cell closest to the path point in the spatial grid model, and calculate the Euclidean distance from the path point to the boundary point; Establish a mapping table between distance and priority parameters. Query the mapping table based on the calculated Euclidean distance and assign a corresponding avoidance priority parameter to each path point. The closer the distance, the higher the priority parameter value.

7. The path planning and obstacle avoidance system for industrial robots according to claim 6, characterized in that, The process by which the path optimization and output module finally outputs the obstacle avoidance planning path is as follows: Traverse all path points on the initial reference trajectory, filter out path points whose avoidance priority parameters exceed a preset activation threshold, and mark the path points as key points to be adjusted. For each of the key points to be adjusted, a set of alternative location points within a preset radius is searched within the planning base area, with the point as the center. The points in the set of alternative location points must be located within the robot's feasible space. From the set of candidate locations, select a point that minimizes the change in smoothness between adjacent trajectory segments before and after the selected point, and replace the original key point to be adjusted. Using all the replaced new path points, reconnect the starting position and the target position to form a new trajectory, and calculate the minimum distance from all points on the new trajectory to the boundary of the nearest obstacle's influence area; Determine whether the minimum distance meets the safety interval requirement. If it does, use the new trajectory as the final obstacle avoidance planning path. If it does not, use the new trajectory as a new initial reference trajectory and repeat the steps starting from screening key points to be adjusted until the safety interval requirement is met.

8. The path planning and obstacle avoidance system for industrial robots according to claim 5, characterized in that, The method employing region growing to identify all available grid cells connected to the seed point through adjacency includes: The adjacency relationship between mesh cells is defined as a six-connected neighborhood, meaning that each mesh cell is adjacent to mesh cells in the six directions of up, down, left, right, front, and back in three-dimensional space; Initialize an empty set to record the grid cells of the current passage area, and add the seed point to the queue to be processed; Take a grid cell from the queue to be processed, mark it as a member of the current passage area, and check all its adjacent grid cells; For each adjacent grid cell, if the adjacent grid cell is in an available state and has not been marked to any passable area, then it is added to the queue to be processed; Repeat the process of retrieving grid cells and checking adjacent cells until the queue to be processed is empty. At this point, all connected available grid cells have been found and recorded in the current passage area.

9. The path planning and obstacle avoidance system for industrial robots according to claim 6, characterized in that, Within the planned basic area, using a path search method, a continuous sequence of cells with the fewest grid cells is found between the starting position and the target position, including: Each available grid cell within the planned basic area is considered as a node in a graph, and connecting edges are established between nodes, wherein there are undirected edges between nodes corresponding to adjacent available grid cells; Assign a uniform weight value to each edge, and use Dijkstra's algorithm to search from the node corresponding to the starting position to calculate the shortest path to the node at the target position. During the search process, a priority queue is maintained to store nodes to be visited, where nodes are sorted according to the weight accumulated from the starting position; When the target location node is visited, the path record is traced back, and a continuous unit sequence from the starting position to the target position is extracted to ensure that the number of grid cells traversed by the continuous unit sequence is minimized. The center points of the continuous unit sequence are connected sequentially to form a path point sequence of the initial reference trajectory.

10. The path planning and obstacle avoidance system for industrial robots according to claim 1, characterized in that, The system also includes a dynamic replanning module, which is used to update the spatial grid model in real time according to environmental changes during the execution of the obstacle avoidance planning path, and to correct local segments in the path that conflict with new obstacles, thereby generating an updated obstacle avoidance planning path.

Images