AGV trajectory tracking control method based on PSO Lyapunov function

Abstract

The present invention discloses an AGV trajectory tracking control method based on PSO Lyapunov function, studies the AGV trajectory tracking motion control, establishes and analyzes an AGV model, which mainly includes an AGV kinematic model, an AGV motion control error model, a laser radar observation model, and a DC motor model, clarifies the goal of designing an AGV trajectory tracking controller, and based on this goal, designs a controller based on PSO Lyapunov function, wherein the Lyapunov function is mainly introduced, and a Lyapunov function controller is designed by using the Lyapunov stability judgment basis, and a controller parameter regulator based on the PSO algorithm is designed to adjust the parameters in the kinematic controller, and a control method composed of a speed controller and a control parameter regulator is constructed, and the influence of the driving wheel speed on the system trajectory tracking is considered, and a PID controller is used to control the speed of two wheels to improve the trajectory tracking effect.

Description

Technical Field

[0001] The present invention relates to the technical field of AGV trajectory tracking motion control, and in particular to an AGV trajectory tracking control method based on PSO Lyapunov function. Background Art

[0002] At present, the research on AGV tracking control mainly includes two aspects: path tracking and trajectory tracking. The difference between the two is that the tracked path has a specific relationship with the time function. Among them, path tracking is independent of time. When the AGV moves, it will not change after the given path. Trajectory tracking is related to time. As the AGV moves, the given trajectory continues to change. Path tracking can be used as a simplified form of trajectory tracking. When the reference trajectory equation has no time variable, the research content is path tracking, but when the reference trajectory equation contains a time parameter, the research content is trajectory tracking. Trajectory tracking control is to ensure that the AGV travels along the specified path. For laser navigation AGV, the laser radar sensor obtains the posture error of the AGV, selects the appropriate bounded control quantity to stabilize the AGV, and finally obtains the angular velocity of the left and right wheels of the AGV, thereby realizing the trajectory tracking control of the AGV.

[0003] The PID control commonly used in traditional control requires a more accurate mathematical model. When the model parameters are uncertain, the mathematical model needs to rely on experience and on-site debugging to set the PID parameters. The error range is large, the results are uncertain, and the adaptive control effect is poor. The classic PID control has great limitations in practical applications. The AGV trajectory tracking control system has typical time-varying nonlinear characteristics, mainly because the given tracking trajectory will change with the movement of the vehicle body and the changes in the external environment. Therefore, this design is based on the characteristics of the AGV itself. A controller with a reasonable control law is designed, so that the AGV can run smoothly according to the expected trajectory, and the posture error tends to zero when the speed is not zero. Summary of the invention

[0004] In view of the problems existing in the prior art, the present invention provides a method for ensuring that the spatial position and navigation angle of an AGV will change during its movement, and that the AGV can accurately follow a given trajectory and arrive at the work site safely and stably. The AGV kinematic model and the position error differential equation are established, and the Lyapunov function is introduced to design the AGV trajectory tracking controller. The controller parameter regulator based on the PSO algorithm is used for parameter adjustment in the kinematic controller, and a control method consisting of a speed controller and a control parameter regulator is constructed. At the same time, the speed tracking of the drive motor based on the incremental PID is considered to affect the AGV motion control. Therefore, during the AGV movement, the influence of the drive wheel speed on the system trajectory tracking is considered, and the PID controller is used to control the two-wheel speed to improve the trajectory tracking effect. Compared with the traditional controller, this controller improves the control accuracy, response performance and robustness of the AGV.

[0005] The technical solution of the present invention is as follows:

[0006] The AGV trajectory tracking control method based on PSO Lyapunov function includes the following steps:

[0007] 1) Establish AGV kinematic model, AGV motion posture error model, lidar observation model and DC motor model;

[0008] 2) Introduce Lyapunov function to design AGV trajectory tracking controller;

[0009] 3) The controller parameter regulator based on particle swarm PSO algorithm is used for parameter adjustment in the kinematic controller;

[0010] 3.1) Build a velocity controller to obtain the posture error, linear velocity, and angular velocity;

[0011] 3.2) Based on the designed kinematic controller, the proportional factor in the control law is optimized based on the PSO algorithm, and a control parameter regulator is constructed to adjust the posture online according to the deviation, thereby ensuring high trajectory tracking accuracy and smooth operation;

[0012] 3.3) Use the error data of the actual operation of the AGV as feedback, observe the changes in the relevant parameters of the AGV, and regularly update the control parameter regulator;

[0013] 4) According to the requirements of AGV motion control, the PID control method is used to design a PID controller based on the motor model, and the PID controller is used to control the speed of the two wheels.

[0014] 4.1) During the control process, the dynamic response capability of the speed information received by the drive system will directly affect the actual effect of trajectory tracking;

[0015] 4.2) The speed tracking control of the drive motor based on incremental PID needs to take into account the impact of the motor and realize the compensation adjustment of the AGV speed;

[0016] 4.3) An incremental PID control algorithm is used to calculate the expected linear speed of the left and right wheels of the AGV bottom return system through a speed converter. Then the error between the target speed of the left and right wheels and the actual output speed of the left and right wheels is used as the input of the controller to output the actual speed of the left and right drive wheels, so as to achieve stable regulation of the system output, improve the stability of AGV operation, and ensure that the uncertainty of the system itself has good robustness.

[0017] Further, the specific process of step 1) is as follows:

[0018] 1.1) Establish the AGV kinematic model: stipulate that the center point of the driving wheel axis is the center of mass of the AGV vehicle, the wheel surface of the driving wheel has no relative sliding with the ground, and only pure rolling without axial movement;

[0019] The AGV using differential drive is subject to non-integrity constraints, that is, the motion constraint equation is:

[0020]

[0021] According to the AGV rigid body motion law, the AGV linear velocity v and angular velocity ω are obtained:

[0022]

[0023] Where: L is the distance between the driving wheels, v L and v R Represent the linear speed of the left and right wheels respectively,

[0024] Taking the center of AGV as the reference point, the kinematic model of AGV is:

[0025]

[0026] From equations (2) and (3), the kinematic model of AGV can be described as:

[0027]

[0028] 1.2) AGV motion posture error model: Therefore, when the period is T, the displacement and angle increments at time k are:

[0029]

[0030] Where: T is the adoption period;

[0031] When the motion state of AGV at time k is P k =[Xk ,Y k ,θ k ] T , when the motion state of AGV at time k+1 is P k+1 =[X k+1 ,Y k+1 ,θ k+1 ] T , then the motion state at time k+1 is:

[0032]

[0033] Define the actual position of the robot in the global coordinate system P = [X, Y, θ] T With reference pose P r =[X r ,Y r ,θ r ] T Therefore, the error between the actual robot posture and the reference posture can be defined as P e =PP r ,Right now:

[0034]

[0035] According to the coordinate transformation, the posture error e of the two-wheel differential mobile robot is:

[0036]

[0037] Combining formula (3), the local posture error, i.e., formula (8), is differentiated, and the dynamic equation relative to the AGV posture error is obtained as follows:

[0038]

[0039] Find the control law q=[v ω] according to the error T Make the AGV move along the desired trajectory, that is, the posture error P e The error terms in the equation tend to zero, that is,

[0040] 1.3) LiDAR observation model: The LiDAR observation model represents the correspondence between the distance and angle of the landmark observed by the sensor in the polar coordinate system and the position of the coordinate in the global coordinate system, as shown in Equation (10);

[0041]

[0042] Where: D, is the distance and angle in the sensor coordinate system;

[0043] x d ,yd The location information of obstacles in the environment;

[0044] x s ,y s ,θ s is the sensor pose coordinate;

[0045] D r ,D θ is the distance and angle error of the observation model;

[0046] The AGV converts the data in the sensor coordinate system to the global coordinate system, and the converted data is used to estimate the AGV's own position and the position of the environment map;

[0047] 1.4) DC motor model: The motor electrical equation and mechanical characteristic equation are combined to obtain the model, as follows;

[0048] DC motor electrical equations:

[0049]

[0050] Mechanical equations for a DC motor:

[0051]

[0052] Where: C a is the potential of the motor, T = C t i a ;

[0053] J is the moment of inertia on the motor shaft;

[0054] By performing Laplace transform on equations (11) and (12), we can obtain:

[0055]

[0056] The mechanical time constant in formula (13) is calculated as:

[0057]

[0058] The electrical time constant is calculated as:

[0059]

[0060] From formula (13), we can see that the transfer function of the DC motor is a second-order lag-free transfer function, as shown in formula (16):

[0061]

[0062] Due to T a <<T m , so it is approximately considered that Tm +T a ≈T m ,

[0063] Further, the step 2) is specifically as follows:

[0064] According to Lyapunov stability theory, the constructed Lyapunov function formula is:

[0065]

[0066] Where k2>0 and is bounded, the derivative of the above Lyapunov function can be obtained:

[0067]

[0068] According to the Lyapunov stability theorem, the control quantity is selected as:

[0069]

[0070] Where k1>0, k3>0 and are bounded, solving equation (21) yields:

[0071]

[0072] According to formula (23), it can be judged that When both v and ω are not 0, It is not always 0, so it is judged that the designed control law can make the AGV motion control system asymptotically stable and realize AGV trajectory tracking control.

[0073] Further, the step 3) is specifically as follows:

[0074] PSO is used to adjust the parameters of the controller and construct the cost function of formula (26):

[0075]

[0076] Where: w1, w2, w3, w4 are weights;

[0077] t r is the rise time;

[0078] j 2 (t) is the performance index, which is used as a parameter to select the minimum objective function;

[0079] u 2 (t) is the square term of the control quantity;

[0080] h 2 (t) is the overshoot;

[0081] After determining the fitness function, the three parameters k1, k2, and k3 of the speed controller are adjusted based on the PSO algorithm to obtain more accurate control parameters, stably control the speed and steering of the AGV, and optimize the performance of the AGV control system.

[0082] The beneficial effects of the present invention are as follows:

[0083] 1) The controller is designed based on the direct method of Lyapunov function, which can determine the designed control law, make the AGV motion control system asymptotically stable, and realize AGV trajectory tracking control.

[0084] 2) Based on the designed kinematic controller, the controller optimizes the proportional factor in the control law based on the PSO algorithm for the control parameters in the control law, constructs a control parameter regulator, and adjusts the posture online according to the deviation, thereby ensuring high trajectory tracking accuracy and smooth operation.

[0085] 3) Based on the established motor model, the controller uses the PID control algorithm to control the speed of the two wheels to improve the overall trajectory tracking effect, achieve stable adjustment output of the system, improve the operation stability of the AGV, and ensure that the uncertainty of the system itself has good robustness.

[0086] 4) During the movement of the AGV, the influence of the driving wheel speed on the system trajectory tracking is considered, and a PID controller is used to control the speed of the two wheels to improve the trajectory tracking effect; compared with the traditional controller, this controller improves the control accuracy, response performance and robustness of the AGV. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 is the kinematic model of the AGV of the present invention;

[0088] Figure 2 Schematic diagram of trajectory error of the present invention;

[0089] Figure 3 It is the AGV observation model under the ideal state of the present invention;

[0090] Figure 4 This is a flow chart of the motor incremental PID speed control operation of the present invention;

[0091] Figure 5 It is the simulation result of the motor incremental PID linear speed of the present invention;

[0092] Figure 6 It is the simulation result of the motor incremental PID angular velocity of the present invention;

[0093] Figure 7 It is the PID control principle diagram of the present invention;

[0094] Figure 8It is the AGV trajectory tracking control system structure of the present invention;

[0095] Fig. 9 It is the principle diagram of the PSO control parameter regulator of the present invention;

[0096] Fig.10 It is the principle diagram of the PID control system of the drive motor of the present invention;

[0097] Fig.11 It is the simulation result of the PID controller of the present invention tracking the straight line trajectory;

[0098] Fig.12 It is the simulation result of the PID controller tracking curve trajectory of the present invention;

[0099] Fig.13 It is the simulation result of the linear trajectory of the Lyapunov function controller based on PSO of the present invention;

[0100] Fig.14 It is the simulation result of the curve trajectory of the Lyapunov function controller based on PSO of the present invention;

[0101] Fig.15 It is the prototype model of the laser navigation AGV of the present invention;

[0102] Fig.16 It is the actual scene diagram of the present invention;

[0103] Fig.17 It is the straight line trajectory tracking result of the present invention;

[0104] Fig.18 It is the curve trajectory tracking result of the present invention;

[0105] Fig.19 is the distance deviation of the present invention;

[0106] Fig. 20 is the angle deviation of the present invention;

[0107] Fig.21 It is the speed effect comparison of the present invention;

[0108] Fig. 22 This is a comparison of the angular velocity effects of the present invention;

[0109] Fig.23 It is a schematic diagram of the range analysis principle of the present invention. DETAILED DESCRIPTION

[0110] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0111] A PSO-based Lyapunov function-based AGV trajectory tracking control method, based on the mechanical structure of the RFID-based dual-magnetic strip navigation latent AGV car, simplifies its structural model, and establishes the AGV kinematic model. The kinematic model is as follows Figure 1 As shown in the figure, the center point of the driving wheel axis is defined as the mass center of the AGV vehicle, and the wheel surface of the driving wheel has no relative sliding with the ground, and only performs pure rolling without axial movement.

[0112] exist Figure 1 In, v L and v R Represent the linear speeds of the left and right wheels respectively. The AGV using differential drive is subject to non-integrity constraints, that is, the motion constraint equation is:

[0113]

[0114] According to the AGV rigid body motion law, the AGV linear velocity v and angular velocity ω can be obtained:

[0115]

[0116] Where: L is the distance between the driving wheels.

[0117] Taking the center of AGV as the reference point, the kinematic model of AGV is:

[0118]

[0119] From equations (2) and (3), the kinematic model of AGV can be described as:

[0120]

[0121] Therefore, when the period is T, the displacement and angle increments at time k are:

[0122]

[0123] Where: T is the adoption period.

[0124] Then when the motion state of AGV at time k is P k =[X k ,Y k ,θ k ] T , when the motion state of AGV at time k+1 is P k+1 =[X k+1 ,Y k+1 ,θ k+1 ] T , then the motion state at time k+1 is:

[0125]

[0126] Define the actual position of the robot in the global coordinate system P = [X, Y, θ] T With reference pose P r =[X r ,Y r ,θ r ] T Therefore, the error between the actual robot posture and the reference posture can be defined as P e =PP r ,Right now:

[0127]

[0128] During the actual driving process of AGV, the trajectory error diagram is as follows Figure 2 shown.

[0129] According to the coordinate transformation, the posture error e of the two-wheel differential mobile robot is:

[0130]

[0131] Combining formula (3), the local posture error, i.e., formula (8), is differentiated, and the dynamic equation relative to the AGV posture error is obtained as follows:

[0132]

[0133] By designing the controller, we can find the control law q=[v ω] according to the error. T Make the AGV move along the desired trajectory, that is, the posture error P e The error terms in the equation tend to zero, that is, This is the design goal of the AGV trajectory tracking controller.

[0134] This design establishes an observation model for the lidar. The observation model of the lidar represents the correspondence between the distance and angle of the landmark observed by the sensor in the polar coordinate system and the position of the coordinate in the global coordinate system, as shown in formula (10).

[0135]

[0136] Where: D, is the distance and angle in the sensor coordinate system;

[0137] x d ,y d The location information of obstacles in the environment;

[0138] x s ,y s ,θ s is the sensor pose coordinate;

[0139] D r ,D θ is the distance and angle error of the observation model.

[0140] The AGV observation model in an ideal state is as follows Figure 3 As shown, the AGV can transform the data in the sensor coordinate system into the global coordinate system, and the transformed data can be used to estimate the position of the AGV itself and the position of the environment map.

[0141] The mathematical model calculation of the DC motor is the core of the speed control system design, which affects the motor control speed input and also affects the operation accuracy of the AGV. The mathematical model of the DC motor can be obtained by combining the electrical equation and the mechanical characteristic equation of the motor, as shown below.

[0142] DC motor electrical equations:

[0143]

[0144] Mechanical equations for a DC motor:

[0145]

[0146] Where: C a is the potential of the motor, T = C t i a ;

[0147] J is the moment of inertia of the motor shaft.

[0148] By performing Laplace transform on equations (11) and (12), we can obtain:

[0149]

[0150] The mechanical time constant in formula (13) is calculated as:

[0151]

[0152] The electrical time constant is calculated as:

[0153]

[0154] From formula (13), we can see that the transfer function of the DC motor is a second-order lag-free transfer function, as shown in formula (16):

[0155]

[0156] Due to T a <<T m , so it is approximately considered that T m +T a ≈Tm ,

[0157] Table 1 DC motor nameplate parameters

[0158]

[0159] According to the motor used, its nameplate parameters are shown in Table 1, which are calculated through the parameter model of the DC motor:

[0160] M=0.02N·m; C=0.003N·m / A; T a =0.013s; T m =0.2s.

[0161] Substituting the result into equation (13), we can get the transfer function:

[0162]

[0163] After the mathematical model of the AGV drive motor is established and analyzed and the motor is selected and determined, simulation is performed on MATLAB to verify the working performance of the motor using the incremental PID control algorithm.

[0164] The AGV system controls the motor through the drive controller to achieve the purpose of controlling the AGV movement. In order to accurately control the AGV movement and ensure accurate operation accuracy, stability and real-time performance, the drive operation cycle is set to 10 milliseconds to facilitate the realization of multiple tasks. The motor incremental PID speed control operation process is as follows Figure 4 As shown. It is mainly divided into two parts: two drive controllers and the main controller. The AGV active wheel is driven by two DC motors. The controller will publish duty cycle information and output PWM control signals. The left and right wheel motors receive control signals. At the same time, the system uses the encoder to obtain control readings and converts them into speed information. Then, the system publishes information such as linear speed and angular speed, and finally controls the AGV to run stably. During this period, the system serial port communication needs to be maintained.

[0165] This article uses an incremental PID control algorithm for motor control, and its principle is as follows:

[0166]

[0167] Where: u(k-1) is the output of the incremental PID controller at time k-1;

[0168] error(k) is the motor tracking speed error at time k;

[0169] k p is the proportionality coefficient;

[0170] k iis the integration coefficient;

[0171] k d is the differential coefficient.

[0172] The algorithm is:

[0173]

[0174] The incremental PID control parameter settings are shown in Table 2.

[0175] Table 2 PID control parameter settings

[0176]

[0177] The motor model tracks the unit step speed and the simulation results using MATLAB are as follows: Figure 5 shown.

[0178] in, Figure 5 This is the simulation result of the motor incremental PID linear speed. Figure 5 (a) The solid line is the linear velocity tracking result, and the dotted line is the ideal velocity change curve. Figure 5 (b) is the motor speed tracking error result. Figure 6 is the simulation result of the motor incremental PID angular velocity, where Figure 6 (a) The solid line is the actual tracking result of the motor angular velocity, and the dotted line is the ideal angular velocity change curve. Figure 6 (b) is the angular velocity tracking error result. Figure 5 and Figure 6 It can be seen that after running for 0.3s, the AGV linear speed and angular velocity change curves basically tend to be stable and meet the requirements. Its step transient response results are shown in Table 3 below.

[0179] Table 3 Step transient response indicators of the motor based on the incremental PID control algorithm

[0180]

[0181] The simulation results show that there is an error in the system speed tracking, but the error is small, which means that the speed tracking affects the AGV posture and will also affect the comprehensive operation performance of the AGV, laying a theoretical foundation for the design of the AGV controller in the following text. Therefore, when designing the trajectory tracking controller in the following text, it is necessary to consider the impact of the motor itself on the AGV trajectory tracking accuracy to further improve the comprehensive operation performance of the AGV.

[0182] The most common control in AGV control systems is PID control, which directly performs closed-loop control on the controlled object, and the three parameters k p , k i , k dIt is online control. The PID control principle is as follows Figure 7 shown.

[0183] For the AGV control system, a stable control system is the premise to ensure the safe and stable operation of the AGV. Otherwise, in practical applications, an unstable system will hardly give full play to the performance of the product. The motion system of the AGV in this paper is a nonlinear system, so the Lyapunov direct method is used to design the trajectory tracking controller of the AGV. Relying on the Lyapunov stability theory to analyze and establish the speed controller, since the three control parameters k1, k2, and k3 are difficult to determine, a controller parameter regulator based on the PSO optimization algorithm is proposed for the parameter adjustment of the speed controller in the kinematic controller. A control system consisting of a speed controller and a control parameter regulator is constructed. At the same time, considering that the system speed is easily disturbed by external factors, the system speed control design is performed based on the motor model. Improve the operation stability of the AGV, thereby achieving the purpose of trajectory tracking.

[0184] This paper designs the trajectory tracking controller as a whole. The block diagram of the trajectory tracking control system is shown in the figure. Figure 8 As shown in the figure, the expected posture of AGV (x r ,y r ,θ r ) and the error (x) between the actual pose (x, y, θ) e ,y e ,θ e ) is used as the kinematic controller input, the linear velocity and angular velocity control quantities (v, ω) are calculated by the speed controller, and the target speeds of the left and right wheels (v) are calculated by the speed converter. Ld ,v Rd ); then the target speed of the left and right wheels (v Ld ,v Rd ) and the actual output left and right wheel speed (v L ,v R ) error (e l ,e r ) is used as the input of the controller and the actual speed of the left and right driving wheels is output. Ultimately, the AGV is guaranteed to move forward stably along the predetermined path, thus achieving the purpose of trajectory tracking.

[0185] Depend on Figure 8 It can be seen that the trajectory tracking control system structure consists of two parts: the design of the control parameter regulator based on PSO and the design of the PID controller based on the motor model, which finally completes the design of the AGV trajectory tracking controller.

[0186] 1. Design of controller based on direct method of Lyapunov function

[0187] Lyapunov's method includes the first method and the second method. The first method analyzes the stability of the system by solving differential equations. The second method is also called the direct method. It does not require a high mathematical model of the system and does not require solving specific differential equations. For a moving system, the stability of the system decreases with the passage of time, and the energy of the system continues to decrease. Then the system will reach a state of equilibrium at a certain point in time. However, for nonlinear systems, it is difficult for the system function to reach equilibrium. Therefore, it is necessary to construct a virtual scalar function through Lyapunov's second method to represent the energy of the entire system and balance the system.

[0188] Lyapunov's direct method determines the stability of a system through a scalar function and the derivative of the scalar function. Since the energy is always greater than zero, the scalar function must be positive definite, and the attenuation characteristics of the system are expressed by the derivative of the scalar function.

[0189] According to Lyapunov stability theory, the constructible Lyapunov function formula is:

[0190]

[0191] Where k2>0 and is bounded, the derivative of the above Lyapunov function can be obtained:

[0192]

[0193] According to the Lyapunov stability theorem, the control quantity is selected as:

[0194]

[0195] Where k1>0, k3>0 and are bounded, solving equation (21) yields:

[0196]

[0197] According to formula (23), it can be judged that When both v and ω are not 0, It is not always 0, so we can also judge that the designed control law can make the AGV motion control system asymptotically stable and realize AGV trajectory tracking control.

[0198] 2. Design of control parameter regulator based on PSO

[0199] 1) Principle of control parameter regulator

[0200] According to the overall design of the AGV trajectory tracking control system, including the design of the control parameter regulator based on PSO, the implementation of the regulator is divided into three steps:

[0201] a. Build a velocity controller to obtain the posture error, linear velocity, and angular velocity.

[0202] b. Based on the designed kinematic controller, the proportional factor in the control law is optimized based on the PSO algorithm for the control parameters in the control law, and a control parameter regulator is constructed to adjust the posture online according to the deviation, thereby ensuring high trajectory tracking accuracy and smooth operation.

[0203] c. Use the error data of the actual operation of the AGV as feedback, observe the changes in AGV related parameters, and regularly update the control parameter regulator.

[0204] 2) PSO algorithm principle

[0205] In the controller design process, since the three parameters k1, k2, and k3 of the speed controller affect the control effect of the kinematic controller, the traditional method is generally complex and has poor robustness when determining the parameters k1, k2, and k3. Through analysis, it can be seen that the k1 value of the speed controller affects the movement speed of the car, and the k2 and k3 values ​​adjust the rotation amplitude of the car. Since the selection of control parameters is relatively complex and the adaptive effect is poor, this paper adopts the particle swarm (PSO) algorithm to adjust the proportional factor of the speed controller designed based on Lyapunov stability in real time.

[0206] The particle swarm algorithm (PSO) originated from the study of the predation behavior of bird flocks, simulating the random search for food by bird flocks. Each bird looking for food is called a particle. Each particle has its own speed, and the direction and distance of particle movement are established, as well as the fitness value determined by the optimization function. The particle swarm algorithm is first initialized as a group of random particles, and then iterates to find the optimal prediction solution. In each iteration, the particles track and update two extreme values. The first particle currently finds the optimal solution, that is, the individual extreme value, and the other particle is the optimal prediction solution currently found in the entire bird flock, also called the subgroup extreme value problem, and the optimal solution in a part of the particles is called the local extreme value point.

[0207] Assume that in a D-dimensional search space, a search unit consisting of m particles is constructed, where the velocity information of the i-th particle can be expressed as V i =(v i1 ,v i2 ,…,v iD ), i = 1, 2, ..., m, the position information can be expressed as X i =(x i1 ,x i2 ,…,x iD ),i=1,2,…,m, then the particle velocity and position update equations are:

[0208]

[0209] Where: is the velocity of particle i in the dth dimension in the kth iteration;

[0210] ω is the inertia factor, a non-negative constant;

[0211] c1, c2 are learning factors, non-negative constants, generally c1=c2=2;

[0212] r1, r2 are random numbers between 0 and 1;

[0213] is the position of the d-th dimension of the i-th particle in the k-th iteration;

[0214] is the position of the individual extreme value of the ith particle in the dth dimension;

[0215] is the position of the population extreme point in the dth dimension of the entire population.

[0216] For this design, the particle swarm optimization algorithm (PSO) is used for parameter optimization. The schematic diagram of the controller is as follows: Fig. 9 shown.

[0217] PSO is used to tune the controller parameters and construct the cost function of formula (26):

[0218]

[0219] Where: w1, w2, w3, w4 are weights;

[0220] t r is the rise time;

[0221] j 2 (t) is the performance index, which is used as a parameter to select the minimum objective function;

[0222] u 2 (t) is the square term of the control quantity;

[0223] h 2 (t) is the overshoot.

[0224] After determining the fitness function, the three parameters of the controller are adjusted based on the PSO algorithm to obtain more accurate control parameters, stably control the speed and steering of the AGV, and optimize the performance of the AGV control system. At the same time, simulation experiments are required to verify the effectiveness of the design method.

[0225] 3. Design of PID controller based on motor model

[0226] During the control process, the dynamic response capability of the speed information received by the drive system will directly affect the actual effect of trajectory tracking. From the previous simulation analysis, it can be seen that the speed tracking control of the drive motor based on incremental PID will affect the AGV motion control. It is necessary to consider the impact of the motor and realize the compensation adjustment of the AGV speed. Therefore, when designing the AGV trajectory tracking controller, it is necessary to consider the system speed. According to the established motor model, the PID control algorithm is used to control the speed of the two wheels to improve the overall effect of trajectory tracking, realize the stable adjustment output of the system, improve the stability of AGV operation, and ensure that the uncertainty of the system itself has good robustness.

[0227] According to the requirements of AGV motion control, the PID control method is used to design a PID controller based on the motor model, where the speed difference is used as the input of the controller and the output is the actual speed of the AGV. The block diagram of the PID control system of the AGV drive motor is shown in the figure. Fig.10 shown.

[0228] Where: v L ,v R The actual output linear speed of the left and right wheels at the AGV mass center of the lower machine;

[0229] v Ld ,v Rd The expected linear speed of the left and right wheels of the AGV bottom return system;

[0230] e l ,e r is the speed error;

[0231] u l ,u r is the output of the controller.

[0232] Aiming at the problem of poor trajectory tracking effect of AGV trajectory tracking, the trajectory tracking control algorithm was designed and analyzed. In order to verify that the PSO-based Lyapunov function controller is more reliable and accurate than the classic PID controller in AGV trajectory tracking control effect, the two controllers were simulated and analyzed by MATLAB. The simulation models of the AGV PSO-based Lyapunov function controller and the classic PID controller were built in MATLAB, and the PID controller trajectory tracking simulation experiment and the PSO-based Lyapunov function controller simulation experiment were carried out respectively. The AGV running speed was set to 0.5m / s, and the simulation results were analyzed as follows.

[0233] 1) Classic PID controller simulation experiment

[0234] The system uses a PID controller for trajectory tracking, and the trajectory tracking effect of the PID controller is verified through a linear trajectory tracking simulation experiment. The expected trajectory is a straight line with an angle of 45° with the positive direction of the x-axis. The initial position of the reference trajectory is set to (0, 0, π / 4), and the actual initial position of the AGV is (0, -2, π). The simulation results are shown in Figure 2. Fig.11 shown.

[0235] Depend on Fig.11 (a) It can be seen that the AGV can achieve straight-line trajectory tracking using the PID controller. After 4.5 seconds of adjustment, the system accurately completes the tracking of the desired trajectory. Fig.11 (b) is the convergence curve of the posture error, in which the posture error converges to 0 within 5 seconds and reaches a stable state. Under the action of the PID controller, the AGV can achieve rapid convergence of the posture error.

[0236] After completing the straight line trajectory tracking, the system conducts a curve trajectory tracking simulation experiment under the action of the PID controller to verify the tracking effect of the algorithm. The expected trajectory is an "S" shaped curve. The initial posture error is set to (0.2, 1.5, 5). The simulation results are shown in Figure 2. Fig.12 shown.

[0237] Fig.12 (a) is the displacement curve of the “S”-shaped curve trajectory tracking. The AGV will produce a certain error before operation, and then the movement can completely coincide with the reference trajectory and gradually tend to a stable state. Fig.12 (b) is the convergence of the AGV’s posture error, where the posture error can converge to 0 within 6 seconds and reach a stable state. Fig.12 As can be seen from (a) and (b), for arcs with large curvature, the AGV tracking trajectory based on the PID controller will have a large overshoot, resulting in low accuracy of the AGV operation process and prone to certain fluctuations. The system adjustment stabilization time is too long and cannot meet the performance and functional requirements.

[0238] 2) Simulation experiment based on PSO Lyapunov function controller

[0239] The control performance of the PSO-based Lyapunov function controller for linear trajectory tracking is tested. The AGV is set to start from the initial posture (0, 0, π / 4) and track a straight line with a reference velocity V = 0.5m / s and a reference angular velocity W = 0rad / s. The initial posture error is (0.5, 2, 0.2), and the control parameters are k1 = 15, k2 = 1.5, and k3 = 5.3. The linear trajectory tracking simulation results are shown in Figure 2. Fig.13 shown.

[0240] Depend on Fig.13It can be seen that the system can track the straight line trajectory under the control of the PSO-based Lyapunov function. Fig.13 (a) It can be seen that the trajectory converges at the horizontal coordinate of 1.5m. Fig.13 (b) is the convergence curve of the pose error. The pose error converges to 0 at 4s and then tends to be stable. Fig.13 (c) shows the changes in AGV speed and angular velocity. It converges to 0.5 m / s at 1.5 s, and the angular velocity converges to 0 at 2 s. Fig.13 (d) is the change of left and right wheel speed. After 2.5s, the speed converges to 0.5m / s. The left and right wheels run at the specified speed and tend to be stable. Fig.13 (e) and (f) show that the distance deviation of the Lyapunov function control based on PSO tends to be stable in 1s and gradually becomes 0, and the angle deviation tends to be stable and gradually converges to 0 in 1.5s, while the distance deviation of the PID control algorithm tends to be stable in 2.5s and gradually becomes 0, and the angle deviation tends to be stable and gradually converges to 0 in 3.5s.

[0241] The control performance of the PSO-based Lyapunov function controller for curve trajectory tracking is tested. The AGV is set to start from the initial posture (0, 0, 0) and track a straight line with a reference velocity V = 0.5m / s and a reference angular velocity W = 0.5rad / s. The initial posture error is (0, 1, 0), and the control parameters are k1 = 5, k2 = 45, and k3 = 8. The curve trajectory tracking simulation results are shown in Figure 2. Fig.14 shown.

[0242] Depend on Fig.14 It can be seen that the AGV system can track the curve trajectory under the action of the controller based on the PSO Lyapunov function. Fig.14 In (a), the actual trajectory of the AGV gradually converges to the reference trajectory at 0.6 m in the negative x direction and completely coincides with the reference trajectory. The results show that the system is in a stable state after converging to the reference trajectory. Fig.14 (b) is the pose error convergence curve. The pose error of the AGV system converges to 0 in 2s. In addition, all the pose error convergence curves are relatively smooth, which indicates that the AGV operates stably. Fig.14 (c) shows the changes in AGV speed and angular velocity, which converge to 0.5 m / s in 2 s, and the angular velocity converges to the reference angular velocity in 2 s. Fig.14 (d) shows the speed change of the left and right wheels. The speed converges to the AGV reference speed after 1.5s, and then tends to be stable. The left and right wheels move in a curve at the specified speed. Fig.14(e) and (f) show that the distance deviation of the Lyapunov function control based on PSO tends to be stable in 4 seconds and gradually becomes 0, and the angle deviation tends to be stable and gradually converges to 0 in 3.8 seconds, while the distance deviation of the PID control algorithm tends to be stable in 7 seconds and gradually becomes 0, and the angle deviation tends to be stable and gradually converges to 0 in 5 seconds.

[0243] Depend on Fig.13 and Fig.14 It can be seen that this paper verifies the performance of the two controllers through simulation experiments of tracking straight and circular trajectories. The AGV system can quickly track the reference trajectory under the action of the PSO-based Lyapunov controller, and the error converges quickly and tends to a stable state. The speed and angular velocity can quickly converge to the reference value, the system responds quickly, and the control effect is good. However, the PID controller requires a large number of debugging parameters, the AGV system takes a long time to reach stability, and the stability error is large. Therefore, the PSO-based Lyapunov controller has a strong applicability to AGV trajectory tracking, can meet the performance and functional requirements of AGV, and improves the stability and operation accuracy of AGV.

[0244] Example:

[0245] At the same time, this design carried out prototype experiments. The laser navigation AGV prototype model is as follows Fig.15 shown.

[0246] The specific features of the AGV prototype are as follows:

[0247] (1) The AGV prototype in this paper adopts a two-wheel differential motion method, including two driving wheels and four driven wheels. It has the ability of differential movement and can flexibly turn and avoid obstacles.

[0248] (2) The driving wheel motor drive uses the DC motor drive system produced by China-Malaysia Transmission Intelligent Technology Co., Ltd. as the driving source. It has the characteristics of large torque and can maintain low-speed and stable operation.

[0249] (3) Navigation uses YDLIDAR laser radar, which is mainly used for map construction. AGV can perceive, search and send information in the environment.

[0250] (4) Raspberry Pi is used as the host computer control platform to remotely control the AGV to complete movement and work.

[0251] The parameters of the AGV prototype are shown in Table 4.

[0252] Table 4 AGV prototype performance parameters

[0253]

[0254] The application object of this experiment is AGV working in indoor environment, which is mainly suitable for transferring large and small weight materials and loading and unloading materials on production lines. The experimental ground selected this time is mostly used in factory environment, which greatly ensures that the actual environment is safe and reliable, and the site meets the requirements of the experimental site. At the same time, the experimental site is planned and arranged. The entire indoor environment is about 15m long, 10m wide, and covers an area of ​​about 150m 2 , so that the AGV can run around the laboratory, and in order to be closer to the real environment, stools are placed on the path to simulate obstacles. Fig.16 shown.

[0255] A trajectory tracking experiment was carried out based on the machined AGV prototype, and the AGV was allowed to run on a global section. In order to verify the overall performance of the AGV and the reliability of the control method, a speed of 0.25 m / s, a load of 100 kg, and three obstacles were selected, and tracking experiments were carried out on straight lines and circular arcs respectively.

[0256] First, conduct a straight line tracking experiment, let the AGV run a circle in the designed environment and collect statistics, then select a straight line segment to verify the running effect of the AGV from the starting point to the target point, and enlarge it at a ratio of 1:5. Fig.17 shown.

[0257] Depend on Fig.17 It can be seen that during the tracking process, the system speed is set to 0.25m / s, and the AGV performs linear trajectory tracking under PSO Lyapunov function control and PID control respectively. It can be seen from the figure that both trajectory tracking effects can meet the trajectory tracking requirements. For a straight line selected in the overall trajectory tracking, the average displacement error in the y direction of the linear tracking based on the PSO Lyapunov function control is 9.32mm, while the average error value of the tracking based on the PID control is 9.99mm, and the error is reduced by 6.71%. Therefore, the linear trajectory tracking effect based on the PSO Lyapunov function control has greater advantages, ensuring that the AGV trajectory tracking control has high accuracy and stable operation.

[0258] Next, we conduct a curve tracking experiment. In addition to moving along the overall planned path, the AGV will encounter interference from external factors such as obstacles during the path tracking experiment. We need to perform curve tracking and bypass obstacles. During the movement, the AGV will perform operations such as turning and U-turning, so the arc path is one of the important links in the trajectory tracking process. We need to conduct an arc curve tracking experiment to compare the curve tracking effects of the two control algorithms and verify the feasibility and accuracy of the method in this paper.

[0259] First, select an arc in the global path segment trajectory tracking to verify the arc trajectory tracking effect, and enlarge it at a ratio of 1:5. The curve trajectory tracking result is shown in the figure below: Fig.18 shown.

[0260] According to the statistical analysis of the selected curve path results, the average displacement error in the y direction of the curve trajectory tracking based on PSO Lyapunov function control is 14.76mm, while the average error value of the tracking based on PID control is 17.44mm, and the error is reduced by 15.37%. Therefore, the tracking accuracy based on PSO Lyapunov function control is higher and has certain advantages.

[0261] Depend on Fig.17 , 18 It can be seen that the control method designed in this paper meets the AGV operation accuracy requirements, with high trajectory tracking accuracy and small error. Under certain conditions, the AGV navigation control effect is good and maintains stable operation.

[0262] Through the trajectory tracking experiment, the AGV distance and angle deviation analysis results are as follows Fig.19 and Fig. 20 shown.

[0263] Depend on Fig.19 It can be seen that the solid line is the distance deviation obtained based on the PSO Lyapunov function control algorithm, and the dotted line is the deviation obtained based on the PID control algorithm. The maximum distance deviation obtained based on the PSO Lyapunov function is 34mm, and the cumulative average error is 3.01mm, while the maximum distance deviation obtained by the PID control algorithm is 53.81mm, and the cumulative average error is 12.8mm. The maximum error is reduced by 36.81%, and the average cumulative error is reduced by 76.48%. The control effect of the PSO Lyapunov function control method is better than that of the PID control method, which ensures that the AGV runs stably under the specified trajectory and meets the performance requirements of the AGV.

[0264] Depend on Fig. 20 It can be seen that the solid line is the angle deviation obtained based on the PSO Lyapunov function control algorithm, and the dotted line is the angle deviation obtained based on the PID control algorithm. The maximum angle deviation obtained based on the PSO Lyapunov function is 5.338°, and the cumulative average angle error is 0.45°. The maximum angle deviation obtained by the PID control algorithm is 9.1432°, and the cumulative average error is 1.247°. Among them, the maximum error is reduced by 41.62%, and the average cumulative error is reduced by 63.91%. Under the control of the PSO Lyapunov function, the AGV has high tracking accuracy, no large trajectory deviation occurs, and the AGV operation accuracy is improved, ensuring the precise and stable operation of the AGV.

[0265] The controller designed based on PSO Lyapunov function and PID controller were used to carry out trajectory tracking experiments, and the linear velocity and angular velocity curve comparison results of the two methods were obtained, as shown in Figure 2. Fig.21 , 22 As shown in the figure, the solid line represents the result based on PSO Lyapunov function control, and the dotted line represents the result of PID control speed tracking control.

[0266] Depend on Fig.21 It can be seen that the actual linear speed is 0.25m / s, and the average linear speed obtained based on the PID control algorithm is 0.15m / s. The trajectory speed deviation of the tracking experiment is large, which makes the AGV unstable and cannot meet the actual work needs. The average linear speed of the AGV based on the PSO Lyapunov function trajectory tracking control is 0.203m / s, which is closer to the target speed.

[0267] Depend on Fig. 22 It can be seen that the average angular velocity is 0.26rad / s. The angular velocity fluctuation based on PID control is relatively large, the AGV direction deviates, and it is far away from the target and cannot accurately reach the designated location. The average angular velocity of the AGV based on PSO Lyapunov function trajectory tracking control is 0.11rad / s, which is closer to the target in the direction of movement, ensuring that the AGV meets the performance requirements and runs stably, and at the same time accurately reaches the designated location with high trajectory tracking accuracy.

[0268] Through comparative experiments, it is verified that the proposed improved algorithm and the PSO Lyapunov function trajectory tracking control method have better operating effects. Therefore, it is necessary to use the research method proposed in this paper to further conduct comprehensive experiments to analyze and verify the comprehensive operating performance of AGV.

[0269] During the operation of AGV, variables such as load, running speed, and number of obstacles all affect the control accuracy and operation effect of AGV. In order to verify the comprehensive operation capability of AGV, the orthogonal test method is used to verify the reliability of the method and provide a theoretical basis for engineering practice application. Among them, load, running speed, and number of obstacles are taken as influencing factors, and three specific data are selected as representatives for each factor to construct an orthogonal level table, as shown in Table 5.

[0270] Table 5 Orthogonal factor level table

[0271]

[0272] According to the designed orthogonal factor level table, the orthogonal test schedule is used for the experiment, and 9 groups of experiments are required. According to the previous linear trajectory and curve trajectory tracking experimental results, the angle deviation is small during the operation of the AGV, and the distance deviation can be used as the experimental result. The experimental results are counted in the orthogonal test level table, as shown in Table 6.

[0273] Table 6 Orthogonal test schedule and results

[0274]

[0275]

[0276] From the analysis of Table 6, it can be obtained that under different variable parameters, the distance deviation values ​​of the straight trajectory and the curve trajectory of the AGV do not fluctuate greatly, and the AGV can operate stably, indicating that the design method in this paper can meet the actual operation requirements of the AGV.

[0277] The range analysis method can accurately analyze the influence of various variables on the operation accuracy of AGV. The principle is as follows: Fig.23 shown.

[0278] R j represents the range of the jth column factor, which means the difference between the maximum and minimum values ​​of the experimental index of the jth column at different levels. The running precision orthogonal test R j The calculation formula is:

[0279]

[0280] Where: R j is the range value of the uncertainty factor in the jth column;

[0281] K jm is the AGV operation accuracy corresponding to the m level of the uncertainty factor in the jth column;

[0282] K jm The average value of .

[0283] according to The size of can select the optimal combination of j columns of external uncertainties and each level.

[0284] R j It reflects the change of the external factors in the jth column, R j The larger the value of is, the greater the impact of the external factor on the AGV operation accuracy, and vice versa. j It can be used to determine the primary and secondary relationships between various uncertain factors in the experiment.

[0285] The experimental results were sorted out, and the various influencing factors were analyzed by the range method. K was calculated according to formula (27) jm , K jm , The analysis results of the straight track distance deviation and the curved track distance deviation are shown in Tables 7 and 8.

[0286] Table 7 Straight track distance deviation results

[0287]

[0288] Table 8 Curve trajectory distance deviation results

[0289]

[0290] From the analysis of Table 7 and Table 8, it can be seen that for the straight track distance deviation, the factor that affects it the most is the running speed, followed by the number of obstacles, and the load has the least impact. For the curve track distance deviation, the running speed is also the most influential factor. Therefore, the AGV running speed will affect the straight track tracking and curve track tracking, and also affect the running accuracy of the AGV. Therefore, the running accuracy of the AGV can be guaranteed by reducing the speed during the experiment. Therefore, it is reasonable to set the speed to 0.25m / s in this paper, which provides a theoretical basis for the practical application of the designed AGV.

Claims

1. The AGV trajectory tracking control method based on PSO Lyapunov function is characterized by comprising the following steps: 1) Establish AGV kinematic model, AGV motion posture error model, lidar observation model and DC motor model; Step 1) The specific process is as follows: 1.1) Establish the AGV kinematic model: stipulate that the center point of the driving wheel axis is the center of mass of the AGV vehicle, the wheel surface of the driving wheel has no relative sliding with the ground, and only pure rolling without axial movement; The AGV using differential drive is subject to non-integrity constraints, that is, the motion constraint equation is: According to the AGV rigid body motion law, the AGV linear velocity v and angular velocity ω are obtained: Where: L is the distance between the driving wheels, v L and v R Represent the linear speed of the left and right wheels respectively, Taking the center of AGV as the reference point, the kinematic model of AGV is: From equations (2) and (3), the kinematic model of AGV can be described as: 1.2) AGV motion posture error model: Therefore, when the period is T, the displacement and angle increments at time k are: Where: T is the adoption period; When the motion state of AGV at time k is P k =[X k ,Y k ,θ k ] T , when the motion state of AGV at time k+1 is P k+1 =[X k+1 ,Y k+1 ,θ k+1 ] T , then the motion state at time k+1 is: Define the actual position of the robot in the global coordinate system P = [X, Y, θ] T With reference pose P r =[X r ,Y r ,θ r ] TTherefore, the error between the actual robot posture and the reference posture can be defined as P e =PP r ,Right now: According to the coordinate transformation, the posture error e of the two-wheel differential mobile robot is: Combining formula (3), the local posture error, i.e., formula (8), is differentiated, and the dynamic equation relative to the AGV posture error is obtained as follows: Find the control law q=[v ω] according to the error T Make the AGV move along the desired trajectory, that is, the posture error P e The error terms in the equation tend to zero, that is, 1.3) LiDAR Observation Model: The LiDAR observation model represents the correspondence between the distance and angle of the landmark observed by the sensor in the polar coordinate system and the position of the polar coordinate system in the global coordinate system, as shown in Equation (10); Where: D, is the distance and angle in the sensor coordinate system; x d ,y d The location information of obstacles in the environment; x s ,y s ,θ s is the sensor pose coordinate; D r ,D θ is the distance and angle error of the observation model; The AGV converts the data in the sensor coordinate system to the global coordinate system, and the converted data is used to estimate the AGV's own position and the position of the environment map; 1.4) DC motor model: The motor electrical equation and mechanical characteristic equation are combined to obtain the model, as follows; DC motor electrical equations: Mechanical equations for a DC motor: in: C a is the potential of the motor, T = C t i a ; J is the moment of inertia on the motor shaft; By performing Laplace transform on equations (11) and (12), we can obtain: The mechanical time constant in formula (13) is calculated as: The electrical time constant is calculated as: From formula (13), we can see that the transfer function of the DC motor is a second-order lag-free transfer function, as shown in formula (16): Due to T a <<T m , so it is approximately considered that T m +T a ≈T m , 2) Introduce Lyapunov function to design AGV trajectory tracking controller; The step 2) is as follows: According to Lyapunov stability theory, the constructed Lyapunov function formula is: Where k2>0 and is bounded, the derivative of the above Lyapunov function can be obtained: According to the Lyapunov stability theorem, the control quantity is selected as: Where k1>0, k3>0 and are bounded, solving equation (21) yields: According to formula (23), we can judge When both v and ω are not 0, It is not always 0, so it is judged that the designed control law can make the AGV motion control system asymptotically stable and realize AGV trajectory tracking control; 3) The controller parameter regulator based on particle swarm PSO algorithm is used for parameter adjustment in the kinematic controller; 3.1) Build a velocity controller to obtain the posture error, linear velocity, and angular velocity; 3.2) Based on the designed kinematic controller, the proportional factor in the control law is optimized based on the PSO algorithm, and a control parameter regulator is constructed to adjust the posture online according to the deviation, thereby ensuring high trajectory tracking accuracy and smooth operation; 3.3) Use the error data of the actual operation of the AGV as feedback, observe the changes in the relevant parameters of the AGV, and regularly update the control parameter regulator; The step 3) is as follows: PSO is used to adjust the parameters of the controller and construct the cost function of formula (26): Where: w1, w2, w3, w4 are weights; t r is the rise time; j 2 (t) is the performance index, which is used as a parameter to select the minimum objective function; u 2 (t) is the square term of the control quantity; h 2 (t) is the overshoot; After determining the fitness function, the three parameters k1, k2, and k3 of the speed controller are adjusted based on the PSO algorithm to obtain more accurate control parameters, stably control the speed and steering of the AGV, and optimize the performance of the AGV control system; 4) According to the requirements of AGV motion control, the PID control method is used to design a PID controller based on the motor model, and the PID controller is used to control the speed of the two wheels; 4.1) During the control process, the dynamic response capability of the speed information received by the drive system will directly affect the actual effect of trajectory tracking; 4.2) The speed tracking control of the drive motor based on incremental PID needs to take into account the impact of the motor and realize the compensation adjustment of the AGV speed; 4.3) An incremental PID control algorithm is used to calculate the expected linear speed of the left and right wheels of the AGV bottom return system through a speed converter. Then the error between the target speed of the left and right wheels and the actual output speed of the left and right wheels is used as the input of the controller to output the actual speed of the left and right drive wheels, so as to achieve stable regulation of the system output, improve the stability of AGV operation, and ensure that the uncertainty of the system itself has good robustness.

Images