CN117301070A - Force sensor decoupling end effector mechanism and variable impedance control method thereof - Google Patents
Force sensor decoupling end effector mechanism and variable impedance control method thereof Download PDFInfo
- Publication number
- CN117301070A CN117301070A CN202311520240.XA CN202311520240A CN117301070A CN 117301070 A CN117301070 A CN 117301070A CN 202311520240 A CN202311520240 A CN 202311520240A CN 117301070 A CN117301070 A CN 117301070A
- Authority
- CN
- China
- Prior art keywords
- end effector
- force
- force sensor
- linear motor
- decoupled
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 239000012636 effector Substances 0.000 title claims abstract description 205
- 230000007246 mechanism Effects 0.000 title claims abstract description 98
- 238000000034 method Methods 0.000 title claims abstract description 28
- 238000013016 damping Methods 0.000 claims abstract description 23
- 238000006243 chemical reaction Methods 0.000 claims abstract description 4
- 238000006073 displacement reaction Methods 0.000 claims description 12
- 230000003993 interaction Effects 0.000 claims description 4
- 230000007613 environmental effect Effects 0.000 abstract description 2
- 230000000875 corresponding effect Effects 0.000 description 5
- 230000004044 response Effects 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 4
- 238000005259 measurement Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000011217 control strategy Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 238000013017 mechanical damping Methods 0.000 description 3
- 238000012546 transfer Methods 0.000 description 3
- 230000002596 correlated effect Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005498 polishing Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1612—Programme controls characterised by the hand, wrist, grip control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J15/00—Gripping heads and other end effectors
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a force sensor decoupling end effector mechanism and a variable impedance control method thereof, wherein the mechanism comprises the following components: a sensor end for being connected with a robot, said sensor end comprising: a force sensor for connection with a robot and a fixed platform disposed on the force sensor; a moving platform in sliding fit with the fixed platform; and the sliding mechanism is arranged between the fixed platform and the movable platform. The variable impedance control method includes: based on the designed end effector mechanism, the reaction force of the mechanical impedance and the nonlinear force in the end effector is measured and calculated through a force sensor, and the partial force is converted into current compensation and is input into the current loop control of the linear motor, so that decoupling of the mechanical impedance and the nonlinear force is realized; and on the basis of decoupling, an impedance controller is added in the controller, and the variable impedance control under different environmental rigidities is realized by combining the required rigidity and damping parameters.
Description
Technical Field
The invention relates to the field of end effectors and automatic control, in particular to a force sensor decoupling end effector mechanism and a variable impedance control method thereof.
Background
The control strategy of the traditional industrial robot end effector is mainly position control, but more and more unstructured environments also need the contact operation task of the end effector and the environment, such as polishing, hole shaft assembly, grabbing and the like, and the traditional high-rigidity position control can accurately reach an accurate position, but cannot meet the requirements of some contact operation tasks. Because of the high stiffness of the end effector, when the end effector contacts the workpiece, the workpiece and the effector are prone to being damaged irreversibly by excessive vibration and impact, and in severe cases, equipment failure and the like may occur.
The method for solving the problem is to reduce the rigidity of the end effector or improve the damping of the end effector, so that the end effector presents certain flexibility, and the impact between the end effector and the workpiece can be effectively reduced.
The problem of the above complaints is solved, and the existing researches are mainly divided into two modes, namely structural design aspect and control strategy aspect. For structural design, related researches realize the change of rigidity and damping by designing different mechanisms, and although the method can realize the change of the rigidity and the damping of the mechanisms, the method is still influenced by a mechanical structure and has poor portability. In terms of control strategies, a control method such as a stiffness model is commonly used, and the stiffness is changed by changing parameters of the stiffness model, but the method has the defects of insufficient variable stiffness range, insufficient stiffness precision and the like. The solution to the above problem is therefore to propose an end effector mechanism with decoupled force sensor and a variable impedance control method thereof.
Disclosure of Invention
In order to solve the problems in the prior art, the invention provides a force sensor decoupling end effector mechanism and a variable impedance control method thereof, which can control the impedance of an end effector by changing the parameters of an impedance controller, and improve and reduce the impact when the end effector contacts with a workpiece by changing the impedance of the end effector mechanism.
The technical scheme adopted by the invention is as follows:
an end effector mechanism for decoupling a force sensor, comprising:
a sensor end for being connected with a robot, said sensor end comprising: a force sensor for connection with a robot and a fixed platform disposed on the force sensor;
a moving platform in sliding fit with the fixed platform;
and the sliding mechanism is arranged between the fixed platform and the movable platform.
The sliding mechanism specifically comprises:
a connecting rod arranged on the fixed platform;
the sliding rail is arranged on the connecting rod;
a slide block matched with the slide rail;
the sliding block is connected with the mobile platform;
and the driving mechanism is used for driving the sliding block to linearly move on the sliding rail.
The driving mechanism is a linear motor.
The linear motor includes:
a linear motor stator arranged on the fixed platform;
and the linear motor rotor is arranged on the mobile platform and matched with the linear motor stator.
And the connecting rod is provided with a linear encoder facing the linear motor rotor.
Further preferred, an end effector mechanism with decoupled force sensors includes a linear motor, a fixed end, a movable end, and a linear encoder.
The linear motor provides a linear power source for the end effector to realize the movement of the end effector, and the linear motor comprises a stator and a rotor;
the fixed end is provided with a fixed platform, a plurality of connecting rods, a force sensor and a stator of the linear motor;
the movable end is provided with a movable platform, a plurality of sliding blocks and a rotor of the linear motor;
the linear encoder is arranged on the connecting rod and faces the linear motor rotor and is used for measuring the relative displacement between the fixed end and the movable end;
and the fixed platform is arranged at the fixed end of the end effector, one side of the fixed platform is connected with the linear motor stator, and the fixed end of the other side is connected with the force sensor.
The movable platform is arranged at the moving end of the end effector and used for realizing the movement of the end effector mechanism.
The connecting rods are provided with a plurality of hanging rods which are hung on the fixed platform, and the guide rail is arranged on the inner side of the connecting rods.
The guide rail and the sliding block realize the guiding function of the movement of the end effector, the guide rail is arranged on the inner side of the connecting rod, the sliding block is arranged on the moving platform, and the guide rail realize the guiding of the movement direction and limit the travel of the end effector.
The force sensor can be a strain force sensor, a piezoelectric force sensor and the like, the force direction is single degree of freedom of the movement direction of the end effector, the force sensor is arranged on the fixed end side of the fixed platform, and the other end of the force sensor is connected with the industrial robot.
Further, the end effector mechanism with the force sensor decoupled is characterized in that the mechanical impedance and the nonlinear force of the end effector with the force sensor decoupled are measured by the force sensor in a decoupling mode, so that the mechanical impedance decoupling of the end effector with the force sensor decoupled is realized by changing the input of the linear motor.
The mechanical impedance of the end effector includes mechanical stiffness of the end effector and mechanical damping of the end effector.
A method of variable impedance control of a force sensor decoupled end effector mechanism employing the force sensor decoupled end effector mechanism, comprising the steps of:
step 1, using a force sensor to directly measure and calculate mechanical impedance and nonlinear force of an end effector mechanism decoupled by the force sensor;
step 2, compensating and decoupling based on the mechanical impedance and the nonlinear force measured and calculated in the step 1, so as to obtain a decoupled end effector mechanism of the force sensor;
and 3, adding an impedance controller to the end effector mechanism decoupled from the force sensor based on the decoupled end effector mechanism of the force sensor in the step 2, and performing variable impedance control on the end effector mechanism decoupled from the force sensor through a current signal output by the impedance controller.
In step 1, the force sensor is used for directly measuring and calculating the mechanical impedance and nonlinear force of the end effector mechanism decoupled by the force sensor, and the method specifically comprises the following steps:
according to the interaction force, the force measured by the force sensor comprises mechanical impedance, nonlinear force and linear motor output force, the force measured by the force sensor is a value at the current moment after the end effector is moved and changed due to the output force of the linear motor at the last moment, and therefore the specific expression of the force measured by the force sensor is as follows:
F s ＝Z z +F t-1 +F n
f in the formula s Z is the force measured by the force sensor z Mechanical impedance of end effector mechanism decoupled for force sensor, F t-1 For the output force at one moment on the linear motor, F n A nonlinear force of the end effector mechanism decoupled from the force sensor;
the output force of the linear motor is calculated according to the force constant of the linear motor and the input current of the linear motor, so the mechanical impedance and the nonlinear force of the end effector mechanism decoupled by the force sensor are the force measured by the force sensor minus the output force of the linear motor, and the specific expression is as follows:
Z z +F n ＝F s -i t-1 K i
i in t-1 For the input current at one moment in the linear motor, K i Is a linear motor force constant;
in step 2, compensating and decoupling based on the mechanical impedance and the nonlinear force measured and calculated in step 1 to obtain a decoupled end effector mechanism of the force sensor, which specifically comprises the following steps of;
the mechanical impedance of the end effector mechanism decoupled by the force sensor, the nonlinear force is converted into a corresponding current by the force constant of the linear motor:
i in c Decoupling current converted for mechanical impedance, nonlinear force;
placing a linear motor on a current loop through a driver;
and adding the decoupling current converted by the mechanical impedance and the nonlinear force to the input of the current loop of the linear motor, wherein the input current of the linear motor comprises the mechanical impedance of the end effector mechanism decoupled by the force sensor and the corresponding current converted by the nonlinear force through the force constant of the linear motor, so that the end effector mechanism decoupled by the force sensor after decoupling is obtained.
In step 3, an impedance controller is added to the end effector mechanism decoupled from the force sensor based on the decoupled end effector mechanism of step 2, and a current signal output by the impedance controller is used for performing variable impedance control on the decoupled end effector mechanism of the force sensor, which specifically comprises:
the end effector mechanism decoupled from the decoupled force sensor realizes variable impedance control on the end effector mechanism decoupled from the decoupled force sensor through a current signal output by the impedance controller;
parameters of the impedance controller include stiffness and damping;
according to the relation between rigidity and displacement, damping and speed, the output current of the impedance controller is specifically:
i in i An output current of the impedance controller; k (K) d For stiffness in impedance controller parameters；B d Damping in the impedance controller parameters; x is the direct relative displacement of the movable platform and the fixed platform;the relative speed of the movable platform and the fixed platform is directly;
and the input current of the linear motor comprises the output current of the impedance controller and the corresponding current of the mechanical impedance and nonlinear force of the end effector mechanism decoupled by the force sensor through conversion of the force constant of the linear motor, so that the variable impedance control of the end effector mechanism decoupled by the force sensor is realized.
Further preferred is a method of variable impedance control of an end effector mechanism with a decoupled force sensor comprising the steps of:
step 1, constructing a physical model of the end effector, so as to determine the mechanical impedance of the end effector;
step 2, using a force sensor to directly measure and calculate the mechanical impedance and nonlinear force of the end effector;
step 3, compensating the mechanical impedance and the nonlinear force of the end effector based on the measurement and calculation in the step 2, and obtaining the end effector after decoupling;
and 4, adding an impedance controller based on the end effector decoupled in the step 3, and realizing variable impedance control of the end effector mechanism decoupled by the force sensor.
The physical model of the end effector is constructed, and the mechanical impedance is specifically:
the end effector physical model is a second-order system, and the model dynamics equation is as follows:
wherein K is z For mechanical rigidity of end effector mechanism, B z Machine for end effectorMechanical damping, M z X is the relative displacement of the movable end and the fixed end of the end effector,the relative speed of the movable end and the fixed end of the end effector; />The relative acceleration between the movable end and the fixed end of the end effector is that F is the output force of the linear motor at the current moment, F n Nonlinear forces for the end effector, including frictional forces and the like.
The mechanical impedance is expressed by the product of rigidity and displacement, the product of damping and speed, and the product of mass and acceleration, and according to the end effector dynamics equation, the mechanical impedance of the end effector mechanism can be obtained as follows:
wherein Z is z Z is the mechanical impedance of the end effector m Is the inertia of the moving end of the end effector.
The force sensor is used for directly measuring and calculating the mechanical impedance and nonlinear force of the end effector by the following specific modes:
according to the interaction force, the stress of the fixed end of the end effector has mechanical impedance, linear motor output force and nonlinear force, and the specific expression is as follows:
F g ＝Z z +F+F n (IV)
wherein F is g Is stressed by the fixed end of the end effector.
Installing a force sensor on the fixed end of the end effector according to the stress of the fixed end of the end effector; one end of the force sensor is arranged at the fixed end of the end effector, the other end of the force sensor is arranged at the industrial robot, and the force, the mechanical impedance and the nonlinear force measured by the force sensor are the forces after the end effector moves and changes due to the output force of the linear motor at the last moment, so the force expression measured by the force sensor is as follows:
F s ＝Z z +F t-1 +F n (V)
f in the formula s For the force measured by the force sensor, F t-1 For the output force of the linear motor at the previous moment, the force measured by the force sensor is equal to the force applied by the fixed end of the end effector.
Compensating the mechanical impedance and the nonlinear force of the end effector measured by the force sensor, and decoupling the mechanical impedance and the nonlinear force of the end effector, wherein the specific implementation modes are as follows:
placing a linear motor of the end effector on the current loop through a driver;
according to the force constant of the linear motor and the input current of the current loop of the linear motor, the output force of the linear motor of the end effector at the current moment can be obtained:
F＝i t K i (VI)
i in t The current is input to the current loop at the current moment of the linear motor, K i Is a linear motor force constant;
the mechanical impedance and the nonlinear force at the current moment can be obtained according to the formulas (V) and (VI):
Z z +F n ＝F s -i t-1 K i (VII)
i in t-1 Input current for current loop at moment on linear motor
According to formula (VII) above, the mechanical impedance and nonlinear forces of the end effector are converted to corresponding currents by linear motor force constants:
i in c A decoupling current converted from the sum of the mechanical impedance and the nonlinear force of the end effector at the current moment;
decoupling current i converted from the sum of mechanical impedance and nonlinear force of end effector at present moment c The actual input current of the linear motor current loop after the mechanical impedance and the nonlinear force are decoupled at the moment can be obtained by adding the actual input current of the linear motor current loop:
i in t The current loop is an actual input current of a current loop of the linear motor at the current moment, and i is a controller output current;
the current input by the current loop of the linear motor comprises a decoupling current converted by the sum of the output current of the controller, the mechanical impedance and the nonlinear force, so that the decoupling of the mechanical impedance and the nonlinear force of the end effector is realized.
An impedance controller is added to realize variable impedance control of the end effector based on force sensor measurement compensation, and the specific mode is as follows:
according to the rigidity and displacement correlation, the damping is correlated with the speed, so the output impedance of the impedance controller is:
z in d Is the output impedance of the impedance controller; k (K) d Stiffness set for the impedance controller; b (B) d Damping set for the impedance controller;
the current output by the impedance controller is:
i in i A current output by the impedance controller;
the output current of the impedance controller is added to the input of the current loop of the linear motor, and then the current actually input to the current loop of the linear motor is:
i t ＝i d +i c +i i (XII)
i in d A desired current for the entire control loop;
the input current of the current loop of the linear motor comprises the expected current of the whole control loop, the decoupling current converted by the sum of the mechanical impedance and the nonlinear force of the end effector and the output current of the impedance controller.
Based on the designed end effector mechanism, the invention calculates the reaction force of the mechanical impedance and the nonlinear force in the end effector through the force sensor, and converts the partial force into current compensation to be input into the current loop control of the linear motor, thereby realizing decoupling of the mechanical impedance and the nonlinear force; and adding an impedance controller into the controller on the basis of decoupling, and finally setting required rigidity and damping parameters according to different requirements to realize variable impedance control under different environmental rigidities. According to the end effector mechanism with the decoupling force sensor and the end effector variable impedance control method, the decoupling force sensor is integrated, the adjustment of the active compliance parameters of the end effector can be accurately achieved, and the active variable impedance control of the end effector is achieved.
Compared with the prior art, the invention has the following advantages:
the end effector mechanism with the decoupling force sensor and the variable impedance control method thereof have the characteristics of simple structure, can realize the change of rigidity and damping without changing a mechanical structure, can realize the control of any rigidity and damping, can directly measure the mechanical impedance of the end effector from the force sensor and decouple the mechanical impedance of the end effector without establishing an accurate dynamic model of the end effector, and can directly set the impedance of the end effector through the setting of the impedance controller, thereby realizing the variable impedance control of the end effector.
Drawings
FIG. 1 is an end effector mechanism of the present invention with a force sensor decoupled;
FIG. 2 is a schematic diagram of a control architecture according to the present invention;
FIG. 3 is a flow chart of an end effector variable impedance control method of force sensor decoupling according to the present invention;
FIG. 4 is a plot of the frequency domain dynamic response of the end effector mechanism and its dynamic characteristics for the variable impedance method end effector mechanism of the present invention;
FIG. 5 is a frequency domain dynamic response of an end effector mechanism and its theoretical frequency domain dynamic response of the end effector mechanism of the present invention with a virtual stiffness of 10000N/m and a virtual damping of 100N x S/m;
reference numerals illustrate: 100-fixed end; 101-a linear motor; 102-a mobile terminal; 1-a force sensor; 2-a fixed platform; 3-linear motor stator; 4-connecting rods; 5-linear motor mover; 6-linear encoder; 7-a slide block; 8-a mobile platform.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the following detailed description of the embodiments of the present invention will be given with reference to the accompanying drawings. Examples of these preferred embodiments are illustrated in the accompanying drawings. The embodiments of the invention shown in the drawings and described in accordance with the drawings are merely exemplary and the invention is not limited to these embodiments.
A force sensor decoupled end effector mechanism, as shown in fig. 1, comprises a linear motor 101, a fixed end 100, a movable end 102 and a linear encoder 6;
the linear motor 101 provides a power source in a linear direction for the end effector to realize the movement of the end effector, and comprises a linear motor stator 3 and a linear motor rotor 5;
the fixed end 100 is provided with a fixed platform 2, a plurality of connecting rods 4, a force sensor 1 and the linear motor stator 3;
the moving end 102 is provided with a moving platform 8, a plurality of sliding blocks 7 and the linear motor rotor 5;
the linear encoder 6 is mounted on the connecting rod 4 through a screw and faces the linear motor rotor 5, and is used for measuring the relative displacement between the fixed end 100 and the movable end 102;
a fixed platform 2, which is arranged at the fixed end 100 of the end effector, one side of which is connected with the stator 3 of the linear motor by using a screw, and the other side of which is connected with the force sensor 1 by using a screw;
the movable platform 8 is a moving end of the end effector and is used for realizing the movement of the end effector mechanism;
the connecting rods 4 are mounted on a plurality of hanging fixed platforms 2 through screws, and the inner sides of the connecting rods are provided with guide rails;
the sliding block 7 is arranged on the moving platform through a screw, and realizes the guiding function of the movement of the end effector and limits the travel of the end effector with the guide rail on the inner side of the connecting rod 4;
the force sensor 1 can be a strain type force sensor, a piezoelectric type force sensor and the like, the force direction is single degree of freedom of the movement direction of the end effector, the force sensor is arranged on the fixed side of the fixed platform 2 through screws, and the other end of the force sensor 1 is connected to the industrial robot.
Further, the end effector mechanism with the force sensor 1 decoupled is that the mechanical impedance and the nonlinear force of the end effector with the force sensor 1 decoupled are measured and calculated by the force sensor 1, so that the mechanical impedance decoupling of the end effector with the force sensor 1 decoupled is realized by changing the input of the linear motor 101.
The mechanical impedance of the end effector includes mechanical stiffness of the end effector and mechanical damping of the end effector.
The variable impedance control method of the end effector mechanism with the force sensor 1 decoupled, the whole control architecture is shown in fig. 2, comprises the following steps:
step 1, constructing a physical model of the end effector, so as to determine the mechanical impedance of the end effector;
step 2, using the force sensor 1 to directly measure and calculate the mechanical impedance and nonlinear force of the end effector;
step 3, compensating the mechanical impedance and the nonlinear force of the end effector based on the measurement and calculation in the step 2, and obtaining the end effector after decoupling;
and 4, adding an impedance controller based on the end effector decoupled in the step 3, and realizing variable impedance control of the end effector mechanism decoupled by the force sensor.
The physical model of the end effector is constructed, and the mechanical impedance is specifically:
the end effector physical model is a second-order system, and the model dynamics equation is as follows:
wherein K is z For mechanical rigidity of end effector, B z For mechanical damping of end effector, M z For the mass of the movable end 102 of the end effector, X is the relative displacement of the movable end 102 and the fixed end 100 of the end effector,is the relative speed of the movable end 102 and the fixed end 100; />The relative acceleration of the movable end 102 and the fixed end 100 is F, the output force of the linear motor 101 of the end effector at the current moment, F n Nonlinear forces for the end effector mechanism, including frictional forces and the like;
the mechanical impedance is expressed by the product of rigidity and displacement, the product of damping and speed, and the product of mass and acceleration, and according to the end effector dynamics equation, the mechanical impedance of the end effector can be obtained as follows:
wherein Z is z Z is the mechanical impedance of the end effector m Inertia for the end effector moving end;
the force sensor 1 is used to directly measure and calculate the mechanical impedance, nonlinear force of the end effector by:
according to the interaction force, the mechanical impedance of the end effector fixed end 100, the output force of the linear motor 101 and the nonlinear force are applied, and the specific expression is as follows:
F g ＝Z z +F+F n (IV)
wherein F is g Is forced by the end effector securing end 100.
Installing a force sensor 1 on the end effector fixing end 100 according to the stress of the end effector fixing end 100; one end of the force sensor 1 is installed at the fixed end 100 of the end effector, the other end is installed at the industrial robot, and the force, mechanical impedance and nonlinear force measured by the force sensor 1 are the forces after the end effector moves and changes due to the output force of the linear motor 101 at the previous moment, so the force expression measured by the force sensor is as follows:
F s ＝Z z +F t-1 +F n (V)
f in the formula s For the force measured by the force sensor 1, F t-1 To output the force of the linear motor 101 at the previous time, the force sensor 1 measures a force equal to the force applied by the end effector fixed end 100.
The mechanical impedance and the nonlinear force of the end effector measured by the force sensor 1 are compensated, so that the decoupling of the mechanical impedance and the nonlinear force of the end effector is realized, and the specific implementation modes are as follows:
placing the linear motor 101 of the end effector on the current loop through the driver;
based on the force constant of the linear motor 101 and the input current of the current loop of the linear motor 101, the output force of the end effector at the current moment of the linear motor 101 can be obtained:
F＝i t K i (VI)
i in t For the current moment of the linear motor 101The current loop inputs current, K i Is a linear motor 101 force constant;
the mechanical impedance and the nonlinear force at the current moment can be obtained according to the formulas (V) and (VI):
Z z +F n ＝F s -i t-1 K i (VII)
i in t-1 Input current for current loop at time on linear motor 101
According to equation (VII) above, the mechanical impedance and nonlinear forces of the end effector are converted to corresponding currents by linear motor 101 force constants:
i in c A decoupling current converted from the sum of the mechanical impedance and the nonlinear force of the end effector at the current moment;
decoupling current i converted from the sum of mechanical impedance and nonlinear force of end effector at present moment c The actual input current of the current loop of the linear motor 101 after the mechanical impedance and the nonlinear force are decoupled can be obtained by adding the actual input current of the current loop of the linear motor:
i in t The actual input current of the current loop of the linear motor 101 at the current moment is i the output current of the controller;
the current input by the current loop of the linear motor 101 at this time comprises a decoupling current converted by the sum of the controller output current, the mechanical impedance and the nonlinear force, so that decoupling of the mechanical impedance and the nonlinear force of the end effector is realized.
An impedance controller is added to realize variable impedance control of the end effector based on measurement compensation of the force sensor 1, and the specific mode is as follows:
according to the rigidity and displacement correlation, the damping is correlated with the speed, so the output impedance of the impedance controller is:
z in d Is the output impedance of the impedance controller; k (K) d Stiffness set for the impedance controller; b (B) d Damping set for the impedance controller;
the current output by the impedance controller is:
i in i A current output by the impedance controller;
when the output current of the impedance controller is added to the input of the current loop of the linear motor 101, the current actually input to the current loop of the linear motor 101 at this time is:
i t ＝i d +i c +i i (XII)
i in d A desired current for the entire control loop;
the input current of the current loop of the linear motor 101 at this time includes the desired current of the entire control loop, the decoupled current converted from the sum of the mechanical impedance and the nonlinear force of the end effector, and the output current of the impedance controller.
To illustrate the benefits of the present invention, since the stiffness and damping of the end effector mechanism are controlled and the end effector is a spring-mass-damping second order system, this example experiment demonstrates the present invention through frequency domain analysis experiments with an end effector mechanism that decouples the force sensor 1 and its variable impedance control method, as shown in fig. 4 and 5. The current in the experiment is input into the control system through the linear motor 101 and output is the relative position measured by the encoder. According to fig. 4, the mechanical end effector mechanism is subjected to parameter identification, and the dynamic response of the mechanical end effector mechanism is obtained through frequency domain fitting, so that the transfer function of the mechanical end effector mechanism is obtained; according to the mechanical system transfer function, the theoretical transfer function using the method of the invention is deduced by changing the impedance controller parameters, and parameter identification is carried out again, and as can be seen from fig. 5, the end effector mechanism with the force sensor 1 decoupled and the variable impedance control method thereof realize the change of rigidity and damping of the end effector, and can obtain the relatively accurate theoretical frequency domain dynamic response thereof.
Claims (9)
1. An end effector mechanism for decoupling a force sensor, comprising:
a sensor end for being connected with a robot, said sensor end comprising: a force sensor for connection with a robot and a fixed platform disposed on the force sensor;
a moving platform in sliding fit with the fixed platform;
and the sliding mechanism is arranged between the fixed platform and the movable platform.
2. The end effector mechanism of claim 1, wherein the slide mechanism comprises:
a connecting rod arranged on the fixed platform;
the sliding rail is arranged on the connecting rod;
a slide block matched with the slide rail;
the sliding block is connected with the mobile platform;
and the driving mechanism is used for driving the sliding block to linearly move on the sliding rail.
3. The force sensor decoupled end effector mechanism of claim 2, wherein the drive mechanism is a linear motor.
4. The force sensor decoupled end effector mechanism of claim 3, wherein the linear motor comprises:
a linear motor stator arranged on the fixed platform;
and the linear motor rotor is arranged on the mobile platform and matched with the linear motor stator.
5. The end effector mechanism of claim 4, wherein the linkage is provided with a linear encoder facing the linear motor mover.
6. A method of variable impedance control of an end effector mechanism decoupled from a force sensor, comprising the steps of:
step 1, using a force sensor to directly measure and calculate mechanical impedance and nonlinear force of an end effector mechanism decoupled by the force sensor;
step 2, compensating and decoupling based on the mechanical impedance and the nonlinear force measured and calculated in the step 1, so as to obtain a decoupled end effector mechanism of the force sensor;
and 3, adding an impedance controller to the end effector mechanism decoupled from the force sensor based on the decoupled end effector mechanism of the force sensor in the step 2, and performing variable impedance control on the end effector mechanism decoupled from the force sensor through a current signal output by the impedance controller.
7. The method of variable impedance control of a force sensor decoupled end effector mechanism of claim 6, wherein in step 1, the force sensor is used to directly measure and calculate the mechanical impedance, nonlinear force of the force sensor decoupled end effector mechanism, comprising:
according to the interaction force, the force measured by the force sensor comprises mechanical impedance, nonlinear force and linear motor output force, the force measured by the force sensor is a value at the current moment after the end effector is moved and changed due to the output force of the linear motor at the last moment, and therefore the specific expression of the force measured by the force sensor is as follows:
F s ＝Z z +F t-1 +F n
f in the formula s Measured by force sensorForce Z of (1) z Mechanical impedance of end effector mechanism decoupled for force sensor, F t-1 For the output force at one moment on the linear motor, F n A nonlinear force of the end effector mechanism decoupled from the force sensor;
the output force of the linear motor is calculated according to the force constant of the linear motor and the input current of the linear motor, so the mechanical impedance and the nonlinear force of the end effector mechanism decoupled by the force sensor are the force measured by the force sensor minus the output force of the linear motor, and the specific expression is as follows:
Z z +F n ＝F s -i t-1 K i
i in t-1 For the input current at one moment in the linear motor, K i Is a linear motor force constant.
8. The method for controlling the variable impedance of the end effector mechanism decoupled from the force sensor according to claim 7, wherein in step 2, compensation decoupling is performed based on the mechanical impedance and the nonlinear force measured and calculated in step 1, so as to obtain the end effector mechanism decoupled from the force sensor after decoupling, and the method specifically comprises the following steps of;
the mechanical impedance of the end effector mechanism decoupled by the force sensor, the nonlinear force is converted into a corresponding current by the force constant of the linear motor:
i in c Decoupling current converted for mechanical impedance, nonlinear force;
placing a linear motor on a current loop through a driver;
and adding the decoupling current converted by the mechanical impedance and the nonlinear force to the input of the current loop of the linear motor, wherein the input current of the linear motor comprises the mechanical impedance of the end effector mechanism decoupled by the force sensor and the corresponding current converted by the nonlinear force through the force constant of the linear motor, so that the end effector mechanism decoupled by the force sensor after decoupling is obtained.
9. The method for variable impedance control of a force sensor decoupled end effector mechanism according to claim 8, wherein in step 3, an impedance controller is added based on the force sensor decoupled end effector mechanism after the decoupling in step 2, and the variable impedance control is performed on the force sensor decoupled end effector mechanism by a current signal output by the impedance controller, specifically comprising:
the end effector mechanism decoupled from the decoupled force sensor realizes variable impedance control on the end effector mechanism decoupled from the decoupled force sensor through a current signal output by the impedance controller;
parameters of the impedance controller include stiffness and damping;
according to the relation between rigidity and displacement, damping and speed, the output current of the impedance controller is specifically:
i in i An output current of the impedance controller; k (K) d Stiffness in the impedance controller parameters; b (B) d Damping in the impedance controller parameters; x is the direct relative displacement of the movable platform and the fixed platform;the relative speed of the movable platform and the fixed platform is directly;
and the input current of the linear motor comprises the output current of the impedance controller and the corresponding current of the mechanical impedance and nonlinear force of the end effector mechanism decoupled by the force sensor through conversion of the force constant of the linear motor, so that the variable impedance control of the end effector mechanism decoupled by the force sensor is realized.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311520240.XA CN117301070A (en) | 2023-11-15 | 2023-11-15 | Force sensor decoupling end effector mechanism and variable impedance control method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311520240.XA CN117301070A (en) | 2023-11-15 | 2023-11-15 | Force sensor decoupling end effector mechanism and variable impedance control method thereof |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117301070A true CN117301070A (en) | 2023-12-29 |
Family
ID=89255414
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311520240.XA Pending CN117301070A (en) | 2023-11-15 | 2023-11-15 | Force sensor decoupling end effector mechanism and variable impedance control method thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117301070A (en) |
-
2023
- 2023-11-15 CN CN202311520240.XA patent/CN117301070A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR101472693B1 (en) | Load inertia estimation method and control parameter adjustment method | |
| JP2014512529A5 (en) | ||
| JP2004144196A (en) | Precision positioning device and processing machine using the same | |
| KR20080079590A (en) | Method and apparatus for compensating a stick motion generated at a quadrant changing position in nc machine tools | |
| CN109129479B (en) | Rigid-flexible coupling motion platform control method based on disturbance force compensation | |
| CN108015564A (en) | A kind of ultrasonic wave added overcomes friction linear motion platform | |
| CN110259879A (en) | For electronic Stewart structure without force feedback vibration isolation control method and system | |
| RU2394674C2 (en) | Self-adaptive electric drive of robot | |
| CN114922935A (en) | Rigid-flexible coupling potential force composite actuating mechanism and constant force control method | |
| WO2004095683A1 (en) | Twin synchronization control method | |
| CN112959217B (en) | Force control method and device for reducing grinding and polishing vibration | |
| CN110941181B (en) | Rigid-flexible coupling motion platform control method for piezoelectric ceramic connection | |
| CN117301070A (en) | Force sensor decoupling end effector mechanism and variable impedance control method thereof | |
| Liu et al. | Design of a control system for a macro-micro dual-drive high acceleration high precision positioning stage for IC packaging | |
| JP2000148210A (en) | Gain calculating device | |
| CN109143858B (en) | Rigid-flexible coupling motion platform control method based on disturbance force measurement compensation | |
| JP2005275588A (en) | Motor controller control parameter sensitivity analyzing device | |
| CN113824351B (en) | Inchworm type piezoelectric push rod motor and hysteresis characteristic compensation method thereof | |
| JP4507071B2 (en) | Motor control device | |
| CN110722523B (en) | Macro-micro composite motion platform based on piezoelectric ceramic measurement and compensation and application | |
| CN111273552B (en) | Chip mounter motion control method and system based on mathematical model | |
| CN109465827B (en) | Single-feedback single-drive rigid-flexible coupling platform control method | |
| RU2359306C2 (en) | Self-adapting electric drive of robot | |
| JP4939388B2 (en) | Feed-forward control device | |
| CN115847119B (en) | Rigid-flexible coupling micro-feeding system based on ultrasonic vibration |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |