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

Abstract

This invention relates to a differential AGV trajectory tracking method and system based on actuator anti-saturation control, comprising: acquiring a tracking trajectory; acquiring an ideal pose and ideal speed based on the tracking trajectory; determining the pose deviation based on the current actual pose and the ideal pose; using the pose deviation and ideal speed as inputs to a kinematic controller to obtain a control speed; using the control speed as input to a dynamics controller to obtain the original wheel torque; using the original wheel torque as input to a limiting control module to obtain a constrained wheel torque; the limiting control module ensures that the constrained wheel torque is within the torque actuator limit range; using the constrained wheel torque as input to a dynamics model to obtain a speed; using this speed as input to the kinematics model to obtain the actual pose at the next moment. The dynamics controller, based on torque limiting and anti-saturation compensation, can effectively track the control speed output by the kinematics controller even when the actuator is saturated.

Description

Technical Field

[0001] This invention relates to the field of AGV trajectory tracking motion control technology, and in particular to a differential AGV trajectory tracking method and system based on actuator anti-saturation control. Background Technology

[0002] Automated Guided Vehicle (AGV) systems are a crucial component of internal logistics, encompassing almost all production sectors, such as supply chains, the tobacco industry, aircraft manufacturing, warehouses, and container terminals. Differential AGVs, as an important member of the AGV family, are widely used in various applications. A differential AGV is a typical strongly coupled, nonlinear, and nonholonomic dynamic system. Therefore, the trajectory tracking control problem of differential AGVs is difficult to solve using methods from traditional linear system theory, making it extremely challenging. The reference trajectory in the trajectory tracking problem is a function dependent on time parameters, requiring simultaneous consideration of the AGV's longitudinal and lateral displacement errors. Through comprehensive control of the vehicle's longitudinal and lateral movements, the AGV must reach the corresponding reference trajectory point within a given time.

[0003] In existing technologies, trajectory tracking control methods for wheeled mobile robots (WMRs) have been studied by comprehensively considering kinematic and dynamic characteristics, improving the performance of the control system and the stability of the vehicle. However, due to factors such as inaccurate measurement, time-varying parameters, and input saturation disturbances, it is difficult to obtain an accurate mathematical model of the WMR system. To address this, active disturbance rejection (ADRP) trajectory tracking control schemes have emerged, such as using extended observers to achieve accurate estimation of disturbance quantities.

[0004] However, current control methods mainly focus on the stability and tracking accuracy of control systems, with most neglecting the saturation constraints of actuators. Since real-world systems are affected by actuator saturation, if only tracking performance indicators are considered during controller design while ignoring control input saturation, it is difficult to guarantee the stability of the closed-loop system in practical applications. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a differential AGV trajectory tracking method and system based on actuator anti-saturation control. The aim is to specifically address the actuator saturation problem, enabling the robot to effectively track the control speed output by the kinematic controller even when the actuator is saturated, thus ensuring the stability of the closed-loop system.

[0006] The technical solution adopted in this invention is as follows:

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

[0008] Acquire the tracking trajectory, and obtain the ideal pose and ideal velocity based on the tracking trajectory;

[0009] The pose deviation is calculated based on the current actual pose of the AGV and the ideal pose. The pose deviation and the ideal speed are used as inputs to the kinematic controller to obtain the control speed.

[0010] The control speed is used as the input to the dynamic controller to obtain the original wheel torque. The original wheel torque is used as the input to the limiting control module to obtain the constrained wheel torque. The limiting control module keeps the constrained wheel torque within the limit range of the torque actuator.

[0011] The constrained wheel torque is used as input to the dynamic model to obtain the AGV speed;

[0012] The AGV speed is used as input to the kinematic model to obtain the actual pose at the next moment;

[0013] in:

[0014] The kinematic model characterizes the relationship between AGV speed and pose, taking into account the influence of slippage disturbances;

[0015] The kinematic controller is based on a kinematic model, with pose as the control parameter and control velocity as the control variable.

[0016] The dynamic model characterizes the relationship between wheel torque and AGV speed, and the total disturbance term is obtained by concentrating all disturbances.

[0017] The dynamic controller is established based on the dynamic model, with control speed as the control parameter and wheel torque as the control quantity, and the total disturbance term is estimated through a nonlinear extended observer;

[0018] The input to the dynamics controller also includes a compensation amount output by an anti-saturation compensator, which is used to compensate for deviations in the dynamics controller when tracking the control speed. The anti-saturation compensator takes the difference between the original wheel torque and the constrained wheel torque as its input.

[0019] The further technical solution is as follows:

[0020] The dynamic model is established based on kinematic model and Lagrange mechanical analysis, and its expression is:

[0021] In the formula, η = [v, w] T Vehicle center speed angular velocity of the vehicle body These represent the angular velocities of the left and right wheels, respectively, and r is the radius of the drive wheel. That is, the vector of central acceleration and angular acceleration;

[0022] τ=[τ l ,τ r ] T , τ l τ r These are the torque values ​​for the left and right wheels, respectively.

[0023] aggregate perturbation Among them, bounded invertible matrices sum matrix This is the system nominal parameter matrix. and This represents the uncertainty in system parameters caused by load changes; ξ is the initial disturbance term.

[0024] The dynamic controller is designed using a sliding mode control method, combining amplitude limiting control and anti-saturation compensation. It employs a reaching law of the exponential superspiral sliding mode to satisfy the sliding mode conditions, and its control equations are as follows:

[0025]

[0026] In the formula, the control speed η c =[v c ,w c ] T The subscript 'c' represents control; The estimated value is obtained by estimating the ensemble perturbation d using a nonlinear extended observer; C = diag(c v ,c w ) is the coefficient, c v ,c w These are the coefficients corresponding to the vehicle's center velocity and angular velocity, respectively, both greater than 0; γ=[γ v ,γ w ] T As the compensation amount, γ v ,γ w These are the compensation amounts for the corresponding vehicle center velocity and angular velocity, respectively. Δτ=τ u -τ v , τ v ,τ u These represent the original wheel torque, the constrained wheel torque, and the speed tracking error vector, respectively.

[0027] λ, D, K1 are all related fractional integral sliding surfaces s = e v +λD α-1 |e v |ε sign(e v The coefficient of ), sign(·) is the sign function. Where δ0 is a positive number less than 1, a is a positive number, and p is a positive integer greater than 1.

[0028] Constrained wheel torque τ u This is obtained from a limiting control module, which is designed based on a Gaussian error function.

[0029]

[0030] in, τ max With τ min These are the upper and lower limits for the wheel torque actuator;

[0031] Gaussian error function

[0032] The kinematic controller is designed using the backstepping method, and its expression is:

[0033]

[0034] Among them, e x ,e y ,e θ These represent the actual pose [x, y, θ] in the carrier coordinate system. T With ideal pose [x r ,y r ,θ r ] T The obtained pose deviation e p Middle element:

[0035]

[0036] Where x and y are the horizontal and vertical coordinates of the vehicle body in the carrier coordinate system, respectively, and θ is the angle between the carrier coordinate system and the navigation coordinate system; the subscript r represents the ideal case; k1, k2, and k3 are the kinematic controller parameters, all of which are positive numbers.

[0037] The total perturbation d is estimated using a nonlinear extended observer, including:

[0038] Introducing the extended state vector [x] 12 ,x 22 ] T =[d1,d2] T Define x 11 =v,x 21 =w, expand the expression of the dynamic model to construct a nonlinear extended observer:

[0039]

[0040] Among them, z 1i , z 2i It is state x 1i x 2i The observer value, β 1i ,β 2i Indicates the observer gain, with subscripts i = 1, 2;

[0041] The nonlinear function fal(·) is:

[0042]

[0043] Where σ > 0, α1 = 0.5, α2 = 0.25, and subscript i = 1, 2;

[0044] about have:

[0045]

[0046] Among them, h i (i = 1, 2) is d i The rate of change of (i = 1, 2);

[0047] The parameter β is determined by pole placement. 1i ,β 2i Then, using a nonlinear extended observer to observe d i Estimate (i = 1, 2).

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

[0049] The beneficial effects of this invention are as follows:

[0050] This invention designs a kinematic model and controller, and based on this, a dynamic model and controller. Dynamic control is achieved using amplitude limiting control and anti-saturation compensation. Amplitude limiting control constrains the torque output of the dynamic controller, while anti-saturation compensation accelerates the control system's escape from saturation, preventing control system failure. This ensures that the AGV control performance does not significantly degrade when the actuator is saturated, and even in the event of actuator saturation, the robot can effectively track the virtual velocity (control velocity) of the kinematic controller.

[0051] This invention improves the accuracy of the model by proposing various disturbances during the motion model establishment process, providing accurate basic information for subsequent controller design.

[0052] Other features and advantages of the invention will be set forth in the following description or may be learned by practicing the invention. Attached Figure Description

[0053] Figure 1 This is a logic block diagram of the control system according to an embodiment of the present invention.

[0054] Figure 2 This is a schematic diagram of the geometric model of the differential AGV in an embodiment of the present invention.

[0055] Figure 3 The graph shows the x-axis error of circular trajectory tracking under this method and the conventional method, obtained for effect verification in the embodiments of the present invention.

[0056] Figure 4 The diagram shows the y-axis error of circular trajectory tracking under this method and the conventional method, obtained for effect verification in the embodiments of the present invention.

[0057] Figure 5 The diagram shows the angle error between the circular trajectory tracking method and the conventional method obtained from the effect verification in the embodiments of the present invention.

[0058] Figure 6 The graph shows the difference in center velocity between the method and the conventional method for tracking circular trajectories, obtained from the effect verification in the embodiments of the present invention.

[0059] Figure 7 The graph shows the difference in angular velocity between the method and the conventional method for tracking circular trajectories, obtained from the effect verification in the embodiments of the present invention.

[0060] Figure 8 The diagram shows the torque output of the left wheel under circular trajectory tracking, obtained from the effect verification of this method and the conventional method in the embodiments of the present invention.

[0061] Figure 9 The diagram shows the torque output of the right wheel under circular trajectory tracking, obtained from the effect verification of this method and the conventional method in the embodiments of the present invention.

[0062] Figure 10 This is a perturbation observation diagram of the method of this invention under circular trajectory tracking. Detailed Implementation

[0063] The specific embodiments of the present invention are described below with reference to the accompanying drawings.

[0064] This embodiment provides a differential AGV trajectory tracking method based on actuator anti-saturation control. Taking a QR code-guided differential AGV as the object, it considers the control input saturation problem and designs an anti-saturation auxiliary system to handle the actuator saturation problem, accelerate the actuator's exit from the saturation state, and ensure the stability of the closed-loop system in practical applications.

[0065] See Figure 2The geometric model of the differential AGV shown is a chassis system consisting of two drive wheels (left and right) and four omnidirectional wheels. It is assumed that the geometric center, center of gravity, and center of gravity of the AGV coincide with the center of the line connecting the two drive wheels. The diameter of the drive wheel is 2r, and the wheel spacing is 2b. In the figure, XY is the navigation coordinate system, xy is the carrier coordinate system, and θ is the angle between the carrier coordinate system and the navigation coordinate system.

[0066] See Figure 1 The method in this embodiment includes:

[0067] Obtain the tracking trajectory, and obtain the ideal pose based on the tracking trajectory [x] r ,y r ,θ r ] T and ideal velocity η r =[v r ,w r ] T ;

[0068] Based on the AGV's current actual pose [x, y, θ] T With the ideal pose [x] r ,y r ,θ r ] T The pose deviation [x] is obtained e ,y e ,θ e ] T The pose deviation [x] e ,y e ,θ e ] T and the ideal velocity η r =[v r ,w r ] T As input to the kinematic controller, the control velocity η is obtained. c =[v c ,w c ] T ;

[0069] The control speed η c =[v c ,w c ] T As input to the dynamics controller, the original wheel torque τ is obtained. v The original wheel torque τ v As input to the amplitude limiting control module, the constrained wheel torque τ is obtained. u The amplitude limiting control module constrains the wheel torque τ u Within the torque actuator limit range;

[0070] The constrained wheel torque τ u As input to the dynamic model, the AGV speed η = [v, w] is obtained. T ;

[0071] The AGV speed η = [v, w] T As input to the kinematic model, the actual pose [x, y, θ] at the next moment is obtained. T ;

[0072] in:

[0073] The kinematic model characterizes the relationship between AGV speed and pose, taking into account the influence of slippage disturbances;

[0074] The kinematic controller is based on a kinematic model, with pose as the control parameter and control velocity as the control variable.

[0075] The dynamic model characterizes the relationship between wheel torque and AGV speed, and the total disturbance term is obtained by concentrating all disturbances.

[0076] The dynamic controller is established based on the dynamic model, with control speed as the control parameter and wheel torque as the control quantity, and the total disturbance term is estimated through a nonlinear extended observer;

[0077] The input to the dynamics controller also includes a compensation amount output by an anti-saturation compensator, which is used to compensate for deviations in the dynamics controller when tracking the control speed. The anti-saturation compensator takes the difference between the original wheel torque and the constrained wheel torque as its input.

[0078] The following details the kinematic model, dynamic model, kinematic controller, and construction methods and ideas for the dynamic controller used in this embodiment. The meanings of the symbols in the above expressions will be explained in detail later.

[0079] I. Construction of the kinematic model:

[0080] Slippage is an unavoidable phenomenon in differential drives, especially during AGV startup. Therefore, slippage disturbances are introduced into the established kinematic model, with the following constraint equations:

[0081]

[0082] In equation (1), x and y are the horizontal and vertical coordinates of the vehicle body, respectively, and u refers to the lateral slip disturbance, which is a constantly changing variable. Let be the velocities along the x-axis and y-axis, respectively; b be half the wheel spacing; r be the radius of the drive wheel; and ζ be the velocities along the y-axis. l ζ r These represent the angular velocity disturbances of the left and right wheels, respectively. θ represents the angular velocities of the left and right wheels, respectively, and θ is the angle between the carrier coordinate system and the navigation coordinate system.

[0083] Equation (1) can be constructed as follows:

[0084]

[0085]

[0086] In formula (2):

[0087]

[0088] Λ=[u,-rζ l ,-rζ r ] T (4)

[0089]

[0090] make

[0091]

[0092] We can obtain A(q)J(q)=0(7), and combining equation (7) with equation (2), we can get:

[0093]

[0094]

[0095] Where: η = [v, w] T , v represents the speed at the center of the vehicle. w represents the angular velocity of the vehicle body. These are the angular velocities of the left and right wheels, respectively. Indicates the longitudinal slip disturbance of the vehicle body Indicates the angular velocity disturbance of the vehicle body Disturbance vector φ(q,u)=[-u sinθ,ucosθ,0,ζ l ,ζ r ] T ;

[0096] ζ l With ζ r Since q is not the object of interest in position and attitude tracking control, it can be simplified to q = [x, y, θ]. T The kinematic model of the AGV is then:

[0097]

[0098] All variables in equation (10) reside in the navigation coordinate system.

[0099] As can be seen from the kinematic model construction process, 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 that the focus is on the values ​​x, y and angle values.

[0100] II. Construction of the dynamic model:

[0101] Compared to kinematic models, dynamic models consider how to make the model move from a mechanical perspective. They are the precursors to kinematic models. The input of a dynamic model is torque, and the output is velocity. The input of a kinematic model is the velocity output by its dynamic model, and the output is pose.

[0102] The dynamic model is established based on kinematic models and Lagrange mechanical analysis, and the specific construction process is as follows:

[0103] According to the analysis of Lagrange mechanics, the dynamic equation of a nonholonomic control system is expressed as:

[0104]

[0105] In equation (11), M(q) is the positive definite inertia matrix. For the centripetal force and Coriolis force matrix, For the unknown ground friction term, G(q) is the gravity vector, B(q) is the input transformation matrix, and τ is the input vector. In this embodiment, τ = [τ l ,τ r ] T , τ l τ r The torques of the left and right wheels are respectively, A T (q) is a nonholonomic constraint matrix, τ d λ represents the input vector interference, and λ is the Lagrange multiplier.

[0106] Assuming the AGV operates on a horizontal plane, then G(q) = 0. Combining equation (7), we can rewrite equation (11), and by substituting and simplifying equations (8) and (9), we can obtain:

[0107]

[0108] In equation (12),

[0109]

[0110]

[0111] m = m c+2m w ,

[0112] I = I c +2m w b 2 +2I m I c =m c b 2 , τ=[τ l ,τ r ] T (17)

[0113] Wherein, equation (14) represents the initial disturbance, m is the total mass, and m c For the main mass, m w Let b be the mass of the drive wheel, and b be half the wheel spacing (see...). Figure 2 ), r is the radius of the drive wheel, I is the moment of inertia of the mobile robot about its geometric center, I c I m and I w These represent the moments of inertia of the vehicle body (including the load) about the vertical axis of the center of gravity, the moments of inertia of the drive wheels about the vertical axis of the center of gravity, and the moments of inertia of the drive wheels about the wheel axis, respectively. τ l τ r These are the torque inputs for the left and right wheels, respectively.

[0114] Considering that the actual robot is loaded, its mass and moment of inertia will change, as well as the influence of external disturbances, an error term will appear in equation (13). Therefore, equation (12) can be rewritten as:

[0115]

[0116] In equation (18), the bounded invertible matrix sum matrix This is the system nominal parameter matrix. and This indicates that load changes cause uncertainty in system parameters;

[0117] Separating the nominal parameter from the uncertain parameter, equation (18) is rewritten as:

[0118]

[0119] In equation (19), Representing the aggregate perturbation, (19) is further simplified to obtain the final expression for the dynamic model:

[0120]

[0121] In equation (20), That is, the vector of central acceleration and angular acceleration.

[0122] III. Kinematic Controller Design:

[0123] The purpose of a kinematic controller is to enable the actual pose to track the target pose within a finite time. Based on the kinematic model described above, a kinematic controller is designed using the backstepping method:

[0124]

[0125] Equation (21), e x ,e y ,e θ These represent the actual pose [x, y, θ] in the carrier coordinate system. T With ideal pose [x r ,y r ,θ r ] T The difference is the pose deviation vector e p Middle element:

[0126]

[0127] In equation (22), the subscript r represents the ideal situation; k1, k2, and k3 are kinematic controller parameters, all of which are positive numbers.

[0128] According to the Lyapunov stability theory, the kinematic controller designed in this embodiment can bring the tracking error to zero.

[0129] IV. Power Controller Design:

[0130] According to the kinematic controller, a mobile robot can track a desired trajectory by running at the designed speed. However, in actual systems, robots often cannot run at the designed speed. Since the actuators may saturate during AGV movement, this embodiment uses amplitude limiting control to constrain the torque output of the dynamic controller and employs anti-saturation compensation to accelerate the control system out of saturation, avoiding control system failure. This ensures that the AGV control performance does not significantly degrade when the actuators saturate, and even in the case of actuator saturation, it ensures that the robot can effectively track the virtual speed (control speed) of the kinematic controller.

[0131] The amplitude limiting control module based on the smooth input-output characteristics of the Gaussian error function is as follows:

[0132]

[0133] In equation (23), τ max With τ minThe upper and lower limits of the wheel torque actuator; Gaussian error function.

[0134] The anti-saturation compensator is designed as follows:

[0135] In equation (24), C = diag(c v ,c w ) is the coefficient, c v ,c w These are the coefficients corresponding to the vehicle's center velocity and angular velocity, respectively, both greater than 0, Δτ=τ u -τ v , τ v ,τ u These are the original wheel torque and the constrained wheel torque, respectively, γ = [γ v ,γ w ] T As the compensation amount, γ v ,γ w These are the compensation amounts for the corresponding vehicle center velocity and angular velocity, respectively.

[0136] The compensation amount γ = [γ v ,γ w ] T The deviation of the dynamic controller in tracking the controlled speed is compensated to obtain the speed tracking error vector:

[0137]

[0138] As can be seen from the above, the actual operating speed η = [v, w] T Control speed η c =[v c ,w c ] T The subscript 'c' represents control;

[0139] Based on fractional-order theory, the following fractional-order integral sliding surface is designed:

[0140] s = [s1, s2] T =e v +λD α-1 |e v | ε sign(e v (25)

[0141] Among them, λ = diag (λ1, λ2), λ1, λ2 are both greater than 0, where 0<α<1, 0<ε<1.

[0142] Differentiating equation (25) yields: Introducing equations (20) and (24), we get:

[0143]

[0144] To satisfy the sliding mode conditions, a reaching law for the exponential superspiral sliding mode is adopted, the expression of which is:

[0145]

[0146] Where K1 = diag(k 11 ,k 21 ), K2 = diag(k 12 ,k 22 ), s = diag(s1, s2), k ij For positive numbers, sign(·) is the sign function. Where δ0 is a positive number less than 1, a is a positive number, and p is a positive integer greater than 1;

[0147] Thus, we can obtain the dynamic controller designed using the sliding diaphragm control method, which combines amplitude limiting control and anti-saturation compensation:

[0148]

[0149] In equation (28), The estimated value is obtained by estimating the ensemble perturbation d using a nonlinear extended observer;

[0150] Specifically, a nonlinear extended observer is used to estimate the total perturbation d, including:

[0151] Introducing the extended state vector [x] 12 ,x 22 ] T =[d1,d2] T Define x 11 =v,x 21 =w, expand the expression of the dynamic model to construct a nonlinear extended observer:

[0152]

[0153] Among them, z 1i , z 2i It is state x 1i x 2i The observer value, β 1i ,β 2i Indicates the observer gain, with subscripts i = 1, 2;

[0154] The nonlinear function fal(·) is:

[0155]

[0156] Where σ > 0, α1 = 0.5, α2 = 0.25, and subscript i = 1, 2;

[0157] about have:

[0158]

[0159] Among them, h i (i = 1, 2) is d i Rate of change of (i = 1, 2).

[0160] Specifically, the parameter β in (29) is determined by pole placement. 1i ,β 2i Then, using a nonlinear extended observer to observe d i The estimation is performed for (i=1,2). Those skilled in the art will understand that the pole placement method is a conventional method, so the specific calculation process will not be described in detail.

[0161] To mitigate external disturbances and parameter uncertainties, an extended state observer can be used to estimate disturbance information and compensate for uncertainties in the dynamic controller, thereby reducing chattering in the control system.

[0162] In summary, the method in this embodiment addresses the slippage disturbance problem in differential AGVs by establishing a kinematic model based on slippage disturbance. Traditional dynamic models contain numerous uncertainties and cannot provide precise data, including variations in load and friction. These uncertainties are aggregated into a disturbance term for unified processing. A nonlinear extended state observer is used to estimate the aggregated disturbance term and compensate it into the dynamic model, ensuring the accuracy of the control model. For dynamic control, an anti-saturation compensator is designed to prevent tracking failure caused by the actuator entering a saturation state.

[0163] To verify the effectiveness and superiority of the fractional sliding mode control (FOSMC+SAT) method based on the exponential superspiral reaching law of anti-saturation control in this embodiment, the trajectory tracking of the AGV was simulated using the Simulink tool in MATLAB software, and compared with the ordinary fractional sliding mode control (FOSMC) control system (method) based on the exponential superspiral reaching law.

[0164] The actual physical parameters of the differential AGV model are: m = 70 kg, m0 = 50 kg, b = 0.3 m, r = 0.08 m, I w =0.144kgcm 2 τ = 0.64 Nm, i = 20. Where m is the robot's mass, m0 is the load capacity, b is the wheelbase between the two drive wheels, r is the radius of the drive wheels, and I... w Let τ be the moment of inertia of the drive wheel, τ be the motor torque, and i be the reduction ratio.

[0165] The parameter C in the saturation compensator is (c v ,c w )=(10,10), the parameters of the extended state observer vary with l ij The adjusted, ensemble perturbation design is as follows:

[0166] To verify the algorithm's performance, a commonly used verification trajectory (circular trajectory) is first used. The trajectory tracking formula is as follows:

[0167]

[0168] Therefore, the reference speed of this trajectory is v. r =2m / s, w r =1 rad / s, actual initial velocity is v = 0 m / s, w = 0 rad / s. Desired initial pose is (x r ,y r ,θ r Given (2, 0, 90), the actual initial pose considering the initial pose error is (x, y, θ) = (1.8, -0.2, 89). Considering the initial slip setting t < 3, [ψ1, ψ2] T =[5,0.5] T When 5 ≤ t,

[0169] [ψ1,ψ2] T =[0,0] T When 3 ≤ t < 5, [ψ1, ψ2] T =[5+10*sin(5*t),0.5+sin(5*t)] T The parameters of the circular trajectory tracking controller are shown in Table 1.

[0170] Table 1 Circular Tracking Configuration Parameters

[0171]

[0172] Simulations were performed based on the parameters in Table 1 above, and the simulation results for tracking the difference are as follows: Figures 3 to 5 As shown in the figure, under the condition of strictly controlled parameters, both the proposed method (FOSMC+SAT) and the conventional method (FOSMC) can track the trajectory within a finite time.

[0173] In addition to the overall trajectory tracking performance, the observed indicators also include the velocity curve and angular velocity curve. Figure 6 and Figure 7 A graph showing the difference between the actual speed and the target speed was displayed. Figure 6 , Figure 7These represent the difference in vehicle center velocity and the difference in angular velocity, respectively. As shown in the figure, the FOSMC+SAT method in this embodiment exhibits excellent performance in terms of both center velocity and angular velocity, with fast response, small overshoot, rapid convergence, and smooth changes.

[0174] This embodiment takes into account the influence of input saturation constraints; therefore, torque output is also used as an evaluation index. Figure 8 , Figure 9 The figure shows the torque output of the left and right wheels respectively, which are tracked by circular trajectory. As can be seen from the figure, the FOSMC+SAT method in this embodiment shows higher response performance in the torque output curve, and has a smoother control curve, which is more in line with the smoothness and stability of actual control.

[0175] like Figure 10 As shown in the figure, NESO represents the estimate of the input disturbance value Input by the nonlinear extended observer. As can be seen from the figure, the observation of the disturbance is very accurate.

[0176] In summary, it can be demonstrated that the differential AGV trajectory tracking method based on actuator anti-saturation control designed using the FOSMC+SAT method in this embodiment has excellent performance in both trajectory tracking and speed tracking.

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

[0178] It will be understood by those skilled in the art that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A differential AGV trajectory tracking method based on actuator anti-saturation control, characterized in that, include: Acquire the tracking trajectory, and obtain the ideal pose and ideal velocity based on the tracking trajectory; The pose deviation is calculated based on the current actual pose of the AGV and the ideal pose. 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 to the dynamic controller to obtain the original wheel torque. The original wheel torque is used as the input to the limiting control module to obtain the constrained wheel torque. The limiting control module keeps the constrained wheel torque within the limit range of the torque actuator. The constrained wheel torque is used as input to the dynamic model to obtain the AGV speed; The AGV speed is used as input to the kinematic model to obtain the actual pose at the next moment; in: The kinematic model characterizes the relationship between AGV speed and pose, taking into account the influence of slippage disturbances; The kinematic controller is based on a kinematic model, with pose as the control parameter and control velocity as the control variable. The dynamic model characterizes the relationship between wheel torque and AGV speed, and the total disturbance term is obtained by concentrating all disturbances. The dynamic controller is established based on the dynamic model, with control speed as the control parameter and wheel torque as the control quantity, and the total disturbance term is estimated through a nonlinear extended observer. The input to the dynamics controller also includes a compensation amount output by an anti-saturation compensator, which is used to compensate for deviations in the dynamics controller when tracking the control speed. The anti-saturation compensator takes the difference between the original wheel torque and the constrained wheel torque as its input. The dynamic model is established based on kinematic model and Lagrange mechanical analysis, and its expression is: In the formula, Vehicle center speed angular velocity of the vehicle body , , These are the angular velocities of the left and right wheels, respectively. The radius of the drive wheel, That is, the vector of central acceleration and angular acceleration; , , , These are the torque values ​​for the left and right wheels, respectively. aggregate perturbation , , Among them, bounded invertible matrices sum matrix This is the system nominal parameter matrix. and This indicates that load changes cause uncertainty in system parameters; This is the initial disturbance term; The dynamic controller is designed using sliding mode control, combining amplitude limiting control and anti-saturation compensation. It employs a reaching law of exponential superspiral sliding mode to satisfy the sliding mode conditions, and its control equations are as follows: In the formula, the speed is controlled. subscript c Represents control; To measure the ensemble perturbation using a nonlinear extended observer The estimated value obtained; For coefficients, These are the coefficients for the corresponding vehicle center velocity and angular velocity, respectively, both of which are greater than 0; For compensation amount, These are the compensation amounts for the corresponding vehicle center velocity and angular velocity, respectively. , , These represent the original wheel torque, the constrained wheel torque, and the speed tracking error vector, respectively. ; All are related to fractional integral sliding surfaces coefficient, For symbolic functions, ,in For positive numbers less than 1 It is a positive number. It is a positive integer greater than 1.

2. The differential AGV trajectory tracking method based on actuator anti-saturation control according to claim 1, characterized in that, Constrained wheel torque This is obtained from a limiting control module, which is designed based on a Gaussian error function. in, , and These are the upper and lower limits for the wheel torque actuator; Gaussian error function .

3. The differential AGV trajectory tracking method based on actuator anti-saturation control according to claim 1, characterized in that, The kinematic controller is designed using the backstepping method, and its expression is: in, Based on the actual pose in the carrier coordinate system and ideal position The obtained pose deviation Middle element: in, These are the horizontal and vertical coordinates of the vehicle body in the carrier coordinate system. The angle between the carrier coordinate system and the navigation coordinate system; the subscript r represents the ideal case. , , These are the parameters of the kinematic controller, all of which are positive numbers.

4. The differential AGV trajectory tracking method based on actuator anti-saturation control according to claim 1, characterized in that, Using a nonlinear extended observer to study lumped perturbations To make an estimate, including: Introducing the extended state vector ,definition , The expression of the dynamic model is extended to construct a nonlinear extended observer: in, , It is a state , The observer value, , Indicates observer gain, subscript i =1,2; nonlinear functions for: in, , , subscript i =1,2; about , have: in, yes The rate of change; Parameters are determined by pole placement. , Then, using a nonlinear extended observer to... Make an estimate.

5. A differential AGV trajectory tracking system based on actuator anti-saturation control, characterized in that, Used to perform the differential AGV trajectory tracking method based on actuator anti-saturation control as described in any one of claims 1 to 4.

Images