JPH0695733A - Method for detecting absolute position of self-propelled truck - Google Patents

Abstract

PURPOSE:To detect the extent of movement of a self-propelled truck in a free plane with a high precision by detecting a skid of the self-propelled truck and a slip of driving wheels based on the output of an accelerometer and calculating the extent of movement in a minute sampling time. CONSTITUTION:Acceleromaters 1L and 1R on a driving axle 41 detect the acceleration in the running direction, and an accelerometer 2 on an intersection O between the driving axle 41 and an axle 81 detects the acceleration in the direction orthogonal to the running direction. A controller 9 detects a skid and a slip of driving wheels 4L based on outputs of acceler 0 meter 1L, 1R, and 2. The controller 9 calculates the extent of movement in the minute sampling time by an operation routine corresponding to each state. The controller 9 successively adds this extent of movement to the extent of movement calculated till then to detect absolute coordinates of the present position in the absolute coordinate system in the free plane. Thus, the simple system using accelerometers 1L, 1R, and 2 and rotation position detectors 6R and 6L is obtained.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method for detecting an absolute position in a moving plane of a self-propelled carriage for unmanned transportation of goods in factories, offices and the like.

[0002]

2. Description of the Related Art Conventionally, as an autonomous traveling guide device for a self-propelled vehicle that moves on a free plane, the travel distance measured by a rotation sensor provided on the wheel of the self-propelled vehicle and the angular velocity by a rate gyro provided on the self-propelled vehicle are used. The position of the self-propelled carriage is detected based on the posture angle obtained by integration, and the offset value of the rate gyro is detected and stored every time the self-propelled carriage is stopped, and can be obtained while the self-propelled carriage is running. There is one that corrects the output of the rate gyro with this offset value (for example, Japanese Patent Laid-Open No. 58-166406).

[0003]

However, since the movement distance is detected by the rotation sensor provided on the wheel, when the running surface is tilted and the vehicle body is skid or the running surface is severely rough, When slipping, the cumulative error is large. Moreover, there is a problem that the rate gyro is expensive. Therefore, it is an object of the present invention to provide a method for detecting a moving distance of a self-propelled vehicle that moves on a free plane with high accuracy and at low cost by correcting side slip of a vehicle body and slip of wheels.

[0004]

In the method for detecting the current position of a self-propelled vehicle by detecting the moving distance of a self-propelled vehicle moving on a free plane by a rotary position detector provided on the left and right wheels,
Two accelerometers mounted on the left and right drive shafts of the self-propelled carriage to detect acceleration in the traveling direction, and accelerations for detecting accelerations in the travel direction and orthogonal direction provided at the intersections of the left and right drive shafts and the axle. A gauge is provided to detect the sideslip of the self-propelled carriage and the slip of the drive wheel based on the output of each accelerometer, and the movement distance within the minute sampling time is calculated by the calculation routine corresponding to each state. It is sequentially added to the moving distance up to and the absolute coordinates of the current position of the self-propelled carriage are detected. In addition, the gain and offset of each accelerometer and rotational position detector are corrected to match each detector.

[0005]

The movement distance for each minute sampling time is corrected in accordance with the situation such as sideslip or slip. Accurately grasp the coordinates of the absolute position of the self-propelled carriage on the free plane for each minute sampling time.

[0006]

Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a plan view showing the structure of a self-propelled carriage according to the present invention. The left and right drive wheels 4R and 4L are arranged in a straight line on the drive shaft 41. The drive wheels 4R, 4L are rotationally driven by respective drive devices 5R, 5L equipped with rotational position detectors 6R, 6L, and the driving direction is controlled by the left and right drive wheels 4R, 4L.
It is performed by applying a rotation speed difference to 4L. Drive shaft 41
At any upper position, the detection direction is the axle 81 direction (hereinafter,
The x-axis direction corresponds to the traveling direction of the self-propelled carriage. ) And accelerometers 1R and 1L for detecting the angular acceleration around the installation position. On the intersection O of the drive shaft 41 and the axle 81, the accelerometer 2 having the detection direction as the drive shaft 41 direction (hereinafter, the y-axis direction corresponding to the direction orthogonal to the traveling direction of the self-propelled carriage) is provided. is there. The control device 9 arranged at an appropriate position on the vehicle body includes a drive device 5 for calculating a moving distance.
An arithmetic circuit for giving a drive command to R and 5L is stored. FIG. 2 shows a block diagram of the drive command and the arithmetic circuit. The pulse signals indicating the rotation amounts of the rotation position detectors 6R and 6L provided in the drive devices 5R and 5L are input to the counters 15R and 15L, respectively. Counter 15
The outputs of R and 15L are latches 16R and 16L, respectively.
The number of rotation pulses is passed in accordance with a payout command issued by the CPU 17 and is stored in the RAM provided in the CPU 17. The outputs of the accelerometers 1R, 1L, and 2 are set to appropriate D by the amplifier circuits 10R, 10L, and 11.
After being converted into a C level signal, it is selected by the multiplexer 13 and digitized by the A / D converter 14. C
The PU 17 is interrupted by the real-time clock 18 at every minute sample time and calls the built-in movement distance calculation routine. The movement distance calculation routine checks the slip of the drive wheel 4 and the side slip of the self-propelled carriage for each sample time, and selects an arithmetic expression. The acceleration measured by the accelerometers 1R, 1L, 2 corresponding to the selected arithmetic expression is sampled from the A / D converter 14,
Each acceleration is integrated to calculate the travel distance of the self-propelled carriage within the sample time. At the same time, the values of the counters 15R and 15L of the rotational position detectors 6R and 6L are also sampled. The calculation result of the moving distance for each sample time is
It is added to the moving distance up to that point, and the origin of the moving plane (0,
The absolute coordinates (X, Y, Θ) of the current position are stored in the memory 19 as the cumulative moving distance from 0, 0) and the cumulative moving direction. When the CPU 17 has map information inside, it is possible to control the drive device by comparing this with the moving distance and grasping the position of the self-propelled carriage on the map. The CPU 17 incorporates a traveling control routine (for example, route, steering, speed, acceleration, etc.) in addition to the moving distance calculation routine, and the motor controllers 20R and 20L.
Issue a command to.

The details of the moving distance calculation routine will be described below. First, a wheel slip detection method will be described. Based on the output accelerations α _SR and α _{SL of the} accelerometers 1R and 1L, the acceleration α _{SX in the} X direction and the angular acceleration dω _S / dt around the point O are calculated by the following formulas. α _SX = (l _SR * α _SL + l _SL * α _SR ) / (l _SR + l _SL ) (1) dω _S / dt =-(α _SL -α _SR ) / (l _SR + l _SL ) (2) However, l _SR and l _SL are acceleration sensors 1R and 1L from point O
Is the distance to. Next, the speeds V _SX and O in the x direction
Calculate the angular velocity ω _S around the point. Generally, the equations (1) and (2) are integrated to obtain, but when the sample time t is small, the following equation may be used. V _{SX (m)} = V _{XS (m-1)} + α _{SX (m)} * t (3) ω _{S (m)} = ω _{S (m-1)} + (dω _S / dt) _(m) * t (4 ) Here, the suffix m indicates the m-th sample time. Therefore, the velocities V _{SWL (m)} and V _SWL of the left and right wheel positions at the m-th sample time of the left and right drive wheels 4L and 4R obtained by using the output acceleration of the accelerometers 1R and 1L as a reference.
_{SWR (m)} is expressed by the following formula. V _{SWL (m)} = V _{SX (m)} -1 _WL * ω _{S (m)} (5) V _{SWR (m)} = V _{SX (m)} +1 _WR * ω _{S (m)} (6) where 1 _WL , 1 _WR is the distance from point O to drive wheels 4L and 4R. On the other hand, the speeds of the left and right driving wheel positions at the m-th sample time, which are obtained with reference to the rotational speeds ω _{WL (m) and} ω _{WR (m)} detected by the rotational position detectors 6L and 6R, are V _{WL (m ) And} V _{WR (m)} . Left and right drive wheel slip V
_SLIPXL and V _SRIPXR are represented by the following formulas. V _SLIPXL = V _{SWL (m)} -V _{WL (m)} (7) V _SRIPXR = V _{SWR (m)} -V _{WR (m)} (8)

Next, with reference to the acceleration α _{Oy in the} y direction detected by the accelerometer 2 provided at the point O, the sideslip acceleration α
_SLIPy is represented by the following formula. α _{SLIPy (m)} = α _{Oy (m)} −V _{SX (m)} * ω _{S (m)} (9) Generally, the sideslip velocity V _{SLIPy (m)} of the wheel is α _{SLIPy (m)}
Is obtained by integrating, but when the sample time t is small,
The following formula can be used. V _{SLIPy (m)} = V _{SLIPy (m-1)} + α _{SLIPy (m)} * t (10) Therefore, the combined speed of the left and right wheels 4L and 4R due to slip and side slip is represented by the following formula. V _{SLIPL (m)} = SQRT (V _SLIPXL ² + V _SLIPyL ² ) (11) V _{SLIPR (m)} = SQRT (V _SLIPXR ² + V _SLIPyR ² ) (12) Either of the formulas (10) and (11) When exceeds a predetermined value, it is determined that slip or skid occurs on either of the left and right wheels.

The traveling situation is judged by the values of the equations (10) and (11), and the distance calculation equation of the self-propelled carriage is properly used at each sampling time. This is because the second-order integral in the distance calculation process is included and there is a certain degree of calculation error. Therefore, when the travel distance of the self-propelled carriage is calculated using only the values of the accelerometers 1R, 1L and 2, the integration error is Because it will grow. Therefore, when the slip or side slip of the wheel is considered to be small, the rotational position detector 6 with a small integration once.
The values of L and 6R are used. (B) When it is determined that slip or skid cannot be ignored. From θ _{S (m)} obtained by integrating V _{SX (m)} and ω _{S (m)} obtained by the equations (3) and (4) and V _{SLIPy (m)} obtained by the equation (10), Subordinates (x _(m) , y _(m) , θ ₎ of the self-propelled carriage at the th sample time
_(m) ) is calculated by the following formula. _{x (m) = x (m} -1) + V Sx (m) * t * cos ((θ S (m) + θ (m-1)) / 2) -V SLIPy (m) * t * sin ((θ _{S (m)} + θ _(m-1) ) / 2) (13) y _(m) = y _(m-1) + V _{S (m)} * t * sin ((θ _{S (m)} + θ _(m-1) ) / 2) + V _{SLIPY (m)} * t * cos ((θ _{S (m)} + θ _(m-1) ) / 2) (14) θ _(m) = θ _(m-1) + ω _Sm * t (15 ) (B) When it is judged that slip or skid can be ignored.
Rotational speed ω _{WL (m)} detected by the rotational position detectors 6L, 6R _,
The speeds of the left and right wheel positions at the m-th sample time obtained with reference to ω _{WR (m)} are V _{WL (m)} and V _WR , and the velocities V _Wx and ω _W of the self-propelled carriage in the x direction are as follows. It is represented by a formula. V _{WX (m)} = (1 _WR * V _{WL (m)} +1 _WL * V _{WR (m)} ) / (l _WL + l _WR ) (16) ω _{W (m)} =-(V _{WL (m)} -V _{WR (m)} ) / (l _WL + l _WR ) (17) where 1 _WR and 1 _WL are the distances from the O point of the left and right wheels. x _(m) = x _(m-1) + V _{WX (m)} * t * cos ((θ _{W (m)} + θ _(m-1) ) / 2) (18) y _(m) = y _{(m-1 )} + V _{WX (m)} * t * sin ((θ _{W (m)} + θ _(m-1) ) / 2) (19) θ _(m) = θ _(m-1) + ω _{W (m)} * t (20 ) Now, the absolute coordinates (X, Y,
Θ) uses equations (13), (14) and (15) in the case of (a), and (18) and (19) in the case of (b).
And using the equation (20), it is calculated by the following equation. X _(m) = ΣX _(m-1) + x _(m) (21) Y _(m) = ΣY _(m-1) + y _(m) (22) Θ _(m) = ΣΘ _(m-1) + θ _{(m )} (23)

When the respective detectors can maintain the absolutely same accuracy, the values obtained above may be used, but normally, the accelerometers 1R, 1L, 2 and the rotational position detectors 6L, 6R are detected. Since the instrument is affected by errors due to variations and temperature drift, it is preferable to make an online correction.
An identification method by estimation is used as a method of correcting the gain and offset of each detector online. The new estimate is given by the general formula: New estimation = old estimation + gain vector * prediction error (24) An example of the method for identifying the gain vector and prediction error in Eq. (24) online is'ADAPTIVE FILTERING PREDICTI.
ON AND CONTROL '(PRENTICE-HALL, INC, Issue 3.3). As an example, when the equation (24) is applied to the detection value α _SL of the accelerometer 1L of the present invention, the following equation is obtained. α _{SL (m + 1)} = α _{SL (m)} + k _(m) * E _{(m + 1)} (25) where k _(m) is accelerometer 1L at sample time m
, E _{(m + 1)} is a prediction error (offset).
K _(m) and E _{(m + 1)} in the equation (25) are represented by the following equations. k _(m) = 1 / Φ _(m) ^T * Φ _(m) (26) E _{(m + 1)} = y _{(m + 1)} −Φ _(m) ^T * α _{SLest. (m)} (27) , Α _{SLest. (M)} is the estimated value at the sample time m, and Φ _(m) ^T is the transposed vector of the difference between the estimated value and the observed value and is represented by the following formula. Φ _(m) ^T = [-y _(m) , α _{SL (m)} ] (28) where y _(m) is an estimated value at the sample time m by a linear or nonlinear model. The initial correction method for obtaining the reference value of the accelerometer will be described below. First, the self-propelled carriage is run on an ideal plane that prevents skidding and slipping. In this state, a disturbance may occur in the outputs of the left and right accelerometers and the rotational position detector, and the gain and offset of the left and right accelerometers and the rotational position detector are corrected. Next, it is considered that Geni is fixed and the offset is corrected by the equation (27). This is done every sample time. After sufficiently performing such initial correction, the distance measurement of the self-propelled carriage is started. The accelerometer is corrected even during distance measurement. Incidentally, the correction of the accelerometer during the distance measurement is performed by the rotational position detector of the wheel when it is determined that the slip of the wheel is negligibly small within the range of the predetermined values of the equations (11) and (12). It should be done from the data of.

[0010]

As described above, according to the present invention, the moving distance of the self-propelled carriage and the coordinates of the current position can be calculated with a relatively simple system configuration in which only the accelerometer and the rotational position detecting device of the drive system are combined. Since it can be detected and the accelerometer is used, it has an effect that it is cheaper than the one using the gyroscope.

[Brief description of drawings]

FIG. 1 is a plan view showing the configuration of a self-propelled carriage according to an embodiment of the present invention.

FIG. 2 is a block diagram of a control device according to an embodiment of the present invention.

FIG. 3 is a plan view showing the arrangement of an acceleration detector and the like according to an embodiment of the present invention.

[Explanation of symbols]

 1R, 1L, 2 Accelerometer 4R, 4L Drive wheel 41 Drive shaft 5R, 5L Drive device 6R, 6L Rotation position detector 81 Axle 9 Control device 10R, 10L, 11 Amplification circuit 13 Multiplexer 14 A / D converter 15R, 15L Counter 16R, 16L Latch 17 CPU 18 Real Time Clock 19 Memory 20R, 20L Motor Controller

Claims (5)

[Claims] 1. A method for detecting a current position of a self-propelled vehicle by detecting a moving distance of the self-propelled vehicle moving on a free plane by a rotational position detector provided on a wheel, and detecting the present position of the self-propelled vehicle on the left and right drive axes of the self-propelled vehicle. , Provided with two accelerometers for detecting acceleration in the traveling direction and an accelerometer for detecting acceleration in the traveling direction and orthogonal direction provided on the intersection of the left and right drive shafts and the axle,
Based on the output of each accelerometer, it detects the sideslip of the self-propelled carriage and the slip of the driving wheel, and the calculation routine corresponding to each state calculates the movement distance within the minute sampling time, and the movement distance up to that point. To the absolute coordinates of the present position of the self-propelled carriage with respect to the absolute coordinate system in the free plane. 2. The absolute position detection of the self-propelled vehicle according to claim 1, wherein the skid of the self-propelled vehicle is detected based on the acceleration in the axle direction detected by an accelerometer provided at the intersection of the drive shaft and the axle. Method. 3. The self-propelled vehicle according to claim 1, wherein slip of the drive wheels is detected from a difference between acceleration detected by left and right accelerometers provided on the drive shaft and rotational speed detected by a rotational position detector. Absolute position detection method of trolley. 4. The absolute position detection of the self-propelled carriage according to claim 2 or 3, wherein an arithmetic expression is selected for the calculation of the moving distance within the minute sampling time in accordance with the situation of the sideslip and slip of the self-propelled carriage. Method. 5. The absolute position detection method for a self-propelled carriage according to claim 1, wherein the gains and offsets of the accelerometers and the rotational position detectors are corrected online.