A trajectory tracking control method for four-steering wheel AGV

Abstract

The present invention relates to a trajectory tracking control method for a four-steering wheel AGV, and belongs to the field of AGV intelligent control. The method comprises: S1: establishing a four-steering wheel AGV kinematic model based on a two-steering wheel AGV kinematic model and Ackerman steering theory; S2: establishing a lateral dynamics model and a longitudinal dynamics model of the vehicle; S3: linearizing the four-steering wheel AGV vehicle model; S4: optimizing and solving the objective function of the prediction model according to the system model; S5: designing a corresponding MPC trajectory tracking controller according to the system constraints on the vehicle speed and angular velocity. The present invention can ensure the trajectory tracking control function of the vehicle while improving the load-bearing performance of the four-steering wheel AGV, and at the same time has high flexibility and fault tolerance.

Description

Technical Field

[0001] The invention belongs to the field of AGV control and relates to a trajectory tracking control method of a four-steering wheel AGV. Background Art

[0002] In the manufacturing industry, production is often not the most time-consuming procedure. It only takes up less than 10% of the entire production process, and the most time is often spent on loading and unloading, transportation, and other processes. Therefore, in order to improve the country's productivity and realize the development of intelligent manufacturing, improving the logistics system is a key to achieving the goal. As an important part of the logistics system, the automatic guided vehicle (AGV) is widely used in various fields such as automobile assembly, tobacco industry, manufacturing plants, warehouses, distribution centers, and docks. Compared with traditional trailers, forklifts, conveyor belts and other transportation tools, AGV has the advantages of good safety, high efficiency, and flexible organization. Therefore, it has gradually become the core transportation tool in the modern logistics system and has been valued by people.

[0003] To further expand the application of AGV in various scenarios, it is necessary to focus on breakthroughs in three important core technologies, including perception, decision-making planning, and the final key tracking control. Among them, trajectory tracking control is one of the core technologies of smart cars. This technology can not only control the smart car to follow the desired trajectory, but also improve the stability of the car during movement. However, after improving the load capacity of AGV, it is necessary to reconsider the trajectory tracking problem of multiple steering wheels. Summary of the invention

[0004] In view of this, the purpose of the present invention is to provide a four-steering wheel AGV trajectory tracking control method. A kinematic model is constructed through the kinematic characteristics of the four-steering wheel AGV, and then a dynamic model of the four-steering wheel AGV is established according to the force analysis of each steering wheel and the center of mass of the AGV; based on the Ackerman principle, the differential drive is used to reduce the lateral deviation and center of mass deviation of the vehicle during trajectory tracking, and the four steering wheels are simplified into a two-steering wheel kinematic model for analysis on the kinematic model; the kinematic model is converted into a two-degree-of-freedom dynamic model with only lateral motion and yaw motion; the four-steering wheel AGV trajectory tracking controller is designed based on the AGV trajectory tracking principle.

[0005] In order to achieve the above object, the present invention provides the following technical solutions:

[0006] A four-steering wheel AGV trajectory tracking control method, the method comprising the following steps:

[0007] S1: The kinematic model of the four-steering-wheel AGV is established based on the two-steering-wheel AGV kinematic model and Ackerman steering theory;

[0008] S2: Establish the lateral dynamics model and longitudinal dynamics model of the car;

[0009] S3: Linearize the four-steering-wheel AGV model;

[0010] S4: Design the prediction model based on the system model to optimize the objective function;

[0011] S5: Design the corresponding MPC trajectory tracking controller based on the system constraints on the vehicle speed and angular velocity.

[0012] Optionally, S1 specifically includes: first using the relationship between the inner and outer wheel angles of the Ackerman principle, simplifying the four-steering wheel model into a double-steering wheel model for analysis, and performing operational modeling. The result of modeling the four-steering wheel AGV is:

[0013]

[0014]

[0015] Where V 1 ~V 4 and α 1 ~α 4 are the speed and deflection angle of the four steering wheels, ω is the turning angular velocity of the vehicle, and D 1 Q, D 2 Q, D 3 Q, D 4 Q are the turning radii of the four steering wheels.

[0016] Optionally, in S2, the lateral dynamics model of the car is:

[0017]

[0018] in,

[0019]

[0020]

[0021]

[0022] The longitudinal dynamics model of the car is:

[0023] F=F a +F D +F G +F R

[0024] Among them, F is the total longitudinal resistance of the car, F a Indicates the resistance encountered during acceleration, F D Represents air resistance, FG represents the resistance of the ramp, F R Indicates rolling resistance.

[0025] Optionally, in S3, the system equation obtained by linearizing the four-steering wheel AGV model is:

[0026]

[0027] Among them, ξ 0 is the state quantity of a certain working point of the nonlinear system, u 0 is the output, is a control input within a given prediction time domain, is the system state quantity.

[0028] Optionally, in S4, the optimization solution objective function of the prediction model is:

[0029]

[0030] in represents the coefficient matrix of the control increment, e t Represents the trajectory tracking error in the prediction time domain.

[0031] Optionally, in S5, the motion model of the system is converted into a linear time-varying model and discretized, and the obtained trajectory tracking controller model is:

[0032]

[0033] in:

[0034]

[0035]

[0036]

[0037] Wherein, T represents the sampling period.

[0038] The beneficial effects of the present invention are:

[0039] (1) The present invention provides a trajectory tracking control method for a four-steering wheel AGV based on model predictive control, which improves the load-bearing performance of the vehicle while ensuring the trajectory tracking control function of the vehicle.

[0040] (2) The present invention provides a method for establishing a kinematic model, a lateral dynamic model, and a longitudinal dynamic model of a four-steering-wheel AGV, which can simplify the design of a trajectory tracking controller for a four-steering-wheel AGV.

[0041] (3) The present invention designs the corresponding MPC trajectory tracking controller based on the system constraints on the vehicle speed and angular velocity.

[0042] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0044] Figure 1 This is the four-steering wheel AGV model involved in the present invention;

[0045] Figure 2 It is the two-degree-of-freedom model of AGV dynamics;

[0046] Figure 3 The controller principle is model predictive control. DETAILED DESCRIPTION

[0047] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0048] Among them, the drawings are only used for illustrative explanations, and they only represent schematic diagrams rather than actual pictures, and should not be understood as limitations on the present invention. In order to better illustrate the embodiments of the present invention, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0049] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right", "front", "rear", etc. indicate the orientation or position relationship, they are based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the terms describing the position relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0050] A method for controlling the motion trajectory of an AGV, the specific steps are as follows:

[0051] 1) Establish the kinematic model of the four-steering wheel AGV

[0052] First, the four-steering-wheel model is simplified and virtualized into a dual-steering-wheel model so that the motion can be solved when the AGV moves, the deflection angle and speed of each wheel can be calculated, and the relationship between the inner and outer wheel angles can be obtained using the Ackerman principle.

[0053] Figure 1 In the equation, O is the instantaneous center of velocity; Q is the center of the vehicle; A is the rotation center of the virtual front steering wheel; B is the foot of the perpendicular line through the instantaneous center of velocity O to AC; C is the rotation center of the virtual rear steering wheel. The steering wheel rotation angle is positive clockwise and negative counterclockwise. The angle θ of the virtual steering wheel rotation 1 ,θ 2 , calculate the instantaneous center of the AGV's velocity.

[0054] The instantaneous center position of velocity can be obtained through trigonometric functions of structural dimensions and rigid body constraints:

[0055]

[0056]

[0057] AC=AB+BC

[0058] Where: θ 1 ,θ 2 The output calculated by the algorithm.

[0059] According to the obtained θ 1 ,θ 2 It can calculate:

[0060]

[0061] BC=AC-AB

[0062]

[0063]

[0064] From this, the turning radius OQ can be obtained as:

[0065]

[0066] According to the currently set speed V and the calculated turning radius OQ, the current turning angular velocity of the vehicle can be obtained as:

[0067]

[0068] According to the Ackerman principle, the turning centers of all steering wheels and the turning centers of the center of mass intersect at point Q. After the turning radius is known, the coordinates of O are known as (O x , O y ), according to the center of mass of the vehicle as the origin, the coordinates of the four steering wheels are also known, and the turning radius D of the four wheels can be calculated according to the four-wheel AGV vehicle 1 Q, D 2 Q, D 3 Q, D 4 The length of Q.

[0069] According to the known angular velocity ω, the speed V of the four steering wheels can be calculated respectively. 1 ~V 4 , and the deflection angles α of the four steering wheels 1 ~α 4 :

[0070]

[0071]

[0072] 2) Establish the lateral dynamics model and longitudinal dynamics model of the car

[0073] The AGV is subjected to dynamic analysis, and the longitudinal dynamics and lateral dynamics are modeled separately. When modeling the lateral dynamics, the vehicle is considered to have a uniform longitudinal velocity, and the dynamic model involved is a two-degree-of-freedom dynamic model with only lateral motion and yaw motion. In addition, some factors need to be ignored and the following idealized assumptions need to be made during modeling:

[0074] (1) The mechanical structure of the vehicle body is rigid. Without considering the influence of the transmission mechanism on steering, the steering angle of the front and rear wheels of the vehicle is equal to the control input.

[0075] (2) Ignoring the effects of the vehicle's aerodynamics on it during motion, regardless of the sideways and vertical forces, and provided the road surface is level.

[0076] (3) Assume that the cornering force on the tire and the tire slip angle are linearly related and within the linear region.

[0077] in accordance with Figure 2 After analysis, we can get the lateral motion of the car:

[0078]

[0079] The lateral motion of the AGV around the Z axis is:

[0080]

[0081] Where u is the longitudinal velocity of the center of mass of the car; m is the total mass of the car; θ is the deflection angle of the center of mass; ω is the angular velocity of the car; F yf F is the front wheel cornering force; yr Rear wheel cornering force; δ f is the front wheel turning angle; δ r is the rear wheel turning angle; I z is the moment of inertia of the vehicle around the Z axis; a is the distance from the front wheel to the center of mass; b is the distance from the rear wheel to the center of mass.

[0082] To increase the speed of solution, when the front and rear wheel turning angles are small, cosδ f and cosδ r If we regard it as 1, the above formula can be simplified to:

[0083]

[0084]

[0085] Since the tires are assumed to work within a linear range, the cornering force and the cornering angle of the front and rear wheels can be regarded as a proportional function:

[0086] F y = kα

[0087] Depend on Figure 3 The sideslip angles of the front and rear wheels are obtained as:

[0088]

[0089] Where k is the lateral stiffness of the front and rear tires; a f is the front wheel slip angle; a r is the rear wheel slip angle.

[0090] The above formula can be equivalent to:

[0091]

[0092] Then we need to expand the above matrix to get the state quantities of the four steering wheels. Control input u=[δ f , δ r ] T , control output Where Y is the lateral displacement of the car; is the yaw angle displacement of the car. Then the two-degree-of-freedom dynamic model of the four-steering wheel car can be expressed in state space as:

[0093]

[0094] Then, through the conversion of the coordinate system, the coordinate system of the car is converted to the ground coordinate system. The formula is:

[0095]

[0096] In the above state space,

[0097]

[0098]

[0099]

[0100] In addition, the longitudinal dynamics model of the AGV can be summarized as follows through Newton's second law:

[0101] F=F a +F D +F G +F R

[0102] In the above formula, the meanings of the various letters are as follows: F is the total longitudinal resistance of the car; F a Indicates the resistance encountered during acceleration; F D Represents air resistance; F G Represents the resistance of the ramp; F R Represents rolling resistance. The expression formulas of each force in the above formula are:

[0103]

[0104] Where m represents the mass of the car; a x represents the longitudinal acceleration of the car; A represents the frontal area of ​​the vehicle; C D is the air resistance coefficient; ρ a is the current air density; u is the longitudinal velocity of the car at this time; g represents the acceleration due to gravity; i G Indicates the slope of the road; f R Indicates the current road friction coefficient.

[0105] 3) Linearization of the four-wheel AGV model

[0106] At a certain operating point of the nonlinear system, the state quantity is ξ 0 , the output is u 0 When a control input is given within the prediction time domain Then its system state quantity is According to the description of nonlinear system, it can be expressed as:

[0107]

[0108] By performing Taylor series expansion on the above equation with only the first-order term retained, we can obtain the linear time-varying system equation after the linearization of the nonlinear discrete system:

[0109]

[0110] 4) Design the prediction model based on the system model to optimize the objective function;

[0111]

[0112] in represents the coefficient matrix of the control increment, e t Represents the trajectory tracking error in the prediction time domain.

[0113] Next, we impose certain constraints on the input quantity, mainly the control quantity and control increment, and the expression is:

[0114]

[0115] And transform the constraint expression that controls the increment, we have:

[0116] u(t+k)=u(t+k-1)+Δu(t+k)

[0117]

[0118]

[0119] In the formula N c Row and column vector, I m is the m-dimensional identity matrix.

[0120] Finally, the above objective function is transformed into a standard quadratic form with constraints as follows:

[0121]

[0122] In the formula represents the coefficient matrix of the control increment, e t Represents the trajectory tracking error in the prediction time domain.

[0123] A series of control input increments are calculated and solved for each control cycle:

[0124]

[0125] The first item in this sequence is used to control the system:

[0126]

[0127] By repeating the above process in each cycle, the control input of each cycle can be obtained, that is, the tracking control of the AGV trajectory is realized.

[0128] 5) Design the corresponding MPC trajectory tracking controller based on the system's constraints on the vehicle's speed and angular velocity

[0129] For trajectory tracking a given trajectory can be expressed as:

[0130]

[0131] Where: x r =[x r y r θ r ] T Expressed as the state quantity of the reference trajectory, u r =[v r ω r ] T Indicates its control amount.

[0132] Linearize the motion model of the system, perform Taylor expansion on the motion model at the reference trajectory point, and retain only the first-order terms, and we can get:

[0133]

[0134] Subtracting from the previous formula we get:

[0135]

[0136] Right now:

[0137]

[0138] The above equation is discretized using approximate discretization:

[0139]

[0140] You can get:

[0141]

[0142] in:

[0143]

[0144]

[0145]

[0146] Where T represents the sampling period.

[0147] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A four-steering wheel AGV trajectory tracking control method, Features: The method comprises the following steps: S1: The kinematic model of the four-steering-wheel AGV is established based on the two-steering-wheel AGV kinematic model and Ackerman steering theory; S2: Establish the lateral dynamics model and longitudinal dynamics model of the car; S3: Linearize the four-steering-wheel AGV model; S4: Design the prediction model based on the system model to optimize the objective function; S5: Design the corresponding MPC trajectory tracking controller according to the system constraints on the vehicle speed and angular velocity; Specifically, S1 is as follows: first, using the relationship between the inner and outer wheel angles of the Ackerman principle, the four-steering wheel model is simplified into a double-steering wheel model for analysis, and operational modeling is performed. The result of the four-steering wheel AGV modeling is: where V 1 ~V 4 and α 1 ~α 4 are the speeds and deflection angles of the four steering wheels respectively, ω is the turning angular velocity of the vehicle, D 1 Q, D 2 Q, D 3 Q, D 4 Q are the turning radii of the four steering wheels respectively; In S2, the lateral dynamics model of the car is: in, The longitudinal dynamics model of the car is: F=F a +F D +F G +F R Among them, F is the total longitudinal resistance of the car, F a Indicates the resistance encountered during acceleration, F D Represents air resistance, F G represents the resistance of the ramp, F R Indicates rolling resistance; In S3, the system equation obtained by linearizing the four-steering wheel AGV model is: Among them, ξ 0 is the state quantity of a certain working point of the nonlinear system, u 0 is the output, is a control input within a given prediction time domain, is the system state quantity; In S4, the optimization solution objective function of the prediction model is: in represents the coefficient matrix of the control increment, e t represents the trajectory tracking error in the prediction time domain; In S5, the motion model of the system is converted into a linear time-varying model and discretized, and the obtained trajectory tracking controller model is: in: Wherein, T represents the sampling period.

Images