JPH0895638A - Travel controller for mobile working robot - Google Patents

Abstract

PURPOSE: To make a robot stably travel over a long distance by correcting shift of reference angle of an angle detection device obtained from the detection data within a range where a distance detection device can detect with the angle detection device in a traveling mode of the robot. CONSTITUTION: A controller 4 consists of a velocity command arithmetic unit 41 and a reference angle correction device 42. The unit 41 calculates the distance between a mobile working robot main body 1 and a wall and also the tilt of the body 1 against the wall and outputs the velocity commands to the drive devices 5 and 6. The device 42 corrects the reference angle of a gyro sensor 3 every time the traveled distances detected by the right and left encoders 9 and 10 connected to a rotary shaft of the wheels 71 and 72 which detect the traveled distances of the body 1 reach a fixed value. Then, the device 42 inputs the corrected reference angle to the device 41. The reference angle of the sensor 3 becomes 0 when a power supply is applied to the sensor 3 and then corrected every time the traveled distance of the body 1 reaches the fixed value.

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 mobile work robot that performs some work while detecting a distance from a wall in a work area, such as a self-propelled cleaner.

[0002]

2. Description of the Related Art Conventionally, as an example of a mobile work robot such as a self-propelled cleaner for cleaning the inside of a train, a mobile work robot equipped with an angle detection device for detecting the inclination between a wall in the work area and the main body of the mobile work robot. There is. This angle detecting device is attached to a side wall of a mobile work robot main body, and two distance sensors for detecting a distance to the wall, and an operation for calculating an inclination between the wall and the robot main body based on detection values obtained by the both distance sensors. It is composed of devices.

FIG. 8 is for explaining this, and is provided with a front distance sensor 13 and a rear distance sensor 14 for detecting the distance from the side wall of the mobile work robot body 1 and the wall 11 in the work area. An arithmetic unit (not shown) for inputting the detection values detected by both the sensors 13 and 14 and for calculating the inclination with respect to the wall 11, that is, the reference angle θ 0, is provided based on the equation (1).

Θ ⁰ = tan ^-1 [(L 1 −L 2) ÷ L 21] (1) where θ 0: clockwise plus (+) L 1: distance between distance sensor 14 and wall 11 L 2: distance sensor 13 and wall Distance L11: Distance between the distance sensor 13 and the distance sensor 14 In such a configuration, the detection values (distance information) obtained by the distance sensors 13 and 14 at the start of traveling are input to the arithmetic device and detected here. The angle is calculated, and the angle detection device is initialized by this. Then, in the actual traveling of the mobile work robot, feedback control is performed by the output of the angle detection device to perform traveling.

[0005]

In the case of such traveling control, it is effective when there is no error in the reference angle θ0 of the angle detecting device. However, if there is an error in the angle detection device drift or the reference angle θ0 that occurs during traveling, a deviation occurs in the traveling route during long-distance traveling, so the traveling is stopped midway and the reference angle of the angle detection device is re-established. It was necessary to reset the angle. Further, in order to extend the correction distance, it is necessary to extend the mounting positions of the distance sensors 13 and 14 or to make the characteristics between the distance sensors 13 and 14 uniform. Such improvement in accuracy and extension of distance have problems that the mobile work robot becomes large in size, weight increases, and adjustment elements increase.

Therefore, according to the present invention, it is possible to travel stably for a long distance, and it is possible to achieve both running stability and responsiveness, and furthermore, it is possible to accurately obtain the inclination with respect to the wall in the work area. An object is to provide a travel control device for a mobile work robot.

[0007]

In order to achieve the above object, the invention according to claim 1 is such that at least three wheels are driven by a drive device and a predetermined work is performed while automatically traveling in a work area having a wall. A mobile work robot main body for carrying out the above, at least one distance detection device attached to a side wall of the mobile work robot main body at a position facing the wall, and detecting a distance to the wall, the mobile work robot main body And an angle detecting device for detecting an angle between the wall and the wall, and a distance and an inclination between the mobile work robot main body and the wall are calculated based on a detection angle obtained by the angle detecting device and a detection distance obtained by the distance detecting device. A speed command calculation device that outputs a speed command to the drive device, a movement amount detection device that detects the movement distance of the mobile work robot body, and A mobile work robot equipped with a reference angle correction device that corrects the reference angle of the angle detection device every time the traveling distance detected by the movement amount detection device reaches a certain value, and inputs this to the speed command calculation device. It is a traveling control device.

In order to achieve the above object, the invention corresponding to claim 2 is the control device for a mobile work robot according to claim 1, wherein the reference angle correction device is a detection angle obtained by the angle detection device, A travel control device for a mobile work robot, wherein a travel distance for correcting a reference angle of the angle detection device is varied according to a deviation angle from an angle obtained based on a detection distance obtained by the distance detection device. Is.

In order to achieve the above-mentioned object, the invention corresponding to claim 3 is the control device for a mobile work robot according to claim 1, wherein as an initial value of the angle detection device, when the main body of the mobile work robot starts traveling. A traveling control of a mobile work robot, characterized in that a value obtained by calculating an inclination with respect to the wall by a specific one of the distance detecting devices is used for the wall by moving the main body of the mobile work robot straight for a predetermined distance. apparatus. Is.

[0010]

According to the invention corresponding to claim 1, in the range where the distance detecting device can detect while traveling by the angle detecting device,
Since the deviation of the reference angle of the angle detecting device is corrected from the detected data, it is possible to drive stably for a long distance.

According to the second aspect of the invention, the traveling stability and the responsiveness can be made compatible by varying the correction traveling distance according to the size of the correction angle of the angle detecting device.

According to the third aspect of the invention, the inclination of the wall can be accurately obtained by a single distance detecting device by moving the robot body straight ahead for a certain distance at the start of traveling of the mobile work robot. it can.

[0013]

Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a diagram showing a schematic configuration of a traveling control device for a mobile work robot according to a first embodiment of the present invention.
It has wheels 71, 72, 8 of which the wheels 71, 7 are
2 are driven by a right drive device 5 and a left drive device 6, respectively, and are a mobile work robot body 1 for performing a predetermined work while automatically traveling in a work area having a wall 11, and a side wall of the mobile work robot body 1. At least one which is mounted at a position facing the wall 11 and detects a distance from the wall 11
Individual distance detecting devices, for example, an ultrasonic distance sensor 2, an angle detecting device for detecting a rotation angle between the mobile work robot body 1 and the wall 11, for example, a gyro sensor 3, and a detection angle and a distance sensor obtained by the gyro sensor 3. 2 and the encoders 9 and 1 described later
The control device 4 is provided for inputting the traveling distance detected by 0 and controlling the operation of the mobile work robot body 1 based on these input values.

The control device 4 comprises a speed command calculation device 41 and a reference angle correction device 42. The speed command calculation device 41 calculates the distance and inclination between the mobile work robot body 1 and the wall 11, and the drive devices 5, 6 are provided. To the reference angle correction device 42, and the reference angle correction device 42 detects the movement distance of the mobile work robot body 1 by a movement amount detection device such as wheels 71, 72.
Right and left encoders 9 and 1 respectively connected to the rotary shafts of
0, the reference angle of the gyro sensor 3 is corrected every time the traveling distance detected by the encoders 9 and 10 reaches a constant value, and this is input to the speed command calculation device 41.

The reference angle of the gyro sensor 3 becomes 0, for example, when the gyro sensor 3 is turned on, and the reference angle is corrected every time the traveling distance of the robot body 1 reaches a constant value.

The operation of the first embodiment configured as described above will be described with reference to FIGS. 2 shows an operation pattern in which the mobile work robot body 1 travels with the gyro sensor 3, FIG. 3 shows output data of the ultrasonic distance sensor 2, and FIG. 4 is for explaining the operation of the control device 4. It is a flowchart of.

First, the case where the robot body 1 travels along the wall 11 having the passage 12 from the point A to the point B in FIG. 2 will be described. The robot body 1 is guided by the gyro sensor 3 from the point A to the point B [step (hereinafter referred to as S) S1 in FIG. 4].

The details of S1 are shown in FIG.
1, the difference between the reference angle and the output angle of the gyro sensor 3 is obtained, and it is determined whether there is an error (S2).
2). When it is determined that there is an error in S22, the deviation is calculated (S23). Next, the wheel speed difference is obtained (S24), and the speed output is obtained (S2).
5). If it is determined in S22 that there is no error, a speed command is output and the process proceeds to S25.

After such processing, the control device 4 calculates the movement amounts ΔYL and YL in the Y-axis direction for each minute unit time from the angle data from the gyro sensor 3 and the distance data from the encoders 9 and 10. (S2).

YL = Σ (ΔYL) (2) Next, it is checked whether the traveling distance X1 of the robot body 1 has reached the set value C1. If the traveling distance X1 has not reached the set value C1, the distance from the distance sensor 2 to the wall 11 is checked. Input the distance data Ln. Passage 12
Input the data when the vehicle is running and the distance data Ln to the wall 11. The data when the road 12 is running and the data of the uneven portion of the wall 11 are filtered by a limiter or the like. Further, the data when the detected data is abnormal due to noise or the like is also deleted (S2 to S6).

In this case, the data of the passage portion may be processed by adding information such as detectable / impossible to a travel map or the like instead of the limiter. After that, the Y-direction error distances ΔL and en are calculated from the filtered data (S7).

.DELTA.L = Ln- (Ln-1) (3) en = en-1 + .DELTA.L (4) where Ln: distance to the surface of the current wall 11 Ln-1: to the surface of the previous wall 11 Distance en −1: Previous Y-direction error distance The above-described processing is repeated until the traveling distance X1 of the robot body 1 reaches the set value C1.

Next, when the traveling distance X1 of the robot body 1 reaches the set value C1, first, the difference eg between the movement amount Y1 in the Y-axis direction by the gyro sensor 3 and the movement amount en in the Y-axis direction by the distance sensor 2 is taken. (Error of gyro sensor 3) is calculated (S8).

Eg = en-Y1 (5) It is judged whether the error eg of the gyro sensor 3 is less than or equal to the set value e1 (S9). When the error eg of the gyro sensor 3 is less than the set value e1, the reference angle θ of the gyro sensor 3
If 0 is set, the processing is performed normally. In S9, the error eg
If the value exceeds the set value e1, it is judged that the reference angle θ0 of the gyro sensor 3 is deviated, and the gyro correction angle θ
gc is calculated by the equation (6) (S10).

Θgc = −tan ⁻¹ (eg / C1) (6) After that, the reference angle θ0 of the gyro sensor 3 is corrected by the gyro correction angle θgc, and the correction operation is repeated again (S11,
S12).

Θ0 = θ0 + θgc en = 0, Y1 = 0, X1 = 0 Thus, in this embodiment, the reference angle θ0 of the gyro sensor 3 is repeated from the detection distance detected by the single distance sensor 2. By correcting, the robot body 1
It is possible to drive stably for a long distance without stopping.

FIG. 6 is a flow chart for explaining the control operation of the control device 4 of FIG. 1 for explaining the second embodiment of the present invention. In the figure, the operation of correcting the reference angle θ0 of the gyro sensor 3 from S1 to S11 with the gyro correction angle θgc is the same as in FIG.

After correcting the reference angle θ0 of the gyro sensor 3, the set value C2 of the next running distance X1 is calculated (S1).
3). The set value C2 is set short when the gyro correction angle θgc is large, and set long when the gyro correction angle θgc is small.

C2 = K ÷ θgc (7) However, K = proportional constant Finally, each constant is set and the auxiliary operation is repeated (S1).
4).

C1 = C2 en = 0, Y1 = 0, X1 = 0 As described above, by varying the correction traveling distance according to the gyro correction angle θgc, both traveling stability and responsiveness can be achieved. it can.

In this case, the variable condition of the correction distance, which is executed in this case, is executed according to the gyro correction angle θgc, but the present invention is not limited to this. For example, the traveling start time is shortened and the correction distance is gradually increased. You can also do it.

Next, a third embodiment of the present invention will be described with reference to FIG. 7. FIG. 7 shows an operation pattern for calculating an initial value of the gyro sensor 3. First, the operator sets the robot body 1 at point C in FIG. At this time, the distance sensor 2 measures the distance L1 to the wall 11. After that, the vehicle travels straight for the travel distance L21, and then D in FIG.
Run to the point. At this time, the distance L2 to the wall 11 is measured. Based on the distances L1 and L2 detected by the distance sensor 2, the reference angle θ0 of the gyro sensor 3 is obtained by the equation (8), and the gyro sensor 3 is initialized.

Θ 0 = tan ⁻¹ (L ¹ −L 2) / L 21 (8) However, θ 0: The clockwise direction is a plus (+). As shown in FIG. 7, when the robot body 1 travels straight for the travel distance L21, the single distance sensor 2 causes the wall 11 to move.
The reference angle θ0 of the gyro sensor 3 can be initialized by calculating the inclinations of and.

Further, by making the traveling distance L21 longer than the body length of the robot body 1, the accuracy of the reference angle θ0 can be improved as compared with the conventional case where two distance sensors 2 are installed on the wall surface of the robot body 1. Can be raised.

In the above embodiment, the case where the output of one distance sensor 2 is input to the control device 4 has been described.
However, the present invention is not limited to this, and a plurality of distance sensors 2 may be provided, and the respective detection outputs may be input to the control device 4 to obtain the speed command.

[0036]

According to the present invention, it is possible to provide a traveling control device for a mobile work robot which can obtain the following operational effects. That is, according to the invention corresponding to claim 1, the deviation of the reference angle of the angle detection device is corrected from the detected data in the range that the distance detection device can detect while traveling by the angle detection device. You can drive stably over long distances.

According to the second aspect of the invention, the traveling stability and the responsiveness can be made compatible by varying the correction traveling distance according to the size of the correction angle of the angle detecting device.

According to the invention according to claim 3, when the traveling of the mobile work robot is started, the robot main body is moved straight for a predetermined distance, whereby the inclination with respect to the wall can be accurately obtained by a single distance detecting device. it can.

[Brief description of drawings]

FIG. 1 is a diagram showing a schematic configuration of a travel control device for a mobile work robot according to a first embodiment of the present invention.

FIG. 2 is an operation pattern diagram for explaining the operation of FIG.

FIG. 3 is a diagram showing an output waveform of the distance sensor shown in FIG.

4 is a flowchart for explaining the operation of the control device in FIG.

FIG. 5 is a flowchart showing details of S1 in FIG.

FIG. 6 is a flowchart for explaining the operation of the second embodiment of the traveling control device for the mobile work robot according to the present invention.

FIG. 7 is an operation pattern diagram for explaining the operation of the second embodiment of the traveling control device for the mobile work robot according to the present invention.

FIG. 8 is an operation pattern diagram for explaining an operation of an example of a conventional traveling control device for a mobile work robot.

[Explanation of symbols]

1 ... Mobile work robot body, 2 ... Ultrasonic distance sensor,
3 ... Gyro sensor, 4 ... Control device, 41 ... Speed command calculation device, 42 ... Reference angle correction device, 5, 6 ... Drive device,
71, 72, 8 ... Wheels.

 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Hideyuki Takahashi No. 1 Toshiba-cho, Fuchu-shi, Tokyo Inside the Fuchu factory, Toshiba Corporation

Claims (3)

[Claims] 1. A mobile work robot main body for performing a predetermined work while automatically traveling in a work area having a wall, at least three wheels being driven by a drive device, and a side wall of the mobile work robot main body which is the wall. At least one distance detecting device which is mounted at a position facing to the wall and detects a distance to the wall; an angle detecting device which detects an angle between the mobile work robot main body and the wall; Calculating the distance and inclination between the mobile work robot body and the wall based on the detection angle obtained and the detection distance obtained by the distance detection device,
A speed command calculation device that outputs a speed command to the drive device, a movement amount detection device that detects a movement distance of the mobile work robot body, and a traveling movement distance detected by the movement amount detection device is a constant value. A travel control device for a mobile work robot, comprising: a reference angle correction device that corrects the reference angle of the angle detection device each time it reaches and inputs this to the speed command calculation device. 2. The control device for a mobile work robot according to claim 1, wherein the reference angle correction device is an angle calculated based on a detection angle obtained by the angle detection device and a detection distance obtained by the distance detection device. A travel control device for a mobile work robot, characterized in that a travel distance for correcting a reference angle of the angle detection device is varied according to a deviation angle from the angle. 3. The control device for a mobile work robot according to claim 1, wherein, as an initial value of the angle detection device, the mobile work robot main body is moved straight ahead by a predetermined distance at the start of traveling of the mobile work robot main body. A travel control device for a mobile work robot, wherein a value obtained by calculating an inclination with respect to the wall by a specific one of the distance detection devices is used for the wall.