Well mine parking trajectory planning method based on Chebyshev distance judgment

Abstract

The invention discloses a well mining parking trajectory planning method based on Chebyshev distance determination, and the method comprises a map information vectorization module which is used for converting real-time map information into a combination form of a plurality of line segments; the SAT collision detection module is used for predicting the feasibility of a path when a child node is expanded; the Chebyshev distance judgment module is used for detecting the minimum distance between the planned path and the road boundary; the heuristic function module is used for exploring the optimal path of the parking track; and the RS curve linking module is used for linking the parking switching point and the target parking point. According to the method, the space utilization rate and the operation efficiency in a narrow scene are effectively improved, a planned path can be tried, adjusted and planned according to sensor data in the absence of GPS positioning correction, a vehicle is far away from a tunnel wall and obstacles as far as possible on the premise that parking planning succeeds, error allowance is provided for a subsequent chassis control module, and the parking efficiency is improved. And the collision risk is effectively reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving decision-making and planning, and specifically provides a shaft mine parking trajectory planning method based on Chebyshev distance determination. Background Art

[0002] Unmanned mining operations are crucial for ensuring safety, increasing production, and maximizing economic and social benefits. Underground construction, due to its large scale and rapid progress, often faces frequent accidents due to complex and lengthy processes, which pose significant challenges to society and the economy. The integration of automation technology is essential for improving the safety, production efficiency, and environmental sustainability of underground coal mines, and is bound to push the mining industry towards a high-end, intelligent, and green development future, aiming to reduce labor intensity and save energy.

[0003] With the expansion of China's 5G intelligent mines and the deployment of advanced mining equipment, the attention to intelligent mining systems is growing, and autonomous driving technology provides significant advantages in the transportation of mining vehicles. However, the lack of GPS signals in underground tunnels makes vehicle positioning difficult; secondly, the narrow and long tunnels not only limit the field of vision but also affect the stability of wireless communication, posing challenges to the construction of large-scale high-precision maps. To ensure the stable operation of underground operations, the planned path should be dynamically adjusted according to the scene characteristics at the parking site; finally, traditional parking trajectory planning methods are based on grid maps and use traditional hybrid A* algorithms to complete. In its obstacle avoidance process, not only does it require manual setting of safety distances, but it may also lead to planning failures due to a narrower feasible region. At the same time, a large amount of drivable space is sacrificed during the gridification process, resulting in slow planning speed, low success rate, and the inability to adaptively adjust obstacle safety distances. The existence of the above problems makes the current shaft mine parking trajectory planning methods in the prior art generally suffer from problems such as low accuracy, large errors, poor passing and obstacle crossing capabilities, etc. Summary of the Invention

[0004] The purpose of the present invention is to provide a shaft mine parking trajectory planning method based on Chebyshev distance determination to solve the above defects.

[0005] To achieve the above purpose, the present invention provides the following technical solutions:

[0006] A shaft mine parking trajectory planning method based on Chebyshev distance determination, comprising the following steps:

[0007] S1. According to SLAM real-time mapping, convert the map scene into a planar map, and convert the road information of the underground roadway into a combination of several line segments through a boundary detection algorithm;

[0008] S2. Combine with the SAT collision detection method, take the road boundary information as a generalized convex polygon, compare it with the vehicle body pose data, and judge the collision relationship;

[0009] S3. Introduce the Chebyshev distance judgment, use the separating axis projection method in the SAT collision detection process to calculate the minimum distance between the vehicle and the boundary;

[0010] S4. Improve the heuristic function according to the result of step S3, add the obstacle distance penalty in the exploration process to make the motion trajectory away from the wall;

[0011] S5. Execute the RS curve linking task, try to use the sub-nodes of the current path as the reverse gear shifting points and connect the parking points; if the connection is successful, jump out of the loop, obtain the final planned path, and then use matlab simulation for verification to obtain the mine parking trajectory planning result with the maximum safety distance; if the connection fails, return to step S2 to recalculate. After trying the rated number of times, if the connection still fails, return the failure information.

[0012] Preferably, in steps S2 and S3, taking the road boundary information as a generalized convex polygon specifically means: taking the line segments in the road boundary information as generalized convex polygons so that the SAT collision detection method can be used; at the same time, add the Chebyshev distance determination in the separating axis algorithm to replace the traditional Euclidean distance calculation.

[0013] Preferably, in step S3, the Chebyshev distance, expressed as the infinity norm distance, is used as a metric in the vector space and defines the maximum absolute difference in the coordinates of two points in each dimension; if two n-dimensional vectors are respectively: p = (p1, p2,..., p n ), q = (q1, q2,..., q n ), their Chebyshev distance can be calculated by formula (1):

[0014] d Chebyshev (p, q) = max(|p1 - q1|, |p2 - q2|,..., |p n - q n |) (2),

[0015] In formula (2), p1, p2, p n respectively represent the projection coordinates of point p on the 1st, 2nd and nth coordinate axes, and q1, q2, q nrespectively represent the projection coordinates of point q on the first, second, and nth coordinate axes; the maximum projection distance of the endpoints of two vectors on the coordinate axes can be obtained in a high-dimensional space through Equation (2); when measuring the maximum projection distance between two convex polygons, each projection direction in SAT can be regarded as a dimension, and perpendicular projections need to be performed along each side of the convex polygon; the top view of a vehicle usually appears as a rectangle, and its projection direction only needs to be determined twice, which reduces the amount of calculation to a certain extent;

[0016] The Chebyshev distance can be used as an upper bound for the Euclidean distance. For two points A(x1, y1) and B(x2, y2) on a two-dimensional plane, their relationship is defined as shown in Equation (3):

[0017]

[0018] In Equation (3), d Chebyshev (A, B) represents the Chebyshev distance between points A and B, and d Euclidean (A, B) represents the Euclidean distance between points A and B;

[0019] In a restricted underground tunnel environment, the orientation bounding box method is used to determine the geometry of the vehicle; assume that the vehicle body is rectangular, with a width of W v , a length of L v , and a rear overhang of R ov . According to geometric relationships, the coordinates of each point of the vehicle can be expressed as Equations (4)-(7) as follows:

[0020]

[0021] In Equations (4)-(7), are the coordinates of the four points of the rectangular body respectively, and -ψ n represents the angle of the current vehicle's attitude relative to the X-axis direction in the Cartesian coordinate system, and p os_X represents the X-axis coordinate of the center of the vehicle's rear axle, and the Y-axis coordinate of the center of the vehicle's rear axle;

[0022] By using online drawing technology, a real-time map composed of several line segments can be obtained, the boundary information of each obstacle can be obtained, and it can be converted into new coordinates under the SAT projection, as shown in Equations (8) and (9):

[0023]

[0024] In Equations (8) and (9), (X os_ψ , Y os_ψ ) and (X oe_ψ , Y oe_ψ ) represent the projections of the starting and ending coordinates of the obstacle boundary segment in the new coordinate system after rotation.

[0025] Preferably, the separation axis projection method refers to rotating and projecting each side of a convex polygon to find the gaps between object projections, thereby determining whether an object collision has occurred. The specific steps are as follows:

[0026] The projection direction of each reference line can be calculated using the arctan2 function, and its range is (-π, π], as shown in formula (10):

[0027]

[0028] In formula (10), ψ ref_i represents the projection direction of the extension direction of the wall boundary numbered i in the Cartesian coordinate system, x 1_ref_i represents the X-axis coordinate at the starting point of the i-th wall surface, x 2_ref_i represents the X-axis coordinate at the ending point of the i-th wall surface, y 1_ref_i represents the Y-axis coordinate at the starting point of the i-th wall surface, y 2_ref_i represents the Y-axis coordinate at the ending point of the i-th wall surface;

[0029] Based on the derivation of formula (2) and formulas (4) to (10), an improved version of the Chebyshev distance formula can be mathematically rewritten and shown as formula (11):

[0030] d SAT_Chebyshev (vehicle, obstacle) = min(d Che_obs_1 , d Che_obs_2 ,..., d Che_obs_n ) (11),

[0031] In formula (11), vehicle represents the vehicle controlled by the autonomous driving system, obstacle represents various obstacles that the autonomous driving vehicle may encounter during driving, and d SAT_Chebyshev represents the minimum Chebyshev distance between the vehicle and all nearby obstacles, and d Che_obs_n represents the maximum Chebyshev distance between the vehicle and the n-th obstacle;

[0032] The distance d Che_obs_i represents the maximum Chebyshev distance between the boundary of each obstacle and the vehicle, i = 1, 2,..., n, as shown in formula (12):

[0033] d Che_obs_i = max(d obs_i_ref_v1 , d obs_i_ref_v2 , d obs_i_ref_o1 , d obs_i_ref_o2 ,..., d obs_i_ref_on )(12),

[0034] d obs_ref (vehicle,obs,ref) = max(d1, d2, d3, d4) (13),

[0035] In Equation (13), d obs_ref (vehicle,obs,ref) represents the maximum value of the difference in the starting and ending positions between two line segments of each vehicle boundary and obstacle boundary in the reference projection direction. To determine whether the projections of the two line segments on the X-axis and Y-axis overlap, d1, d2, d3, and d4 are the differences in the starting and ending positions of the two line segments in the shaded part of the projection direction; when the condition: vehMinX > obsLineMaxX || vehMaxX < obsLineMinX || vehMinY >

[0036] obsLineMaxY || wehMaxY < obsLineMinY, that is, when the two line segments do not intersect, the values of d1, d2, d3, and d4 are obtained according to Formulas (14) and (15), specifically as follows:

[0037]

[0038]

[0039] In Formulas (14) and (15), obsMaxX represents the maximum value of the current obstacle boundary line segment on the X-axis in the reference projection direction, obsMinX represents the minimum value of the current obstacle boundary line segment on the X-axis in the reference projection direction, obsMaxY represents the maximum value of the current obstacle boundary line segment on the Y-axis in the reference projection direction, obsMaxY represents the minimum value of the current obstacle boundary line segment on the Y-axis in the reference projection direction, vehMaxX represents the maximum value of the current vehicle boundary line segment on the X-axis in the reference projection direction, vehMinX represents the minimum value of the current vehicle boundary line segment on the X-axis in the reference projection direction, vehMaxY represents the maximum value of the current vehicle boundary line segment on the Y-axis in the reference projection direction, and vehMinY represents the minimum value of the current vehicle boundary line segment on the Y-axis in the reference projection direction.

[0040] Preferably, in the step S4, the specific steps are as follows:

[0041] To ensure that the planned path is as short as possible, the cumulative cost function includes a term for the total path length; in addition, to prevent excessive reverse and steering operations, the penalties for these actions are integrated into the cumulative cost function; the formula for the cumulative cost function is as shown in Formula (15):

[0042]

[0043] In Equation (17), G shift(s) represents the cumulative cost of gear shifting actions, which is used to reduce the number of gear shifts and make the driving process smoother, r i is the forward and reverse conversion flag of the i-th node, λ shift is the gear shifting penalty weight coefficient;

[0044] This improvement adds the Chebyshev distance of vehicle obstacles to the heuristic function of the local trajectory, which can be used as part of the heuristic; in each exploration of the algorithm, a penalty for the obstacle distance is added, as shown in formula (18):

[0045]

[0046] In formula (18), H Chebyshev (s) represents the overall obstacle distance penalty cost of the planned path, d Chebyshev_i represents the Chebyshev metric of the distance between the vehicle and the obstacle during each exploration, and λ distance is the corresponding collision avoidance penalty coefficient;

[0047] Through formulas (17) and (18), the improved evaluation function is obtained, as shown in formula (19):

[0048] F(s) = G(s) + G shift (s) + H(s) + H Chebyshev (s) (19),

[0049] In formula (19), H(s) represents the heuristic function guiding to the target position, and G(s) represents the penalty for reversing and changing directions in the planned path, as shown in formula (20):

[0050]

[0051] In formula (20), λ scc represents the penalty coefficient when the front wheel steering angle of the vehicle changes; λ sc represents the penalty coefficient of the front wheel steering angle, the larger the angle, the heavier the penalty; ψ i represents the front wheel steering angle of the current child node; ψ i-1 represents the front wheel steering angle at the previous moment; |δ d | represents the change amount of the vehicle attitude angle of each child node relative to the previous moment; g d (δ d ) represents the weighted penalty function of vehicle forward and backward actions, which is obtained from formula (21) as follows:

[0052]

[0053] In formula (21), δ d represents the single displacement distance in the planned trajectory, λ bcThe penalty coefficient representing the reverse driving of the vehicle; at each parent node, search for child nodes in the front and rear sectors; assume that the wheels are turned in one direction and there are m gears to choose from, with a total of 4m + 2 possibilities; after calculating the vehicle coordinates and directions represented by each child node, the program will attempt to connect the RS curve.

[0054] The beneficial effects of the present invention are as follows:

[0055] (1) A well - underground mine parking trajectory planning method based on Chebyshev distance determination according to the present invention, aiming at the characteristics of the lack of GPS positioning signals and poor wireless communication environment in the underground tunnel environment, and at the same time to avoid the space waste caused by the grid method, the present invention vectorizes the map information and vehicle attitude information, improves the space utilization rate and operation efficiency in narrow scenarios, and can adjust the planned path according to sensor data in the case of lack of high - precision positioning correction and high - precision maps, and makes the vehicle stay away from the tunnel wall and obstacles as much as possible on the premise of ensuring the success of parking planning, with small errors and high precision.

[0056] (2) A well - underground mine parking trajectory planning method based on Chebyshev distance determination according to the present invention, aiming at the characteristic that traditional planning algorithms require artificial setting of safety distances, by introducing the Chebyshev distance algorithm to improve the existing separating axis projection method, the planned path can automatically adjust the safety distance from the mine wall according to the scene characteristics, while improving the space utilization rate, reducing the risk of planning failure, and having high passing and obstacle - crossing capabilities.

[0057] (3) A well - underground mine parking trajectory planning method based on Chebyshev distance determination according to the present invention, aiming at the difficulty of completing dynamic mining auxiliary tasks such as reverse parking, parallel parking, and dynamic scheduling in the narrow environment and limited - vision scenarios of the mine, the present invention introduces the improved SAT algorithm of Chebyshev distance into the hybrid A* algorithm, and by improving the heuristic function, reduces the number of reverse driving times, reduces the steering amplitude, improves the reliability of the planned path, avoids the vehicle from repeatedly moving at intersections, and the optimized planned path effectively improves the operation efficiency of the loading and unloading area. Description of the Drawings

[0058] Figure 1 It is the bicycle model diagram of the present invention;

[0059] Figure 2 It is the SAT algorithm diagram of the present invention that can output Chebyshev distance;

[0060] Figure 3 It is the schematic diagram of Chebyshev distance in the SAT algorithm of the present invention;

[0061] Figure 4 It is the SLAM mapping of the underground tunnel;

[0062] Figure 5 Real-time online mapping of the present invention;

[0063] Figure 6 Schematic diagram of exploring child nodes from the current node of the present invention;

[0064] Figure 7 Flowchart of the RS curve linking method of the present invention;

[0065] Figure 8 RS parking path planning result diagram of the present invention;

[0066] Figure 9 Comparison diagram of the minimum obstacle distance between the improved Chebyshev SAT algorithm and the traditional hybrid A* algorithm of the present invention;

[0067] Figure 10 Distribution diagram of child nodes traversed in the algorithm of the present invention. Detailed implementation mode

[0068] The following further describes the present invention in conjunction with embodiments. It should be noted that this is only an example and explanation of the inventive concept. Those skilled in the art of the present technology can make various modifications or supplements to the described specific embodiments or use similar methods for substitution, as long as they do not deviate from the inventive concept or exceed the scope defined by this claim book, they should be regarded as falling within the protection scope of the present invention.

[0069] Embodiment 1:

[0070] A mine parking trajectory planning method based on Chebyshev distance determination, comprising the following steps:

[0071] S1. According to SLAM real-time mapping, convert the map scene into a planar map, and convert the road information of the underground roadway into a combination of several line segments through a boundary detection algorithm.

[0072] S2. Combine the SAT collision detection method, use the road boundary information as a generalized convex polygon, and compare it with the vehicle body pose data to determine the collision relationship.

[0073] First, the parking trajectory planning is realized through a preset parking trajectory planning algorithm module, specifically as follows:

[0074] Figure 1 For the bicycle model diagram, the position and direction of the moving vehicle change over time as Figure 1 shown.

[0075] After determining the position of the center point of the vehicle's rear axle and the vehicle direction, establish a vehicle kinematic model: To describe the position of the vehicle, assume the iteration step size δ required by the algorithm d, after discretization, the vehicle's attitude is updated each time according to the steering angle of the wheel, and a Cartesian coordinate is established for the vehicle. Its kinematic equation can be simplified to the form shown in formula (1).

[0076]

[0077] Among them, the instantaneous steering center of the vehicle at the center of the rear axle is defined, denoted by the symbol P n (x n , y n ) represents; ψ n represents the current vehicle angle; θ n represents the wheel steering angle; L v represents the distance between the front and rear axles of the vehicle; therefore, the position P of the vehicle at the next moment can be calculated through formula (1) n+1 (x n+1 , y n+1 ) and the angle ψ n+1 .

[0078] Secondly, the road boundary information is regarded as a generalized convex polygon, specifically referring to: regarding the line segments in the road boundary information as generalized convex polygons so that the SAT collision detection method can be used.

[0079] SAT is often used to judge whether two convex polygons overlap and is widely used in the field of collision detection. It is very useful for many applications such as physical simulation and path planning. This is a fast and general algorithm that can reduce the need for collision detection code between each shape, thereby reducing the computational pressure.

[0080] S3, Figure 2 is a SAT algorithm that can output the Chebyshev distance. As Figure 2 shown, the Chebyshev distance judgment is introduced, and the minimum distance between the vehicle and the boundary is calculated using the separating axis projection method during the SAT collision detection process.

[0081] The SAT theorem asserts that if two convex shapes do not intersect, then there exists an axis such that their projections on this axis do not coincide. Figure 3 is a schematic diagram of the Chebyshev distance in the SAT algorithm. As Figure 3 shown, the Chebyshev distance is introduced to characterize the distance to the obstacle. The light blue part in the figure is used to evaluate the path generated by the Reeds-Shepp curve (RS curve).

[0082] At the same time, the Chebyshev distance judgment is added to the separating axis algorithm to replace the traditional Euclidean distance calculation.

[0083] The Chebyshev distance, usually denoted as the infinity norm distance (L∞ metric), is used as a metric in a vector space and defines the maximum absolute difference in the coordinates of two points in each dimension; if two n-dimensional vectors are respectively: p = (p1, p2,..., p n ), q = (q1, q2,..., q n ), their Chebyshev distance can be calculated by formula (1):

[0084] d Chebyshev (p, q) = max(|p1 - q1|, |p2 - q2|,..., |p n - q n |) (2),

[0085] In equation (2), p1, p2, p n respectively represent the projected coordinates of point p on the 1st, 2nd, and nth coordinate axes, and q1, q2, q n respectively represent the projected coordinates of point q on the 1st, 2nd, and nth coordinate axes; through equation (2), the maximum projected distance between the endpoints of two vectors in a high-dimensional space can be obtained. When measuring the maximum projected distance between two convex polygons, each projection direction in SAT can be regarded as a dimension, and perpendicular projections need to be carried out along each side of the convex polygon.

[0086] The top view of a vehicle usually appears as a rectangle, and its projection direction only needs to be determined twice, which reduces the amount of calculation to a certain extent.

[0087] The Chebyshev distance can be used as an upper bound for the Euclidean distance. For two points A(x1, y1) and B(x2, y2) on a two-dimensional plane, their relationship is defined as shown in formula (3):

[0088]

[0089] In formula (3), d Chebyshev (A, B) represents the Chebyshev distance between points A and B, and d Euclidean (A, B) represents the Euclidean distance between points A and B.

[0090] Therefore, using the Chebyshev distance can limit the distance between objects.

[0091] In a restricted underground tunnel environment, the oriented bounding box (OBB) method is used to determine the geometry of a vehicle. Compared with the axis-aligned bounding box (AABB) method, the edges of the OBB can rotate freely according to the vehicle's pose, providing a more compact bounding box, which is more suitable for the narrow underground tunnel environment; during the path planning process, it can more accurately reflect the shape of the object, reduce unnecessary detection areas, and improve the spatial utilization rate and the accuracy of collision detection.

[0092] Assume that the vehicle body is rectangular with a width of W v , a length of L v , and a rear overhang of R ov , according to the geometric relationship, the coordinates of each point of the vehicle can be expressed by formulas (4)-(7) as follows:

[0093]

[0094] In formulas (4)-(7), are the coordinates of the four points of the rectangular body respectively, and -ψ n represents the angle of the current vehicle's pose relative to the X-axis direction in the Cartesian coordinate system, and p os_X represents the X-axis coordinate of the center of the vehicle's rear axle, and p os_Y represents the Y-axis coordinate of the center of the vehicle's rear axle.

[0095] Figure 4 For underground tunnel SLAM mapping, as Figure 4 shown, since it cannot be guaranteed that the rock pillars formed by the mine walls have strictly convex polygon horizontal cross-sections, and due to the limited visibility in the mine, the visible range of the on-vehicle lidar is limited.

[0096] Figure 5 For real-time online mapping, as Figure 5 shown, by using online mapping technology, a real-time map composed of several line segments can be obtained, the boundary information of each obstacle can be obtained, and it is converted into new coordinates under the SAT projection as shown in formulas (8) and (9):

[0097]

[0098] In formulas (8) and (9), (X os_ψ , Y os_ψ ) and (X oe_ψ , Y oe_ψ ) represent the projections of the starting and ending coordinates of the obstacle boundary segment in the new coordinate system after rotation.

[0099] In the traditional definition of a convex polygon, the polygon is completely contained within the closed half-plane defined by each of its sides. In other words, for each side, all interior points lie on the same side of the line defined by the side. This also means that no points of the polygon can be found in the other half-plane divided by each side of the convex polygon. Therefore, we can consider a line segment as a generalized convex polygon, which also applies to the separating axis projection algorithm.

[0100] The separating axis projection method refers to rotating and projecting each side of a convex polygon to find the gaps between the object projections to determine whether an object collision has occurred. The specific steps are as follows:

[0101] The projection direction of each reference line can be calculated using the arctan2 function, and its range is (-π, π], as shown in formula (10):

[0102]

[0103] In formula (10), ψ ref_i represents the projection direction of the extension direction of the boundary of the wall numbered i in the Cartesian coordinate system, x 1_ref_i represents the X-axis coordinate at the starting point of the i-th wall surface, x 2_ref_i represents the X-axis coordinate at the ending point of the i-th wall surface, y 1_ref_i represents the Y-axis coordinate at the starting point of the i-th wall surface, y 2_ref_i represents the Y-axis coordinate at the ending point of the i-th wall surface.

[0104] Based on the derivation of formula (2) and formulas (4) to (10), an improved version of the Chebyshev distance formula can be mathematically rewritten and shown as formula (11):

[0105] d SAT_Chebyshev (vehicle, obstacle) = min(d Che_obs_1 , d Che_obs_2 ,..., d Che_obs_n ) (11),

[0106] In formula (11), vehicle represents the vehicle controlled by the autonomous driving system, obstacle represents various obstacles that the autonomous driving vehicle may encounter during driving, d SAT_Chebyshev represents the minimum Chebyshev distance between the vehicle and all nearby obstacles, d Che_obs_n represents the maximum Chebyshev distance between the vehicle and the n-th obstacle.

[0107] The distance d Che_obs_i represents the maximum Chebyshev distance between the boundary of each obstacle and the vehicle, i = 1, 2,..., n, as shown in formula (12):

[0108] d Che_obs_i = max(d obs_i_ref_v1 , d obs_i_ref_v2 , d obs_i_ref_o1 , d obs_i_ref_o2 ,..., d obs_i_ref_on )(12),

[0110] d obs_ref (vehicle, obs, ref) = max(d1, d2, d3, d4) (13),

[0111] In Equation (13), d obs_ref (vehicle, obs, ref) represents the maximum value of the difference in the starting and ending positions between two line segments of each vehicle boundary and the obstacle boundary in the reference projection direction. To determine whether the projections of the two line segments overlap on the X-axis and Y-axis, d1, d2, d3, and d4 represent the differences in the starting and ending positions of the shaded parts of the two line segments in the projection direction. When the conditions: vehMinX > obsLineMaxX || vehMaxX < obsLineMinX || vehMinY >

[0112] obsLineMaxY || wehMaxY < obsLineMinY are met (i.e., when the two line segments do not intersect), calculate, the values of d1, d2, d3, and d4 are obtained according to Formulas (14) and (15), specifically as follows:

[0113]

[0114] In Formulas (14) and (15), obsMaxX represents the maximum value of the current obstacle boundary line segment on the X-axis in the reference projection direction, obsMinX represents the minimum value of the current obstacle boundary line segment on the X-axis in the reference projection direction, obsMaxY represents the maximum value of the current obstacle boundary line segment on the Y-axis in the reference projection direction, obsMaxY represents the minimum value of the current obstacle boundary line segment on the Y-axis in the reference projection direction, vehMaxX represents the maximum value of the current vehicle boundary line segment on the X-axis in the reference projection direction, vehMinX represents the minimum value of the current vehicle boundary line segment on the X-axis in the reference projection direction, vehMaxY represents the maximum value of the current vehicle boundary line segment on the Y-axis in the reference projection direction, and vehMinY represents the minimum value of the current vehicle boundary line segment on the Y-axis in the reference projection direction.

[0115] At this time, we can use the simpler and more efficient Chebyshev distance to approximately calculate the distance between the vehicle and the walls and obstacles in the mine, and only need to make a small improvement to the traditional SAT algorithm to integrate the collision detection algorithm and the distance calculation into one.

[0116] S4. Improve the heuristic function according to the result of step S3, add the obstacle distance penalty during the exploration process, and keep the motion trajectory away from the wall. The specific steps are as follows:

[0117] The evaluation function of the traditional hybrid A* algorithm consists of two parts, as shown in formula (16), specifically as follows:

[0118] F(s) = G(s) + H(s) (16),

[0119] In formula (16), G(s) represents the cumulative cost function of the planned path step length, which quantifies the total cost from the starting point to the current state node; H(s) is the heuristic cost function, which provides the estimated cost from the current state node to the target.

[0120] To ensure that the planned path is as short as possible, the cumulative cost function includes the term of the total path length; in addition, to prevent excessive reverse and steering operations, the penalties for these actions are integrated into the cumulative cost function; the formula of the cumulative cost function is as shown in formula (17):

[0121]

[0122] In formula (17), G shift (s) represents the cumulative cost of the shifting action, r i is the forward and reverse conversion flag of the i-th node, and λ shift is the shifting penalty weight coefficient.

[0123] This improvement adds the Chebyshev distance of the vehicle obstacle to the heuristic function of the local trajectory, which can be used as part of the heuristic. In each exploration of the algorithm, the penalty of the obstacle distance is added, specifically as shown in formula (18):

[0124]

[0125] In formula (18), H Chebyshev (s) represents the overall obstacle distance penalty cost of the planned path, d Chebyshev_i represents the Chebyshev metric of the distance between the vehicle and the obstacle during each exploration, and λ distance is the corresponding collision avoidance penalty coefficient.

[0126] Through formulas (17) and (18), the improved evaluation function is obtained, as shown in formula (19):

[0127] F(s) = G(s) + G shift (s) + H(s) + H Chebyshev (s) (19),

[0128] In Equation (19), it includes the penalties for changing direction and reversing in the planned path, as shown in Equation (20):

[0129]

[0130] In Equation (20), λ scc represents the penalty coefficient when the steering angle of the vehicle's front wheels changes; λ sc represents the penalty coefficient of the front-wheel steering angle, and the greater the angle, the heavier the penalty; ψ i represents the front-wheel steering angle of the current child node; ψ i-1 represents the front-wheel steering angle at the previous moment; |δ d | represents the change amount of the vehicle's attitude angle of each child node relative to the previous moment, and so on; and g d (δ d ) represents the weighted penalty function for the forward and backward actions of the vehicle, which is obtained from Equation (21), specifically as follows:

[0131]

[0132] In Equation (21), λ bc represents the penalty coefficient for the vehicle to reverse.

[0133] Figure 6 Figure is a schematic diagram for exploring child nodes from the current node. As Figure 6 shown, at each parent node, child nodes are searched in the front and rear sectors. Assuming the wheels are turned in one direction and there are m gears to choose from, there are a total of 4m + 2 possibilities.

[0134] After calculating the vehicle coordinates and directions represented by each child node, the program will attempt to connect the RS curve.

[0135] S5. Execute the RS curve linking task, and attempt to use the child nodes of the current path as the reverse gear shift points and connect the parking points; if the connection is successful, jump out of the loop, obtain the final planned path, and then use matlab simulation for verification to obtain the mine parking trajectory planning result with the maximum safety distance; if the connection fails, return to step S2 for recalculation. After attempting the rated number of times, if the connection still fails, return a failure message.

[0136] Figure 7 Figure is the flow chart of the RS curve linking method, and its specific algorithm flow is as Figure 7 shown.

[0137] Figure 8 Figure is the RS parking path planning result diagram. Using the optimized algorithm for matlab planning simulation, the result is as Figure 8 shown, Figure 8The area swept by the vehicle's border can avoid the tunnel wall in narrow spaces, and the gear shift points are reasonably selected.

[0138] Figure 9 This is the comparison chart of the minimum obstacle distance between the improved Chebyshev SAT algorithm and the traditional hybrid A* algorithm of the present invention. As Figure 9 shown, the improved Chebyshev SAT algorithm of the present invention is superior to the traditional hybrid A* algorithm in maintaining the obstacle distance. The unimproved traditional hybrid A* algorithm is very close to the tunnel wall near the parking point, and the closest point is only 0.0955 meters, while the improved Chebyshev SAT algorithm actively maintains a distance of more than 0.2536 meters.

[0139] Figure 10 This is the distribution chart of the child nodes traversed in the algorithm of the present invention. As Figure 10 shown, before the RS curve connection, for the child nodes of all path points explored by the algorithm, their exploration directions are guided by the heuristic formula (18).

[0140] A well - mining parking trajectory planning method based on Chebyshev distance determination according to the present invention. Aiming at the characteristics of the lack of GPS positioning signals and poor wireless communication environment in the underground tunnel environment, and at the same time to avoid the space waste brought by the grid method, the present invention vectorizes the map information and vehicle attitude information, improves the space utilization rate and operation efficiency in narrow scenarios, and can adjust the planned path according to sensor data in the case of lacking high - precision positioning correction and high - precision maps, and makes the vehicle stay away from the tunnel wall and obstacles as much as possible on the premise of ensuring the success of parking planning.

[0141] A well - mining parking trajectory planning method based on Chebyshev distance determination according to the present invention. Aiming at the characteristic that traditional planning algorithms require artificially set safety distances, by introducing the Chebyshev distance algorithm to improve the existing separating axis projection method, the planned path can automatically adjust the safety distance from the mine wall according to the scene characteristics, while improving the space utilization rate and reducing the risk of planning failure.

[0142] A well - mining parking trajectory planning method based on Chebyshev distance determination according to the present invention. Aiming at the difficulties in completing dynamic mining auxiliary tasks such as reverse parking, parallel parking, and dynamic scheduling in the narrow environment and limited - vision scenarios of the mine, the present invention introduces the improved SAT algorithm of Chebyshev distance into the hybrid A* algorithm. By improving the heuristic function, it reduces the number of reverse drives, reduces the steering amplitude, improves the reliability of the planned path, avoids the vehicle from repeatedly moving at intersections, and the optimized planned path effectively improves the operation efficiency of the loading and unloading area.

[0143] The above is an exemplary description of the invention. Obviously, the specific implementation of the present invention is not limited by the above methods. As long as such non-substantial improvements are made by adopting the method concept and technical solution of the present invention, or the concept and technical solution of the invention are directly applied to other occasions without improvement, they are all within the protection scope of the present invention.

Claims

1. A parking trajectory planning method for underground coal mines based on Chebyshev distance determination, characterized in that It includes the following steps: S1. According to SLAM real-time mapping, convert the map scene into a planar map, and convert the road information of the underground roadway into a combination of several line segments through a boundary detection algorithm; S2. Combine the SAT collision detection method, use the road boundary information as a generalized convex polygon, and compare it with the vehicle pose data to judge the collision relationship; S3. Introduce the Chebyshev distance judgment, and use the separating axis projection method during the SAT collision detection process to calculate the minimum distance between the vehicle and the boundary; S4. Improve the heuristic function according to the result of step S3, add the obstacle distance penalty during the exploration process, and make the movement trajectory away from the wall; S5. Execute the RS curve linking task, and try to use the sub-nodes of the current path as the reverse gear shifting points and connect the parking points; If the connection is successful, jump out of the loop, obtain the final planned path, and then use matlab simulation for verification to obtain the mine parking trajectory planning result with the maximum safety distance; If the connection fails, return to step S2 to recalculate. After trying the rated number of times, if the connection still fails, return the failure information.

2. The method for planning the parking trajectory of an underground mine based on Chebyshev distance determination according to claim 1, characterized in that In steps S2 and S3, using the road boundary information as a generalized convex polygon specifically means: using the line segments in the road boundary information as generalized convex polygons so that the SAT collision detection method can be used; at the same time, add the Chebyshev distance judgment in the separating axis algorithm to replace the traditional Euclidean distance calculation.

3. A well - mining parking trajectory planning method based on Chebyshev distance determination according to claim 1, characterized in that In step S3, the Chebyshev distance, expressed as the infinity norm distance, is used as a metric in the vector space and defines the maximum absolute difference in the coordinates of two points in each dimension; if two points in an n-dimensional space are respectively: p = (p1, p2,..., p n ), q = (q1, q2,..., q n ), their Chebyshev distance can be calculated by formula (2): d Chebyshev (p,q) = max(|p1 - q1|, |p2 - q2|,..., |p n -q n |) (2), In formula (2), p1, p2, p n respectively represent the projection coordinates of point p on the first, second, and nth coordinate axes, and q1, q2, q n respectively represent the projection coordinates of point q on the first, second, and nth coordinate axes; the maximum projection distance of the endpoints of two vectors on the coordinate axes can be obtained in high dimensions through formula (2); when measuring the maximum projection distance of two convex polygons, each projection direction in SAT can be regarded as a dimension, and perpendicular projections need to be performed along each side of the convex polygon; the top view of a vehicle usually appears as a rectangle, and its projection direction only needs to be determined twice, which reduces the amount of calculation to a certain extent; The Chebyshev distance can be used as the upper bound of the Euclidean distance. For two points A(x1, y1) and B(x2, y2) on the two-dimensional plane, their relationship is defined as shown in formula (3): In formula (3), d Chebyshev (A, B) represents the Chebyshev distance between two points A and B, and d Euclidean (A, B) represents the Euclidean distance between two points A and B; In a restricted underground tunnel environment, the geometric shape of a vehicle is determined using an oriented bounding box method; it is assumed that the vehicle body is rectangular with a width of W v , a length of L v , and a rear overhang of R ov , according to the geometric relationship, the coordinates of each point of the vehicle can be expressed by formulas (4)-(7) as follows: In formulas (4)-(7), are the coordinates of four points of the rectangular vehicle body, -ψ n represents the angle of the current vehicle's attitude relative to the X-axis direction in the Cartesian coordinate system, p os_X represents the X-axis coordinate of the center of the vehicle's rear axle, p os_Y represents the Y-axis coordinate of the center of the vehicle's rear axle; By using the online drawing technology, a real-time map composed of several line segments can be obtained, and the boundary information of each obstacle can be obtained, and it is converted into new coordinates under the SAT projection, as shown in formulas (8) and (9): In formulas (8) and (9), (X os_ψ , Y os_ψ ) and (X oe_ψ , Y oe_ψ ) represent the projections of the starting and ending coordinates of the obstacle boundary segment in the new coordinate system after rotation.

4. A well - operated mine parking trajectory planning method based on Chebyshev distance determination according to claim 1, characterized in that, The separating axis projection method refers to rotating and projecting each side of the convex polygon to find the gap between the object projections to judge whether the object collides; the specific steps are as follows: The projection direction of each reference line can be calculated using the arctan2 function, and its range is (-π, π], as shown in formula (10): In formula (10), ψ ref_i represents the projection direction of the extension direction of the boundary of the wall numbered i in the Cartesian coordinate system, x 1_ref_i represents the X-axis coordinate at the starting point of the i-th wall surface, x 2_ref_i represents the X-axis coordinate at the ending point of the i-th wall surface, y 1_ref_i represents the Y-axis coordinate at the starting point of the i-th wall surface, y 2_ref_i represents the Y-axis coordinate at the ending point of the i-th wall surface; Based on the derivation of formulas (2) and (4) to (10), the improved version of the Chebyshev distance formula can be rewritten and shown in formula (11) mathematically: d SAT_Chebyshev (vehicle, obstacle) = min(d Che_obs_1 , d Che_obs_2 ,..., d Che_obs_n ) (11), In formula (11), vehicle represents the vehicle controlled by the autonomous driving system, obstacle represents various obstacles that the autonomous driving vehicle may encounter during driving, and d SAT_Chebyshev represents the minimum Chebyshev distance between the vehicle and all nearby obstacles, and d Che_obs_n represents the maximum Chebyshev distance between the vehicle and the nth obstacle; Distance d Che_obs_i represents the maximum Chebyshev distance between each obstacle boundary and the vehicle, where i = 1, 2,..., n, as shown in Equation (12): d Che_obs_i = max(d obs_i_ref_v1 , d obs_i_ref_v2 , d obs_i_ref_o1 , d obs_i_ref_o2 ,..., d obs_i_ref_on ) (12), d obs_ref (vehicle, obs, ref) = max(d1, d2, d3, d4) (13), In Equation (13), d obs_ref (vehicle, obs, ref) represents the maximum value of the difference in the starting and ending positions between the two line segments of each vehicle boundary and obstacle boundary in the reference projection direction. To determine whether the projections of the two line segments overlap on the X-axis and Y-axis, d1, d2, d3, and d4 are introduced to represent the differences in the starting and ending positions of the two line segments in the shaded part of the projection direction; when the condition is satisfied: vehMinX > obsLineMaxX || vehMaxX < obsLineMinX||vehMinY>obsLineMaxY||wehMaxY<obsLineMinY, that is, when the two line segments do not intersect, the values of d1, d2, d3, and d4 can be obtained according to formulas (14) and (15), specifically as follows: In equations (14) and (15), obsMaxX represents the maximum value of the current obstacle boundary segment on the X-axis of the reference projection direction, obsMinX represents the minimum value of the current obstacle boundary segment on the X-axis of the reference projection direction, obsMaxY represents the maximum value of the current obstacle boundary segment on the Y-axis of the reference projection direction, obsMaxY represents the minimum value of the current obstacle boundary segment on the Y-axis of the reference projection direction, vehMaxX represents the maximum value of the current vehicle boundary segment on the X-axis of the reference projection direction, vehMinX represents the minimum value of the current vehicle boundary segment on the X-axis of the reference projection direction, vehMaxY represents the maximum value of the current vehicle boundary segment on the Y-axis of the reference projection direction, and vehMinY represents the minimum value of the current vehicle boundary segment on the Y-axis of the reference projection direction.

5. A method for planning the parking trajectory of an underground mine based on Chebyshev distance determination according to claim 1, characterized in that, The specific steps of step S4 are as follows: To ensure that the planned path is as short as possible, the cumulative cost function includes a term for the total path length; in addition, to prevent excessive reverse and steering operations, the penalties for these actions are integrated into the cumulative cost function; the formula for the cumulative cost function is shown in equation (17): In Equation (17), G shift (s) represents the cumulative cost of the shifting action, and r i is the forward and reverse conversion flag of the i-th node, and λ shift is the shifting penalty weight coefficient; This improvement adds the vehicle-obstacle Chebyshev distance to the heuristic function of the local trajectory, which can be used as part of the heuristic; in each exploration of the algorithm, a penalty for the obstacle distance is added, as shown in equation (18): In Equation (18), H Chebyshev (s) represents the obstacle distance penalty cost of the entire planned path, and d Chebyshev_i represents the Chebyshev metric of the distance between the vehicle and the obstacle during each exploration, and λ distance is the corresponding collision avoidance penalty coefficient; Through equations (17) and (18), the improved evaluation function is obtained, as shown in equation (19): F(s) = G(s) + G shift (s) + H(s) + H Chebyshev (s) (19), In equation (19), H(s) represents the heuristic function guiding to the target position, and G(s) represents the penalty for reverse and direction change in the planned path, as shown in equation (20): In formula (20), λ scc represents the penalty coefficient when the steering angle of the vehicle's front wheels changes; λ sc represents the penalty coefficient of the front-wheel steering angle, and the larger the angle, the heavier the penalty; ψ i represents the front-wheel steering angle of the current child node; ψ i-1 represents the front-wheel steering angle at the previous moment; |δ d | represents the change in the vehicle's attitude angle of each child node relative to the previous moment; g d (δ d ) represents the weighted penalty function for the forward and backward movements of the vehicle, which is obtained from formula (21) and is specifically as follows: In formula (21), δ d represents the single displacement distance in the planned trajectory, and λ bc represents the penalty coefficient for the vehicle to reverse; at each parent node, search for child nodes in the front and rear sectors; assume that the wheels are turned in one direction and there are m gears to choose from, with a total of 4m + 2 possibilities; after calculating the vehicle coordinates and directions represented by each child node, the program will attempt to connect the RS curve.

Images