JPH09305231A - Travel controller for self-traveling carriage - Google Patents

Abstract

PROBLEM TO BE SOLVED: To eliminate an angle error which is possibly generated in sensor output owing to the direction error on the side of the carriage when azimuth sensors are reset to reduce a drift difference by providing a direction correction control means, a reset means, a direction change control means, and a steering control means which are all specific. SOLUTION: The direction correction control means turns the carriage so as to set the angle of displacement from the reference azimuth of the carriage to zero when the self-traveling carriage is temporarily stopped for turning. The reset means resets the azimuth sensor 15 to the reference-directional angle when it is confirmed that the angle of displacement of the carriage from the reference azimuth is nearly zero. The direction change control means (main controller 22) after resetting the azimuth sensor 15 turns the carriage to a next steering direction for turning. The steering control means (main controller 22) makes the carriage, directed to the next steering direction, retravel along a travel path.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a traveling control device for controlling traveling of a self-propelled carriage.

[0002]

2. Description of the Related Art An orientation sensor is often used for controlling the traveling of a self-propelled carriage. However, this azimuth sensor generally has a drift, which generally increases over time as the error angle of the sensor. For example, it increases with the characteristics shown in FIG. Further, the signal processing circuit that processes the output signal of the sensor includes an integrator and the like, and the integrator and the like also have a drift. Both drifts are overlapped and the error of the sensor output, that is, the displacement angle signal increases with the passage of time.

Therefore, in order to reduce the drift error, a certain time has elapsed from the start of traveling, and the traveling direction substantially coincides with the direction at the beginning of traveling, and the stability of the displacement angle signal with respect to the displacement is large. At this time, an interrupt is generated in the normal traveling control process to stop the traveling of the bogie, and a reset method is applied to reset the direction sensor during the stop, that is, the output displacement angle signal is corrected to zero at the start of traveling. A method of correcting the value has been proposed (JP-A-6-23).
6212).

[0004]

However, when resetting such a direction sensor, the following problems occur. That is, as shown in FIG. 11, after the start of traveling and before a lapse of a certain time, the loading condition of the self-propelled carriage 10 and the floor and the wheels 11a and 11b at the time of turning for changing the direction and stopping for measuring the direction. The direction of the self-propelled carriage 10 at the time of stop is 0.6 degrees from the correct 0 ° direction P due to a slip between
It is known that a shift of up to about ° can occur. It is obvious that the error angle θ at this time adversely affects the directional control after the sensor is reset. For example, in FIG. 12, the self-propelled carriage 10 starts traveling at the traveling start point at the lower left position in the figure, travels straight in the 0 ° direction, and stops traveling at "Q point" to make the first 90 ° right turn. When the azimuth sensor is reset by means of the azimuth sensor, the output of the azimuth sensor and the azimuth of the dolly are correctly matched when the dolly 10 is correctly oriented in the 0 ° direction. However, when the sensor output is reset even though the truck is not correctly facing the 0 ° direction at “Q point”, the direction sensor will eventually have a sign opposite to the actual direction of the truck. An error will occur.
The error angle at this time is accumulated for each reset operation of the sensor at the "Q point". If the carriage 10 reciprocates n times, the azimuth sensor finally has a maximum accumulated error corresponding to 0.6 ° × n. In the example shown in FIG. 12, since there are five “Q points”, 0.6 ° × 5 =
An error angle of 3.0 ° is accumulated in the orientation sensor. If the trolley | bogie 10 shall drive forward 20 m under such a condition, the trolley | bogie 10 will be 20xtan 3.0 degree =
A deviation of 1.0 (m) occurs in the lateral direction from the 0 ° direction.

Therefore, the present invention is directed to a traveling control apparatus for a self-propelled vehicle capable of eliminating an angular error that may occur in a sensor output due to a direction error on the vehicle side when the direction sensor is reset in order to reduce a drift error. The purpose is to provide.

[0006]

In order to solve the above-mentioned problems, the present invention has a carriage set in advance so as to detect a displacement angle from a reference azimuth by an azimuth sensor and set the displacement angle to zero. In a traveling control device for a self-propelled vehicle that controls traveling in a predetermined direction along a travel route, when the bogie is temporarily stopped to change the direction, the self-propelled vehicle automatically sets the displacement angle from the reference azimuth to zero. Direction correction control means for turning the traveling vehicle, reset means for resetting the orientation sensor to the reference direction angle when it is confirmed that the displacement angle of the self-propelled vehicle from the reference orientation is almost zero, and resetting the orientation sensor After that, the direction change control means for turning the bogie toward the next driving direction to change the direction and the driving control for re-driving the bogie directed to the next driving direction along the traveling route. Characterized in that and means.

The direction correction control means turns the carriage at a higher speed when the displacement angle from the reference azimuth is larger than a predetermined value, and at a lower speed when the displacement angle from the reference azimuth is smaller than the predetermined value. It should be turned.

[0008]

BEST MODE FOR CARRYING OUT THE INVENTION A travel control device for a self-propelled carriage according to an embodiment of the present invention will be described below with reference to the drawings.

First, referring to FIG. 3, an outline of a traveling mechanism and a control device of a self-propelled carriage 10 which is a subject of traveling control according to the present invention will be described.

FIG. 3 shows a self-propelled carriage 10 traveling in a traveling direction indicated by an arrow P, which is provided with a pair of wheels on the left and right sides, namely, a left wheel 11a and a right wheel 11b.
The wheels 11a and 11b are rotationally driven independently by a left drive motor 12a and a right drive motor 12b, respectively.
The drive motors 12a and 12b are driven by the left driver 13a and the right driver 13b.

Each of the drive motors 12a and 12b is separately provided with a left pulse oscillator 14a and a right pulse oscillator 14b which generate a pulse at a constant rotation angle. Based on the integrated value of the pulses generated by these pulse oscillators, the traveling distance of the vehicle and the traveling speed can be determined. Further, a direction sensor 15 for detecting a direction for autonomous traveling, a front obstacle sensor 1 for detecting a front obstacle
6a, a left side obstacle sensor 16b and a right side obstacle sensor 16c for detecting left side and right side obstacles are provided. The detection signals of the obstacle sensor and the pulse transmitter are input to the control device 20. An operation panel 17 is attached to the control device 20, and various initial settings can be made by manual operation. The control device 20 controls the drivers 13a, 13a based on the detected output of each sensor and the setting content of the operation panel 17.
b through the drive motors 12a and 12b
Control the rotation of a and 11b.

FIG. 4 shows a more detailed structure of the controller 20 and its input / output section. The control device 20 includes interfaces 21a to 21c, a main controller 22, and a counter 23. Obstacle sensor 16a,
The detection signals of 16b and 16c are the interface 21a.
The azimuth sensor 15 detects the traveling azimuth of the self-propelled carriage 10 relative to the reference azimuth and inputs the azimuth detection signal to the main controller 22 via the interface 21b. Operation panel 17
The information input by the operator from the interface 2
It is input to the main controller 22 via 1b. Further, by counting the pulses generated by the pulse transmitters 14a and 14b by the counter 23, a count value proportional to the traveling distance of the self-propelled carriage 10 is obtained, which is input to the main controller 22. Here, the traveling distance of the carriage 10 can be obtained by multiplying the count value by a predetermined coefficient, and the traveling speed of the carriage 10 can be obtained by calculating the traveling distance per unit time.

The main controller 22 is the obstacle sensor 1
6a to 16c, the detection output of the azimuth sensor 15, the count output of the counter 23, and the operation panel 1.
A predetermined control calculation is performed based on the operation content of No. 7, and the output signal obtained as a result is sent to the drivers 13a and 13b via the third interface 21c, via which the drive motors 12a and 12b, that is, the wheels. 11a, 11b
When the rotation of the vehicle is controlled and, for example, as shown in FIG. 5, the vehicle travels in a predetermined traveling area 51 on a predetermined traveling route indicated by arrows D1 to D11 starting from the left front position, D1 goes straight, right 90 degree turn, D2 straight forward, right 90 degree turn, D3 straight forward, left 90 degree turn, D4 straight forward, and left 90
The control device 2 executes a series of steps for turning the vehicle based on the detection signal of each sensor and the information about the preset traveling route.
Drive motor 12a by the main controller 22 of 0,
It is possible to repeatedly travel through the travel area 51 through the control of 12b.

The self-propelled carriage 10 is stopped at each "Q" point in FIG. 12, that is, at the point where it advances in the initially set traveling direction and makes a 90 ° right turn, and the direction sensor 15 is reset.
In that case, the trolley 10 is turned left and right at that point until the angle data of the azimuth sensor 15 becomes 0 ° (start direction) or the reference angle direction corresponding to the predetermined angle direction,
When the angle data of the azimuth sensor 15 reaches 0 °, the bogie 10 stops turning. This control operation is the direction correction control according to the present invention. FIG. 6 shows the carriage 10 with respect to the 0 ° direction.
Shows an example in which there is a deviation angle + δ in the positive direction, and in this case, the deviation angle + δ can be canceled to zero by the turning motion of the angle −δ to the left. FIG. 7 exemplifies a case where the carriage 10 is displaced with a deviation angle −δ in the negative direction with respect to the 0 ° direction.
In this case, the deviation angle −δ can be canceled out to zero by the turning motion of the angle + δ to the right. After this operation, the orientation sensor 15 is reset.

Azimuth sensor 15 which is the target of direction correction control
The angle data of 6 contains some error, and as shown in FIG. 8, if the direction sensor 15 is reset at the time ta when the error is small before the error sharply increases, the angle error will be reduced. It can be made as small as possible. The reset point may be set appropriately so as to satisfy this condition.

The outline of the direction correction control will be described below. When the carriage 10 stops at a relatively large deviation angle, the left and right wheels 11a and 11b are rotated at a very small speed to rotate the carriage 10 at a low speed (3.0 ° / s), and the azimuth sensor is set to 0 ° (start direction). ) Is detected, the turning is stopped. In order to prevent the trolley 10 from overrunning by turning and stop turning accurately at 0 ° (start direction), it is preferable to set the set speed to a plurality of stages, for example, two stages. That is, for example, in the case of deviation with an angle error of ± 0.5 ° or more, a slightly higher turning speed (for example, 3.
0 ° / s) is set, and if the angle error is within ± 0.5 °, a lower turning speed (for example, 0.5 ° / s) is set.

Velocity shown here: 3.0 ° / s or 0.5
° / s may be set to the optimum value that is experimentally obtained. In this example, the description is based on the absolute azimuth of 0 ° (start direction), but the same can be applied to the case of the absolute azimuth of 90 ° or 180 °.

Before the resetting of the bearing sensor 15 after a certain period of time, the control angle of the wheels 11a and 11b calculated from the rotation of the wheels and the detection of the bearing sensor 15 due to the slip between the wheels and the floor surface at the time of stoppage, etc. It has already been described that the difference between the angle and the angle is accumulated and the accumulated error may cause the trolley 10 to deviate from the set trajectory, but according to the present invention, due to the direction change, etc., the set straight traveling direction (normally Is a relatively short time in which the trolley 10 stops at 0 ° at the time of start), and when the error is small, the azimuth sensor 15 is reset, so that the error is not accumulated. When correcting the direction, turn left and right at a slower speed than the normal turning speed, and further slow down the turning speed when approaching the set straight ahead direction (0 ° direction). ) Is detected and stopped. As a result, slippage due to the turning operation and overrun at the time of turning are eliminated, and highly accurate stop can be realized.

Hereinafter, the arithmetic control for traveling the carriage executed by the main controller 22 will be described in detail with reference to the flowcharts and with reference to FIG.

1. Main Routine In FIG. 9, the self-propelled carriage 10 is caused to travel in the 0 ° direction (or another set direction, for example, 90 ° or 180 °) based on the angle data from the direction sensor 15 (step 4).
1). The trolley 10 based on the count content of the counter 23
Is calculated (step 42), it is determined whether or not the traveled distance exceeds a predetermined value (step 43), and if it is exceeded, the carriage 10 is decelerated and stopped (step 44), and direction correction control is performed. (Step 45), the direction of the trolley 10 is adjusted to the start direction or another set direction. This direction correction control will be described in detail separately with reference to the subroutines of FIGS. 1 and 2. When the traveling distance does not exceed the predetermined value, steps 41 to 43 are repeated and the carriage 10 continues traveling. After the direction correction control, it is checked whether or not the azimuth sensor 15 has passed a certain time since the last sampling (step 46), and if the certain time has passed, the azimuth sensor 15 is sampled (step). 47) and the direction sensor 15 is reset (step 48), and the angle data of the direction of the carriage 10 is set to 0 °.
Or reset to the set direction. If a certain period of time has not yet passed from the previous sampling of the azimuth sensor 15, the azimuth sensor 15 is not sampled.

(Direction Correction Control Subroutine) The direction correction control subroutine will be described with reference to FIGS. 1 and 2. First, the definition of the plus / minus (plus / minus) of the deviation angle and the value of the angle state flag used in the direction correction control will be described. As shown in FIG. 10, a target azimuth, for example, absolute azimuth 0 °, 90 °, 180 °, etc., is set as 0 ° in a relative sense, and the right half thereof is a plus deviation angle region and the left half. Is defined as a negative deviation angle region. Also, the angle state flag SSPFG and the trolley 1 at that time
The correspondence with the operation of 0 is set as shown below.

Deviation angle SSPFG Self-propelled carriage operation -180 ° to -0.5 ° 1 High speed right turn -0.5 ° to 0 ° 2 Low speed right turn 0 ° to 0.5 ° 3 Low speed left turn 0. 5 ° to 180 ° 4 High speed left turn When the angle data is positive, the control according to the flowchart of step 53 onward of FIG. 1 is executed, and when the angle data is negative, the control according to the flowchart of step 71 onward of FIG. 2 is executed. .

Continuing from step 48 in FIG. 9, the angle data from the azimuth sensor 15 in FIG. 1 is read (step 51), and it is checked whether the read deviation angle is negative (step 52). If the deviation angle is negative, the process proceeds to step 71 (FIG. 2). If the deviation angle is not negative, it is further checked whether or not the deviation angle is 0.0 ° (step 53). Here, when it is 0.0 °, that is, when the trolley 10 coincides with the starting direction or the set direction, the vehicle is decelerated and stopped (step 54). 0.
If it is not 0 °, that is, if the deviation angle has a positive value, it is determined whether or not the value of the flag SSPFG is 3 or more (step 55), and SSPFG ≦ 2, that is, the state of the cart 10 is clockwise. When the minus deviation angle is changed to the plus deviation angle by turning, the vehicle 10 is first decelerated / stopped so as to turn left at a low speed or a high speed according to the deviation angle value (step 56). In step 52, whether the deviation angle is positive or negative is judged, and since only positive cases should be targeted, SSPFG ≧ 3 normally. Therefore, when SSPFG ≦ 2, it means that the vehicle has made a right turn from step 52 to step 55. Here, the deviation angle is rechecked, that is, it is checked whether the deviation angle is 0.5 ° or more (step 57). In step 55, when SSPFG ≧ 3, step 57 is executed without executing step 56. At this time, it is normally in the positive deviation angle state. In step 57,
When the deviation angle is 0.5 ° or more, the flag SSPF is set.
G is set to "4" (step 58), high-speed data is acquired assuming that the angle is far from the target angle direction (step 59), and if the deviation angle is 0.5 ° or less, the flag SSPFG is set. Is set to "3" (step 60), low speed data is acquired assuming that the vehicle is in an angle state close to the target angle direction (step 61), and the left wheel 11a is moved backward in the backward direction by the acquired high speed data or low speed data. The right wheel 11b is driven in the forward direction to turn left (step 62). After this, the process returns to step 51 again.

If the deviation angle is negative in step 52, steps 71 et seq. (FIG. 2) similar to step 55 et seq are executed. In this case, flag S
It is determined whether the SPFG value is 3 or more (step 7).
1), SSPFG ≧ 3, that is, when the state of the bogie 10 changes from a plus deviation angle to a minus deviation angle due to a left turn, the vehicle 10 is decelerated or decelerated to make a right turn at a low speed or a high speed according to the deviation angle value. Stop (Step 7)
2). Here, the deviation angle is rechecked, that is, whether the deviation angle is −0.5 ° or less is checked (step 7).
3). When SSPFG ≦ 2 in step 71, step 73 is executed without executing step 72. At this time, it is normally in a negative deviation angle state. In step 73, when the deviation angle is −0.5 ° or less (the deviation angle is shifted to the left by 0.5 ° or more), the flag SSPFG is set to “1” (step 74), and the target angle is set. High-speed data is acquired assuming that the angle is far from the direction (step 75), and the deviation angle is −0.5 ° or more (the deviation angle is −0.5 ° to −0.1 °.
If it is within the range), the flag SSPFG is set to "2" (step 76), and the low speed data is acquired assuming that the angle state is close to the target angle direction (step 7).
7) The left wheel 11a is driven in the forward direction and the right wheel 11b is driven in the backward direction based on the acquired high speed data or low speed data, respectively, to perform a right turn (step 7).
8). After this, the process returns to step 51 (FIG. 1) again.

[0025]

According to the traveling control device for a self-propelled carriage of the present invention described above, when the direction sensor is reset in order to reduce the drift error, the sensor output relatively occurs due to the direction error on the carriage side. It is possible to eliminate the obtained angle error and perform traveling control of the self-propelled carriage with high accuracy for a long time.

[Brief description of drawings]

FIG. 1 is a flowchart showing a procedure of direction correction control according to an embodiment of the present invention.

FIG. 2 is a flowchart showing a procedure of direction correction control according to the embodiment of the present invention.

FIG. 3 is an explanatory diagram showing an outline of a traveling mechanism and a control device of a self-propelled vehicle to which the direction correction control of the present invention is applied.

FIG. 4 is a block diagram showing details of a control device in the device of FIG.

FIG. 5 is a route diagram showing an example of a traveling route of a self-propelled vehicle to which the present invention is applied.

FIG. 6 is an explanatory diagram showing a state in which the carriage has stopped traveling with a positive angle error.

FIG. 7 is an explanatory view showing a state in which the carriage stops traveling with a negative angle error.

FIG. 8 is a diagram illustrating an execution timing of a reset operation performed to compensate an angle error of the azimuth sensor.

FIG. 9 is a flowchart showing a main routine of a bogie traveling control including the direction correction control of the present invention.

FIG. 10 is an explanatory diagram for explaining setting contents of an angle state flag according to the present invention.

FIG. 11 is an explanatory diagram for explaining an angular error of the self-propelled carriage when the self-propelled carriage has traveled a predetermined distance and stopped.

FIG. 12 is an explanatory diagram showing an example of a travel route of a self-propelled vehicle, a reset point of a direction sensor, and a direction correction control point.

[Explanation of symbols]

 10 Self-propelled vehicle 11a, 11b Wheel 12a, 12b Drive motor 13a, 13b Driver 14a, 14b Pulse transmitter 15 Direction sensor 16a-16c Obstacle sensor 17 Operation panel 20 Control device 21a-21c Interface 22 Main controller 23 Counter D1-D11 Driving route

Claims (2)

[Claims] 1. A self-propelled vehicle that detects a displacement angle from a reference azimuth by an azimuth sensor and controls the trolley to travel in a predetermined direction along a preset travel route so that the displacement angle becomes zero. In the traveling control device, when the bogie is once stopped for changing the direction, direction correction control means for turning the self-propelled carriage so as to make the displacement angle of the self-propelled carriage from the reference azimuth zero, Reset means for resetting the azimuth sensor to a reference direction angle when it is confirmed that the displacement angle of the self-propelled carriage from the reference azimuth is substantially zero; and after resetting the azimuth sensor, for changing the direction. Direction change control means for turning the dolly in the next operation direction, and operation control means for re-traveling the dolly in the next operation direction along the travel route. A traveling control device for a self-propelled trolley characterized by being provided. 2. The direction correction control means turns the carriage at a higher speed when the displacement angle from the reference azimuth is larger than a predetermined value, and when the displacement angle from the reference azimuth angle is smaller than the predetermined value. The traveling control device for the self-propelled carriage according to claim 1, wherein the carriage turns the carriage at a lower speed.