JPH0852675A - Device and method for selectively disturbance compensating hybrid controlling of manipulator - Google Patents

Abstract

PURPOSE:To provide a selectively disturbance compensating hybrid control device for a manipulator difficult to receive an influence of interfering force of a point end with an outside environment by eliminating necessity for an accurate dynamic characteristic model of the manipulator. CONSTITUTION:An estimating part 3 of angular speed and angular acceleration with an articulation angle of an articulated manipulator 2 serving as an input and an error torque calculating part 4 estimating modeled error torque as disturbance torque by utilizing articulation input torque to arm actuator, estimated angular acceleration of the manipulator and a force signal from a force sensor are provided. Further, a filter 5 of inputting an output of the error torque calculating part 4 to remove a noise and a position force hybrid control part 6 inputting the articulation angle of the manipulator, its estimated angular speed and the force signal to calculate command articulation torque for making a position related to a movable direction and force related to a lock direction, in a manipulator point end, follow a target track are provided.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a motion control device for a robot manipulator, and more particularly to a selective disturbance compensation hybrid control method for a manipulator for controlling the position and force of the tip of the manipulator.

[0002]

2. Description of the Related Art In a conventional hybrid controller, in order to achieve stable and highly accurate work that requires dynamic contact with the external environment, such as deburring, the dynamic characteristics of the manipulator are calculated and linearized by the calculation torque method. It is necessary to design based on compensation.

[0003]

However, it has been very difficult to accurately estimate the complicated dynamic characteristics of the multi-joint manipulator and to calculate it in real time to perform linearization compensation. Further, the interference force such as frictional force at the contact point between the tip of the manipulator and the external environment has been a factor that deteriorates the control performance.

The present invention has been made in view of the above circumstances and does not require a precise dynamic characteristic model of a manipulator.
It is an object of the present invention to provide a selective disturbance compensation type hybrid controller for a manipulator that is not easily affected by the interference force between the tip and the external environment.

[0005]

In order to achieve the above object, a selective disturbance compensation type hybrid controller for a manipulator according to the present invention is a multi-joint equipped with a force sensor at the tip, an angle detecting device for each joint axis, and an actuator. A hybrid control device relating to the position and force of the tip of a manipulator, wherein an estimation unit of an angular velocity and an angular acceleration that outputs an angular velocity and an angular acceleration with a joint angle of the multi-joint manipulator as an input, a joint input torque to the actuator, An error torque calculation unit that estimates the modeling error torque as a disturbance torque using the estimated angular acceleration of the manipulator and the force signal from the force sensor obtained by the angular velocity and angular acceleration estimation unit, and the error torque calculation unit A filter that removes noise with the output as input, and the joint angle and front of the articulated manipulator The position and force that calculates the command joint torque to make the manipulator tip position related to the moving direction and the force related to the constraining direction follow the target trajectory by inputting the estimated angular velocity and force signal of the manipulator obtained by the angular velocity and angular acceleration estimation unit. A hybrid control unit, wherein the error torque calculation unit receives the force signal of the manipulator as an input, and detects the component in the direction in which the tip of the manipulator is constrained from the external environment; An inertial matrix model multiplication that multiplies the output from the equivalent joint torque calculation unit that calculates the joint torque equivalent to this input from the constraint direction component detection unit and the output from the estimation unit of the angular velocity and angular acceleration by the inertial matrix model And a joint input torque to the actuator from the output of the inertia matrix model multiplication unit and By subtracting the output of the equivalent joint torque calculation unit is characterized in that the output as a disturbance torque.

Further, a selective disturbance compensation type hybrid control method for a manipulator according to the present invention is a hybrid control method relating to the position and force of the tip of a multi-joint manipulator equipped with a force sensor at the tip, an angle detection device for each joint axis, and an actuator. In the above, first means for estimating the angular velocity and angular acceleration from the joint angle detected by the angle detecting device for each joint axis, and the joint angular acceleration estimated by the first means are multiplied by the inertia matrix model of the manipulator. Second means for calculating the inertia torque, and third means for calculating the equivalent joint torque generated by selecting the component in the contact surface vertical direction of the contact force with the external environment detected by the force sensor. And a fourth hand for estimating the error torque by subtracting the joint input torque and the equivalent joint torque from the inertia torque calculated by the second means. Fifth means for limiting the frequency band of the error torque compensation effect estimated by the fourth means and filtering for removing noise, and angular velocity and force sensor estimated for the angle of each joint. Sixth means for calculating a command joint torque for performing position control in the movable direction of the tip and force control in the constraining direction from the information by using the geometric model of the target environment and the inertia matrix model of the manipulator, and the sixth means. In the process of detecting the joint angle and force sensor information and outputting the joint input torque, the seventh means for calculating the joint input torque by subtracting the filtered error torque from the command joint torque calculated by It is characterized by having.

[0007]

According to the present invention, the dynamic characteristic model of the manipulator and the dynamic characteristic of the manipulator are selectively estimated and compensated as the disturbance torque by the interference force from the external environment with respect to the dynamic characteristic model of the manipulator and the moving direction. This makes it possible to easily configure a hybrid control system of the tip position and force based on the model.

[0008]

Embodiments of the present invention will now be described in detail with reference to the drawings. FIG. 1 is a block diagram showing an embodiment of a selective disturbance compensation type hybrid controller for a manipulator according to the present invention. That is, there is provided a selective disturbance compensation hybrid control device 1 for the position and force of the tip of an articulated manipulator 2 including a force sensor at the tip, an angle detection device for each joint axis, and an actuator, and the joint of the articulated manipulator 2 is An angular velocity and angular acceleration estimation unit 3 that outputs an angular velocity and an angular acceleration with an angle as an input, a joint input torque to the actuator 2, an estimated angular acceleration of the manipulator 2 obtained by the angular velocity and angular acceleration estimation unit 3, and An error torque calculation unit 4 that estimates the modeling error torque as a disturbance torque using the force signal from the force sensor;
A filter 5 that receives the output of the error torque calculation unit 4 to remove noise, a joint angle of the multi-joint manipulator 2, an estimated angular velocity of the manipulator 2 and a force signal obtained by the angular velocity and angular acceleration estimation unit 3 are output. A position / force hybrid control unit 6 for calculating a command joint torque for causing the position of the tip of the manipulator 2 in the movable direction and the force in the restraining direction to follow the target trajectory as inputs is provided. Reference numeral 7 is a joint input torque and disturbance addition unit, and 8 is a subtraction unit that subtracts the filtered error torque from the command joint torque to calculate the joint input torque.

FIG. 2 is a block diagram showing an example of the error torque calculation unit 4 of FIG. That is, the error torque calculation unit 4
Is a constraint direction component detection unit 23 that receives the force signal of the manipulator 2 as an input and detects a component in the direction in which the tip of the manipulator 2 is constrained from the external environment, and inputs the output from this constraint direction component detection unit 23. As an equivalent joint torque calculation unit 24 that calculates a joint torque equivalent to this input
And an inertia matrix model multiplication unit 22 that multiplies an output from the angular velocity and angular acceleration estimation unit 3 by an inertia matrix model. The output of the inertia matrix model multiplication unit 22 and the joint input torque to the actuator are The outputs of the equivalent joint torque calculation unit 24 are subtracted by the subtraction units 25 and 26 and output as disturbance torque.

FIG. 3 is a flowchart included in the process of detecting the joint angle and force sensor information and outputting the joint input torque in the selective disturbance compensation hybrid control method for the manipulator according to the present invention. That is, in a,
A joint angle of a multi-joint manipulator including a force sensor at the tip, an angle detection device for each joint axis, and an actuator, a contact force with the outside world, and a joint input tokule are detected. Next, in (b), the angular velocity and angular acceleration of each joint are estimated from the joint angle detected by the angle detecting device for each joint axis.
Next, in C, the joint angular acceleration is multiplied by the inertia matrix model of the manipulator to calculate the inertia torque. Next, in D, the component perpendicular to the contact surface (constraint direction component) of the contact force with the external environment is detected, and the equivalent joint torque generated thereby is calculated. Next, in E, the error torque is calculated by subtracting the joint input torque and the equivalent joint torque from the inertia torque. Next, in (f), the frequency band of the effect of compensating for the error torque is limited and filtering is performed so as to remove noise. Next, in g, using the geometric model of the target environment and the inertia matrix model of the manipulator from the angular velocities estimated from the joints and the force sensor information, the position control of the tip in the moving direction and the force control in the restraining direction are performed. The command joint torque for performing the hybrid control of is calculated. Next, in H, the joint input torque is calculated by subtracting the filtered error torque from the command joint torque. Next, an n-degree-of-freedom articulated manipulator given by

[0011]

[Equation 1] Consider the dynamic hybrid control of. Where q is the joint angle vector of the multi-joint manipulator, τ is the joint driving force vector, M is the inertia matrix, h is Coriolis force, centripetal force,
Represents frictional force and gravity term. F represents the force exerted by the tip of the manipulator on the target environment. Further, let r be the position vector of the manipulator tip in the work coordinates and J be the Jacobian matrix relating to the manipulator tip, and assume that there is no redundancy.
On the other hand, the mechanical impedance matrix of the target environment is Z _e (s)
Then, the following relational expression holds in the s region.

[0012]

[Equation 2] Further, it is assumed that the constraint equation (constrained curved surface) received from the target environment surface with which the manipulator tip is in contact is known and given as follows.

[0013]

(Equation 3)

However, r _F (r) is an n _F dimensional vector. Here, the (n−n _F ) dimensional vector r _P (r) is selected such that each element of {r _F (r), r _P (r)} is independent, and generalized coordinates r _c = [r _P ^T , r _F ^T ] ^T is defined. Also, the generalization force corresponding to this

[0015]

[Equation 4] Is defined. At this time, the following control algorithm for the hybrid control that simultaneously realizes the position control in the movable direction of the manipulator and the force control in the restraining direction is the measurement and control vol. 30, no. 5,1991
Yoshikawa's reference "Manipulator Force Control".

[0016]

(Equation 5)

Where r _Pr is the target trajectory of the position in the movable direction, f _Fr is the target trajectory of the force in the restraining direction, K
_Pp- , K _Pd and K _F1 are feedback gains. On the other hand, when the manipulator is in contact with the target environment surface, the force sensor attached to the tip of the manipulator detects not only the pressing force in the restraining direction but also the frictional force accompanying the movement in the movable direction. Alternatively, when an appropriate work piece is equipped at the tip of the force sensor, a reaction force corresponding to the dynamic characteristics in the movable direction is also added. Furthermore, when the error of the dynamic characteristic model of the manipulator itself cannot be ignored (5),
There may be a case where sufficient stability and control performance cannot be obtained only by the servo compensation of the equation (6).

Therefore, in order to solve the problems of the prior art, a disturbance compensation type hybrid control system which is not easily affected by modeling error and interference from the external environment is constructed. Specifically, we consider an appropriate dynamic characteristic model of the manipulator, and estimate and compensate the error dynamics with the actual dynamic characteristic. In particular, consider separately the force acting from the outside on the manipulator tip in the movable direction and restraining direction, torque J ^T E generated by the interference force of the friction force and the like applied to the movable direction
Consider a selective disturbance compensation control that estimates and compensates for ^T [f _P ^T , 0] ^T as the disturbance torque in combination with the error dynamics of the manipulator body. Here, the following dynamic characteristic model is defined.

[0019]

(Equation 6) Represents the ideal response of the joint angle vector and the force vector, and S represents the following selection matrix.

[0020]

(Equation 7)

[0021] It this case, the force manipulator receives from the target environment a direction perpendicular to the surface (constraint direction) -E ^T SE ^-T F, is this equivalent joint torque represented by -J ^T E ^T SE ^-T F become. Therefore, the manipulator error dynamics for the dynamic characteristic model of equation (7) is given by the following equation.

[0022]

[Equation 8]

This disturbance torque is estimated using the second expression in the equation (9). For example, the joint angular velocity and the angular acceleration are estimated as follows using the sampling value of the joint angle vector.

[0024]

[Equation 9] Indicates the estimated joint angular velocity and angular acceleration, respectively, and the subscript k of each variable indicates that it is a sample value at time t = kT _s . Of course, these may be estimated by constructing an appropriate state observer. By multiplying this by the inertia matrix model and subtracting the input torque one sample before and the joint torque equivalent to the reaction force acting in the restraining direction from the target environment surface, the disturbance torque can be calculated as follows.

[0025]

[Equation 10] If τ _d ^* is the estimated disturbance torque and φ (s) is the transfer function of the disturbance estimation determined by the disturbance observer,
The following equation holds.

[0026]

[Equation 11] τ _d ^* via an appropriate filter k (s)

[0027]

[Equation 12] If the joint torque input is fed back, the following result is obtained.

[0028]

[Equation 13]

Further, for the sake of convenience, h is represented as follows, which is divided into those that dynamically change due to the movement of the manipulator and those that do not directly correlate with the movement of the manipulator.

[0030]

[Equation 14]

Therefore, considering the equation (15), it is understood that the dynamic characteristic model of the equation (7) is realized in the frequency region where at least kφ = 1. At this time, the command joint drive vector τ for hybrid control of position and force
_c can be given as follows.

[0032]

(Equation 15)

Where r _Pr is the target trajectory of the position in the movable direction, f _Fr is the target trajectory of the force in the restraining direction, K _Pp ,
K _Pd and K _Fi are feedback gains. A hybrid control simulation is performed using the two-degree-of-freedom manipulator having the physical constants shown in Table 1 to verify the control performance of the hybrid control system by the selective disturbance compensation control. As shown in Fig. 4, the manipulator tip has a nominal value of 5 kg.
It is assumed that the target environment surface represented by the spring, the mass, and the damper model is in contact via the load. Table 2 shows each physical constant of the target environment.

[0034]

[Table 1]

[0035]

[Table 2]

When the impedance of the target environment in the x direction is sufficiently large with respect to the target pressing force, the equation of the constraining surface is E = diag (1,1) because x is almost constant. The target trajectory for position control in the y direction is

[0037]

[Equation 16] Where f = 1 Hz, the following equation is used for the target trajectory of the force control in the x direction.

[0038]

[Equation 17]

The angular velocity and the angular acceleration for the selective disturbance compensation control are estimated using the equations (10) and (11). The error torque (disturbance torque) is calculated as follows by including the influence of the load in the nominal state in the inertia matrix model.

[0040]

(Equation 18)

T _s is a sampling time and T _s = 5 mse
Let be c. At this time, the filter k is, for example, a collection of the Institute of Instrument and Control Engineers, vol. 30, no. 7, 1994, Yoshida et al. “Disturbance compensation control type manipulator system μ
Designed by the method described in "Synthesis".

[0042]

[Formula 19] Is used with T _s = 5 msec for bilinear transformation. The simulation compares the control method based on the calculation torque method of equations (4), (5), and (6) with the case of using the selective disturbance compensation control. The feedback gain of the position control system is k _Pp
= 100 sec ^-2 , k _Pd = 20 sec ^-1 , and the feedback gain of the force control system is selected as k _Fi = 100 sec ^-1 .

FIG. 5 shows a simulation result by each control system when the load has a nominal value. The target trajectory is represented by a thin line, but the response of the position and force when the calculated torque method is used almost agrees with the target trajectory. When the disturbance compensation control system is used, the position response shows almost the same characteristics as the trajectory tracking control, but the force response converges to the target value in a stable manner, but is slightly oscillating.

6 and 7 show the simulation results when the load weight is decreased and when it is increased, respectively.
In the control by the calculated torque method, the response of the position and force causes a large error with respect to the target trajectory according to the increase and decrease of the load, and it is shown that servo compensation is not enough. In the case of disturbance compensation control, although a slight tracking error is generated when the load is increased in position control, a nearly good response is obtained, but in force control, the control performance deteriorates or becomes unstable. .

[0045]

As described above, according to the present invention, by estimating and compensating the error dynamics with the dynamic characteristic model of the manipulator and the interference force from the external environment acting in the moving direction of the tip as the disturbance torque, Even if an accurate dynamic characteristic model regarding the interaction between the manipulator and its external environment cannot be obtained, it is possible to realize highly accurate hybrid control of position and force.

[Brief description of drawings]

FIG. 1 is a block diagram showing an embodiment of a selective disturbance compensation type hybrid controller for a manipulator according to the present invention.

FIG. 2 is a block diagram showing an example of an error torque calculation unit in FIG.

FIG. 3 is an example of a flowchart included in a process of detecting a joint angle and force sensor information and outputting a joint input torque in the hybrid control method according to the present invention.

FIG. 4 is an explanatory diagram showing an example of a model of a two-degree-of-freedom manipulator and an external environment used for the hybrid control simulation according to the present invention.

FIG. 5 is a characteristic diagram comparing the control based on the conventional calculation torque method when the load is in the nominal state and the followability to the target trajectory when the present invention is applied.

FIG. 6 is a characteristic diagram comparing the control based on the conventional calculation torque method when the load is light and the followability to the target trajectory when the present invention is applied.

FIG. 7 is a characteristic diagram comparing the control based on the conventional calculation torque method when the load is heavy with the followability to the target trajectory when the present invention is applied.

[Explanation of symbols]

DESCRIPTION OF SYMBOLS 1 ... Selective disturbance compensation type hybrid controller, 2 ... Manipulator, 3 ... Angular velocity and angular acceleration estimation unit, 4 ... Error torque calculation unit, 5 ... Filter, 6 ... Hybrid control unit, 7 ... Addition unit, 8 ... Subtraction 22 ... Inertia matrix model multiplication unit, 23 ... Restraining direction component detection unit, 24 ... Equivalent joint torque calculation unit, 25, 26 ... Subtraction unit.

Claims (2)

[Claims] 1. A hybrid controller for the position and force of the tip of an articulated manipulator comprising a force sensor at the tip, an angle detection device for each joint axis, and an actuator, the angular velocity using the joint angle of the articulated manipulator as an input. And an angular velocity and angular acceleration estimation unit that outputs angular acceleration, joint input torque to the actuator, estimated angular acceleration of the manipulator obtained by the angular velocity and angular acceleration estimation unit, and a force signal from the force sensor are used. An error torque calculation unit that estimates the modeling error torque as a disturbance torque, a filter that removes noise by using the output of the error torque calculation unit as an input, and the joint angle of the multi-joint manipulator and the estimation of the angular velocity and the angular acceleration. Of the manipulator tip with the estimated angular velocity and force signal of the manipulator found in A position and force hybrid control unit that calculates a command joint torque for causing a force relating to a movable direction and a force relating to a restraining direction to follow a target trajectory is provided, and the error torque calculation unit receives the force signal of the manipulator as an input. A constraint direction component detection unit that detects the component in the direction in which the tip of the manipulator is constrained from the external environment, and an equivalent joint torque that calculates the joint torque equivalent to this input by using the output from this constraint direction component detection unit as an input A calculation unit and an inertia matrix model multiplication unit that multiplies an output from the estimation unit of the angular velocity and the angular acceleration by an inertia matrix model, and a joint input torque to the actuator from the output of the inertia matrix model multiplication unit and the Selective disturbance compensation for a manipulator characterized by reducing the output of the equivalent joint torque calculation unit and outputting it as disturbance torque Compensatory hybrid controller. 2. A hybrid control method relating to the position and force of the tip of a multi-joint manipulator comprising a force sensor at the tip, an angle detector for each joint axis, and an actuator, and a joint detected by the angle detector for each joint axis. A first means for estimating an angular velocity and an angular acceleration from an angle; a second means for multiplying the joint angular acceleration estimated by the first means by an inertia matrix model of a manipulator to calculate an inertia torque; A third means for selecting a component in the direction perpendicular to the contact surface of the contact force with the external environment detected by the sensor to calculate the equivalent joint torque generated thereby, and a joint input from the inertia torque calculated by the second means. Fourth means for estimating the error torque by subtracting the torque and the equivalent joint torque, and a frequency band for compensating the error torque estimated by the fourth means are limited. And a fifth means for filtering so as to remove noise, and a tip model using a geometric model of the target environment and an inertia matrix model of the manipulator from the angular velocity and the force sensor information estimated from the angle of each joint. Sixth means for calculating a command joint torque for performing position control for the movable direction and force control for the restraining direction, and a joint by subtracting the filtered error torque from the command joint torque calculated by the sixth means 7. A selective disturbance compensation hybrid control method for a manipulator, comprising a seventh means for calculating an input torque in a process of detecting a joint angle and force sensor information and outputting a joint input torque.