CN115236973A - AGV trajectory tracking control method based on PSO Lyapunov function - Google Patents
AGV trajectory tracking control method based on PSO Lyapunov function Download PDFInfo
- Publication number
- CN115236973A CN115236973A CN202210891951.7A CN202210891951A CN115236973A CN 115236973 A CN115236973 A CN 115236973A CN 202210891951 A CN202210891951 A CN 202210891951A CN 115236973 A CN115236973 A CN 115236973A
- Authority
- CN
- China
- Prior art keywords
- agv
- control
- controller
- speed
- pso
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 230000033001 locomotion Effects 0.000 claims abstract description 44
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 38
- 230000000694 effects Effects 0.000 claims abstract description 26
- 230000006870 function Effects 0.000 claims description 75
- 238000013461 design Methods 0.000 claims description 27
- 239000002245 particle Substances 0.000 claims description 22
- 230000008569 process Effects 0.000 claims description 19
- 230000008859 change Effects 0.000 claims description 13
- 238000005457 optimization Methods 0.000 claims description 10
- 230000004044 response Effects 0.000 claims description 8
- 238000012546 transfer Methods 0.000 claims description 7
- 230000033228 biological regulation Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 6
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 238000005498 polishing Methods 0.000 claims description 3
- 238000005096 rolling process Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 239000000126 substance Substances 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000004088 simulation Methods 0.000 description 28
- 238000002474 experimental method Methods 0.000 description 21
- 238000004458 analytical method Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 9
- 230000009471 action Effects 0.000 description 6
- 238000013178 mathematical model Methods 0.000 description 6
- 238000011160 research Methods 0.000 description 5
- 244000290594 Ficus sycomorus Species 0.000 description 4
- 230000001276 controlling effect Effects 0.000 description 4
- 230000006641 stabilisation Effects 0.000 description 3
- 238000011105 stabilization Methods 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 235000013305 food Nutrition 0.000 description 2
- 230000001052 transient effect Effects 0.000 description 2
- 235000011034 Rubus glaucus Nutrition 0.000 description 1
- 244000235659 Rubus idaeus Species 0.000 description 1
- 235000009122 Rubus idaeus Nutrition 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000007619 statistical method Methods 0.000 description 1
- 238000010998 test method Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B11/00—Automatic controllers
- G05B11/01—Automatic controllers electric
- G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
- G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P. I., P. I. D.
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
The invention discloses an AGV track tracking control method based on a PSO Lyapunov function, which is used for researching AGV track tracking motion control, establishing and analyzing an AGV model, mainly comprising an AGV kinematics model, an AGV motion control error model, a laser radar observation model and a direct current motor model, defining a target for designing an AGV track tracking controller, designing a controller based on the PSO Lyapunov function based on the target, mainly introducing the Lyapunov function, utilizing the Lyapunov stability judgment basis, designing a controller based on the Lyapunov function, designing a controller parameter regulator based on a PSO algorithm to regulate parameters in the kinematics controller, constructing a control method consisting of a speed controller and a control parameter regulator, considering the influence of the speed of a driving wheel on system track tracking, and adopting a PID (proportional-integral-derivative) controller to control two-wheel speed to improve track tracking effect.
Description
Technical Field
The invention relates to the technical field of AGV trajectory tracking motion control, in particular to an AGV trajectory tracking control method based on a PSO Lyapunov function.
Background
At present, AGV tracking control research mainly comprises two aspects of path tracking and trajectory tracking. The difference between the two is mainly that the traced path has a specific relation with the time function. Where path tracking is time independent, no changes occur after a given path as the AGV moves. The trajectory tracking is related to time, the given trajectory is changed continuously along with the movement of the AGV, and the path tracking can be used as a simplified form of the trajectory tracking. When the reference trajectory equation has no time variable, the content of the research is path tracking, but when the reference trajectory equation contains a time parameter, the content of the research is trajectory tracking. And the track tracking control is to ensure that the AGV runs along a specified path, for the laser navigation AGV, a laser radar sensor acquires the pose error of the AGV, a proper bounded control quantity is selected to stably regulate and control the AGV, and finally the rotation angular speed of the left wheel and the right wheel of the AGV is obtained, so that the track tracking control of the AGV is realized.
The PID control commonly used in the traditional control needs to obtain a relatively accurate mathematical model, when the model parameters are uncertain, the mathematical model needs to rely on experience and field debugging for setting the PID parameters, the error range is large, the result has uncertainty, the self-adaptive control effect is poor, and the classical PID control has great limitation in practical application. The AGV track tracking control system has a typical time-varying nonlinear characteristic, and a given tracking track can change along with the change of the movement of a vehicle body and the external environment, so that the design designs a controller with a reasonable control law according to the characteristics of the AGV, so that the AGV can stably run according to a scheduled track, and the pose error tends to zero under the condition that the speed is not zero.
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides a method for controlling a driving motor based on incremental PID to influence the motion control of an AGV, aiming at the problems that the spatial position and the navigation angle of the AGV can change in the motion process, in order to ensure that the AGV can accurately follow a given track and safely and stably reach a working place, an AGV kinematics model and a pose error differential equation are established, a Lyapunov function is introduced to design an AGV track tracking controller, a controller parameter regulator based on a PSO algorithm is adopted for parameter regulation in the kinematics controller, and a control method consisting of a speed controller and a control parameter regulator is established. Compared with the traditional controller, the controller improves the AGV control precision, the response performance and the robustness.
The technical scheme of the invention is as follows:
an AGV trajectory tracking control method based on a PSO Lyapunov function comprises the following steps:
1) Establishing an AGV kinematics model, an AGV movement pose error model, a laser radar observation model and a direct current motor model;
2) Introducing a Lyapunov function to design an AGV track tracking controller;
3) A controller parameter regulator based on particle swarm PSO algorithm is adopted for parameter regulation in the kinematics controller;
3.1 Constructing a speed controller to obtain pose errors, linear speed and angular speed;
3.2 Based on a designed kinematic controller, aiming at control parameters in a control law, optimizing a scale factor in the control law based on a PSO (particle swarm optimization) algorithm, constructing a control parameter regulator, and realizing online pose adjustment according to deviation, thereby ensuring high track tracking precision and stable operation;
3.3 Using error data of the AGV actually running as feedback, observing the change of related parameters of the AGV, and periodically updating the control parameter regulator;
4) According to the requirement of AGV motion control, a PID controller based on a motor model is designed by utilizing a PID control method, and the two-wheel speed is controlled by adopting the PID controller.
4.1 In the control process, the dynamic response capability of the speed information received by the driving system can directly influence the actual effect of the track tracking;
4.2 The drive motor is controlled based on the speed tracking of the incremental PID, and the influence brought by the motor needs to be considered to realize the compensation adjustment of the AGV speed;
4.3 An incremental PID control algorithm is selected, the expected linear speeds of the left wheel and the right wheel of the AGV bottom layer returning system are calculated through a speed converter, then the error between the target speeds of the left wheel and the right wheel and the actual output speed of the left wheel and the right wheel is used as the input of a controller, the actual speeds of the left driving wheel and the right driving wheel are output, the stable adjustment output of the system is realized, the running stability of the AGV is improved, and the uncertainty of the system is guaranteed to have good robustness.
Further, the specific process of the step 1) is as follows:
1.1 Build a kinematic model of AGV): the central point of the axis of the driving wheel is specified as the mass center of the AGV trolley, the wheel surface of the driving wheel does not slide relative to the ground, only pure rolling is carried out, and axial movement does not occur;
the AGV adopting the differential driving mode is subjected to non-integrity constraint, namely a motion constraint equation is as follows:
according to the rigid body motion law of the AGV, obtaining the linear velocity v and the angular velocity omega of the AGV:
in the formula: l is the distance between the drive wheels, v L And v R Respectively representing the linear velocity of the left and right wheels,
with the center of the AGV as a reference point, the kinematic model of the AGV is:
from equations (2) and (3), the kinematic model of the AGV is described as:
1.2 AGV motion pose error model: thus, when a period of time T is used, then the displacement and angle increments at time k are:
in the formula: t is a sampling period;
when the motion state of the AGV at the moment k is P k ＝[X k ,Y k ,θ k ] T When the AGV moves at the time k +1, the moving state is P k+1 ＝[X k+1 ,Y k+1 ,θ k+1 ] T Then the motion state at time k +1 is:
defining the actual pose P = [ X, Y, theta ] of the robot in a global coordinate system] T And reference pose P r ＝[X r ,Y r ,θ r ] T So that an error between the actual pose of the robot and the reference pose can be defined as P e ＝P-P r Namely:
the pose error e of the double-wheel differential mobile robot can be obtained according to the coordinate transformation:
combining formula (3), the local position attitude error, namely formula (8), is derived, and the dynamic equation relative to the AGV position attitude error is obtained as follows:
finding a control law q = [ v ω ]) according to the amount of error] T Moving the AGV along a desired trajectory, i.e. the attitude error P e In which error terms tend to zero, i.e.
1.3 Laser radar observation model: the observation model of the laser radar represents the corresponding relation between the distance and the angle of the landmark observed by the sensor under the polar coordinate system and the position of the coordinate under the global coordinate system, and is shown as a formula (10);
in the formula: d, performing a Chemical Mechanical Polishing (CMP) process,
the distance and the angle under a sensor coordinate system;
x and y are position information of obstacles in the environment;
x s ,y s ,θ s is the sensor pose coordinate;
D r ,D θ distance and angle errors of the observation model;
the AGV converts data in a sensor coordinate system into a global coordinate system, and the converted data is used for estimating the self pose of the AGV and the position of an environment map;
1.4 Direct current motor model: the method comprises the following steps of solving through the simultaneous calculation of an electrical equation and a mechanical characteristic equation of a motor, wherein the calculation is as follows;
electrical equation of dc motor:
mechanical equation of the dc motor:
wherein: c a Potential of the motor, T = C t i a ；
J is the rotational inertia on the shaft of the motor;
laplace transform of equations (11) and (12) yields:
the mechanical time constant in equation (13) is calculated as:
the electrical time constant is calculated as:
as shown in equation (13), the transfer function of the dc motor is a second-order lag-free transfer function, as shown in equation (16):
due to T a ＜＜T m Therefore, approximately consider T m +T a ≈T m ,
Further, the step 2) is specifically as follows:
according to the Lyapunov stability theory, the constructed Lyapunov function formula is as follows:
wherein k is 2 > 0 and bounded, derivation of the lyapunov function of the above form yields:
according to the Lyapunov stability theorem, the control quantity is selected as follows:
wherein k is 1 ＞0,k 3 (> 0 and bounded), solving equation (21) yields:
can be judged according to the formula (23)
And when both v and ω are not 0,
the value is not always 0, so that the designed control law is judged, the AGV motion control system can be gradually stabilized, and the AGV track tracking control is realized.
Further, the step 3) is specifically as follows:
and (3) adjusting the parameters of the controller by using the PSO to construct a cost function of an equation (26):
in the formula: w is a 1 ,w 2 ,w 3 ,w 4 Are all weights;
t r is the rise time;
∫j 2 (t) is a performance index, which is used as a parameter to select a minimum objective function;
u 2 (t) is the square term of the control quantity;
y (t) is overshoot;
after the fitness function is determined, three parameters k of the speed controller are determined based on the PSO algorithm 1 ，k 2 ，k 3 Is adjusted to obtainMore accurate control parameters are obtained, the speed and the steering of the AGV are stably controlled, and the performance of the AGV control system is optimized.
The invention has the following beneficial effects:
1) The controller is designed based on the Lyapunov function direct method, the designed control law can be judged, the AGV motion control system can be gradually stabilized, and the AGV track tracking control is realized.
2) The controller optimizes the scale factor in the control law based on a PSO algorithm aiming at the control parameters in the control law on the basis of a designed kinematics controller, constructs a control parameter regulator, and realizes online pose adjustment according to deviation, thereby ensuring high track tracking precision and stable operation.
3) The controller adopts a PID control algorithm to control the speed of the two wheels according to the established motor model to improve the integral effect of track tracking, realize the stable regulation output of the system, improve the running stability of the AGV and ensure that the uncertainty of the system has good robustness.
4) In the AGV moving process, the influence of the speed of a driving wheel on the track tracking of the system is considered, and a PID controller is adopted to control the speed of two wheels to improve the track tracking effect; compared with the traditional controller, the controller improves the AGV control precision, response performance and robustness.
Drawings
FIG. 1 is an AGV kinematics model of the present invention;
FIG. 2 is a schematic illustration of the trajectory error of the present invention;
FIG. 3 is an AGV observation model in an ideal state according to the present invention;
FIG. 4 is a flow chart of the motor incremental PID speed control operation of the present invention;
FIG. 5 is a simulation result of the incremental PID linear velocity of the motor of the invention;
FIG. 6 shows the simulation results of the incremental PID angular velocity of the motor according to the present invention;
FIG. 7 is a PID control schematic of the invention;
FIG. 8 is a diagram of an AGV trajectory tracking control system configuration of the present invention;
FIG. 9 is a schematic diagram of the PSO control parameter regulator of the present invention;
FIG. 10 is a schematic diagram of a PID control system for the drive motor of the present invention;
FIG. 11 is a simulation result of a PID controller tracking a linear trajectory according to the invention;
FIG. 12 is a simulation result of the PID controller tracking the curve trace according to the invention;
FIG. 13 is a PSO-based Lyapunov function controller linear trajectory simulation result of the present invention;
FIG. 14 is a plot trace simulation result of a PSO-based Lyapunov function controller of the present invention;
FIG. 15 is a prototype model of a laser navigation AGV of the present invention;
FIG. 16 is a diagram of an actual scenario of the present invention;
FIG. 17 is the results of the straight-line trajectory tracking of the present invention;
FIG. 18 is a graph trace tracking result of the present invention;
FIG. 19 is a distance offset of the present invention;
FIG. 20 is an angular offset of the present invention;
FIG. 21 is a comparison of the speed effects of the present invention;
FIG. 22 is a comparison of the angular velocity effects of the present invention;
FIG. 23 is a schematic diagram of a range analysis of the present invention.
Detailed Description
The invention is further described with reference to the drawings and examples.
An AGV track tracking control method based on a PSO Lyapunov function is characterized in that a mechanical structure of a double-magnetic-strip navigation latent type AGV based on RFID is simplified, and an AGV kinematics model is established and is shown in figure 1. The center point of the axis of the specified driving wheel is the mass center of the AGV trolley, the wheel face of the driving wheel does not slide relative to the ground, only pure rolling is carried out, and axial movement does not occur.
In FIG. 1, v L And v R Respectively show the linear velocity of left and right wheels, and the AGV that adopts the differential drive mode receives non-integrality restraint, and the motion constraint equation is:
according to the rigid body motion law of the AGV, the linear velocity v and the angular velocity omega of the AGV can be obtained:
in the formula: l is the distance between the drive wheels.
With the center of the AGV as a reference point, the kinematic model of the AGV is:
from equations (2) and (3), the kinematic model of the AGV may be described as:
thus, when a period of time T is used, then the displacement and angle increments at time k are:
in the formula: t is the sampling period.
Then when the AGV's motion state at time k is P k ＝[X k ,Y k ,θ k ] T When the AGV moves at the time k + 1, the moving state is P k+1 ＝[X k+1 ,Y k+1 ,θ k+1 ] T Then the motion state at time k +1 is:
defining the actual pose of the robot in the global coordinate systemP＝[X,Y,θ] T And reference pose P r ＝[X r ,Y r ,θ r ] T So that an error between the actual pose of the robot and the reference pose can be defined as P e ＝P-P r Namely:
an illustrative diagram of the trajectory error during actual travel of the AGV is shown in fig. 2.
The pose error e of the double-wheel differential mobile robot can be obtained according to the coordinate transformation:
combining formula (3), the local position attitude error, i.e. formula (8), is derived, and the dynamic equation relative to the AGV attitude error is obtained as follows:
through designing the controller, a control law q = [ v omega ] is found according to the error amount] T Moving AGV along desired trajectory, i.e. attitude error P e In which the error terms tend to zero, i.e.
This is the design goal of the AGV trajectory tracking controller.
The design establishes an observation model for the laser radar, wherein the observation model of the laser radar represents the corresponding relation between the distance and the angle of a landmark observed by a sensor under a polar coordinate system and the position of the coordinate under a global coordinate system, and the formula (10) shows.
In the formula: d, performing a Chemical Mechanical Polishing (CMP) process,
the distance and the angle under a sensor coordinate system;
x and y are position information of obstacles in the environment;
x s ,y s ,θ s is the sensor pose coordinate;
D r ,D θ the distance and angle errors of the observation model.
In an ideal state, the AGV observation model is as shown in fig. 3, the AGV may convert data in the sensor coordinate system to the global coordinate system, and the converted data may be used to estimate the position of the AGV and the position of the environment map.
The calculation of a mathematical model of the direct current motor is the core of the design of the speed regulating system, and influences the input of the control speed of the motor and the running precision of the AGV. The mathematical model of the dc motor can be obtained by simultaneously establishing an electrical equation and a mechanical property equation of the motor, and is calculated as follows.
Electrical equation of dc motor:
mechanical equation of the dc motor:
wherein: c a Potential of the motor, T = C t i a ；
J is the moment of inertia on the motor shaft.
Laplace transform of equations (11) and (12) yields:
the mechanical time constant in equation (13) is calculated as:
the electrical time constant is calculated as:
as shown in equation (13), the transfer function of the dc motor is a second-order lag-free transfer function, as shown in equation (16):
due to T a ＜＜T m Therefore, approximately consider T m +T a ≈T m ,
TABLE 1 DC MOTOR nameplate parameters
According to the adopted motor, the nameplate parameters are shown in table 1, and are obtained by calculating a parameter model of the direct current motor:
M＝0.02N·m；C＝0.003N·m/A；T a ＝0.013s；T m ＝0.2s。
the transfer function obtainable by substituting the result into equation (13) is:
after mathematical models of the AGV driving motor are established and analyzed and the motor is selected and determined, simulation is carried out on MATLAB, and the working performance of the motor adopting an incremental PID control algorithm is verified.
The AGV system controls the motor through the driving controller, so that the purpose of controlling the AGV to move is achieved, in order to accurately control the AGV to move and guarantee accurate operation precision, stability and real-time performance, the operation period of the driver is set to be 10 milliseconds, multiple tasks are convenient to achieve, and the motor incremental PID speed control operation flow is shown in figure 4. The system mainly comprises two driving controllers and a main controller, wherein an AGV driving wheel is driven by two direct current motors, the controllers issue duty ratio information and output PWM control signals, a left wheel motor and a right wheel motor acquire the control signals, meanwhile, the system acquires control reading and converts the control reading into speed information by using an encoder, then the system issues information such as linear speed and angular speed, and finally the AGV is controlled to run stably, and the system serial port communication needs to be kept during the period.
The motor control adopts an incremental PID control algorithm, and the principle is as follows:
in the formula: u (k-1) is the output of the incremental PID controller at the time of k-1;
error (k) is the motor tracking speed error at the moment k;
k p is a proportionality coefficient;
k i is an integral coefficient;
k d is a differential coefficient.
The algorithm is as follows:
the incremental PID control parameter settings are shown in table 2.
TABLE 2 PID control parameter settings
The unit step velocity tracking for the motor model is shown in fig. 5 by using the MATLAB simulation result.
Wherein, fig. 5 is a simulation result of the incremental PID linear velocity of the motor, the solid line in fig. 5 (a) is a linear velocity tracking result, the dashed line is an ideal velocity variation curve, and fig. 5 (b) is a tracking error result of the motor velocity. Fig. 6 is a simulation result of the incremental PID angular velocity of the motor, in which the solid line in fig. 6 (a) is an actual tracking result of the angular velocity of the motor, the dotted line is an ideal variation curve of the angular velocity, and fig. 6 (b) is an error result of the tracking of the angular velocity. As can be seen from fig. 5 and 6, the AGV linear velocity and angular velocity curves substantially tend to be stable after 0.3s of operation, and the results of the step transient response are shown in table 3 below.
TABLE 3 step transient response index of motor based on incremental PID control algorithm
The simulation result shows that the system speed tracking has errors, but the errors are smaller, so that the speed tracking influences the position and the posture of the AGV, and meanwhile, the comprehensive operation performance of the AGV is influenced, and a theoretical basis is laid for the design of an AGV controller. Therefore, when the trajectory tracking controller is designed later, the influence of the motor on the tracking precision of the AGV trajectory needs to be considered, and the comprehensive operation performance of the AGV is further improved.
The most common control in an AGV control system is PID control, which directly performs closed-loop control on a controlled object and has 3 parameters k p ，k i ，k d Is an on-line control. The PID control principle is shown in fig. 7.
For an AGV control system, a stable control system is a precondition for ensuring safe and stable operation of the AGV, otherwise, in practical application, an unstable system hardly gives play to the performance of a product. The motion system of the AGV is a non-linear system, and therefore the lyapunov direct method is used to design a trajectory tracking controller of the AGV. The velocity controller is analytically established by means of the Lyapunov stability theory, since k 1 ，k 2 ，k 3 Three control parameters are difficult to determine, a controller parameter regulator based on a PSO optimization algorithm is provided for parameter regulation of a speed controller in a kinematic controller, a control system composed of the speed controller and the control parameter regulator is constructed, meanwhile, the system speed is considered to be easily interfered by external factors, and the control design of the system speed is carried out based on a motor model. The running stability of the AGV is improved, and therefore the purpose of tracking the track is achieved.
The overall design of the trajectory tracking controller is shown in fig. 8, and a block diagram of a trajectory tracking control system is shown. As can be seen from the graph, the AGV expects a pose (x) r ,y r ,θ r ) Error (x) between actual pose (x, y, theta) e ,y e ,θ e ) As the input of the kinematic controller, linear velocity and angular velocity control quantities (v, omega) are calculated by a velocity controller, and target velocities (v, omega) of the left wheel and the right wheel are calculated by a velocity converter Ld ,v Rd ) (ii) a Then the target speeds (v) of the left and right wheels Ld ,v Rd ) And actual output left and right wheel speeds (v) L ,v R ) Error of (e) l ,e r ) The actual speeds of the left and right drive wheels are output as inputs to the controller. And finally, the AGV is ensured to stably advance along a preset path, so that the aim of tracking the track is fulfilled.
As can be seen from FIG. 8, the trajectory tracking control system structure is composed of two parts, namely PSO-based control parameter regulator design and motor model-based PID controller design, and finally the design of the AGV trajectory tracking controller is completed.
1. Design of controller based on Lyapunov function direct method
The lyapunov method includes both a first method and a second method, wherein the first method analyzes the stability of the system by solving a differential equation. The second method is also called a direct method, which has low requirements on a system mathematical model, does not need to solve a specific differential equation, and for a motion system, the stability of the system is reduced along with the time, so that the system can reach an equilibrium state at a certain time point, but for a nonlinear system, a system function is difficult to reach an equilibrium state, so that a virtual scalar function is required to be constructed by the Lyapunov second method to represent the energy of the whole system, and the system is balanced.
The lyapunov direct method for judging the stability of the system needs to judge through a scalar function and a derivative of the scalar function, the scalar function must be positive because the energy is always larger than zero, and the attenuation characteristic of the system is expressed by the derivative of the scalar function.
According to the Lyapunov stability theory, a configurable Lyapunov function formula is as follows:
wherein k is 2 > 0 and bounded, derivation of the lyapunov function of the above form yields:
according to the Lyapunov stability theorem, the control quantity is selected as follows:
wherein k is 1 ＞0,k 3 > 0 and bounded, solving equation (21) can result:
can be judged according to the formula (23)
And when both v and ω are not 0,
the value is not always 0, so that the designed control law can be judged, and the AGV motion control system can be enabled toAnd the gradual stability is realized, and the tracking control of the AGV track is realized.
2. PSO-based control parameter regulator design
1) Principle of controlling a parameter regulator
According to the overall design of an AGV track tracking control system, the AGV track tracking control system comprises a PSO-based control parameter regulator design, and the regulator is realized by three steps:
a. and constructing a speed controller to obtain pose errors, linear speed and angular speed.
b. On the basis of a designed kinematics controller, aiming at control parameters in a control law, a control parameter regulator is constructed based on a proportional factor in a PSO algorithm optimization control law, and the online pose adjustment is realized according to deviation, so that the high track tracking precision and the stable operation are ensured.
c. And taking error data of the AGV in actual operation as feedback, observing the change of relevant parameters of the AGV, and periodically updating the control parameter regulator.
2) Principle of PSO algorithm
During the design of the controller, three parameters k of the speed controller are used 1 ，k 2 ，k 3 Influencing the control effect of a kinematic controller, the parameter k is determined by a general conventional method 1 ，k 2 ，k 3 The algorithm is complex and the robustness is poor. Through analysis, the k of the speed controller 1 The value influences the speed of movement, k, of the carriage 2 And k 3 The rotation amplitude of the trolley is adjusted by the value. Because the selection of control parameters is complex and the self-adaptive effect is poor, the proportional factor of the speed controller designed on the basis of the Lyapunov stability is adjusted in real time by adopting a Particle Swarm Optimization (PSO) algorithm.
Particle Swarm Optimization (PSO) is derived from the research on the predation behavior of a bird swarm, and simulates the behavior of a bird swarm for randomly searching food. Each bird seeking food is called a particle, each particle has its own velocity of motion, and the direction and distance of movement of the particle, as well as the adaptation value determined by the optimization function, are established. The particle swarm optimization is firstly initialized to a group of random particles, then an optimal prediction solution is searched through iteration, in the process of each iteration, the particles track and update two extreme values, the first particle is found to be the optimal solution at present, namely an individual extreme value, the other particle is the optimal prediction solution found in the whole bird swarm at present, and is also called a sub-swarm extreme value problem, and the optimal solution in a part of the particles is called a local extreme point.
Assuming that a search unit consisting of m particles is constructed in a search space of D dimension, the velocity information of the ith particle can be represented as V i ＝(v i1 ,v i2 ,…,v iD ) I =1,2, …, m, the location information may be represented as X i ＝(x i1 ,x i2 ,…,x iD ) I =1,2, …, m, the particle velocity and position update equation is:
in the formula:
is the velocity of the particle i in the d dimension in the kth iteration;
omega is an inertia factor and is a non-negative constant;
c 1 ,c 2 as learning factor, non-negative constant, generally let c 1 ＝c 2 ＝2；
r 1 ,r 2 Is a random number between 0 and 1;
For the design, a Particle Swarm Optimization (PSO) algorithm is used for parameter optimization, and a schematic diagram of a controller of the design is shown in fig. 9.
Setting the parameters of the controller by using the PSO, and constructing a cost function of an equation (26):
in the formula: w is a 1 ,w 2 ,w 3 ,w 4 Are all weights;
t r is the rise time;
∫j 2 (t) is a performance index, which is used as a parameter to select a minimum objective function;
u 2 (t) is the square term of the control quantity;
y (t) is the overshoot.
After the fitness function is determined, three parameters of the controller are adjusted based on the PSO algorithm, more accurate control parameters are obtained, the speed and steering of the AGV are stably controlled, the performance of the AGV control system is optimized, and meanwhile, the effectiveness of the design method needs to be verified through later simulation.
3. PID controller design based on motor model
In the control process, the dynamic response capability of the speed information received by the driving system can directly influence the actual effect of the track tracking. According to the simulation analysis, the driving motor influences the AGV motion control based on the speed tracking control of the incremental PID, and the influence caused by the motor needs to be considered, so that the compensation adjustment of the AGV speed is realized. Therefore, the system speed needs to be considered when the AGV track tracking controller is designed, the overall track tracking effect is improved by controlling the two-wheel speed through the PID control algorithm according to the established motor model, the stable regulation output of the system is realized, the running stability of the AGV is improved, and the uncertainty of the system is ensured to have good robustness.
And designing a PID controller based on a motor model by utilizing a PID control method according to the AGV motion control requirement, wherein the speed difference is used as the input of the controller, and the output is the actual speed of the AGV. A block diagram of a PID control system for the AGV drive motor is shown in fig. 10.
Wherein: v. of L ,v R The actual output linear speed of the left wheel and the right wheel is the AGV mass center position of the lower computer;
v Ld ,v Rd the linear speed is expected for the left and right wheels of the AGV bottom layer returning system;
e l ,e r is the speed error;
u l ,u r is the output of the controller.
Aiming at the problem that the track tracking effect of AGV track tracking is poor, the design and analysis of a track tracking control algorithm are carried out, and in order to verify that the PSO-based Lyapunov function controller has higher reliability and accuracy in the AGV track tracking control effect than a classical PID controller, simulation analysis comparison is carried out on the two controllers through MATLAB. And (3) building simulation models of the AGV based on the PSO Lyapunov function controller and the classic PID controller in the MATLAB, respectively carrying out a PID controller track tracking simulation experiment and a PSO Lyapunov function controller based simulation experiment, setting the AGV running speed to be 0.5m/s, and analyzing the simulation result as follows.
1) Classic PID controller simulation experiment
The system adopts the PID controller to track, and the track tracking effect of the PID controller is verified through a linear track tracking simulation experiment. Wherein the expected track is a straight line which forms an angle of 45 degrees with the positive direction of the x axis, the initial pose of the reference track is set to be (0,0, pi/4), and the actual initial pose of the AGV is set to be (0, -2, pi). The simulation results are shown in fig. 11.
As shown in fig. 11 (a), the AGV can track a straight trajectory by using a PID controller. The system accurately completes the tracking of the expected track through the adjustment of 4.5 s. Fig. 11 (b) is a pose error convergence curve in which the pose errors all converge to 0 within 5s to reach a steady state. The AGV can realize rapid convergence of the pose error under the action of the PID controller.
After the linear track tracking is finished, the system performs a curve track tracking simulation experiment under the action of the PID controller, the tracking effect of the algorithm is verified, and the expected track is an S-shaped curve. Wherein the initial pose error is set to (0.2,1.5,5). The simulation results are shown in fig. 12.
Fig. 12 (a) is a displacement curve of the trajectory tracking of the "S" curve, where the AGV generates a certain error before the AGV runs, and then the AGV can completely coincide with the reference trajectory and gradually approach to a steady state. Fig. 12 (b) shows the situation of converging the position error of the AGV, in which the position error can converge to 0 within 6s, and reach the steady state. As can be seen from fig. 12 (a) and (b), for an arc with a large curvature, the AGV tracking trajectory based on the PID controller is greatly overshot, which results in low accuracy of the AGV operation process, easy generation of certain fluctuation, and too long system adjustment stabilization time, which cannot meet the performance and function requirements.
2) Simulation experiment based on PSO Lyapunov function controller
The control performance of a PSO-based Lyapunov function controller on linear trajectory tracking is checked, the AGV is set to start from an initial pose (0,0, pi/4), a tracking reference speed V =0.5m/s, a reference angular speed W =0rad/s straight line, an initial pose error is (0.5,2,0.2), and control parameters are k respectively 1 ＝15,k 2 ＝1.5,k 3 And =5.3. The results of the straight-line trajectory tracking simulation are shown in fig. 13.
As can be seen from fig. 13, the system can track the linear trajectory under the action of the controller based on the PSO lyapunov function. As can be seen from fig. 13 (a), convergence of the trajectory is achieved at an abscissa of 1.5 m. Fig. 13 (b) is a pose error convergence curve, and the pose error converges to 0 at 4s and thereafter tends to a steady state. FIG. 13 (c) shows the change of AGV speed and angular velocity, which converges to 0.5m/s at 1.5s and converges to 0 at 2 s. Fig. 13 (d) shows the change in the speed of the left and right wheels, and after 2.5 seconds, the speed converges to 0.5m/s, and the left and right wheels run at a predetermined speed and tend to stabilize. As can be seen from fig. 13 (e) and (f), the distance deviation of the pseo-based lyapunov function tends to be stable at 1s and gradually becomes 0, the angle deviation tends to be stable at 1.5s and gradually converges to 0, while the distance deviation of the PID control algorithm tends to be stable at 2.5s and gradually becomes 0, and the angle deviation tends to be stable at 3.5s and gradually converges to 0.
The control performance of a PSO-based Lyapunov function controller on curve track tracking is checked, the AGV is set to start from an initial pose (0,0,0), the tracking reference speed V =0.5m/s, the reference angular speed W =0.5rad/s of straight line, the initial pose error is (0,1,0), and control parameters are k respectively 1 ＝5,k 2 ＝45,k 3 And (8). The results of the curve trace simulation are shown in fig. 14.
As can be seen from FIG. 14, the AGV system realizes the tracking of the curve trajectory under the action of the controller based on the Lyapunov function of the PSO. In fig. 14 (a), the actual trajectory of the AGV converges gradually to the reference trajectory at 0.6m in the negative x direction, completely coinciding with the reference trajectory, and the result shows that the system is in a steady state after converging to the reference trajectory. Fig. 14 (b) is a pose error convergence curve, where the pose error of the AGV system converges to 0 at 2s, and in addition, all the pose error convergence curves are relatively smooth, which indicates that the AGV operates stably. FIG. 14 (c) shows the change of the AGV speed and the angular velocity, and the change converges to 0.5m/s at 2s, and the change converges to the reference angular velocity at 2 s. Fig. 14 (d) shows the change of the left and right wheel speeds, which converge to the AGV reference speed after 1.5s and then stabilize, and the left and right wheels perform a curve motion at a predetermined speed. As can be seen from fig. 14 (e) and (f), the distance deviation of the pseo-based lyapunov function tends to be stable at 4s and gradually becomes 0, the angle deviation tends to be stable at 3.8s and gradually converges to 0, while the distance deviation of the PID control algorithm tends to be stable at 7s and gradually becomes 0, and the angle deviation tends to be stable at 5s and gradually converges to 0.
As can be seen from fig. 13 and 14, the performance of the two controllers is verified through a tracking simulation experiment of a linear track and a circular arc track, the AGV system can quickly realize the tracking of the reference track under the action of the pse-based lyapunov controller, the error is quickly converged and tends to a stable state, the speed and the angular speed can be quickly converged to the reference value, the response speed of the system is high, and the control effect is good. And a large amount of debugging parameters are needed for the PID controller, and the AGV system has long stabilization time and large stabilization error. Therefore, the Lyapunov controller based on the PSO has strong applicability to the tracking of the AGV, the performance and the function requirements of the AGV can be met, and the stability and the running precision of the AGV are improved.
Example (b):
meanwhile, prototype experiments are carried out on the design, and a laser navigation AGV prototype model is shown in FIG. 15.
The AGV prototype has the following specific characteristics:
(1) The AGV model machine adopts a two-wheel differential motion mode, comprises two driving wheels and four driven wheels, has the capability of differential motion, and can flexibly steer and avoid obstacles.
(2) The driving wheel motor drives a direct current motor driving system produced by a medium-voltage transmission intelligent technology limited company as a driving source, and the direct current motor driving system has the characteristics of large torque and capability of maintaining low-speed stable operation.
(3) The navigation uses YDLIDAR laser radar, the function of which is mainly used for map construction, and the AGV can sense and search and send information in the environment.
(4) Meanwhile, a raspberry group is used as an upper computer control platform, and the AGV is controlled through remote connection to complete movement and work.
AGV prototype parameters are shown in Table 4.
TABLE 4 AGV prototype Performance parameters
The application object of this experiment is AGV of indoor environment work, and it mainly is applicable to carrying the work such as the material loading and unloading transportation that moves big, little weight and carry out the production line. The selected experimental ground is used by multiple applicationsThe factory environment guarantees practical environment safe and reliable to a great extent, and the place accords with the requirement in experiment place, plans the arrangement to the experiment place simultaneously. The whole indoor environment has a length of about 15m, a width of about 10m and an area of about 150m 2 The AGV runs for a circle around a laboratory conveniently, and in order to be close to a real environment better, a stool simulation barrier is placed on the path. The experimental operating environment is shown in fig. 16.
The method comprises the steps of carrying out a track tracking experiment on the basis of a built AGV prototype, enabling the AGV to run a global road section, selecting 3 obstacles with the speed of 0.25m/s, the load of 100kg and the speed of 100kg for verifying the overall performance of the AGV and the reliability of a control method, and respectively carrying out tracking experiment verification on straight lines and circular arcs.
Firstly, carrying out a linear tracking experiment, enabling the AGV to run for a circle in a designed environment and counting data, then selecting a section of linear line segment to verify the running effect of the AGV from a starting point to a target point, and according to the following steps of 1:5, scaling up. The result of the straight-line trajectory tracking is shown in fig. 17.
As can be seen from FIG. 17, the system speed is set to 0.25m/s during the tracking process, the AGV respectively performs linear trajectory tracking based on PSO Lyapunov function control and PID control, and as can be seen from the figure, both trajectory tracking effects can meet the trajectory tracking requirements. The average error of y-direction displacement of a section of straight line selected in the whole track tracking is 9.32mm based on PSO Lyapunov function control straight line tracking, while the average error value of tracking is 9.99mm based on PID control, and the error is reduced by 6.71%, so that the straight line track tracking effect based on PSO Lyapunov function control has greater advantages, and the high precision of AGV track tracking control and stable operation are ensured.
Next, curve tracing experiments were performed. When the AGV path tracking experiment does not move according to the overall planned path, the interference of external factors such as obstacles can be encountered, the curve tracking path is carried out, and the obstacles are bypassed. The AGV can perform operations such as steering and turning around in the moving process, so that the arc path is one of important links in the track tracking process, an arc curve tracking experiment needs to be performed, curve tracking effects of two control algorithms are compared, and the feasibility and the accuracy of the method are verified.
Firstly, selecting a section of circular arc in the global path section track tracking to verify the circular arc track tracking effect, and according to the following steps of 1:5, scaling up. The graph of the curve trace tracking result is shown in fig. 18.
According to the statistical analysis of the selected curve path result, the average error of y-direction displacement of curve track tracking based on PSO Lyapunov function control is 14.76mm, while the average error value of tracking based on PID control is 17.44mm, and the error is reduced by 15.37%, so that the tracking accuracy based on PSO Lyapunov function control is higher, and certain superiority is achieved.
As can be seen from FIGS. 17 and 18, the control method designed herein meets the requirement of AGV running accuracy, and has high trajectory tracking accuracy and small error. Under certain environment, the AGV navigation control effect is good and the stable work is kept.
The analysis results of the AGV distance and angle deviation are shown in fig. 19 and 20 through the trajectory tracking experiment.
As can be seen from fig. 19, the solid line indicates the distance deviation obtained based on the PSO lyapunov function control algorithm, and the dotted line indicates the deviation obtained based on the PID control algorithm. The maximum distance deviation obtained based on the PSO Lyapunov function is 34mm, the accumulated average error is 3.01mm, the maximum distance deviation obtained by the PID control algorithm is 53.81mm, the accumulated average error is 12.8mm, the maximum error is reduced by 36.81%, the average accumulated error is reduced by 76.48%, the control effect based on the PSO Lyapunov function control method is better than that of PID control, the stable running of the AGV under the specified track is ensured, and the performance requirement of the AGV is met.
As can be seen from fig. 20, the solid line indicates the angle deviation obtained based on the PSO lyapunov function control algorithm, and the dotted line indicates the angle deviation obtained based on the PID control algorithm. The maximum angle deviation obtained based on the PSO Lyapunov function is 5.338 degrees, the accumulated average angle error is 0.45 degrees, the maximum angle deviation obtained by the PID control algorithm is 9.1432 degrees, and the accumulated average error is 1.247 degrees. The maximum error is reduced by 41.62%, the average accumulated error is reduced by 63.91%, and under the control action based on the PSO Lyapunov function, the tracking accuracy of the AGV is high, large track deviation does not occur, the running accuracy of the AGV is improved, and the accurate and stable running of the AGV is guaranteed.
A controller designed based on a PSO Lyapunov function and a PID controller are adopted to carry out a track tracking experiment, and linear velocity and angular velocity curve comparison results of the two methods are obtained, as shown in FIGS. 21 and 22. Wherein the solid line represents the result of the control based on the PSO lyapunov function, and the dotted line represents the result of the PID control velocity tracking control.
As can be seen from fig. 21, the actual linear velocity is 0.25m/s, the average linear velocity obtained based on the PID control algorithm is 0.15m/s, and the deviation of the track velocity in the tracking experiment is large, which results in unstable operation of the AGV and fails to meet the actual working requirement. And the average linear speed of the AGV based on PSO Lyapunov function track tracking control is 0.203m/s, which is closer to the target speed.
As can be seen from fig. 22, the average angular velocity is 0.26rad/s, and the angular velocity fluctuation based on PID control is large, the AGV direction deviates, and the remote target cannot accurately reach the designated location. The average angular speed of the AGV based on PSO Lyapunov function track tracking control is 0.11rad/s, the AGV is closer to a target in the moving direction, the performance requirement is met, the AGV can stably run, meanwhile, the AGV can accurately reach a specified place, and the track tracking precision is high.
Compared with the improved algorithm and the PSO Lyapunov function-based trajectory tracking control method, the improved algorithm and the PSO Lyapunov function-based trajectory tracking control method have better operation effects, so that the comprehensive experiment needs to be further carried out by adopting the research method provided by the text, and the comprehensive operation performance of the AGV is analyzed and verified.
In the running process of the AGV, variables such as load, running speed, the number of obstacles and the like all influence the control precision and the running effect of the AGV. In order to verify the comprehensive operation capability of the AGV, an orthogonal test method is adopted to verify the reliability of the method, and a theoretical basis is provided for engineering practice application. The load, the running speed and the number of obstacles are used as influencing factors, and each factor selects three specific data as representatives to construct an orthogonal level table, as shown in table 5.
TABLE 5 orthogonal factor horizon
According to the designed orthogonal factor horizontal table, an orthogonal test arrangement table is adopted for carrying out the experiment, and 9 groups of experiments are required. According to the results of the straight line track and curve track tracking experiments, in the process of AGV operation, the angle deviation is small, the distance deviation can be used as an experiment result, and the experiment result is counted in an orthogonal experiment horizontal table, as shown in table 6.
TABLE 6 orthogonal test Schedule and results
Analysis table 6 shows that, under different variable parameters, the deviation value of the distance of the straight line track and the deviation value of the distance of the curve track of the AGV run do not fluctuate greatly, the AGV can run stably, and the design method can meet the actual running requirements of the AGV.
The influence degree of each variable on the running precision of the AGV can be accurately analyzed by adopting a range analysis method, and the principle of the method is shown in the following chart 23.
R j Representing the extreme difference value of the j-th column factor, representing the difference value of the maximum value and the minimum value of the j-th column experimental index under different levels, and running a precision orthogonal test R j The calculation formula of (2) is as follows:
wherein: r j The range value of the uncertain factors in the jth column;
K jm AGV running accuracy corresponding to m level of uncertainty factor of j column；
According to
Can select the optimal combination of the external uncertainty factors of j columns and each level.
R j Reflects the amplitude change condition of the external factors of the operation precision in the jth column R j The larger the value of (A) is, the larger the influence of the external factor on the running accuracy of the AGV is, and vice versa. Thus R j Can be used for judging the primary and secondary relationship among various uncertain factors in the experiment.
The experimental results are collated, each influence factor is analyzed by a range method, and K is calculated according to the formula (27) jm ，K jm ，
The results of the linear trajectory distance deviation and the curved trajectory distance deviation analysis are shown in tables 7 and 8.
TABLE 7 results of distance deviations of straight-line trajectories
TABLE 8 Curve track distance deviation amount results
As can be seen from analysis of tables 7 and 8, the largest factor affecting the linear trajectory distance deviation amount is the running speed, and the next is the number of obstacles, so that the load influence is minimal. For curve trajectory distance deviations, the speed of travel is also the most influential factor. Therefore, the running speed of the AGV can influence the tracking of a straight line track and the tracking of a curve track, and meanwhile, the running precision of the AGV is influenced, so that the running precision of the AGV can be ensured by reducing the speed in the experimental process, the speed set by the method is reasonable to be 0.25m/s, and theoretical basis is provided for the practical application of the designed AGV.
Claims (4)
1. An AGV track tracking control method based on a PSO Lyapunov function is characterized by comprising the following steps:
1) Establishing an AGV kinematics model, an AGV motion pose error model, a laser radar observation model and a direct current motor model;
2) Introducing a Lyapunov function to design an AGV track tracking controller;
3) A controller parameter regulator based on particle swarm PSO algorithm is adopted for parameter regulation in the kinematics controller;
3.1 Constructing a speed controller to obtain pose errors, linear speed and angular speed;
3.2 Based on a designed kinematic controller, aiming at control parameters in a control law, a control parameter regulator is constructed based on a proportional factor in a PSO algorithm optimization control law, and the online pose adjustment is realized according to deviation, so that the high track tracking precision and the stable operation are ensured;
3.3 Using error data of the AGV actually running as feedback, observing the change of related parameters of the AGV, and periodically updating the control parameter regulator;
4) According to the requirement of AGV motion control, a PID controller based on a motor model is designed by utilizing a PID control method, and the two-wheel speed is controlled by adopting the PID controller.
4.1 In the control process, the dynamic response capability of the speed information received by the driving system can directly influence the actual effect of the track tracking;
4.2 The drive motor is controlled based on the speed tracking of the incremental PID, and the influence brought by the motor needs to be considered to realize the compensation adjustment of the AGV speed;
4.3 An incremental PID control algorithm is selected, the expected linear speeds of the left wheel and the right wheel of the AGV bottom layer returning system are calculated through a speed converter, then the errors of the target speeds of the left wheel and the right wheel and the actual output speeds of the left wheel and the right wheel are used as the input of a controller, the actual speeds of the left driving wheel and the right driving wheel are output, the stable adjustment output of the system is realized, the running stability of the AGV is improved, and the uncertainty of the system is ensured to have good robustness.
2. The method for AGV trajectory tracking control based on PSO Lyapunov function according to claim 1, wherein the specific process of step 1) is as follows:
1.1 Build a kinematic model of AGV): the central point of the axis of the driving wheel is specified as the mass center of the AGV trolley, the wheel surface of the driving wheel does not slide relative to the ground, only pure rolling is carried out, and axial movement does not occur;
the AGV adopting the differential driving mode is subjected to non-integrity constraint, namely a motion constraint equation is as follows:
according to the rigid body motion law of the AGV, obtaining the linear velocity v and the angular velocity omega of the AGV:
in the formula: l is the distance between the drive wheels, v L And v R Respectively, the linear velocities of the left and right wheels,
with the center of the AGV as a reference point, the kinematic model of the AGV is:
from equations (2) and (3), the kinematic model of the AGV is described as:
1.2 AGV motion pose error model: thus, when a period of time T is used, then the displacement and angle increments at time k are:
in the formula: t is a sampling period;
when the motion state of the AGV at the moment k is P k ＝[X k ,Y k ,θ k ] T When the AGV moves at the time of k +1, the movement state is P k+1 ＝[X k+1 ,Y k+1 ,θ k+1 ] T Then the motion state at time k +1 is:
defining the actual pose P = [ X, Y, theta ] of the robot in a global coordinate system] T And reference pose P r ＝[X r ,Y r ,θ r ] T So that an error between the actual pose of the robot and the reference pose can be defined as P e ＝P-P r Namely:
the pose error e of the double-wheel differential mobile robot can be obtained according to the coordinate transformation:
combining formula (3), the local position attitude error, namely formula (8), is derived, and the dynamic equation relative to the AGV position attitude error is obtained as follows:
finding a control law q = [ v ω ]) according to the amount of error] T Moving the AGV along a desired trajectory, i.e. the attitude error P e In which the error terms tend to zero, i.e.
1.3 Lidar observation model: the observation model of the laser radar represents the corresponding relation between the distance and the angle of the landmark observed by the sensor under the polar coordinate system and the position of the coordinate under the global coordinate system, and is shown as a formula (10);
in the formula: d, performing a Chemical Mechanical Polishing (CMP) process,
the distance and the angle under a sensor coordinate system;
x and y are position information of obstacles in the environment;
x s ,y s ,θ s is the sensor pose coordinate;
D r ,D θ distance and angle errors of the observation model;
the AGV converts data in a sensor coordinate system into a global coordinate system, and the converted data is used for estimating the self pose of the AGV and the position of an environment map;
1.4 Direct current motor model: the method comprises the following steps of solving through the simultaneous calculation of an electrical equation and a mechanical characteristic equation of a motor, wherein the calculation is as follows;
electrical equation of dc motor:
mechanical equation of the dc motor:
wherein: c a Potential of the motor, T = C t i a ；
J is the rotational inertia on the shaft of the motor;
laplace transform of equations (11) and (12) yields:
the mechanical time constant in equation (13) is calculated as:
the electrical time constant is calculated as:
as shown in equation (13), the transfer function of the dc motor is a second-order lag-free transfer function, as shown in equation (16):
due to T a ＜＜T m Therefore, approximately consider T m +T a ≈T m ,
3. The method for AGV trajectory tracking control based on the PSO Lyapunov function of claim 1, wherein the step 2) is specifically as follows:
according to the Lyapunov stability theory, the constructed Lyapunov function formula is as follows:
wherein k is 2 > 0 and bounded, derivation of the lyapunov function of the above form yields:
according to the Lyapunov stability theorem, the control quantity is selected as follows:
wherein k is 1 ＞0,k 3 > 0 and bounded, solving equation (21) can result:
can be judged according to the formula (23)
And when both v and ω are not 0,
the value is not always 0, so that the designed control law is judged, the AGV motion control system can be gradually stabilized, and the AGV track tracking control is realized.
4. The method for AGV trajectory tracking control based on the PSO Lyapunov function of claim 1, wherein the step 3) is specifically as follows:
and (3) adjusting the parameters of the controller by using the PSO to construct a cost function of an equation (26):
in the formula: w is a 1 ,w 2 ,w 3 ,w 4 Are all weights;
t r is the rise time;
∫j 2 (t) as a performance index, selecting a minimum objective function as a parameter;
u 2 (t) is the square term of the control quantity;
y (t) is overshoot;
after the fitness function is determined, three parameters k of the speed controller are determined based on a PSO algorithm 1 ，k 2 ，k 3 And adjusting to obtain more accurate control parameters, stably controlling the speed and steering of the AGV and optimizing the performance of the AGV control system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210891951.7A CN115236973A (en) | 2022-07-27 | 2022-07-27 | AGV trajectory tracking control method based on PSO Lyapunov function |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210891951.7A CN115236973A (en) | 2022-07-27 | 2022-07-27 | AGV trajectory tracking control method based on PSO Lyapunov function |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115236973A true CN115236973A (en) | 2022-10-25 |
Family
ID=83676415
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210891951.7A Pending CN115236973A (en) | 2022-07-27 | 2022-07-27 | AGV trajectory tracking control method based on PSO Lyapunov function |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115236973A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116958189A (en) * | 2023-09-20 | 2023-10-27 | 中国科学院国家空间科学中心 | Moving point target time-space domain track tracking method based on line segment correlation |
| CN117087669A (en) * | 2023-10-20 | 2023-11-21 | 小米汽车科技有限公司 | Vehicle speed tracking control method and device, vehicle and storage medium |
| CN117706910A (en) * | 2023-12-27 | 2024-03-15 | 哈尔滨工业大学 | Robot cluster coverage method and system based on sliding mode control and with different maximum speeds |
-
2022
- 2022-07-27 CN CN202210891951.7A patent/CN115236973A/en active Pending
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116958189A (en) * | 2023-09-20 | 2023-10-27 | 中国科学院国家空间科学中心 | Moving point target time-space domain track tracking method based on line segment correlation |
| CN116958189B (en) * | 2023-09-20 | 2023-12-12 | 中国科学院国家空间科学中心 | Moving point target time-space domain track tracking method based on line segment correlation |
| CN117087669A (en) * | 2023-10-20 | 2023-11-21 | 小米汽车科技有限公司 | Vehicle speed tracking control method and device, vehicle and storage medium |
| CN117087669B (en) * | 2023-10-20 | 2024-01-30 | 小米汽车科技有限公司 | Vehicle speed tracking control method and device, vehicle and storage medium |
| CN117706910A (en) * | 2023-12-27 | 2024-03-15 | 哈尔滨工业大学 | Robot cluster coverage method and system based on sliding mode control and with different maximum speeds |
| CN117706910B (en) * | 2023-12-27 | 2024-06-07 | 哈尔滨工业大学 | Robot cluster coverage method and system based on sliding mode control and with different maximum speeds |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111273551B (en) | Double-steering wheel automatic guided vehicle trajectory tracking control method and system | |
| CN115236973A (en) | AGV trajectory tracking control method based on PSO Lyapunov function | |
| Wang et al. | A hybrid visual servo controller for robust grasping by wheeled mobile robots | |
| Hashemi et al. | Model-based PI–fuzzy control of four-wheeled omni-directional mobile robots | |
| CN110244735B (en) | Heuristic dynamic programming control method for tracking preset track by mobile robot | |
| CN103324196A (en) | Multi-robot path planning and coordination collision prevention method based on fuzzy logic | |
| CN109857100B (en) | Composite track tracking control algorithm based on inversion method and fast terminal sliding mode | |
| CN111538328A (en) | Priority hierarchical prediction control method for obstacle avoidance trajectory planning and tracking control of autonomous driving vehicle | |
| Andaluz et al. | Robust control with redundancy resolution and dynamic compensation for mobile manipulators | |
| Zangina et al. | Non-linear PID controller for trajectory tracking of a differential drive mobile robot | |
| Chen et al. | Real-time predictive sliding mode control method for AGV with actuator delay | |
| Zhang et al. | Motion planning and tracking control of a four-wheel independently driven steered mobile robot with multiple maneuvering modes | |
| Lin et al. | Simulation of robot automatic control model based on artificial intelligence algorithm | |
| Shin et al. | Explicit path tracking by autonomous vehicles | |
| Korayem et al. | Regulation of cost function weighting matrices in control of WMR using MLP neural networks | |
| Ni et al. | Path tracking motion control method of tracked robot based on improved LQR control | |
| Rubí et al. | Adaptive nonlinear guidance law using neural networks applied to a quadrotor | |
| Hou | Haptic teleoperation of a multirotor aerial robot using path planning with human intention estimation | |
| Farag | Model-predictive-control complex-path tracking for self-driving cars | |
| CN116719320A (en) | Wheeled robot track tracking control and obstacle avoidance method and system | |
| Bacha et al. | Autonomous vehicle path tracking using nonlinear steering control and input-output state feedback linearization | |
| Wu et al. | Design of ugv trajectory tracking controller in ugv-uav cooperation | |
| CN114895556A (en) | Wheel type robot track tracking control method and computer readable medium | |
| Ning et al. | Time-optimal point stabilization control for WIP vehicles using quasi-convex optimization and B-spline adaptive interpolation techniques | |
| Xie et al. | Parameter self‐learning feedforward compensation‐based active disturbance rejection for path‐following control of self‐driving forklift trucks |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |