JP4678007B2 - Environmental map generation method and mobile robot - Google Patents

Description

  The present invention relates to an environment map generation method and a mobile robot.

The autonomous mobile robot recognizes the environment of the movement route and moves to the destination. For this reason, the mobile robot generates an environment map using measurement data obtained by a visual sensor such as a stereo camera or a range sensor, and moves to the destination with reference to the environment map.
If the autonomous mobile robot is a biped walking robot, walking is planned by determining an area where the foot can land using an environmental map and sequentially planning the landing position of the foot.
Therefore, in order to plan a stable walking of the mobile robot, it is necessary to generate a dense environmental map with little data missing.

The environment map can be generated by detecting a plane group from a three-dimensional position data group calculated based on measurement information such as a parallax image obtained by a visual sensor and a distance image with a measurement target. As a method for detecting a plane from a three-dimensional position data group, a plane detection method using Hough transform, a plane detection method using random sampling, and the like are well known. For example, Patent Document 1 detects a plurality of planes including a floor surface on which a foot of a biped walking robot is in contact by calculating a plane parameter from a three-dimensional position data group using a Hough transform. A technique for generating an environmental map in which an obstacle area and a movable area are specified using a detection result of a plane is disclosed.
Further, in Patent Document 2, a 3D obstacle map and a reference height map indicating the height from a 2D reference plane are created from an external state, and the reference plane is determined based on the 3D obstacle map. An environmental map generation method for changing a height map is disclosed.
JP 2005-92820 A JP 2006-11880 A

However, with any known environmental map generation method, there is little data loss when the resolution of measurement data is low or when data loss or data shortage occurs due to a communication error with the visual sensor. A dense environmental map could not be obtained. For example, when there is missing data in the environmental map, if it is not possible to land on an area that can be landed originally, a route other than the optimum route must be generated, and unnecessary direction changes may occur. There is a problem that occurs.
On the other hand, as a method of complementing such data omission, a method of complementing data omission by superimposing the previous frame acquired before the current frame on the current frame on which the measurement data is plotted can be considered. . When the previous frame is superimposed on the current frame, the previous frame is coordinated by odometry to correspond to the current frame. However, when superposition is performed by such association, consistency of the data of the previous frame and the current frame after coordinate conversion cannot be obtained due to an odometry error, and such current frame and previous frame are overlapped. A highly accurate plane cannot be detected from the combined frames.
In particular, when the mobile robot is a biped robot, the height, pitch, and roll direction fluctuations are large, and accuracy in the height direction is lost due to odometry errors. For this reason, for example, the height component of the plane, which is particularly important when the mobile robot lands on a plane such as a step, cannot be detected with high accuracy.

As described above, it is insufficient to use the plane detected from the frame obtained by superimposing the current frame and the previous frame for generating the environmental map, and also refer to the environmental map including missing data and insufficient data. Therefore, there is a problem that the operation plan of the mobile robot cannot be stabilized.
The present invention has been made to solve such a problem, and provides an environment map generation method and a mobile robot that can reduce the influence of errors caused by odometry and generate a dense environment map with less data loss. The purpose is to do.

Environmental map generation method according to the present invention, the mobile robot with reference to a environment map generating method for generating an environmental map complementing data loss, a step of detecting plane information only from the current frame, the mobile Based on the amount of movement of the robot, converting the coordinate system of the previous frame acquired before the current frame to the coordinate system of the current frame, and superimposing the coordinate-converted previous frame on the current frame And generating the environment map based on the detected plane information and the superimposed current frame.

As a result, the data omission in the 3D position data group of the current frame can be complemented by the 3D position data group of the previous frame, and a dense environmental map with less data omission can be generated. In addition, since the plane is detected only from the current frame, it is possible to detect a plane with higher accuracy without being affected by errors due to odometry, compared to the case of detecting the plane from the frame in which the current frame and the previous frame are overlapped. Can do.
Therefore, the plane information detected in this way and the superimposed three-dimensional position data group of the current frame can generate a dense environment map with little data loss, The solution can be stabilized.

  The superimposing step specifies a corresponding plane in the current frame with respect to the plane in the previous frame subjected to coordinate conversion, and calculates a height component of the three-dimensional position data of the plane in the previous frame subjected to coordinate conversion, The superposition process may be executed by replacing with the height component of the three-dimensional position data of the plane specified in the current frame. As a result, the three-dimensional position data of the previous frame can be replaced with the height of the specified plane of the current frame as the reference height. Even in the case where the characteristics are lost, the deviation in the height direction can be corrected.

  Furthermore, the step of detecting plane information samples a set of two-dimensional three-dimensional position data from a three-dimensional position data group and calculates a plane parameter based on the two-point three-dimensional position data set; A step of determining a plane to which the three-dimensional position data belongs with respect to the plane composed of the calculated plane parameters. Thereby, a plane can be extracted accurately and stably 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.

  The mobile robot may be provided with two legs as moving means and move by bipedal walking motion. When the mobile robot is a biped robot, the accuracy in the height direction is lost due to an error due to odometry, but even in such a case, the error in the height direction can be corrected.

A mobile robot according to the present invention is a mobile robot that moves by referring to an environmental map supplemented with missing data, and uses a visual sensor that visually recognizes the environment and measurement data obtained by the visual sensor. A three-dimensional position data generation unit that generates a three-dimensional position data group indicating a position of a measurement target existing in the environment, a plane detection unit that detects plane information only from the three-dimensional position data group of the current frame, and the movement Based on the amount of movement of the robot, a coordinate conversion unit that converts the coordinate system of the previous frame acquired before the current frame to the coordinate system of the current frame, and superimposing the coordinate-converted previous frame on the current frame A superposition unit for matching, and an environmental map generation unit for generating the environmental map based on the detected plane information and the superposed current frame. Than is.

Thereby, data omission in the current frame can be complemented by the previous frame, and a dense environmental map with less data omission can be generated. In addition, since the plane is detected only from the current frame, it is possible to detect a plane with higher accuracy without being affected by errors due to odometry, compared to the case of detecting the plane from the frame in which the current frame and the previous frame are overlapped. Can do.
Therefore, the plane information detected in this way and the superimposed three-dimensional position data group of the current frame can generate a dense environment map with little data loss, The solution can be stabilized.

  Further, the superposition unit specifies a corresponding plane in the current frame with respect to the plane in the previous frame subjected to coordinate transformation, and calculates a height component of the three-dimensional position data of the plane in the previous frame subjected to coordinate transformation, The superposition process may be executed by replacing with the height component of the three-dimensional position data of the plane specified in the current frame. As a result, the three-dimensional position data of the previous frame can be replaced with the height of the specified plane of the current frame as the reference height, so that the matching of the three-dimensional position data by the replacement is affected by the error due to odometry. Even in the case where the characteristics are lost, the deviation in the height direction can be corrected.

  Furthermore, the plane detection unit samples a set of two-dimensional three-dimensional position data from the three-dimensional position data group, and calculates a plane parameter based on the two-point three-dimensional position data set. And a labeling processing unit for determining a plane to which the three-dimensional position data belongs with respect to the plane composed of the calculated plane parameters. Thereby, 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.

  The mobile robot may be provided with two legs as moving means and move by bipedal walking motion. When the mobile robot is a biped robot, the accuracy in the height direction is lost due to an error due to odometry, but even in such a case, the error in the height direction can be corrected.

According to the present invention reduces the influence of errors due to odometry, the environment map generation method capable of generating a less dense environment map of data loss, and it is possible to provide a mobile robot.

  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. Furthermore, the mobile robot has an environment map generation device. The environment map generation device detects plane information from the current frame, and converts the coordinate system of the previous frame acquired before the current frame into the coordinate system of the current frame based on the movement amount of the mobile robot. Next, the coordinate-converted three-dimensional position data group of the previous frame is superimposed on the three-dimensional position data group of the current frame, and the environment is determined based on the detected plane information and the superimposed three-dimensional position data group of the current frame. A device that generates a map.
The mobile robot refers to the environment map generated by the environment map generation device and determines the landing position of the foot provided at the tip of each of the two leg links of the mobile robot.
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 environment map generation device 1 includes a plane detection unit 12, a coordinate conversion unit 13, an overlay unit 14, and an environment map generation unit 15.

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. 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. Further, an area 162 surrounded by a broken line in FIG. 4 represents a data missing area generated due to a communication error with the sensor.
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 the encoder 20 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. As a result, the detection error in the pitch direction that has a large influence on the three-dimensional position data in the walking robot is corrected.

The plane detection unit 12 detects a plane from the 3D position data group generated by the 3D position data generation unit 11. More specifically, the plane detection unit 12 samples two sets of three-dimensional position data from the three-dimensional position data group, and calculates a plane parameter based on the two sets of three-dimensional position data. A parameter calculation unit; and a labeling processing unit that determines a plane to which the three-dimensional position data belongs with respect to the plane including the calculated plane parameters.
Such a two-point random sampling method can extract a plane accurately and stably as compared with a method using a 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.
Note that the plane detection method is not limited to this, and a plane may be detected by using a conventionally known method using Hough transform.

The coordinate conversion unit 13 converts the coordinate system of the previous frame into the coordinate system of the current frame based on the movement amount of the mobile robot 100. In the present embodiment, a frame generated immediately before the current frame is used as the previous frame.
Since errors due to odometry accumulate, frames generated earlier than the current frame are greatly affected by errors due to odometry. For this reason, it is preferable to use the immediately preceding frame for overlaying with a relatively small error due to odometry with respect to the current frame.

The superimposing unit 14 superimposes the previous frame after coordinate conversion on the current frame. Further, the superposition unit 14 specifies a corresponding plane in the current frame with respect to the plane in the previous frame subjected to coordinate conversion, and calculates the height component of the three-dimensional position data of the plane in the previous frame subjected to coordinate conversion in the current frame. The superposition process is executed by replacing with the height component of the three-dimensional position data of the specified plane.
The details of the overlay process on the current frame by the overlay unit 14 will be described later.

The environment map generation unit 15 generates an environment map as a set of a plurality of planes based on the plane information detected by the plane detection unit 12 and the current frame superimposed by the overlapping unit 14.
More specifically, first, based on the plane detection result by the plane detection unit 12, the floor area is recognized as a movable area where the mobile robot 100 can walk, and an area including a plane different from the floor is included. Is recognized as an obstacle area.
The floor surface means a plane on which the right foot 102f and the left foot 103f of the mobile robot 100 are in contact. However, such region recognition is an example. For example, even if it is a flat surface or a slope with a different height from the floor surface, if the difference in height from the floor surface is smaller than a predetermined value, it is possible to land the right foot 103f and the left foot 103f. These planes may be recognized as movable areas. Furthermore, it may be determined whether to recognize the plane as a movable region based on the result of comparing the area of the detected plane with the areas of the right foot 103f and the left foot 103f.
Next, the environment map update generation unit 15 integrates the recognized movable region and obstacle region with the three-dimensional position data group of the current frame superimposed by the overlapping unit 14 to generate an environment map.

  Here, the environment map in the present embodiment is generated as grid data obtained by dividing a two-dimensional plane (referred to as an xy plane) into a grid pattern. 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 15 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 shown in FIG. 5A, the plane detection unit 12 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 15 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 15 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 addition, in order to detect the unevenness of the road surface at the landing positions of the right foot 102f and the left foot 103f by the difference in the plane ID between adjacent grids and the difference in the height in the z-axis direction, each grid constituting the environment map 200 is detected. The area may be selected to be sufficiently smaller than the areas of the bottom surfaces of the right foot 102f 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 16 determines a target position with reference to the environment map generated by the environment map generation device 1, and calculates the landing positions of the right foot 103f and the left foot 103f for reaching the determined target position.

  The motion generation unit 17 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 18 receives the motion data generated by the motion generation unit 17 and calculates a target joint angle of each joint by inverse kinematics calculation. Further, the control unit 18 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 20. The mobile robot 100 is walked by operating the actuator 19 for driving each joint according to the torque control value calculated by the control unit 18.

  Next, details of the environment map generation method performed by the environment map generation apparatus 1 will be described below with reference to FIGS. FIG. 6 is a flowchart showing an environment map generation procedure by the environment map generation apparatus 1. FIG. 7 is a diagram for explaining the process of superimposing the previous frame and the current frame. FIG. 8 is a diagram for explaining the process of specifying the plane of the current frame corresponding to the plane of the previous frame. FIG. 9 is a conceptual diagram for explaining how to replace the height component of the three-dimensional position data in the previous frame with the height component of the three-dimensional position data in the current frame. 7 and 8 are obtained by plotting 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.

First, in step S101, the plane detection unit 12 detects a plane from the three-dimensional position data group in the current frame. That is, plane parameters for a plurality of planes as plane information are calculated, and a plane to which each three-dimensional position data in the current frame belongs is determined.
When a plane is detected from a frame obtained by superimposing the previous frame and the current frame, the processing time required for plane detection is about twice that of the conventional method, which is a restriction on high-speed movement of the mobile robot 100. For this reason, by detecting a plane from only the current frame, it is possible to maintain the same processing time as before without increasing the processing time required for the plane detection process.
Also, when a plane is detected from a frame obtained by superimposing the previous frame and the current frame, due to an error caused by odometry, the consistency of the three-dimensional position data group in the frame after superposition cannot be obtained, and a plane with high accuracy is obtained. It cannot be detected. For this reason, it is more preferable to detect the plane only from the current frame.
Although the current frame includes missing data, it does not cause a problem in plane detection processing, and a plane with desired accuracy can be detected.

In step S102, the coordinate conversion unit 13 converts the coordinate system of the previous frame into the coordinate system of the current frame based on the movement amount of the mobile robot 100. Table Here, the current frame _{M i,} the previous frame when expressed as _{M i-1,} the previous frame _{M i-1} of the coordinate transformation F _{(M i-1)} generally by the following formula (1) Is done. In Equation (1), R is a rotation matrix and t is a translation vector. The rotation matrix R and the translation vector t are determined by reflecting the movement amount and posture change of the mobile robot 100 obtained from measurement information such as the encoder 20.
F (M _i-1 ) = RM _i-1 + t (1)

In step S103, the superimposing unit 14 superimposes the previous frame after coordinate conversion on the current frame. More specifically, the coordinate-converted three-dimensional position data of the previous frame is superimposed on the current frame.
The image in FIG. 7A represents the current frame, and the image in FIG. 7B represents the previous frame generated before the current frame. When the mobile robot 100 shown in FIG. 7B moves forward (in the direction of the white arrow D), the three-dimensional position data group changes between the previous frame and the current frame, and the region where data loss has occurred also changes. To do.
The image of FIG. 7C shows a frame after the coordinate conversion is performed on the previous frame shown in FIG. 7B and the current frame shown in FIG.

In step S <b> 104, the superimposing unit 14 identifies a plane corresponding to the current frame with respect to the plane in the previous frame subjected to coordinate conversion. For example, the plane A _j in the current frame corresponding to the plane A _i in the previous frame is specified based on the plane parameters. As the plane parameters, the plane height is represented as h, the inclination is represented as φ and θ, and the thresholds for comparing the plane parameters are TH _h , TH _φ , and TH _θ , respectively. For the plane A _i in the previous frame, a current frame plane A _j that satisfies all of the following expressions (2) to (4) is specified.
| A _j (h) −A _i (h) | <TH _h (2)
| A _j (φ) −A _i (φ) | <TH _φ (3)
| A _j (θ) −A _i (θ) | <TH _θ (4)
Thus, by specifying the corresponding plane using the plane parameter, the corresponding plane can be specified quickly.
For example, with respect to the plane A _{i of the} previous frame shown in FIG. 8A (region A _i surrounded by a broken line in the figure), the plane A _j in the current frame shown in FIG. Region A _j ) is identified.

In step S105, the height component of the plane three-dimensional position data in the previous frame subjected to coordinate conversion is replaced with the height component of the plane three-dimensional position data specified in the current frame. That is, for the three-dimensional position data (A _ix , A _iy , A _iz ) belonging to the plane A _i in the previous frame, the height component A _iz is replaced with the height component A _jz of the plane A _j in the current frame.
The image in FIG. 9A is a conceptual diagram showing the height direction (z-axis direction in the drawing) as if the previous frame after coordinate conversion was superimposed on the current frame. As shown in FIG. 9A, due to an error due to odometry, the 3D position data belonging to the plane A _i in the previous frame is not consistent with the 3D position data belonging to the plane A _j in the current frame. It will be a thing.
On the other hand, the image of FIG. 9B is obtained by superimposing the coordinate-converted previous frame on the current frame, specifying the plane A _j of the current frame corresponding to the plane A _{i of the} previous frame, It is a conceptual diagram which shows that the thickness component was replaced. As shown in FIG. 9B, the height component of the three-dimensional position data belonging to the plane A _{i of the} previous frame is replaced with the height component of the three-dimensional position data belonging to the plane A _{j of the} current frame. The height components of the planes in the combined frames are horizontal and consistent.
For example, the image of FIG. 7D shows a frame in which the height component of the plane is replaced with the height component of the current frame in the frame after superposition shown in FIG. 7C.
As a result, the three-dimensional position data of the previous frame can be replaced with the height of the specified plane of the current frame as the reference height, so that the consistency of the three-dimensional position data is lost due to the error due to odometry. Even in such a case, the deviation in the height direction can be corrected.

  In step S <b> 106, the environment map generation unit 15 generates an environment map based on the plane information detected by the plane detection unit 12 and the current frame superimposed by the overlay unit 14.

  Note that the processing executed by the above-described three-dimensional position data generation unit 11, plane detection unit 12, coordinate conversion unit 13, superposition unit 14, and environment map generation unit 15 can be realized using a typical computer system. is there. 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, the mobile robot 100 according to the present embodiment can supplement the missing data in the 3D position data group of the current frame with the 3D position data group of the previous frame, and can perform a dense operation with less data missing. A simple environmental map can be generated. In addition, since the plane is detected only from the current frame, it is possible to detect a plane with higher accuracy without being affected by errors due to odometry, compared to the case of detecting the plane from the frame in which the current frame and the previous frame are overlapped. Can do. Therefore, according to the plane information detected in this way and the superimposed three-dimensional position data group of the current frame, it is possible to generate a dense environment map with few data omissions, and the operation plan of the mobile robot 100 The solution of can be stabilized.
In addition, since the recognition range is expanded by using the previous frame as compared to the environment map obtained only from the current frame, for example, the back of the mobile robot 100 that cannot be recognized from only the current frame can be reflected in the environment map. It is possible to make a more stable walking plan.

Other embodiments.
Embodiment 1 of the invention shows an example in which the present invention is applied to a legged mobile robot, 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 environment map production | generation apparatus concerning embodiment of this invention. It is a figure for demonstrating the superimposition process concerning embodiment of this invention. It is a figure for demonstrating the process which specifies the plane concerning embodiment of this invention. It is a conceptual diagram for demonstrating replacing the height component concerning embodiment of this invention.

Explanation of symbols

1 Environmental map generator,
10 laser range finder, 11 3D position data generator,
12 plane detection unit, 13 coordinate conversion unit, 14 superposition unit,
15 Environmental map generator,
16 walking planning unit, 17 motion generating unit,
18 control unit, 19 actuator, 20 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, 162 data missing area,
P11, 12 Floor, 200 Environmental map,
P Passenger, R11, 12 area

Claims (8)

An environment map generation method for generating an environment map that is referenced by a mobile robot and supplements data loss ,
Detecting plane information from only the current frame;
Converting the coordinate system of the previous frame acquired before the current frame to the coordinate system of the current frame based on the movement amount of the mobile robot;
Superimposing the coordinate-transformed previous frame on the current frame;
An environment map generation method comprising: generating the environment map based on the detected plane information and the superimposed current frame. The overlapping step includes
For the plane in the previous frame that has undergone coordinate transformation, the corresponding plane is identified in the current frame, and the height component of the three-dimensional position data of the plane in the previous frame that has undergone coordinate transformation is identified in the current frame. The environment map generation method according to claim 1, wherein the overlay process is executed by replacing the height component of the three-dimensional position data of the plane. The step of detecting the plane information includes:
Sampling a set of two-dimensional three-dimensional position data from the three-dimensional position data group, and calculating plane parameters based on the two-point three-dimensional position data set;
The environmental map generation method according to claim 1, further comprising: determining a plane to which the three-dimensional position data belongs for a plane composed of the calculated plane parameters. The method for generating an environmental map according to any one of claims 1 to 3, wherein the mobile robot has two legs as moving means, and moves by biped walking. A mobile robot that moves with reference to an environmental map that complements missing data ,
A visual sensor for visually recognizing the environment;
Using the measurement data obtained by the visual sensor, a three-dimensional position data generation unit that generates a three-dimensional position data group indicating the position of the measurement target existing in the environment;
A plane detection unit that detects plane information only from the three-dimensional position data group of the current frame;
A coordinate conversion unit that converts the coordinate system of the previous frame acquired before the current frame into the coordinate system of the current frame based on the movement amount of the mobile robot;
A superimposing unit that superimposes the coordinate-converted previous frame on the current frame;
A mobile robot comprising: an environment map generation unit that generates the environment map based on the detected plane information and the superimposed current frame. The overlapping portion is
For the plane in the previous frame that has undergone coordinate transformation, the corresponding plane is identified in the current frame, and the height component of the three-dimensional position data of the plane in the previous frame that has undergone coordinate transformation is identified in the current frame. The mobile robot according to claim 5, wherein the superposition process is executed by replacing with a height component of the three-dimensional position data of the plane. The plane detector is
A plane parameter calculation unit that samples a set of three-dimensional position data of two points from a group of three-dimensional position data and calculates a plane parameter based on the set of three-dimensional position data of the two points;
The mobile robot according to claim 5, further comprising: a labeling processing unit that determines a plane to which the three-dimensional position data belongs with respect to a plane composed of the calculated plane parameters. The mobile robot according to any one of claims 5 to 7, wherein the mobile robot includes two legs as moving means, and moves by biped walking motion.

Images