JP4985420B2 - Floor surface detection system, mobile robot, and floor surface detection method - Google Patents

Description

  The present invention relates to a floor detection system, and more particularly to a floor detection system attached to a mobile robot.

  A mobile robot that performs walking and moving operations may include a floor surface detection system that detects a floor surface state in order to perform stable walking and moving operations even in places where the floor surface state changes (for example, patents). References 1, 2, 3, 4). The mobile robot described in Patent Document 1 detects the floor surface with a sensor provided at the waist, and always stably measures the distance to the floor surface and the floor surface shape. Further, Patent Document 2 discloses a robot that generates a gait signal based on a walking road surface condition and a current posture by a road surface state determination unit and a posture determination unit and performs stable running. Further, Patent Document 3 detects a plane parameter based on a distance image generated by a distance image generation unit, selects a point on the floor using this, and recognizes an obstacle based on this point. Techniques to do this are disclosed. Patent Document 4 discloses a technique for obtaining an estimated value of a posture angle of a predetermined part from a drift correction value of an angular velocity sensor determined according to a deviation between an estimated value of a posture angular velocity and a detected posture angular velocity value by an angular velocity sensor. ing.

  In order to detect a floor surface using a distance image sensor in a mobile robot, it is necessary to acquire information about the height from the floor surface to the distance image sensor and the inclination of the distance image sensor. Here, since the posture angle of the mobile robot changes during movement, the posture angle of the mobile robot is measured by the posture angle sensor in order to obtain the inclination of the distance image sensor. Then, the posture angle data obtained by the measurement by the posture angle sensor is transmitted to a computer that executes the floor surface detection process.

JP 2002-144278 A JP 2006-255798 A JP 2003-269937 A Republished Patent Publication WO2003 / 090981

  However, when the attitude angle data obtained by measurement by the attitude angle sensor is transmitted to a computer that performs floor surface detection processing, a communication delay occurs. Therefore, when the floor surface detection process is performed in the computer, the posture angle data before the current posture angle data may be used, which may cause the floor detection to fail.

  This point will be described in more detail with reference to FIG. FIG. 2 schematically shows the state of the mobile robot 1 on the floor surface 100. The mobile robot 1 includes a wheel 11 at a lower portion of the robot body 10, and further includes a distance image sensor 12 and an attitude angle sensor 13 above the wheel 11. In the state shown in FIG. 2A before t [ms] from the present time, for example, it is assumed that the posture angle sensor 13 detects the tilt angle x [deg]. Further, in the current state shown in FIG. 2B, since the inclination angle of the mobile robot 1 is 0, it is obtained by adding 0 of the inclination angle to the attachment angle θ of the distance image sensor 12. The floor detection process should be executed based on the tilt angle (θ + 0) of the distance image sensor 12 to be obtained. However, due to the occurrence of the communication delay as described above, the inclination angle x detected in the state t [ms] before the current time shown in FIG. 2A is added to the attachment angle θ of the posture angle sensor 13. Since the floor detection process is executed based on the inclination angle (θ + x) of the distance image sensor 12 obtained by this, the floor detection fails. An image 120 in FIG. 2 shows a result of failure in floor detection, and a hatched area is an area where the floor can be detected.

  The present invention has been made to solve such a problem, and an object thereof is to provide a floor surface detection system, a mobile robot, and a floor surface detection method capable of accurately detecting a floor.

  A floor surface detection system according to the present invention is a floor surface detection system that is provided in a mobile robot and detects a floor surface on which the mobile robot moves, and generates distance image data corresponding to the distance to the floor surface. Based on the distance image sensor, the posture angle sensor that generates posture angle data of the mobile robot, the distance image data generated by the distance image sensor, and the posture angle data generated by the posture angle sensor, the floor surface A data processing unit for obtaining an angle of the distance image sensor with respect to the sensor angle candidate setting means for setting a plurality of sensor angle candidates based on posture angle data detected by the posture angle sensor; Floor detection means for performing floor detection based on the distance image data for each of the plurality of sensor angle candidates, and the plurality of sensor angle candidates Among them, the most floor detected candidate, and has a candidate selecting means for selecting as the angle of the distance image sensor with respect to the floor surface.

  Here, the sensor angle candidate setting means includes a sensor reference angle setting means for setting a sensor reference angle based on posture angle data detected by the posture angle sensor, and a sensor reference angle set by the sensor reference angle setting means. Further, the sensor angle candidate calculating means for calculating a plurality of sensor angle candidates by subtracting and adding a predetermined value.

  The floor detection means includes an ideal distance calculation means for calculating an ideal distance to the floor for sensor angle candidates, an ideal distance calculated by the ideal distance calculation means, and a distance image generated by the distance image sensor. It is desirable to have floor surface determination means that obtains a difference in distance to the floor surface in the data and determines that the floor surface has been detected when the difference is equal to or less than a threshold value.

  A mobile robot according to the present invention is a mobile robot that detects a floor surface on which the mobile robot moves, a distance image sensor that generates distance image data according to a distance to the floor surface, and attitude angle data of the mobile robot A data processing unit for obtaining an angle of the distance image sensor with respect to the floor surface based on the attitude angle sensor that generates the image, the distance image data generated by the distance image sensor, and the attitude angle data generated by the attitude angle sensor The data processing unit includes sensor angle candidate setting means for setting a plurality of sensor angle candidates based on posture angle data detected by the posture angle sensor, and the distance image for each of the plurality of sensor angle candidates. Floor detection means for performing floor detection based on the data, and the candidate with the most floor detection among the plurality of sensor angle candidates is the floor surface Those having a candidate selecting means for selecting as the angle of the range image sensor against.

  Here, the sensor angle candidate setting means includes a sensor reference angle setting means for setting a sensor reference angle based on posture angle data detected by the posture angle sensor, and a sensor reference angle set by the sensor reference angle setting means. Further, the sensor angle candidate calculating means for calculating a plurality of sensor angle candidates by subtracting and adding a predetermined value.

  The floor detection means includes an ideal distance calculation means for calculating an ideal distance to the floor for sensor angle candidates, an ideal distance calculated by the ideal distance calculation means, and a distance image generated by the distance image sensor. It is desirable to have floor surface determination means that obtains a difference in distance to the floor surface in the data and determines that the floor surface has been detected when the difference is equal to or less than a threshold value.

  Furthermore, it is preferable to generate gait data based on the distance image data corresponding to the sensor angle candidate selected by the candidate selecting means.

  Here, the mobile robot may be a coaxial two-wheel mobile robot or a biped walking mobile robot.

  A floor surface detection method according to the present invention is a floor surface detection method for detecting a floor surface on which a mobile robot moves, and sets sensor angle candidates for a plurality of distance image sensors based on posture angle data; Performing floor detection based on distance image data generated by the distance image sensor for each of a plurality of sensor angle candidates, and selecting a candidate with the most floor detection among the plurality of sensor angle candidates as the floor surface Selecting as an angle of the distance image sensor with respect to.

  ADVANTAGE OF THE INVENTION According to this invention, the floor detection system and mobile robot which can perform a floor detection correctly can be provided.

  First, a schematic configuration of a floor surface detection system according to an embodiment of the present invention will be described with reference to FIG. The floor surface detection system is provided in the mobile robot 1 whose posture changes with movement. The mobile robot 1 shown in this example is an autonomously moving coaxial two-wheeled robot, but is not limited to this, and can be applied to a biped walking robot, a quadruped walking robot, and the like. Furthermore, the present invention can be applied to a human boarding robot. The mobile robot 1 includes a robot body 10 that includes wheels 11 that are rotationally driven by a drive unit (not shown). The mobile robot 1 tilts in the front-rear direction around the rotation axis of the wheel 11.

  The robot body 10 of the mobile robot 1 is provided with a distance image sensor 12. The distance image sensor 12 is, for example, a TOF (Time Of Flight) type sensor. Here, the TOF method includes a phase TOF method and a pulse TOF method. In the phase TOF method, an intensity-modulated measurement light beam is irradiated onto a measurement object, reflected light from the object is detected, photoelectron conversion is performed, and the converted photoelectron is temporally transmitted to one of a plurality of storage means. It is trying to accumulate by shifting. According to this method, the farther the measurement target is, the more photoelectrons are stored in the storage means that is stored at a later timing in time among the plurality of storage means. That is, the relative ratio or difference of the number of photoelectrons stored in each of the plurality of storage units is determined according to the distance to the measurement target. The distance to the measurement object can be obtained by obtaining this ratio or difference. The pulse TOF method is a method for irradiating a measurement object with a pulsed measurement light beam and obtaining a distance from the phase difference between the reflected light from the measurement object and the measurement light beam, and scanning the measurement light beam two-dimensionally. Then, the distance is measured at each point to measure the three-dimensional shape.

  The distance image sensor 12 is inclined with respect to the robot body 10 so as to detect the floor surface in front of the mobile robot 1. In this example, in the state where the robot body 10 is positioned perpendicular to the floor surface, an angle with respect to the vertical direction (that is, the vertical direction) is represented as an attachment angle θ (deg).

  The robot body 10 of the mobile robot 1 is provided with a posture angle sensor 13. The attitude angle sensor 13 is a sensor that measures the tilt angle of the robot body 10, that is, the mobile robot 1. For example, the attitude angle sensor 13 is composed of a gyro sensor that detects the angular velocity of the robot body 10 and an acceleration sensor that detects the vertical direction. The

  As shown in the block diagram of FIG. 3, the distance image data measured and generated by the distance image sensor 12 and the posture angle data measured and generated by the posture angle sensor 13 are output to the data processing unit 14. The

  The data processing unit 14 is based on the control program and control parameters stored in the ROM and RAM, as well as the distance image data input from the distance image sensor 12 and the posture angle data input from the posture angle sensor 13. A CPU that performs control for realizing the overall operation and processing is provided. The data processing unit 14 according to this example executes floor surface detection processing based on the posture angle data detected by the posture angle sensor 13 in addition to the distance image data input from the distance image sensor 12.

  Next, floor surface detection processing in the floor surface detection system according to the present embodiment will be described with reference to the flowchart shown in FIG.

  First, the data processing unit 14 inputs posture angle data from the posture angle sensor 13. The posture angle data to be processed is data before t [ms] due to a communication delay between the posture angle sensor 13 and the data processing unit 14 as described in detail in the section of the problem to be solved by the invention. . The data processing unit 14 sets a sensor reference angle based on the input attitude angle data (S101). The sensor reference angle is an angle obtained by adding the attachment angle θ of the distance image sensor 12 to the posture angle x [deg] specified by the posture angle data. For example, when x = 5 and θ = 20, the sensor reference angle is x + θ = 25 [deg].

Next, the data processing unit 14 sets sensor angle candidates based on the sensor reference angle (S102). The sensor angle candidate is an angle obtained by subtracting and adding a plurality of predetermined values based on the sensor reference angle. Preferably, the sensor angle candidates are values obtained by subtracting a ₁ , a ₂ , a ₃ ... A _{n from} the sensor reference angle s, a ₁ , a with respect to the sensor reference angle s and the sensor reference angle s. ₂ , a ₃ ... A _n respectively. _{That, s-a n, ··· s} -a 3, s-a 2, s-a 1, s, s + a 1, s + a 2, s + a 3 ···· s + a 2n + 1 or a sensor angle of _n [deg] Be a candidate. More specifically, they are sm,... S-3, s-2, s-1, s, s + 1, s + 2, s + 3,... S + m [deg]. In this example, a value obtained by subtracting and adding 1 [deg] is used. However, the value is not limited to this, and an arbitrary value may be used, and the value may be changed even if it is not fixed. For example, 1 [deg] may be subtracted and added in a range close to the sensor reference angle value, and 2 [deg] may be subtracted and added in a far range.

The data processing unit 14 calculates an ideal distance to the floor surface for an arbitrary angle candidate A among the set sensor angle candidates (S103). The initial value, for example, most value is less sensor angle candidate s-a _n [deg] is selected. The ideal distance to the floor is obtained by geometrically calculating the distance from the distance image sensor 12 to the floor based on the selected arbitrary angle candidate A. The ideal distance to the floor is calculated over the entire range of the area acquired by the distance image sensor 12, that is, for all pixels.

Further, the data processing unit 14 inputs the distance image data from the distance image sensor 12, and calculates the difference between the distance to the floor surface of the pixel i included in the distance image data and the ideal distance at the same position as the pixel i. (S104).
The data processing unit 14 determines whether or not the difference value calculated in step S104 is equal to or less than a predetermined threshold (S105). If the data processing unit 14 determines that the difference value is greater than the threshold value, the data processing unit 14 proceeds to step S107.

  On the other hand, when it is determined that the difference value is equal to or less than the threshold value, the data processing unit 14 determines that the floor surface has been detected, and increases the value of the floor detection count number by 1 (S106).

  When it is determined in step S105 that the difference value is larger than the threshold value and when the process of increasing the value of the floor detection count number by 1 is performed in step S106, the data processing unit 14 includes all the distance image data. The pixel is scanned to determine whether the calculation has been performed (S107). If the data processing unit 14 determines that calculation has not been performed for all pixels, i = i + 1 is set, and the process of step S104 is performed for the next pixel.

  On the other hand, if the data processing unit 14 scans all the pixels and determines that the calculation has been performed, it is further determined whether all the sensor angle candidates have been calculated (S108). If the data processing unit 14 determines that all sensor angle candidates have not yet been calculated, the data processing unit 14 sets A = A + 1 and executes the process of step S103 for the next sensor angle candidate.

  On the other hand, if the data processing unit 14 determines that calculation has been performed for all sensor angle candidates, a sensor angle candidate having a large number of floor detection counts is selected, and the distance image data corresponding to the sensor angle candidate is displayed on the floor surface. The shape data is adopted (S109). Then, the data processing unit 14 generates gait data based on the adopted distance image data. The mobile robot 1 executes a running or walking operation based on the gait data.

  Next, an example of floor surface detection processing will be described with reference to FIG. In this example, the attitude angle x detected by the attitude angle sensor 13 is 5 [deg], and the attachment angle θ of the distance image sensor 12 is 15 [deg]. Therefore, the sensor reference angle s = 5 + 15 = 20 [deg]. A value 15 to 25 [deg] obtained by adding or subtracting 1 to 5 [deg] from 20 [deg], which is the sensor reference angle s, was used as a sensor angle candidate. The hatched portions in the upper left and upper right areas in FIG. 5 indicate areas corresponding to pixels in which the difference value is determined to be equal to or smaller than the threshold value in step S105 in FIG. 4 and the floor surface is determined to be detected. Yes.

  As shown in FIG. 5, when the sensor angle candidate is 20 °, the area where the floor surface is detected is wider and the floor detection count is larger than when the sensor angle candidate is 25 °. And among all the sensor angle candidates, since the case of 20 ° has the largest number of floor detection counts, it is determined as the current sensor inclination angle of 20 °.

  In the flowchart shown in FIG. 4, the difference between the distance to the floor in pixel i included in the distance image data in step S104 and the ideal distance at the same position as pixel i is calculated and obtained, and the difference is obtained in step S105. The value is compared with the threshold value. However, the present invention is not limited to this, and the distance to the floor surface of the pixel i included in the distance image data is compared with the ideal distance at the same position as the pixel i. Based on the comparison result Thus, a value substantially corresponding to the difference between the two may be obtained, and this value may be compared with a threshold value.

1 is a schematic configuration diagram of a floor surface detection system according to the present invention. It is explanatory drawing for demonstrating the process of this invention and the conventional floor surface detection system. It is a block diagram which shows the structure of the floor surface detection system concerning this invention. It is a flowchart which shows the flow of the process in the floor surface detection system concerning this invention. It is explanatory drawing which shows the example of a process result of the floor detection system concerning this invention.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Robot 10 Robot main body 11 Wheel 12 Distance image sensor 13 Attitude angle sensor 14 Data processing part 100 Floor surface 120 Processing result

Claims (10)

A floor surface detection system that is provided in a mobile robot and detects a floor surface on which the mobile robot moves.
A distance image sensor that generates distance image data according to the distance to the floor;
A posture angle sensor for generating posture angle data of the mobile robot;
A data processing unit for obtaining an angle of the distance image sensor with respect to a floor surface based on the distance image data generated by the distance image sensor and the posture angle data generated by the posture angle sensor;
The data processing unit
Sensor angle candidate setting means for setting a plurality of sensor angle candidates based on posture angle data detected by the posture angle sensor;
Floor detection means for performing floor detection based on the distance image data for each of the plurality of sensor angle candidates;
A floor surface detection system comprising candidate selection means for selecting, as the angle of the distance image sensor with respect to the floor surface, a candidate having the largest floor detection among the plurality of sensor angle candidates. The sensor angle candidate setting means includes:
Sensor reference angle setting means for setting a sensor reference angle based on posture angle data detected by the posture angle sensor;
2. The floor according to claim 1, further comprising sensor angle candidate calculation means for calculating a plurality of sensor angle candidates by subtracting and adding a predetermined value from the sensor reference angle set by the sensor reference angle setting means. Detection system. The floor detection means includes
Ideal distance calculation means for calculating the ideal distance to the floor for sensor angle candidates;
The difference between the ideal distance calculated by the ideal distance calculation means and the distance to the floor in the distance image data generated by the distance image sensor is obtained, and the floor is detected when the difference is equal to or less than the threshold value The floor surface detection system according to claim 1, further comprising a floor surface determination unit that determines that A mobile robot that detects the floor surface on which it moves,
A distance image sensor that generates distance image data according to the distance to the floor;
A posture angle sensor for generating posture angle data of the mobile robot;
A data processing unit for obtaining an angle of the distance image sensor with respect to a floor surface based on the distance image data generated by the distance image sensor and the posture angle data generated by the posture angle sensor;
The data processing unit
Sensor angle candidate setting means for setting a plurality of sensor angle candidates based on posture angle data detected by the posture angle sensor;
Floor detection means for performing floor detection based on the distance image data for each of the plurality of sensor angle candidates;
A mobile robot comprising candidate selection means for selecting, as the angle of the distance image sensor with respect to the floor surface, a candidate having the most floor detection among the plurality of sensor angle candidates. The sensor angle candidate setting means includes:
Sensor reference angle setting means for setting a sensor reference angle based on posture angle data detected by the posture angle sensor;
5. The mobile robot according to claim 4, further comprising sensor angle candidate calculation means for calculating a plurality of sensor angle candidates by subtracting and adding a predetermined value from the sensor reference angle set by the sensor reference angle setting means. . The floor detection means includes
Ideal distance calculation means for calculating the ideal distance to the floor for sensor angle candidates;
The difference between the ideal distance calculated by the ideal distance calculation means and the distance to the floor in the distance image data generated by the distance image sensor is obtained, and the floor is detected when the difference is equal to or less than the threshold value The mobile robot according to claim 4, further comprising a floor surface determination unit that determines that   7. The mobile robot according to claim 4, wherein gait data is generated based on distance image data corresponding to a sensor angle candidate selected by the candidate selection unit.   The mobile robot according to claim 4, wherein the mobile robot is a coaxial two-wheeled mobile robot.   The mobile robot according to claim 4, wherein the mobile robot is a biped mobile robot. A floor surface detection method for detecting a floor surface on which a mobile robot moves,
The distance to the floor surface based on distance image data corresponding to the distance from the distance image sensor to the floor surface generated by the distance image sensor and posture angle data of the mobile robot generated by the posture angle sensor. A data processing step for determining the angle of the image sensor;
The data processing step includes
A sensor angle candidate setting step for setting a plurality of sensor angle candidates based on posture angle data detected by the posture angle sensor;
A floor detection step of performing floor detection based on the distance image data for each of the plurality of sensor angle candidates;
A floor surface detection method comprising: a candidate selection step of selecting, from among the plurality of sensor angle candidates, a candidate having the most floor detection as an angle of the distance image sensor with respect to the floor surface.

Images