CN114800522B - Hydraulic mechanical arm contact operation self-adaptive impedance control system without end force sensor - Google Patents
Hydraulic mechanical arm contact operation self-adaptive impedance control system without end force sensor Download PDFInfo
- Publication number
- CN114800522B CN114800522B CN202210572595.2A CN202210572595A CN114800522B CN 114800522 B CN114800522 B CN 114800522B CN 202210572595 A CN202210572595 A CN 202210572595A CN 114800522 B CN114800522 B CN 114800522B
- Authority
- CN
- China
- Prior art keywords
- force
- controller
- adaptive
- hydraulic cylinder
- mechanical arm
- 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.)
- Active
Links
- 230000007613 environmental effect Effects 0.000 claims abstract description 14
- 230000008859 change Effects 0.000 claims abstract description 12
- 230000003044 adaptive effect Effects 0.000 claims description 36
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000000034 method Methods 0.000 claims description 17
- 230000033001 locomotion Effects 0.000 claims description 10
- 238000013016 damping Methods 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 6
- 230000003993 interaction Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 230000004044 response Effects 0.000 claims description 5
- 238000013461 design Methods 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 238000013507 mapping Methods 0.000 claims description 4
- 238000000691 measurement method Methods 0.000 claims description 4
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 238000002715 modification method Methods 0.000 claims description 3
- OIGNJSKKLXVSLS-VWUMJDOOSA-N prednisolone Chemical compound O=C1C=C[C@]2(C)[C@H]3[C@@H](O)C[C@](C)([C@@](CC4)(O)C(=O)CO)[C@@H]4[C@@H]3CCC2=C1 OIGNJSKKLXVSLS-VWUMJDOOSA-N 0.000 claims description 3
- 238000005312 nonlinear dynamic Methods 0.000 abstract description 3
- 238000012360 testing method Methods 0.000 description 17
- 239000012636 effector Substances 0.000 description 9
- 238000005259 measurement Methods 0.000 description 4
- 238000010998 test method Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 239000011121 hardwood Substances 0.000 description 2
- 239000002023 wood Substances 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000035939 shock Effects 0.000 description 1
Images
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/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
- B25J13/085—Force or torque sensors
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Human Computer Interaction (AREA)
- Feedback Control In General (AREA)
Abstract
The invention discloses a hydraulic mechanical arm contact operation self-adaptive impedance control system without a tail end force sensor, which comprises two loops: the inner ring adopts a controller based on a model to compensate nonlinear dynamics characteristics of the hydraulic mechanical arm, so that accurate position tracking of the tail end is realized; the outer ring comprises an impedance controller and a force error compensator based on model reference self-adaptive control, which is added on the basis, so as to realize the active compliance and high-precision force tracking control of self-adaptive unknown environmental change.
Description
Technical Field
The invention relates to the field of hydraulic control, in particular to a self-adaptive impedance control system for contact operation of a hydraulic mechanical arm without a tail end force sensor.
Background
Precise, compliant control of the contact operation requires tip contact force feedback. The contact force feedback of the traditional electric driving mechanical arm generally depends on a multidimensional force/moment sensor on the end effector, but the hydraulic mechanical arm has large contact operation impact and much overload, and is extremely easy to damage and lose efficacy when applied to the end force sensor in the actual rescue occasion. Thus, the hydro-mechanical arm requires a "soft measurement" system that accurately estimates the tip contact force in real time. Although the research at present has proposed the hydraulic mechanical arm soft measurement method based on inverse dynamic model, its dynamic model all is based on three-dimensional model measurement and gets, and the force measurement precision is only 4.4% under the terminal constant load, and the force measurement precision is more reduced to 8.9% under the terminal variable force, is difficult to satisfy the demand of contact operation accurate control.
The hydraulic mechanical arm operation site is a typical non-structural environment, and two difficulties exist in the restraint motion control of the interaction process: firstly, the hydraulic system of the hydraulic mechanical arm has the problems of nonlinearity, parameter uncertainty, interference, complex variability of environmental load and the like, and meanwhile, the high bearing performance requires the large position servo rigidity and large mechanical structure rigidity of the mechanical arm, so that strong impact is easy to be caused when the mechanical arm is contacted with a non-structural environment. Secondly, the rescue contact operation object has the unstructured characteristics, the rigidity, the position and the shape of the rescue contact operation object are difficult to know, the control error of the traditional impedance control with fixed impedance parameters is difficult to converge to 0, even the situation of oscillation and even divergence is encountered, and the expected flexibility cannot be shown in the aspect of force control.
The technical content of the invention adopts a published hydraulic mechanical arm end force soft measurement method without end force sensor feedback, which is mentioned in the application number 2021112466820 of the invention.
Disclosure of Invention
The invention provides a hydraulic mechanical arm contact operation self-adaptive impedance control system without an end force sensor, which aims at the problems, wherein the control system comprises two loops: the inner ring adopts a controller based on a model to compensate nonlinear dynamics characteristics of the hydraulic mechanical arm, so that accurate position tracking of the tail end is realized; the outer ring is additionally provided with a force error compensator based on model reference self-adaptive control based on a traditional impedance controller, and the self-adaptive impedance controller realizes active compliance and high-precision force tracking control of self-adaptive unknown environmental change.
Wherein the inner loop model-based controller consists essentially of 3 independent parts: the hydraulic cylinder position feedback controller, the force controller, the position controller and the feedforward flow compensator, wherein the output of the position controller is the force deviation compensation of the hydraulic cylinder.
Further, the position controller performs position feedback control by using the length deviation of the hydraulic cylinder, compensates for the influence of the nonlinear factor, and compensates for the force deviation caused by the hydraulic cylinder.
Further, the force controller is the core, the force controller input comprising two parts, the feed-forward desired driving force and the position controller output. The position controller is used for eliminating the force deviation required by the hydraulic actuator; and feedforward the expected driving force, actively generating the required driving force according to the expected motion, and compensating the dynamic force in the hydraulic mechanical arm dynamic model.
Furthermore, the feedforward flow compensator is a specific feedforward term designed according to the movement of the hydraulic mechanical arm, calculates the flow compensation quantity based on a hydraulic cylinder dynamics model, and improves control response and precision.
Further, the inner loop model-based controller output is composed of a superposition of the force controller output and the feedforward flow compensator output signal.
Wherein, the outer loop impedance controller mainly comprises two parts: an impedance controller and a force error adaptive compensator; the position-based impedance controller establishes the interaction of the robotic arm with the environment as a typical "spring-mass-damping" system, calculating the applied to the preset trajectory position X based on the tip contact force and the expected value error r Change value Δx of (a) 1 . Because of the unknown environmental position and stiffness, the desired force contact performance cannot be achieved by only the impedance control, and meanwhile, the difficulty of on-line adjustment of three parameters of spring, mass and damping in the impedance model is also high. Therefore, a force error compensator based on model reference adaptive control is added on the basis of improved impedance control. The compensator constructs a system observation equation and a Lyapunov function thereof according to force tracking errors of an actual system and a reference model and the change rate of the errors. Based on the above, an adaptive control law which gives consideration to system stability and force error is designed, and another preset track correction value delta X is output 2 The impedance control can adapt to unknown environmental changes, and the tail end contact force is directly obtained by a hydraulic mechanical arm tail end force soft measurement method (application number is 2021112466820) without adding a force sensor.
Further, the implementation steps of the inner ring model-based controller are as follows:
the position controller equation expression is as follows:
F fb ＝ψ p (l des -l act )+ψ d (l des -l act ) (1)
wherein, psi is p Sum phi d For the positive-to-negative angle matrix, respectively comprising the proportional and differential control gains of the hydraulic cylinder, l des And l act The desired length and the actual length of the hydraulic cylinder, respectively.
Step 2, calculating the expected driving force of the joint, and performing force feedback control on the output force deviation of the hydraulic cylinder; the joint expected driving force comprises two parts, namely feedforward expected driving force and position controller output, wherein the feedforward expected driving force firstly solves an expected joint angle through inverse kinematics, then obtains joint expected driving moment through inverse dynamics equation, then can solve the feedforward expected driving force of each joint through mechanical arm joint geometrical relationship, and is input into a force controller for compensating dynamic force in the dynamics of the hydraulic mechanical arm, the position controller output is used for compensating force deviation, and the actual output force is calculated by measuring the pressure at two ends of a hydraulic cylinder through a hydraulic mechanical arm pressure sensor; the force controller equation is expressed as follows:
wherein v p 、υ i And v d Proportional, integral and differential gains, respectively, and greater than zero; f (F) act And F des The expected force and the actual force of the hydraulic cylinder are respectively. F (F) act And F des The expression is as follows:
F act ＝p a A a -p b A b (3)
F des ＝F ff +F fb (4)
p a ，p b the pressure of the rodless cavity and the rod cavity of the hydraulic cylinder can be measured by a pressure sensor, A a ，A b Is the area of a rodless cavity and a rod cavity of the hydraulic cylinder, wherein F ff Desired driving force for dynamic equation ff Expressed as:
F ffi ＝R i -1 (q)τ i (5)
wherein,,
r i is the arm of force, tau of the hydraulic cylinder i As joint drive torque, expressed as:
wherein M is E R n×n Is a symmetrical inertial matrix; c epsilon R n×n Is a centrifugal force and coriolis force matrix; g epsilon R n A gravity vector; f epsilon R n Is a friction vector.
Step 3, according to the movement required by the hydraulic mechanical arm, calculating the flow required in the movement process, and obtaining a feedforward control signal of the servo valve through the mapping relation between the flow and the valve control signal; first, valve flow rate Q flowing into and out of hydraulic cylinder 1 And Q 2 Expressed by the following nonlinear equation:
wherein,, u for control signal of valve C PA 、C PB 、C AT And C BT For valve port channel flow coefficient, p a And p a ，p b Is the pressure of the rodless cavity and the rod cavity, p s And p T For system pressure and tank pressure, the sign function sign (u) is defined as
Further, neglecting the internal leakage characteristic, the hydraulic cylinder flow continuity equation is:
wherein,, E c, the effective bulk modulus m For the maximum stroke of the piston, c for displacement of the piston, A a And A b The area of the piston of the rodless cavity and the piston of the rod cavity of the hydraulic cylinder are adopted, and the output force of the hydraulic cylinder can be obtained by the pressure of the two cavities in the step 2.
Further, the valve flow Q may be mapped to a unique control signal u ff The expression is as follows:
and 4, superposing the output signal of the force controller obtained in the step 2 and the output signal of the feedforward flow compensator to form a servo valve control signal, wherein the expression is as follows:
u＝u fb +u ff (11)。
further, the outer loop impedance controller comprises the following specific implementation steps:
step a, a traditional position-based impedance controller is established, the interaction between the mechanical arm and the environment is established as a typical spring-mass-damping system, and the position X applied to the preset track is calculated according to the error between the contact force of the tail end and the expected value r Change value Δx of (a) 1 The method comprises the steps of carrying out a first treatment on the surface of the Force tracking errors ΔF and ΔX 1 The kinetic relationship between them is expressed as follows:
wherein M is d 、C d And K d The desired mass, damping and stiffness of the target impedance, respectively.
Further, a position-based impedance controller output DeltaX is obtained 1 The expression is as follows:
ΔX 1 ＝ΔF·G(s) (13)
wherein G(s) =1/(M) d s 2 +B d s+K d )
When the contact force model of the mechanical arm and the environment is established, the environment is regarded as a first-order spring system, and the contact force F of the mechanical arm and the environment e Can be expressed as:
F e ＝K e (X e -X act )＝K e (X e -X c ) (14)
step b, adding a force error compensation loop delta X based on model reference adaptive control on the basis of the impedance controller based on the position in the step a 2 ；
ΔX 2 The model is obtained by adjusting the force tracking error delta F through model reference adaptive control, and the expression is as follows:
wherein g (t) is ΔF and
the relevant auxiliary function terms, p (t) and d (t), are the adaptive proportional and differential feedback gains, respectively.
In combination with the impedance control loop and the adaptive force compensation loop, the overall reference position correction is expressed as:
ΔX＝ΔX 1 +ΔX 2 (16)
further deriving the control equation of the MRAC from steps a and b:
wherein,,
the above equation is expressed as a form of state equation:
wherein the method comprises the steps of
Coefficient a p (t),b p (t) and R p (t) includes the adjustable control parameters and the system-known parameters.
Further, based on MRAC theory, a reference model is established according to target requirements, and the design of the self-adaptive impedance controller is carried out through Lyapunov stability theorem to obtain a coefficient a p (t),b p (t) and R p The regulation rule of (t) is that the output of the system follows the output of a reference model, and the reference model is taken as an ideal second-order system model:
further, in the form of a state equation:
wherein,,
is a state variable of the ideal reference model.
Further, an error equation of the response of the reference model and the actual system is obtained:
wherein the method comprises the steps of
Is the state vector of the total error state equation.
Further, energy function V (E e ,t)：
Wherein,,
β 0 ，β 1 and beta 2 Are positive constants, and P is a nonsingular positive definite real symmetric matrix. The lyapunov function V (E e T) can be expressed as:
V(E e t) > 0, further, according to the lyapunov stability theorem, if there is a positive definite real symmetry matrix Q, the formula is satisfied
The system satisfies the stability condition; the derivative of the lyapunov function is expressed as:
wherein,,
p 2 、p 3 is an element in matrix P; according to Lyapunov stability theorem, only ensure +.>
It can be demonstrated that the total error equation is asymptotically stable, the derivative according to the energy functionFurther, the following adaptive control law with respect to time-varying coefficients is ensured:
to implement the above adaptive control law, the state variable of the ideal reference model tracking zero input is set to 0; further, the adjustment law of the coefficient d (t), p (t), g (t) is:
wherein lambda is p ，λ v ，η，μ 1 Sum mu 2 All have positive values, d 0 ,p 0 ,g 0 The values of the initial moments of the three corresponding time-varying coefficients are respectively, and the constant can be taken as 0 together because the output continuity is ensured by the adjusting function of the impedance controller.
Further, according to step a, the relationship between the additional contact force of the robot tip and the environmental position can be determined by
Replace->
Meanwhile, in order to enhance the robustness of the system, the adaptive control law can be modified by a sigma modification method by considering the influence of unmodeled dynamics, and the control law is as follows:
compared with the traditional impedance control, the self-adaptive impedance control system for the hydraulic mechanical arm contact operation without the end force sensor can obtain higher force tracking precision and stability under the condition that the rigidity, the position, the structure and the like of a contact environment are unknown and changed, and compared with the traditional impedance controller, the self-adaptive impedance control system for the hydraulic mechanical arm contact operation without the end force sensor has smaller instant impact on the contact environment.
Drawings
FIG. 1 is a block diagram of an inner loop model-based controller of the present invention;
fig. 2 is a block diagram of an adaptive impedance control system of the present invention.
FIG. 3, a graph of the hydraulic mechanical arm and the flexible environment contact test process with different rigidities (3400N/m spring rigidity contact test)
FIG. 4, a graph of the contact test process of the hydraulic mechanical arm and the flexible environment with different rigidities (8000N/m spring rigidity test contact test)
FIG. 5, contact Environment stiffness 3400N/m contact test results (end effector position)
FIG. 6, contact environmental stiffness 3400N/m contact test results (end effector contact force)
FIG. 7, results of a contact environment stiffness 8000N/m contact test (end effector position)
FIG. 8, contact environmental stiffness 8000N/m contact test results (end effector contact force)
FIG. 9, contact test procedure with a rigid uncertainty Environment
Fig. 10, rigid environment test results (end effector position (desired contact force F d ＝200N)
Fig. 11, rigid environment test results (end effector position (desired contact force F d ＝500N)
FIG. 12, results of a rigid environmental test (end effector contact force)
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Example 1 as shown in figures 1-2,
an adaptive impedance control system for contact operation of a hydraulic mechanical arm without a tail end force sensor, wherein the control system comprises two loops: a position controller of the inner ring and an adaptive impedance controller of the outer ring. The inner ring adopts a controller based on a model to compensate nonlinear dynamics characteristics of the hydraulic mechanical arm, so that accurate position tracking of the tail end is realized; the outer ring is additionally provided with a force error compensator based on model reference self-adaptive control on the basis of the impedance controller to form a self-adaptive impedance controller, so that the active compliance and high-precision force tracking control of self-adaptive unknown environmental change is realized; the end contact force is directly calculated by a soft measuring method of the end force of the hydraulic mechanical arm without feedback of an end force sensor in an invention soft measuring method of the end force of the hydraulic mechanical arm disclosed by the inventor (application number is 2021112466820), and a force sensor is not required to be added.
Referring to fig. 1, the inner loop model-based controller consists essentially of 3 independent parts: the hydraulic cylinder position feedback controller, the force controller, the position controller and the feedforward flow compensator are characterized in that the force controller is a core, the position controller eliminates force deviation required by a hydraulic actuator, and the feedforward flow compensator calculates flow compensation quantity based on a hydraulic cylinder dynamics model according to a specific feedforward term of the motion design of the hydraulic mechanical arm, so that control precision is improved. The force controller inputs the desired driving force for each joint, which includes both the feedforward desired driving force and the position controller output. The feedforward expected driving force is firstly obtained by solving the expected joint angle through inverse kinematics, then the joint expected driving moment is obtained through an inverse kinetic equation, and the feedforward expected driving force of each joint can be obtained by utilizing the geometrical relationship of the joints of the mechanical arm. The position controller outputs a force offset compensation for the hydraulic cylinder. The controller output of the inner ring based on the model is formed by superposition of the output of the force controller and the output signal of the feedforward flow compensator. The inner ring model-based controller comprises the following specific implementation steps:
The position controller equation expression is as follows:
F fb ＝ψ p (l des -l act )+ψ d (l des -l act ) (1)
wherein, psi is p Sum phi d For the positive-to-negative angle matrix, respectively comprising the proportional and differential control gains of the hydraulic cylinder, l des And l act The desired length and the actual length of the hydraulic cylinder, respectively.
And 2, calculating the expected driving force of the joint, and performing force feedback control on the output force deviation of the hydraulic cylinder. The feedforward expected driving force is firstly obtained through inverse kinematics to obtain an expected joint angle, then an expected joint driving moment is obtained through an inverse kinematics equation, the feedforward expected driving force of each joint can be obtained through the geometrical relationship of the joints of the mechanical arm, the feedforward expected driving force is input into a force controller for compensating dynamic force in the dynamics of the hydraulic mechanical arm, and the output of the position controller is used for compensating force deviation. The actual output force is calculated by measuring the pressure at the two ends of the hydraulic cylinder by the hydraulic mechanical arm pressure sensor. The force controller equation is expressed as follows:
wherein v p 、υ i And v d Proportional, integral and differential gains, respectively, and greater than zero; f (F) act And F des The expected force and the actual force of the hydraulic cylinder are respectively. F (F) act And F des The expression is as follows:
F act ＝p a A a -p b A b (3)
F des ＝F ff +F fb (4)
p a ，p b the pressure of the rodless cavity and the rod cavity of the hydraulic cylinder can be measured by a pressure sensor, A a ，A b Is hydraulic pressureCylinder rodless and rod cavity area, where F ff Desired driving force for dynamic equation ff Expressed as:
F ffi ＝R i -1 (q)τ i (5)
wherein,,
r i is the arm of force, tau of the hydraulic cylinder i As joint drive torque, expressed as:
wherein M is E R n×n Is a symmetrical inertial matrix; c epsilon R n×n Is a centrifugal force and coriolis force matrix; g epsilon R n A gravity vector; f epsilon R n Is a friction vector.
And step 3, according to the movement required by the hydraulic mechanical arm, calculating the flow required in the movement process, and obtaining a feedforward control signal of the servo valve through the mapping relation between the flow and the valve control signal. The specific feedforward term considers the hydraulic system characteristics such as valve port flow nonlinearity, rong Qiangya force response and the like, so that the influence of the hydraulic system parameter time variation and nonlinearity on the control precision can be reduced. First, valve flow rate Q flowing into and out of hydraulic cylinder 1 And Q 2 Expressed by the following nonlinear equation:
wherein u is a control signal of the valve, C PA 、C PB 、C AT And C BT For valve port channel flow coefficient, p a And p b Is the pressure of the rodless cavity and the rod cavity, p s And p T For system pressure and tank pressure, the sign function sign (u) is defined as
Further, neglecting the internal leakage characteristic, the hydraulic cylinder flow continuity equation is:
wherein E is the effective bulk modulus, c m For maximum travel of the piston, c is displacement of the piston, A a And A b The area of the piston of the rodless cavity and the piston of the rod cavity of the hydraulic cylinder are adopted, and the output force of the hydraulic cylinder can be obtained by the pressure of the two cavities in the step 2.
Further, the valve flow Q may be mapped to a unique control signal u ff The expression is as follows:
and 4, superposing the output signal of the force controller obtained in the step 2 and the output signal of the feedforward flow compensator to form a servo valve control signal, wherein the expression is as follows:
u＝u fb +u ff (11)
the analysis of the control algorithm proves that the force deviation of the inner ring is compensated through the position feedback of the hydraulic cylinder, the force controller formed by solving the inverse dynamic equation and feeding back the pressure is used for compensating the dynamic force (inertial force, coriolis force, centripetal force, gravity and friction force) of the hydraulic mechanical arm, and the feedforward flow compensation is combined to play a role in improving the precision of the control precision of the inner ring.
Referring to fig. 2, the outer loop adaptive impedance controller is mainly composed of two parts: an impedance controller and a force error adaptive compensator; the impedance controller is in second-order dynamic compliance control, and when a load force acts on the system, the load force signal is converted into a position input signal for controlling the inner ring, so that the system has expected active compliance; the force error adaptive compensator is used for compensating force errors caused by environmental change characteristics. The outer loop self-adaptive impedance controller comprises the following specific implementation steps:
wherein M is d 、C d And K d The desired mass, damping and stiffness of the target impedance, respectively.
Further, a position-based impedance controller output DeltaX is obtained 1 The expression is as follows:
ΔX 1 ＝ΔF·G(s) (13)
wherein G(s) =1/(M) d s 2 +B d s+K d )
When the contact force model of the mechanical arm and the environment is established, the environment is regarded as a first-order spring system, and the contact force F of the mechanical arm and the environment e Can be expressed as:
F e ＝K e (X e -X act )＝K e (X e -X c ) (14)。
step 2, taking the defects of the traditional impedance control and the uncertainty of the environment into consideration, adding a force error compensation loop delta X based on model reference self-adaptive control on the basis of the traditional impedance control in step 1 2 。
ΔX 2 The model is obtained by adjusting the force tracking error delta F through model reference adaptive control, and the expression is as follows:
wherein g (t) is ΔF and
the relevant auxiliary function terms, p (t) and d (t), are the adaptive proportional and differential feedback gains, respectively.
In combination with the impedance control loop and the adaptive force compensation loop, the overall reference position correction is expressed as:
ΔX＝ΔX 1 +ΔX 2 (16)
further obtaining a control equation of the MRAC according to the step 1 and the step 2:
wherein,,
the above equation is expressed as a form of state equation:
wherein the method comprises the steps of
Coefficient a p (t),b p (t) and R p (t) includes the adjustable control parameters and the system-known parameters.
Further, based on MRAC theory, a reference model is established according to target requirements, the design of the self-adaptive impedance controller is carried out through Lyapunov stability theorem,obtaining coefficient a p (t),b p (t) and R p The regulation rule of (t) is that the output of the system follows the output of a reference model, and the reference model can be taken as an ideal second-order system model:
further, in the form of a state equation:
wherein,,
is a state variable of the ideal reference model.
Further, an error equation of the response of the reference model and the actual system is obtained:
wherein the method comprises the steps of
Is the state vector of the total error state equation.
Further, energy function V (E e ,t)：
Wherein,,
β 0 ，β 1 and beta 2 Are positive constants, and P is a nonsingular positive definite real symmetric matrix. The lyapunov function V (E e T) can be expressed as:
obviously, V (E e T) > 0, further, according to the lyapunov stability theorem, if there is a positive definite real symmetry matrix Q, the formula is satisfied
The system satisfies the stability condition. The derivative of the lyapunov function is expressed as:
wherein,,
p 2 、p 3 is an element in the matrix P. According to Lyapunov stability theorem, only ensure +.>
The overall error equation may prove asymptotically stable. Further, from the mathematical expression of the derivative of the energy function, the following adaptive control law is guaranteed with respect to the time-varying coefficients:
to implement the adaptive control law above, the state variable of the ideal reference model tracking zero input is set to 0. Further, the adjustment law of the coefficient d (t), p (t), g (t) is:
wherein lambda is p ，λ v ，η，μ 1 Sum mu 2 All have positive values, d 0 ,p 0 ,g 0 The values of the initial moments of the three corresponding time-varying coefficients are respectively, and the constant can be taken as 0 together because the output continuity is ensured by the adjusting function of the impedance controller.
Further, according to step 1, the relationship between the additional contact force of the robot tip and the environmental position can be calculated from
Replace->
Meanwhile, in order to enhance the robustness of the system, the adaptive control law can be modified by a sigma modification method by considering the influence of unmodeled dynamics, and the control law is as follows:
the compensator constructs a system observation equation and a Lyapunov function thereof according to force tracking errors of an actual system and a reference model and the change rate of the errors; based on the above, an adaptive control law which gives consideration to system stability and force error is designed, and another preset track correction value delta X is output 2 Enabling the impedance control to accommodate unknown environmental changes.
Experimental example 1:
the contact operation test is carried out under the flexible environment and the rigid environment respectively, and compared with the traditional impedance control and the contact force control performance of the proposed self-adaptive impedance control (shown that Non-IC is Non-impedance control, CIC is traditional impedance control and AIC is self-adaptive impedance control). First, the flexible environment contact environment test is divided into two groups of springs (3400N/m and 8000N/m) with different rigidities of the contact object, and the contact position is unknown.
The test procedure is shown in fig. 3 and 4, and the test results are shown in fig. 5 to 8. By adopting the traditional impedance control and the self-adaptive impedance control provided by the project, the end effector can realize active flexibility. However, the proposed adaptive impedance control can achieve higher force tracking accuracy and stability with varying stiffness locations of the contact environment than conventional impedance control. The experimental tests were carried out with the desired contact forces 200N and 500N as targets for decomposition. The test procedure is shown in fig. 9, and the test results are shown in fig. 10 to 12. Compared with the traditional impedance controller, the proposed adaptive impedance controller has smaller contact moment impact with a rigid environment. In addition, the root mean square error of force control is reduced from about 70N to 60N, and the force tracking precision is improved by 5% while the active flexibility is ensured.
In summary, compared with the traditional impedance control, the adaptive impedance control system for the hydraulic mechanical arm provided by the embodiment can obtain higher force tracking precision and stability under the condition that the contact environment is unknown and changed; the contact moment with a rigid environment is less shock compared to conventional impedance controllers. Secondly, the rigid contact environment is formed by splicing hard wood boards, the wood board surface has a certain inclination, the environment is a typical unknown position, and the rigidity of different positions of the hard wood boards is slightly different due to the splicing of a plurality of different wood boards.
The embodiments of the present invention are disclosed as preferred embodiments, but not limited thereto, and those skilled in the art will readily appreciate from the foregoing description that various extensions and modifications can be made without departing from the spirit of the present invention.
Claims (2)
1. The hydraulic mechanical arm contact operation self-adaptive impedance control system without the end force sensor is characterized by comprising an inner ring and an outer ring: the inner ring is a model-based controller, and the outer ring is an adaptive impedance controller consisting of an impedance controller and a force error adaptive compensator;
the model-based controller includes: the hydraulic cylinder position feedback controller, the force controller, the position controller and the feedforward flow compensator are arranged in the hydraulic cylinder, and the output of the position controller is the force deviation compensation of the hydraulic cylinder; the input of the force controller is the expected driving force of each joint, and the expected driving force of each joint comprises two parts, namely feedforward expected driving force and position controller output; the feedforward expected driving force is firstly obtained by solving an expected joint angle through inverse kinematics, then a joint expected driving moment is obtained through an inverse kinetic equation, and the feedforward expected driving force of each joint can be obtained by utilizing the geometrical relationship of the joints of the mechanical arm; the controller output of the inner ring based on the model is formed by superposing the output of the force controller and the output signal of the feedforward flow compensator;
the self-adaptive impedance controller mainly comprises two parts: an impedance controller and a force error adaptive compensator; the position-based impedance controller establishes the interaction between the mechanical arm and the environment as a spring-mass-damping system, and calculates the position X applied to the preset track according to the error of the contact force of the tail end and the expected value r A change value of (2); the force error self-adaptive compensator constructs a system observation equation and a Lyapunov function thereof according to the force tracking errors of the actual system and the reference model and the change rate of the errors; based on the method, another preset track correction value is output, so that impedance control can adapt to an unknown environment, and the tail end contact force is calculated and obtained by adopting a hydraulic mechanical arm tail end force soft measurement method based on feedback of a non-tail end force sensor;
the inner ring model-based controller comprises the following specific implementation steps:
step 1, performing position feedback control on the length deviation of a hydraulic cylinder; compensating force deviation caused by the hydraulic cylinder through the position controller;
the position controller equation expression is as follows:
wherein, psi is p Sum phi d For the positive-to-negative angle matrix, respectively comprising the proportional and differential control gains of the hydraulic cylinder, l des And l act The expected length and the actual length of the hydraulic cylinder are respectively;
step 2, calculating the expected driving force of the joint, and performing force feedback control on the output force deviation of the hydraulic cylinder; the joint expected driving force comprises a feedforward expected driving force and a position controller output; the feedforward expected driving force is firstly obtained by solving an expected joint angle through inverse kinematics, then an expected joint driving moment is obtained through an inverse kinetic equation, the feedforward expected driving force of each joint can be obtained through the geometrical relationship of the joints of the mechanical arm, the feedforward expected driving force is input into a force controller for compensating dynamic force in the dynamics of the hydraulic mechanical arm, the output of the position controller is used for compensating force deviation, and the actual output force is obtained by calculating the pressure at two ends of a hydraulic cylinder measured by a pressure sensor of the hydraulic mechanical arm; the force controller equation is expressed as follows:
wherein v p 、υ i And v d Proportional, integral and differential gains, respectively, and greater than zero; f (F) act And F des The expected force and the actual force of the hydraulic cylinder are respectively; f (F) act And F des The expression is as follows:
F act ＝p a A a -p b A b (3)
F des ＝F ff +F fb (4)
p a ，p b the pressure of the rodless cavity and the rod cavity of the hydraulic cylinder can be measured by a pressure sensor, A a ，A b Is the area of a rodless cavity and a rod cavity of the hydraulic cylinder, wherein F ff Desired driving force for dynamic equation ff Expressed as:
F ffi ＝R i -1 (q)τ i (5)
wherein,,
r i is the arm of force, tau of the hydraulic cylinder i As joint drive torque, expressed as:
wherein M is E R n×n Is a symmetrical inertial matrix; c epsilon R n×n Is a centrifugal force and coriolis force matrix; g epsilon R n A gravity vector; f epsilon R n Is a friction vector;
step 3, according to the movement required by the hydraulic mechanical arm, calculating the flow required in the movement process, and obtaining a feedforward control signal of the servo valve through the mapping relation between the flow and the valve control signal; first, valve flow rate Q flowing into and out of hydraulic cylinder 1 And Q 2 Expressed by the following nonlinear equation:
wherein,, u for control signal of valve C PA 、C PB 、C AT And C BT For valve port channel flow coefficient, p a And p b Is the pressure of the rodless cavity and the rod cavity, p s And p T For system pressure and tank pressure, the sign function sign (u) is defined as
Further, neglecting the internal leakage characteristic, the hydraulic cylinder flow continuity equation is:
wherein,, E c, the effective bulk modulus m For the maximum stroke of the piston, c for displacement of the piston, A a And A b The area of the piston of the rodless cavity and the piston of the rod cavity of the hydraulic cylinder, and the output force of the hydraulic cylinder can be obtained by the pressure of the two cavities in the step 2;
further, the valve flow rate Q mayControl signal u with mapping as unique ff The expression is as follows:
and 4, superposing the output signals of the force controller obtained in the step 2 and the step 3 and the output signal of the feedforward flow compensator to form a control signal of the servo valve, wherein the expression is as follows:
u＝u fb +u ff (11)。
2. the adaptive impedance control system for contact operation of a hydraulic robotic arm without an end force sensor of claim 1, wherein the outer loop impedance controller comprises the following specific implementation steps:
step a, establishing an impedance controller, establishing interaction between the mechanical arm and the environment as a spring-mass-damping system, and calculating and applying the mechanical arm and the environment to a preset track position X according to the error of the tail end contact force and an expected value r Change value Δx of (a) 1 The method comprises the steps of carrying out a first treatment on the surface of the Force tracking errors Δf and Δx 1 The kinetic relationship between them is expressed as follows:
wherein M is d 、C d And K d The required mass, damping and stiffness of the target impedance, respectively;
further, an impedance controller output DeltaX is obtained 1 The expression is as follows:
ΔX 1 ＝ΔF·G(s) (13)
wherein G(s) =1/(M) d s 2 +B d s+K d )
When the contact force model of the mechanical arm and the environment is established, the environment is regarded as a first-order spring system, and the contact force F of the mechanical arm and the environment e Can be expressed as:
F e ＝K e (X e -X act )＝K e (X e -X c ) (14)
step b, adding a force error compensation loop delta X based on model reference self-adaptive control on the basis of the impedance controller in the step a 2 ；ΔX 2 The model is obtained by adjusting the force tracking error delta F through model reference adaptive control, and the expression is as follows:
wherein g (t) is ΔF and
the relevant auxiliary function terms, p (t) and d (t), are the adaptive proportional and differential feedback gains, respectively;
in combination with the impedance control loop and the adaptive force compensation loop, the overall reference position correction is expressed as:
ΔX＝ΔX 1 +ΔX 2 (16)
further deriving the control equation of the MRAC from steps a and b:
wherein,,
the above equation is expressed as a form of state equation:
wherein the method comprises the steps of
Coefficient a p (t)，b p (t) and R p (t) includes adjustable control parameters and system-known parameters;
further, based on MRAC theory, a reference model is established according to target requirements, and the design of the self-adaptive impedance controller is carried out through Lyapunov stability theorem to obtain a coefficient a p (t)，b p (t) and R p The regulation rule of (t) is that the output of the system follows the output of a reference model, and the reference model is taken as an ideal second-order system model:
further, in the form of a state equation:
further, an error equation of the response of the reference model and the actual system is obtained:
wherein the method comprises the steps of
A state vector that is the total error state equation;
further, energy function V (E e ,t)：
Wherein,,
β 0 ，β 1 and beta 2 Are positive constants, and P is a nonsingular positive definite real symmetric matrix; the lyapunov function V (E e T) can be expressed as:
V(E e t) > 0, further, according to the lyapunov stability theorem, if there is a positive definite real symmetry matrix Q, the formula is satisfied
The system satisfies the stability condition; the derivative of the lyapunov function is expressed as:
wherein,,
p 2 、p 3 is an element in matrix P; according to Lyapunov stability theorem, only ensure +.>
The total error equation is proved to be asymptotically stable, further guaranteed to be based on the mathematical expression of the derivative of the energy functionThe following adaptive control law for time-varying coefficients:
to implement the above adaptive control law, the state variable of the ideal reference model tracking zero input is set to 0; further, the adjustment law of the coefficient d (t), p (t), g (t) is:
wherein lambda is p ，λ v ，η，μ 1 Sum mu 2 All have positive values, d 0 ,p 0 ,g 0 The values of the corresponding three time-varying coefficient initial moments are respectively, and the constant can be taken as 0 together because the output continuity is ensured by the adjusting function of the impedance controller;
further, according to step a, the relationship between the additional contact force of the robot tip and the environmental position can be determined by
Instead of
Meanwhile, in order to enhance the robustness of the system, the adaptive control law can be modified by a sigma modification method by considering the influence of unmodeled dynamics, and the control law is as follows:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210572595.2A CN114800522B (en) | 2022-05-25 | 2022-05-25 | Hydraulic mechanical arm contact operation self-adaptive impedance control system without end force sensor |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210572595.2A CN114800522B (en) | 2022-05-25 | 2022-05-25 | Hydraulic mechanical arm contact operation self-adaptive impedance control system without end force sensor |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114800522A CN114800522A (en) | 2022-07-29 |
| CN114800522B true CN114800522B (en) | 2023-07-11 |
Family
ID=82516713
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210572595.2A Active CN114800522B (en) | 2022-05-25 | 2022-05-25 | Hydraulic mechanical arm contact operation self-adaptive impedance control system without end force sensor |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114800522B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115639750A (en) * | 2022-10-19 | 2023-01-24 | 浙江大学 | Force tracking control method of electro-hydrostatic elastic actuator |
| CN115670505B (en) * | 2022-10-24 | 2024-09-24 | 华南理工大学 | Ultrasonic scanning control system based on multi-mode fusion |
| CN116079739B (en) * | 2023-02-24 | 2024-07-16 | 北京工业大学 | Mechanical arm tail end position and force variable impedance cooperative control method based on tracking differentiator |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103831831B (en) * | 2014-03-18 | 2016-07-06 | 西华大学 | There is non-linear remote control system position and the force tracing control system of time-vary delay system |
| CN103878790B (en) * | 2014-04-18 | 2015-08-12 | 南京工程学院 | Towards the multi-mode elastic driver of lower limb assistance exoskeleton robot |
| CN108406765B (en) * | 2018-02-06 | 2021-05-07 | 南京航空航天大学 | Impedance control method for open-chain multi-arm robot |
| US20220314467A1 (en) * | 2019-10-25 | 2022-10-06 | Sony Interactive Entertainment Inc. | Foot structure of legged mobile robot, and legged mobile robot |
| CN111546350B (en) * | 2020-04-30 | 2021-09-10 | 浙江大学 | Multi-joint heavy-load hydraulic robot system and high-precision motion control method |
| CN112180761A (en) * | 2020-09-29 | 2021-01-05 | 燕山大学 | Impedance control simulation platform for leg of foot robot based on position and construction method |
-
2022
- 2022-05-25 CN CN202210572595.2A patent/CN114800522B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN114800522A (en) | 2022-07-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114800522B (en) | Hydraulic mechanical arm contact operation self-adaptive impedance control system without end force sensor | |
| CN105563489B (en) | Flexible mechanical arm control method based on non-linear Auto Disturbances Rejection Control Technique | |
| CN109927032B (en) | Mechanical arm track tracking control method based on high-order sliding-mode observer | |
| CN111546350B (en) | Multi-joint heavy-load hydraulic robot system and high-precision motion control method | |
| CN107561935B (en) | Motor position servo system friction compensation control method based on multilayer neural network | |
| CN104260107B (en) | The method of a kind of implementation space mechanical arm flexible joint compensation of gear clearance | |
| CN111152225A (en) | Uncertain mechanical arm fixed time trajectory tracking control method with input saturation | |
| CN105045103B (en) | One kind is based on LuGre friction models servo manipulator friciton compensation control system and method | |
| CN109426150B (en) | Load simulator backstepping control method based on extended state observer | |
| CN107765548B (en) | Launching platform high-precision motion control method based on double observers | |
| CN113325716B (en) | Underwater hydraulic mechanical arm nonlinear robust control method based on extended observer | |
| CN104965413B (en) | The friciton compensation self-adaptation control method of controlledization flat pad | |
| Tumari et al. | The direct strain feedback with PID control approach for a flexible manipulator: Experimental results | |
| Chen et al. | Observer-based adaptive robust precision motion control of a multi-joint hydraulic manipulator | |
| CN117289612B (en) | Hydraulic mechanical arm self-adaptive neural network control method | |
| CN110561427B (en) | Series elastic driver compliance control system and method based on compensation | |
| CN113219841B (en) | Nonlinear control method for underwater multi-joint hydraulic mechanical arm based on adaptive robustness | |
| CN115230805B (en) | Accurate steering control method for heavy-load AGV | |
| CN109773827B (en) | Method and system for load estimation gravity compensation of mechanical arm | |
| Zhang et al. | An Adaptive Robust Controller for Hydraulic Robotic Manipulators with a Flow-Mapping Compensator | |
| US8670869B2 (en) | Robot controller | |
| CN112460095B (en) | Active and passive load composite control method of electro-hydraulic composite cylinder | |
| CN114755921A (en) | Interference compensation control method for underwater electric hydrostatic actuator | |
| Li et al. | Robust control of a two-link flexible manipulator with neural networks based quasi-static deflection compensation | |
| JP6647562B2 (en) | Actuator drive control method and actuator drive control device |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |