Underground shovel path planning method and system

Abstract

The application discloses a kind of underground shovel path planning method and system, including obtaining the environment data information of target underground mine roadway, constructs environment map and is preprocessed to obtain binary raster map;Generalized Voronoi diagram is constructed, roadway center line is extracted and is divided into regions;Path guidance is carried out using algorithm and is segmented, and the reference state set of target underground mine roadway is obtained;Based on hybrid algorithm, regional dynamic node expansion strategy and multi-objective heuristic cost function, the path planning of underground shovel is realized, and the path planning of underground shovel is realized.The application obtains the roadway center line and divides region by the data acquisition and data processing of target underground mine roadway, then path guidance is carried out by algorithm, and hybrid algorithm is optimized based on regional dynamic node expansion strategy and multi-objective heuristic cost function, then combined with hybrid algorithm, Dubins curve, collision detection scheme, path smoothness penalty and path distance penalty, not only realize the path planning of underground shovel, but also higher reliability, better accuracy, and better effect.

Description

Technical Field

[0001] This invention belongs to the field of intelligent mining technology, specifically relating to a path planning method and system for underground loaders. Background Technology

[0002] To ensure personnel safety, the autonomous driving technology of underground loaders in the mining industry has developed rapidly. Path planning algorithms are a crucial part of the autonomous driving technology field, directly affecting the navigation performance and operational efficiency of underground loaders during autonomous operation. Therefore, research on path planning schemes for underground loaders has always been a key focus of research on the unmanned transformation of heavy-duty underground vehicles.

[0003] Currently, common path planning schemes for underground loader operators include search-based path planning schemes, sampling-based path planning schemes, and artificial intelligence-based path planning schemes. Search-based path planning schemes include, for example, Dijkstra's algorithm and... Algorithms such as [specific algorithms] are the most widely used because the generated paths possess determinism and global optimality; however, these schemes suffer from huge computational overhead, low search efficiency, and insufficient real-time performance. Sampling-based path planning schemes, such as PRM and RRT algorithms, have higher search efficiency in high-dimensional search spaces and complex environments; however, their collision detection mechanisms are relatively complex, making them prone to getting trapped in local optima during the search process, and the generated paths often have poor smoothness. Artificial intelligence-based path planning schemes, such as deep learning and reinforcement learning schemes, autonomously learn path optimization strategies based on large-scale environmental perception data, possessing stronger environmental adaptability and path diversity control capabilities; however, the practical application of these schemes is still limited by high-quality training data and high computational resource requirements. Summary of the Invention

[0004] One of the objectives of this invention is to provide a reliable, accurate, and effective method for planning the path of underground scrapers.

[0005] The second objective of this invention is to provide a system for implementing the underground scraper path planning method.

[0006] The underground loader path planning method provided by this invention includes the following steps:

[0007] S1. Obtain environmental data information of the target underground mine tunnels and construct an environmental map of the target underground mine tunnels;

[0008] S2. Preprocess 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 Let x be the global x-coordinate of the starting position of the loader. The global ordinate of the starting position of the loader. The heading angle of the front body of the loader at its starting position. Let x be the global x-coordinate of the end position of the loader. The global ordinate of the loader's final pose. The heading angle of the front body of the loader at the final position;

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

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

[0025] Step S5 includes the following steps:

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

[0027] B. Based on the specific location of the basic node, and using a regional dynamic node expansion strategy, calculate the set of adjacent nodes of the basic 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 neighboring node in the set of neighboring nodes, and determine the next base node for current expansion based on the obtained cost function value;

[0030] E. Return to step B to perform the next round of iterative calculations until all nodes have been iteratively calculated;

[0031] F. Finally, the path planning results for the underground loader are obtained.

[0032] Step S5 specifically includes the following steps:

[0033] (1) Create an open list OpenSet and a closed list CloseSet, and initialize them as 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 OpenSet as the base node for the current expansion. And add the selected nodes to the CloseSet list;

[0035] (3) If the basic node The distance to the endpoint G is less than the threshold. Then, a bidirectional Dubins curve is used to connect the basic nodes. Find the endpoint G and perform collision detection on the bidirectional Dubins curve: if the bidirectional Dubins curve passes the collision detection, backtrack from the endpoint, and the path planning task is completed; if the bidirectional Dubins curve fails the collision detection, continue to the subsequent steps.

[0036] (4) Node forward expansion phase:

[0037] (4-1) If the basic node Located in a closed tunnel area and not belonging to the reference state set Any curve segment in the reference state set, then Select distance from base node The most recent curve segment ,exist Find the distance from the base node. nearest point From point Start, along the curve segment Traverse both sides and the basic nodes one by one The distance is less than the set value point At this point, if point Able to connect to the base node via a collision-free Dubins curve If a connection is established, the traversal stops, and the node is added to the set of adjacent nodes. For points and ,like Then set the expansion direction to positive order. Then set the expansion direction to negative order; if there is no collision-free Dubins curve that can pass through the base node... Connecting points If so, then skip to step (4-4).

[0038] (4-2) If the basic node Located in the intersection area And does not belong to the set of reference states Any curve segment in the equation will traverse the intersection region. Intersecting curve segments From the curve segment Endpoints closer to the junction Begin by traversing the distances from the endpoints one by one. Less than the set value point If a point exists Able to connect to the base node via a collision-free Dubins curve If a connection is made, the traversal stops, and the points are connected. Add to the set of adjacent nodes For points and ,like Then set the expansion direction to positive order. Then set the expansion direction to negative order; if there is no collision-free Dubins curve that can pass through the base node... Connecting points If so, then skip to step (4-4).

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

[0040] (4-4) If the basic node Not in the set of reference states On any curve segment in the equation, it is also impossible to pass through a collision-free Dubins curve and the reference state set. For any curve segment connection in the code, a hybrid approach is used. The algorithm's node forward expansion method generates nodes based on kinematic models and discrete control variables;

[0041] (5) Value calculation:

[0042] (5-1) For the set of adjacent nodes any node in The corresponding cumulative cost is calculated using the following formula. :

[0043]

[0044] In the formula Basic Node The cumulative cost; This is a displacement penalty term, and , The set parameter value, and , For calculation and Euclidean distance; As a penalty term for steering dynamics, The specific dynamic penalty coefficient is set. For the hinge angle increment, and , The hinge angle of the underground scraper at the adjacent node. The hinge angle of the underground scraper at the foundation node; This is the steady-state penalty term for the hinge angle. The set hinge angle steady-state penalty coefficient, It is the hinge angle; It is a penalty item for reversing, and , The velocity vector of the basic node, The velocity vector of the adjacent nodes. The set number of times a vehicle is penalized for reversing; For safe distance penalties, The set safety distance penalty coefficient, This represents the safe distance margin from adjacent nodes to the nearest obstacle.

[0045] (5-2) For the set of adjacent nodes any node in The heuristic cost is calculated using the following formula. :

[0046]

[0047] In the formula This is the cost term for Euclidean distance; For unconstrained cost terms, it indicates that the cost is calculated using the Dubins curve. The length of the shortest feasible path to G;

[0048] (5-3) Calculate the set of adjacent nodes any node in Cost function value for ;

[0049] (6) If node If the node is not in the OpenSet list, then... Add to the OpenSet list;

[0050] If node It is already in the OpenSet list, and the cost function value is... If the cost function value is less than the original value, then update the node. The cost function value, and setting the node. The parent node is the base node If node It is already in the OpenSet list, and the cost function value is... If the cost function value is greater than or equal to the original value, no update will be performed;

[0051] If node If a node is already in the closed list CloseSet, skip that node. ;

[0052] (7) Return to step (2) to perform the next round of iterative calculation until all nodes have been iteratively calculated;

[0053] (8) Finally, the path planning results of the underground loader are obtained.

[0054] Step (4-4) describes the use of a hybrid The algorithm's node forward expansion method, based on the kinematic model and discrete control variables, generates nodes and specifically includes the following steps:

[0055] The central articulation point of the underground loader is selected as the reference point, and a set linear velocity is used. and discrete hinge angular rate As control input, discrete extended nodes are generated using integral kinematic equations, and extended nodes that pass collision detection are added to the set of adjacent nodes. ;

[0056] The kinematic model of the underground loader is represented as follows:

[0057]

[0058] In the formula X-axis coordinates of the underground loader reference point The partial derivative, Y-axis coordinate of the underground loader reference point The partial derivative, The current heading angle of the underground loader. The partial derivative, For hinge angular velocity, The central hinge angle, The heading angle of the front vehicle body in the global coordinate system. This is the distance from the front axle to the central hinge point. v is the distance from the rear axle to the central articulation point, and v is the current travel speed of the underground loader.

[0059] with as a state variable, and as a control input.

[0060] 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 in series; 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, obtaining 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.

[0061] The underground shovel path planning method and system provided by the application can extract a tunnel center line and divided regions through data acquisition and data processing of a target underground mine tunnel, perform path guiding through a algorithm, optimize a hybrid algorithm based on a regional dynamic node expansion strategy and a multi-objective heuristic cost function, and further combine a hybrid algorithm, a regional dynamic node expansion strategy and a multi-objective heuristic cost function, so as to not only realize path planning of the underground shovel, but also have higher reliability, better accuracy and better effect. BRIEF DESCRIPTION OF DRAWINGS

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

[0063] 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 tunnel skeleton diagram after one iteration of pruning algorithm processing, Figure 2 (c) is a tunnel centerline skeleton diagram after pruning algorithm processing.

[0064] Figure 3 It is a generation reference state set diagram of the method of the application.

[0065] Figure 4 It is a closed tunnel expansion strategy diagram of the method of the application.

[0066] Figure 5 It is a junction area expansion strategy diagram of the method of the application.

[0067] Figure 6 It is a shovel kinematic model diagram of the method of the application.

[0068] Figure 7 It is a single tunnel scene diagram of an embodiment of the method of the application.

[0069] Figure 8 It is a multi-branch tunnel scene diagram of an embodiment of the method of the application.

[0070] Figure 9 It is a grid-shaped tunnel scene diagram of an embodiment of the method of the application.

[0071] Figure 10 It is a path length comparison diagram of an embodiment of the method of the application; wherein, Figure 10 (a) is a single tunnel path length comparison diagram, Figure 10 (b) is a multi-branch tunnel path length comparison diagram, Figure 10 (c) is a grid-shaped tunnel path length comparison diagram.

[0072] Figure 11 It is a node number comparison diagram of an embodiment of the method of the application; wherein, Figure 11 (a) is a single tunnel node number comparison diagram, Figure 11 (b) is a multi-branch tunnel node number comparison diagram, Figure 11 (c) is a grid-shaped tunnel node number comparison diagram.

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

[0074] Figure 13 Multi-branch lane scene node expansion visualization schematic diagram of the method embodiment of the application; wherein, Figure 13 (a) is the traditional mixed Multi-branch lane scene node expansion visualization schematic diagram of the algorithm; Figure 13 (b) is the two-way mixed Multi-branch lane scene node expansion visualization schematic diagram of the algorithm; Figure 13 (c) is the multi-branch lane scene node expansion visualization schematic diagram of the application.

[0075] Figure 14 Grid lane scene node expansion visualization schematic diagram of the method embodiment of the application; wherein, Figure 14 (a) is the traditional mixed Grid lane scene node expansion visualization schematic diagram of the algorithm; Figure 14 (b) is the two-way mixed Grid lane scene node expansion visualization schematic diagram of the algorithm; Figure 14 (c) is the grid lane scene node expansion visualization schematic diagram of the application.

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

[0077] Figure 16 Functional module schematic diagram of the system of the application. DETAILED DESCRIPTION

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

[0079] 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:

[0080] Collect the 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.

[0081] 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:

[0082] 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.

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

[0084] 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;

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

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

[0087] 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.

[0088] S4. According to the center line and area division result obtained in step S3, the algorithm is used 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

[0089] The start pose and the end pose of the center articulation point of the shovel-truck are set, 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 body heading angle of the shovel end position;

[0090] The node closest to the start point in the centerline skeleton network is selected And the node closest to the end point , the initial guide path is generated on the centerline skeleton by using Algorithm ;

[0091] Equal-distance discrete sampling is performed along , the nodes located in the intersection area are deleted, and the remaining nodes are divided into a reference state set including n curve segments .

[0092] S5. According to the reference state set obtained in step S4, a multi-objective heuristic cost function is constructed based on the hybrid Algorithm and the regional dynamic node expansion strategy, combined with the path length, heading change, hinge angle deviation and safety margin, to realize the rapid, safe and high-quality path planning of the underground shovel; including the following steps:

[0093] A. Starting from the starting node and taking the starting node as the current expansion base node;

[0094] B. According to the specific position of the base node, the set of adjacent nodes of the base node is calculated based on the regional dynamic node expansion strategy

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

[0096] D. Calculate the cost function value of each adjacent node in the adjacent node set, and determine the next current expansion base node according to the obtained cost function value;

[0097] E. Return to step B for the next round of iteration calculation until all nodes are iteratively calculated;

[0098] F. Finally, the path planning result of the underground shovel is obtained.

[0099] In specific implementation, the following steps can be used:

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

[0101] ​(2) Select the node with the smallest cost function value in the OpenSet as the base node for the current expansion. And add the selected nodes to the CloseSet list;

[0102] (3) If the basic node The distance to the endpoint G is less than the threshold. Then, a bidirectional Dubins curve is used to connect the basic nodes. Find the endpoint G and perform collision detection on the bidirectional Dubins curve: if the bidirectional Dubins curve passes the collision detection, backtrack from the endpoint, and the path planning task is completed; if the bidirectional Dubins curve fails the collision detection, continue to the subsequent steps.

[0103] (4) Node forward expansion phase:

[0104] (4-1) If the basic node Located in a closed tunnel area and not belonging to the reference state set Any curve segment in the reference state set, then Select distance from base node The most recent curve segment ,exist Find the distance from the base node. nearest point From point Start, along the curve segment Traverse both sides and the basic nodes one by one The distance is less than the set value point At this point, if point Able to connect to the base node via a collision-free Dubins curve If a connection is established, the traversal stops, and the node is added to the set of adjacent nodes. For points and ,like Then set the expansion direction to positive order. Then set the expansion direction to negative order; if there is no collision-free Dubins curve that can pass through the base node... Connecting points If so, then skip to step (4-4).

[0105] (4-2) If the basic node Located in the intersection area And does not belong to the set of reference states Any curve segment in the equation will traverse the intersection region. Intersecting curve segments From the curve segment Endpoints closer to the junction Begin by traversing the distances from the endpoints one by one. Less than the set value point If a point exists Able to connect to the base node via a collision-free Dubins curve If a connection is made, the traversal stops, and the points are connected. Add to the set of adjacent nodes For points and ,like Then set the expansion direction to positive order. Then set the expansion direction to negative order; if there is no collision-free Dubins curve that can pass through the base node... Connecting points If so, then skip to step (4-4).

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

[0107] (4-4) If the basic node Not in the set of reference states On any curve segment in the equation, it is also impossible to pass through a collision-free Dubins curve and the reference state set. For any curve segment connection in the code, a hybrid approach is used. The algorithm's node forward expansion method generates nodes based on kinematic models and discrete control variables; specifically, it includes the following steps:

[0108] The central articulation point of the underground loader is selected as the reference point, and a set linear velocity is used. and discrete hinge angular rate As control input, discrete extended nodes are generated using integral kinematic equations, and extended nodes that pass collision detection are added to the set of adjacent nodes. ;

[0109] The kinematic model of the underground loader is represented as follows:

[0110]

[0111] In the formula X-axis coordinates of the underground loader reference point The partial derivative, Y-axis coordinate of the underground loader reference point The partial derivative, The current heading angle of the underground loader. The partial derivative, For hinge angular velocity, The central hinge angle, The heading angle of the front vehicle body in the global coordinate system. This is the distance from the front axle to the central hinge point. v is the distance from the rear axle to the central articulation point, and v is the current travel speed of the underground loader.

[0112] by As a state variable, with For control input;

[0113] (5) Value calculation:

[0114] (5-1) For the set of adjacent nodes any node in The corresponding cumulative cost is calculated using the following formula. :

[0115]

[0116] In the formula Basic Node The cumulative cost; This is a displacement penalty term, and , The set parameter value, and , For calculation and Euclidean distance; As a steering dynamics penalty term, The specific dynamic penalty coefficient is set. For the hinge angle increment, and , The hinge angle of the underground scraper at the adjacent node. The hinge angle of the underground scraper at the foundation node; This is the steady-state penalty term for the hinge angle. The set hinge angle steady-state penalty coefficient, The hinge angle; It is a penalty item for reversing, and , The velocity vector of the basic node, Let the velocity vectors of the adjacent nodes be _____. The set number of times a vehicle is penalized for reversing; For safe distance penalties, The set safety distance penalty coefficient, This represents the safe distance margin from adjacent nodes to the nearest obstacle.

[0117] (5-2) Calculate the cost of any node in the neighbor node set , using the following formula :

[0118]

[0119] wherein is the Euclidean distance cost term; is the non-constrained based cost term, representing the length of the shortest feasible path from to G calculated using Dubins curve;

[0120] (5-3) Calculate the cost function value of any node in the neighbor node set ;

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

[0122] If node is already in the open list OpenSet, and the cost function value is less than the original cost function value, update the cost function value of node , and set the parent node of node 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 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 will be made;

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

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

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

[0126] The above scheme is a traditional hybrid A* algorithm ​​Based on the algorithm, the set sub-region dynamic node expansion strategy and multi-objective heuristic cost function are optimized, which 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.

[0127] The method of the application significantly improves the search efficiency: by constructing a reference state set based on the center line of the tunnel to generate a guide, the generation of redundant nodes can be effectively suppressed; in a large-scale tunnel map, the number of expanded 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, the search efficiency is significantly improved, and can be applied to scenes with higher requirements for real-time search.

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

[0129] The cost function designed by the method of the application considers the change of the vehicle steering angle, the continuity of the heading and the curvature control, suppresses the large-amplitude articulated angle steering and frequent direction switching, reduces the curvature mutation phenomenon in the path, and generates a smoother and more continuous overall path, which is beneficial to the stable path tracking of the controller and improves the smoothness and controllability of the vehicle driving.

[0130] The method of the application considers the global feasibility and adaptability to complex environments: since the method of the application adopts a sub-region dynamic node expansion strategy, the center line is used as a guide in a closed environment, and discrete nodes are generated based on the kinematic model in the intersection area; this mechanism considers the global optimality and local feasibility of the path, effectively avoids falling into local optimality, and improves the adaptability to complex underground environments.

[0131] The following embodiments are used to further illustrate the method of the application:

[0132] 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:

[0133]

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

[0135] A single tunnel scene (as shown in FIG. 2a): a long straight tunnel structure without branches, the expansion efficiency of the test algorithm in a regular environment; Figure 7 A multi-branch tunnel scene (as shown in FIG. 2b): containing T-shaped and Y-shaped intersections, evaluating the path feasibility and navigation flexibility of the algorithm in the cross-topology area;

[0136] Figure 8 A grid-shaped tunnel scene (as shown in FIG. 2c): simulating a large-scale dense intersection tunnel system, testing the scalability and stability of the algorithm.

[0137] The two types of comparison methods used in the embodiment experiment include: Figure 9 The traditional hybrid

[0138] algorithm is adapted to the motion constraints of the articulated vehicle and has the same cost function as the present application;

[0139] The bidirectional hybrid algorithm introduces a bidirectional search mechanism for optimization based on the traditional hybrid

[0140] algorithm. In practical applications, a shorter path length can significantly shorten the running time of the vehicle and reduce energy consumption, as shown in FIGS. 3a and 3b, in single tunnel and multi-branch tunnel, due to the relatively fixed road sections, the three methods can generate paths with lengths close to each other in different tunnel scenes, with a path length difference of less than 2m; in the grid-shaped tunnel, the road sections of the paths generated by the three algorithms differ significantly; as shown in FIG. 3c, the algorithm proposed in the present application shortens the path length by more than 12m compared to the traditional hybrid algorithm and the bidirectional hybrid

[0141] algorithm, showing the optimality of the path. Figure 10 Figure 10 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 the related algorithms under different scene and endpoint combinations (as shown in FIG. 4), it can be seen that in all test scenes: the number of expanded nodes of the traditional hybrid Figure 10 algorithm will increase exponentially with the increase of environmental complexity, such as from 14 to 9463 in a single tunnel, and soaring to 287522 in a grid-shaped tunnel scene; the bidirectional hybrid algorithm can effectively control the number of expanded nodes, but the path length is longer than that of the present application.

[0142] Figure 11 ​​​​​​The algorithm adopts a bidirectional search mechanism to effectively alleviate the redundant expansion phenomenon, but in some complex scenes (multi-branch tunnel environment 7-12 endpoint combination and grid-shaped tunnel environment), the node number increases significantly; and the proposed algorithm shows lower expansion node number in all scenes , and almost no significant growth with the increase of environment complexity.

[0143] Through repeated experiments of 20 search times of the traditional hybrid algorithm, the bidirectional hybrid algorithm and the algorithm proposed in the present application in actual scenes, the experimental results are recorded in the statistical table of average search time as shown in Table 2:

[0144]

[0145] The results show that the search time of the traditional hybrid algorithm increases exponentially with the increase of search space complexity and reaches several seconds; the bidirectional hybrid algorithm improves significantly, and the search time increases more slowly with the increase of search space complexity, but increases significantly in complex multi-branch tunnel and grid-shaped tunnel scenes, and there are significant outliers; the improved algorithm proposed in the present application always maintains a relatively low search time (less than 1 second) , and the time consumption increases smoothly with the increase of environment complexity, showing the best real-time performance; in the table, the traditional hybrid algorithm is abbreviated as HA, the bidirectional hybrid algorithm is abbreviated as BHA, and the improved planning algorithm proposed in the present application is called HHA.

[0146] Through visualization of the node expansion process in a typical path (as shown in Figure 12~Figure 14 ), it can be known that the traditional hybrid algorithm presents a large range of redundant expansion nodes in all scenes; the expansion nodes of the bidirectional hybrid algorithm are relatively convergent, but there is still a significant redundant expansion phenomenon; compared with the above, the expansion nodes of the improved algorithm proposed in the present application are almost completely distributed along the reference state set, effectively avoiding the invalid expansion of nodes in complex environments.

[0147] The safety margin refers to the minimum interval between the nodes on the path and the environment obstacles. Since the underground shovel travels in the tunnel, the maximum distance between the vehicle body and the two side walls is often less than 1 m, and any slight approach may induce a potential collision accident. By comparing the average safety margin of the paths generated by the three path planning algorithms in different tunnel scenes (as shown in Figure 15 ), it can be known that the average safety margin of the algorithm proposed in the present application is significantly higher than that of the traditional hybrid and bidirectional hybrid algorithm; compared with the worst bidirectional hybrid algorithm; compared with the worst bidirectional hybrid; can provide a safety redundancy of 0.3m or more; indicates that the method of the present application can provide a higher safety margin control for the underground shovel, thereby reducing the risk of collision in a confined space.

[0148] As Figure 16 The functional module schematic diagram of the system of the present application is shown: the system for implementing the underground shovel path planning method disclosed in the present 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, 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 regional division result, and the mixed algorithm, obtaining 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 the mixed algorithm and the regional dynamic node expansion strategy, combining the path length, the heading change, the articulation angle deviation and the safety margin, and realizing the fast, safe and high-quality path planning of the underground shovel.

Claims

1. A path planning method for underground loaders, characterized in that... Includes the following steps: S1. Obtain environmental data information of the target underground mine tunnels and construct an environmental map of the target underground mine tunnels; S2. Preprocess the environmental map obtained in step S1 to obtain a binary raster map of the target underground mine tunnel; S3. Based on the binary raster map obtained in step S2, construct a generalized Voronoi diagram, extract the center line of the alleyway, and divide the region. S4. Based on the centerline and region division results obtained in step S3, use... The algorithm guides the path and segments it to obtain a reference state set of the target underground mine tunnel; S5. Based on the reference state set obtained in step S4, and using the hybrid... 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 realize the path planning of underground loaders.

2. The underground loader path planning method according to claim 1, characterized in that... Step S1 specifically includes the following steps: Collect environmental map data of the target underground mine tunnels; construct the environmental map of the target underground mine tunnels using the SLAM method.

3. The underground loader path planning method according to claim 2, characterized in that... 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 binarized raster map of the target underground mine tunnel.

4. The underground loader path planning method according to claim 3, characterized in that... Step S3 specifically includes the following steps: Based on the binary raster map obtained in step S2, a generalized Voronoi diagram is constructed, and the centerline skeleton of free space is extracted. The pruning algorithm is used to remove redundant branches, resulting in the corresponding Voronoi node set. ,in The set of obstacles in the map; The processed centerline skeleton only shows intersection points in the intersection area of ​​the roadways; With each intersection point With the center of the circle, With the set safety margin A circular area is defined by radius as the intersection area of ​​the lanes.

5. The underground loader path planning method according to claim 4, characterized in that... Step S4 specifically includes the following steps: Set the starting position of the center hinge point of the loader and final pose ,in Let x be the global x-coordinate of the starting position of the loader. The global ordinate of the starting position of the loader. The heading angle of the front body of the loader at its starting position. Let x be the global x-coordinate of the end position of the loader. The global ordinate of the loader's final pose. The heading angle of the front body of the loader at the final position; Select the node closest to the starting point in the centerline skeleton network. and the node closest to the destination ,use The algorithm generates an initial guide path on the centerline skeleton. ; Along Perform equidistant discrete sampling, delete nodes located within the intersection region, and divide the remaining nodes into n curve segments. reference state set .

6. The underground loader path planning method according to claim 5, characterized in that... Step S5 includes the following steps: A. Start from the initial node and use the initial node as the base node for the current expansion; B. Based on the specific location of the base node, and using a region-based dynamic node expansion strategy, calculate the set of adjacent nodes of the base 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 neighboring node in the set of neighboring nodes, and determine the next base node for current expansion based on the obtained cost function value; E. Return to step B to perform the next round of iterative calculations until all nodes have been iteratively calculated; F. Finally, the path planning results for the underground loader are obtained.

7. The underground loader path planning method according to claim 6, characterized in that... Step S5 specifically includes the following steps: (1) Create an open list OpenSet and a closed list CloseSet, and initialize them as 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 OpenSet as the base node for the current expansion. And add the selected nodes to the CloseSet list; (3) If the basic node The distance to the endpoint G is less than the threshold. Then, a bidirectional Dubins curve is used to connect the basic nodes. Find the endpoint G and perform collision detection on the bidirectional Dubins curve: if the bidirectional Dubins curve passes the collision detection, backtrack the path from the endpoint, and the path planning task is completed; If the bidirectional Dubins curve fails the collision detection, proceed to the next step; (4) Node forward expansion phase: (4-1) If the basic node Located in a closed tunnel area and not belonging to the reference state set Any curve segment in the reference state set is then... Select distance from base node The most recent curve segment ,exist Find the distance from the base node nearest point From point Start, along the curve segment Traverse both sides and the basic nodes one by one The distance is less than the set value point At this point, if point Able to connect to the base node via a collision-free Dubins curve If a connection is established, the traversal stops, and the node is added to the set of adjacent nodes. For points and ,like Then set the expansion direction to positive order. Then set the expansion direction to negative order; if there is no collision-free Dubins curve that can pass through the base node... Connecting points If so, then skip 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 curve will traverse the intersection region. Intersecting curve segments From the curve segment Endpoints closer to the junction Begin by traversing the distances from the endpoints one by one. Less than the set value point If a point exists Able to connect to the base node via a collision-free Dubins curve If a connection is made, the traversal stops, and the points are connected. Add to the set of adjacent nodes ; For point and ,like Then set the expansion direction to positive order. Then set the expansion direction to negative order; if there is no collision-free Dubins curve that can pass through the base node... Connecting points If so, then skip to step (4-4). (4-3) If the basic node Located in the reference state set For any curve segment in the equation, the next adjacent node is added to the set of adjacent nodes according to the direction of expansion. ; (4-4) If the basic node Not in the set of reference states On any curve segment in the equation, it is also impossible to pass through a collision-free Dubins curve and the reference state set. For any curve segment connection in the code, a hybrid approach is used. The algorithm's node forward expansion method generates nodes based on kinematic models and discrete control variables; (5) Value calculation: (5-1) For the set of adjacent nodes any node in The corresponding cumulative cost is calculated using the following formula. : In the formula Basic Node The cumulative cost; This is a displacement penalty term, and , The set parameter value, and , For calculation and Euclidean distance; As a steering dynamics penalty term, The specific dynamic penalty coefficient is set. For the hinge angle increment, and , The hinge angle of the underground scraper at the adjacent node. The hinge angle of the underground scraper at the foundation node; This is the steady-state penalty term for the hinge angle. The set hinge angle steady-state penalty coefficient, The hinge angle; It is a penalty item for reversing, and , The velocity vector of the basic node, Let the velocity vectors of the adjacent nodes be _____. The set number of times a vehicle is penalized for reversing; For safe distance penalties, The set safety distance penalty coefficient, This represents the safe distance margin from adjacent nodes to the nearest obstacle. (5-2) For the set of adjacent nodes any node in The heuristic cost is calculated using the following formula. : In the formula This is the cost term for Euclidean distance; For unconstrained cost terms, it indicates that the cost is calculated using the Dubins curve. The length of the shortest feasible path to G; (5-3) Calculate the set of adjacent nodes any node in Cost function value for ; (6) If node If the node is not in the OpenSet list, then... Add to the OpenSet list; If node It is already in the OpenSet list, and the cost function value is... If the cost function value is less than the original value, then update the node. The cost function value, and setting the node. The parent node is the base node If node It is already in the OpenSet list, and the cost function value is... If the cost function value is greater than or equal to the original value, no update will be performed; If node If a node is already in the closed list CloseSet, skip that node. ; (7) Return to step (2) to perform the next round of iterative calculation until all nodes have been iteratively calculated; (8) Finally, the path planning results of the underground loader are obtained.

8. The underground loader path planning method according to claim 7, characterized in that... Step (4-4) describes the use of a hybrid The algorithm's node forward expansion method, based on the kinematic model and discrete control variables, generates nodes and specifically includes the following steps: The central articulation point of the underground loader is selected as the reference point, and a set linear velocity is used. and discrete hinge angular rate As control input, discrete extended nodes are generated using integral kinematic equations, and extended nodes that pass collision detection are added to the set of adjacent nodes. ; The kinematic model of the underground loader is represented as follows: In the formula X-axis coordinates of the underground loader reference point The partial derivative, Y-axis coordinate of the underground loader reference point The partial derivative, The current heading angle of the underground loader. The partial derivative, For hinge angular velocity, The central hinge angle, The heading angle of the front vehicle body in the global coordinate system. This is the distance from the front axle to the central hinge point. v is the distance from the rear axle to the central articulation point, and v is the current travel speed of the underground loader. by As a state variable, with For controlling input.

9. A system for implementing the underground loader 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 segmentation module, a path guidance module, and a path planning module; the data acquisition module, data processing module, extraction and segmentation module, path guidance module, and path planning module are connected in series; the data acquisition module is used to acquire environmental data information of the target underground mine roadway, construct an environmental map of the target underground mine roadway, and upload the data information to the data processing module; The data processing module is used to preprocess the obtained environmental map based on the received data information to 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 and the obtained binary raster map, extract the center line of the alley and divide the area, and upload the data information to the path guidance module. The path guidance module is used to determine the path based on the received data and the obtained centerline and region division results. The algorithm guides and segments the path to obtain a reference state set for the target underground mine tunnel, and uploads this data to the path planning module. The path planning module then uses the received data and the obtained reference state set to perform a hybrid path planning algorithm. 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 realize the path planning of underground loaders.

Images