JP2889257B2 - Moving object position detection device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION (Industrial application field) The present invention relates to a light-reflecting device having a known position.
The present invention relates to an apparatus for detecting the position of a moving body by detecting the angle between the moving body and the moving body.

(Background of the Invention) As means for detecting the position of a moving body, it is conceivable to measure and add a moving amount and a moving direction from the rotation of a wheel, but this method accumulates errors and causes inaccuracy. There is a problem.

Therefore, a method has been proposed in which three reflectors whose relative positions are known are used to detect the included angle of the two reflectors from the moving body to determine the position of the moving body (see JP-A-59-67476). In other words, in this method, one side having a known distance connecting two reflectors is a chord, and two circles determined by the trajectory of the included angle sandwiching the chord are obtained, and the position of the moving body is determined as an intersection of the two circles. However, this method is based on the premise that the reflected light from the three reflectors is simultaneously detected and the two included angles are known at the same time. If the two included angles cannot be simultaneously detected, it is impossible or inaccurate. There was an inconvenience of becoming.

Therefore, a method of accurately detecting a position by detecting the azimuths of three light reflecting means whose positions are known at three positions and detecting the moving amount and moving direction of the moving body between the three positions has been proposed by the same applicant. (Patent application (1) filed on the same day).

However, actually, not all three light reflecting means always reflect correctly. For example, when one of them becomes difficult to reflect due to dirt or the like, there is an obstacle, or the combination of the three light reflecting means used changes during traveling, it may happen that the reflected light cannot be received temporarily. In such a case, there may be a problem that the position detection of the moving body becomes temporarily impossible.

(Object of the Invention) The present invention has been made in view of such circumstances,
When detecting the position of the moving body using the reflected light from the three light reflecting means whose positions are known, there is a case where it is difficult for any of the light reflecting means to project and receive the light, or with the movement of the moving body. It is an object of the present invention to provide a position detecting device for a moving object that can capture reflected light with high precision without causing any inconvenience even if the combination of the three light reflecting means used changes.

(Constitution of the Invention) According to the present invention, an object of the present invention is to detect the position of a moving body by reflecting light emitted from the moving body in the direction of the moving body by three of a large number of light reflecting means having known positions. In the device,
A plurality of light emitting and receiving devices provided on the moving body, a tracking direction managing means for each light emitting and receiving device, an angle detecting means for detecting a light receiving direction from each light reflecting means to obtain a light receiving azimuth vector, and a moving amount of the moving body. A moving direction detecting means for detecting a moving direction of the moving object to obtain a moving azimuth vector, and a calculating means. The light receiver tracks the three light reflecting means for position detection, and the calculating means corresponds to the directions of the three light reflecting means for position detection detected by the other light emitting and receiving devices at three positions of the moving body, respectively. Using the light receiving direction vector, the moving direction vector obtained by the moving direction detecting means at the three positions of the moving object, and the amount of movement between the three positions, the inner product of the moving direction vector and the light receiving direction vector Solve the equation Position detecting apparatus for a mobile body and calculates the location is accomplished by.

Here, the tracking direction management means directs one light emitter / receiver to the farthest light reflection means of the three light reflection means used for position detection, or to the light reflection means to be used next, It can be managed so as to direct the light to the light reflecting means for which the number of light reception failures has reached a certain number or more.

(Principle) A description will be given, based on FIG. 5, that the above object can be achieved by this configuration.

In this figure, P ₀ is the current position of the moving object, and P ₁ and P ₂ are the positions of the moving object at a certain time before. Coordinate system x
−y is taken as shown in the figure, and the coordinates of each position are determined as follows.

P ₀ (x, y) P ₁ (x−Δx ₁ , y−Δy ₁ ) P ₂ (x−Δx ₂ , y−Δy ₂ ) Here, x and y are coordinates to be obtained, and Δx ₁ , Δx
₂ , Δy ₁ and Δy ₂ can be obtained by monitoring the moving amount and moving direction of the moving body.

Also the direction of movement with respect to the x-axis direction at the position P ₀ of the moving body, assuming theta, movement direction at the position P _1, P ₂ is determined as _{θ-Δθ 1, θ-Δθ} 2. Where Δθ
₁ , Δθ ₂ is the amount of change in the moving direction during movement, and thus can be obtained by continuously monitoring during movement.

R ₁ , R ₂ , R ₃ are reflectors as light reflecting means made of a corner cube or the like, and their coordinates are known as shown in the figure. If the angles (light receiving angles) between the moving directions at the positions P ₀ , P ₁ , and P ₂ and the light receiving directions by the reflectors R ₁ , R ₂ , and R ₃ are α, β, and γ, the light receiving angles can also be detected. is there.

An azimuth vector ( ₀ , ₁ , ₂ ) indicating a moving direction at the positions P ₀ , P ₁ , and P ₂ is defined as follows.

₀ = (cos θ, sin θ) ₁ = (cos (θ−Δθ ₁ ), sin (θ−Δθ ₁ )) ₂ = (cos (θ−Δθ ₂ ), sin (θ−Δθ ₂ )) And the light receiving vector indicating the distance are defined by the following equation.

= (X _r1 −x, y _r1 −y) = (x _r2 −x + Δx ₁ , y _r2 −y + Δy ₁ ) = (x _r3 −x + Δx ₂ , y _r3 −y + Δy ₂ ) Therefore, from the definition of the inner product (scalar product), _{· 0 = (x r1 -x)} cosθ + (y r1 -y) sinθ = || · | 0 | cosα ... (1) · 1 = || · | 1 | cosβ ... (2) · 2 = || · | ₂ | cosγ ... (3) As the solution of the simultaneous equations of (1), (2) and (3), x,
y and θ can be obtained. That is, the present invention provides an azimuth vector ( ₀ , ₁ , ₂ ) at the current position P ₀ and the past positions P ₁ and P ₂ , a light receiving vector,
And the movement amount between P ₂ , P ₁ , and P ₀ are detected, and the coordinates (x, y) of the current position P ₀ and the movement direction θ are obtained as solutions of simultaneous equations.

(Embodiment) FIG. 1 is a functional block diagram showing a configuration of the present invention, FIG. 2 is a block diagram showing a control system of a moving body, FIG. 3 is a side view of a golf cart as a moving body, and FIG. It is operation | movement explanatory drawing.

In FIG. 3, reference numeral 10 denotes a body frame, 12 (12a, 12
b) is a pair of left and right rear wheels, and 14 is one steering front wheel. The body frame 10 has an upper frame 10a which rises upward from between the rear wheels 12 and has an upper end extending horizontally forward. The rotation of the electric traveling motor 16 is transmitted to the rear wheels 12 via the chains 18, 20 and the differential device 22. The rotation amounts of the left and right rear wheels 12a, 12b are separately detected by a pair of left and right encoders 24 (24a, 24b). The front wheel 14 is attached to a lower end of a steering shaft 28 held by a steering shaft tube 26. A clutch 30 is provided at the upper end of the steering shaft 28.
, The steering motor 32 is connected, and the link 34 transmits the rotation of the handle shaft 36. That is, the front wheel 14 is steered by either the motor 32 or the steering wheel 38 due to the connection / disconnection of the clutch 30.

40 is a controller, each motor for traveling and steering
It has a circuit for performing power control of 16, 32, an interface, and the like. Reference numeral 42 denotes a lead storage battery mounted at a lower portion near the center of the vehicle body. A step 46 is provided at the rear of the body frame 10 so that the driver stands. The driver can operate the input device 48, the main switch 50, the steering wheel 38, and the like, if necessary, at the step 46.

Reference numerals 52 (52a to 52d) denote laser emitters and receivers, which are attached to the front end of the upper frame 10a. These emitters and receivers 52
The laser beam is rotated in a substantially vertical direction by a polygonal mirror that rotates at a high speed about a horizontal axis, and the laser beam is rotated about the vertical axis and independently scanned in the circumferential direction. The motor 54 (54a-d) rotates the light emitter / receiver 52 in this sub-scanning direction, while the rotation angle is
56 (56a-d). Further, a light receiving element for detecting the reflected light of the emitted laser by the reflector is attached to these light emitting and receiving devices 52, and the laser light is transmitted to the fifth light emitting device.
The light reflected by the reflector R described in the drawing and returned is detected.

Next, the CPU device 60 will be described. This device 60 includes a CPU 62 composed of a digital computer, a semiconductor memory (storage device) 6
4. It has a pulse counter 66 (66a, 66b), 68 (68a-d) and the like. Both pulse counters 66 separately count pulses output by the encoder 24 as the left and right rear wheels 12 rotate. The four pulse counters 68 separately count the pulses output by the encoder 56 with the rotation of each of the light emitting and receiving devices 52. The count values of these counters 66 and 68 are read into the CPU 62 via an interface (not shown) provided in the controller 40. In the memory 64, in addition to the operation program of the CPU 62, a reflector R (fifth
(See the figure), and data such as a predetermined traveling route.

The CPU 62 repeatedly performs the operations of the functions shown in FIG. 1 in accordance with the program stored in the memory 64. CPU62
First, a signal for driving the steering motor 32 and the traveling motor 16 is sent to the controller 40 according to the scheduled traveling route stored in the memory 64. As a result, the front wheels 14 are steered and the rear wheels 12 are driven, and the vehicle runs substantially along the scheduled traveling route. CPU62
The tracking direction management means 62A operates the light emitter / receiver 52 during traveling, and makes the laser perform sub scanning about the vertical axis while performing main scanning in the vertical direction. At this time, the direction managing means 62A selects the _three closest r ₁ , r ₂ , and r ₃ where the position of the reflector detected from the memory 64 is as shown in FIG. 4, and directs the separate light emitting and receiving devices 52 to each. For example emitter and receiver 52 at the position of the P ₂ in FIG. 4
The b to r _3, and 52c to r _2, 52 d to be tracked by the sub-scanning in the appropriate angular range toward the r _1. In this case, the angle range of the sub-scanning is made larger as the distance between the moving body and the light reflecting means r is smaller and smaller.

Further, the light emitter / receiver 52a is used as an auxiliary, and a reflector such as r ₃ in which the number of light reception failures exceeds a certain number within a predetermined time.
Turned to If the number of times exceeds a certain value, it is determined that the reflector is not functioning, and the light emitter / receiver 52a is used to search for another reflector. Laser is reflected by reflector r ₁ and emitter and receiver 52d
The reflected light is detected by the light receiving element, and the count value n of the pulse counter 68d is set to C based on the light receiving signal at this time.
Read by PU62. Based on the count value n, the angle detection means 62B of the CPU 62 uses the reflector r ₁ for the vehicle traveling direction.
Is obtained. CPU62 whereas stores vehicle position at this time _{P 2 (x-Δx 2,} y-Δy 2) memory 64 as a direction vector ₂ showing the direction of travel at this position P _2, the light receiving direction of the reflector r ₁ The vector is determined as follows.

_{2 = (cos (θ-Δθ} 2), sin (θ-Δθ 2) = (x r3 -x + Δx 2, y r3 -y + Δy 2) CPU62 is allowed to run the vehicle in accordance with the then planned travel course, emitter and receiver 52c is a reflector When the reflected light from r ₂ is received, the angle β of the reflector r ₂ at that time is obtained, and
And stores the position in the _{P 1 = (x-Δx 1} , y-Δy 1) memory 64 as a P _1. CPU62 also has an orientation vector
₁ and the light receiving direction vector are determined as follows.

_{1 = (cos (θ-Δθ} 1), sin (θ-Δθ 1)) = reflection from _{(x r2 -x + Δx 1,} y r2 -y + Δy 1) emitter and receiver 52b is a reflector r ₃ further CPU62 is traveling when receiving light, with determining the angle α of the reflector r ₃ at that time, P ₀ and (x, y) to the memory of the position as P _0, the azimuth vector _0, determines a light-receiving direction vector by the following equation.

₀ = (cos θ, sin θ) = (x _r1 −x, y _r1 −y) Here, the moving amounts Δx ₁ , Δx ₂ , Δy ₁ , Δy ₂ and the moving direction Δθ
₁ and Δθ ₂ are calculated by the movement amount detecting means 62C and the moving direction detecting means 62D in FIG. 1 using the average value of the rotation amounts of the left and right rear wheels 12 and the difference between the rotation amounts. .

The CPU 62 then calculates these azimuth vectors ₀ , ₁ , ₂
, The light receiving direction vector, and the angles α, β, γ, and (1), (2),
The simultaneous equations of (3) are solved (calculation means 62E).

By detecting the reflection directions of the different reflectors r ₁ , r ₂ , and r ₃ at the different positions P ₂ , P ₁ , and P ₀ , the movement amount and the movement direction of the vehicle therebetween, the current position P ₀ The coordinates (x, y) and the traveling direction θ can be obtained with high accuracy.

In this way, the robot moves while detecting the position, and P ₀
Comes the direction management unit 62A to the position of changing the orientation r ₄ using then the emitter and receiver 52a from the reflector r _3, to prepare for detecting the position by using reflected light from the r _4. Thereafter, the vehicle travels while performing position detection using the reflectors r ₂ , r ₃ , and r ₄ . The emitter and receiver 52d closer reflector r ₃ which was facing the reflector r ₁ to farthest made while further moves to the position P ₀
To perform position detection using r ₂ , r ₃ , and r ₄ . Then the light receiver 52b is tracking a reflector r ₅ scheduled to be used next.
In this manner, by tracking one light emitting / receiving device 52 to the reflector to be used next, substantially continuous position detection can be smoothly performed.

In this embodiment, the movement direction Δθ is detected from the difference between the rotation amounts of the left and right rear wheels, but may be detected from a gyroscope or the like.

In this embodiment, the directions of the three light reflecting means are detected by the four light emitting and receiving devices 52a to 52d.
The two light reflecting means may be sequentially tracked with a time difference, and the light reflecting means which is least likely to emit and receive light by the other light emitting and receiving device may be tracked.

(Effect of the Invention) As described above, according to the present invention, three directions of a plurality of reflectors whose coordinates are known are obtained at three positions, and the moving direction of the moving body at the three positions, and further, these three positions By monitoring the amount of movement during the period, the moving direction vector and the light receiving direction vector of the moving object are obtained, and 3
From the simultaneous equations, the current position of the moving object and its moving direction are always tracked by three light reflecting means for position detection by another light emitting and receiving device except for one light emitting and receiving device. By using the light emitting and receiving device as an auxiliary light emitting and receiving device, the light reflecting means can be detected with high accuracy. For example, this one light emitter / receiver is used for tracking the farthest light reflecting means, which is considered to be the most difficult to track, among the three position detecting light reflecting means, so that this light reflecting means is temporarily
It is possible to improve the detection accuracy by tracking the two light emitting and receiving devices (claim 2).

In addition, if the auxiliary light emitter / receiver first tracks the light reflecting means to be used next, the position detection is continuously performed when the combination of the three light reflecting means to be used changes as the moving body moves. It can be performed smoothly (claim 3).
Further, if the auxiliary light emitter / receiver is used to track the light reflecting means in which the number of light reception failures is equal to or more than a certain number, the detection accuracy is improved and highly reliable position detection is possible (claim 4).

[Brief description of the drawings]

FIG. 1 is a functional block diagram showing a configuration of the present invention, FIG. 2 is a block diagram showing a control system of a moving body, FIG. 3 is a side view of a golf cart as a moving body, FIG. FIG. 5 is an explanatory view of the principle. 52a-d: Emitter / receiver 62, CPU, 62A: Tracking direction management means, 62B: Angle detection means, 62C: Movement amount detection means, 62D: Movement direction detection means, 62E ... Arithmetic means, R ₁ R ₂ R ₃ , r _{1 to} r ₅ ... Light reflection means.

──────────────────────────────────────────────────続 き Continued on the front page (58) Fields surveyed (Int.Cl. ⁶ , DB name) G05D 1/02 G01S 5/00-5/30

Claims (4)

(57) [Claims] An apparatus for detecting the position of a moving body by reflecting light emitted from the moving body in the direction of the moving body by three of a large number of light reflecting means having known positions. A light emitting and receiving device, a tracking direction managing means for each light emitting and receiving device, an angle detecting means for detecting a light receiving direction from each light reflecting means to obtain a light receiving azimuth vector, and a moving amount detecting means for obtaining a moving amount of the moving body. Moving direction detecting means for detecting a moving direction of a moving body to obtain a moving azimuth vector, and calculating means, wherein the tracking direction managing means is for detecting a position of another light emitting and receiving device except one light emitting and receiving device. The light receiving direction vectors corresponding to the directions of the three light reflecting means for position detection detected by the other light emitting and receiving devices at the three positions of the moving body, respectively, The moving direction detecting means is provided at the three positions of the moving body. Using the moving azimuth vector obtained in step (1) and the amount of movement between the three positions to solve a simultaneous equation based on an inner product of the moving azimuth vector and the light receiving azimuth vector to calculate the position of the moving body. Position detection device. 2. The moving body according to claim 1, wherein the tracking direction managing means directs one light emitting and receiving device to the farthest light reflecting means among the three light reflecting means used for position detection. Position detection device. 3. The moving object according to claim 1, wherein the tracking direction managing means directs one light emitting and receiving device to light reflecting means newly added as three light reflecting means to be used next. Position detection device. 4. The tracking direction management means directs one light emitter / receiver to one of the three light reflection means for position detection, the light reflection means having a predetermined number or more of failed light receptions. Item (1).