JPS6093524A - Controller of moving robot - Google Patents

Abstract

PURPOSE:To correct deviation from a target track speedily and stably all the time by correcting the position of a robot by the specific quantity determined by the quantity of the deviation and an angle difference from a track direction when the robot deviates from the track. CONSTITUTION:When the robot body 30 deviates from the track to the right in the moving direction by distance (d) and its angle difference from the track in the moving direction is theta, a CPU judges that the robot deviates from the track to the right and decides on whether d+tantheta is positive or negative. When positive, the right-side driving wheel 41 is rotated at a reference rotating speed V0 and the rotating speed V of the left-side driving wheel 42 is controlled to V0-(d+tantheta) to curve the robot to the left. When negative, on the other hand, the rotating speed of the driving wheel 42 is held at V0 and the rotating speed V of the driving wheel 41 is controlled so that V=V0+(d+tantheta), curving the main body 30 to the right. When the robot deviates to the left, the control is performed reversely.

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) Regarding a mobile robot that performs cleaning work while moving on the floor by unmanned running, the present applicant has proposed, for example, Japanese Patent Application No. 57
-2' No. 32269, etc.

This robot has a cleaning area learning function, and while recognizing its current position within the cleaning area, it moves along a predetermined route and thoroughly cleans the floor within the area. It has the feature 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.

(Objective of the Invention) The present invention aims to provide a control device for a mobile robot, which makes it possible to quickly and stably correct deviations from a target trajectory of the robot during movement in such a mobile robot. purpose.

<Disclosure of the Invention) In a mobile robot that is self-propelled via left and right drive wheels,
A sensor that detects the distance traveled by the robot, a sensor that detects changes in the direction of travel, a position identification means that calculates the robot's position on two-dimensional coordinates based on the outputs of these two sensors, and A learning means for storing the movement area in a map divided into unit blocks of two-dimensional coordinates, a means for making the robot travel in a straight line on a trajectory set along the vertical or horizontal lines of each block of the map, means for inverting the robot on the spot and moving to the next row when the boundary of the area is reached; and means for determining whether the robot is deviated to the right or left with respect to the trajectory during the straight-line traveling; During this deviation, when the deviation amount d with respect to the track and the angle θ in the running direction are positive or negative, and the rotational speed V of the left or right drive wheel is the reference speed VO, V=VO(d +tanθ).

Therefore, according to the present invention, when a cleaning sweeper or a vacuum cleaner is attached to a mobile robot and the robot moves while cleaning the floor, the cleaning area can be specified in advance and learned, and then the travel pattern is determined completely automatically. , without deviating from that running pattern (cleaning can be done most efficiently).

(Example) The present invention will be explained in detail below.

In FIG. 1, 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 a drive wheel; 2 is a direction sensor consisting of a gas rate gyro or the like that detects changes in the robot's travel direction; 3 is distance sensor 1
The distance traveled by the robot is measured by counting the pulse signals from the robot, and the moving direction of the robot is determined from the output of the direction sensor 2, and the current position on the two-dimensional coordinates for each unit distance traveled by the °C10 robot is determined moment by moment. This is a position identification means determined by calculation.

4 are provided on the front, both sides and rear of the robot,
Obstacle sensor 5 detects the presence or absence of obstacles such as walls and pillars and the distance to the obstacles while transmitting ultrasonic waves, and similarly separate from these obstacle sensor 4, it determines obstacles by mechanical contact. These sensors 4 and 5 are touch sensors.
The output is input to a control circuit 6 composed of a microprocessor via an amplifier 7 and an input port 8D. At the same time, the position signal from the identification means 3 is also input to the control circuit 6 via the input/output port 8A.

The control circuit 6 includes 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 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 controls the rotation of the drive motors (servo motors or step motors) 13 and 14 provided on the left and right drive wheels for traveling. At the same time, the cleaning sweeper drive motor 1 is installed on the robot.
Controls the rotation of 5.

Reference numeral 16 indicates an operation unit that can appropriately perform operations such as turning on and off the system power, switching the running mode, setting the start position, and adjusting the sensitivity of the direction sensor 2. Reference numerals 17 and 18 are used to make the robot learn the boundaries of the movement area. , a remote control transmitting and receiving unit which allows the drive circuit 12 to receive a travel command preferentially by radio control and perform the operation at will, and its output is also sent to the control circuit 6 via the input/output port 8B. input.

FIG. 2 is a schematic diagram showing the specific structure of the mobile robot in plan view. are provided, and each bumper 31 to 34
The above-mentioned obstacle touch sensor 5 is attached to the
The system detects when the bumper comes into contact with an obstacle.

In addition, on the front of the robot body 30, there are ultrasonic sensors 4A at the center and both corners, and one ultrasonic sensor 4B on each side, and one on the rear. Ultrasonic sensors 4C in both corners...
is provided 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 robot's bumper may be detected. If 31 to 34 touch lightly, this can be detected.

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

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

When cleaning the area shown in Fig. 4, in order to memorize the movement boundary by learning, set the learning driving mode using the operation unit 16, and then use the remote control transmitting/receiving unit 17.18 to move the lower jCt. Guide the user to the start position (S) shown in the figure, press the set button on the operation unit 16 at that position, and set the start point on the two-dimensional coordinates to
yo) and advance (tooth direction reference θ0 are respectively set.

Then the remote control transmitter/receiver unit 17.1
8 to start the robot traveling along the planned course shown by the dotted line, CPtJ of 1 valley 0 circuit 6
9 sequentially stores the current position (x, y) and traveling direction θ of the robot sent from the position identification means 3 in the storage unit 10, thereby learning the boundary of the area to which the mouth 4 should move.

When the learning course is completed, the moving boundary force (on the two-dimensional coordinate system, the movement boundary force is stored on the map as blocks divided into individual vertical distances corresponding to the X and Y axes.

Next, when the robots 1 to 1 are placed at the start point or point A adjacent to the start point and switched to the unmanned running mode using the operation unit 16, 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 passed through are sequentially stored and held in the storage unit 10.

- When the position identification means 3 determines that the boundary has been reached after traveling in a straight line, move to the left from that position (in the direction of the untraveled line).
The vehicle is then reversed and moved to the next running line (along the X-axis), and then runs in a straight line again. At the same time, the ultrasonic sensor 4B on both sides detects whether there are any obstacles on the left or right side of the running block.
When there is an obstacle, it is stored in the storage unit 10, and when there is no obstacle, it is stored as a runnable block.

Straight travel is repeated sequentially, the number of passing blocks increases by -row, and at the same time the memorized travelable area is traversed.

On the other hand, when the front ultrasonic sensor 4A detects an obstacle at one point in front, the robot is reversed in the direction of the untraveled 511, and at the same time the block where the obstacle was detected 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 you pass the side of the obstacle, there will be blocks that can run simultaneously on both sides of the 071°C cut.

In this case, after reaching the boundary, the vehicle reverses direction (reverse direction) and travels in the untraversed area on the back, where it was not possible to travel due to an obstacle. Point B, where 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 left untraveled area is moved while sequentially reversing in the column direction in the same way as described above, and when it is identified that there are 70 wheels that have already been traveled in the next reversal direction at the 0 point, the untraveled area is moved. Judging that the run is over, move along the row l to the point B.
The vehicle returns in a straight line in the direction f, 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 stroked movement area, and the entire movement is completed.

It is determined whether an untraveled area remains or not by comparing the predetermined running area stored in the storage unit 10 and the obstacle area.

In this embodiment, if there is an untraveled area in the travel column of the default row, the vehicle is moved in this direction immediately when the obstacle disappears. After the vehicle is finished, the driver 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, the drive wheels 41, 42 slip and the drive motor 13.
Due to the rotation error of 14, the robot 1 is rotated as shown in Figure 5.
~ may deviate from the target running trajectory.

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

A means for determining whether the robot is deviated to the right or left side with respect to the trajectory, the deviation of the robot from the trajectory I'd, and the angular difference in the running direction of the robot with respect to the trajectory direction: θ, A means for determining whether d+tanθ is positive or negative, and a means for determining whether d+tanθ≧0 and when ti+tanθ〈0, the rotational speed of the left and right driving wheels 41 or 42 is determined by V=Vo −(d+tanθ
) (where VO: reference rotational speed).

Based on the information (position and direction) from the position identification means 3, the CPU 9 controls the operation of each of these means so that they are repeated at a predetermined period, so that the position of the robot deviates from the trajectory. This is corrected when the vehicle is running straight ahead.

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

As shown in FIG. 5, it is assumed that the robot is now deviated from the trajectory by a distance i to the right in the traveling direction, and that the angular difference in the traveling direction with respect to the trajectory is θ.

In this case, it is determined that the robot's deviation is on the right side of the trajectory, and it is determined by calculation whether d+tanθ is positive or negative.

Here, (i) d+tanθ≧O, (2) d+tanθ
〈Consider two situations of 0.

First, in case (1), (d+tanθ≧o), this means that d is large, or θ is a relatively small value, or the direction of the robot is in a positive angular range, and in this case, the rotational speed of the right drive wheel is While maintaining vo, the rotational speed of the left drive is controlled to be V-Vo-(d+tanθ) (however, the minimum value of V is limited to =Vo), and the motor is curved or rotated to the left.

Next, in case (2) (d + tan θ < 0), this is a case where θ is a negative value and the direction of the trajectory is steep. Then, the rotational speed V of the right drive wheel is expressed as V=VO+(d +t
anθ) to curve or rotate the robot to the right.

In this way, the robot that has moved away from the orbit is brought closer to the orbit, in other words, the position is corrected.

The case where the vehicle deviates to the right side with respect to the trajectory has been described above, but if the vehicle deviates to the left side, the angle is reversed, and the controls for the left and right drive wheels may be completely reversed.

By the way, since the tan function was used as a correction term for the angle θ, if the MAX value is sufficiently large in the range of -M A There is a position and an angle, 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 11td is significantly off, the trajectory is rapidly corrected, and as it approaches the correct trajectory, it approaches at a gentle angle. You can make course corrections.

In addition, positive constants α and β are added to each term of d and tanθ above.
By multiplying by multiplication, it is also possible to arbitrarily set the trajectory correction characteristic up to the point where αd + βtan θ-〇, that is, the point where the speed of the left and right drive wheels is the same and the vehicle moves straight.

(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 equipment of the present invention.
Fig. 2 is a schematic plan view of the robot body, Fig. 3 is a flowchart showing the control operation, Figs. 4 and 5 are explanatory diagrams showing the running state of the robot, and Fig. 6 is a flowchart showing the posture correction control operation. be. DESCRIPTION OF SYMBOLS 1... Distance sensor, 2... Direction sensor, 3... Position identification means, 4 (To 4, 4B, 4C)... Obstacle sensor, 5... Touch sensor, 6... Control circuit, 8A
~8D...I/O board, 9...Central processing circuit (C
PU), 10...Storage unit (ROM, RAM), 12
... Drive circuit, 13.14 ... Wheel drive motor, 1
6... Operating unit, 30... Robot body, 31-34
...Bumper, 41°42...Drive wheel, 43.44
...Sweepa. Figure 2 Figure 3

Claims (1)

[Claims] In a mobile robot that moves by itself via left and right drive wheels,
A sensor that detects the distance traveled by the robot, a sensor that detects changes in the direction of travel, a position identification means that calculates the robot's position on two-dimensional coordinates based on the outputs of these two sensors, and A learning means for storing the movement area in a map divided into unit blocks of two-dimensional coordinates, a means for making the robot travel in a straight line on a trajectory set along the vertical or horizontal lines of each block of the map, means for inverting the robot on the spot and moving to the next row when it reaches the boundary of the area; and means for determining whether the robot is deviated to the right or left side with respect to the trajectory when traveling in a straight line. , at the time of this deviation, the deviation amount d with respect to the track and the angle θ in the running direction, if d + tan θ is positive or negative, the rotational speed V of the left or right drive wheel
A control device for a mobile robot, comprising means for correcting and controlling V=VO(d + tan θ) as @reference speed VO.