JPH06236212A - Travelling controller for self-travelling carriage - Google Patents

Abstract

PURPOSE:To control the travelling of a carriage for a long term with high accuracy only by using a gyro device without requiring any complicated and expensive device such as a constant temperature device or an interlock device. CONSTITUTION:This device is provided with motors 15 and 25 for driving a pair of driving wheels, velocity encoders 14 and 24 for detecting the revolving velocity of the respective driving wheels, gyro 18 for detecting a displacement angle in a reference direction, and main and sub controllers 31 and 33 for controlling the motors 15 and 25 so as to advance the carriage in the reference direction based on the detected revolving velocity and displacement angle, and the main controller 31 is provided with an interruption generating means for generating interruption when the advancing direction of the carriage is almost same as the advancing direction at the time of travelling start after the lapse of fixed time just while normal processing is performed, means for stopping the drive of the motors for prescribed time when the interruption is generated, and means for performing sampling by applying a sampling instruction to the gyro 18 while stopping the travelling of the carriage and for performing normal processing again after a displacement angle signal outputted from the gyro 18 is corrected.

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 a self-propelled vehicle, and more particularly to a device for controlling an unmanned traveling vehicle such as a robot for cleaning a floor surface.

[0002]

2. Description of the Related Art A gyro is often used for traveling control of a self-propelled carriage. However, the error of the displacement angle signal increases due to the drift of the gyro itself and the drift of the integrator included in the signal processing circuit as the operation time elapses.

Therefore, conventionally, when the power is turned on, the rate of change of the signal of the gyro device is measured for a certain period of time and stored as sampling data used for correction. Then, the signal of the gyro device is corrected by sampling data in accordance with the operation time of the gyro and used for traveling control. One such device is disclosed in Japanese Patent Laid-Open No. 62-221708. Such sampling data was collected only once when the power was turned on.

However, with such control, it is difficult to control with high accuracy for a long time. As shown in FIG. 9, the error increases with the passage of time due to the drift of the gyro itself or the integrator, etc., and the deviation greatly deviates from the reference angle at the start of traveling. Generally, the deviation from the reference angle becomes remarkable after about 10 minutes have passed from the start of running. As a result, it becomes impossible to ensure the linearity of the signal output from the gyro, and it becomes impossible to correct the signal even if sampling is performed.

Further, conventionally, in order to suppress drift of the gyro device, a device for holding the gyro device in a constant temperature state has been provided. Furthermore, the output level of the gyro device is
An interlock device is provided to detect whether the gyro device is within the specified value when it is operating normally and to stop the traveling of the self-propelled vehicle when the output level is detected not to be the specified value. It was However, the device for holding the gyro device in a constant temperature state and the interlock device are complicated and expensive, which has been a cause of increasing the manufacturing cost of the entire device.

[0006]

As described above, it is difficult for the conventional traveling control device to perform accurate control for a long time due to the drift of the gyro device, and further, a device for keeping the gyro device at a constant temperature is required, which results in cost reduction. Was increasing.

The present invention has been made in view of the above circumstances, and does not require a complicated and expensive device such as a thermostatic device or an interlock device, and can accurately drive a trolley for a long time using only a gyro device. It is an object of the present invention to provide a traveling control device for a self-propelled carriage that can be controlled by the.

[0008]

A traveling control device for a self-propelled carriage according to the present invention is a pair of drive wheels arranged on the same axis, a motor for driving the drive wheels, and a drive wheel connected to the drive wheels.
A speed encoder that detects each rotation speed and outputs a speed feedback signal, a gyro device that detects a displacement angular speed with respect to a reference direction, and outputs a displacement angular speed signal,
A controller that receives the displacement angular velocity signal and outputs a velocity command signal so as to travel in a predetermined direction, the velocity feedback signal and the velocity command signal, and based on the velocity command signal the motor And a motor control device for performing feedback control, the control device,
An interrupt that generates an interrupt when a certain time has elapsed during the normal processing for outputting the speed command signal and the traveling direction of the carriage is substantially the same as the reference direction. Generating means and means for causing the motor control device to stop driving of the motor for a predetermined time when the interrupt generating means generates an interrupt, and the gyro device while the driving of the motor is stopped. It is characterized by further comprising means for giving a sampling command to perform sampling, correcting the displacement angle signal output from the gyro device, and then performing the normal process again.

[0009]

When the control device performs the normal processing required for traveling control, the error contained in the displacement angle signal gradually increases due to the drift of the gyroscope or the like with the passage of time. Therefore, when a certain period of time has passed and the traveling direction is almost the same as the direction at the start of traveling and the stability of the displacement angle signal with respect to the displacement is large, an interrupt is generated in normal processing to drive the truck. By controlling the displacement angle signal by causing the gyro to perform sampling while it is stopped, it is possible to control traveling with high accuracy for a long time without the need for expensive equipment such as a thermostatic device. You can

[0010]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS A travel control system for a self-propelled carriage according to an embodiment of the present invention will be described below with reference to the drawings.

In the present embodiment, during the processing of the main routine that is normally executed to control the traveling of the bogie,
An interrupt is made using the timer interrupt function.
When an interrupt occurs, the dolly temporarily stops, the gyro performs sampling, and processing for correcting the displacement angular velocity signal is performed.

Here, the interrupt occurs when the following two conditions are satisfied.

(1) When a predetermined time has elapsed.

(2) When the traveling direction of the dolly coincides with the direction at the start of traveling.

In this embodiment, the condition (1) is 1
0 minutes have passed. As described above with reference to FIG. 9, the angle deviated from the reference direction increases with the passage of time from the start of travel due to the drift of the gyro, the integrator, and the like. This deviation increases significantly after about 10 minutes have passed, so one condition is that it has passed 10 minutes.

The condition (2) is based on the following reason. The angle deviated from the reference traveling direction at the start of traveling is 0
The degree is most stable, and at other angles, a slight difference in angle is a large difference in data. Therefore, in the present embodiment, a condition is set that the traveling direction of the bogie coincides with the reference direction at the time of starting traveling.

A flag is set only when these two conditions are satisfied, and after confirming this flag, a sampling command is output to the gyro.

FIG. 1 shows the configuration of the travel control device according to this embodiment.
Fig. 2 shows a state in which this device is mounted on a self-propelled carriage.

A left wheel 16 and a right wheel 26 are disposed on the left and right of the self-propelled carriage 11 that travels in the direction of arrow A, and are driven by a left drive motor 15 and a right drive motor 25, respectively. The operations of the left drive motor 15 and the right drive motor 25 are controlled by the motor control device 32.

The gyro 18 detects the displacement angular velocity of the carriage with respect to the reference traveling direction and outputs a displacement angular velocity signal. The displacement angular velocity signal output from the gyro 18 is the integrator 17
Are integrated and given to the main controller 31 as a displacement angle signal.

The main controller 31 receives a displacement angle signal from the integrator 17, and outputs a speed command signal so that the carriage travels in a preset reference direction.

The speed command signal output from the main controller 31 is given to the motor controller 32 described above. The motor control device includes a sub control device 33 and drivers 13 and 23.
The sub-control device 33, which is provided with the speed command signal, controls the drivers 13 and 23 based on this signal. The drivers 13 and 23 are based on the control of the sub control device 33 and the rotational angular velocities of the motors 15 and 25 detected by the encoders 14 and 24, respectively.
Control 5 and 25.

As shown in FIG. 8, it is assumed that the self-propelled carriage 11 deviates from the reference direction indicated by the arrow A by an angle θ due to unevenness of the road surface, slippage of the wheels 16 and 26, or the like. In this case, the angular velocity, which is a temporal change of the deviated angle θ, is detected by the gyro 18, and the traveling direction is corrected by controlling the rotation of the motors 15 and 25 and turning the carriage in the direction of arrow C. . Specifically, the self-propelled carriage 11 is turned in the direction of arrow C by changing the rotation speed of the left wheel 16 shown by the arrow VL and the rotation speed of the right wheel 26 shown by the arrow VR. You can

As shown in FIG. 3, the main control unit 31 includes a CPU 41 for performing calculations and various routines,
It has a ROM 42 and a RAM 43 which are connected to the input / output terminals of the CPU 41 and hold data, and a timer interrupt section 40 which is connected to the input / output terminals of the CPU 41.

The timer interrupt section 40 has an interrupt circuit 44, a time constant circuit 46, and a counter 45 which are connected to the input / output terminals of the CPU 41. The time constant circuit 46 is
The timer time is set, and the counter 45 sets the timer count data TIM set in the time constant circuit 46.
E10D is given and the time is measured by subtracting one by one. The counter 45 outputs a signal to the interrupt circuit 44 when the count value becomes 0, and the interrupt circuit 44 causes the CPU 41 to perform an interrupt process when this signal is given.

Next, the control operation of the traveling control device according to this embodiment will be described.

FIG. 4 shows the relationship between the main routine processes 101 and 103 normally executed by the CPU 41 during traveling and the above-mentioned timer interrupt processes 102 and 104. CPU41
When a certain time elapses while performing the main routine processing 101, the timer interrupt processing 102 is performed by the output from the interrupt circuit 44 of the main control device 31, and then the main routine processing 103 is returned to and the certain time is returned. When the time has passed, the timer processing 104 is performed again. As described above, the CPU 41 performs the timer interrupt process every time a fixed time elapses during the main routine process.

Next, a timer interrupt process is performed during the main routine process, and the operation of the sampling flag for causing the gyro 18 to perform the sampling process is shown in FIG. During the processing of the main routine, it is determined in step 201 whether the value of the sampling flag SAMPF is 1H. If the value of this flag SAMPF is not 1H, the processing of the main routine is continued, and if it is 1, step 202
Move to. After the sampling command is output to the gyro 18 and the sampling flag processing is performed, the value of the sampling flag SAMPF1 is set to 0H and the processing of the main routine is restarted.

Next, FIG. 6 shows a flowchart of the timer interrupt process. CPU used for main routine processing
The data of the register in 41 is saved in the stack area of the RAM 43 (step 301).

The 10-minute count data TIME10D stored in the RAM 43 is transferred to the register 1 to prepare for measurement for 10 minutes (step 302).

Similarly, the 4-second counter data TIME0D, which means that 4 seconds have passed since the turning operation started in the direction of 0 ° from the start, was transferred from the RAM 43 to the register 2 and the 4-second counter data was transferred for 4 seconds. Prepare for measurement (step 303).

The measurement for 10 minutes is started, and it is determined whether or not the value of the flag SAMPF indicating whether or not the time has elapsed is 1H (step 304). If this value is 1H, it is considered that 10 minutes have elapsed, and the process jumps to step 314 described later. If 10 minutes have not elapsed and the value of the flag SAMPF is 0H, the value of the register 1 is decremented by 1 and the measurement is continued (step 305).

It is judged whether or not the value of the register 1 is 0 (step 306). If it is not 0, that is, if 10 minutes have not elapsed, the process jumps to step 311 which will be described later. If the value of register 1 is 0, it means that 10 minutes have passed and register 1
1200D as the initial data of the counter for 10 minutes
Is stored (step 307). Here, the value 1 is 0.5
It corresponds to a second, and 1200 means 10 minutes (= 600 seconds). Further, the value of the flag SAMPF is set to 1H.
(Step 308). The register 2 stores the counter initial value 8D for the 4-second timer. The value of the sampling flag SAMT0F is set to 0 (step 310) so that the sampling instruction is not output to the gyro 18.

After step 311 described above, the value of register 1 is set to 10 in the RAM 43 as post-processing.
The minute count data TIME10D is transferred to update the data. Similarly, the value of the register 2 is transferred to the count data TIME0D for 4 seconds in the RAM 43 to update the data (step 312). The data saved in the stack area of the RAM 43 is restored to the register of the CPU 41 to prepare for the processing in the main routine again (step 313).

In step 304, the flag SAMPF is set.
Is 1H, step 31 as described above.
Go to 4. In step 314, it is determined whether or not the turning movement of the carriage has started in the direction of 0 degree with respect to the reference direction at the time of start, based on whether the flag TRIM0F is 1H. When the 0-degree direction flag TRIM0F is 0H, the turning operation in the 0-degree direction is not started, and the operation of jumping to step 306 and measuring 4 seconds is not performed.

When the flag TRIM0F is 1H,
That is, when the turning operation in the 0 degree direction is started, the value of the register 2 is decremented by 1 (step 315). It is determined whether the value of this register 2 is 0 (step 31).
6). If this value is not 0, 4 seconds have not elapsed yet, and the process jumps to step 306. When the value of the register 2 is 0, that is, when 4 seconds have passed, it is determined that the turning operation in the 0 degree direction is completed and the traveling direction of the carriage substantially coincides with the reference direction at the start of traveling. Jumping to step 317, the sampling flag SAMT0F is set to 1H. As a result, a sampling command is output to the gyro device. Furthermore, the value of the flag TRIM0F that was 1H is cleared to 0H, and the next sampling output is prepared (step 31).
8).

Next, FIG. 7 shows a flowchart of the main routine. At the time of starting the main routine, first, initial setting is performed (step 401). Further, since the reference direction at the start point is set to 0 degree, the 0 degree direction flag TRIM0F is set to 1H (step 40).
2).

First, as described above, it is determined whether 10 minutes have elapsed. It is determined whether the flag SAMPF is 1H (step 403), and if 10 minutes have passed and it is 1H, the process jumps to step 404. If 10 minutes have not elapsed and the flag SAMPF is not 1H, another control routine is executed (step 411) and then step 40
It loops to 3 and checks the value of the flag SAMPF again.

When 10 minutes have passed and the process proceeds to step 404, it is determined whether or not the value of the flag SAMT0F indicating that the sampling command is output is 1H. When this value is 0H, it is not time to output the sampling command, but the process jumps to step 411 described above.

When the value of the flag SAMT0F is 1H,
Jump to step 405 to move to the execution of the sampling instruction. First, the left drive motor 15 and the right drive motor 2 of the truck
Turn off 5 and stop the dolly. The signal required for the gyro sampling command is output to the gyro 18 (step 406). Further, while the gyro 18 is performing the sampling operation, a timer for measuring 15 seconds is operated so that the truck does not run (step 407). When the gyro 18 performs the sampling process to complete the operation of correcting the displacement angular velocity signal, and when 15 seconds have elapsed since the carriage was stopped, the flag SAMPF is reset to 0H (step 409). Left drive motor 15
Then, the right drive motor 25 is driven again to drive the carriage (step 410).

As described above, according to this embodiment, the gyro 1
When the displacement angular velocity signal output from 8 has passed the limit of 10 minutes, which is the limit for ensuring linearity, and when the traveling direction and the traveling direction are almost the same, the main routine processing is interrupted and the gyro Since the output signal is corrected by performing sampling, traveling can be controlled with high accuracy for a long time. Moreover, it is not necessary to use a device that keeps the gyro constant temperature, which is conventionally required, or an expensive device such as an interlock device, and the cost can be reduced.

The above-described embodiments are merely examples and do not limit the present invention. For example, in this embodiment, the predetermined time which is one condition for interrupting is set to 10 minutes. However, depending on the characteristics of the gyro to be used, the output signal may be set to any other time that can ensure the linearity.

[0043]

As described above, the traveling control device for a self-propelled vehicle according to the present invention allows a certain period of time to elapse and the traveling direction to start while the normal processing required for traveling control is being performed. When the direction of time is almost the same, the displacement angle signal is corrected by causing the gyro to perform sampling while interrupting normal processing and stopping the traveling of the trolley. It is possible to control traveling with high accuracy for a long time without requiring an expensive device such as an interlock.

[Brief description of drawings]

FIG. 1 is a block diagram showing a schematic configuration of a traveling control device for a self-propelled carriage according to an embodiment of the present invention.

FIG. 2 is an explanatory diagram showing a state in which each component of the travel control device is mounted in a self-propelled carriage.

FIG. 3 is a block diagram showing a configuration of a main control device in the traveling control device.

FIG. 4 is a flowchart showing a relationship between a main routine process and a timer interrupt process in the travel control device.

FIG. 5 is a flowchart showing an operation of a sampling flag during main routine processing in the traveling control device.

FIG. 6 is a flowchart showing a timer interrupt process in the traveling control device.

FIG. 7 is a flowchart showing a main routine process in the traveling control device.

FIG. 8 is an explanatory diagram showing a state in which the self-propelled carriage deviates from the target direction by an angle θ.

FIG. 9 is an explanatory view showing a state in which the self-propelled carriage deviates from the reference direction as time elapses from the start of traveling.

[Explanation of symbols]

 13, 23 Driver 14 Speed Encoder 15 Left Drive Motor 16 Left Wheel 17 Integrator 18 Gyro 24 Speed Encoder 25 Right Drive Motor 26 Right Wheel 31 Main Control Unit 32 Motor Control Unit 33 Sub Control Unit 40 Timer Interrupt Unit 41 CPU 42 ROM 43 RAM 44 interrupt circuit 45 counter 46 time constant

Claims (1)

[Claims] 1. A pair of drive wheels arranged on the same shaft, a motor for driving the drive wheels, and a speed encoder connected to the drive wheels for detecting respective rotation speeds and outputting a speed feedback signal. A gyro device that detects a displacement angular velocity with respect to a reference direction and outputs a displacement angular velocity signal; a control device that receives the displacement angular velocity signal and outputs a velocity command signal so that the vehicle travels in a predetermined direction; A signal and the speed command signal are given, in a traveling control device of a self-propelled vehicle including a motor control device that performs feedback control of the motor based on the speed command signal, wherein the control device is the speed command signal. When a certain period of time has passed and the traveling direction of the carriage is almost the same as the reference direction during the normal processing for outputting In this case, an interrupt generating means for generating an interrupt, a means for causing the motor control device to stop the driving of the motor for a predetermined time when the interrupt generating means generates an interrupt, and While stopped, a means for giving a sampling command to the gyro device to perform sampling, correcting the displacement angle signal output by the gyro device, and then performing the normal process again is provided. A feature of the traveling control device for the self-propelled carriage.