JP4844459B2 - Plane detection method and mobile robot - Google Patents

Description

  The present invention relates to a plane detection method and a mobile robot.

The autonomous mobile robot recognizes the environment of the movement route and moves to the destination. For this reason, it is necessary for the mobile robot to accurately and quickly detect a plane existing in the environment. In particular, when the mobile robot is a walking robot, the processing time required for plane detection is a restriction on high-speed movement, so it is important to detect the plane accurately in a shorter time.
Conventionally, many methods for detecting a plane from the environment have been proposed. As a method for detecting three parameters (plane parameters) representing a plane equation from a large number of measurement points (three-dimensional position data group), the Hough transform method and the random sampling method are well known. As a technique using the Hough transform, for example, in Patent Document 1, a plane parameter is calculated by sampling three or more points of data from a three-dimensional position data group, and directly voting the plane parameter to a voting space. Techniques for detection are disclosed.
In the two-point random sampling method, a two-point data set is randomly sampled from the three-dimensional position data group, and a plane normal vector is calculated from a vector consisting of the two-point data set. The plane is detected by searching the distance to the plane along the vector.
JP 2003-271975 A

However, although the technique described in Patent Document 1 can stably extract a plane even from a noisy 3D position data group, the 3D position data is randomly sampled from the 3D position data group. Therefore, there is a problem that a great amount of processing time is required for plane detection.
On the other hand, the two-point random sampling method has an advantage that a plane can be extracted accurately and stably as compared with the method using the Hough transform. For example, when there are a plurality of planes such as stairs in the environment, more planes can be detected stably and accurately than the method using the Hough transform.
However, in the two-point random sampling method, as in the technique described in Patent Document 1, three-dimensional position data is randomly sampled from the three-dimensional position data group, so that a large amount of processing time is required for plane detection. There is a point.
The present invention has been made to solve such a problem, and an object of the present invention is to provide a plane detection method and a mobile robot that can reduce the processing time required for plane detection.

  The plane detection method according to the present invention is a plane detection method for detecting a plane, wherein an arbitrary template is selected from a plurality of templates having different sampling target areas according to the movement state of the mobile robot, Sampling a set of two-dimensional three-dimensional data from a three-dimensional data group in a sampling target region defined by the template, and detecting a plane based on the two-point three-dimensional data set. .

  Thereby, it is not necessary to sample a region that is not necessary for plane detection according to the moving state of the mobile robot. For this reason, the processing time required for plane detection can be shortened.

  Further, the template may be selected based on plane information in the past movement state of the mobile robot.

  Thereby, it is possible to specify a sampling area where the mobile robot moves and the plane detection process is to be executed intensively at the next timing from the past plane information. For this reason, the template containing the specified sampling object area | region can be selected autonomously and easily.

  Furthermore, the template may be selected based on the number of sets of the sampled two-dimensional three-dimensional data.

  Thereby, the degree of suitability of the selected template with respect to the measurement environment can be determined based on the number of sets of three-dimensional data of two points sampled on the template. That is, a more appropriate template can be selected according to the environment.

  In addition, the template may be one in which a sampling target area is determined in advance and mounted on the mobile robot.

  As a result, the template can be quickly selected by the mobile robot holding the template in which the sampling area is determined in advance according to the environment.

  Furthermore, the set of three-dimensional data of the two points may be randomly sampled in advance in the sampling target region defined by the template.

  Thereby, it is not necessary to repeat the process of randomly sampling a two-point three-dimensional data set, and a two-point three-dimensional data set can be extracted at a higher speed.

  A mobile robot according to the present invention is a mobile robot that moves within a predetermined environment, and a template selection unit that selects an arbitrary template from a plurality of templates having different sampling target areas according to the movement state of the mobile robot; A plane detection unit that samples two sets of three-dimensional data from a three-dimensional data group in a sampling target region defined by the selected template and detects a plane based on the two-point three-dimensional data sets Are provided.

  Thereby, it is not necessary to sample a region that is not necessary for plane detection according to the moving state of the mobile robot. For this reason, the processing time required for plane detection can be shortened.

  The template selection unit may select the template based on plane information in the past movement state of the mobile robot.

  Thereby, it is possible to specify a sampling area where the mobile robot moves and the plane detection process is to be executed intensively at the next timing from the past plane information. For this reason, the template containing the specified sampling object area | region can be selected autonomously and easily.

  Furthermore, the template selection unit may select the template based on the number of sets of the sampled two-point three-dimensional data.

  Thereby, the degree of suitability of the selected template with respect to the measurement environment can be determined based on the number of sets of three-dimensional data of two points sampled on the template. That is, a more appropriate template can be selected according to the environment.

  In addition, the template may be one in which a sampling target area is determined in advance and mounted on the mobile robot.

  As a result, the template can be quickly selected by the mobile robot holding the template in which the sampling area is determined in advance according to the environment.

  Furthermore, the set of three-dimensional data of the two points may be randomly sampled in advance in the sampling target region defined by the template.

  Thereby, it is not necessary to repeat the process of randomly sampling a two-point three-dimensional data set, and a two-point three-dimensional data set can be extracted at a higher speed.

  According to the present invention, it is an object to provide a plane detection method and a mobile robot that can shorten the processing time required for plane detection.

  Hereinafter, specific embodiments to which the present invention is applied will be described in detail with reference to the drawings. In addition, in each drawing, the same code | symbol is attached | subjected to the same element, and duplication description is abbreviate | omitted as needed for clarification of description.

Embodiment 1 of the Invention
The mobile robot according to the present embodiment is a legged mobile robot having two leg links. Further, the mobile robot has a flat surface detection device 1. The flat surface detection apparatus 1 selects an arbitrary template from a plurality of templates having different sampling target areas according to the moving state of the mobile robot, and in the sampling target area defined by the selected template, 2 from the three-dimensional data group. This is a device for sampling a set of three-dimensional data of points and detecting a plane based on the set of three-dimensional data of two points.
The mobile robot determines environmental map data including three-dimensional information of the walking surface on which the mobile robot walks based on the plane detected by the plane detection device 1, and is provided at the tip of each of the two leg links of the mobile robot. Determine the landing position of the foot.
Further, the mobile robot generates motion data for realizing the determined foot landing position, and walks by driving a joint group of the mobile robot by an actuator based on the generated motion data.

The configuration of the mobile robot 100 according to the present invention will be described with reference to FIG. FIG. 1 is a schematic view schematically showing a state in which the mobile robot 100 is viewed from the side (leftward when facing the direction in which the mobile robot 100 moves). A passenger P is on the mobile robot 100. It represents the situation. As shown in FIG. 1, a laser range finder 10 as a visual sensor is attached to a mobile robot 100.
In FIG. 1, for convenience of explanation, the direction in which the moving body 1 travels (front-rear direction) is the x-axis, and the direction in which the moving body 1 travels is orthogonal to the horizontal direction (left-right direction), the y-axis. The direction (vertical direction) extending in the vertical direction from the plane on which the lens moves is defined as the z-axis, and will be described using a coordinate system composed of these three axes. That is, in FIG. 1, the x-axis indicates the left-right direction toward the paper surface, the y-axis indicates the depth method of the paper surface, and the z-axis indicates the vertical direction in the paper surface.

As shown in FIG. 1, the mobile robot 100 rotates via a body part 101 including a mounting table (not shown) on which the passenger P is boarded, and a waist part (not shown) that supports the body part 101. It is comprised by the leg part 104 fixed movably, and the laser range finder 10 attached to the waist | hip | lumbar part.
The leg 104 includes a right leg 102 and a left leg 103. The right leg 102 includes a right knee joint 102c, a right ankle joint 102e, and a right foot 102f. Similarly, the left leg 103 includes a left knee joint 103c, A left ankle joint 103e and a left foot 103f are provided.
A driving force from a motor (not shown) is transmitted through a pulley and a belt (not shown), so that each joint is driven to a desired angle. As a result, the right leg 102 and the left leg 103 are caused to perform a desired movement. be able to.

The upper body 101 of the mobile robot 100 is provided with a laser range finder 10 that detects distance image data of an environment in which the mobile robot 100 walks through a waist (not shown). More specifically, for example, in FIG. 1, the laser range finder 10 is tilted from the horizontal direction toward the feet of the mobile robot 100 and infrared rays or the like are directed toward the front including the vicinity of the feet of the mobile robot 100. Laser light is irradiated and the reflected light is received.
In this way, high resolution can be secured for the distance image data near the feet of the mobile robot 100, and the distance image data near the feet with high density can be acquired.

  The laser range finder 10 acquires distance image data of the external environment including an obstacle on the floor surface. The distance image data is two-dimensional data having a distance value to a measurement target existing in the environment as a pixel value of each pixel. As described later, by performing coordinate conversion of the distance image data, it is possible to generate external three-dimensional point cloud data.

Further, the laser range finder 10 is provided on the waist of the mobile robot 100 and is configured to be swingable about the vertical axis. As a result, the front of the mobile robot 100 in the moving direction can be scanned.
In this way, in the top view, the scanning range by the laser beam can be configured to have a fan-shaped shape that spreads to the left and right toward the front in the moving direction of the mobile robot 100, and only the area necessary for movement is efficiently used. Can be scanned automatically.

Here, a method in which the laser range finder 10 generates three-dimensional point cloud data from two-dimensional distance image data will be described.
First, when the laser range finder 10 is fixed without swinging left and right, an obstacle on a plane perpendicular to the floor surface is irradiated by irradiating laser light vertically to the front in the movement direction perpendicular to the floor surface. Get the distance image data. Then, the range image data on a plurality of different planes is acquired by scanning the laser range finder 10 while swinging left and right about the vertical axis of the mobile robot 100 toward the front in the moving direction. Next, the plurality of distance image data are coordinate-converted and aligned based on the angle of the joint that supports the laser range finder 10 so as to be swingable. Thereby, the range image data obtained on a plurality of planes can be integrated. Alternatively, the distance image data may be aligned with each other by obtaining corresponding points from a plurality of distance image data.
In this way, the three-dimensional distance image data included in the front area can be easily acquired with a simple configuration.

  Subsequently, a control system of the mobile robot 100 according to the present embodiment will be described below. The configuration of the control system of the mobile robot 100 is shown in FIG. As illustrated in FIG. 2, the flat surface detection apparatus 1 includes a template selection unit 12 and a flat surface detection unit 13.

In FIG. 2, the laser range finder 10 acquires distance image data of the external environment of the mobile robot 100 as described above. More specifically, for example, as shown in FIG. 3, when the mobile robot 100 moves in the moving direction (here, the x-axis direction), the laser range finder 10 emits laser light forward in the moving direction. Thus, distance image data ahead in the movement direction is detected.
As a visual sensor, a stereo camera may be used instead of the laser range finder. That is, a distance image is obtained by using a plurality of cameras including an image sensor such as a CCD (Charge Coupled Device) image sensor or a CMOS (Complementary Metal Oxide Semiconductor) image sensor, and using parallax of images captured by the plurality of cameras. Data may be generated. More specifically, corresponding points are detected from image data captured by a plurality of cameras, and the three-dimensional positions of the corresponding points are restored by stereo vision. Here, the search for corresponding points in a plurality of captured images may be performed by applying a known method such as a gradient method or a correlation method using a time-space differential constraint formula for the plurality of captured images.

The three-dimensional position data generation unit 11 performs coordinate conversion of the distance image data to generate a three-dimensional position data group as a three-dimensional data group. The three-dimensional position data group is a set of data in which position vectors of a large number of measurement points included in the distance image data are represented in a three-dimensional orthogonal coordinate system.
For example, the image in FIG. 4 shows an example of a three-dimensional position data group generated by the mobile robot 100. FIG. 4 is a plot of a three-dimensional position data group on an xy plane parallel to the floor surface, and height information (position information in the z-axis direction) exists for each plotted point.
Here, the image of FIG. 4 is called a frame. A current frame to be processed by the mobile robot 100 is called a current frame, and a frame generated before the current frame is called a previous frame. As the mobile robot 100 moves, the three-dimensional position data group changes between the previous frame and the current frame.

Further, the three-dimensional position data generation unit 11 corrects distortion of the generated three-dimensional position data using odometry. More specifically, the detection error generated by the movement of the mobile robot 100 during the scan by the laser range sensor 10 is corrected. That is, since the mobile robot 100 moves during the scan by the laser range sensor 10, the detected floor surface shape data includes a detection error corresponding to the movement amount of the mobile robot 100. The amount of movement of the mobile robot 100 is calculated by odometry based on the output of an encoder or the like provided on each joint axis, and the detection error due to the movement of the mobile robot 100 is corrected.
Furthermore, the three-dimensional position data generation unit 11 corrects the distortion in the pitch direction of the three-dimensional position data group based on an output from a gyro (not shown) provided in the upper body 101 of the mobile robot 100. . That is, the rotation component in the pitch direction is detected from the output of a gyro (not shown) provided on the waist of the mobile robot 100, and the detection error in the pitch direction of the three-dimensional position data is corrected.
Thereby, it is possible to correct a detection error in the pitch direction that has a large influence on the three-dimensional position data in the walking robot.

  The template selection unit 12 selects an arbitrary template from a plurality of templates having different sampling target areas according to the moving state of the mobile robot 100. Details of the template selection method by the template selection unit 12 will be described later.

  The plane detection unit 13 samples a set of two three-dimensional position data from the three-dimensional position data group in the sampling target region defined by the template selected by the template selection unit 12, and obtains two points of three-dimensional position data. A plane is detected based on the set of The details of the plane detection method by the plane detection unit 13 will be described later.

  The environment map generation unit 14 generates an environment map as a set of a plurality of planes based on the plane detected by the plane detection unit 13. More specifically, the environment map of the present embodiment is generated as grid data obtained by dividing a two-dimensional plane (referred to as an xy plane) into a grid. As data corresponding to each grid of the environmental map, the height in the z-axis direction perpendicular to the xy plane of each grid, the plane ID that can uniquely identify the plane to which each grid belongs, and the normal of the plane to which each grid belongs Vector is retained. The environment map generation unit 14 recognizes a set of adjacent grids having substantially the same height as one plane, and assigns a unique plane ID to the recognized plane.

For example, when the mobile robot 100 walks in the environment 160 illustrated in FIG. 5A, the plane detection device 1 detects the plane P11 corresponding to the floor surface and the plane P12 corresponding to the upper surface of the obstacle 161. Then, the environment map generator 14 recognizes the plane P11 as a movable area and recognizes the plane P12 as an obstacle area. At this time, the environment map 200 generated by the environment map generation unit 14 is as shown in FIG.
The environment map 200 in FIG. 5B includes, as data of each grid, a plane ID that can uniquely identify each plane, the height of each grid in the z-axis direction, and a normal vector (na, nb, nc). ) Is held.
In order to detect road surface irregularities at the landing positions of the right foot 102f and the left foot 103f based on the difference in the plane ID between adjacent grids and the difference in the height in the z-axis direction, the area of each grid constituting the environment map 200 is detected. May be selected to be sufficiently smaller than the area of the bottom surface of the right foot 103f and the left foot 103f.

  Furthermore, the environment map 200 in FIG. 5B holds information indicating the area type as data of each grid, that is, identification information indicating whether the area is a movable area or an obstacle area. In the example of FIG. 5B, a region R11 corresponding to the floor surface P11 is a movable region, and a region R12 corresponding to the obstacle 161 (shaded area in FIG. 5B) is an obstacle region.

  The walking plan unit 15 determines the target position with reference to the environment map generated by the environment map generation unit 14, and calculates the landing positions of the right foot 102f and the left foot 103f for reaching the determined target position.

  The motion generation unit 16 generates motion data for realizing the landing positions of the right foot 102f and the left foot 103f generated by the walking plan unit 16. Here, the motion data includes the ZMP position, the gravity center position, the position and posture of the right foot 102f and the left foot 103f, and the position and posture of the upper body 101 of the mobile robot 100.

  The control unit 17 receives the motion data generated by the motion generation unit 16 and calculates a target joint angle of each joint by inverse kinematic calculation. Further, the control unit 17 calculates a torque control value for driving each joint based on the calculated target joint angle and the current joint angle measured by the encoder 19. The mobile robot 100 is walked by operating the actuator 18 for driving each joint according to the torque control value calculated by the control unit 17.

  Next, details of the plane detection method by the plane detection apparatus 1 will be described below with reference to FIGS. FIG. 6 is a flowchart showing a plane detection procedure by the plane detection apparatus 1. 7 and 8 are diagrams for explaining template creation and an example of a template. FIG. 9 is a diagram for explaining how the template selection unit 12 selects a template according to the movement state of the mobile robot 100.

  First, in step S <b> 101, the template selection unit 12 acquires plane detection information of the previous frame as plane information in the past movement state of the mobile robot 100. More specifically, for example, plane information such as a floor surface, a step, a slope, and a staircase included in the plane detection result of the previous frame is acquired.

  In step S102, the template selection unit 12 selects a template based on the plane information of the previous frame. From the plane information of the previous frame, in the direction in which the mobile robot 100 moves, it is possible to specify a region where a plane is desired to be detected intensively at the next timing when the plane detection process is executed. For this reason, an appropriate template can be selected autonomously and easily based on the plane information of the previous frame.

Here, the template will be described with reference to FIGS.
FIG. 7 is a diagram for explaining a sampling target area defined in the template. FIG. 8 is a diagram for explaining a plurality of templates having different sampling target areas. In the present embodiment, a plurality of templates having different sampling target areas are generated in advance and mounted on the mobile robot 100. Thereby, a template can be selected promptly according to the environment.

  The plane detection apparatus 1 samples a set of two-dimensional three-dimensional position data from a three-dimensional position data group in a sampling target region defined by a template, as will be described later. For example, when the template T1 shown in FIG. 7A is used, sampling is performed in the sampling target region S1 (the hatched region in FIG. 7A).

Each region on the template T1 is associated with each three-dimensional position data of the three-dimensional position data group. By designating i and j for Data (i, j) indicating an area on the template T1, three-dimensional position data (x, y, z) corresponding to Data (i, j) can be obtained.
Further, in the sampling target region S1, a set of two-dimensional three-dimensional position data is sampled and set in advance at random. More specifically, with respect to Data (i ₁ , j ₁ ) and Data (i ₂ , j ₂ ) respectively corresponding to a set of two-dimensional three-dimensional position data, (i ₁ , j ₁ ) and (i ₂ , j ₂ ) is randomly set in the sampling target area S1.

Thus, for the template, first, the range of the sampling target region S1 is determined by determining the ranges of i and j. Furthermore, a plurality of (i ₁ , j ₁ ) and (i ₂ , j ₂ ) sets are randomly sampled and set in advance in the sampling target region S1. Therefore, a vector composed of a set of three-dimensional position data corresponding to Data (i ₁ , j ₁ ) and Data (i ₂ , j ₂ ) set in advance on the template is sampled (see, for example, FIG. 7B). ).
Thereby, it is not necessary to repeat the process of randomly sampling the set of two-dimensional three-dimensional position data, and the three-dimensional corresponding to the preset Data (i ₁ , j ₁ ) and Data (i ₂ , j ₂ ). By using a set of position data, a set of two-dimensional three-dimensional position data can be extracted at higher speed.

In accordance with the environment in which the mobile robot 100 moves, a plurality of templates are created in advance with sampling target areas as areas where the plane detection sensitivity is to be increased. In other words, a plurality of templates having different areas for performing vector sampling in a concentrated manner are created in advance.
For example, the template T0 shown in FIG. 8A is an example of a template in which the area near the foot of the mobile robot 100 is the sampling target area S0 (the hatched area in FIG. 8A).
The template T1 shown in FIG. 8B is a sampling target region S1 (in the vicinity of the center when there is a plane to be detected such as a step or a slope in the vicinity of the center of the frame in front of the moving direction of the mobile robot 100. It is an example of the template made into the hatched area | region of FIG.8 (b).
The template T2 shown in FIG. 8C is a sampling target region S2 (FIG. 8) in which the region far in the traveling direction is located in front of the mobile robot 100 in the traveling direction when there is no plane to be detected such as a step or a slope in the frame. It is an example of the template made into (hatched area | region of (c)).
A template T3 illustrated in FIG. 8D is an example of a template in which the area near the feet of the mobile robot 100 is the sampling target area S3 (the hatched area in FIG. 8D) when the mobile robot 100 performs a rotation operation. is there.
A template T4 shown in FIG. 8E shows a template that does not define the sampling target area.

  For example, when the mobile robot 100 starts walking, it is necessary to accurately perform plane detection in a region near the feet of the mobile robot 100. In this case, by performing sampling using a template (see, for example, FIG. 8A) in which the region near the foot of the mobile robot 100 is a sampling target region, it is not necessary to perform plane detection processing in an unnecessary region. . For this reason, it is possible to more efficiently detect the plane in the foot vicinity region.

Next, FIG. 9 is a diagram for explaining an example in which the template selection unit 12 selects a template according to the movement state of the mobile robot 100. In FIG. 9, a three-dimensional position data group is plotted on an xy plane parallel to the floor surface, and height information (position information in the z-axis direction) exists for each plotted point. Here, it is assumed that the mobile robot 100 moves in the direction of the white arrow D.
As shown in FIG. 9A, from the plane information of the previous frame, a plane to be detected such as a step or a slope in front of the moving direction of the mobile robot 100 (region V surrounded by a broken line in FIG. 9A). If it is determined that it exists, the template selection unit 12 selects a template (for example, the template shown in FIG. 8B) whose sampling target is the region U2 near the center of the frame.
Furthermore, for example, as shown in FIG. 9B, when it is determined from the plane information of the previous frame that there is no plane to be detected such as a step or a slope in front of the moving direction of the mobile robot 100, The template selection unit 12 selects a template (for example, the template shown in FIG. 8C) whose sampling target is the region U3 far from the traveling direction, the side region U4 of the sensing region of the sensor, or the like.

Returning to FIG. 6 and continuing the description, in step S103, the plane detection unit 13 sets Data (i ₁ , j ₁ ) and Data (i ₂ , j) preset on the template for the template selected in step S103. _A vector is sampled by extracting a set of two-dimensional three-dimensional position data corresponding to ₂ ). More specifically, a vector composed of a set of three-dimensional position data of two points corresponding to Data (i ₁ , j ₁ ) and Data (i ₂ , j ₂ ) satisfying the following inequality is sampled. Note that Distance (Data (i ₁ , j ₁ ), Data (i ₂ , j ₂ )) is the three-dimensional position data corresponding to Data (i ₁ , j ₁ ) and Data (i ₂ , j ₂ ), respectively. It means the Euclidean distance between (x ₁ , y ₁ , z ₁ ) and (x ₂ , y ₂ , z ₂ ). D _min and D _max are condition values related to the magnitude of the vector determined according to the range of the plane to be detected, and are determined in advance and stored in a memory (not shown).
D _min <Distance (Data (i ₁ , j ₁ ), Data (i ₂ , j ₂ )) <D _max
Here, it is assumed that a set of three-dimensional position data of two points corresponding to Data (i ₁ , j ₁ ) and Data (i ₂ , j ₂ ) that does not satisfy the above inequality is not adopted.

In step S104, the template selection unit 12 determines whether or not the number of vectors sampled in step S103 is equal to or greater than a predetermined number.
As a result of the determination, if the number of sampled vectors is greater than or equal to a predetermined number, the process proceeds to step S105.

  On the other hand, if the result of determination in step S104 is that the number of sampled vectors is not greater than or equal to the predetermined number, processing returns to step S102 and a template is selected again. Here, when selecting a template again, it is preferable that the template selection unit 11 is configured to select a template that increases the number of vectors to be sampled.

Since the measurement environment changes as the mobile robot 100 moves, the number of vectors obtained satisfying D _min and D _max , which are conditions related to the magnitude of the vector, changes depending on the template used. That is, the fact that the number of vectors is insufficient means that the selected template is not sufficiently adapted to the environment in which the mobile robot 100 moves, and the template selected in step S102 is necessary for plane detection. It is estimated that the number of vectors could not be secured.
Therefore, a template adapted to the environment is selected by switching templates according to the number of vectors obtained by satisfying the conditions.

In step S105, the plane detection unit 12 searches for a normal line of the plane included in the three-dimensional position data group based on the vector sampled in step S103.
Specifically, first, the sampled vector is normalized and moved to the origin. Then, since the vector groups existing on the same plane are distributed in a disk shape centered on the origin, the angle of the vector orthogonal to the disk is calculated as the normal line of the plane.

In step S106, the plane detection unit 12 searches for the distance between the planes searched in step S105.
Specifically, first, a plane in which the density of vectors existing on the plane is equal to or more than a predetermined threshold is searched in the direction of the normal vector of the plane searched in step S105 from the origin. The distance from the origin to the searched plane is set as the distance to the detection candidate plane.

In step S107, the plane detection unit 12 labels the plane ID of the plane to which the three-dimensional position data belongs with respect to all the three-dimensional position data included in the three-dimensional position data group.
The plane ID is set corresponding to a plurality of planes searched through the processing from step S105 to S106.

  In step S108, the plane detection unit 12 performs a least square fitting process using the least square method so that the error between each plane and the three-dimensional position data associated with the plane is minimized. .

  The processing executed by the above-described three-dimensional position data generation unit 11, template selection unit 12, plane detection unit 13, and environment map generation unit 14 can be realized using a typical computer system. Specifically, a program for causing the computer system to perform the processing shown in the flowchart of FIG. 6 may be executed in response to a timer interrupt that occurs at regular time intervals. Note that the program for causing the computer system to execute the processing shown in the flowchart of FIG. 6 does not have to be a single program, and may be configured by a plurality of program modules divided according to the processing content.

As described above, it is not necessary to sample a region that is unnecessary for plane detection according to the movement state of the mobile robot 100. For this reason, the processing time required for plane detection can be shortened.
In particular, in the two-point random sampling method, since vectors have been randomly sampled in the entire region, the probability that a vector satisfying the size condition is sampled is reduced, and a desired number of vectors can be efficiently obtained. Could not be extracted. Furthermore, since it is necessary to repeat the sampling process until a desired number of vectors satisfying the conditions are extracted, the processing time required for plane detection is very long.
According to the present embodiment, a set of two-dimensional three-dimensional position data in the sampling target area is set in advance, and sampling is performed based on the two-point three-dimensional position data set in the area. As a result, a vector that satisfies the conditions can be sampled efficiently, so that the two-point random sampling method can be performed at a higher speed.
In this way, by shortening the processing time required for plane detection, it is possible to alleviate the fact that the plane detection process becomes a restriction on high-speed movement of the mobile robot.

Further, according to the present embodiment, sampling can be efficiently performed in an area where plane detection is desired according to the moving state of the mobile robot 100, so that more accurate plane detection can be performed. .
Since the measurement environment changes as the mobile robot 100 moves, the probability of obtaining the number of vectors extracted satisfying the conditions also changes. For this reason, when all regions are set as sampling target regions or when sampling is continuously performed using the same template, a desired number of vectors may not be extracted from the region to be detected. .
According to the present embodiment, by switching templates according to the measurement environment, it is possible to improve the probability of obtaining the number of vectors sampled satisfying the conditions according to the environment. In other words, the sampling target area can be adjusted by switching the template according to the measurement environment, and the detection sensitivity for plane detection can be improved. Therefore, more accurate plane detection can be performed.

  In this embodiment, since sampling is performed in a predetermined sampling target region, the randomness is slightly reduced as compared with the case where the conventional random sampling method in which the entire region is the sampling target region is adopted. To do. However, by preparing a plurality of templates having different sampling target areas and switching the templates according to the measurement environment, it is possible to mitigate the decrease in randomness.

  In the above example, a plurality of templates having different sampling target areas are generated in advance and mounted on the mobile robot 100. However, the present invention is not limited to this. For example, at the start of the mobile robot 100, a template (for example, the template T4 in FIG. 8E) that uses the entire region of the frame as a sampling target region is used. The sampling target area of the template may be updated according to the information and the movement amount of the mobile robot 100.

Other embodiments.
Embodiment 1 of the invention shows an example in which a plane is detected by a two-point random sampling method, but the present invention is not limited to this. For example, an arbitrary template is selected from a plurality of templates having different sampling target areas in accordance with the moving state of the mobile robot 100, and a plurality of 3's from the three-dimensional position data group are selected in the sampling target area defined by the selected template. The configuration may be such that the dimensional position data is sampled, and the plane is detected from the sampled three-dimensional position data using the Hough transform.

  In the above example, an example in which the present invention is applied to a legged mobile robot is shown, but the present invention can also be applied to a mobile robot having other moving means such as wheels.

  Furthermore, the present invention is not limited to the above-described embodiments, and various modifications can be made without departing from the gist of the present invention described above.

It is a side view which shows the structure of the mobile robot concerning embodiment of this invention. It is a block block diagram of the control system which the mobile robot concerning embodiment of this invention has. It is a figure for demonstrating the mode of the scanning by a laser range finder, when the mobile robot concerning embodiment of this invention moves. It is a figure which shows an example of the three-dimensional position data group concerning embodiment of this invention. It is a figure which shows an example of the environment where the mobile robot concerning embodiment of this invention moves, and an example of an environment map. It is a flowchart which shows the processing content of the plane detection apparatus concerning embodiment of this invention. It is a figure which shows an example of the template concerning embodiment of this invention. It is a figure which shows the template from which the sampling object area | region concerning embodiment of this invention differs. It is a figure which shows the example of application of the template concerning embodiment of this invention.

Explanation of symbols

1 plane detection device,
10 laser range finder, 11 3D position data generator,
12 template selection unit, 13 plane detection unit, 14 environment map generation unit,
15 walking planning unit, 16 motion generating unit,
17 Control unit, 18 Actuator, 19 Encoder

100 mobile robots, 104 legs,
102 right leg, 102c right knee joint, 102e right ankle joint, 102f right foot,
103 left leg, 103c left knee joint, 103e left ankle joint, 103f left foot,

160 environment, 161 obstacle, P11, 12 floor, 200 environment map,
P Passenger, R11, 12 area, V area,
T0 to T4 template, S0 to S3 sampling target area

Claims (8)

A plane detection method for detecting a plane,
According to the moving state of the mobile robot, the method comprising: sampling target area to select a template from a plurality of different templates,
In the sampling target region defined by the selected template, a two-point three-dimensional data set corresponding to a two-point set randomly set in advance from the three-dimensional data group is sampled, and the two-point three-dimensional data Detecting a plane based on a set of:
A plane detection method comprising: The plane detection method according to claim 1, wherein the template is selected based on plane information in a past movement state of the mobile robot. 3. The plane detection method according to claim 1, wherein the template is selected based on the number of sets of the sampled two-dimensional three-dimensional data. 4. The plane detection method according to claim 1, wherein a sampling target area is determined in advance and the template is mounted on the mobile robot. 5. A mobile robot that moves within a predetermined environment,
According to the moving state of the mobile robot, the template selecting unit sampled area to select a template from a plurality of different templates,
In the sampling target region defined by the selected template, a two-point three-dimensional data set corresponding to a two-point set randomly set in advance from the three-dimensional data group is sampled, and the two-point three-dimensional data A plane detection unit for detecting a plane based on the set of
A mobile robot comprising: The template selection unit
The mobile robot according to claim 5, wherein the template is selected based on plane information in a past movement state of the mobile robot. The template selection unit
7. The mobile robot according to claim 5, wherein the template is selected based on the number of sets of the sampled two-dimensional three-dimensional data. 8. The mobile robot according to claim 5, wherein a sampling target area is determined in advance and the template is mounted on the mobile robot. 9.

Images