JPH0379723B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION (Technical Field) The present invention relates to a control device for a mobile robot that cleans, for example, a floor surface.

(Technical Background) The present applicant has proposed, for example, Japanese Patent Application No. 57-232269, regarding a mobile robot that performs cleaning work while moving on the floor by unmanned running.

This robot has a cleaning area learning function, and while recognizing its current position within the cleaning area, it moves along a predetermined path and thoroughly cleans the floor within the area. As such, it is characterized in that it does not require any position observation means or guidance means arranged along the planned travel route, and can be applied to cleaning any place.

(Object of the invention) The present invention provides a mobile robot as described above.
It is an object of the present invention to provide a control device for a mobile robot that can quickly and stably correct deviations from a target trajectory of the robot during travel.

(Disclosure of the Invention) The present invention provides a mobile robot that is self-propelled via left and right drive wheels. A position identification means that calculates the robot's position on two-dimensional coordinates based on the robot, a learning means that stores the movement area of the robot in a map divided into unit blocks of two-dimensional coordinates, and a column of each block of the map. Alternatively, means for causing the robot to travel in a straight line on a track set along a horizontal row, means for moving the robot to the next row by reversing the robot on the spot when the robot travels in a straight line and reaches the boundary of the area, and a means for moving the robot along the trajectory. deviation amount d
, means for detecting the angular difference θ of the robot's running direction with respect to the track, the speed of the right drive wheel of the robot as V _R , the speed of the left drive wheel as V _L , and α and β as constants. When, V _R −V _L = αd + βtanθ (However, if the deviation amount d is defined as positive when it deviates to the right with respect to the orbit, and negative when it deviates to the left, α is a positive constant. , in the opposite case, α is a negative constant, and when the angular difference θ is defined as positive in the clockwise direction with respect to the traveling direction of the orbit, β is a positive constant, and the angular difference θ is When the counterclockwise direction is defined as positive, β is a negative constant).

Therefore, according to the present invention, when a cleaning sweeper or a vacuum cleaner is attached to a mobile robot to clean the floor surface while moving, the cleaning area can be designated in advance and learned, and the traveling pattern can then be fully automatically determined. By determining the travel pattern, cleaning can be performed most efficiently without deviating from the travel pattern.

(Example) Examples of the present invention will be described below.

In FIG. 1, numeral 1 is a distance sensor that outputs a pulse signal proportional to the travel distance of the mobile robot, for example, the amount of rotation of the drive wheels; 2 is a direction sensor consisting of a gas rate dial or the like that detects changes in the robot's travel direction; 3 measures the distance traveled by the robot by counting pulse signals from the distance sensor 1, determines the moving direction of the robot from the output of the direction sensor 2, and calculates the current position on the two-dimensional coordinates for each unit distance traveled by the robot. This is a position identification means that calculates the position from time to time by calculation.

Obstacle sensors 4 are installed on the front, both sides, and rear of the robot and detect the presence or absence of obstacles such as walls and pillars, as well as the distance to the obstacles, while emitting ultrasonic waves. 5 is also an obstacle sensor. In addition to 4, there is a touch sensor that determines obstacles through mechanical contact, and the outputs of these sensors 4 and 5 are sent to a control circuit 6 consisting of a microprocessor and an amplifier 7.
and is input via input port 8D. At the same time, the position signal of the identification means 3 is also transmitted to the input/output port 8A.
The signal is input to the control circuit 6 via.

The control circuit 6 is composed of a central processing circuit (CPU) 9 and a storage section 10 consisting of a read-only memory (ROM) and a read/write memory (RAM).
11A is a clock pulse generator that outputs clock pulses, and 11B is an interrupt controller.

The CPU 9 outputs a drive signal to the drive circuit 12 via the input/output port 8C as described later, and drives the drive motors (servo motors or step motors) 13, 1 provided on the left and right drive wheels for running.
4, and at the same time controls the rotation of a drive motor 15 of a cleaning sweeper attached to the robot.

Reference numeral 16 denotes an operation unit that can appropriately turn on and off the system power, change the driving mode, set the start position, adjust the sensitivity of the direction sensor 2, etc.;
Reference numerals 17 and 18 refer to remote control transmitting/receiving units that allow the robot to learn the boundaries of the moving area by preferentially interrupting the drive circuit 12 with a traveling command by radio control, and to perform the operation as desired. The respective outputs are also input to the control circuit 6 via the input/output port 8B.

FIG. 2 is a schematic diagram showing the specific structure of the mobile robot in plan view. The above-described obstacle touch sensor 5 is attached to each of the bumpers 31 to 34 to sense when the bumper comes into contact with an obstacle.

Further, on the front surface of the robot main body 30, ultrasonic sensors 4A are installed at the center and both corners, respectively.
Further, one ultrasonic sensor 4B is provided on each side of the vehicle, and ultrasonic sensors 4C are provided at both corners of the rear surface to detect obstacles as described above.

The ultrasonic sensors 4A and 4C normally detect obstacles before the touch sensor 5 comes into contact with the obstacle, but depending on the orientation of the robot, even if an obstacle enters the blind spot, the bumper 31 of the robot ~ 34 is designed so that it can be sensed if it touches lightly.

In this example, the robot main body 30 can move freely by a front running wheel 40 and rear left and right drive wheels 41 and 42, and at the same time two rotary sweepers 4 provided on the lower front surface of the robot main body 30.
The running floor surface is cleaned by steps 3 and 44.

Characteristic parts of the present invention will be further explained with reference to FIGS. 3 to 6 regarding such a robot.

When cleaning the area shown in FIG. 4, in order to memorize the movement boundaries by learning, the operating section 16 is set to learning travel mode, and the remote control transmitting/receiving units 17 and 18 are used to start the robot as shown in the diagram. The robot is guided to position (S), and at that position, the set button on the operation unit 16 is pressed to set the starting point (x0, y0) on the two-dimensional coordinates and the reference θ ₀ for the direction of movement.

Next, when the remote control transmitting/receiving units 17 and 18 are used to start the robot's learning run according to the planned course shown by the dotted line, the CPU 9 of the control circuit 6 receives the robot's current position (x) sent from the position identifying means 3. , y) and the traveling direction θ are sequentially stored in the storage unit 10, thereby learning the area boundaries to which the robot should move.

When the learning course is completed, the moving boundary is stored on the map as blocks divided into unit distances corresponding to the x-axis and y-axis on the two-dimensional coordinate system.

Next, the robot is brought to the start point or a point A adjacent to the start point and switched to the unmanned running mode using the operating section 16, and the control circuit 6 sends a drive signal to the drive circuit 12 to start the robot running.

The robot moves as follows by the CPU 9.

First, move in a straight line over each block in the vertical direction along the x-axis on the map.

At this time, the blocks that the robot has passed through at the same time are sequentially stored and held in the storage section 10.

When the position identification means 3 determines that the boundary has been reached after traveling in a straight line, it is reversed from that position to the left (in the direction of the untraveled column) and transferred to the next traveling column (along the x-axis), and then again. Drive in a straight line. At the same time, the ultrasonic sensors 4B on both sides detect whether or not there are obstacles on the left and right sides of the running block, and if an obstacle is encountered, it is stored in the storage unit 10, and if there is no obstacle, it is stored as a runnable block. do.

Straight travel is repeated sequentially, and the number of passing blocks increases one row at a time, and at the same time, the memorized possible travel range is erased.

On the other hand, when the front ultrasonic sensor 4A detects an obstacle in front at point F, the robot is reversed in the direction of the untraveled row, and at the same time the block that detected the obstacle is The information is stored in the storage unit 10.

In this way, the vehicle travels back and forth between the obstacle and the boundary without colliding with the obstacle while detecting it, and then, when no obstacle is detected in front of the vehicle, the boundary is identified in the column direction. Proceed until.

At this time, if the robot passes the side of the obstacle, there will be blocks on both sides of the robot that can run at the same time.

In this case, after reaching the boundary, the vehicle reverses direction and travels in the untraversed area on the back, which has been made impossible due to obstacles. The point B at which the inversion was performed at this time is stored in the storage unit 10 for the next time it is returned to its original position.

This remaining untraveled area is moved while sequentially reversing in the column direction in the same manner as described above, and when it is identified at point C that there is a block that has already been traveled in the next reversal direction, the vehicle moves in the untraversed area. When it is determined that this is the end, the vehicle travels in a straight line in the row direction to point B and returns, and starts traveling in the column direction again from point D, which is the block next to point B.

In this way, when the vehicle reaches point G, it is determined that there is no untraveled area in the learned movement area, and the entire movement is completed.

Determination as to whether or not an untraveled area remains is performed by comparing the traveled area stored in the storage unit 10 with the obstacle area.

In this example, if there is an untraversed area in the already-traveled train, the train immediately moves in that direction as soon as there are no obstacles, but once all the trains within the boundaries are stopped. After finishing, you may move to the remaining untraveled area.

By keeping the sweepers 43 and 44 rotating while the robot is moving, the floor surface of the moving area can be thoroughly and efficiently cleaned.

In this way, the mobile robot is made to run on its own, but at this time, due to slippage of the drive wheels 41, 42, rotation error of the drive motors 13, 14, etc., the robot deviates from the target running trajectory as shown in Fig. 5. Sometimes it comes.

The present invention is configured as follows in order to correct this deviation from the target trajectory.

A means for detecting the amount of deviation of the robot from the orbit, a means for detecting the angular difference of the robot from the orbit, and a means for detecting the deviation of the robot from the orbit when the robot is deviating from the orbit in either direction. amount:
d, angular difference in the robot's running direction with respect to the trajectory:
When θ is assumed, d is positive when the robot is deviated to the right with respect to the orbit, and angular difference θ is positive in the clockwise direction, (rotational speed of the right drive wheel 41) -
Means is provided for controlling the rotational speed of the left and right driving wheels so that (rotational speed of the left driving wheel 42)=αd+βtanθ (where α and β are positive constants).

In this case, if d is positive when the robot deviates to the left with respect to the orbit, then if the polarity of α is reversed to the above, the meaning of this equation is exactly the same, and similarly, If θ is made positive in the counterclockwise direction, the polarity of β may be reversed.

Specifically, when d is positive when the deviation is to the right, α is positive, when d is positive when the deviation is to the left, α is negative, and when θ is positive when the clockwise direction is positive, β is When θ is positive and θ is positive in the counterclockwise direction, β is negative.

In other words, in the previous equation, θ gives αd+βtanθ=0 for any polarity of d and θ.
so that the direction of is the direction approaching the target orbit,
Select the polarity of α and β.

Based on the information (position and direction) from the position identification means 3, the CPU 9 controls the operations of these means so that they are repeated at a predetermined cycle, and when the robot's position deviates from the orbit, , this is corrected to make the vehicle run straight.

The operation routine is shown in FIG. 6, and will be explained with reference to FIG.

First, the amount of deviation d of the current position from the target and the angle difference θ are read, and it is determined whether the position has deviated from the orbit. If there is no deviation, the same reference speed Vo is maintained as the speed of the left and right drive wheels, and the vehicle travels straight along the track.

On the other hand, if the robot has deviated from the trajectory, for example, as shown in Fig. 5, the robot has now deviated by d (positive) to the right in the direction of movement with respect to the trajectory, and the trajectory in the direction of movement is The angle difference is θ
(Correct).

In order to correct this deviation of the robot, the rotation speed of the drive wheels is corrected and controlled so that the speed difference between the right drive wheels and the left drive wheels (right side speed - left side speed) becomes αd+βtanθ.

In this case, to give such a speed difference to the left and right drive wheels, for example, while maintaining the speed of the left drive wheel at the reference speed Vo, the speed of the right drive wheel is V = Vo + (αd + β tan θ ), or the speed of the right drive wheel is set as the reference speed Vo,
The speed of the left driving wheel may be V=Vo-(αd+βtanθ).

Regarding the specific behavior of the robot when performing such control, (1) d + tan θ > 0, (2) d + tan θ
=0, (3) d+tanθ<0 will be explained separately.
Note that here, for convenience of explanation, α=β=1.

(1) When d+tan θ>0 This is the case when d is large, θ is a relatively small value in the negative direction, or the robot is oriented in a positive angle range. d+tanθ>0 means that the rotation speed of the right drive wheel is high,
The robot moves forward while turning to the left.

(2) When d+tanθ=0 The speeds of the left and right drive wheels are the same, and the vehicle moves straight. However, in the above settings, since d>0, tanθ
<0, that is, θ<0, and the robot heads toward the orbit facing left.

(3) When d+tanθ<0 This is the case when d is small or θ is in the angle range with a large value in the negative direction. d+tanθ<0 means that the rotation speed of the left drive wheel is high,
The robot moves forward while turning to the right.

In this way, the robot, which has now deviated to the right with respect to the orbit, is gradually corrected in position toward the orbit by applying a speed difference of (d+tanθ) to the left and right drive wheels. Then, until the orbit is correct, the value of (d + tanθ) is actually (1) to (3)
As the robot changes as shown in the figure, the robot, which had shifted to the right, first moves forward while rotating largely to the left, then continues straight toward the orbit.
After approaching orbit, it then turns in the opposite direction and slowly approaches orbit.

The above explained the case where the robot deviates to the right side of the trajectory, but if it deviates to the left side, d+
The position correction can be performed in the same way as long as the condition that tanθ=0 is symmetrical with respect to the orbit.

By the way, since the tan function was used as the correction term for the angle θ, in the range of -MAX<tanθ<MAX
If the MAX value is sufficiently large, even if the deviation d from the orbit is quite large, the position where d + tan θ = 0,
An angle exists, and at that point, the left and right drive wheels are at constant speed, and the larger d is, the closer the direction is perpendicular to the orbit, and conversely, the smaller d is, the smaller the angle of intersection.

Therefore, when the distance d deviates greatly, the trajectory is corrected rapidly, and as the trajectory approaches the correct trajectory, it approaches at a gentler angle. This prevents excessive control such as hunting during trajectory correction.
You can make smooth course corrections.

In addition, positive constants α and β are added to each term of d and tanθ above.
By multiplying, it is possible to arbitrarily set the trajectory correction characteristics up to the point where αd+βtanθ=0, that is, the point where the speed of the left and right drive wheels is the same and the vehicle moves straight. At this time, the larger the value of α, the faster the return to the orbit is given, and the larger the value of β, the more the property is given to keep the object parallel to the orbit.

(Effects of the Invention) As described above, according to the present invention, when the robot deviates from the target trajectory while traveling straight, the position and orientation can be quickly and stably corrected according to the amount and direction of the deviation. There is an effect.

[Brief explanation of drawings]

Fig. 1 is a block circuit diagram of the robot control device of the present invention, Fig. 2 is a schematic plan view of the robot main body, Fig. 3 is a flowchart showing the control operation, Fig. 4,
Figure 5 is an explanatory diagram showing the running state of the robot;
The figure is a flowchart showing the posture correction control operation. 1... Distance sensor, 2... Direction sensor, 3...
Position identification means, 4 (4A, 4B, 4C)... Obstacle sensor, 5... Touch sensor, 6... Control circuit, 8A to 8D... Input/output port, 9... Central processing circuit (CPU), 10... …Storage unit (ROM,
RAM), 12... Drive circuit, 13, 14... Wheel drive motor, 16... Operating unit, 30... Robot body, 31-34... Bumper, 41, 42...
...Drive wheel, 43, 44...Sweeper.

Claims (1)

[Claims] 1. In a mobile robot that moves by itself via left and right drive wheels, there is a sensor that detects the travel distance of the robot, a sensor that detects a change in the travel direction of the robot, and a sensor that detects a change in the travel direction of the robot, based on the outputs of both sensors. A position identification means that calculates the position of the robot on two-dimensional coordinates, a learning means that stores the movement area of the robot in a map divided into unit blocks of two-dimensional coordinates, and a learning means that stores the movement area of the robot in a map divided into unit blocks of two-dimensional coordinates, and means for causing the robot to travel in a straight line on a trajectory set along the above-mentioned trajectory; means for causing the robot to travel in a straight line and, upon reaching the boundary of the above-mentioned area, turn the robot on the spot and move to the next line; A means for detecting the amount d, a means for detecting the angular difference θ of the robot's running direction with respect to the track, and a means for detecting the speed of the right drive wheel of the robot as V _R , the speed of the left drive wheel as V _L ,
Further, when α and β are constants, V _R −V _L = αd + βtanθ (However, if the deviation amount d is defined as positive when it deviates to the right with respect to the orbit, and negative when it deviates to the left) α is a positive constant; vice versa, α is a negative constant; and when the angle difference θ is defined as positive in the clockwise direction with respect to the direction of travel of the orbit, β is positive. When the angle difference θ is defined as positive in the counterclockwise direction, β is a negative constant)
1. A control device for a mobile robot, comprising means for performing correction control so that the following is achieved.