Flexible reconfigurable three-dimensional space modeling and robot path planning method

Abstract

The invention discloses a flexible reconfigurable three-dimensional space modeling and robot path planning method. The method comprises the following steps: 1) determining an adaptive grid size according to a minimum projection area of a minimum obstacle; 2) initializing a three-dimensional grid model and an obstacle recording matrix, and marking obstacle distribution in the matrix; and 3) dynamically expanding the collision detection range by combining the radius of the mechanical arm. And 4) initializing a starting point and a target point, carrying out coordinate normalization, and converting the starting point and the target point into grid indexes. 5) initializing a core data structure of the Dijkstra algorithm, wherein the core data structure comprises a priority queue, a distance dictionary, an access set and the like; and 6) traversing 26 neighborhood nodes and dynamically updating the path cost by adopting a node with the minimum heap maintenance path cost, and when a target point is accessed, backtracking a precursor node and outputting a shortest path sequence. By means of the method, efficient and accurate three-dimensional path searching is achieved, and the method is widely applied to the fields of robot path planning, intelligent navigation, three-dimensional map searching and the like.

Description

Technical Field

[0001] The present invention relates to the technical field of robot navigation, and particularly relates to a flexible and reconfigurable three-dimensional space modeling and robot path planning method, which is applicable to autonomous motion planning in complex three-dimensional scenarios such as industrial robotic arms and warehouse robots. Background Art

[0002] In the technical field of robot three-dimensional path planning, traditional methods have long been limited by the assumptions of static environments and rigid modeling systems, and it is difficult to meet the engineering requirements of complex dynamic scenarios. Existing technologies generally adopt a three-dimensional grid modeling method with a fixed resolution. Although this homogeneous spatial discretization strategy simplifies the data structure, it results in a large number of redundant grid calculations in non-obstacle areas and simple geometric feature areas, not only causing ineffective occupation of storage resources, but also significantly increasing the traversal complexity of subsequent path search algorithms. Especially when dealing with complex geometric features, traditional models cannot dynamically adjust the grid accuracy according to the shape of obstacles. For example, for large regular obstacles, the same high-precision grid is still forced to be used as that for small protrusion structures. This "one-size-fits-all" modeling strategy will largely lead to waste of computing resources. More prominently, traditional obstacle modeling lacks a dynamic compensation mechanism for the safety radius of the end effector of the robotic arm, and does not incorporate the physical size of the tool end into the collision detection system, resulting in potential collision risks in the actual execution of the planned path.

[0003] At the algorithm level, traditional path planning methods are mainly based on the Dijkstra algorithm. Its optimality guarantees the ability to solve the global shortest path and can ensure the reliability of path planning. However, when the traditional Dijkstra algorithm is extended in three-dimensional space, there are problems such as high computational complexity and limited search directions, resulting in its difficulty in real-time application in large-scale complex environments. Moreover, planners based on the basic Dijkstra architecture are limited by 6-direction or 18-direction search modes and are difficult to generate smooth paths that meet kinematic constraints in three-dimensional space. The limitations of traditional three-dimensional path planning technologies are concentrated in the dual dilemmas of modeling redundancy and algorithm inefficiency. By adopting an adaptive modeling method with dynamic accuracy adjustment, optimizing the search efficiency and path smoothness of the Dijkstra algorithm, the defects of existing technologies can be effectively overcome, and the path planning ability of industrial robots and flexible manufacturing systems in complex environments can be improved to meet the requirements of safety and resource efficiency in the industrial 4.0 era. Summary of the Invention

[0004] To solve the problems of insufficient accuracy in obstacle modeling and low efficiency in path search in existing three-dimensional path planning methods, the present invention provides a flexible and reconfigurable three-dimensional space modeling and robot path planning method. When constructing a three-dimensional grid map, this method considers using the size of the smallest obstacle as the accuracy of the grid square, adopts modeling strategies for obstacles of various geometric shapes, and dynamically compensates for obstacles by expanding the safety radius of the end of the robotic arm, thereby ensuring the safety of collision detection; at the same time, using heap optimization technology, 26-neighborhood expansion, and non-uniform movement cost calculation, the search efficiency is significantly improved on the basis of ensuring the optimality of the solution.

[0005] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0006] A flexible and reconfigurable three-dimensional space modeling and robot path planning method, the steps of which are as follows:

[0007] Step 1): Determine the adaptive three-dimensional grid size according to the minimum projected area of the smallest obstacle.

[0008] 1.1) Find the smallest obstacle and determine its projected area: Let the length, width, and height of the smallest obstacle be all L min , and its projected area A min , which depends on the maximum projection of the obstacle from different perspectives. In a three-dimensional environment, the projected area depends on the orientation of the obstacle. Take the area of its smallest face, that is, A min = L min * L min ;

[0009] 1.2) Calculate the adaptive grid cell size, and set the grid size to be not less than the projected area of the smallest obstacle, so that each obstacle occupies at least one grid cell. Let the side length of the grid cell be s = L min ;

[0010] 1.3) According to the size of the working space and the determined grid cell size, calculate the number of divisions of the grid in the three directions of X, Y, and Z:

[0011]

[0012] Among them, X max , Y max , Z max is the maximum size of the environment.

[0013] Step 2): Initialize the three-dimensional grid model and the obstacle record matrix according to the determined grid size, and mark the obstacle distribution in the matrix.

[0014] 2.1) Initialize the three-dimensional grid model. Use a three-dimensional boolean matrix to represent the spatial state, where False represents the passable area and True represents the obstacle area. Set the working space size (X max , Y max, , Z max ), which correspond to the maximum coordinate values in the x, y, and z directions respectively, and calculate the number of three-dimensional grids:

[0015]

[0016] where Δx, Δy, and Δz are the basic unit sizes of the grids, thus discretizing the entire three-dimensional space;

[0017] Through the above calculations, the grid coordinate indices are obtained: X = {0, Δx, 2Δx,..., X max}

[0018] Y = {0, Δy, 2Δy,..., Y max}

[0019] Z = {0, Δz, 2Δz,..., Z max}

[0020] The coordinate system of the three-dimensional grid space is constructed, and each grid cell corresponds to a cubic region in the three-dimensional space;

[0021] 2.2) Construct an obstacle record matrix. Establish a three-dimensional boolean matrix G(x, y, z) to store the distribution of obstacles, where:

[0022]

[0023] This matrix exactly matches the size of the three-dimensional grid. Initially, all elements are set to False, indicating no obstacles;

[0024] Expand the influence weight matrix of the obstacle area according to requirements:

[0025]

[0026] where λ is a penalty coefficient, usually set to 0.5 - 0.8 to reduce the possibility that the path planning algorithm selects an area close to the obstacle area; if no idle area is found, the algorithm will first select a grid with a smaller λ value to join the path;

[0027] 2.3) Mark the distribution of obstacles in the matrix, and propose four basic types of obstacles: spherical obstacles, cubic obstacles, ellipsoidal obstacles, and cylindrical obstacles. Then, use appropriate mathematical models to accurately model and bound the boundaries of the obstacles in the environment. The above four models are combined and cross-fused with each other to adapt to the complex and variable obstacle distribution, and improve the adaptability and generality of the environmental modeling. Since the shapes of each obstacle are different, it is necessary to establish mathematical models separately and mark them in the three-dimensional grid matrix G(x, y, z).

[0028] ① Spherical obstacle

[0029] Assume that the center coordinates of the spherical obstacle are (x c , y c , z c ), and the radius is R obs , then the spherical obstacle satisfies:

[0030]

[0031] ② Cubic obstacle

[0032] Set the center of the cubic obstacle to be (x c , y c , z c ), and the side lengths are L x, L y , L z , then the range occupied by the obstacle is:

[0033]

[0034] ③ Cylindrical obstacle

[0035] Set the geometric center coordinates of the cylindrical obstacle to be (x c , y c , z c ), the unit vector in the direction of the cylinder axis is (v x , v y , v z ), the radius is R obs , and the height is H. Then the obstacle satisfies:

[0036]

[0037] ④ Ellipsoidal obstacle

[0038] Set the geometric center coordinates of the ellipsoidal obstacle to be (x c , y c , z c), with radii a, b, and c respectively. Considering the modeling simplification and calculation efficiency in the real industrial environment, the present invention defines the obstacle as an axis-aligned ellipsoid, and the ellipsoidal obstacle satisfies:

[0039]

[0040] 2.4) Mark the obstacle distribution, traverse the obstacle information in the environment, and convert it into the corresponding grid index:

[0041]

[0042] where, (x obs , y obs , z obs) is the actual coordinate position of the obstacle. By calculating its grid index, set G(x index , y index , z index) to 1 to indicate that there is an obstacle in this cell. If the obstacle size is larger than one grid cell, the marking range of the obstacle needs to be extended:

[0043]

[0044] where L obs , W obs , H obs are the length, width, and height of the obstacle respectively;

[0045] 2.5) Perform adaptive optimization on the three-dimensional grid model. For large-scale scenarios, if the number of grids is too large, increase Δx, Δy, Δz to reduce the calculation overhead; if the obstacle density is high in some areas, refine the grids in this area and use the hierarchical grid method to improve the accuracy.

[0046] Step 3): Combine the radius of the end of the robotic arm to dynamically expand the obstacle area and correct the collision detection range.

[0047] 3.1) Let the effective radius of the end of the robotic arm be R arm , that is, the minimum safety distance between the nearest obstacle and the trajectory point during the movement of the robotic arm. Since the obstacle recording matrix G(x, y, z) only marks the position of the obstacle itself and does not consider the size of the robotic arm itself, the obstacle area needs to be expanded:

[0048] For spherical obstacles:

[0049] R safe = R obs ±R arm

[0050] For cubic obstacles:

[0051] x safe = x obs ±R arm

[0052] y safe = y obs ±R arm

[0053] z safe = z obs ±R arm

[0054] For a cylindrical obstacle:

[0055] H safe = H ± R arm

[0056] R safe = R obs ± R arm

[0057] For an ellipsoidal obstacle:

[0058] a safe = a ± R arm

[0059] b safe = b ± R arm

[0060] c safe = c ± R arm

[0061] Where: R arm represents the effective radius of the end of the robotic arm, indicating the minimum safe distance that should be maintained between the trajectory point of the robotic arm and the obstacle; R obs represents the original radius of a spherical or cylindrical obstacle; (x obs , y obs , z obs) the original boundary range of a cubic obstacle; H represents the original height of a cylindrical obstacle; a, b, c: the major, medium, and minor axis radii of an ellipsoid; a safe , b safe , c safe represent the expanded safety boundary;

[0062] The occupied range of each obstacle is expanded by a safety boundary of R arm , and at the same time, the movement range of the end of the robotic arm is expanded from 6 directions to 26;

[0063] 3.2) Dynamically expand the obstacle grid area. The collision detection of the robotic arm needs to consider all obstacles that may intersect the path, and the expanded area of the obstacle should cover all grids where collisions may occur:

[0064] For a spherical obstacle:

[0065] (x obs - x c ) 2 +(y obs - y c ) 2 +(z obs - z c ) 2 ≤(R obs + R arm ) 2

[0066]

[0067]

[0068] For a cubic obstacle:

[0069]

[0070] For a cylindrical obstacle:

[0071]

[0072] ((x obs - x c ) 2 +(y obs - y c ) 2 +(z obcs - z c ) 2 ) - ((x obs - x c ) × v x +(y obs - y c ) × v y +(z obs - z c ) × v c ) 2 ≤(R obs + R arm ) 2

[0073]

[0074] For an ellipsoidal obstacle:

[0075]

[0076]

[0077] Where: x obs , y obs , z obs represent the coordinates within and on the boundary of the obstacle; x c , y c , z c represent the coordinates of the geometric center of the obstacle, Δx, Δy, Δz represent the grid sizes in the three-dimensional grid space, and x index , y index , z index represent the index coordinates of the corresponding discrete grid, and (v x , v y , v z ) is the unit vector in the axial direction of the cylinder.

[0078] Step 4): Initialize the starting point and the target point, perform coordinate normalization, and convert them into grid indices, using the ceiling function to ensure alignment with the three-dimensional grid environment.

[0079] 4.1) To ensure the calculation accuracy and consistency of path planning, the starting point P S =(x s , y s , z s ) and the target point P g =(x g , y g , z g ) need to be initialized, and their coordinates are normalized and converted into grid index representations. The grid index I=(i, j, k) corresponding to any coordinate P=(x, y, z) is calculated as follows:

[0080]

[0081] where, represents the ceiling operation to ensure that the coordinate points always fall into the appropriate grid cells and avoid path deviation or accuracy loss caused by numerical rounding;

[0082] 4.2) According to the above normalization formula, the index representations of the starting point P S and the target point P g in the grid space are respectively:

[0083]

[0084] to ensure that the path planning algorithm can correctly identify the starting point and the target point in the discretized three-dimensional environment and perform calculations in the unified grid coordinate system.

[0085] Step 5): Initialize the core data structures of the Dijkstra algorithm, including the priority queue, distance dictionary, predecessor node dictionary, and visited set;

[0086] 5.1) Initialize the priority queue, using a Min - Heap structure to store the nodes to be processed, so that the node with the shortest current distance can be obtained each time. The initial state of the priority queue is:

[0087] Q = {(d(P S ), P S )}

[0088] where d(P S ) = 0, representing the initial distance of the starting point P S , and the distances of all other nodes P v ≠P S are set to infinity, that is:

[0089]

[0090] 5.2) Initialize the distance dictionary, which is used to store the current shortest distances from the starting point P S to each node P v , and is defined as follows:

[0091] D = {d(P v ) | P v ∈V}

[0092] where the initial state is:

[0093]

[0094] 5.3) Initialize the predecessor node dictionary, which is used to record the path information, that is, the predecessor node of each node, so that the shortest path can be reconstructed finally. Initialize it as:

[0095] D p = {None | P v ∈V

[0096] where:

[0097]

[0098] 5.4) Initialize the visited set, which is used to record the nodes whose shortest paths have been determined, that is, the nodes that have been popped from the priority queue:

[0099]

[0100] Only when a node's shortest path is determined will it be added to this set to prevent repeated processing. After the initialization of these data structures is completed, the Dijkstra algorithm can enter the main loop, gradually relax the edge weights, update the shortest path, and finally find the shortest paths from the starting point P s to all reachable nodes.

[0101] Step 6): Use a minimum heap to maintain the node with the minimum path cost, gradually expand the search range, traverse the 26 adjacent directions of the current node, calculate the movement cost, and dynamically update the distance information according to the constraint conditions; when the target point is visited, construct the shortest path by backtracking the predecessor nodes, calculate the total path cost, and output the optimized path sequence.

[0102] 6.1) Starting point P S and its initial distance d(P S ) = 0 are inserted into the priority queue Q, i.e.:

[0103] Q = {P S , 0}

[0104] 6.2) While the priority queue Q is not empty, loop and perform the following operations:

[0105] The node P with the minimum current distance in the priority queue Q u is taken out and added to the visited set V visited :

[0106]

[0107] V visited = V visited ∪ {P u}

[0108] If P u is the target node, terminate the algorithm and return the shortest path;

[0109] 6.3) To calculate the distance of the new neighbor nodes, traverse all the nodes P u that are directly adjacent to the current node P v . If P v ∈ V visited , then calculate the candidate path length to reach P u via P v :

[0110] d′(P v ) = d(P u ) + w(P u , P v )

[0111] where w(P u , P v ) represents the weight of the edge (P u , P v ). In the three-dimensional grid model, its value is

[0112] If this path is shorter than the currently recorded d(P v) is shorter, then update d(P v ) and modify the predecessor node:

[0113] d(P v ) = d′(P v ), D p (P v ) = P u

[0114] And insert the updated (P v , d(P v )) into the priority queue Q;

[0115] 6.4) Repeat the above steps until all nodes are visited or the target node is found;

[0116] 6.5) When the target node P g is found, the algorithm terminates. At this time, d(P g ) is the shortest path length from the starting point P s to the target node P g ;

[0117] If the priority queue Q is empty but P g is still not found, it means that P s cannot reach P g , then return unreachable;

[0118] If the target node P g is found, then trace back the path backward from D p (P g ):

[0119] P g → D p (P g ) → D p (D p (P g )) → … → P s

[0120] That is, the shortest path sequence.

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

[0122] In view of the problems of low path search efficiency and high computational complexity in three-dimensional space modeling, the present invention proposes an efficient path planning method based on grid modeling. This method first divides the three-dimensional space into regular grids, discretizes the complex environment into computable grid nodes, and combines the minimum heap optimization strategy to preferentially expand and search for the nodes with the minimum path cost. During the path search process, for the 26 neighboring directions of each grid node, the movement cost is calculated and the path information is dynamically adjusted according to the spatial constraints. At the same time, the minimum heap structure is used to maintain the nodes with the minimum path cost, gradually expanding the search range, traversing all possible movement directions of the current node, calculating the movement cost, and dynamically updating the path information according to the constraint conditions. By preferentially accessing the nodes with smaller costs, the path convergence is accelerated, the invalid traversal is reduced, and at the same time, combined with the dynamic path cost update and relaxation optimization method, the path selection is made more flexible and can adapt to complex environment constraints. Through the above method, the present invention realizes efficient and accurate three-dimensional path search, which is widely applicable to fields such as robot path planning, intelligent navigation, and three-dimensional map search, improving the search efficiency and optimizing the path selection. BRIEF DESCRIPTION OF THE DRAWINGS

[0123] Figure 1 It is a flowchart of the method of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0124] A flexible and reconfigurable three-dimensional space modeling and robot path planning method applicable to a three-dimensional industrial environment includes the following steps:

[0125] 1), Determine the adaptive three-dimensional grid size according to the minimum projected area of the smallest obstacle to ensure the rationality of environmental modeling.

[0126] (1) Find the smallest obstacle and determine its projected area. Let the length, width, and height of the smallest obstacle be all L min , and its projected area A min , which depends on the maximum projection of the obstacle from different perspectives. In a three-dimensional environment, the projected area usually depends on the direction of the obstacle. We take the area of its smallest face, that is, A min = L min * L min

[0127] (2) Calculate the adaptive grid cell size, and set the grid size to be not less than the projected area of the smallest obstacle, so that each obstacle occupies at least one grid cell. Let the side length of the grid cell be,

[0128] s = L min

[0129] (3) According to the size of the working space and the determined grid cell size, we calculate the division numbers of the grid in the X, Y, and Z directions:

[0130]

[0131] Among them, X max , Y max , Z max are the maximum dimensions of the environment.

[0132] 2), According to the determined grid size, initialize the three-dimensional grid model and the obstacle record matrix, and mark the obstacle distribution in the matrix.

[0133] (1) Initialize the three-dimensional grid model. Use a three-dimensional boolean matrix to represent the space state. False represents the passable area, and True represents the obstacle area. Set the working space size (X max , Y max, Z max ), which correspond to the maximum coordinate values in the x, y, and z directions respectively, and calculate the number of three-dimensional grids:

[0134]

[0135] Among them, Δx, Δy, Δz are the basic unit sizes of the grid, thus discretizing the entire three-dimensional space.

[0136] \nThrough the above calculations, the grid coordinate indices are obtained: X = {0, Δx, 2Δx, …, X max}

[0137] Y = {0, Δy, 2Δy, …, Y max}

[0138] Z = {0, Δz, 2Δz, …, Z max}

[0139] In this way, the coordinate system of the three-dimensional grid space is constructed, and each grid cell corresponds to a cube region in the three-dimensional space.

[0140] (2) Construct the obstacle record matrix. Establish a three-dimensional boolean matrix G(x, y, z) to store the obstacle distribution, where:

[0141]

[0142] This matrix exactly matches the size of the three-dimensional grid. Initially, all elements are set to False, indicating no obstacles.

[0143] In addition, the influence weight matrix of the obstacle area can be extended according to requirements:

[0144]

[0145] Among them, λ is a penalty coefficient, usually set to 0.5 - 0.8, to reduce the possibility that the path planning algorithm selects areas close to obstacles. If no idle area can be found, the algorithm will first select squares with a smaller λ value to join the path.

[0146] (3) Mark the distribution of obstacles in the matrix. The present invention proposes four basic types of obstacles: spherical obstacles, cubic obstacles, ellipsoidal obstacles, and cylindrical obstacles, and uses appropriate mathematical models to accurately model and bound the boundaries of obstacles in the environment. The above four models can be combined and cross - fused with each other to adapt to complex and variable obstacle distributions and improve the adaptability and generality of environmental modeling. Since the shapes of each type of obstacle are different, it is necessary to establish mathematical models separately and mark them in the three - dimensional grid matrix G(x, y, z).

[0147] ① Spherical obstacle

[0148] Assume that the center coordinates of the spherical obstacle are (x c , y c , z c ), and the radius is R [[ID=******]] obs , then the spherical obstacle satisfies:

[0149]

[0150] ② Cubic obstacle

[0151] Set the center of the cubic obstacle to be (x c , y c , z c ), and the side lengths are L x, L y , L z , then the range occupied by the obstacle is:

[0152]

[0153] ③ Cylindrical obstacle

[0154] Set the geometric center coordinates of the cylindrical obstacle to be (x c , y c , z c ), the unit vector of the axis direction of the cylinder is (v x , v y , v z ), the radius is R obs , and the height is H, then the obstacle satisfies: [[ID=******]]

[0155]

[0156] ④ Ellipsoidal obstacle

[0157] Let the geometric center coordinates of the ellipsoidal obstacle be (x c , y c , z c ), and the radii be a, b, c respectively. Considering the modeling simplification and calculation efficiency in the real industrial environment, the present invention defines the obstacle as an axis-aligned ellipsoid, then the ellipsoidal obstacle satisfies:

[0158]

[0159] (4) Mark the obstacle distribution, traverse the obstacle information in the environment, and convert it into the corresponding grid index:

[0160]

[0161] Among them, (x obs , y obs , z obs) is the actual coordinate position of the obstacle. By calculating its grid index, set

[0162] G(x index , y index , z index) to 1 to indicate that there is an obstacle in this cell. If the obstacle size is larger than one grid cell, the marking range of the obstacle needs to be extended:

[0163]

[0164] Among them, L obs , W obs , H obs are the length, width, and height of the obstacle respectively.

[0165] (5) Perform adaptive optimization on the three-dimensional grid model. For large-scale scenarios, if the number of grids is too large, Δx, Δy, and Δz can be appropriately increased to reduce the calculation overhead. If the obstacle density is high in some areas, the grids can be refined in this area, and the hierarchical grid method can be used to improve the accuracy.

[0166] 3), Combine the radius of the end of the robotic arm, dynamically expand the obstacle area, correct the collision detection range, and improve the safety of path planning.

[0167] (1) Let the effective radius of the end of the robotic arm be R arm , that is, the minimum safe distance between the nearest obstacle and the trajectory point during the movement of the robotic arm. Since the obstacle recording matrix G(x, y, z) only marks the position of the obstacle itself and does not consider the size of the robotic arm itself, the obstacle area needs to be expanded:

[0168] For spherical obstacles:

[0169] Rsafe =R obs ±R arm

[0170] For the cube obstacle:

[0171] x safe =x obs ±R arm

[0172] y safe =y obs ±R arm

[0173] z safe =z obs ±R arm

[0174] For cylindrical obstacles:

[0175] H safe =H±R arm

[0176] R safe =R obs ±R arm

[0177] For ellipsoidal obstacles:

[0178] a safe =a±R arm

[0179] b safe =b±R arm

[0180] c safe =c±R arm

[0181] Among them, R arm Indicates the effective radius of the robot arm end, indicating the minimum safe distance that should be maintained between the robot arm trajectory point and the obstacle; R obs Indicates the original radius of a spherical or cylindrical obstacle; (x obs ,y obs ,z obs) The original boundary range of the cubic obstacle; H represents the original height of the cylindrical obstacle; a, b, c: the radius of the major axis, median axis, and minor axis of the ellipsoid; a safe ,b safe ,c safe Indicates the expanded security boundary.

[0182] In this way, the occupied area of each obstacle is expanded by one R armThe safety margin. At the same time, the movement range of the end of the robotic arm expands from 6 directions to 26 directions, and the path is relatively smoother.

[0183] (2) Dynamically expand the obstacle grid area. The collision detection of the robotic arm needs to consider all obstacles that may intersect the path. Therefore, the expanded area of the obstacle should cover all grids where collisions may occur:

[0184] For spherical obstacles:

[0185] (x obs -x c ) 2 +(y obs -y c ) 2 +(z obs -z c ) 2 ≤(R obs +R arm ) 2

[0186]

[0187] For cubic obstacles:

[0188]

[0189] For cylindrical obstacles:

[0190]

[0191] ((x obs -x c ) 2 +(y obs -y c ) 2 +(z obs -z c ) 2 )-((x obs -x c )×v x +(y obs -y c )×v y +(z obs -z c )×v c ) 2 ≤(R obs +R arm ) 2

[0192]

[0193] For ellipsoidal obstacles:

[0194]

[0195] Among them, x obs , y obs , z obs represent the coordinates within and on the boundary of the obstacle object; x c , y c , z c represent the coordinates of the geometric center of the obstacle, Δx, Δy, Δz represent the grid sizes in the three-dimensional grid space, x index , y index , z index represent the index coordinates of the corresponding discrete grid, (v x , v y , v z ) is the unit vector in the direction of the axis of the cylinder.

[0196] 4) Initialize the starting point and the target point, perform coordinate normalization, and convert them into grid indices, using the ceiling function to ensure alignment with the three-dimensional grid environment.

[0197] (1) To ensure the calculation accuracy and consistency of path planning, it is necessary to initialize the starting point P S =(x s , y s , z s ) and the target point P g =(x g , y g , z g ), and normalize their coordinates and convert them into grid index representations. The grid index I=(i, j, k) corresponding to any coordinate P=(x, y, z) is calculated as follows:

[0198]

[0199] Among them, represents the ceiling operation to ensure that the coordinate points always fall into the appropriate grid cells and avoid path deviation or accuracy loss caused by numerical rounding.

[0200] (2) According to the above normalization formula, the index representations of the starting point P S and the target point P g in the grid space are respectively:

[0201]

[0202] So as to ensure that the path planning algorithm can correctly identify the starting point and the target point in the discretized three-dimensional environment and perform calculations in the unified grid coordinate system, improving the accuracy and executability of the planned path.

[0203] 5) Initialize the core data structures of Dijkstra's algorithm, including a priority queue, a distance dictionary, a predecessor node dictionary, and an access set, to ensure an efficient and stable search process.

[0204] (1) Initialize the priority queue, which uses a Min-Heap structure to store nodes to be processed, enabling efficient retrieval of the node with the shortest current distance each time. The initial state of the priority queue is:

[0205] Q = {(d(P S ), P S )}

[0206] where d(P S ) = 0, representing the initial distance of the starting point P S , and the distances of all other nodes P v ≠ P S are set to infinity, i.e.:

[0207]

[0208] (2) Initialize the distance dictionary, which is used to store the current shortest distances from the starting point P S to each node P v , defined as follows:

[0209] D = {d(P v ) | P v ∈ V}

[0210] where the initial state is:

[0211]

[0212] (3) Initialize the predecessor node dictionary, which is used to record path information, i.e., the predecessor node of each node, enabling the reconstruction of the shortest path eventually. Initialize it as:

[0213] D p = {None | P v ∈ V

[0214] where:

[0215]

[0216] (4) Initialize the access set, which is used to record the nodes for which the shortest paths have been determined, i.e., the nodes that have been popped from the priority queue:

[0217]

[0218] Only when a node is determined to have the shortest path will it be added to the set to prevent duplicate processing. After the initialization of these data structures is completed, the Dijkstra algorithm can enter the main loop, gradually relax the edge weights, update the shortest path, and finally find the node from the starting point P. s The shortest path to all reachable nodes.

[0219] 6) A minimum heap is used to maintain the node with the lowest path cost. The search range is gradually expanded, and the 26 adjacent directions of the current node are traversed. The movement cost is calculated and the distance information is dynamically updated according to the constraints. When the target point is visited, the shortest path is constructed by backtracking to the predecessor nodes, the total path cost is calculated, and the optimized path sequence is output.

[0220] 6.1) Starting point P S and its initial distance d(P S )=0 is inserted into the priority queue Q, that is:

[0221] Q={P S ,0}

[0222] 6.2) If the queue Q is not empty, perform the following operations in a loop:

[0223] The node P with the smallest current distance in the priority queue Q u , and add it to the access set V visited :

[0224]

[0225] V visited =V visited ∪{P u}

[0226] If P u If it is the target node, the algorithm is terminated and the shortest path is returned.

[0227] 6.3) The distance between the new neighbor node and the current node P u All directly adjacent nodes P v ,like

[0228] P v ∈V visited , then the calculation is done through P u Arrival P v The candidate path length is:

[0229] d′(P v )=d(P u )+w(P u ,P v )

[0230] Where w(Pu , P v ) represents the edge (P u , P v )'s weight. Its value within the three - dimensional grid model is

[0231] If this path is shorter than the currently recorded d(P v ), then update d(P v ) and modify the predecessor node:

[0232] d(P v ) = d'(P v ), D p (P v ) = P u

[0233] And insert the updated (P v , d(P v )) into the priority queue Q.

[0234] 6.4) Repeat the above steps until all nodes are visited or the target node is found.

[0235] 6.5) When the target node P g is found, the algorithm terminates. At this time, d(P g ) is the shortest path length from the starting point P s to the target node P g .

[0236] If the priority queue Q is empty but P g is still not found, it means that it is impossible to reach P s from P g , then return unreachable.

[0237] If the target node P g is found, then trace back the path backward from D p (P g ):

[0238] P g → D p (P g ) → D p (D p (P g )) → … → P s

[0239] That is, the shortest path sequence.

[0240] Example: This embodiment is based on a typical working space in an industrial production line to construct a three - dimensional obstacle environment model to verify the path planning and obstacle avoidance capabilities of the present invention in a complex environment.

[0241] Let the industrial operation space be a closed area with a length of 10 meters, a width of 5 meters, and a height of 5 meters, and the origin of the coordinate system is set at one bottom corner. The following obstacles are set in the environment, and the specific layout is shown in the following table:

[0242] Table 1: Obstacle Distribution Information Table

[0243]

[0244] Among them, the 5th to 12th obstacles are linearly arranged in the x-axis direction with the center point (5, 2.5, 2.5) as the axis of symmetry, forming a group of spindle-shaped obstacles (the radius of the central obstacle is the largest, 1.5 meters, and gradually decreases to 0.6 meters on both sides). The height of all cylindrical obstacles is 1 meter and they are arranged along the x-axis.

[0245] This layout simulates the following industrial scenarios:

[0246] 1. The middle part is an area with dense equipment or materials, commonly found in intelligent warehousing, high heat source operation areas, or collaborative robot intersection areas;

[0247] 2. The four corners are common fixed small obstacles, such as workstation brackets, transportation supports, etc.;

[0248] In addition, the starting point and the target point are set as follows: Starting point coordinates: (0.15, 0.5, 2), End point coordinates: (10, 5, 2). The radius of the end of the robotic arm is 0.05 meters.

[0249] First, according to the minimum projected area of the smallest obstacle in the environment, the grid size is adaptively determined to construct the spatial resolution of the three-dimensional environment model. In this experiment, the size of the obstacle with the smallest projection is 0.3m×0.3m×1m, and the grid side length is set slightly smaller than this value to ensure that all obstacles in the model can be accurately captured.

[0250] Next, initialize the three-dimensional grid model and the obstacle record matrix, corresponding to the occupancy state of the entire environmental space and the obstacle identification information respectively. Then, expand the obstacle grid in the model to construct a buffer zone to simulate the "non-collision" range in reality.

[0251] After completing the obstacle mapping, initialize the starting point and the target point required for path planning. Map them to the discrete grid index space through coordinate normalization to ensure that the subsequent search algorithm can perform path deduction based on the grid unit.

[0252] Subsequently, construct the core data structure of the Dijkstra algorithm. Including:

[0253] 1) A priority queue for selecting the node with the minimum estimated path cost currently;

[0254] 2) Distance dictionary, recording the current minimum path cost from the starting point for each node;

[0255] 3) Visited set, used to track the nodes for which the shortest paths have been determined, avoiding repeated expansion;

[0256] 4) Predecessor node dictionary, used to record the predecessor node of this node.

[0257] Based on the above structure, the algorithm is executed to perform a shortest path search in the three-dimensional grid space, returning the complete path and the total travel distance. The path is output in the form of a grid index sequence, representing the specific movement sequence in the discrete space from the starting point to the end point. The final path length obtained is 12.1089 meters, and the movement sequence (grid index sequence) (only listing the main inflection points of the path here): (1,4,19), (23,20,13), (35,31,12), (54,39,18), (99,49,19).

[0258] Based on this industrial environment modeling, the present invention adds a breadth-first search algorithm (DFS) and an A* heuristic search algorithm to perform path planning for this example. The results show that the final path length obtained by the breadth-first search algorithm is 13.3477 meters, and the main movement sequence is: (1,4,19), (4,1,16), (5,0,15), (50,0,0), (80,30,0), (99,49,19). The final path length obtained by the A* heuristic search algorithm is 12.1089 meters, and the main movement sequence is: (1,4,19), (23,20,13), (35,31,12), (54,39,18), (99,49,19), which is consistent with the path obtained by the present invention.

[0259] It can be seen that in this example, the algorithm of the present invention is significantly superior to the traditional breadth-first search algorithm in terms of path planning effect. The generated path is not only shorter, but also more in line with the requirements of spatial optimality. In terms of path accuracy, the algorithm of the present invention is completely consistent with the A* heuristic search algorithm in this example, but it is more superior in terms of execution efficiency and algorithm stability. Compared with A*, this algorithm can still stably output the optimal path without relying on a complex heuristic function, has stronger adaptability and versatility, and has a wider application prospect and promotion value in complex industrial environments.

Claims

1. A flexible and reconfigurable three-dimensional space modeling and robot path planning method, characterized in that The steps are as follows: Step 1), determine the adaptive three-dimensional grid size based on the minimum projected area of the smallest obstacle; Step 2), initialize the three-dimensional grid model and the obstacle record matrix according to the determined grid size, and mark the obstacle distribution in the matrix; Step 3), combine the radius of the end of the robotic arm to dynamically expand the obstacle area and correct the collision detection range; Step 4), initialize the starting point and the target point, perform coordinate normalization, convert them into grid indices, and use the ceiling method to ensure alignment with the three-dimensional grid environment; Step 5), initialize the core data structures of the Dijkstra algorithm, including the priority queue, distance dictionary, predecessor node dictionary, and access set; Step 6), use the minimum heap to maintain the node with the minimum path cost, gradually expand the search range, traverse the 26 adjacent directions of the current node, calculate the movement cost, and dynamically update the distance information according to the constraint conditions; When the target point is visited, construct the shortest path by backtracking the predecessor nodes, calculate the total path cost, and output the optimized path sequence.

2. According to the method for flexible reconfigurable three-dimensional space modeling and robot path planning described in claim 1, in the said step 1), the specific method is: 1.1) Find the smallest obstacle and determine its projected area: Assume the length, width, and height of the smallest obstacle are all L min , and its projected area A min , which depends on the maximum projection of this obstacle from different perspectives. In a three-dimensional environment, the projected area depends on the orientation of the obstacle. Take the area of its smallest face, that is, A min = L min * L min ; 1.2) Calculate the size of the adaptive grid cells, and set the grid size to be not less than the projected area of the smallest obstacle, so that each obstacle occupies at least one grid cell. Let the side length of the grid cell be s = L min ; 1.3) Calculate the number of divisions of the grid in the X, Y, and Z directions according to the size of the working space and the determined grid cell size: Among them, X max , Y max , Z max is the maximum size of the environment.

3. According to the method for flexible reconfigurable three-dimensional space modeling and robot path planning described in claim 1, in the said step 2), the specific method is: 2.1) Initialize the three-dimensional grid model, using a three-dimensional boolean matrix to represent the spatial state, where False represents the passable area and True represents the obstacle area. Set the working space size (X max , Y max , Z max ), corresponding to the maximum coordinate values in the x, y, and z directions respectively, and calculate the number of three-dimensional grids: Among them, Δx, Δy, and Δz are the basic unit sizes of the grid, thereby discretizing the entire three-dimensional space; Through the above calculations, the grid coordinate index is obtained: X = {0, Δx, 2Δx, …, X max} Y = {0, Δy, 2Δy, …, Y max} Z = {0, Δz, 2Δz, …, Z max} The coordinate system of the three-dimensional grid space is constructed, and each grid cell corresponds to a cubic region in the three-dimensional space; 2.2) Construct the obstacle record matrix, establish a three-dimensional boolean matrix G(x, y, z) for storing the obstacle distribution, where: This matrix completely matches the size of the three-dimensional grid, and all elements are initially set to False, indicating no obstacles; Expand the influence weight matrix of the obstacle area according to requirements: Among them, λ is a penalty coefficient, usually set to 0.5 - 0.8 to reduce the possibility that the path planning algorithm selects areas close to the obstacle region; if no idle area can be found, the algorithm will first select the square with a smaller λ value to join the path; 2.3) Mark the obstacle distribution in the matrix, propose four basic obstacle types: spherical obstacles, cubic obstacles, ellipsoidal obstacles, and cylindrical obstacles, and use appropriate mathematical models to accurately model and bound the boundaries of the obstacles in the environment; the above four models are combined and cross-fused with each other to adapt to complex and changing obstacle distributions, improve the adaptability and generality of environmental modeling. Since the shapes of each obstacle are different, mathematical models need to be established separately and marked in the three-dimensional grid matrix G(x, y, z); ① Spherical obstacle Assume that the center coordinates of the spherical obstacle are (x c , y c , z c ), and the radius is R obs , then the spherical obstacle satisfies: ② Cubic obstacle Set the center of the cubic obstacle as (x c , y c , z c ), and the side lengths are L x, L y , L z . Then the range occupied by the obstacle is: ③ Cylindrical obstacle Let the geometric center coordinates of the cylindrical obstacle be (x c , y c , z c ), the unit vector in the direction of the cylinder axis is (v x , v y , v z ), the radius is R obs , and the height is H. Then the obstacle satisfies: ((x - x c )) 2 +(y - y c )) 2 +(z - z c )) 2 )) ④ Ellipsoidal obstacle Let the geometric center coordinates of the ellipsoidal obstacle be (x c , y c , z c ), and the radii are a, b, and c respectively. Considering the modeling simplification and calculation efficiency in the real industrial environment, the present invention defines the obstacle as an axis-aligned ellipsoid, so the ellipsoidal obstacle satisfies: 2.4) Mark the obstacle distribution, traverse the obstacle information in the environment, and convert it into the corresponding grid index: Among them, (x obs , y obs , z obs) is the actual coordinate position of the obstacle. By calculating its grid index, G(x index , y index , z index) is set to 1 to indicate that there is an obstacle in this cell. If the size of the obstacle is larger than one grid cell, the marking range of the obstacle needs to be extended: where L obs , W obs , H obs are the length, width, and height of the obstacle, respectively; 2.5) Adaptively optimize the 3D grid model. For large-scale scenes, if the number of grids is too large, increase Δx, Δy, and Δz to reduce the computational overhead; if the obstacle density is high in certain areas, refine the grids in that area and use the hierarchical grid method to improve the accuracy.

4. According to the flexible and reconfigurable 3D space modeling and robot path planning method described in claim 1, in step 3), the specific method is: 3.1) Let the effective radius at the end of the robotic arm be R arm , that is, the minimum safety distance between the nearest obstacle and the trajectory point during the movement of the robotic arm. Since the obstacle recording matrix G(x, y, z) only marks the position of the obstacle itself and does not consider the size of the robotic arm itself, it is necessary to expand the obstacle area: For spherical obstacles: R safe = R obs ±R arm For cubic obstacles: x safe = x obs ±R arm y safe = y obs ±R arm z safe = z obs ±R arm For cylindrical obstacles: H safe = H ± R arm R safe = R obs ±R arm For ellipsoidal obstacles: a safe = a ± R arm b safe = b ± R arm c safe = c ± R arm Wherein: R arm represents the effective radius at the end of the robotic arm, indicating the minimum safe distance that should be maintained between the trajectory point of the robotic arm and the obstacle; R obs represents the original radius of a spherical or cylindrical obstacle; (x obs , y obs , z obs) the original boundary range of a cubic obstacle; H represents the original height of a cylindrical obstacle; a, b, c: the major axis, intermediate axis, and minor axis radii of an ellipsoid; a safe , b safe , c safe represents the expanded safety boundary; The occupied range of each obstacle is expanded by an R arm of the safety margin, and at the same time, the movement range of the end of the robotic arm is expanded from 6 directions to 26; 3.2) Dynamically expand the obstacle grid area. The collision detection of the robotic arm needs to consider all obstacles that may intersect the path, and the expansion area of the obstacle should cover all grids where collisions may occur: For spherical obstacles: (x obs -x c ) 2 +(y obs -y c ) 2 +(z obs -z c ) 2 ≤(R obs +R arm ) 2 For cubic obstacles: For cylindrical obstacles: ((x obs -x c ) 2 +(y obs -y c ) 2 +(z obs -z c ) 2 ) For ellipsoidal obstacles: Where: x obs , y obs , z obs represent the coordinates of the boundary and inside of the obstacle; x c , y c , z c , represent the coordinates of the geometric center of the obstacle, Δx, Δy, Δz represent the grid size of the three-dimensional grid space, x index , y index , z index represent the index coordinates of the corresponding discrete grid, (v x , v y , v z ) is the unit vector in the axial direction of the cylinder.

5. According to the flexible and reconfigurable 3D space modeling and robot path planning method described in claim 1, in step 4), the specific method is: 4.1) To ensure the calculation accuracy and consistency of path planning, the starting point P S =(x s , y s , z s ) and the target point P g =(x g , y g , z g ) need to be initialized, and their coordinates are normalized and converted into grid index representation; the grid index I=(i, j, k) corresponding to any coordinate P=(x, y, z) is calculated as follows: Among them, Denotes the ceiling operation to ensure that the coordinate points always fall into the appropriate grid cells, avoiding path deviation or precision loss caused by numerical rounding; 4.2) According to the above normalization formula, the starting point P S and the target point P g in the grid space are respectively represented by indices: To ensure that the path planning algorithm can correctly identify the starting point and the target point in the discretized 3D environment and perform calculations in the unified grid coordinate system.

6. According to the flexible and reconfigurable 3D space modeling and robot path planning method described in claim 1, in step 5), the specific method is: 5.1) Initialize the priority queue. Use the minimum heap Min-Heap structure to store the nodes to be processed, so that the node with the shortest current distance can be obtained each time. The initial state of the priority queue is: Q = {(d(P S ), P S )} where d(P S ) = 0 represents the initial distance of the starting point P S , while the distances of all other nodes P v ≠ P S are set to infinity, i.e.: 5.2) Initialize the distance dictionary to store the current shortest distances from the starting point P S to each node P v as defined below: D = {d(P v ) | P v ∈ V} Among them, The initial state is: 5.3) Initialize the predecessor node dictionary, which is used to record the path information, that is, the predecessor node of each node, so that the shortest path can be reconstructed finally. Initialize it as: D p = {None|P v ∈V Where: 5.4) Initialize the visited set, which is used to record the nodes for which the shortest path has been determined, that is, the nodes that have been popped from the priority queue: Only when the shortest path of a node is determined will it be added to the set to prevent duplicate processing. After the initialization of these data structures is completed, the Dijkstra algorithm can enter the main loop, gradually relax the edge weights, update the shortest path, and finally find the shortest paths from the starting point P s to all reachable nodes.

7. According to the flexible and reconfigurable 3D space modeling and robot path planning method described in claim 1, in step 6), the specific method is: 6.1) Starting point P S and its initial distance d(P S ) = 0 is inserted into the priority queue Q, i.e.: Q = {P S , 0} 6.2) When the queue Q is not empty, loop and perform the following operations: The node P with the smallest current distance in the priority queue Q u and add it to the visited set V visited : V visited = V visited ∪ {P u} If P u is the target node, terminate the algorithm and return the shortest path; 6.3) Distance of the new neighbor node, traverse all nodes P u directly adjacent to the current node P v , if P v ∈V visited , then calculate the candidate path length via P u to P v : d′(P v ) = d(P u ) + w(P u , P v ) where w(P u , P v ) represents the weight of the edge (P u , P v ), and its value within the three-dimensional grid model is If this path is shorter than the currently recorded d(P v ), then update d(P v ) and modify the predecessor node: d(P v ) = d′(P v ), D p (P v ) = P u and insert the updated (P v , d(P v )) into the priority queue Q; 6.4) Repeat the above steps until all nodes are visited or the target node is found; 6.5) When the target node P g is found, the algorithm terminates. At this time, d(P g ) is the shortest path length from the starting point P s to the target node P g ; If the priority queue Q is empty but P has not been found yet g , it means that starting from P s it is impossible to reach P g , then return unreachable; If the target node P is found g , then start backtracking the path backwards from D p (P g ): P g →D p (P g )→D p (D p (P g ))→…→P s That is, the shortest path sequence.

Images