CN114800522B - Hydraulic mechanical arm contact operation self-adaptive impedance control system without end force sensor - Google Patents

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

Adaptive impedance control system for contact operation of hydraulic manipulator without end force sensor

Technical Field

The invention relates to the field of hydraulic control, and in particular to a contact operation adaptive impedance control system for a hydraulic mechanical arm without an end force sensor.

Background Art

Precise and smooth control of contact operations requires end contact force feedback. The contact force feedback of traditional electric-driven manipulators generally relies on multi-dimensional force/torque sensors on the end effector, but hydraulic manipulators have large impacts and many overloads in contact operations. When applied to actual emergency rescue situations, the end force sensors are easily damaged and fail. Therefore, hydraulic manipulators need a "soft measurement" system that accurately estimates the end contact force in real time. Although current research has proposed a soft measurement method for hydraulic manipulators based on an inverse dynamics model, its dynamic model is based on three-dimensional model measurements. The force measurement accuracy under constant load at the end is only 4.4%, and the force measurement accuracy under variable force at the end drops to 8.9%, which is difficult to meet the needs of precise control of contact operations.

The hydraulic manipulator operation site is generally a typical non-structural environment. There are two difficulties in the constrained motion control of its interactive process: first, the hydraulic system of the hydraulic manipulator has problems such as nonlinearity, parameter uncertainty, interference and complex and variable environmental loads. At the same time, high load-bearing performance requires large position servo stiffness and mechanical structure stiffness and inertia of the manipulator, so it is easy to cause strong impact when in contact with the non-structural environment. Secondly, the objects of emergency rescue operations are non-structural, and their stiffness, position and shape are difficult to know. The traditional impedance control with fixed impedance parameters is difficult to converge to 0, and even encounters oscillation or even divergence, and cannot show the expected flexibility in force control.

The technical content of the present invention adopts the calculation of the hydraulic manipulator arm end force softness measurement method without end force sensor feedback mentioned in a published invention "A hydraulic manipulator arm end force softness measurement method" (application number 2021112466820).

Summary of the invention

In view of the above-mentioned problems, the present invention proposes an adaptive impedance control system for contact operations of a hydraulic manipulator without an end force sensor, wherein the control system includes two loops: the inner loop adopts a model-based controller to compensate for the nonlinear dynamic characteristics of the hydraulic manipulator and realize accurate position tracking of the end; the outer loop adds a force error compensator based on model reference adaptive control and an adaptive impedance controller on the basis of the traditional impedance controller to realize active compliance and high-precision force tracking control that adapts to unknown environmental changes.

The inner loop model-based controller is mainly composed of three independent parts: a hydraulic cylinder position feedback controller, a force controller, a position controller and a feedforward flow compensator. The output of the position controller is the force deviation compensation of the hydraulic cylinder.

Furthermore, the position controller utilizes the length deviation of the hydraulic cylinder to perform position feedback control, compensate for the influence of nonlinear factors, and compensate for the force deviation caused by the hydraulic cylinder.

Furthermore, the force controller is the core, and the force controller input includes two parts: feedforward desired driving force and position controller output. The position controller is used to eliminate the force deviation required by the hydraulic actuator; and the feedforward desired driving force actively generates the required driving force according to the desired movement, which is used to compensate the dynamic force in the dynamic model of the hydraulic manipulator.

Furthermore, the feedforward flow compensator is a specific feedforward item designed according to the movement of the hydraulic mechanical arm - the flow compensation amount is calculated based on the hydraulic cylinder dynamics model to improve the control response and accuracy.

Furthermore, the output of the inner loop model-based controller is composed of the superposition of the force controller output and the feedforward flow compensator output signal.

Among them, the outer loop impedance controller is mainly composed of two parts: an impedance controller and a force error adaptive compensator; the position-based impedance controller establishes the interaction between the manipulator and the environment as a typical "spring-mass-damper" system, and calculates the change value ΔX ₁ applied to the preset trajectory position X _r according to the error between the end contact force and the expected value. Due to the unknown position and stiffness of the environment, the desired force contact performance cannot be achieved by relying solely on the impedance control, and it is also difficult to adjust the three parameters of spring, mass and damping in the impedance model online. Therefore, a force error compensator based on model reference adaptive control is added on the basis of the improved impedance control. The compensator constructs the system observation equation and its Lyapunov function based on the force tracking error between the actual system and the reference model and the rate of change of the error. Based on this, an adaptive control law that takes into account both system stability and force error is designed, and another preset trajectory correction value ΔX ₂ is output, so that the impedance control can adapt to unknown environmental changes, and the end contact force is directly obtained through a hydraulic manipulator end force soft measurement method (application number 2021112466820), without the need to add a force sensor.

Furthermore, the specific implementation steps of the inner loop model-based controller are as follows:

Step 1, performing position feedback control on the length deviation of the hydraulic cylinder; compensating the force deviation caused by the hydraulic cylinder through a position controller;

The position controller equation is expressed as follows:

F _fb ＝ψ _p (l _des -l _act )+ψ _d (l _des -l _act ) (1)

Among them, ψ _p and ψ _d are positive diagonal matrices, containing the proportional and differential control gains of the hydraulic cylinder, respectively, and l _des and l _act are the desired length and actual length of the hydraulic cylinder, respectively.

Step 2, calculate the joint expected driving force, and perform force feedback control on the hydraulic cylinder output force deviation; the joint expected driving force includes two parts: feedforward expected driving force and position controller output, wherein the feedforward expected driving force is first solved by inverse kinematics to obtain the expected joint angle, and then the joint expected driving torque is obtained by the inverse dynamics equation, and then the geometric relationship of the mechanical arm joints is used to obtain the feedforward expected driving force of each joint, which is input into the force controller to compensate for the dynamic force in the hydraulic mechanical arm dynamics, and the position controller output is used to compensate for the force deviation, and the actual output force is calculated by the pressure at both ends of the hydraulic cylinder measured by the hydraulic mechanical arm pressure sensor; the force controller equation is expressed as follows:

Among them, υ _p , υ _i and υ _d are proportional, integral and differential gains respectively, and are greater than zero; F _act and F _des are the expected force and actual force of the hydraulic cylinder respectively. The expressions of F _act and F _des are as follows:

_Fact ＝ _{paAa - pbAb} (3 ₎

_Fdes ＝ _Fff + _Ffb (4)

p _a , p _b are the pressures of the rodless and rod chambers of the hydraulic cylinder, which can be measured by the pressure sensor. A _a , _Ab are the areas of the rodless and rod chambers of the hydraulic cylinder. In the formula, F _ff is the expected driving force obtained by the dynamic equation. F _ff is expressed as:

F _ffi =R _i ^-1 (q)τ _i (5)

r_i为液压缸力臂，τ_i为关节驱动力矩，表示为：in,

_ri is the hydraulic cylinder force arm, _τi is the joint driving torque, expressed as:

Among them, M∈R ^n×n is the symmetric inertia matrix; C∈R ^n×n is the centrifugal force and Coriolis force matrix; G∈R ⁿ is the gravity vector; f∈R ⁿ is the friction vector.

Step 3, according to the required movement of the hydraulic manipulator, the required flow rate during the movement is calculated, and the feedforward control signal of the servo valve is obtained through the mapping relationship between the flow rate and the valve control signal; first, the valve control flow rates _Q1 and _Q2 flowing into and out of the hydraulic cylinder are expressed by the following nonlinear equations:

Where _u is the control signal of the valve, _CPA , _CPB , _CAT and _CBT are the flow coefficients of the valve port channel, _pa and _pa , _pb are the pressures of the rodless chamber and the rod chamber, _ps and _pT are the system pressure and the tank pressure, and the sign function sign(u) is defined as

Furthermore, ignoring the internal leakage characteristics, the flow continuity equation of the hydraulic cylinder is:

Where, _E is the effective bulk modulus, _cm is the maximum stroke of the piston, _c is the displacement of the piston, _Aa and _Ab are the piston areas of the rodless chamber and the rod chamber of the hydraulic cylinder, and the output force of the hydraulic cylinder can be obtained from the pressure of the two chambers in step 2.

Furthermore, the valve-controlled flow Q can be mapped to a unique control signal u _ff , which is expressed as follows:

Step 4: The force controller output signal obtained in step 2 and step 3 is superimposed with the feedforward flow compensator output signal to form a servo valve control signal, which is expressed as follows:

u＝u _fb +u _ff (11).

Furthermore, the specific implementation steps of the outer loop impedance controller are as follows:

Step a, establish a traditional position-based impedance controller, establish the interaction between the manipulator and the environment as a typical "spring-mass-damper" system, and calculate the change value ΔX ₁ applied to the preset trajectory position X _r according to the error between the end contact force and the expected value; the dynamic relationship between the force tracking error ΔF and ΔX ₁ is expressed as follows:

where M _d , C _{d ,} and K _d are the required mass, damping, and stiffness for the target impedance, respectively.

Furthermore, the expression of the position-based impedance controller output ΔX ₁ is obtained as follows:

ΔX ₁ = ΔF·G(s) (13)

Among them, G(s)=1/(M _d s ² +B _d s +K _d )

When establishing the contact force model between the robot and the environment, the environment is regarded as a first-order spring system, and the contact force _Fe between the robot and the environment 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 ΔX ₂ based on model reference adaptive control to the position-based impedance controller in step a;

ΔX ₂ is obtained by adjusting the force tracking error ΔF through the model reference adaptive control, and its expression is:

有关的辅助函数项，p(t)和d(t)分别是自适应比例和微分反馈增益。Where g(t) is the relationship between ΔF and

The related auxiliary function terms, p(t) and d(t), are the adaptive proportional and derivative feedback gains, respectively.

Combining the impedance control loop and the adaptive force compensation loop, the overall reference position correction is expressed as:

ΔX＝ΔX ₁ +ΔX ₂ (16)

According to step a and step b, the control equation of MRAC is further obtained:

in,

Transform the above equation into the form of state equation:

系数a_p(t),b_p(t)和R_p(t)包含了可调控制参数和系统已知参数。in

The coefficients a _p (t), b _p (t) and R _p (t) include adjustable control parameters and known system parameters.

Furthermore, based on the MRAC theory, a reference model is established according to the target requirements, and the adaptive impedance controller is designed through the Lyapunov stability theorem to obtain the adjustment rules of the coefficients a _p (t), b _p (t) and R _p (t), so that the output of the system follows the output of the reference model. The reference model is taken as the ideal second-order system model as follows:

Further, it is transformed into the form of state equation:

是理想参考模型的状态变量。in,

are the state variables of the ideal reference model.

Furthermore, the error equation between the reference model and the actual system response is obtained:

为总的误差状态方程的状态向量。in

is the state vector of the total error state equation.

Furthermore, according to Lyapunov's stability theorem, the quadratic energy function V(E _e ,t) is constructed:

β₀，β₁和β₂均为正的常数，P为非奇异正定实对称矩阵。则李雅普诺夫函数V(E_e,t)可以表示为：in,

β ₀ , β ₁ and β ₂ are all positive constants, and P is a non-singular positive definite real symmetric matrix. Then the Lyapunov function V(E _e ,t) can be expressed as:

则系统满足稳定性条件；李雅普诺夫函数导数表示为：V(E _e ,t)＞0. Further, according to Lyapunov's stability theorem, if there exists a positive definite real symmetric matrix Q that satisfies the formula

Then the system satisfies the stability condition; the derivative of the Lyapunov function is expressed as:

p₂、p₃是矩阵P中的元素；根据李雅普诺夫稳定性定理可知，只要保证

就可证明总的误差方程是渐近稳定的,根据能量函数导数的数学表达式，进一步地，得到保证以下关于时变系数的自适应控制律：in,

p ₂ and p ₃ are elements in the matrix P. According to Lyapunov's stability theorem, as long as

It can be proved that the total error equation is asymptotically stable. According to the mathematical expression of the energy function derivative, the following adaptive control law with respect to time-varying coefficients is obtained:

In order to realize the above adaptive control law, the state variables of the ideal reference model tracking zero input are set to 0; further, the adjustment law of the coefficients d(t), p(t), and g(t) is obtained as follows:

Among them, λ _p , λ _v , η, μ ₁ and μ ₂ are all positive values, d ₀ , p ₀ , g ₀ are the values of the corresponding three time-varying coefficients at the initial moment, and since the regulation of the impedance controller has ensured the continuity of the output, the constants can be taken as 0.

代替

同时考虑到未建模动力学的影响，为了增强系统的鲁棒性能，可将以上的自适应控制律用σ修正法做修正，此时控制律为：Further, according to step a, the relationship between the additional contact force at the end of the robot and the environmental position can be expressed as

replace

At the same time, considering the influence of unmodeled dynamics, in order to enhance the robust performance of the system, the above adaptive control law can be corrected by the σ correction method. At this time, the control law is:

Compared with traditional impedance control, the adaptive impedance control system for contact operation of a hydraulic manipulator arm without an end force sensor in the present invention can obtain higher force tracking accuracy and stability when the stiffness, position, structure, etc. of the contact environment are unknown and changing. Compared with traditional impedance controllers, the instantaneous impact of contact with a rigid environment is smaller.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 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.

Figure 3. Diagram of the contact test process of the hydraulic manipulator and flexible environments with different stiffness (contact test with 3400N/m spring stiffness)

Figure 4. Contact test process of hydraulic manipulator and flexible environment with different stiffness (8000N/m spring stiffness test)

Figure 5. Contact test results of contact environment stiffness 3400N/m (end effector position)

Figure 6. Contact test results of contact environment stiffness 3400N/m (end effector contact force)

Figure 7. Contact test results of contact environment stiffness 8000N/m (end effector position)

Figure 8. Contact test results of contact environment stiffness 8000N/m (end effector contact force)

Figure 9. Contact test process with a rigid uncertain environment

Figure 10. Rigid environment test results (end effector position (expected contact force F _d = 200N)

Figure 11. Rigid environment test results (end effector position (expected contact force F _d = 500N)

Figure 12. Rigid environment test results (end effector contact force)

DETAILED DESCRIPTION

The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

Embodiment 1, as shown in Figure 1-2,

An adaptive impedance control system for contact operation of a hydraulic manipulator without an end force sensor, wherein the control system includes two loops: an inner loop position controller and an outer loop adaptive impedance controller. The inner loop adopts a model-based controller to compensate for the nonlinear dynamic characteristics of the hydraulic manipulator and achieve accurate position tracking of the end; the outer loop adds a force error compensator based on model reference adaptive control on the basis of the impedance controller to form an adaptive impedance controller to achieve active compliance and high-precision force tracking control that adapts to unknown environmental changes; the end contact force is directly calculated by the soft measurement method for the end force of a hydraulic manipulator without end force sensor feedback mentioned in an invention already disclosed by the inventor, "A method for soft measurement of end force of a hydraulic manipulator" (application number 2021112466820), without the need to add a force sensor.

Referring to Figure 1, the inner loop model-based controller is mainly composed of three independent parts: a hydraulic cylinder position feedback controller, a force controller, a position controller and a feedforward flow compensator. The force controller is the core. The position controller eliminates the force deviation required by the hydraulic actuator. The feedforward flow compensator calculates the flow compensation amount based on the hydraulic cylinder dynamics model according to the specific feedforward items of the motion design of the hydraulic manipulator to improve the control accuracy. The input of the force controller is the expected driving force of each joint. The joint expected driving force includes two parts: the feedforward expected driving force and the position controller output. The feedforward expected driving force is first solved by inverse kinematics to obtain the expected joint angle, and then the joint expected driving torque is obtained by the inverse dynamics equation. Then, the geometric relationship of the manipulator joints can be used to calculate the feedforward expected driving force of each joint. The output of the position controller is the force deviation compensation of the hydraulic cylinder. The output of the inner loop model-based controller is composed of the superposition of the force controller output and the feedforward flow compensator output signal. The specific implementation steps of the inner loop model-based controller are as follows:

Step 1: Perform position feedback control on the length deviation of the hydraulic cylinder. Due to the influence of factors such as nonlinear friction between the joints of the robot arm and pressure loss along the pipeline, the force deviation caused by the hydraulic cylinder is compensated by the position controller.

The position controller equation is expressed as follows:

F _fb ＝ψ _p (l _des -l _act )+ψ _d (l _des -l _act ) (1)

Among them, ψ _p and ψ _d are positive diagonal matrices, containing the proportional and differential control gains of the hydraulic cylinder, respectively, and l _des and l _act are the desired length and actual length of the hydraulic cylinder, respectively.

Step 2, calculate the expected driving force of the joint, and perform force feedback control on the hydraulic cylinder output force deviation. The feedforward expected driving force is first solved by inverse kinematics to obtain the expected joint angle, and then the expected joint driving torque is obtained by the inverse dynamics equation. The geometric relationship of the robot joints is then used to calculate the feedforward expected driving force of each joint, which is input into the force controller to compensate for the dynamic force in the hydraulic robot dynamics, and the position controller output is used to compensate for the force deviation. The actual output force is calculated by measuring the pressure at both ends of the hydraulic cylinder by the hydraulic robot pressure sensor. The force controller equation is expressed as follows:

Among them, υ _p , υ _i and υ _d are proportional, integral and differential gains respectively, and are greater than zero; F _act and F _des are the expected force and actual force of the hydraulic cylinder respectively. The expressions of F _act and F _des are as follows:

_Fact ＝ _{paAa - pbAb} (3 ₎

_Fdes ＝ _Fff + _Ffb (4)

p _a , p _b are the pressures of the rodless and rod chambers of the hydraulic cylinder, which can be measured by the pressure sensor. A _a , _Ab are the areas of the rodless and rod chambers of the hydraulic cylinder. In the formula, F _ff is the expected driving force obtained by the dynamic equation. F _ff is expressed as:

F _ffi =R _i ^-1 (q)τ _i (5)

r_i为液压缸力臂，τ_i为关节驱动力矩，表示为：in,

_ri is the hydraulic cylinder force arm, _τi is the joint driving torque, expressed as:

Among them, M∈R ^n×n is the symmetric inertia matrix; C∈R ^n×n is the centrifugal force and Coriolis force matrix; G∈R ⁿ is the gravity vector; f∈R ⁿ is the friction vector.

Step 3, according to the required movement of the hydraulic manipulator, the required flow rate during the movement is calculated, and the feedforward control signal of the servo valve is obtained through the mapping relationship between the flow rate and the valve control signal. This specific feedforward term takes into account the hydraulic system characteristics such as the nonlinearity of the valve port flow rate and the pressure response of the chamber, so it can reduce the influence of the time-varying and nonlinear parameters of the hydraulic system on the control accuracy. First, the valve control flow rates _Q1 and _Q2 flowing into and out of the hydraulic cylinder are expressed by the following nonlinear equations:

Where u is the control signal of the valve, _CPA , _CPB , _CAT and _CBT are the flow coefficients of the valve port channel, _pa and _pb are the pressures of the rodless chamber and the rod chamber, _ps and _pT are the system pressure and the tank pressure, and the sign function sign(u) is defined as

Furthermore, ignoring the internal leakage characteristics, the flow continuity equation of the hydraulic cylinder is:

Where, E is the effective bulk modulus, _cm is the maximum stroke of the piston, c is the displacement of the piston, _Aa and _Ab are the piston areas of the rodless chamber and the rod chamber of the hydraulic cylinder, and the output force of the hydraulic cylinder can be obtained from the pressure of the two chambers in step 2.

Furthermore, the valve-controlled flow Q can be mapped to a unique control signal u _ff , which is expressed as follows:

Step 4: The force controller output signal obtained in step 2 and step 3 is superimposed with the feedforward flow compensator output signal to form a servo valve control signal, which is expressed as follows:

u＝u _fb +u _ff (11)

From the above analysis of the control algorithm, it can be seen that the force deviation of the inner loop is compensated by the hydraulic cylinder position feedback, and the force controller composed of the inverse dynamics equation solution and pressure feedback is used to compensate for the dynamic force of the hydraulic manipulator (inertia force, Coriolis force and centripetal force, gravity and friction force), and combined with the feedforward flow compensation, it plays a role in improving the accuracy of the inner loop control.

Referring to Figure 2, the outer loop adaptive impedance controller mainly consists of two parts: an impedance controller and a force error adaptive compensator; wherein the impedance controller is a second-order dynamic compliance control. When the load force acts on the system, the load force signal is converted into a position input signal for controlling the inner loop, so that the system has the desired active compliance; the force error adaptive compensator is used to compensate for the force error caused by the environmental change characteristics. The specific implementation steps of the outer loop adaptive impedance controller are as follows:

Step 1: Establish a position-based impedance controller, establish the interaction between the manipulator and the environment as a typical "spring-mass-damper" system, and calculate the change value ΔX ₁ applied to the preset trajectory position X _r based on the error between the end contact force and the expected value. The dynamic relationship between the force tracking error ΔF and ΔX ₁ is expressed as follows:

where M _d , C _{d ,} and K _d are the required mass, damping, and stiffness for the target impedance, respectively.

Furthermore, the expression of the position-based impedance controller output ΔX ₁ is obtained as follows:

ΔX ₁ = ΔF·G(s) (13)

Among them, G(s)=1/(M _d s ² +B _d s +K _d )

When establishing the contact force model between the robot and the environment, the environment is regarded as a first-order spring system, and the contact force _Fe between the robot and the environment can be expressed as:

F _e =K _e (X _e -X _act ) =K _e (X _e -X _c ) (14).

Step 2: Considering the defects of traditional impedance control and the uncertainty of the environment, a force error compensation loop ΔX ₂ based on model reference adaptive control is added to the traditional impedance control in step 1.

ΔX ₂ is obtained by adjusting the force tracking error ΔF through the model reference adaptive control, and its expression is:

有关的辅助函数项，p(t)和d(t)分别是自适应比例和微分反馈增益。Where g(t) is the relationship between ΔF and

The related auxiliary function terms, p(t) and d(t), are the adaptive proportional and derivative feedback gains, respectively.

Combining the impedance control loop and the adaptive force compensation loop, the overall reference position correction is expressed as:

ΔX＝ΔX ₁ +ΔX ₂ (16)

According to step 1 and step 2, the control equation of MRAC is further obtained:

in,

Transform the above equation into the form of state equation:

系数a_p(t),b_p(t)和R_p(t)包含了可调控制参数和系统已知参数。in

The coefficients a _p (t), b _p (t) and R _p (t) include adjustable control parameters and known system parameters.

Furthermore, based on the MRAC theory, a reference model is established according to the target requirements, and the adaptive impedance controller is designed through the Lyapunov stability theorem to obtain the adjustment rules of the coefficients a _p (t), b _p (t) and R _p (t), so that the output of the system follows the output of the reference model. The reference model can be taken as the ideal second-order system model as follows:

Further, it is transformed into the form of state equation:

是理想参考模型的状态变量。in,

are the state variables of the ideal reference model.

Furthermore, the error equation between the reference model and the actual system response is obtained:

为总的误差状态方程的状态向量。in

is the state vector of the total error state equation.

Furthermore, according to Lyapunov's stability theorem, the quadratic energy function V(E _e ,t) is constructed:

β₀，β₁和β₂均为正的常数，P为非奇异正定实对称矩阵。则李雅普诺夫函数V(E_e,t)可以表示为：in,

β ₀ , β ₁ and β ₂ are all positive constants, and P is a non-singular positive definite real symmetric matrix. Then the Lyapunov function V(E _e ,t) can be expressed as:

则系统满足稳定性条件。李雅普诺夫函数导数表示为：Obviously, V(E _e ,t)＞0. Furthermore, according to Lyapunov's stability theorem, if there exists a positive definite real symmetric matrix Q that satisfies the formula

Then the system satisfies the stability condition. The derivative of the Lyapunov function is expressed as:

p₂、p₃是矩阵P中的元素。根据李雅普诺夫稳定性定理可知，只要保证

就可证明总的误差方程是渐近稳定的。根据能量函数导数的数学表达式，进一步地，得到保证以下关于时变系数的自适应控制律：in,

p ₂ and p ₃ are elements in the matrix P. According to Lyapunov's stability theorem, as long as

It can be proved that the total error equation is asymptotically stable. According to the mathematical expression of the energy function derivative, we can further obtain the following adaptive control law that guarantees the time-varying coefficients:

In order to realize the above adaptive control law, the state variables of the ideal reference model tracking zero input are set to 0. The adjustment law of the coefficients d(t), p(t), and g(t) is further obtained as:

Among them, λ _p , λ _v , η, μ ₁ and μ ₂ are all positive values, d ₀ , p ₀ , g ₀ are the values of the corresponding three time-varying coefficients at the initial moment, and since the regulation of the impedance controller has ensured the continuity of the output, the constants can be taken as 0.

代替

同时考虑到未建模动力学的影响，为了增强系统的鲁棒性能，可将以上的自适应控制律用σ修正法做修正，此时控制律为：Further, according to step 1, the relationship between the additional contact force at the end of the robot and the environmental position can be obtained by

replace

At the same time, considering the influence of unmodeled dynamics, in order to enhance the robust performance of the system, the above adaptive control law can be corrected by the σ correction method. At this time, the control law is:

The compensator constructs the system observation equation and its Lyapunov function according to the force tracking error between the actual system and the reference model and the rate of change of the error. Based on this, an adaptive control law that takes into account both system stability and force error is designed, and another preset trajectory correction value ΔX ₂ is output, so that the impedance control can adapt to unknown environmental changes.

Experimental Example 1:

Contact operation tests were carried out in flexible environment and rigid environment respectively, and the contact force control performance of traditional impedance control and the proposed adaptive impedance control were compared (Note: Non-IC is non-impedance control, CIC is traditional impedance control, AIC is adaptive impedance control). First, the flexible environment contact environment test is divided into two groups of springs with different stiffness (stiffness 3400N/m and 8000N/m) as the contact objects, and the contact position is unknown.

The test process is shown in Figures 3 and 4, and the test results are shown in Figures 5 to 8. The end effector can achieve active compliance using both traditional impedance control and the adaptive impedance control proposed in this project. However, compared with traditional impedance control, the proposed adaptive impedance control can achieve higher force tracking accuracy and stability when the stiffness position of the contact environment changes. The test was carried out with the expected contact force of 200N and 500N as the target decomposition. The test process is shown in Figure 9, and the test results are shown in Figures 10 to 12. Compared with the traditional impedance controller, the proposed adaptive impedance controller has a smaller instantaneous impact when in contact with the rigid environment. In addition, the root mean square error of force control was reduced from approximately 70N to 60N, and the force tracking accuracy was improved by 5% while ensuring active compliance.

In summary, the adaptive impedance control system of the hydraulic manipulator provided by the embodiment can obtain higher force tracking accuracy and stability when the contact environment is unknown and changing compared with the traditional impedance control; compared with the traditional impedance controller, the instantaneous impact of contact with the rigid environment is smaller. Secondly, the rigid contact environment is made up of hardwood boards, and the surface of the board has a certain slope. It is a typical unknown position environment, and due to the splicing of multiple different wooden boards, the stiffness of different positions also has slight differences.

The embodiments of the present invention disclose preferred embodiments, but are not limited thereto. A person skilled in the art can easily understand the spirit of the present invention based on the above embodiments and make different extensions and changes. However, as long as they do not deviate from the spirit of the present invention, they are all within the protection scope of the present invention.

Claims (2)

1. An adaptive impedance control system for contact operation of a hydraulic manipulator without an end force sensor, characterized in that it comprises two loops, an inner loop and an outer loop: the inner loop is a model-based controller, and the outer loop is an adaptive impedance controller composed of an impedance controller and a force error adaptive compensator; The model-based controller includes: a hydraulic cylinder position feedback controller, a force controller, a position controller and a feedforward flow compensator, wherein 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 joint driving force includes two parts: the feedforward expected driving force and the position controller output; the feedforward expected driving force first solves the expected joint angle by inverse kinematics, and then obtains the expected joint driving torque by inverse dynamics equation, and then uses the geometric relationship of the robot arm joint to obtain the feedforward expected driving force of each joint; the output of the inner loop model-based controller is composed of the superposition of the force controller output and the feedforward flow compensator output signal; The adaptive impedance controller is mainly composed of two parts: an impedance controller and a force error adaptive compensator; the position-based impedance controller establishes the interaction between the manipulator and the environment as a "spring-mass-damper" system, and calculates the change value applied to the preset trajectory position _Xr according to the error between the end contact force and the expected value; the force error adaptive compensator constructs the system observation equation and its Lyapunov function according to the force tracking error between the actual system and the reference model and the rate of change of the error; based on this, another preset trajectory correction value is output, so that the impedance control can adapt to the unknown environment. The aforementioned end contact force is calculated by a hydraulic manipulator end force soft measurement method based on feedback from a force sensor without an end force sensor; The specific implementation steps of the inner loop model-based controller are as follows: Step 1, performing position feedback control on the length deviation of the hydraulic cylinder; compensating the force deviation caused by the hydraulic cylinder through a position controller; The position controller equation is expressed as follows:

Among them, ψ _p and ψ _d are positive diagonal matrices, containing the proportional and differential control gains of the hydraulic cylinder, respectively, and l _des and l _act are the desired length and actual length of the hydraulic cylinder, respectively; Step 2, calculate the joint expected driving force, and perform force feedback control on the hydraulic cylinder output force deviation; the joint expected driving force includes two parts: feedforward expected driving force and position controller output; wherein, the feedforward expected driving force is first solved by inverse kinematics to obtain the expected joint angle, and then the joint expected driving torque is obtained by the inverse dynamics equation, and then the geometric relationship of the mechanical arm joints can be used to obtain the feedforward expected driving force of each joint, which is input into the force controller to compensate for the dynamic force in the hydraulic mechanical arm dynamics, and the position controller output is used to compensate for the force deviation, and the actual output force is calculated by the pressure at both ends of the hydraulic cylinder measured by the hydraulic mechanical arm pressure sensor; the force controller equation is expressed as follows:

Among them, υ _p , υ _i and υ _d are proportional, integral and differential gains respectively, and are greater than zero; F _act and F _des are the expected force and actual force of the hydraulic cylinder respectively; the expressions of F _act and F _des are as follows: _Fact ＝ _{paAa - pbAb} (3 _{) Fdes} ＝ _Fff + _Ffb (4) p _a , p _b are the pressures of the rodless and rod chambers of the hydraulic cylinder, which can be measured by the pressure sensor. A _a , _Ab are the areas of the rodless and rod chambers of the hydraulic cylinder. In the formula, F _ff is the expected driving force obtained by the dynamic equation. F _ff is expressed as: F _ffi =R _i ^-1 (q)τ _i (5) in,

_ri is the hydraulic cylinder force arm, _τi is the joint driving torque, expressed as:

Among them, M∈R ^n×n is the symmetric inertia matrix; C∈R ^n×n is the centrifugal force and Coriolis force matrix; G∈R ⁿ is the gravity vector; f∈R ⁿ is the friction vector; Step 3, according to the required movement of the hydraulic manipulator, the required flow rate during the movement is calculated, and the feedforward control signal of the servo valve is obtained through the mapping relationship between the flow rate and the valve control signal; first, the valve control flow rates _Q1 and _Q2 flowing into and out of the hydraulic cylinder are expressed by the following nonlinear equations:

Where _u is the control signal of the valve, _CPA , _CPB , _CAT and _CBT are the flow coefficients of the valve port channel, _pa and _pb are the pressures of the rodless chamber and the rod chamber, _ps and _pT are the system pressure and the tank pressure, and the sign function sign(u) is defined as

Furthermore, ignoring the internal leakage characteristics, the flow continuity equation of the hydraulic cylinder is:

Where, _E is the effective bulk modulus, _cm is the maximum stroke of the piston, _c is the displacement of the piston, _Aa and _Ab are the piston areas of the rodless cavity and the rod cavity of the hydraulic cylinder, and the output force of the hydraulic cylinder can be obtained from the pressure of the two cavities in step 2; Furthermore, the valve-controlled flow Q can be mapped to a unique control signal u _ff , which is expressed as follows:

Step 4, the force controller output signal obtained in step 2 and step 3 is superimposed with the feedforward flow compensator output signal to form the servo valve control signal, which is expressed as follows: u＝u _fb +u _ff (11). 2. According to the adaptive impedance control system for contact operation of hydraulic manipulator without end force sensor of claim 1, it is characterized in that the specific implementation steps of the outer loop impedance controller are as follows: Step a, establish an impedance controller, establish the interaction between the manipulator and the environment as a "spring-mass-damper" system, and calculate the change value ΔX ₁ applied to the preset trajectory position X _r according to the error between the end contact force and the expected value; the dynamic relationship between the force tracking error △F and ΔX ₁ is expressed as follows:

Where M _d , C _d and K _d are the required mass, damping and stiffness of the target impedance respectively; Furthermore, the output _ΔX1 of the impedance controller is expressed as follows: ΔX ₁ = ΔF·G(s) (13) Among them, G(s)=1/(M _d s ² +B _d s +K _d ) When establishing the contact force model between the robot and the environment, the environment is regarded as a first-order spring system, and the contact force _Fe between the robot and the environment 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 ΔX ₂ based on model reference adaptive control to the impedance controller in step a; ΔX ₂ is obtained by adjusting the force tracking error ΔF by model reference adaptive control, and its expression is:

Where g(t) is the relationship between ΔF and

The related auxiliary function terms, p(t) and d(t), are the adaptive proportional and derivative feedback gains, respectively; Combining the impedance control loop and the adaptive force compensation loop, the overall reference position correction is expressed as: ΔX＝ΔX ₁ +ΔX ₂ (16) According to step a and step b, the control equation of MRAC is further obtained:

in,

Transform the above equation into the form of state equation:

in

The coefficients a _p (t), b _p (t) and R _p (t) include adjustable control parameters and known system parameters; Furthermore, based on the MRAC theory, a reference model is established according to the target requirements, and the adaptive impedance controller is designed through the Lyapunov stability theorem to obtain the adjustment rules of the coefficients a _p (t), _bp (t) and R _p (t), so that the output of the system follows the output of the reference model. The reference model is taken as the ideal second-order system model as follows:

Further, it is transformed into the form of state equation:

Furthermore, the error equation between the reference model and the actual system response is obtained:

in

is the state vector of the total error state equation; Furthermore, according to Lyapunov's stability theorem, the quadratic energy function V(E _e ,t) is constructed:

in,

β ₀ , β ₁ and β ₂ are all positive constants, and P is a non-singular positive definite real symmetric matrix; then the Lyapunov function V(E _e ,t) can be expressed as:

V(E _e ,t)＞0. Further, according to Lyapunov's stability theorem, if there exists a positive definite real symmetric matrix Q that satisfies the formula

Then the system satisfies the stability condition; the derivative of the Lyapunov function is expressed as:

in,

p ₂ and p ₃ are elements in the matrix P. According to Lyapunov's stability theorem, as long as

It can be proved that the total error equation is asymptotically stable. According to the mathematical expression of the energy function derivative, the following adaptive control law with respect to time-varying coefficients is obtained:

In order to realize the above adaptive control law, the state variables of the ideal reference model tracking zero input are set to 0; further, the adjustment law of the coefficients d(t), p(t), and g(t) is obtained as follows:

Among them, λ _p , λ _v , η, μ ₁ and μ ₂ are all positive values, d ₀ , p ₀ , g ₀ are the values of the three corresponding time-varying coefficients at the initial moment, and since the regulation of the impedance controller has ensured the continuity of the output, the constants can be taken as 0; Further, according to step a, the relationship between the additional contact force at the end of the robot and the environmental position can be expressed as

replace

At the same time, considering the influence of unmodeled dynamics, in order to enhance the robust performance of the system, the above adaptive control law can be corrected by the σ correction method. At this time, the control law is:

Images