CN115877841A - Trajectory tracking control method, device, equipment and storage medium for unmanned mine car - Google Patents
Trajectory tracking control method, device, equipment and storage medium for unmanned mine car Download PDFInfo
- Publication number
- CN115877841A CN115877841A CN202211540669.0A CN202211540669A CN115877841A CN 115877841 A CN115877841 A CN 115877841A CN 202211540669 A CN202211540669 A CN 202211540669A CN 115877841 A CN115877841 A CN 115877841A
- Authority
- CN
- China
- Prior art keywords
- determining
- error
- unmanned
- weight matrix
- mine car
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention belongs to the technical field of unmanned driving, and discloses a method, a device, equipment and a storage medium for controlling trajectory tracking of an unmanned mine car. The method comprises the following steps: establishing a track tracking error model of the unmanned mine car, discretizing the model, counteracting actuator delay by setting course angle pre-aiming distance, determining course angle error after pre-aiming to construct a target state variable, determining a gain coefficient by adopting a TS fuzzy model, determining a Q weight matrix and an R weight matrix after gain according to transverse error and road curvature, realizing a real-time transformation matrix, determining an optimal feedback control sequence according to the Q weight matrix, the R weight matrix and a discretized state space model, determining a target control quantity according to the optimal feedback control sequence and the target state variable, and controlling the unmanned mine car according to the target control quantity. Through the mode, the control quantity is given in advance when the track of the unmanned mine car is tracked, and the accuracy of track tracking control of the unmanned mine car is improved.
Description
Technical Field
The invention relates to the technical field of unmanned driving, in particular to a method, a device, equipment and a storage medium for controlling trajectory tracking of an unmanned mine car.
Background
In order to meet the increasing demand of people for mineral resources, the mining intensity is continuously increased. The mining enterprises improve the yield by increasing the number of mining equipment, but the severe working environment of the mine prevents human drivers from working for a long time, and the labor cost is increased. At present, a machine is used for completely replacing a person to operate, the limitation of severe operation environment is avoided, and the transportation safety and efficiency are improved. The unmanned mine car has larger delay in response of the actuator due to self reasons, and cannot accurately realize the track tracking control.
The above is only for the purpose of assisting understanding of the technical aspects of the present invention, and does not represent an admission that the above is prior art.
Disclosure of Invention
The invention mainly aims to provide a method, a device, equipment and a storage medium for controlling the trajectory tracking of an unmanned mine car, and aims to solve the technical problem that the unmanned mine car cannot accurately realize the trajectory tracking control due to the fact that the response of an actuator is delayed greatly.
In order to achieve the aim, the invention provides a trajectory tracking control method of an unmanned mine car, which comprises the following steps:
establishing a track tracking error model of the unmanned mine car;
discretizing the trajectory tracking error model to obtain a discretized state space model;
determining a course angle error after the preview according to the course angle preview distance;
constructing a target state variable according to the pre-aiming course angle error;
determining a Q weight matrix and an R weight matrix after gain according to the transverse error and the road curvature of the unmanned mine car;
determining an optimal feedback control sequence according to the Q weight matrix, the R weight matrix and the discretized state space model;
determining a target control quantity according to the optimal feedback control sequence and the target state variable;
and controlling the unmanned mine car according to the target control quantity.
Optionally, before determining the heading angle error after the preview according to the heading angle preview distance, the method further includes:
acquiring a preset distance, a first gain coefficient and a second gain coefficient;
determining a vehicle speed related distance according to the first gain coefficient and the vehicle speed of the unmanned mine vehicle;
determining a road curvature related distance according to the second gain coefficient and the road curvature of the unmanned tramcar;
and determining a course angle pre-aiming distance according to the preset distance, the vehicle speed related distance and the road curvature related distance.
Optionally, the determining a heading angle error after the preview according to the heading angle preview distance includes:
determining the expected advancing direction of the track point after the preview according to the course angle preview distance;
and determining a pre-aiming heading angle error according to the yaw angle of the unmanned tramcar relative to a geodetic coordinate system and the expected advancing direction.
Optionally, the determining the gained Q weight matrix and R weight matrix according to the lateral error and the road curvature of the unmanned mine car includes:
inquiring a first fuzzy rule table according to the transverse error of the unmanned mine car and the curvature of the road, and determining a transverse error gain coefficient;
determining a Q weight matrix after gain according to the transverse error gain coefficient;
inquiring a second fuzzy rule table according to the transverse error and the road curvature of the unmanned mine car, and determining a control quantity gain coefficient;
and determining the R weight matrix after the gain according to the control quantity gain coefficient.
Optionally, before the querying the first fuzzy rule table according to the lateral error and the road curvature of the unmanned mining vehicle, the method further comprises:
setting a transverse error domain and a road curvature domain, and performing fuzzification processing on the transverse error domain and the road curvature domain to obtain a plurality of transverse error subsets and a plurality of road curvature subsets;
and constructing a first fuzzy rule table and a second fuzzy rule table which take the plurality of transverse error subsets and the plurality of road curvature subsets as attribute names according to an expert rule formed by real vehicle debugging experience by adopting a TS fuzzy model.
Optionally, the determining an optimal feedback control sequence according to the Q weight matrix, the R weight matrix, and the discretized state space model includes:
constructing a system performance function according to the Q weight matrix, the R weight matrix and the discretized state space model;
constructing a Lagrange control function with multiplication constraint according to the system performance function;
constructing a Hamiltonian, and simplifying the Lagrange control function based on the Hamiltonian to obtain a simplified function;
deriving a target function for the simplified function according to a Riccati equation;
and carrying out iterative solution on the objective function to determine an optimal feedback control sequence.
Optionally, the constructing a target state variable according to the pre-aiming heading angle error includes:
and constructing a target state variable according to the pre-aiming course angle error, the change rate of the pre-aiming course angle error, the transverse error of the unmanned mine car and the change rate of the transverse error.
In order to achieve the above object, the present invention provides a trajectory tracking control device for an unmanned mining vehicle, including:
the model establishing module is used for establishing a track tracking error model of the unmanned mine car;
the discretization module is used for discretizing the trajectory tracking error model to obtain a discretized state space model;
the pre-aiming module is used for determining a pre-aimed course angle error according to a course angle pre-aiming distance;
the construction module is used for constructing a target state variable according to the pre-aiming course angle error;
the gain module is used for determining a Q weight matrix and an R weight matrix after gain according to the lateral error and the road curvature of the unmanned mine car;
the sequence determination module is used for determining an optimal feedback control sequence according to the Q weight matrix, the R weight matrix and the discretized state space model;
the control quantity determining module is used for determining a target control quantity according to the optimal feedback control sequence and the target state variable;
and the control module is used for controlling the unmanned mine car according to the target control quantity.
Further, in order to achieve the above object, the present invention also provides a trajectory tracking control apparatus for an unmanned mining vehicle, including: the system comprises a memory, a processor and a trajectory tracking control program of the unmanned mine car stored on the memory and capable of running on the processor, wherein the trajectory tracking control program of the unmanned mine car is configured to realize the trajectory tracking control method of the unmanned mine car.
Further, in order to achieve the above object, the present invention also proposes a storage medium having stored thereon a trajectory tracking control program for an unmanned mining vehicle, which when executed by a processor, implements the trajectory tracking control method for an unmanned mining vehicle as described above.
In the invention, a track tracking error model of the unmanned mine car is established; discretizing the trajectory tracking error model to obtain a discretized state space model; determining a course angle error after the preview according to the course angle preview distance so as to resist the delay of the actuator; constructing a target state variable according to the course angle error after the pre-aiming; determining a Q weight matrix and an R weight matrix after gain according to the transverse error and the road curvature of the unmanned mine car, and realizing matrix transformation along with the actual running condition; determining an optimal feedback control sequence according to the Q weight matrix, the R weight matrix and the discretized state space model; determining a target control quantity according to the optimal feedback control sequence and the target state variable; and controlling the unmanned mine car according to the target control quantity. Through the mode, the control quantity is given in advance when the track of the unmanned mine car is tracked, for example, the corner of the front wheel is given in advance before a curve, and the accuracy and the stability of the track tracking control of the vehicle can be considered under the conditions that the actuator is delayed greatly and roads with different curvatures.
Drawings
FIG. 1 is a schematic diagram of the construction of an unmanned mining vehicle trajectory tracking control apparatus for a hardware operating environment according to an embodiment of the present invention;
FIG. 2 is a schematic flow chart of a first embodiment of the trajectory tracking control method of the unmanned mining vehicle according to the invention;
FIG. 3 is a schematic view of a two degree-of-freedom mining vehicle dynamics model according to the present invention;
FIG. 4 is a schematic flow chart of a second embodiment of the trajectory tracking control method for the unmanned mining vehicle according to the invention;
FIG. 5 is a schematic flow chart of a third embodiment of the trajectory tracking control method for an unmanned mining vehicle according to the invention;
FIG. 6 is a schematic flow chart showing the trajectory tracking control method of the unmanned mining vehicle according to the present invention;
FIG. 7 is a block diagram showing the construction of a trajectory tracking control apparatus for an unmanned mining vehicle according to a first embodiment of the present invention.
The implementation, functional features and advantages of the objects of the present invention will be further explained with reference to the accompanying drawings.
Detailed Description
It should be understood that the specific embodiments described herein are merely illustrative of the invention and do not limit the invention.
Referring to fig. 1, fig. 1 is a schematic diagram of a trajectory tracking control device of an unmanned mining vehicle in a hardware operating environment according to an embodiment of the present invention.
As shown in fig. 1, the trajectory tracking control apparatus 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. Wherein a communication bus 1002 is used to enable connective communication between these components. The user interface 1003 may include a Display screen (Display), an input unit such as a Keyboard (Keyboard), and the optional user interface 1003 may also include a standard wired interface, a wireless interface. The network interface 1004 may optionally include a standard wired interface, a Wireless interface (e.g., a Wireless-Fidelity (Wi-Fi) interface). The Memory 1005 may be a Random Access Memory (RAM) or a Non-Volatile Memory (NVM), such as a disk Memory. The memory 1005 may alternatively be a storage device separate from the processor 1001 described previously.
Those skilled in the art will appreciate that the configuration shown in FIG. 1 does not constitute a limitation of the trajectory tracking control device for unmanned mining vehicles, and may include more or fewer components than those shown, or some components may be combined, or a different arrangement of components.
As shown in fig. 1, the memory 1005, which is a storage medium, may include therein an operating system, a network communication module, a user interface module, and a trajectory tracking control program of the unmanned mining vehicle.
In the trajectory tracking control device of the unmanned mining vehicle shown in fig. 1, the network interface 1004 is mainly used for data communication with a network server; the user interface 1003 is mainly used for data interaction with a user; the processor 1001 and the memory 1005 in the trajectory tracking control apparatus of the unmanned mine vehicle according to the present invention may be provided in the trajectory tracking control apparatus of the unmanned mine vehicle that calls the trajectory tracking control program of the unmanned mine vehicle stored in the memory 1005 through the processor 1001 and executes the trajectory tracking control method of the unmanned mine vehicle according to the embodiment of the present invention.
The embodiment of the invention provides a trajectory tracking control method of an unmanned mine car, and referring to FIG. 2, FIG. 2 is a schematic flow chart of a first embodiment of the trajectory tracking control method of the unmanned mine car.
In this embodiment, the trajectory tracking control method for the unmanned mine car includes the following steps:
step S10: and establishing a trajectory tracking error model of the unmanned mine car.
It is understood that the execution subject of the embodiment is the trajectory tracking control device of the unmanned tramcar, and the trajectory tracking control device of the unmanned tramcar may be a controller installed on the unmanned tramcar, and may also be other devices having the same or similar functions, and the embodiment is not limited thereto.
Referring to FIG. 3, FIG. 3 is a schematic view of a two-degree-of-freedom mining vehicle dynamics model according to the present invention; establishing a two-degree-of-freedom mine car dynamic model, establishing a trajectory tracking error model of the unmanned mine car based on the two-degree-of-freedom mine car dynamic model, and deducing a state space equation, wherein the two degrees of freedom refer to yaw and lateral motion of the vehicle, and the lateral motion refers to mass center and lateral deflection angle or lateral speed representation of the vehicle; preferably, the state variables are defined as
Wherein, the first and the second end of the pipe are connected with each other,
represents a lateral error and a heading angle error, respectively>
Represents the rate of change of the lateral error>
The state space equation of a track tracking error model of the unmanned mine car, which represents the change rate of the course angle error, is shown as the formula (1):
wherein, the first and the second end of the pipe are connected with each other,
C αf ,C αr ,v x ,m,I z ,l f ,l r ,δ,c R respectively representing the cornering stiffness of the front wheel of the mine car, the cornering stiffness of the rear wheel, the longitudinal speed of the vehicle, the mass of the mine car, the yaw moment of the mine car, the distance from the center of mass of the mine car to the front axle, the distance from the center of mass to the rear axle, the front wheel turning angle and the road curvature. />
Step S20: and discretizing the trajectory tracking error model to obtain a discretized state space model.
It should be appreciated that in one implementation, the above state-space equation is converted into a discrete system at fixed time steps by equation (2) using forward euler and backward euler methods:
in a specific implementation, the above formula (1) can be abbreviated as:
in the formula, x k Is the state quantity at time k, u k Is the amount of control at time k,
t is the sampling period.
Step S30: and determining the course angle error after the preview according to the course angle preview distance.
It should be appreciated that alternatively, the delay time of the vehicle actuator is determined by comparing the target front wheel steering angle of the real vehicle with the actual front wheel steering angle, and the heading angle pre-sight distance is determined from the delay time and the current vehicle speed, the delay time being, for example, 0.5s. Preferably, the heading angle preview distance consists of three parts: fixed distance, vehicle speed-related distance, and road curvature-related distance. In the specific implementation, track points after the pre-aiming on the running track of the vehicle are determined according to the pre-aiming distance of the heading angle, and the heading angle error after the pre-aiming is determined according to the advancing direction of the track points and the yaw angle of the unmanned mine car.
Step S40: and constructing a target state variable according to the pre-aimed course angle error.
Specifically, the step S40 includes: and constructing a target state variable according to the pre-aiming course angle error, the change rate of the pre-aiming course angle error, the transverse error of the unmanned mine car and the change rate of the transverse error.
It should be noted that the target state variable is constructed according to the heading angle error after the preview, the transverse error and the change rate of the transverse error and the transverse error
Step S50: and determining a Q weight matrix and an R weight matrix after gain according to the lateral error and the road curvature of the unmanned mine car.
It should be understood that the Q weight matrix is a weight matrix of the performance index function for the state quantity in the LQR control, and the R weight matrix is a weight matrix of the performance index function for the controlled quantity in the LQR control. Optionally, a plurality of transverse error ranges, a plurality of road curvature ranges and LQR weight matrix gain coefficients are set in advance, the mapping relationship is queried according to the current transverse error and road curvature of the unmanned mine car, corresponding first gain coefficients and second gain coefficients are determined, parameters in the Q weight matrix are adjusted according to the first gain coefficients, a Q weight matrix after gain is obtained, and parameters in the R weight matrix are adjusted according to the second gain coefficients, and an R weight matrix after gain is obtained. Preferably, a TS fuzzy model is adopted to construct a first fuzzy rule table and a second fuzzy rule table according to expert rules formed by real vehicle debugging experience, the first fuzzy rule table and the second fuzzy rule table are respectively inquired according to the current transverse error and road curvature of the unmanned mine vehicle, the gain coefficient of the LQR weight matrix is determined, the weight matrix is adjusted based on the gain coefficient of the LQR weight matrix, and a Q weight matrix and an R weight matrix after gain are obtained.
Step S60: and determining an optimal feedback control sequence according to the Q weight matrix, the R weight matrix and the discretized state space model.
Specifically, the step S60 includes: constructing a system performance function according to the Q weight matrix, the R weight matrix and the discretized state space model; constructing a Lagrange control function with multiplication constraint according to the system performance function; constructing a Hamiltonian, and simplifying the Lagrange control function based on the Hamiltonian to obtain a simplified function; deducing a target function from the simplified function according to a Riccati equation; and carrying out iterative solution on the objective function to determine an optimal feedback control sequence.
It should be noted that, considering the accuracy and stability of trajectory tracking, a system performance function is constructed as formula (3):
the lagrangian control problem with multiplicative constraints is constructed according to equation (3) above as equation (4):
constructing a Hamiltonian as equation (5):
the following formula (6) is obtained by simplifying the formula (4):
the following formula (7) can be derived and obtained from the above formula:
shown in equation (8):
obtaining an optimal sequence K = [ K ] of feedback control through iterative solution 1 ,k 2 ,k 3 ,k 4 ]。
Step S70: and determining a target control quantity according to the optimal feedback control sequence and the target state variable.
It should be understood that the state variables are based on the target state variables
And an optimal feedback control sequence K = [ K ] 1 ,k 2 ,k 3 ,k 4 ]The target control amount u is determined by the formula (9) k ：
Step S80: and controlling the unmanned mine car according to the target control quantity.
It should be noted that the target control quantity is an optimal feedback control rate, a vehicle front wheel corner is obtained by solving according to the optimal feedback control rate and in combination with the state variable, and the unmanned mine car is controlled based on the vehicle front wheel corner, so that the front wheel corner is given in advance before the curve to resist the delay of the actuator, and the accuracy of the track tracking control is improved.
In the embodiment, a trajectory tracking error model of the unmanned mine car is established; discretizing the trajectory tracking error model to obtain a discretized state space model; determining a course angle error after the preview according to the course angle preview distance so as to resist the delay of the actuator; constructing a target state variable according to the course angle error after the pre-aiming; determining a Q weight matrix and an R weight matrix after gain according to the transverse error and the road curvature of the unmanned mine car, and realizing matrix transformation along with the actual running condition; determining an optimal feedback control sequence according to the Q weight matrix, the R weight matrix and the discretized state space model; determining a target control quantity according to the optimal feedback control sequence and the target state variable; and controlling the unmanned mine car according to the target control quantity. Through the mode, the control quantity is given in advance when the track of the unmanned mine car is tracked, for example, the corner of the front wheel is given in advance before a curve, and the accuracy and the stability of the track tracking control of the vehicle can be considered under the conditions that the actuator is delayed greatly and roads with different curvatures.
Referring to fig. 4, fig. 4 is a schematic flow chart of a second embodiment of the trajectory tracking control method of the unmanned mining vehicle according to the invention.
Based on the first embodiment, before the step S30, the trajectory tracking control method for the unmanned mining vehicle in this embodiment further includes:
step S301: and acquiring a preset distance, a first gain coefficient and a second gain coefficient.
It can be understood that the course angle pre-aiming distance is used for pre-aiming track points on the driving track at a certain distance from the current position. Optionally, the preset distance is a fixed value set in advance, the first gain coefficient is a gain coefficient calibrated by a real vehicle and related to vehicle speed, and the second gain coefficient is a gain coefficient calibrated by a real vehicle and related to road curvature.
Step S302: and determining the vehicle speed related distance according to the first gain coefficient and the vehicle speed of the unmanned mine vehicle.
Step S303: and determining the road curvature related distance according to the second gain coefficient and the road curvature of the unmanned mine car.
Step S304: and determining a course angle pre-aiming distance according to the preset distance, the vehicle speed related distance and the road curvature related distance.
It should be noted that the heading angle pre-aiming distance is calculated according to the following formula (10):
wherein: d const Is a predetermined distance, v, c R Vehicle speed and road curvature, gain 1 ，gain 2 And the first gain coefficient and the second gain coefficient are respectively calibrated for the real vehicle.
Accordingly, the step S30 includes: determining the expected advancing direction of the track point after the preview according to the course angle preview distance; and determining a pre-aiming heading angle error according to the yaw angle of the unmanned tramcar relative to a geodetic coordinate system and the expected advancing direction.
In a specific implementation, the heading angle error after the preview is calculated by the following formula (11):
wherein the content of the first and second substances,
a yaw angle of the unmanned mine car relative to a geodetic coordinate system; />
The expected advancing direction of the vehicle after the preview is determined by the reference path direction of the preview.
In this embodiment, a preset distance, a first gain coefficient and a second gain coefficient are obtained; determining a vehicle speed related distance according to the first gain coefficient and the vehicle speed of the unmanned mine vehicle; determining a road curvature related distance according to the second gain coefficient and the road curvature of the unmanned mine car; and determining a course angle pre-aiming distance according to the preset distance, the vehicle speed related distance and the road curvature related distance. By the mode, the course angle pre-aiming distance is set according to the vehicle speed and the road curvature under the actual driving condition, so that the set course angle pre-aiming distance is more accurate, and the accuracy of track tracking control of the unmanned mine car is further improved.
Referring to FIG. 5, FIG. 5 is a schematic flow chart of a third embodiment of the trajectory tracking control method of the unmanned mining vehicle according to the invention.
Based on the first embodiment, the step S50 of the trajectory tracking control method for the unmanned mine car in this embodiment includes:
step S501: and inquiring a first fuzzy rule table (as shown in the table 1) according to the transverse error and the road curvature of the unmanned mine car, and determining a transverse error gain coefficient.
It is understood that by e y ,c R Querying a first fuzzy rule table (for lateral error and road curvature, respectively) to determine a lateral error gain factor q 1_gain As the gain of the first term of the weight matrix Q.
Further, before the step S501, the method further includes: setting a transverse error domain and a road curvature domain, and performing fuzzification processing on the transverse error domain and the road curvature domain to obtain a plurality of transverse error subsets and a plurality of road curvature subsets; and constructing a first fuzzy rule table and a second fuzzy rule table which take the plurality of transverse error subsets and the plurality of road curvature subsets as attribute names according to an expert rule formed by real vehicle debugging experience by adopting a TS fuzzy model.
In order to consider stability and accuracy of track tracking, avoid 'snaking' of an unmanned mine car when the unmanned mine car runs on a straight road, and reduce a transverse error as much as possible when the unmanned mine car passes a curve, a TS fuzzy model (Takagi-Sugeno) is adopted in the embodiment, and a first fuzzy rule table and a second fuzzy rule table are constructed according to an expert system rule formed by real car debugging experience.
In a specific implementation, the universe of discourse of input and output is determined and fuzzified. Setting a lateral error e y Has a discourse field of [ -1,1]Road curvature c R Has a discourse field of [ -0.2,0.2]In the fuzzification processing, the domain of discourse is divided into 5 subsets, namely { VL (minimum), L (small), M (medium), H (large) and VL (maximum) }. The output of TS fuzzy control is an accurate quantity, namely a constant or a linear combination of inputs, and the domain of the output weight matrix parameter gain is set to be {0.5, 0.8, 1.1 and 1.5}. GeneratingSee tables 1 and 2 for the first fuzzy rule table and the second fuzzy rule table of (1).
Table 1:
table 2:
step S502: and determining a Q weight matrix after the gain according to the transverse error gain coefficient.
Step S503: and inquiring a second fuzzy rule table according to the transverse error and the road curvature of the unmanned mine car, and determining a control quantity gain coefficient.
It will be appreciated that by e y ,c R (lateral error and road curvature, respectively) to determine the control gain factor r gain As the gain of the weight matrix R.
Step S504: and determining the R weight matrix after the gain according to the control quantity gain coefficient.
It should be noted that, in order to satisfy the control accuracy and stability, the weight matrix parameters need to be adjusted, specifically, the first term weight coefficient Q of the weight matrix Q is adjusted according to the following formula (12) 1 The adjustment is performed to adjust the weight coefficient R of the weight matrix R according to the following formula (13):
q 1_new ＝q 1_gain ·q 1 (12)
r new ＝r _gain ·r (13)
in the formula, q 1_new ,r _new The adjusted lateral error weight coefficient and the adjusted control quantity weight coefficient are respectively.
And constructing a Q weight matrix according to the adjusted transverse error weight coefficient, and constructing an R weight matrix according to the adjusted control quantity weight coefficient.
In a specific implementation, referring to fig. 6, fig. 6 is a specific flowchart of the trajectory tracking control method of the unmanned mine car according to the invention; the specific flow of the trajectory tracking control method of the unmanned mine car comprises the following steps:
step 1, establishing a two-degree-of-freedom dynamic model of the mine car shown in FIG. 3;
step 2, establishing a trajectory tracking error model according to the mine car two-degree-of-freedom dynamic model;
step 31, constructing a state space equation of the unmanned mine car trajectory tracking error model, as shown in formula (1); discretizing the state space model shown in the formula (1) by utilizing a forward Euler method and a backward Euler method to obtain a discretized state space model shown in a formula (2); establishing a controlled object model;
step 32, designing a heading angle pre-aiming distance related to the vehicle speed and the road curvature to obtain a pre-aiming heading angle error, as shown in a formula (10) and a formula (11);
step 33, constructing a fuzzy rule table according to expert experience by using a TS fuzzy method to obtain a weight coefficient gain coefficient, wherein the fuzzy rule table is shown in the above table 1 and table 2, and the gain weight is shown in the formula (12) and the formula (13);
step 4, final definition of the problem and problem solving;
step 41, constructing a performance index function as shown in formula (3);
step 42, obtaining an optimal feedback sequence K by utilizing the Riccati equation to carry out iterative solution, wherein the optimal feedback sequence K is shown in a formula (7) and a formula (8);
step 5, solving the optimal feedback control rate u k Acting on the unmanned mine car.
In the embodiment, a first fuzzy rule table is inquired according to the transverse error and the road curvature of the unmanned mine car, and a transverse error gain coefficient is determined; determining a Q weight matrix after gain according to the transverse error gain coefficient; inquiring a second fuzzy rule table according to the transverse error and the road curvature of the unmanned mine car, and determining a control quantity gain coefficient; and determining the R weight matrix after the gain according to the control quantity gain coefficient. Through the mode, the gain coefficient of the weight matrix related to the transverse error and the road curvature is designed through the TS fuzzy rule, so that the matrix is changed along with the actual running condition of the unmanned tramcar, the finally determined target control quantity can meet the running requirement, and the accuracy and the stability of track tracking can be improved under the condition of roads with different curvatures.
In addition, the embodiment of the invention also provides a storage medium, wherein the storage medium is stored with a trajectory tracking control program of the unmanned mine car, and the trajectory tracking control program of the unmanned mine car realizes the trajectory tracking control method of the unmanned mine car when being executed by a processor.
Since the storage medium adopts all technical solutions of all the embodiments, at least all the beneficial effects brought by the technical solutions of the embodiments are achieved, and no further description is given here.
Referring to fig. 7, fig. 7 is a block diagram showing the configuration of the first embodiment of the trajectory tracking control device of the unmanned mine car according to the present invention.
As shown in fig. 7, the trajectory tracking control device for an unmanned mine car according to the embodiment of the present invention includes:
and the model establishing module 10 is used for establishing a track tracking error model of the unmanned mine car.
And the discretization module 20 is configured to discretize the trajectory tracking error model to obtain a discretized state space model.
And the preview module 30 is used for determining the course angle error after preview according to the course angle preview distance.
And the construction module 40 is used for constructing a target state variable according to the pre-aiming course angle error.
And the gain module 50 is used for determining a Q weight matrix and an R weight matrix after gain according to the lateral error and the road curvature of the unmanned mine car.
A sequence determination module 60, configured to determine an optimal feedback control sequence according to the Q weight matrix, the R weight matrix, and the discretized state space model.
And a control quantity determining module 70, configured to determine a target control quantity according to the optimal feedback control sequence and the target state variable.
And the control module 80 is used for controlling the unmanned mine car according to the target control quantity.
It should be understood that the above is only an example, and the technical solution of the present invention is not limited in any way, and in a specific application, a person skilled in the art may set the technical solution as needed, and the present invention is not limited thereto.
In the embodiment, a track tracking error model of the unmanned mine car is established; discretizing the trajectory tracking error model to obtain a discretized state space model; determining a course angle error after the preview according to the course angle preview distance so as to resist the delay of the actuator; constructing a target state variable according to the course angle error after the pre-aiming; determining a Q weight matrix and an R weight matrix after gain according to the transverse error of the unmanned mine car and the curvature of the road, and realizing matrix transformation along with the actual running condition; determining an optimal feedback control sequence according to the Q weight matrix, the R weight matrix and the discretized state space model; determining a target control quantity according to the optimal feedback control sequence and the target state variable; and controlling the unmanned mine car according to the target control quantity. Through the mode, the control quantity is given in advance when the track of the unmanned mine car is tracked, for example, the corner of the front wheel is given in advance before a curve, and the accuracy and the stability of the track tracking control of the vehicle can be considered under the conditions that the actuator is delayed greatly and roads with different curvatures.
It should be noted that the above-mentioned work flows are only illustrative and do not limit the scope of the present invention, and in practical applications, those skilled in the art may select some or all of them according to actual needs to implement the purpose of the solution of the present embodiment, and the present invention is not limited herein.
In addition, the technical details that are not elaborated in this embodiment may refer to the trajectory tracking control method for the unmanned mining vehicle provided by any embodiment of the present invention, and are not described herein again.
In an embodiment, the preview module 30 is further configured to obtain a preset distance, a first gain coefficient, and a second gain coefficient; determining a vehicle speed related distance according to the first gain coefficient and the vehicle speed of the unmanned mine vehicle; determining a road curvature related distance according to the second gain coefficient and the road curvature of the unmanned mine car; and determining a course angle pre-aiming distance according to the preset distance, the vehicle speed related distance and the road curvature related distance.
In an embodiment, the preview module 30 is further configured to determine an expected forward direction of the track point after preview according to the heading angle preview distance; and determining a pre-aiming heading angle error according to the yaw angle of the unmanned tramcar relative to a geodetic coordinate system and the expected advancing direction.
In an embodiment, the gain module 50 is further configured to query a first fuzzy rule table according to the lateral error of the unmanned mining vehicle and the road curvature, and determine a lateral error gain coefficient; determining a Q weight matrix after gain according to the transverse error gain coefficient; inquiring a second fuzzy rule table according to the transverse error and the road curvature of the unmanned mine car, and determining a control quantity gain coefficient; and determining the R weight matrix after the gain according to the gain coefficient of the control quantity.
In an embodiment, the gain module 50 is further configured to set a lateral error domain and a road curvature domain, and perform fuzzification processing on the lateral error domain and the road curvature domain to obtain a plurality of lateral error subsets and a plurality of road curvature subsets; and constructing a first fuzzy rule table and a second fuzzy rule table which take the plurality of transverse error subsets and the plurality of road curvature subsets as attribute names according to an expert rule formed by real vehicle debugging experience by adopting a TS fuzzy model.
In an embodiment, the sequence determination module 60 is further configured to construct a system performance function according to the Q weight matrix, the R weight matrix, and the discretized state space model; constructing a Lagrange control function with multiplication constraint according to the system performance function; constructing a Hamiltonian, and simplifying the Lagrange control function based on the Hamiltonian to obtain a simplified function; deriving a target function for the simplified function according to a Riccati equation; and carrying out iterative solution on the objective function to determine an optimal feedback control sequence.
In an embodiment, the building module 40 is further configured to build a target state variable according to the pre-aiming heading angle error, the change rate of the pre-aiming heading angle error, the lateral error of the unmanned mining vehicle, and the change rate of the lateral error.
Furthermore, it should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or system. Without further limitation, an element defined by the phrase "comprising a … …" does not exclude the presence of another identical element in a process, method, article, or system that comprises the element.
The above-mentioned serial numbers of the embodiments of the present invention are merely for description and do not represent the merits of the embodiments.
Through the above description of the embodiments, those skilled in the art will clearly understand that the method of the above embodiments can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware, but in many cases, the former is a better implementation manner. Based on such understanding, the technical solution of the present invention or portions thereof that contribute to the prior art may be embodied in the form of a software product, where the computer software product is stored in a storage medium (e.g. Read Only Memory (ROM)/RAM, magnetic disk, optical disk), and includes several instructions for enabling a terminal device (e.g. a mobile phone, a computer, a server, or a network device) to execute the method according to the embodiments of the present invention.
The above description is only a preferred embodiment of the present invention, and not intended to limit the scope of the present invention, and all modifications of equivalent structures and equivalent processes, which are made by using the contents of the present specification and the accompanying drawings, or directly or indirectly applied to other related technical fields, are included in the scope of the present invention.
Claims (10)
1. A trajectory tracking control method of an unmanned mine car is characterized by comprising the following steps:
establishing a track tracking error model of the unmanned mine car;
discretizing the trajectory tracking error model to obtain a discretized state space model;
determining a course angle error after the preview according to the course angle preview distance;
constructing a target state variable according to the pre-aimed course angle error;
determining a Q weight matrix and an R weight matrix after gain according to the transverse error and the road curvature of the unmanned mine car;
determining an optimal feedback control sequence according to the Q weight matrix, the R weight matrix and the discretized state space model;
determining a target control quantity according to the optimal feedback control sequence and the target state variable;
and controlling the unmanned mine car according to the target control quantity.
2. The method for controlling trajectory tracking of an unmanned mining vehicle as claimed in claim 1, wherein before determining the post-heading angle error based on the heading angle pre-heading distance, the method further comprises:
acquiring a preset distance, a first gain coefficient and a second gain coefficient;
determining a vehicle speed related distance according to the first gain coefficient and the vehicle speed of the unmanned mine vehicle;
determining a road curvature related distance according to the second gain coefficient and the road curvature of the unmanned mine car;
and determining a course angle pre-aiming distance according to the preset distance, the vehicle speed related distance and the road curvature related distance.
3. The method for controlling trajectory tracking of an unmanned mining vehicle as claimed in claim 1, wherein said determining a pre-target heading angle error based on a heading angle pre-target distance comprises:
determining the expected advancing direction of the track point after the preview according to the course angle preview distance;
and determining a pre-aiming heading angle error according to the yaw angle of the unmanned tramcar relative to a geodetic coordinate system and the expected advancing direction.
4. The method for controlling trajectory tracking of an unmanned mining vehicle according to claim 1, wherein the determining of the Q-weight matrix and the R-weight matrix after the gain based on the lateral error and the road curvature of the unmanned mining vehicle comprises:
inquiring a first fuzzy rule table according to the transverse error of the unmanned mine car and the curvature of the road, and determining a transverse error gain coefficient;
determining a Q weight matrix after gain according to the transverse error gain coefficient;
inquiring a second fuzzy rule table according to the transverse error and the road curvature of the unmanned mine car, and determining a control quantity gain coefficient;
and determining the R weight matrix after the gain according to the gain coefficient of the control quantity.
5. The method for controlling trajectory tracking of an unmanned mining vehicle according to claim 4, wherein before querying the first fuzzy rule table based on lateral error and road curvature of the unmanned mining vehicle, the method further comprises:
setting a transverse error domain and a road curvature domain, and performing fuzzification processing on the transverse error domain and the road curvature domain to obtain a plurality of transverse error subsets and a plurality of road curvature subsets;
and constructing a first fuzzy rule table and a second fuzzy rule table which take the plurality of transverse error subsets and the plurality of road curvature subsets as attribute names according to an expert rule formed by real vehicle debugging experience by adopting a TS fuzzy model.
6. The method for trajectory tracking control of unmanned mining vehicles according to claim 1, wherein said determining an optimal feedback control sequence based on said Q weight matrix, said R weight matrix, and said discretized state space model comprises:
constructing a system performance function according to the Q weight matrix, the R weight matrix and the discretized state space model;
constructing a Lagrange control function with multiplication constraint according to the system performance function;
constructing a Hamiltonian, and simplifying the Lagrange control function based on the Hamiltonian to obtain a simplified function;
deducing a target function from the simplified function according to a Riccati equation;
and carrying out iterative solution on the objective function to determine an optimal feedback control sequence.
7. The method for controlling trajectory tracking of an unmanned mining vehicle as claimed in any one of claims 1 to 6, wherein said constructing a target state variable based on said pre-aimed heading angle error comprises:
and constructing a target state variable according to the pre-aiming course angle error, the change rate of the pre-aiming course angle error, the transverse error of the unmanned mine car and the change rate of the transverse error.
8. A trajectory tracking control device for an unmanned mining vehicle, characterized by comprising:
the model establishing module is used for establishing a track tracking error model of the unmanned mine car;
the discretization module is used for discretizing the track tracking error model to obtain a discretized state space model;
the pre-aiming module is used for determining a pre-aimed course angle error according to a course angle pre-aiming distance;
the construction module is used for constructing a target state variable according to the pre-aiming course angle error;
the gain module is used for determining a Q weight matrix and an R weight matrix after gain according to the transverse error and the road curvature of the unmanned mine car;
the sequence determination module is used for determining an optimal feedback control sequence according to the Q weight matrix, the R weight matrix and the discretized state space model;
the control quantity determining module is used for determining a target control quantity according to the optimal feedback control sequence and the target state variable;
and the control module is used for controlling the unmanned mine car according to the target control quantity.
9. An apparatus for trajectory tracking control of an unmanned mining vehicle, the apparatus comprising: a memory, a processor, and a trajectory tracking control program of an unmanned mining vehicle stored on the memory and executable on the processor, the trajectory tracking control program of the unmanned mining vehicle being configured to implement the trajectory tracking control method of the unmanned mining vehicle according to any one of claims 1 to 7.
10. A storage medium on which a trajectory tracking control program of an unmanned mining vehicle is stored, the trajectory tracking control program of the unmanned mining vehicle, when executed by a processor, implementing the trajectory tracking control method of the unmanned mining vehicle according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211540669.0A CN115877841A (en) | 2022-12-02 | 2022-12-02 | Trajectory tracking control method, device, equipment and storage medium for unmanned mine car |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211540669.0A CN115877841A (en) | 2022-12-02 | 2022-12-02 | Trajectory tracking control method, device, equipment and storage medium for unmanned mine car |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115877841A true CN115877841A (en) | 2023-03-31 |
Family
ID=85765667
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211540669.0A Pending CN115877841A (en) | 2022-12-02 | 2022-12-02 | Trajectory tracking control method, device, equipment and storage medium for unmanned mine car |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115877841A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116540527A (en) * | 2023-05-12 | 2023-08-04 | 中国矿业大学 | Mining truck model prediction speed change track tracking control method |
| CN117284277A (en) * | 2023-11-27 | 2023-12-26 | 理工雷科智途(北京)科技有限公司 | Underground articulated unmanned vehicle path tracking method based on hierarchical steering |
-
2022
- 2022-12-02 CN CN202211540669.0A patent/CN115877841A/en active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116540527A (en) * | 2023-05-12 | 2023-08-04 | 中国矿业大学 | Mining truck model prediction speed change track tracking control method |
| CN116540527B (en) * | 2023-05-12 | 2024-02-06 | 中国矿业大学 | Mining truck model prediction speed change track tracking control method |
| CN117284277A (en) * | 2023-11-27 | 2023-12-26 | 理工雷科智途(北京)科技有限公司 | Underground articulated unmanned vehicle path tracking method based on hierarchical steering |
| CN117284277B (en) * | 2023-11-27 | 2024-02-13 | 理工雷科智途(北京)科技有限公司 | Underground articulated unmanned vehicle path tracking method based on hierarchical steering |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115877841A (en) | Trajectory tracking control method, device, equipment and storage medium for unmanned mine car | |
| CN108445750B (en) | Method and system for vehicle motion planning | |
| CN109017984B (en) | Track following control method and system for unmanned vehicle and related device | |
| Li et al. | Indirect shared control for cooperative driving between driver and automation in steer-by-wire vehicles | |
| US20200278686A1 (en) | Iterative Feedback Motion Planning | |
| US20100228427A1 (en) | Predictive semi-autonomous vehicle navigation system | |
| Novi et al. | Real-time control for at-limit handling driving on a predefined path | |
| Yang et al. | An optimal goal point determination algorithm for automatic navigation of agricultural machinery: Improving the tracking accuracy of the Pure Pursuit algorithm | |
| Lima et al. | Experimental validation of model predictive control stability for autonomous driving | |
| WO2023010854A1 (en) | Path tracking method and apparatus, vehicle, and storage medium | |
| CN105867377A (en) | Automatic navigation control method of agricultural machine | |
| Chen et al. | Design and experimental evaluation of an efficient MPC-based lateral motion controller considering path preview for autonomous vehicles | |
| Timings et al. | Vehicle trajectory linearisation to enable efficient optimisation of the constant speed racing line | |
| Bayuwindra et al. | Extended look-ahead tracking controller with orientation-error observer for vehicle platooning | |
| Zhang et al. | Computationally aware control of autonomous vehicles: a hybrid model predictive control approach | |
| CN114291109B (en) | Shared driving conflict resolution method, system and computer equipment | |
| Weiskircher et al. | Predictive adas: A predictive trajectory guidance scheme for advanced driver assistance in public traffic | |
| Tan et al. | Driver directional control using two-point preview and fuzzy decision | |
| CN114721272A (en) | Trajectory tracking control method, apparatus, device and computer readable storage medium | |
| WO2023060586A1 (en) | Automatic driving instruction generation model optimization method and apparatus, device, and storage medium | |
| CN112793572A (en) | Adaptive cruise control method and device, electronic equipment and storage medium | |
| Hoeller et al. | Offroad navigation using adaptable motion patterns | |
| Yao et al. | Path Tracking Control for Four-Wheel-Independent-Driven Agricultural High Clearance Sprayer with New Front-Rear-Dual-Steering-Axle | |
| CN112486018B (en) | Model-free unmanned vehicle path tracking method based on speed adaptive preview | |
| CN111806444A (en) | Vehicle transverse control method and device, medium, equipment and vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |