Self-diagnosis and compensation method for variable load dynamics of robot end under quick-change form

Abstract

The present application relates to a kind of quick-change form under mechanical arm end variable load dynamics self-diagnosis and compensation method, its technical features are: based on iterative least square online identification to establish dynamics model;Variable load parameter identification under the condition based on Jacobian matrix mapping;Establish the hybrid dynamics model including connecting rod rigid body dynamics, connecting rod error compensation dynamics and load dynamics;Variable load dynamics model is established;When mechanical arm end load changes, the parameter in hybrid dynamics model is updated to control robot joint;Through real-time collection sensor data, online identification and compensation are carried out.The present application can effectively improve the accuracy and robustness of dynamic representation when robot faces multiple uncertain factors such as connecting rod deformation, variable load and measurement noise, improve the safety and reliability of physical human-computer interaction of robot, and can be widely used in industrial automation, medical operation assistance and rescue robot and other fields.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology and relates to a dynamic control method for multi-degree-of-freedom robotic arms, particularly a method for self-diagnosis and compensation of dynamics of variable load at the end effector of a robotic arm under a quick-change configuration. Background Technology

[0002] A robotic arm is a strongly coupled, nonlinear, multi-input multi-output system. The accuracy of the dynamic model parameters within the system directly impacts the performance of human-robot physical safety interaction. Due to the presence of unknown parameters in robot dynamics, establishing an accurate dynamic model is challenging; therefore, system identification becomes one of the best options for accurate evaluation. However, due to various complex uncertainties, such as flexible links, variable loads, assembly clearances, and measurement noise, robot dynamic models exhibit strong variability and unpredictability. These factors are mainly reflected in the dynamic characteristics of the robot system and friction recognition system, the accuracy of joint torque sensing, and the effectiveness of tactile perception and interactive control.

[0003] Currently, many researchers have conducted in-depth studies on robot dynamics modeling and identification methods. For example, to address the impact of nonlinear friction on high-precision motion systems, a feedforward compensator was designed based on the identified friction model to achieve nonlinear friction compensation. To solve the problem of identifying dynamic parameters under varying loads in multi-task environments, an adaptive dynamic parameter identification method combining a non-model-based adaptive control algorithm with recursive least squares was proposed to address the issue of poor control performance under complex varying load conditions. To address the problem that some parameters identified in the least squares method lack geometric or physical meaning, linear matrix inequality constraints and semidefinite optimization are used to ensure that the identified model parameters have practical physical meaning. In addition, many intelligent optimization algorithms have been widely applied to the identification of robot dynamic parameters, solving to some extent the problem of physically infeasible model parameters and laying a solid foundation for overcoming the challenge of obtaining accurate robot dynamic models.

[0004] However, existing research methods still have limitations. Current researchers have performed offline identification of load dynamic parameters based on replanned excitation trajectories; however, due to the more complex motion space constraints of robots in actual loading scenarios, the spatial conditions for safe operation of the loading excitation trajectory are usually unavailable. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the prior art and provide a method for self-diagnosis and compensation of dynamics of variable load at the end of a robotic arm in a quick-change mode. This method solves the problems of lack of coupling of time-varying factors and difficulty in dynamic compensation modeling and online accurate identification in the field of robot dynamics, thereby improving the accuracy of dynamic models.

[0006] The present invention solves the existing technical problems by adopting the following technical solution:

[0007] A method for self-diagnosis and compensation of dynamics of variable load at the end effector of a robotic arm in a quick-change configuration includes the following steps:

[0008] Step 1: Establish a dynamic model based on iterative least squares online identification;

[0009] Step 2: Identify load parameters under varying operating conditions based on Jacobian matrix mapping;

[0010] Step 3: Establish a hybrid dynamics model that includes link rigid body dynamics, link error compensation dynamics, and load dynamics;

[0011] Step 4: Establish a dynamic model of variable load;

[0012] Step 5: Determine the method for identifying new load dynamic parameters: When the load needs to be changed in the robot's working scenario, a step-by-step method is used to identify the load dynamic parameters.

[0013] Step 6: When the load at the end of the robotic arm changes, control the robot joints by updating the parameters in the hybrid dynamics model;

[0014] Step 7: Perform online identification and compensation by collecting sensor data in real time.

[0015] Furthermore, the implementation method of step 1 includes the following steps:

[0016] Step 1.1: Derive the rigid body dynamics model of the robotic arm link using the Newton-Euler method;

[0017] Step 1.2: Linearize the rigid body dynamics model;

[0018] Step 1.3: Recursively update the parameter estimates using the iterative least squares method.

[0019] Furthermore, the method for linearization in step 1.2 is as follows: extract the steady inertial parameter constant from the rigid body dynamics model of the robotic arm link, and establish a linearized reconstruction model of the rigid body dynamics of the robotic arm link.

[0020] Furthermore, the implementation method of step 2 includes the following steps:

[0021] Step 2.1: Determine the coupling relationships and decoupling methods of the load dynamics model;

[0022] Step 2.2: Identify load parameters under varying operating conditions using Jacobi matrix mapping.

[0023] Furthermore, the hybrid dynamics model τ established in step 3... efor:

[0024]

[0025] Where, τ e 'This is a dynamic model for link error compensation, τ load τ represents the additional joint torque generated by the end effector of the robotic arm when it is under load. link Represents joint torque, Δτ link q represents the error in the rigid body dynamics model of the link. and These represent joint position, velocity, and acceleration, respectively. This represents the joint inertial torque. The corresponding eccentric torque and Coriolis torque of the joint; G(q) represents the gravitational torque of the joint; τ f Y represents the joint friction torque. d Represents the observation matrix, Θ d Unidentified parameters are indicated, ΔΘd represents the parameters of the error compensation dynamic model, and Φ represents the parameters of the error compensation dynamic model. b E represents the set of minimum inertial parameters for a load. b Φ b The corresponding full-rank observation matrix.

[0026] Furthermore, the motion states of the robotic arm in the variable load dynamics model include:

[0027] When unloaded, the robot joints move from the zero position to the target position;

[0028] Under load, all robot joints maintain position servo mode while determining gravity and inertial parameters;

[0029] Under load, the robot joints move from the zero position to the target position.

[0030] Furthermore, the implementation method of step 6 is as follows: by starting the online identification program, the load gravity parameters are identified under static conditions using the recursive least squares method, these parameters are decoupled and updated, and then the identification process is judged to be completed by the set convergence index. Finally, these updated parameters are applied in the hybrid dynamics model.

[0031] Furthermore, step 7 employs recursive least squares to estimate and correct unknown parameters in the model, thereby achieving the compensation function of online identification and compensation of the dynamic parameters of the robotic arm.

[0032] Furthermore, the robotic arm is a 6-axis robotic arm.

[0033] The advantages and positive effects of this invention are:

[0034] 1. This invention establishes a hybrid dynamics model that includes link rigid body dynamics, link error compensation dynamics, and load dynamics, achieving comprehensive dynamic modeling and online compensation functions for robotic arms. It utilizes an improved recursive least squares method to identify and compensate for the dynamic parameters of the robotic arm in real time, significantly improving the control accuracy and robustness of the robotic arm when facing uncertainties such as link deformation, changing loads, and measurement noise. Experimental verification on a 6-DOF robotic arm platform demonstrates the effectiveness of the method under both unloaded and loaded conditions, and experimental data supports the theoretical analysis.

[0035] 2. This invention effectively improves the accuracy and robustness of the dynamic characterization of robots when facing various uncertainties such as link deformation, changing loads and measurement noise. It provides important theoretical and practical contributions to the safety and reliability of robot-human physical interaction, especially in applications requiring precise control, such as industrial automation, medical surgery assistance and rescue robots. Attached Figure Description

[0036] Figure 1 This is an overall flowchart of the self-diagnosis and compensation method for variable load dynamics at the end of a robotic arm according to the present invention;

[0037] Figure 2 This is a schematic diagram illustrating the principle of online identification and compensation of the rigid body dynamics model of the robotic arm link in this invention;

[0038] Figure 3 This is a flowchart of the step-by-step online identification of load dynamic parameters under variable load disturbance according to the present invention;

[0039] Figure 4 This is a simplified schematic diagram of the variable load at the end of the quick-change robotic arm according to the present invention.

[0040] Figure 5 This is a simplified schematic diagram of the 6-DOF robotic arm of the present invention. Detailed Implementation

[0041] The embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

[0042] This invention focuses on online accurate identification and compensation methods for dynamic models, aiming to solve the problems of lack of coupling of time-varying factors and difficulties in dynamic compensation modeling and online accurate identification in the field of robot dynamics. This invention is mainly applied to robotic arm systems under variable load working conditions. The system mainly includes a human-machine interface control system, a teaching platform, a 6-axis robotic arm, and a control system. The 6-axis robotic arm has higher degrees of freedom and flexibility, enabling it to perform more complex actions and tasks. Its movement in three-dimensional space is more precise, making it suitable for precision assembly and complex operations. It is also adaptable to more working environments, and is particularly important in automated production lines, improving production efficiency and product quality.

[0043] Based on the aforementioned variable load robotic arm system, this invention proposes a self-diagnosis and compensation method for the dynamics of variable load at the end effector of a robotic arm in a quick-change configuration, such as... Figures 1 to 5 As shown, it includes the following steps:

[0044] Step 1: Establish a dynamic model based on iterative least squares online identification. This dynamic model reduces identification error compared to traditional offline identification.

[0045] In this step, the robot's dynamic equations are first linearized to construct a linear state equation. Then, during the robot's operation, new joint state data are continuously collected, and the parameter estimates are recursively updated using the iterative least squares method. To improve the model's ability to track the latest dynamic characteristics, a forgetting factor is introduced to adjust the weights of new and old data. Finally, the predicted torque is calculated using the identified dynamic parameters and compared with the actual torque to obtain the model residuals. A compensation control strategy is then designed to reduce the impact of identification errors.

[0046] The specific implementation method for this step is as follows:

[0047] Step 1.1: The rigid body dynamics model of the robotic arm link is derived using the Newton-Euler method:

[0048]

[0049] Where, q, and These represent joint position, velocity, and acceleration, respectively; τ link Indicates joint torque; Indicates the joint inertial torque; The corresponding eccentric torque and Coriolis torque of the joint; G(q) represents the gravitational torque of the joint; τ f f represents the joint friction torque; c and f v These represent the Coulomb friction coefficient and the viscous friction coefficient, respectively; sign() represents the sign function.

[0050] Step 1.2: Linearize the dynamic equations, that is, linearize the rigid body dynamic model.

[0051] To simplify the parameter identification process, steady inertial parameter constants are extracted from the rigid body dynamics model of the robotic arm link, and a linearized reconstruction model of the rigid body dynamics of the robotic arm link is established, expressed as:

[0052]

[0053] In the formula This represents the observation matrix formed by combining the transformed motion state information; Θ represents the inertial parameter constant; Θ = [Θ1Θ2…Θ] i ], where Θ i Let be the inertial parameter constant of joint i.

[0054]

[0055] in, and Let represent the inertia tensor matrix of link i; and The mass moment of link i; m i Indicates the mass of link i; Let represent the Coulomb friction coefficient and viscous friction coefficient of connecting rod i, respectively.

[0056] Since the observation matrix Y is usually not full rank, it increases the complexity of identification. Therefore, QR decomposition or parameter recombination can be used to eliminate some inertial parameters and obtain the minimum inertial parameter set Θ. b .

[0057]

[0058] In the formula Θ b Y represents the minimum set of inertial parameters. b This represents the full-rank observation matrix.

[0059] Step 1.3: Recursively update the parameter estimates using the iterative least squares method.

[0060] Currently, the most widely used method for offline system identification and unknown parameter fitting is based on the least squares method. The least squares method estimates parameters by minimizing the sum of squared prediction errors, thus obtaining the statistically optimal parameter fitting result. The process of identifying the minimum set of inertial parameters based on the least squares method can be described by the following formula:

[0061]

[0062] Step 2: Identify load parameters under varying operating conditions based on Jacobi matrix mapping.

[0063] Assuming that the linear regression reconstruction of the robotic arm's dynamics model and the extraction of the minimum inertial parameter set have been completed at the current moment, the expression for the joint torque in terms of unknown parameters and the observation matrix is ​​obtained:

[0064] τ d =Y d ·Θ d

[0065] In the formula τ d Represents joint torque; Θ d Indicates an unrecognized parameter; Y d The above represents the observation matrix, and all of them are parameters in the recursive identification process.

[0066] Based on the principle of recursive least squares with a forgetting factor, by introducing a forgetting factor Z... N] By increasing the weight ratio of newly sampled data and decreasing the weight ratio of old data, we can derive a parameter identification method using recursive least squares with a forgetting factor.

[0067]

[0068] In the formula D N ] represents the gain matrix; Z N W represents the forgetting factor. N ] represents the covariance matrix.

[0069] Since offline parameter identification is difficult to accurately characterize the dynamic characteristics of the robotic arm links, and considering the error in characterizing the dynamic characteristics of the robotic arm links, a link compensation torque representation model related to the motion state information of the robotic arm is established.

[0070]

[0071] In the formula Δτ link ΔΘd represents the error in the rigid body dynamics model of the link; ΔΘd represents the parameter of the error-compensated dynamics model.

[0072]

[0073] Furthermore, a dynamic model of a rigid body with connecting rods and a dynamic model τ for connecting rod error compensation were established. e Hybrid dynamics model of the robotic arm:

[0074]

[0075] In the formula τ link It was obtained during the offline identification process in step 1.

[0076] Step 3: Establish a hybrid dynamics model that includes rigid body dynamics of the connecting rod, error compensation dynamics of the connecting rod, and load dynamics. This hybrid dynamics model can improve the accuracy of the dynamics model under multivariable uncertain disturbances.

[0077] In this step, a compensating dynamic model is designed to address the deformation and modeling errors of the link. Then, the Jacobian matrix is ​​used to map the load dynamics from the end effector dynamics to the joint space, establishing a decoupled load dynamics model. A framework for characterizing the dynamics of the robotic arm is proposed, including link rigid body dynamics, link error compensation dynamics, and load dynamics.

[0078] When the robotic arm moves under load, the force / torque W acting on the end effector of the robotic arm can be mapped to the joint space using the Jacobian matrix J(q). This allows the calculation of the additional load torque τ generated at each joint.

[0079]

[0080] In the formula τ load J represents the additional joint torque generated when the end effector of the robotic arm is under load; T (q) represents the Jacobian matrix R. n×6 ;F load and N load These represent the force and torque vectors acting on the end effector of the robotic arm, respectively.

[0081] Based on dynamic analysis, the expressions for the force and torque acting on the end effector of the robotic arm are as follows:

[0082]

[0083] In the formula Indicates the angular velocity of the end effector; This represents the angular acceleration of the end effector; This indicates the linear velocity of the end effector; This represents the linear acceleration of the end effector; m l Indicates load quality; Indicates the location of the load's center of mass; Indicates that the center of mass of the load is in X l ,Y l Z l Position on the axis; I l Let be the inertial tensor matrix at the center of mass of the load; These represent the loads around X. l ,Y l Z l Mass and moment of inertia of the coordinate axes; These represent the load at X. l Yl Y l Z l Z l X l Moment of inertia on a plane.

[0084] The linearized recombination expression for the force and torque acting on the end effector of the robotic arm can be simplified as follows:

[0085]

[0086] In the formula These represent the quantities in the force and torque generated by the load of the robotic arm that are independent of the load's moment of inertia parameter, respectively; Φ represents the load's moment of inertia constant.

[0087] At this point, a linearized reconstruction model of the dynamic load on the robotic arm can be obtained:

[0088]

[0089] In the formula This represents the load observation matrix.

[0090] Typically, in the observation matrix E, some columns consist entirely of zeros or are linearly dependent, leading to the presence of parameters in the corresponding load inertia parameter set Φ that cannot be completely independently identified. Similarly, the QR decomposition method can yield the minimum set of inertia parameters for the load. In this case, the load dynamic model can be expressed as:

[0091]

[0092] In the formula Φ b E represents the set of minimum inertial parameters for a load. b Φ b The corresponding full-rank observation matrix.

[0093] In addition to gravitational loads, inertial loads also play a role in motion. Typically, robotic arm manufacturers only provide interfaces for correcting the dynamic inertial parameters of the load's gravity term, neglecting the coupling relationship between the gravity and inertial terms. However, in high-precision robotic arm control, the influence of the load's inertial term must also be considered. This paper proposes a novel method for identifying dynamic load parameters, which involves identifying the dynamic load parameters step-by-step when the load needs to be changed during the robotic arm's operation.

[0094] To facilitate parameter identification, it is best to specify the parameter to be identified as Φ. b The inertial parameters related to gravity are separated and collected into a single vector Φ. g , Similarly, the observation matrix representing the load gravity term can be extracted and used with vector E g Therefore, the following relationship holds:

[0095] G load (q)=E g (q)·Φ g

[0096] It is easy to see that when the robotic arm is not subjected to external forces, all joints are in position servo mode, i.e., τ ext =0、 At that time, the measured value of the joint torque sensor is equal to the torque provided by the link gravity term and the load gravity term.

[0097] τ load =G link (q)+G load (q)

[0098] In the formula, τ load G represents the measured value of the joint torque sensor under varying load conditions. link (q) represents the joint torque generated by the link gravity term, G load (q) represents the joint torque generated by the load gravity term.

[0099] The dynamic parameters of the link have been determined previously. The gravitational term G of the link can be obtained by using the rigid body dynamics model of the robotic arm link. link (q). This allows for the loading of the gravity term load G. load (q) Decoupling from joint torque. Therefore, in static equilibrium without external forces, recursive least squares can be used to identify dynamic parameters related to the load gravity term, thereby obtaining a more accurate estimate of the load gravity term parameters.

[0100] Indicators C1 and C2 define the load gravity term and the inertia term.

[0101]

[0102] in, and This indicates the trigger threshold for the completion of the recognition process, which is selected by balancing the accuracy and time of the recognition process.

[0103] Finally, the hybrid dynamics model of the robotic arm, including the link rigid body dynamics model, the link error compensation dynamics model, and the load dynamics model, can be represented as:

[0104]

[0105] Step 4: Establish a variable load dynamic model. The motion state of the robotic arm in this variable load dynamic model includes the following:

[0106] When unloaded, the robot joints move from the zero position to the target position;

[0107] Under load, all robot joints maintain position servo mode while determining gravity and inertial parameters;

[0108] Under load, the robot joints move from the zero position to the target position.

[0109] Step 5: Determine the method for identifying the dynamic parameters of the new load:

[0110] The specific implementation method of this step is as follows: it is adopted when the load needs to be changed in the robot's working scenario. This method first initiates an online identification program, and then uses the recursive least squares (RLS) method to identify the gravity parameters of the load under static conditions. These parameters are then decoupled and updated to more accurately reflect the new load characteristics. After the update process is complete, the completion of the identification process is determined by a set convergence metric. These metrics are typically set based on the determinant of the observation matrix. Once the identification process is confirmed to be complete, the updated parameters are applied to the hybrid dynamics model to achieve accurate prediction and effective control of the robot's joint torques. This process ensures that the robot can maintain the accuracy and robustness of its dynamic performance when facing load changes.

[0111] Step 6: When the load at the end of the robotic arm changes, control the robot joints by updating the parameters in the hybrid dynamics model.

[0112] The specific implementation method of this step is as follows: When a change in load is detected, the robot initiates an online identification program, using the recursive least squares (RLS) method to identify the gravity parameters of the load in real time under static conditions. After these parameters are decoupled and updated, a preset convergence index is used to determine whether the identification process has been completed. Once confirmed, the updated parameters are applied to the hybrid dynamics model for compensation in the control algorithm. This process involves a comprehensive consideration of link rigid body dynamics, link error compensation dynamics, and load dynamics, ensuring that the robot can reduce prediction errors and improve the accuracy and reliability of operation by using a compensation control strategy when facing load changes. In addition, to adapt to possible further changes or disturbances, the online identification and compensation algorithm will continue to run, ensuring that the robot maintains optimal performance throughout the entire operation.

[0113] Step 7: Online identification and compensation of the robotic arm's dynamic parameters by collecting sensor data in real time.

[0114] The specific implementation method of this step is as follows: Recursive Least Squares (RLS) is used to collect and process sensor data of robot joints in real time, such as torque, position, velocity, and acceleration. This method allows the system to recursively update the parameter estimates in the dynamic model when new data arrives, without reprocessing the entire dataset. By comparing the predicted torque with the actual torque, the system can calculate the model residuals and design a compensation control strategy to reduce the impact of these errors. In addition, a closed-loop feedback mechanism ensures that the control strategy can be adjusted according to the residuals fed back in real time, thereby improving the accuracy and reliability of robot operation. The entire process involves the online updating of link dynamic parameters and load dynamic parameters, as well as the application of a hybrid dynamics model that combines link rigid body dynamics, link error compensation dynamics, and load dynamics. Through continuous monitoring and adjustment, the robot can adapt to constantly changing dynamic conditions, thereby maintaining the accuracy and stability of its operation when faced with model imperfections and external disturbances.

[0115] It should be emphasized that the embodiments described in this invention are illustrative rather than limiting. Therefore, this invention includes, but is not limited to, the embodiments described in the specific implementation. Any other implementations derived by those skilled in the art based on the technical solutions of this invention are also within the scope of protection of this invention.

Claims

1. A quick-change form of robot arm end variable load dynamics self-diagnosis and compensation method, characterized by: The method comprises the following steps: Step 1, establishing a dynamic model based on online identification of iterative least squares; Step 2, load parameter identification under variable working conditions based on Jacobian matrix mapping; Step 3, establishing a hybrid dynamic model including rigid body dynamics of a connecting rod, error compensation dynamics of the connecting rod and load dynamics; Step 4, establishing a variable load dynamic model; Step 5, determining a new load dynamic parameter identification method: when the load needs to be changed in the robot working scene, the load dynamic parameters are identified step by step; Step 6, when the end load of the robot arm changes, the robot joints are controlled by updating the parameters in the hybrid dynamic model; Step 7, online identification and compensation are realized by collecting sensor data in real time; The implementation method of the step 1 comprises the following steps: Step 1.1, deriving a rigid body dynamics model of the connecting rod of the robot arm by Newton-Euler method; Step 1.2, linearizing the rigid body dynamics model; Step 1.3, recursively updating parameter estimation by using iterative least squares method; The linearization method of the step 1.2 is that constant parameters are extracted from the rigid body dynamics model of the connecting rod of the robot arm, and a linearized reconstruction model formula of the rigid body dynamics of the connecting rod of the robot arm is established; The implementation method of the step 2 comprises the following steps: Step 2.1, determining the coupling relationship and decoupling method of the load dynamic model; Step 2.2, load parameter identification under variable working conditions by Jacobian matrix mapping; The step 3 establishes the hybrid kinetic model τ e is: where τ e is the link error-compensated dynamics model, τ load represents the additional joint torque generated when the manipulator end-effector operates with a load, τ link represents the joint torque, Δτ link represents the error in the link rigid-body dynamics model, q, and represent the joint position, velocity, and acceleration, respectively, represents the joint inertia torque, correspond to the centrifugal and Coriolis torques of the joint; G(q) represents the joint gravitational torque; τ f represents the joint friction torque, Y d represents the observation matrix, Θ d represents the un-identified parameters, ΔΘ d represents the parameters of the error-compensated dynamics model, Φ b represents the set of load-minimal inertia parameters, E b represents Φ b corresponds to the full-rank observation matrix; The motion state of the robot arm in the variable load dynamic model comprises: In the case of no load, the robot joints move from zero position to target position; In the case of load, all robot joints keep position servo mode, and the gravity and inertia parameters are determined; In the case of load, the robot joints move from zero position to target position; The implementation method of the step 6 is that the online identification program is started, the load gravity parameters are identified under static conditions by using recursive least squares method, the parameters are decoupled and updated, then it is judged whether the identification process is completed by using the set convergence index, and finally the updated parameters are applied in the hybrid dynamic model; The step 7 uses recursive least squares method to estimate and correct unknown parameters in the model, so as to realize the compensation function of online identification and compensation of dynamic parameters of the robot arm.

2. The method of claim 1, wherein the method further comprises: The robot arm is a 6-axis robot arm.

Images