JPS61208513A - Controller of traveling object - Google Patents

Abstract

PURPOSE:To perform continuously and smoothly three processes, i.e., the approximation, contact and pressing by controlling the speed of a Z-axis motor according to the difference between the output of a synthesizing part of the input command value and the force measurement value and a real speed. CONSTITUTION:A processor CPU 82 multiplying the force measurement value Fr of a force sensor 3 by the prescribed gain alpha by a gain means 82a and subtracting the output of multiplication from an input command Vc by a synthe sizing means 85 to produce a command output Vc'. A servo circuit 86 obtains the difference between the output Vc' and the real speed Vr and gives the conduction control to a Z-axis motor 26 via a power amplifier 86a. Here the sensor 3 consists of a parallel plate spring and a strain gauge 30. The value Fr is equal to zero while a hand 5 is approximating to a disk 9, and the motor 26 receives the speed control with Vc'=Vc and moves down a ball screw 26b with revolutions. When the hand 5 has a contact with the disk 9, the value Fr is supplied to a CPU 28 from the gauge 30. Then the output Vc' is reduced and therefore the motor 26 is decelerated. Then the value Fr increases and the traveling object stops finally at a prescribed position.

Description

[Detailed Description of the Invention] [Table of Contents] Overview Industrial Application Fields Prior Art Problems to be Solved by the Invention Means for Solving the Problems (Fig. 1) Working Examples (AL-Description of Examples) (Fig. 2, Fig. 3) (b) Explanation of other embodiments (Fig. 4, Fig. 5) (Explanation of general application to Cl robot (Fig. 6, Fig. 7, Fig. 8, (Figure 9) Effects of the Invention [Summary] A moving object control device that causes a moving object to approach, come into contact with, and press against an object by driving a driving means, which includes a force detecting means for detecting an external force applied to the moving object and a driving means. By providing a servo means for speed control and a control means for supplying a signal obtained by combining a command value and a detected external force to the servo means, approaching, contacting, and pressing can be performed continuously.

[Industrial application field]

The present invention relates to a moving object control device for causing a moving object such as a robot hand to approach and contact an object and then press the object with a constant force. The present invention relates to a mobile object control device capable of successively proceeding through three processes.

For example, when a robot takes out an object, the robot's hand approaches the object, makes contact with it, and then presses the object with a certain force to grasp it. A hand holding an object approaches the object, makes contact with the object, and then presses the gripped object against the object with a constant force to fit and attach the object.

For this reason, it is necessary to control the hand, which is a moving object, through the three processes of approach, contact, and constant force generation, and generally a control system that combines speed (or torque) control and force control is used.

[Prior Art] For example, when a magnetic disk (hereinafter referred to as a disk) 9 is attracted by a vacuum suction hand 5 shown in FIG. It is necessary to control the motor 26 by switching between two control modes: speed control for lowering the hand and force control for generating force for necessary pressing.

Ideally, this mode switching is performed using the contact between the hand 5 and the disc 9 as a boundary.

For this reason, conventionally, the force sensor 3 supports the hand 5 so that the force applied to the hand 5 can be measured, and the speed detector 26a detects the speed of the Z-axis motor 26 (i.e., the moving speed of the hand 5). On the other hand, the control unit controls the command speed VC and the actual speed Vr of the speed detector 26a.
and a second differential AP2 that takes the difference between the command force Fc and the force measurement value Fr of the force sensor 3.
A speed feed bank system and a feedback system are constituted by these, and the outputs of these differential devices API and AP2 are configured to be switched by an analog switch SW and supplied to the Z-axis motor 26 via an amplifier AMP.

For this switching, a proximity sensor NS is provided at the tip of the hand 5 to detect the approach between the hand 5 and the disc 9.
As a result, the analog switch SW was operated before and after contact to switch from the speed control system to the force control system.

[Problem that the invention seeks to solve]

However, in the conventional configuration, since the loop is switched from the speed control system to the force control system, the operation becomes discontinuous, and a sudden force is applied immediately after contact, which may cause damage to the disk 9.

Also, depending on how the sensitivity of the proximity sensor NS is adjusted, it may not be possible to accurately detect the point of contact, and if the switch is made before contact, high speed performance will be impaired, and conversely, if the switch is made after contact, the disc 9 may have excess There was also the problem of imparting impact force.

SUMMARY OF THE INVENTION An object of the present invention is to provide a mobile object control device that can continuously and smoothly perform the three processes of approaching, contacting, and pressing, and autonomously.

[Means for solving problems]

FIG. 1 is a diagram explaining the principle of the present invention, FIG. 1(A) is a configuration diagram thereof, and FIGS. 1(B) and (C) are diagrams explaining its operation.

In Fig. 1 (A), the same parts as those shown in Fig. 10 are indicated by the same symbols, and 8 is a control section, which includes an input command value (velocity command or force command) Vc and a force measurement value. a synthesizing section 85 that synthesizes the output Vc' of the synthesizing section 85 and the actual speed V.
and a servo circuit 86 that controls the speed of the Z-axis motor 26 based on the difference between the rotation speed and the rotation speed.

That is, in the present invention, a system is constructed in which both the actual velocity Vr and the actual side force Fr are constantly fed back through the three processes of approach, contact, and pressing.

[Operation] As shown in the movement amount vs. speed characteristic diagram in FIG. 1(B) and the time vs. force retention diagram in FIG. 1(C), the disc (
While approaching the object) 9, the hand 5 is not pushing the disc 9, so the force measurement value Fr is zero, and only the actual velocity Vr is being fed back, so the input command VC is commanding the speed, and the speed of the Z-axis motor 26 is controlled by the servo circuit 86. Next, when the hand 5 contacts the disk 9 at that speed (time t
1), the force sensor 3 is deformed, and the force measurement value Fr is changed accordingly.
occurs, and therefore the value of the input command Vc is subtracted from the force measurement value Fr, resulting in a smaller apparent improvement command value, and the hand 5 begins to decelerate.

In the final equilibrium state, the Z-axis motor 26 stops at the position Po at time to, and the actual speed value becomes zero.

At this time, the input command Vc and the force measurement value Fr are equal, and the force measurement value Fr is pressed against the object 9 by a force Fo due to the deformation of the force sensor 3, so the input command Vc acts as a force command. Therefore, the magnitude of the input command Vc controls the pressing force.

Therefore, while the shock at the time of contact is absorbed by the deflection (deformation) of the force sensor 3, the rotational speed of the motor 26 is continuously reduced, and when the speed reaches zero, appropriate pressing generates force. This can be ideally done.

In this way, the three processes of approaching, contacting, and pressing are performed continuously and smoothly, and moreover, they are performed autonomously without using a proximity sensor.

〔Example〕

(81-Explanation of an embodiment.

FIG. 2 is a configuration diagram of an embodiment of the present invention.

In the figure, the same components as shown in FIGS. 1 and 10 are indicated by the same symbols, and 82 is a processor (hereinafter referred to as CP).
U), which is composed of a microprocessor, multiplies the force measurement value Fr by a predetermined gain α by a gain means 82a,
The multiplication output α・F' is subtracted from the input command Vc by a synthesizing means 85 to generate a command output Vc', 86 is a servo circuit that takes the difference between the command output Vc' and the actual speed VrO, and 86a is a power amplifier. It amplifies the output of the servo circuit 86 and supplies current to the Z-axis motor 26.The force sensor 3 is composed of a parallel plate spring and a strain gauge 30.

The operation of the configuration shown in FIG. 2 will be explained using the third operation explanatory diagram.

When the hand 5 is approaching the disk 9, the force measurement value F of the force sensor 3
Since r is zero, Vc'=Vc, and Z-axis motor 2
6 is speed-controlled by a servo circuit 86 via a power amplifier 86a, rotates a ball screw 26b, and lowers the hand 5 via a force sensor 3. On the other hand, when the hat 5 contacts the disk 9 at the position Pl, the force sensor 3 is deflected because it is made of a leaf spring, and the strain gauge 30 that detects this deflection generates a force measurement value Fr, and the CPU 82 determines the force measurement value Fr. becomes (Vc-α·Fr). This causes the output Vc' to decrease and the motor 26 to begin decelerating. At this time, the slope of deceleration is controlled by gain α, and the parallel leaf spring
It is further deflected by rotation 6, and the force measurement value Fr increases. Eventually, the actual speed Vr becomes zero at position PO, and at this time, vC
-α·Fr, and force is generated when the FO is pressed.

The meaning of multiplying by this gain α is as follows.

Since the input command Vc serves as both the speed command value when approaching and the force command value when pressing forces are generated, depending on the relative magnitude of the output levels of the speed measurement value Vr and the force measurement value Fr,
There is a possibility that one input Vc cannot satisfy both the optimum moving speed and the optimum pressing force at the same time.

For this reason, an input is provided to make the feedbank amount of the force measurement value Fr variable, that is, a gain α is set, and the input command Vc and gain α are independently set, and the optimum speed command value and force command are set. The value can be obtained with one input command.

As described above, the present invention does not require an analog switch such as a proximity sensor unlike the conventional method, and furthermore, since the rotational speed of the motor decreases continuously before and after contact, the shock at the time of contact is kept small. Can strain gauge 30
Using the linear force output value of , it is possible to switch from speed control mode to force control mode in a well-timed and smooth manner. In addition, when pressing is under force control, the feedbank of the speed measurement value (actual speed) is treated as a so-called damping term of the state variable feedbank, so the control system is extremely stable.

According to the present invention, when the hand 5 picks up the magnetic disk 9 or the spacer 90, as shown in FIG. Even when the depth of the robot changes, the desired hand descending speed and the desired pressing force can always be obtained automatically, making it unnecessary to teach the distance in the depth direction, and the robot This will reduce the burden on humans who have to instruct robots to perform tasks, and will make robots one step more intelligent.

(b) Description of other embodiments.

FIG. 4 is a block diagram of a second embodiment of the present invention.

In the figure, the same parts as those shown in FIG. 2 are indicated by the same symbols, and 82b is a dead band portion, which shows the input/output characteristic of the nonlinear characteristic of the centered dead band width W.

In this embodiment, the desired speed and desired pressing force are obtained by the dead band portion 82b.

That is, the dead band section 82b performs a feed bank of a signal passed through a nonlinear element to the force measurement value Fr, and is performed by the calculation of the CPU 82.

The calculation method for this nonlinear element is as follows.

First, when the absolute value of the input is smaller than the dead band width setting value W, the output is set to zero. On the other hand, when the absolute value of the input is larger than the dead band width setting value W, if the input is a positive value, (input - dead band width setting value) is output, and if the input is a negative value, (input + dead band width set value) as the output.

Next, the operation of the configuration of the embodiment shown in FIG. 4 will be explained using FIG.

As shown in FIG. 5, even if contact is made at position P1, the pressing force corresponding to the dead zone width W continues to move at the speed given by the command Vc until it becomes a force, and after that, the third It decelerates and stops as shown in the figure.

The pressing force at this time is the sum of the force command value based on the input command Vc and the force corresponding to the dead zone width W. On the other hand, the speed during approach depends only on the input command Vc, so the moving speed and the pushing force are the same. The attached power can be controlled independently.

This embodiment has the following advantages over the embodiment shown in FIG. In other words, since the feed bank gain of the force measurement value Fr can be fixed, the pressing force can be varied over a wide range while keeping the loop gain as a closed loop control system constant (that is, while guaranteeing stability as a control system). What you can do, and with hand 5 floating in the air (i.e. hand 5
is not pushing any other object), input Vc is set to zero,
In the embodiment shown in FIG. 2, if there is an offset variation in the force measurement value of the strain gauge 30, when you want to stop the strain gauge at that location,
As a result, the position of the hand 5 may drift, but in this embodiment, it becomes insensitive to offset fluctuations smaller than the dead zone width, and no drift phenomenon occurs.

In addition, in FIG. 4, input Vc is set to zero, and hand 5
When the instructor guides the cylinder up and down with his hand, the strain gauge 30 of the force sensor 3 senses this, and the movement of the robot's hand follows the movement of the worker's hand. So-called direct teaching is possible. this is,
Since the input Vc is zero, the motor 26 is controlled so that the value obtained by calculating the dead zone on the force measurement value Fr becomes zero.

In this case as well, depending on the setting of the dead band width, the instructor can
To prevent a drift phenomenon when the hand is removed from the cylindrical part of the hand 5, and to generate an appropriate resistance force (corresponding to the width of the dead zone) as a response to the teacher when the teacher guides the cylindrical part of the hand 5 by hand. I can do it.

As another embodiment, both the gain control and the dead zone control may be performed within the CPU 82, and the gain means 82a or the dead zone section 82b may be configured by separate hardware without depending on the execution of the program by the CPU 82. Alternatively, the actual speed may not be measured by the detector 26a, but may be obtained as an estimated value from the state of the motor 26 by an observer or the like.

+Explanation of application example to Cl robot.

FIG. 6 is a configuration diagram when the present invention is applied to an orthogonal robot, FIG. 7 is a configuration diagram of the hand and force sensor configured in FIG. 8, and FIG. 8 is a composition section 85 and servo control section 86 configured in FIG. and a block diagram of a force control section.

In the figure, the same parts as those shown in FIGS. 1, 2, and 10 are indicated by the same symbols, and 1az1b is an X-axis module, which constitutes the X-axis positioning mechanism of the robot.
Transport pallets lla by each X-axis motor 10a, 10b
, Ilb is transported and positioned in the X-axis direction, and 2 is a gate-type robot, which includes a pair of support bases 20 and 21 provided on both sides of the X-axis modules la and lb, and a Z-axis block that moves in the Y-axis direction. 22, a two-axis movable part (arm) 23 that moves in the X-axis direction, and a Y-axis motor 24 that sends the Z-axis block 22, rotates the pole screw 24a, and drives it in the Y-axis direction along guides 25a and 25b. Z-axis block 2
2, and a Z-axis motor 26 that drives the Z-axis movable section 23 in the X-axis direction via a ball screw feeding mechanism (not shown). 3 is the aforementioned force sensor, as shown in Fig. 7(A).
As not shown in detail in , a pair of leaf springs 3
a, 3b are fixed in parallel with screws or the like, and further fixed to a support member to be described later, and a pair of strain gauges 30a, 30b are formed on the inner surfaces of leaf springs 3a, 3b, respectively.

31a, 31. The strain gauges 302 to 31b are connected in a bridge as shown in FIG. 7(B), and an output voltage Out is obtained for the input voltage Vin. , 3b suction hand 5
The strain gauge 30a detects the displacement in the axial direction of the strain gauge 30a.
, 30b are the sides that receive compressive loads, and 31a and 31b are the sides that receive tensile loads.

Reference numeral 4 denotes a hand support member, which supports the suction hand 5 and has parallel plate springs 3a and 3b fixed thereto as shown in FIG. 7(A), and 5 represents a vacuum suction hand;
As shown in the figure, a suction surface 51 is provided at the tip of the cylindrical body 50, and an intake tube 52 connected to an intake pump.
6 is a jig that fixes the disk 9 on the pallet lla, 7 is a base and the pallet lla
b, and a disc 9 is attached to it.

80 is an operation panel, which is operated by the operator to instruct playback mode, teaching mode, etc., 81 is a memory, which stores teaching data, etc., 82 is a processor (hereinafter referred to as CPU), It is composed of a microprocessor, etc., and reads out the contents of the memory 81 during playbank and gives commands to each part. Reference numeral 83 is an X- and Y-axis servo control unit, which controls the X-axis motors 10a, 10b and In order to control the position of the Y-axis motor 24, command positions CX2, CX+, CY from the CPU 82 and current position PX2, from a driver position detection circuit to be described later are used.
84 is a power amplifier that outputs the difference between PXl and PY, and amplifies the input to drive the X-axis motors 10a, 10b, Y
85 is the above-mentioned combining unit that supplies current to the axis motor 24, and as described later in FIG. 8, the Z-axis command value VZ
86 is the aforementioned servo circuit, which will be described later in FIG. 8, 87 is a force control section, As will be described later in the figure, a device receives the detection output FZ of the force sensor 3, converts it into a digital value FZ, sets a dead zone, and outputs the control output PFZ, and 88 is a hand position detection section, which detects each axis. motors 10a, 10b, 24.
The current position PXI, PX2, PY, PZ of each axis is obtained from the output of the rotary encoder provided at 26, and the current position of the hand 5 is obtained. 89 is a bus, and the CPU 8
2, memory 81, operation panel 80, servo control section 83,
Synthesis unit 85, force control unit 87, and hand position detection circuit 88
It is used to exchange data and commands.

In Fig. 8, 850 is a digital/analog converter (hereinafter referred to as D/A converter) that converts the speed command VZ given from the CPU 82 via the bus 89 into an analog voltage command, and 851 is a differential amplifier. Yes, D/
86b is a differential circuit that takes the difference between the voltage command of the A converter 850 and the control output PFZ from the dead band circuit 870, and the difference between the output Vc' of the differential amplifier 851 and the actual speed Vr. 870 is a dead band circuit, and the CPU 8
2 through the bus 89, and when the detection output FZ of the force sensor 3 is -W≦M≦W, the control output PFZ is set to zero, and in the case of MOW, the positive control output PFZ is set as
If M<-W, a negative control output PFZ is given to the differential amplifier 851 of the synthesis section 85. 871 is an analog/digital converter (hereinafter referred to as A/D converter), which converts the analog detection output FZ into a digital one. It converts it into a value and outputs it to the bus 89.

Next, the suction and take-out operation of the robot will be described with reference to the ninth suction operation processing flowchart.

First, the CPU 82 sends a position command CX1 and CY to the X and Y coordinate positions of the disk 9 to the servo control section 83 via the bus 89, and a drive current SX+ and SY from the power amplifier 84 to the X-axis motor 10a and Y-axis motor 24, respectively. supply to. By this,
X-axis motor 10a and Y-axis motor 24 of axis module 1a
is driven, and the vacuum suction hand 5 moves to the X-axis module Ia.
The upper bank) is positioned X-Y on the disk 9 of the jig 6 of lla.

■Next, the CPU 82 checks whether or not the hand has reached a predetermined position from the current position PX2, py, pz of each axis of the hand position detection circuit 88, and determines whether a predetermined time has elapsed after stopping at the predetermined position. After that (approximately 0.5 seconds), it is assumed that the force sensor 3 has stopped vibrating, and the feed puncture is turned on. For example, a switch (not shown) between the force control section 87 and the synthesis section 85 is turned on to enable the output PFZ of the force control section 87 to be input to the synthesis section 85 .

■The CPU 82 sends the command value VZ to the synthesis unit 8 via the bus 89.
Give to 5. As mentioned above, while approaching the disk 9, PFZ=
Since it is 0, it is output as a command speed to the servo circuit 86, and the speed of the Z-axis motor 26 is controlled. Therefore, the suction hand 5 descends toward the disk 9 at a commanded speed of 1.

■On the other hand, the CPU 82 connects the force controller 87 to A via the bus 89.
The force measurement value FZ of the /D converter 871 is monitored, and when FZ reaches a predetermined value M, it is determined that the suction hand 5 has contacted the disk 9. From then on, as shown in FIG. 5, the feed bank operates, the output Vc' of the amplifier 851 of the combining section decreases, the descending speed of the two-axis motor 26, that is, the suction hand 5 decreases, and the circle of the suction hand 5 decreases. Pressing against the plate 9 generates force, and the pressing force gradually increases.

Further, the CPU 82 connects to the intake tube 52 via the bus 82.
The output of a pressure sensor (not shown) that detects negative pressure is monitored to detect whether or not the suction hand 5 has suctioned the disk 9.

(2) When the CPU 82 determines that the suction hand 5 has suctioned the disk 9, the CPU 82 sets the command value vZ to zero, and then turns off the above-mentioned switch to turn off the cup feedback.

Furthermore, in order to raise the suction hand 5, the CPU 82 gives a command value VZ in the opposite direction to the synthesis unit 85 via the bus 89, thereby causing the Z-axis motor 26 to rotate in the reverse direction, and the suction hand 5 to rise and move in a circular motion. The board 9 is taken out.

Furthermore, in the same manner as described above, drive the X-axis motor lOb, and the Y-axis motor 24 to position the suction hand 5 at a predetermined position on the base 7 of the pallet Ilb of the X-axis module 1b, and move it in the Z direction similarly to steps The disc 9 is lowered to approach, touch, and press the base 7, and the suction is released to attach the disc 9 to the base 7.

Although the present invention has been described above using examples, the present invention can be modified in various ways according to the gist of the present invention, and these are not excluded from the present invention.

〔Effect of the invention〕

As explained above, according to the present invention, it is possible to approach, touch, and press an object continuously and smoothly without switching loops, and without applying sudden force to the object. It can operate smoothly and is especially suitable for removing and attaching robots, etc.

Furthermore, since the command value is a speed command until contact, there is an effect that the high-speed approach is not impaired, and high-speed driving is possible. Furthermore, since such operations can be performed autonomously, there is no need to provide an analog switch such as a proximity switch, and an inexpensive device with stable operation can be provided.

[Brief explanation of drawings]

FIG. 1 is a diagram explaining the principle of the present invention. FIG. 2 is a configuration diagram of one embodiment of the invention. FIG. 3 is a diagram explaining the operation of the configuration shown in FIG. , FIG. 5 is an explanatory diagram of the operation of the configuration shown in FIG. 4, FIG. 6 is a configuration diagram in which the present invention is applied to a robot, FIG. 7 is a configuration diagram of the hand and force sensor of the configuration shown in FIG. 6, and FIG. FIG. 9 is a block diagram of the synthesis unit and force control unit in the configuration shown in FIG. 9. FIG. 9 is a flowchart of the adsorption operation process in the configuration shown in FIG. In the figure, 3-force sensor (force detection means), 5-=hand (moving body), 9-disk (object), 26-Z-axis motor (driving means), 85--combining unit (controlling means) , 86-servo circuit.

Claims (1)

[Scope of Claims] A driving means (26) for driving the moving body (5), a force detecting means (3) that is displaced by an external force applied to the moving body (5) and detects the external force, and the driving means Servo means (26) for controlling the speed of
86), and control means (85) for supplying a signal (Vc') that is a combination of the command value (Vc) and the output (Fr) of the force detection means (3) to the servo means (86), A moving body control device characterized in that the moving body (5) continuously approaches, contacts, and presses an object.