U-turn planning method for autonomous vehicles in narrow passages in underground mines

Abstract

This invention discloses a method for planning U-turns of automated vehicles in narrow passages in mines, comprising: S1, determining the starting area for U-turn planning; S2, performing tunnel area planning to generate interval navigation paths and local paths; S3, performing chamber area planning: the automated vehicle travels to the starting area for U-turn planning based on the tunnel area planning results, establishes a discrete map of the chamber planning area and sets a step size Sn, expands nodes until the target point is reached, and generates a collision-free path; S4, post-processing the collision-free path generated in step S3: a nonlinear optimization planning problem is modeled using a constructed objective function for the discrete points of the collision-free path, and a corresponding solver is used to obtain the final smooth path solution. This method can quickly and effectively complete U-turn trajectory planning in narrow areas using the chamber space, achieving relatively coherent U-turn actions, ensuring the real-time performance of the planning algorithm, and improving vehicle traffic efficiency.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving, and more particularly to a method for planning U-turns of autonomous vehicles in narrow passages in underground mines. Background Technology

[0002] Unlike autonomous driving in surface transportation, underground mine tunnels are narrow and have uneven road widths. Traditionally, vehicles making U-turns in mines are limited by tunnel conditions and road width, relying on the driver's subjective judgment of the U-turn location within the tunnel. This presents a significant challenge for autonomous vehicles making U-turns in mines. U-turns for autonomous vehicles underground in this scenario are complex, with a higher probability of planning failure.

[0003] A tunnel is a horizontal tunnel with a large cross-section and relatively short length that does not lead directly to the surface. Its function is to install various equipment and machinery, store materials and tools, or serve other specialized purposes. Compared to ordinary tunnels, tunnel construction has the following characteristics: 1. Tunnels have large and varied cross-sections, but are relatively short in length; 2. Tunnels are often connected to other tunnels, roadways, and shafts; 3. Tunnels have high engineering specifications and quality.

[0004] If autonomous vehicles can utilize underground mine chambers for autonomous U-turn planning, it will reduce planning difficulty, accelerate the planning speed, and improve the success rate of U-turn planning. Currently, there are no known methods for planning U-turns for autonomous vehicles in narrow underground passages using underground chambers. Summary of the Invention

[0005] This invention provides a method for planning U-turns of autonomous vehicles in narrow underground mine passages. Based on the narrow underground mine passages and utilizing the chambers to ensure a sufficiently large safety space, it solves the problem of U-turns for autonomous vehicles in narrow tunnel scenarios.

[0006] Therefore, the present invention adopts the following technical solution:

[0007] A method for planning U-turns for autonomous vehicles in narrow passages in underground mines includes the following steps:

[0008] S1, Determine the starting area for the U-turn plan: The autonomous vehicle travels along the original navigation route in the mine. When it receives a U-turn instruction, it searches for the nearest chamber in the tunnel based on the high-precision map and determines the starting area for the U-turn plan based on the layout of the tunnel and chamber.

[0009] S2, perform tunnel area planning and generate interval navigation path and local path: The autonomous vehicle determines its current position, takes the previous position and the above-mentioned U-turn planning starting area as the starting point and ending point of the route planning, respectively, and generates a global navigation path within this interval; then, based on the global navigation path, the projection point of the autonomous vehicle on the reference line is used as the coordinate origin to establish the Frenet coordinate system, and construct the planning space for the local planning during the process of driving to the U-turn planning starting area;

[0010] S3, Perform tunnel area planning: The autonomous vehicle travels to the starting area of ​​the U-turn planning based on the tunnel area planning results, establishes a discrete map of the tunnel planning area and sets a step size Sn, expands the nodes until the target point is reached, and generates a collision-free path.

[0011] S4. Post-process the collision-free path generated in step S3: construct the objective function, model the discrete points of the collision-free path using a nonlinear optimization programming problem, and use a corresponding solver to obtain the final smooth path solution.

[0012] In step S1, the starting area for the U-turn plan is located near the chamber area and narrow tunnel entrance within the mine, and is situated within a narrow tunnel.

[0013] Step S2 includes the following sub-steps:

[0014] S21, find the matching point of the autonomous vehicle's coordinates on the navigation reference line, and obtain the information of the corresponding coordinate point in the Frenet coordinate system according to the transformation relationship between the Cartesian coordinate system and the Frenet coordinate system.

[0015] S22 decouples the three-dimensional SLT problem in the Frenet coordinate system into two two-dimensional problems at the SL and ST levels. Based on the SL map and the projection information of static obstacles, a spatial discretization method combined with quadratic programming is used for path decision-making and planning. With the path results, an ST map is then built and dynamic obstacle projection is performed. A spatial discretization method combined with quadratic programming is used for velocity decision-making and planning. The final navigation path planning results and velocity planning results are merged to obtain the trajectory information.

[0016] The specific process of step S22 is as follows:

[0017] The autonomous vehicle establishes a Frenet coordinate system based on the navigation path, and then sequentially builds SL and ST graphs to construct the path planning and velocity planning spaces, respectively:

[0018]

[0019] A path decision planning method is performed based on path planning using static and low-speed obstacle projections and discretization of the state space. A speed decision planning method is also performed based on dynamic obstacle trajectory prediction and ST graph, with the state space discretized. A speed decision planning method is then performed based on dynamic obstacle trajectory prediction.

[0020] In the path planning in the SL graph and the velocity planning in the ST graph, different non-uniform sampling scales are set according to the tunnel scenario. First, the state space is discretized, and the cost value of each discrete point is allocated according to the cost function. An improved A* algorithm is used for heuristic search to quickly obtain an initial solution. The initial solution opens up a safety space as a decision scheme. Based on the safety space, the original problem is transformed into a convex optimization problem. Under constraints, a convex optimization solution method is used to obtain the optimal trajectory solution. In the numerical optimization process of velocity planning in the SL graph, the cost function is:

[0021]

[0022] Where w i Let l be the weight of each term in the cost function. i l centre These represent the vertical offsets of the path and reference line in the SL diagram, respectively; each term of the cost function represents the first derivative cost, second derivative cost, third derivative cost, lateral offset cost, and centerline cost, respectively, with a corresponding cost weight preceding each term. The constraints are as follows:

[0023] A eq x = b eq

[0024] Ax≤b

[0025] lb≤x≤ub

[0026] x=(l1,l′1,l″1,…l n ,l′ n ,l″ n ) T

[0027] In the formula, x is the state variable, and A eq ,b eq Used to describe state equality constraint equations, Ax≤b describes state inequality constraints, and lb,ub are the lower bound and upper bound of the range of values ​​for state x.

[0028] The corresponding constraints are continuity equality constraints, collision inequality constraints, and upper and lower bound constraints, respectively. These are obtained using a specific solver (such as OSQP, a commonly used open-source solver for the corresponding problem types in the industry), yielding l1, l′1, l″1, ... l n ,l′n ,l″ n With s1, s2, s3, ... s n The combined quadratic programming path results;

[0029] In the numerical optimization process of velocity planning in the ST diagram, the cost function is:

[0030]

[0031] A eq x = b eq

[0032] Ax≤b

[0033] lb≤x≤ub

[0034] Where w i s represents the weight of each term in the cost function. i v ref Let S represent the lateral displacement and reference velocity of the path in the ST diagram, respectively; the corresponding constraints in the formula are continuity constraints, velocity and acceleration inequality constraints, respectively. The velocity planning result is obtained through the solver, and the path planning result is combined to complete the trajectory planning.

[0035] Based on the local planning results, the vehicle can easily avoid static and dynamic obstacles in the mine along the navigation path and safely drive to the starting area of ​​the planned U-turn.

[0036] The described step S3 includes the following sub-steps:

[0037] S31, Discretize the chamber planning area: The discretization method uses a grid map to describe the environmental information of the starting area of ​​the U-turn planning and the chamber area. Obstacles and boundaries are expanded in the grid map, and the discretization scale is dynamically adjusted according to the chamber area.

[0038] S32, Node variable step size expansion based on discretized state space:

[0039] Motion elements of different lengths are generated in the U-turn planning area X at the tunnel entrance and the U-turn planning area T inside the tunnel; node x g In the discrete search space (x,y,θ), forward and backward motion primitives are expanded, and node x g It is not necessarily at the center of the discrete grid, where each motion primitive in the forward and backward directions satisfies the kinematic characteristics of an autonomous vehicle in an underground mine, and the minimum step size of a single expansion exceeds the cell where the current node is located, and the change in the front wheel angle is an integer multiple of the discretized unit heading angle:

[0040] 0≤r A ≤r max

[0041] Δδ=kσ θ ,k∈Z

[0042] Where r A r is the turning radius of the motion primitive during node expansion. max To satisfy the maximum steering radius of the vehicle's kinematic constraints, Δδ is the change in front wheel steering angle, and σ θ Discretized unit heading angle;

[0043] Long step size primitives s are used in narrow tunnel regions long Short step size primitives s are used in the chamber area. short By increasing the number of nodes, the U-turn planning process can be facilitated to improve flexibility and obstacle avoidance capabilities.

[0044] The variable step size expansion strategy is as follows:

[0045]

[0046] The cost function F(n) for node expansion includes the actual cost term, the heuristic cost term, and the directional cost term. Each expansion is performed on the node n with the minimum cost, and the cost function F(n) is:

[0047] F(n)=(1+λ t +λ b )*g(n)+λ h *h(n)+r(n);

[0048] S33, RS curve planning;

[0049] Based on the variable step size expansion strategy, during the continuous node expansion process, the motion primitives at fixed intervals in different regions adopt RS curve planning once every m expansions, and the interval of RS curve planning is reduced within the planning area T inside the tunnel; the RS curve planning strategy is expressed as:

[0050]

[0051] Where Rs n RS curve planning is executed when the value is 1, otherwise RS curve planning is not executed when the value is 0; m X m T is a positive integer, representing the interval of the number of RS curve planning operations in different regions, and:

[0052] m X >m T

[0053] Furthermore, the path segments in the RS curve satisfy the specified initial and final tangent direction conditions, the circular arc path segments are used for left and right turns, and the curvature κ of the turning path segments...s Meets the maximum curvature limit for vehicle operation:

[0054] κ s ≤κ max

[0055] In each RS curve planning process, a combination of straight line segments and circular arcs is considered to generate forward and backward paths. C represents circular motion, S represents straight line motion, | represents reverse motion, and the subscript u indicates that the rotation angles of the two circular arcs are the same. The combination method for RS curve planning is as follows:

[0056]

[0057] The RS curve planning combination mentioned above includes linear motion (S), left turn (L), right turn (R), forward movement, and backward movement modes; in the resulting path using RS curve planning, the state information of the starting point and the ending point are as follows: and All are combinations of circular arcs, and the curvature of each arc is 0.1m^-1. The corresponding pattern combinations are 'L', 'R', and 'L'. If RS planning is successful and the path collision detection result is no collision, the motion primitive expansion result and RS curve planning result are merged, the turn-around planning path is output, and the process proceeds to step S4. Otherwise, the process returns to step S302 and continues to loop through node expansion until the target point is reached, generating a collision-free path.

[0058] The method for path post-processing in step S4 is as follows:

[0059] The objective function for post-processing consists of four terms: collision cost, curvature constraint, path smoothing, and potential field. Their specific components are as follows:

[0060]

[0061] The first term is the collision cost term, x i For the path node coordinates, O i d represents the coordinates of the nearest obstacle. max The maximum distance threshold is set.

[0062] The second term is the curvature constraint term, κ. max Let Δφ be the upper limit of curvature to restrict the change in curvature, and let Δφ be expressed as:

[0063]

[0064] The third term is the smoothing term, which calculates the square of the displacement difference between each node to limit the non-uniform distribution of path points and ensure path smoothness.

[0065] The last term is the potential field function term. Here, an adaptive artificial potential field function is used to improve the inaccuracy of traditional potential fields. The potential field function ρ... s (x i Expressed as the gravitational function U rep (q) and repulsive function F rep Sum of (q):

[0066] ρ s (x i )=U rep (q)+F rep (q)

[0067] Gravitational function U rep (q) and repulsive function F rep (q) can be further expressed as:

[0068]

[0069]

[0070] Where q represents the spatial location of the autonomous vehicle, ρ(q,q obs ) represents the position q of the autonomous vehicle and the position q of the obstacle target point. obs The Euclidean distance between them, β>0 is the repulsive gain coefficient, ρ0 is the set safety distance, and k and n are real numbers greater than 0;

[0071] Based on the objective function constructed above, a nonlinear optimization programming problem is used to model the discrete points of the collision-free path, and a corresponding solver is used to obtain the final smooth path solution. The U-turn planning method for autonomous vehicles in narrow underground mine passages of this invention, based on the underground chamber space connected to the narrow passage and a phased planning method, enables autonomous vehicles to safely and quickly complete autonomous U-turns.

[0072] Compared with the prior art, the present invention has the following beneficial effects:

[0073] 1. The present invention uses a two-stage planning method to plan the tunnel area and the chamber area in sequence, and takes into account the obstacle avoidance problem in the planning process. It can effectively complete the U-turn trajectory planning in narrow areas by utilizing the chamber space, and at the same time takes into account the feasibility and smoothness of the planned trajectory, and can achieve a relatively smooth U-turn action.

[0074] 2. This invention improves the U-turn planning algorithm by taking into account the characteristics of the U-turn planning scenario, further enhancing the planning speed and ensuring the real-time performance of the planning algorithm, thereby improving the efficiency of autonomous driving vehicles in mines. Attached Figure Description

[0075] Figure 1This is a flowchart of the U-turn planning method for an automated vehicle in a narrow underground mine passage according to the present invention;

[0076] Figure 2 This is a schematic diagram of the U-turn planning area in this invention;

[0077] Figure 3 This is a flowchart of the algorithm according to an embodiment of the present invention;

[0078] Figure 4 This is a schematic diagram of node expansion considering kinematic constraints in this invention;

[0079] Figure 5 This is a schematic diagram of a U-turn planning path generated for narrow underground passages and chambers in a mine, according to one embodiment of the present invention.

[0080] In the picture:

[0081] 1. U-turn planning starting area; 2. Chamber planning area boundary; T. U-turn planning area inside the chamber; X. U-turn planning area at the tunnel entrance; 5. Tunnel planning area. Detailed Implementation

[0082] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0083] The method for planning U-turns of automated vehicles in narrow underground mine passages according to the present invention mainly consists of two planning steps: tunnel area planning and chamber area planning. See also... Figure 1 and 2 After receiving a U-turn command, the autonomous vehicle obtains the location information of the nearest chamber and generates a U-turn planning starting area 1 within the narrow tunnel at the junction of the chamber and the tunnel. Then, the first stage of U-turn planning, namely tunnel area planning, is performed with the starting area 1 as the target area, within a space of 5. After the first stage of planning, the autonomous vehicle enters the starting area 1 and proceeds to the second stage of planning. The second stage of planning is completed within the chamber planning area boundary 2, passing through both the tunnel entrance U-turn planning area X and the chamber U-turn planning area T, ultimately completing the U-turn planning process.

[0084] See Figure 3 The aforementioned method for planning U-turns for autonomous vehicles in narrow passages in underground mines includes the following steps:

[0085] S1, Determine the starting area for the U-turn plan: The autonomous vehicle travels along the original navigation route in the mine. When it receives a U-turn command, it searches for the nearest chamber location in the tunnel based on a high-precision map and determines the starting area for the U-turn plan based on the layout of the tunnel and chambers. The starting area for the U-turn plan should be close to the chamber area and narrow tunnel entrance in the mine, and located within the narrow tunnel.

[0086] S2, perform tunnel area planning and generate interval navigation paths and local paths: The autonomous vehicle determines its current location and takes its previous location and the above-mentioned U-turn planning starting area as the starting point and ending point of the route planning, respectively, and generates a global navigation path within this interval; then, based on the global navigation path, the projection point of the autonomous vehicle on the reference line is used as the origin of the coordinate system to establish the Frenet coordinate system, and the planning space is constructed for the local planning during the process of driving to the U-turn planning starting area.

[0087] Specifically as follows:

[0088] S21, find the matching point of the autonomous vehicle's coordinates on the navigation reference line, and obtain the information of the corresponding coordinate point in the Frenet coordinate system according to the transformation relationship between the Cartesian coordinate system and the Frenet coordinate system.

[0089] S22 decouples the three-dimensional SLT problem in the Frenet coordinate system into two two-dimensional problems at the SL and ST levels. Based on the SL map and the projection information of static obstacles, a spatial discretization method combined with quadratic programming is used for path decision-making and planning. With the path results, an ST map is then built and dynamic obstacle projection is performed. A similar spatial discretization method combined with quadratic programming is used for velocity decision-making and planning. The final navigation path planning results and velocity planning results are merged to obtain the trajectory information.

[0090] The SLT three-dimensional problem decoupling is considered from the SL level and the ST level respectively. The SL level problem is mainly solved based on the SL graph and spatial discretization method, while the ST level problem is mainly solved based on the ST graph and spatial discretization method. The discretization scale can be dynamically adjusted according to the width of the underground road and the speed limit of the autonomous vehicle.

[0091] The navigation path planning process is a built-in algorithm of the autonomous vehicle system, including the smoothing process of the navigation reference line. The reference line has relatively dense discrete points. In the autonomous vehicle projection process based on the Frenet coordinate system, the searched matching points can be used as projection points approximately.

[0092] The methods for path decision-making and planning are as follows:

[0093] Based on the SL graph, the A* algorithm is used to search for paths composed of discrete grid points. These paths are then used as path decisions to expand the safe space under the current road conditions. Finally, a path optimization problem is constructed in the SL coordinate system:

[0094] l = f(s)

[0095] The path optimization problem within the safe space is constructed as a quadratic programming problem, and the cost function can be expressed as:

[0096]

[0097] Where w i Let l be the weight of each term in the cost function. i l centre These represent the vertical offsets of the path and reference line in the SL diagram, respectively; each term of the cost function represents the first derivative cost, second derivative cost, third derivative cost, lateral offset cost, and centerline cost, respectively, with a corresponding cost weight preceding each term. The constraints are as follows:

[0098] A eq x = b eq

[0099] Ax≤b

[0100] lb≤x≤ub

[0101] x=(l1,l′1,l″1,…l n ,l′ n ,l″ n ) T

[0102] In the formula, x is the state variable, and A eq ,b eq Used to describe state equality constraint equations, Ax≤b describes state inequality constraints, and lb,ub are the lower and upper bounds of the range of values ​​for state x.

[0103] The corresponding constraints are continuity equality constraints, collision inequality constraints, and upper and lower bound constraints, respectively. Using a specific solver, l1, l′1, l″1, ... l can be quickly obtained. n ,l′ n ,l″ n With s1, s2, s3, ... s n The combined quadratic programming path results.

[0104] The aforementioned speed decision-making and planning are similar to path planning, based on the obstacle trajectory predicted by projection onto the ST graph and spatial discretization. The obstacle trajectory prediction relies on data from the perception module. A speed decision scheme is obtained using dynamic programming, a safety space is created, and the final speed optimization is constructed as a quadratic programming problem. The cost function includes a reference speed cost term and first to third order derivative cost terms. The total cost function can be expressed as:

[0105]

[0106] A eq x = b eq

[0107] Ax≤b

[0108] lb≤x≤ub

[0109] In the formula, x is the state variable, and A eq ,b eq Used to describe state equality constraint equations, Ax≤b describes state inequality constraints, and lb,ub are the lower and upper bounds of the range of values ​​for state x; w i s represents the weight of each term in the cost function. i v ref Let represent the lateral displacement and reference velocity of the path and in the ST diagram, respectively; the corresponding constraints in the formula are continuity constraints, velocity and acceleration inequality constraints, respectively. The velocity planning result is obtained through the solver, and then the path planning result is merged to complete the trajectory planning.

[0110] S3: The autonomous vehicle travels to the planned starting area for the U-turn based on the tunnel area planning results and begins tunnel area planning. Details are as follows:

[0111] S31, Discretize the chamber planning area: The discretization method uses a grid map to describe the environmental information of the starting area of ​​the U-turn planning and the chamber area. Obstacles and boundaries are expanded in the grid map, and the discretization scale is dynamically adjusted according to the chamber area.

[0112] S32, Node variable step size expansion based on discretized state space:

[0113] Based on the discretized state space and the following node variable step-size extension strategy considering vehicle kinematic constraints, motion elements with different step sizes are used in the two main parts of the narrow tunnel U-turn planning region X at the tunnel entrance and the U-turn planning region T inside the tunnel in the tunnel region planning step; see [link to relevant documentation]. Figure 4 Node x g In the discrete search space (x,y,θ), forward and backward motion primitives are expanded, and node x g It is not necessarily at the center of the discrete grid, as shown by s in the figure. n Each motion primitive in the forward and backward directions satisfies the kinematic characteristics of an autonomous vehicle in a mine, and the minimum step size of a single expansion exceeds the cell where the current node is located. The change in the front wheel angle is an integer multiple of the discretized unit heading angle.

[0114] 0≤r A ≤r max

[0115] Δδ=kσ θ ,k∈Z

[0116] Where r A r is the turning radius of the motion primitive during node expansion.max To satisfy the maximum steering radius of the vehicle's kinematic constraints, where Δδ is the change in front wheel steering angle, σ θ The discretized unit heading angle.

[0117] Considering the difficulty of U-turn planning and ensuring a high success rate, the starting area for U-turn planning in narrow tunnels should not be too small. Furthermore, U-turn actions involve multiple movements, and due to the narrow tunnel area, most U-turn actions are completed within the tunnel chamber. Therefore, the motion element s during the node expansion process... n Setting different step sizes for different areas helps reduce planning time and improve the efficiency of U-turn planning. In narrow tunnel areas where there are generally no obstacles, a longer step size primitive s is used. long This allows for faster guidance to the vicinity of the tunnel interior area with fewer nodes; the tunnel interior area will involve multiple turning actions, utilizing short-step primitives s. short By increasing the number of nodes, the turning planning process is facilitated, offering good flexibility and strong obstacle avoidance capabilities. The variable step-size expansion strategy can be described as follows:

[0118]

[0119] The cost function for node expansion mainly includes the actual cost term, the heuristic cost term, and the directional cost term. Each expansion is performed on the node n with the minimum cost.

[0120] F(n)=(1+λ t +λ b )*g(n)+λ h *h(n)+r(n)

[0121] Where g(n) represents the actual cost term, i.e., the length l and λ required to travel from the starting node to the current node. t and λ b This represents the weights of the steering primitive and the reversing primitive; r(n) represents the penalty cost for changing direction, in order to reduce the occurrence of frequent forward and reverse changes in the planned path.

[0122]

[0123] h(n) = max(h1(n), h2(n))

[0124] h(n) represents the heuristic cost term, λ h It is its weight coefficient, and h(n) is taken as the larger of the nonholonomic constraint cost h1(n) considering obstacles and the holonomic constraint cost h2(n) not considering obstacles.

[0125]

[0126] In the heuristic cost term, the nonholonomic constraint cost h1(n) is the result value obtained from the improved A* algorithm, and the sub-heuristic cost function h1(n) in the search process of the nonholonomic constraint cost is... h1 The weight is higher than the sub-actual cost function g h1 This improves the search speed for node expansion. The complete constraint cost h2(n) in the heuristic cost term is the result value of Rs curve programming, and the obstacle avoidance cost is not included in the Rs curve programming process.

[0127] S33, RS curve planning:

[0128] Based on the variable step size expansion strategy, considering that the U-turn planning space mainly consists of two parts: the U-turn planning area X at the tunnel entrance of the narrow tunnel and the U-turn planning area T inside the tunnel, and that the space is relatively limited, during the continuous node expansion process, the motion primitives at fixed intervals in different regions adopt RS curve planning once every m expansions, and the interval of RS curve planning is reduced within the U-turn planning area T inside the tunnel; the RS curve planning strategy can be expressed as:

[0129]

[0130] Where Rs n When the value is 1, RS curve planning is executed; otherwise, when it is 0, RS curve planning is not executed. X m T is a positive integer, representing the interval of the number of RS curve planning operations in different regions, and && indicates that both conditions are satisfied simultaneously, where:

[0131] m X >m T

[0132] Furthermore, the path segments in the RS curve satisfy the specified initial and final tangent direction conditions, the circular arc path segments are used for left and right turns, and the curvature κ of the turning path segments... s Meets the maximum curvature limit for vehicle operation:

[0133] κ s ≤κ max

[0134] In each RS curve planning process, a combination of straight line segments and circular arcs is considered to generate forward and backward paths. C represents circular motion, S represents straight line motion, | represents reverse motion, and the subscript u indicates that the rotation angles of the two circular arcs are the same. The combination method for RS curve planning is as follows:

[0135]

[0136] The RS curve planning combination mentioned above includes linear motion (S), left turn (L), right turn (R), forward, and backward modes; please refer to [link to relevant documentation]. Figure 4 In the resulting path planned using RS curves, the state information for the start and end points are as follows: and All are combinations of circular arcs, with a curvature of 0.1 m^-1, and the corresponding pattern combinations are 'L', 'R', and 'L'. If RS planning is successful and the path collision detection result is no collision, the motion primitive expansion result and RS curve planning result are merged, the turn-around planning path is output, and the process proceeds to step S4; otherwise, the process returns to step S302 and continues to loop through node expansion until the target point is reached, generating a collision-free path.

[0137] Step S4: Post-process the collision-free path generated in step S3, construct the objective function, model the discrete points of the collision-free path using a nonlinear optimization programming problem, and use the corresponding solver to obtain the final smooth path solution.

[0138] The above path result represents the actual drivable path for a U-turn vehicle, satisfying the vehicle's kinematic constraints. However, to make the U-turn path smoother and easier for the controller to track, post-processing is required. The objective function of the post-processing mainly includes collision penalty terms, curvature constraint terms, path smoothing terms, and potential field terms, specifically structured as follows:

[0139]

[0140] The first term is the collision cost term, x i For the path node coordinates, O i d represents the coordinates of the nearest obstacle. max The first term is the maximum distance threshold; the second term is the curvature constraint term, κ. max Let Δφ be the upper limit of curvature to restrict the change in curvature, and let Δφ be expressed as:

[0141]

[0142] The third term is the smoothing term, which calculates the square of the displacement difference between each node to limit the non-uniform distribution of path points and ensure path smoothness. The last term is the potential field function term, where an adaptive artificial potential field function is used to improve the inaccuracy of traditional potential fields, where the potential field function ρ... s (x i It can be expressed as the gravitational function U. rep (q) and repulsive function F rep Sum of (q):

[0143] ρ s (x i )=Urep (q)+F rep (q)

[0144] Gravitational function U rep (q) and repulsive function F rep (q) can be further expressed as:

[0145]

[0146]

[0147] Where q represents the spatial location of the autonomous vehicle, ρ(q,q) obs ) represents the position q of the autonomous vehicle and the position q of the obstacle target point. obs The Euclidean distance between them is given, where β>0 is the repulsive gain coefficient, ρ0 is the set safety distance, and k and n are real numbers greater than 0. Based on the objective function constructed above, a nonlinear optimization programming problem is used to model the discrete points of the collision-free path, and a corresponding solver is employed to obtain the final smooth path solution.

[0148] like Figure 5 The figure shows a schematic diagram of the turning-around planning path for narrow passages and chambers in a mine generated according to the present invention. After multiple experiments, the algorithm completed the planning within 600ms in this scenario. As can be seen from the figure, the present invention can quickly and efficiently plan a reasonable and smooth path in the chamber area.

[0149] The foregoing has provided a detailed description of a decision-making and planning method for autonomous vehicles meeting other vehicles in underground mines, as disclosed in the embodiments of this application. Specific examples have been used to illustrate the principles and implementation methods of this application. The descriptions of the embodiments above are merely for the purpose of helping to understand the method and its core ideas. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method for planning U-turns of autonomous vehicles in narrow passages in underground mines, comprising the following steps: S1, Determine the starting area for the U-turn plan: The autonomous vehicle travels along the original navigation route in the mine. When it receives a U-turn instruction, it searches for the nearest chamber in the tunnel based on the high-precision map and determines the starting area for the U-turn plan based on the layout of the tunnel and chamber. S2, perform tunnel area planning and generate interval navigation path and local path: The autonomous vehicle determines its current position, takes the previous position and the above-mentioned U-turn planning starting area as the starting point and ending point of the route planning, respectively, and generates a global navigation path within this interval; Then, based on the global navigation path, the projection point of the autonomous vehicle on the reference line is used as the origin of the coordinate system to establish the Frenet coordinate system, and a planning space is constructed for the local planning during the process of driving to the planning starting area of ​​the U-turn. S3, Perform tunnel area planning: The autonomous vehicle travels to the starting area of ​​the U-turn planning based on the tunnel area planning results, establishes a discrete map of the tunnel planning area and sets a step size Sn, expands the nodes until the target point is reached, and generates a collision-free path. Step S3 includes the following sub-steps: S31, Discretize the chamber planning area: The discretization method uses a grid map to describe the environmental information of the starting area of ​​the U-turn planning and the chamber area. Obstacles and boundaries are expanded in the grid map, and the discretization scale is dynamically adjusted according to the chamber area. S32, Node variable step size expansion based on discretized state space: Motion elements of different lengths are generated in the U-turn planning area X at the tunnel entrance and the U-turn planning area T inside the tunnel; node x g In the discrete search space (x,y,θ), forward and backward motion primitives are expanded, and node x g It is not necessarily at the center of the discrete grid, where each motion primitive in the forward and backward directions satisfies the kinematic characteristics of an autonomous vehicle in an underground mine, and the minimum step size of a single expansion exceeds the cell where the current node is located, and the change in the front wheel angle is an integer multiple of the discretized unit heading angle: 0≤r A ≤r max Δδ=kσ θ ,k∈Z Where r A r is the turning radius of the motion primitive during node expansion. max To satisfy the maximum steering radius of the vehicle's kinematic constraints, Δδ is the change in front wheel steering angle, and σ θ Discretized unit heading angle; Long step size primitives s are used in narrow tunnel regions long Short step size primitives s are used in the chamber area. short By increasing the number of nodes, the U-turn planning process can be facilitated to improve flexibility and obstacle avoidance capabilities. The variable step size expansion strategy is as follows: The cost function F(n) for node expansion includes the actual cost term, the heuristic cost term, and the directional cost term. Each expansion is performed on the node n with the minimum cost, and the cost function F(n) is: F(n)=(1+λ t +λ b )*g(n)+λ h *h(n)+r(n); Where g(n) represents the actual cost term, i.e., the length l and λ required to travel from the starting node to the current node. t and λ b Represents the extended steering primitive weights and reversing primitive weights; h(n) represents the heuristic cost term, λ h Its weighting coefficient; r(n) represents the penalty cost for changing direction; S33, RS curve planning; Based on the variable step size expansion strategy, during the continuous node expansion process, the motion primitives at fixed intervals in different regions adopt RS curve planning once every m expansions, and the interval of RS curve planning is reduced within the planning area T inside the tunnel; the RS curve planning strategy is expressed as: Where Rs n When the value is 1, RS curve planning is executed; otherwise, when it is 0, RS curve planning is not executed. X m T is a positive integer, representing the interval of the number of RS curve planning operations in different regions, and: m X >m T Furthermore, the path segments in the RS curve satisfy the specified initial and final tangent direction conditions, the circular arc path segments are used for left and right turns, and the curvature κ of the turning path segments... s Meets the maximum curvature limit for vehicle operation: k s ≤κ max In each RS curve planning process, a combination of straight line segments and circular arcs is considered to generate forward and backward paths. C represents circular motion, S represents straight line motion, | represents reverse motion, and the subscript u indicates that the rotation angles of the two circular arcs are the same. The combination method for RS curve planning is as follows: The RS curve planning combination mentioned above includes linear motion (S), left turn (L), right turn (R), forward movement, and backward movement modes; in the resulting path using RS curve planning, the state information of the starting point and the ending point are as follows: and All of them are combinations of circular arcs, and the curvature of each arc is 0.1m. -1 The corresponding mode combinations are 'L', 'R', and 'L'. If RS planning is successful and the path collision detection result is no collision, merge the motion primitive expansion result and RS curve planning result, output the turn-around planning path, and proceed to step S4. Otherwise, return to step S32 and continue to loop the node expansion until the target point is reached, generating a collision-free path. S4. Post-process the collision-free path generated in step S3: construct the objective function, model the discrete points of the collision-free path using a nonlinear optimization programming problem, and use a corresponding solver to obtain the final smooth path solution.

2. The U-turn planning method according to claim 1, characterized in that: the starting area of ​​the U-turn planning in step S1 is close to the chamber area and the narrow tunnel entrance in the mine, and is located in the narrow tunnel.

3. The U-turn planning method according to claim 1, characterized in that, Step S2 includes the following sub-steps: S21, find the matching point of the autonomous vehicle's coordinates on the navigation reference line, and obtain the information of the corresponding coordinate point in the Frenet coordinate system according to the transformation relationship between the Cartesian coordinate system and the Frenet coordinate system. S22 decouples the three-dimensional SLT problem in the Frenet coordinate system into two two-dimensional problems at the SL and ST levels. Based on the SL map and the projection information of static obstacles, a spatial discretization method combined with quadratic programming is used for path decision-making and planning. With the path results, an ST map is then built and dynamic obstacle projection is performed. A spatial discretization method combined with quadratic programming is used for velocity decision-making and planning. The final navigation path planning results and velocity planning results are merged to obtain the trajectory information.

4. The U-turn planning method according to claim 3, characterized in that, The specific process of step S22 is as follows: The autonomous vehicle establishes a Frenet coordinate system based on the navigation path, and then sequentially builds SL and ST graphs to construct the path planning and velocity planning spaces, respectively: A state space graph (SL) is constructed based on static and low-speed obstacle projections, and the state space is discretized. Heuristic search methods and numerical optimization methods are used for path decision planning. Based on dynamic obstacle trajectory prediction, an ST graph is established and the state space is discretized. A heuristic search method and a numerical optimization method are used for velocity decision planning. In the path planning in the SL graph and the velocity planning in the ST graph, different non-uniform sampling scales are set according to the tunnel scenario. First, the state space is discretized, and the cost value of each discrete point is allocated according to the cost function. An improved A* algorithm is used for heuristic search to quickly obtain an initial solution. The initial solution opens up a safety space as a decision scheme. Based on the safety space, the original problem is transformed into a convex optimization problem. Under constraints, a convex optimization solution method is used to obtain the optimal trajectory solution. In the numerical optimization process of velocity planning in the SL graph, the cost function is: Where w i Let l be the weight of each term in the cost function. i , represents a path in the SL graph, l centre This represents the longitudinal offset of the path relative to the reference line; each term of the cost function represents the first derivative cost, second derivative cost, third derivative cost, lateral offset cost, and centerline cost, respectively, with a corresponding cost weight preceding each term. The constraints are as follows: A eq x=b eq Ax≤b lb≤x≤ub x＝(l1,l1',l1”,…l n ,l n ',l n ”) T In the formula, x is the state variable, and A eq ,b eq Used to describe state equality constraint equations, Ax≤b describes state inequality constraints, and lb,ub are the lower and upper bounds of the range of values ​​for state x. The corresponding constraints are continuity equality constraints, collision inequality constraints, and upper and lower bound constraints, respectively. The solver yields l1, l1', l1”, ... l n ,l' n ,l″ n With s1, s2, s3, ... s n The combined quadratic programming path results; In the numerical optimization process of velocity planning in the ST diagram, the cost function is: A eq x=b eq Ax≤b lb≤x≤ub In the formula, x is the state variable, and A eq ,b eq Used to describe state equality constraint equations, Ax≤b describes state inequality constraints, and lb,ub are the lower and upper bounds of the range of values ​​for state x; w i s represents the weight of each term in the cost function. i V represents the path in the ST graph. ref The reference velocity is represented by the formula; the corresponding constraints are continuity constraints, velocity and acceleration inequality constraints, respectively. The velocity planning result is obtained through the solver, and the path planning result is combined to complete the trajectory planning. Based on the local planning results, the vehicle can easily avoid static and dynamic obstacles in the mine along the navigation path and safely drive to the starting area of ​​the planned U-turn.

5. The U-turn planning method according to claim 1, characterized in that the method for post-processing the path in step S4 is as follows: The objective function for post-processing consists of four terms: collision cost, curvature constraint, path smoothing, and potential field. Their specific components are as follows: in, λ1, λ2, λ3, and λ4 are the weights of each term in the objective function; the first term is the collision cost term, x i For the path node coordinates, O i d represents the coordinates of the nearest obstacle. max The maximum distance threshold is set. The second term is the curvature constraint term, κ. max Let Δφ be the upper limit of curvature to restrict the change in curvature, and let Δφ be expressed as: The third term is the smoothing term, which calculates the square of the displacement difference between each node to limit the non-uniform distribution of path points and ensure path smoothness. The last term is the potential field function term. Here, an adaptive artificial potential field function is used to improve the inaccuracy of traditional potential fields. The potential field function ρ... s (x i Expressed as the gravitational function U rep (q) and repulsive function F rep Sum of (q): ρ s (x i )=U rep (q)+F rep (q) Gravitational function U rep (q) and repulsive function F rep (q) can be further expressed as: Where q represents the spatial location of the autonomous vehicle, ρ(q,q) obs ) represents the position q of the autonomous vehicle and the position q of the obstacle target point. obs The Euclidean distance between them, β>0 is the repulsive gain coefficient, ρ0 is the set safety distance, and k and n are real numbers greater than 0; Based on the objective function constructed above, a nonlinear optimization programming problem is used to model the discrete points of the collision-free path, and a corresponding solver is used to obtain the final smooth path solution.

Images