JPS61201311A - Follow-up control system - Google Patents

Abstract

PURPOSE:To perform follow-up control over a target according to the 1st zone in which the follow-up control over is started and the 2nd zone in which the follow-up control is carried out by determining the 1st zone nearby the target and the 2nd zone nearby and outside the 1st zone. CONSTITUTION:An ultrasonic wave (a) that the moving target (TAG) 1 transmits is received individually by an automatic running robot (ROB) 2 and a reception part (RX) 3 generates time-difference pulses (h)-(m) with pulse width corresponding to time differences between the transmission and reception of the ultrasonic wave (a). Respective pulse width periods of the time-difference pulses (h)-(m) are converted by a counter group 6 into counted values of clock pulses CLK and supplied to an arithmetic part ALU 7. The ALU 7 finds individual distances to the TAG1 according to combinations of couples of the counted values, and those are averaged to find the distance to the TAG 1 and also calculates the relative angle to the TAG 1, and further judges which zone the TAG1 is in. Namely, the ROB 2 starts the follow-up control only when the TAG 1 is in a zone 16 and outside a zone 18.

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Industrial Application] The present invention relates to a follow-up control method for an automatic device that follows a moving target while maintaining a constant distance.

[Conventional technology]

When automatic devices such as robots and self-driving vehicles follow a moving target such as a human body, the distance and relative angle to the target are measured using ultrasonic waves, etc., and the distance and relative angle to the target are measured accordingly. Means for performing follow-up control while maintaining the distance has been developed, and an "Ultrasonic Target Position Measuring Device" (Japanese Patent Application No. 60-5858) filed separately by the present applicant has been proposed for this purpose.

[Problem that the invention seeks to solve]

However, if tracking control is performed simply by keeping the distance to the target constant, at the start of tracking, a means for scanning the surrounding area such as an omnidirectional radar to search for the target is required.
The configuration becomes complicated, and even if a target object or an obstacle exists nearby, it immediately starts moving, causing problems such as becoming dangerous.

Furthermore, if a target object and a plurality of objects equivalent to the target object exist at the same time, a problem arises in which objects other than the target object are mistakenly recognized as the target object and tracking thereof is started.

[Means for solving problems]

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

That is, a first zone for starting follow-up control for the target object is determined near the target object, a second zone for performing follow-up control is established outside the first zone, and a method is applied to each of these zones. Follow-up control is performed accordingly.

[Effect]

Therefore, if there is no target in the first zone, the tracking control will not start, the automatic device will remain stopped, and will start moving only after the target is detected in the first zone, Tracking is performed only while the target exists in the second zone, and if the target deviates from this, tracking is stopped.

〔Example〕

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

FIG. 2 is a block diagram of the entire configuration, and FIG. 3 is a timing chart showing waveforms of each part in FIG. pulsed ultrasonic waves (
a) is a constant period of, for example, 50 m5ec 1. The light is transmitted repeatedly, 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 S2 to S-Z are arranged, for example, on the same straight line in the horizontal direction at predetermined intervals, and these receive the ultrasonic waves (a) individually. and received wave output (C
) to (g) are generated, and these are given to the receiving section (hereinafter referred to as RX) 3, and the fixed period 1 on the TAGl side is
．． synchronous pulse (b) and clockhals C
Clock pulse generator (hereinafter referred to as CPG) that generates LK
Based on the synchronization pulse (b) from 4, RX3 sends out time difference pulses (h) to - with a pulse width corresponding to the time difference t2 to t-2 between the transmission and reception of the ultrasonic waves (&). It has become.

Note that each of the received wave outputs (C) to (g) is received at the time when each reaches a predetermined level L8, but in this example, TAGl is present on the straight line in front of the receiver So. It's Toshide.

Immediately before TAGl performs guidance control of ROB2, it applies the light emitted from the projector 1b to the receiver 5 on the ROB Z side,
This sets CPG4' to a state synchronized with period t], and from this point on, TAGI and CPG4 perform original oscillation using a highly stable crystal oscillator, etc. The synchronized state between the two is maintained for a certain period of time, which does not cause any practical problems.

Each of the time difference pulses (h) to - is a counter group (hereinafter referred to as
CUT) 6, where counting of the clock pulse CLK is started according to the synchronization pulse (b), and each pulse width period of the time difference pulse (h) to - is applied to the count value of the clock pulse CLK. The signal is converted into , and then provided to an arithmetic unit (hereinafter referred to as ALU) 7 .

Then, the ALU 7 consisting of a processor such as a microprocessor, memory, etc. starts interrupt processing in response to the synchronization pulse (b), and starts clock processing based on each count value corresponding to each receiver S2 to S-2. Calculations are performed according to the pulse CLK, and individual distances to TAG 1 are determined according to the combination of any one pair of each count value, and the distance to TAG 1 is determined by averaging these, and
The relative angle with TAG 1 is determined, and furthermore, it is determined in which zone TAG 1 is located in each zone described below. Based on this result, a decision is made regarding travel control, and the data based on this is sent to a control unit similar to ALU 7. (hereinafter referred to as CNT) 8.

Therefore, the CNT8 converts the data from the ALU7 into the number of pulses, applies this pulse train to the servo drive circuits (hereinafter referred to as 5DR) 9L and 9R, respectively, and drives the servo motors (hereinafter referred to as SM) 10L and 10R. the left and right driving wheels 11L, which are controlled and individually connected to these;
11R and the distance from TAG 1 is, for example, 2.
Make j5ROB2 travel in a direction that is a constant distance of 5 m and at a speed that corresponds to the moving speed of TAG 1.

However, in this example, the moving mechanism is 5M10L, 10
It is composed of R and driving wheels 10L and 10R, and the required number of wheels are separately provided to change directions in various directions depending on the driving conditions of the driving wheels 10L and 10R.As a moving mechanism, various engines, crawlers, 5DR9L is constructed by combining walking legs etc. according to the conditions.
, 9R can be replaced with a suitable one.

In addition, the time difference pulses (h) to - are sent out in the form of differential pulses when the received wave output (C) to excellent) reaches a predetermined level L, so that the CUT 6 stops counting. The same goes for things.

FIG. 1 is a diagram showing zones for follow-up control.
TAG 1 via distance t along the direction X immediately before TAG 2
, and has a depth and width of 2·xl in the X direction and 2·yl in the Y direction orthogonal to this in the vicinity of the periphery.
A zone (hereinafter referred to as Zl) 16 is defined, and the depth in the X direction x2@+xzb and the depth in the Y direction 2 are
・Define a second zone (hereinafter referred to as Z2) 17 having a width of y2, and
A third zone (hereinafter referred to as Zs) 18 having a depth in the direction 2 x 3 and a width in the Y direction 2 x y3 is fixed, and a negative value is set, for example, as O x + =0. 2m yl =0.4mx2m
= 1.0 m x zl) = 1.0 m 72 = 1.0 mx
s = 0.05m y3 = 0.05
In addition, Zl-16 starts follow-up control when TAG 1 is present in it, z217 performs follow-up control only while TAG 1 is present in it, and z318:
6 values x for the actually measured distance and relative angle, which is determined as the allowable error range for TAG 1 tracking control.
After adding and subtracting 1 to x3 and "I l-)'s,
z, 16 to 2318 are to be determined.

FIG. 4 is a flowchart of arithmetic processing by the ALU 7, in which the processor in the ALU 7 executes instructions stored in the same memory and accesses necessary data from this memory. .

That is, a distance/relative angle calculation 1101 with TAG 1 is performed based on the data from CUT 6, and depending on the result, 'In TAGeZl? “Judge 102, this is N
If (No), check T1103 after starting 1 follow-up control, and depending on this +7)Y (YES), T'TAG@Z
2nd year? '104, and when this is Y, 'TA
G#Z3 middle school? 〃 is determined, and according to this N, the following speed is changed by 1. According to 111, the traveling speed is adjusted in the direction where the actual measured distance matches the constant distance t, and the upper limit of PID, etc., speed, and acceleration is regulated. After performing 1 control processing calculation 112 such as, 1 output processing 1113 is performed to calculate each driving wheel 11L,
Determine the rotational speed of 11H, and use the data showing this as CNT.
8, and repeat steps 101 and subsequent steps.

Step 102 is Y1 Step 10 only when TAGI Kaz+ 16 is inside or outside Z31B.
5kaN and Natte ROB2 start following, and TAGl z
Only while it is in 217, tracking is performed in the direction where TAG 1 is in 23, and TAG 1 is in z217.
If it deviates from this, tracking will stop.

The details of step 101 are described in the above-mentioned patent application 1986-5.
858.

FIG. 5 is a block diagram of the TAG I side. An oscillator (hereinafter referred to as oscT) 21 using a high-stability crystal oscillator or the like generates a primary oscillation of, for example, I MHz, and its output is sent to a counter, etc. A frequency divider (hereinafter referred to as DVT) 22 divides the frequency by υ to produce a synchronizing pulse with a period t, and then a pulse generator such as a monostable multivibrator (hereinafter referred to as DVT) generates a pulse with a pulse width suitable for driving the projector 1b. PGT) 23 is driven, and its output is applied to the projector 1b via a static gate G1 that is turned on when the power is turned on, so that light is emitted at a period t.

In addition, the DVT22 (7) output is the same as PGT23.
GT24 is also driven, this is for example 500μsec
A pulse having a pulse width t is generated with a period t, and is applied to one input of the AND gate G2.
The other input is given an output with a frequency of, for example, 40 KHz from the ultrasonic oscillator (hereinafter referred to as 5SO) 25, so this output is applied to the AND game according to the output of the PGT 24).
2 and drives the transmitter 1& via a power amplifier (hereinafter referred to as PA) 26, whereby the period t
, pulsed ultrasound waves are repeatedly transmitted.

On the other hand, a timer (hereinafter referred to as TM) 27 using a counter or the like is operated by the output of the DvT 22, and when the timer expires after a set time of 10 minutes, for example, a stain source control circuit (hereinafter referred to as PC) 28 is activated by the output of the DvT 22. is reset, the power supply to each part is automatically cut off, and the light emitting diode LD, which indicates that the power is on, is 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 Rs is turned on.
e is grounded, and in response, a retrigger prevention circuit (hereinafter referred to as RTI) 29 using a flip-flop circuit etc. is set, so PO28 turns on the power by this output, and after TM27 is cleared, While starting, the gate Gl of M is turned on.

Further, once the RTI 29 is set, it remains set until the PO 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 is possible. FIG.
) is a side view, and (B) is a front view shown in approximately the original size.The transmitter 1a is fixed onto a base plate 41 via an inclined mounting plate 42, and its directional direction is approximately 10 mm from the horizontal direction. ° It is set upward and faces the receivers S2 to S-2 of ROB2 at a predetermined distance.

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 transmitter 11L,
A plug (not shown in the figure) is connected to the tip of this, and connection with the main body is thereby established.

Figure 7 shows the appearance of the main body in approximately the original size, (5) is a plan view, (B) is a side view, and (C) is a front view.The outer casing is made of lightweight metal plate, etc. Each part shown in FIG. 3 and a battery are accommodated in 51, 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, there is a non-locking switch SW1 and a light emitting diode LD exposed in a through hole drilled into the button of this switch, as well as a connector 53 into which a plug from the transmitter 1& is inserted. The cover can be freely removed by removing the provided foot, screws 54, etc., and the battery can be replaced and the interior inspected.

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. 8 is a block diagram of the CPG 4, in which a stable oscillator (hereinafter referred to as OSC) 61 such as a crystal oscillator generates a clock pulse CLK of, for example, I MHz, like the 08CT21 in FIG. is divided by the same frequency division ratio by first and second frequency dividers (hereinafter referred to as DVR) 62 and 63, and the period t is the same as the period t3 in FIG.
, a local synchronization pulse SL and a synchronization pulse SYN with
The DVRs 62 and 63 are designed to send out
It consists of a setting section (hereinafter referred to as DST) 64 for setting data, a down counter (hereinafter referred to as 0CT) 65.66, and an OR gate 67.68, and each DCT
65.66 is the output R, C, t of the own counting pagoda or the start pulse ST 6 or the preset pulse n
is applied to the load terminal via the OR gate 67 or 68, the data from the DST64 is preset and subtraction based on the clock pulse CLK from the 08CR61 is started, so the start pulse sound or the preset pulse H is applied. , each synchronized Hals SL.

The synchronization state can be set.

Further, as for the light reception pulse opening corresponding to the light emission with the period t shown in FIG. Since only one pulse of the start pulse ST is synchronized with this period and has the same width as this, the DCT 65 follows this and stably shifts to the synchronized state with the received light pulse ST. The trigger pulse number 7 is repeatedly sent out in response to the received light pulse H, and if the restart pulse R8 is given, the start pulse number 7 is sent out in the same way.
It is designed to repeatedly send out only pulses.

The trigger pulse and the local synchronization pulse (2) are given to a phase difference detection unit (hereinafter referred to as PDT) 72, where the phase difference between them is detected, and if the phase difference is within a predetermined value, the load pulse (LD) is It sends out only one pulse, and repeatedly sends out a detection pulse plane that is synchronized with trigger pulse 5. However, if the phase difference exceeds a predetermined value, the detection pulse cannot be sent out, and the load pulse LD is replaced by the trigger pulse TO. It is synchronized with and sent out repeatedly.

Each of these pulses PD and LD is given to a DCTt-based continuous detection section (hereinafter referred to as CDT) 73, and a constant value is preset to the DCT in the CDT 73 according to the load pulse LD, and a detection pulse penalty is applied to the DCT in the CDT 73. Therefore, the subtraction is performed, and when the count value becomes zero, the detection completion pulse O
(The detection completion pulse surface and the load pulse surface are input inverted OR gates.)
74 as a restart pulse R8 to the TRG 71, 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 local synchronization pulse S
If only one pulse of L is passed and sent out as preset pulse (2), it will turn off until the next detection completion pulse OK is given, and the preset pulse plane will not be sent out continuously.

Note that the TRG 71, PDT 72, and CDT 73 are given an initial reset pulse (2) which is generated in response to power-on, thereby setting the initial state.

Therefore, even if the light-receiving pulses ■ are continuously given, only one start pulse is sent out in response to the first pulse, and the DVR 62 accordingly receives the light-receiving pulses ■.
, and if the phase difference between the trigger pulse 5 synchronized with the received light pulse ■ and the local synchronization pulse ■ is within a predetermined value, the pulse is repeatedly reset from the constant value reset by the 1-pulse load pulse]. Subtraction is performed in the CDT 73 according to the detection pulse, and a detection completion pulse OK is sent out upon completion of the subtraction. Assuming that the pulse ■ and the local synchronization pulse SL are in a synchronized state, the GAT75 is turned on, the preset pulse ■ is sent out, the DVR 63 is also in the same synchronization state as the DVR 62, and the synchronization pulse δ synchronized with the received light pulse] ~ is sent.

Therefore, the synchronizing pulse 5 does not cause any unnecessary timing changes, and the various parts shown in FIG. 2 that operate in response to this change are in stable operating conditions.

Each part in Fig. 8 is based on the above-mentioned patent application No. 60-5858
The details are disclosed in , so the explanation will be omitted.

The above method does not particularly require an omnidirectional scanning search means, the configuration is simplified, and since tracking starts only when TAG 1 is present in z116, it is possible to run when TAG 1 is nearby. TAG
While the probability of misidentifying something other than 1 as TAG 1 decreases, if TAGl deviates from z217, tracking will stop, and if TAGI is in z318, excessive fine tracking will not be performed, causing hunting in control. First, the tracking control situation becomes stable.

In addition, the ultrasonic waves transmitted from TAG 1 are transmitted to receivers S2~
Since the ultrasonic wave is directly received by S-2 and the position is measured by distance and relative angle, changes in ambient conditions are less The influence of noise caused by As a result, the measured values are accurate and highly reliable9, and the safety of follow-up control is maintained.

However, each part in Fig. 1 may be determined according to the conditions, and in some cases, 2318 may be omitted. In Fig. 4, the order of steps is changed according to the situation,
Alternatively, other equivalent steps may be substituted, or unnecessary steps may be omitted.

Note that the number of receivers S2 to S-Z may be determined according to the conditions, and the settings of each constant and time are also the same.
The OB 2 may transmit ultrasonic waves, and the TAG 1 may reflect them. The means for determining distance and relative angle and the configuration of each part can be arbitrarily selected according to the situation. Various modifications are possible, such as application to various automatic devices having a moving function such as self-propelled transport vehicles.

〔Effect of the invention〕

As is clear from the above explanation, according to the present invention, safety in υ tracking control is achieved with a simple configuration, the tracking situation is stable, and the probability of misidentification of TAG is reduced. This results in remarkable effects in various automatic devices.

[Brief explanation of drawings]

The figures show an embodiment of the present invention, Fig. 1 is a diagram showing each zone, Fig. 2 is a block diagram of the entire configuration, Fig. 3 is a timing chart showing waveforms of each part in Fig. 1, and Fig. 4 is a diagram showing each zone. A flowchart of the calculation situation, Figure 5 is a block diagram of the TAG side, Figure 6 is a diagram showing the situation with a liquid sending wave transmitter on the TAG side, and Figure 7 shows the external appearance of the main body used with the wave transmitter in Figure 6. 8 is a block diagram of the CPG. 1... TAG (target object), 1a... snout transmitter, 1
b...Floodlight, 2...War/ROB (robot),
3...RX (receiving section), 4...CPG (clock pulse generator), 5...light receiver, 6...Φ...
CUT (counter group), 7*e...ALU (calculation unit), 8...CNT (control unit'), 10L. 10R...SM (servo motor), 11L, 1
1R...Driving wheel, 16...21 (first zone), 17...Z2 (second zone), 18...
・z3 (third zone), 81 to s-2... Membership fee wave device.

Claims (1)

[Claims] In a follow-up control method for an automatic device that determines the distance and relative angle to a moving target object and follows the target object while keeping a certain distance from the target object according to these, the follow-up control for the target object is started. A first zone for performing the following control is defined near the periphery of the target object, and a second zone for performing the follow-up control is defined outside the first zone, and the follow-up control is performed according to each of these zones. A follow-up control method that is characterized by: