JP2002333341A - Device for detecting location of movable body - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a location detecting device capable of reducing errors as much as possible in the case of detecting the direction of movement of a movable body by a gyro and highly accurately detecting the location of the movable body. SOLUTION: In the case of detecting the location of the movable body from the distance traveled by the movable body on the basis of the number of rotations of left and right wheels 5 and 6 detected by encoders 1 and 2 and the direction of movement of the movable body detected by the gyro 3, the difference in the number of rotations between the wheels 5 and 6 is multiplied by the inverse number of the tread between the wheels 5 and 6 to obtain the amount of direction change dpe of the movable body. The amount of direction change dpj of the movable body is detected by the gyro 3. By a selecting switch 23, the amount of change dpe in direction is selected in a region in which an angular velocity is small, and the amount of direction change dpj is selected in a region in which an angular velocity is large.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a position detecting device for a moving object, and is particularly useful when applied to detecting the position of a moving object such as a forklift.

[0002]

2. Description of the Related Art For example, as a position detecting device of a moving body such as a forklift, a short-time movement of the moving body is detected by a sensor (encoder, gyro, etc.) attached to the moving body, and the movement is integrated. A device for estimating a position has been proposed. FIG. 3 shows a moving body employing such a position detecting device. As shown in the figure, in the moving object, the output signals of the encoders 1 and 2 and the gyro 3
The position is detected by performing a predetermined calculation in the position calculator 3 on the basis of the output signal. More specifically, the encoders 1 and 2 include wheels 5 and 6 attached to the vehicle body 4.
The number of output pulse signals is counted by counters 7 and 8, and a number-of-rotations signal representing the number of rotations of the wheels 5 and 6 is sent to the position calculator 9. On the other hand, the gyro 3 sends the output signal to the position calculator 9 as a direction change signal converted into a digital signal by the A / D converter 10. The position calculator 9 calculates the position of the moving object based on the rotation speed signal and the direction change signal.

FIG. 4 is a block diagram showing a specific configuration of the position calculator 9 together with sensors such as the encoders 1 and 2 and the gyro 3. As shown in FIG.
In addition, the adder 11 adds the output signals of the counters 1 and 2 to both rotation speed signals, and the multiplier 12 multiplies it by 0.5 to calculate an average rotation speed based on the both rotation speed signals.
The number of pulses set in the coefficient setting unit 13 is multiplied by a coefficient K for converting the distance into a distance, and is converted into a moving distance dl. On the other hand, the traveling direction p is obtained by time-integrating the direction change dp, which is the output signal of the A / D converter 10, by the integrator 14.
At steps 5 and 16, the cos component and the sin component of the traveling direction p are calculated, respectively. The multipliers 17 and 18 decompose the moving distance dl into two orthogonal components (X-axis direction component and Y-axis direction component) by multiplying the moving distance dl by the cos component and the sin component, respectively. The X-axis component and the Y-axis component of the moving position are obtained by integrating the obtained components with time by the integrators 19 and 20, respectively. Finally, the movement position to be detected is obtained by combining the two.

[0004]

In the prior art as described above, the traveling distance detected based on the rotation speed of the wheels 5 and 6 has an error due to slippage of the wheels 5 and 6, and as a result, It becomes a detection error of the moving position. The occurrence of such an error is particularly remarkable in a high acceleration region of the moving body. On the other hand, when the moving azimuth of the moving object is detected by the inexpensive gyro 3, a detection error occurs due to offset, temperature drift, noise, and the like. The occurrence of such an error
This is particularly noticeable in the low angular acceleration region.

SUMMARY OF THE INVENTION In view of the above prior art, the present invention is directed to a position detecting device capable of detecting a moving direction of a moving object by a gyro as much as possible to detect the position of the moving object with high accuracy. The purpose is to provide.

[0006]

The structure of the present invention that achieves the above object has the following features.

1) The traveling distance is detected based on the circumference and the rotation speed of each wheel of the moving body moving via the left and right wheels, and the moving distance and the moving direction are detected by detecting the moving direction of the moving body. In the position detection device for a moving body that detects the position of the moving body based on the information of the first moving body, the difference between the rotation speeds of the left and right wheels is multiplied by the reciprocal of the tread that is the distance between the left and right wheels. While the direction change amount is obtained, the second direction change amount of the moving body is detected by the gyro, and when the direction change amount is equal to or less than the set value, the first direction change amount is selected and when the value exceeds the set value. Means that the second direction change amount is selected to obtain information on the moving direction based on the first or second direction change amount.

2) The traveling distance is detected based on the circumference and the rotation speed of each wheel of the moving body that moves via the left and right wheels, and the moving distance and the moving direction are detected by detecting the moving direction of the moving body. In the position detection device for a moving body that detects the position of the moving body based on the information of the first moving body, the difference between the rotation speeds of the left and right wheels is multiplied by the reciprocal of the tread that is the distance between the left and right wheels. While the direction change amount is obtained and passed through a low-pass filter, the second direction change amount of the moving object is detected by a gyro and passed through a high-pass filter. When the direction change amount is small, the first direction selected by the low-pass filter is used. Information on the moving direction is obtained based on the amount of change and, when the amount of change in direction is large, based on the second amount of change in direction selected by the high-pass filter.

[0009]

Embodiments of the present invention will be described in detail with reference to the drawings. Each of the embodiments described below is an improvement of the position calculator 9 shown in FIG. Therefore,
In each figure, the same parts as those in FIG. 4 are denoted by the same reference numerals, and duplicate description will be omitted.

<First Embodiment> As shown in FIG.
In the position detection device according to the present embodiment, the movement distance is represented by multiplying the signals representing the rotation speeds of the wheels 5 and 6, which are the output signals of the counters 7 and 8, by the coefficients set in the coefficient setting units 13 and 13, respectively. Each is converted to a signal. The two signals obtained as a result are added by the adder 11 and the multiplier 1
Multiply 0.5 by 2 to get the average travel distance based on both signals as dl
Is obtained. The average moving distance dl is a physical quantity similar to the moving distance dl of the related art shown in FIG. That is, there is no essential difference in the method of calculating the moving distance dl.

In this embodiment, a subtractor 21 calculates a difference between the two moving distances based on signals representing the moving distances, which are output signals of the coefficient setting units 13, 13. Is multiplied by the reciprocal of the tread which is the distance between the axes of the wheels 5 and 6. As a result, a signal representing the direction change amount _dpe of the moving object is obtained as the output signal of the setting unit 22. Since this signal is based on the rotation speed of the wheels 5 and 6,
When the angular velocity is large, the error tends to increase due to slippage of the wheels 5, 6 and the like.

The selection switch 23 switches the switch to provide a signal representing the direction change amount d _pe which is an output signal of the setter 22 or a gyro 3 which is an output signal of the A / D converter 10.
In and selects one of signals representing the detected direction variation d _pj. In this selection, when the direction change amount d _pe > set value L _dp with respect to the set value L _dp based on the predetermined angular velocity, the direction change amount d _pj based on the gyro 3 is selected. On the other hand, when the amount of direction change d _pe ≤ set value L _dp ,
The direction change amount d _pe based on the encoders 1 and 2 is selected.

According to this embodiment, the encoders 1, 2
And the gyro 3 are used to estimate the moving position.
Can be. Here, the direction change based on encoders 1 and 2
Amount d_peIs a highly accurate direction when the angular velocity of the moving object is small.
Change d_peCan be detected, and the direction change amount based on the gyro 3
d_pjIs a highly accurate direction change when the angular velocity of the moving object is large
Quantity d_pjCan be detected. One of the selection switches 23
Direction change amount d_pe, D _pjBut this is then
Based on the angular velocity of the moving object, always changing the direction with high accuracy
Quantity d_pe, D_pjSelect Thus highly accurate direction change
d_pe, D_pjAnd the output signal of the multiplier 12
The information shown in FIG.
Performs the same calculations as in the technology to detect the position of the moving object
I do.

<Second Embodiment> As shown in FIG.
The position detection device according to the present embodiment uses a low-pass filter 24 and a high-pass filter 25 instead of the selection switch 23 in the embodiment shown in FIG. Here, the low-pass filter 24 and the high-pass filter 25 are complementary filters [H (s), 1−H (s)], and the direction change amounts d _pe and d _pj are complementary to each other. The signal is synthesized by the adder 26 through a filter [H (s), 1-H (s)]. That is, in the region where the angular velocity is small, a signal representing the direction change amount d _pe is selected via the low-pass filter 24, and in the region where the angular velocity is large, a signal representing the direction change amount d _pj is selected via the high-pass filter 25. In addition, switching between the direction change amount d _pe and the direction change amount d _pe is continuously performed at the boundary between the two. Accordingly, as in the first embodiment shown in FIG. 1, the output signal of the adder 26 has a more accurate direction change amount d according to the angular velocity.
_The signals representing _pe and d _pj are selected. At this time, the adder 26
Is a signal representing the direction change amount that changes more smoothly and continuously than the output signal of the selection switch 23 in the first embodiment.

[0015]

According to the present invention, the direction change amount based on the signal indicating the number of rotations of the wheel and the direction change amount based on the output signal of the gyro can be expressed by the angular velocity, as specifically described with the above embodiments. Since it is possible to select the direction change amount with high accuracy by using it properly, even when using an inexpensive gyro,
Highly accurate position detection can be performed. Here, in the case where the direction change amount based on the signal indicating the number of rotations of the wheel and the direction change amount based on the output signal of the gyro are properly used by using a filter, continuous quality can be obtained without generating a shock at the time of switching. Can be obtained.

[Brief description of the drawings]

FIG. 1 is a block diagram illustrating a position detection device for a moving object according to a first embodiment of the present invention.

FIG. 2 is a block diagram illustrating a moving object position detecting device according to a second embodiment of the present invention.

FIG. 3 is an explanatory diagram conceptually showing a moving body on which a position detecting device for the moving body is mounted.

FIG. 4 is a block diagram showing a position detecting device for a moving body according to the related art mounted on the moving body of FIG. 3;

[Explanation of symbols]

 1, 2 Encoder 3 Gyro 5, 6 Wheel 11 Adder 12 Multiplier 13 Coefficient setter 21 Subtractor 22 Setter 23 Selection switch 24 Low-pass filter 25 High-pass filter 26 Adder

 ──────────────────────────────────────────────────続 き Continued on the front page F term (reference) 2F024 AA01 AB07 AC01 AF03 2F029 AA08 AC01 AC02 AC04 2F069 AA06 GG59 HH11 HH30 NN03 NN04 3F333 AA02 DA10 FA05 FA28 FD04 FD20 FE10

Claims (2)

[Claims] 1. A travel distance is detected based on a circumference and a rotation speed of each wheel of a moving body that moves via right and left wheels, and a moving direction and a moving direction of the moving body are detected by detecting a moving direction of the moving body. In the position detecting device for a moving body, which detects the position of the moving body based on the information of the first moving object, the difference between the rotation speeds of the left and right wheels is multiplied by the reciprocal of the tread which is the distance between the left and right wheels. While obtaining the direction change amount, the second direction change amount of the moving body is detected by the gyro, and when the direction change amount is equal to or less than the set value, the first direction change amount is selected and when the set value is exceeded. Wherein a second direction change amount is selected to obtain a moving direction information based on the first or second direction change amount. 2. The moving distance is detected based on the circumference and rotation speed of each wheel of the moving body moving via the left and right wheels, and the moving distance and the moving direction are detected by detecting the moving direction of the moving body. In the position detecting device for a moving body, which detects the position of the moving body based on the information of the first moving object, the difference between the rotation speeds of the left and right wheels is multiplied by the reciprocal of the tread which is the distance between the left and right wheels. While the direction change amount is obtained and passed through a low-pass filter, the second direction change amount of the moving object is detected by a gyro and passed through a high-pass filter. If the direction change amount is small, the first direction selected by the low-pass filter The information of the moving direction is obtained based on the amount of change, and when the amount of direction change is large, based on the second amount of direction change selected by the high-pass filter. Mobile position detection device.