JPH0783673A - Azimuth estimation device of traveling body - Google Patents

Abstract

PURPOSE:To reduce the error between an estimated and calculated azimuth and an actual carrier azimuth by canceling the drifting of an optical fiber gyro for each temporary stop while an autonomously guided type carrier is driving. CONSTITUTION:When an autonomously guided type carrier 10 drives along a set driving path, an induction control routine is executed at a driving control part 14b, a resetting instruction routine is executed at a specific time interval, and at the same time, a processing for safety check, etc., is executed, for example a top for safety check at an intersection. When the carrier 10 stops, no pulse signals of rotary encoders 28a-28d are output and hence the driving control part 14b instructs an azimuth estimation part 14a to reset a reference value. In this manner, by resetting the reference value for each stop of the carrier 10, the drifting of an optical fiber gyro 30 can be eliminated and the error between the azimuth which is estimated by the azimuth estimation part 14a and the actual azimuth of the carrier 10 is reduced.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a moving body azimuth estimating apparatus which is mounted on a moving body such as an autonomous guided vehicle and estimates and calculates the traveling direction of the moving body.

[0002]

2. Description of the Related Art Conventionally, a traveling control device having a distance estimating means for detecting and calculating a moving distance by detecting the rotational speed of a wheel and an azimuth estimating means for detecting and calculating a azimuth by detecting the rotation of a moving body is mounted. Then, an autonomous guided vehicle is known in which the vehicle is controlled to travel along a preset travel route. As an azimuth estimation device mounted on a moving body such as this autonomous guided vehicle, an optical fiber gyro that detects the rotation of the moving body, and an azimuth estimation that estimates and calculates the azimuth of the moving body based on the output of the optical fiber gyro There is known a azimuth estimating device for a moving body, which is provided with a means.

This optical fiber gyro has a characteristic that it has a very short rise time and is resistant to vibrations and shocks, and is mounted on an autonomous guided vehicle which is expected to be quickly transported on a not-smooth road surface such as a factory. Can be said to be suitable for. However, as is well known, in optical fiber gyros, drift due to various noise factors such as the optical path difference between the two lights, the frequency difference between the two lights in the fiber, and the fluctuation of the polarization state is unavoidable. It is necessary to cancel the drift after every running time. This drift cancellation operation needs to be performed with the optical fiber gyro, that is, the autonomous guided vehicle stopped, and for example, the autonomous guided vehicle is stopped for each fixed-point sign installed on or near the travel route. , The drift of the optical fiber gyro is canceled, and the position and traveling direction errors estimated and calculated based on the output of the optical fiber gyro are corrected.

[0004]

However, stopping the autonomous guided vehicle for each of a large number of fixed-point markers reduces transport efficiency, and installation of fixed-point markers requires a local burden and installation. The time burden was also heavy.

[0005]

In order to solve the above problems, the present invention employs the following means. That is, FIG.
As illustrated in 0, the device for estimating a direction of a moving body of the present invention is an optical fiber gyro that is attached to the moving body and detects the rotation of the moving body, and the direction of the moving body based on the output of the optical fiber gyro. In the azimuth estimating apparatus for a mobile body, which includes a azimuth estimating means for estimating and calculating, a stop determining means for determining the stop of the mobile body is provided, and the azimuth estimating means determines when the mobile body is stopped. The output of the optical fiber gyro is reset as a value corresponding to the non-rotating state of the moving body.

[0006]

In the azimuth estimating apparatus having the above structure, the optical fiber gyro detects the rotation of the moving body. The azimuth estimating means estimates and calculates the azimuth of the moving body based on the output of the optical fiber gyro.

[0007] For example, an autonomous guided vehicle travels along a preset traveling route, and during the traveling, safety confirmation is made when crossing the route of another vehicle, and when the presence of an oncoming vehicle is detected, The vehicle may be temporarily stopped when an obstacle is detected ahead of its own traveling direction. When such a stop of the moving body is judged by the stop judging means, the direction estimating means resets the output of the optical fiber gyro as a value corresponding to the non-rotating state of the moving body. Specifically, even though the optical fiber gyro (moving body) is in a stopped state, the output signal of the optical fiber gyro output according to the drift up to that point is used when the moving body is not rotating. Is set as an output signal having a value corresponding to. As a result, the drift amount of the optical fiber gyro is eliminated in the subsequent estimation calculation, so that the error between the estimated calculation direction and the actual direction of the moving body becomes extremely small.

Since the drift of the optical fiber gyro is canceled at each temporary stop as exemplified above, for example, the autonomous guided vehicle travels along the set travel route faithfully. Further, there is no need to install a special fixed point marker for stopping the autonomous guided vehicle and executing the drift cancellation.

[0009]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT A preferred embodiment of the present invention will now be described with reference to the drawings. The present embodiment is an example of an autonomous guided vehicle equipped with the orientation estimation device of the present invention. As shown in FIG. 1, an autonomous guided vehicle 10 includes a cargo bed 12 for placing articles to be conveyed and a diesel engine (not shown) as a drive source.
And a storage unit 1 in which a control device 14 for traveling control is stored
The structure is roughly divided into 6.

Swivel shafts 18a, 18b, 18c, 18d are erected at four positions on the lower surface side (road surface side) of the cargo bed portion 12, and the swivel shafts 18a to 18d are swivel shafts 18a to 18d.
Axles 20a, 20b, 20c, 2 which can be swiveled around d
0d is attached. An actuator (not shown) is attached to each of the axles 20a to 20d, and the actuators are operated in accordance with an instruction from the control device 14, so that each of the axles 20a to 20d can be swivel-driven about the swing shafts 18a to 18d. In addition, each axle 20a-20d
The inner ring 22a, 22b, 22c, 22d and the outer ring 2
4a, 24b, 24c and 24d are attached. These inner and outer wheels 22a-24d are hydraulic motors 26a, 26b, 26c, 26d driven by hydraulic oil from a hydraulic pressure generator (not shown) connected to the diesel engine.
It is a structure that is rotated by transmitting the rotation of. Further, rotary encoders 28a, 28b installed on the sides of the axles 20a-20d facing the hydraulic motors 26a-26d,
28c and 28d detect the rotation of the axles 20a to 20d and output pulse signals Pa to Pd according to the number of rotations. The output signals of these rotary encoders 28a to 28d are input to the control device 14 via a path (not shown).

Each of the swivel shafts 18a to 18d is mounted on the cargo bed 12.
An optical fiber gyro 30 is installed at a position approximately in the center of a rectangle having an apex. The optical fiber gyro 30 is installed so that its axis is substantially vertical, and when the autonomous guided vehicle 10 rotates about the vertical axis,
An angular velocity signal S corresponding to the angular velocity of this rotation is output. The angular velocity signal S of the optical fiber gyro 30 is input to the control device 14 via a path (not shown).

As shown in FIG. 2, the control device 14 is composed of an azimuth estimating unit 14a and a traveling control unit 14b. These direction estimating unit 14a and traveling control unit 14b
Each has a well-known microcomputer equipped with a CPU, a RAM, a ROM, an A / D converter, etc., and can execute various processes according to a program stored in advance.

The azimuth estimating unit 14a is an optical fiber gyro 3
0, which is electrically connected to the optical fiber gyro 30 according to the direction estimation routine shown in FIG.
It has a function of reading, integrating this, and storing it in a predetermined memory as azimuth data. In the azimuth estimation routine, first, the angular velocity signal S of the optical fiber gyro 30 is read (step 110). Then, the reference value B corresponding to the value of the angular velocity signal S and the non-rotating state of the autonomous guided vehicle 10.
Then, the difference D between and is calculated, and the direction of the autonomous guided vehicle 10 is estimated and calculated by the integrated value of the difference D (step 120). Further, the data obtained by the estimation calculation is stored in a predetermined memory as orientation data (step 130). This azimuth estimation routine is repeatedly executed at predetermined time intervals.

In the azimuth estimating unit 14a, the reference value B
The resetting process is also executed, but the details will be described later. On the other hand, the traveling control unit 14b is electrically connected to the rotary encoders 28a to 28d, and corresponds to the rotation of the axles 20a to 20d according to the pulse signals Pa to Pd of the rotary encoders 28a to 28d.
The cumulative rotation speeds of 22d and 24a to 24d are calculated, and thereby the moving distance of the autonomous guided vehicle 10 is estimated and calculated. Further, the traveling control unit 14b reads the azimuth data calculated by the azimuth estimating unit 14a, and estimates and calculates the latest position (coordinates) of the autonomous guided vehicle 10 based on the estimated moving distance. These coordinates have the starting point as the origin, and are set so that the point corresponding to the center of the optical fiber gyro 30 is calculated as the coordinates of the autonomous guided vehicle 10. Further, the steering amounts of the axles 20a to 20d are controlled so that the autonomous guided vehicle 10 travels along the set travel route according to the deviation between the travel route data stored in advance and the coordinates. . This estimation calculation is repeatedly performed at preset sampling timings, and the calculation formulas thereof are the following formulas (1) and (2).
As shown in.

[0015]

[Equation 1]

However, n: sampling times ΔL: moving distance during sampling Δθ: azimuth change amount during sampling X _n-1 , Y _n-1 : coordinates X _{n of} the previous autonomous guided vehicle 10, Y _n : Coordinates of the current autonomous guided vehicle 10 θ _n-1 : Direction of the previous autonomous guided vehicle 10 Here, the position (coordinates) estimated and calculated by the traveling control unit 14b as described above and A procedure for controlling the traveling of the autonomous guided vehicle 10 based on the azimuth data will be described with reference to FIG.

First of all, the traveling control unit 14b is used for the azimuth data and pulse signals Pa to P of the rotary encoders 28a to 28d.
d, the position signal of the actuator corresponding to the steering amount of each axle 20a to 20d is fetched (step 21).
0). Next, by the calculation shown in the above equations (1) and (2),
The coordinates of the autonomous guided vehicle 10 are estimated and calculated (step 220).

In the next step 230, step 220
The deviation between the coordinate calculated in step 1 and the travel route data, that is, the deviation EN between the current position of the autonomous guided vehicle 10 and the set travel route is estimated. The deviation EN is shown in FIG.
As shown in, the normal line of the traveling route and the autonomous guided vehicle 10
Corresponds to the distance from the set center (center axis of the optical fiber gyro 30). In addition, when the deviation EN exceeds a predetermined value, the traveling control unit 14b determines that the vehicle is off the course and performs a process of stopping the traveling of the autonomous guided vehicle 10.

In step 240, the autonomous guided vehicle 1
When 0 is continued from the current position and the traveling conditions such as the current speed, acceleration, and steering angle are maintained and the vehicle travels a predetermined distance L, the position where the autonomous guided vehicle 10 is expected to exist is predicted and calculated. To do. For example, as illustrated in FIG. 5, it is assumed that the vehicle travels the distance L while maintaining the assumed traveling route R, and the expected position Y of the autonomous guided vehicle 10 at that time is calculated.

In the following step 250, the previous step 24
A deviation ENL between the predicted position Y calculated by 0 and the set travel route is calculated, and a steering amount that makes this deviation 0 is calculated. By sending a steering amount signal corresponding to the calculated steering amount to the actuator, the axle 2
The steering amount of 0a to 20d is controlled (step 260).
Further, the current position, speed, and other states are transmitted to the command center, and this routine ends (step 270). It should be noted that this traveling control routine is repeatedly executed at predetermined time intervals.

In addition to executing the above-mentioned traveling control, the traveling control unit 14b follows the reset instruction routine shown in FIG.
The azimuth estimating unit 14a is instructed to reset the reference value B when the azimuth of the autonomous guided vehicle 10 is estimated and calculated by the above azimuth estimating routine. First, in step 310, it is determined whether or not the pulse signals Pa to Pd of the rotary encoders 28a to 28d have been input within a predetermined time. Here, when none of the pulse signals Pa to Pd is input within the predetermined time, the traveling control unit 14b determines that the autonomous guided vehicle 10 is stopped, and instructs the azimuth estimation unit 14a. Then, a signal for instructing the resetting of the reference value B when the direction of the autonomous guided vehicle 10 is estimated and calculated by the above-described direction estimation routine is output, and this routine is ended (step 32).
0). In step 310, one or more pulse signals P
This routine also ends when a to Pd are input.
After that, this routine is repeatedly executed at predetermined time intervals.

On the other hand, the azimuth estimating unit 14a includes the traveling control unit 1
In response to the resetting command signal from 4b, the resetting routine shown in FIG. 7 is used to set the reference value B for estimating and calculating the azimuth of the autonomous guided vehicle 10 in the azimuth estimating routine. The angular velocity signal S of 30 is newly set as the reference value B corresponding to the non-rotating state of the autonomous guided vehicle 10 (step 410). This reset routine is repeatedly executed every time there is a reset instruction from the traveling control unit 14b, and a new reference value B is used in the subsequent orientation estimation routine.

In addition to the functions described above, the traveling control unit 1
Signals from sensors (not shown) are input to 4b, and when, for example, an obstacle or the like is detected, processing such as instructing stop of the autonomous guided vehicle 10 can be executed. Further, the control device 14 is wirelessly connected to a command center (not shown), transmits and receives data to and from the command center, performs arithmetic processing based on an instruction transmitted from the command center, and outputs various control signals. It is also possible to remotely control the autonomous guided vehicle 10 by, for example, an instruction from a command center.

The autonomous guided vehicle 10 is also provided with a device for manual traveling, which allows an operator on board to perform manual traveling, and has various well-known structures. Are omitted from the drawings and description. Next, a case where the autonomous guided vehicle 10 travels by autonomous guidance will be described.

Prior to traveling by autonomous guidance, map data relating to a preset traveling route is input to the traveling control unit 14b of the control device 14 and stored therein, and traveling preparation processing such as initialization of various variables and flags is performed. Be implemented.
Here, the map data stored in the traveling control unit 14b will be described with reference to FIG. The map data is converted into data according to the traveling route C in which straight lines and curved lines are continuous. Each straight line portion and curved line portion of the travel route C is divided into a plurality of sections Q _x by each connection point or inflection point between the straight line and the curve, and the map data includes these sections Q _x.
It is configured as a collection of section data relating to shapes such as distance and curvature for each, and is stored in a predetermined memory.

During the autonomous guidance travel, the traveling control unit 14b controls the autonomous guided vehicle 10 to travel along the set traveling route C according to the autonomous guidance routine shown in FIG. When the autonomous guidance routine is started after the above-described running preparation processing is executed, it is first determined whether or not the section data should be read (step 510). However, immediately after the start of this routine, the section data corresponding to the first section Q ₁ is set to be read and stored. Subsequent determination, by comparison of the travel distance and the interval data calculated by integration of the pulse signal Pa~Pd the rotary encoder 28a to 28d, and enters the section Q _{n + 1} through, for example, interval Q _n When it is determined, it is determined that the new section data needs to be read.

If it is determined that the section data needs to be read, the section data is updated in step 520. That is, the section data corresponding to the new section Q _{n + 1} is read from the map data and stored in a predetermined memory, and the section data of the old section is erased from the memory.

When it is determined in step 510 that the section data is not required to be read or after the section data is read in step 520, the autonomous guided vehicle 10 starts running, and the above-described running control routine is executed every predetermined sampling time. It is executed (step 530). The azimuth estimating unit 1 is separately provided.
Even at 4a, the azimuth estimation routine is executed at predetermined time intervals. Therefore, the traveling control unit 14b can read the azimuth data.

Next, in step 550, it is determined whether the autonomous guided vehicle 10 has finished traveling along the set traveling route (step 560). This determination is made by comparing the travel distance based on the pulse signals Pa to Pd of the rotary encoders 28a to 28d with the map data. If it is determined that the processing has not ended, the process returns to step 510,
If it is determined that the traveling has ended, the process proceeds to step 570.
In step 570, a process for stopping the autonomous guided vehicle 10 is executed. In the next step 580, the end state is transmitted to the command center, and the guide traveling routine is ended.

As described above, the autonomous guided vehicle 10
Travels along the set travel route C. At the time of this traveling, in the traveling control unit 14b, in addition to the above-described guidance control routine, a reset instruction routine is executed at predetermined time intervals, and processing such as safety confirmation is also executed, for example, at an intersection. A stop for safety confirmation is made. When the autonomous guided vehicle 10 stops, the pulse signals Pa to Pd of the rotary encoders 28a to 28d are not output, so the traveling control unit 14b instructs the azimuth estimating unit 14a to reset the reference value B (resetting instruction routine. ). In the azimuth estimating unit 14a that has received this instruction, the reference value B is newly set by the resetting routine. Therefore, the reference value B for estimating and calculating the azimuth based on the angular velocity signal S of the optical fiber gyro is the angular velocity signal S of the optical fiber gyro 30 at each time when the autonomous guided vehicle 10 is stopped, for example, at an intersection. Is newly set based on.

By resetting the reference value B every time the autonomous guided vehicle 10 is stopped in this manner, the drift amount of the optical fiber gyro 30 is eliminated, so that the azimuth estimating unit 14a is used.
Azimuth estimated and calculated by the actual autonomous guided vehicle 10
The error from the azimuth is extremely small. Therefore, the autonomous guided vehicle 10 can travel along the set travel route C faithfully. Further, it is not necessary to stop the autonomous guided vehicle 10 and install a special fixed point marker for canceling the drift.

Although the embodiment has been described above, in the above embodiment, the bearing estimating unit 14a corresponds to the bearing estimating means of the present invention, and the traveling control unit 14b and the rotary encoders 28a to 28d correspond to the stop determining means. Further, the present invention is not limited to such an embodiment, and can be variously implemented without departing from the scope of the present invention.

For example, if a fixed point is installed on the traveling route and a position correcting device equipped with a well-known CCD camera is mounted on an autonomous guided vehicle, for example, a displacement such as a skew or a parallel movement due to a slip that cannot be detected by an optical fiber gyro. By correcting the traveling error due to, it becomes possible to perform more accurate autonomous guidance traveling. However, since it is not necessary to stop the traveling in the position correction by the position correction device equipped with this CCD camera, it is not necessary to temporarily stop in the vicinity of the fixed point, and the number of fixed points may be smaller than in the past.

Further, the above embodiment shows an example in which the moving body direction estimating apparatus of the present invention is mounted on an autonomous guided vehicle driven by a hydraulic motor whose hydraulic pressure source is a diesel engine. You can also use a gasoline engine. Further, it can be used for estimating the direction of various moving bodies other than the autonomous guided vehicle.

[0035]

As described above, in the moving body orientation estimating apparatus of the present invention, the drift of the optical fiber gyro is canceled every time the moving body is stopped. As a result, the drift amount of the optical fiber gyro is eliminated in the subsequent estimation calculation, so that the error between the estimated calculation direction and the actual direction of the moving body becomes extremely small.

If this azimuth estimating device is mounted on, for example, an autonomous guided vehicle, the autonomous guided vehicle can travel along the set traveling route faithfully. Also,
There is also no need to install a special fixed-point sign to stop the autonomous guided vehicle and perform drift cancellation.

[Brief description of drawings]

FIG. 1 is an explanatory diagram of a configuration of an autonomous guided vehicle according to an embodiment.

FIG. 2 is an explanatory diagram of signal paths in the autonomous guided vehicle of the embodiment.

FIG. 3 is a flowchart of an azimuth estimation routine in the azimuth estimation unit of the embodiment.

FIG. 4 is a flowchart of a travel control routine in the travel control unit of the embodiment.

FIG. 5 is an explanatory diagram of position prediction in the autonomous guided vehicle of the embodiment.

FIG. 6 is a flowchart of a reset instruction routine in the traveling control unit according to the embodiment.

FIG. 7 is a flowchart of a reference value resetting routine in the azimuth estimating unit of the embodiment.

FIG. 8 is an exemplary diagram of a travel route in the autonomous guided vehicle of the embodiment.

FIG. 9 is a flowchart of an autonomous guidance routine in the traveling control unit of the embodiment.

FIG. 10 is an exemplary diagram of a configuration of an azimuth estimation device of the present invention.

[Explanation of symbols]

Reference numeral 10 ... Autonomous guided vehicle, 14 ... Control device (direction estimation device), 14a ... Direction estimation unit (direction estimation unit), 14b ... Travel control unit (stop determination unit), 28a- 28d ... Rotary encoder (stop determination means), 30 ... Optical fiber gyro.

Claims (1)

[Claims] 1. A moving body equipped with an optical fiber gyro attached to the moving body for detecting rotation of the moving body, and a direction estimating means for estimating and calculating the direction of the moving body based on the output of the optical fiber gyro. In the azimuth estimating device, the stop determining means for determining the stop of the moving body is provided, and the azimuth estimating means outputs the output of the optical fiber gyro when it is determined that the moving body is stopped, An azimuth estimating apparatus for a mobile body, which is reset as a value corresponding to a rotation state.