CN108189037B

CN108189037B - Manipulator primary and secondary coordination control method based on disturbance observer

Info

Publication number: CN108189037B
Application number: CN201810049340.1A
Authority: CN
Inventors: 邓华; 张翼; 钟国梁
Original assignee: Central South University
Current assignee: Central South University
Priority date: 2018-01-18
Filing date: 2018-01-18
Publication date: 2021-04-06
Anticipated expiration: 2038-01-18
Also published as: CN108189037A

Abstract

A disturbance observer and primary/secondary coordinated control method for manipulators, aiming at the problems of complex operating environment, unknown grasping object information, lack of sensors and disturbance in the environment when the manipulator grasps objects, and the problems of existing force sensors due to Uncertain contact points, incomplete detection information and other limitations, the present invention provides a disturbance observer for estimating disturbance information in the environment, and then using the estimated disturbance amount, combined with the estimated mass, stiffness and surface friction of the grasped object Coefficients are used to estimate the reflex grip force in reflex control. Finally, combined with the main mode control method, a primary and secondary coordinated control strategy is established to control the manipulator to grasp objects stably under disturbance. It replaces the traditional method that can only obtain disturbance information through high-cost sensors. The method is simple and effective, and improves the efficiency of the manipulator's stable and reliable grasping problem. At the same time, the error of the disturbance observer is analyzed, which lays the foundation for the accurate analysis of the disturbance in the grasping process of the manipulator. foundation.

Description

Manipulator primary and secondary coordination control method based on disturbance observer

Technical Field

The invention relates to a manipulator primary and secondary coordination control method based on a disturbance observer, which is particularly suitable for operations of a manipulator for grabbing objects under an uncertain environment and the like.

Background

The manipulator imitates a human hand, the operation environment is very complex, the objects are different in gripping, different gripping forces are required for objects with different qualities, and various disturbances exist in the environment. When a person grasps an object in a disturbance environment, the touch sense of the hand of the person feels a sliding signal of the object, the brain of the person can estimate a proper grasping force according to the characteristics of the object, and stable grasping of the object is achieved through a nervous system. For the manipulator, only a few sensors are provided, and the manipulator is not as flexible as a human hand, cannot know the gripping state of a gripped object, and cannot adjust the gripping force of the manipulator according to the change of the gripping state of the gripped object. The force adjustment is thus not real-time. Moreover, if the gripping force of the manipulator is too small, the gripping force is not timely increased, the object may slip, conversely, if the gripping force is too large, and the gripping force is not timely decreased, the object may be deformed or even destroyed, and meanwhile, the weight, the material and the rigidity of daily articles are not known, and external disturbance is uncertain, so that the control of the gripping force is very difficult. How to determine the proper gripping force of the manipulator, and when the gripping force is too small or is disturbed by the environment, how to adjust the size of the gripping force becomes a difficult problem which needs to be solved urgently in the application of the manipulator. In order to improve the disturbance resistance of the manipulator, a plurality of scholars at home and abroad carry out extensive research, mainly comprising open-loop control and closed-loop control.

The open-loop control of the manipulator is mainly researched by adopting a biological decoding technology to directly control the manipulator. The main biological signals studied are electromyographic signals (EMG), electroencephalographic signals (EEG) and Peripheral Nerve Signals (PNS). The human sport will is obtained by directly decoding the biological signal, and then the manipulator is directly controlled by amplifying the scale. When the manipulator is not stably gripped, the output biological signal is adjusted through visual feedback, and the gripping force state is adjusted according to the change of the biological signal. However, the bio-decoding signal is very weak and changes continuously with the lapse of time. The signals from the brain are coupled spatiotemporally from the central nerve, the spinal nerve, the human nerve to the muscle. In addition, the collection of physiological signals has great relation with the application position of the sensor, the degree of dense distribution and the quality of decoding effect. The fatigue state of the human body also brings the change of the electromyographic signals, thereby causing the mismatch of the classification decoder and the signals. And the decoding control algorithm is complex, the operation time is long, and the stability is poor. The closed-loop control of the manipulator is studied on the basis of sensors for force, position and slip. A commonly used control strategy is impedance control, which indirectly controls the magnitude of the gripping force by controlling the reference position of the manipulator through position feedback of the manipulator, and this control method enables the manipulator to smoothly transit between the free motion space and the constrained motion space. Closed loop force control has better stability than open loop control, and has certain regulating force capability to force overshoot. To cope with the problem of environmental disturbance, reflection control is proposed. The main problem solved is how to detect external disturbance, how much force needs to be increased, and the reflection control is faster than the reflection control response of the myoelectricity open loop, and the real-time performance is better.

For a manipulator, the design requires that the weight and the size of the manipulator are limited, a plurality of sensors cannot be configured, and a plurality of sensors can be arranged at limited positions. Torque sensors are typically large in size and expensive, and are difficult to incorporate into compact manipulators, such as manipulators. Generally, a mechanical arm has a plurality of force sensors, however, when the mechanical arm contacts with an object, the contact point is uncertain, that is, the sensor may not detect whether the mechanical arm grabs the object or not, or may not detect the information completely. Lack of sufficient sensor feedback tends to cause the gripping force to exceed the desired gripping force and not be adjusted in time, possibly damaging the gripped object, and more likely causing damage to the manipulator. Therefore, there is a need to obtain disturbance information in the environment by an effective method, predict and control a proper gripping force during the gripping process, and achieve the purpose of stably gripping an object.

Disclosure of Invention

The invention aims to provide a disturbance controller for estimating disturbance information in an environment, and simultaneously provides a primary and secondary coordination control strategy for predicting and controlling gripping force according to the estimated disturbance information, so as to replace the functions of a force sensor and torque sensing or add protection to the force overload of a manipulator. The method comprises the steps of firstly establishing a dynamic model of the manipulator, capturing force information due to the manipulator force sensor, establishing a nonlinear disturbance observer based on the force information, and analyzing errors of the disturbance observer. And finally, establishing a primary and secondary coordination control strategy by combining a primary mode control method, and controlling the manipulator to stably grab the object under the action of disturbance.

The technical scheme adopted by the invention for solving the technical problems is as follows:

a disturbance observer and primary and secondary coordination control method of a manipulator comprises the following steps:

step 1: establishing a dynamic model of the manipulator according to the stress condition when the manipulator grips an object;

step 2: establishing a nonlinear disturbance observer by taking the gripping force of a tracking manipulator, the speed term and the disturbance quantity of the gripping force as state variables, and analyzing the error of the disturbance observer by adopting a Lyapunov function;

and step 3: determining expected gripping force of the manipulator under the disturbance condition according to the disturbance amounts estimated in the step 1 and the step 2, and simultaneously, controlling gripping by combining a main mode, and obtaining total expected gripping force of each finger according to the expected gripping force of each finger determined by a multi-finger coordination control strategy;

and 4, step 4: and determining primary and secondary control quantities based on fuzzy control according to a primary and secondary coordination control principle, and determining the primary and secondary coordination control quantities by combining the control quantities determined by the disturbance observer to control the gripping of the manipulator.

Drawings

FIG. 1 is a schematic diagram of the process of the present invention.

Fig. 2 is a schematic diagram of a disturbance observer of a robot according to the present invention.

Fig. 3 is a schematic diagram of primary and secondary cooperative control of the robot of the present invention.

Detailed Description

The invention is further described with reference to the following figures and detailed description.

As shown in fig. 1, the specific steps of establishing the disturbance observer and the primary and secondary coordination control method of the manipulator are as follows:

step 1: establishment of manipulator dynamic model

The dynamic model of the manipulator is

Where q is the angular displacement of the joint, H (q) is an inertia matrix,

the torque is joint torque caused by gravity, friction and the like, tau is driving torque, r is a coupling coefficient of the driving torque to a certain joint of the manipulator (for the manipulator with single degree of freedom, r is 1), and J is a Jacobian matrix; f represents the reaction force of the gripped object to the manipulator, and can be represented by the following model:

wherein: x represents the position of the contact point of the manipulator and the grasped object, K represents the rigidity matrix of the grasped object, and L represents the distance matrix from the contact point to the joint point.

The formula (2) is substituted for the formula (1) to obtain

In the formula

Step 2: establishment of disturbance observer

Refer to FIG. 2

In the formula, p (f)₁,f₂) Is a non-linear function expressed as

Where w represents the amount of disturbance.

Based on (4), a nonlinear disturbance observer is established

In the formula z₁,z₂,z₃Is a state variable, f is a manipulator grasping force, beta₁,β₂,β₃B is a coefficient, sat (-) represents a saturation function.

Error analysis using the Lyapunov function of the form

In the formula e₂＝z₂-f₂And A, B and C represent normal numbers.

Is derived to obtain

Wherein X is (1.5A beta)₁-Bβ₂)，Y＝(1.5A-2Cβ₂+Bβ₂|e₁|^0.5)|e₁|^-0.25And Z is B, and p is a nonlinear function given by the formula (5).

The condition for the quadratic part of the quadratic function to be negative is

X＞0,Y＞0,Z＞0,Y²-4XZ＜0 (9)

Based on this condition, the following parameter design principle can be derived

When the quadratic function (8) does not satisfy the negative definite condition, that is, the region taking the positive value is the variable

And e₂Paraboloid of

And variable e₁And e₂Plane g (f) of₁,f₂,w)

The upper part of the intersection, and the variable e at this intersection₁Is the order of

Root of (E)₁＝(Bg/X)²And (4) the same level. The expression of X indicates that X and B beta₂Same level, therefore root e₁And (Bg/beta)₂)²And (4) the same level. Therefore, only take β₂＞＞g(f₁,f₂W) of

The right side in the equation is the error of the linear observer. This result demonstrates that the state tracking efficiency of the non-linear disturbance observer is much higher than that of the linear state observer. From the above disturbance observer and error analysis, wherein z₁Is to track the gripping force of the manipulator, z₂Is the velocity term of the grip force, and z₃Is the amount of disturbance. The disturbance observer is applied to disturbance observation of the gripping force of the manipulator. Once the disturbance amount is estimated, the estimated mass, stiffness and friction coefficient of the surface of the grasped object are combined, so that the reflecting grasping force in the reflecting control can be reasonably estimated.

And step 3: establishment of primary and secondary coordination control strategies

When the object to be grabbed is detected to slide, the grabbing force of the mechanical arm needs to be adjusted, and the adjustment of the grabbing force is carried out in the following self-adaptive mode

(a) The grip force of the main mode is small, and mg-2f exists in the case_dMu is more than 0, the object to be grabbed slides under the action of gravity, and the expected grabbing force of reflection is taken as

In the formula z₃For the output of the disturbance observer, z₀Z before obtaining slip information₃Amount of (eta) (. eta.)₁And η₂For the weighting coefficient, δ is the following expression

(b) The main mode gripping force can stably gripWhen the object is held but the outside disturbance causes the gripped object to slide, the mg-2f exists at the moment_dMu is less than 0, the grabbed object generates slippage under the action of disturbance, and the disturbance self-adaptive adjustment mode is designed as

Under the control of the main mode, according to the multi-finger coordination control strategy, the expected value of the gripping force of each finger of the manipulator is

R represents the number of fingers participating in the grip. In the same way, according to the expected gripping force of the manipulator

Obtaining the expected gripping force of each finger as

According to the principle of primary and secondary coordinated control, the final primary and secondary control increments are as follows

Where g (-) denotes a fuzzy controller, i.e. the input is

Mapping of delta, f to control output u_m。

Based on the disturbance observer mentioned above, obtain

In the formula

Finally, the primary and secondary coordination control quantity is obtained

u＝u_m+u_r (20)。

Claims

1. a disturbance observer of a manipulator and a primary and secondary coordinated control method are applicable to the operation of a manipulator grasping an object in an uncertain environment, and it is characterized in that: at first, the dynamic model of the manipulator is established, and the force sensor provided according to the manipulator can Obtain the grasping force, establish a nonlinear disturbance observer, and analyze the error of the disturbance observer; then use the estimated disturbance amount, combined with the estimated mass, stiffness and surface friction coefficient of the grasped object, to estimate the reflection Reflected grasping force in control; finally, combined with the main mode control method, a primary and secondary coordinated control strategy is established to control the manipulator to grasp objects stably under disturbance; the specific steps to establish a nonlinear disturbance observer and primary and secondary coordinated control strategy are as follows :

The first step is to establish a dynamic model of the manipulator:

where q is the angular displacement of the joint, H(q) is the inertia matrix,

is the joint torque caused by gravity and friction, τ is the driving torque, r is the coupling coefficient of the driving torque to one of the joints of the manipulator, for a single-degree-of-freedom manipulator, r=1, J is the Jacobian matrix, and f represents the grasped The reaction force of the object to the manipulator can be represented by the following model:

Among them: x represents the position of the contact point between the manipulator and the grasped object, K represents the stiffness matrix of the grasped object, L represents the distance matrix from the contact point to the joint point,

Substitute equation (2) into equation (1) to get

in the formula

Step 2: Create a nonlinear disturbance observer:

Assume

where p(f ₁ , f ₂ ) is a nonlinear function, and its expression is:

In the formula, w represents the disturbance amount. According to formula (4), a nonlinear disturbance observer is established:

where z ₁ , z ₂ , z ₃ are state variables, β ₁ , β ₂ , β ₃ , b are coefficients, and sat( ) represents the saturation function,

Error analysis is performed using a Lyapunov function of the form:

where e ₂ =z ₂ -f ₂ , A, B and C represent positive numbers,

Derive:

where X=(1.5Aβ ₁ -Bβ ₂ ), Y=(1.5A-2Cβ ₂ +Bβ ₂ |e ₁ | ^0.5 )|e ₁ | ^-0.25 , Z=B, p is given by formula (5) nonlinear function;

The third step is to establish the primary and secondary coordinated control strategy:

When the grasped object is detected to slip, the grip force of the manipulator needs to be adjusted. The adjustment of the grip force is carried out in the following adaptive way:

If the grasping force in the main mode is small, then mg-2f _d μ>0, f _d is the desired grasping force, and the grasped object slips under the action of gravity, and the desired grasping force of the reflex control is:

where z ₃ is the output of the disturbance observer, z ₀ is the amount of z ₃ before the slip information is obtained, η ₁ and η ₂ are the weighting coefficients, and δ is the following expression:

Secondly, if the grasping force of the main mode can stably grasp the grasped object, but the external disturbance causes the grasped object to slip, at this time, mg-2f _d μ<0, and the grasped object slips under the disturbance. , the design disturbance adaptive adjustment method is:

Under the master mode control, according to the multi-finger coordinated control strategy, the expected value of the grasping force of each finger of the manipulator is