JPH0446785A - Hybrid control device for position and force of manipulator - Google Patents

Abstract

PURPOSE:To facilitate pin insertion work by making directions of position control and force control to become translation and rotation on the basis of wrist coordinate system. CONSTITUTION:Deviations DELTAX, DELTAY of position and force in earth coordinate system of a manipulator 20 are obtained. Next, these deviations are converted to deviations DELTAXW, DELTAFW in wrist coordinate system of the manipulator 20. Position control gain and force control gain are multiplied against the deviations DELTAXW, DELTAFW of this wrist coordinate system to obtain the corrected speed VP, VF in the wrist coordinate, and these corrected speed VP, VF are converted to rotating speed of each joint of the manipulator 20 to be output to the joint type manipulator 20. Consequently, direction of position control and force control becomes rotation and translation on the basis of the wrist coordinate system, and when direction of position control is set in a ZW axis direction of the wrist coordinate system and direction of force control is set in any direction except for the ZW axis direction, about directions except for the ZW axis direction, insertion work of a pin 13 is performed from a hole 14 to the direction for reducing the force to be applied to the pin 13.

Description

DETAILED DESCRIPTION OF THE INVENTION <Industrial Application> The present invention relates to a hybrid position and force control device applied to a manipulator.

<Prior art> Articulated manipulators generally have a shape similar to a human arm, and each joint is equipped with an actuator, a speed sensor,
A position sensor is provided, and a force sensor is provided on the wrist. Also, as a manipulator of other machines,
Torque is estimated by the force or motor current provided by a torque sensor attached to each joint.

Here, a conventional device is shown in FIG. In FIG. 3, the position control system includes angular position conversion means 1 for determining the position in the orthogonal coordinate system from the angle, position control selection means 2 for selecting the direction in which position control is to be performed, position control gain means 3, and velocity angular velocity conversion means. 4, and further includes a servo amplifier 9, an actuator 10, and a position sensor 12 provided within the manipulator body 20.

The positional target x1 is compared with the position X on orthogonal coordinates (O-xyz coordinate system) obtained by inputting the output θ from the position sensor 12 of the manipulator to the angular position converting means 1 and obtaining the positional deviation ΔX. It will be done.

The positional deviation ΔX is input to the position control selection means 2, and only the deviation in the direction in which position control is to be performed is outputted from the position control selection means 2, and the corrected speed Vp for position control is derived by multiplying by the position control gain. Ru. On the other hand, the force control system converts the force detected in the wrist coordinate system to force in another orthogonal coordinate system (o-xyz coordinate system). Force conversion means 7, force control selection means 5 for selecting the direction in which force control is performed, force control gain means 6, and velocity/angular velocity conversion means 4, as well as a servo amplifier 9 and an actuator 1 provided in the manipulator body 20.
0, wrist sensor 8.

The force target value F is obtained by inputting the output F from the wrist sensor 13 of the manipulator to the force converting means 7. It is compared with the force F of the orthogonal coordinate system (〇-xYZ coordinate system) and the force deviation ΔF is calculated. Desired.

The force deviation ΔF is input to the force control selection means 5, and the deviation only in the direction in which force control is to be performed is outputted from the force control selection means 5 and sent to the force control gain means 6, where a corrected speed vF for force control is derived.

The corrected velocities vP and Vl are added and input to the velocity angular velocity conversion means 4. In this manual rotation, the speed commands for each joint of the manipulator 20 are compared with the speed detected by the speed sensor 11, and the servo amplifier 9 adjusts the actuator 10 to match the tide by speed feedback.
to drive.

<Problem to be solved by the invention> In the conventional system shown in FIG. 3, the direction of position control and force control is based on the earth coordinate system (o-xyz coordinate system) shown in FIG.
Translation and rotation based on In such a case, when attempting to insert the pin 13, the axial direction of the pin and the hole 14
This can be achieved by controlling the position to match the direction of the object, and controlling the force to control the direction outside of the breath.

However, since this earth coordinate system is a spatially fixed coordinate system, the insertion direction of the pin 13 and the
The above operations cannot be performed unless the axes, Y-axis, or Z-axis coincide.

SUMMARY OF THE INVENTION Accordingly, an object of the present invention is to provide a hybrid position and force control device for a manipulator that allows work to be performed smoothly even if the work direction does not match the earth coordinate system.

Means for Solving the Problems> The present invention achieves the above-mentioned objects by providing information on the position and force of this manipulator in the earth coordinate system for an articulated manipulator that is equipped with a force sensor on the wrist and inputs the velocity of each joint. A means for determining the wheel difference, a means for converting this deviation into a deviation as seen from the wrist coordinate system of the manipulator, a means for selecting a direction for position control and force control, and a means for converting the deviation in position or force as seen from the wrist coordinate system. means for converting into a correction speed;
In a position/force hybrid control device comprising means for converting the correction speed into a rotational speed of each joint of the manipulator, the directions of position control and force control are translation and rotation based on the wrist coordinate system of the manipulator. Features.

<Operation> Find the deviation between the position and force of the manipulator in the earth coordinate system, and then convert this deviation into the deviation in the manipulator's wrist coordinate system. Then, by multiplying this deviation in the wrist coordinate system by the position control gain and force control gain, the correction speed in the wrist coordinate system is obtained, and this correction speed is converted into the rotational speed of each joint of the manipulator and output to the articulated manipulator. do.

Therefore, the direction of position control/force control is rotation and translation based on the wrist coordinate system, and if the direction of position control is the 28-axis direction of the wrist coordinate system and the other directions are force control, then zl,
For directions other than one direction, the insertion operation is performed while the bottle adjusts in the direction that reduces the force that the pin receives from the hole.

Embodiments The present invention will now be described in detail with reference to FIGS. 1 and 2. As an example, an example of application to the articulated manipulator 20 shown in FIG. 2 will be described. As shown in FIGS. 1 and 2, each joint of the articulated manipulator 20 is equipped with a servo amplifier 9, an actuator 10, a speed sensor 11, and a position sensor 12. is equipped with a wrist force sensor 8. This force sensor 8 detects forces applied to the wrist in three directions (X, direction, Yu force direction, Z, direction).

In FIG. 1, S is a selection command for position control f11I/force control, and setting an element in S to 1 indicates force control, and setting it to 0 indicates position control.

Further, the position target value X is the position sensor 1 of the manipulator.
The output Q from 2 is input into the angle and position conversion means and compared with the position X of the earth coordinate system (0-XYZ coordinate system), and the position deviation ΔX according to the earth coordinate system is determined.

Next, the position control system is provided with speed inverse conversion means 24 that inputs ΔX. This speed inversion means 24 is Δ
Convert X into a positional deviation Δx2 in the wrist coordinate system.

The positional deviations ΔX, .

The position control gain means 22 derives the corrected speed (2) in the wrist coordinate system for position control.

Next, the speed conversion means 25 converts the corrected speed xpu into a corrected speed 2 in the earth coordinate system.

Let's talk about the force control system.

Force target value F1 in wrist coordinate system. The full force conversion means 7 converts it into a force target value F in the earth coordinate system. and,
This force target value F1 is set as the output F1 of the wrist force sensor 8 of the manipulator. 1 is compared with the force F in the earth coordinate system obtained through the force conversion means 7, and a force deviation ΔF is determined.

Then, similarly to the position control system, the force deviation ΔF is converted by the speed inverse conversion means 24 into the force deviation F in the wrist coordinate system, and this force deviation ΔF is obtained.

is input to the force control selection means 5 to output only the deviation in the direction in which force control is to be performed, and then by multiplying the force control gain means 23 by the gain, the corrected velocity v in the wrist coordinate system for force control is determined. l, is derived and converted to this modified velocity VF.

The corrected speeds V and 1 of the position control system and force control system are speeds in mutually orthogonal directions, and these are added to obtain the speed
It is input to the angular velocity conversion means 4. The output θ of this velocity/angular velocity conversion means 4 becomes a velocity command to each joint in the manipulator. The speed command θ2 is compared with the speed detected by the speed sensor 11, and the servo amplifier 9 drives the actuator 10 by speed feedback so that δ matches the tide.

Although the embodiment shown in FIG. 1 is equipped with a wrist force sensor, the present invention also includes a case where the torque is estimated using a torque sensor of each joint or a motor current to determine the force applied to the hand. It will be done.

<Effects of the Invention> As explained above, according to the present invention, by providing hybrid control in the position control system and the force control system, by processing the earth coordinate system based on the wrist coordinate system, for example, bottle insertion Work becomes easier.

[Brief explanation of the drawing]

FIG. 1 is a block diagram of an embodiment of the present invention, FIG. 2 is a diagram for explaining the operation of an articulated manipulator, and FIG. 3 is a block diagram of a conventional example. In the figure, 22 is a position control gain means, 23 is a force control gain means, 24 is a speed inverse conversion means, and 25 is a speed conversion means.

Claims (1)

[Scope of Claims] Means for determining the position and force deviation of the manipulator in the earth coordinate system for an articulated manipulator having a force sensor on the wrist and inputting the velocity of each joint; means for converting the deviation into a deviation seen from the coordinate system; means for selecting a direction for position control and force control; means for converting the position or force deviation from the wrist coordinate system into a correction speed; and a means for converting the rotational speed of each joint into the rotational speed of each joint, the hybrid control device characterized in that the directions of position control and force control are translation and rotation based on the wrist coordinate system of the manipulator. .