Underground carry-scraper path planning method and system

Abstract

The invention discloses an underground carry-scraper path planning method and system, and the method comprises the steps: obtaining environment data information of a target underground mine roadway, constructing an environment map, and carrying out the preprocessing of the environment map, and obtaining a binary grid map; constructing a generalized Voronoi diagram, extracting a roadway center line and carrying out region division; performing path guidance and segmentation by adopting an algorithm to obtain a reference state set of the target underground mine roadway; and based on a hybrid algorithm, a regional dynamic node expansion strategy and a multi-target heuristic cost function, realizing path planning of the underground carry-scraper, and realizing path planning of the underground carry-scraper. According to the invention, through data acquisition and data processing of a target underground mine roadway, a roadway center line and divided areas are extracted, then path guidance is carried out through an algorithm, and based on a divided area dynamic node expansion strategy and a multi-target heuristic cost function optimization hybrid algorithm, path guidance is carried out. By combining a hybrid algorithm, a Dubins curve, a collision detection scheme, path smoothness punishment and path distance punishment, the path planning of the underground carry-scraper is realized, the reliability is higher, the accuracy is better, and the effect is better.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent mining, and in particular relates to a path planning method and system for an underground scraper. Background Art

[0002] To ensure personnel safety, autonomous driving technology for underground LHDs (LHDs) in the mining industry has rapidly advanced. Path planning algorithms are a crucial component of autonomous driving technology, directly impacting the navigation performance and efficiency of LHDs during autonomous operation. Therefore, research on path planning solutions for underground LHDs has long been a key focus of research on autonomous driving for heavy-duty underground vehicles.

[0003] At present, common underground scraper path planning solutions include search-based path planning solutions, sampling-based path planning solutions and artificial intelligence-based path planning solutions. Search-based path planning solutions, such as Dijkstra algorithm and Algorithms, etc., are the most widely used because the paths they generate are deterministic and globally optimal; however, such solutions have problems such as huge computational overhead, low search efficiency, and insufficient real-time performance. Sampling-based path planning solutions, such as PRM and RRT algorithms, have higher search efficiency in high-dimensional search spaces and complex environments; however, the collision detection mechanism of such solutions is relatively complex, and they are prone to falling into local optimality during the search process, and the generated paths are often less smooth. Artificial intelligence-based path planning solutions, such as deep learning and reinforcement learning solutions, autonomously learn path optimization strategies based on large-scale environmental perception data, and have stronger environmental adaptability and path diversity control capabilities; however, the practical application of such solutions is still limited by problems such as high-quality training data and high demand for computing resources. Summary of the Invention

[0004] One of the purposes of the present invention is to provide a path planning method for an underground scraper with high reliability, good accuracy and good effect.

[0005] A second object of the present invention is to provide a system for implementing the underground scraper path planning method.

[0006] The underground scraper path planning method provided by the present invention comprises the following steps:

[0007] S1. Obtaining environmental data information of the target underground mine tunnel and constructing an environmental map of the target underground mine tunnel;

[0008] S2 preprocesses the environmental map obtained in step S1 to obtain a binary raster map of the target underground mine tunnel;

[0009] S3. Constructing a generalized Voronoi diagram according to the binary grid map obtained in step S2, extracting a center line of the roadway and performing region division;

[0010] S4. According to the center line and region division result obtained in step S3, using the algorithm to guide the path and perform segmentation, obtaining a reference state set of the target underground mine roadway;

[0011] S5. According to the reference state set obtained in step S4, based on the hybrid algorithm and the regional dynamic node expansion strategy, and combining the path length, heading change, hinge angle deviation and safety margin to construct a multi-objective heuristic cost function, realizing the fast, safe and high-quality path planning of the underground shovel.

[0012] The step S1 specifically comprises the following steps:

[0013] Collecting the environmental map data information of the target underground mine roadway; using the SLAM method to construct the environmental map of the target underground mine roadway.

[0014] The step S2 specifically comprises the following steps:

[0015] The environmental map obtained in step S1 is smoothed to smooth the irregular contour of the roadway boundary, thereby obtaining a binary grid map of the target underground mine roadway.

[0016] The step S3 specifically comprises the following steps:

[0017] Based on the binary grid map obtained in step S2, a generalized Voronoi diagram is constructed to extract the center line skeleton of the free space;

[0018] A pruning algorithm is used to remove redundant branches to obtain a corresponding Voronoi node set , wherein is the obstacle set in the map;

[0019] The processed center line skeleton only has intersection points in the intersection area of the roadway;

[0020] Each intersection point is taken as the center, and a set safety margin is taken as the radius to demarcate a circular area as the intersection area of the roadway.

[0021] The step S4 specifically comprises the following steps:

[0022] The start pose and the end pose of the center hinge point of the shovel are set, wherein is the global horizontal coordinate of the starting position of the scraper, is the global vertical coordinate of the starting position of the scraper, is the front vehicle heading angle of the starting position of the scraper, is the global horizontal coordinate of the end position of the scraper, is the global vertical coordinate of the end position of the scraper, The front vehicle heading angle of the scraper's terminal position;

[0023] Select the node closest to the starting point in the centerline skeleton network The node closest to the destination ,use The algorithm generates an initial guidance path on the centerline skeleton ;

[0024] Along Perform equidistant discrete sampling, delete the nodes in the intersection area, and divide the remaining nodes into n curve segments The reference state set .

[0025] The step S5 comprises the following steps:

[0026] A. Start from the starting node and use the starting node as the base node for the current expansion;

[0027] B. Based on the specific location of the base node and the regional dynamic node expansion strategy, calculate the set of adjacent nodes of the base node;

[0028] C. Construct a multi-objective heuristic cost function based on path length, heading change, articulation angle deviation, and safety margin;

[0029] D. Calculate the cost function value of each adjacent node in the adjacent node set, and determine the next base node for current expansion based on the obtained cost function value;

[0030] E. Return to step B for the next round of iterative calculation until all nodes have completed iterative calculations.

[0031] F. Finally, the path planning result of the underground loader is obtained.

[0032] The step S5 specifically includes the following steps:

[0033] (1) Create an open list OpenSet and a closed list CloseSet, and initialize them to empty lists respectively; the open list OpenSet is used to store the nodes to be checked, and the closed list CloseSet is used to store the nodes that have been checked; put the starting node S into the open list OpenSet;

[0034] (2) Select the node with the smallest cost function value in the open list OpenSet as the basic node for the current expansion , and put the selected nodes into the closed list CloseSet;

[0035] (3) If the basic node The distance to the end point G is less than the threshold , then a bidirectional Dubins curve is used to connect the basic nodes and the end point G, and perform collision detection on the bidirectional Dubins curve: If the bidirectional Dubins curve passes the collision detection, the path is backtracked from the end point, and the path planning task is completed; if the bidirectional Dubins curve does not pass the collision detection, continue with the subsequent steps;

[0036] (4) Node forward expansion stage:

[0037] (4-1) If the basic node Located in a closed tunnel area and not belonging to the reference state set For any curve segment in the reference state set Select the distance base node Nearest curve segment ,exist Find the distance base node on The nearest point ; From point Start along the curve Traverse both sides of the base node one by one The distance is less than the set value point ; At this time, if you click Ability to pass collision-free Dubins curves and base nodes If it is connected, stop traversal and add the point to the adjacent node set ; For point and ,like Then set the expansion direction to positive sequence, Then set the expansion direction to negative order; if there is no Dubins curve that can pass through without collision and the basic node Connect the dots , then jump to step (4-4);

[0038] (4-2) If the basic node Located in the intersection area and does not belong to the reference state set Any curve segment in the intersection area Intersecting curve segments , from the curve segment The endpoint closer to the junction Start by going through the distance endpoints one by one Less than the set value point If there is a point Ability to pass collision-free Dubins curves and base nodes Connect, then stop traversal and point Join the adjacent node set ; For point and ,like Then set the expansion direction to positive sequence, Then set the expansion direction to negative order; if there is no Dubins curve that can pass through without collision and the basic node Connect the dots , then jump to step (4-4);

[0039] (4-3) If the basic node Located in the reference state set For any curve segment in , the next adjacent node is added to the adjacent node set according to the expansion direction. ;

[0040] (4-4) If the basic node Neither in the reference state set On any curve segment in the , there is no way to pass the collision-free Dubins curve and the reference state set If any curve segment in the The algorithm's node forward expansion method generates nodes based on the kinematic model and discrete control quantities;

[0041] (5) Calculation of cost value:

[0042] (5-1) For the set of adjacent nodes Any node in , use the following formula to calculate the corresponding cumulative cost :

[0043] In the formula As a basic node the cumulative cost of is a displacement penalty term, and , is a parameter value set, and , is a Euclidean distance between and ; is a steering dynamics penalty term, is a set dynamics penalty coefficient, is a joint angle increment, and , is a joint angle of the underground scraper at the adjacent node, is a joint angle of the underground scraper at the base node; is a joint angle steady state penalty term, is a set joint angle steady state penalty coefficient, is a joint angle; is a reverse driving penalty term, and , is a velocity vector of the base node, is a velocity vector of the adjacent node, is a set reverse driving penalty number; is a safety distance penalty term, is a set safety distance penalty coefficient, is a safety distance margin of the adjacent node to the nearest obstacle;

[0044] (5-2) For any node in the adjacent node set , the corresponding heuristic cost is calculated as follows:

[0045] where is a Euclidean distance cost term; is a non-constraint based cost term, representing the shortest feasible path length from to G using Dubins curves;

[0046] (5-3) The cost function value of any node in the adjacent node set is calculated as ;

[0047] (6) If node is not in the open list OpenSet, node is put into the open list OpenSet;

[0048] If node Already in the open list OpenSet, and the cost function value If the cost function value is less than the original one, the node is updated. The cost function value of the node The parent node is the base node If the node Already in the open list OpenSet, and the cost function value If it is greater than or equal to the original cost function value, no update will be performed;

[0049] If the node If the node is already in the closed list CloseSet, skip the node ;

[0050] (7) Return to step (2) to perform the next round of iterative calculation until all nodes have completed the iterative calculation;

[0051] (8) Finally, the path planning result of the underground scraper is obtained.

[0052] Step (4-4) described in the mixed The algorithm's node forward expansion method generates nodes based on the kinematic model and discrete control quantities, specifically including the following steps:

[0053] Select the central hinge point of the underground scraper as the reference point and set the linear speed. and discrete articulation angular rates As control input, discrete expansion nodes are generated by integrating the kinematic equations, and the expansion nodes that pass the collision detection are added to the adjacent node set. ;

[0054] The kinematic model of the underground LHD is expressed as

[0055] In the formula X-axis coordinate of the underground scraper reference point The partial derivative, Y-axis coordinate of the underground scraper reference point The partial derivative, The current heading angle of the underground LHD The partial derivative, is the articulation angular velocity, is the central hinge angle, is the heading angle of the front vehicle in the global coordinate system, is the distance from the front axle to the central hinge point, is the distance from the rear axle to the central hinge point, v is the current speed of the underground LHD;

[0056] by is a state variable, and is a control input.

[0057] The application also provides a system for implementing the underground shovel path planning method, comprising a data acquisition module, a data processing module, an extraction and division module, a path guiding module and a path planning module; the data acquisition module, the data processing module, the extraction and division module, the path guiding module and the path planning module are sequentially connected; the data acquisition module is used for acquiring environmental data information of a target underground mine tunnel, constructing an environmental map of the target underground mine tunnel, and uploading the data information to the data processing module; the data processing module is used for pre-processing the obtained environmental map according to the received data information, obtaining a binary grid map of the target underground mine tunnel, and uploading the data information to the extraction and division module; the extraction and division module is used for constructing a generalized Voronoi diagram according to the received data information and the obtained binary grid map, extracting a tunnel center line and performing regional division, and uploading the data information to the path guiding module; the path guiding module is used for performing path guiding and segmentation according to the received data information, the obtained center line and regional division result, and a algorithm, to obtain a reference state set of the target underground mine tunnel, and uploading the data information to the path planning module; the path planning module is used for constructing a multi-objective heuristic cost function based on a hybrid algorithm and a regional dynamic node expansion strategy, in combination with path length, heading change, articulation angle deviation and safety margin, according to the received data information and the obtained reference state set, to realize rapid, safe and high-quality path planning of the underground shovel.

[0058] The underground shovel path planning method and system provided by the application realize path planning of the underground shovel, have higher reliability, better accuracy and better effect, by data acquisition and data processing of the target underground mine tunnel, extraction of a tunnel center line and division of regions, path guiding by a algorithm, optimization of a hybrid algorithm based on a regional dynamic node expansion strategy and a multi-objective heuristic cost function, and combination of the hybrid algorithm, the regional dynamic node expansion strategy and the multi-objective heuristic cost function. BRIEF DESCRIPTION OF DRAWINGS

[0059] Figure 1 is a method flowchart of the method of the application.

[0060] Figure 2 is a skeleton extraction and regional classification diagram of the method of the application; wherein, Figure 2 (a) is a tunnel skeleton diagram without pruning algorithm processing, Figure 2(b) is a schematic diagram of a tunnel skeleton processed by one iteration of the pruning algorithm, Figure 2 (c) is a schematic diagram of a tunnel centerline skeleton processed by the pruning algorithm.

[0061] Figure 3 is a schematic diagram of a generated reference state set for the method of the invention.

[0062] Figure 4 is a schematic diagram of a closed tunnel expansion strategy for the method of the invention.

[0063] Figure 5 is a schematic diagram of a junction area expansion strategy for the method of the invention.

[0064] Figure 6 is a schematic diagram of a LHD kinematic model for the method of the invention.

[0065] Figure 7 is a schematic diagram of a single tunnel scenario for an embodiment of the method of the invention.

[0066] Figure 8 is a schematic diagram of a multi-branch tunnel scenario for an embodiment of the method of the invention.

[0067] Figure 9 is a schematic diagram of a grid-shaped tunnel scenario for an embodiment of the method of the invention.

[0068] Figure 10 is a schematic diagram of a path length comparison for an embodiment of the method of the invention; wherein, Figure 10 (a) is a schematic diagram of a path length comparison for a single tunnel, Figure 10 (b) is a schematic diagram of a path length comparison for a multi-branch tunnel scenario, Figure 10 (c) is a schematic diagram of a path length comparison for a grid-shaped tunnel.

[0069] Figure 11 is a schematic diagram of a node count comparison for an embodiment of the method of the invention; wherein, Figure 11 (a) is a schematic diagram of a node count comparison for a single tunnel, Figure 11 (b) is a schematic diagram of a node count comparison for a multi-branch tunnel scenario, Figure 11 (c) is a schematic diagram of a node count comparison for a grid-shaped tunnel.

[0070] Figure 12 is a visualization of node expansion for a single tunnel scenario for an embodiment of the method of the invention; wherein, Figure 12 (a) is a visualization of node expansion for a single tunnel scenario for a traditional hybrid algorithm; Figure 12 (b) is a visualization of node expansion for a single tunnel scenario for a bidirectional hybrid algorithm; Figure 12(c) is a single lane scene node expansion visualization schematic diagram of the present application.

[0071] Figure 13 is a multi-branch lane scene node expansion visualization schematic diagram of the method embodiment of the present application; wherein, Figure 13 (a) is a multi-branch lane scene node expansion visualization schematic diagram of the traditional hybrid algorithm; Figure 13 (b) is a multi-branch lane scene node expansion visualization schematic diagram of the bidirectional hybrid algorithm; Figure 13 (c) is a multi-branch lane scene node expansion visualization schematic diagram of the present application.

[0072] Figure 14 is a grid-shaped lane scene node expansion visualization schematic diagram of the method embodiment of the present application; wherein, Figure 14 (a) is a grid-shaped lane scene node expansion visualization schematic diagram of the traditional hybrid algorithm; Figure 14 (b) is a grid-shaped lane scene node expansion visualization schematic diagram of the bidirectional hybrid algorithm; Figure 14 (c) is a grid-shaped lane scene node expansion visualization schematic diagram of the present application.

[0073] Figure 15 is an average safety margin schematic diagram of the method embodiment of the present application; wherein, Figure 15 (a) is an average safety margin schematic diagram of a single lane scene; Figure 15 (b) is an average safety margin schematic diagram of a multi-branch lane scene; Figure 15 (c) is an average safety margin schematic diagram of a grid-shaped lane scene.

[0074] Figure 16 is a functional module schematic diagram of the system of the present application. DETAILED DESCRIPTION

[0075] As Figure 1 shown is a method flow schematic diagram of the method of the present application: the underground shovel path planning method disclosed by the present application comprises the following steps:

[0076] S1. Obtain the environmental data information of the target underground mine tunnel, and construct an environmental map of the target underground mine tunnel; specifically comprising the following steps:

[0077] Collect the environmental map data information of the target underground mine tunnel; use the SLAM method to construct an environmental map of the target underground mine tunnel.

[0078] S2. Preprocessing the environment map obtained in step S1 to obtain a binary grid map of the target underground mine tunnel; specifically including the following steps:

[0079] The environment map obtained in step S1 is smoothed to smooth the irregular contour of the tunnel boundary, thereby obtaining a binary grid map of the target underground mine tunnel; the binary grid map has continuous free space and can be used for subsequent center line extraction and collision detection.

[0080] S3. Constructing a generalized Voronoi diagram according to the binary grid map obtained in step S2, extracting a tunnel center line and performing region division; specifically including the following steps (as shown in Figure 2

[0081] Based on the binary grid map obtained in step S2, a generalized Voronoi diagram is constructed to extract the center line skeleton of the free space;

[0082] A pruning algorithm is used to remove redundant branches to obtain a corresponding Voronoi node set , where is the obstacle set in the map;

[0083] The processed center line skeleton only has intersection points in the intersection area of the tunnel;

[0084] Each intersection point is taken as the center, a circular area is demarcated as the intersection area of the tunnel with a safety margin as the radius; the circular area is taken as the intersection area of the tunnel, which is used as a reference for adjusting the different node expansion strategies.

[0085] S4. According to the center line and region division result obtained in step S3, using algorithm for path guidance and segmentation to obtain a reference state set of the target underground mine tunnel; specifically including the following steps (as shown in Figure 3

[0086] Setting the start pose and the end pose of the center hinge point of the shovel-truck, where is the global horizontal coordinate of the start pose of the shovel-truck, is the global vertical coordinate of the start pose of the shovel-truck, is the front vehicle body heading angle of the start pose of the shovel-truck, is the global horizontal coordinate of the end pose of the shovel-truck, is the global vertical coordinate of the end pose of the shovel-truck, ​​The front vehicle heading angle of the scraper's terminal position;

[0087] Select the node closest to the starting point in the centerline skeleton network The node closest to the destination ,use The algorithm generates an initial guidance path on the centerline skeleton ;

[0088] Along Perform equidistant discrete sampling, delete the nodes in the intersection area, and divide the remaining nodes into n curve segments The reference state set .

[0089] S5. Based on the reference state set obtained in step S4, The algorithm and regional dynamic node expansion strategy are combined with path length, heading change, articulation angle deviation and safety margin to construct a multi-objective heuristic cost function to achieve fast, safe and high-quality path planning for underground loaders. The method includes the following steps:

[0090] A. Start from the starting node and use the starting node as the base node for the current expansion;

[0091] B. According to the specific location of the basic node, based on the regional dynamic node expansion strategy, calculate the adjacent node set of the basic node

[0092] C. Construct a multi-objective heuristic cost function based on path length, heading change, articulation angle deviation, and safety margin;

[0093] D. Calculate the cost function value of each adjacent node in the adjacent node set, and determine the next base node for current expansion based on the obtained cost function value;

[0094] E. Return to step B for the next round of iterative calculation until all nodes have completed iterative calculations;

[0095] F. Finally, the path planning result of the underground scraper is obtained.

[0096] The following steps can be taken for implementation:

[0097] (1) Create an open list OpenSet and a closed list CloseSet, and initialize them to empty lists respectively; the open list OpenSet is used to store the nodes to be checked, and the closed list CloseSet is used to store the nodes that have been checked; put the starting node S into the open list OpenSet;

[0098] (2) Select the node with the smallest cost function value in the open list OpenSet as the basic node for the current expansion , and put the selected nodes into the closed list CloseSet;

[0099] (3) If the basic node The distance to the end point G is less than the threshold , then a bidirectional Dubins curve is used to connect the basic nodes and the end point G, and perform collision detection on the bidirectional Dubins curve: If the bidirectional Dubins curve passes the collision detection, the path is backtracked from the end point, and the path planning task is completed; if the bidirectional Dubins curve does not pass the collision detection, continue with the subsequent steps;

[0100] (4) Node forward expansion stage:

[0101] (4-1) If the basic node Located in a closed tunnel area and not belonging to the reference state set For any curve segment in the reference state set Select the distance base node Nearest curve segment ,exist Find the distance base node on The nearest point ; From point Start along the curve Traverse both sides of the base node one by one The distance is less than the set value point ; At this time, if you click Ability to pass collision-free Dubins curves and base nodes If it is connected, stop traversal and add the point to the adjacent node set ; For point and ,like Then set the expansion direction to positive sequence, Then set the expansion direction to negative order; if there is no Dubins curve that can pass through without collision and the basic node Connect the dots , then jump to step (4-4);

[0102] (4-2) If the basic node Located in the intersection area and does not belong to the reference state set Any curve segment in the intersection area Intersecting curve segments , from the curve segment The endpoint closer to the junction Start by going through the distance endpoints one by one Less than the set value point If there is a point Ability to pass collision-free Dubins curves and base nodes Connect, then stop traversal and point Join the adjacent node set ; For point and ,like Then set the expansion direction to positive sequence, Then set the expansion direction to negative order; if there is no Dubins curve that can pass through without collision and the basic node Connect the dots , then jump to step (4-4);

[0103] (4-3) If the basic node Located in the reference state set For any curve segment in , the next adjacent node is added to the adjacent node set according to the expansion direction. ;

[0104] (4-4) If the basic node Neither in the reference state set On any curve segment in the , there is no way to pass the collision-free Dubins curve and the reference state set If any curve segment in the The algorithm's node forward expansion method generates nodes based on the kinematic model and discrete control quantities; specifically, it includes the following steps:

[0105] Select the central hinge point of the underground scraper as the reference point and set the linear speed. and discrete articulation angular rates As control input, discrete expansion nodes are generated by integrating the kinematic equations, and the expansion nodes that pass the collision detection are added to the adjacent node set. ;

[0106] The kinematic model of the underground LHD is expressed as

[0107] In the formula X-axis coordinate of the underground scraper reference point The partial derivative, Y-axis coordinate of the underground scraper reference point The partial derivative, The current heading angle of the underground LHD The partial derivative, is the articulation angular velocity, is the central hinge angle, is the heading angle of the front vehicle in the global coordinate system, is the distance from the front axle to the central hinge point, is the distance from the rear axle to the central hinge point, v is the current speed of the underground LHD;

[0108] by is the state variable, is the control input;

[0109] (5) Calculation of cost value:

[0110] (5-1) For the set of adjacent nodes Any node in , use the following formula to calculate the corresponding cumulative cost :

[0111] In the formula As a basic node the cumulative cost of is the displacement penalty term, and , is the set parameter value, and , To calculate and The Euclidean distance of is the steering dynamics penalty term, is the set special dynamics penalty coefficient, is the hinge angle increment, and , is the articulation angle of the underground scraper at the adjacent node, is the articulation angle of the underground LHD at the foundation node; is the hinge angle steady-state penalty term, is the set steady-state penalty coefficient of the articulation angle, is the hinge angle; is a reversing penalty item, and , is the velocity vector of the base node, is the velocity vector of the adjacent node, The number of reversing penalties set; is the safety distance penalty term, is the set safety distance penalty coefficient, is the safety distance margin from the adjacent node to the nearest obstacle;

[0112] (5-2) For the set of adjacent nodes Any node in The corresponding heuristic cost is calculated by using the following formula :

[0113] In the formula is the Euclidean distance cost term; is the non-constrained-based cost term, representing the length of the shortest feasible path from the Dubins curve to G;

[0114] (5-3) The cost function value of any node in the adjacent node set is calculated as ;

[0115] (6) If the node is not in the open list OpenSet, the node is put into the open list OpenSet;

[0116] If the node is already in the open list OpenSet, and the cost function value is less than the original cost function value, the cost function value of the node is updated, and the parent node of the node is set as the base node (This time, since the base node has been set, no change of the base node will be made when returning to step (2)); if the node is already in the open list OpenSet, and the cost function value is greater than or equal to the original cost function value, no update is made;

[0117] If the node is already in the closed list CloseSet, the node is skipped;

[0118] (7) Return to step (2) for the next round of iterative calculation until all nodes are iteratively calculated;

[0119] (8) Finally, the path planning result of the underground shovel is obtained.

[0120] The above scheme, on the basis of the traditional hybrid algorithm, optimizes the set regional dynamic node expansion strategy and multi-objective heuristic cost function, can significantly improve the path search efficiency while ensuring the quality and safety of the generated path by suppressing the redundant diffusion of discrete nodes in the free space. ​​​

[0121] The method of the present application significantly improves search efficiency: by constructing a reference state set generated based on the center line of the tunnel as a guide, the generation of redundant nodes can be effectively inhibited; in a large-scale tunnel map, the number of extended nodes can be reduced by more than 50%, thereby greatly shortening the calculation time, compared with the traditional hybrid algorithm and bidirectional hybrid search algorithm.

[0122] The method of the present application significantly improves path safety: the reference state guide of the upper layer combined with the safety distance cost term in the multi-objective cost function can effectively avoid the path nodes being too close to the two side walls; in the process of underground driving of the underground shovel, the distance between the vehicle body and the wall is often less than 1m, and the present application can improve the average safety margin by 0.2-0.3m compared with the traditional hybrid algorithm and bidirectional hybrid search algorithm.

[0123] The cost function designed by the method of the present application comprehensively considers the change of the steering angle of the vehicle, the continuity of the heading and the curvature control, and by inhibiting large-amplitude articulated angle steering and frequent direction switching, the phenomenon of curvature mutation in the path is reduced. The generated path is overall smoother and has good continuity, which is beneficial to the stable path tracking of the controller and improves the smoothness and controllability of the whole vehicle driving.

[0124] The method of the present application takes into account the global feasibility and adaptability to complex environment: since the method of the present application adopts a dynamic node expansion strategy in different regions, the center line is used as a guide in the closed region, and discrete nodes are generated based on the kinematic model in the intersection region; this mechanism takes into account the global optimality and local feasibility of the path, effectively avoids falling into local optimum, and at the same time improves the adaptability to complex underground environment.

[0125] The method of the present application is further described in conjunction with the following embodiments:

[0126] A variety of structured environment maps are constructed for simulation experiments; the experiments are carried out based on the ROS framework under the Ubuntu 20.04 system (Intel Core i77820X processor, 48GB RAM), and the vehicle model adopts a hinged structure with non-complete constraints, and the specific parameters are shown in Table 1:

[0127] The tunnel map is derived from the modeling of the structural characteristics of the actual mine tunnel, and includes three typical scenes:

[0128] Single tunnel scene (such as Figure 7Long straight tunnel structure, no branch, test the expansion efficiency of the algorithm in regular environment;

[0129] Multi-branch tunnel scenario (as shown in Figure 8 ): Contains T-shaped and Y-shaped intersections, evaluate the path feasibility and navigation flexibility of the algorithm in the cross-topology area;

[0130] Grid-like tunnel scenario (as shown in Figure 9 ): Simulate large-scale dense intersection tunnel system, test the scalability and stability of the algorithm.

[0131] The two types of comparison methods used in the embodiment experiment include:

[0132] Traditional hybrid algorithm, adapted to the motion constraints of articulated vehicles, with the same cost function as the present invention;

[0133] Bidirectional hybrid algorithm, introduces a bidirectional search mechanism to optimize the traditional hybrid algorithm.

[0134] In practical applications, shorter path length can significantly shorten the running time of the vehicle and reduce energy consumption, such as Figure 10 (a) Figure 10 (b) shows that in single tunnel and multi-branch tunnel, due to the relatively fixed road sections, the three methods can generate paths with similar lengths in different tunnel scenarios, with a path length difference of less than 2m; In the grid-like tunnel, the road sections of the paths generated by the three algorithms differ significantly; as shown in Figure 10 (c), the algorithm proposed in the present invention shortens the path length by more than 12m compared to the traditional hybrid algorithm and bidirectional hybrid algorithm, demonstrating the optimality of the path.

[0135] The number of expanded nodes can intuitively describe the state exploration cost of the path planning algorithm. By comparing the average number of nodes of related algorithms in different scenarios and different endpoint combinations (as shown in Figure 11 ), it can be seen that in all test scenarios: the number of expanded nodes of the traditional hybrid algorithm will increase exponentially with the complexity of the environment, such as from 14 to 9463 in a single tunnel, and to 287522 in a grid-like tunnel scenario; the bidirectional hybrid algorithm uses a bidirectional search mechanism to effectively alleviate the phenomenon of redundant expansion, but the number of nodes increases significantly in some complex scenarios (multi-branch tunnel environment 7-12 endpoint combination and grid-like tunnel environment); while the proposed algorithm shows a lower number of expanded nodes in all scenarios (less than 1000) ), and there is almost no significant growth as the complexity of the environment increases.

[0136] By comparing traditional hybrid Algorithm, two-way hybrid The algorithm and the algorithm proposed in this invention were tested repeatedly for 20 search times, and the experimental results were recorded in the statistical table of average search time, as shown in Table 2:

[0137] The results showed that the traditional mixed The search time of the algorithm will increase exponentially with the increase of the complexity of the search space and reach the level of several seconds; bidirectional hybrid The algorithm is significantly improved, and the search time increases more slowly with the increase of the complexity of the search space. However, it increases significantly in complex multi-branch lanes and grid lane fields, and there are significant outliers. The improved algorithm proposed in this invention always maintains a lower search time ( ), the time consumption increases steadily with the complexity of the environment, showing the best real-time performance; the traditional hybrid The algorithm is abbreviated as HA, which stands for two-way hybrid The algorithm is abbreviated as BHA, and the improved planning algorithm proposed in this invention is called HHA.

[0138] By expanding the nodes under the typical path (such as Figure 12~Figure 14 As shown in the figure, it can be visualized that: the traditional hybrid The algorithm presents a large range of discretely distributed redundant expansion nodes in all scenarios; bidirectional hybrid The distribution of the algorithm's extended nodes is relatively convergent, but there is still an obvious redundant expansion phenomenon; in contrast, the extended nodes of the improved algorithm proposed in this invention are almost completely concentrated along the reference state set, effectively avoiding the invalid expansion of nodes in complex environments.

[0139] The safety margin refers to the minimum distance between a node on the path and an environmental obstacle. When an underground scraper is driving in a tunnel, the maximum distance between the vehicle body and the walls on both sides is often less than 1 meter. Any slight approach may trigger a potential collision accident. By comparing the average safety margins of the paths generated by the three path planning algorithms in different tunnel scenarios, we can see that (e.g. Figure 15 The average safety margin of the algorithm proposed in this invention is significantly higher than that of the traditional mixed and two-way mixing Algorithms; with the worst performing two-way hybrid Compared with the proposed algorithm, it can provide a safety redundancy of more than 0.3m, indicating that the proposed method can provide a higher safety margin control for underground scrapers, thereby reducing the risk of collision in confined spaces.

[0140] As Figure 16 The functional module diagram of the system of the application is shown in the figure: the system for implementing the underground shovel path planning method disclosed in the application comprises a data acquisition module, a data processing module, an extraction and division module, a path guiding module and a path planning module; the data acquisition module, the data processing module, the extraction and division module, the path guiding module and the path planning module are sequentially connected in series; the data acquisition module is used for acquiring the environmental data information of the target underground mine tunnel and constructing the environmental map of the target underground mine tunnel, and uploading the data information to the data processing module; the data processing module is used for pre-processing the obtained environmental map according to the received data information, obtaining the binary grid map of the target underground mine tunnel, and uploading the data information to the extraction and division module; the extraction and division module is used for constructing a generalized Voronoi diagram according to the received data information and the obtained binary grid map, extracting the tunnel center line and performing regional division, and uploading the data information to the path guiding module; the path guiding module is used for performing path guiding and segmentation according to the received data information, the obtained center line and the regional division result, and adopting an algorithm to obtain the reference state set of the target underground mine tunnel, and uploading the data information to the path planning module; the path planning module is used for constructing a multi-objective heuristic cost function based on a hybrid algorithm and a regional dynamic node expansion strategy in combination with path length, heading change, articulation angle deviation and safety margin according to the received data information and the obtained reference state set, so as to realize rapid, safe and high-quality path planning of the underground shovel.

Claims

1. A path planning method for an underground scraper, characterized in that The steps include: S1. Obtaining environmental data information of the target underground mine tunnel and constructing an environmental map of the target underground mine tunnel; S2 preprocesses the environmental map obtained in step S1 to obtain a binary raster map of the target underground mine tunnel; S3. Based on the binary grid map obtained in step S2, a generalized Voronoi diagram is constructed to extract the lane centerline and perform regional division; S4. Based on the centerline and area division results obtained in step S3, use The algorithm performs path guidance and segmentation to obtain a reference state set of the target underground mine tunnel; S5. Based on the reference state set obtained in step S4, An algorithm and a regional dynamic node expansion strategy are proposed, and a multi-objective heuristic cost function is constructed by combining path length, heading change, articulation angle deviation and safety margin to realize path planning of underground loaders.

2. The underground scraper path planning method according to claim 1, characterized in that The step S1 specifically includes the following steps: Collect environmental map data information of the target underground mine tunnel; use the SLAM method to construct the environmental map of the target underground mine tunnel.

3. The underground scraper path planning method according to claim 2, characterized in that The step S2 specifically includes the following steps: The environmental map obtained in step S1 is smoothed to smooth the irregular contours of the tunnel boundaries, thereby obtaining a binary raster map of the target underground mine tunnel.

4. The underground scraper path planning method according to claim 3, characterized in that The step S3 specifically includes the following steps: Based on the binary grid map obtained in step S2, a generalized Voronoi diagram is constructed to extract the centerline skeleton of the free space; Use pruning algorithm to remove redundant branches and obtain the corresponding Voronoi node set ,in is the set of obstacles in the map; After processing, the centerline skeleton only has intersection points in the intersection area of ​​the lanes; At each intersection is the center of the circle, , with a set safety margin With as radius, a circular area is defined as the intersection area of ​​the lanes.

5. The underground scraper path planning method according to claim 4, characterized in that The step S4 specifically includes the following steps: Set the starting position of the center hinge point of the scraper and end position ,in is the global horizontal coordinate of the starting position of the scraper, is the global vertical coordinate of the starting position of the scraper, is the front vehicle heading angle of the starting position of the scraper, is the global horizontal coordinate of the end position of the scraper, is the global vertical coordinate of the end position of the scraper, The front vehicle heading angle of the scraper's terminal position; Select the node closest to the starting point in the centerline skeleton network and the node closest to the end point ,use The algorithm generates an initial guidance path on the centerline skeleton ; Along Perform equidistant discrete sampling, delete the nodes in the intersection area, and divide the remaining nodes into n curve segments The reference state set .

6. The underground scraper path planning method according to claim 5, characterized in that The step S5 comprises the following steps: A. Start from the starting node and use the starting node as the base node for the current expansion; B. According to the specific location of the basic node, based on the regional dynamic node expansion strategy, calculate the adjacent node set of the basic node C. Construct a multi-objective heuristic cost function based on path length, heading change, articulation angle deviation, and safety margin; D. Calculate the cost function value of each adjacent node in the adjacent node set, and determine the next base node for current expansion based on the obtained cost function value; E. Return to step B for the next round of iterative calculation until all nodes have completed iterative calculations. F. Finally, the path planning result of the underground loader is obtained.

7. The underground scraper path planning method according to claim 6, characterized in that The step S5 specifically includes the following steps: (1) Create an open list OpenSet and a closed list CloseSet, and initialize them to empty lists respectively; the open list OpenSet is used to store the nodes to be checked, and the closed list CloseSet is used to store the nodes that have been checked; put the starting node S into the open list OpenSet; (2) Select the node with the smallest cost function value in the open list OpenSet as the basic node for the current expansion , and put the selected nodes into the closed list CloseSet; (3) If the basic node The distance to the end point G is less than the threshold , then a bidirectional Dubins curve is used to connect the basic nodes and the end point G, and perform collision detection on the bidirectional Dubins curve: If the bidirectional Dubins curve passes the collision detection, then backtrack the path from the end point, and the path planning task is completed; If the bidirectional Dubins curve fails the collision test, proceed to the next step; (4) Node forward expansion stage: (4-1) If the basic node Located in a closed tunnel area and not belonging to the reference state set For any curve segment in the reference state set Select the distance base node Nearest curve segment ,exist Find the distance base node on The nearest point ; From point Start along the curve Traverse both sides of the base node one by one The distance is less than the set value point ; At this time, if you click Ability to pass collision-free Dubins curves and base nodes If it is connected, stop traversal and add the point to the adjacent node set ; For point and ,like Then set the expansion direction to positive sequence, Then set the expansion direction to negative order; if there is no Dubins curve that can pass through without collision and the basic node Connect the dots , then jump to step (4-4); (4-2) If the basic node Located in the intersection area and does not belong to the reference state set Any curve segment in the intersection area Intersecting curve segments , from the curve segment The endpoint closer to the junction Start by going through the distance endpoints one by one Less than the set value point If there is a point Ability to pass collision-free Dubins curves and base nodes Connect, then stop traversal and point Join the adjacent node set ; For point and ,like Then set the expansion direction to positive sequence, Then set the expansion direction to negative order; if there is no Dubins curve that can pass through without collision and the basic node Connect the dots , then jump to step (4-4); (4-3) If the basic node Located in the reference state set For any curve segment in , the next adjacent node is added to the adjacent node set according to the expansion direction. ; (4-4) If the basic node Neither in the reference state set On any curve segment in the , there is no way to pass the collision-free Dubins curve and the reference state set If any curve segment in the The algorithm's node forward expansion method generates nodes based on the kinematic model and discrete control quantities; (5) Calculation of cost value: (5-1) For the set of adjacent nodes Any node in , use the following formula to calculate the corresponding cumulative cost : In the formula As a basic node the cumulative cost of is the displacement penalty term, and , is the set parameter value, and , To calculate and The Euclidean distance of is the steering dynamics penalty term, is the set special dynamics penalty coefficient, is the hinge angle increment, and , is the articulation angle of the underground scraper at the adjacent node, is the articulation angle of the underground LHD at the foundation node; is the hinge angle steady-state penalty term, is the set steady-state penalty coefficient of the articulation angle, is the hinge angle; is a reversing penalty item, and , is the velocity vector of the base node, is the velocity vector of the adjacent node, The number of reversing penalties set; is the safety distance penalty term, is the set safety distance penalty coefficient, is the safety distance margin from the adjacent node to the nearest obstacle; (5-2) For the set of adjacent nodes Any node in , use the following formula to calculate the corresponding heuristic cost : In the formula is the Euclidean distance cost term; is a non-constrained cost term, indicating that it is calculated using the Dubins curve The length of the shortest feasible path to G; (5-3) Calculate the adjacent node set Any node in The cost function value for ; (6) If the node If the node is not in the open list OpenSet, Put it into the open list OpenSet; If the node Already in the open list OpenSet, and the cost function value If the cost function value is less than the original one, the node is updated. The cost function value of the node The parent node is the base node If the node Already in the open list OpenSet, and the cost function value If it is greater than or equal to the original cost function value, no update will be performed; If the node If the node is already in the closed list CloseSet, skip the node ; (7) Return to step (2) to perform the next round of iterative calculation until all nodes have completed the iterative calculation; (8) Finally, the path planning result of the underground scraper is obtained.

8. The underground scraper path planning method according to claim 7, characterized in that Step (4-4) described in the mixed The algorithm's node forward expansion method generates nodes based on the kinematic model and discrete control quantities, specifically including the following steps: Select the central hinge point of the underground scraper as the reference point and set the linear speed. and discrete articulation angular rates As control input, discrete expansion nodes are generated by integrating the kinematic equations, and the expansion nodes that pass the collision detection are added to the adjacent node set. ; The kinematic model of the underground LHD is expressed as In the formula X-axis coordinate of the underground scraper reference point The partial derivative, Y-axis coordinate of the underground scraper reference point The partial derivative, The current heading angle of the underground LHD The partial derivative, is the articulation angular velocity, is the central hinge angle, is the heading angle of the front vehicle in the global coordinate system, is the distance from the front axle to the central hinge point, is the distance from the rear axle to the central hinge point, v is the current speed of the underground LHD; by is the state variable, For control input.

9. A system for implementing the underground scraper path planning method according to any one of claims 1 to 8, characterized in that It includes a data acquisition module, a data processing module, an extraction and division module, a path guidance module and a path planning module; the data acquisition module, the data processing module, the extraction and division module, the path guidance module and the path planning module are connected in series in sequence; the data acquisition module is used to obtain the environmental data information of the target underground mine tunnel, and construct the environmental map of the target underground mine tunnel, and upload the data information to the data processing module; The data processing module is used to pre-process the obtained environmental map based on the received data information, obtain a binary raster map of the target underground mine tunnel, and upload the data information to the extraction and division module; The extraction and division module is used to construct a generalized Voronoi diagram based on the received data information and the obtained binary raster map, extract the lane centerline and perform regional division, and upload the data information to the path guidance module; The path guidance module is used to use the received data information and the obtained center line and area division results. The algorithm guides and segments the path, obtains the reference state set of the target underground mine tunnel, and uploads the data information to the path planning module; the path planning module is used to calculate the path based on the received data information and the reference state set. An algorithm and a regional dynamic node expansion strategy are proposed, and a multi-objective heuristic cost function is constructed by combining path length, heading change, articulation angle deviation and safety margin to realize path planning of underground loaders.

Images