JPS61156413A - Automatic follow-up device - Google Patents

Abstract

PURPOSE:To attain the automatic follow-up control by obtaining the distance and the relative angle to a target synchronously with a fixed cycle based on the output of each wave receiver for the impulsive ultrasonic wave sent cyclically from the target and controlling a shift mechanism according to said distance and relative angle. CONSTITUTION:The impulsive ultrasonic waves (a) are delivered from a ventilator 1a of a traveling target TAG1, and a projector 1b repeats emission of light synchronously with the delivery of the waves (a). The wave receivers S2-S-2 are distributed with prescribed spaces on a horizontal straight line, for example, at the side of a robot ROB2 for reception of the waves (a) and produce wave reception output (c)-(g). These outputs are given to an RX3 and this RX3 transmits the time difference pulses (h)-(m) of the prescribed width based on a synchronizing pulse (b) given from a CPG4 which produces clock pulses CLK. The pulses (h)-(m) are applied to each counter of a counter part CUT6. Thus a counting action is started for pulses CLK according to the pulse (b), and these pulses are given to an ALU7 after conversion into each count value. Thus the pulses CLK are averaged to obtain the distance and the relative angle to the TAG1. These distance and angle are given to a control part 8.

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial Application Field] The present invention relates to an automatic device such as a robot that follows a target object while maintaining a fixed distance according to its movement.

[Conventional technology]

In recent years, various robots have been developed for exhibitions, industrial use, etc., and are gradually being used for various purposes.
There is a need for automatic equipment for transporting various items.

[Problem that the invention seeks to solve]

Therefore, one aspect of the present invention is directed to a moving target object.

The present invention provides an automatic device that automatically follows and moves while maintaining a fixed distance.

[Means for solving problems]

In order to solve the above-mentioned problem, the present invention is constructed by the following means.

That is, a plurality of receivers each individually receiving pulsed ultrasonic waves transmitted from a moving target object at a constant period;
a calculation unit that calculates the distance and relative angle to the target object in synchronization with a certain period based on the valley received wave output of each of these receivers;
It may be equipped with a control section that controls the moving mechanism based on the distance and relative angle and performs tracking control to maintain a constant distance from the target object.

[For production]

Therefore, pulsed ultrasonic waves transmitted from a moving target at regular intervals are individually received by multiple receivers, and the distance and relative angle to the target are determined according to the received wave outputs. The control unit controls the movement mechanism according to the result, and performs tracking control while maintaining a certain distance from the target object, and the tracking movement is automatically performed according to the movement of the target object. It is done.

〔Example〕

Hereinafter, the present invention will be explained in detail with reference to figures showing examples.

FIG. 1 is a block diagram of the entire configuration, and FIG. 2 is a timing chart showing waveforms at the troughs in FIG. A pulsed ultrasonic wave (-) is repeatedly transmitted from the transmitter 1a at a constant period t3 of, for example, 5Qmasc, and the projector 1b also repeatedly emits light in synchronization with this. .

On the other hand, robots (hereinafter referred to as
On the ROB) 2 side, receivers S3 to S-1 are arranged, for example, on the same straight line in the horizontal direction at predetermined intervals, and these receive the ultrasonic waves (,) individually. Wave outputs (C) to (g) are generated, and these are given to the receiving section (hereinafter referred to as RX) 3, which generates a synchronizing pulse (b) and a clock pulse αX synchronized with a constant period t3 on the TAGl side. Based on the synchronization pulse (b) from the generated clock pulse generator (hereinafter referred to as CPG) 4,
Time difference t8-1- from transmission to reception of ultrasound (a).

The time difference pulse (h) ~←) with a pulse width according to RX3
is to be sent.

It should be noted that each of the received wave outputs (c) to (2)) is received at the time when each reaches a predetermined level L, and in this example, the wave receiver S. It is assumed that TAGl exists on the straight line in front of .

Also, immediately before TAGI performs guidance control of ROB2, the light emitted from the emitter 1b is applied to the receiver 5 on the ROBZ side, thereby setting the CPG4 in a state synchronized with the period t8. , TAGl and C
Since the original oscillation is performed in the PG4 by a highly stable crystal oscillator or the like, the synchronized state between the two is maintained for a certain period of time, for example, 30 minutes, which is not a problem in practice.

The time difference pulses (h) to (e) are each given to each counter (hereinafter referred to as CUT) 6, and the clock pulse C is given to each counter in accordance with the synchronization pulse (b).
LK counting starts and 1 time difference pulse (g))~-
Each pulse width period of is converted into a count value of the clock pulse CLK, and then
? given to.

Then, the ALU 7, which consists of a processor such as a microprocessor, a memory, etc., starts interrupt processing in response to the synchronization pulse (b), and performs interrupt processing based on each receiver S, l-S-, and the corresponding count value. , performs calculations according to the clock pulse CLK, and calculates the individual distance to TAGI according to any one pair combination of each count value, and
Average these to find the distance to TAGl, and
The relative angle with AGl is determined, and the result is sent to a control unit similar to ALtJ7 (hereinafter referred to as "control").

CNT) 8.

Therefore, the CNT 8 makes judgments regarding travel control according to the distance data and angle data from the ALU 7, and drives the push-bo motors (hereinafter referred to as SM) 10L and 10R via length-bore drive circuits (hereinafter referred to as SDR) 9L and 9R. each of the left and right driving wheels 11L, which are controlled and individually connected to these;
11R is driven, and the ROB2 is made to travel in a direction in which the distance from TAGI is a constant distance of, for example, 2 m, and at a speed corresponding to the moving speed of 'l'AG1.

In this example, the movement mechanism is SMI QL, 10
R and driving wheels 10L and 10R, and the required number of wheels are separately provided to change direction depending on the driving conditions of the driving wheels 10L and 10H, but the moving mechanism uses various engines, crawlers, and walking legs. etc. are combined and configured according to the conditions, and 5DR9L, 9R are constructed accordingly.
Just replace it with a suitable one.

In addition, the time difference pulses (h) to (e) are the received wave outputs (,)
The same applies to the case where a differential pulse is sent out when .about.(g) reaches a predetermined level L, and the CUT 6 thereby stops counting.

FIG. 3 is a block diagram of the TAGl side, in which an oscillator (hereinafter referred to as OSCT) 21 using a highly stable crystal oscillator or the like performs the primary oscillation of, for example, IMH, and its output is sent to a frequency divider such as a counter ( After dividing the frequency by DVT (hereinafter referred to as DVT) 22 to obtain a synchronizing pulse with period t, the pulse generator (hereinafter referred to as PGT) 23, such as a monostable multivibrator, which generates a pulse with a pulse width suitable for driving the projector 1b is driven. , the output of this is applied to the light emitter 1b via the gate G0, which is turned on when the power is turned on, to cause it to emit light with a period t.

In addition, the output of DvT22 is a PGT similar to PGT23.
24 is also driven, and this generates a pulse having a pulse width t of, for example, 500 μsec at a period t, and applies it to one input of the piece gate G, and the other input receives an ultrasonic wave. Since an output with a frequency of 40KH, for example, is given from the oscillator (hereinafter referred to as 880) 25, this output passes through the gate G in accordance with the output of the PGT 24, and is transmitted via the power amplifier (hereinafter referred to as PA) 26. Transmitter 1
a, thereby repeatedly transmitting pulsed ultrasonic waves with one period t.

On the other hand, a timer (hereinafter referred to as TM) 27 using a counter or the like is operating based on the output of I) VT 22.

When this time-up occurs after a set time of 10 minutes, for example, this output resets the power supply control circuit (hereinafter referred to as PC) 28, automatically cuts off the power supply to each part, and lights up the light emitting diode LD to indicate that the power is on. The lights are also turned off.

In addition, when turning on the power, if the power switch SW is turned on, the power supply vc applied from the battery via the resistor R3 is turned on.
c is grounded, and in response, a re-trigger prevention circuit (hereinafter referred to as PTI) 29 using a flip-flop circuit etc. is set, so this output powers on the PC 28 and starts after TM27 is cleared. Meanwhile, AND gate Gl is turned on.

Furthermore, once the RTI 29 is sent, it remains set until the PC 28 is reset and the power is turned off, thereby preventing re-triggering due to erroneous operation of the switch SW.

Therefore, the battery is prevented from being consumed more than necessary, and efficient operation can be achieved.

FIG. 4 is a diagram showing the mounting situation of the transmitter 1a, where (B) is a side view, (B) is a front view and (B) is an actual size diagram.
The transmitter 1& is fixed onto the base plate 41 via the inclined mounting plate 42, and its pointing direction is set approximately 10 degrees upward from the horizontal direction, and is spaced a predetermined distance from the receivers S, ~S-, of ROB2. The relationship is that they are facing each other.

In addition, mounting holes 43, 44, etc. are formed in the base plate 41 to facilitate mounting on a moving body, but when used by a human, the transmitter 1a should be protruded from the back of a jacket, etc. A through hole is provided, and a pocket for accommodating the base plate 41 is provided on the inner side of the hole, so that the shape and size are convenient for holding the entire body.

Note that a lead wire 45 is drawn out from the wave transmitter 1a, and a plug (not shown in the figure) is connected to the tip of the lead wire 45, thereby establishing connection with the main body.

Fig. 5 shows the external appearance of the main body in y actual size, 0 is a plan view, (B) is a side view, and (0 is a front view. Each part shown in the figure and a battery are accommodated, and an angular through hole 52 is bored in the upper surface, and the light emitting surface of the projector 1b is arranged inside this hole.

Also, on the side, a non-locking switch SW and a light emitting diode LD are provided which are exposed in a through hole drilled into the button of the switch SW, and a connector 53 into which a plug from the wave transmitter 1a is inserted. The cover can be freely removed by removing the screws 54, etc., making it possible to replace the battery and inspect the inside.

Since the entire device is small and lightweight, it can be easily stored in a jacket pocket, etc., and is manufactured in a shape that does not require any particular storage space.

FIG. 6 is a flowchart of the operation performed by the ALU 7, in which the process executes instructions in the memory and accesses necessary data to the memory.

That is, the interrupt processing “INT
For example, 25 seconds until the count value becomes stable.
After performing “signal settling time standby” 101 defined as mm5+e, “cty” r is performed by “input processing” 102.
Each count value from s, BUSY signal, and CUT
6 and stores it in the memory.

Note that the abnormal signal is sent when the received wave outputs (C) to (g) from the receivers S and S-3 do not occur, and the abnormal signal is BUSY.
The signal is sent when the CUT 6 continues counting even after the signal settling time has elapsed.

Next, in response to the abnormal signal, "Receiver abnormality?" 111 is determined, and in response to the BUSY signal, "BUSY・3C" is determined.
More than H? ” 112, and if both are No, perform the “Basic data/validity check” 121 described below according to the normal count value, and perform the “Distance/Angle check” described later using only valid basic data.・Perform operations” 122 and output these results to CNT8 through “output processing” 131.
Send to.

Then, in either step 111 or 112, πS
If so, the process moves to "abnormality processing" 141, where a warning is displayed on a display section (not shown in the figure) via X8, and control to stop the vehicle running is performed.

Therefore, according to steps 111 and 112, the receiver S
, ~s, ~5CH (channel) by CUT 6
If up to 3 channels in the receiving system are normal, step 121
The following steps are executed, and a highly reliable and accurate measurement result is obtained by the "validity check" in step 121.

Note that the “validity check” is based on the time difference pulse (g)
This is to judge whether each value according to ) is reliable or not, and is performed by calculations described later.

Figures 7 and 8 are principle diagrams for determining individual distances and angles.As shown in Figure 7, receivers S, ~S-3 are
If they are arranged at intervals on the axis and the facing distance between each of them and TAGl is R8~Ll, then the receivers S, ~5
-1l Any two of them are considered as a pair, and each and TAG
When the facing distance from the receiver is R1, Rj, the respective X-axis coordinate positions of the pair of receivers are D-i, D- as shown in FIG.
j and the central receiver S. is the origin O, and the individual distance between this and TAGl is R-B, and
The distance between the y-axis perpendicular to the origin 0 and TAGl is X. , T.A.
The distance between Gl and the X axis is y. Then, the following equation holds from the Pythagorean theorem.

R13=(xo-D-1)8+yo''...(1
)Rj2=(xo-D-j)2+yo'10101. (
2) Rlj ” xo' ” )'o”
......(3) Therefore, the following equation is obtained from the equations (1) and (2+).

Also, the following equation can be derived from equation (2) and +31.

R"1j=R"j(I)j)" + 2(I)j)x
” (5) If we substitute equation (4) into equation (5).

4-n11. ! -4-0,,IQs Tasoshi, ΔR"Rj-R1 Transforming equation (6):

In this, Also.

Then, equation 18) becomes the following equation.

In this case, ΔR=Rj-R1, so if R1 and Rj are found based on the time difference pulses from any one pair of receivers S, ~s-3, then 19)
The individual distance *aij is obtained from the formula, and this is sent to the receiver 82~
If the average value is calculated using a linear equation after each vibration of s-3, the distance R to TAGl can be obtained accurately and with high reliability.

Here, N is the number of individual distances R1j determined by each pair of receivers S, ~S-,.

Note that the same effect can be obtained even if the average value is obtained by weighted averaging of Equation 11.

On the other hand, the relative angle θij of TAGl with respect to the y-axis with respect to the origin O shown in FIG. 8 is determined by the following calculation.

In other words, θij is t41, (substituting equation 51 into equation (11),...
...(12) In this\, if θ1j<<1.

...(13) Therefore, if R1 and Rj are found in the same way as in the case of the t1c formula, the angle θij is found from the formula (13), and this is calculated from each of the counter vibrations of the receivers S and ~S-3. By averaging each angle, accurate and reliable relative angle θ can be obtained.
is required.

In other words, (Similar to formula 11, The relative angle θ can be found by

Note that the same effect can be obtained by calculating the average value using weighted averaging in equation (14).

FIGS. 9 to 11 are diagrams of the principle of validity checking. Based on FIG. 7, if any pair of opposing distances R2 to R2 to circle and inside are R1 and Rj, then FIG. Therefore, the following relationship exists between the receiver spacing +t-jlD and R1, Rj.

l R1-Rjl≦I i-j ID...
...(21) In this \, the equality sign in equation (21) holds true for D-i, D-j,! : This is the case where TAGI and TAGI exist on a straight line, and considering the condition of the following formula with variable k being O≦≦1, I Rs −RjI = It−jlD−k ・・
(22) When i and j in equation (22) are each set to fixed values, a hyperbola with k as a parameter is drawn as shown in FIG.

In addition, as shown by the broken lines in Figures 10 and 11, the measurement range of TAGl is 1 m to +1 m from the y axis in the direction along the X axis, and 1 to 4 m from the X axis in the direction along the y axis.
If we constrain it as m, the maximum value of is.

It is a function of the distance R from the origin O to TAGl.

In addition, in Figure 9, 1. for.

0<k,<k! The relationship is <1.

On the other hand, as shown in FIGS. 10 and 11, if the measurement range of TAGl is determined according to the above-mentioned constraints, k in equation (22) becomes the maximum value l, j due to distance R.

The coordinate positions Tx and Ty of TAGI are expressed by the following equation.

In the case of Fig. 10 In the case of Fig. 11 In equations (23) and (24), the value symmetric with respect to the y axis is i
.

j + Tz are shown, Ty are equal, and k is equal.

When ('l+T'z, Ty)=(2+1+t, ffπ) is expanded, the following equation is obtained.

IR, -Rj and R2-R engineering ...(25) Also.

R2-R.

(2-1)D However, the distance R is ultimately determined, and when checking the validity, T
The minimum value RytIN that is not unreasonable within the measurement range of AGl may be determined, and this value may be used in place of R to perform kt-calculation using the following equation.

Therefore, by introducing k into equation (21) and taking into account the tolerance α, check the conditions of the following equation for each of the 10 combinations of each pair of receivers 82 to S-2. However, if this condition is satisfied, each facing distance R1 as basic data
, Rj have validity, but if there is no nickname, it can be determined that there is no validity.

l R1-Rjl < l i -j l D ”k+
α ・・・・・・(28) Also, check the formula (28), and if it is valid, convert it to the value of CB = 1 if C1j = O1 is not present, and convert these to the value of CB = 1 as shown in the table below. After storing it in the memory as , and summing up each position in the horizontal direction, if this is 3 or more, the facing distance R obtained from the corresponding receiver is invalidated and it is not used in subsequent calculations. As a result, the reliability of the distance R that is finally determined can be ensured.

Then, create the above table according to the number of receivers S2 to S-2, and set the limit value for determining the use or non-use of each total value ΣC1j by 3.
In addition to this, it may be selected as appropriate.

Therefore, by performing one or more validity checks, high reliability of the measured distance can be obtained, accuracy is also improved, and tracking control of the ROB 2 is performed safely and accurately.

FIG. 12 is a block diagram of the CPG4, in which a stable oscillator (hereinafter referred to as 08CR) 61 such as a crystal oscillator provides
Like the 08CT21 shown in the figure, a clock pulse CLK of, for example, IMH is generated, and this is sent to the first and second frequency dividers (
Hereinafter, DVR) 62 and 63 divide the frequency by the same frequency division ratio, and the period t is the same as the period t in FIG. 3.
, and each D'vR 62, 63 includes a setting section (hereinafter referred to as DST) 64 for setting data, and a down counter (hereinafter referred to as DcT). 65, 66 and Nior Kate 67.68! Each #(7)D
CT65.66 has its own count value zero output R, C1
or.

When start pulse ST6 or preset pulse PR is applied to the load terminal via OR gate 67 or 68, data from DST64 is preset and subtraction is started based on clock pulse CLK from 08cR61. Each synchronization pulse SL, 8YN by pulse “ST” or preset pulse PR
It is possible to set the synchronization status of.

Further, the light receiving pulse opening corresponding to the light emission with the period t3 shown in FIG.

Only the first pulse is selected in TRG) 71, and a start pulse "total pulse" which is synchronized with the clock pulse CLK and has the same width as the period of the clock pulse CLK is accordingly selected.
Since only one pulse is sent out from the beginning, the DCT 65 stably shifts to a synchronized state with the received light pulse "Ichigo", and the same trigger pulse is repeatedly sent out in response to the received light pulse. The sending of the start pulse ST is based on the restart pulse R.
If 8 is given, similarly, only one pulse is sent out repeatedly.

The trigger pulse and the local synchronization pulse (■) are given to the phase difference detection section (hereinafter referred to as PDT) 72.
, the phase difference between the two is detected, and if the phase difference is within a predetermined value, only one pulse of the load pulse field is sent out, and the detection pulse synchronized with the trigger pulse bottom δ is repeatedly sent out. When the phase difference exceeds a predetermined value, the detection pulse box is not sent out, and the load pulse is repeatedly sent out in synchronization with the trigger pulse 〒δ.

These pulses E and fans are applied to a continuous detection unit (hereinafter referred to as CDT) 73 mainly consisting of DCT, and a constant value is preset to the DCT in CDT 73 according to the load pulse m, and the detection pulses are Therefore, the subtraction is performed, and when the count value becomes zero, the detection completion pulse OK and LD are sent out, and the detection completion pulse surface and the load pulse LD are input to the input inversion type OR gate 7
4 is applied to the TRG 71 as a restart pulse R8, while a detection completion pulse OK is applied to a gate section (hereinafter referred to as GAT) 75 to turn it on.

However, GATT5 turns on and generates the local synchronization pulse S.
If only one pulse of L is passed through and sent out as a preset pulse PR, it will turn off until the next detection completion pulse OK is given, and the preset pulse PR will not be sent out continuously.

In addition, TRG71, PDT72, CDT73
An initial reset pulse "K" which is generated in response to power-on is applied to the controller, and the initial state is set by this pulse.

Therefore, even if the received light pulse -LS'' is continuously given, only one start pulse is sent in response to the first pulse, and accordingly the DVR 62 becomes synchronized with the received light pulse m, and the received light pulse If the phase difference between the trigger pulse ■ synchronized with τ and the local synchronization pulse π is within a predetermined value, the CDT73 will subtract from the constant value preset by one load pulse 100 according to the number of repeated detection pulses. Since the detection completion pulse OK and OK are sent when the subtraction is completed, the phase difference is within a predetermined value during a certain period until the subtraction is completed, and the received light pulse and the local synchronization pulse SL are in a synchronous state. Assuming that, the GATT 5 is turned on, the preset pulse PR is sent out, the DvR 63 is brought into the same synchronized state as the DvR 62, and the synchronization pulse SYN synchronized with the light receiving pulse port is sent out.

For this reason, the synchronization pulse SYN does not cause any unnecessary timing changes, and the various parts shown in FIG. 1 that operate in response to this change are in stable operating conditions.

FIG. 13 is a block diagram of 'l'RG71, and FIG. 14 is a timing chart showing the waveforms of each part in FIG. After the initial state of each part is set according to
” (low level), the output of the inverter (hereinafter referred to as IN) 211 becomes “H'° (high level), and accordingly, a D-type flip-flop circuit (hereinafter referred to as FFC) 212 is set in response to the rise of the clock pulse (1) from L"° to ")IN, and in order to make the output Q(e) "H", the FFC 213 is also set and makes the output Q(d) "H". ”, the output Q(e) is set to “L”, and the output (,) is sent out as the trigger pulse TO.

Further, the output (d) is given to the NAND gate 214,
This includes clock pulses (,
) is also given, so depending on L11 of this, NAN
The output (f) of the D gate 214 changes to “Lo”, and
When the output (f) returns to "Hll" at the rising edge of the clock pulse (a) from Lll to "H", the FFC 216 is set and the output Q(g) goes "H".
Accordingly, the output of the NOR gate 217 becomes “I,
It becomes II.

This resets the FFCs 213 and 216, and as a trigger pulse, a pulse that is synchronized with the clock pulse (,) and has the same pulse width as the clock pulse (,) is sent out, and the above operations are performed depending on the received light pulse fan. repeat.

On the other hand, in response to the end of the initial reset pulse (m), the output of the IN221 becomes "LIT", turning on the NOR gate 222 and allowing the passage of the restart pulse Rs (h). FFC213゜2
24 are all reset, the outputs Q(j) and (k) are 'L', and the input inversion type Φ gate 225 is also on, so the trigger pulse 〒6 is passed through, and this is input to lN226. The output <1> is set to "Lll" and sent as a start pulse ST.

Then, in response to the completion of the transmission, the FFC 223 is set, and in order to set the output (j) to "H", the FFC 224 is set according to the rising edge of the next clock pulse (a).
is also set, and the output (k) is set to H''.) 2
By turning off 25, the restart pulse (h)
The start pulse (2) will not be sent until the FFCs 223 and 224 are reset.

FIG. 15 is a block diagram of the PDT 72 and CDT 73, and FIG. 16 is a timing chart showing the waveforms of each part in FIG. 15. FIG. 15 (4) shows a synchronous state, and FIG. ing.

In this case, the trigger pulse TO (a) and the local synchronization pulse 5t (b) are given to an input inversion type OR gate 311, and the rising edge from "Lll" to "H" whichever occurs earlier Depending on the pulse generator (hereinafter referred to as PGR)
) 312 is driven, and the PGR 312 using a retriggerable monostable multivibrator or the like generates a pulse (c) with a time width t according to the setting of the variable resistor RV, and provides this to the FFC 313.

The initial state of PG, 312, FFC 313, 314 is set by the initial reset pulse IR(j), and in (A), the initial state is set to 1 time width t.
&, the phase difference td between each pulse (a) and (b) is small,
Moreover, the trigger pulse pulse occurs earlier, and the PGB 312 is triggered again by the local synchronization pulse that follows it, and the time width of the pulse (C) is extended.

The FFC513 is designed to repeat setting and resetting in response to the rising edge of the pulse (C) from "Lll" to "". When set by the first pulse (C), the FFC513 outputs the output Q(d). In order to set the output Q(e) to "H", the FFC314 is also set by the next trigger pulse 5, and the output Q(e) is set to "H".
Hll, and FFC31 depending on the next pulse (C).
3 is reset, the FFC 314 is also reset in response to the subsequent trigger pulse (,), and the above operation is repeated.

These outputs (d) and (e) are each directly fed to the AND gates 321 and 322 of inverted input type, and are also fed to the gate 32 via INs 323 and 324.
1,322 and MΦge) 32
Since each output of 1,322 is given to the NOR gate 325, the output of the same gate 325 is output (d), (e
) becomes 'H' only when both are "Hll" or "Lll", and this is given to the NAND gate 326, and is inverted by the IN327 to become the output (f), and the NA
Provided to ND gate 328.

Therefore, while the output (f) is "H", the NAND gate 3
28 is turned on, trigger pulse (
a) and output (g), which is the detection pulse P.
It is sent out after repetition as D, but the NAND gate 326
is turned on due to the opposite relationship with the NAND gate 328, so only the trigger pulse (,) before the FFC 313 is set passes, and this output (h) is sent out as the load pulse LD, but after this, the same pulse The fan is not sent out.

Therefore, as long as the phase difference t,1 between the pulses (a) and t(b) is within a predetermined value determined by the time width t, the above state is maintained.

On the other hand, if the time width t1 is set small as shown in (B), the phase difference t exceeds this, and the FFC3
13,314 set and reset are both pulses (a).

(b) is repeated according to NAND/AP')
When 326 is on, the trigger pulse (a) passes, and the load pulse low is repeatedly sent out, but on the other hand,
The output (g) of the NAND gate 328 is fixed to "R", and the detection pulse (2) is not sent out.

Therefore, if the phase difference td becomes an asynchronous state exceeding the time width t1, the state of 1 or more is maintained.

On the other hand, in the CDT 73, the D
In this example, "16" is preset to the CT413 as a constant value, and subtraction is performed according to the detection pulse PD via the IN414, and when the count value reaches zero, it sends out the "r detection completion pulse OK" and , IN 415 sends out the same pulse plane of "L" inverted, and it self presets via the NOR gate 412 to repeat one or more operations.

Therefore, 'in the synchronous state of both pulses (,), (b).

In addition to being reset by the load pulse LD.

If the synchronization state is maintained for a certain period of time until the end of counting corresponding to the period t of the detection pulse PD, the detection completion pulse O
K and OK are sent out, and in the asynchronous state, presetting by load pulses... is repeated, subtraction is not performed, and detection completion pulses OK and OK are not sent out.

FIG. 17 is a block diagram of the GATT5, in which an input inverted arm gate 511 given a local synchronization pulse,
and lN512 are connected in cascade, and initially lN5
When the output of No. 12 becomes "H", the FFC 521 is set and the "Hll" of the output Q becomes 'L' by the IN522.

is applied to the preset terminal PRI of the FFC523, so the F'FC523 also enters the set state, sets the output Q to "Hll", sets the output ζ to "L", turns off the AND gate 511, and resets the FFC521. This state is maintained.

In the above state, when the detection completion pulse OK is given, the FFC313 is reset and the output Q is set to "L°°".
In order to turn on the output Qt-''H1l and turn on the arm gate 511, the subsequent local synchronization pulse π passes through the same gate 511 and is sent out as the preset pulse PR via IN512, while the clear state of the FFC 521 is released. The preset pulse sent out has finished, and
In response to the output of IN512 returning to "", FFC5
21 is set, and the FFC 523 is set in the same way as described above, thereby resetting the FFC 521.
It will be in a standby state.

Therefore, the preset pulse plane is the detection completion pulse O
After K is given, only one pulse is sent out and again,
Sending is not performed until the detection completion pulse OK is given.

As described above, in order to directly receive the ultrasonic waves transmitted from TAGl by the receivers S, ~S-, and measure the distance and angle, the ultrasonic waves are transmitted from the ROB2 side, and the TA of this
Compared to the case of receiving reflected waves from GL, the influence of noise due to changes in ambient conditions is reduced, the signal-to-noise ratio of the received wave output is improved, the measurement situation becomes stable, and multiple reception waves can be received. Vessel S, I-S-.

, the measured values are accurate and highly reliable.
The safety of tracking control is maintained.

However, the number of receivers S, ~S, can be determined according to the conditions, and the settings of each constant and time are the same, and the configuration of each part can be arbitrarily selected according to the situation. Various modifications are possible, such as application not only to robots but also to various automatic devices having a movement function such as self-propelled transport vehicles.

〔Effect of the invention〕

As is clear from the above explanation, according to the present invention, TA
Tracking control corresponding to the movement of the vehicle is performed accurately and with high reliability, making the tracking situation of the automatic device safe, and automatically tracking the lead vehicle, leader, etc. while maintaining a certain distance. The device has been realized and has remarkable effects as an automatic device for exhibition robots, self-propelled transport vehicles, etc.

[Brief explanation of drawings]

The figures show an embodiment of the present invention, Fig. 1 is a block diagram of the entire configuration, Fig. 2 is a timing chart showing waveforms of each part in Fig. 1, Fig. 3 is a block diagram of the TAG side, and Fig. 4 is a block diagram of the entire configuration. Figure 5 is a diagram showing the installation status of the TAG side transmitter, Figure 5 is a diagram showing the appearance of the main body used with the transmitter in Figure 4, Figure 6 is a flowchart of the calculation status, @? Figures 9 and 8 are principle diagrams for determining individual distances and angles, Figures 9 to 11 are principle diagrams of validity checking, and Figure 12 is a block diagram of CPG.
13 is a block diagram of the TRG, FIG. 14 is a timing chart showing waveforms of each part in FIG. 13, FIG. 15 is a block diagram of the PDT and CDT, and FIG. 16 is a block diagram of the 15
Timing chart showing the waveforms of each part in the figure, 1st
FIG. 7 is a block diagram of GAT. 1...TAG (target object), 1a...transmitter, 1b...emitter, 2...ROB (robot), 3...RX (receiving unit), 4... ...C
PG (clock pulse generator), 5... light receiver,
6...CUT (counter group), 7...g (calculation unit), 8...CNT (control unit), 10L, 10R
...SM (Bubo Motor), 11L. 11R...Driving wheel, S, ~S-,...Receiver, R
...Distance, θ...Relative angle. Patent Applicant: Yamatake Honeywell Co., Ltd. Agent: Kazuyoshi Yamakawa (2 e-ji Tame) Figure 5 (A) Figure 6 Tower Figure 9

Claims (1)

[Claims] a plurality of receivers that individually receive pulsed ultrasonic waves transmitted from a moving target object at a fixed period; The vehicle is characterized by comprising: a calculation section that calculates the distance and relative angle to the target object; and a control section that controls the movement mechanism based on the distance and relative angle to perform tracking control to maintain a constant distance from the target object. automatic tracking device.