JP2017198517A - Three dimensional map generation system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To generate a three dimensional map.SOLUTION: A three dimensional map generation system 10 includes a robot 12 and a remote control device 14, and while moving the robot by using the remote control device, and scans its circumference by using a three dimensional laser ranging system 28, so as to obtain scanned data (data set). The three dimensional map generation system generates a two dimensional grid map by using a plurality of data sets (S1), matches the two dimensional grid map with a prescribed three dimensional coordinate system (S5), corrects positional deviations in the three dimensional coordinate system of all the points in the data set, and plots each point of a three dimensional map of a voxel form (S41). Then a three dimensional map is generated.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a three-dimensional map generation system, and more particularly to a three-dimensional map generation system that generates a three-dimensional map using, for example, three-dimensional scan data acquired by a three-dimensional laser rangefinder mounted on a mobile robot.

  In Patent Document 1, distance data obtained by a laser rangefinder such as a laser range finder (LRF) is converted into position data based on the position and orientation of a moving body at the time of measurement, and the position data is converted into position data. A method of creating a map with only necessary position data by erasing unnecessary position data by clustering into a plurality of clusters according to a predetermined rule and erasing clusters that do not satisfy the conditions is disclosed.

JP 2014-174275 [G09B 29/00 G06T 11/60 ...]

  In Patent Document 1, a three-dimensional map is not considered.

  Therefore, a main object of the present invention is to provide a novel three-dimensional map generation system.

  Another object of the present invention is to provide a 3D map generation system that can easily generate a 3D map.

  The present invention employs the following configuration in order to solve the above problems. The reference numerals in parentheses, supplementary explanations, and the like indicate correspondence relationships with embodiments described later to help understanding of the present invention, and do not limit the present invention in any way.

  The first invention is a mobile object, an omnidirectional laser distance meter mounted on the mobile object, a two-dimensional map generation means for generating a two-dimensional grid map based on a data set obtained by scanning with the omnidirectional laser distance meter, Matching means for matching the three-dimensional grid map with a predetermined three-dimensional coordinate system, conversion means for converting each point of the data set into a three-dimensional coordinate system based on the position and direction of the moving body, and each of the points converted into the three-dimensional coordinate system A 3D map generation system comprising: matching means for matching points according to a time-series data set; and generation means for writing each matched point to a voxel map to generate a 3D map.

  In the first invention, the three-dimensional map generation system (10: reference numerals exemplifying corresponding parts in the embodiment; the same applies hereinafter) is a moving body (12) whose movement is controlled by, for example, a remote control device (14). The omnidirectional laser rangefinder (28) such as an omnidirectional laser range finder is mounted on the moving body. When the moving body is moved, scanning with an omnidirectional laser rangefinder is performed, and the two-dimensional map generation means (30, S1) generates a two-dimensional grid map based on the data set obtained thereby. The matching means (30, S5) matches the two-dimensional grid map with a predetermined three-dimensional coordinate system. The conversion means (30, S25) converts each point of the data set into a three-dimensional coordinate system based on the position and direction of the moving object. A matching means (30, S27-S39) matches each point converted to the three-dimensional coordinate system according to a time-series data set. Thereby, each point (Pj) is aligned with the correct position in the three-dimensional coordinate system. A generation means (30, S41) writes each matched point on the voxel map to generate a three-dimensional map.

  According to the first invention, a three-dimensional map can be easily generated from scan data of an omnidirectional laser rangefinder.

  A second invention is dependent on the first invention, and the matching means matches a specific point included in a plane in a three-dimensional coordinate system, and matches the remaining points according to the result. System.

  In the second invention, the matching means (S27-S39) performs matching of specific points (P1, Pk) included in the plane in the three-dimensional coordinate system (S27-S37), and the remaining points are determined according to the result. Matching is performed (S39).

  According to the second invention, by first performing matching using only a specific point, not only the required time can be shortened, but also the matching accuracy is improved.

  A third invention is dependent on the first or second invention, and generates a synthesized three-dimensional map based on a storage means for storing data of a three-dimensional map and a plurality of three-dimensional maps stored by the storage means A three-dimensional map generation system further comprising a synthetic map generation means.

  In the third invention, the storage means (72) stores the three-dimensional map data generated by the generation means (S41), and the composite map generation means (30, S11, S13, S51-S61) Specifically, a composite three-dimensional map is generated based on a sequential three-dimensional map according to a time series.

  According to the third aspect of the invention, a synthetic 3D map can be generated.

4th invention is performed by the computer of the three-dimensional map production | generation system provided with the moving body and the omnidirectional laser rangefinder mounted in a moving body, and a computer is based on the data set obtained by the scan by an omnidirectional laser rangefinder. A two-dimensional map generating means for generating a two-dimensional grid map and a matching means for matching the two-dimensional grid map with a predetermined three-dimensional coordinate system;
Conversion means for converting each point of the data set to a three-dimensional coordinate system based on the position and direction of the moving body. Matching means for matching each point converted to the three-dimensional coordinate system according to a time-series data set, and matching It is a 3D map generation program that causes each point to be written on a voxel map and functions as a generation unit that generates a 3D map.

  According to the fourth invention, the same effect as that of the first invention can be expected.

  According to the present invention, a three-dimensional map can be easily generated using scan data (data set) of a three-dimensional laser rangefinder.

  The above object, other objects, features, and advantages of the present invention will become more apparent from the following detailed description of embodiments with reference to the drawings.

FIG. 1 is a block diagram showing a three-dimensional map generation system according to an embodiment of the present invention. FIG. 2 is a block diagram showing an example of the electrical configuration of the mobile robot of FIG. 1 embodiment. FIG. 3 is an illustrative view showing one example of a place where the 3D map generation system of FIG. 1 embodiment is experimented. FIG. 4 is an illustrative view showing an example of actual scan data of an omnidirectional laser rangefinder mounted on a mobile robot. FIG. 5 is an illustrative view showing an example of a two-dimensional grid map created based on scan data of an omnidirectional laser rangefinder. FIG. 6 is an illustrative view showing one example of a three-dimensional voxel map. FIG. 7 is an illustrative view showing one example of a memory map of a memory of the remote control device in FIG. 1 embodiment. FIG. 8 is a flow chart showing an example of an operation for creating a three-dimensional composite map. FIG. 9 is a flowchart showing an example of an operation for generating a three-dimensional map in the embodiment of FIG. FIG. 10 is a flowchart showing an example of the operation of updating the three-dimensional composite map with the three-dimensional map generated in FIG.

  Referring to FIG. 1, a three-dimensional map generation system (hereinafter simply referred to as “map generation system”) 10 of this embodiment is a mobile robot (hereinafter simply referred to as “robot”) as a moving body. And a remote control 14 that controls the movement of the robot 12. The robot 12 and the remote control device 14 can communicate with each other via the network 16.

  As shown in FIG. 1, the robot 12 includes a base 18 on which the main body 20 is placed, and the base 18 is provided with wheels 22 for moving the robot 12. A support part 24 is provided on the main body 20, and an RGB camera 26 is provided on the support part 24 facing forward, and an omnidirectional laser rangefinder 28 is provided on the upper end of the support part. The omnidirectional laser rangefinder 28 is, for example, a three-dimensional laser rangefinder having a detection angle (vertical viewing angle) of 40 ° up and down (+ 30 ° −−10 °) with respect to the horizontal. The three-dimensional omnidirectional laser rangefinder (hereinafter, simply referred to as “laser rangefinder”) 28 measures a distance of up to about 100 m, for example, by making one revolution per 0.1 second. In the experiment, an imaging unit LiDAR (HDL-32e) (trade name) manufactured by Velodine was used as the laser distance meter 28. The movement of the robot 12 is controlled by the remote operation device 14.

  The remote operation device 14 includes a computer 30. When an operator operates an operation key or a joystick (not shown) provided on an operation console 32 connected to the computer 30, the computer 30 is operated according to the operation input. Controls the wheel 22 of the robot 12, that is, the movement of the robot 12.

  A memory 34 is connected to the computer 30 and a display 36 is connected. As will be described later, the memory 34 stores not only a program for generating a map, but also scan data (data set) acquired by the laser distance meter 28 described above, as well as two-dimensional map data and three-dimensional map data. Stores composite map data and the like. The display unit 36 displays the moving state of the robot 12 and an image taken by the camera 26 to show to the operator in order to safely move the robot 12.

  A wireless communication module 38 is attached to the computer 30, and the computer 30 can perform wireless communication with the computer 42 through the network 16 and through the wireless communication module 40 (FIG. 2) of the robot 12.

  A memory 46 is connected to the computer 42 of the robot 12 via a bus 44, and a sensor interface 48 and a motor driver 50 are connected to the computer 42. The RGB interface 26 and the laser distance meter 28 are connected to the sensor interface 48. The sensor interface 48 receives the image data from the camera 26 and the sensor data from the laser distance meter 28, and is controlled by the computer 42. The data is sent to the computer 30 (FIG. 1) through the wireless communication module 40, the network 16, and the wireless communication module 38.

  On the other hand, operation data from the remote operation device 14 is sent to the computer 42 through the wireless communication module 38, the network 16, and the wireless communication module 40, and is given from the computer 42 to the motor driver 50. Therefore, the wheel motor 52 is driven and the robot 12 is moved by remote operation.

  The robot 12 of such a map generation system 10 acquires scan data by the laser distance meter 28 while moving in a place as shown in FIG. The place illustrated in FIG. 3 is a part of the second floor of a commercial facility used by the inventors for the experiment of the map generation system 10. In the experiment, the robot 12 moves about 530 m in this commercial facility for about 25 minutes in one run, and outputs position data (data set) of about 182 million point clouds. And in order to produce | generate one 3D map, the same place was run 7 times as an example, and the data set for 7 times was acquired.

  FIG. 4 shows representative three-dimensional scan data 54 acquired by the laser distance meter 28, and the arc-shaped line in FIG. 4 schematically shows how the laser distance meter 28 is scanning. As shown in FIG. 4, the scan data includes not only static objects such as walls and floors, but also data from dynamic objects such as humans. On the other hand, although not understood in FIG. 4, the actual scan data is colored in the height direction, and the low position is expressed in red and the high position is expressed in blue.

  Such scan data 54 is stored in the memory 34 of the computer 30 of the remote control device 14 as described above. Then, the computer 30 creates a two-dimensional map 56 shown in FIG. 5 using a data set indicating position data of 182 million point clouds obtained in one run. As shown in FIG. 5, this two-dimensional map 56 is a grid map of 10 cm, for example. When creating this map, each point included in the data set is dropped into the 10 cm grid (downsampled).

  In the two-dimensional grid map 56 of FIG. 5, the gray portion is a location where the laser distance meter 28 of the robot 12 could not be scanned, and the white portion is a location scanned by the laser distance meter 28.

  Thereafter, a three-dimensional map is generated. This three-dimensional map is a map represented by 10 cm voxel as shown in FIG. This voxel map is generated by generating a two-dimensional grid map as shown in FIG. 5 and then using the exact position and orientation of the robot 12 in the three-dimensional coordinate system, as shown in FIG. Is generated based on a two-dimensional grid map for each run. As will be described in detail later, when voxels are arranged in the height direction of the plane corresponding to the grid of the two-dimensional map and an object exists in the grid of the two-dimensional map, “1” is written in the corresponding voxel. For example, an object that is on the same grid in all seven two-dimensional maps as a result of seven runs of the robot 12 has its “1” accumulated in the three-dimensional map as shown in FIG. A dimension map is generated.

  Here, the memory 34 of the computer 30 of the remote control device 14 of FIG. 1 includes a program storage area 60 and a data storage area 62 as shown in FIG. 7, for example.

  The program storage area 60 includes a necessary control program such as an OS, a movement control program 64 for controlling the movement of the robot 12, and a map creation program 66 described in detail later with reference to FIGS. It is set in advance.

  The data storage area 62 is a measurement data buffer 68 for temporarily storing scan data 54 (data set) obtained from the laser rangefinder 28, and two-dimensional map data for storing data of the two-dimensional grid map shown in FIG. A storage area 70, a 3D map data storage area 72 for storing data of a 3D voxel map 58 as shown in FIG. 6, and a composite map data storage area for storing data of a composite map obtained by combining two or more 3D maps 74 etc. are included.

  The computer 30 of the remote operation device 14 shown in FIG. 1 causes the robot 12 to travel in a place as shown in FIG. 3, for example, based on operation data from the console 32 operated by the operator according to the movement control program 64. Scan data (data set of measurement data) obtained by one run of the robot 12 is stored in the measurement data buffer 68 of the memory 34.

  Then, in the first step S1 of FIG. 8 defined by the map creation program 66 (FIG. 7), the computer 30 is based on scan data (data set) for one run stored in the measurement data buffer 68. Then, a two-dimensional grid map as shown in FIG. 5 is generated. The data of the two-dimensional map is stored in the two-dimensional map data area 70 (FIG. 7).

  In subsequent step S3, the computer 30 determines whether or not the two-dimensional map is the first one. If “YES”, in the next step S5, the computer 30 matches the position and orientation on the first two-dimensional map with a predetermined three-dimensional coordinate system. Thereafter, or when “NO” in the step S3, that is, when the two-dimensional map is not the first one, the process proceeds to a step S7.

  Step S7 is specifically composed of a subroutine as shown in FIG.

  In the first step S21 of FIG. 9 defined by the map creation program 66 (FIG. 7), a variable i indicating the number of scans (running) is set to i = 0. Therefore, the data set measured in the i-th scan (running) is represented by Si (S0, S1,... Si,... Sn). In the subsequent step S23, a variable j indicating the point position is set to j = 0. Therefore, the j-th point in the i-th data set is expressed as SiPj. That is, Si = {P1, P2,... Pj,.

  In step S25, the computer 30 uses the RPi (position and orientation on the two-dimensional map of the robot 12 when Si is used) to convert the point position Pj into the three-dimensional coordinates on the two-dimensional map matched in step S5. Convert.

  In the next step S27, the computer 30 applies a local plan using all points within the distance R around the point Pj. That is, in this step S27, a local plane including the point Pj is estimated.

  In step S29, the computer 30 calculates the flatness α as α = number of points in the local plane ÷ number of points in the distance R. That is, how much of the points in the vicinity of the point Pj is within the local plane estimated in step S27 is calculated. Therefore, if α = 1, it means that all points within the distance R around Pj are in the same local plane. In other words, the closer α is to “1”, the closer to the plane.

  The point Pj is defined as Pj (xj, yj, zj, Nj, αj) using the flatness α thus calculated and the normal (Normal) of the local plane.

  In step S31, the computer 30 determines whether or not the processing in steps S25 to S29 has been completed for all points in the data set Si (j <NPSi). If “NO”, the point number j is incremented in step S33, and the process returns to step S25.

  In this way, when the definition on the three-dimensional coordinates is completed for all the points P of the data set Si, “YES” is determined in the step S31, and the computer 30 determines the data set number i in the next step S35. Is “0”, that is, whether or not the above-described processing for the first data set is completed.

  If “NO” in the step S35, the computer 30 in the next step S37, the flatness α described above in the current data set Si exceeds the threshold T (in the embodiment, 0.9 is taken as an example). A point Pl (Pl) ({PlεSi / Pl.α> T}) and a point Pk {PkεSi−1 / Pk where the flatness α exceeds the threshold T in the previous data set Si−1. .α> T}). Note that scan matching is to detect a shift between corresponding points in different data sets (scans) by a method similar to ICP (Interactive Closest Point).

  When performing scan matching in this way, not only can the required time be shortened by performing scan matching using only the points Pl and Pk at which the flatness α calculated in step S29 exceeds a threshold T, for example, 0.9, Since each point is estimated to be on a plane, there is an advantage that the accuracy of matching is improved.

  Using the result of the scan matching executed in step S37, in step S39, the computer 30 performs alignment (shift correction) for all points in the data set Si. Thereby, all the points of Si = {P1, P2,... Pj,... PN} are arranged at correct positions on the three-dimensional coordinate system.

  After step S39 or if “YES” in the step S35, the process proceeds to a step S41. In step S41, the computer 30 joins all the points of the data set Si to a voxel-type three-dimensional map as shown in FIG.

  In this manner, a two-dimensional grid map (FIG. 5) is generated using the data set Si in step S1 of FIG. 8, and the two-dimensional map is aligned with a predetermined three-dimensional coordinate system in step S5 to obtain data. A three-dimensional map is generated by correcting the positional shift in the three-dimensional coordinate system of all the points in the set Si and plotting each point on a three-dimensional map in the voxel format.

  However, when correcting the misalignment, for the reasons described above, the misalignment is first detected at a specific point extracted from the data set, and all the points are detected using the detected misalignment information (scan matching result). Correct the misalignment.

  When the three-dimensional map is generated in step S41, the computer 30 determines in step S43 whether the scan number, that is, the number of data sets i has reached the total number of data sets NS. If “YES”, after the variable i is incremented in step S45, the process returns to step S23 and the above-described processing is repeatedly executed. That is, one three-dimensional map is generated using all data sets (seven sets in the above-described experiments by the inventors). This 3D map data is stored in the 3D map data storage area 72 of FIG.

In this way, after the three-dimensional map is generated in step S7 of FIG. 8, in step S9, the computer 30 determines whether the three-dimensional map created in step S7 is the first one. If “YES”, the composite map is initialized in step S11. Here, the composite map is a combination of information from three-dimensional maps created at different times, and means a three-dimensional map generated by combining or integrating information of a plurality of three-dimensional maps. In step S11, the three-dimensional map data generated in step S7 is copied to the configuration map data area 74 shown in FIG. Here, the original three-dimensional map data is still stored in the three-dimensional map data area 72. That is, as the 3D map data is generated, it is sequentially stored in the 3D map data area 72. The composite map is generated by combining the three-dimensional maps accumulated in this way.
FIG. 10 is a flowchart showing a method of updating the composite map included in the map creation program 66 of FIG. 7. In brief, in this update process, the nth three-dimensional map Mn is replaced with the immediately preceding composite map CMn−. 1 is combined to create a synthetic map CMn. However, the immediately preceding synthetic map CMn-1 is a three-dimensional voxel map obtained by synthesizing the three-dimensional maps M1, M2,. The three-dimensional map Mn includes Jn voxels, and the immediately preceding composite map CMn-1 includes In-1 voxels.

  In steps S51 to S55 of FIG. 10, the computer 30 first sets all the voxels corresponding to the immediately preceding composite map CMn-1 to all the voxels of the composite map CMn (the composite map obtained as a result of updating this time). .

  In steps S57 to S61, the corresponding voxel of the current three-dimensional map Mn is written in each voxel of the current composite map. That is, when the corresponding voxel (j) of the current three-dimensional map is “1”, “1” is added to the voxel (j) of the corresponding composite map CMn (voting). However, if the corresponding voxel (j) in the current three-dimensional map is “0”, the voxel (j) of the corresponding composite map CMn is not voted.

  In this manner, a plurality of accumulated three-dimensional map data can be combined to generate a combined three-dimensional map.

  The inventors have proposed that a robot 12 is circulated in a commercial facility as shown in FIG. 3 to generate and store a three-dimensional map once or several times daily. In that sense, step S15 in FIG. 8 is unnecessary, but in order to end the process in FIG. 8, it is determined whether or not it is the last in step S15. If “NO”, the process returns to step S1, and “YES” data is present. Then, the synthetic map creation operation is terminated.

  In the above-described embodiment, the composite map is generated by updating the composite map according to the data of the three-dimensional map that is sequentially generated. However, a composite map can be generated using any one of a plurality of three-dimensional maps stored in, for example, the three-dimensional map data storage area 72 acting as storage means. The procedure for generating a composite map using a plurality of arbitrary three-dimensional map data is the same as the procedure described with reference to FIGS. 8 to 10. For example, the earliest three-dimensional map is converted to the first composite map in step S11. Then, the composite map may be updated as in step S13 according to each three-dimensional map.

  For example, if metadata such as a time stamp is added to the 3D map data stored in the 3D map data storage area 72, the user can see it and select an arbitrary 3D map. Then, it is possible not only to simply synthesize a plurality of three-dimensional maps accumulated once a day in time series, but also to easily create a composite map for each specific day of the week, for example. In such a composite map, an object (object) with a large number of votes (hits) from the corresponding voxel in the three-dimensional map is likely to be a stationary object. Because any 3D map exists in that voxel. Conversely, an object (object) with a small number of votes (hits) from the corresponding voxel on the three-dimensional map is likely to be a moving object. For example, a wagon of a wagon sale performed in a passage on a specific day of the week, for example, Sunday, or a signage (signage) for customer incentive is typical.

  Note that the specific numerical values such as the specific materials and dimensions mentioned above are merely examples, and can be appropriately changed according to the needs of the product specifications and the like.

DESCRIPTION OF SYMBOLS 10 ... Three-dimensional map production system 12 ... Mobile robot 14 ... Remote control device 28 ... Three-dimensional laser rangefinder 30 ... Computer 66 ... Map creation program

Claims (4)

Moving body,
An omnidirectional laser rangefinder mounted on the moving body,
2D map generation means for generating a 2D grid map based on a data set obtained by scanning with the omnidirectional laser rangefinder;
A matching means for matching the two-dimensional grid map with a predetermined three-dimensional coordinate system;
Conversion means for converting each point of the data set into a three-dimensional coordinate system based on the position and direction of the moving object;
A three-dimensional map generation system comprising: matching means for matching each point converted into the three-dimensional coordinate system according to a time-series data set; and generation means for writing each matched point to a voxel map to generate a three-dimensional map .   The three-dimensional map generation system according to claim 1, wherein the matching unit performs matching of specific points included in a plane in the three-dimensional coordinate system, and matches the remaining points according to the result. The storage means which accumulate | stores the data of the said 3D map, The synthetic | combination map production | generation means which produces | generates a synthetic | combination 3D map based on the some 3D map accumulate | stored by the said accumulation | storage means is further provided. 3D map generation system. Executed by a computer of a three-dimensional map generation system comprising a moving body and an omnidirectional laser rangefinder mounted on the moving body,
2D map generation means for generating a 2D grid map based on a data set obtained by scanning with the omnidirectional laser rangefinder;
A matching means for matching the two-dimensional grid map with a predetermined three-dimensional coordinate system;
Conversion means for converting each point of the data set into a three-dimensional coordinate system based on the position and direction of the moving object;
3D map generation that functions as a matching means for matching each point converted to the 3D coordinate system according to a time-series data set, and a generating means for writing each matched point to a voxel map to generate a 3D map program.

Images