JP2507891B2 - Manipulator impedance control system - Google Patents

Description

TECHNICAL FIELD The present invention relates to a control system for manipulators.

[Prior Art] The importance of considering force information as well as position in motion of a manipulator accompanied by contact with an object or motion under restraint conditions has long been discussed.

One typical method of force control is position / force hybrid control, but in the actual assembly work the problem of selecting the position and direction to apply the position and force, and the method of boundary detection and mode switching Unresolved issues remain.

Another typical method is impedance control. Impedance control is a control whose main concept is dynamic interaction between the robot and the environment, and stable force control can be realized especially when performing contact work. In this control, when the robot performs a contact work, the relationship between the robot and the environment is defined by impedance, and a change in dynamic interaction between the robot and the environment is regarded as a change in impedance. Specifically, this appears as a change in the apparent dynamics (inertia, viscosity, elasticity) of the robot.

The main feature of this impedance control is to control the change of dynamic impedance and change not only the rigidity and viscosity of the end effector but also the apparent inertia.

[Problems to be Solved by the Invention] However, since all the conventional impedance control methods use a force sensor or a torque sensor, the number of parts of the manipulator increases and the structure becomes complicated, and the manufacturing cost increases. Due to the force-measuring mechanism, the manipulator may have reduced rigidity or weakened structure. Further, when the force sensor is attached, there is a problem that the external force cannot be measured at a place other than the tip of the manipulator.

The present invention has been made in view of the above circumstances, without using a force sensor, therefore the structure of the manipulator is simplified, there is no reduction in the mechanical strength of the manipulator due to the introduction of force control, and It is an object of the present invention to provide an impedance control method for a manipulator, which can control the impedance according to which part of the manipulator receives an external force.

[Means for Solving the Problems] In order to achieve this object, an impedance control system for a manipulator according to the present invention has a distance or rotation angle sensor, a speed or angular velocity sensor for a manipulator that includes a plurality of links connected through joints. And an acceleration or angular acceleration sensor, and an impedance control method for estimating an internal state without using a force sensor and a torque sensor, wherein an output torque Ta of _a motor for driving the joint is expressed by I: inertia Matrix J ^T : Jacobian transposed matrix M: Virtual inertia matrix J: Jacobian θ: Rotation angle vector of each axis t: Time D _v : Viscous friction matrix B: Virtual viscous friction matrix K: Virtual stiffness matrix X _o : Virtual equilibrium point L: the transformation matrix from the joint coordinates to Cartesian coordinates C (θ, dθ / dt) : when the nonlinear terms such as gravity term Coulomb _{friction, T a = (I-J} T MJ) (d ² θ / dt ² ) + {D _v −J ^T M (dJ / dt) −J ^T BJ} (dθ / dt) + J ^T K {X _o −L (θ)} + C (θ, dθ / dt) It is characterized by setting.

[Operation] rotation angle from the internal sensor of the manipulator, and substituted into the above equation to measure the angular velocity and angular acceleration, obtaining the output torque T _a of the actuator. The output torque T _a of this actuator realizes the target impedance of the actuator and enables the impedance control of the work of the manipulator arm.

[Embodiment] FIG. 1 and FIG. 2 show a manipulator arm 10.

That is, the manipulator arm 10 has the first link L ₁ ,
It has a second link L ₂ and a third link L ₃ . All links are made of duralumin.

The first link L ₁ is connected to the manipulator body 4 by a joint 1 and is rotatable about a vertical axis of rotation 11.

The first link L ₁ and the second link L ₂ are connected by a joint 2 and can rotate relative to a horizontal rotation axis 12.

The second link L ₂ and the third link L ₃ are connected by a joint 3 and can rotate relative to a horizontal rotation axis 13.

Joint 1, joint 2 and joint 3 are each actuator 1
It is driven by 4,15,16, but as these actuators, a direct drive type DC torque motor that adopts a DD (direct drive) motor for estimating an accurate internal model and for precise force control is used. use. As such a DC motor, one manufactured by Inland can be used. The specifications are shown in FIG. Fig. 4 shows the specifications of the links L ₁ , L ₂ and L ₃ .

The control system has a computer 17, a D / A converter 18, a U / D counter 21, and a servo amplifier 22 as shown in FIG.
And actuators at each joint of the manipulator arm 10
It has 14, 15 and 16 and a rotary encoder 23.

Using the above hardware, the control method of the present invention is performed as follows.

The configuration of the impedance control system of the present invention is described below.
Shown in the figure.

The signal from the rotation sensor of each axis is taken into the computer 17, and after calculating the rotation angle, angular velocity and angular acceleration, it is output as a motor torque by applying an appropriate gain. The servo amplifier controls the current.

 The equation of motion of the manipulator is expressed as follows.

I (d ² θ / dt ² ) ＋ D _v (dθ / dt) ＋ C (θ, dθ / dt) ＝ T _a ＋ J ^T F _e … (1) where I: inertia matrix D _v : viscous friction matrix C (θ, dθ / dt): Non-linear term such as gravity term Coefficients above are known. In addition, T _{a is} the output matrix of the actuator F _{e is} the matrix of the external force received from the environment θ is the rotation angle matrix of each axis J is the Jagobian J ^{T is} the transposed matrix of the Jagobian, and then the virtual impedance Z when in contact with the environment
When (S) is set as Z (S) = S / (MS ² + BS + K), the external force F _e is F _e = M (d ² X / dt ² ) + B (dX / dt) + K (X−X ₀ ) ... (2) where M: virtual inertia matrix B: virtual viscous friction matrix K: virtual stiffness matrix X: coordinate matrix in Cartesian space X ₀ : virtual equilibrium point represents a trajectory of a function of time t.

 Here, θ and X have the following relationship by coordinate conversion.

dX / dt = J (dθ / dt) (3) d ² X / dt ² = J (d ² θ / dt ² ) + (dJ / dt) (dθ / dt) (4) X = L (θ ) (5) Substituting these into equation (2), J ^T Fe = J ^T MJ (d ² θ / dt ² ) + J ^T M (dJ / dt) (d θ / dt) + J ^T BJ (d θ / dt ) + J ^T K {L (θ) −X ₀ } (6) Therefore, the actual output torque of the actuator is T _a = (I−J ^T MJ) (d ² θ / dt ² ) + {D _v − ^{J T M (dJ / dt)} -J T BJ} (dθ / dt) + J T K {X 0 -L (θ)} + C (θ, dθ / dt) ... (7) as can be seen from equation (7) In addition, by identifying each coefficient of the manipulator and measuring the rotation angle, angular velocity, and angular acceleration of the motor from the internal sensor of the manipulator, the torque of the actuator can be calculated without using a force sensor,
Control with the target impedance of equation (2) is possible.

[Experimental example] The above impedance control system is a two-axis vertical articulated type.
A DD manipulator was used to perform a control experiment of contact work in a plane.

Manipulator Configuration This manipulator is of the vertical articulated type and has three degrees of freedom. This time, the first axis is fixed and used as a two-degree-of-freedom manipulator that operates in a vertical plane. The actuator for each axis uses a DD motor for precise force control.

Derivation of equation of motion Lagrange's equation of motion was used to derive the equation of motion. In order to identify the parameters of the equation of motion, each axis was damped and oscillated with the fulcrum of the pendulum, and the inertia I, viscous friction coefficient D _v , and Coulomb friction coefficient were obtained from the period and amplitude. FIG. 5 shows the fixed parameters.

Control circuit and program A rotary encoder of 2000p / r was multiplied by 4 and used as a rotation sensor for each axis. When controlling equation (7), accurate values of angular velocity and angular acceleration are required, but if they are calculated from the difference in the number of pulses, the number of effective digits will be small in the low-speed rotation range, and the accuracy will be extremely high. Deteriorate. Therefore, in this device, the pulse interval was measured at a reference clock of 1 MHz, and the reciprocal thereof was used as the angular velocity.

In addition, C is used as the programming language and the control cycle is
It is 5.0 ms.

Control Experiment Next, an outline of the control experiment performed using this system will be described. Virtual impedance is set in each of the horizontal axis and the vertical axis, and the device is operated along the plane while being in contact with the plane. When the virtual equilibrium point is moved at a constant velocity slightly below the surface while keeping a constant distance from the surface, the manipulator follows the virtual equilibrium point while contacting the plane and applying a constant force perpendicular to the plane. At this time, if a semi-circular obstacle is placed on the way, a force proportional to the position deviation from the virtual equilibrium point is applied to the obstacle and the obstacle moves along the obstacle. The value of the force applied in each axial direction at this time is shown in FIG.

Looking at this figure, the force values in the vertical direction along the time axis are not semi-circular, but this requires a constant force in the tangential direction of the curved surface when the manipulator operates on the curved surface. Because. Therefore, in the first half of the obstacle, the robot waits until the positional deviation from the virtual equilibrium point becomes large enough to obtain sufficient force in the tangential direction, and in the latter half, it operates with a small positional deviation.

Thus, the manipulator was able to perform stable contact work while maintaining a virtual impedance.

EFFECTS OF THE INVENTION In the present invention, an impedance control method for estimating the internal state can be obtained without using a force sensor, etc. Therefore, since the force sensor is not used, the structure of the manipulator is simplified, and force control is introduced. There is no decrease in the mechanical strength of the manipulator. Further, even if any part of the manipulator receives an external force, impedance control can be performed accordingly.

[Brief description of drawings]

1 is a front view of the manipulator arm, FIG. 2 is a side view of the manipulator arm, and FIG. 3 drives each joint.
Table showing specifications of the DD motor, FIG. 4 is a table showing specifications of the manipulator hand, FIG. 5 is a table showing parameters of each identified axis, FIG. 6 is a configuration diagram of the control device, and FIG. Is a block diagram showing an impedance control method, and FIG. 8 is a graph showing vertical and horizontal forces acting on the manipulator arm along the time axis. L ₁ …… First link, L ₂ …… Second link, L ₃ …… Third link, 1,2,3 …… Joint, 10 …… Manipulator arm, 11,12,13 …… Rotary axis, 14 , 15,16 …… actuator, 17 …… computer, 18 …… D / A converter, 21 …… U / D counter, 22 …… servo amplifier

 ─────────────────────────────────────────────────── ─── Continuation of the front page (56) Reference “Robot control basics” by Tsuneo Yoshikawa Published by Corona Publishing Co., Ltd.

Claims (1)

(57) [Claims] 1. A manipulator having a plurality of links connected through joints, wherein at least one of a distance or rotation angle sensor, a velocity or angular velocity sensor, an acceleration or an angular acceleration sensor is used, and a force sensor or a torque sensor is not used. Is an impedance control method that estimates the internal state, and the output torque T _a of the motor that drives the joint is I: inertia matrix J ^T : transposed matrix of Jacobian M: virtual inertia matrix J: Jacobian θ: of each axis Rotation angle vector t: time D _v : viscous friction matrix B: virtual viscous friction matrix K: virtual stiffness matrix X _o : virtual equilibrium point L: transformation matrix from joint coordinates to Cartesian coordinates C (θ, dθ / dt): gravity When a nonlinear term such as Coulomb friction is used, T _a = (I−J ^T MJ) (d ² θ / dt ² ) + {D _v −J ^T M (dJ / dt) −J ^T BJ} (d θ / dt) + J ^T K {X _o −L (θ)} + C (θ, dθ / dt) Impedance control method for manipulators