Reversing control method, device, equipment and medium
Through the state space model based on the reversing dynamics and error rate change model, combined with a linear secondary regulator, the target front wheel angle of the unmanned mine car is determined, which solves the problem of accurate reversing control of the unmanned mine car and improves the control accuracy and Stability.
Patent Information
- Application Number
- CN202210862135.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-07-20
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2042-07-20
AI Technical Summary
During the reversing process, unmanned mine cars are difficult to achieve precise control due to their large size, high load capacity, narrow unloading ports and variable parking positions.
Based on the pre-constructed reverse dynamic model and error rate model, the state space model is determined, the current position information and reverse reference path information are obtained, the time-varying weight matrix is set, and the target front wheel angle is determined using a linear quadratic regulator to achieve Reversing control of unmanned mine cars.
It realizes precise control of the reverse working conditions of unmanned mine vehicles, improves control accuracy and stability, and reduces the risk of overshooting.
Smart Images

Figure CN115042791B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of unmanned vehicles, and in particular to a reversing control method, device, equipment and medium. Background Art
[0002] As the industrial sector pursues transportation safety and efficiency, advanced driver assistance systems (ADAS) are increasingly being used in transportation vehicles. Unmanned mining trucks, combined with 5G, big data and cloud computing, can autonomously control the return to and from loading and unloading points, achieving high-precision autonomous tracking. Among them, the unloading point, parking lot and shoveling area all require the vehicle to park autonomously according to the reversing trajectory.
[0003] However, unmanned mine trucks are large in size, heavy in load capacity, have narrow discharge ports, and the parking position and excavator position in the loading area are variable, making it impossible to accurately control the reversing of unmanned mine trucks. Summary of the invention
[0004] The main purpose of the present invention is to propose a reversing control method, device, equipment and medium, aiming to achieve precise control of the reversing condition of an unmanned mining vehicle.
[0005] To achieve the above object, the present invention provides a reversing control method, which comprises the following steps:
[0006] Determine a corresponding state space model based on a pre-built reversing dynamics model and a pre-built error change rate model, wherein the reversing dynamics model is created based on a linear two-degree-of-freedom model of the unmanned mining vehicle, and the error change rate model is created based on a lateral error change rate and a heading angle error change rate of the unmanned mining vehicle;
[0007] Acquire current posture information and reversing reference path information of the unmanned mining vehicle, and determine a current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information;
[0008] Based on the current state quantity, a corresponding time-varying weight matrix is set, and based on a preset linear quadratic regulator and the state space model, a target front wheel turning angle is determined to perform reverse control on the unmanned mining vehicle.
[0009] Preferably, before the step of determining the corresponding state space model based on the pre-built reversing dynamics model and the pre-built error change rate model, the reversing control method further includes:
[0010] Obtain the linear two-degree-of-freedom model of the unmanned mining vehicle, the cornering stiffness of the front wheel, the cornering stiffness of the rear wheel, the distance from the center of mass of the vehicle to the front axle, the distance from the center of mass of the vehicle to the rear axle, and the front wheel turning angle;
[0011] Determine a front wheel sideslip angle and a rear wheel sideslip angle according to the linear two-degree-of-freedom model, a distance from the vehicle center of mass to the front axle, a distance from the vehicle center of mass to the rear axle, and the front wheel turning angle;
[0012] Determining a corresponding front wheel lateral force according to the front wheel slip angle and the front wheel cornering stiffness;
[0013] Determining a corresponding rear wheel lateral force according to the rear wheel slip angle and the rear wheel cornering stiffness;
[0014] According to the front wheel lateral force and the rear wheel lateral force, a simplified force analysis is performed on the linear two-degree-of-freedom model to obtain a reversing dynamics model of the unmanned mining vehicle.
[0015] Preferably, the step of determining the corresponding state space model based on the pre-built reversing dynamics model and the pre-built error change rate model comprises:
[0016] Analyze the error change rate model to obtain analytical expressions of lateral change rate and heading angle change rate;
[0017] According to the reversing dynamics model, the error change rate model and the analytical expression, a corresponding state space model is determined.
[0018] Preferably, the step of determining the current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information comprises:
[0019] According to the current posture information, traverse the reference path points in the reversing reference path information, and determine the reference path point closest to the vehicle center of mass as the path projection point;
[0020] Calculating the lateral error and heading angle error of the unmanned mining vehicle according to the vehicle centroid point and the path projection point;
[0021] Obtaining a calculation cycle, a lateral error of a previous calculation cycle, and a heading angle error of a previous calculation cycle of the unmanned mining vehicle, and determining corresponding lateral error change rates and heading angle error change rates according to the calculation cycle, the lateral error of the previous calculation cycle, the heading angle error of the previous calculation cycle, the lateral error, and the heading angle error;
[0022] The current state quantity of the unmanned mining vehicle is determined according to the lateral error, the lateral error change rate, the heading angle error and the heading angle error change rate.
[0023] Preferably, the spatial state model includes characteristic parameters, and the steps of setting a corresponding time-varying weight matrix based on the current state quantity, and determining a target front wheel turning angle based on a preset linear quadratic regulator and the state space equation to perform reverse control on the unmanned mining vehicle include:
[0024] According to the current state quantity, setting a corresponding time-varying weight matrix;
[0025] According to the time-varying weight matrix, the characteristic parameters, and by using a preset linear quadratic regulator, a corresponding feedback gain rate is calculated;
[0026] Determining a corresponding target steering angle according to the feedback gain rate and the current state quantity, and performing a change rate limit process on the target steering angle to obtain a corresponding corrected front wheel steering angle;
[0027] The corrected front wheel turning angle is limited to determine the corresponding target front wheel turning angle, and a signal of the target front wheel turning angle is sent to the actuator of the unmanned mining vehicle to control the unmanned mining vehicle to reverse.
[0028] Preferably, after the step of setting a corresponding time-varying weight matrix based on the current state quantity and determining a target front wheel turning angle based on a preset linear quadratic regulator and the state space model to perform reverse control on the unmanned mining vehicle, the reverse control method further comprises:
[0029] Enter the next calculation cycle and return to step: obtain the current posture information and the reversing reference path information of the unmanned mining vehicle, and determine the current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information.
[0030] In addition, to achieve the above-mentioned purpose, the present invention further provides a reversing control device, the reversing control device comprising:
[0031] A determination module, used to determine a corresponding state space model based on a pre-built reversing dynamics model and a pre-built error change rate model, wherein the reversing dynamics model is created based on a linear two-degree-of-freedom model of the unmanned mining vehicle, and the error change rate model is created based on a lateral error change rate and a heading angle error change rate of the unmanned mining vehicle;
[0032] An acquisition module is used to acquire the current posture information and the reversing reference path information of the unmanned mining vehicle, and determine the current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information;
[0033] The control module is used to set a corresponding time-varying weight matrix based on the current state quantity, and determine a target front wheel turning angle based on a preset linear quadratic regulator and the state space model to perform reverse control on the unmanned mining vehicle.
[0034] In addition, to achieve the above-mentioned purpose, the present invention also provides a device, which is a reversing control device, and the reversing control device includes: a memory, a processor, and a reversing control program stored in the memory and executable on the processor, and when the reversing control program is executed by the processor, the steps of the reversing control method as described above are implemented.
[0035] In addition, to achieve the above-mentioned purpose, the present invention also provides a medium, which is a computer-readable storage medium, on which a reversing control program is stored, and when the reversing control program is executed by a processor, the steps of the reversing control method as described above are implemented.
[0036] The present invention proposes a reversing control method, device, equipment and medium; the reversing control method comprises: determining a corresponding state space model based on a pre-constructed reversing dynamics model and a pre-constructed error change rate model, wherein the reversing dynamics model is created based on a linear two-degree-of-freedom model of an unmanned mining vehicle, and the error change rate model is created based on a lateral error change rate and a heading angle error change rate of the unmanned mining vehicle; obtaining current posture information and reversing reference path information of the unmanned mining vehicle, and determining the current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information; setting a corresponding time-varying weight matrix based on the current state quantity, and determining a target front wheel turning angle based on a preset linear quadratic regulator and the state space model, so as to perform reversing control on the unmanned mining vehicle. The present invention determines the state space model corresponding to the unmanned mine vehicle according to a pre-constructed reversing dynamics model and a pre-constructed error change rate model; obtains the current posture information and reversing reference path information of the unmanned mine vehicle, and determines the current state quantity of the unmanned mine vehicle according to the current posture information and the reversing reference path information; sets the corresponding time-varying weight matrix according to the current state quantity; determines the corresponding target front wheel turning angle according to the time-varying weight matrix, the state quantity space model and a preset linear quadratic regulator, and performs reversing control on the unmanned mine vehicle through the target front wheel turning angle; thereby realizing precise control of the reversing working condition of the unmanned mine vehicle. BRIEF DESCRIPTION OF THE DRAWINGS
[0037] Figure 1 It is a schematic diagram of the device structure of the hardware operating environment involved in the embodiment of the present invention;
[0038] Figure 2 It is a flowchart of a first embodiment of a reversing control method of the present invention;
[0039] Figure 3 A schematic diagram of a sub-process of the first embodiment of the reverse control method of the present invention;
[0040] Figure 4 A schematic diagram of a flow chart of a second embodiment of a reversing control method of the present invention;
[0041] Figure 5 It is a schematic flow chart of a linear two-degree-of-freedom unmanned mining vehicle model according to the second embodiment of the reversing control method of the present invention;
[0042] Figure 6 A schematic diagram of a flow chart of a third embodiment of a reversing control method of the present invention;
[0043] Figure 7 A schematic diagram of a flow chart of a fourth embodiment of a reversing control method of the present invention;
[0044] Figure 8 A schematic diagram of lateral error and heading angle error of a fourth embodiment of a reversing control method of the present invention;
[0045] Fig. 9 A schematic diagram of a flow chart of a fifth embodiment of a reversing control method according to the present invention;
[0046] Fig.10 It is a schematic diagram of functional modules of the first embodiment of the reversing control device of the present invention.
[0047] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION
[0048] It should be understood that the specific embodiments described herein are only used to explain the present invention, and are not used to limit the present invention.
[0049] like Figure 1 As shown, Figure 1 It is a schematic diagram of the device structure of the hardware operating environment involved in the embodiment of the present invention.
[0050] The device in the embodiment of the present invention may be a mobile terminal or a server device.
[0051] like Figure 1As shown, the device may include: a processor 1001, such as a CPU, a network interface 1004, a user interface 1003, a memory 1005, and a communication bus 1002. Among them, the communication bus 1002 is used to realize the connection and 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 and a wireless interface. The network interface 1004 may optionally include a standard wired interface and a wireless interface (such as a WI-FI interface). The memory 1005 may be a high-speed RAM memory, or it may be a stable memory (non-volatile memory), such as a disk memory. The memory 1005 may also be a storage device independent of the aforementioned processor 1001.
[0052] Those skilled in the art will understand that Figure 1 The device structure shown in the figure does not constitute a limitation of the device, and may include more or less components than shown in the figure, or combine certain components, or arrange the components differently.
[0053] like Figure 1 As shown, the memory 1005 as a computer storage medium may include an operating system, a network communication module, a user interface module and a reversing control program.
[0054] Among them, the operating system is a program for managing and controlling the reversing control device and software resources, supporting the operation of the network communication module, user interface module, reversing control program and other programs or software; the network communication module is used to manage and control the network interface 1002; the user interface module is used to manage and control the user interface 1003.
[0055] exist Figure 1 In the reverse control device shown, the reverse control device calls the reverse control program stored in the memory 1005 through the processor 1001, and executes the operations in each embodiment of the following reverse control method.
[0056] Based on the above hardware structure, an embodiment of the reverse control method of the present invention is proposed.
[0057] Reference Figure 2 , Figure 2 This is a flow chart of a first embodiment of a reversing control method of the present invention, wherein the reversing control method comprises:
[0058] Step S10, determining a corresponding state space model based on a pre-constructed reversing dynamics model and a pre-constructed error change rate model, wherein the reversing dynamics model is created based on a linear two-degree-of-freedom model of the unmanned mining vehicle, and the error change rate model is created based on a lateral error change rate and a heading angle error change rate of the unmanned mining vehicle;
[0059] Step S20, obtaining the current posture information and the reversing reference path information of the unmanned mine vehicle, and determining the current state quantity of the unmanned mine vehicle according to the current posture information and the reversing reference path information;
[0060] Step S30, based on the current state quantity, a corresponding time-varying weight matrix is set, and based on a preset linear quadratic regulator and the state space model, a target front wheel turning angle is determined to perform reverse control on the unmanned mining vehicle.
[0061] This embodiment determines the state space model corresponding to the unmanned mine vehicle according to a pre-constructed reversing dynamics model and a pre-constructed error change rate model; obtains the current posture information and reversing reference path information of the unmanned mine vehicle, and determines the current state quantity of the unmanned mine vehicle according to the current posture information and the reversing reference path information; sets the corresponding time-varying weight matrix according to the current state quantity; determines the corresponding target front wheel turning angle according to the time-varying weight matrix, the state quantity space model and the preset linear quadratic regulator, and controls the reversing of the unmanned mine vehicle through the target front wheel turning angle; thereby realizing precise control of the reversing condition of the unmanned mine vehicle.
[0062] The following is a detailed description of each step:
[0063] Step S10, determining a corresponding state space model based on a pre-constructed reversing dynamics model and a pre-constructed error change rate model, wherein the reversing dynamics model is created based on a linear two-degree-of-freedom model of the unmanned mining vehicle, and the error change rate model is created based on a lateral error change rate and a heading angle error change rate of the unmanned mining vehicle.
[0064] In this embodiment, by analyzing the force relationship during the reversing process, the reversing dynamics model is made more accurate, and by designing a time-varying weight matrix that converges faster than a fixed value and has a smaller overshoot, the unmanned mine car driving is smoother and the control accuracy is higher. By establishing an unmanned mine car reversing model, designing a time-varying weight matrix, and calculating the target front wheel angle based on the linear quadratic regulator theory, the angle information is sent to the lower-level controller for response; thereby achieving precise control of the unmanned mine car reversing condition.
[0065] The state space model of the unmanned mine car is determined by analyzing the pre-built reversing dynamics model and the pre-built error change rate model. Among them, the linear two-degree-of-freedom model of the unmanned mine car is based on the mechanics to study the operation law of the unmanned mine car, including changes in vehicle position, speed, lateral force of the front and rear wheels, etc.; the reversing dynamics model is created based on the linear two-degree-of-freedom model of the unmanned mine car. Specifically, the reversing dynamics model of the unmanned mine car is obtained by performing a force analysis on the linear two-degree-of-freedom model of the unmanned mine car.
[0066] By analyzing the force relationship during the reversing process, the reversing dynamics model is made more accurate; the reversing dynamics model can include the longitudinal speed, longitudinal acceleration, lateral speed, lateral acceleration, yaw angular velocity and yaw angular acceleration of the unmanned mine car and other changes in the operating rules of the unmanned mine car.
[0067] The error change rate model is created based on the lateral error change rate and heading error change rate of the unmanned mining vehicle. Specifically, the lateral error change rate of the unmanned mining vehicle is calculated by and the rate of change of heading angle error Analyze and construct the corresponding error change rate model. Among them, the lateral error e y is the component of the relative distance between the vehicle center of mass and the reference path point on the y-axis of the vehicle coordinate system; the heading angle error e ψ is the difference between the vehicle heading and the reference path point heading; lateral error change rate Used to characterize the lateral error e of the unmanned mining vehicle y Speed of change; rate of change of heading angle error Used to characterize the heading angle error e of the unmanned mining vehicle ψ How fast the changes are.
[0068] Among them, the expression of the error change rate model is as follows:
[0069]
[0070] in, represents the lateral error change rate, v y represents the lateral velocity, e ψ Indicates the heading angle error, v x Indicates the longitudinal speed, Indicates the rate of change of heading angle error, is the yaw angular velocity, represents the yaw angular acceleration, Denotes the reference yaw rate.
[0071] The error change rate model is used to reduce the error of the unmanned mining vehicle's reversing process and improve the vehicle tracking accuracy.
[0072] By analyzing the force relationship during the reversing process, the reversing dynamics model is made more accurate; the reversing dynamics model can include the longitudinal speed, longitudinal acceleration, lateral speed, lateral acceleration, yaw angular velocity and yaw angular acceleration of the unmanned mine car and other changes in the operating rules of the unmanned mine car.
[0073] Among them, the state space model of the unmanned mine vehicle can be adjusted in time according to the changes in the posture information of the unmanned mine vehicle; among them, the state space model is expressed as follows:
[0074]
[0075] Among them, e y Indicates the lateral error of the unmanned mining vehicle, represents the lateral error change rate of the unmanned mining vehicle, e ψ Indicates the heading angle error of the unmanned mining vehicle, Indicates the rate of change of the heading angle error of the unmanned mining vehicle, v x Indicates the longitudinal speed of the unmanned mining vehicle, represents the longitudinal acceleration of the unmanned mining vehicle, v y Indicates the lateral speed of the unmanned mining vehicle, represents the lateral acceleration of the unmanned mining vehicle, m represents the mass of the unmanned mining vehicle, and I z represents the moment of inertia of the unmanned mining vehicle, represents the yaw angular velocity of the unmanned mining vehicle, represents the yaw angular acceleration of the unmanned mining vehicle, The reference heading angular velocity representing the reverse reference path information; C f represents the front wheel cornering stiffness of the unmanned mining vehicle, C r represents the rear wheel lateral stiffness of the unmanned mining vehicle, l f represents the distance from the vehicle's center of mass to the front axle, l r represents the distance from the vehicle's center of mass to the rear axle, δ f Indicates the front wheel turning angle of the unmanned mining vehicle.
[0076] make
[0077]
[0078]
[0079] That is, the final state space model is expressed as: Among them, X represents the state of the unmanned mine car, and the state X includes the lateral error e of the unmanned mine car y , Transverse error change rate Heading angle error e ψ And the rate of change of heading angle error
[0080] Step S20, obtaining the current position information and the reversing reference path information of the unmanned mining vehicle, and determining the current state quantity of the unmanned mining vehicle according to the current position information and the reversing reference path information.
[0081] In this embodiment, the current position information and the reversing reference path information of the unmanned mining vehicle are obtained from different channels; this embodiment does not limit the channels for the current position information and the reversing reference path information of the unmanned mining vehicle.
[0082] Among them, the posture information is used to represent the position and running posture of the unmanned mining vehicle.
[0083] The current posture information includes the center of mass point P of the unmanned mining vehicle ego (x ego ,y ego ,ψ ego ).
[0084] The reversing reference path information is ideal reversing path information, which is calculated based on the reversing starting point and the reversing ending point, and can be represented by coordinate points or curve equations.
[0085] By using the vehicle's center of mass point P ego (x ego ,y ego ,ψ ego ) and the reversing reference path information, calculate the projection point that matches the vehicle position in the reference path, that is, the path projection point P proj (x proj ,y proj ,ψ proj ); thus according to the vehicle center of mass point P ego (x ego ,y ego ,ψ ego ) and path projection point P proj (x proj ,y proj ,ψ proj ) Determine the current state quantity corresponding to the current posture information.
[0086] Step S30, based on the current state quantity, a corresponding time-varying weight matrix is set, and based on a preset linear quadratic regulator and the state space model, a target front wheel turning angle is determined to perform reverse control on the unmanned mining vehicle.
[0087] In this embodiment, according to the current state quantity obtained above, a time-varying weight matrix corresponding to the current state quantity is set; the expression of the time-varying weight matrix is as follows:
[0088]
[0089] R=r
[0090] Where q1 represents the lateral error e y The weight of q2 represents the lateral error change rate The weight of q3 represents the heading angle error e ψ The weight of , and q4 represents the rate of change of heading angle error The weight of .
[0091] Among them, the larger the weight values of q1, q2, q3 and q4, the higher the control degree of the control system of the unmanned mine car to the state quantity; that is, by adjusting the relative weights of q1, q2, q3 and q4, the reversing control accuracy of the unmanned mine car can be adjusted. R is the input of the state space model, that is, the weight corresponding to the steering wheel angle. The larger the weight, the greater the restriction on the steering wheel angle, that is, the smaller the steering angle. Since only the relative relationship between the Q matrix and the R matrix needs to be considered when designing, R is set as a constant value, and the weight matrix Q is designed to be time-varying.
[0092] According to the time-varying weight matrix, the state space model and the preset linear quadratic regulator, the feedback gain rate K corresponding to the current state quantity is calculated;
[0093] The target turning angle is calculated according to the current state quantity and the feedback gain rate K, and the target front wheel turning angle is determined by performing rate limit and amplitude limit processing on the target turning angle; then the unmanned mining vehicle is reversed according to the target front wheel turning angle.
[0094] Further, in one embodiment, referring to Figure 3 After step S30, the reversing control method further includes:
[0095] Step F10, enter the next calculation cycle, and return to step: obtain the current posture information and the reversing reference path information of the unmanned mining vehicle, and determine the current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information.
[0096] In this embodiment, after the steps of setting a corresponding time-varying weight matrix according to the current state quantity; determining the corresponding target front wheel angle according to the time-varying weight matrix, the state quantity space model and the preset linear quadratic regulator, and reversing the unmanned mine vehicle through the target front wheel angle, the next calculation cycle is entered, and the execution step is returned to: obtaining the current posture information and the reversing reference path information of the unmanned mine vehicle, and determining the current state quantity of the unmanned mine vehicle according to the current posture information and the reversing reference path information; after entering the next calculation cycle, the operation is repeated so that the unmanned mine vehicle can reverse according to the target front wheel angle; when the calculation of the target front wheel angle enters the next calculation cycle, the above calculation is repeated to complete the reversing control of the unmanned mine vehicle; thereby improving the efficiency of the reversing control of the unmanned mine vehicle.
[0097] This embodiment determines the state space model corresponding to the unmanned mine vehicle according to a pre-constructed reversing dynamics model and a pre-constructed error change rate model; obtains the current posture information and reversing reference path information of the unmanned mine vehicle, and determines the current state quantity of the unmanned mine vehicle according to the current posture information and the reversing reference path information; sets the corresponding time-varying weight matrix according to the current state quantity; determines the corresponding target front wheel turning angle according to the time-varying weight matrix, the state quantity space model and the preset linear quadratic regulator, and controls the reversing of the unmanned mine vehicle through the target front wheel turning angle; thereby realizing precise control of the reversing condition of the unmanned mine vehicle.
[0098] Furthermore, based on the first embodiment of the reversing control method of the present invention, a second embodiment of the reversing control method of the present invention is proposed.
[0099] The difference between the second embodiment of the reversing control method and the first embodiment of the reversing control method is that in this embodiment, before the step of determining the corresponding state space model based on the pre-built reversing dynamics model and the pre-built error change rate model in step S10, the Figure 4 , the reversing control method further includes:
[0100] Step A10, obtaining a linear two-degree-of-freedom model of the unmanned mining vehicle, a front wheel cornering stiffness, a rear wheel cornering stiffness, a distance from the center of mass of the vehicle to the front axle, a distance from the center of mass of the vehicle to the rear axle, and a front wheel turning angle;
[0101] Step A20, determining the front wheel sideslip angle and the rear wheel sideslip angle according to the linear two-degree-of-freedom model, the distance from the vehicle center of mass to the front axle, the distance from the vehicle center of mass to the rear axle, and the front wheel turning angle;
[0102] Step A30, determining a corresponding front wheel lateral force according to the front wheel sideslip angle and the front wheel sideslip stiffness;
[0103] Step A40, determining a corresponding rear wheel lateral force according to the rear wheel slip angle and the rear wheel cornering stiffness;
[0104] Step A50: performing simplified force analysis on the linear two-degree-of-freedom model according to the front wheel lateral force and the rear wheel lateral force to obtain a reversing dynamics model of the unmanned mining vehicle.
[0105] In this embodiment, the linear two-degree-of-freedom model, the front wheel cornering stiffness, the rear wheel cornering stiffness, the distance from the center of mass of the vehicle to the front axle, the distance from the center of mass of the vehicle to the rear axle, and the front wheel turning angle of the unmanned mining vehicle are obtained; the front wheel slip angle and the rear wheel slip angle are determined according to the linear two-degree-of-freedom model, the distance from the center of mass of the vehicle to the front axle, the distance from the center of mass of the vehicle to the rear axle, and the front wheel turning angle; the corresponding front wheel lateral force is determined according to the front wheel slip angle and the front wheel cornering stiffness; the corresponding rear wheel lateral force is determined according to the rear wheel slip angle and the rear wheel cornering stiffness; the linear two-degree-of-freedom model is simplified in force analysis according to the front wheel lateral force and the rear wheel lateral force to obtain the reversing dynamics model of the unmanned mining vehicle; and the accuracy of the reversing dynamics model is further improved by analyzing the force relationship during the reversing process.
[0106] The following is a detailed description of each step:
[0107] Step A10, obtaining the linear two-degree-of-freedom model of the unmanned mining vehicle, the front wheel cornering stiffness, the rear wheel cornering stiffness, the distance from the vehicle center of mass to the front axle, the distance from the vehicle center of mass to the rear axle, and the front wheel turning angle.
[0108] In this embodiment, the linear two-degree-of-freedom model of the unmanned mining vehicle and the front wheel cornering stiffness C are obtained from different channels. f , rear wheel cornering stiffness C r , the distance from the vehicle's center of mass to the front axle l f , the distance from the vehicle's center of mass to the rear axle l r and the front wheel turning angle δ f ; This embodiment obtains the linear two-degree-of-freedom model of the unmanned mining vehicle and the front wheel lateral stiffness C f , rear wheel cornering stiffness C r , the distance from the vehicle's center of mass to the front axle l f , the distance from the vehicle's center of mass to the rear axle l r and the front wheel turning angle δ f There is no limitation on channels.
[0109] The linear two-degree-of-freedom model of the unmanned mine car is based on mechanics to study the operation rules of the unmanned mine car, including changes in vehicle position, speed, and lateral force of the front and rear wheels. Figure 5 , Figure 5 The figure is a schematic diagram of the linear two-degree-of-freedom model of the unmanned mining vehicle; the expression of the linear two-degree-of-freedom model is as follows:
[0110]
[0111] in, Indicates the lateral acceleration of the unmanned mining vehicle, represents the yaw angular acceleration of the unmanned mining vehicle, m represents the mass of the unmanned mining vehicle, and F f Indicates the lateral force of the front wheel of the unmanned mining vehicle, F r represents the lateral force of the rear wheel of the unmanned mining vehicle, v x Indicates the longitudinal speed of the unmanned mining vehicle, v y Indicates the lateral speed of the unmanned mining vehicle, represents the yaw angular velocity of the unmanned mining vehicle, I z represents the moment of inertia of the unmanned mining vehicle, l f represents the distance from the vehicle's center of mass to the front axle, l r Represents the distance from the vehicle's center of mass to the rear axle.
[0112] Step A20, according to the linear two-degree-of-freedom model, the distance l from the center of mass of the vehicle to the front axle f , the distance from the vehicle center of mass to the rear axle and the front wheel turning angle, to determine the front wheel sideslip angle and the rear wheel sideslip angle.
[0113] In this embodiment, the longitudinal velocity v in the linear two-degree-of-freedom model obtained above is x , lateral velocity v y , yaw rate Distance from vehicle center of mass to front axle l f , the distance from the vehicle's center of mass to the rear axle l r , and the front wheel steering angle δ f , the front wheel slip angle and rear wheel slip angle of the unmanned mine car are analyzed; the expressions of the front wheel slip angle and rear wheel slip angle of the unmanned mine car are obtained as follows:
[0114]
[0115] Among them, α f represents the front wheel slip angle, α r Indicates the rear wheel slip angle.
[0116] Step A30, determining the corresponding front wheel lateral force according to the front wheel sideslip angle and the front wheel sideslip stiffness.
[0117] In this embodiment, the corresponding front wheel lateral force is determined according to the front wheel slip angle and the front wheel lateral stiffness of the unmanned mining vehicle; wherein the front wheel lateral force is the lateral force exerted on the front wheel tire of the unmanned mining vehicle, and the tire characteristics of the front wheel lateral force in the linear range can be expressed as:
[0118] F yf =2C f α f
[0119] Among them, F yf Expressed as the front wheel lateral force, C f Expressed as the front wheel cornering stiffness, α f Expressed as the front wheel slip angle.
[0120] Step A40: determining the corresponding rear wheel lateral force according to the rear wheel slip angle and the rear wheel cornering stiffness.
[0121] In this embodiment, the corresponding rear wheel lateral force is determined according to the rear wheel slip angle and the rear wheel lateral stiffness of the unmanned mining vehicle; wherein the rear wheel lateral force is the lateral force exerted on the rear wheel tire of the unmanned mining vehicle, and the tire characteristics of the rear wheel lateral force in the linear range can be expressed as:
[0122] F yr =2C r α r
[0123] Among them, F yr Expressed as rear wheel lateral force, C r Expressed as the rear wheel cornering stiffness, α r Expressed as the rear wheel slip angle.
[0124] Step A50: performing simplified force analysis on the linear two-degree-of-freedom model according to the front wheel lateral force and the rear wheel lateral force to obtain a reversing dynamics model of the unmanned mining vehicle.
[0125] In this embodiment, a simplified force analysis is performed on the linear two-degree-of-freedom model according to the front wheel lateral force and the rear wheel lateral force determined above, and a reversing dynamics model of the unmanned mining vehicle is obtained, wherein the expression of the reversing dynamics model is as follows:
[0126]
[0127] in, Indicates the lateral acceleration of the unmanned mining vehicle, represents the yaw angular acceleration of the unmanned mining vehicle, m represents the mass of the unmanned mining vehicle, and F yf represents the lateral force of the front wheel of the unmanned mining vehicle, F yr represents the lateral force of the rear wheel of the unmanned mining vehicle, v x Indicates the longitudinal speed of the unmanned mining vehicle, v y Indicates the lateral speed of the unmanned mining vehicle, represents the yaw angular velocity of the unmanned mining vehicle, I z represents the moment of inertia of the unmanned mining vehicle, l f represents the distance from the vehicle's center of mass to the front axle, l r represents the distance from the vehicle's center of mass to the rear axle, δ f Indicates the front wheel turning angle.
[0128] By using the expressions of the front wheel lateral force and the rear wheel lateral force determined by the front and rear wheel lateral stiffness and the front and rear wheel sideslip angles of the unmanned mining vehicle, the front wheel lateral force and the rear wheel lateral force in the linear two-degree-of-freedom model are equivalently replaced to obtain the reversing dynamics model of the unmanned mining vehicle.
[0129] In this embodiment, the linear two-degree-of-freedom model, the front wheel cornering stiffness, the rear wheel cornering stiffness, the distance from the center of mass of the vehicle to the front axle, the distance from the center of mass of the vehicle to the rear axle, and the front wheel turning angle of the unmanned mining vehicle are obtained; the front wheel slip angle and the rear wheel slip angle are determined according to the linear two-degree-of-freedom model, the distance from the center of mass of the vehicle to the front axle, the distance from the center of mass of the vehicle to the rear axle, and the front wheel turning angle; the corresponding front wheel lateral force is determined according to the front wheel slip angle and the front wheel cornering stiffness; the corresponding rear wheel lateral force is determined according to the rear wheel slip angle and the rear wheel cornering stiffness; the linear two-degree-of-freedom model is subjected to simplified force analysis according to the front wheel lateral force and the rear wheel lateral force to obtain the reversing dynamics model of the unmanned mining vehicle; and the accuracy of the reversing dynamics model is further improved by analyzing the force relationship during the reversing process.
[0130] Furthermore, based on the first and second embodiments of the reversing control method of the present invention, a third embodiment of the reversing control method of the present invention is proposed.
[0131] The third embodiment of the reversing control method is different from the first, second and third embodiments of the reversing control method in that the present embodiment is a refinement of the corresponding state space model determined based on the pre-constructed reversing dynamics model and the pre-constructed error change rate model in step S10, referring to Figure 6 , the step specifically includes:
[0132] Step S11, analyzing the error change rate model to obtain analytical expressions of lateral change rate and heading angle change rate;
[0133] Step S12, determining a corresponding state space model according to the reversing dynamics model, the error change rate model and the analytical expression.
[0134] In this embodiment, by analyzing the error change rate model, analytical expressions of the lateral change rate and the heading angle change rate are obtained; according to the reversing dynamics model, the error change rate model and the analytical expression, the state space model of the unmanned mining vehicle is determined; thereby, the reversing control accuracy of the unmanned mining vehicle is improved through the state space model.
[0135] The following is a detailed description of each step:
[0136] Step S11, analyzing the error change rate model to obtain analytical expressions of the lateral error change rate and the heading angle error change rate.
[0137] In this embodiment, the error change rate model is analyzed, wherein the expression of the error change rate model is as follows:
[0138]
[0139] in, represents the lateral error change rate, v y represents the lateral velocity, e ψ Indicates the heading angle error, v x Indicates the longitudinal speed, Indicates the rate of change of heading angle error, is the yaw angular velocity, represents the yaw angular acceleration, Denotes the reference yaw rate.
[0140] The specific analysis process is as follows: Assuming that during the reversing process of the unmanned mine car, the longitudinal speed v of the unmanned mine car x is a constant speed. At the same time, the reference yaw acceleration in the error change rate model is assumed to be On this basis, the error change rate model is analyzed to obtain the analytical expressions of the lateral error change rate and the heading angle error change rate. The analytical expressions are as follows:
[0141]
[0142] in, Indicates the lateral error change rate, Indicates the rate of change of heading angle error, represents the lateral acceleration, v x Indicates the longitudinal speed, represents the yaw angular velocity, represents the reference yaw rate, represents the yaw angular acceleration.
[0143] Step S12, determining a corresponding state space model according to the reversing dynamics model, the error change rate model and the analytical expression.
[0144] In this embodiment, the state space model corresponding to the unmanned mining vehicle is determined according to the reversing dynamics model, the error change rate model and the analytical expression; the specific process is as follows:
[0145] Among them, the expression of the reversing dynamics model is as follows:
[0146]
[0147] The expression of the error change rate model is as follows:
[0148]
[0149] The analytical expression is as follows:
[0150]
[0151] The above-mentioned reversing dynamics model, error change rate model and analytical expression are combined to determine the state space model corresponding to the unmanned mining vehicle, and the state space model corresponding to the unmanned mining vehicle is obtained. The state space model is expressed as follows:
[0152]
[0153] make And remember
[0154]
[0155]
[0156] That is, the final state space model is expressed as: Among them, X represents the state of the unmanned mine car, and the state X includes the lateral error e of the unmanned mine car y , Transverse error change rate Heading angle error e ψ And the rate of change of heading angle error Among them, the state space model of the unmanned mine car can be adjusted in time according to the changes in the posture information of the unmanned mine car.
[0157] In this embodiment, by analyzing the error change rate model, analytical expressions of the lateral change rate and the heading angle change rate are obtained; according to the reversing dynamics model, the error change rate model and the analytical expression, the state space model of the unmanned mining vehicle is determined; thereby, the reversing control accuracy of the unmanned mining vehicle is improved through the state space model.
[0158] Furthermore, based on the first, second and third embodiments of the reversing control method of the present invention, a fourth embodiment of the reversing control method of the present invention is proposed.
[0159] The fourth embodiment of the reversing control method is different from the first, second and third embodiments of the reversing control method in that the present embodiment is a refinement of step S20, which determines the current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information, referring to Figure 7 , the step specifically includes:
[0160] Step S21, according to the current posture information, traverse the reference path points in the reversing reference path information, and determine the reference path point closest to the vehicle centroid as the path projection point;
[0161] Step S22, calculating the lateral error and heading angle error of the unmanned mining vehicle according to the vehicle centroid point and the path projection point;
[0162] Step S23, obtaining the calculation cycle, the lateral error of the previous calculation cycle, and the heading angle error of the previous calculation cycle of the unmanned mining vehicle, and determining the corresponding lateral error change rate and heading angle error change rate according to the calculation cycle, the lateral error of the previous calculation cycle, the heading angle error of the previous calculation cycle, the lateral error, and the heading angle error;
[0163] Step S24, determining the current state of the unmanned mining vehicle according to the lateral error, the lateral error change rate, the heading angle error and the heading angle error change rate.
[0164] This embodiment traverses the reference path points in the reversing reference path information according to the current posture information, and determines the reference path point closest to the vehicle center of mass as the path projection point; calculates the lateral error and heading angle error of the unmanned mining vehicle according to the vehicle center of mass and the path projection point; obtains the calculation cycle, the lateral error of the previous calculation cycle, and the heading angle error of the previous calculation cycle of the unmanned mining vehicle, and determines the corresponding lateral error change rate and heading angle error change rate according to the calculation cycle, the lateral error of the previous calculation cycle, the heading angle error of the previous calculation cycle, the lateral error, and the heading angle error; determines the current state quantity of the unmanned mining vehicle according to the lateral error, the lateral error change rate, the heading angle error, and the heading angle error change rate; thereby improving the accuracy of the current state quantity.
[0165] The following is a detailed description of each step:
[0166] Step S21, according to the current posture information, traverse the reference path points in the reversing reference path information, and determine the reference path point closest to the vehicle center of mass as the path projection point.
[0167] In this embodiment, refer to Figure 8 , Figure 8 is a schematic diagram of lateral error and heading angle error; the posture information is used to represent the position and running posture of the unmanned mining vehicle; the position includes but is not limited to the vehicle center of mass; the current posture information includes the vehicle center of mass point P of the unmanned mining vehicle ego (x ego ,y ego ,ψ ego ); According to the vehicle center of mass, calculate the projection point that matches the vehicle position in the reference path, that is, the path projection point P proj (x proj ,y proj ,ψ proj ).
[0168] By traversing the reference path points in the reversing reference path information, find the reference path point closest to the vehicle centroid, and determine the reference path point as the path projection point P proj (x proj ,y proj ,ψ proj ).
[0169] Step S22, calculating the lateral error and heading angle error of the unmanned mining vehicle according to the vehicle centroid point and the path projection point.
[0170] In this embodiment, refer to Figure 8 , Figure 8 It is a schematic diagram of lateral error and heading angle error; according to the above vehicle center of mass point P ego (x ego ,y ego ,ψ ego ) and path projection point P proj (x proj ,y proj ,ψ proj ), calculate the vehicle center of mass point P ego (x ego ,y ego ,ψ ego ) and the reference path projection point P proj (x proj ,y proj ,ψ proj ) in the global coordinate system are ΔX and ΔY, which are calculated as follows:
[0171]
[0172] Define the lateral error e when the unmanned mining vehicle is on the left side of the road y Positive, lateral error e when on the right side of the road y Negative; lateral error e y The calculation is as follows:
[0173] e y =ΔYcos(ψ proj )-ΔXsin(ψ proj )
[0174] Among them, e y represents the lateral error, ΔX represents the vehicle center of mass point P ego (x ego ,y ego ,ψ ego ) and the reference path projection point P proj (x proj ,y proj ,ψ proj) on the x-axis of the global coordinate system, ΔY represents the center of mass of the vehicle P ego (x ego ,y ego ,ψ ego ) and the reference path projection point P proj (x proj ,y proj ,ψ proj ) on the y-axis of the global coordinate system, ψ ego represents the vehicle heading angle, ψ proj Indicates the road direction angle;
[0175] The heading angle error is defined as positive when the vehicle's head faces outward, and negative when the vehicle's head faces inward. The heading angle error is calculated as follows:
[0176] e ψ =ψ ego -ψ proj
[0177] Among them, e ψ represents the heading angle error, ψ ego represents the vehicle heading angle, ψ proj Indicates the road direction angle.
[0178] Step S23, obtaining the calculation cycle of the unmanned mining vehicle, the lateral error of the previous calculation cycle, and the heading angle error of the previous calculation cycle, and determining the corresponding lateral error change rate and heading angle error change rate according to the calculation cycle, the lateral error of the previous calculation cycle, the heading angle error of the previous calculation cycle, the lateral error, and the heading angle error.
[0179] In this embodiment, the calculation cycle of the unmanned mining vehicle, the lateral error of the previous calculation cycle, and the heading angle error of the previous calculation cycle are obtained from different channels; this embodiment does not limit the channel for obtaining the calculation cycle of the unmanned mining vehicle, the lateral error of the previous calculation cycle, and the heading angle error of the previous calculation cycle.
[0180] According to the calculation cycle, the lateral error of the previous calculation cycle, the heading angle error of the previous calculation cycle, the lateral error and the heading angle error, the corresponding lateral error change rate and heading angle error change rate are determined.
[0181] The calculation method of the lateral error change rate is as follows: the lateral error at the kth moment is e y (kT), the heading angle error at the k+1th moment is e y [(k+1)T], then the lateral error change rate at the k+1th moment is The calculation method is as follows:
[0182]
[0183] Wherein, T represents the calculation period.
[0184] The calculation method of the heading angle error change rate is: the heading angle error at the kth moment is e ψ (kT), the heading angle error at the k+1th moment is e ψ [(k+1)T], then the rate of change of heading angle error at the k+1th moment is The calculation method of is expressed as follows.
[0185]
[0186] Wherein, T represents the calculation period.
[0187] Step S24, determining the current state of the unmanned mining vehicle according to the lateral error, the lateral error change rate, the heading angle error and the heading angle error change rate.
[0188] In this embodiment, according to the current lateral error e of the unmanned mining vehicle y , Transverse error change rate Heading angle error e ψ And the rate of change of heading angle error Determine the current state quantity X of the unmanned mining vehicle. The expression of the current state quantity is The state quantity X includes the lateral error e of the unmanned mining vehicle y , Transverse error change rate Heading angle error e ψ And the rate of change of heading angle error
[0189] In this embodiment, according to the current posture information, the reference path points in the reversing reference path information are traversed, and the reference path point closest to the vehicle center of mass is determined as the path projection point; according to the vehicle center of mass and the path projection point, the lateral error and the heading angle error of the unmanned mining car are calculated; the calculation cycle, the lateral error of the previous calculation cycle, and the heading angle error of the previous calculation cycle of the unmanned mining car are obtained, and according to the calculation cycle, the lateral error of the previous calculation cycle, the heading angle error of the previous calculation cycle, the lateral error, and the heading angle error, the corresponding lateral error change rate and heading angle error change rate are determined; according to the lateral error, the lateral error change rate, the heading angle error, and the heading angle error change rate, the current state quantity of the unmanned mining car is determined; thereby improving the accuracy of the current state quantity.
[0190] Furthermore, based on the first, second, third and fourth embodiments of the reversing control method of the present invention, a fifth embodiment of the reversing control method of the present invention is proposed.
[0191] The fifth embodiment of the reversing control method is different from the first, second, third and fourth embodiments of the reversing control method in that the present embodiment is to set a corresponding time-varying weight matrix based on the current state quantity in step S30, and determine the target front wheel turning angle based on the preset linear quadratic regulator and the state space equation, so as to refine the reversing control of the unmanned mining vehicle, referring to Fig. 9 , the step specifically includes:
[0192] Step S31, setting a corresponding time-varying weight matrix according to the current state quantity;
[0193] Step S32, calculating a corresponding feedback gain rate according to the time-varying weight matrix, the characteristic parameters, and a preset linear quadratic regulator;
[0194] Step S33, determining a corresponding target steering angle according to the feedback gain rate and the current state quantity, and performing a change rate limit process on the target steering angle to obtain a corresponding corrected front wheel steering angle;
[0195] Step S34, limiting the corrected front wheel turning angle, determining the corresponding target front wheel turning angle, and sending a signal of the target front wheel turning angle to the actuator of the unmanned mining vehicle to control the unmanned mining vehicle to reverse.
[0196] This embodiment sets a corresponding time-varying weight matrix according to the current state quantity; calculates the corresponding feedback gain rate according to the time-varying weight matrix, the characteristic parameters of the state space model, and by presetting a linear quadratic regulator; determines the corresponding target turning angle according to the feedback gain rate and the current state quantity, and performs a change rate limit process on the target turning angle to obtain the corresponding corrected front wheel turning angle; performs a limit process on the corrected front wheel turning angle to determine the corresponding target front wheel turning angle, and sends the signal of the target front wheel turning angle to the actuator of the unmanned mine car to control the unmanned mine car to reverse; thereby achieving the control accuracy of the unmanned mine car reversing.
[0197] The following is a detailed description of each step:
[0198] Step S31, setting a corresponding time-varying weight matrix according to the current state quantity.
[0199] In this embodiment, according to the current state quantity X, a time-varying weight matrix corresponding to the current state quantity X is set, and the time-varying weight matrix includes a Q matrix and an R matrix. The expression of the time-varying weight matrix is as follows:
[0200]
[0201] R=r
[0202] Where q1 represents the lateral error ey The weight of q2 represents the lateral error change rate The weight of q3 represents the heading angle error e ψ The weight of , and q4 represents the rate of change of heading angle error The weight of .
[0203] Among them, the larger the weight values of q1, q2, q3 and q4, the higher the control degree of the control system of the unmanned mine car to the state quantity; that is, by adjusting the relative weights of q1, q2, q3 and q4, the reversing control accuracy of the unmanned mine car can be adjusted. R is the input of the state space model, that is, the weight corresponding to the steering wheel angle. The larger the weight, the greater the restriction on the steering wheel angle, that is, the smaller the steering angle. Since only the relative relationship between the Q matrix and the R matrix needs to be considered when designing, R is set as a constant value, and the weight matrix Q is designed to be time-varying.
[0204] For the lateral error e y The heading angle error e ψ The corresponding weights q1 and q3, if q1 and q3 are too large, it may cause the control system of the unmanned mining vehicle to overshoot; if q1 and q3 are too small, the adjustment time will be long. In the process of reversing, the vehicle heading should be corrected as soon as possible in the early stage of reversing, and the position of the vehicle head should be corrected, that is, the heading angle error e should be paid more attention to. ψ , the heading angle error e should be appropriately increased ψ The weight q3; in the later stage of reversing, in order to avoid collision with other vehicles or fixed obstacles, the vehicle tracking accuracy should be high, that is, the lateral error e should be given more attention. y Weight, so the lateral error weight q1 is appropriately increased.
[0205] In order to avoid too many control parameters for the unmanned mining vehicle, which would make the adjustment difficult, the lateral error change rate should be guaranteed. and the rate of change of heading angle error Not large, design lateral error change rate The rate of change of heading angle error The corresponding weights q2 and q4 are fixed values.
[0206] Step 32, according to the time-varying weight matrix, the characteristic parameters, and through a preset linear quadratic regulator, a corresponding feedback gain rate is calculated.
[0207] In this embodiment, the state space model includes characteristic parameters, wherein the state space model is expressed as: The characteristic parameters are preferably parameters A and B in the state space model. The preset linear quadratic regulator is preferably a linear quadratic regulator.
[0208] The time-varying weight matrix includes the Q matrix and the R matrix; on this basis, according to the linear quadratic regulator theory, the corresponding feedback gain rate is calculated, and the specific calculation method is as follows:
[0209] The Riccati equation is solved iteratively. The expression of the Riccati equation is as follows:
[0210] P=A T PA-A T PB(R+B T PB) -1 B T PA+Q
[0211] The maximum number of iterations is set to 150, and the iteration tolerance is set to 0.01. The initial value of the matrix P iteration is set to Q. After repeated iterations, the iteration is exited when the maximum number of iterations or the iteration tolerance is reached.
[0212] After the iteration is completed, the feedback gain rate K can be obtained by the following formula. The calculation method of the feedback gain rate K is as follows:
[0213] K=(R+B T PB) -1 B T PA
[0214] Among them, K represents the feedback gain rate, A represents the parameter A in the state space model, B represents the parameter B in the state space model, and R represents the R matrix in the time-varying weight matrix.
[0215] Step 33, determining a corresponding target steering angle according to the feedback gain rate and the current state quantity, and performing a change rate limit process on the target steering angle to obtain a corresponding corrected front wheel steering angle.
[0216] In this embodiment, the corresponding target turning angle δ is determined according to the above-mentioned feedback gain rate K and the current state quantity X. The target turning angle δ is calculated as follows:
[0217] δ=-KX
[0218] Among them, δ represents the target turning angle, K represents the feedback gain rate, and X represents the current state quantity.
[0219] For safety reasons, the steering wheel angle needs to be limited:
[0220] The target steering angle δ is processed by rate-limiting to obtain the corresponding corrected front wheel steering angle δ(kT). The specific processing process is as follows: the front wheel steering angle at the previous moment is δ[(k-1)T], the steering wheel angle at this moment is δ(kT), and the front wheel steering angle change amplitude of adjacent calculation cycles is Δδ. Then the expression of the corrected front wheel steering angle is as follows:
[0221]
[0222] Among them, δ(kT) represents the corrected front wheel steering angle, Δδ represents the amplitude of the front wheel steering angle change in adjacent calculation cycles, and δ[(k-1)T] represents the front wheel steering angle at the previous moment.
[0223] Step 34, limiting the corrected front wheel turning angle, determining the corresponding target front wheel turning angle, and sending a signal of the target front wheel turning angle to the actuator of the unmanned mining vehicle to control the unmanned mining vehicle to reverse.
[0224] In this embodiment, the corrected front wheel steering angle is limited to determine the corresponding target front wheel steering angle δ(kT). The specific processing process is as follows: the maximum value of the front wheel steering angle is δ max , generally 40°; the minimum front wheel turning angle is δ min , usually -40°. Then the expression of the target front wheel steering angle δ(kT) is as follows:
[0225]
[0226] Among them, δ(kT) represents the target front wheel turning angle, δ max Expressed as the maximum front wheel turning angle, δ min Expressed as the minimum front wheel turning angle.
[0227] After the target front wheel turning angle δ(kT) is obtained, the signal of the target front wheel turning angle δ(kT) is sent to the actuator of the unmanned mine car to control the unmanned mine car to reverse, thereby achieving the control accuracy of the unmanned mine car reversing.
[0228] In this embodiment, a corresponding time-varying weight matrix is set according to the current state quantity; a corresponding feedback gain is calculated according to the time-varying weight matrix, the characteristic parameters of the state space model, and by a preset linear quadratic regulator; a corresponding target turning angle is determined according to the feedback gain and the current state quantity, and a change rate limit processing is performed on the target turning angle to obtain a corresponding corrected front wheel turning angle; the corrected front wheel turning angle is limited to determine the corresponding target front wheel turning angle, and a signal of the target front wheel turning angle is sent to the actuator of the unmanned mine car to control the unmanned mine car to reverse; thereby achieving the control accuracy of the unmanned mine car reversing.
[0229] The present invention also provides a reversing control device. Fig.10 The reverse control device of the present invention comprises:
[0230] A determination module 10 is used to determine a corresponding state space model based on a pre-constructed reversing dynamics model and a pre-constructed error change rate model, wherein the reversing dynamics model is created based on a linear two-degree-of-freedom model of the unmanned mining vehicle, and the error change rate model is created based on a lateral error change rate and a heading angle error change rate of the unmanned mining vehicle;
[0231] An acquisition module 20 is used to acquire the current posture information and the reversing reference path information of the unmanned mining vehicle, and determine the current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information;
[0232] The control module 30 is used to set a corresponding time-varying weight matrix based on the current state quantity, and determine a target front wheel turning angle based on a preset linear quadratic regulator and the state space model to perform reverse control on the unmanned mining vehicle.
[0233] In addition, the present invention further provides a medium, which is a computer-readable storage medium on which a reversing control program is stored. When the reversing control program is executed by a processor, the steps of the reversing control method described above are implemented.
[0234] The method implemented when the reversing control program running on the processor is executed can refer to the various embodiments of the reversing control method of the present invention, which will not be described in detail here.
[0235] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or system. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or system including the element.
[0236] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.
[0237] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes a number of instructions for a terminal device (which can be a mobile phone, computer, server, air conditioner, or network device, etc.) to execute the methods described in each embodiment of the present invention.
[0238] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.
Claims
1. A reversing control method, characterized in that: The reverse control method comprises the following steps: Determine a corresponding state space model based on a pre-built reversing dynamics model and a pre-built error change rate model, wherein the reversing dynamics model is created based on a linear two-degree-of-freedom model of the unmanned mining vehicle, and the error change rate model is created based on a lateral error change rate and a heading angle error change rate of the unmanned mining vehicle; Acquire current posture information and reversing reference path information of the unmanned mining vehicle, and determine a current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information; Based on the current state quantity, a corresponding time-varying weight matrix is set, and based on a preset linear quadratic regulator and the state space model, a target front wheel turning angle is determined to perform reverse control on the unmanned mining vehicle; The state space model includes characteristic parameters, and the steps of setting a corresponding time-varying weight matrix based on the current state quantity, and determining a target front wheel turning angle based on a preset linear quadratic regulator and the state space model to perform reverse control on the unmanned mining vehicle include: According to the current state quantity, setting a corresponding time-varying weight matrix; According to the time-varying weight matrix, the characteristic parameters, and by using a preset linear quadratic regulator, a corresponding feedback gain rate is calculated; Determining a corresponding target steering angle according to the feedback gain rate and the current state quantity, and performing a change rate limit process on the target steering angle to obtain a corresponding corrected front wheel steering angle; The corrected front wheel turning angle is limited to determine the corresponding target front wheel turning angle, and a signal of the target front wheel turning angle is sent to the actuator of the unmanned mining vehicle to control the unmanned mining vehicle to reverse.
2. The reversing control method according to claim 1, characterized in that: Before the step of determining the corresponding state space model based on the pre-built reversing dynamics model and the pre-built error change rate model, the reversing control method further includes: Obtain the linear two-degree-of-freedom model of the unmanned mining vehicle, the cornering stiffness of the front wheel, the cornering stiffness of the rear wheel, the distance from the center of mass of the vehicle to the front axle, the distance from the center of mass of the vehicle to the rear axle, and the front wheel turning angle; Determine a front wheel sideslip angle and a rear wheel sideslip angle according to the linear two-degree-of-freedom model, a distance from the vehicle center of mass to the front axle, a distance from the vehicle center of mass to the rear axle, and the front wheel turning angle; Determining a corresponding front wheel lateral force according to the front wheel slip angle and the front wheel cornering stiffness; Determining a corresponding rear wheel lateral force according to the rear wheel slip angle and the rear wheel cornering stiffness; According to the front wheel lateral force and the rear wheel lateral force, a simplified force analysis is performed on the linear two-degree-of-freedom model to obtain a reversing dynamics model of the unmanned mining vehicle.
3. The reverse control method according to claim 1, characterized in that: The step of determining the corresponding state space model based on the pre-built reverse dynamics model and the pre-built error change rate model comprises: Analyze the error change rate model to obtain analytical expressions of the lateral error change rate and the heading angle error change rate; According to the reversing dynamics model, the error change rate model and the analytical expression, a corresponding state space model is determined.
4. The reverse control method according to claim 1, characterized in that: The step of determining the current state of the unmanned mining vehicle according to the current posture information and the reversing reference path information comprises: According to the current posture information, traverse the reference path points in the reversing reference path information, and determine the reference path point closest to the vehicle centroid as the path projection point; Calculating the lateral error and heading angle error of the unmanned mining vehicle according to the vehicle centroid point and the path projection point; Obtaining a calculation cycle, a lateral error of a previous calculation cycle, and a heading angle error of a previous calculation cycle of the unmanned mining vehicle, and determining corresponding lateral error change rates and heading angle error change rates according to the calculation cycle, the lateral error of the previous calculation cycle, the heading angle error of the previous calculation cycle, the lateral error, and the heading angle error; The current state quantity of the unmanned mining vehicle is determined according to the lateral error, the lateral error change rate, the heading angle error and the heading angle error change rate.
5. The reverse control method according to claim 1, characterized in that: After the step of setting a corresponding time-varying weight matrix based on the current state quantity, and determining a target front wheel turning angle based on a preset linear quadratic regulator and the state space model to perform reverse control on the unmanned mining vehicle, the reverse control method further includes: Enter the next calculation cycle and return to step: obtain the current posture information and the reversing reference path information of the unmanned mining vehicle, and determine the current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information.
6. A reversing control device, characterized in that: The reversing control device comprises: A determination module, used to determine a corresponding state space model based on a pre-built reversing dynamics model and a pre-built error change rate model, wherein the reversing dynamics model is created based on a linear two-degree-of-freedom model of the unmanned mining vehicle, and the error change rate model is created based on a lateral error change rate and a heading angle error change rate of the unmanned mining vehicle; An acquisition module is used to acquire the current posture information and the reversing reference path information of the unmanned mining vehicle, and determine the current state quantity of the unmanned mining vehicle according to the current posture information and the reversing reference path information; A control module, configured to set a corresponding time-varying weight matrix based on the current state quantity, and determine a target front wheel turning angle based on a preset linear quadratic regulator and the state space model, so as to perform reverse control on the unmanned mining vehicle; Wherein, the state space model includes characteristic parameters, and the control module is further used for: According to the current state quantity, setting a corresponding time-varying weight matrix; According to the time-varying weight matrix, the characteristic parameters, and by using a preset linear quadratic regulator, a corresponding feedback gain rate is calculated; Determining a corresponding target steering angle according to the feedback gain rate and the current state quantity, and performing a change rate limit process on the target steering angle to obtain a corresponding corrected front wheel steering angle; The corrected front wheel turning angle is limited to determine the corresponding target front wheel turning angle, and a signal of the target front wheel turning angle is sent to the actuator of the unmanned mining vehicle to control the unmanned mining vehicle to reverse.
7. A device, the device being a reversing control device, characterized in that: The reversing control device comprises: a memory, a processor and a reversing control program stored in the memory and executable on the processor, wherein the reversing control program implements the steps of the reversing control method according to any one of claims 1 to 5 when executed by the processor.
8. A medium, the medium being a computer-readable storage medium, characterized in that: The computer-readable storage medium stores a reversing control program, and when the reversing control program is executed by the processor, the steps of the reversing control method according to any one of claims 1 to 5 are implemented.
Citation Information
Patent Citations
Parameter adaptive transverse motion LQR control method for autonomous automobile
CN110568758A
Transverse control method and device for automatic driving vehicle and vehicle
CN113120001A
Lateral control method of vehicle and controller
CN113525384A
Transverse control method and device for autonomous vehicle and vehicle
CN114655248A
