JPS61265289A - Controller for moving body - Google Patents

Abstract

(57) [Summary] This bulletin contains application data before electronic filing, so abstract data is not recorded.

Description

[Detailed Description of the Invention] [Table of Contents] Overview Industrial Field of Application Conventional Technology Problems to be Solved by the Invention Means for Solving the Problems (Fig. 1) Working Examples (Al-Overall Examples) Explanation (Figures 2 and 3) (b)
Explanation of the configuration of the control unit (Figs. 4, 5, and 6) (C) Explanation of the operation of the control unit (dl) Description of other embodiments (Figs. 7 and 8) (e)
Description of still other embodiments (FIGS. 9 and 10) Effects of the invention [Summary] In a mobile body control device that controls the movement of a mobile body by a moving means, the output of a non-contact sensor for monitoring the operating environment of the mobile body By providing a control means that controls the moving means by combining the state quantity with the movement command, the output of the non-contact sensor is incorporated into the control system as energy to control the movement.

[Industrial application field]

The present invention relates to a moving body control device that controls the movement of a moving body, and more particularly to a moving body control device that controls the movement of a moving body by incorporating the output of an external non-contact sensor that detects the operating environment of the moving body into a control system.

Mobile object control devices that control the movement of mobile objects are widely used. For example, in a robot, a desired task is executed by controlling the movement of a hand, which is an end effector.

In such movement control, an internal servo sensor is used to detect the current, speed, position, etc. of the actuator (e.g. motor) that is the movement means, and the state quantity of the internal servo sensor is fed back to control the drive. Movement is controlled to stabilize the system.

With only such internal sensors, the operating environment in the moving space is unknown, so the operating environment of the moving object (position with objects,
By providing an external sensor that monitors the position of the moving body itself, the environment (temperature, humidity, gas, liquid flow rate, etc.), control is performed in accordance with external conditions. In other words, the servo internal sensor is an essential sensor for the servo system, and it constantly outputs a signal while the servo system is operating, and the output signal is fed to the servo system to ensure stable system operation. This is a banked sensor.

On the other hand, an external sensor is a sensor whose output signal does not affect the stability of the servo system in any way, and the servo system does not become unstable even if the signal is cut off.

As a sensor for externally monitoring the environment of a moving body, a non-contact sensor that performs detection without contacting the moving body or an external object is widely used.

Examples of such non-contact sensors include ultrasonic sensors that measure the distance to objects, laser distance sensors, temperature sensors that detect proximity to heat sources, and eddy current sensors that detect the distance to conductors such as metals. It will be done.

[Conventional technology]

Conventionally, as a method for reflecting the output of such a non-contact sensor in movement control, a control unit constantly reads the output of the non-contact sensor, determines the environmental state from the output, and calculates the state quantity.
It was decided whether to change the control mode or not, and then the movement command was changed.

In this conventional method, the state from the non-contact sensor is determined and the movement command itself is changed to change the control mode.

It was intended to perform state-adaptive control by making changes.

(Problems to be Solved by the Invention) However, in this prior art, since the process of judgment by the control unit is involved, it is necessary to perform extremely high-speed processing in order to perform real-time processing, and therefore the burden on the control unit is reduced. This poses a problem in that state quantities cannot be reflected in real time during high-speed movement.This becomes a particularly big problem when there are a large number of sensors.

Therefore, even though the moving object needs to stop due to a change in the environmental condition, control starts after a delay corresponding to the time required for the judgment process, which may cause a situation in which the moving object collides with an obstacle, etc. .

In order to solve this problem, it is possible to make the sensor wider. However, making the sensor wider will make the judgment process of the control unit even more complicated due to the mixing of necessary state quantities and unnecessary state quantities (noise). , a problem has arisen in that the burden on the control section becomes even greater.

Therefore, an object of the present invention is to provide a mobile object control device that can incorporate the detected state quantity of a non-contact sensor into a control system and perform real-time processing while reducing the burden on the control section.

[Means for solving problems]

FIG. 1 is a diagram explaining the principle of the present invention.

FIG. 1(A) is a configuration diagram of the present invention, FIG. 1(B),
(C) is an explanatory diagram thereof.

In the figure, M is a moving body, such as a robot arm or hand, MT is a moving means, which moves the moving body M, such as a motor, and NS is a non-contact sensor. CT is a control means (control unit) for monitoring the environmental condition of the moving body M, and is composed of, for example, an ultrasonic distance sensor.
It receives the output of the non-contact sensor NS as a state quantity ps, and controls the movement of the moving means MT by combining it with the movement command PC.

Therefore, in the present invention, the output of the non-contact sensor is converted into the state quantity ps
A system is constructed that is incorporated into the control system in such a way that it is taken as a composite with the movement command PC.

[Effect]

In the present invention, the movement of a moving device (hereinafter referred to as a robot) including a moving body M and a moving means MT is controlled in the following manner.

That is, regarding a position control system that stably realizes high rigidity and high-speed response in a robot, let us consider one in which the moving means MT is composed of a DC motor.

It is assumed that the characteristics of one axis of a robot driven by a DC motor are expressed by the following voltage equation and motion equation.

v=R-4+L-i+ B1・x (11f
=Bj! -4(2) f -M -M+D -x 10Ff
(31 However, ■ = Voltage between terminals of DC motor R: DC resistance i of DC motor: Current L of DC motor: Inductance Bl of DC motor: Force constant f of DC motor: Generated force M of DC motor: Mass D: Viscous braking coefficient of the moving part Ff: Frictional force of the moving part The control block diagram for this case is shown in Figure 1 (B
).

Expressing this as a transfer function, the robot's displacement X with respect to the control system's displacement command P (S) shown in Figure 1 (B)
The transfer function of (S) is as follows.

However, aO-Ap-Bl - is 3 al-Ap-Bj! − to 2+Ap−kiD + R−
D + BNa2=Ap-kl・M + (,-D +
R-Ma3=L-M Ap: Open loop gain of operational amplifier P: 1 for displacement command; 2 for current feedback gain: 3 for speed feedback gain: 3: Displacement feedback gain Here, assuming Ap-■ When formula (4) is transformed, it becomes.

From equation (5), the robot throat is equivalent to a moving object in which the spring SP and dash board 1-DP are connected to external restraints as shown in FIG. 1(C).

In other words, it is a moving object whose stiffness K is (+ to B 1·k3/) and whose viscous damping coefficient is (++D to B 1·k2/).

Therefore, by increasing k3 and stabilizing by kl and kl, it is possible to stably control the position of a robot with high rigidity and good responsiveness.

On the other hand, in the present invention, the non-contact sensor NS as shown in FIG.
Since the control system is such that the detected state quantity PS (S) is combined with the command P (S), the control block diagram of Fig. 1 (B) has <PS (S) added as indicated by the arrow. Therefore, the transfer function of equation (5) becomes It will be done.

However, α is a gain.

From equation (6), the force F (11) generated by the motor is F t! ri-CP l3)-αPSt3)-X (3
1) ・B l −k3/kl (7) Therefore, when the signal from the non-contact sensor NS is not input, X (sl −P (sl
In state (8), the generated force F (S) becomes zero and stops.

On the other hand, when the non-contact sensor NS generates an output PS (S) due to the environmental condition of the moving body M, the force generated by the motor becomes Equation (7), and therefore, X (S) = P (
3)-α·P S (31 (it will stop at the position 91, or it will generate the force F(S) of equation (7)).

Therefore, even if the output of the non-contact sensor NS (regardless of whether it is nonlinear or linear) is incorporated as a state quantity, the stability of the system is maintained.
Moreover, the control mode is autonomously changed based on the output of the non-contact sensor NS.

This means that a plurality of non-contact sensors NS are provided, and P S (
s) −P S + (s) +−−−+ P S n
(s) α[ holds true in the same way.

In addition, by using the linear component (linear region) of the non-linear output of the non-contact sensor NS, sudden changes in control mode are not performed, and mode changes such as deceleration can be realized gradually, thereby further stabilizing the system. .

〔Example〕

+al Overall description of one embodiment.

FIG. 2 is an overall configuration diagram of an embodiment of the present invention, taking a four-axis orthogonal robot as an example.

In the figure, 1 is the base of the robot body, which has an X-axis drive source (motor) 11 that drives an arm to be described later in the arrow A (X-axis) direction, and 2 is the first arm. It has a Z-axis drive source (motor) 12 that drives the second arm (described later) in the direction of arrow B (Z-axis), and is moved in the X-axis direction by the X-axis drive source 11 of the base 1. 2, and the arrow C (
It has a Y-axis drive source (motor) 13 that drives in the Y-axis direction, and moves in the Y-axis direction.

Reference numeral 4 denotes a θ-axis rotation unit, which has a θ-axis drive source (motor) 14, is supported by the second arm 3, and rotates a hand to be described later about the θ-axis. , and is attached to the θ-axis rotation unit 4.

Therefore, the robot hand 5 is positioned in three dimensions of the XSY and Z axes, and rotated about the θ axis.

In this embodiment, the arm 2.3, the rotating part 4, and the hand 5 correspond to the moving body M shown in FIG.
．． 13 and 14 correspond to the moving means MT. Reference numeral 6 denotes a control unit, which, as will be described later in FIG.
Actual velocity (actual angular velocity) Vx-Vγ, current position Xp-γp servo-drives the motors 11 to 14 of the square axis, and also synthesizes the state quantity and displacement command from the ultrasonic sensor, which will be described later. This is an ultrasonic sensor, which is installed at the tip of the arm 3 and transmits ultrasonic waves in three dimensions (X.

This is a well-known non-contact sensor that measures the distance to an object in three-dimensional directions by emitting waves in the Y and Z directions and receiving the reflected waves.

Therefore, the ultrasonic sensor 7 outputs outputs psx, PSY, P according to the distance of the hand 5 from the external object in the X, Y1Z directions.
SZ will be detected.

FIG. 3 is an explanatory diagram of the operation of an embodiment of the configuration shown in FIG.

In the embodiment shown in FIG. 3(A), when the hand 5 moves from the position (XPO, YPO) on the X-Y plane to the position (XPI, YP 1) of the target object 8 as shown by the solid line, (solid line) shows an example in which there is an obstacle 9 inside.

Assume that a displacement command from YPO to YPl is given in the Y-axis direction as shown in FIG. 3CB), and that the expected speed command is vcy.

It is assumed that displacement commands from xPO to XPI are similarly given in the X-axis direction.

The hand 5 is moved by X-axis and Y-axis motors 11.13.
When it is driven in the Y direction and reaches the position (XP2, YP2), the ultrasonic sensor 7 detects the obstacle 9, and as shown in FIG. 3(B)
It emits an output as shown by the dashed-dotted line, and is given a gain α to reduce the movement command (displacement command) within the control unit 6 as α·PSY.

Therefore, as shown in Figure 3 (C), the velocity in the Y-axis direction is
It decreases from P2. On the other hand, since there are no obstacles in the X-axis direction, the speed in the X-axis direction does not decrease.
As shown by the dotted line in A), move the hand 5 so as to go around the obstacle 9.
is controlled to move, and when the ultrasonic sensor 7 no longer detects the obstacle 9 in the Y-axis direction, the velocity vcy in the Y-axis direction increases again, and after the hand 5 detours, the target position (XPI, YP') is reached.
move towards.

Here, if the gain α is large, the hand 5 will bypass the obstacle 9 without contacting it, whereas if the gain α is small, the hand 5 will bypass the obstacle 9 while approaching or contacting it.

At this time, if the output state quantity of the ultrasonic sensor 7 is fed back to the command speed, the command position (XPI, YPi) can be bypassed without being changed, while if it is fed back to the displacement command, the displacement command is changed and the target There is a risk that the position will shift, and in this case, this can be prevented by re-instructing the target position after the detour. Further, if there is an obstacle 9 in the X-axis direction, the robot can similarly perform a detour operation, and if there is an obstacle 9 in both the X1 and Y-axis directions, it can be stopped in front of the obstacle 9.

(Explanation of the configuration of the bl control unit.

FIG. 4 is a block diagram of the control section 6 in the configuration shown in FIG.

60 is a processor (hereinafter referred to as CPU) which issues displacement commands X C% Y C% Z C1θC for each axis according to teaching data (stored in a memory not shown);
61, 62, 63, and 64 are XSZ and Y1θ axis servo controllers, respectively, and as detailed in Fig. 6, displacement commands X C% V C% Z C% θC and sensor signal processing section (described later) The motors 11 to 14 of each axis are servo-controlled by combining the state quantities SX, 5YSSZ, and Sθ, and 65 is a sensor signal processing unit that processes the signals from the ultrasonic sensor 7 as detailed in FIG. Detection signals PSX, ,PSY
, PSZ is subjected to dead zone processing, multiplied by a gain, and outputted to each servo controller 61 to 64.

Reference numerals 110 to 140 are current detectors, which detect the actual current 1x-10 flowing through the motors 11 to 14 of the corresponding axis and feed back the current to the servo controllers 61 to 64. Reference numerals 111 to 141 are tacho generators (speed detectors). ), and the actual speed Vx of the motors 11 to 14 of the corresponding axis is
~ ■ θ is detected and the speed is fed back to the servo controllers 61 to 64. Reference numerals 112 to 142 are encoders, which are composed of rotary encoders, and are rotated every predetermined angle to detect the position of the motors 11 to 14 of the corresponding axis. The servo controller 61 generates two-phase detection pulses.
~64 for position feedback. Note that these are internal sensors of the servo system. 66 is a data bus for exchanging data between the CPU 60 and each of the servo controllers 61 to 64 and the sensor signal processing section 65; 67 is an address bus for transferring data from the CPU 60 to the servo controllers 61 to 64 and the sensor signal processing unit 65; It sends an address to the signal processing section 65.

Therefore, the CPU 60 selects the sensor signal processing section 65 via the address bus 67, sets the dead band width W1 gain α via the data bus 66, and
4 and set the current feedback gain to 1 via data bus 66.
, set the speed feedback gain to 2 and the position feedback gain to 3, select the servo controllers 61 to 64 of the required axis via the address bus 67 according to the teaching data, and transfer the displacement to the servo controllers 61 to 64 via the data bus 66. Directive X
Set C% Y C% Z c, θC. Further, the current position of each axis is read from each servo controller 61 to 64 via the data bus 66.

Each servo controller 61 to 64 has a displacement command Xc −,
Y c % Z c s θC and sensor signal processing section 65
The motors 11 to 14 of the corresponding axes are servo-controlled by combining the state quantities SX, SY, SZ, and Sθ of each axis.

FIG. 5 is a detailed circuit diagram of the sensor signal processing section 65 configured in FIG. 4.

Although FIG. 5 only shows the X-axis component from the sensor, the Y, Z, and θ axes also have the same configuration.

In the figure, 65a is a dead zone processing section, which performs non-contact processing called a dead zone on the signal from the sensor and outputs it.When the absolute value of the input is smaller than the dead zone 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, when the human power is a positive value, (large car dead band width setting value) is output, and when the input is a negative value, (input + The dead band width setting value) is output.

65b is a gain multiplier which multiplies the output of the dead zone processor 65a by a gain α and outputs the state quantity SX.

650 is an analog latch circuit, which, as detailed in FIG. 6, has a chip select section connected to the address bus 67, a data register connected to the data bus 66, and the contents of the data register by the chip select section of the chip select section. and a digital/analog converter (hereinafter referred to as a D/A converter) that converts the digital output of the launch section into an analog quantity. It is latched, converted to analog dead band width, and output. 651 and 652 are inverting amplifiers, and the inverting amplifier 651 has a dead band width W of the analog launch circuit 650.
The inverting amplifier 652 inverts the output of the inverting amplifier 651 and outputs it to the adding amplifier 654 (described later). It is a summing amplifier, and the output of the ultrasonic sensor, psx, and the input dead band width W,
655 is a polarity determination unit that performs addition with -W;
Determine the polarity of the output LX of the non-contact sensor, and set u according to the polarity.
Those that output p instruction/DOWN instruction, 656a, 65
6b are analog switches, and summing amplifiers 6
53. The output of 654 is UP/DOW of the polarity determination unit 655.
It is output in response to the N instruction.

657 is an analog latch circuit, and a dead band processing section 65
It has the same configuration as the analog latch circuit 650 of C.
A device that latches the gain α from the PU 60 and outputs it as an analog quantity. 658 is an analog multiplier that multiplies the output of each analog switch 656a, 656b by the gain α of the analog latch circuit 657 and outputs the result.
Reference numeral 9 denotes a voltage/frequency converter (referred to as a V/F converter), which outputs a pulse having a frequency corresponding to the absolute value of the output (voltage) of the multiplier 658 as a state quantity SX.

FIG. 6 is a detailed circuit diagram of the servo controller.

Although only the X-axis servo controller 61 is shown in FIG. 6, other Y, Z, and θ-axis servo controllers 62 are shown.
-64 also have the same configuration.

In the figure, the same parts as those shown in FIG. .612 and a data register 614.615.61 connected to the data bus 66.
6, and the data register 614.615.61 according to the selection instruction of the chip select section 610.611.612.
Launch part 617.618.619 that latches the contents of 6
and a D/A converter 621. which converts the contents (digital values) of the latch sections 617, 618, and 619 into analog quantities.
622 and 623, the analog latch circuit 611 latches a current feedback gain of 1 and outputs it as an analog quantity, and the analog latch circuit 61V latches a speed feedback gain of 2 and outputs it as an analog quantity. The analog launch circuit 61P latches a position feedback gain of 3 and outputs it as an analog quantity.

61a is an encoder processing circuit, and the encoder 112
61b is a current position counter which determines the rotation direction from the two-phase position pulses from the encoder processing circuit 61a and outputs a four-fold position pulse, and 61b is a current position counter that counts up or down the position pulse according to the rotation direction determined by the encoder processing circuit 61a. The item indicating the current position Xp, 61C, is a D/A converter,
61M is a displacement command generation circuit that converts the current position Xp of the current position counter 61b into an analog current position Xp, and a chip select section 613 connected to the address bus 67 and a data A register 625, a launch section 620 that latches the contents of the data register 625 according to a select instruction from the chip select section 613, and a number corresponding to the contents of the launch section 620.
It has a pulse generator 624 that outputs displacement command pulses Xc with a cycle corresponding to the maximum acceleration, latches the displacement command Xc instructed by the CPU 60, and outputs displacement commands of a number corresponding to the value of the displacement command with a cycle of the maximum acceleration. It generates a pulse Xc.

61g is a pulse addition circuit which switches and outputs the displacement command pulse Xc to the up side or down side according to the windability of the displacement command sent to the latch circuit 620, and outputs the displacement command pulse Xc from the sensor signal processing unit 65 ( Pulse) SX is switched to the up side or down side in response to the UP/DOWN instruction from the scratchiness determining section 655 (FIG. 5), and 61h is t.
This is a command position snow counter, which counts up the pulses on the 7 knob side of the pulse addition circuit 61g and counts down the pulses on the down side to create a composite command position Xc, which is then applied to the D/A.
It is something to convert. 61d, 61e, 61f,
61i is an analog multiplier, and the multiplier 61d is the real current X from the current detector 110 and the analog latch circuit 61.
A multiplier 61e that multiplies a current feedback gain of 1 by 1.
is the one that multiplies the actual speed Vx from the speed detector 111 and the speed feedback gain of the analog launch circuit 61p by 2, and the multiplier 61f multiplies the current position Xp of the D/A converter 61 and the position feedback gain of the analog latch circuit 61p by 3. The multiplier 61i multiplies the composite command position X'c of the command position counter 61h and the position feedback gain of the analog latch circuit 61p by 3, R1-R4 are addition resistors, and each multiplier This is to add the outputs of 61d, 61e, 61f, and 61t. Therefore, the addition output U is (k3
・3 to X'c - 2 to Xp- ・Vx-kl ・l
x), and shows the format of FIG. 1(B).

61h is a power amplifier section, which includes an amplifier AMP that amplifies the addition power U and a pair of transistors TR1 and TR2; if the addition power U is positive, the transistor TR2 is turned on, the drive current ib is passed to the motor 11, and the addition If the human power U is negative, turn on the transistor TR1, and drive current ia in the polar direction
is supplied to the motor 11.

(C) Description of the operation of the control unit■ The CPU 60 first sets the current, speed, and position feedback gains to 1, k2, and k3 to each analog launch circuit 611, 61.
V and 61P are selected by the address bus 67 and sent out from the data bus 66 for launch. Further, the dead band width W and gain α are similarly set in the analog launch circuits 650 and 657 of the dead band processing section 65a and the gain multiplication section 65b.

■ Next, the CPU 60 sends the displacement command Xc to the data bus 66.
It is latched by the displacement command generation circuit 61M via. As a result, the pulse generator 624 outputs a displacement command pulse Xc at the frequency of the maximum acceleration and in accordance with the displacement command Xc, and the amplifier side of the pulse addition circuit 61g ( ■)
Or command position counter 61h from the down side (in case of ○)
is output to. The command position counter 61h counts up the pulses from the amplifier side and down counts the pulses from the down side, so if the displacement command Xc is in the e direction,
The displacement command pulse Xc is amplified and counted.

Therefore, the additional human power u (X) is (k3 ・X'c −
k3 · drive in the direction.

During this period, while the sensor signal processing section 65 does not issue the X-axis state quantity SX, the X-axis motor 11 is servo-controlled in accordance with the displacement command Xc.

■ On the other hand, in the sensor signal processing section 65, the ultrasonic sensor 7
In the dead zone processing unit 65a, the polarity determination unit 655 determines the polarity of the output psx of UP/DOWN (negative/
Correct) Emit output. In addition, the output PSX is output from each adding amplifier 65.
3.654 dead band width -w, w and additional sale, -(FSX
-W) and -(FSX+W) are output. Output FSX
When psx is positive, the analog switch 656b is selected, and when the output PSX is negative, the analog switch 656a is selected. Therefore, if psx is positive, (-FSX+W), PS
If X is negative, (PSX-W) is output to the multiplier. This dead band processing prevents the non-contact sensor from emitting an output if the output is less than W, thereby preventing the generation of a state quantity SX due to a minute output due to a characteristic error of the non-contact sensor. This enables stable external state feedback operation even when the external state fluctuates.

On the other hand, the outputs of the analog switches 656 a and 656 b are input to a gain multiplier 65 b, multiplied by a gain α in a multiplier 658, and input to a V/F converter 659.

Therefore, the input of the V/F converter 659 is α・IPSX−
Wl, and the V/F converter 659 inputs a pulse with a frequency corresponding to this input value to the pulse addition circuit 61g of the servo controller 61 as the state quantity SX.

■ Therefore, the output PSX from the sensor 7 as shown in Figure 3 (in the figure, it is PSY in the Y-axis direction, but the same goes for the X-axis direction)
is generated as positive, the V/F converter 659 generates a frequency state quantity SX corresponding to the value of α・IPs
) UP/DOWN output, in this case the DOWN instruction and state quantity pulse SX are output to the down side, thus causing the command position counter 61h to count down.

As a result, the composite displacement command Xc of the command position counter 61h decreases, so that the additional human power u (X) decreases or becomes negative, and the motor 11 is decelerated and stopped.

Therefore, if the moving direction in FIG. 3(A) is the +X direction, the positive output PsX of the ultrasonic sensor 7 causes the displacement command (P
-Po) decreases and thereby decelerates (or stops).Conversely, for movement in the -X direction, the ultrasonic sensor 7 outputs a negative detection output ps in the 6X direction.
Since x occurs, the vehicle decelerates and stops in the same way.

Similarly, for the Y and Z axes, each servo controller 62.6
3, deceleration and stopping are performed according to the YSZ-axis state quantities sy and sz of the sensor signal processing section. Note that since the θ-axis direction is not detected, the θ-axis state quantity Sθ is not generated.

In this way, the energy of the non-contact sensor is taken into the control system, and motor control, that is, movement control, is performed according to the output of the sensor. As this non-contact sensor, if a sensor for measuring temperature with respect to a heat source is used, the vehicle can be moved without approaching the heat source, and if a linear region is used, smooth deceleration is possible.

(d) Description of other embodiments FIG. 7 is an explanatory diagram of another embodiment of the present invention, in which the same parts as shown in FIG. 2 are indicated by the same symbols. is a target object, which is held by the hand 5.

In the example of FIG. 7, at the indicated speed ■Cz of FIG. 7(B),
As the hand 5 approaches the object 8 in the X-axis direction, the ultrasonic sensor 7 generates an output PsZ and decelerates.
Hand 5 stops at VCz=α·PsZ. Therefore, the hand 5 stops in front of the object 8 or lightly contacts the object 8, and whether to stop in front of the object 8 or to stop in contact with the object 8 can be executed by changing the gain α.

FIG. 8 is an explanatory diagram of another embodiment of the present invention, showing an example in which a pair of robots performs work by cooperative operation.

In the figure, the second object 8b gripped by the hand 5b is moved by the arm 3b of one robot to the first object, which is left still.
8a of the other robot, and then the other robot's AJA.
3b, the third object 8c grasped by hand F5a
An example of fitting the second object 8b into the second object 8b is shown.

Even in such an operation, one robot uses the hand 5b to transfer the second object 8b to the first object 8a, as shown in (3).
After fitting, during the return operation ■, even if the other robot is transferring the third object 8C with its hand 5a as shown in ■, the ultrasonic sensor 7a detects that the hand 5b of one robot is in the command path. When the hand 5b comes off the command path, the hand 5a of the other robot can immediately insert the third object 8C. In other words, the other robot can work without having to wait for the return operation of one robot to complete.

This allows for cooperative behavior without requiring complete synchronization of the two-mouthed bots, which can be performed in loop processing within each control system itself.

In the above example, we used a 4-axis Cartesian robot as an example, but the number of axes is not limited to 4, and may be one or multiple axes, and may be an articulated rotating coordinate robot. There may be.

Further, the present invention is applicable not only to robots but also to any moving body, and may be a rotating body such as a θ-axis, for example.

In the above example, a DC motor is used as the transportation means, but
Other actuators such as a pulse motor or an AC motor may also be used. Furthermore, although an ultrasonic sensor has been described as an example of a non-contact sensor, other well-known sensors such as a laser distance meter or a temperature sensor may also be used, and various types may be used depending on the required control. However, it may not be provided on the moving body itself, but may be provided at another observation point.

(e) Description of still another embodiment FIG. 9 is an explanatory diagram of still another embodiment of the present invention, and in the figure, the same parts as shown in FIG. 2 are indicated by the same symbols. , 8 is the target object, which has a hole 8a, 9
It is an object that fits together, is held by the hand 5, and is inserted into the hole 8a.
Those fitted together, 70a and 70b, are photoelectric sensors that emit a detection output PS when an object crosses between the photoelectric sensors 70a and 70b.

In this embodiment, photoelectric sensors 7Qa and 7 are located near the object 8.
0b, and while the hand 5 holding the object 9 is moving toward the hole 8a of the object 8 at a speed Vz in the Z-axis direction, when the object 9 held by the hand 5 crosses between the photoelectric sensors 70a and 70b, As shown in Figure 9 (B), the Z-axis state quantity S is α・PS.
Z is generated, thereby reducing the speed Vz of the hand 5, softening the impact between the objects 8 and 9, and making it possible to fit them into the hole 8a.

In this case, the gain α may be set so that Vz>α·PS.

FIG. 10 is an explanatory diagram of still another embodiment of the present invention. In the figure, the same parts as those shown in FIG. 9 are indicated by the same symbols, and 71 is a proximity switch, As shown in FIG. 10(B), a nonlinear output PS of a multidimensional curve is generated depending on the proximity state to the object 9.

In the embodiment shown in FIG. 10, as the hand 5 approaches the object 8 in the Z-axis direction, the proximity switch 71 generates a nonlinear output PS, and the hand 5 stops at Vz-α·PS. Therefore, the hand 5 stops in front of the object 8 or in contact with the object 8, and whether to stop in front of the object 8 or to stop in contact with the object 8 can be executed by changing the gain α.

In the above example, a 4-axis Cartesian robot was explained, but the number of axes is not limited to 4, and may be one axis or multiple axes, and may be an articulated rotating coordinate type robot. It's okay. Furthermore, the present invention is applicable not only to robots but also to any moving object, such as a rotating object such as a θ-axis.

In the above example, a DC motor is used as the transportation means, but
Other actuators such as a pulse motor or an AC motor may also be used.

Furthermore, although non-contact photoelectric sensors and proximity sensors have been described as non-linear sensors, other non-linear non-contact types may be used, and various non-contact sensors can be used depending on the application to the moving object. can.

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, the environmental state detection signal of the non-contact sensor can be used as a state quantity and incorporated into the control system to control movement. The present invention has the advantage that the movement control can be changed according to the operating environment, and it also has the effect that the load on the control section can be reduced and adaptive control of the operating environment can be performed.

[Brief explanation of drawings]

Fig. 1 is an explanatory diagram of the principle of the present invention, Fig. 2 is an overall configuration diagram of an embodiment of the invention, Fig. 3 is an explanatory diagram of the operation of the configuration in Fig. 2, and Fig. 4 is a diagram of the control section of the configuration in Fig. 2. 5 is a detailed circuit diagram of the sensor signal processing unit configured in FIG. 4, FIG. 6 is a detailed circuit diagram of the servo controller configured in FIG. 4, and FIG. 7 is an explanatory diagram of another embodiment of the present invention. , FIG. 8 is an explanatory diagram of another embodiment of the present invention, FIG. 9 is an explanatory diagram of still another embodiment of the present invention, and FIG. 10 is an explanatory diagram of still another embodiment of the present invention. In the figure, M--mobile body MT--moving means NS--external non-contact sensor CT--control means.

Claims (1)

[Claims] A moving means for moving a moving object, an external non-contact sensor for monitoring the operating environment of the moving object, and a method based on a synthesis of a state quantity and a movement command based on the output of the external non-contact sensor. A moving object control device, comprising: a control means for controlling the movement of the moving means, and controlling the movement of the moving object by combining the state quantity and the movement command.