JP2602064B2 - Travel position control device for self-propelled vehicles - Google Patents

Description

Description: FIELD OF THE INVENTION The present invention relates to a position control device for a self-propelled vehicle, and more particularly, to the traveling of a self-propelled vehicle such as an automobile, an unmanned mobile conveyance device in a factory, and an agricultural and civil engineering machine. The present invention relates to a position control device.

(Prior Art) Conventionally, as a device for detecting the current position of a moving object such as the above-mentioned self-propelled vehicle, the following technology has been proposed in, for example, JP-A-59-67476. This technology is an apparatus for detecting the position of a moving body by scanning a light beam generated from the moving body in a circumferential direction around the moving body, and a light reflecting means for reflecting light in an incident light direction. Are fixed to at least three places apart from the moving body, and the moving body has light generating means, light beam scanning means for scanning a light beam generated from the light generating means, and reflected light from the light reflecting means. And a light receiving means for receiving the information.

Then, an opening angle between the three light reflecting means centered on the moving body is detected based on the light receiving output of the light receiving means, and based on the detected opening angle and the preset position information of the light reflecting means. To calculate the position of the moving object.

(Problems to be Solved by the Invention) However, in the above-described technology, even a slight error in the preset position information of the light reflecting means causes a large adverse effect on the entire system. Every time is changed, the position information of the light reflecting means to be installed, that is, the distance and the relative angle between the light reflecting means provided at the three places are accurately measured in advance before the work, and the measurement result is sent to the control device. I was doing the work of typing. As described above, there is a problem that it is extremely difficult to accurately measure and input the interval and the relative angle of the light reflecting means arranged in a wide work area.

SUMMARY OF THE INVENTION An object of the present invention is to solve the above-mentioned problems of the prior art, to enable the position of the light reflecting means to be detected by a simple operation procedure, and to control the traveling direction of a moving body based on the position information. It is to provide a traveling position control device.

(Means and Actions for Solving the Problems) In order to solve the above-mentioned problems, the present invention provides a method of operating a light beam generated from a self-propelled vehicle in a circumferential direction around the self-propelled vehicle. A traveling position control device for a self-propelled vehicle, which detects reflected light of the light beam by light reflecting means installed at at least three places away from the self-propelled vehicle and detects a position of the self-propelled vehicle; A light beam generating unit provided on the self-propelled vehicle and generating the light beam; a light beam scanning unit provided on the self-propelled vehicle and scanning the light beam in a circumferential direction around the self-propelled vehicle; A light receiving unit provided on the self-propelled vehicle and receiving the reflected light of the light reflecting unit; and a detection for detecting an opening angle between the light reflecting units as viewed from the self-propelled vehicle based on a light reception output of the light receiving unit. Means and said light reflection measured by any means The distance from one of the means to the other two light reflecting means, and the other two light reflecting means centered on the installation position of the one light reflecting means detected by the opening angle detecting means. Means for calculating coordinates of the two light reflecting means based on an opening angle;
It is characterized in that there is provided means for calculating the position of the self-propelled vehicle based on the opening angle between the three light reflecting means as viewed from the self-propelled vehicle and the coordinates of the light reflecting means.

In the present invention having the above configuration, information on the distance from one of the three light reflecting means to the other two light reflecting means, and the other two light reflecting means centered on the one light reflecting means The coordinates of the three light units are calculated based on the opening angle of the vehicle, and the traveling position of the self-propelled vehicle can be controlled using the coordinates as they are. In other words, by effectively utilizing the opening angle detecting means provided for the self-propelled vehicle to detect its own position, the system can be configured without measuring all the distances between the three light reflecting means. The coordinates of all the light reflecting means can be determined only by measuring the respective distances from the reflecting means to the other two light reflecting means.

Embodiment An embodiment of the present invention will be described below with reference to the drawings. FIG. 8 is a perspective view showing a self-propelled vehicle equipped with the control device of the present invention, and a light reflector disposed in an area where the self-propelled vehicle runs.

In FIG. 1, a self-propelled vehicle 1 is, for example, a working self-propelled vehicle such as a lawnmower. A rotary table 4 driven by a motor 5 is provided above the self-propelled vehicle 1. And
The turntable 4 is equipped with a light emitter 2 for generating a light beam and a light receiver 3 for receiving reflected light of the light beam. The light emitting device 2 includes a light emitting diode that emits light, and the light receiving device 3 includes a photodiode that receives incident light and converts it into an electric signal (both are not shown). Further, the rotary encoder 7 is provided so as to be linked with the drive shaft of the rotary table 4, and the rotation angle of the rotary table 4 can be detected by counting the pulses output from the rotary encoder 7. An operation panel 1a is used for inputting information necessary for controlling the self-propelled vehicle 1 and displaying an input result.

Reference numeral 6 denotes a reflector provided around the work area of the vehicle 1. The reflector 6 has a reflecting surface for reflecting incident light in the incident direction, and a commercially available so-called corner cube prism or the like can be used.

Next, the configuration of the control device according to the present embodiment will be described with reference to the block diagram shown in FIG. In FIG.
Is scanned in the rotation direction of the turntable 4 and is reflected by the reflector 6. The light beam reflected by the reflector 6 enters the light receiver 3 and is detected as a signal indicating an azimuth which is the position of the reflector 6 with respect to the traveling direction of the vehicle body.

The counter 9 counts the number of pulses output from the rotary encoder 7 as the rotary table 4 rotates. Then, the count value of the pulse is transferred to the angle detecting unit 10 each time the light receiving unit 3 receives the reflected light. The angle detection unit 10 calculates the opening angle between the reflectors 6 as viewed from the self-propelled vehicle 1 based on the count value (= azimuth angle) of the pulse transmitted each time the reflected light is received.

The distance setting unit 8 is provided with any means (not shown)
The distance from one of the reflectors 6 to the other is measured and the result is input. The input of the distance is
The operation is performed by the operator using the operation panel 1a provided on the self-propelled vehicle 1.

SW1 and SW2 are switches for switching between the reference coordinate calculation mode and the steering control mode. When the switching of SW1 and SW2 is set to the reference coordinate calculation mode M1, the detection output of the angle detection unit 10 and the distance information set in the distance setting unit 8 are input to the reference coordinate calculation unit 11. In the reference coordinate calculation unit 11, the distance information input from the distance setting unit 8 and the angle detection unit 10
The coordinates of the other two light reflectors 6 having one of the light reflectors 6 as an origin are calculated based on the opening angle obtained in the above-described step by a calculation formula described later.

The coordinates of the other two light reflectors 6 having the origin at one of the light reflectors 6 obtained by the reference coordinate calculation unit 11 are stored in a backup memory provided in the position / traveling direction calculation unit 13. Is stored.

On the other hand, when the switching of SW1 and SW2 is set to the steering control mode M2, the data of the distance setting unit 8 is not input to the reference coordinate calculation unit 11, and the detection output of the angle detection unit 10 is the position / traveling direction calculation. The data is input to the unit 13.

In the position / traveling direction calculation unit 13, the coordinates and traveling direction of the self-propelled vehicle 1 are calculated based on the detection output of the angle detection unit 10 and the reference coordinates by a calculation formula described later, and the calculation result is sent to the steering unit 14. Is entered.

In the steering unit 14, the calculation result of the position / heading calculation unit 13 is compared with the scanning course set in the traveling course setting unit 16, and based on the comparison result, the steering connected to the front wheel 17 of the self-propelled vehicle is performed. A motor (not shown) is driven. The steering angle of the front wheels 17 by the steering motor is fed back to the steering unit 14 detected by a steering angle sensor 15 provided on the front wheels of the self-propelled vehicle 1.

The drive unit 18 controls the start / stop of the engine 19 and the operation of the clutch 20 that transmits the power of the engine 19 to the rear wheels 21.

Next, the procedure for determining the coordinates of the reflector 6 will be described. FIG. 4 (a) is a flowchart showing a procedure for preparing a work by the self-propelled vehicle 1, FIG. 4 (b) is a layout diagram of the reflector 6, and FIG. 5 is a flowchart of coordinate calculation control of the reflector.

First, the reflector 6 is set at points A and C according to the work area, and a marker is set at point B (step S100). At point B, one of the reflectors 6 will be installed later.

Next, distance l1 from point B to point A, and C
The distance l2 to the point is measured (step S200). Distance l1, l
The measurement of 2 can be performed by any means such as an optical distance measuring instrument or a tape measure.

In step S300, the self-propelled vehicle 1 is moved to the point B. At this time, the self-propelled vehicle 1 is moved such that the rotation centers of the light emitter 2 and the light receiver 3 are located immediately above the marker.

In step S400, the calculation of the reference coordinates is performed according to a routine described later.

In step S500, the self-propelled vehicle 1 is moved to a work start position.

In step S600, the remaining one reflector 6 is set on the marker.

When the above processing is completed, steering control is performed according to a routine described later.

 Next, reference coordinate calculation control of the reflector 6 will be described.

In FIG. 5, first, in step S401, the light emitting device 2
Then, the light receiver 3 is rotated at the point B, and the opening angle θ between the points C and A viewed from the point B is measured.

In step S402, the opening angle θ is displayed on the display unit of the operation panel 1a. By displaying the measurement result on the display, the reflection of the light beam is detected,
It is confirmed that the measurement of the opening angle θ has been performed.

In step S403, the operator determines whether or not the measurement result of the opening angle θ is displayed on the display unit. When the display is confirmed, the confirmation switch (not shown) on the operation panel 1a is operated to perform step
Proceed to S404.

If the reflector 6, the light-emitting device 2, the light-receiving device 3 and the like are contaminated, the reflection of the light beam is not normally received, and the measurement result is not displayed on the display. In such a case, the reflector 6
After removing obstacles such as dirt, the user operates a reset switch (not shown) on the operation panel to return to step S401 and perform measurement again.

In step S404, an input request for the distance l1 is displayed on the display unit.

In step S405, it is determined whether or not the distance l1 has been input. If it is determined that the distance 11 has been input, the process proceeds to step S406.

In step S406, an input request for the distance l2 is displayed on the display unit.

In step S407, it is determined whether or not the distance l2 has been input. If it is determined that the distance l2 has been input, the process proceeds to step S408.

In step S408, distances l1, l2 and opening angle θ are displayed on the display unit.

In step S409, the distances l1, l2 and the opening angle θ are displayed on the display unit, and it is determined whether or not the values are correct. When the display is performed, the process proceeds to step S410.

In step S410, the coordinates of the arrangement positions A and C of each reflector 6 are calculated based on the input distances l1 and l2 and the opening angle θ. Here, point B is the origin, and point B and C
A point passing through the point is defined as the x-axis of the coordinates. The equations for calculating the coordinate values of points A and C are as shown in the flowchart.

In step S411, points A and C calculated in step S410
The coordinate values of the points (reference coordinate values) are stored in the backup memory of the position / traveling direction calculation unit 13.

The reference coordinate values are used as data for detecting the position and traveling direction of the vehicle 1 described below.

The control for detecting the position and the traveling azimuth of the self-propelled vehicle 1 at the coordinates calculated by the above-described procedure is described as a second
Description will be made with reference to FIG. 3 and FIG.

2 and 3, the vehicle 1 is located at a position indicated by a point T, and reflectors 6 are disposed at points A, B, and C in the working area of the vehicle 1. . The positions of the self-propelled vehicle 1 and the reflectors 6 are represented in an xy coordinate system in which the point B is the origin and the line passing through the points B and C is the x axis.

As can be seen from the figure, the position T of the self-propelled vehicle 1 is a triangle BT
At the same time as being on the circumcircle of C, it is also on the circumcircle of the triangle ATB. Therefore, the position of the self-propelled vehicle 1 can be determined by calculating the two intersections of the circumscribed circles P and Q of the triangle BTC and the triangle ATB, respectively.

Since the point B is the origin of the coordinates, the position of the self-propelled vehicle 1 can be obtained by calculating the other intersection T of the circumscribed circles P and Q according to the following procedure.

First, the center of the circumcircle P of the triangle BTC is represented by P
Then, P is on the perpendicular bisector of the line segment BC, and ∠BPW ′ = β from the relationship between the central angle and the circumferential angle.

Here, W 'is a point on the perpendicular bisector of the line segment BC, and is assumed to be sufficiently far away from the point T with respect to the straight line BC.

Focusing on the triangle BPW (W is the midpoint of the line segment BC), the coordinates of the center of the circle P are {xc / 2, (xc / 2) cotβ} and the radius is | xc / (2sinβ) | P is represented by the following equation.

(X−xc / 2) ² + {y− (xc / 2) cotβ} ² = {xc / (2sinβ)} ² Further, the following expression is obtained by rearranging the expression.

x ² −xc · x + y ² −xc · y · cotβ = 0 (1) Further, the center of the circumscribed circle Q of the triangle ATB is represented by Q
Then, Q is on the perpendicular bisector of line segment AB, and ∠BQV ′ = α.

Here, V 'is a point on the perpendicular bisector of the line segment BA, and is assumed to be sufficiently far away from the point T with respect to the straight line BA.

Focusing on the triangle BQV (V is the middle point of the line segment BA), the coordinates of the center of the circle Q are {xa / 2 + (ya / 2) cotα, ya / 2− (xa / 2) cotα} and the radius is | √xa ² + ya ² / (2sinα) |
Is represented by the following equation.

x ² −x (xa + ya · cotα) + y ² −y (ya−xa · cotα) = 0 (2) From the above equations (1) and (2), the coordinates (x, y) of the point T are given by the following equations. Is calculated.

x = xc {(1 + k · cotβ) / (1 + k ² )} (3) y = kx (4) where k = (xc−xa−yc · cotα) / (ya−xa · cotα−xc ·) cotβ) (5) represents the slope of the straight line BT.

The traveling direction of the self-propelled vehicle 1 is calculated as follows. In FIG. 3, the angle between the traveling direction of the self-propelled vehicle 1 and the x-axis is θf, and the reflector 6 based on the traveling direction is referred to as θf.
The rotation angles up to (points A, B, C) are θa, θb, θc
Then, since the line segment BT inclination is k, θf = 180 ゜ − (θb−tan ⁻¹ k) (6) Next, the position information of the self-propelled vehicle 1 calculated by the above procedure is Based on this, the steering control of the self-propelled vehicle 1 will be described. FIG. 6 is a view showing a traveling course of the self-propelled vehicle 1 and an arrangement state of the reflector 6, and FIG. 7 is a flowchart of steering control.

In FIG. 6, points A, B, and C indicate the positions where the reflectors 6 are arranged. The coordinate system of the self-propelled vehicle 1 has a coordinate system with the point B as the origin and a line passing through the points B and C as the x-axis. Location and work area 22
Is represented. (Xret, Yret) indicates the return position of the self-propelled vehicle 1, and the work area 22 has coordinates (Xst, Yst), (Xst, Ye),
This is an area connecting points indicated by (Xe, Yst) and (Xe, Ye).

In the figure, the coordinates of the position T of the self-propelled vehicle 1 are (Xp, Yp).

FIG. 6 shows an example in which four sides of the work area 22 are parallel to the x-axis or the y-axis for the sake of simplicity, but the reflector 6 may be provided around the work area 22. If so, the orientation of the work area 22 is arbitrary.

The control procedure will be described with reference to the flowchart of FIG.

First, in step S1, the current position (Xret, Yret) of the self-propelled vehicle 1 calculated by the position / traveling direction calculation unit 13
Then, the steering amount of the front wheel 17 of the self-propelled vehicle 1 is calculated in the steering unit 14 based on the coordinates (Xst, Yst) of the work start position set in the traveling course setting unit 16.

In step S2, the front wheel 17 is steered by the steering unit 14 and the engine is started by the drive unit 18 in the direction determined by the steering amount, and the self-propelled vehicle 1
To advance the work start position (Xst, Yst).

In step S3, Xst is set as the traveling course Xn, and the traveling course is determined.

When the traveling of the self-propelled vehicle 1 is started in step S4, the self-propelled vehicle 1 calculates the own position (Xp, Yp) and the traveling direction θf (step S5).

In step S6, the deviation amount from the traveling course (ΔX = Xp−
Xn, Δθf) are calculated, and in step S7, steering angle control is performed by the steering unit 14 according to the deviation amount.

In step S8, it is determined whether the self-propelled vehicle 1 is traveling in the y-axis direction in a direction away from the origin (outbound direction) or in a direction approaching the origin (return direction).

If it is for the direction, it is determined in step S9 whether one stroke has been completed (Yp> Ye), and if it is the return direction, in step S10, whether or not one stroke has been completed (Yp <Yst). Is determined. In step S9 or S10,
If it is determined that one process is not completed, steps S5 to S8
Is performed.

If it is determined in step S9 or S10 that one stroke has been completed, then it is determined in step S11 whether all the strokes have been completed (Xp> Xe).

If all the strokes have not been completed, the process moves from step S11 to step S12 to control the U-turn of the self-propelled vehicle. In step S13, (Xn + L) is set in Xn, and the next traveling course is set. . If the next traveling course is set, the process returns to step S5, and the above processing is performed.

When all the steps are completed, the process returns to the return position (Xret, Yret) (step S14), and the traveling is stopped (step S14).
15).

The U-turn control in step S12 does not depend on the processing in steps S5 to S7 in which the position information of the self-propelled vehicle 1 calculated by the position / traveling direction calculation unit 13 is fed back to the steering unit 14, and is set in advance. It is performed according to the program.

For example, at the time of transition from straight running to turning, the steering angle of the self-propelled vehicle 1 is fixed to a fixed angle and the turning operation is performed.
When the self-propelled vehicle 1 reaches the scheduled position, the fixed steering angle is released, and the vehicle shifts to straight running again.

In the above embodiment, an example is described in which the coordinates (Xst, Yst) of the work start position with respect to the origin B (0, 0) are set in the traveling course setting unit 16 in advance. In addition, self-propelled car 1
Can be guided to an arbitrary position by wireless guidance or the like, the position is defined as a work start position (Xst, Yst), and traveling can be started.

As described above, in the present embodiment, first, of the reflectors 6 installed at three locations, the distance from one reflector to the other two is measured in advance. Next, the opening angle of the reflector 6 with respect to the other 2 as viewed from the installation position of the one reflector 6 is measured by the opening angle detecting means provided on the vehicle 1. Then, based on the distance information and the opening angle, the coordinates of each reflector 6 having the one reflector as the origin are calculated, and based on the coordinate values of the reflector 6, the current position and the traveling direction of the self-propelled vehicle 1 are calculated. Is calculated.

As described above, in the present invention, by using the opening angle detection function provided in the self-propelled vehicle 1, even if all the distances between the reflectors installed in the work area are not known, the self-propelled vehicle 1 The correlation positional relationship with the reflector 6 can be detected, and the steering control of the self-propelled vehicle 1 can be performed based on the detection result.

Further, the distance between the reflectors arranged at two places among the reflectors 6 arranged at the three places is measured, and the result is set in a distance setting unit 8, and the distance and the two places are measured. Even if the reference coordinates are calculated based on the opening angle between the other two reflectors viewed from the reflector 6, the same effect as in the above embodiment can be obtained.

(Effects of the Invention) As is clear from the above description, according to the present invention, the following effects can be achieved.

(1) Opening angle information of a reflector obtained by an angle detecting function mounted on a self-propelled vehicle for detecting its own position and minimum distance information serve as a reference point for work traveling. Reference coordinates can be easily calculated.

(2) Even if the working area of the self-propelled vehicle changes and the light reflectors are re-installed each time, it is not necessary to measure all the distances between the reflectors. Saves time.

[Brief description of the drawings]

FIG. 1 is a block diagram showing an embodiment of the present invention, FIG. 2 is a diagram for explaining the principle of position detection of a self-propelled vehicle, FIG. 3 is a diagram for explaining the principle of traveling direction detection of a self-propelled vehicle, and FIG. a) is a flowchart showing a work preparation procedure, FIG. 6 (b) is a layout diagram of reflectors, FIG. 5 is a flowchart of coordinate calculation control, and FIG. FIG. 7 is a flowchart of steering control, and FIG. 8 is a perspective view of a self-propelled vehicle and a reflector. DESCRIPTION OF SYMBOLS 1 ... Self-propelled vehicle, 2 ... Light emitting device, 3 ... Light receiving device, 4 ... Rotary table, 5 ... Motor, 6 ... Reflector, 7 ... Rotary encoder, 8 ... Distance setting unit, 9 ……counter,
10 ... Angle detection unit, 11 ... Coordinate calculation unit, 13 ... Position and heading calculation unit, 14 ... Steering unit

Claims (4)

(57) [Claims] 1. A light beam generated from a self-propelled vehicle is scanned in a circumferential direction around the self-propelled vehicle, so that light beams are reflected by light reflecting means installed at at least three places away from the self-propelled vehicle. In a traveling position control device for a self-propelled vehicle that detects reflected light of the light beam and detects a position of the self-propelled vehicle, a light beam generating unit that is provided for the self-propelled vehicle and generates the light beam; Light beam scanning means provided on the self-propelled vehicle and scanning the light beam in a circumferential direction around the self-propelled vehicle, and light receiving means provided on the self-propelled vehicle and receiving the reflected light of the light reflecting means Detecting means for detecting an opening angle between the light reflecting means viewed from the self-propelled vehicle based on a light receiving output of the light receiving means, and one of the light reflecting means measured by an arbitrary means. Distance to the other two light reflecting means, and said opening Means for calculating the coordinates of the two light reflecting means based on the opening angle of the other two light reflecting means centered on the installation position of the one light reflecting means detected by the detecting means; A traveling position control device for a self-propelled vehicle, comprising: means for calculating a position of the self-propelled vehicle based on an opening angle between the three light reflecting means and coordinates of the light reflecting means as viewed from the side. 2. A light beam generated from a self-propelled vehicle is scanned in a circumferential direction around the self-propelled vehicle, whereby light beams are reflected by light reflecting means installed at at least three places away from the self-propelled vehicle. In a traveling position control device for a self-propelled vehicle that detects reflected light of the light beam and detects a position of the self-propelled vehicle, a light beam generating unit that is provided in the self-propelled vehicle and generates the light beam; Light beam scanning means provided on the self-propelled vehicle and scanning the light beam in a circumferential direction around the self-propelled vehicle, and light receiving means provided on the self-propelled vehicle and receiving the reflected light of the light reflecting means A detecting means for detecting an opening angle between the light reflecting means viewed from the self-propelled vehicle based on a light receiving output of the light receiving means; and a detecting means for detecting an opening angle between the two light reflecting means measured by an arbitrary means. Distance, and detected by the opening angle detecting means. Means for calculating the coordinates of two of the light reflecting means based on the opening angles of the other two light reflecting means centered on the respective installation positions of the two light reflecting means; Running position control for the self-propelled vehicle, comprising means for calculating the position of the self-propelled vehicle based on the opening angle between the three light reflecting means and the coordinates of the light reflecting means as viewed from the self-propelled vehicle. apparatus. 3. Coordinates of said light reflecting means are coordinates with one of said light reflecting means as an origin and a straight line passing through one of said one light reflecting means and one of said other light reflecting means as one coordinate axis. 3. The method according to claim 1, wherein the calculation is performed on a system.
The traveling position control device for a self-propelled vehicle according to the above. 4. A self-propelled vehicle is provided with an operation panel, and a distance from one of the light reflecting means to two other light reflecting means measured by an arbitrary means is inputted from the operation panel. 4. The traveling position control device for a self-propelled vehicle according to claim 1, 2 or 3, wherein