Mining crawler advancing trajectory tracking control method and system based on dynamic planning

Abstract

The invention discloses a mining tracked vehicle advancing trajectory tracking control method and system based on dynamic planning, and the method comprises the steps: presetting a global reference path for a mining tracked vehicle, building an MPC controller based on a tracked vehicle kinematic model, and carrying out the global tracking control through the MPC controller; using an MPC controller to obtain prediction information of a limited number of future states, and judging topographic conditions of a future tracking path coverage area based on the prediction information; if the terrain is judged to be a complex terrain, performing local extraction on an area covered by a future path, entering a dynamic planning layer, performing local path optimization to obtain a recommended path avoiding the complex terrain, partially replacing and updating an original reference path, and performing path tracking control by reusing the MPC controller; if it is judged that the terrain is a non-complex terrain, path tracking control continues to be conducted through the MPC controller; and repeating the steps until the end point of the reference path, and generating an actual tracking trajectory of the crawler.

Description

Technical Field

[0001] This invention belongs to the field of modern mining engineering technology, specifically relating to a method and system for tracking and controlling the trajectory of a tracked mining vehicle based on dynamic programming. Background Technology

[0002] Tracked mining vehicles are commonly used in the mining and transportation processes of coal mines. The working environment in coal mines is typically harsh, with persistent safety hazards. Manually driving tracked vehicles is not only difficult but also prone to driver errors under such complex operating conditions, leading to dangerous situations. Unmanned tracked vehicles, on the other hand, can automatically complete transportation tasks, allowing workers to avoid entering dangerous areas and fundamentally reducing the risk of accidents. Furthermore, unmanned tracked vehicles can operate around the clock, unaffected by human fatigue or rest periods, significantly improving production efficiency. Through intelligent path planning and load management, the coal transportation process can be optimized, reducing resource waste and increasing utilization. As the coal mining environment becomes increasingly complex, the difficulty of manual operation is also gradually increasing, while automated equipment is better suited to extreme conditions such as deep mines, high temperatures, and high humidity. Unmanned tracked vehicles not only help reduce operating costs but also promote the development of intelligent and environmentally friendly coal mining operations, making coal mining safer, more efficient, and greener.

[0003] Most controller designs focused on improving the path tracking accuracy and robustness of tracked vehicles under ground driving conditions have not considered the additional power consumption caused by the complex conditions faced in mining areas. Although tracked vehicles have excellent obstacle-crossing capabilities, high-intensity or frequent obstacle crossings in complex terrain can increase energy consumption and potentially affect vehicle performance. Therefore, research on dynamic movement scheduling decisions for tracked vehicles in complex terrain conditions is both practically necessary and has room for improvement. Dynamic programming-based movement planning and control for tracked vehicles, fully considering the practical significance and constraints of the control variables, proposes a reasonable control model, which has profound theoretical and engineering implications for the unmanned and intelligent movement scheduling of tunneling machines. Summary of the Invention

[0004] This invention aims to address the shortcomings of existing technologies and provides the following solutions: A dynamic programming-based method for tracking and controlling the trajectory of a tracked mining vehicle includes the following steps: S1. Based on the mine roadway environment, a global reference path is preset for the tracked vehicle. An MPC controller is constructed based on the kinematic model of the tracked vehicle, and the MPC controller is initially used for global tracking control. S2. During the global tracking control process, the MPC controller is used to obtain prediction information for a finite number of future states, and the terrain conditions of the area covered by the future tracking path are judged based on the prediction information; S3. If the terrain is determined to be complex, the area covered by the future path is locally extracted, enters the dynamic programming layer, and local path optimization is performed to obtain a recommended path that avoids the complex terrain. The original reference path is replaced and updated locally, and the MPC controller is reused for path tracking control. If the terrain is determined to be non-complex, the MPC controller is continued to be used for path tracking control. S4. Repeat S1-S3 until the end of the reference path, and finally generate an actual tracking trajectory of the tracked vehicle that reasonably avoids complex terrain but always tends to the preset global reference path.

[0005] Preferably, the method for constructing an MPC controller includes: Analysis of control variables during the movement of a tracked vehicle: in, v Indicates the velocity of the center of mass. v 1 indicates the speed of the left track of the vehicle. v 2 indicates the speed of the right track of the vehicle. oh Indicates angular velocity. oh 1 indicates the speed of the left drive wheel. oh 2 indicates the speed of the right drive wheel. r Indicates the track drive wheel. B Indicates the distance between the left and right tracks; The kinematic state equation of the tracked vehicle is obtained based on its movement process: in, Indicates the longitudinal translation speed of the vehicle body. Indicates the angular velocity of the tracked vehicle. Indicates the lateral translation speed of the vehicle body. α This indicates the angle between the tangent direction of the turning radius at the center of gravity and the forward direction of the tracked vehicle. i 1 indicates the slippage rate of the left track. i 2 indicates the slippage rate of the right track; The MPC controller is constructed based on the kinematic formula of the tracked vehicle: in, Represents the cost function, This indicates the subsequent condition that minimizes the cost function within the constraints. NC Step control input, NP Indicates the prediction interval. NC Indicates the control interval. k Represents the current discrete time. i and j These represent times within the interval after the current time k. Indicates the subsequent system NP The deviation between the predicted output and the expected output and Represents the weight matrix. This indicates the increment of the control quantity change. r Represents the weighting function. M Indicates the relaxation factor. This represents the minimum constraint on the control quantity. This represents the maximum constraint on the control variable. This represents the minimum constraint controlling the increment. This represents the maximum constraint controlling the increment.

[0006] Preferred methods for determining terrain conditions include: The MPC controller is used to obtain prediction information for a finite number of future states; Based on the prediction information, the height difference between adjacent prediction points is calculated to obtain a height difference sequence; When the absolute value of the height difference in the height difference sequence exceeds a preset threshold, the terrain change in the area is identified, and points exceeding the preset threshold are marked as potential obstacles using binary labeling; otherwise, they are marked as non-obstacles. When an element marked as a potential obstacle first appears, it is considered the starting point of the obstacle; when an element marked as a non-obstacle first appears, it is considered the ending point of the obstacle; if multiple starting and ending points are found during the search, multiple obstacle areas are marked, and the first obstacle area is selected. After determining the first obstacle zone, widen the area by three units before and after the first obstacle zone to leave space for the local path planning.

[0007] Preferred methods for local path optimization include: Particle swarm optimization (PSO) is used for local path planning. The fitness function used in PSO is: Where fitness represents the overall fitness function. Flength This represents the cost component resulting from path tendency. Height This represents the cost components arising from the obstacle avoidance tendency. Fturn This represents the cost component resulting from the cornering tendency. l 1. l 2 and l3 represents the weighting ratio; The cost components resulting from path tendency calculation: in, n This represents the number of walks along a local path. k Represents the current discrete time, ( xk , yk () represents the current actual vehicle body pose coordinates. ycenter Indicates the center line of the tunnel. Dy This indicates the maximum distance from the center line of the alleyway to the two side walls; Calculate the cost components of the obstacle avoidance tendency: in, e The base of the natural logarithm. hk This indicates the terrain elevation at the current coordinates. Indicates obstacle avoidance count. hmin This indicates the maximum height difference for a vehicle going downhill. hmax This indicates the maximum height difference for a vehicle going uphill; Calculate the cost components resulting from the cornering tendency: in, This indicates the turning cost of a tracked vehicle. This indicates the maximum steering angle that a tracked vehicle can turn while in motion; Based on the path tendency, obstacle avoidance tendency, and corner turning tendency, an overall fitness function is constructed for local path optimization.

[0008] The present invention also provides a dynamic programming-based tracking control system for the movement trajectory of a mining tracked vehicle. The control system applies the above-mentioned control method and includes: a controller construction module, a terrain judgment module, a local path optimization module, and a trajectory generation module. The controller construction module is used to pre-set a global reference path for the tracked mining vehicle according to the mine roadway environment, construct an MPC controller based on the kinematic model of the tracked vehicle, and initially use the MPC controller for global tracking control. The terrain judgment module is used to obtain prediction information of a finite number of future states using the MPC controller during the global tracking control process, and to judge the terrain conditions of the area covered by the future tracking path based on the prediction information. In the local path optimization module, if the terrain is determined to be complex, the area covered by the future path is locally extracted, enters the dynamic programming layer, and local path optimization is performed to obtain a recommended path that avoids the complex terrain. The original reference path is replaced and updated locally, and the MPC controller is reused for path tracking control. If the terrain is determined to be non-complex, the MPC controller is continued to be used for path tracking control. The trajectory generation module is used to generate an actual tracking trajectory for the tracked vehicle that reasonably avoids complex terrain but always tends towards a preset global reference path.

[0009] Preferably, the workflow of the controller construction module includes: Analysis of control variables during the movement of a tracked vehicle: in, v Indicates the velocity of the center of mass. v 1 indicates the speed of the left track of the vehicle. v 2 indicates the speed of the right track of the vehicle. oh Indicates angular velocity. oh 1 indicates the speed of the left drive wheel. oh 2 indicates the speed of the right drive wheel. r Indicates the track drive wheel. B Indicates the distance between the left and right tracks; The kinematic state equation of the tracked vehicle is obtained based on its movement process: in, Indicates the longitudinal translation speed of the vehicle body. Indicates the angular velocity of the tracked vehicle. Indicates the lateral translation speed of the vehicle body. α This indicates the angle between the tangent direction of the turning radius at the center of gravity and the forward direction of the tracked vehicle. i 1 indicates the slippage rate of the left track. i 2 indicates the slippage rate of the right track; The MPC controller is constructed based on the kinematic formula of the tracked vehicle: in, Represents the cost function, This indicates the subsequent condition that minimizes the cost function within the constraints. NC Step control input, NP Indicates the prediction interval. NC Indicates the control interval. k Represents the current discrete time. i and j These represent times within the interval after the current time k. Indicates the subsequent system NP The deviation between the predicted output and the expected output and Represents the weight matrix. This indicates the increment of the control quantity change. r Represents the weighting function. M Indicates the relaxation factor. This represents the minimum constraint on the control quantity. This represents the maximum constraint on the control variable. This represents the minimum constraint controlling the increment. This represents the maximum constraint controlling the increment.

[0010] Preferably, the workflow of the terrain determination module includes: The MPC controller is used to obtain prediction information for a finite number of future states; Based on the prediction information, the height difference between adjacent prediction points is calculated to obtain a height difference sequence; When the absolute value of the height difference in the height difference sequence exceeds a preset threshold, the terrain change in the area is identified, and points exceeding the preset threshold are marked as potential obstacles using binary labeling; otherwise, they are marked as non-obstacles. When an element marked as a potential obstacle first appears, it is considered the starting point of the obstacle; when an element marked as a non-obstacle first appears, it is considered the ending point of the obstacle; if multiple starting and ending points are found during the search, multiple obstacle areas are marked, and the first obstacle area is selected. After determining the first obstacle zone, widen the area by three units before and after the first obstacle zone to leave space for the local path planning.

[0011] Preferably, in the local path optimization module, the local path optimization process includes: Particle Swarm Optimization (PSO) is used for local path planning. The fitness function used in PSO is: Where fitness represents the overall fitness function. Flength This represents the cost component resulting from path tendency. Height This represents the cost components arising from the obstacle avoidance tendency. This represents the cost component resulting from the cornering tendency. l 1. l 2 and l 3 represents the corresponding weight ratio; The cost components resulting from path tendency calculation: in, n This represents the number of walks along a local path. k Represents the current discrete time, ( xk , yk () represents the current actual vehicle body pose coordinates. ycenter Indicates the center line of the tunnel. Dy This indicates the maximum distance from the center line of the alleyway to the two side walls; Calculate the cost components of the obstacle avoidance tendency: in, e The base of the natural logarithm. hk This indicates the terrain elevation at the current coordinates. Indicates obstacle avoidance count. hmin This indicates the maximum height difference for a vehicle going downhill. hmax This indicates the maximum height difference for a vehicle going uphill; Calculate the cost components resulting from the cornering tendency: in, This indicates the turning cost of a tracked vehicle. This indicates the maximum steering angle that a tracked vehicle can turn while in motion; Based on the path tendency, obstacle avoidance tendency, and corner turning tendency, an overall fitness function is constructed for local path optimization.

[0012] Compared with the prior art, the beneficial effects of the present invention are as follows: Based on the MPC multi-objective rolling optimization control mode, this invention allows for flexible switching between global planning and tracking and local trajectory optimization and re-tracking, which can solve the problem of how to perform trajectory tracking in complex environments using a vehicle model that is not accurate enough.

[0013] This invention solves the problem of poor optimization results when incorporating terrain cost into the MPC objective function. By using a trajectory planning strategy of dynamic alternating local optimization, it not only demonstrates the strong obstacle-crossing ability of tracked vehicles, but also reduces unnecessary losses caused by global optimization during the tracking process. Attached Figure Description

[0014] To more clearly illustrate the technical solution of the present invention, the drawings used in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0015] Figure 1 This is a schematic diagram of the method flow according to an embodiment of the present invention; Figure 2 This is a flowchart of a method according to an embodiment of the present invention. Detailed Implementation

[0016] The technical solutions of the embodiments of the present invention will be clearly and completely described below 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 embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0017] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0018] Example 1 In this embodiment, as Figure 1 , Figure 2 As shown, a dynamic programming-based method for tracking and controlling the trajectory of a tracked mining vehicle includes the following steps: S1. A global reference path is preset for the tracked vehicle based on the mine roadway environment. An MPC controller is constructed based on the kinematic model of the tracked vehicle, and the MPC controller is initially used for global tracking control.

[0019] Methods for constructing an MPC controller include: In this embodiment, the control variables during the movement of the tracked vehicle are analyzed: in, v Indicates the velocity of the center of mass. v 1 indicates the speed of the left track of the vehicle. v 2 indicates the speed of the right track of the vehicle. oh Indicates angular velocity. oh 1 indicates the speed of the left drive wheel. oh 2 indicates the speed of the right drive wheel. r Indicates the track drive wheel. B Indicates the distance between the left and right tracks; The kinematic state of the tracked vehicle is obtained based on its movement: During the movement of a tracked vehicle, the ground slip characteristics are characterized by the slip rate parameter. This parameter is theoretically defined as the ratio of slip velocity to theoretical travel speed, and its numerical characteristics are controlled by the kinematic parameters of the track mechanism. i 1 and i 2 represents the slip rate of the left and right tracks respectively, and its value is in the range of 0 to 1. In engineering, it is obtained by experience or statistics based on the ground soil quality.

[0020] Under ideal no-slip conditions: When considering the slip effect, the modified state equation is expressed as: in, Indicates the longitudinal translation speed of the vehicle body. Indicates the angular velocity of the tracked vehicle. Indicates the lateral translation speed of the vehicle body. α This represents the angle between the tangent direction of the turning radius at the center of mass and the forward direction of the tracked vehicle; considering the kinematic characteristics of the mass, the turning radii of each mass are not orthogonal to the longitudinal axis of the vehicle body, and the vehicle's traveling speed... v Depend on The sum of its linear velocity and the linear velocity of its circular motion, based on the low-speed turning assumption, allows us to ignore the influence of rotational linear velocity, thus approximating the vehicle's speed as equal to its forward velocity. .

[0021] The differentials of the above-mentioned tracked vehicle motion state variables, after discretization, constitute the general form of its dynamic equation: in, X ( t )and Y ( t ) indicates the system t time n dimensional state vector and m 3D output vector ,matrix A express n × n dimensional state matrix, matrix B express n × p For the input matrix,u ( k )express p ×1 is the control vector, matrix C express m × n 3D observation matrix. Specifically, n =3, x 1= x , x 2= y , x 3= f ; p =2, u 1= v , u 2= oh ; m =2, y 1= x , y 2= y Furthermore, taking the state variable deviation and control variable deviation at each moment as inputs, the state equation in the above formula becomes: in, k Represents the current discrete time. This indicates the deviation of the state variable. Indicates the deviation of the control quantity; Among them, the subscript ref This represents the reference or expected value of a certain state or control variable. T represents the sampling time, and the matrix... A t and B t represents: Pick For the new state variables, we obtain new discrete dynamic equations: in, Indicates the increment of the control input. This is the new output. The prediction interval (time step) is taken. NP and control range Nc The predicted output in the above formula can be expressed as: .

[0022] MPC controller constructed based on the kinematic formula of tracked vehicle: Among them, among them, Represents the cost function, This indicates the subsequent condition that minimizes the cost function within the constraints. NC Step control input, NP Indicates the prediction interval. NC Indicates the control interval. k Represents the current discrete time. i and j These represent times within the interval after the current time k. Indicates the subsequent system NP The deviation between the predicted output and the expected output and Represents the weight matrix. This indicates the increment of the control quantity change. r Represents the weighting function. M Let represent the relaxation factor. In the constructed cost function, the first term has the general form: This indicates the degree of deviation between the current state and the desired path, reflecting the tracked vehicle's ability to successfully follow the desired path. N Indicates the range of states under investigation, with subscripts. t Represents each time point within the interval. xt express t Always the center of gravity of the vehicle body x Coordinate status, xref , t express t Always the center of gravity of the vehicle body x Coordinate reference values yt express t Always the center of gravity of the vehicle body y Coordinate status, yref , t express t Always the center of gravity of the vehicle body y The first term represents the coordinate reference value. The second term indicates the magnitude of the change in the control variable, reflecting the tracked vehicle's ability to smoothly turn during tracking. The third term represents the terminal constraint, ensuring that the control strategy satisfies all hard constraints, such as the vehicle's maximum speed, maximum acceleration, and minimum turning radius. For quadratic programming, the cost function is transformed into the following form: in, Represented by the cost function The standard quadratic programming function that has been constructed is... The new state variables set during the standardization process of quadratic programming are augmented expressions of the deviations between the original state variables and the expected values ​​of the system. T This indicates the matrix transpose.

[0023] The tracked vehicle is driven by a DC motor through a transmission system to power the drive wheels on the left and right sides. Therefore, it is necessary to consider actuator saturation constraints during the trajectory tracking process, imposing constraints on the control quantity and control increment. The constraint expression is as follows: in, This represents the minimum constraint on the control quantity. This represents the maximum constraint on the control variable. This represents the minimum constraint controlling the increment. This represents the maximum constraint controlling the increment.

[0024] S2. During the global tracking control process, the MPC controller is used to obtain prediction information for a finite number of future states, and the terrain conditions of the area covered by the future tracking path are judged based on the prediction information.

[0025] The method for judging terrain conditions includes: using the MPC controller to obtain prediction information for a finite number of future states; based on the prediction information, calculating the height difference between adjacent prediction points to obtain a height difference sequence; when the absolute value of the height difference in the height difference sequence exceeds a preset threshold, it is determined that a terrain change has occurred in the area, and points exceeding the preset threshold are marked as potential obstacles using binary labeling; otherwise, they are marked as non-obstacles; when an element marked as a potential obstacle appears for the first time, it is considered the starting point of the obstacle; when an element marked as a non-obstacle appears for the first time, it is considered the ending point of the obstacle; if multiple starting points and ending points are found during the search process, they are marked as multiple obstacle areas, and the first obstacle area is selected; after determining the first obstacle area, the area before and after the first obstacle area is widened by three units respectively to leave space for local path planning.

[0026] In this embodiment, the MPC controller is used to obtain prediction information for a finite number of future states. Based on the prediction information, a sequence of height differences is obtained by calculating the height difference between adjacent prediction points (usually calculated using a simple difference). These height differences reflect the rate of change of the terrain. When the absolute value of the height difference exceeds a certain preset threshold, it is determined that a sudden change in terrain has occurred in the area, and there may be obstacles. This method effectively extracts obstacle information from complex environments, especially in areas with complex terrain or large height changes. After obtaining the height difference sequence, the sequence is further analyzed, and the location of potential obstacles is marked by obtaining the maximum value of the absolute value of the height difference. Suppose we define a binarization rule: if the absolute value of the height difference is greater than the threshold, the location is considered to be an obstacle. In the height difference sequence, for each adjacent prediction point, if the absolute value of its height difference exceeds the threshold, it is marked as a potential obstacle (marked as 1); otherwise, it is marked as a non-obstacle (marked as 0). This marking process can be implemented through logical condition judgment, generating a binary matrix or array, where 1 represents the obstacle location and 0 represents the non-obstacle location. By traversing the height difference sequence after binary labeling, the possible start and end points of obstacles are located. When an element labeled 1 first appears, it is considered the start point of the obstacle; when an element labeled 0 first appears, it is considered the end point. This method effectively extracts start and end points from continuous obstacle regions, identifying a complete obstacle. If multiple start and end points are found during the search (i.e., multiple alternating occurrences of 1 and 0), it means that multiple obstacle regions exist, and only the first obstacle region is selected. After determining the first obstacle region, to ensure that the tracked vehicle has sufficient time and distance for obstacle avoidance and to guarantee the smoothness and stability of obstacle avoidance, a three-unit extension is added before and after the first obstacle region to leave ample space for subsequent local path planning.

[0027] S3. If the terrain is determined to be complex, the area covered by the future path is locally extracted, entered into the dynamic programming layer, and local path optimization is performed to obtain a recommended path that avoids the complex terrain. The original reference path is replaced and updated locally, and the MPC controller is reused for path tracking control. If the terrain is determined to be non-complex, the MPC controller is used for path tracking control.

[0028] Methods for local path optimization include: Particle swarm optimization (PSO) is used for local path planning. The fitness function used in PSO is: Where fitness represents the overall fitness function. Flength This represents the cost component resulting from path tendency. HeightThis represents the cost components arising from the obstacle avoidance tendency. Fturn This represents the cost component resulting from the cornering tendency. l 1. l 2 and l 3 represents the weighting ratio; The cost components resulting from path tendency calculation: Space constraints are a significant issue during underground movement. The initial reference path for tracked vehicles is typically designed along the tunnel centerline to ensure the vehicle stays in the middle of the tunnel, avoiding collisions with the side walls. When partially replanning the initial reference path, it's crucial to ensure the new path deviates as little as possible from the original path. After avoiding obstacles, the tracked vehicle should be able to return to the original reference path promptly. Excessive deviation may cause the vehicle to exceed the tunnel's spatial limits, failing to meet actual driving conditions. Path planning must balance obstacle avoidance and space constraints to guarantee the safe and efficient movement of the tracked vehicle. (The text abruptly ends here, likely due to an incomplete sentence or missing information.) ycenter With actual location yk Between them, there are: in, n This represents the number of walks along a local path. k Represents the current discrete time, ( xk , yk () represents the current actual vehicle body pose coordinates. yk Indicates the actual location. ycenter Indicates the center line of the tunnel. Dy This indicates the maximum distance from the center line of the alley to the walls on both sides.

[0029] Calculate the cost components of the obstacle avoidance tendency: This embodiment uses the height difference between the front and rear positions of the vehicle during travel as the main parameter of obstacle-crossing cost. Vehicle travel resistance is related to the ground pressure and soil settlement between the vehicle body and the ground. The power consumption of the chassis overcoming the terrain height difference is equivalent to the travel power consumption of the chassis under certain ground settlement conditions. By calculating the height difference between two adjacent coordinate positions on the tracking path, the travel cost of the vehicle during climbing or descending is evaluated. When the vehicle is climbing uphill, if the height difference is too large, the travel power consumption is high; when the vehicle is descending, if the height difference is too large, there is a risk of tipping over (which can be considered a cost). These situations result in different costs incurred during obstacle crossing. If there are continuous and large-scale repeated changes in height difference, the tracked vehicle will inevitably incur even greater costs. The relevant constraint expression is as follows: in, e The base of the natural logarithm. hk This indicates the terrain elevation at the current coordinates. Indicates obstacle avoidance count. hmin This indicates the maximum height difference for a vehicle going downhill. hmax This indicates the maximum height difference for a vehicle going uphill.

[0030] Calculate the cost components resulting from the cornering tendency: in, This represents the turning cost of a tracked vehicle, which should not be turned too frequently during movement. Due to the significant slippage of the tracks, unnecessary turns should be avoided during path planning. Furthermore, to ensure smoother path following, the constraint is expressed as follows: in, This indicates the maximum steering angle that a tracked vehicle can turn while in motion.

[0031] Based on path preference, obstacle avoidance preference, and corner turning preference, construct the overall fitness function: Where fitness represents the overall fitness function. l 1. l 2 and l 3 represents the weight ratio; local path optimization is performed based on the overall fitness function.

[0032] S4. Repeat S1-S3 until the end of the reference path, and finally generate an actual tracking trajectory of the tracked vehicle that reasonably avoids complex terrain but always tends to the preset global reference path.

[0033] Example 2 In this embodiment, a dynamic programming-based tracking control system for a mining tracked vehicle includes: a controller construction module, a terrain judgment module, a local path optimization module, and a trajectory generation module.

[0034] The controller construction module is used to pre-set a global reference path for the tracked mining vehicle based on the mine roadway environment, construct an MPC controller based on the kinematic model of the tracked vehicle, and initially use the MPC controller for global tracking control.

[0035] The workflow of the controller construction module includes: analyzing the control variables during the movement of the tracked vehicle. in, v Indicates the velocity of the center of mass. v 1 indicates the speed of the left track of the vehicle. v2 indicates the speed of the right track of the vehicle. oh Indicates angular velocity. oh 1 indicates the speed of the left drive wheel. oh 2 indicates the speed of the right drive wheel. r Indicates the track drive wheel. B The distance between the left and right tracks is represented; the kinematic equation of the tracked vehicle is obtained based on its movement process: in, Indicates the longitudinal translation speed of the vehicle body. Indicates the angular velocity of the tracked vehicle. Indicates the lateral translation speed of the vehicle body. α This indicates the angle between the tangent direction of the turning radius at the center of gravity and the forward direction of the tracked vehicle. i 1 indicates the slippage rate of the left track. i 2 represents the slippage rate of the right track; an MPC controller is constructed based on the kinematic formula of the tracked vehicle: in, Represents the cost function, This indicates the subsequent condition that minimizes the cost function within the constraints. NC Step control input, NP Indicates the prediction interval. NC Indicates the control interval. k Represents the current discrete time. i and j These represent times within the interval after the current time k. Indicates the subsequent system NP The deviation between the predicted output and the expected output and Represents the weight matrix. This indicates the increment of the control quantity change. r Represents the weighting function. M Indicates the relaxation factor. This represents the minimum constraint on the control quantity. This represents the maximum constraint on the control variable. This represents the minimum constraint controlling the increment. This represents the maximum constraint controlling the increment.

[0036] The terrain assessment module is used to obtain prediction information of a finite number of future states by the MPC controller during the global tracking control process, and to assess the terrain conditions of the area covered by the future tracking path based on the prediction information.

[0037] The workflow of the terrain assessment module includes: obtaining prediction information for a finite number of future states using the MPC controller; calculating the height difference between adjacent predicted points based on the prediction information to obtain a height difference sequence; when the absolute value of the height difference in the height difference sequence exceeds a preset threshold, it is determined that a terrain change has occurred in the area, and points exceeding the preset threshold are marked as potential obstacles using binary labeling; otherwise, they are marked as non-obstacles; when an element marked as a potential obstacle appears for the first time, it is considered the starting point of the obstacle; when an element marked as a non-obstacle appears for the first time, it is considered the ending point of the obstacle; if multiple starting points and ending points are found during the search process, they are marked as multiple obstacle areas, and the first obstacle area is selected; after determining the first obstacle area, the area before and after the first obstacle area is widened by three units to leave space for local path planning.

[0038] In the local path optimization module, if the terrain is determined to be complex, the area covered by the future path is locally extracted, enters the dynamic programming layer, and local path optimization is performed to obtain a recommended path that avoids the complex terrain. The original reference path is replaced and updated locally, and the MPC controller is reused for path tracking control. If the terrain is determined to be non-complex, the MPC controller is used for path tracking control.

[0039] In the local path optimization module, the local path optimization process includes: using the Particle Swarm Optimization (PSO) algorithm for local path planning. The fitness function used by PSO is: Where fitness represents the overall fitness function. Flength This represents the cost component resulting from path tendency. Height This represents the cost components arising from the obstacle avoidance tendency. This represents the cost component resulting from the cornering tendency. l 1. l 2 and l 3 represents the corresponding weight ratio; the cost component resulting from path bias is calculated as follows: in, n This represents the number of walks along a local path. k Represents the current discrete time, ( xk , yk () represents the current actual vehicle body pose coordinates. ycenter Indicates the center line of the tunnel. Dy Indicate the maximum distance from the centerline of the alleyway to the two side walls; calculate the cost components of the obstacle avoidance tendency: in, e The base of the natural logarithm. hk This indicates the terrain elevation at the current coordinates. Indicates obstacle avoidance count. hmin This indicates the maximum height difference for a vehicle going downhill. hmax Indicates the maximum height difference of the vehicle going uphill; calculates the cost component caused by the turning tendency: in, This indicates the turning cost of a tracked vehicle. This represents the maximum steering angle that the tracked vehicle can turn during its journey; based on path tendency, obstacle avoidance tendency, and turning angle tendency, an overall fitness function is constructed for local path optimization.

[0040] The trajectory generation module is used to generate an actual tracking trajectory for the tracked vehicle that reasonably avoids complex terrain but always tends towards the preset global reference path.

[0041] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made to the technical solutions of the present invention by those skilled in the art without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A method for tracking and controlling the trajectory of a tracked mining vehicle based on dynamic programming, characterized in that, Includes the following steps: S1. Based on the mine roadway environment, a global reference path is preset for the tracked vehicle. An MPC controller is constructed based on the kinematic model of the tracked vehicle, and the MPC controller is initially used for global tracking control. S2. During the global tracking control process, the MPC controller is used to obtain prediction information for a finite number of future states, and the terrain conditions of the area covered by the future tracking path are judged based on the prediction information; S3. If the terrain is determined to be complex, the area covered by the future path is locally extracted, enters the dynamic programming layer, and local path optimization is performed to obtain a recommended path that avoids the complex terrain. The original reference path is replaced and updated locally, and the MPC controller is reused for path tracking control. If the terrain is determined to be non-complex, the MPC controller is continued to be used for path tracking control. S4. Repeat S1-S3 until the end of the reference path, and finally generate an actual tracking trajectory of the tracked vehicle that reasonably avoids complex terrain but always tends to the preset global reference path.

2. The method for tracking and controlling the trajectory of a tracked mining vehicle based on dynamic programming as described in claim 1, characterized in that, Methods for constructing an MPC controller include: Analysis of control variables during the movement of a tracked vehicle: in, v Indicates the velocity of the center of mass. v 1 indicates the speed of the left track of the vehicle. v 2 indicates the speed of the right track of the vehicle. ω Indicates angular velocity, ω 1 indicates the rotational speed of the left drive wheel. ω 2 indicates the speed of the right drive wheel. r Indicates the track drive wheel. B Indicates the distance between the left and right tracks; The kinematic state equation of the tracked vehicle is obtained based on its movement process: in, Indicates the longitudinal translation speed of the vehicle body. Indicates the angular velocity of the tracked vehicle. Indicates the lateral translation speed of the vehicle body. α This indicates the angle between the tangent direction of the turning radius at the center of gravity and the forward direction of the tracked vehicle. i 1 indicates the slippage rate of the left track. i 2 indicates the slippage rate of the right track; The MPC controller is constructed based on the kinematic formula of a tracked vehicle. in, Represents the cost function, This indicates the subsequent condition that minimizes the cost function within the constraints. NC Step control input, NP Indicates the prediction interval. NC Indicates the control interval. k Represents the current discrete time. i and j These represent times within the interval after the current time k. Indicates the subsequent system NP The deviation between the predicted output and the expected output and Represents the weight matrix. This indicates the increment of the control quantity change. ρ Represents the weighting function. M Indicates the relaxation factor. This represents the minimum constraint on the control quantity. This represents the maximum constraint on the control variable. This represents the minimum constraint controlling the increment. This represents the maximum constraint controlling the increment.

3. The method for tracking and controlling the trajectory of a tracked mining vehicle based on dynamic programming as described in claim 1, characterized in that, Methods for assessing terrain conditions include: The MPC controller is used to obtain prediction information for a finite number of future states; Based on the prediction information, the height difference between adjacent prediction points is calculated to obtain a height difference sequence; When the absolute value of the height difference in the height difference sequence exceeds a preset threshold, the terrain change in the area is identified, and points exceeding the preset threshold are marked as potential obstacles using binary labeling; otherwise, they are marked as non-obstacles. When an element marked as a potential obstacle first appears, it is considered the starting point of the obstacle; when an element marked as a non-obstacle first appears, it is considered the ending point of the obstacle; if multiple starting and ending points are found during the search, multiple obstacle areas are marked, and the first obstacle area is selected. After determining the first obstacle zone, widen the area by three units before and after the first obstacle zone to leave space for the local path planning.

4. The method for tracking and controlling the trajectory of a tracked mining vehicle based on dynamic programming as described in claim 1, characterized in that, Methods for local path optimization include: Particle swarm optimization (PSO) is used for local path planning. The fitness function used in PSO is: Where fitness represents the overall fitness function, Flength This represents the cost component resulting from path tendency. Fheigth This represents the cost components arising from the obstacle avoidance tendency. Fturn This represents the cost component resulting from the cornering tendency. λ 1. λ 2 and λ 3 represents the weighting ratio; The cost components resulting from path tendency calculation: in, n This represents the number of walks along a local path. k Represents the current discrete time, ( xk , yk () represents the current actual vehicle body pose coordinates. ycenter Indicates the center line of the tunnel. Dy This indicates the maximum distance from the center line of the alleyway to the two side walls; Calculate the cost components of the obstacle avoidance tendency: in, e The base of the natural logarithm. HK This indicates the terrain elevation at the current coordinates. Indicates obstacle avoidance count. hmin This indicates the maximum height difference for a vehicle going downhill. hmax This indicates the maximum height difference for a vehicle going uphill; Calculate the cost components resulting from the cornering tendency: in, This indicates the turning cost of a tracked vehicle. This indicates the maximum steering angle that a tracked vehicle can turn while in motion; Based on the path tendency, obstacle avoidance tendency, and corner turning tendency, an overall fitness function is constructed for local path optimization.

5. A dynamic programming-based tracking control system for the trajectory of a mining tracked vehicle, wherein the control system applies the control method described in any one of claims 1-4, characterized in that, include: The module includes a controller construction module, a terrain assessment module, a local path optimization module, and a trajectory generation module. The controller construction module is used to pre-set a global reference path for the tracked mining vehicle according to the mine roadway environment, construct an MPC controller based on the kinematic model of the tracked vehicle, and initially use the MPC controller for global tracking control. The terrain judgment module is used to obtain prediction information of a finite number of future states using the MPC controller during the global tracking control process, and to judge the terrain conditions of the area covered by the future tracking path based on the prediction information. In the local path optimization module, if the terrain is determined to be complex, the area covered by the future path is locally extracted, enters the dynamic programming layer, and local path optimization is performed to obtain a recommended path that avoids the complex terrain. The original reference path is replaced and updated locally, and the MPC controller is reused for path tracking control. If the terrain is determined to be non-complex, the MPC controller is continued to be used for path tracking control. The trajectory generation module is used to generate an actual tracking trajectory for the tracked vehicle that reasonably avoids complex terrain but always tends towards a preset global reference path.

6. The dynamic programming-based tracking control system for tracked mining vehicles according to claim 5, characterized in that, The workflow of the controller construction module includes: Analysis of control variables during the movement of a tracked vehicle: in, v Indicates the velocity of the center of mass. v 1 indicates the speed of the left track of the vehicle. v 2 indicates the speed of the right track of the vehicle. ω Indicates angular velocity, ω 1 indicates the rotational speed of the left drive wheel. ω 2 indicates the speed of the right drive wheel. r Indicates the track drive wheel. B Indicates the distance between the left and right tracks; The kinematic state equation of the tracked vehicle is obtained based on its movement process: in, Indicates the longitudinal translation speed of the vehicle body. Indicates the angular velocity of the tracked vehicle. Indicates the lateral translation speed of the vehicle body. α This indicates the angle between the tangent direction of the turning radius at the center of gravity and the forward direction of the tracked vehicle. i 1 indicates the slippage rate of the left track. i 2 indicates the slippage rate of the right track; The MPC controller is constructed based on the kinematic formula of a tracked vehicle. in, Represents the cost function, This indicates the subsequent condition that minimizes the cost function within the constraints. NC Step control input, NP Indicates the prediction interval. NC Indicates the control interval. k Represents the current discrete time. i and j These represent times within the interval after the current time k. Indicates the subsequent system NP The deviation between the predicted output and the expected output and Represents the weight matrix. This indicates the increment of the control quantity change. ρ Represents the weighting function. M Indicates the relaxation factor. This represents the minimum constraint on the control quantity. This represents the maximum constraint on the control variable. This represents the minimum constraint controlling the increment. This represents the maximum constraint controlling the increment.

7. The dynamic programming-based tracking control system for tracked mining vehicles according to claim 5, characterized in that, The workflow of the terrain assessment module includes: The MPC controller is used to obtain prediction information for a finite number of future states; Based on the prediction information, the height difference between adjacent prediction points is calculated to obtain a height difference sequence; When the absolute value of the height difference in the height difference sequence exceeds a preset threshold, the terrain change in the area is identified, and points exceeding the preset threshold are marked as potential obstacles using binary labeling; otherwise, they are marked as non-obstacles. When an element marked as a potential obstacle first appears, it is considered the starting point of the obstacle; when an element marked as a non-obstacle first appears, it is considered the ending point of the obstacle; if multiple starting and ending points are found during the search, multiple obstacle areas are marked, and the first obstacle area is selected. After determining the first obstacle zone, widen the area by three units before and after the first obstacle zone to leave space for the local path planning.

8. The dynamic programming-based tracking control system for tracked mining vehicles according to claim 5, characterized in that, The local path optimization module includes the following process: Particle Swarm Optimization (PSO) is used for local path planning. The fitness function used in PSO is: Where fitness represents the overall fitness function, Flength This represents the cost component resulting from path tendency. Fheigth This represents the cost components arising from the obstacle avoidance tendency. This represents the cost component resulting from the cornering tendency. λ 1. λ 2 and λ 3 represents the corresponding weight ratio; The cost components resulting from path tendency calculation: in, n This represents the number of walks along a local path. k Represents the current discrete time, ( xk , yk () represents the current actual vehicle body pose coordinates. ycenter Indicates the center line of the tunnel. Dy This indicates the maximum distance from the center line of the alleyway to the two side walls; Calculate the cost components of the obstacle avoidance tendency: in, e The base of the natural logarithm. HK This indicates the terrain elevation at the current coordinates. Indicates obstacle avoidance count. hmin This indicates the maximum height difference for a vehicle going downhill. hmax This indicates the maximum height difference for a vehicle going uphill; Calculate the cost components resulting from the cornering tendency: in, This indicates the turning cost of a tracked vehicle. This indicates the maximum steering angle that a tracked vehicle can turn while in motion; Based on the path tendency, obstacle avoidance tendency, and corner turning tendency, an overall fitness function is constructed for local path optimization.

Images