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

Abstract

The invention discloses an active-disturbance-rejection feedforward compensation cascade nonlinear position control method for a flexible joint of a robot. The method comprises the following steps: establishing a dynamic model for a flexible joint system of the robot; determining an active-disturbance-rejection expansion state observer according to the system dynamics model; determining a system nonlinear feedback controller; determining a feedforward compensation cascade nonlinear feedback controller based on the active-disturbance-rejection extended state observer; and position control is carried out based on the determined feedforward compensation cascade nonlinear feedback controller based on the active-disturbance-rejection expansion state observer. According to the active-disturbance-rejection feed-forward compensation cascade nonlinear position control method for the flexible joint of the robot, on the basis of traditional compliance control, through combination of a feed-forward control strategy and an active-disturbance-rejection control strategy, high-precision position control and adaptive compliance characteristics of the flexible joint are achieved; and it is ensured that the robot has higher safety and response speed when interacting with a person or an uncertain environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot flexible joint control, and particularly relates to a self-disturbance rejection feedforward compensation cascade nonlinear position control method for robot flexible joints. Background Technique

[0002] The output position control of the joint aims to make the actual position of the joint output end accurately and quickly follow the preset position or motion trajectory. However, in the field of human-robot collaboration, higher requirements are put forward for the position control of flexible joints. It not only needs to meet the requirements of high precision and fast response, but also needs to have a certain degree of compliance and safety when interacting with humans or uncertain environments.

[0003] Traditional position control is mainly applied to industrial servo systems and industrial robots, and its control strategy focuses on improving the accuracy of position control. This control method usually ignores the collision situation between the robot and the outside world, so additional safety protection measures are often required during application. Such industrial robots complete repetitive tasks according to preset programs, and people are usually prohibited from entering the working space, and they usually do not have collaborative capabilities.

[0004] In human-robot collaboration, the position control of the robot must consider the interaction requirements with the environment and people. Usually, when tracking a certain position trajectory, attention needs to be paid to the collision problem with the environment. Traditional collaborative manipulators usually use a current loop to estimate the output torque of the robot, identify the collision by detecting the torque, and then switch the control strategy according to the set threshold. However, this method needs to comprehensively consider the influence of the reducer in the current estimation process, the switching response speed, and the smoothness of the switching process.

[0005] Another method is to install a torque sensor or an elastic element at the joint output end to detect the output torque. Due to the high rigidity and high cost of the torque sensor, the former usually lacks hardware flexibility and has a high cost. And the stiffness of the elastic element of the flexible joint cannot be adjusted. Therefore, a more advanced human-robot collaboration control strategy is to use output impedance control. This method can dynamically adjust the output impedance during the position control process, but the ultimate goal of impedance control is not to minimize the position error. The addition of the elastic element will lead to a decrease in the position control accuracy. Summary of the Invention

[0006] The present invention provides a self-disturbance rejection feedforward compensation cascade nonlinear position control method for robot flexible joints to solve the above-mentioned technical problems, and specifically adopts the following technical solutions:

[0007] A self-disturbance rejection feedforward compensation cascade nonlinear position control method for robot flexible joints, comprising the following steps:

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

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

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

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

[0012] S5. Perform position control based on the determined feedforward compensation cascade nonlinear feedback controller based on the active disturbance rejection extended state observer.

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

[0014]

[0015] In the formula, τ h represents the control torque, I h is the inertia term, g(θ h ) represents the gravity model term, represents the damping term, δ is the unknown disturbance, and θ h is the joint angle;

[0016] Deform the above formula to obtain the following expression:

[0017]

[0018] Further, in the step S2, take the unknown disturbance δ in the load as the observation target and define

[0019] where τ h is the input of the controlled object, θ h is the system output, d is the total system disturbance, and expand it into a state variable of the system. The state space equation of the system is described as follows:

[0020]

[0021] y = Cx

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

[0023]

[0024] After expanding the state space equation, the following state space expression is obtained:

[0025]

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

[0027]

[0028] where is the gain matrix of the observer, is the estimated value of the state variable. Expanding the above formula, the following observer algorithm is obtained:

[0029]

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

[0031] Furthermore, in the 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] where K p and K d are control gain parameters, and e is the control error.

[0034] Furthermore, in the step S4, the disturbance is divided into a known part and an unknown part. For the known part of the disturbance, it is compensated through the system model information, and for the unknown part of the disturbance, it is estimated through the extended observer.

[0035] Furthermore, in the step S4, the total disturbance is The total disturbance consists of two parts, the known disturbance gravity g(θ h ) and the damping The unknown disturbance δ includes model parameter errors and load changes. On the basis of using the extended state observer for disturbance compensation, through the known system model information, the known disturbance gravity g(θ h ) and the damping are compensated, and the improved control law is:

[0036]

[0037] where τ h represents the control torque, K p and K d are control gain parameters, e is the control error, I h is the inertia term, g(θd ) represents the gravity model term, C h θ d represents the damping term.

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

[0039]

[0040] where K p and K d are control gain parameters, e is the control error, B ff1 、B ff2 and B ff3 are acceleration feedforward gain coefficients.

[0041] The advantage of the present invention lies in the provided robot flexible joint active disturbance rejection feedforward compensation cascade nonlinear position control method. Based on the traditional compliant control, through the combination of feedforward and active disturbance rejection control strategies, it realizes high-precision position control and adaptive compliant characteristics of the flexible joint, ensuring higher safety and response speed when the robot interacts with humans or an uncertain environment.

[0042] The advantage of the present invention lies in the provided robot flexible joint active disturbance rejection feedforward compensation cascade nonlinear position control method. By introducing feedforward control, it can effectively estimate and cancel external disturbances, reducing the sensitivity of the system to uncertain environmental factors. At the same time, combined with active disturbance rejection control, it further improves the anti-disturbance ability and control stability of the system, thus achieving a dynamic balance between position accuracy and compliance, and meeting the requirements of high precision and high compliance for collaborative robots.

[0043] The control strategy proposed by the present invention is applicable to scenarios that require frequent human-robot interaction and an uncertain environment, such as human-robot collaborative production, service robots, etc. By implementing the control method of the present invention at the output end of the flexible joint, the robot can achieve a relatively high position control accuracy on the premise of ensuring a certain compliance, so as to effectively cope with complex and changing working environments. Brief Description of the Drawings

[0044] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0045] Figure 1 is the flow chart of the cascade position control method for the robot flexible joint active disturbance rejection feedforward compensation proposed by the present invention;

[0046] Figure 2 It is the principle block diagram of the cascade position control method with self-disturbance rejection and feedforward compensation for the robot flexible joint proposed by the present invention;

[0047] Figure 3 It is the observation effect of the disturbance under different observer gains of the present invention;

[0048] Figure 4 It is the principle block diagram of the known disturbance compensation separation proposed by the present invention;

[0049] Figure 5 It is the simulation result diagram of the disturbance estimation error and the gravity modeling accuracy of the present invention;

[0050] Figure 6 It is the frame diagram of the cascade position control with self-disturbance rejection and feedforward compensation for the flexible joint proposed by the invention;

[0051] Figure 7 It is the test platform diagram of the application example of the present invention;

[0052] Figure 8 It is the position trajectory tracking result diagram of the PD controller under the 0.5 kg load of the present invention example;

[0053] Figure 9 It is the position control result diagram of the cascade with self-disturbance rejection and feedforward compensation under the 0.5 kg load of the present invention example;

[0054] Figure 10 It is the trajectory tracking result diagram of the cascade position control with self-disturbance rejection and feedforward compensation under the 1 kg load of the present invention example;

[0055] Figure 11 It is the trajectory tracking result diagram of the cascade position control with self-disturbance rejection and feedforward compensation under the load mutation of the present invention example. Specific implementation manners

[0056] The following details the embodiments of the present invention. The examples of the embodiments are shown in the drawings, where the same or similar reference numerals represent the same or similar elements or elements with the same or similar functions throughout. The embodiments described below by referring to the drawings are exemplary and are intended to explain the present invention, and should not be construed as a limitation to the present invention.

[0057] As Figure 1-2 shown, a robot flexible joint self-disturbance rejection and feedforward compensation cascade non-linear position control method of the present application includes the following steps:

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

[0059] In step S1, for the flexible joint system of the robot, a mathematical model based on its kinematic and dynamic characteristics 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 as follows:

[0061]

[0062] In the formula, τ h represents the control torque, I h is the inertia term, g(θ h ) represents the gravity model term, represents the damping term, δ is the unknown disturbance, and θ h is the joint angle.

[0063] The above formula is deformed to obtain the following expression:

[0064]

[0065] S2. According to the system dynamic model, determine the extended state observer (ESO).

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

[0067]

[0068] Among them, τ h is the input of the controlled object, θ h is the system output, d is the total system disturbance, which is expanded into a state variable of the system. The state space equation of this system is described as follows:

[0069]

[0070] y = Cx

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

[0072]

[0073] After expanding the state space equation, the following state space expression is obtained:

[0074]

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

[0076]

[0077] Among them, is the gain matrix of the observer, is the estimated value of the state variable. Expanding the above formula, the following observer algorithm is obtained:

[0078]

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

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

[0081] In a traditional feedback control system, a fixed linear feedback gain is usually adopted. However, in practical applications, the control error of the system is small near the equilibrium point. At this time, a larger gain is often required to enhance the disturbance rejection ability of the system; while when it is far from the equilibrium point, in order to suppress the overshoot of the system, a smaller gain is needed. The linear feedback gain uses the same gain value in all error ranges and it is difficult to meet these requirements. To solve this problem, in this application, a nonlinear feedback control strategy is adopted.

[0082] Specifically, the arctangent function is selected as the core of the nonlinear feedback because the arctangent function has a large slope when approaching zero, which can increase the feedback strength, and the slope is small when far from zero, thus naturally reducing the feedback strength. This characteristic is consistent with the system control requirements. Therefore, the nonlinear feedback control law can be determined as:

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

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

[0085] The extended state observer can estimate the disturbance of the system, but its estimation ability is related to the gain parameters β 1 、β 2 、β 3It is related. Increasing the observer gain usually increases its bandwidth, making it more sensitive to observing disturbances in the system. However, a high bandwidth reduces the system's resistance to high-frequency noise. In the case of low bandwidth, the response time to observe large disturbances is long, which will cause a certain estimation lag.

[0086] There is usually noise in the actual system. Therefore, the bandwidth of the extended state observer needs to be limited to avoid being too sensitive to noise. But this also results in a lag in the estimation of large disturbances by the extended state observer, thus affecting the control accuracy. To reduce this error, a low-pass filter or a Kalman filter can be used to reduce the disturbance amplitude of the observed part, thereby weakening the lag effect and improving the 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 a known part and an unknown part. For the known part of the disturbance, it is compensated through the system model information. For the unknown part of the disturbance, it is estimated through the extended observer. This can reduce the workload of the extended state observer, enabling it to still work well at a lower gain without significantly reducing the control performance of the system.

[0089] By compensating for the known disturbance, the estimation error of the extended state observer can be effectively reduced, improving the overall stability and accuracy of the system. This method, as Figure 3 shown, uses some known model information to compensate for the system disturbance, reducing the error of the extended state observer and further improving the control effect of the system.

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

[0091] are compensated, thereby reducing the burden on the extended state observer. The improved control law is:

[0092]

[0093] Among them, τ h represents the control torque, K p and K d are control gain parameters, e is the control error, I his the inertial term, g(θ d ) represents the gravity model term, C h θ d represents the damping term. This control law uses the known gravity and damping models for disturbance compensation, effectively improving the control accuracy of the system under disturbance conditions. When the damping modeling is relatively accurate, the control effect is mainly affected by the accuracy of gravity modeling and the estimation error of the actual disturbance by the extended state observer.

[0094] As Figure 5 shown, through MATLAB simulation, it can be obtained that with the improvement of the accuracy of the known disturbance modeling, the extended state observer can effectively reduce the disturbance estimation error without increasing the observer gain. The extended state observer gains are shown in Table 1.

[0095] Table 1 Different observer gain parameters

[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 the simulation Figure 6 it can be seen that with the improvement of the accuracy of the known disturbance modeling, 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 a significant improvement in control performance.

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

[0099]

[0100] where, K p and K d are control gain parameters, B ff1 , B ff2 and B ff3 are acceleration feedforward gain coefficients. Its control block diagram is as Figure 4 shown. The controller proposed by the present invention can effectively compensate for the control error generated in the region with a large acceleration and improve the response accuracy of the system.

[0101] S5. Perform position control based on the determined feedforward compensation cascade nonlinear feedback controller with an extended state observer based on active disturbance rejection.

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

[0103] Flexible joints are usually used for joint settings of collaborative robotic arms. Since collaborative robotic arms need to adapt to various working environments, when setting the position controller of flexible joints, it is necessary to fully consider their performance under different working conditions. First, we studied the position tracking control performance of flexible joints under the condition of a fixed load at the output end. Since the working environment of flexible joints is often uncertain and the load may also change, the position tracking control ability under load variation was also investigated. Finally, we tested the anti-interference performance of the position controller when dealing with sudden load disturbances.

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

[0105] The test environment is as Figure 7 shown. First, the test was carried out under the condition of constant load. The output end of the joint was equipped with a link load, where the weight of the link load was 0.5 kg, its own weight was 0.9 kg, and the length was 0.8 m. The zero position of the joint was set vertically downward so that the output end of the joint could 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 Figures 8 to 10 shown, where the dotted line represents the reference position trajectory and the solid line represents the actual position trajectory.

[0106] From Figure 8 and Figure 9 it can be clearly seen that compared with the PD controller, the cascade controller based on active disturbance rejection has a significant improvement in steady-state and dynamic control accuracy. The average value of the absolute position error is reduced from 0.0735 rad to 0.0027 rad, and there is almost no overshoot and oscillation phenomenon during the movement process. Therefore, the position controller proposed in the present invention can achieve good position trajectory tracking performance under the condition of fixed load.

[0107] Figure 10 shows the test results after increasing the load to 1.0 kg without changing the controller parameters. It can be seen from the figure that even when the load changes, the position controller still has good tracking performance, indicating that the controller has a certain adaptability to load changes.

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

[0109] The application environment of collaborative robots usually has high uncertainty, and the load at the output end of flexible joints is often not constant. In order to test the position control performance of flexible joints under sudden load changes, in this experiment, a sudden load change test was set up to manually increase a certain amount of load when the flexible joint moved to the horizontal position. The experimental results are as Figure 11as shown

[0110] In Figure 11 it, the dashed line represents the position reference signal, and the solid line is the actual output position of the flexible joint. When the joint moves to the horizontal position, an additional load of 0.5 kg is suddenly added on the basis of the original load of 0.5 kg. 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 investigate the position control performance under sudden load changes, a load of 1 kg was continuously added in the experiment. The results show that the position deviation is more obvious than before, but it can also return to near the reference signal within a certain period of time. Thus, it can be seen that the position controller proposed by the present invention exhibits good compliance under sudden load change conditions, has strong anti-interference ability, and can effectively resist the disturbance influence brought by load changes.

[0112] The above shows and describes 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 form. Any technical solutions obtained by using equivalent replacements or equivalent transformations fall within the protection scope of the present invention.

Claims

1. A robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method, characterized in that: The following steps are involved: S1. Establish a dynamic model for the robot's flexible joint system; S2. Determine the ADRC extended state observer according to the system dynamics model; S3, determining a nonlinear feedback controller for the system; S4, determining a feedforward compensation cascade nonlinear feedback controller based on an auto-disturbance rejection extended state observer; S5. Position control is performed based on a determined feedforward compensation cascade nonlinear feedback controller based on an ADRO extended state observer.

2. The robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method according to claim 1, characterized in that: In step S1, the mathematical description of the kinetic model is as follows: In the formula, τ h Indicates the control torque, I h is the inertia term, g(θ h ) represents the gravity model term, represents the damping term, δ is the unknown disturbance, θ h is the joint angle; Transforming the above formula, we get the following expression:

3. The robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method according to claim 2, characterized in that: In step S2, the unknown disturbance δ in the load is taken as the observation target, and the definition is Among them, τ h is the input of the control object, θ h is the system output, d is the total disturbance of the system, which is expanded into a state variable of the system. The state space equation of the system is described as follows: y=Cx In the formula, the system matrix A, input matrix B, disturbance input matrix E and state variable x are defined as follows: After expanding the state space equation, we get the following state space expression: According to the PBH criterion, the definition Since the rank of Q is equal to 3 and equal to the dimension of the state space, the system is observable and the extended state observer is constructed as shown in the following formula: in, is the observer gain matrix, is the estimated value of the state variable, expanding the above formula to obtain the following observer algorithm: When the extended state observer gain satisfies A-GC<0 and When , the observation error of the extended state observer will converge to zero.

4. The robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method according to claim 3, characterized in that: In step S3, the inverse tangent function is selected as the core of the nonlinear feedback, and the nonlinear feedback control law is: u=K p arcane(e)+K d teacher Among them, K p and K d is the control gain parameter, and e is the control error.

5. The robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method according to claim 4, characterized in that: In step S4, the disturbance is divided into a known part and an unknown part. The disturbance of the known part is compensated by the system model information, and the disturbance of the unknown part is estimated by the extended observer.

6. The robot flexible joint self-disturbance rejection feedforward compensation cascade nonlinear position control method according to claim 5, characterized in that: In step S4, the total disturbance is The total disturbance consists of two parts. The known disturbance gravity g(θ h ) and damping The unknown disturbance δ includes model parameter errors and load changes. Based on the use of the extended state observer for disturbance compensation, the known disturbance gravity g(θ h ) and damping After compensation, the improved control law is: Among them, τ h Indicates the control torque, K p and K d is the control gain parameter, e is the control error, I h is the inertia term, g(θ d ) represents the gravity model term, C h θ d represents the damping term.

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

Images