Stratified path planning method for crawler mechanism of strip mine dump truck

Abstract

This invention belongs to the field of autonomous navigation technology, specifically a hierarchical path planning method for a tracked mechanism of an open-pit mine dumper. It includes: S1: acquiring 3D point cloud data of the open-pit mine environment and generating a dynamic 2D grid map with safety margins; S2: performing global hierarchical path planning on the dynamic 2D grid map to obtain a continuously executable globally optimized path; S3: performing local path planning under the constraints of the globally optimized path to achieve dynamic obstacle avoidance; S4: matching the local trajectory obtained from the local path planning with the globally optimized path, and applying deviation constraints and continuity corrections to the local trajectory to form a final travel path that satisfies the kinematic constraints of the tracked mechanism. This invention uses UAV point clouds to construct a dynamic grid map, breaking through the limitations of traditional vehicle-mounted SLAM in large scenes, and improving the comprehensiveness and safety of environmental perception through periodic updates and safety margin design.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation technology, specifically a layered path planning method for tracked mechanisms of open-pit mine dumping machines. Background Technology

[0002] With the development of intelligent mining technology, autonomous navigation of open-pit mine vehicles has become a core element in achieving safe and efficient production. Tracked mechanisms used in open-pit mines (such as dump trucks and spoil heaps) are characterized by heavy loads, low speeds, and high inertia. Their operating environment presents challenges such as large terrain elevation differences, dynamic changes in obstacles, and severe dust interference, making it difficult for traditional path planning algorithms to be directly adapted.

[0003] In existing technologies, map building largely relies on vehicle-mounted SLAM methods, which are prone to poor global consistency and insufficient real-time performance in large-scale open-pit mine scenarios; global path planning algorithms (such as traditional A / B mapping) also suffer from these issues. RRT The algorithm suffers from low search efficiency and excessive node redundancy. It also fails to adequately consider the kinematic constraints of the tracked mechanism, resulting in non-executable segments such as sharp turns with small radii in the generated path. Trajectory optimization is mostly a post-processing mode, which is disconnected from global planning and makes it difficult to ensure overall continuity. The evaluation function of local obstacle avoidance algorithms (such as traditional DWA) does not take into account both heading smoothness and energy consumption constraints, and is prone to getting trapped in local optima, leading to frequent turning of the tracked mechanism and excessive energy consumption.

[0004] The aforementioned problems collectively restrict the autonomous navigation performance of tracked mechanisms in open-pit mines under complex scenarios. Therefore, there is an urgent need for a hierarchical path planning method that takes into account global optimality, local real-time performance, and engineering feasibility. Summary of the Invention

[0005] In order to solve the problems existing in the prior art, the present invention provides a layered path planning method for tracked mechanisms of open-pit mine dumping machines.

[0006] This invention adopts the following technical solution: a layered path planning method for a tracked mechanism of an open-pit mine dump truck, comprising: S1: Acquire three-dimensional point cloud data of the open-pit mine environment, preprocess the point cloud data, and generate a dynamic two-dimensional raster map with safety margin. S2: Perform global hierarchical path planning on a dynamic two-dimensional grid map. After searching through the coarse planning layer and the fine planning layer, an initial global path is generated. The initial global path is then smoothed to obtain a continuous and executable global optimized path. S3: Under the constraints of the global optimization path, local path planning is performed. Based on the current environmental perception information, a speed candidate set is generated through adaptive speed sampling, and the optimal speed command is selected by combining the "environment-attitude-energy" dynamic weighted evaluation function to correct the local driving trajectory and achieve dynamic obstacle avoidance. S4: Match the local trajectory obtained from the local path planning with the global optimized path, and perform deviation constraints and continuity corrections on the local trajectory to form a final travel path that satisfies the kinematic constraints of the track mechanism.

[0007] In some embodiments, step S1 includes: S11: Periodically collect three-dimensional point cloud data of the open-pit mine operation area using a drone equipped with a lidar to obtain raw point cloud data; S12: The original point cloud data is sequentially subjected to voxel filtering downsampling, DGCNN point cloud segmentation to separate the ground and obstacles, and binarization to obtain an initial raster map; S13: Perform dilation processing on the initial raster map, set the dilation radius, and establish a safety margin; S14: By periodically revisiting and updating point cloud data using drones, dynamic incremental updates of the raster map are achieved, resulting in a dynamic two-dimensional raster map with a safety margin.

[0008] In some embodiments, step S2 includes: S21: Perform coarse planning layer path search, perform geometric shortest path search based on the grid map to obtain an initial connected path composed of discrete grid nodes; sample the initial connected path in segments at predetermined path arc length intervals, extract waypoint sets, and use the waypoint sets as the segment start and end points of the subsequent fine planning layer path search stage in sequence. S22: Based on the waypoints generated in the coarse planning layer, perform path search in the fine planning layer to generate an initial global path composed of discrete nodes; S23: After completing the global fine-planning layer path search and obtaining the initial global path composed of discrete nodes, the initial global path is smoothed using a cubic B-spline curve.

[0009] In some embodiments, step S22 includes: S221: Construct a segmented search space on a dynamic two-dimensional raster map, limiting the search area to a preset width centered on the waypoint lines. D A strip-shaped area; S222: Establish a node cost function within the segmented search space: in, To start from the segmentation point The cumulative path cost to node n It is a heuristic function; The heuristic function is: ; in , The coordinates of the current node. The coordinates of the target waypoint , These are the weighting coefficients. The vehicle's current facing angle. The heading angle from the current waypoint to the next waypoint. For the angle normalization operator, mapped to , , These are the weighting coefficients. The desired attitude angle for the target point; S223: During the node expansion process, the segmented search space is randomly sampled with a preset probability to generate sampled nodes. If the sampled node satisfies the passability constraint, it is inserted into the open list and participates in subsequent path expansion. S224: Select the node with the smallest integrated node cost function from the open list for neighborhood expansion, and participate in path update together with the sampled nodes; S225: When the search reaches the end of a segment, backtrack to generate segmented paths, and sequentially concatenate all segmented paths to obtain an initial global path composed of discrete nodes.

[0010] In some embodiments, step S3 includes: S31: Based on the kinematic constraints of the track mechanism, construct a dynamic window for velocity and angular velocity; S32: Adaptively sample the speed parameters within the dynamic window to generate a speed candidate set, and predict the corresponding local driving trajectory based on the speed candidate set; S33: Based on the predicted local driving trajectory, a dynamic weighted evaluation function of "environment-attitude-energy" is constructed in combination with environment, attitude, and energy constraints. The candidate speeds are scored, and the speed command with the best score is selected as the current control command, so that the tracked mechanism travels along a collision-free trajectory, thereby achieving dynamic obstacle avoidance.

[0011] In some embodiments, step S31 includes: S311: Establish the differential kinematic model of the track mechanism: Among them, the linear velocity of the track mechanism is angular velocity is The posture is ; S312: Based on the physical limits of the track mechanism, set the linear velocity and angular velocity constraint range; S313: Let the linear velocity and angular velocity at the current moment be respectively... , The acceleration limit is , The sampling time interval is The achievable speed range is: , The intersection of the linear velocity and angular velocity constraint intervals and the acceleration reachable intervals yields the dynamic window.

[0012] In some embodiments, the adaptive speed sampling process in step S32 is as follows: S321: Define the velocity sampling space, which is determined by kinematic and mechanical constraints. , in , For linear velocity boundary, , Where B is the track center distance; By applying an acceleration reachability constraint, an acceleration reachability interval is formed, and the rate of change of velocity satisfies the acceleration constraint. , ; in , These are the upper limits of linear acceleration and angular acceleration, respectively. To control the cycle; Then, the intersection of the acceleration reachable interval and the velocity sampling space is taken to obtain the sampleable velocity interval for the current period; S322: Calculate the adaptive sampling step size based on the obstacle density factor. and heading deviation Adaptive adjustment of sampling step size: , in , The nominal sampling step size, , This is the adjustment coefficient; S323: After determining the constraint interval and sampling step size, the velocity interval is discretized to generate a velocity candidate set: ; S324: Based on the kinematic model of the tracked mechanism, for each set of velocity pairs According to the control cycle Perform forward prediction to generate a corresponding set of local predicted trajectories. .

[0013] In some embodiments, step S33 includes: S331: Perform collision detection on the local predicted trajectory set output in step S32. If the trajectory overlaps with the obstacle grid or the nearest obstacle is located... If the velocity pair corresponding to the trajectory is deemed infeasible, it will be removed from the candidate set. S332: For speed pairs that were not eliminated Construct a dynamic weighted evaluation function: In the formula, , , , , For dynamic weighting coefficients, satisfying ; in, The target deviation term is used to guide the vehicle in tracking the global waypoint heading; its expression is: ; In the formula, The desired heading angle from the vehicle's current position to the local target waypoint. This is the vehicle's current real-time heading angle; The obstacle distance term is used to quantify the collision risk of the trajectory, and its expression is: In the formula, To predict the distance between the trajectory and the nearest obstacle, This represents the upper limit of the maximum sensing distance for vehicle-mounted sensors. The preset safe distance threshold; The speed maintenance term encourages vehicles to maintain a steady speed, and its expression is: In the formula, This represents the vehicle's maximum forward linear speed. The heading smoothing term is used to constrain abrupt changes in angular velocity, and its expression is: In the formula, For the current period, candidate angular velocity, This represents the actual output angular velocity of the previous cycle. This is the vehicle's maximum angular velocity; This is the energy consumption cost term, used to optimize vehicle driving energy consumption, and its expression is: In the formula, For the instantaneous power consumption model of the vehicle, , These are the power coefficients corresponding to linear velocity and angular velocity, respectively. S333: Select the candidate velocity pairs that satisfy the score. The speed is output as the current control command to drive the tracked mechanism to travel along the corresponding collision-free predicted trajectory, thereby achieving dynamic obstacle avoidance.

[0014] In some embodiments, in step S1, the update cycle of the dynamic grid map is consistent with the UAV revisit cycle; the safety margin is set by a morphological dilation operator, the dilated grid map is marked as feasible and infeasible regions, and a safety cost is assigned to the pixels covered by the dilation circle. .

[0015] Step S4 includes: S41: Obtain the global optimized path and the set of local predicted trajectories; S42: For each local trajectory The End In the global optimization path Search for the nearest projection point To obtain the corresponding parameters : and will As a global matching point for this local trajectory; S43: Calculate the lateral deviation of the local trajectory endpoint relative to the global matching point. deviation from heading angle : , in, The heading angle at the local trajectory endpoint. To optimize the path globally The tangential direction angle at point , if it satisfies: and Then it is determined that the local trajectory satisfies the deviation constraint; S44: For local trajectories that satisfy the deviation constraints, select the connection interval between them and the global optimization path, and construct a cubic B-spline connection segment. This ensures that the connecting segments satisfy the pose continuity constraint: , ; in, For this local trajectory The final pose, To optimize the pose of matching points on the global path; and to ensure that the connecting segments are at the starting point The tangential direction at the endpoint is consistent with the tangential direction at the local trajectory endpoint. The tangent at the point is consistent with the tangent of the globally optimized path at the matching point to ensure that the position and heading of the path are continuous at the connection point; Local trajectory Connecting section In the global optimization path The following paragraph By piecing together the data in chronological order of travel, the merged path is obtained: ; In the formula, Optimize the path globally upper matching point The corresponding curve parameters, To optimize the remaining path segments from the matching point to the destination in the global optimization path; S45: Among all fusion paths that satisfy the deviation constraints and complete the continuity correction, select the path with the lowest path cost as the final driving path output.

[0016] Compared with the prior art, the present invention has the following beneficial effects: This invention utilizes UAV point clouds to construct a dynamic grid map, overcoming the limitations of traditional vehicle-mounted SLAM in large-scene scenarios. Through periodic updates and safety margin design, it enhances the comprehensiveness and safety of environmental perception. Combining orientation prediction heuristics and key point selection strategies with B-spline smoothing significantly reduces redundant nodes and non-executable small-radius turns, resulting in shorter path lengths compared to traditional A / B SLAM. RRT The time taken for navigation was shortened by 2.46% and 8.21% respectively, and the number of inflection points was reduced by 88.62% and 61.60% respectively. Through adaptive speed sampling and a dynamic weighted evaluation function, both heading smoothing and energy consumption optimization were considered, avoiding local oscillations and resulting in faster dynamic obstacle avoidance response. The global-local collaborative architecture balanced planning efficiency and execution stability. The prototype test showed a maximum lateral deviation of only 9.1cm and a maximum heading error of 7.39°, meeting the requirements for low-speed, heavy-load operations in open-pit mines. Furthermore, the average running time of the algorithm was significantly shorter than that of Hybrid A. It shortens the time by 86.04% and has strong applicability to engineering projects. Attached Figure Description

[0017] Figure 1 This is a schematic diagram of the layered path planning method for the tracked mechanism of the open-pit mine dumping machine in an embodiment of the present invention; Figure 2 This is a flowchart of the global path planning algorithm according to an embodiment of the present invention; Figure 3 This is an improvement A of the present invention. The simulation results of the ablation comparison of the algorithm in scenario A are shown in the figure. Figure 4 This is an improvement A of the present invention. The simulation results of the ablation comparison of the algorithm in scenario B are shown in the figure; Figure 5This is an improvement A of the present invention. The simulation results of the ablation comparison of the algorithm in scenario C are shown in the figure; Figure 6 These are simulation results of path planning in scenario A for different algorithms according to embodiments of the present invention; Figure 7 These are simulation results of path planning in scenario B using different algorithms from this embodiment of the invention. Figure 8 These are simulation results of path planning in scenario C for different algorithms according to embodiments of the present invention; Figure 9 This is a kinematic model diagram of the tracked mechanism for open-pit mines according to an embodiment of the present invention; Figure 10 This is an azimuth view of the open-pit mine tracked mechanism according to an embodiment of the present invention; Figure 11 This is a flowchart of the improved DWA algorithm according to an embodiment of the present invention; Figure 12 This is an obstacle layout diagram of scenes A and C in the Gazebo simulation test of this invention embodiment; Figure 13 This is a dynamic obstacle avoidance path diagram of scenarios A and C in the Rviz simulation test of this invention embodiment; Figure 14 This is an architecture diagram of the tracked prototype navigation system according to an embodiment of the present invention; Figure 15 This is a path tracking route map for global navigation testing according to an embodiment of the present invention; Figure 16 This is an obstacle layout and obstacle avoidance route diagram for a local obstacle avoidance test according to an embodiment of the present invention. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other implementation methods obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0019] like Figure 1 As shown, a layered path planning method for a tracked mechanism of an open-pit mine dumper includes: S1: Acquire three-dimensional point cloud data of the open-pit mine environment, preprocess the point cloud data, and generate a dynamic two-dimensional raster map with safety margin. S2: Perform global hierarchical path planning on a dynamic two-dimensional grid map. After searching through the coarse planning layer and the fine planning layer, an initial global path is generated. The initial global path is then smoothed to obtain a continuous and executable global optimized path. S3: Under the constraints of the global optimization path, local path planning is performed. Based on the current environmental perception information, a speed candidate set is generated through adaptive speed sampling, and the optimal speed command is selected by combining the "environment-attitude-energy" dynamic weighted evaluation function to correct the local driving trajectory and achieve dynamic obstacle avoidance. S4: Match the local trajectory obtained from the local path planning with the global optimized path, and perform deviation constraints and continuity corrections on the local trajectory to form a final travel path that satisfies the kinematic constraints of the track mechanism.

[0020] In a specific embodiment, step S1 includes: S11: Periodically collect 3D point cloud data of the open-pit mine operating area using a drone equipped with a lidar system to obtain raw point cloud data; in this embodiment, a drone equipped with a lidar system scans the open-pit mine area to collect raw 3D point cloud data. The lidar system, combined with a high-frequency scanning and precise positioning system, comprehensively and in real-time acquires the terrain information of the mining area.

[0021] S12: The original point cloud data is sequentially subjected to voxel filtering downsampling, DGCNN point cloud segmentation to separate the ground and obstacles, and binarization processing to obtain an initial grid map; In this embodiment, the original point cloud data is processed by voxel grid filtering, and the amount of point cloud data is reduced by downsampling to eliminate noise; the preprocessed point cloud data is segmented into ground, and the ground and non-ground parts are automatically identified and extracted by the existing DGCNN point cloud segmentation algorithm; the point cloud is binarized, and the segmented ground and obstacle areas are marked as feasible (0) or infeasible (1) grids by using a point cloud library (PCL).

[0022] S13: The initial grid map is dilated, the dilation radius is set, and a safety margin is established. In this embodiment, the grid map is first dilated using a morphological dilation operator to set the dilation radius to a combined margin of 3m for vehicle width and positioning error. The obstacles are spatially expanded to establish a safety margin. Finally, boundary constraints are applied to the map and the working area of ​​vehicle activity is preserved to obtain an dilated map for global path search.

[0023] Algorithm 1 below provides pseudocode for generating a safe buffer on an occupied grid. This process uses a binary occupied graph. Safe distance With security costs As input, first in the planning area ( Morphological expansion of the barrier is performed, with a construction radius of [missing information]. The disk-shaped structural elements are used to expand the obstacle boundaries, resulting in an inflated map with a safety margin. Subsequently, a security cost is assigned to each node on the set of pixels covered by the expansion circle. This forms a cost layer consistent with the safety buffer. The mathematical expression for a safety buffer is: in, It is the geometric center of obstacle o (or the centroid of the set of outline pixels). For any grid node, the safety radius is... The cost is increased to: To facilitate implementation on a grid, the continuous domain disk of the safety buffer is pixelated by radius. The discrete disk structure element is approximated; the cost of any grid node corresponds to the cost of the pixel within the dilation zone.

[0024] S14: By periodically revisiting and updating point cloud data using drones, dynamic incremental updates of the raster map are achieved, resulting in a dynamic two-dimensional raster map with a safety margin.

[0025] In specific embodiments, such as Figure 2 As shown, step S2 includes: S21: Perform coarse planning layer path search, perform geometric shortest path search based on the grid map to obtain an initial connected path composed of discrete grid nodes; sample the initial connected path in segments at predetermined path arc length intervals, extract waypoint sets, and use the waypoint sets as the segment start and end points of the subsequent fine planning layer path search stage.

[0026] Specifically, step S21 includes: on a dynamic two-dimensional raster map with a safety margin, starting from the raster node... s With the endpoint grid node g To constrain this, an initial connected path is obtained using shortest path search based on adjacency expansion. , , , ; The search objective is to minimize the path cost. ,in, The distance between adjacent nodes. This refers to the distance weighting coefficient. Calculate the initial connected path cumulative arc length Let the predetermined sampling interval be... In order to satisfy nodes Extract waypoints to obtain a set of waypoints. , , ; Adjacent waypoint pairs The segment boundaries that constitute the global path search in the refinement stage will... As the starting point and ending point of the segment in step S22, a fine-planning layer path search is performed between each waypoint to obtain segmented paths, which are then spliced ​​together to form an initial global path.

[0027] S22: Based on the waypoints generated by the coarse planning layer, a fine planning layer path search is performed, introducing a heuristic function and an RRT-style random sampling mechanism to generate an initial global path composed of discrete nodes; Step S22 includes: Adjacent waypoint pairs Using the boundary as the boundary, a segmented search space is constructed on a dynamic two-dimensional raster map. The search area is limited to a preset width centered on the line connecting waypoints. D A strip-shaped area; In the segmented search space Internal node creation cost function: in, To start from the segmentation point The cumulative path cost to node n An improved heuristic function; The improved heuristic function is defined as follows: ; in , The coordinates of the current node. The coordinates of the target waypoint; , These are the weighting coefficients. The vehicle's current facing angle. The heading angle from the current waypoint to the next waypoint. For the angle normalization operator, mapped to ; , These are the weighting coefficients. The desired attitude angle for the target point; During node expansion, with a preset probability For the segmented search space Perform random sampling to generate sampling nodes. If the sampling node satisfies the passability constraint, it is inserted into the open list and participates in subsequent path expansion; Select comprehensive cost from the open list The smallest node undergoes neighborhood expansion and participates in path updating together with the randomly sampled nodes; when the search reaches the segment endpoint... At that time, backtrack to generate segmented paths: For all segmented paths By sequentially concatenating the nodes, we obtain an initial global path composed of discrete nodes: .

[0028] S23: After completing the global fine-planning layer path search and obtaining the initial global path composed of discrete nodes, the initial global path is smoothed using a cubic B-spline curve.

[0029] The smoothing process for cubic B-spline curves satisfies the following constraints: Let the set of discrete path nodes be... ,in, Starting from, The endpoint is defined as the set of control points for the cubic B-spline curve, with the nodes in the discrete node set serving as the control point set.

[0030] The control point set is fitted using a cubic B-spline curve, and the expression for the cubic B-spline curve is as follows: , ,in The basis functions are cubic B-spline functions; for the smooth path Calculate the curvature, the curvature of the curve is and satisfy the following constraints ;in The minimum turning radius of the tracked mechanism is defined; if the curvature does not meet the constraints, the control points are locally adjusted and a new B-spline curve is generated. Endpoint constraints are used to fix the coordinates and orientation of the start and end points. A smooth path that satisfies the curvature constraints is then established. It serves as the output of the global optimized path and as a reference constraint in the subsequent local path planning stage.

[0031] This application introduces a "prediction" mechanism to construct an improved heuristic function based on the traditional distance heuristic: using the Euclidean distance from the current node to the target waypoint as the basic heuristic, if there is a next waypoint, the deviation between the vehicle's current orientation and the next target direction is added to the heuristic to encourage the vehicle to gradually adjust its attitude before reaching the current waypoint; in the final segment, the heuristic function assigns higher weight to the deviation between the vehicle's final attitude and the target attitude to ensure that the vehicle can meet the final orientation requirements while reaching the target position.

[0032] In this embodiment, as Figure 3-5 The simulation results of ablation in different scenarios are shown in the figure. To verify the effectiveness of the proposed method, A... Using the original algorithm as a baseline, ablation experiments were conducted to verify the proposed A algorithm based on an improved heuristic function and keypoint selection strategy. The superiority of an algorithm can be evaluated by metrics including running time, number of inflection points, and path length.

[0033] As shown in Table 1 below, compared with traditional A Application A Improved heuristics and A Compared to the keypoint mechanism algorithm, the algorithm in this application reduces the average running time by 97.97%, 79.10%, and 73.95%, respectively. The path length is reduced by 4.56%, 0.32%, and 0.36%, respectively. The number of inflection points is reduced by 91.96%, 8.33%, and 40.00%, respectively.

[0034] Table 1 In this embodiment, as Figure 6-8 The simulation results of different algorithms in scenarios A, B, and C are shown in the figure. To verify the effectiveness of the proposed method, the simulation results are compared with those of the traditional method A. RRT Hybrid A A comparative analysis was conducted.

[0035] As shown in Table 2 below, although the improved algorithm of this application has a shorter runtime compared to the RRT that does not consider kinematic constraints, the improved algorithm of this application still achieves better performance in terms of runtime. With A There's a slight increase, but compared to the Hybrid A, which also considers motion constraints but isn't fully adapted to open-pit mining scenarios... Search time was reduced by approximately 86.04% or more; in terms of path length, it was significantly improved compared to Hybrid A, which did not adequately adapt to this type of constraint. Slightly increased compared to the average length, but compared to A and RRT On average, they shortened by 2.46% and 8.21% respectively; in terms of the number of inflection points compared to traditional A... On average, it reduced by 88.62%, compared to RRT. and Hybrid A They also decreased by an average of 61.60% and 44.17%.

[0036] Table 2 In a specific embodiment, step S3 includes: S31: Based on the kinematic constraints of the track mechanism, construct a dynamic window for velocity and angular velocity; Step S31 includes establishing a differential kinematic model of the track mechanism: Among them, the linear velocity of the track mechanism is angular velocity is The posture is ; Based on the physical limits of the track mechanism, the linear velocity and angular velocity constraint ranges are set as follows: , in, For the maximum linear velocity, This is the maximum angular velocity; Let the linear velocity and angular velocity at the current moment be respectively , The acceleration limit is , The sampling time interval is The achievable speed range is: , The intersection of the physical velocity constraint interval and the acceleration reachable interval yields the dynamic window: in: , The dynamic window W This serves as the constraint space for velocity sampling in step S32.

[0037] S32: Adaptively sample the speed parameters within the dynamic window to generate a speed candidate set, and predict the corresponding local driving trajectory based on the speed candidate set; In step S32, the adaptive speed sampling process is as follows: First, the velocity sampling space is determined, which is defined by kinematic and mechanical constraints: , in , For linear velocity boundary, , Where B is the track center distance; Apply an acceleration reachability constraint to form a dynamic reachability interval, where the rate of change of velocity satisfies the acceleration constraint. , in , These are the upper limits of linear acceleration and angular acceleration, respectively. To control the cycle; Then, the intersection of the acceleration reachable interval and the velocity sampling space is taken to obtain the sampleable velocity interval for the current period; S322: Calculate the adaptive sampling step size based on the obstacle density factor. and heading deviation Adaptive adjustment of sampling step size: , in , The nominal sampling step size, , This is the adjustment coefficient; After the constraint interval and sampling step size are determined, the velocity interval is discretized to generate a velocity candidate set: And based on the kinematic model of the track mechanism, the velocity pairs of each group were analyzed. According to the control cycle Perform forward prediction to generate a corresponding set of local predicted trajectories. This is provided for evaluation in step S33.

[0038] S33: Based on the predicted local driving trajectory, a dynamic weighted evaluation function of "environment-attitude-energy" is constructed in combination with environment, attitude, and energy constraints. The candidate speeds are scored, and the speed command with the best score is selected as the current control command, so that the tracked mechanism travels along a collision-free trajectory, thereby achieving dynamic obstacle avoidance.

[0039] Collision detection is performed on the local predicted trajectory set output in step S32. If the trajectory overlaps with the obstacle grid or the nearest obstacle is located, the collision detection is performed. If the velocity pair corresponding to the trajectory is deemed infeasible, it will be removed from the candidate set.

[0040] S331: Perform collision detection on the local predicted trajectory set output in step S32. If the trajectory overlaps with the obstacle grid or the nearest obstacle is located... If the velocity pair corresponding to the trajectory is deemed infeasible, it will be removed from the candidate set. S332: For speed pairs that were not eliminated Construct a dynamic weighted evaluation function: In the formula, , , , , For dynamic weighting coefficients, satisfying ; in, The target deviation term is used to guide the vehicle in tracking the global waypoint heading; its expression is: ; In the formula, The desired heading angle from the vehicle's current position to the local target waypoint. This is the vehicle's current real-time heading angle; The obstacle distance term is used to quantify the collision risk of the trajectory, and its expression is: In the formula, To predict the distance between the trajectory and the nearest obstacle, This represents the upper limit of the maximum sensing distance for vehicle-mounted sensors. The preset safe distance threshold; The speed maintenance term encourages vehicles to maintain a steady speed, and its expression is: In the formula, This represents the vehicle's maximum forward linear speed. The heading smoothing term is used to constrain abrupt changes in angular velocity, and its expression is: In the formula, For the current period, candidate angular velocity, This represents the actual output angular velocity of the previous cycle. This is the vehicle's maximum angular velocity; This is the energy consumption cost term, used to optimize vehicle driving energy consumption, expressed as: In the formula, For the instantaneous power consumption model of the vehicle, , These are the power coefficients corresponding to linear velocity and angular velocity, respectively. S333: Select the candidate velocity pairs that satisfy the score. The speed is output as the current control command to drive the tracked mechanism to travel along the corresponding collision-free predicted trajectory, thereby achieving dynamic obstacle avoidance.

[0041] In this embodiment, to verify the obstacle avoidance performance of this application in a dynamic open-pit mine scenario, a realistic-scale tracked model of a dump truck and an open-pit mine operating environment were built in the Gazebo simulation platform, such as... Figure 12The image shows the obstacle layout for scenarios A and C in the Gazebo simulation test. The simulation system runs on Ubuntu 20.04 and ROS Noetic platforms. Navigation packages are installed in ROS, and relevant nodes are added and modified. In Navigation, move_base loads the map and path planning plugin through a preset interface, reads setting parameters, and publishes relevant speed command topics. The / base_control node receives the speed commands published by move_base.

[0042] The local obstacle avoidance simulation test process is as follows: This application first generates an optimal reference path at a global scale and extracts key waypoints to pass to the local DWA planner; when the vehicle encounters dynamic obstacles while traveling along the path, the local planning module updates the perception information in real time, samples and generates multiple sets of feasible trajectories in the velocity space through a dynamic window mechanism, and comprehensively evaluates indicators such as path deviation, obstacle distance, and speed smoothness to select the optimal obstacle avoidance speed and attitude adjustment strategy. Figure 13 As shown, the entire obstacle avoidance process achieves safe local detours and smooth attitude control while maintaining global path constraints.

[0043] In a specific embodiment, step S4 includes: S41: Input and Path Representation: Obtain the global optimization path obtained in step S23 and the set of local predicted trajectories obtained in step S3 The local predicted trajectory is represented by a discrete point sequence. The global optimization path It can be a continuous curve or a sequence of discrete sampling points.

[0044] S42: Local trajectory and global path matching: For each local trajectory The End In the global optimization path Search for the nearest projection point To obtain the corresponding parameters : And This serves as the global matching point for the local trajectory.

[0045] S43: Deviation Constraint Calculation Calculate the lateral deviation of the local trajectory endpoint relative to the global matching point. deviation from heading angle : , ;in, The heading angle at the local trajectory endpoint. To optimize the path globally The tangential direction angle at that point. If the following conditions are met: and Then it is determined that the local trajectory satisfies the deviation constraint.

[0046] S44: Continuity correction and path stitching: For local trajectories that satisfy the deviation constraints, select the connection interval between them and the global optimization path, and construct cubic B-spline connection segments. This ensures that the connecting segments satisfy the pose continuity constraint: , ; in, For this local trajectory The final pose, To optimize the pose of matching points on the global path; And make the connecting segment at the starting point The tangential direction at the endpoint is consistent with the tangential direction at the local trajectory endpoint. The tangent at the point is consistent with the tangent of the globally optimized path at the matching point to ensure that the position and heading of the path are continuous at the connection point; Local trajectory Connecting section In the global optimization path The following paragraph By piecing together the data in chronological order of travel, the merged path is obtained: In the formula, Optimize the path globally upper matching point The corresponding curve parameters, To optimize the remaining path segments from the matching point to the destination in the global optimization path; S45: Output the final driving path: the fused path that meets all deviation constraints and completes continuity correction. In the process, the path with the minimum path cost is selected as the final driving path output, where the path cost is used to characterize the path length and steering smoothness constraints.

[0047] In this embodiment, to further verify the adaptability and stability of the invention in real-world complex environments, a self-developed 1:10 scale tracked navigation prototype platform was used. Its overall structure is consistent with the actual equipment, and it employs dual-track independent motors for differential steering. For example... Figure 14The diagram shows the navigation system architecture of the tracked prototype. The prototype mainly consists of three parts: sensor equipment, host computer, and drive equipment. The sensor equipment includes a Livox Avia lidar with a built-in positioning IMU and an MV-CU120-10UC industrial camera, used to acquire environmental point cloud, visual image data, and chassis pose. The host computer uses NVIDIA Jetson AGX Orin as the main control platform, running Ubuntu 20.04 and ROS Noetic, and is responsible for the real-time calculation of the fusion perception and path planning algorithms. The drive equipment communicates with the ROS system via a CAN bus to realize track drive and attitude control. Specific parameters are shown in Table 3 below.

[0048] Table 3 Optionally, it also includes: conducting global path planning and tracking tests and local obstacle avoidance tests on the tracked prototype in a test field.

[0049] In this embodiment, the tracked prototype is placed in an open-pit mine simulation test field. The prototype needs to travel along the completed work area to the target point of the other end of the work area to process the next work area. Figure 15 The diagram shows the path tracking route for the global navigation test. The prototype's starting point A and target point B were set, and global planning and navigation tasks were executed in Rviz, enabling the prototype to successfully reach the target point with minimal positional error. As shown in Table 4, to verify the stability of the navigation system, path tracking tests were conducted during the navigation process. After five experiments, the maximum lateral deviation was 9.1 cm, the maximum average lateral deviation was 5.93 cm, and the average standard deviation was 2.4 cm. Simultaneously, the maximum heading deviation was 7.39°, the maximum average heading deviation was 5.66°, and the average standard deviation was 1.30°. The prototype's lateral deviation remained consistently within the centimeter range throughout the test route, and the heading deviation was also minimal.

[0050] Table 4 In this embodiment, based on the completion of the global navigation task, a local obstacle avoidance test was conducted at the test site to further verify the obstacle avoidance capability of the present invention in dynamic scenes. For example... Figure 16 The diagram shows the obstacle layout and obstacle avoidance route for the local obstacle avoidance test. The prototype achieved smooth and reliable dynamic obstacle avoidance in the field test, without any oscillation, sharp turns or stalling. It was also able to automatically return to the global path and continue to drive towards the target position after the obstacle avoidance was completed.

[0051] The above description is merely a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A layered path planning method for a tracked mechanism of an open-pit mine dumping machine, characterized in that, include: S1: Acquire three-dimensional point cloud data of the open-pit mine environment, preprocess the point cloud data, and generate a dynamic two-dimensional raster map with safety margin. S2: Perform global hierarchical path planning on a dynamic two-dimensional grid map. After searching through the coarse planning layer and the fine planning layer, an initial global path is generated. The initial global path is then smoothed to obtain a continuous and executable global optimized path. S3: Under the constraints of the global optimization path, local path planning is performed. Based on the current environmental perception information, a speed candidate set is generated through adaptive speed sampling, and the optimal speed command is selected by combining the "environment-attitude-energy" dynamic weighted evaluation function to correct the local driving trajectory and achieve dynamic obstacle avoidance. S4: Match the local trajectory obtained from the local path planning with the global optimized path, and perform deviation constraints and continuity corrections on the local trajectory to form a final travel path that satisfies the kinematic constraints of the track mechanism.

2. The layered path planning method for the tracked mechanism of an open-pit mine spoil heap according to claim 1, characterized in that, Step S1 includes: S11: Periodically collect three-dimensional point cloud data of the open-pit mine operation area using a drone equipped with a lidar to obtain raw point cloud data; S12: The original point cloud data is sequentially subjected to voxel filtering downsampling, DGCNN point cloud segmentation to separate the ground and obstacles, and binarization to obtain an initial raster map; S13: Perform dilation processing on the initial raster map, set the dilation radius, and establish a safety margin; S14: By periodically revisiting and updating point cloud data using drones, dynamic incremental updates of the raster map are achieved, resulting in a dynamic two-dimensional raster map with a safety margin.

3. The layered path planning method for the tracked mechanism of an open-pit mine dumping machine according to claim 1, characterized in that, Step S2 includes: S21: Perform coarse planning layer path search, perform geometric shortest path search based on the grid map to obtain an initial connected path composed of discrete grid nodes; sample the initial connected path in segments at predetermined path arc length intervals, extract waypoint sets, and use the waypoint sets as the segment start and end points of the subsequent fine planning layer path search stage in sequence. S22: Based on the waypoints generated in the coarse planning layer, perform path search in the fine planning layer to generate an initial global path composed of discrete nodes; S23: After completing the global fine-planning layer path search and obtaining the initial global path composed of discrete nodes, the initial global path is smoothed using a cubic B-spline curve.

4. The layered path planning method for the tracked mechanism of an open-pit mine dumping machine according to claim 3, characterized in that, Step S22 includes: S221: Construct a segmented search space on a dynamic two-dimensional raster map, limiting the search area to a preset width centered on the waypoint lines. D A strip-shaped area; S222: Establish a node cost function within the segmented search space: in, To start from the segmentation point The cumulative path cost to node n It is a heuristic function; The heuristic function is: ; in , The coordinates of the current node. The coordinates of the target waypoint , These are the weighting coefficients. The vehicle's current facing angle. The heading angle from the current waypoint to the next waypoint. For the angle normalization operator, mapped to , , These are the weighting coefficients. The desired attitude angle for the target point; S223: During the node expansion process, the segmented search space is randomly sampled with a preset probability to generate sampled nodes. If the sampled node satisfies the passability constraint, it is inserted into the open list and participates in subsequent path expansion. S224: Select the node with the smallest integrated node cost function from the open list for neighborhood expansion, and participate in path update together with the sampled nodes; S225: When the search reaches the end of a segment, backtrack to generate segmented paths, and sequentially concatenate all segmented paths to obtain an initial global path composed of discrete nodes.

5. The layered path planning method for the tracked mechanism of an open-pit mine dumping machine according to claim 1, characterized in that, Step S3 includes: S31: Based on the kinematic constraints of the track mechanism, construct a dynamic window for velocity and angular velocity; S32: Adaptively sample the speed parameters within the dynamic window to generate a speed candidate set, and predict the corresponding local driving trajectory based on the speed candidate set; S33: Based on the predicted local driving trajectory, a dynamic weighted evaluation function of "environment-attitude-energy" is constructed in combination with environment, attitude, and energy constraints. The speed candidate set is scored, and the speed command with the best score is selected as the current control command, so that the track mechanism travels along a collision-free trajectory, thereby realizing dynamic obstacle avoidance.

6. The layered path planning method for the tracked mechanism of an open-pit mine dumper according to claim 5, characterized in that, Step S31 includes: S311: Establish the differential kinematic model of the track mechanism: Among them, the linear velocity of the track mechanism is angular velocity is The posture is ; S312: Based on the physical limits of the track mechanism, set the linear velocity and angular velocity constraint range; S313: Let the linear velocity and angular velocity at the current moment be respectively... , The acceleration limit is , The sampling time interval is The achievable speed range is: ， The intersection of the linear velocity and angular velocity constraint intervals and the acceleration reachable intervals yields the dynamic window.

7. The layered path planning method for the tracked mechanism of an open-pit mine dumping machine according to claim 5, characterized in that, In step S32, the adaptive speed sampling process is as follows: S321: Define the velocity sampling space, which is determined by kinematic and mechanical constraints. ， in , For linear velocity boundary, , Where B is the track center distance; By applying an acceleration reachability constraint, an acceleration reachability interval is formed, and the rate of change of velocity satisfies the acceleration constraint. , ; in , These are the upper limits of linear acceleration and angular acceleration, respectively. To control the cycle; Then, the intersection of the acceleration reachable interval and the velocity sampling space is taken to obtain the sampleable velocity interval for the current period; S322: Calculate the adaptive sampling step size based on the obstacle density factor. and heading deviation Adaptive adjustment of sampling step size: ， in , The nominal sampling step size, , This is the adjustment coefficient; S323: After determining the constraint interval and sampling step size, the velocity interval is discretized to generate a velocity candidate set: ; S324: Based on the kinematic model of the tracked mechanism, for each set of velocity pairs According to the control cycle Perform forward prediction to generate a corresponding set of local predicted trajectories. .

8. The layered path planning method for the tracked mechanism of an open-pit mine dumping machine according to claim 5, characterized in that, Step S33 includes: S331: Perform collision detection on the local predicted trajectory set output in step S32. If the trajectory overlaps with the obstacle grid or the nearest obstacle is located... If the velocity pair corresponding to the trajectory is deemed infeasible, it will be removed from the candidate set. S332: For speed pairs that were not eliminated Construct a dynamic weighted evaluation function: In the formula, , , , , For dynamic weighting coefficients, satisfying ; in, The target deviation term, used to guide the vehicle in tracking the global waypoint heading, is expressed as: ; In the formula, The desired heading angle from the vehicle's current position to the local target waypoint. This is the vehicle's current real-time heading angle; The obstacle distance term is used to quantify the collision risk of the trajectory, and its expression is: In the formula, To predict the distance between the trajectory and the nearest obstacle, This represents the upper limit of the maximum sensing distance for vehicle-mounted sensors. The preset safe distance threshold; The speed maintenance term encourages vehicles to maintain a steady speed, and its expression is: In the formula, This represents the vehicle's maximum forward linear speed. The heading smoothing term is used to constrain abrupt changes in angular velocity, and its expression is: In the formula, For the current period, candidate angular velocity, This represents the actual output angular velocity of the previous cycle. This is the vehicle's maximum angular velocity; This is the energy consumption cost term, used to optimize vehicle driving energy consumption, and its expression is: In the formula, For the instantaneous power consumption model of the vehicle, , These are the power coefficients corresponding to linear velocity and angular velocity, respectively. S333: Select the candidate velocity pairs that satisfy the score. The speed is output as the current control command to drive the tracked mechanism to travel along the corresponding collision-free predicted trajectory, thereby achieving dynamic obstacle avoidance.

9. The layered path planning method for the tracked mechanism of an open-pit mine dumping machine according to claim 2, characterized in that, In step S1, the update cycle of the dynamic grid map is consistent with the revisit cycle of the UAV. The safety margin is set using a morphological dilation operator. The dilated raster map is marked as feasible and infeasible regions, and a safety cost is assigned to the pixels covered by the dilation circle. .

10. The layered path planning method for the tracked mechanism of an open-pit mine dumping machine according to claim 1, characterized in that, Step S4 includes: S41: Obtain the global optimized path and the set of local predicted trajectories; S42: For each local trajectory The End In the global optimization path Search for the nearest projection point To obtain the corresponding parameters : and will As a global matching point for this local trajectory; S43: Calculate the lateral deviation of the local trajectory endpoint relative to the global matching point. deviation from heading angle : ， in, The heading angle at the local trajectory endpoint. To optimize the path globally The tangential direction angle at point , if it satisfies: and Then it is determined that the local trajectory satisfies the deviation constraint; S44: For local trajectories that satisfy the deviation constraints, select the connection interval between them and the global optimization path, and construct a cubic B-spline connection segment. This ensures that the connecting segments satisfy the pose continuity constraint: , ; in, For this local trajectory The final pose, To optimize the pose of matching points on the global path; And make the connecting segment at the starting point The tangential direction at the endpoint is consistent with the tangential direction at the local trajectory endpoint. The tangent at the point is consistent with the tangent of the globally optimized path at the matching point to ensure that the position and heading of the path are continuous at the connection point; Local trajectory Connecting section In the global optimization path The following paragraph By piecing together the data in chronological order of travel, the merged path is obtained: In the formula, Optimize the path globally upper matching point The corresponding curve parameters, To optimize the remaining path segments from the matching point to the destination in the global optimization path; S45: Among all fusion paths that satisfy the deviation constraints and complete the continuity correction, select the path with the lowest path cost as the final driving path output.

Images