Trajectory tracking method and system for underground coal mine unmanned vehicle

Abstract

The invention discloses a coal mine underground unmanned vehicle trajectory tracking method and system. The method comprises the steps that S1, a trajectory prediction module of an unmanned vehicle and a vehicle transverse dynamics tracking error model are established; s2, designing a transverse controller based on the vehicle transverse dynamics tracking error model, and obtaining a front wheel steering angle control instruction of the unmanned vehicle; s3, designing a longitudinal controller to obtain the expected acceleration of the unmanned vehicle; s4, establishing a longitudinal inverse dynamic model of the unmanned vehicle, inputting the expected acceleration of the unmanned vehicle, and calculating to obtain the opening degree of an accelerator or the braking pressure; and S5, the speed of the unmanned vehicle is obtained based on the accelerator opening degree or the brake pressure, the front wheel rotation angle of the unmanned vehicle is obtained based on the front wheel rotation angle control instruction of the unmanned vehicle, and unmanned vehicle trajectory tracking is completed through transverse and longitudinal combined control. According to the method, a transverse and longitudinal cooperative control strategy is provided for the real-time performance and robustness requirements of the coal mine underground complex environment for trajectory tracking control, and the accurate tracking and anti-interference capability of the vehicle speed are achieved.

Description

Technical Field

[0001] The present invention relates to the field of automatic driving of unmanned vehicles in coal mines, and designs a trajectory tracking method and system for unmanned vehicles in coal mines. Background Art

[0002] With the development of unmanned vehicle technology, their application has expanded into various fields. Their use in underground coal mines is also increasing. The underground environment in coal mines differs significantly from that of ordinary roads. The space is narrow, the ground may be uneven, with accumulated water and coal slag, and there are complex obstacles such as equipment and supports. These factors make the movement environment of unmanned vehicles more complex, placing higher demands on real-time control and robustness. Furthermore, underground communication may be limited, requiring unmanned vehicles to have stronger autonomous decision-making capabilities. To improve the safety of unmanned vehicles, motion control has also become a key focus.

[0003] Existing lateral control technologies widely utilize model prediction algorithms, sliding mode control algorithms, and fuzzy control. Model prediction requires solving optimization problems online and in real time, placing high demands on the computing power of embedded systems, while computing resources are often limited for underground equipment. The switching characteristics of sliding mode control can induce high-frequency chattering, leading to mechanical wear of the steering motor or hydraulic system, potentially causing failures during long-term underground operation. The performance of fuzzy control relies heavily on a rule base developed based on expert experience and lacks a systematic design approach, which can lead to rule conflicts in special underground scenarios (such as multiple obstacles). Traditional PID control for longitudinal control frequently changes underground vehicle load, slope, and road friction coefficient. Traditional PID relies on fixed parameters and cannot be dynamically adjusted, which can easily lead to overshoot or response lag.

[0004] Therefore, there is an urgent need for a method and system for tracking the trajectory of unmanned vehicles in coal mines. Summary of the Invention

[0005] The purpose of the present invention is to provide a method and system for tracking the trajectory of an unmanned vehicle in an underground coal mine, so as to overcome the deficiencies of the prior art.

[0006] The present invention adopts the following technical solutions to solve the above technical problems: In a first aspect, the present invention provides a method for tracking the trajectory of an unmanned vehicle in an underground coal mine, comprising the following steps: S1, establish the unmanned vehicle trajectory prediction module and vehicle lateral dynamic tracking error model. The trajectory prediction module predicts the future trajectory points of the unmanned vehicle based on the input planning trajectory; S2, designing a lateral controller based on the vehicle lateral dynamics tracking error model, and obtaining a front wheel angle control command of the unmanned vehicle through the lateral controller; S3, designing a longitudinal controller based on a BP neural network, wherein the longitudinal controller is a PID controller, and obtaining the desired acceleration of the unmanned vehicle according to the PID controller; S4, establishing a longitudinal inverse dynamics model of the unmanned vehicle, inputting the desired acceleration of the unmanned vehicle into the longitudinal inverse dynamics model of the unmanned vehicle to calculate the throttle opening size or the brake pressure size; In step S5, the throttle opening size or the brake pressure size is input into the unmanned vehicle to obtain the speed of the unmanned vehicle, and then the front wheel angle control instruction of the unmanned vehicle is input into the unmanned vehicle to obtain the front wheel angle of the unmanned vehicle, and the trajectory tracking of the unmanned vehicle is completed through the combined lateral and longitudinal control.

[0007] Furthermore, the S1 specifically includes: Establish a trajectory prediction module for the unmanned vehicle, and predict the future trajectory points of the unmanned vehicle based on the current state of the unmanned vehicle and the planned trajectory state; Based on the future trajectory of the unmanned vehicle, the front wheel angle of the unmanned vehicle is controlled in real time to effectively track the path; Establish the vehicle's lateral and yaw degrees of freedom to establish the vehicle's lateral dynamics model; The trajectory prediction module formula is:

[0008] in, is the predicted horizontal axis; is the predicted ordinate; To predict the yaw rate; To predict the lateral velocity; To predict the longitudinal velocity; For the prediction time; is the longitudinal velocity; is the lateral velocity; is the yaw angular velocity; is the yaw angle; The vehicle lateral dynamics model is established based on the two degrees of freedom of vehicle lateral and vehicle yaw. The formula is: ; Wherein, m is the mass of the vehicle; is the moment of inertia around the Z axis; a is the distance from the center of mass to the front axle, and b is the distance from the center of mass to the rear axle; is the front wheel turning angle; is the cornering stiffness of the front tire, is the cornering stiffness of the rear tire; is the yaw angular velocity; ; The lateral acceleration is calculated based on the vehicle lateral dynamics model formula and yaw angular acceleration ; Its formula is:

[0009]

[0010] Define the distance from the vehicle's center of mass to the road centerline as the distance deviation , the deviation between the vehicle heading and the road direction is defined as the heading deviation , heading deviation The calculation formula is: ; in, is the heading angle of the unmanned vehicle; is the ideal heading angle at the current moment; Based on distance deviation and heading error The state space equation of the vehicle lateral dynamics error tracking model is obtained, where the state vector ; The state space equation of the dynamic error tracking model is: ;

[0011]

[0012] .

[0013] Furthermore, in said S2, A lateral controller is designed based on the vehicle lateral dynamics tracking error model, and the front wheel angle control command of the unmanned vehicle is obtained through the lateral controller: Design LQR feedback controller; The ant colony algorithm is introduced to optimize the weight matrix of the LQR feedback controller and the optimized control law of the LQR feedback controller is obtained. Design a lateral feedforward controller and obtain its control law; The control law of the optimized LQR feedback controller and the control law of the lateral feedforward controller are added together to obtain the total control law of the lateral controller, namely the front wheel angle control command.

[0014] Furthermore, in S2, the process of designing the LQR feedback controller is specifically as follows: Determine the performance index of LQR feedback controller : ; Wherein, Q is a weighting matrix, and is a positive definite weighting matrix or a semi-positive definite weighting matrix; R is a positive definite weighting matrix; is the state vector; is the control input vector; Q and R are the weight matrices of the LQR feedback controller, is the state vector, is the control input vector; Based on the performance index of LQR feedback controller Constructing Hamiltonian function : ; Where A and B are matrices of the state space equations, and λ is the adjoint vector; Hamiltonian function Taking the derivative about U(t) and setting the derivative to 0, we get ; in, ; Riccati equation The solution; The control law of the LQR feedback controller is: ; in, is the LQR feedback controller gain; The weight of the LQR feedback controller is optimized by the ant colony optimization algorithm to obtain the optimized weight of the LQR feedback controller; The control law of the optimized LQR feedback controller is obtained by the A matrix, B matrix and weight of the optimized LQR feedback controller obtained through the state space equation: ; in, = is the optimized LQR gain.

[0015] Furthermore, the process of designing the lateral feedforward controller is as follows: The front wheel steering angle output by the lateral feedforward controller is Then, the overall control law of the lateral controller is: ; Substituting the overall control law of the lateral controller into the state space equation yields: ; and perform Laplace transform on this formula to find the extreme value to obtain the steady-state error of distance deviation and steady-state error of heading deviation They are: ; ; in, is the LQR gain after ant colony algorithm optimization; is the road curvature; Steady-state error of distance deviation The lateral feedforward controller eliminates the steady-state error of the heading deviation It is impossible to eliminate the control law of the lateral feedforward controller: ; in, is the LQR gain after ant colony algorithm optimization; is the road curvature.

[0016] Furthermore, the weight of the LQR feedback controller is optimized by the ant colony optimization algorithm. The specific process is as follows: S21, select different Q matrix parameter solutions, select 4 values ​​in the parameter solution as an array, discretize the array, and select the R matrix as the unit matrix; set a length of 16-bit digital sequence , Storing ants The location information passed, the array corresponds to the 16-bit digital sequence; S22, place m artificial ants at the starting point and set the initial parameters of the algorithm; S23, the iteration starts, the ant sets out, and each time it passes a node, it calculates the state transition probability according to the state transition probability formula; S24, use the roulette method to select the next node for the ant, move to that point, and update the position array at the same time; S25, after 16 units of time, m Each ant passes through 16 integer points, completes an iteration, obtains the corresponding array, and calculates the corresponding evaluation value according to the evaluation index function, and judges whether the Q matrix parameters are optimal by the corresponding evaluation value; S26, when m Only one artificial ant completes one iteration and updates the pheromone content between two nodes; S27, repeat S23-S26, select the path of each iteration until the preset number of iterations is reached , the iteration ends, and the final 16-bit digital sequence is the Q matrix parameter of the LQR feedback controller.

[0017] Furthermore, the pheromone content between the two nodes Update, update pheromone as follows: ; ; ; in, The volatility coefficient of pheromone should be Among them, select S ants with small evaluation index function as elite ants, For elite ants The pheromone increment from point j to point j; For the The pheromone content between two nodes left by elite ants in this search; For ordinary ants at this time The pheromone increment from point j to point j; For the The pheromone content left between two nodes by ordinary ants in this search; The calculation formula is: ; in ,in Dynamic weight coefficient, which increases with the number of iterations; is the basic weight; is the growth rate; is the total concentration of pheromones released by elite ants in the current path search; The path length of elite ants; The calculation formula is: ; in, is the information intensity factor, indicating that the common ant The total concentration of pheromones released during the current path search; For ants The total length of the path traversed in this path search.

[0018] Furthermore, the S3 is specifically: Incremental PID is used as PID controller, 4-5-3 three-layer feedforward network structure is adopted, and BP neural network is established; The expected speed of the autonomous vehicle , current vehicle speed , the error between the actual speed and the expected speed , threshold 1 is input to the input layer of the BP neural network; After extracting hidden layer features through BP neural network, the gradient descent algorithm with additional inertia term is used to update the output layer weight coefficient through the performance index function to complete the weight update from hidden layer to output layer and from input layer to hidden layer. Generate three optimized parameters in the output layer of the BP neural network 、 、 , complete the design of PID controller; Based on the designed PID controller, the expected acceleration of the unmanned vehicle is obtained according to the expected speed and actual speed. .

[0019] Furthermore, the S4 is specifically: Establish an unmanned vehicle inverse dynamics model based on the unmanned vehicle longitudinal dynamics model; Input the desired acceleration of the unmanned vehicle into the longitudinal inverse dynamics model of the unmanned vehicle; Set the critical acceleration. When the expected acceleration of the unmanned vehicle is greater than the critical acceleration, the unmanned vehicle will drive. Throttle opening when the unmanned vehicle is driving for ; in, is the maximum torque of the motor; is the transmission ratio; is the tire radius; is the transmission efficiency; is the total driving force; When the desired acceleration of the unmanned vehicle is less than the critical acceleration, the unmanned vehicle adopts the braking mode; Brake master cylinder pressure when the unmanned vehicle brakes for: ; in, is the brake system gain, .

[0020] In a second aspect, the present invention provides a coal mine underground unmanned vehicle trajectory tracking system, which applies the above-mentioned coal mine underground unmanned vehicle trajectory tracking method, comprising: Establish a module to build the unmanned vehicle's trajectory prediction module and vehicle lateral dynamics tracking error model. The trajectory prediction module predicts the unmanned vehicle's future trajectory points based on the input planned trajectory. A front wheel steering angle module designs a lateral controller based on the vehicle lateral dynamics tracking error model, and obtains the front wheel steering angle control command of the unmanned vehicle through the lateral controller; The expected acceleration module designs a longitudinal controller based on the BP neural network. The longitudinal controller is a PID controller, and the expected acceleration of the unmanned vehicle is obtained according to the PID controller. The calculation module establishes a longitudinal inverse dynamics model of the unmanned vehicle, inputs the desired acceleration of the unmanned vehicle into the longitudinal inverse dynamics model of the unmanned vehicle, and calculates the throttle opening size or the brake pressure size; The trajectory tracking module inputs the throttle opening size or brake pressure size into the unmanned vehicle to obtain the unmanned vehicle speed, and then inputs the front wheel angle control command of the unmanned vehicle into the unmanned vehicle to obtain the front wheel angle of the unmanned vehicle, and completes the unmanned vehicle trajectory tracking through joint lateral and longitudinal control.

[0021] Compared with the prior art, the present invention has the following beneficial technical effects: The present invention provides a trajectory tracking method for an unmanned vehicle in an underground coal mine. Aiming at the real-time and robust requirements of trajectory tracking control in the complex environment of underground coal mines, a lateral and longitudinal coordinated control strategy is proposed. A lateral controller is designed based on a lateral dynamics tracking error model of the vehicle to obtain the front wheel angle control command of the unmanned vehicle. A BP neural network adaptive PID controller is used in the longitudinal controller to obtain the expected acceleration of the unmanned vehicle, and the throttle opening is calculated based on an inverse longitudinal dynamics model to achieve accurate tracking of the vehicle speed and anti-interference capabilities.

[0022] Furthermore, the proposed lateral controller utilizes a composite control architecture combining a feedforward controller with an improved LQR feedback controller. By incorporating an improved ant colony algorithm to optimize the LQR weight matrix Q, this approach addresses the low efficiency and tendency to fall into local optima associated with traditional parameter tuning. By discretizing the Q matrix parameter space and combining it with a dynamic pheromone update mechanism and an elite ant guidance strategy, the lateral controller more accurately tracks the desired trajectory, significantly improving trajectory tracking accuracy in the complex underground coal mine environment. This effectively addresses interference from narrow spaces, uneven surfaces, and obstacles, enhancing system robustness.

[0023] Furthermore, the longitudinal controller utilizes a BP neural network adaptive PID controller. This controller uses expected vehicle speed, actual vehicle speed, and speed error as inputs to dynamically adjust PID parameters. This allows for real-time adaptation to frequently changing conditions in underground coal mines, such as vehicle load, slope, and road friction coefficient. Through the self-learning capabilities of the BP neural network, the longitudinal control achieves precise vehicle speed tracking and strong anti-interference capabilities, avoiding the overshoot and response lag associated with traditional PID fixed parameters and ensuring stable operation of the unmanned vehicle in complex road conditions.

[0024] Furthermore, the coordinated lateral and longitudinal control strategy achieves deeply coupled optimization of steering and speed control. Lateral control, optimized through an improved ant colony algorithm, rapidly responds to trajectory deviations and adjusts the front wheel angle. Longitudinal control utilizes a neural network PID controller to dynamically adapt to environmental changes and precisely output driving or braking torque. Working together, these two strategies significantly enhance the real-time control capabilities of unmanned vehicles in coal mines. This meets the stringent requirements for high real-time and high reliability of control algorithms in the complex underground environment, effectively ensuring the safe and stable operation of unmanned vehicles in high-risk environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 Schematic diagram of a trajectory tracking method for an unmanned vehicle in an underground coal mine according to an embodiment of the present invention.

[0026] Figure 2 Schematic diagram of Q matrix data dispersion in an embodiment of the present invention.

[0027] Figure 3 Schematic diagram of the ant colony algorithm process in an embodiment of the present invention.

[0028] Figure 4 Schematic diagram of the control of the lateral controller in an embodiment of the present invention.

[0029] Figure 5 Schematic diagram of the longitudinal controller control in an embodiment of the present invention. DETAILED DESCRIPTION

[0030] In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0031] It should be noted that the terms "first", "second", etc. in the description and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that the numbers used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0032] See Figure 1 The present invention provides a method for tracking and controlling an unmanned vehicle in a coal mine, which comprises the following steps: S1, establish the unmanned vehicle trajectory prediction module and the vehicle lateral kinematic tracking error model. The trajectory prediction module predicts the future trajectory points of the unmanned vehicle based on the input planned trajectory; First, a prediction module is established to predict the future trajectory of the unmanned vehicle based on its current state and expected trajectory state. The trajectory prediction module formula is:

[0033] in, is the predicted horizontal axis; is the predicted ordinate; To predict the yaw rate; To predict the lateral velocity; To predict the longitudinal velocity; For the prediction time; is the longitudinal velocity; is the lateral velocity; is the yaw angular velocity; is the yaw angle; Based on the future trajectory of the unmanned vehicle, the front wheel angle of the unmanned vehicle is controlled in real time to effectively track the path. The input information of the prediction module can help the system determine the deviation between the current state of the vehicle and the target trajectory, thereby determining the appropriate front wheel angle adjustment amount. A vehicle lateral dynamics model is established based on the two degrees of freedom of vehicle lateral and vehicle yaw. The formula of the vehicle lateral kinematic tracking error model is:

[0034] Wherein, m is the mass of the vehicle; is the moment of inertia around the Z axis; a is the distance from the center of mass to the front axle, and b is the distance from the center of mass to the rear axle; is the front wheel turning angle; is the cornering stiffness of the front tire, is the cornering stiffness of the rear tire; is the yaw angular velocity; ; Lateral acceleration ; Yaw angular acceleration ; The lateral acceleration is obtained by transforming the formula based on the vehicle lateral dynamics model and yaw angular acceleration :

[0035]

[0036] When an unmanned vehicle tracks a target path, two errors are involved: distance deviation and heading deviation. The distance from the center of mass of the vehicle to the center line of the road is defined as the distance deviation. , the deviation between the vehicle heading and the road direction is defined as the heading deviation For heading deviation , we can get:

[0037] in, Represents the heading angle of the unmanned vehicle; Represents the ideal heading angle at the current moment.

[0038] Based on vehicle kinematic analysis, the expected lateral acceleration can be expressed as follows:

[0039] in, is the vehicle turning radius; The actual lateral acceleration of the vehicle for:

[0040] Then we can get:

[0041]

[0042] Will Type introduction In the formula, after sorting, we get:

[0043]

[0044] neglect ,Will The state space equation of the dynamic tracking error model can be obtained by changing the equation into a state space equation, where the state vector is: ; The state space equation of the dynamic tracking error model is obtained as:

[0045] A, B, and C are all matrices; U is the front wheel angle, and G is the desired yaw angle.

[0046]

[0047]

[0048]

[0049]

[0050] S2, designing a lateral controller based on the vehicle lateral dynamics tracking error model, and obtaining the front wheel steering angle of the unmanned vehicle through the lateral controller; The lateral controller designed based on the vehicle lateral dynamics tracking error model includes a lateral feedforward controller and an LQR feedback controller; The process of obtaining the front wheel angle of the unmanned vehicle through the lateral controller is as follows: Design LQR feedback controller; The ant colony algorithm is introduced to optimize the weight matrix of the LQR feedback controller and the optimized control law of the LQR feedback controller is obtained. Design a lateral feedforward controller and obtain its control law; The control law of the optimized LQR feedback controller and the control law of the lateral feedforward controller are added together to obtain the total control law of the lateral controller, that is, the total front wheel steering angle.

[0051] like Figure 4 As shown in Figure 1, the LQR control theory is used to design a lateral motion controller for trajectory tracking. The specific process of designing the LQR feedback controller is as follows: Determine the performance index of LQR feedback controller : ; Wherein, Q is a weighting matrix, and is a positive definite weighting matrix or a semi-positive definite weighting matrix; R is a positive definite weighting matrix; is the state vector; is the control input vector; The essence of LQR controller is to design the control law to minimize the equation. Constructing Hamiltonian function : ; Where A and B are matrices of the state space equations, and λ is the adjoint vector; Hamiltonian function Taking the derivative about U(t) and setting the derivative to 0, we get ; in, ; Riccati equation The solution; The control rate of the LQR feedback controller is obtained as: ; in, is the LQR feedback controller gain; The gain of the LQR controller Before solving the problem, it is necessary to determine the matrices A, B, Q, and R. Since the matrices A and B have been obtained in S1, the Q and R matrices are further determined.

[0052] The weight of the LQR feedback controller is optimized by the ant colony optimization algorithm to obtain the optimized weight of the LQR feedback controller; The control law of the optimized LQR feedback controller is obtained by the A matrix, B matrix and weight of the optimized LQR feedback controller obtained through the state space equation: ; in, = is the optimized LQR gain.

[0053] like Figures 2 to 3As shown in Figure 2, the specific process of optimizing the weight of the LQR feedback controller using the ant colony optimization algorithm is as follows: S21, select different Q matrix parameter solutions, select 4 values ​​in the parameter solution as an array, discretize the array, and select the R matrix as the unit matrix; set a length of 16-bit digital sequence , Storing ants The location information passed, the array corresponds to the 16-bit digital sequence; Optimize the four parameters in the Q array and select the range of the four quantities. To determine the Q array, you must determine The value of this patent is to take the parameter value as 4 digits, 2 digits for the integer part and 2 digits for the decimal part. It can correspond to a 16-bit digital sequence. Discretize the array in Q and start the experiment. There are 10 numbers to choose from at each horizontal coordinate point (0 to 9). From the horizontal coordinate point, each ant only moves forward one unit length at a time. After multiple iterations, an optimal path can be determined, and a 16-digit sequence can be obtained. According to Figure 2 As shown, determine the array

[0054] S22, the program starts to select m artificial ants and place them at the starting point, and sets the initial parameters of the algorithm; the initial parameters of the algorithm include: information heuristic factor The importance of visibility , pheromone volatility coefficient , initial pheromone concentration , pheromone strength and the maximum number of iterations .

[0055] S23, the iteration starts, the ant sets out, and each time it passes a node, it calculates the state transition probability according to the state transition probability formula; The state transition probability formula is:

[0056] in, is the pheromone that the ant transfers from node i to node j, i.e., the trajectory strength; For visibility information, usually = in is the distance from point i to point j; Information heuristic factor.

[0057] S24, use the roulette method to select the next node for the ant, move to that point, and update the position array at the same time; S25, after 16 units of time,m Each ant passes through 16 integer points, completes one iteration, and obtains the corresponding array ( ), and calculate the corresponding evaluation value according to the evaluation index function, and judge whether the Q matrix parameters are optimal by the corresponding evaluation value; the evaluation index function is:

[0058] Where T is the sampling time, LP is the simulation calculation point, and i is the time step index, from 1 to LP (the total number of time steps). The smaller the value, the better the parameter value of the Q matrix.

[0059] S26, when m Only artificial ants complete one iteration, and the pheromone content between two nodes Update, update pheromone as follows: ; ; ; in, The volatility coefficient of pheromone should be Among them, select S ants with small evaluation index function as elite ants, For elite ants The pheromone increment from point j to point j; For the The pheromone content between two nodes left by elite ants in this search; For ordinary ants at this time The pheromone increment from point j to point j; For the The pheromone content left between two nodes by ordinary ants in this search; The calculation formula is:

[0060] in ,in Dynamic weight coefficient, which increases with the number of iterations; is the basic weight; is the growth rate; is the total concentration of pheromones released by elite ants in the current path search; The path length of elite ants; The calculation formula is:

[0061] in, is the information intensity factor, indicating that the common ant The total concentration of pheromones released during the current path search; For ants The total length of the path traversed in this path search.

[0062] S27, repeat S23-S26, select the path of each iteration until the preset number of iterations is reached , the iteration ends, and the final 16-bit digital sequence is the Q matrix parameter of the LQR feedback controller After the improved ant colony algorithm is optimized, the optimal Q matrix parameters are obtained. Combined with the A, B matrix and R matrix obtained in step 1, the gain of the optimized LQR controller can be calculated. The control rate of the optimized LQR controller is

[0063] in, = is the optimized LQR gain.

[0064] Furthermore, if only the LQR algorithm is used , bring equation (15) back to the state space equation get:

[0065] The system has a steady-state error, which means that it cannot guarantee that the distance deviation and heading deviation of the unmanned vehicle are simultaneously zero during lateral movement. Therefore, a feedforward controller is designed to eliminate the steady-state error of the system.

[0066] like Figure 5 As shown, the feedforward controller is further designed. The process of designing the lateral feedforward controller is as follows: The front wheel steering angle output by the lateral feedforward controller is Then, the overall control law of the lateral controller is:

[0067] Will Type introduction The formula can be obtained

[0068] Performing Laplace transform on the above formula and finding the extreme value can obtain the steady-state error of the distance deviation and steady-state error of heading deviation They are: ; ; in, is the LQR gain after ant colony algorithm optimization; is the road curvature.

[0069] Steady-state error of distance deviation The steady-state error of the heading deviation can be compensated and eliminated by feedforward control. It cannot be eliminated. Then the control law of the lateral feedforward controller is obtained as follows:

[0070] in is the LQR gain after ant colony algorithm optimization; is the road curvature.

[0071] The control law of the optimized LQR feedback controller and the control law of the lateral feedforward controller are added together to obtain the total control law of the lateral controller, that is, the total front wheel steering angle. The total front wheel steering angle is sent to the unmanned vehicle, and the unmanned vehicle then feeds back the actual trajectory to control the front wheel steering angle of the unmanned vehicle.

[0072] S3, designing a longitudinal controller based on a BP neural network, wherein the longitudinal controller is a PID controller, and obtaining the desired acceleration of the unmanned vehicle according to the PID controller; The incremental PID is used as the PID controller, and the control algorithm of the PID controller is:

[0073] A 4-5-3 three-layer feedforward network structure is used to establish a BP neural network, and the input and output of the network structure are selected to convert the expected speed of the unmanned vehicle into , current vehicle speed , the error between the actual speed and the expected speed , threshold 1 is input to the input layer of BP neural network; three parameters of PID 、 、 As the output of BP neural network; The number of input variables in the input layer of the BP neural network is 4, and the input of the input layer is:

[0074] in,; The input and output of the hidden layer are shown as follows:

[0075] in,; The input and output of the output layer are:

[0076] in, , , is the weight coefficient of the hidden layer. The superscripts (1), (2), and (3) represent the input layer, hidden layer, and output layer respectively; Take the performance index function as

[0077] For input, is the output.

[0078] Since neural network is used to adjust the control parameters of PID, the number of output nodes corresponds to the three parameters of PID. Obviously, the output cannot be negative. Therefore, a non-negative s function is used as the neuron activation function, as shown in formula (34):

[0079] The weight training process uses the gradient descent algorithm with additional inertia term. Based on the weighted coefficient, the negative gradient direction search adjustment is realized, and the inertia term of searching for fast convergence of the global minimum is also added:

[0080] in, is the output layer weight coefficient The update amount at time k, η represents the learning rate, and α represents the inertia coefficient.

[0081]

[0082] Will use To approximate replacement; Finally, the three parameter outputs of the PID control algorithm are expressed as follows:

[0083] After analysis, the learning algorithm from the hidden layer to the output layer is:

[0084] In the formula, Similarly, the learning algorithm for the hidden layer weight coefficient can be obtained

[0085] in ; First, the BP neural network is used to predict the vehicle's expected speed. , current actual vehicle speed , speed error and a fixed threshold as the input layer, feature extraction is performed through the nonlinear activation function (34) of the hidden layer, and finally the optimized PID parameters are generated in the output layer. 、 、 The PID controller uses the current and historical errors Calculate the control quantity increment and add it up to get the total control quantity , which is directly mapped to the desired acceleration During the training process, the system is tested through performance indicators. To evaluate the error, the gradient descent method with additional inertia term (Equation 35) is used to adjust the weights of each layer through back propagation. The weight update from hidden layer to output layer (Equation 38) and the weight update from input layer to hidden layer (Equation 39) are both calculated through error signal and Finish,.

[0086] Calculate performance metrics after each training , judge whether the set input accuracy requirement is met. If the accuracy requirement is met, the training ends and the training weight is retained. Otherwise, the training continues until the accuracy requirement is met. The outputs of the three nodes in the output layer above are respectively used for the PID controller 、 、 This also means that the BP neural network can realize the self-tuning of PID controller parameters according to the real-time status of the vehicle.

[0087] S4, establishing a longitudinal inverse dynamics model of the unmanned vehicle, inputting the desired acceleration of the unmanned vehicle into the longitudinal inverse dynamics model of the unmanned vehicle to calculate the throttle opening size or the brake pressure size; Establish an unmanned vehicle inverse dynamics model based on the unmanned vehicle longitudinal dynamics model;

[0088] In the formula is the total driving force; is the expected longitudinal velocity and longitudinal acceleration; is the road slope; Input the desired acceleration of the unmanned vehicle into the longitudinal inverse dynamics model of the unmanned vehicle; Set the critical acceleration. When the expected acceleration of the unmanned vehicle is greater than the critical acceleration, the unmanned vehicle will drive. Throttle opening when the unmanned vehicle is driving for ; in, is the maximum torque of the motor; is the transmission ratio; is the tire radius; is the transmission efficiency; is the total driving force; When the desired acceleration of the unmanned vehicle is less than the critical acceleration, the unmanned vehicle adopts the braking mode; Brake master cylinder pressure when the unmanned vehicle brakes for: ; in, is the brake system gain, .

[0089] After judgment, the throttle opening or brake pressure of the unmanned vehicle is sent to the unmanned vehicle. The unmanned vehicle then feeds back the actual vehicle speed, thus achieving longitudinal control of the unmanned vehicle.

[0090] In step S5, the throttle opening size or the brake pressure size is input into the unmanned vehicle to obtain the speed of the unmanned vehicle, and then the front wheel angle control command of the unmanned vehicle is input into the unmanned vehicle. The unmanned vehicle then provides feedback on its actual state, thereby performing trajectory tracking control on the unmanned vehicle.

[0091] In a second aspect, the present invention further provides a coal mine underground unmanned vehicle trajectory tracking system, which applies the above-mentioned coal mine underground unmanned vehicle trajectory tracking method, comprising: Establish a module to build the unmanned vehicle's trajectory prediction module and vehicle lateral dynamics tracking error model. The trajectory prediction module predicts the unmanned vehicle's future trajectory points based on the input planned trajectory. A front wheel steering angle module designs a lateral controller based on the vehicle lateral dynamics tracking error model, and obtains the front wheel steering angle control command of the unmanned vehicle through the lateral controller; The expected acceleration module designs a longitudinal controller based on the BP neural network. The longitudinal controller is a PID controller, and the expected acceleration of the unmanned vehicle is obtained according to the PID controller. The calculation module establishes a longitudinal inverse dynamics model of the unmanned vehicle, inputs the desired acceleration of the unmanned vehicle into the longitudinal inverse dynamics model of the unmanned vehicle, and calculates the throttle opening size or the brake pressure size; The trajectory tracking module inputs the throttle opening size or brake pressure size into the unmanned vehicle to obtain the unmanned vehicle speed, and then inputs the front wheel angle control command of the unmanned vehicle into the unmanned vehicle to obtain the front wheel angle of the unmanned vehicle, and completes the unmanned vehicle trajectory tracking through joint lateral and longitudinal control.

[0092] The above shows and describes the basic principles and main features of the present invention and the advantages of the present invention. For those skilled in the art, it is obvious that the present invention is not limited to the details of the above exemplary embodiments, and the present invention can be implemented in other specific forms without departing from the spirit or basic characteristics of the present invention; therefore, no matter from which point of view, the embodiments should be regarded as exemplary and non-restrictive. The scope of the present invention is limited by the appended claims rather than the above description. Therefore, it is intended that all changes that fall within the meaning and scope of the equivalent elements of the claims are included in the present invention, and any figure signs in the claims should not be regarded as limiting the claims involved.

[0093] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A method for tracking the trajectory of an unmanned vehicle in a coal mine, characterized in that: The following steps are involved: S1, establish the unmanned vehicle trajectory prediction module and vehicle lateral dynamic tracking error model. The trajectory prediction module predicts the future trajectory points of the unmanned vehicle based on the input planning trajectory; S2, designing a lateral controller based on the vehicle lateral dynamics tracking error model, and obtaining a front wheel angle control command of the unmanned vehicle through the lateral controller; S3, design a longitudinal controller based on BP neural network, and obtain the expected acceleration of the unmanned vehicle according to the longitudinal controller; S4, establishing a longitudinal inverse dynamics model of the unmanned vehicle, inputting the desired acceleration of the unmanned vehicle into the longitudinal inverse dynamics model of the unmanned vehicle to calculate the throttle opening size or the brake pressure size; S5, the speed of the unmanned vehicle is obtained based on the throttle opening size or the brake pressure size, the front wheel angle of the unmanned vehicle is obtained based on the front wheel angle control instruction of the unmanned vehicle, and the trajectory tracking of the unmanned vehicle is completed through the combined lateral and longitudinal control.

2. The method for tracking the trajectory of an unmanned vehicle in an underground coal mine according to claim 1, characterized in that: Said S1 specifically includes: Establish a trajectory prediction module for the unmanned vehicle, and predict the future trajectory points of the unmanned vehicle based on the current state of the unmanned vehicle and the planned trajectory state; Based on the future trajectory of the unmanned vehicle, the front wheel angle of the unmanned vehicle is controlled in real time to effectively track the path; Establish the vehicle's lateral and yaw degrees of freedom to establish the vehicle's lateral kinematic tracking error model; The trajectory prediction module formula is: in, is the predicted horizontal axis; is the predicted vertical coordinate; To predict the yaw rate; To predict the lateral velocity; To predict the longitudinal velocity; For the prediction time; is the longitudinal velocity; is the lateral velocity; is the yaw angular velocity; is the yaw angle; The vehicle lateral dynamics model is established based on the two degrees of freedom of vehicle lateral and vehicle yaw. The formula is: ； Wherein, m is the mass of the vehicle; is the moment of inertia around the Z axis; a is the distance from the center of mass to the front axle, and b is the distance from the center of mass to the rear axle; is the front wheel turning angle; is the cornering stiffness of the front tire, is the cornering stiffness of the rear tire; is the yaw angular velocity; ; The lateral acceleration is calculated based on the vehicle lateral dynamics model formula and yaw angular acceleration ; Its formula is: Define the distance from the vehicle's center of mass to the road centerline as the distance deviation , the deviation between the vehicle heading and the road direction is defined as the heading deviation , heading deviation The calculation formula is: ； Distance Deviation The derivative calculation formula is: ； in, is the heading angle of the unmanned vehicle; is the ideal heading angle at the current moment; Based on distance deviation and heading error The state space equation of the vehicle lateral dynamics error tracking model is obtained, where the state vector ; The state space equation of the dynamic error tracking model is: ； 。 3. The method for tracking the trajectory of an unmanned vehicle in an underground coal mine according to claim 2, wherein: In S2, a lateral controller is designed based on the vehicle lateral dynamics tracking error model, and the front wheel angle control instruction of the unmanned vehicle is obtained through the lateral controller: Design LQR feedback controller; The ant colony algorithm is introduced to optimize the weight matrix of the LQR feedback controller and the optimized control law of the LQR feedback controller is obtained. Design a lateral feedforward controller and obtain its control law; The control law of the optimized LQR feedback controller and the control law of the lateral feedforward controller are added together to obtain the total control law of the lateral controller, namely the front wheel angle control command.

4. A method for tracking the trajectory of an unmanned vehicle in an underground coal mine according to claim 3, characterized in that: In S2, the process of designing the LQR feedback controller is specifically as follows: Determine the performance index of LQR feedback controller : ； Wherein, Q is a weighting matrix, and is a positive definite weighting matrix or a semi-positive definite weighting matrix; R is a positive definite weighting matrix; is the state vector; is the control input vector; Q and R are the weight matrices of the LQR feedback controller, is the state vector, is the control input vector; Based on the performance index of LQR feedback controller Constructing Hamiltonian function : ； Where A and B are matrices of the state space equations, and λ is the adjoint vector; Hamiltonian function Taking the derivative about U(t) and setting the derivative to 0, we get ; in, ; Riccati equation The solution; The control law of the LQR feedback controller is: ; in, is the LQR feedback controller gain; The weight of the LQR feedback controller is optimized by the ant colony optimization algorithm to obtain the optimized weight of the LQR feedback controller; The control law of the optimized LQR feedback controller is obtained by the A matrix, B matrix and weight of the optimized LQR feedback controller obtained through the state space equation: ； in, = is the optimized LQR gain.

5. The method for tracking the trajectory of an unmanned vehicle in an underground coal mine according to claim 4, characterized in that: The process of designing a lateral feedforward controller is as follows: The front wheel steering angle output by the lateral feedforward controller is Then, the overall control law of the lateral controller is: ; Substituting the overall control law of the lateral controller into the state space equation yields: ; and perform Laplace transform on this formula to find the extreme value to obtain the steady-state error of distance deviation and the steady-state error of heading deviation They are: ； ； in, is the LQR gain after ant colony algorithm optimization; is the road curvature; Steady-state error of distance deviation The lateral feedforward controller eliminates the steady-state error of the heading deviation It is impossible to eliminate the control law of the lateral feedforward controller: ； in, is the LQR gain after ant colony algorithm optimization; is the road curvature.

6. The method for tracking the trajectory of an unmanned vehicle in an underground coal mine according to claim 4, characterized in that: The specific process of optimizing the weight of the LQR feedback controller by the ant colony optimization algorithm is as follows: S21, select different Q matrix parameter solutions, select 4 values ​​in the parameter solution as an array, discretize the array, and select the R matrix as the unit matrix; set a length of 16-bit digital sequence , Storing ants The location information passed, the array corresponds to the 16-bit digital sequence; S22, place m artificial ants at the starting point and set the initial parameters of the algorithm; S23, the iteration starts, the ant sets out, and each time it passes a node, it calculates the state transition probability according to the state transition probability formula; S24, use the roulette method to select the next node for the ant, move to that point, and update the position array at the same time; S25, after 16 units of time, m Each ant passes through 16 integer points, completes an iteration, obtains the corresponding array, and calculates the corresponding evaluation value according to the evaluation index function, and judges whether the Q matrix parameters are optimal by the corresponding evaluation value; S26, when m Only one artificial ant completes one iteration and updates the pheromone content between two nodes; S27, repeat S23-S26, select the path of each iteration until the preset number of iterations is reached , the iteration ends, and the final 16-bit digital sequence is the Q matrix parameter of the LQR feedback controller.

7. A method for tracking the trajectory of an unmanned vehicle in an underground coal mine according to claim 6, characterized in that: The pheromone content between the two nodes Update, update pheromone as follows: ； ； ； in, is the pheromone volatility coefficient, Among them, S is the number of ants with the smallest evaluation index function as elite ants. For elite ants The pheromone increment from point j to point j; For the The pheromone content between two nodes left by elite ants in this search; For ordinary ants at this time The pheromone increment from point j to point j; For the The pheromone content left between two nodes by ordinary ants in this search; The calculation formula is: ； in ,in Dynamic weight coefficient, which increases with the number of iterations; is the basic weight; is the growth rate; is the total concentration of pheromones released by elite ants in the current path search; The path length of elite ants; The calculation formula is: ； in, is the information intensity factor, indicating that the common ant The total concentration of pheromones released during the current path search; For ants The total length of the path traversed in this path search.

8. The method for tracking the trajectory of an unmanned vehicle in an underground coal mine according to claim 1, characterized in that: The S3 is specifically: Incremental PID is used as PID controller, 4-5-3 three-layer feedforward network structure is adopted, and BP neural network is established; The expected speed of the autonomous vehicle , current vehicle speed , the error between the actual speed and the expected speed , threshold 1 is input to the input layer of the BP neural network; After extracting hidden layer features through BP neural network, the gradient descent algorithm with additional inertia term is used to update the output layer weight coefficient through the performance index function to complete the weight update from hidden layer to output layer and from input layer to hidden layer. Generate three optimized parameters in the output layer of the BP neural network 、 、 , complete the design of PID controller; Based on the designed PID controller, the expected acceleration of the unmanned vehicle is obtained according to the expected speed and actual speed. .

9. The method for tracking the trajectory of an unmanned vehicle in an underground coal mine according to claim 8, characterized in that: The S4 is specifically: Establish an unmanned vehicle inverse dynamics model based on the unmanned vehicle longitudinal dynamics model; Input the desired acceleration of the unmanned vehicle into the longitudinal inverse dynamics model of the unmanned vehicle; Set the critical acceleration. When the expected acceleration of the unmanned vehicle is greater than the critical acceleration, the unmanned vehicle will drive. Throttle opening when the unmanned vehicle is driving for ； in, is the maximum torque of the motor; is the transmission ratio; is the tire radius; is the transmission efficiency; is the total driving force; When the desired acceleration of the unmanned vehicle is less than the critical acceleration, the unmanned vehicle adopts the braking mode; Brake master cylinder pressure when the unmanned vehicle brakes for: ； in, is the brake system gain, .

10. A coal mine underground unmanned vehicle trajectory tracking system, applying the coal mine underground unmanned vehicle trajectory tracking method according to any one of claims 1 to 9, characterized in that: include: Establish a module to build the unmanned vehicle's trajectory prediction module and vehicle lateral dynamics tracking error model. The trajectory prediction module predicts the unmanned vehicle's future trajectory points based on the input planned trajectory. A front wheel steering angle module designs a lateral controller based on the vehicle lateral dynamics tracking error model, and obtains the front wheel steering angle control command of the unmanned vehicle through the lateral controller; The expected acceleration module designs a longitudinal controller based on the BP neural network. The longitudinal controller is a PID controller, and the expected acceleration of the unmanned vehicle is obtained according to the PID controller. The calculation module establishes a longitudinal inverse dynamics model of the unmanned vehicle, inputs the desired acceleration of the unmanned vehicle into the longitudinal inverse dynamics model of the unmanned vehicle, and calculates the throttle opening size or the brake pressure size; The trajectory tracking module inputs the throttle opening size or brake pressure size into the unmanned vehicle to obtain the unmanned vehicle speed, and then inputs the front wheel angle control command of the unmanned vehicle into the unmanned vehicle to obtain the front wheel angle of the unmanned vehicle, and completes the unmanned vehicle trajectory tracking through joint lateral and longitudinal control.

Images