JPS63241609A - Running control method for free-running robot - Google Patents

Abstract

PURPOSE:To precisely execute the control of running by correcting the error of the attitude angle of a free-running robot caused by the resolution of a gyroscope with a soft program every time the angle speed data of the gyroscope is read. CONSTITUTION:The attitude angle (an angle on a coordinate showing a progress direction) of the free-running robot is computed by integrating angle alteration quantity of the attituted angle obtained from the gyroscope 16. In this case, the error of the angle alteration quantity caused by the resolution of the gyroscope is corrected every time the angle alteration quantity data of the attitude angle is inputted in a controller 20 from the gyroscope 16. Since the attitude angle of the free-running robot is always computed as an accurate angle, the error between the computed value of the coordinate of the position of the free-running robot, which is obtained by computing based on the attitude angle, and a real coordinate is removed.

Description

[Detailed Description of the Invention] [Industrial Application Field] The present invention relates to a self-propelled robot, and particularly relates to a highly accurate traveling control method. [Prior Art] Conventionally, the posture of an unmanned vehicle has been As described in Japanese Utility Model Application Publication No. 58-185511, the angle (direction of travel) is corrected by correcting errors in the attitude angle caused by fluctuations in the zero point of the gyro every time the vehicle stops or by adjusting a This method detects marks and makes corrections when the vehicle deviates from the designated driving course and needs to be corrected. However, no consideration was given to correcting errors due to the resolution of the gyro.

[Problem that the invention seeks to solve]

In addition to the zero point fluctuation of the gyro, causes of errors in the gyro that affect the attitude angle include the quality of the gyro resolution. The error in the attitude angle caused by the gyro resolution is equivalent to the error in the attitude angle due to the zero point variation, or if the resolution is poor, it causes an error greater than the zero point variation. Therefore, attitude angle correction based only on gyro zero point fluctuations in the prior art is insufficient.

Additionally, errors in attitude angle due to gyro resolution constantly accumulate during driving. Therefore, with conventional technology that corrects the attitude angle each time the vehicle stops or when correction is necessary by comparing marks, the trajectory of the vehicle until it stops or corrects the attitude angle is incorrect, resulting in The object of the present invention is to correct errors in the attitude angle caused by the resolution of the gyro each time the gyro angle data is input, and to make the vehicle run smoothly along a specified course. It's about controlling.

[Means for resolving issues]

The purpose of the above is to correct the angular error due to the gyro resolution from the attitude angle of the vehicle, which was not corrected from the attitude angle obtained from the gyro in the conventional technology, and the timing of correction, but in the past, when the vehicle stopped Or, the attitude angle is corrected when it becomes necessary to correct the attitude angle obtained from the gyro data by detecting marks set on the side walls of the running road and comparing them with the marks seen from the designated course. On the other hand, this is achieved by correcting the angular error due to the gyro resolution each time the attitude angle change data is input from the gyro.

[Effect]

The attitude angle (the angle on the coordinates indicating the direction of movement) of the self-propelled robot (traveling vehicle) is calculated by integrating the amount of angular change in the attitude angle obtained from the gyro.

The attitude angle correction method of the present invention corrects an error in the amount of change in angle caused by the gyro resolution each time data on the amount of change in attitude angle is input from the gyro to the control device. As a result, the attitude angle of the self-propelled robot is always calculated to be an accurate angle, so there is no difference between the calculated value of the self-propelled robot's position coordinates, which is calculated based on the attitude angle, and the actual coordinates. , it is possible to control the movement of a self-propelled robot with good precision.

〔Example〕

DESCRIPTION OF THE PREFERRED EMBODIMENTS An example of a self-propelled cleaning robot for the purpose of cleaning will be described below with reference to the drawings as an embodiment of the present invention.

FIG. 1 is a perspective view showing the overall configuration of a self-propelled cleaning robot according to this embodiment. 1 is a left wheel, 2 is a left wheel drive motor that drives the left wheel 1, 6 is an encoder for the left wheel that measures 10 revolutions of the left wheel, 4 is the left wheel, 6 .

This is a gear case that houses a gear that connects the wheel 1 and the left wheel drive motor 2 and a gear that connects the left wheel 1 and the encoder 3. 5 is a right wheel, 6 is a right wheel drive motor that drives the right wheel 5, 7 is a right wheel encoder that measures the number of rotations of the right wheel 5, and 8 is a gear that connects the right wheel 5 and the right wheel drive motor 6. and a gear case that houses gears that connect the right wheel 5 and the encoder 7. 9 is an ultrasonic transceiver that emits and receives ultrasonic waves, and 10 is a rotating disk. Reference numeral 12 is a parabolic antenna for ultra-FTL, which has the function of reflecting the ultrasonic waves emitted from the ultrasonic transmitter/receiver 9 in the direction of the dotted line as ultrasonic waves with sharp directivity, and the function of reflecting the ultrasonic waves emitted from the ultrasonic transmitter/receiver 9 in the direction of the dotted line when it hits the wall of the room or the surface of an obstacle. It functions to guide the ultrasonic waves returned in the opposite direction to the ultrasonic transmitter/receiver 9. The ultrasonic transmitter/receiver 9 and parabolic antenna 12 are fixed to a rotating disk 10 and rotate together with the disk 10 around a rotating shaft 11. 13 is an ultrasonic radar rotation motor that rotates the rotary disk 10; 14 is an encoder for ultrasonic radar that measures the ultrasonic transmission and reception directions of the parabolic antenna 12; 15 is an ultrasonic radar; 4;

This is a gear case that houses gears that connect the rotary motor 13, the rotary shaft, and the radar encoder 14. The gear case also includes an ultrasonic transceiver 9 and a parabolic antenna 12,
It also serves as a frame that supports the rotating disk 10. The above-described ultrasonic transmitter/receiver 9, parabolic antenna 12, rotating disk 10, rotating shaft 11, radar rotation motor, radar encoder 14, and gear case 15 are collectively referred to as an ultrasonic radar.

16 is a gyro, which indicates the attitude angle (progressing direction) of the self-propelled robot.
Measure the amount of angular change. 17 is a vacuum cleaner, and its cleaning method is a vacuum cleaner type.

18 is a dirt suction port connected to the vacuum cleaner 17.

Reference numeral 19 denotes a measurement circuit section which is composed of a circuit for detecting data from an ultrasonic radar, a measurement circuit for a gyro, and a measurement circuit for left and right wheel encoders. 20 is a travel control section of the self-propelled cleaning robot. 22 is a control power source connected to the travel control unit 20; 23 is a left and right wheel drive motor 2, 6;
and the motor of the vacuum cleaner 17 and the ultrasonic radar rotation motor 13
This is the drive power supply connected to. 24 is a robot main body frame on which the above-mentioned components are arranged and fixed. 25 is a caster attached to the main body frame 24,
The main body frame 2 is supported by three points on the left and right wheels 1 and 5.
4 is supported parallel to the urine. 26 is a robot body that houses the above components. Reference numeral 27 denotes an operation unit for switching the driving method and switching between automatic driving and manual driving.

FIG. 3 is a system diagram of travel control, and each component corresponds to FIG. 2. In FIG. 5, 20 is a travel control section. The travel control unit 20 includes an ultrasonic radar detection circuit 28
The ultrasonic radar encoder measuring circuit 29, the gyro measuring circuit 30, and the left and right wheel encoder measuring circuit 31 are connected to a data input port. Further, in the travel control unit 20, the left and right wheel drive motors 2.6, the motor of the cleaner 17, and the ultrasonic radar rotation motor 13 are connected to ports that output travel commands and start/stop of cleaning. Furthermore, the travel control unit 20 includes a memory 20a that stores the robot's own position coordinates and the position coordinates of walls and obstacles in the room, and a control power supply 2.
2 are connected. The ultrasonic radar detection circuit 28 includes an ultrasonic transmitter/receiver 9 or a radar encoder measurement circuit 29.
has an ultrasonic radar encoder 14, a gyro measurement circuit 30 has a gyro 16, and a wheel encoder measurement circuit 31.
Left and right wheel encoders 3.7 are respectively connected to the left and right wheel encoders 3. Further, the drive power source 23 is connected to the left and right wheel drive motors 2,
6, vacuum cleaner 17 motor, and ultrasonic radar rotation motor 13
is connected to.

Next, a control method performed by the travel control section 20 will be explained. The running method of the self-propelled cleaning robot of this embodiment is as shown at 34 in FIG. 5, by repeatedly going straight and making U-turns, the robot runs along the walls of the room, and thoroughly cleans the inside of the room. Then, the travel control unit 20 calculates the robot's own position coordinates, calculates the position coordinates of obstacles including the walls of the room, and calculates the position coordinates of the obstacles at points 35, 36, and 37 in FIG.
The driving method is determined based on the memory as a scene map as shown in 58, the self-position coordinates, and the scene map.

The travel control unit 20 repeatedly performs the self-position coordinate calculation, obstacle position calculation, and travel judgment at a constant period Δ every second.

First, a method for calculating self-position coordinates will be explained with reference to FIGS. 2 and 4. FIG. 2 is a flowchart of the travel control performed by the travel control section 20, and the processing from TART 5 to connector 1 at the lower right is performed at the constant period Δ in seconds. FIG. 4 is a diagram in which the starting point is the coordinate origin 0, and the vehicle travels from point a to point b in seconds at the above-mentioned period Δ at a certain time after departure.

Further, a situation is shown in which the attitude angle changes from θ1 to θb. Here, 01 is a line ay parallel to the y-axis in the direction of movement of arrow 32
, and θb is also the same as the angle ab)'b parallel to the y-axis in the traveling direction of the arrow 63.

Note that the angle θ1. θb is IIIA a3'a ! parallel to the y-axis passing through each self-position a+b. Based on the positive direction of by b, counterclockwise rotation is treated as a positive angle, and clockwise rotation is treated as a negative angle.

To calculate the self-position of the self-propelled robot, as shown in FIG. 2, first, pulse number data nL+nH for increasing the wheel rotation speed is input from the left and right wheel encoders 3.7. From the input pulse number data, the wheel rotation speed and travel distance Δ1 are calculated using equation (1).

Here, Dh and DR are the left and right wheel diameters, and DR is the number of pulses per wheel rotation of the chain encoder.

Next, the attitude angle θ of the self-propelled robot is determined from the gyro 16.

Input angular velocity data ωi ('/s) of θb. From this angular velocity data, the attitude angles θ at points a and b shown in FIG. 4, and the angular changes Δθ&b of θb are calculated using equation (2).

Δθab=ωt·Δt (2) Here, Δt is the cycle of the above-mentioned running feeding restriction.

Next, soft correction of attitude angle, which is a feature of the present invention, will be described. The gyro 16 generates angular velocity data ωi (o/s
) to measure. As shown in FIG. 6, the output from the gyro 16 is a voltage VL corresponding to the angular velocity data ωL.If the resolution of the gyro is Δω, the true angular velocity data in FIG. ωz (o/s), output voltage vL (V), angular velocity data at point d ωa
(0/s) and output voltage Va (V), and an error occurs in the angular velocity data due to the resolution of the gyro. Generally, the resolution of commercially available gyros is 0.04 to 0.1 (
073).

The running error of the self-propelled robot due to this is 40. in Figure 7.
41. When running for 5 m as shown in 42, the resolution is 0.
If the resolution is 0.04 (o/s), an error of 3 to 5 m will occur, and if the resolution is 0.1 (o/s), an error of 6 to 8 crn will occur.

In order to eliminate this running error, in the present invention, 4
The attitude angle shown in 5 is subjected to soft correction with a control period Δ every second. In other words, the soft correction of the attitude angle is to correct the angular velocity take ωL by the gyro in a soft manner every time the boot roller is taken. The method is to use the angle correction amount Δω based on the gyro resolution m(0/S) shown in Figure 6.
Calculate ■ using equation (3), and calculate the angular change amount △θm of the attitude angle.
A correction according to equation (4) is performed by adding the correction amount ΔωH to b, and the resulting Δθab+Δω11 is set as the angular change amount Δθt of the attitude angle of the self-propelled robot.

△ω11=△ω8・△t ・・・・・・・・・(3
) Δθ device = △θ8b ten △ spider ・・・・・・・・・(4)
This software correction is illustrated in FIG. 8.

FIG. 8 shows the attitude angle θ4 of the self-propelled robot by overlapping points a and b in FIG. It is a diagram showing θb and the angle correction amount Δθyo, where θ and θb are the actual posture angles of the self-propelled robot. The amount of change in attitude angle determined from the angular velocity take ωL (a/s) obtained by the gyro is △θab=ω2・△t
However, in the present invention, the angle correction amount △ due to the gyro resolution
By adding θ8 to Δθab, the angular change amount Δθ of the posture angle is calculated.

In addition, the timing of the above soft correction of the attitude angle is 45 in Figure 9.
As shown in 59 to 59, the control cycle Δ for reading the angular velocity take ωt from the gyro, calculating the robot's position coordinates, and instructing the robot to travel is performed every second. .

Returning to Figure 2, next is the amount of angular change in the attitude angle obtained above.
Based on θi, the attitude angle θb at point b in FIG. 4 is calculated using the cω formula.

θb: θa + Δθ machine ・・・・・・・・・(6) Next, the position coordinates (xb, yb) of the self-propelled robot shown in b in Fig. 4
is calculated using equations (6) and (7). However, the coordinates of point a are set as the starting point of the self-propelled robot to the coordinate origin (0,0), and the coordinates of point a are already known.
Find the coordinates of point b one after another.

Also, the initial value of the attitude angle θ is 0° (y@direction in Figure 5)
shall be.

Stripe yb = y, 1△J-cQs (θ company +] 2)...
...(7) Next, input the distance take 1a from the ultrasonic radar to the obstacle shown in 9.12 in Figures 1 and 6,
Furthermore, obstacle direction data θ8 is input from 14 ultra-high frequency radar encoders. Then, the position coordinates of the obstacle are (8
) and (9). The relationship between the position of the obstacle and the robot position is shown in FIG.

X#=')Ca-J-11・5in(θ2+θ, )
,,, ,,, -to+yG=yMai 18・L:IJ
s(θ, 108) (9) For the above obstacle position, the takes of 35, 36, 37, and 38 in FIG. 5 are stored in the memory 20a in FIG. 3.

Next, the travel control unit 20 determines a travel command for how the vehicle should travel per second during the next control period Δ.

In FIG. 2, it is first determined whether there is an obstacle ahead, and if there is no obstacle, the left and right wheel motors 2, 6 and their drive circuits are instructed to run straight ahead.

Conversely, if there is an obstacle, it issues a stop command and a U-turn command.

Next, the attitude angle θb at point b in Figure 4 calculated using equation (6) is 0.
In the case of Figure 4, the speed of the left wheel is controlled to be higher than the speed of the right wheel, and the robot's direction of movement is changed so that it follows the y-axis. conduct.

However, if the traveling direction is the negative direction of the y-axis, the attitude angle θ
Control is performed along the y-axis so that b becomes 180°.

Next, the position coordinates (Xb, yb) and attitude angle θゎ of point b of the self-propelled robot are updated as the values of point a in FIG.

Finally, a determination is made as to whether the cleaning has been completed by determining whether the robot's traveling trajectory 34 has traveled throughout the range surrounded by the scene maps 35 to 68 in which the obstacle positions in FIG. 5 are memorized. If the cleaning is finished, the running of the self-propelled robot is finished, and if the cleaning is not finished, the process returns to the point immediately after the start of step 1.

According to this embodiment, the posture angle θb of the self-propelled robot at point b in FIG. 4 and the position coordinates (Xb, yb) of the robot can be calculated without error, and the straight running and
This has the effect of accurately controlling the running of the machine by repeating turns. An example is shown in FIG.

When the self-propelled robot departs from 61 and moves straight upward along the wall 60 of the room, θb is always calculated to be smaller than ΔθH unless the soft correction of the attitude angle shown in FIG. 8 is performed. Therefore, since the robot is controlled to follow the y-axis based on the attitude angle θb, the robot advances along the trajectory shown by the broken line up to 63.

However, the robot recognizes that it has gone straight to 62. Therefore, the self-propelled robot collides with the wall at position 66.

In this embodiment, since the attitude angle θb at point b is accurately calculated by correcting the angular correction error Δθ■ due to the gyro resolution using software, the vehicle travels straight ahead as shown by the solid line in FIG. There will be no collision.

Furthermore, when the self-propelled robot of this embodiment is used as a self-propelled cleaning robot, the self-propelled robot can be driven close to walls and obstacles in the room due to highly accurate travel control. It also has the effect of reducing the amount of residue left after cleaning.

615゜ [Effects of the Invention] According to the present invention, since the error in the attitude angle of the self-propelled robot caused by the gyro resolution is corrected by a software program each time the angular velocity take of the gyro is read out, the attitude angle of the self-propelled robot is The robot's position coordinates can be calculated without error, and the self-propelled robot can be controlled accurately.

Therefore, it is possible to prevent the vehicle from colliding with walls, obstacles, or people while driving.

[Brief explanation of drawings]

FIG. 1 is a perspective view of a self-propelled cleaning robot according to an embodiment of the present invention, FIG. 2 is a system diagram of the travel control section m of the self-propelled robot according to the present invention, and FIG. Fig. 5 shows a method for calculating the position coordinates and attitude angle of the robot according to the present invention. 8 and 9 are explanatory diagrams showing the method of soft correction of gyro error according to the present invention, and FIG. 10 is an explanatory diagram showing the +16+ travel locus of the present invention. 1... Left wheel, left wheel drive motor for 2, 3... left wheel encoder, 5... right wheel, 6... right wheel drive motor, 7... right wheel encoder, 16... gyro, 20. ...Traveling control unit, 22... Source for control, 2
3... Drive power supply, 30... Gyro measurement circuit, 3
1...Wheel encoder measurement circuit. Kurin Burial Officer Katsuo Ogawa

Claims (1)

[Claims] 1. A travel distance measurement device that measures the travel distance of the robot, a gyro measurement device that measures angular velocity data of a gyro to determine the amount of change in the robot posture angle, a wheel drive device that drives wheels, and the travel distance. It is equipped with a travel control device that calculates the robot's attitude angle and self-position coordinates based on the data from the measuring device and the attitude angle angular change measurement device, determines the traveling method, and instructs the wheel drive device on the traveling method. A travel control method for a self-propelled robot, comprising: correcting an attitude angle error angle due to resolution of the gyro from an attitude angle calculated by a travel control device each time angular velocity data of the gyro is measured in the self-propelled robot.