Robot trajectory control method and system based on PSO linear active disturbance rejection control

Abstract

The application discloses a robot trajectory control method and system based on PSO linear active disturbance rejection control, converts the dynamic equation of a mobile robot into multiple independent second-order integral series channels through coordinate transformation and input reconstruction, and obtains multiple decoupling channels; each decoupling channel is controlled by using the same LADRC controller; an independent three-order LESO is designed for each decoupling channel to estimate states and disturbances; a linear PD control law is used to generate a basic control amount, and the estimated total disturbance value is used to compensate in real time in the control amount; the bandwidths of the observer and the controller are taken as the positions of particles, the tracking error index and the control cost index are taken as the fitness function, and the optimal parameter group of the minimum value of the fitness function is searched through a particle swarm optimization algorithm. The application decouples the mobile robot, greatly simplifies the design and implementation complexity of the control system, and introduces the particle swarm optimization algorithm to accurately set key parameters.

Description

Technical Field

[0001] This invention belongs to the field of robot trajectory tracking technology. It relates to a robot trajectory control method and system based on PSO linear active disturbance rejection control. Background Technology

[0002] To meet the stringent requirements of dynamic response speed and robustness for high-precision trajectory tracking tasks, control systems must be capable of handling system uncertainties and nonlinear dynamics. Although existing technologies have completed kinematic and dynamic modeling and system parameter identification for omnidirectional mobile robots (MWMRs), MWMRs still face multiple complex challenges in actual operation: on the one hand, there are uncertainties within the system, such as unmodeled dynamics and parameter perturbations (e.g., time-varying moment of inertia and frictional nonlinearity); on the other hand, there are various disturbances in the external working environment, such as uneven ground and load variations. Traditional control strategies, such as PID control or linear quadratic regulators (LQR), due to their inherent linearity and limited disturbance suppression capabilities, often struggle to achieve ideal tracking performance and control robustness when dealing with the aforementioned strong nonlinearities and complex uncertainties. Therefore, designing a control algorithm that can effectively estimate and compensate for the total system disturbance while possessing both fast response and strong robustness has become a key scientific problem for improving the trajectory tracking accuracy of MWMRs.

[0003] Professor Gao Zhiqiang proposed Linear Active Disturbance Rejection Control (LADRC) in 2003. LADRC inherits the principle of ADRC, treating both unmodeled internal dynamics and external disturbances as a unified "total disturbance," estimating and compensating for them in real time using an Extended State Observer (ESO). However, by employing a Linear Extended State Observer (LESO) and a Linear State Error Feedback Control Law (LSEF), the parameter tuning process is significantly simplified. Nevertheless, since MWMR systems are redundant driven systems (four inputs corresponding to three degrees of freedom), directly designing a controller faces challenges related to input coupling and redundancy allocation. The control problem of MWMR systems is extremely complex, resulting in high complexity in the design and implementation of the control system, as well as the parameter tuning process. Summary of the Invention

[0004] The purpose of this invention is to provide a robot trajectory control method and system based on PSO linear active disturbance rejection control. This method transforms the complex dynamic equations of the mobile robot system into a second-order integral cascade form suitable for control, decouples the mobile robot system, and transforms the multi-input multi-output system into multiple independent single-input single-output systems. This greatly simplifies the design and implementation complexity of the control system, and finally introduces the particle swarm optimization algorithm to accurately tune the key parameters.

[0005] The technical solution to achieve the purpose of this invention is as follows:

[0006] A robot trajectory control method based on PSO linear active disturbance rejection control includes the following steps:

[0007] S01: The dynamic equations of the mobile robot are transformed into multiple independent second-order integral cascaded channels through coordinate transformation and input reconstruction, resulting in multiple decoupled channels;

[0008] S02: Each decoupled channel is controlled by the same LADRC controller;

[0009] S03: Design an independent third-order linear extended state observer for each decoupling channel to estimate the state and disturbances;

[0010] S04: The basic control quantity is generated by using a linear PD control law, and the estimated total disturbance value is used to perform real-time compensation in the control quantity.

[0011] S05: Using the observer bandwidth and controller bandwidth as the positions of particles in the particle swarm optimization algorithm, and the tracking error index and control cost index as the fitness function, the optimal parameter combination that minimizes the fitness function is searched through the particle swarm optimization algorithm.

[0012] In the preferred technical solution, the multiple decoupling channels obtained in step S01 include:

[0013] The dynamic equations of the mobile robot are:

[0014]

[0015] in, Output acceleration vector for robot dynamics. For the control gain matrix, For physical control input, It is a lumped disturbance term. This is the system disturbance vector;

[0016] By employing coordinate transformation and input reconstruction, the multi-input multi-output system is transformed into multiple independent single-input single-output systems. pseudo-reversal To design physical control input :

[0017]

[0018] Virtual control input As the control law output of linear active disturbance rejection control In the global coordinate system x , y , θ Virtual control inputs for three directional channels;

[0019] Will Substituting the dynamic equations, the mobile robot system is decoupled as follows:

[0020]

[0021] Among them, the new total disturbance ;

[0022] After the above decoupling process, the mobile robot system is transformed into three independent second-order integral cascaded channels:

[0023]

[0024] in, In the global coordinate system The robot's dynamics output acceleration vectors for the three channels; , , These represent the total disturbance of the three channels respectively.

[0025] In the preferred technical solution, an independent third-order linear extended state observer is designed for each decoupling channel to estimate the state and disturbances, including:

[0026] For each decoupling channel Design an independent third-order linear extended state observer to estimate the state. , Total channel disturbance ,aisle , , Channels Position and velocity state variables;

[0027] The third-order linear extended state observer is designed as follows:

[0028]

[0029] In the formula: It is a channel Location The estimate; It is a channel speed The estimate; It is a channel Total disturbance The estimate; , , These are the actual observer gain coefficients, Variables Estimates of velocity and acceleration, For disturbance An estimate of the derivative.

[0030] In the preferred technical solution, step S03 further includes:

[0031] Using linear error feedback term , j The poles are set to 1, 2, and 3, and the bandwidth method is used for gain configuration, placing all poles of the linearly extended state observer on the negative real axis. Among them For observer bandwidth:

[0032]

[0033] in, For the Laplace operator, , , These are the observer gain coefficients;

[0034] The analytical expression for the observer gain is obtained by comparing the coefficients:

[0035] .

[0036] In the preferred technical solution, a linear PD control law is used to generate the basic control quantity, and real-time compensation is performed on the control quantity using the estimated total disturbance value, including:

[0037] Design virtual control input To achieve tracking error Fast convergence is achieved by using a linear PD controller to drive the system and a state estimate from a linear extended state observer. and Provide feedback:

[0038]

[0039] in, and These are the reference signal and its differential signal, respectively. and These are the proportional and differential gain coefficients, respectively;

[0040] Feedback control law ;

[0041] Final virtual control input Combined with feedback control law Total disturbance estimate :

[0042]

[0043] The final physical control input applied to the motor for:

[0044] .

[0045] In the preferred technical solution, the proportional and differential gain coefficients , Based on the bandwidth of the linear PD controller Configure:

[0046]

[0047] The analytical expression for the controller gain is obtained by comparing the coefficients:

[0048]

[0049] The poles of the closed-loop system are configured at... At this point, a critical damping system is formed.

[0050] In the preferred technical solution, the bandwidth of the x-axis, y-axis, and θ-axis observers is... , , x-axis, y-axis, and θ-axis controller bandwidth , , Represented by a 6-dimensional vector, this vector is used as a particle. j Location :

[0051]

[0052] The LADRC parameter search space is a 6-dimensional hypercube space bounded by the range of values ​​for these parameters; the particle swarm optimization algorithm is used to search for the fitness function. Optimal parameter combination to achieve minimum value ;

[0053] Each particle j During the search process, location updates are achieved by tracking the following two key extreme values: 1) the individual's optimal location. 1) The parameter position that minimizes the fitness value found by particle j in each iteration; 2) The global optimal position. The parameter position that minimizes the fitness value found by the entire particle swarm in each iteration;

[0054] The velocity of particle j in the (k+1)th iteration and location Update according to the following formula:

[0055]

[0056]

[0057] in, It is the inertia factor. and It is a learning factor. , yes A random number between [a certain number of points].

[0058] In the preferred technical solution, the fitness function J ( X The goal is to minimize the weighted sum of various performance indicators. J ( X ):

[0059]

[0060] , , They are respectively Tracking error index of the shaft To control energy, , , and As weight;

[0061] The time-multiplied absolute error integral index is used for the three degrees of freedom. Quantify the tracking error:

[0062]

[0063] t Let T be the time, and T be the duration of the time. For degrees of freedom i Tracking error;

[0064] Controlling energy :

[0065]

[0066] in, Input signal to motor j.

[0067] This invention also discloses a robot trajectory control system based on PSO linear active disturbance rejection control, comprising:

[0068] The robot decoupling module transforms the dynamic equations of the mobile robot into multiple independent second-order integral cascaded channels through coordinate transformation and input reconstruction, thus obtaining multiple decoupling channels;

[0069] The LADRC control module controls each decoupled channel using the same LADRC controller;

[0070] A linear extended state observer is designed for each decoupled channel to estimate the state and disturbances.

[0071] The linear state error feedback control law module uses a linear PD control law to generate basic control quantities and uses the estimated total disturbance value to perform real-time compensation in the control quantities.

[0072] The LADRC parameter tuning module uses the observer bandwidth and controller bandwidth as the positions of particles in the particle swarm optimization algorithm, and the tracking error index and control cost index as the fitness function. It then uses the particle swarm optimization algorithm to search for the optimal parameter combination that minimizes the fitness function.

[0073] The present invention also discloses a computer storage medium storing a computer program, which, when executed, implements the above-described robot trajectory control method based on PSO linear active disturbance rejection control.

[0074] Compared with the prior art, the significant advantages of this invention are:

[0075] 1. This invention transforms the complex dynamic equations of a mobile robot system into a second-order integral cascade form suitable for control, decoupling the mobile robot system and converting a multi-input multi-output system into multiple independent single-input single-output systems, greatly simplifying the design and implementation complexity of the control system. Through the designed linear state error feedback control law, combined with LESO disturbance estimation and compensation, Linear Active Disturbance Rejection Control (LADRC) achieves effective control of the uncertain system, simplifying the complex mobile robot control problem into PD control of a standard integral cascade system, significantly improving the performance and practicality of the control system.

[0076] 2. To address the challenge of tuning LADRC parameters, a novel particle swarm optimization algorithm was introduced. A comprehensive fitness function, considering both tracking accuracy and control energy consumption, was constructed, and the optimal observer-controller bandwidth combination was searched through a six-dimensional parameter space. Optimization results show that PSO-LADRC significantly improves the stability and robustness of the control system while maintaining high tracking accuracy. MATLAB simulation experiments using trefoil and rectangular trajectories were conducted to compare and analyze the performance of PSO-LADRC with unoptimized LADRC and traditional PID control. The results demonstrate that PSO-LADRC exhibits the best performance in trajectory tracking accuracy, disturbance suppression capability, and control input smoothness, particularly demonstrating superior control performance in challenging scenarios such as curve curvature and right-angle turns. Simulation data confirms that the X-axis tracking error of PSO-LADRC is reduced by 34.8% compared to PID and by 13.2% compared to unoptimized LADRC, while also providing smoother control input and effectively suppressing high-frequency chattering. This study demonstrates that the LADRC parameter tuning strategy based on particle swarm optimization can fully leverage the potential of linear active disturbance rejection control, providing an effective solution for trajectory tracking control of mobile robots in complex dynamic environments, and possessing both theoretical value and promising engineering applications. Attached Figure Description

[0077] Figure 1 This is a flowchart of the robot trajectory control method based on PSO linear active disturbance rejection control in this embodiment;

[0078] Figure 2 The block diagram of a linear active disturbance rejection controller based on particle swarm optimization algorithm is shown.

[0079] Figure 3 Optimize the fitness curve for the six parameters of LADRC;

[0080] Figure 4 A schematic diagram illustrating the bandwidth optimization process for the observer and controller;

[0081] Figure 5 Optimize the fitness convergence curve for the six parameters of LADRC;

[0082] Figure 6 For the trajectory tracking and comparison of the three-leaf rose;

[0083] Figure 7 For the X-axis error of the three-leaf rose controller;

[0084] Figure 8 For the Y-axis error of the three-leaf rose controller;

[0085] Figure 9 This refers to the azimuth angle error of the three-leaf rose controller.

[0086] Figure 10Input for the PID controller of the three-leaf rose;

[0087] Figure 11 Input for the three-leaf rose LADRC controller;

[0088] Figure 12 Input for the Three-Leaf Rose PSO-LADRC controller;

[0089] Figure 13 For rectangular trajectory tracking comparison;

[0090] Figure 14 For the X-axis error of the rectangular controller;

[0091] Figure 15 For the Y-axis error of the rectangular controller;

[0092] Figure 16 This refers to the orientation angle error of the rectangular controller.

[0093] Figure 17 For input to a rectangular PID controller;

[0094] Figure 18 For input to the rectangular LADRC controller;

[0095] Figure 19 For inputs to the rectangular PSO-LADRC controller. Detailed Implementation

[0096] The principle of this invention is as follows: This invention transforms the complex dynamic equations of a mobile robot system into a second-order integral cascade form suitable for control, decoupling the mobile robot system and converting a multi-input multi-output system into multiple independent single-input single-output systems, greatly simplifying the design and implementation complexity of the control system. Addressing the difficulty of LADRC parameter tuning, a particle swarm optimization algorithm is innovatively introduced to construct a comprehensive fitness function that considers both tracking accuracy and control energy consumption. The optimal observer and controller bandwidth combination is searched through a six-dimensional parameter space. PSO-LADRC significantly improves the stability and robustness of the control system while maintaining high tracking accuracy.

[0097] Example 1:

[0098] like Figure 1 As shown, a robot trajectory control method based on PSO linear active disturbance rejection control includes the following steps:

[0099] S01: The dynamic equations of the mobile robot are transformed into multiple independent second-order integral cascaded channels through coordinate transformation and input reconstruction, resulting in multiple decoupled channels;

[0100] S02: Each decoupled channel is controlled by the same LADRC controller;

[0101] S03: Design an independent third-order linear extended state observer for each decoupling channel to estimate the state and disturbances;

[0102] S04: The basic control quantity is generated by using a linear PD control law, and the estimated total disturbance value is used to perform real-time compensation in the control quantity.

[0103] S05: Using the observer bandwidth and controller bandwidth as the positions of particles in the particle swarm optimization algorithm, and the tracking error index and control cost index as the fitness function, the optimal parameter combination that minimizes the fitness function is searched through the particle swarm optimization algorithm.

[0104] like Figure 2 The LADRC block diagram shown illustrates that this controller mainly consists of the following core components: 1) Tracking Differentiator (TD): As the pre-processing module of LADRC, the TD's main function is to smooth and extract the differentiation of the given reference signal. It provides the control system with a noise-free differential signal while avoiding system shocks caused by sudden changes in the reference signal. By arranging a suitable transient response, the TD can effectively resolve the contradiction between system speed and overshoot. 2) Linear Extended State Observer (LESO): As a core component of LADRC, LESO estimates not only the system's normal states (such as position and velocity) but also specifically estimates the system's "total disturbance" by extending the state variables. This observer adopts a linear feedback structure, adjusting the balance between estimated velocity and noise immunity by configuring the observer bandwidth. 3) Linear State Error Feedback Control Law (LSEF): Based on the reference signal processed by the TD and the state estimates provided by the LESO, a linear PD control law is used to generate the basic control quantity. This part is responsible for achieving fast system response and accurate tracking, adjusting the system's dynamic performance through the controller bandwidth parameter. 4) Disturbance compensation mechanism: The total disturbance value estimated by LESO is used to compensate in real time in the control variable, thereby offsetting the effects of disturbances inside and outside the system. This feedforward compensation mechanism makes the control system highly robust to disturbances.

[0105] For omnidirectional mobile robots, a LADRC algorithm is designed based on a simplified robot model. Utilizing the decoupling characteristics of the MWMR system, the multiple-input multiple-output (MIMO) system is transformed into three independent single-input single-output (SISO) LADRC channels. To address the difficulty of LADRC parameter tuning, a particle swarm optimization (PSO) algorithm is innovatively introduced to construct a comprehensive fitness function that considers both tracking accuracy and control energy consumption. The optimal observer and controller bandwidth combination is searched through a six-dimensional parameter space. Optimization results show that PSO-LADRC significantly improves the stability and robustness of the control system while maintaining high tracking accuracy.

[0106] In a preferred embodiment, the multiple decoupling channels obtained in step S01 include:

[0107] The dynamic equations of the mobile robot are:

[0108]

[0109] in, Output acceleration vector for robot dynamics; For control gain matrix; , The radius of the driving wheel; The nominal moment of inertia of the robot; The transformation matrix for the robot coordinate system variables. For physical control input, It is a lumped disturbance term. This is the system disturbance vector;

[0110] By employing coordinate transformation and input reconstruction, the multi-input multi-output system is transformed into multiple independent single-input single-output systems. pseudo-reversal To design physical control input :

[0111]

[0112] Virtual control input As the control law output of linear active disturbance rejection control In the global coordinate system x , y , θ Virtual control inputs for three directional channels;

[0113] Will Substituting the dynamic equations, the mobile robot system is decoupled as follows:

[0114]

[0115] Among them, the new total disturbance ;

[0116] After the above decoupling process, the mobile robot system is transformed into three independent second-order integral cascaded channels:

[0117]

[0118] in, In the global coordinate system The robot's dynamics output acceleration vectors for the three channels; , , These represent the total disturbance of the three channels respectively.

[0119] In a preferred embodiment, designing an independent third-order linear extended state observer for each decoupled channel to estimate the state and disturbances includes:

[0120] For each decoupling channel Design an independent third-order linear extended state observer to estimate the state. , Total channel disturbance ,aisle , , Channels Position and velocity state variables;

[0121] The third-order linear extended state observer is designed as follows:

[0122]

[0123] In the formula: It is a channel Location The estimate; It is a channel speed The estimate; It is a channel Total disturbance The estimate; , , These are the actual observer gain coefficients, Variables Estimates of velocity and acceleration, For disturbance An estimate of the derivative.

[0124] In a preferred embodiment, step S03 further includes:

[0125] Using linear error feedback term , j The poles are set to 1, 2, and 3, and the bandwidth method is used for gain configuration, placing all poles of the linearly extended state observer on the negative real axis. Among them For observer bandwidth:

[0126]

[0127] The analytical expression for the observer gain is obtained by comparing the coefficients:

[0128] .

[0129] in, For the Laplace operator, , , These are the observer gain coefficients;

[0130] In a preferred embodiment, a linear PD control law is used to generate a basic control quantity, and real-time compensation is performed on the control quantity using the estimated total disturbance value, including:

[0131] Design virtual control input To achieve tracking error Fast convergence is achieved by using a linear PD controller to drive the system and a state estimate from a linear extended state observer. and Provide feedback:

[0132]

[0133] in, and These are the reference signal and its differential signal, respectively. and These are the proportional and differential gain coefficients, respectively;

[0134] Feedback control law ;

[0135] Final virtual control input Combined with feedback control law Total disturbance estimate :

[0136]

[0137] The final physical control input applied to the motor for:

[0138] .

[0139] In a preferred embodiment, the scaling and differential gain coefficients , Based on the bandwidth of the linear PD controller Configure:

[0140]

[0141] The analytical expression for the controller gain is obtained by comparing the coefficients:

[0142]

[0143] The poles of the closed-loop system are configured at... At this point, a critical damping system is formed.

[0144] In a preferred embodiment, the bandwidth of the x-axis, y-axis, and θ-axis observers is... , , x-axis, y-axis, and θ-axis controller bandwidth , , Represented by a 6-dimensional vector, this vector is used as a particle. j Location :

[0145]

[0146] The LADRC parameter search space is a 6-dimensional hypercube space bounded by the range of values ​​for these parameters; the particle swarm optimization algorithm is used to search for the fitness function. Optimal parameter combination to achieve minimum value ;

[0147] Each particle j During the search process, location updates are achieved by tracking the following two key extreme values: 1) the individual's optimal location. 1) The parameter position that minimizes the fitness value found by particle j in each iteration; 2) The global optimal position. The parameter position that minimizes the fitness value found by the entire particle swarm in each iteration;

[0148] The velocity of particle j in the (k+1)th iteration and location Update according to the following formula:

[0149]

[0150]

[0151] in, It is the inertia factor. and It is a learning factor. , yes A random number between [a certain number of points].

[0152] A preferred embodiment, fitness function J ( X The goal is to minimize the weighted sum of various performance indicators. J ( X ):

[0153]

[0154] , , They are respectively Tracking error index of the shaft To control energy, , , and As weight;

[0155] The time-multiplied absolute error integral index is used for the three degrees of freedom. Quantify the tracking error:

[0156]

[0157] t Let T be the time, and T be the duration of the time. For degrees of freedom i Tracking error;

[0158] Controlling energy :

[0159]

[0160] in, Input signal to motor j.

[0161] In another embodiment, a computer storage medium stores a computer program that, when executed, implements the above-described robot trajectory control method based on PSO linear active disturbance rejection control.

[0162] The above implementation method will not be elaborated further here.

[0163] Another embodiment, a robot trajectory control system based on PSO linear active disturbance rejection control, includes:

[0164] The robot decoupling module transforms the dynamic equations of the mobile robot into multiple independent second-order integral cascaded channels through coordinate transformation and input reconstruction, thus obtaining multiple decoupling channels;

[0165] The LADRC control module controls each decoupled channel using the same LADRC controller;

[0166] A linear extended state observer is designed for each decoupled channel to estimate the state and disturbances.

[0167] The linear state error feedback control law module uses a linear PD control law to generate basic control quantities and uses the estimated total disturbance value to perform real-time compensation in the control quantities.

[0168] The LADRC parameter tuning module uses the observer bandwidth and controller bandwidth as the positions of particles in the particle swarm optimization algorithm, and the tracking error index and control cost index as the fitness function. It then uses the particle swarm optimization algorithm to search for the optimal parameter combination that minimizes the fitness function.

[0169] Specifically, the design process of a robot trajectory control system based on PSO linear active disturbance rejection control is illustrated below using a preferred embodiment as an example:

[0170] System Model Analysis and Decoupling

[0171] The MWMR dynamic model is a redundant drive system with three degrees of freedom and four inputs. To apply LADRC, the system first needs to be transformed from complex dynamic equations into a second-order integral cascade form suitable for control. The MWMR dynamic model is as follows:

[0172] (6.1)

[0173] in, yes The input matrix, It is a lumped disturbance term.

[0174] Since the MIMO system is a redundant drive system (four inputs corresponding to three degrees of freedom), directly designing the controller faces challenges related to input coupling and redundancy allocation. To achieve independent channel control, system decoupling is necessary. The core idea of ​​decoupling is to transform the MIMO system into multiple independent SISO systems through coordinate transformation and input reconstruction. pseudo-reversal To design physical control input Virtual control input It will be used as the control law output of LADRC.

[0175] (6.2)

[0176] in The specific expression is:

[0177] (6.3)

[0178] Will Substituting into the original system, the system is decoupled as follows:

[0179] (6.4)

[0180] Among them, the new total disturbance .

[0181] After the above decoupling process, the original system is transformed into three independent second-order integral cascaded channels:

[0182] (6.5)

[0183] in, , , These represent the total disturbances of the three channels, including coupling terms, nonlinear terms, and external disturbances in the original system. Each channel has a standard second-order integral cascaded form, laying a good foundation for the application of LADRC.

[0184] The pseudo-inverse computation introduced during decoupling alters the distribution of disturbance terms, but LADRC's disturbance estimation and compensation mechanisms effectively handle this change, ensuring the system's robust performance. This decoupling process lays a solid foundation for subsequent LESO design and LADRC controller implementation. Each decoupled channel can be considered an independent second-order system, controlled using the same LADRC structure, greatly simplifying the design and implementation complexity of the control system.

[0185] Linear Extended State Observer (LESO) Design

[0186] For each decoupling channel An independent third-order LESO algorithm can be designed to estimate the state. , Total channel disturbance Taking any channel as an example Its state space is .

[0187] The third-order LESO design is as follows:

[0188] (6.6)

[0189] In the formula: It is a channel Location The estimate; It is a channel speed The estimate; It is a channel Total disturbance Estimate ; , , The observer gain determines the convergence speed and estimation accuracy of LESO.

[0190] To simplify parameter tuning and ensure that LESO has a sufficiently fast convergence speed, a linear error feedback term is used. Gain configuration was performed using the bandwidth method. All poles of LESO were configured on the negative real axis. Among them For observer bandwidth:

[0191] (6.7)

[0192] The analytical expression for the observer gain is obtained by comparing the coefficients:

[0193] (6.8)

[0194] In actual LESO implementations, negative gain is typically used. The poles are placed in the left half-plane. Additionally, the observer bandwidth... The larger the value, the faster the observer converges, but the higher its sensitivity to measurement noise.

[0195] Design of linear state error feedback control law

[0196] LESO provides accurate state estimation. , and disturbance estimation Next, the control law for LADRC needs to be designed. This involves designing the virtual control input. To achieve tracking error Fast convergence. A linear PD controller is used to drive the system, and LESO state estimates are used. and Provide feedback:

[0197] (6.9)

[0198] in, and These are the reference signal and its differential signal, respectively. and These are the proportional and differential gain coefficients, respectively.

[0199] Controller gain , Based on controller bandwidth Configure the system to ensure rapid response and stability of the closed-loop system:

[0200] (6.10)

[0201] The analytical expression for the controller gain is obtained by comparing the coefficients:

[0202] (6.11)

[0203] This configuration method places the poles of the closed-loop system at... At this point, a critically damped system is formed, ensuring both rapid response and avoiding overshoot. Furthermore, to ensure the control system can respond quickly and operate stably, the principle that the observer bandwidth must be greater than the controller bandwidth must be satisfied, i.e.: , For the first i Bandwidth of each channel observer, For the first i Each channel controller bandwidth.

[0204] Final virtual control input Combined with feedback control law Total disturbance estimate :

[0205] (6.12)

[0206] The final physical control input applied to the motor for:

[0207] (6.13)

[0208] By using the linear state error feedback control law designed above, combined with LESO disturbance estimation and compensation, LADRC achieves effective control of uncertain systems, simplifying complex control problems into PD control of standard integral cascade systems, and significantly improving the performance and practicality of the control system.

[0209] Particle swarm optimization algorithm for LADRC parameter tuning

[0210] In the controller design described above, the LADRC algorithm uses the bandwidth method to configure the gain, and its performance is uniquely determined by six core bandwidth parameters. These six parameters constitute the optimization variables in this study: 1) the bandwidth of the three observers. (x-axis) (y-axis) (θ-axis); 2) Bandwidth of the three controllers (x-axis) (y-axis) (θ axis).

[0211] Therefore, the dimension D of this optimization problem is 6. Each potential combination of LADRC parameters can be represented by a 6-dimensional vector, which is the position of particle j. :

[0212] (6.15)

[0213] The LADRC parameter search space is the range of values ​​(upper and lower bounds) of these parameters. , The PSO algorithm searches within a 6-dimensional hypercube space bounded by the boundary ∠(x) and ∠(y) = 6.5. The task of the PSO algorithm is to efficiently search within this 6-dimensional space for a hypercube that satisfies the fitness function ∠(x) = 6.5. Optimal parameter combination to achieve minimum value .

[0214] During the search process, each particle j updates its position by tracking the following two key extreme values: 1) the individual's optimal position ( 1) The parameter position found by particle j in each iteration that maximizes (minimizes) the fitness value; 2) The global optimal position ( : The parameter position found by the entire particle swarm in each iteration that results in the optimal (minimum) fitness value.

[0215] The velocity of particle j in the (k+1)th iteration and location Update according to the following formula:

[0216] (6.16)

[0217] (6.17)

[0218] in, It is the inertia factor, used to balance global and local search capabilities; a larger one... This helps particles explore new regions (global optimization), while smaller ones... This helps to perform a fine search (local optimization) in the vicinity of the current location; and It is the learning factor (or acceleration constant), which controls the degree to which particles are influenced by individual and group experiences; , yes A random number between [a certain number of points].

[0219] Fitness function (objective function) The parameter set (LADRC) is the only metric for evaluating the performance of LADRC. Its design goal is to guide the particle swarm to find an optimal combination of parameters in a six-dimensional search space. To balance the three conflicting performance objectives of rapid system response, high tracking accuracy, and stable control input, this study employs a multi-objective weighted summation approach, combining the tracking error index (ITAE) with the control cost index (control energy). ( ) combined.

[0220] The overall fitness function is a weighted sum of various performance metrics, with the objective of minimizing J(X):

[0221] (6.18)

[0222] To evaluate the transient and steady-state performance of trajectory tracking, the Time-integrated Absolute Error (ITAE) metric is used to assess the performance of the three degrees of freedom ( The tracking error is quantified.

[0223] (6.19)

[0224] ITAE is a commonly used time-domain integral error metric. It introduces a time factor t to multiply the absolute error. The time factor t has a stronger penalty effect on errors and oscillations that exist over long periods. This prompts optimization algorithms to choose parameters that enable the system to converge to steady state faster and with smaller steady-state oscillations, thereby improving the system's fast convergence performance and steady-state accuracy.

[0225] To assess the stability of the control system and avoid motor saturation, the controller's output signal needs to be constrained.

[0226] (6.20)

[0227] This metric evaluates the input signals of four motors. The total energy (integral of the sum of squares). Minimize This means that while achieving the tracking target, the output amplitude and rapid changes of the controller should be minimized as much as possible, thereby suppressing high-frequency jitter and reducing hardware load.

[0228] Comprehensive fitness function The weighting of each performance indicator reflects the priority requirements for the MWMR trajectory tracking control system.

[0229] Table 1. Weight Allocation of Fitness Function

[0230]

[0231] As shown in Table 1, the positional error and This accounts for 70% of the total weight, emphasizing that positional accuracy in trajectory tracking is the primary objective of this optimization task. Control energy. The weight of is relatively low at 10%, ensuring it acts as a constraint rather than a dominant factor. This means that PSO will only choose the option with lower control cost when multiple parameter combinations have similar tracking accuracy, effectively balancing the trade-off between performance and energy consumption / chatter. Through this weighted summation, the PSO algorithm can transform the complex six-dimensional multi-objective optimization problem into a single index minimization problem, thereby efficiently searching for the optimal LADRC parameter combination that balances tracking accuracy and control stability.

[0232] Figure 3 This study demonstrates the fitness convergence characteristics of the particle swarm optimization algorithm during LADRC parameter tuning. The optimization process used 6 particles for 15 iterations, starting with an initial fitness value of 56.2173 and ultimately reaching 32.2493, representing a fitness improvement of 42.6%. Three distinct optimization stages can be observed from the convergence curve: In the first 5 iterations, the algorithm exhibits rapid global search capability, with the fitness value quickly decreasing from 56.2173 to 32.2744; this stage primarily involves a large-scale exploration of the parameter space. During iterations 6 to 10, the algorithm transitions to a local fine-grained search, with the fitness value fluctuating within a small range, further improving from 32.2744 to 32.2493. In the final 5 iterations, the algorithm tends towards stable convergence, with the fitness value remaining stable around 32.2493, indicating that a near-optimal parameter combination has been found.

[0233] Figure 4 and Figure 5 The optimization process for six key bandwidth parameters is demonstrated, revealing the coordinated evolution among these parameters. Among the observer bandwidth parameters, the X-axis channel... Converging to 78.46 rad / s, the Y-axis channel... Converging to 50.00 rad / s, while the heading angle channel... The gradient converges to 8.00 rad / s, and this gradient distribution reflects the differences in the dynamic characteristics of the system across different degrees of freedom. The controller bandwidth parameter also exhibits a reasonable distribution pattern. , and The convergence values ​​were found to be 20.00 rad / s, 17.83 rad / s, and 2.00 rad / s, respectively. All parameter combinations strictly satisfied the given conditions. The design constraints include bandwidth ratios of 3.92:1, 2.80:1, and 4.00:1 for the x-axis, y-axis, and θ-axis, respectively. This ratio ensures that the dynamic response speed of the state estimation is always faster than that of the control loop, providing an important guarantee for the stability of the control system.

[0234] The optimized parameter combination exhibits significant advantages in several aspects: the parameters demonstrate good coordination, and the bandwidth of each channel is rationally distributed according to the system's dynamic characteristics; a fine balance is achieved between speed, stability, and robustness; and the parameter values ​​are all within the engineering-feasible range, avoiding problems such as noise sensitivity caused by excessive bandwidth. The final optimization result is... =78.46, =50.00, =8.00, =20.00, =17.83, =2.00, this parameter combination provides the optimal configuration for subsequent trajectory tracking control.

[0235] The optimized PSO-LADRC controller was compared with unoptimized LADRC and PID controllers using MATLAB 2022a / Simulink software for numerical simulation analysis. Two trajectory tracking test schemes, a trefoil rose trajectory and a rectangular trajectory, were designed to verify the effectiveness of the proposed PSO-LADRC algorithm.

[0236] The following three-leaf rose trajectory is used:

[0237] (6.21)

[0238] In the formula, , , For time variables, Let x be the position component of the desired trajectory of the mobile robot. Let be the y-direction position component of the mobile robot's desired trajectory. Let be the angular components of the desired trajectory of the mobile robot.

[0239] In the simulation of three-leaf rose trajectory tracking, from Figure 6 The trajectory comparison shows that the actual trajectory of PSO-LADRC best matches the reference trajectory, especially maintaining smooth tracking in areas with large curve curvature, while PID and unoptimized LADRC show significant tracking deviations. This visual difference is reflected in the error curve. Figures 7-9 Further quantification revealed that the X-axis error amplitude of PSO-LADRC was significantly smaller than that of the other two controllers, and its fluctuation range was the smallest during the steady-state phase. Table 2 provides numerical evidence for this: PSO-LADRC exhibited the best position tracking error among the three controllers in both the X and Y axes, with an X-axis RMSE of 0.0289m, a 34.8% reduction compared to PID's 0.0443m and a 13.2% reduction compared to the unoptimized LADRC's 0.0333m. This result is highly consistent with the design where the position error weight accounts for 70% in the fitness function, demonstrating that the PSO optimization process effectively improved position tracking performance.

[0240] Table 2. Root Mean Square Error of the Trifoliate Rose Trajectory

[0241]

[0242] It is worth noting that although the PSO-LADRC outperforms the unoptimized LADRC in steering angle control, its RMSE of 0.3577 rad is still higher than that of the PID controller (0.2107 rad), reflecting the priority given to position tracking performance during the optimization process. (From control input) Figures 10-12 As can be seen, the output voltage curve of PSO-LADRC is the smoothest, with no obvious high-frequency jitter, demonstrating its good disturbance suppression capability.

[0243] The reference trajectory is set to a rectangle with a length of 1.5m and a width of 1m, and the heading angle is set to zero. In the rectangular trajectory tracking simulation, Figure 13 The trajectory comparison shows that all three controllers perform well on straight sections, while at right-angle turns, the PSO-LADRC exhibits the fastest recovery and the smallest overshoot. This characteristic is reflected in the error curve. Figures 14-16 The difference is even more pronounced in the PSO-LADRC: the PSO-LADRC has the smallest error peak at the corner and the fastest error convergence speed.

[0244] Table 3. Root Mean Square Error of Rectangular Trajectory

[0245]

[0246] The root mean square error (RMSE) data in Table 3 show that the PSO-LADRC has RMSEs of 0.0228m and 0.0258m on the X and Y axes, respectively. While not optimal across all dimensions, it exhibits the most balanced overall performance. Of particular note is the control input... Figures 17-19 As can be seen, the voltage curve of PSO-LADRC does not exhibit severe jitter at the point of trajectory abrupt change. This demonstrates the optimization effect of the control energy term as a constraint term in the fitness function. Under the premise of ensuring the main tracking performance, the optimization algorithm selects a parameter combination with lower control cost.

[0247] A comprehensive analysis of the simulation results for both types of trajectories shows that the curve shapes in the figure and the error data in the table together demonstrate that PSO-LADRC possesses superior overall performance. This performance advantage stems directly from the weight-based optimization strategy: the high weight (70%) of position error in the fitness function ensures a significant improvement in planar trajectory tracking accuracy, which... Figures 7-8 and Figures 14-15 This is clearly reflected in the error curve; the appropriate weighting of the control energy (10%) optimizes control stability while ensuring accuracy, which is evident from... Figure 12 and Figure 19The results were validated using smooth control input curves. Compared to unoptimized LADRC, PSO-LADRC fully leverages the potential of the linear active disturbance rejection control structure through parameter optimization; compared to traditional PID, it exhibits stronger disturbance suppression capability and adaptability. Simulation results fully demonstrate that the parameter tuning strategy based on particle swarm optimization can effectively solve the parameter tuning problem of LADRC, providing an effective solution for trajectory tracking control of mobile robots.

[0248] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A robot trajectory control method based on PSO linear active disturbance rejection control, characterized in that, Includes the following steps: S01: The dynamic equations of the mobile robot are transformed into multiple independent second-order integral cascaded channels through coordinate transformation and input reconstruction, resulting in multiple decoupled channels; S02: Each decoupled channel is controlled by the same LADRC controller; S03: Design an independent third-order linear extended state observer for each decoupling channel to estimate the state and disturbances, specifically including: For each decoupling channel Design an independent third-order linear extended state observer to estimate the state. , Total channel disturbance ,aisle , , Channels Position and velocity state quantities, For channel Virtual control input; The third-order linear extended state observer is designed as follows: In the formula: It is a channel Location The estimate; It is a channel speed The estimate; It is a channel Total disturbance The estimate; , , These are the actual observer gain coefficients, Variables Estimates of velocity and acceleration, For disturbance An estimate of the derivative; S04: A basic control quantity is generated using a linear PD control law. Real-time compensation is then performed on the control quantity using the estimated total disturbance value, specifically including: Design virtual control input To achieve tracking error Fast convergence is achieved by using a linear PD controller to drive the system and a state estimate from a linear extended state observer. and Provide feedback: in, and These are the reference signal and its differential signal, respectively. and These are the proportional and differential gain coefficients, respectively; Feedback control law ; Final virtual control input Combined with feedback control law Total disturbance estimation : The final physical control input applied to the motor for: in, For the control gain matrix, for The false rebellion, The radius of the driving wheel; The nominal moment of inertia of the robot; This is the transformation matrix for the robot's coordinate system variables; S05: Using the observer bandwidth and controller bandwidth as the particle positions in the particle swarm optimization algorithm, and the tracking error index and control cost index as the fitness function, the optimal parameter combination that minimizes the fitness function is searched using the particle swarm optimization algorithm; the observer bandwidths of the x-axis, y-axis, and θ-axis are used... , , x-axis, y-axis, and θ-axis controller bandwidth , , Represented by a 6-dimensional vector, this vector is used as a particle. j Location : The LADRC parameter search space is a 6-dimensional hypercube space bounded by the range of values ​​for these parameters; the particle swarm optimization algorithm is used to search for the fitness function. Optimal parameter combination to achieve minimum value ; Each particle j During the search process, location updates are achieved by tracking the following two key extreme values: 1) the individual's optimal location. 1) The parameter position that minimizes the fitness value found by particle j in each iteration; 2) The global optimal position. The parameter position that minimizes the fitness value found by the entire particle swarm in each iteration; The velocity of particle j in the (k+1)th iteration and location Update according to the following formula: in, It is the inertia factor. and It is a learning factor. , yes A random number between [a certain number of points].

2. The robot trajectory control method based on PSO linear active disturbance rejection control according to claim 1, characterized in that, The multiple decoupling channels obtained in step S01 include: The dynamic equations of the mobile robot are: in, Output acceleration vector for robot dynamics. For the control gain matrix, For physical control input, It is a lumped disturbance term. This is the system disturbance vector; By employing coordinate transformation and input reconstruction, the multi-input multi-output system is transformed into multiple independent single-input single-output systems. pseudo-reversal To design physical control input : Virtual control input As the control law output of linear active disturbance rejection control In the global coordinate system x , y , θ Virtual control inputs for three directional channels; Will Substituting the dynamic equations, the mobile robot system is decoupled as follows: Among them, the new total disturbance ; After the above decoupling process, the mobile robot system is transformed into three independent second-order integral cascaded channels: in, In the global coordinate system The robot's dynamics output acceleration vectors for the three channels; , , These represent the total disturbance of the three channels respectively.

3. The robot trajectory control method based on PSO linear active disturbance rejection control according to claim 1, characterized in that, Step S03 also includes: Using linear error feedback term , j The poles are set to 1, 2, and 3, and the bandwidth method is used for gain configuration, placing all poles of the linearly extended state observer on the negative real axis. Among them For observer bandwidth: in, For the Laplace operator, , , These are the observer gain coefficients; The analytical expression for the observer gain is obtained by comparing the coefficients: 。 4. The robot trajectory control method based on PSO linear active disturbance rejection control according to claim 1, characterized in that, Proportional and differential gain coefficients , Based on the bandwidth of the linear PD controller Configure: The analytical expression for the controller gain is obtained by comparing the coefficients: The poles of the closed-loop system are configured at... At this point, a critical damping system is formed.

5. The robot trajectory control method based on PSO linear active disturbance rejection control according to claim 1, characterized in that, The fitness function J ( X The goal is to minimize the weighted sum of various performance indicators. J ( X ): , , They are respectively Tracking error index of the shaft To control energy, , , and As weight; The time-multiplied absolute error integral index is used for the three degrees of freedom. Quantify the tracking error: t Let T be the time, and T be the duration of the time. For degrees of freedom i Tracking error; Controlling energy : in, Input signal to motor j.

6. A robot trajectory control system based on PSO linear active disturbance rejection control, used to implement the robot trajectory control method based on PSO linear active disturbance rejection control as described in any one of claims 1-5, characterized in that, include: The robot decoupling module transforms the dynamic equations of the mobile robot into multiple independent second-order integral cascaded channels through coordinate transformation and input reconstruction, thus obtaining multiple decoupling channels; The LADRC control module controls each decoupled channel using the same LADRC controller; A linear extended state observer is designed for each decoupled channel to estimate the state and disturbances. The linear state error feedback control law module uses a linear PD control law to generate basic control quantities and uses the estimated total disturbance value to perform real-time compensation in the control quantities. The LADRC parameter tuning module uses the observer bandwidth and controller bandwidth as the positions of particles in the particle swarm optimization algorithm, and the tracking error index and control cost index as the fitness function. It then uses the particle swarm optimization algorithm to search for the optimal parameter combination that minimizes the fitness function.

7. A computer storage medium having a computer program stored thereon, characterized in that, When the computer program is executed, it implements the robot trajectory control method based on PSO linear active disturbance rejection control as described in any one of claims 1-5.

Images