Robot flexible joint active disturbance rejection feedforward compensation cascade nonlinear position control method

Abstract

This invention discloses a method for cascade nonlinear position control of robot flexible joints with active disturbance rejection (ADRROC) and feedforward compensation. The method includes: establishing a dynamic model for the robot flexible joint system; determining an ADRROC extended state observer based on the system dynamic model; determining a nonlinear feedback controller for the system; determining a feedforward compensated cascade nonlinear feedback controller based on the ADRROC extended state observer; and performing position control based on the determined feedforward compensated cascade nonlinear feedback controller based on the ADRROC extended state observer. This invention's method for cascade nonlinear position control of robot flexible joints, based on traditional compliant control, achieves high-precision position control and adaptive compliant characteristics of flexible joints through a combination of feedforward and ADRROC control strategies, ensuring higher safety and response speed for the robot when interacting with humans or uncertain environments.

Description

Technical Field

[0001] This invention belongs to the field of robot flexible joint control technology, specifically relating to a cascade nonlinear position control method for robot flexible joints with self-disturbance rejection and feedforward compensation. Background Technology

[0002] Joint output position control aims to ensure that the actual position of the joint output end accurately and quickly follows a preset position or motion trajectory. However, in the field of human-robot collaborative robots, higher requirements are placed on the position control of flexible joints. Not only do they need to achieve high precision and fast response, but they also need to have a certain degree of compliance and safety when interacting with humans or uncertain environments.

[0003] Traditional position control is mainly used in industrial servo systems and industrial robots, and its control strategy focuses on improving the accuracy of position control. This control method usually ignores the possibility of collisions between the robot and its surroundings, so additional safety measures are often required when applying it. These industrial robots complete repetitive tasks according to preset programs, and personnel are usually prohibited from entering the workspace. They also typically lack collaborative capabilities.

[0004] In human-robot collaboration, robot position control must consider the interaction needs with the environment and humans. Typically, when tracking a specific trajectory, collision issues with the environment need to be addressed. Traditional collaborative robotic arms usually use a current loop to estimate the robot's output torque and detect collisions by monitoring the torque, then switch control strategies based on a set threshold. However, this method requires comprehensive consideration of the reducer's influence during current estimation, switching response speed, and the smoothness of the switching process.

[0005] Another approach is to install a torque sensor or elastic element at the joint output end to detect the output torque. Torque sensors are rigid and expensive, while the former typically lacks hardware flexibility and is also costly. The stiffness of flexible joint elastic elements is not adjustable; therefore, a more advanced human-machine collaborative control strategy is to use output impedance control. This method can dynamically adjust the output impedance during position control, but the ultimate goal of impedance control is not to minimize position error. The addition of an elastic element can lead to a decrease in position control accuracy. Summary of the Invention

[0006] This invention provides a cascade nonlinear position control method for robot flexible joints with self-disturbance rejection and feedforward compensation to solve the aforementioned technical problems. Specifically, the technical solution is as follows:

[0007] A method for cascaded nonlinear position control of a robot's flexible joint with active disturbance rejection and feedforward compensation includes the following steps:

[0008] S1. Establish a dynamic model for the robot's flexible joint system;

[0009] S2. Determine the active disturbance rejection extended state observer based on the system dynamics model;

[0010] S3. Determine the nonlinear feedback controller for the system;

[0011] S4. Determine the feedforward compensation cascade nonlinear feedback controller based on the active disturbance rejection extended state observer;

[0012] S5. Position control is performed based on a predetermined feedforward compensated cascade nonlinear feedback controller based on an active disturbance rejection extended state observer.

[0013] Further, in step S1, the mathematical description of the dynamic model is as follows:

[0014]

[0015] In the formula, τ h Indicates control torque, I h For the inertial term, g(θ) h ) represents the gravity model term. Let δ represent the damping term, θ be the unknown disturbance, and θ be the damping term. h For joint angle;

[0016] Transforming the above equation, we obtain the following expression:

[0017]

[0018] Furthermore, in step S2, the unknown disturbance δ in the load is taken as the observation target, and a definition is made.

[0019] Where, τ h θ is the input for the control object. h Let d be the system output and d be the total system disturbance. Expanding this disturbance into a single state variable, the system's state-space equations describe it as follows:

[0020]

[0021] y = Cx

[0022] In the formula, the system matrix A, input matrix B, disturbance input matrix E, and state variable x are defined as follows:

[0023]

[0024] Expanding the state-space equations, we obtain the following state-space expression:

[0025]

[0026] According to the PBH criterion, define Since the rank of Q is 3 and equal to the dimension of the state space, the system is observable. An extended state observer can be constructed as shown in the following formula:

[0027]

[0028] in, Here is the gain matrix of the observer. Given the estimated values ​​of the state variables, expanding the above equation yields the following observer algorithm:

[0029]

[0030] When the extended state observer gain satisfies A-GC<0 and At that time, the observation error of the extended state observer will converge to zero.

[0031] Furthermore, in step S3, the arctangent function is selected as the core of the nonlinear feedback, and the nonlinear feedback control law is:

[0032] u=K p arctan(e)+K d e

[0033] Among them, K p and K d Here, e represents the control gain parameter, and e represents the control error.

[0034] Furthermore, in step S4, the disturbance is divided into a known part and an unknown part. For the disturbance in the known part, compensation is performed using system model information, and for the disturbance in the unknown part, estimation is performed using an extended observer.

[0035] Further, in step S4, the total disturbance is The total disturbance consists of two parts, and the known disturbance gravity g(θ) h ) and damping The unknown disturbance δ includes model parameter errors and load changes. Based on disturbance compensation using an extended state observer, and using known system model information, the known disturbance gravity g(θ) is calculated. h ) and damping After compensation, the improved control law is as follows:

[0036]

[0037] Where, τ h K represents the control torque. p and K d Here, e is the control gain parameter, and I is the control error. h For the inertial term, g(θ)d ) represents the gravity model term, C h θ d This represents the damping term.

[0038] Furthermore, in step S4, an acceleration feedforward term is added to compensate for errors under high acceleration, and the improved control law is:

[0039]

[0040] Among them, K p and K d Here, B is the control gain parameter, e is the control error, and B is the control error. ff1 B ff2 and B ff3 This is the acceleration feedforward gain coefficient.

[0041] The advantage of this invention lies in the robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method provided. Based on traditional compliant control, it achieves high-precision position control and adaptive compliant characteristics of flexible joints by combining feedforward and self-disturbance rejection control strategies, ensuring that the robot has higher safety and response speed when interacting with humans or uncertain environments.

[0042] The advantage of this invention lies in the cascade nonlinear position control method for robot flexible joints with active disturbance rejection and feedforward compensation. By introducing feedforward control, it can effectively predict and cancel external disturbances, reducing the system's sensitivity to uncertain environmental factors. Simultaneously, combined with active disturbance rejection control, it further enhances the system's anti-interference capability and control stability, thereby achieving a dynamic balance between positional accuracy and compliance, meeting the requirements of collaborative robots for high precision and high compliance.

[0043] The control strategy proposed in this invention is applicable to scenarios requiring frequent human-machine interaction and involving uncertain environments, such as human-machine collaborative production and service robots. By implementing the control method of this invention at the output end of the flexible joint, the robot can achieve high position control accuracy while ensuring a certain degree of compliance, thereby effectively coping with complex and changing working environments. Attached Figure Description

[0044] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0045] Figure 1 This is a flowchart of the cascaded position control method for robot flexible joint self-disturbance rejection feedforward compensation proposed in this invention.

[0046] Figure 2 This is a block diagram illustrating the principle of the cascaded position control method for self-disturbance rejection feedforward compensation of robot flexible joints proposed in this invention.

[0047] Figure 3 The observation effect of the present invention on disturbances under different observer gains;

[0048] Figure 4 This is a block diagram of the known disturbance compensation and separation principle proposed in this invention;

[0049] Figure 5 This is a simulation result diagram showing the relationship between the disturbance estimation error and the accuracy of gravity modeling in this invention;

[0050] Figure 6 A diagram of the cascaded position control framework for flexible joint self-disturbance rejection feedforward compensation proposed in the invention.

[0051] Figure 7 This is a test platform diagram for an application example of the present invention;

[0052] Figure 8 This is a diagram showing the position trajectory tracking results of the PD controller under a 0.5kg load in an example of the present invention.

[0053] Figure 9 This is a diagram showing the cascade position control results of a 0.5kg self-disturbance rejection feedforward compensation in Example 0 of the present invention;

[0054] Figure 10 This is a diagram showing the trajectory tracking results of a cascade position control system with an active disturbance rejection feedforward compensation for a 1kg load, as an example of the present invention.

[0055] Figure 11 This is a diagram showing the trajectory tracking results under load abrupt changes in the self-disturbance rejection feedforward compensation cascade position control system, as an example of the present invention. Detailed Implementation

[0056] Embodiments of the present invention are described in detail below. Examples of these embodiments are illustrated in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0057] like Figure 1-2 As shown, this application discloses a cascade nonlinear position control method for a robot flexible joint with self-disturbance rejection and feedforward compensation, comprising the following steps:

[0058] S1. Establish a dynamic model for the robot's flexible joint system.

[0059] In step S1, a mathematical model based on the kinematic and dynamic characteristics of the flexible joint system for the robot is constructed. This model can describe the dynamic characteristics of the system under different load conditions, providing a theoretical basis for subsequent control determination.

[0060] The mathematical description of the dynamic model is shown below:

[0061]

[0062] In the formula, τ h Indicates control torque, I h For the inertial term, g(θ) h ) represents the gravity model term. Let δ represent the damping term, θ be the unknown disturbance, and θ be the damping term. h This refers to the joint angle.

[0063] Transforming the above equation, we obtain the following expression:

[0064]

[0065] S2. Based on the system dynamics model, determine the Active Disturbance Rejection Extended State Observer (ESO).

[0066] In step S2, the unknown disturbance δ in the load is taken as the observation target. This disturbance is mainly caused by parameter variations under different loads. Since the disturbance observed by the extended state observer is based on the system input side, it is defined as follows:

[0067]

[0068] Where, τ h θ is the input for the control object. h Let d be the system output and d be the total system disturbance. Expanding this disturbance into a single state variable, the system's state-space equations describe it as follows:

[0069]

[0070] y = Cx

[0071] In the formula, the system matrix A, input matrix B, disturbance input matrix E, and state variable x are defined as follows:

[0072]

[0073] Expanding the state-space equations, we obtain the following state-space expression:

[0074]

[0075] According to the PBH criterion, define Since the rank of Q is 3 and equal to the dimension of the state space, the system is observable. An extended state observer can be constructed as shown in the following formula:

[0076]

[0077] in, Here is the gain matrix of the observer. Given the estimated values ​​of the state variables, expanding the above equation yields the following observer algorithm:

[0078]

[0079] When the extended state observer gain satisfies A-GC<0 and At that time, the observation error of the extended state observer will converge to zero.

[0080] S3. Determine the nonlinear feedback controller for the system.

[0081] In traditional feedback control systems, a fixed linear feedback gain is typically used. However, in practical applications, the control error is small near the equilibrium point, often requiring a larger gain to enhance the system's disturbance rejection capability; while further away from the equilibrium point, a smaller gain is needed to suppress overshoot. Using the same gain value across all error ranges with linear feedback makes it difficult to meet these requirements. To address this issue, this application employs a nonlinear feedback control strategy.

[0082] Specifically, the arctangent function is chosen as the core of the nonlinear feedback because it has a large slope near zero, which increases the feedback strength, while its slope decreases further away from zero, naturally reducing the feedback strength. This characteristic aligns with the system control requirements. Therefore, the nonlinear feedback control law can be determined as follows:

[0083] u=K p arctan(e)+K d e

[0084] Among them, K p and K d Let be the control gain parameter and 'e' be the control error. This control law provides a larger gain when the error is small to enhance disturbance rejection, and a smaller gain when the error is large to suppress overshoot.

[0085] Extended state observers can estimate system disturbances, but their estimation capability is related to the observer's gain parameters β1, β2, and β3. Increasing the observer gain usually increases its bandwidth, making it more sensitive to observing disturbances in the system. However, high bandwidth reduces the system's resistance to high-frequency noise. In the case of low bandwidth, the response time for observing large-amplitude disturbances is longer, which introduces a certain estimation lag.

[0086] Real-world systems typically contain noise, thus the bandwidth of the extended state observer needs to be limited to avoid excessive sensitivity to noise. However, this also leads to a lag in the extended state observer's estimation of large-amplitude disturbances, affecting control accuracy. To reduce this error, low-pass filters or Kalman filters can be used to reduce the amplitude of disturbances in the observed part, thereby mitigating the lag effect and improving estimation accuracy.

[0087] S4. Determine the feedforward compensation cascade nonlinear feedback controller based on the active disturbance rejection extended state observer.

[0088] In this application, the disturbance is divided into known and unknown parts. For the known part of the disturbance, compensation is performed using system model information, while for the unknown part of the disturbance, estimation is performed using an extended observer. This reduces the workload of the extended state observer, allowing it to operate well even with lower gain without significantly degrading the system's control performance.

[0089] By compensating for known disturbances, the estimation error of the extended state observer can be effectively reduced, improving the overall stability and accuracy of the system. This method is as follows: Figure 3 As shown, by using some known model information to compensate for system disturbances, the error of the extended state observer is reduced, thereby further improving the control effect of the system.

[0090] Specifically, in step S4, the total disturbance is The total disturbance consists of two parts, and the known disturbance gravity g(θ) h ) and damping The unknown disturbance δ includes model parameter errors and load changes. Based on disturbance compensation using an extended state observer, and using known system model information, the known disturbance gravity g(θ) is calculated. h ) and damping

[0091] To compensate for the increased burden on the extended state observer, the improved control law is as follows:

[0092]

[0093] Where, τ h K represents the control torque. p and K d Here, e is the control gain parameter, and I is the control error. h For the inertial term, g(θ) d ) represents the gravity model term, C h θ dThis represents the damping term. This control law utilizes known gravity and damping models for disturbance compensation, effectively improving the system's control accuracy under disturbance conditions. When the damping model is relatively accurate, the control effect is mainly affected by the accuracy of the gravity model and the estimation error of the actual disturbance by the extended state observer.

[0094] like Figure 5 As shown in the figure, MATLAB simulation shows that as the accuracy of known perturbation modeling improves, the extended state observer can effectively reduce the perturbation estimation error without increasing the observer gain. The extended state observer gain is shown in Table 1.

[0095] Table 1 Gain parameters for different observers

[0096] Observer Gain Parameter 1 Parameter 2 Parameter 3 Parameter 4 Parameter 5 <![CDATA[β1]]> 3 6 15 30 60 <![CDATA[β2]]> 3 12 75 300 1200 <![CDATA[β3]]> 1 8 125 1000 8000

[0097] From simulation Figure 6 As can be seen, with the improvement of the accuracy of modeling known disturbances, the extended state observer can still reduce the disturbance estimation error under low gain conditions. This means that accurate model information can not only reduce the workload of the observer, but also bring significant improvements in control performance.

[0098] When the reference trajectory contains significant acceleration, the traditional control law will produce a large error in the high-acceleration region. Therefore, an acceleration feedforward term is added to the control law to compensate for the error under high acceleration. The improved control law is as follows:

[0099]

[0100] Among them, K p and K d To control the gain parameter, B ff1 B ff2 and B ff3 This represents the acceleration feedforward gain coefficient. Its control block diagram is shown below. Figure 4 As shown, the controller proposed in this invention can effectively compensate for control errors generated in regions with large acceleration, thereby improving the system's response accuracy.

[0101] S5. Position control is performed based on a predetermined feedforward compensated cascade nonlinear feedback controller based on an active disturbance rejection extended state observer.

[0102] The purpose of this invention is to provide a cascade nonlinear position control method with self-disturbance rejection and feedforward compensation for flexible joints in robots, to meet the requirements of human-robot collaborative robots for compliance and position control accuracy. The effectiveness of this method is further verified through examples below, demonstrating its application in high-precision and fast-response tasks.

[0103] Flexible joints are commonly used in the joint design of collaborative robotic arms. Since collaborative robotic arms need to adapt to various working environments, the performance of the flexible joint position controller under different working conditions must be fully considered when designing it. First, we investigated the position tracking control performance of the flexible joint under a fixed output load. Because the working environment of the flexible joint is often uncertain, and the load may also change, we also examined its position tracking control capability under load variations. Finally, we tested the position controller's anti-interference performance in the face of sudden load disturbances.

[0104] (1) Position tracking performance test under different loads

[0105] Test environment such as Figure 7 As shown in the figure. First, tests were conducted under constant load conditions. The joint output end had a linkage load, with the linkage load weighing 0.5 kg, its own weight being 0.9 kg, and its length being 0.8 m. The zero position of the joint was set vertically downward, allowing the joint output end to reciprocate between the vertical and horizontal directions. The reference position signal was set to vary from 0 rad to 1.57 rad, and the host computer recorded the reference position signal and the actual position signal in real time. The experimental results are as follows. Figures 8 to 10 As shown, the dashed line represents the reference position trajectory, and the solid line represents the actual position trajectory.

[0106] from Figure 8 and Figure 9 It is evident that, compared to the PD controller, the cascade controller based on active disturbance rejection exhibits a significant improvement in steady-state and dynamic control accuracy. The average absolute position error is reduced from 0.0735 rad to 0.0027 rad, and almost no overshoot or oscillation occurs during motion. Therefore, the position controller proposed in this invention can achieve excellent position trajectory tracking performance under a fixed load.

[0107] Figure 10 The results show the test results after increasing the load to 1.0 kg without changing the controller parameters. As can be seen from the figure, even with changes in load, the position controller still maintains good tracking performance, indicating that the controller has a certain degree of adaptability to load changes.

[0108] (2) Anti-interference load sudden change test

[0109] The application environment of collaborative robots is often highly uncertain, and the output load of flexible joints is often not constant. To test the position control performance of flexible joints under sudden load changes, this experiment included a manually increased load change test when the flexible joint moved to a horizontal position. The experimental results are as follows: Figure 11 As shown.

[0110] exist Figure 11 In the diagram, the dashed line represents the position reference signal, and the solid line represents the actual output position of the flexible joint. When the joint moves to the horizontal position, an additional 0.5 kg load is suddenly added to the original 0.5 kg load. It can be observed that the actual output position of the joint deviates downward relative to the reference signal, but returns to near the reference signal after a period of time. Subsequently, the added 0.5 kg load is removed, and it can be seen that the actual output position of the joint deviates upward relative to the reference signal, but also returns to near the reference signal within a short time.

[0111] To further examine the position control performance under sudden load changes, a 1kg load was added to the experiment. The results showed that the position deviation was more pronounced than before, but it was still able to recover to near the reference signal within a certain time. This demonstrates that the position controller proposed in this invention exhibits good compliance under sudden load changes, possesses strong anti-interference capabilities, and can effectively resist the disturbances caused by load variations.

[0112] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the above embodiments do not limit the present invention in any way, and all technical solutions obtained by equivalent substitution or equivalent transformation fall within the protection scope of the present invention.

Claims

1. A cascade nonlinear position control method with self-disturbance rejection and feedforward compensation for flexible joints of a robot, characterized in that, Includes the following steps: S1. Establish a dynamic model for the robot's flexible joint system; S2. Determine the active disturbance rejection extended state observer based on the system dynamics model; S3. Determine the nonlinear feedback controller for the system; S4. Determine the feedforward compensation cascade nonlinear feedback controller based on the active disturbance rejection extended state observer; S5. Position control is performed based on a predetermined feedforward compensated cascade nonlinear feedback controller based on an active disturbance rejection extended state observer. In step S1, As shown: In the formula, Indicates control torque. Inertia term, Represents the gravity model term. Indicates the damping term. For unknown disturbances, For joint angle; Transforming the above equation, we obtain the following expression: ； In step S2, the unknown disturbance in the load is... As the observation target, define ,in, For the input of the control object, Let d be the system output and d be the total system disturbance. Expanding this disturbance into a single system state variable, the system's state-space equations are described as follows: In the formula, the system matrix A, input matrix B, disturbance input matrix E, and state variable x are defined as follows: Expanding the state-space equations, we obtain the following state-space expression: According to the PBH criterion, define Since the rank of Q is 3 and equal to the dimension of the state space, the system is observable. An extended state observer can be constructed as shown in the following formula: in, Here is the gain matrix of the observer. Given the estimated values ​​of the state variables, expanding the above equation yields the following observer algorithm: When the gain of the extended state observer satisfies and At that time, the observation error of the extended state observer will converge to zero.

2. The robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method according to claim 1, characterized in that, In step S3, the arctangent function is selected as the core of the nonlinear feedback, and the nonlinear feedback control law is: Among them, K p and K d Here, e represents the control gain parameter, and e represents the control error.

3. The robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method according to claim 2, characterized in that, In step S4, the disturbance is divided into known and unknown parts. For the known part of the disturbance, compensation is performed using system model information, and for the unknown part of the disturbance, estimation is performed using an extended observer.

4. The robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method according to claim 3, characterized in that, In step S4, the total disturbance is The total disturbance consists of two parts, and the known disturbance gravity is... and damping Unknown disturbance Including model parameter errors and load changes, based on disturbance compensation using an extended state observer, and using known system model information, the known disturbance gravity is compensated. and damping After compensation, the improved control law is as follows: in, K represents the control torque. p and K d Here, e represents the control gain parameter, and e represents the control error. For inertia, Represents the gravity model term. This represents the damping term.

5. The robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method according to claim 3, characterized in that, In step S4, an acceleration feedforward term is added to compensate for errors under high acceleration. The improved control law is as follows: Among them, K p and K d Here, B is the control gain parameter, e is the control error, and B is the control error. ff1 B ff2 and B ff3 This is the acceleration feedforward gain coefficient.

Images