Compensation force feedback method and system based on third-order adaptive sliding mode observer

Abstract

The application belongs to the technical field of force feedback, and provides a compensation force feedback method and system based on a third-order adaptive sliding mode observer, which comprises the following steps: a robot dynamics model representing the relationship between robot movement and force is constructed; an observer state variable is set, and a relationship between the observer state variable, an adaptive gain, a disturbance estimation auxiliary variable, an observation velocity and an observation acceleration is established according to the robot dynamics model to obtain a third-order adaptive sliding mode observer; the adaptive gain is dynamically updated according to a position error in the observer state variable; a total disturbance term in the robot movement process is determined based on the disturbance estimation auxiliary variable in the third-order adaptive sliding mode observer and the robot dynamics model; and a compensated force feedback amount is calculated according to the total disturbance term and preset input and output parameters. The scheme can obtain a more real force feedback amount, and improves the accuracy, safety and reliability of the force feedback link in robot control.

Description

Technical Field

[0001] This invention relates to the field of force feedback technology, and in particular to a compensating force feedback method and system based on a third-order adaptive sliding mode observer. Background Technology

[0002] In applications requiring high precision and a sense of presence, such as medical surgery and nuclear industry operations, robotic systems rely on precise force feedback to assist operators in performing precise and safe remote control.

[0003] In related technologies, traditional control schemes often suffer from the challenge of accurately estimating and compensating for complex nonlinear disturbances such as robot joint friction and external environmental disturbances. This results in severely distorted force feedback transmitted to the operator, failing to accurately reflect the interaction forces between the robot and its environment, thus affecting the accuracy and safety of the operation.

[0004] On the other hand, commonly used first- or second-order sliding mode observers are sensitive to high-frequency chattering phenomena, and their fixed observer gain is difficult to adapt to time-varying working conditions such as load changes and speed fluctuations, which reduces the accuracy of robot state estimation (especially force and torque), thereby increasing the position and force tracking error of the master and slave ends and reducing control accuracy.

[0005] On the other hand, because the dynamics of the master and slave ends are not completely decoupled, the distorted force feedback is often accompanied by a significant delay. This lag not only reduces the real-time performance and immersive experience of the operation, but also easily causes oscillations or even instability in the entire teleoperation system, severely restricting the reliability of the system in high-precision scenarios and failing to meet the needs of practical applications.

[0006] This demonstrates that the force feedback mechanism in traditional robot control suffers from insufficient accuracy, safety, and reliability. Summary of the Invention

[0007] This invention provides a compensating force feedback method and system based on a third-order adaptive sliding mode observer, which addresses the shortcomings of insufficient accuracy, safety, and reliability in the force feedback loop of traditional robot control.

[0008] On one hand, the present invention provides a compensation force feedback method based on a third-order adaptive sliding mode observer, comprising:

[0009] Construct a robot dynamics model that represents the relationship between robot motion and forces;

[0010] By setting the observer state variables and establishing the relationship between the observer state variables, adaptive gain, and disturbance estimation auxiliary variables and the observation velocity and observation acceleration based on the robot dynamics model, a third-order adaptive sliding mode observer is obtained.

[0011] The adaptive gain is dynamically updated based on the position error in the observer's state variables;

[0012] Based on the disturbance estimation auxiliary variables in the third-order adaptive sliding mode observer and the robot dynamics model, the total disturbance term during the robot's motion is determined.

[0013] Based on the total disturbance term and the preset input and output parameters, the compensated force feedback quantity is calculated.

[0014] According to the compensated force feedback method based on a third-order adaptive sliding mode observer provided by the present invention, a robot dynamics model characterizing the relationship between robot motion and force is constructed, including:

[0015] Determine the position, velocity, and acceleration of the robot's joints during movement;

[0016] Based on the position, velocity, and acceleration of the joint, establish the inertia matrix, Coriolis matrix, and gravity vector;

[0017] By establishing the equations between the inertia matrix, the Coriolis matrix, the gravity vector, and the preset friction torque, control input, and total disturbance term, a robot dynamics model is obtained.

[0018] According to the compensation force feedback method based on a third-order adaptive sliding mode observer provided by the present invention, the adaptive gain includes a first gain, a second gain, and a third gain.

[0019] Based on the robot dynamics model and the observer state variables, the relationship between the observer state variables, adaptive gain, and disturbance estimation auxiliary variables and the observation velocity and observation acceleration is established, resulting in a third-order adaptive sliding mode observer, including:

[0020] Establish an equation relating the updated observation velocity to the previous observation velocity, the first gain, and the position error in the observer state variables to obtain the first dynamic equation;

[0021] The second dynamic equation is obtained by establishing the equations between the updated value of the observed acceleration and the inertia matrix, control input, Coriolis matrix, previous observed velocity, gravity vector, second gain, position error in the observer state variables, and disturbance estimation auxiliary variables.

[0022] Establish an equation relating the updated value of the disturbance estimation auxiliary variable to the third gain and the position error in the observer state variable to obtain the third dynamic equation;

[0023] The first dynamic equation, the second dynamic equation, and the third dynamic equation are used as a third-order adaptive sliding mode observer.

[0024] According to the compensation force feedback method based on a third-order adaptive sliding mode observer provided by the present invention, the first dynamic equation is:

[0025]

[0026] in, The updated value representing the observation velocity. This indicates the previous observation speed. Indicates the first gain value. This represents the position error in the observer's state variables. Represents a symbolic function.

[0027] According to the compensation force feedback method based on a third-order adaptive sliding mode observer provided by the present invention, the second dynamic equation is:

[0028]

[0029] in, This represents the updated value of the observed acceleration. Represents the inertia matrix. Represents the Coriolis matrix. Represents the gravity vector. Indicates the control input quantity. Indicates the location of the joint. Indicates the speed of the joint. This indicates the previous observation speed. This represents the second gain value. This represents the position error in the observer's state variables. Represents a symbolic function. This represents an auxiliary variable for perturbation estimation.

[0030] According to the compensation force feedback method based on a third-order adaptive sliding mode observer provided by the present invention, the third dynamic equation is:

[0031]

[0032] in, This represents the updated value of the auxiliary variable used for perturbation estimation. This represents the third gain quantity. This represents the position error in the observer's state variables. Represents a symbolic function.

[0033] According to the compensating force feedback method based on a third-order adaptive sliding mode observer provided by the present invention, the total disturbance term during robot motion is determined based on the disturbance estimation auxiliary variable in the third-order adaptive sliding mode observer and the robot dynamics model, including:

[0034] The disturbance estimation auxiliary variable in the third-order adaptive sliding mode observer is multiplied with the inertia matrix in the robot dynamics model to obtain the total disturbance term during the robot's motion.

[0035] According to the compensation force feedback method based on a third-order adaptive sliding mode observer provided by the present invention, the input and output parameters include control input, desired end-effector pose, and actual end-effector pose.

[0036] Based on the total disturbance term and preset input / output parameters, the compensated force feedback quantity is calculated, including:

[0037] Determine the Jacobian matrix with the joint position as the independent variable, and calculate the inverse of the transpose of the Jacobian matrix;

[0038] Multiply the inverse matrix by the difference between the control input and the total disturbance term to obtain the first intermediate quantity;

[0039] The second intermediate quantity is obtained by multiplying the preset position error gain by the difference between the desired end-effector pose and the actual end-effector pose.

[0040] Add the first intermediate quantity to the second intermediate quantity to obtain the compensated force feedback quantity.

[0041] According to the compensation force feedback method based on a third-order adaptive sliding mode observer provided by the present invention, the adaptive gain includes a first gain, a second gain, and a third gain.

[0042] The adaptive gain is dynamically updated based on the position error in the observer's state variables, including:

[0043] The first preset adjustment parameter is multiplied by the first power of the absolute value of the position error in the observer state variable to obtain the updated first gain.

[0044] The updated second gain is obtained by multiplying the second preset adjustment parameter by a quarter power of the absolute value of the position error in the observer state variable.

[0045] The third preset adjustment parameter is multiplied by the zeroth power of the absolute value of the position error in the observer state variable to obtain the updated third gain.

[0046] On the other hand, the present invention also provides a compensating force feedback system based on a third-order adaptive sliding mode observer, comprising:

[0047] The building module is used to construct a robot dynamics model that represents the relationship between robot motion and forces.

[0048] The observation module is used to set the observer state variables and establish the relationship between the observer state variables, adaptive gain, and disturbance estimation auxiliary variables and the observation velocity and observation acceleration based on the robot dynamics model, thereby obtaining a third-order adaptive sliding mode observer.

[0049] The update module is used to dynamically update the adaptive gain based on the position error in the observer state variables;

[0050] The determination module is used to determine the total disturbance term during robot motion based on the disturbance estimation auxiliary variables in the third-order adaptive sliding mode observer and the robot dynamics model.

[0051] The compensation module is used to calculate the compensated force feedback quantity based on the total disturbance term and preset input / output parameters.

[0052] This invention provides a compensated force feedback method and system based on a third-order adaptive sliding mode observer. The method involves constructing a robot dynamics model characterizing the relationship between robot motion and force; defining observer state variables; establishing the relationship between the observer state variables, adaptive gain, disturbance estimation auxiliary variables, and observed velocity and acceleration based on the robot dynamics model, thus obtaining a third-order adaptive sliding mode observer; dynamically updating the adaptive gain based on the position error in the observer state variables; determining the total disturbance term during robot motion based on the disturbance estimation auxiliary variables in the third-order adaptive sliding mode observer and the robot dynamics model; and calculating the compensated force feedback quantity based on the total disturbance term and preset input / output parameters. This scheme utilizes the robot dynamics model and the third-order adaptive sliding mode observer to obtain an accurate total disturbance quantity, thereby enabling the calculation of a more realistic force feedback quantity, improving the accuracy, safety, and reliability of the force feedback loop in robot control. Attached Figure Description

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

[0054] Figure 1 This is a flowchart illustrating the compensation force feedback method based on a third-order adaptive sliding mode observer provided in an embodiment of the present invention.

[0055] Figure 2 This is a schematic diagram illustrating the principle of the compensation force feedback method based on a third-order adaptive sliding mode observer in an embodiment of the present invention;

[0056] Figure 3 This is a schematic diagram of the structure of the compensation force feedback system based on a third-order adaptive sliding mode observer provided in an embodiment of the present invention. Detailed Implementation

[0057] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0058] The following is combined Figures 1 to 3 This invention describes the detailed scheme of the compensation force feedback method and system based on a third-order adaptive sliding mode observer provided in the embodiments of the present invention.

[0059] like Figure 1 As shown, the compensation force feedback method based on a third-order adaptive sliding mode observer provided in this embodiment of the invention mainly includes the following steps:

[0060] Step 110: Construct a robot dynamics model that represents the relationship between robot motion and forces.

[0061] This step requires establishing a robot dynamics model that includes state variables such as joint position, velocity, and acceleration. During this process, it is necessary to define parameters such as the inertia matrix related to joint position, the Coriolis matrix representing nonlinear terms, the gravity vector, friction torque, and control input quantities. The total disturbance term, including joint friction and external environmental disturbances, is incorporated into the equation to form a robot dynamics model that describes the relationship between robot motion and force.

[0062] Step 120: Set the observer state variables. Based on the robot dynamics model, establish the relationship between the observer state variables, adaptive gain, and disturbance estimation auxiliary variables and the observation velocity and observation acceleration to obtain a third-order adaptive sliding mode observer.

[0063] In this step, three observer state variables need to be defined. The first variable is the difference between the actual position and the observed position of the joint, i.e., the position error; the second variable is the difference between the actual velocity and the observed velocity of the joint, i.e., the velocity error; and the third variable is the integral term of the position error.

[0064] Subsequently, update rules were constructed for the observation velocity, observation acceleration, and disturbance estimation auxiliary variables, respectively. In the third-order adaptive sliding mode observer, a convergence law containing a sign function and error terms of different powers was introduced, and an adaptive gain was set to dynamically adjust the response characteristics of the third-order adaptive sliding mode observer.

[0065] Step 130: Dynamically update the adaptive gain based on the position error in the observer's state variables.

[0066] In this step, a dynamic update rule is designed for the adaptive gain based on the position error in the observer's state variables, and the rule is verified by Lyapunov stability theory to ensure that the adaptive gain adjustment process converges, thereby guaranteeing the stability of the third-order adaptive sliding mode observer.

[0067] Step 140: Based on the disturbance estimation auxiliary variables in the third-order adaptive sliding mode observer and the robot dynamics model, determine the total disturbance term during the robot's motion process.

[0068] In this step, based on the disturbance estimation auxiliary variables and the inertia matrix in the robot dynamics model, the total disturbance term during the robot's motion is calculated, thereby completing the quantitative estimation of nonlinear disturbances.

[0069] Step 150: Calculate the compensated force feedback quantity based on the total disturbance term and the preset input and output parameters.

[0070] In this step, the final force feedback quantity can be calculated by combining the Jacobian matrix used for mapping joint space and task space, control input quantity, total disturbance term, position error gain, and the desired end-effector pose and actual end-effector pose in task space, and then output to the operator to achieve high-fidelity force feedback.

[0071] In one embodiment, constructing a robot dynamics model characterizing the relationship between robot motion and forces specifically includes:

[0072] First, determine the position, velocity, and acceleration of the robot's joints during movement.

[0073] Then, based on the position, velocity, and acceleration of the joints, the inertia matrix, Coriolis matrix, and gravity vector are established.

[0074] Finally, the equations between the inertia matrix, Coriolis matrix, gravity vector, and preset friction torque, control input, and total disturbance term are established to obtain the robot dynamics model.

[0075] In this embodiment, the robot dynamics model can be represented as:

[0076] (1)

[0077] in, These represent the position, velocity, and acceleration of the joint, respectively, and are presented in vector form. The inertia matrix is ​​a function of the joint positions; The Coriolis matrix represents the nonlinear terms in the robot dynamics model. Represents the gravity vector; Indicates frictional torque; To control the input amount; is the total disturbance term, and is the target estimate in this embodiment.

[0078] In this embodiment, the observer state variable can be represented as:

[0079] (2)

[0080] in, Indicates positional error. Indicates the actual location. Indicates the observation location. Indicates speed error, Indicates actual speed. Indicates the observation speed. This represents the integral term.

[0081] Understandably, positional error It is the core error metric of the third-order adaptive sliding mode observer, directly reflecting the accuracy of position observation and serving as the benchmark error signal for subsequent disturbance estimation and gain adjustment. Velocity error The essence is positional error. The first derivative with respect to time describes the rate of change of the position error and serves as a state supplement for the velocity level in a third-order adaptive sliding mode observer. However, it is not used as a separate independent variable in the explicit calculation of the dynamic equations because the velocity error and position error are related by derivatives, and the change in velocity error can be indirectly reflected through the dynamic characteristics of the position error. (Integral term) The core function is to suppress high-frequency chattering. It is a robust optimization term of the third-order adaptive sliding mode observer. This variable smooths the high-frequency fluctuations of the position error through integral action, reducing the chattering problem of the traditional sliding mode observer. However, it does not need to participate in the explicit calculation of the observation position and velocity update in the dynamic equation. It only plays an implicit role as an anti-interference mechanism inside the third-order adaptive sliding mode observer.

[0082] In one embodiment, the adaptive gain specifically includes a first gain, a second gain, and a third gain.

[0083] Furthermore, based on the robot dynamics model and the observer state variables, the relationship between the observer state variables, adaptive gain, and disturbance estimation auxiliary variables and the observed velocity and observed acceleration is established, resulting in a third-order adaptive sliding mode observer, specifically including:

[0084] On the one hand, an equation is established between the updated value of the observation velocity and the previous observation velocity, the first gain, and the position error in the observer state variables, thus obtaining the first dynamic equation.

[0085] In this embodiment, the first dynamic equation can be specifically expressed as:

[0086] (3)

[0087] in, The updated value representing the observation velocity. This indicates the previous observation speed. Indicates the first gain value. This represents the position error in the observer's state variables. Represents a symbolic function.

[0088] On the other hand, by establishing equations between the updated value of the observed acceleration and the inertia matrix, control input, Coriolis matrix, previous observed velocity, gravity vector, second gain, position error in the observer state variables, and disturbance estimation auxiliary variables, the second dynamic equation is obtained.

[0089] In this embodiment, the second dynamic equation can be expressed as:

[0090] (4)

[0091] in, This represents the updated value of the observed acceleration. Represents the inertia matrix. Represents the Coriolis matrix. Represents the gravity vector. Indicates the control input quantity. Indicates the location of the joint. Indicates the speed of the joint. This indicates the previous observation speed. This represents the second gain value. This represents the position error in the observer's state variables. Represents a symbolic function. This represents an auxiliary variable for perturbation estimation.

[0092] On the other hand, an equation is established between the updated value of the disturbance estimation auxiliary variable and the third gain and the position error in the observer state variable, thus obtaining the third dynamic equation.

[0093] In this embodiment, the third dynamic equation can be specifically expressed as:

[0094] (5)

[0095] in, This represents the updated value of the auxiliary variable used for perturbation estimation. This represents the third gain quantity. This represents the position error in the observer's state variables. Represents a symbolic function.

[0096] Finally, the first, second, and third dynamic equations are used as a third-order adaptive sliding mode observer.

[0097] In this embodiment, the third-order adaptive sliding mode observer can be specifically represented as:

[0098] (6)

[0099] In one embodiment, the adaptive gain includes a first gain, a second gain, and a third gain.

[0100] Furthermore, based on the position error in the observer's state variables, the adaptive gain is dynamically updated, specifically including:

[0101] On the one hand, the first preset adjustment parameter is multiplied by the first power of the absolute value of the position error in the observer state variable to obtain the updated first gain.

[0102] On the other hand, the second preset adjustment parameter is multiplied by a quarter power of the absolute value of the position error in the observer state variable to obtain the updated second gain.

[0103] On the other hand, the third preset adjustment parameter is multiplied by the zeroth power of the absolute value of the position error in the observer state variable to obtain the updated third gain.

[0104] In this embodiment, the dynamic update expression for the adaptive gain is as follows:

[0105] (7)

[0106] in, This represents the first gain after the update. This indicates the first preset adjustment parameter. This represents the position error in the observer's state variables. This represents the updated second gain value. This indicates the second preset adjustment parameter. This represents the updated third gain value. This indicates the third preset adjustment parameter.

[0107] Understandably, the adaptive gain can be dynamically adjusted according to the position error, which can increase the gain of the robot control loop by 3 times under impact disturbances, and automatically reduce the gain in steady state to avoid overestimation problems.

[0108] In one embodiment, based on the disturbance estimation auxiliary variables in the third-order adaptive sliding mode observer and the robot dynamics model, the total disturbance term during the robot's motion is determined, specifically including:

[0109] The total disturbance term during robot motion is obtained by multiplying the disturbance estimation auxiliary variable in the third-order adaptive sliding mode observer with the inertia matrix in the robot dynamics model.

[0110] In this embodiment, the expression for the total disturbance term is specifically as follows:

[0111] (8)

[0112] in, This represents the total disturbance term that is updated in real time. Represents the inertia matrix. Indicates the location of the joint. This represents an auxiliary variable for perturbation estimation.

[0113] In one embodiment, the input and output parameters specifically include control input, desired end-effector pose, and actual end-effector pose.

[0114] Furthermore, based on the total disturbance term and preset input / output parameters, the compensated force feedback quantity is calculated, specifically including:

[0115] First, determine the Jacobian matrix with the joint position as the independent variable, and calculate the inverse of the transpose of the Jacobian matrix.

[0116] Then, the inverse matrix is ​​multiplied by the difference between the control input and the total disturbance term to obtain the first intermediate quantity.

[0117] Simultaneously, the preset position error gain is multiplied by the difference between the desired end-effector pose and the actual end-effector pose to obtain the second intermediate quantity.

[0118] Finally, the first intermediate quantity and the second intermediate quantity are added together to obtain the compensated force feedback quantity.

[0119] In this embodiment, the compensated force feedback quantity can be specifically expressed as:

[0120] (9)

[0121] in, This represents the final virtual force feedback output to the operator, i.e., the compensated force feedback amount. Represents the Jacobian matrix. Indicates the location of the joint. Indicates the control input quantity. This represents the total disturbance term that is updated in real time. Indicates the position error gain. and These represent the desired end-effector pose and the actual end-effector pose in the task space (i.e., Cartesian space), respectively.

[0122] Understandably, this is achieved by updating the total disturbance term in real time. Injecting a control loop can cancel out dynamic nonlinear terms, making the slave end equivalent to a unit mass system, thereby simplifying force mapping.

[0123] The implementation principle of the entire method can be found in [reference needed]. Figure 2 ,like Figure 2 As shown, the teleoperator inputs control intentions at the master end by operating the master robotic arm, such as the desired position and posture of the slave robotic arm end effector. Then, the kinematic forward kinematics algorithm is used to convert the joint space and task space, and the operation intention is converted into standardized control commands. The control commands are transmitted to the slave end in real time via a communication link based on a communication protocol. At the same time, the master end will synchronously feed back joint motion correlation parameters to the slave end, such as the angles of each joint at the master end, to ensure the coordination of master and slave movements.

[0124] After receiving control commands from the slave end, the inverse kinematics algorithm is used to convert the command content into parameters such as the desired position and velocity of each joint of the slave end robotic arm. Simultaneously, the slave end robotic arm executes actions based on these parameters and collects joint position data in real time. ,speed and acceleration .

[0125] Simultaneously, the slave device calls a pre-built robot dynamics model to collect the joint positions. ,speed acceleration The relevant model parameters are input into a third-order adaptive sliding mode observer. Based on three preset dynamic equations, the third-order adaptive sliding mode observer updates the observation velocity, observation acceleration, and disturbance estimation auxiliary variable z, respectively. It also dynamically updates the adaptive gain based on the position error in the observer's state variables. Subsequently, the disturbance estimation auxiliary variable z is multiplied by the inertia matrix using the robotic arm's dynamic equations to obtain the total disturbance term. .

[0126] Finally, the total disturbance term is transferred from the end. The control input, desired end-effector pose, actual end-effector pose, preset position error gain, and Jacobian matrix are input to the full dynamic compensator to obtain the compensated force feedback. The force feedback is then fed back to the remote operator, who adjusts subsequent operating commands based on the compensated force feedback and initiates master-end control again, forming a complete closed loop of master-end command - slave-end execution - disturbance observation - force feedback compensation - master-end sensing - command adjustment, thereby enabling high-precision force feedback control.

[0127] In summary, the force feedback method based on a third-order adaptive sliding mode observer provided in this invention, compared with the traditional force feedback scheme for teleoperated robots, achieves a leap in system performance through a third-order adaptive sliding mode observer and a fully dynamic compensation mechanism. The specific beneficial effects are as follows:

[0128] First, the fidelity of force feedback is significantly improved:

[0129] Traditional methods suffer from significant force signal distortion due to the lack of compensation for nonlinear interference; however, this embodiment, through high-precision disturbance observation and dynamic feedforward compensation, can make the contact force perceived by the operator highly consistent with the force in the real environment.

[0130] Second, a breakthrough has been achieved in real-time response:

[0131] Traditional force feedback suffers from significant delays, which can easily lead to a sense of sluggish operation and the risk of system oscillation. This embodiment uses rapid joint space perturbation estimation and decoupling mapping to compress the response delay to near the limit of human-computer interaction perception, enabling operators to obtain an immersive experience with almost no lag.

[0132] Third, the ability to suppress disturbances is enhanced:

[0133] Traditional solutions can only cope with low-frequency, slowly varying disturbances, and their performance degrades sharply under time-varying conditions. The adaptive sliding mode mechanism designed in this embodiment can cover a wide frequency band of disturbance spectrum and can still maintain robust estimation capability for strong nonlinear disturbances such as joint friction abrupt changes and load impacts.

[0134] Based on the same general inventive concept, this invention also protects a compensating force feedback system based on a third-order adaptive sliding mode observer. The compensating force feedback system based on a third-order adaptive sliding mode observer provided by this invention will be described below. The compensating force feedback system based on a third-order adaptive sliding mode observer described below can be referred to in correspondence with the compensating force feedback method based on a third-order adaptive sliding mode observer described above.

[0135] like Figure 3 As shown, the compensation force feedback system based on a third-order adaptive sliding mode observer provided in this embodiment of the invention specifically includes:

[0136] Module 210 is used to construct a robot dynamics model that represents the relationship between robot motion and forces.

[0137] The observation module 220 is used to set the observer state variables. Based on the robot dynamics model, it establishes the relationship between the observer state variables, adaptive gain, and disturbance estimation auxiliary variables and the observation velocity and observation acceleration, thus obtaining a third-order adaptive sliding mode observer.

[0138] The update module 230 is used to dynamically update the adaptive gain based on the position error in the observer's state variables.

[0139] The determination module 240 is used to determine the total disturbance term during robot motion based on the disturbance estimation auxiliary variables in the third-order adaptive sliding mode observer and the robot dynamics model.

[0140] The compensation module 250 is used to calculate the compensated force feedback based on the total disturbance term and preset input and output parameters.

[0141] Regarding the system in the above embodiments, the specific ways in which each module performs operations have been described in detail in the embodiments of the relevant methods, and will not be elaborated further here.

[0142] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A compensating force feedback method based on a third order adaptive sliding mode observer, characterized by, The method comprises the following steps: a robot dynamics model representing the relationship between robot motion and force is constructed, including: determining the position, velocity and acceleration of the joint in the robot motion; establishing an inertia matrix, a Coriolis matrix and a gravity vector according to the position, velocity and acceleration of the joint; establishing an equation relationship between the inertia matrix, the Coriolis matrix, the gravity vector and a preset friction torque and a control input and a total disturbance term, to obtain a robot dynamics model; an observer state variable is set, and the adaptive gain includes a first gain, a second gain and a third gain; according to the robot dynamics model, a relationship between the observer state variable, the adaptive gain and the disturbance estimation auxiliary variable and the observed velocity and the observed acceleration is established, to obtain a third-order adaptive sliding mode observer, including: an equation relationship between the updated value of the observed velocity and the previous observed velocity, the first gain, the position error in the observer state variable is established, to obtain a first dynamic equation; an equation relationship between the updated value of the observed acceleration and the inertia matrix, the control input, the Coriolis matrix, the previous observed velocity, the gravity vector, the second gain, the position error in the observer state variable and the disturbance estimation auxiliary variable is established, to obtain a second dynamic equation; an equation relationship between the updated value of the disturbance estimation auxiliary variable and the third gain and the position error in the observer state variable is established, to obtain a third dynamic equation; the first dynamic equation, the second dynamic equation and the third dynamic equation are taken as the third-order adaptive sliding mode observer; the adaptive gain is dynamically updated according to the position error in the observer state variable; based on the disturbance estimation auxiliary variable in the third-order adaptive sliding mode observer and the robot dynamics model, a total disturbance term in the robot motion process is determined; a compensated force feedback is calculated according to the total disturbance term and a preset input-output parameter.

2. The method of claim 1, wherein the third-order adaptive sliding mode observer-based compensatory force feedback is characterized by, The first dynamic equation is: wherein represents an updated value of the observed velocity, represents a previous observed velocity, represents a first gain quantity, represents a position error in the observer state variable, represents a sign function.

3. The method of claim 1, wherein the third-order adaptive sliding mode observer-based compensatory force feedback is characterized by, The second dynamic equation is: wherein represents an updated value of the observed acceleration, represents an inertia matrix, represents a Coriolis matrix, represents a gravity vector, represents a control input quantity, represents a position of a joint, represents a velocity of a joint, represents a previous observed velocity, represents a second gain quantity, represents a position error in an observer state variable, represents a sign function, represents a disturbance estimation auxiliary variable.

4. The method of claim 1, wherein the third-order adaptive sliding mode observer-based compensatory force feedback is characterized by, The third dynamic equation is: wherein denotes an updated value of the disturbance estimation auxiliary variable, denotes a third gain quantity, denotes a position error in the observer state variable, denotes a sign function.

5. The method of claim 1 to 4, wherein based on the disturbance estimation auxiliary variable in the third-order adaptive sliding mode observer and the robot dynamics model, a total disturbance term in the robot motion process is determined, including: the disturbance estimation auxiliary variable in the third-order adaptive sliding mode observer is multiplied by the inertia matrix in the robot dynamics model, to obtain the total disturbance term in the robot motion process.

6. The method of claim 1 to 4, wherein The input-output parameter includes the control input, the expected end pose and the actual end pose; a compensated force feedback is calculated according to the total disturbance term and a preset input-output parameter, including: determining the Jacobian matrix of the joint position as the independent variable, and calculating the inverse matrix of the transpose of the Jacobian matrix; multiplying the inverse matrix by the difference between the control input and the total disturbance term to obtain a first intermediate quantity; multiplying a preset position error gain by the difference between the expected end pose and the actual end pose to obtain a second intermediate quantity; adding the first intermediate quantity and the second intermediate quantity to obtain the compensated force feedback.

7. The method of claim 1, wherein the third-order adaptive sliding mode observer based compensation force feedback is characterized by, The adaptive gain includes a first gain, a second gain and a third gain; The adaptive gain is dynamically updated according to the position error in the observer state variable, including: The first preset adjustment parameter is multiplied by the square of the absolute value of the position error in the observer state variable to obtain an updated first gain quantity; The second preset adjustment parameter is multiplied by the fourth power of the absolute value of the position error in the observer state variable to obtain an updated second gain quantity; The third preset adjustment parameter is multiplied by the zeroth power of the absolute value of the position error in the observer state variable to obtain an updated third gain quantity.

8. A compensating force feedback system based on a third order adaptive sliding mode observer, characterized in that, including: The construction module is configured to construct a robot dynamics model representing the relationship between robot motion and force, including: determining the position, velocity and acceleration of the joint in the robot motion; establishing an inertia matrix, a Coriolis matrix and a gravity vector according to the position, velocity and acceleration of the joint; establishing an equation relationship between the inertia matrix, the Coriolis matrix, the gravity vector and the preset friction torque and the control input quantity and the total disturbance term to obtain a robot dynamics model; The observation module is configured to set an observer state variable, and the adaptive gain includes a first gain quantity, a second gain quantity and a third gain quantity; according to the robot dynamics model, a relationship between the observer state variable, the adaptive gain and the disturbance estimation auxiliary variable and the observation velocity and the observation acceleration is established to obtain a third-order adaptive sliding mode observer, including: an equation relationship between the updated value of the observation velocity and the previous observation velocity, the first gain quantity and the position error in the observer state variable is established to obtain a first dynamic equation; an equation relationship between the updated value of the observation acceleration and the inertia matrix, the control input quantity, the Coriolis matrix, the previous observation velocity, the gravity vector, the second gain quantity, the position error in the observer state variable and the disturbance estimation auxiliary variable is established to obtain a second dynamic equation; an equation relationship between the updated value of the disturbance estimation auxiliary variable and the third gain quantity and the position error in the observer state variable is established to obtain a third dynamic equation; the first dynamic equation, the second dynamic equation and the third dynamic equation are taken as a third-order adaptive sliding mode observer; The update module is configured to dynamically update the adaptive gain according to the position error in the observer state variable; The determination module is configured to determine the total disturbance term in the robot motion process based on the disturbance estimation auxiliary variable in the third-order adaptive sliding mode observer and the robot dynamics model; The compensation module is configured to calculate a compensated force feedback quantity according to the total disturbance term and a preset input-output parameter.

Images