JPH0255804B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION The present invention provides a mobile object with one receiver having a direction and distance searching function and another receiver having a distance searching function. It relates to a method of moving the body in a desired direction.

Generally, the position of a moving object on a traveling plane is measured by a three-point survey method, but the present invention can measure the position of a moving object using a much simpler method than the three-point survey method. , which attempts to move it to an arbitrary destination point, in particular, by installing a source at only one location on the traveling plane and controlling a receiver on the moving body that detects the direction of the source. Using calculations in the device, the receiver is always pointed in the direction of the source, and at the destination point or an appropriate intermediate point, the direction of the source is confirmed by simply swinging the receiver by a small angle, and the receiver is rotated 360 degrees. The feature is that the time required for position search is significantly shortened compared to the case where the position search is rotated across the body.

Hereinafter, the method of the present invention will be explained in detail with reference to the drawings.

As illustrated in FIG. 1, the present invention has a moving body 1 at a point B on a running plane, a moving body reference line l passing through the B point in the moving body, and an arbitrary point on the running plane. The purpose is to move the moving object 1 in a desired direction when there is a source 2 of ultrasonic waves or the like at A, and in this case, the object B on the reference line in the moving object is
A first receiver having a direction and distance search function is provided at each point, and a second receiver having only a distance search function is provided at a point C, which is separated by a prescribed interval S on the reference line. The transmission source 2 is provided at only one location on the running plane, and the transmission signal from this transmission source can be not only ultrasonic waves but also electromagnetic waves or the like.

Note that here, for convenience, it is assumed that the moving object can move in any arbitrary direction from a stationary state, and therefore the initial position of the moving object on the traveling plane, that is, the distance between the moving object 1 and the source 2. R ₁ and the angle θ formed by the straight line connecting the moving object and the source and the moving object reference line l will be described as being able to be measured by the direction and distance search function described above. Further, the arrow of the moving object reference line 1 means the front head direction of a car, for example.

In order to provide a moving body with the above-mentioned direction and distance search function, it is sufficient to install a device as described below (see Japanese Patent Application No. 54-39203) on the moving body. Any method can also be adopted.

First, as an example of a device to provide a direction search function, as shown in FIG. A peak detector 6 detects the maximum value of the received signal level output from the receiver 5 while rotating the receiver 3 by driving the motor 4 with a control signal from a control device such as a computer. The rotation angle θ of the receiver when the maximum value is detected can be sent to the control device. This direction search function makes it possible to measure the angle θ between the source A and the moving object reference line l as seen from the moving object.

In addition, in FIG. 2, it is assumed that the first receiver 3 is installed at point B in FIG. 1, and the second receiver 9 is installed at point C in FIG. It is assumed that the motor 4 always points the container in the same direction. Since the second receiver 9 only measures distance, there is no need to point it exactly at the source.

Further, the distance search function in a moving body can be obtained by a device such as the one shown in FIG.
This device causes counter circuits 14 and 15 to start counting pulses from a pulse generator 16 in response to a distance measurement signal from a control device, and at the same time opens a gate circuit 18 provided in a high frequency oscillator 17. A gate circuit 24 for sending a signal to a radio wave receiving device 22 on the ground via 21, and superimposing a signal wave from the signal wave transmitter 23 on a carrier wave such as an ultrasonic wave in a transmitting device 25 for transmitting ultrasonic waves or the like on the ground. The transmitter 26 transmits a superimposed wave in which a signal wave is superimposed, which is received by two receivers 3 and 9 on the moving body that are spaced apart by a specified interval, and from each received wave is sent to the receiving device. The counter circuits 14 and 15 are divided by the signal wave components separated by 7 and 10.
The system stops the counts at , and measures the respective distances based on the counted numbers.

In addition, in FIG. 2, the configuration of this distance search function device is shown as receiving devices 7, 10, distance measuring device 8,
11.

By providing such a device on a moving body, the moving body can be provided with a direction and position search function, but in the present invention, based on these outputs, each mobile body on the route on which the moving body is traveling is Calculating the angle θ at the point in the control device,
As a result, the receiver is always pointed in the direction of the source, and if a large angle error occurs, the position is confirmed again using the direction and position search function, and the position is set as the initial movement point. It operates to return to a predetermined trajectory.

Next, a typical example of moving a moving object in a desired direction will be described together with the functions of the control device.

First, as shown in FIG. 4, a reference line p on a traveling plane parallel to the moving body reference line l and passing through the source A
There is a normal trajectory that is perpendicular to point A,
A case will be described in which the moving object is returned from its current position, point B, to its normal trajectory by the shortest distance, ie, point E, and then directed toward point A along the trajectory. In this case, the moving body is first moved from its current position at point B to point E along a line that intersects at right angles to the normal trajectory, but the distance traveled during this time is
It can be determined by =R ₁ ·cosθ. When the moving object moves, let the arbitrary moving point above be point D, and if point D is the point that internally divides into m:n, then the receiver 3 faces point A and its reception level increases. The expected maximum angle θ _D is θ _D = tan ^-1 (m + n/m・1/cos θ)...(1), and if the moving object moves above by x, then θ _D = tan ^-1 (R ₁ sinθ/R ₁ cosθ−x) ...(2) It can also be expressed.

In the control device shown in FIG. 2, since R ₁ and θ at point B are known in advance by measurement using the direction and distance search function, by giving the coordinates (preferably polar coordinates) of point E, the above (1) or (2)
It is possible to calculate θ _D according to the formula, and therefore, based on the calculation result, the receiver 3 on the moving body is set in the direction of θ _D at any point D above, thereby making the receiver the source of the signal. If you drive a moving object while pointing at the source, the receiver will never lose sight of the source.
You can run the moving object while checking that it is following the direction. However, in a moving object, it is necessary to always measure the moving distance x.

When the moving body travels a certain distance or reaches a turning point such as the above point E, at that point (these points are called intermediate points), as described below, a command from the control device The direction of the receiver is swung by a small angle ±Δ from θ _D , and the maximum value of the reception level during that period is searched. At this time, if it is determined that the angle error is very large, the distance R _D ′ and true angle θ _D ′ to point A are measured based on the direction and distance search function, and this point is used as the initial movement of the moving object. Use the same method again to reach point E. Note that in this example, θ _D =π/2 at point E. From point E, the direction of the receiver is swung by ±Δ in the same way, the maximum value of the received signal level is searched during that period, and the mobile object is moved in the direction of the maximum level, so that it returns to point A on the normal orbit. get closer to

When performing the above operations, the attitude direction of the moving body changes depending on the structural factors of the moving body or the external environment such as the condition of the road surface, so it is necessary to monitor the attitude direction at regular time intervals. In that case, the distance R _D between any point D where the first receiver 3 with direction/distance search function is located and the source A, the line segment
The angle between AD and the moving object reference line l is θ _D , and the second receiver having a distance search function is installed at a point D to S of the first receiver 3 on the moving object reference line l. If the distance between the device 9 and the source A is R _D2 , then R _D2 =√(+ _{D D} ) ² +( _{D D} ) ² ...
...(3) can be expressed. This R _D2 is the measured value actually obtained from the counter circuit 15 in Fig. 3.
By comparing the direction of the moving object with R _D2 ′ and rotating the direction of the moving object so as to make the difference zero, the attitude direction of the moving object can always be kept constant.

Next, a case will be described in which the moving body is moved from point B to an arbitrary point F.

In this case, as shown in Fig. 5, the angle at which the received signal level of the receiver 3 at point F is predicted to be maximum is θ _F , and a line perpendicular to the reference line p on the traveling plane is drawn at point A. Also, draw a perpendicular line from point B to the line segment, and calculate the angle ∠EBF of the line segment connecting point B and point F with respect to the above as θ _i ,
Letting ∠BAF be θ〓 and the distance from point B to point F be x, applying the law of sine to ∠AFB, we get θ〓＝cot ^-1 {R ₁ /x・1/sin(θ−θ _i )−cot(θ
−θ _i )} ...(4) holds true, so the angle θ _F of the receiver 3 at point F
is calculated using the following formula.

θ _F = θ + cot ^-1 {R ₁ /x・1/sin(θ−θ _i )−cot(
θ−θ _i )} ...(5) Therefore, in order to move the moving body from point B to an arbitrary point F, the coordinates of point F are given to the control device, and the control device calculates the above x and Calculate θ _i ,
Based on this, θ _F at point F is calculated and the direction of the receiver 3 is set.

When the moving body reaches point F, as described above, the direction of the receiver is swung by ±Δ with respect to θ _F to search for the maximum value between them. In this case, there is no problem if the received signal level is maximum at θ _F , but errors actually occur depending on the structure of the moving object, the condition of the road surface, etc. When it is determined that this error has become large, use the direction and distance search function to measure θ _F ′ and R _F ′ in Figure 6, and use that point as the initial movement point and move the moving object sequentially by the error amount. let

If the receiver is swung by ±Δ according to a command from the control device at the above point F or an appropriate intermediate point, the received signal levels shown in Figure 7 A to C are obtained, but normally, If the peak value of the received signal level is within the range of ±Δ as shown in Figure 2, and the peak value of the received signal level is detected to be within the range of ±Δ by the peak detector 6 in Figure 2, the mobile object is within the tolerance range. It is not corrected as it is within the range.

However, if the received signal level does not have a peak value as shown in FIG. 7B or C, it is necessary to correct the error, and the correction described below is performed.

In this correction, first, the distance search function device on the moving object is operated to measure the distance R _F ' from the source, and this is compared with the distance R _F in FIG.

The above distance R _F can be calculated by applying the law of cosines or the law of sine to △AFB in Figure 5. R _F =√ ₁ ² + ² −2 ₁・(− _i )……(6
) R _F = sin(θ−θ _i )/sin θ〓x ...(7) Therefore, in the arithmetic circuit 12 of the control device, R _F
Calculate and compare with the actually measured R _F ′.

As a result of comparing the two, the difference is |R _F −R _F ′|
If the value of Measure _F ′.

Then, using the measured R _F ′ and θ _F ′ as the current position F′ of the moving object, move the moving object again to point F, and repeat the same operation to adjust the receiver to ± with respect to θ _F.
When the movement is made by Δ, when the peak value of the received signal level falls within the range of ±Δ as shown in FIG. 7A, it is assumed that point F has been reached and the movement is terminated.

Further, if the value of |R _F −R _F '| is smaller than the threshold value, it is determined that the direction of the moving object itself has changed while the moving object moves from point B to point F. In this case, as shown in Figure 8, the moving object that should theoretically be at point F is actually at point F', and the angle of the receiver with respect to point A should theoretically be θ _F. , θ _F ′, which contains a large error.
Note that although the difference between θ _F and θ _F ′ is exaggerated in the figure, in reality there is no extreme difference, and moreover, the difference between θ F and θ F ′ is
As mentioned above, since |R _F −R _F ′| is small, the F′ points are often located at positions that can be considered to be the same.

The calculation of the above-mentioned equation (5) is performed in the calculation circuit 12 in the control device shown in FIG.
outputs the receiver angle θ _F (theoretical value) at which the received signal level is predicted to be maximum at point F. Now, assuming that the moving object actually moves to point F' and the actual measurement value θ _F ' is obtained, this output is added to the subtraction circuit 13 along with the above θ _F , and for the angular error θ _e , θ _e = θ _F −θ _F ′ is calculated. As a result, when θ _e becomes larger than a predetermined threshold, R _F ′ is further measured, and θ _F ′ and R _F ′ obtained by that measurement are assumed to be the initial position F′ and the Calculations are performed up to the first target position F, and the position of the moving object is corrected.

Note that when the position of the moving body is not corrected, an output zero command signal is output from the arithmetic circuit 12 in the control device to the subtraction circuit 13 so that the output of the subtraction circuit 13 becomes zero.

In addition, the above |R _F −R _F ′| and the threshold value of θ _e are Δ
is set appropriately in advance along with the value of .

If the moving object is a flying object or an underwater vehicle, it is possible to always keep the attitude of the moving object in a fixed direction by having a gyroscope, a spirit level, etc. Some measures are required for moving objects. As an example, it is also possible to measure the direction using the earth's magnetism, but here we will explain a method using the above direction and position search function.

As shown in Fig. 9, this moving body has a direction/distance search receiver (Fig. 2,
Another distance measurable receiver (second
The receiver 9) shown in Figs. and 3 is installed.
Assuming that the moving body moves by x in an arbitrary direction, point B at that time moves to point F, and if the moving body moves without changing its attitude, point C becomes point G. In reality, as described above, the posture direction of the moving body changes depending on the structural factors of the moving body or the external environment such as the condition of the road surface, so the result of the movement of point C becomes point G'. In FIG. 9, assuming that the angle reference line for measuring the attitude of the moving object is placed on the line connecting the above two receiving devices, the attitude angle of the moving object at point F is theoretically ∠GFA, but
Actually, in order to measure θ _F ′, ∠G′FA is found. If the line segment is R _G and ′ is R _G ′, R _G is a theoretical value, and R _G ′ is the value of the receiver 9 and receiver 1 described above.
This is the measured value as a result of measuring the distance to point A using 0 and the distance measuring device 11.

If these relationships are considered with point A, where the source 2 is located, as the center, they will be concentric circles centered on point A, as shown in FIG. Here, = S and
Since FG′=S, find the theoretical point G on a line parallel to F, and from this, the theoretical value R _G
can be found. Also, from measurement point G′
Calculate R _G ′. Using these R _G and R _G ′, R _G ′−R _G
If ≒0, that is, ∠G′FG=δ, then δ
Correct the posture by rotating the moving object so that ≒0. This correction operation is not performed frequently while the moving object is moving, but the posture is corrected at predetermined time intervals.

The theoretical value of R _G at this time is R _G = √ (+ _{F F} ) ² + ( _{F F} ) ² ...
It can be calculated as (8).

FIG. 11 shows a circuit configuration for driving the drive motor 27 and steering motor 28 based on commands from the control device.

In the control device, first, R ₁ at the initial position actually measured by the direction and position search function device,
R ₂ and θ are given, and any F to be moved
R _F and θ _F for a point are given, and based on these, an arithmetic circuit calculates whether the maximum received signal level can be obtained by pointing in the direction of a receiver or the like on the way to point F. This calculation result is applied to the motor 4 that changes the direction of the receiver 3, and the drive amount of the drive motor 27 and steering motor 28 up to point F is determined by the interpolated voltage taking into consideration acceleration/deceleration time, etc. Each driver circuit 2
Added to 9,30.

Drive motor 27 and steering motor 28
Potentiometers 31 and 32 attached to detect the driving amount of the respective motors 27 and 28,
This information is fed back to the control device, and these constitute a servo circuit.The control device uses the feedback information to calculate the current position of the moving object, and also maximizes the received signal level at that time. The predicted direction of the receiver 3 is calculated and the motor 4 is driven.

As a result, when the moving object reaches point F or at an appropriate intermediate point, the receiver is instructed by the control device to move by ±Δ with respect to the direction in which the received signal level is expected to be maximum at that point. By driving the motor 4 for swinging and detecting the peak value as described above, it is possible to confirm whether or not the moving object has moved accurately.

Regarding the attitude direction of the moving body, R _G and R _G ' are determined by the method described above, and the steering motor 28 is driven so that the difference becomes zero to perform correction.

As is clear from the detailed description above, the method of the present invention is a simple method in which a source is provided at only one location on the traveling plane and detected by receivers at two locations on the moving body. With this configuration, the moving object can be easily moved to any position, and if there is no error during movement, the moving object can be moved while confirming its position only by direction search. In this system, the receiver is swung by a small angle unless the error is extremely large, so the position search can be performed in an extremely short time.

[Brief explanation of drawings]

FIG. 1 is an explanatory diagram of the basic structure of the method of the present invention, FIGS. 2 and 3 are block diagrams of the direction and position search function device, and FIGS. 4 to 6 are movement diagrams according to the method of the present invention. FIGS. 7A to 7C are explanatory diagrams of typical control examples of a moving body. FIGS. 7A to 10C are explanatory diagrams of the received signal level of the receiver. , FIG. 11 is a block diagram of a drive system driven by commands from a control device. 1... Mobile object, 2... Source, 3, 9... Receiver, 2
7... Drive motor, 28... Steering motor,
31, 33... Potentiometer.

Claims (1)

[Claims] 1. A first receiver that can search for the direction and distance of a source located at one place on the moving plane and a second receiver that can search for the distance are installed in a moving body, separated by a specified distance. The system has a direction/position search function consisting of a direction distance search function and a distance search function, and provides the coordinates of the destination point to the control device on the moving object, and controls the drive motor to move the moving object toward the destination point. and a steering motor for direction control, the first receiver is controlled to always point toward the source, and the first receiver measures the direction and distance of the source, and at the same time the second receiver The first receiver is swung at a small angle relative to the source at the destination point or an appropriate intermediate point, and the peak value of the received signal level during that time is used to determine the distance between the moving object and the source. At the same time as measuring the azimuth angle of the transmitter, the distance to the source is measured, and the measurement results obtained from the three pieces of information, these direction and distance search results and the distance search result of the second receiver, are compared with the theoretical value. A driving method for searching for the direction and position of a moving body, characterized in that the moving body is moved to a destination point by controlling a drive motor and a steering motor for direction control so that there is no difference between the two.