A 6-degree-of-freedom robot arm disturbance compensation control method based on an iterative learning observer

Abstract

The application discloses a 6-DOF mechanical arm disturbance compensation control method based on an iterative learning observer, and steps are as follows: taking a visual servo system as a research object, the system comprises a mechanical arm, a sensing part and a target object, a state space model is constructed based on a depth-independent Jacobian matrix model with joint speed disturbance; an auxiliary state system and an error transmission equation are constructed based on joint sensor information, an iterative learning observer is designed to estimate joint speed disturbance; a disturbance estimation value is embedded into a visual model predictive controller to generate an optimal control signal, joint speed input is corrected through a feedforward compensation mechanism, and active interference suppression is realized. The application can improve the tracking response speed of sudden disturbance, maintain system stability in the case that joint speed exists interference, and thus realizes a visual servo task of tracking an expected track.

Description

Technical Field

[0001] This invention belongs to the field of visual servo technology for robotic arms, specifically relating to a disturbance compensation control method for a 6-DOF robotic arm based on an iterative learning observer. Background Technology

[0002] In the field of robot vision servo control, kinematic model-based designs are widely used in tasks such as grasping, assembly, and guidance due to their relatively simple structure, high computational efficiency, and ability to effectively utilize image features to directly generate control laws. These methods typically rely on the mapping relationship (i.e., the image Jacobian matrix) between target feature information acquired by a camera (such as image coordinates and distances between feature points) and the robot's joint space kinematics to drive the robotic arm's movement, enabling it to track the desired trajectory in image space. However, when the system performs tasks in a dynamic real-world environment, designs based on idealized kinematic assumptions face significant challenges. External disturbances (such as unexpected movement or vibration of the target, slight shaking of the camera platform, or even micro-movements of the robotic arm caused by human contact) can disrupt the stable acquisition of image features or alter the pre-defined kinematic mapping relationship. These disturbances directly contaminate the image feature signals or equivalently introduce unmodeled dynamics at the kinematic level, leading to deviations in the calculated control commands, severely weakening the system's trajectory tracking accuracy and robustness, and even causing servo task failure.

[0003] In existing technologies, while robust control theory-based methods (such as sliding mode control and adaptive control) can enhance the system's robustness to bounded disturbances, they often require the introduction of high-gain feedback or complex online parameter update laws. High gain can easily amplify image noise or induce system chattering, while complex adaptive mechanisms may increase computational burden and reduce system response speed. When traditional disturbance observers (DOBs) or state observers are applied in visual servoing, their design typically relies heavily on accurate system dynamics models. When relying solely on kinematic models, the lack of effective dynamic state information makes it difficult for traditional observers to accurately estimate and isolate external disturbances acting on the visual servoing loop (disturbances may manifest as changes in image features or joint velocity / position levels).

[0004] Therefore, for the external interference problem faced by kinematic model-based visual servoing systems in dynamic execution environments, there is an urgent need for an innovative interference suppression method that can observe and reconstruct the external interference signals acting on the visual servoing loop in real time without relying on an accurate dynamic model; based on high-precision interference observations, dynamically generate feedforward compensation terms, directly inject them into the control law, and actively counteract the influence of interference on image feature tracking or equivalent joint motion. Summary of the Invention

[0005] To address the aforementioned problems in existing technologies, this invention proposes a disturbance compensation control method for a 6-DOF robotic arm based on an iterative learning observer. The method is rationally designed, overcomes the shortcomings of existing technologies, and achieves good results.

[0006] The present invention adopts the following technical solution:

[0007] A disturbance compensation control method for a 6-DOF robotic arm based on an iterative learning observer includes the following steps:

[0008] Step 1: Taking the visual servoing system as the research object, the system includes a robotic arm, a sensing part and a target object. A state space model is constructed based on the depth-independent Jacobian matrix model with joint velocity perturbation.

[0009] Step 2: Construct an auxiliary state system and error propagation equation based on joint sensor information, and design an iterative learning observer to estimate joint velocity perturbations;

[0010] Step 3: Embed the disturbance estimate into the visual model predictive controller to generate the optimal control signal, and correct the joint velocity input through the feedforward compensation mechanism to achieve active disturbance suppression.

[0011] Furthermore, the characteristic is that step 1 specifically comprises:

[0012] First, feature points are constructed. The formula for the coordinates of feature points in pixel coordinates is:

[0013]

[0014] Among them, y=(u,v) T y is the coordinate of the feature point in the pixel plane; c z(t) represents the depth of the feature point in camera coordinates. For the i-th row of C, It is the product of the intrinsic parameter matrix and the extrinsic parameter matrix. p represents the homogeneous transformation matrix of the robot arm's end effector coordinate system relative to the base coordinate system. b Represents the three-dimensional coordinates of visual feature points in the robot arm's base coordinate system;

[0015] By performing time differentiation on the coordinates of the feature point in pixel coordinates, the relationship between the rate of change of the feature point's image coordinates and the speed of the robotic arm joints is expressed as:

[0016]

[0017] Where t is continuous time. For the joint speed of the robotic arm, and A(y,q(t)) is a depth-independent Jacobian matrix, and its expression is as follows:

[0018]

[0019] in, To indicate the derivative sign, q(t) represents the joint angle of the robotic arm;

[0020] To design the control law using predictive model control, a discrete-time state-space model was established. Considering the disturbances in the joint velocities of the robotic arm, the discrete-time state-space model is expressed as:

[0021]

[0022] Where u(k) is the controller input signal, z(k) is the camera depth, and T e v is the sampling time. u This is a speed disturbance signal.

[0023] Furthermore, step 2 specifically includes:

[0024] set up The measured value of joint velocity, v m Represented as:

[0025]

[0026] For v m Differentiating, we get:

[0027]

[0028] Let x1 = q and Introduce an auxiliary variable x a Construct an auxiliary state system, represented as:

[0029]

[0030] Among them, K r It is a coefficient matrix, let x e =x a -x1, for x e Taking the derivative, we obtain the error propagation equation:

[0031]

[0032] x e =x a Substituting -x1 into equation (8) yields:

[0033]

[0034] In equation (10), the joint angle x1 and the joint velocity vm It is measurable, x a The auxiliary state x is calculated using formula (10) and updated in each control cycle. a Then through x e =x a -x1 can obtain x e ;

[0035] An iterative learning observer is designed. Through an iterative update mechanism, combining historical estimation information and the error propagation equation, a high-precision estimation of the system state and unknown disturbances is achieved. The expression is:

[0036]

[0037] in, It is x e The estimate of (k), It is v u The estimate of (k), where T is the time interval, K r K v L1 and L2 are coefficient matrices;

[0038] The first equation in equation (11) is the state update equation, based on the estimated state at the current time. and disturbance estimation By introducing the feedback gain matrix K r Adjust the dynamic characteristics of the system and use the learned gain matrix L1 to adjust the actual state x. e (k) is corrected for the deviation from the estimated state to generate the state estimate for the next time step. The second equation in equation (11) is the perturbation estimation update equation, which is obtained through historical perturbation estimation. and feedback gain matrix K v By combining the learning gain matrix L2 of the current state estimation error, the unknown disturbance v can be addressed. u Iterative correction of (k);

[0039] The iteration ends when the perturbation estimation error in the i-th iteration can be reduced to the preset error boundary.

[0040] Furthermore, step 3 specifically includes:

[0041] Based on formula (5), the quadratic cost function J(k) of the visual model predictive controller with joint velocity perturbation is defined as:

[0042]

[0043] Where, matrix Q≥0, R≥0, F≥0 is the weight matrix of each term, e is the visual servoing image error, i is the prediction time domain, i=0,1,…,N-1, N is a finite positive integer, and is the prediction time domain of the model predictive controller, e(k+i|k)=y(k+i|k)-r d r d It is the expected feature point trajectory;

[0044] The optimization problem is described as follows:

[0045]

[0046] in, u min u max To control input constraints, y min y max For image coordinate constraints, r is the desired image coordinate;

[0047] The joint velocity perturbation signal estimated in real time by the iterative learning observer. Embedded state prediction equations are used to generate future state sequences y(k+i|k) based on the state prediction equations with perturbation estimation within the prediction time domain i = 0, 1, ..., N-1, and a cost function J(k) is constructed from these sequences. At each sampling time, the cost function J(k) is minimized, and the above constraints are considered to obtain the optimal control sequence. And at each sampling time, the first term of the optimal control sequence is... As a control signal;

[0048] By embedding the iterative learning mechanism into the MPC feedforward compensation loop, and using the iterative learning observer to observe joint velocity disturbances, combined with the MPC control input, the optimal control signal u(k) is obtained as follows:

[0049]

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

[0051] This invention provides a perturbation compensation control method for a robotic arm based on an iterative learning observer. By constructing a depth-independent Jacobian matrix model of the robotic arm, an MPC control scheme for compensating joint velocity perturbations is designed. Its advantages include: 1) Constructing a kinematic model of the robotic arm based on the depth-independent Jacobian matrix, establishing a state-space equation including joint velocity perturbation terms, and generating control commands through online rolling optimization, achieving a visual servo control architecture without explicit depth parameters; 2) Designing an iterative learning observer, and achieving accurate estimation of joint velocity perturbations by constructing an auxiliary state system and error propagation equations. The observer integrates iterative learning from historical data with real-time state feedback, significantly improving the tracking response speed to sudden perturbations; 3) Embedding the perturbation estimate into the model predictive control framework, and correcting the joint velocity input through a feedforward compensation mechanism, achieving the dual objectives of control optimization and disturbance suppression. This architecture can maintain system stability even with partial sensor failure, thereby realizing the visual servo task of tracking the desired trajectory. Attached Figure Description

[0052] Figure 1 This is a block diagram of the visual servo system of the present invention;

[0053] Figure 2 This is a flowchart of the control method of the present invention;

[0054] Figure 3 This is a feature point image trajectory diagram of the present invention;

[0055] Figure 4 This represents the velocity disturbance of joint 1 and its estimated value.

[0056] Figure 5 For the velocity disturbance of joint 2 and the estimated value of the disturbance; Detailed Implementation

[0057] The specific embodiments of the present invention will be further described below with reference to specific examples:

[0058] A disturbance compensation control method for a 6-DOF robotic arm based on iterative learning observers, such as... Figure 2 As shown, it includes the following steps:

[0059] Step 1: Taking the visual servoing system as the research object, the system includes a robotic arm, a sensing part and a target object. A state space model is constructed based on the depth-independent Jacobian matrix model with joint velocity perturbation.

[0060] This invention is based on an eye-in-hand vision system, where a camera is mounted on the end effector of a robotic arm, such as... Figure 1As shown, it is mainly divided into three parts. The first part is the robotic arm, which is the main execution and controlled part. After the controller generates input commands, the robotic arm performs the motion operations to complete the servo task. The second part is the perception part, which uses a camera to acquire images of objects in space and transmit them to the computing platform for processing. The main task is to extract and locate feature points in the acquired images. After localization, feature point matching is needed between the desired image and the current image to construct the task function and control law. The third part is the target object, a three-dimensional object in space, whose information needs to be acquired by the camera to construct feature points.

[0061] The process of building a visual servoing system includes first constructing feature points. The formula for the coordinates of feature points in pixel coordinates is:

[0062]

[0063] Among them, y=(u,v) T y is the coordinate of the feature point in the pixel plane; c z(t) represents the depth of the feature point in camera coordinates. For the i-th row of C, It is the product of the intrinsic parameter matrix and the extrinsic parameter matrix. p represents the homogeneous transformation matrix of the robot arm's end effector coordinate system relative to the base coordinate system. b Represents the three-dimensional coordinates of visual feature points in the robot arm's base coordinate system;

[0064] By performing time differentiation on the coordinates of the feature point in pixel coordinates, the relationship between the rate of change of the feature point's image coordinates and the speed of the robotic arm joints is expressed as:

[0065]

[0066] Where t is continuous time. For the joint speed of the robotic arm, and A(y,q(t)) is a depth-independent Jacobian matrix, and its expression is as follows:

[0067]

[0068] in, To indicate the derivative sign, q(t) represents the joint angle of the robotic arm;

[0069] A model predictive controller is designed using a depth-independent Jacobian matrix model, and the optimal input and optimal state sequences are solved under constraints. The input of the depth-independent Jacobian matrix model is joint velocity, and the output is pixel plane coordinates.

[0070] In order to design the control law using the predictive model control method, a discrete-time state-space model was established to replace the continuous-time model (2). The discrete-time state-space model of each feature point can be expressed as:

[0071]

[0072] Where u(k) is the controller input signal, and also the joint speed of the robotic arm. The actual joint speed of the robotic arm may be disturbed by aging, interference, etc., so a joint speed disturbance term v is introduced. u Furthermore, by incorporating the properties of depth-independent Jacobian matrices, a discrete-time model that better reflects real-world scenarios was constructed. Considering the joint velocity perturbations of the robotic arm, the discrete-time state-space model is expressed as:

[0073]

[0074] Where z(k) is the camera depth, T e v is the sampling time. u This is a speed disturbance signal.

[0075] Due to the depth independence of the Jacobian matrix A(k), the system model does not need to rely on complex environmental depth information, reducing the complexity of disturbance separation and compensation. Furthermore, in disturbed scenarios, this characteristic ensures that the controller only needs to focus on the disturbance v of the input channel. u This eliminates the need to handle coupling interference caused by depth uncertainty.

[0076] Step 2: Construct an auxiliary state system and error propagation equation based on joint sensor information, and design an iterative learning observer to estimate joint velocity perturbations;

[0077] In the implementation of robot feedback control systems, joint position feedback can usually be reliably obtained through the encoder built into the motor. However, the dynamic high-precision measurement of joint velocity often faces challenges. When there are disturbances in the joint velocity (typical forms include: fixed offset, time-varying drift, pulse interference, and periodic noise), the feedback signal will be distorted. This distortion, once introduced into the closed-loop control law, will directly affect the calculation accuracy of torque / current commands, thereby causing trajectory tracking deviations, system oscillations, or even instability, severely restricting the realization of high-speed, high-precision control tasks.

[0078] Step 2 is as follows:

[0079] set up The measured value of joint velocity, v m Represented as:

[0080]

[0081] For v m Differentiating, we get:

[0082]

[0083] Let x1 = q and Introduce an auxiliary variable x a Construct an auxiliary state system, represented as:

[0084]

[0085] Among them, K r It is a coefficient matrix, let x e =x a -x1, for x e Taking the derivative, we obtain the error propagation equation:

[0086]

[0087] x e =x a Substituting -x1 into equation (8) yields:

[0088]

[0089] In equation (10), the joint angle x1 and the joint velocity v m It is measurable, x a The auxiliary state x is calculated using formula (10) and updated in each control cycle. a Then through x e =x a -x1 obtains x e .

[0090] Therefore, rebuild v u The work can be changed to use only x e To estimate the system state, an iterative learning observer is designed. This observer, through an iterative update mechanism, combines historical estimation information with the error propagation equation to achieve high-precision estimation of the system state and unknown disturbances. The expression is:

[0091]

[0092] in, It is x e The estimate of (k), It is v u The estimate of (k), where T is the time interval, K r K v L1 and L2 are coefficient matrices;

[0093] The first equation in equation (11) is the state update equation, based on the estimated state at the current time. and disturbance estimation By introducing the feedback gain matrix K r Adjust the dynamic characteristics of the system and use the learned gain matrix L1 to adjust the actual state x. e (k) is corrected for the deviation from the estimated state to generate the state estimate for the next time step. The second equation in equation (11) is the perturbation estimation update equation, which is obtained through historical perturbation estimation. and feedback gain matrix K v By combining the learning gain matrix L2 of the current state estimation error, the unknown disturbance v can be addressed. u Iterative correction of (k);

[0094] By reasonably selecting the parameters in the perturbation estimation observer that ensure the eventual boundedness of the perturbation estimation error, the iteration ends when the perturbation estimation error in the i-th iteration can be reduced to the preset error boundary.

[0095] Step 3: Embed the disturbance estimate into the visual model predictive controller to generate the optimal control signal, and correct the joint velocity input through the feedforward compensation mechanism to achieve active disturbance suppression.

[0096] This invention establishes a kinematic model of a 6-DOF robotic arm based on a depth-independent Jacobian matrix (i.e., formula (2)), establishes a state-space model with joint velocity perturbations, embeds the perturbation term into the prediction model, and generates ideal control quantities through rolling optimization. Real-time acquisition of velocity sensor data, fusion of historical disturbance information and current state error, and online reconstruction of the disturbance signal through iterative learning of the observer. And it compensates for joint speed.

[0097] Step 3 specifically includes:

[0098] Based on formula (5), the quadratic cost function J(k) of the visual model predictive controller with joint velocity perturbation is defined as:

[0099]

[0100] Where, matrix Q≥0, R≥0, F≥0 is the weight matrix of each term, e is the visual servoing image error, i is the prediction time domain, i=0,1,…,N-1, N is a finite positive integer, is the prediction time domain of the model predictive controller, and the cost function includes a finite time domain stage cost to describe the control performance index (state performance and energy index), and a terminal cost function to constrain the terminal state in the prediction time domain to ensure system stability. This can be represented as a stage cost function, e(k+N|k). T Fe(k+N|k) represents the terminal cost, e(k+i|k)=y(k+i|k)-r d rd It is the expected feature point trajectory;

[0101] The optimization problem is described as follows:

[0102]

[0103] in, u min u max To control input constraints, y min y max For image coordinate constraints, r is the desired image coordinate;

[0104] The joint velocity perturbation signal estimated in real time by the iterative learning observer. The embedded state prediction equation, i.e., in formula (13)

[0105] Within the prediction time domain i = 0, 1, ..., N-1, a future state sequence y(k+i|k) is generated based on the state prediction equation containing perturbation estimation, and a cost function J(k) is constructed using this sequence. At each sampling time, the cost function J(k) is minimized, and considering the above constraints, the optimal control sequence is obtained. And at each sampling time, the first term of the optimal control sequence is... As a control signal;

[0106] Embedding an iterative learning mechanism into the MPC feedforward compensation loop, the dynamic suppression capability against periodic disturbances is significantly improved through the synergy of dual closed loops (state estimation closed loop + disturbance learning closed loop). Using an iterative learning observer to observe joint velocity disturbances, combined with the MPC control input, the optimal control signal u(k) is obtained as follows:

[0107]

[0108] This invention utilizes an iterative learning observer to detect and reconstruct the joint velocity perturbations of a robotic arm. This method compensates for joint velocity disturbance signals, thereby correcting the input signal of the model predictive controller. While ensuring control accuracy, it significantly reduces the risk of visual servoing task failure caused by disturbances.

[0109] Additive perturbations are introduced into the velocities of the first and second joints, and perturbation estimation is performed using an iteratively learned observer. The estimation results are as follows: Figure 4 and Figure 5 As shown, the iterative learning observer can quickly detect the perturbation that occurs in the fifth second, ultimately achieving the tracking of the desired trajectory for the visual servoing task. The trajectory tracking results in the image plane are as follows. Figure 3 As shown.

[0110] Of course, the above description is not intended to limit the present invention, and the present invention is not limited to the examples given above. Any changes, modifications, additions or substitutions made by those skilled in the art within the scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. A disturbance compensation control method for a 6-DOF robotic arm based on an iterative learning observer, characterized in that, Includes the following steps: Step 1: Taking the visual servoing system as the research object, the system includes a robotic arm, a sensing part and a target object. A state space model is constructed based on the depth-independent Jacobian matrix model with joint velocity perturbation. Step 2: Construct an auxiliary state system and error propagation equation based on joint sensor information, and design an iterative learning observer to estimate joint velocity perturbations; Step 3: Embed the disturbance estimate into the visual model predictive controller to generate the optimal control signal, and correct the joint velocity input through the feedforward compensation mechanism to achieve active disturbance suppression; Step 1 specifically involves: First, feature points are constructed. The formula for the coordinates of feature points in pixel coordinates is: (1)； in, , These are the coordinates of the feature point in the pixel plane; , This represents the depth of the feature point in camera coordinates. for The OK, It is the product of the intrinsic parameter matrix and the extrinsic parameter matrix. This represents the homogeneous transformation matrix of the robot arm's end effector coordinate system relative to the base coordinate system. Represents the three-dimensional coordinates of visual feature points in the robot arm's base coordinate system; By performing time differentiation on the coordinates of the feature point in pixel coordinates, the relationship between the rate of change of the feature point's image coordinates and the speed of the robotic arm joints is expressed as: (2)； in, For continuous time, For the joint speed of the robotic arm, and ; It is a depth-independent Jacobian matrix, and its expression is as follows: (3)； in, For the sign of differentiation, For the joint angle of the robotic arm; To design the control law using predictive model control, a discrete-time state-space model was established. Considering the disturbances in the joint velocities of the robotic arm, the discrete-time state-space model is expressed as: (5)； in, Input signals to the controller, For camera depth, Sampling time, For speed disturbance signals; Step 2 specifically involves: set up These are joint velocity measurements. Represented as: (6)； right Differentiating, we get: (7)； make and Introduce an auxiliary variable Construct an auxiliary state system, represented as: (8)； in, It is a coefficient matrix, let ,right Taking the derivative, we obtain the error propagation equation: (9)； Will Substituting into equation (8), we get: (10)； In equation (10), the joint angle and joint velocity It is measurable. The auxiliary state is calculated using formula (10) and updated in each control cycle. Then through able to obtain ; An iterative learning observer is designed. Through an iterative update mechanism, combining historical estimation information and the error propagation equation, a high-precision estimation of the system state and unknown disturbances is achieved. The expression is: (11)； in, yes The estimate, yes The estimate, It is a time interval. , , , It is a coefficient matrix; The first equation in equation (11) is the state update equation, based on the estimated state at the current time. and disturbance estimation By introducing a feedback gain matrix Adjusting the dynamic characteristics of the system and utilizing the learned gain matrix Regarding the actual state The deviation from the estimated state is corrected to generate the state estimate for the next time step. The second equation in equation (11) is the perturbation estimation update equation, which is obtained through historical perturbation estimation. and feedback gain matrix The learning gain matrix is ​​combined with the current state estimation error. To achieve the ability to handle unknown disturbances Iterative correction; When the When the perturbation estimation error decreases to the preset error boundary in the next iteration, the iteration ends. Step 3 specifically includes: Based on Equation (5), the quadratic cost function of the visual model predictive controller with joint velocity perturbation is... Defined as: (12)； Among them, matrix , , , is the weight matrix of each item. For visual servo image errors, To predict the time domain, , It is a finite positive integer, representing the prediction time domain of the model predictive controller. , It is the expected feature point trajectory; The optimization problem is described as follows: (13)； in, ; , To control input constraints, , For image coordinate constraints, The coordinates of the desired image; The joint velocity perturbation signal estimated in real time by the iterative learning observer. Embedded state prediction equations in the prediction time domain Within, a future state sequence is generated based on the state prediction equation containing perturbation estimation. And use this to construct the cost function At each sampling time, minimize the cost function. Taking into account the above constraints, the optimal control sequence is obtained. And at each sampling time, the first term of the optimal control sequence is... As a control signal; An iterative learning mechanism is embedded in the MPC feedforward compensation loop. The iterative learning observer is used to observe joint velocity disturbances, and combined with the MPC control input, the optimal control signal is obtained. for: (14)。

Images