CN116991064A - Differential AGV trajectory tracking method and system based on actuator anti-saturation control - Google Patents

Abstract

The invention relates to a differential AGV trajectory tracking method and system based on actuator anti-saturation control, which includes: obtaining the tracking trajectory, obtaining the ideal pose and ideal speed according to the tracking trajectory; and obtaining the position based on the current actual pose and the ideal pose. Posture deviation, the posture deviation and the ideal speed are used as the input of the kinematic controller to obtain the control speed; the control speed is used as the input of the dynamics controller to obtain the original wheel torque, and the original wheel torque is used as the input of the limiting control module to obtain The constrained wheel torque, the limiting control module makes the constrained wheel torque within the limit range of the torque actuator; the constrained wheel torque is used as the input of the dynamics model to obtain the speed, and it is used as the input of the kinematic model to obtain the next moment Actual pose. The kinematic controller is based on torque limiting and anti-saturation compensation, so that the control speed output by the kinematic controller can be effectively tracked even when the actuator is saturated.

Description

Differential AGV trajectory tracking method and system based on actuator anti-saturation control

Technical field

The invention relates to the technical field of AGV trajectory tracking motion control, in particular to a differential AGV trajectory tracking method and system based on actuator anti-saturation control.

Background technique

The Automated Guided Vehicle (AGV) system is an important part of intralogistics, covering almost all production areas, such as supply chain, tobacco industry, aircraft manufacturing, warehouses and container terminals. As an important member of the AGV family, differential AGV is widely used in a variety of occasions. Differential AGV is a typical strongly coupled, nonlinear, and non-holonomic dynamic system. Therefore, the trajectory tracking control problem of differential AGV is difficult to solve using methods in traditional linear system theory, which also makes its trajectory tracking control problem extremely challenging. The reference trajectory in the trajectory tracking problem is a function that depends on time parameters. It is necessary to consider the longitudinal displacement and lateral displacement error of the AGV at the same time. Through comprehensive control of the longitudinal and lateral motion of the vehicle, the AGV can reach the corresponding location at a given time. Reference track point.

In the existing technology, the kinematic characteristics and dynamic characteristics are comprehensively considered to study the trajectory tracking control method of wheeled mobile robot (WMR), which improves the performance of the control system and the stability of the vehicle body. Due to factors such as inaccurate measurements, time-varying parameters, and input saturation disturbances, it is difficult to obtain an accurate mathematical model of the WMR system. In response to this, an active-disturbance-rejection trajectory tracking control scheme emerged as the times require, such as achieving accurate estimation of the disturbance amount by expanding the observer.

However, the control methods currently proposed mainly focus on the stability and tracking accuracy of the control system, and most do not consider the saturation constraints of the actuator. Since the actual system is affected by the actuator saturation factor, when designing the controller, if only the tracking performance indicators are considered and the control input saturation problem is ignored, it will be difficult to ensure the stability of the closed-loop system in practical applications.

Contents of the invention

In view of the shortcomings of the existing technology, the present invention provides a differential AGV trajectory tracking method and system based on actuator anti-saturation control. The purpose is to specifically deal with the actuator saturation problem and realize that the robot can effectively track the actuator even when the actuator is saturated. Track the control speed output by the kinematic controller to ensure the stability of the closed-loop system.

The technical solutions adopted by the present invention are as follows:

This application provides a differential AGV trajectory tracking method based on actuator anti-saturation control, including:

Obtain the tracking trajectory and obtain the ideal pose and ideal speed based on the tracking trajectory;

The pose deviation is obtained according to the current actual pose of the AGV and the ideal pose, and the pose deviation and the ideal speed are used as inputs to the kinematic controller to obtain the control speed;

The control speed is used as the input of the dynamics controller to obtain the original wheel torque. The original wheel torque is used as the input of the limiting control module to obtain the constrained wheel torque. The limiting control module puts the constrained wheel torque at the torque actuator. within the limits;

Use the constrained wheel torque as the input of the dynamics model to obtain the AGV speed;

Use the AGV speed as the input of the kinematic model to obtain the actual pose at the next moment;

in:

The kinematic model represents the relationship between AGV speed and posture, taking into account the influence of slip disturbance;

The kinematics controller is established based on the kinematics model, with posture as the control parameter and control speed as the control quantity;

The dynamic model represents the relationship between wheel torque and AGV speed, and all disturbances are concentrated to obtain the total disturbance term;

The dynamics controller is established based on a dynamics model, uses control speed as a control parameter, wheel torque as a control variable, and estimates the aggregate disturbance term through a nonlinear expansion observer;

The input of the dynamics controller also includes a compensation amount output by an anti-windup compensator, which is used to compensate for the deviation of the dynamics controller when tracking the control speed. The anti-windup compensator is based on the original wheel torque. and the difference between the constrained wheel torque as input.

Further technical solutions are:

The dynamic model is established based on the kinematic model and Lagrangian mechanical analysis, and the expression is:

In the formula, η=[v,w] ^T , the center speed of the vehicle body Car body angular velocity/> are the left and right wheel angular speeds respectively, r is the drive wheel radius,/> That is, the vectors of central acceleration and angular acceleration;

τ=[τ _l ,τ _r ] ^T , τ _l , τ _r are the left and right wheel torques respectively;

Total set perturbation Among them, bounded invertible matrix/> and matrix/> is the system nominal parameter matrix,/> and/> Represents the uncertainty of system parameters caused by load changes; ξ is the initial disturbance term.

The dynamic controller is designed using the sliding film control method in combination with limiting control and anti-saturation compensation, and uses the approaching law of the exponential superhelical sliding mode to satisfy the sliding mode conditions. Its control equation is:

In the formula, the control speed η _c =[v _c ,w _c ] ^T , the subscript c represents control; is the estimated value obtained by estimating the total set disturbance d through the nonlinear expansion observer; C=diag(c _v , c _w ) is the coefficient, c _v , c _w are the coefficients corresponding to the center velocity and angular velocity of the vehicle body respectively, both are greater than 0; γ = [γ _v , γ _w ] ^T is the compensation amount, γ _v , γ _w are the compensation amounts corresponding to the center velocity and angular velocity of the vehicle body respectively, and/> Δτ＝τ _u -τ _v , τ _v , τ _u are the original wheel torque and the constrained wheel torque respectively; the speed tracking error vector/>

λ, D, K ₁ are all coefficients related to the fractional-order integral sliding mode surface s=e _v +λD ^α-1 |e _v | ^ε sign(e _v ), sign(·) is the sign function, Among them, δ ₀ is a positive number less than 1, a is a positive number, and p is a positive integer greater than 1.

The constrained wheel torque τ _u is obtained based on the limiting control module, which is designed based on the Gaussian error function:

in, τ _max and τ _min are the upper and lower limits of the wheel torque actuator;

Gaussian error function

The kinematic controller is designed using the back-stepping method, and the expression is:

Among them, e _x , e _y , e _θ are respectively the pose deviation e _p obtained based on the actual pose [x, y, θ] ^T and the ideal pose [x _r , y _r , θ _r ] ^T in the carrier coordinate system. Medium element:

Among them, x and y are respectively the horizontal and vertical coordinates of the body in the carrier coordinate system, θ is the angle between the carrier coordinate system and the navigation coordinate system; the subscript r represents the ideal situation; k ₁ , k ₂ , k ₃ are motion The parameters of the learning controller are all positive numbers.

The nonlinear expansion observer is used to estimate the total set disturbance d, including:

Introduce the expansion state vector [x ₁₂ , x ₂₂ ] ^T = [d ₁ , d ₂ ] ^T , define x ₁₁ = v, x ₂₁ = w, expand the expression of the dynamic model, and construct a nonlinear expansion observer:

Among them, z _1i and z _2i are the observer values of states x _1i and x _2i , β _1i and β _2i represent the observer gains, and the subscript i=1,2;

The nonlinear function fal(·) is:

Among them, σ>0, α ₁ =0.5, α ₂ =0.25, subscript i = 1, 2;

about have:

Among them, h _i (i=1,2) is the change rate of di ( _i =1,2);

The parameters β _1i and β _2i are determined through pole configuration, and then the nonlinear expansion observer is used to estimate _di (i=1,2).

This application also provides a differential AGV trajectory tracking system based on actuator anti-saturation control, which is used to execute the differential AGV trajectory tracking method based on actuator anti-saturation control.

The beneficial effects of the present invention are as follows:

The present invention designs a kinematic model and a controller. On this basis, a dynamic model and a controller are designed, in which dynamic control based on limiting control and anti-saturation compensation is used, and the torque of the dynamic controller is constrained through limiting control. output, and uses anti-saturation compensation to speed up the control system out of saturation and avoid control system failure. This ensures that the AGV control performance will not be significantly reduced when the actuator is saturated. Even when the actuator is saturated, it can also ensure that the robot can effectively track the virtual speed (control speed) of the kinematic controller.

The invention improves the accuracy of the model, proposes various disturbances during the establishment process of the motion model, and provides accurate basic information for subsequent controller design.

Additional features and advantages of the invention will be set forth in the description which follows, or may be learned by practice of the invention.

Description of the drawings

Figure 1 is a logical block diagram of a control system according to an embodiment of the present invention.

Figure 2 is a schematic diagram of the differential AGV geometric model in the embodiment of the present invention.

Figure 3 is a diagram of the x-axis error of circular trajectory tracking between this method and the conventional method obtained from the effect verification in the embodiment of the present invention.

Figure 4 is a diagram of the y-axis error of circular trajectory tracking between this method and the conventional method obtained from the effect verification in the embodiment of the present invention.

Figure 5 is a diagram of the circular trajectory tracking angle error between this method and the conventional method obtained from the effect verification in the embodiment of the present invention.

Figure 6 is a graph showing the velocity difference between the circular trajectory tracking center of this method and the conventional method obtained from the effect verification in the embodiment of the present invention.

Figure 7 is a diagram showing the difference in angular velocity of circular trajectory tracking between this method and the conventional method obtained from the effect verification in the embodiment of the present invention.

Figure 8 is a diagram showing the circular trajectory tracking left wheel torque output of this method and the conventional method obtained from the effect verification in the embodiment of the present invention.

Figure 9 is a diagram of the right wheel torque output diagram of circular trajectory tracking obtained by the method and the conventional method obtained from the effect verification in the embodiment of the present invention.

Figure 10 is a disturbance observation diagram under circular trajectory tracking according to the embodiment of the present invention.

Detailed ways

Specific embodiments of the present invention will be described below with reference to the accompanying drawings.

This embodiment provides a differential AGV trajectory tracking method based on actuator anti-saturation control. Taking the differential AGV based on QR code navigation as the object, the control input saturation problem is considered, and the anti-saturation auxiliary system is designed to deal with the actuator saturation problem. Accelerate the actuator's escape from saturation and ensure the stability of the closed-loop system in practical applications.

Referring to the geometric model of the differential AGV shown in Figure 2, the differential AGV consists of two left and right driving wheels and four universal wheels forming a chassis system. It is assumed that the AGV's geometric center, center of gravity and the center of the connection between the two driving wheels coincide. The driving wheel diameter is 2r, and the wheel spacing is 2b. In the figure, X-Y is the navigation coordinate system, x-y is the carrier coordinate system, and θ is the angle between the carrier coordinate system and the navigation coordinate system.

Referring to Figure 1, the method in this embodiment includes:

Obtain the tracking trajectory, and obtain the ideal pose [x _r , y _r , θ _r ] ^T and ideal speed η _r = [v _r , w _r ] ^T according to the tracking trajectory;

According to the current actual pose [x, y, θ] ^T of the AGV and the ideal pose [x _r , y _r , θ _r ] ^T , the pose deviation [x _e , y _e , θ _e ] ^T is obtained. The above-mentioned posture deviation [x _e , y _e , θ _e ] ^T and the above-mentioned ideal speed η _r = [v _r , w _r ] ^T are used as inputs of the kinematic controller to obtain the control speed η _c = [v _c , w _c ] ^T ;

The control speed η _c = [v _c , w _c ] ^T is used as the input of the dynamics controller to obtain the original wheel torque τ _v , and the original wheel torque τ _v is used as the input of the limiting control module to obtain the constrained wheel Torque τ _u , the limiting control module makes the constrained wheel torque τ _u within the torque actuator limit range;

Using the constrained wheel torque τ _u as the input of the dynamic model, the AGV speed η = [v, w] ^T is obtained;

Use the AGV speed η = [v, w] ^T as the input of the kinematic model to obtain the actual pose [x, y, θ] ^T at the next moment;

in:

The kinematic model represents the relationship between AGV speed and posture, taking into account the influence of slip disturbance;

The kinematics controller is established based on the kinematics model, with posture as the control parameter and control speed as the control quantity;

The dynamic model represents the relationship between wheel torque and AGV speed, and all disturbances are concentrated to obtain the total disturbance term;

The dynamics controller is established based on a dynamics model, uses control speed as a control parameter, wheel torque as a control variable, and estimates the aggregate disturbance term through a nonlinear expansion observer;

The input of the dynamics controller also includes a compensation amount output by an anti-windup compensator, which is used to compensate for the deviation of the dynamics controller when tracking the control speed. The anti-windup compensator is based on the original wheel torque. and the difference between the constrained wheel torque as input.

The following is a detailed introduction to the construction methods and ideas of the kinematic model, dynamic model, kinematic controller, and dynamic controller used in this embodiment. The meaning of each symbol in the above expressions will be explained in detail in the following introduction.

1. Construction of kinematic model:

Slip effects are unavoidable in differential driving, especially when the AGV starts. Therefore, slip disturbance is introduced into the established kinematic model. The constraint equation is:

In formula (1), x and y are the horizontal and vertical coordinate positions of the vehicle body respectively, and u refers to the lateral slip disturbance, which is a constantly changing variable. are the speed along the x-axis and along the y-axis respectively, b is half of the wheel spacing, r is the radius of the driving wheel, ζ _l and ζ _r represent the left and right wheel angular velocity disturbances respectively, /> are the left and right wheel angular velocities respectively, and θ is the angle between the carrier coordinate system and the navigation coordinate system.

Formula (1) can be constructed as:

In formula (2):

Λ＝[u,-rζ _l ,-rζ _r ] ^T (4)

make

It can be obtained that A(q)J(q)=0(7), combining equation (7) with equation (2), we can get:

Among them: η=[v,w] ^T , v represents the center speed of the car body/> w represents the angular velocity of the car body/> are the left and right wheel angular speeds respectively,/> Represents the longitudinal slip disturbance amount of the car body Represents the angular velocity disturbance of the vehicle body/> Disturbance vector φ(q,u)=[-u sinθ,ucosθ,0,ζ _l ,ζ _r ] ^T ;

ζ _l and ζ _r are not objects of concern in position and attitude tracking control, so q can be simplified to q = [x, y, θ] ^T , then the AGV kinematics model is:

All variables in equation (10) are located in the navigation coordinate system.

It can be seen from the kinematic model construction process that equations (8) and (9) reflect the influence of slip disturbance, which will be used in the subsequent dynamic model construction, and will ultimately be reflected in the dynamic control. The simplification of the kinematic model here means focusing on the values x, y and angle values.

2. Construction of dynamic model:

Compared with the kinematic model, the dynamic model considers how to make the model move from the mechanical level. Its role is the pioneer of the kinematic model. Its input moment (torque) is the output speed, while the input of the kinematic model is The velocity output by its dynamic model is the pose.

The dynamic model is established based on the kinematic model and Lagrangian mechanical analysis. The specific construction process is as follows:

According to the analysis of Lagrangian mechanics, the dynamic equation of the non-holonomic control system is expressed as:

In formula (11), M(q) is the positive definite inertia matrix, is the centripetal force and Coriolis force matrix, /> is the unknown ground friction term, G(q) is the gravity vector, B(q) is the input transformation matrix, τ is the input vector, in this embodiment τ=[τ _l ,τ _r ] ^T , τ _l , τ _r are respectively Left and right wheel torque, A ^T (q) is the non-holonomic constraint matrix, τ _d is the input vector interference, and λ is the Lagrange multiplier.

Assuming that the AGV works on the horizontal plane, then G(q)=0, combined with equation (7), rewrite equation (11), and substitute equations (8) and (9) into and simplify:

In formula (12),

m=m _c +2m _w ,

I=I _c +2m _w b ² +2I _m , I _c =m _c b ² , τ=[τ _l ,τ _r ] ^T (17)

Among them, formula (14) represents the initial disturbance, m is the total mass, m _c is the main body mass, m _w is the mass of the driving wheel, b is half of the wheel spacing (see Figure 2), r is the radius of the driving wheel, and I is the mobile robot Regarding the moment of inertia of the geometric center, I _c , Im and _{I w respectively} represent the moment of inertia of the vehicle body (including the load) around the vertical axis of the center of gravity, the moment of inertia of the driving wheels around the vertical axis of the center of gravity and the moment of inertia of the driving wheels around the wheel axis. τ _l and τ _r are the left and right wheel torque inputs respectively.

Considering that the actual robot has a load, its mass and moment of inertia will change, and the influence of external disturbances of the system will occur. At this time, an error term will appear in equation (13), so equation (12) can be rewritten as:

In formula (18), the bounded invertible matrix and matrix/> is the system nominal parameter matrix,/> and/> Represents the uncertainty of system parameters caused by load changes;

Separate the nominal parameters from the uncertain parameters, and rewrite equation (18) as:

In formula (19), represents the total set of disturbances, and further abbreviates (19) to obtain the expression of the final dynamic model:

In formula (20), That is, the vectors of central acceleration and angular acceleration,

3. Kinematic controller design:

The purpose of the kinematic controller is to make the actual pose track the target pose within a limited time. Based on the previous kinematic model, the back-stepping method is used to design the kinematic controller:

Formula (21), e _x , e _y , e _θ are respectively the difference between the actual pose [x, y, θ] ^T and the ideal pose [x _r , y _r , θ _r ] ^T in the carrier coordinate system, which is the pose deviation. Elements in vector e _p :

In formula (22), the subscript r represents the ideal situation; k ₁ , k ₂ , and k ₃ are kinematic controller parameters, all of which are positive numbers.

According to Lyapunov stability theory verification, the kinematic controller designed in this embodiment can make the tracking error converge to 0.

4. Power controller design:

According to the kinematic controller, the mobile robot can track the desired trajectory if it runs at the designed speed. However, in real systems, robots often cannot operate at the designed speed. Since the actuator may be saturated during AGV movement, this embodiment uses limiting control to constrain the torque output of the dynamics controller, and uses anti-saturation compensation to accelerate the control system out of the saturated state and avoid control system failure. This ensures that the AGV control performance will not be significantly reduced when the actuator is saturated. Even when the actuator is saturated, it can also ensure that the robot can effectively track the virtual speed (control speed) of the kinematic controller.

The limiting control module with smooth input and output characteristics based on Gaussian error function is:

In formula (23), τ _max and τ _min are the upper and lower limits of the wheel torque actuator; Gaussian error function/>

The anti-saturation compensator is designed as:

In formula (24), C=diag(c _v ,c _w ) is the coefficient, c _v , c _w are the coefficients corresponding to the center velocity and angular velocity of the vehicle body respectively, both are greater than 0, Δτ=τ _u -τ _v , τ _v , τ _u are the original wheel torque and the constrained wheel torque respectively, γ = [γ _v , γ _w ] ^T is the compensation amount, γ _v , γ _w are the compensation amounts corresponding to the center velocity and angular velocity of the vehicle body respectively;

Compensate the compensation amount γ = [γ _v , γ _w ] ^T to the deviation of the dynamic controller when tracking the control speed, and obtain the speed tracking error vector:

It can be seen from the above that the actual operating speed η = [v, w] ^T , the control speed η _c = [v _c , w _c ] ^T , and the subscript c represents control;

According to the fractional order theory, the following fractional order integral sliding mode surface is designed:

s＝[s ₁ ,s ₂ ] ^T ＝e _v +λD ^α-1 |e _v | ^ε sign(e _v ) (25)

Among them, λ=diag(λ ₁ , λ ₂ ), λ ₁ , λ ₂ are both greater than 0, where 0＜α＜1, 0＜ε＜1.

Differentiating equation (25) we can get: Introducing equations (20) and (24) we get:

In order to satisfy the sliding mode conditions, the approaching law of the exponential superhelical sliding mode is adopted, and its expression is:

Among them, K ₁ = diag (k ₁₁ , k ₂₁ ), K ₂ = diag (k ₁₂ , k ₂₂ ), s = diag (s ₁ , s ₂ ), k _ij is a positive number, and sign (·) is a sign function , Among them, δ ₀ is a positive number less than 1, a is a positive number, and p is a positive integer greater than 1;

At this point, a dynamic controller designed using the sliding film control method that combines limiting control and anti-saturation compensation can be obtained:

In formula (28), is the estimated value obtained by estimating the total set disturbance d through the nonlinear expansion observer;

Specifically, a nonlinear expansion observer is used to estimate the total set disturbance d, including:

Introduce the expansion state vector [x ₁₂ , x ₂₂ ] ^T = [d ₁ , d ₂ ] ^T , define x ₁₁ = v, x ₂₁ = w, expand the expression of the dynamic model, and construct a nonlinear expansion observer:

Among them, z _1i and z _2i are the observer values of states x _1i and x _2i , β _1i and β _2i represent the observer gains, and the subscript i=1,2;

The nonlinear function fal(·) is:

Among them, σ>0, α ₁ =0.5, α ₂ =0.25, subscript i = 1, 2;

about have:

Among them, h _i (i=1,2) is the rate of change of di ( _i =1,2).

Specifically, the parameters β _1i and β _2i in (29) are determined through pole configuration, and then the nonlinear expansion observer is used to estimate _di (i=1,2). Those skilled in the art can understand that the pole configuration method is a conventional method, so the specific calculation process will not be described again.

For external disturbances and parameter uncertainties, the expanded state observer is used to estimate the disturbance information and compensate for the uncertain information in the dynamic controller, which can reduce the chattering of the control system.

In summary, the method of this embodiment establishes a kinematic model based on slip disturbance to address the slip disturbance problem existing in differential AGVs. In view of the fact that there are many uncertainties in the traditional dynamic model and cannot provide accurate data, including load changes and friction changes, various uncertainties are aggregated into disturbance terms and processed uniformly. The nonlinear extended state observer is used to estimate the aggregate disturbance term and compensate it into the dynamic model to ensure the accuracy of the control model. For dynamic control, an anti-saturation compensator is designed to avoid tracking failure caused by the actuator entering a saturated state.

In order to verify the effectiveness and superiority of the fractional-order sliding mode control (FOSMC+SAT) method based on the exponential superhelical reaching law of anti-saturation control in this embodiment, the Simulink tool in the MATLAB software was used to track the trajectory of the AGV. Simulate and compare with the ordinary exponential superhelical reaching law fractional order sliding mode control (FOSMC) control system (method).

The actual physical parameters of the differential AGV model are: m = 70kg, m ₀ = 50kg, b = 0.3m, r = 0.08m, I _w = 0.144kgcm ² , τ = 0.64Nm, i = 20. Among them, m is the mass of the robot, m ₀ is the load mass, b is the wheelbase of the two driving wheels, r is the radius of the driving wheel, I _w is the moment of inertia of the driving wheel, τ is the motor torque, and i is the reduction ratio

Among them, the parameters C in the saturation compensator = (c _v ,c _w ) = (10,10), the parameters of the expanded state observer are adjusted with l _ij , and the total set disturbance is designed as:

In order to verify the performance of the algorithm, first use the commonly used verification trajectory (circular trajectory). The trajectory tracking formula is:

It can be seen that the reference running speed of this trajectory is v _r =2m/s, w _r =1rad/s, and the actual starting speed is v =0m/s, w =0rad/s. The expected initial pose is (x _r , y _r , θ _r ) = (2,0,90). Considering the initial pose error, the actual starting pose is (x, y, θ) = (1.8, -0.2,89), considering the initial slip setting t<3 [ψ ₁ ,ψ ₂ ] ^T = [5,0.5] ^T , when 5 ≤ t,

[ψ ₁ ,ψ ₂ ] ^T = [0,0] ^T , when 3≤t<5 [ψ ₁ ,ψ ₂ ] ^T = [5+10*sin(5*t),0.5+sin(5* t)] ^T , the circular trajectory tracking controller parameters are shown in Table 1.

Table 1 Circular trajectory tracking configuration parameters

Simulation is performed based on the parameters in Table 1 above, and the tracking difference simulation results are shown in Figures 3 to 5. It can be seen from the figure that under the condition of strictly controlling the same parameters, both this method (FOSMC+SAT) and the conventional method (FOSMC) can track the trajectory within a limited time.

In addition to the entire trajectory tracking performance, the observed indicators also include the velocity curve and angular velocity curve. Figures 6 and 7 show the difference between the actual speed and the target speed. Figures 6 and 7 show the center speed difference and angular speed difference of the vehicle body respectively. It can be seen from the figure that the FOSMC+SAT method in this embodiment shows excellent performance of fast response, small overshoot, fast convergence, and smooth change in terms of central velocity and angular velocity.

This embodiment considers the influence of the input saturation constraint, therefore, the torque output is also used as the evaluation index. Figures 8 and 9 show the torque output of the left and right wheels tracked by circular trajectories respectively. It can be seen from the figures that the FOSMC+SAT method in this embodiment shows higher response performance in the torque output curve and has a smoother control curve. Smoother and more stable than actual control.

As shown in Figure 10, NESO in the figure represents the estimated value of the input disturbance value Input by the nonlinear expansion observer. It can be seen from the figure that the observation of the disturbance is very accurate.

In summary, it can be proved that the differential AGV trajectory tracking method based on actuator anti-saturation control designed by the FOSMC+SAT method adopted in this embodiment has excellent performance in both trajectory tracking and speed tracking effects.

This embodiment also provides a differential AGV trajectory tracking system based on actuator anti-saturation control, which is used to execute the differential AGV trajectory tracking method based on actuator anti-saturation control.

Those of ordinary skill in the art can understand that the above are only preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, for those skilled in the art, It is still possible to modify the technical solutions recorded in the foregoing embodiments, or to make equivalent replacements for some of the technical features. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection scope of the present invention.

Claims (7)

1. A differential AGV trajectory tracking method based on actuator anti-saturation control, which is characterized by including: Obtain the tracking trajectory and obtain the ideal pose and ideal speed based on the tracking trajectory; The pose deviation is obtained according to the current actual pose of the AGV and the ideal pose, and the pose deviation and the ideal speed are used as inputs to the kinematic controller to obtain the control speed; The control speed is used as the input of the dynamics controller to obtain the original wheel torque. The original wheel torque is used as the input of the limiting control module to obtain the constrained wheel torque. The limiting control module puts the constrained wheel torque at the torque actuator. within the limits; Use the constrained wheel torque as the input of the dynamics model to obtain the AGV speed; Use the AGV speed as the input of the kinematic model to obtain the actual pose at the next moment; in: The kinematic model represents the relationship between AGV speed and posture, taking into account the influence of slip disturbance; The kinematics controller is established based on the kinematics model, with posture as the control parameter and control speed as the control quantity; The dynamic model represents the relationship between wheel torque and AGV speed, and all disturbances are concentrated to obtain the total disturbance term; The dynamics controller is established based on a dynamics model, uses control speed as a control parameter, wheel torque as a control variable, and estimates the aggregate disturbance term through a nonlinear expansion observer; The input of the dynamics controller also includes a compensation amount output by an anti-windup compensator, which is used to compensate for the deviation of the dynamics controller when tracking the control speed. The anti-windup compensator is based on the original wheel torque. and the difference between the constrained wheel torque as input. 2. The differential AGV trajectory tracking method based on actuator anti-saturation control according to claim 1, characterized in that the dynamic model is established based on the kinematic model and Lagrangian mechanics analysis, and the expression is: In the formula, η=[v,w] ^T , the center speed of the vehicle body Car body angular velocity/> are the left and right wheel angular speeds respectively, r is the drive wheel radius,/> That is, the vectors of central acceleration and angular acceleration; τ=[τ _l ,τ _r ] ^T , τ _l , τ _r are the left and right wheel torques respectively; Total set perturbation Among them, the bounded invertible matrix and matrix/> is the system nominal parameter matrix,/> and/> Represents the uncertainty of system parameters caused by load changes; ξ is the initial disturbance term. 3. The differential AGV trajectory tracking method based on actuator anti-saturation control according to claim 2, characterized in that the dynamic controller is designed using a sliding film control method in combination with limiting control and anti-saturation compensation, and adopts The approaching law of the exponential superhelical sliding mode satisfies the sliding mode conditions, and its governing equation is: In the formula, the control speed η _c =[v _c ,w _c ] ^T , the subscript c represents control; is the estimated value obtained by estimating the total set disturbance d through the nonlinear expansion observer; C=diag(c _v , c _w ) is the coefficient, c _v , c _w are the coefficients corresponding to the center velocity and angular velocity of the vehicle body respectively, both are greater than 0; γ = [γ _v , γ _w ] ^T is the compensation amount, γ _v , γ _w are the compensation amounts corresponding to the center velocity and angular velocity of the vehicle body respectively, and/> Δτ＝τ _u -τ _v , τ _v , τ _u are the original wheel torque and the constrained wheel torque respectively; the speed tracking error vector/> λ, D, K ₁ are all coefficients related to the fractional-order integral sliding mode surface s=e _v +λD ^α-1 |e _v | ^ε sign(e _v ), sign() is the sign function, Among them, δ ₀ is a positive number less than 1, a is a positive number, and p is a positive integer greater than 1. 4. The differential AGV trajectory tracking method based on actuator anti-saturation control according to claim 3, characterized in that the constrained wheel torque τ _u is obtained based on a limiting control module, and the limiting control module is based on a Gaussian error function. design: in, τ _max and τ _min are the upper and lower limits of the wheel torque actuator; Gaussian error function 5. The differential AGV trajectory tracking method based on actuator anti-saturation control according to claim 3, characterized in that the kinematic controller is designed using a back-stepping method, and the expression is: Among them, e _x , e _y , e _θ are respectively the pose deviation e _p obtained based on the actual pose [x, y, θ] ^T and the ideal pose [x _r , y _r , θ _r ] ^T in the carrier coordinate system. Medium element: Among them, x and y are respectively the horizontal and vertical coordinates of the body in the carrier coordinate system, θ is the angle between the carrier coordinate system and the navigation coordinate system; the subscript r represents the ideal situation; k ₁ , k ₂ , k ₃ are motion The parameters of the learning controller are all positive numbers. 6. The differential AGV trajectory tracking method based on actuator anti-saturation control according to claim 3, characterized in that a nonlinear expansion observer is used to estimate the total set disturbance d, including: Introduce the expansion state vector [x ₁₂ , x ₂₂ ] ^T = [d ₁ , d ₂ ] ^T , define x ₁₁ = v, x ₂₁ = w, expand the expression of the dynamic model, and construct a nonlinear expansion observer: Among them, z _1i and z _2i are the observer values of states x _1i and x _2i , β _1i and β _2i represent the observer gains, and the subscript i=1,2; The nonlinear function fal(·) is: Among them, σ>0, α ₁ =0.5, α ₂ =0.25, subscript i = 1, 2; about have: Among them, h _i (i=1,2) is the change rate of di ( _i =1,2); The parameters β _1i and β _2i are determined through pole configuration, and then the nonlinear expansion observer is used to estimate _di (i=1,2). 7. A differential AGV trajectory tracking system based on actuator anti-saturation control, characterized in that it is used to perform the differential AGV trajectory tracking method based on actuator anti-saturation control as described in any one of claims 1 to 6 .