Trajectory tracking control method, device, equipment and storage medium for unmanned mining vehicles

Abstract

This invention belongs to the field of unmanned driving technology and discloses a trajectory tracking control method, device, equipment, and storage medium for unmanned mining trucks. The method includes: establishing a trajectory tracking error model for the unmanned mining truck; discretizing the model; counteracting actuator delay by setting a heading angle pre-aiming distance; determining the heading angle error after pre-aiming to construct the target state variable; determining the gain coefficient using a TS fuzzy model; determining the Q-weight matrix and R-weight matrix after gain based on lateral error and road curvature to achieve real-time transformation matrices; determining the optimal feedback control sequence based on the Q-weight matrix, R-weight matrix, and the discretized state space model; determining the target control quantity based on the optimal feedback control sequence and the target state variable; and controlling the unmanned mining truck based on the target control quantity. Through this method, the unmanned mining truck can provide control quantities in advance during trajectory tracking, improving the accuracy of trajectory tracking control for unmanned mining trucks.

Description

Technical Field

[0001] This invention relates to the field of unmanned driving technology, and in particular to a trajectory tracking control method, device, equipment and storage medium for unmanned mining trucks. Background Technology

[0002] To meet the growing demand for mineral resources, mining efforts are constantly intensifying. Mining companies are increasing production by adding more mining equipment, but the harsh working environment in mines makes it impossible for human drivers to work for extended periods, increasing labor costs. Currently, machines are completely replacing humans in mining operations, eliminating the limitations of harsh working environments and improving transportation safety and efficiency. However, due to inherent limitations, driverless mining trucks suffer from significant actuator response delays, making precise trajectory tracking control impossible.

[0003] The above content is only used to help understand the technical solution of the present invention and does not represent an admission that the above content is prior art. Summary of the Invention

[0004] The main objective of this invention is to provide a trajectory tracking control method, device, equipment, and storage medium for unmanned mining vehicles, aiming to solve the technical problem that unmanned mining vehicles cannot accurately achieve trajectory tracking control due to the large delay in actuator response.

[0005] To achieve the above objectives, the present invention provides a trajectory tracking control method for an unmanned mining vehicle, the method comprising the following steps:

[0006] Establish a trajectory tracking error model for unmanned mining vehicles;

[0007] The trajectory tracking error model is discretized to obtain a discretized state-space model;

[0008] Determine the heading angle error after pre-aiming based on the heading angle pre-aiming distance;

[0009] The target state variables are constructed based on the heading angle error after pre-aiming;

[0010] The Q-weight matrix and R-weight matrix after gain are determined based on the lateral error of the unmanned mining vehicle and the road curvature.

[0011] The optimal feedback control sequence is determined based on the Q-weight matrix, the R-weight matrix, and the discretized state-space model.

[0012] The target control quantity is determined based on the optimal feedback control sequence and the target state variable;

[0013] The unmanned mining vehicle is controlled according to the target control quantity.

[0014] Optionally, before determining the heading angle error after aiming based on the heading angle aiming distance, the method further includes:

[0015] Obtain the preset distance, the first gain coefficient, and the second gain coefficient;

[0016] The speed-related distance is determined based on the first gain coefficient and the speed of the unmanned mining vehicle;

[0017] The road curvature-related distance is determined based on the second gain coefficient and the road curvature of the unmanned mining vehicle.

[0018] The heading angle aiming distance is determined based on the preset distance, the vehicle speed-related distance, and the road curvature-related distance.

[0019] Optionally, determining the heading angle error after aiming based on the heading angle aiming distance includes:

[0020] Determine the desired forward direction of the trajectory point after pre-aiming based on the heading angle and pre-aiming distance;

[0021] The heading angle error after pre-aiming is determined based on the yaw angle of the unmanned mining vehicle relative to the geodetic coordinate system and the desired direction of travel.

[0022] Optionally, determining the Q-weight matrix and R-weight matrix after gain based on the lateral error of the unmanned mining vehicle and the road curvature includes:

[0023] The lateral error gain coefficient is determined by querying the first fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature.

[0024] The Q-weight matrix after gain is determined based on the lateral error gain coefficient;

[0025] The control gain coefficient is determined by querying the second fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature.

[0026] The R-weight matrix after gain is determined based on the control quantity gain coefficient.

[0027] Optionally, before querying the first fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature, the method further includes:

[0028] Define the lateral error universe and the road curvature universe, and fuzzify the lateral error universe and the road curvature universe to obtain multiple lateral error subsets and multiple road curvature subsets;

[0029] Using the TS fuzzy model and expert rules formed based on real vehicle debugging experience, a first fuzzy rule table and a second fuzzy rule table are constructed with the attribute names of the multiple lateral error subsets and the multiple road curvature subsets.

[0030] Optionally, determining the optimal feedback control sequence based on the Q-weight matrix, the R-weight matrix, and the discretized state-space model includes:

[0031] The system performance function is constructed based on the Q-weight matrix, the R-weight matrix, and the discretized state-space model;

[0032] Construct a Lagrange control function with multiplication constraints based on the system performance function;

[0033] Construct a Hamiltonian function, and simplify the Lagrange control function based on the Hamiltonian function to obtain a simplified function;

[0034] The objective function is derived from the simplified function based on the Riccati equation;

[0035] The objective function is solved iteratively to determine the optimal feedback control sequence.

[0036] Optionally, constructing the target state variable based on the pre-aiming heading angle error includes:

[0037] The target state variables are constructed based on the heading angle error after pre-aiming, the rate of change of the heading angle error after pre-aiming, the lateral error of the unmanned mining vehicle, and the rate of change of the lateral error.

[0038] Furthermore, to achieve the above objectives, the present invention also proposes a trajectory tracking control device for an unmanned mining truck, the trajectory tracking control device for the unmanned mining truck comprising:

[0039] The model building module is used to build a trajectory tracking error model for unmanned mining vehicles.

[0040] The discretization module is used to discretize the trajectory tracking error model to obtain a discretized state-space model;

[0041] The aiming module is used to determine the heading angle error after aiming based on the aiming distance.

[0042] The construction module is used to construct target state variables based on the heading angle error after pre-aiming;

[0043] The gain module is used to determine the Q-weight matrix and R-weight matrix after gain based on the lateral error of the unmanned mining vehicle and the road curvature.

[0044] The sequence determination module is used to determine the optimal feedback control sequence based on the Q-weight matrix, the R-weight matrix, and the discretized state-space model.

[0045] A control quantity determination module is used to determine a target control quantity based on the optimal feedback control sequence and the target state variable;

[0046] The control module is used to control the unmanned mining vehicle according to the target control quantity.

[0047] Furthermore, to achieve the above objectives, the present invention also proposes a trajectory tracking control device for unmanned mining trucks. The trajectory tracking control device for unmanned mining trucks includes: a memory, a processor, and a trajectory tracking control program for unmanned mining trucks stored in the memory and executable on the processor. The trajectory tracking control program for unmanned mining trucks is configured to implement the trajectory tracking control method for unmanned mining trucks as described above.

[0048] In addition, to achieve the above objectives, the present invention also proposes a storage medium storing a trajectory tracking control program for an unmanned mining vehicle, wherein the trajectory tracking control program for the unmanned mining vehicle is executed by a processor to implement the trajectory tracking control method for the unmanned mining vehicle as described above.

[0049] This invention establishes a trajectory tracking error model for an unmanned mining vehicle; discretizes the trajectory tracking error model to obtain a discretized state-space model; determines the heading angle error after pre-aiming based on the heading angle pre-aiming distance to counteract actuator delay; constructs target state variables based on the pre-aiming heading angle error; determines the Q-weight matrix and R-weight matrix after gain based on the unmanned mining vehicle's lateral error and road curvature, realizing matrix transformation according to actual driving conditions; determines the optimal feedback control sequence based on the Q-weight matrix, R-weight matrix, and discretized state-space model; determines the target control quantity based on the optimal feedback control sequence and target state variables; and controls the unmanned mining vehicle based on the target control quantity. Through the above methods, the unmanned mining vehicle can provide control quantities in advance during trajectory tracking, such as providing the front wheel steering angle in advance before curves, thus balancing the accuracy and stability of vehicle trajectory tracking control under conditions of large actuator delay and roads with varying curvature. Attached Figure Description

[0050] Figure 1 This is a schematic diagram of the structure of the trajectory tracking control device for the unmanned mining truck in the hardware operating environment of the embodiment of the present invention;

[0051] Figure 2 This is a flowchart illustrating the first embodiment of the trajectory tracking and control method for unmanned mining vehicles of the present invention.

[0052] Figure 3 This is a schematic diagram of the two-degree-of-freedom mine car dynamics model of the present invention;

[0053] Figure 4 This is a flowchart illustrating the second embodiment of the trajectory tracking control method for unmanned mining vehicles of the present invention.

[0054] Figure 5 This is a flowchart illustrating the third embodiment of the trajectory tracking control method for unmanned mining vehicles of the present invention.

[0055] Figure 6 This is a schematic diagram illustrating the specific process of the trajectory tracking and control method for the unmanned mining vehicle of the present invention;

[0056] Figure 7 This is a structural block diagram of the first embodiment of the trajectory tracking control device for the unmanned mining vehicle of the present invention.

[0057] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0058] It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the scope of the invention.

[0059] Reference Figure 1 , Figure 1 This is a schematic diagram of the trajectory tracking control device for an unmanned mining vehicle in the hardware operating environment of an embodiment of the present invention.

[0060] like Figure 1 As shown, the trajectory tracking and control device of the unmanned mining vehicle may include: a processor 1001, such as a central processing unit (CPU), a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. The communication bus 1002 is used to enable communication between these components. The user interface 1003 may include a display screen or an input unit such as a keyboard; optionally, the user interface 1003 may also include a standard wired interface or a wireless interface. The network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a Wireless-Fidelity (Wi-Fi) interface). The memory 1005 may be high-speed random access memory (RAM) or stable non-volatile memory (NVM), such as a disk storage device. Optionally, the memory 1005 may also be a storage device independent of the aforementioned processor 1001.

[0061] Those skilled in the art will understand that Figure 1 The structure shown does not constitute a limitation on the trajectory tracking control device for unmanned mining vehicles, and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0062] like Figure 1As shown, the memory 1005, which serves as a storage medium, may include an operating system, a network communication module, a user interface module, and a trajectory tracking control program for the unmanned mining vehicle.

[0063] exist Figure 1 In the trajectory tracking control device of the unmanned mining vehicle shown, the network interface 1004 is mainly used for data communication with the network server; the user interface 1003 is mainly used for data interaction with the user; the processor 1001 and the memory 1005 in the trajectory tracking control device of the unmanned mining vehicle of the present invention can be set in the trajectory tracking control device of the unmanned mining vehicle. The trajectory tracking control device of the unmanned mining vehicle calls the trajectory tracking control program of the unmanned mining vehicle stored in the memory 1005 through the processor 1001 and executes the trajectory tracking control method of the unmanned mining vehicle provided in the embodiment of the present invention.

[0064] This invention provides a trajectory tracking control method for unmanned mining vehicles, referring to... Figure 2 , Figure 2 This is a flowchart illustrating the first embodiment of the trajectory tracking control method for unmanned mining vehicles according to the present invention.

[0065] In this embodiment, the trajectory tracking control method for the unmanned mining vehicle includes the following steps:

[0066] Step S10: Establish a trajectory tracking error model for unmanned mining vehicles.

[0067] It is understood that the execution subject of this embodiment is the trajectory tracking control device of the unmanned mining truck. The trajectory tracking control device of the unmanned mining truck can be a controller installed on the unmanned mining truck, or it can be other devices with the same or similar functions. This embodiment does not limit it.

[0068] It should be noted that, referring to Figure 3 , Figure 3 This is a schematic diagram of the two-degree-of-freedom mining truck dynamics model of the present invention. A two-degree-of-freedom mining truck dynamics model is established, and based on this model, a trajectory tracking error model for the unmanned mining truck is established, and the state-space equation is derived. The two degrees of freedom refer to the vehicle's yaw and lateral motion, where lateral motion is represented by the vehicle's center-of-gravity sideslip angle or lateral velocity. Preferably, the state variables are defined as follows: in, These represent the lateral error and the heading angle error, respectively. This represents the rate of change of the lateral error. The state-space equation of the trajectory tracking error model of the unmanned mining vehicle, which represents the rate of change of the heading angle error, is shown in Equation (1):

[0069]

[0070] in, C αf C αr ,v x ,m,I z ,l f ,l r ,δ,c R These represent the lateral stiffness of the front wheel of the mine car, the lateral stiffness of the rear wheel, the longitudinal speed of the vehicle, the mass of the mine car, the yaw moment of inertia of the mine car, the distance from the center of gravity of the mine car to the front axle, the distance from the center of gravity to the rear axle, the front wheel angle, and the road curvature, respectively.

[0071] Step S20: Discretize the trajectory tracking error model to obtain a discretized state-space model.

[0072] It should be understood that, in one implementation, the forward and backward Euler methods are used to transform the above state-space equations into a discrete system with a fixed time step using formula (2):

[0073]

[0074] In practical implementation, the above formula (1) can be simplified as follows:

[0075]

[0076] In the formula, x k Let u be the state variable at time k. k Let k be the control quantity at time k. T is the sampling period.

[0077] Step S30: Determine the heading angle error after pre-aiming based on the heading angle pre-aiming distance.

[0078] It should be understood that, optionally, the delay time of the vehicle actuator is determined by comparing the target front wheel angle of the actual vehicle with the actual front wheel angle. The heading angle prediction distance is then determined based on the delay time and the current vehicle speed. For example, the delay time is 0.5 seconds. Preferably, the heading angle prediction distance consists of three parts: a fixed distance, a speed-related distance, and a road curvature-related distance. In specific implementation, the predicted trajectory point on the vehicle's driving trajectory is determined based on the heading angle prediction distance, and the predicted heading angle error is determined based on the forward direction of the trajectory point and the yaw angle of the unmanned mining vehicle itself.

[0079] Step S40: Construct target state variables based on the heading angle error after pre-aiming.

[0080] Specifically, step S40 includes: constructing target state variables based on the pre-aiming heading angle error, the rate of change of the pre-aiming heading angle error, the lateral error of the unmanned mining vehicle, and the rate of change of the lateral error.

[0081] It should be noted that the target state variables are constructed based on the heading angle error after pre-aiming, combined with the lateral error and the rate of change of both.

[0082] Step S50: Determine the Q-weight matrix and R-weight matrix after gain based on the lateral error of the unmanned mining vehicle and the road curvature.

[0083] It should be understood that the Q-weight matrix is ​​the weight matrix of the performance index function for the state variable in LQR control, and the R-weight matrix is ​​the weight matrix of the performance index function for the control variable in LQR control. Optionally, multiple mapping relationships between lateral error ranges, multiple road curvature ranges, and the gain coefficients of the LQR weight matrix are pre-set. Based on the current lateral error and road curvature of the unmanned mining vehicle, the mapping relationship is queried to determine the corresponding first and second gain coefficients. The parameters in the Q-weight matrix are adjusted according to the first gain coefficient to obtain the gained Q-weight matrix. The parameters in the R-weight matrix are adjusted according to the second gain coefficient to obtain the gained R-weight matrix. Preferably, a first fuzzy rule table and a second fuzzy rule table are constructed using the TS fuzzy model based on expert rules formed from actual vehicle debugging experience. Based on the current lateral error and road curvature of the unmanned mining vehicle, the first and second fuzzy rule tables are queried respectively to determine the gain coefficients of the LQR weight matrix. The weight matrix is ​​adjusted based on the gain coefficients of the LQR weight matrix to obtain the gained Q-weight matrix and R-weight matrix.

[0084] Step S60: Determine the optimal feedback control sequence based on the Q weight matrix, the R weight matrix, and the discretized state-space model.

[0085] Specifically, step S60 includes: constructing a system performance function based on the Q-weight matrix, the R-weight matrix, and the discretized state-space model; constructing a Lagrange control function with multiplicative constraints based on the system performance function; constructing a Hamiltonian function; simplifying the Lagrange control function based on the Hamiltonian function to obtain a simplified function; deriving an objective function from the simplified function based on the Riccati equation; and iteratively solving the objective function to determine the optimal feedback control sequence.

[0086] It should be noted that, taking into account both the accuracy and stability of trajectory tracking, the system performance function is constructed as shown in formula (3):

[0087]

[0088] Based on the above formula (3), the Lagrange control problem with multiplication constraints is constructed as follows: formula (4):

[0089]

[0090] Construct the Hamiltonian function as shown in equation (5):

[0091]

[0092] Simplifying equation (4), we obtain the following formula (6):

[0093]

[0094] From the above formula and by finding the mechanism, we can obtain the following formula (7):

[0095]

[0096] As shown in formula (8):

[0097]

[0098] The optimal sequence K = [k1, k2, k3, k4] for feedback control is obtained through iterative solution.

[0099] Step S70: Determine the target control quantity based on the optimal feedback control sequence and the target state variable.

[0100] It should be understood that, based on the target state variable The target control quantity u is determined by formula (9) using the optimal feedback control sequence K = [k1, k2, k3, k4]. k :

[0101]

[0102] Step S80: Control the unmanned mining vehicle according to the target control quantity.

[0103] It should be noted that the target control quantity is the optimal feedback control law. Based on the optimal feedback control law and the state variables, the front wheel angle of the vehicle is calculated. The unmanned mining truck is controlled based on the front wheel angle, so that the unmanned mining truck can give the front wheel angle in advance before the curve to counteract the actuator delay and improve the accuracy of trajectory tracking control.

[0104] In this embodiment, a trajectory tracking error model for the unmanned mining truck is established. The trajectory tracking error model is discretized to obtain a discretized state-space model. The heading angle error after pre-aiming is determined based on the heading angle pre-aiming distance to counteract actuator delay. A target state variable is constructed based on the pre-aiming heading angle error. The Q-weight matrix and R-weight matrix after gain are determined based on the unmanned mining truck's lateral error and road curvature, realizing matrix transformation according to actual driving conditions. The optimal feedback control sequence is determined based on the Q-weight matrix, R-weight matrix, and the discretized state-space model. The target control quantity is determined based on the optimal feedback control sequence and the target state variable. The unmanned mining truck is controlled based on the target control quantity. Through the above methods, the unmanned mining truck can provide control quantities in advance during trajectory tracking, such as providing the front wheel steering angle before a curve, thus balancing the accuracy and stability of vehicle trajectory tracking control under conditions of large actuator delay and roads with varying curvature.

[0105] refer to Figure 4 , Figure 4 This is a flowchart illustrating the second embodiment of the trajectory tracking control method for unmanned mining vehicles of the present invention.

[0106] Based on the first embodiment described above, the trajectory tracking control method for the unmanned mining vehicle in this embodiment further includes, before step S30:

[0107] Step S301: Obtain the preset distance, the first gain coefficient, and the second gain coefficient.

[0108] Understandably, the heading angle preview distance is used to preview a trajectory point on the driving path a certain distance from the current position. Optionally, the preset distance is a fixed value set in advance, the first gain coefficient is a speed-related gain coefficient calibrated on the actual vehicle, and the second gain coefficient is a road curvature-related gain coefficient calibrated on the actual vehicle.

[0109] Step S302: Determine the speed-related distance based on the first gain coefficient and the speed of the unmanned mining vehicle.

[0110] Step S303: Determine the road curvature-related distance based on the second gain coefficient and the road curvature of the unmanned mining vehicle.

[0111] Step S304: Determine the heading angle aiming distance based on the preset distance, the vehicle speed-related distance, and the road curvature-related distance.

[0112] It should be noted that the heading angle aiming distance is calculated according to the following formula (10):

[0113]

[0114] Where: d const For the preset distance, v,c RThese represent vehicle speed and road curvature, respectively. gain1 and gain2 are the first and second gain coefficients calibrated on the actual vehicle, respectively.

[0115] Accordingly, step S30 includes: determining the desired forward direction of the trajectory point after pre-aiming based on the pre-aiming distance of the heading angle; and determining the heading angle error after pre-aiming based on the yaw angle of the unmanned mining vehicle relative to the geodetic coordinate system and the desired forward direction.

[0116] In the specific implementation, the heading angle error after pre-aiming is calculated using the following formula (11):

[0117]

[0118] in, The yaw angle of the unmanned mining vehicle relative to the geodetic coordinate system; The desired direction of the vehicle after aiming is determined by the direction of the reference path used for aiming.

[0119] In this embodiment, a preset distance, a first gain coefficient, and a second gain coefficient are obtained; a speed-related distance is determined based on the first gain coefficient and the speed of the unmanned mining truck; a road curvature-related distance is determined based on the second gain coefficient and the road curvature of the unmanned mining truck; and a heading angle prediction distance is determined based on the preset distance, the speed-related distance, and the road curvature-related distance. By setting the heading angle prediction distance according to the actual driving speed and road curvature, the set heading angle prediction distance is more accurate, further improving the precision of the unmanned mining truck trajectory tracking control.

[0120] refer to Figure 5 , Figure 5 This is a flowchart illustrating the third embodiment of the trajectory tracking control method for unmanned mining vehicles of the present invention.

[0121] Based on the first embodiment described above, step S50 of the trajectory tracking control method for the unmanned mining vehicle in this embodiment includes:

[0122] Step S501: Based on the lateral error of the unmanned mining vehicle and the road curvature, query the first fuzzy rule table (as shown in Table 1 below) to determine the lateral error gain coefficient.

[0123] It should be understood that, through e y ,c R (Representing lateral error and road curvature respectively) Query the first fuzzy rule table to determine the lateral error gain coefficient q. 1_gain , which is the gain as the first term of the weight matrix Q.

[0124] Further, before step S501, the method further includes: setting a lateral error domain and a road curvature domain, and performing fuzzification processing on the lateral error domain and the road curvature domain to obtain multiple lateral error subsets and multiple road curvature subsets; using the TS fuzzy model to construct a first fuzzy rule table and a second fuzzy rule table with the multiple lateral error subsets and the multiple road curvature subsets as attribute names based on expert rules formed from actual vehicle debugging experience.

[0125] To balance the stability and accuracy of trajectory tracking, prevent the unmanned mining truck from "snaking" when traveling on straight roads, and minimize lateral errors when cornering, this embodiment uses the Takagi-Sugeno fuzzy model. The first and second fuzzy rule tables are constructed based on expert system rules formed from real vehicle debugging experience.

[0126] In the specific implementation, the universe of discourse of the input and output quantities is first determined, and then fuzzified. The lateral error e is set. y The universe of discourse is [-1,1], and the road curvature is c. R The universe of discourse is [-0.2, 0.2]. During fuzzification, the universe of discourse is divided into 5 subsets: {VL (minimal), L (small), M (medium), H (large), VL (maximum)}. The output of TS fuzzy control is a precise quantity, i.e., a constant or a linear combination of the inputs. The universe of discourse for the output weight matrix parameter gain is set to {0.5, 0.8, 1.1, 1.5}. The generated first and second fuzzy rule tables are shown in Tables 1 and 2.

[0127] Table 1:

[0128]

[0129] Table 2:

[0130]

[0131] Step S502: Determine the Q-weight matrix after gain based on the lateral error gain coefficient.

[0132] Step S503: Based on the lateral error of the unmanned mining vehicle and the road curvature, query the second fuzzy rule table to determine the control quantity gain coefficient.

[0133] Understandably, through e y ,c R (These represent lateral error and road curvature, respectively) Query the second fuzzy rule table (as shown in Table 2 above) to determine the control gain coefficient r. gain , which is the gain of the weight matrix R.

[0134] Step S504: Determine the weighted R matrix after gain based on the control quantity gain coefficient.

[0135] It should be noted that, in order to meet the requirements of control accuracy and stability, the parameters of the weight matrix need to be adjusted. Specifically, the first weight coefficient q1 of the weight matrix Q is adjusted according to the following formula (12), and the weight coefficient r of the weight matrix R is adjusted according to the following formula (13):

[0136] q 1_new =q 1_gain ·q1 (12)

[0137] r new =r _gain ·r (13)

[0138] In the formula, q 1_new ,r _new These are the adjusted horizontal error weighting coefficient and the adjusted control quantity weighting coefficient, respectively.

[0139] Construct the Q-weight matrix based on the adjusted lateral error weighting coefficients, and construct the R-weight matrix based on the adjusted control quantity weighting coefficients.

[0140] In the specific implementation, refer to Figure 6 , Figure 6 This is a schematic diagram illustrating the specific flow of the trajectory tracking control method for the unmanned mining vehicle of the present invention; the specific flow of the trajectory tracking control method for the unmanned mining vehicle includes:

[0141] Step 1, establish as follows Figure 3 The mining truck's two-degree-of-freedom dynamic model is shown below;

[0142] Step 2: Establish a trajectory tracking error model based on the two-degree-of-freedom dynamic model of the mining car;

[0143] Step 31: Construct the state space equation of the unmanned mining truck trajectory tracking error model, as shown in Equation (1); Discretize the state space model shown in Equation (1) using the forward Euler and backward Euler methods to obtain the discretized state space model, as shown in Equation (2); Establish the controlled object model.

[0144] Step 32: Design the heading angle prediction distance related to vehicle speed and road curvature, and obtain the heading angle prediction error, as shown in formula (10) and formula (11);

[0145] Step 33: Using the TS fuzzy method, a fuzzy rule table is constructed based on expert experience to obtain the weight coefficients and gain coefficients. The fuzzy rule table is shown in Table 1 and Table 2 above, and the gain weights are shown in formula (12) and formula (13).

[0146] Step 4: Final definition of the problem, problem-solving;

[0147] Step 41, construct the performance index function, as shown in formula (3);

[0148] Step 42: Iteratively solve the Riccati equation to obtain the optimal feedback sequence K, as shown in formulas (7) and (8);

[0149] Step 5, calculate the optimal feedback control law u. k It is used in unmanned mining trucks.

[0150] In this embodiment, the lateral error gain coefficient is determined by querying the first fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature; the Q-weight matrix after gain is determined based on the lateral error gain coefficient; the control quantity gain coefficient is determined by querying the second fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature; and the R-weight matrix after gain is determined based on the control quantity gain coefficient. Through the above method, the gain coefficients of the weight matrix related to the lateral error and road curvature are designed using TS fuzzy rules, allowing the unmanned mining vehicle to change the matrix according to the actual driving conditions. This makes the final determined target control quantity more suitable for driving requirements, improving the accuracy and stability of trajectory tracking under different road curvature conditions.

[0151] Furthermore, this embodiment of the invention also proposes a storage medium storing a trajectory tracking control program for an unmanned mining vehicle. When the trajectory tracking control program for the unmanned mining vehicle is executed by a processor, it implements the trajectory tracking control method for the unmanned mining vehicle as described above.

[0152] Since this storage medium adopts all the technical solutions of all the above embodiments, it has at least all the beneficial effects brought about by the technical solutions of the above embodiments, which will not be repeated here.

[0153] Reference Figure 7 , Figure 7 This is a structural block diagram of the first embodiment of the trajectory tracking control device for the unmanned mining vehicle of the present invention.

[0154] like Figure 7 As shown, the trajectory tracking and control device for unmanned mining trucks proposed in this embodiment of the invention includes:

[0155] Model building module 10 is used to build a trajectory tracking error model for unmanned mining vehicles.

[0156] Discretization module 20 is used to discretize the trajectory tracking error model to obtain a discretized state-space model.

[0157] The aiming module 30 is used to determine the heading angle error after aiming based on the aiming angle and aiming distance.

[0158] The construction module 40 is used to construct target state variables based on the heading angle error after pre-aiming.

[0159] The gain module 50 is used to determine the Q-weight matrix and R-weight matrix after gain based on the lateral error of the unmanned mining vehicle and the road curvature.

[0160] The sequence determination module 60 is used to determine the optimal feedback control sequence based on the Q weight matrix, the R weight matrix, and the discretized state-space model.

[0161] The control quantity determination module 70 is used to determine the target control quantity based on the optimal feedback control sequence and the target state variable.

[0162] The control module 80 is used to control the unmanned mining vehicle according to the target control quantity.

[0163] It should be understood that the above are merely illustrative examples and do not constitute any limitation on the technical solutions of the present invention. In specific applications, those skilled in the art can make settings as needed, and the present invention does not impose any restrictions on this.

[0164] In this embodiment, a trajectory tracking error model for the unmanned mining truck is established. The trajectory tracking error model is discretized to obtain a discretized state-space model. The heading angle error after pre-aiming is determined based on the heading angle pre-aiming distance to counteract actuator delay. A target state variable is constructed based on the pre-aiming heading angle error. The Q-weight matrix and R-weight matrix after gain are determined based on the unmanned mining truck's lateral error and road curvature, realizing matrix transformation according to actual driving conditions. The optimal feedback control sequence is determined based on the Q-weight matrix, R-weight matrix, and the discretized state-space model. The target control quantity is determined based on the optimal feedback control sequence and the target state variable. The unmanned mining truck is controlled based on the target control quantity. Through the above methods, the unmanned mining truck can provide control quantities in advance during trajectory tracking, such as providing the front wheel steering angle before a curve, thus balancing the accuracy and stability of vehicle trajectory tracking control under conditions of large actuator delay and roads with varying curvature.

[0165] It should be noted that the workflow described above is merely illustrative and does not limit the scope of protection of this invention. In practical applications, those skilled in the art can select some or all of the workflow to achieve the purpose of this embodiment according to actual needs, and no restrictions are imposed here.

[0166] In addition, for technical details not described in detail in this embodiment, please refer to the trajectory tracking and control method of unmanned mining vehicles provided in any embodiment of the present invention, which will not be repeated here.

[0167] In one embodiment, the pre-aiming module 30 is further configured to acquire a preset distance, a first gain coefficient, and a second gain coefficient; determine a speed-related distance based on the first gain coefficient and the speed of the unmanned mining vehicle; determine a road curvature-related distance based on the second gain coefficient and the road curvature of the unmanned mining vehicle; and determine a heading angle pre-aiming distance based on the preset distance, the speed-related distance, and the road curvature-related distance.

[0168] In one embodiment, the pre-aiming module 30 is further configured to determine the desired forward direction of the pre-aimed trajectory point based on the pre-aiming distance of the heading angle; and to determine the heading angle error after pre-aiming based on the yaw angle of the unmanned mining vehicle relative to the geodetic coordinate system and the desired forward direction.

[0169] In one embodiment, the gain module 50 is further configured to: query a first fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature to determine a lateral error gain coefficient; determine a Q-weight matrix after gain based on the lateral error gain coefficient; query a second fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature to determine a control quantity gain coefficient; and determine a R-weight matrix after gain based on the control quantity gain coefficient.

[0170] In one embodiment, the gain module 50 is further configured to set the lateral error domain and the road curvature domain, and to perform fuzzification processing on the lateral error domain and the road curvature domain to obtain multiple lateral error subsets and multiple road curvature subsets; and to construct a first fuzzy rule table and a second fuzzy rule table with the multiple lateral error subsets and the multiple road curvature subsets as attribute names using the TS fuzzy model based on expert rules formed from actual vehicle debugging experience.

[0171] In one embodiment, the sequence determination module 60 is further configured to: construct a system performance function based on the Q-weight matrix, the R-weight matrix, and the discretized state-space model; construct a Lagrange control function with multiplicative constraints based on the system performance function; construct a Hamiltonian function; simplify the Lagrange control function based on the Hamiltonian function to obtain a simplified function; derive an objective function from the simplified function based on the Riccati equation; and iteratively solve the objective function to determine the optimal feedback control sequence.

[0172] In one embodiment, the construction module 40 is further configured to construct target state variables based on the pre-aiming heading angle error, the rate of change of the pre-aiming heading angle error, the lateral error of the unmanned mining vehicle, and the rate of change of the lateral error.

[0173] Furthermore, it should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or system. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or system that includes that element.

[0174] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0175] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as read-only memory (ROM) / RAM, magnetic disk, optical disk) and includes several instructions to cause a terminal device (which may be a mobile phone, computer, server, or network device, etc.) to execute the methods described in the various embodiments of the present invention.

[0176] The above are merely preferred embodiments of the present invention and do not limit the scope of the patent. Any equivalent structural or procedural transformations made based on the description and drawings of the present invention, or direct or indirect applications in other related technical fields, are similarly included within the scope of patent protection of the present invention.

Claims

1. A TS-LQR unmanned mining truck trajectory tracking control method considering actuator hysteresis response, characterized in that, The method includes: Establish a trajectory tracking error model for unmanned mining vehicles; The trajectory tracking error model is discretized to obtain a discretized state-space model; Determine the heading angle error after pre-aiming based on the heading angle pre-aiming distance; The target state variables are constructed based on the heading angle error after pre-aiming; The lateral error gain coefficient is determined by querying the first fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature. The Q-weight matrix after gain is determined based on the lateral error gain coefficient; The control gain coefficient is determined by querying the second fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature. The weighted R matrix after gain is determined based on the control quantity gain coefficient. The optimal feedback control sequence is determined based on the Q-weight matrix, the R-weight matrix, and the discretized state-space model. The target control quantity is determined based on the optimal feedback control sequence and the target state variable; The unmanned mining vehicle is controlled according to the target control quantity.

2. The TS-LQR unmanned mining truck trajectory tracking control method considering actuator hysteresis response as described in claim 1, characterized in that, Before determining the heading angle error after aiming based on the heading angle aiming distance, the method further includes: Obtain the preset distance, the first gain coefficient, and the second gain coefficient; The speed-related distance is determined based on the first gain coefficient and the speed of the unmanned mining vehicle; The road curvature-related distance is determined based on the second gain coefficient and the road curvature of the unmanned mining vehicle. The heading angle aiming distance is determined based on the preset distance, the vehicle speed-related distance, and the road curvature-related distance.

3. The TS-LQR unmanned mining truck trajectory tracking control method considering actuator hysteresis response as described in claim 1, characterized in that, The determination of the heading angle error after pre-aiming based on the heading angle pre-aiming distance includes: Determine the desired forward direction of the trajectory point after pre-aiming based on the heading angle and pre-aiming distance; The heading angle error after pre-aiming is determined based on the yaw angle of the unmanned mining vehicle relative to the geodetic coordinate system and the desired direction of travel.

4. The TS-LQR unmanned mining truck trajectory tracking control method considering actuator hysteresis response as described in claim 1, characterized in that, Before querying the first fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature, the method further includes: Define the lateral error universe and the road curvature universe, and fuzzify the lateral error universe and the road curvature universe to obtain multiple lateral error subsets and multiple road curvature subsets; Using the TS fuzzy model and expert rules formed based on real vehicle debugging experience, a first fuzzy rule table and a second fuzzy rule table are constructed with the attribute names of the multiple lateral error subsets and the multiple road curvature subsets.

5. The TS-LQR unmanned mining truck trajectory tracking control method considering actuator hysteresis response as described in claim 1, characterized in that, Determining the optimal feedback control sequence based on the Q-weight matrix, the R-weight matrix, and the discretized state-space model includes: The system performance function is constructed based on the Q-weight matrix, the R-weight matrix, and the discretized state-space model; Construct a Lagrange control function with multiplication constraints based on the system performance function; Construct a Hamiltonian function, and simplify the Lagrange control function based on the Hamiltonian function to obtain a simplified function; The objective function is derived from the simplified function based on the Riccati equation; The objective function is solved iteratively to determine the optimal feedback control sequence.

6. The TS-LQR unmanned mining truck trajectory tracking control method considering actuator hysteresis response as described in any one of claims 1 to 5, characterized in that, The step of constructing target state variables based on the pre-aiming heading angle error includes: The target state variables are constructed based on the heading angle error after pre-aiming, the rate of change of the heading angle error after pre-aiming, the lateral error of the unmanned mining vehicle, and the rate of change of the lateral error.

7. A TS-LQR unmanned mining truck trajectory tracking control device considering actuator hysteresis response, characterized in that, The device includes: The model building module is used to build a trajectory tracking error model for unmanned mining vehicles; The discretization module is used to discretize the trajectory tracking error model to obtain a discretized state-space model; The aiming module is used to determine the heading angle error after aiming based on the aiming distance. The construction module is used to construct target state variables based on the heading angle error after pre-aiming; The gain module is used to query a first fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature to determine the lateral error gain coefficient; determine the Q-weight matrix after gain based on the lateral error gain coefficient; query a second fuzzy rule table based on the lateral error of the unmanned mining vehicle and the road curvature to determine the control quantity gain coefficient; and determine the R-weight matrix after gain based on the control quantity gain coefficient. The sequence determination module is used to determine the optimal feedback control sequence based on the Q-weight matrix, the R-weight matrix, and the discretized state-space model. A control quantity determination module is used to determine a target control quantity based on the optimal feedback control sequence and the target state variable; The control module is used to control the unmanned mining vehicle according to the target control quantity.

8. A TS-LQR unmanned mining truck trajectory tracking control device considering actuator hysteresis response, characterized in that, The device includes: a memory, a processor, and a TS-LQR unmanned mining truck trajectory tracking control program stored in the memory and executable on the processor, the program being configured to implement the TS-LQR unmanned mining truck trajectory tracking control method considering actuator hysteresis response as described in any one of claims 1 to 6.

9. A storage medium, characterized in that, The storage medium stores a TS-LQR unmanned mining truck trajectory tracking control program that takes into account actuator hysteresis response. When the program is executed by the processor, it implements the TS-LQR unmanned mining truck trajectory tracking control method that takes into account actuator hysteresis response as described in any one of claims 1 to 6.

Images