WO2021111613A1 - Three-dimensional map creation device, three-dimensional map creation method, and three-dimensional map creation program - Google Patents

Abstract

A three-dimensional map creation device (100) includes: a three-dimensional map generation unit (110) that, on the basis of sensor data detected by a sensor that moves on a floor, generates a first three-dimensional map for each region that includes an object of interest; and a map registration unit (120) that acquires a floor map (300) of the floor, and arranges one or more first three-dimensional maps generated by the three-dimensional map generation unit (110) on the floor map (300) to generate a second three-dimensional map that includes the one or more first three-dimensional maps.

Description

3D map creation device, 3D map creation method, and 3D map creation program

The present invention relates to a three-dimensional map creation device, a three-dimensional map creation method, and a three-dimensional map creation program.

A positioning system using WiFi or Beacon is known as a system for detecting a self-position in a large-scale indoor environment such as a factory or a building. However, for example, in an autonomous mobile robot which is a robot mounted on an Automated Guided Vehicle (AGV), there is a demand to detect a posture in addition to a position of an object to be inspected. Further, in the device for detecting the position and orientation of the object, there is a demand to eliminate as much as possible additional equipment for detecting the position and orientation from the viewpoint of introduction cost. As a method of satisfying these demands, a method of using a three-dimensional map is known in an application of an autonomous mobile robot and an application of augmented reality in which contents as virtual visual information are superimposed on an existing landscape.

As a method of creating a three-dimensional map, Simultaneous Localization And Mapping (Simultaneous Localization And Mapping), which estimates the self-position and creates a map at the same time based on sensor data acquired from sensors such as Laser Imaging Detection and Range (LiDAR) and a camera. SLAM) is known.

Japanese Unexamined Patent Publication No. 2014-229020

However, since SLAM accumulates position errors according to the distance traveled, there is a problem that a highly accurate 3D map cannot be created when SLAM is used in a large-scale indoor environment.

Here, FIGS. 1 (A) and 1 (B) show the accumulation of positional errors when the indoor environment is scanned while moving along the floor 201 and the wall surface of the floor 201, which are the target areas for creating a three-dimensional map by SLAM. The scan results 202, which are the appearances, are shown respectively. Further, FIGS. 2A and 2B show a tablet PC (Tablet Personal Computer) 210 that normally superimposes the content 212 on the images 211 of the actual objects (for example, devices) A1 to A3 and abnormally. The tablet PC 210s to be displayed on top of each other are shown.

The present invention has been made to solve the above problems, and provides a three-dimensional map creation device capable of generating a highly accurate three-dimensional map, a three-dimensional map creation method, and a three-dimensional map creation program. The purpose is.

The three-dimensional map creating device according to one aspect of the present invention generates a three-dimensional map for each area including an object based on sensor data detected by a sensor moving on the floor. By acquiring the unit and the floor map of the floor and arranging one or more first three-dimensional maps generated by the three-dimensional map generation unit on the floor map, the one or more first It has a floor map registration unit that generates a second 3D map including the 3D map of.

The three-dimensional map creation method according to another aspect of the present invention includes a step of generating a first three-dimensional map for each area including an object based on sensor data detected by a sensor moving on the floor. A second 3 that includes the one or more first 3D maps by acquiring the floor map of the floor and arranging the generated one or more first 3D maps on the floor map. It has a step of generating a three-dimensional map.

According to the present invention, it is possible to generate a highly accurate three-dimensional map.

(A) is a diagram showing one actual floor which is a target area for creating a three-dimensional map by SLAM, and (B) is a position error when scanning an indoor environment while moving along the wall surface of one floor. It is a figure which shows the scan result which is the state of the accumulation of. (A) is a diagram showing a tablet PC that normally superimposes content as virtual visual information on an image displaying an existing object, and (B) is a diagram showing an image displaying an existing object. It is a figure which shows the tablet PC which displays the content abnormally superimposed. It is a figure which shows the example of the hardware composition of the 3D map making apparatus which concerns on Embodiment 1 of this invention. It is a functional block diagram which shows schematic structure of the 3D map making apparatus which concerns on Embodiment 1. FIG. (A) is a plan view showing an example of a floor map, and (B) is a perspective view showing an example of an object represented by a small-scale first three-dimensional map. (A) is a perspective view showing the rotation of the first three-dimensional map around the xyz axis, and (B) is a plan view showing the rotation of the first three-dimensional map around the normal of the ground. It is a functional block diagram which shows schematic structure of the floor map registration part shown in FIG. It is a figure which shows the example of the floor map in the xyz Cartesian coordinate system. It is a figure which shows the range division processing executed by the range division part shown in FIG. It is a functional block diagram which shows schematic structure of the range division part shown in FIG. It is a figure which shows the screen at the time of the range designation operation using the range designation part of the 3D map making apparatus which concerns on Embodiment 1. FIG. (A) is a flowchart showing a three-dimensional map generation process, and (B) is a flowchart showing a position / orientation estimation process. FIG. 5 is a flowchart showing a floor map registration process executed by the floor map registration unit of the three-dimensional map creation device according to the first embodiment. It is a flowchart which shows the range division processing executed by the range division part of the 3D map making apparatus which concerns on Embodiment 1. FIG. It is a figure for demonstrating the case where the similarity is calculated and the case where it is not calculated. It is a flowchart which shows the similarity determination process which is executed by the range division part of the 3D map making apparatus which concerns on Embodiment 1. FIG. It is a functional block diagram which shows schematic structure of the 3D map making apparatus which concerns on Embodiment 2 of this invention. (A) is a flowchart showing a three-dimensional map generation process, and (B) is a flowchart showing a process for displaying content superimposed. It is a functional block diagram which shows schematic structure of the 3D map making apparatus which concerns on Embodiment 2 of this invention. (A) is a flowchart showing a three-dimensional map generation process, and (B) is a flowchart showing a position / orientation estimation process.

The three-dimensional map creation device, the three-dimensional map creation method, and the three-dimensional map creation program according to the embodiment of the present invention will be described below with reference to the drawings. The following embodiments are merely examples, and various modifications can be made within the scope of the present invention.

The three-dimensional map creating device according to the embodiment is, for example, an autonomous mobile robot having a computer. The three-dimensional map creating device according to the embodiment may be a tablet PC that superimposes a content that is visual information on an image that displays an existing object. However, the three-dimensional map creating device according to the embodiment may be a personal computer or a smartphone that can be moved by the user.

In the present application, in order to facilitate the understanding of the invention, the floor map or the figure showing the object (for example, equipment, equipment, etc.) which is an object on the floor shows the coordinate axes of the xyz orthogonal coordinate system and around each coordinate axis. The direction of rotation is shown. The floor is an example of a reference plane and is generally parallel to the ground. The x-axis and z-axis are coordinate axes parallel to the plane including the floor. The y-axis is a coordinate axis in a direction orthogonal to the plane including the floor. The + Rz direction is a clockwise direction when facing the + z axis direction, and the −Rz direction is a counterclockwise direction which is the opposite direction of the + Rz direction. The + Rx direction is a clockwise direction when facing the + x-axis direction, and the −Rx direction is a counterclockwise direction which is the opposite direction of the + Rx direction. The + Ry direction is a clockwise direction when facing the + y-axis direction, and the −Ry direction is a counterclockwise direction which is the opposite direction of the + Ry direction.

<< 1 >> Embodiment 1
<< 1-1 >> configuration <3D map creation device 100>
FIG. 3 is a diagram showing an example of a hardware (HW) configuration of the three-dimensional map creating device 100 according to the first embodiment. The three-dimensional map creation device 100 is a device capable of implementing the three-dimensional map creation method according to the first embodiment. The three-dimensional map creating device 100 may be a tablet PC that superimposes a content that is visual information on an image that displays an existing object. The actual object is, for example, a device or equipment.

As shown in FIG. 3, the three-dimensional map creating device 100 has a computer 10. The computer 10 has a memory 12 as a storage unit capable of storing a program as software, and a processor 11 as an information processing unit capable of executing a program stored in the memory 12. The program includes a three-dimensional map creation program capable of causing the computer 10 to execute the three-dimensional map creation method according to the first embodiment. The program can be recorded on a recording medium that can be read by the computer 10. The recording medium is, for example, a magnetic disk, an optical disk, a semiconductor memory, or the like.

The three-dimensional map creation device 100 has a three-dimensional map DB 40. The three-dimensional map DB 40 is a storage device in which a database (DB) used for managing the three-dimensional map is stored. However, the three-dimensional map DB 40 may be provided in an external storage device or a server on the network that is communicably connected to the three-dimensional map creation device 100.

The three-dimensional map creating device 100 may have one or more of various sensors such as a distance sensor 21, a camera 22, a gyro sensor 23, an acceleration sensor 24, and a geomagnetic sensor 25. The distance sensor 21 is a sensor that measures a distance using LiDAR, infrared rays, or the like. The camera 22 is a sensor that acquires an image (for example, a color image). The gyro sensor 23 is a sensor that acquires an angular velocity. The acceleration sensor 24 is a sensor that acquires acceleration. The geomagnetic sensor 25 is a sensor that acquires an orientation. The various sensors may be part of the three-dimensional mapping apparatus 100. However, various sensors may be provided in an external device communicably connected to the three-dimensional map creating device 100. For example, various sensors 21 to 25 shown in FIG. 3 may be provided on the AGV, and the three-dimensional mapping device 100 may be configured by a computer 10 placed in a place other than the AGV.

Further, the three-dimensional map creating device 100 has a display 30. The display 30 is a display device for displaying an image. When the three-dimensional map creating device 100 is a tablet PC carried by the user, the display 30 displays an image displaying a real object as shown in FIG. 2A and augmented reality content. .. However, the three-dimensional map creating device 100 may be a device that does not include the display 30.

For example, when performing maintenance and inspection of an object using augmented reality, a user holding a tablet PC, which is a three-dimensional map creation device 100, moves to the front of the object. The three-dimensional map creating device 100 estimates the position and posture of the object based on the sensor data acquired while moving to the object and at the front position of the object. In the case of a tablet PC, the position of the object is considered to be the same as the self-position, which is the position of the user. When performing maintenance and inspection of an object using augmented reality, if a 3D map of the object and its surroundings (that is, a nearby area) is accurately created, a 3D map of a position far from the object is created. Content about the object, whether not done or inaccurate, can be displayed on the display 30 of the tablet PC at an appropriate position on or near the image of the object.

Further, when the maintenance and inspection of the object is performed using the autonomous mobile robot which is the three-dimensional map creation device 100, the autonomous mobile robot moves to the front of the object. The autonomous mobile robot estimates the position and posture of the object based on the sensor data while moving to the object and at the front position of the object. When an autonomous mobile robot performs maintenance and inspection of an object, if the 3D map of the object and its vicinity is created accurately, even if the 3D map of the position far from the object is not created or it is inaccurate. Even so, the object can be operated by a robot hand or the like. That is, the autonomous mobile robot requires a three-dimensional map having high position accuracy around the object, but there is no problem even if the position accuracy of the three-dimensional map is low in the passage used for movement.

In a small environment such as a single device or an area of several meters square, SLAM can be used to create a 3D map with high accuracy. This is because when creating a three-dimensional map of a small-scale environment using SLAM, the accumulation of errors is small, and loop detection is easy. Loop detection is, for example, a process called loop closure performed in SLAM.

The three-dimensional map creating device 100 according to the first embodiment creates a large-scale second three-dimensional map that enables highly accurate estimation of position and posture only in the object and its surroundings. Specifically, the three-dimensional map creating device 100 according to the first embodiment has one or more first three-dimensional maps on a floor map 300 on which a layout including the positions of objects such as equipment to be inspected is drawn. By arranging (ie, a small 3D map), one second 3D map (ie, a large 3D map) is created.

When the 3D map creation device 100 is a tablet PC, when the user carrying the tablet PC moves to the front of the object, the 3D map creation device 100 has one or more thirds in one or more areas of the floor map. By registering each of the three-dimensional maps of No. 1, as shown in FIG. 2A, the content 212 is superimposed and displayed at an appropriate position in the image 211 in which the objects A1 to A3 are displayed.

FIG. 4 is a functional block diagram schematically showing the configuration of the three-dimensional map creating device 100 according to the first embodiment. As shown in FIG. 4, the three-dimensional map creating device 100 generates a first three-dimensional map for each area including an object based on the sensor data detected by the sensor moving on the floor. By acquiring the map generation unit 110 and the floor map of the floor and arranging one or more first three-dimensional maps generated by the three-dimensional map generation unit 110 on the floor map, one or more first ones. It has a floor map registration unit 120 that generates a large-scale second three-dimensional map including the three-dimensional map of the above, and a range division unit 130 that divides the second three-dimensional map into a plurality of ranges. Further, the three-dimensional map creating device 100 may have a range designation unit 140 and a position / orientation estimation unit 150.

<3D map generator 110>
FIG. 5A is a plan view showing an example of the floor map 300, and FIG. 5B is a perspective view showing an example of an object represented by a small-scale first three-dimensional map 400. .. The three-dimensional map generation unit 110 generates one or more first three-dimensional maps by using, for example, SLAM or the like. The first three-dimensional map 400 is a three-dimensional map of an object on the floor and a small area around it. Corresponding areas 301 to 305, which are locations for arranging objects, are drawn on the floor map 300. For example, the first 3D map 400 is fitted to the corresponding area 303 by performing any one or more of rotation, translation, and scale adjustment.

<Floor map registration unit 120>
FIG. 6A is a perspective view showing the rotation of the first 3D map 400 around the xyz axis, and FIG. 6B is a floor map 300 parallel to the ground of the first 3D map 400. It is a top view which shows the rotation around a normal line (that is, the rotation around the y-axis in the ± Ry direction). In the first embodiment, as shown in FIG. 6B, the floor map registration unit 120 acquires the floor map 300 on which the corresponding areas 301 to 305 are drawn, for example, from an external storage device. The floor map registration unit 120 may have a storage unit that stores in advance the floor map 300 on which the corresponding areas 301 to 305 are drawn. Alternatively, the floor map registration unit 120 may acquire the floor map 300 on which the corresponding areas 301 to 305 are drawn from an external storage device or a server on the network according to a user input operation from the operation input unit.

For example, from the floor map 300, the corresponding area 303 reconstructed as a small-scale first three-dimensional map is designated by a user operation. At this time, as shown in FIG. 6 (A), since the three rotation adjustments around the xyz axis are complicated tasks for the user, the floor map registration unit 120 is as shown in FIG. 6 (B). , Automatically select the adjustment around one axis of rotation required for floor map registration (eg, rotation around the y-axis in the ± Ry direction) and make unnecessary adjustments around the axis of rotation (eg, ± Rx around the x-axis). The process of invalidating the rotation in the direction and the rotation in the ± Rz direction around the z-axis may be performed. By adding this process, the user registers the first three-dimensional map (for example, 400) in the designated corresponding area (for example, 303) on the floor map 300 without adjusting the three-dimensional rotation. It is possible. For example, by specifying the rotation angle and the amount of translation of the first three-dimensional map around the normal of the ground by user operation, the floor map registration unit 120 floors one or more first three-dimensional maps. Register in map 300.

FIG. 7 is a functional block diagram schematically showing the configuration of the floor map registration unit 120 shown in FIG. The floor map registration unit 120 includes a ground detection unit 121, an external parameter calculation unit 122, an external parameter input unit 123, and an external parameter action unit 124.

The ground detection unit 121 detects the ground based on a three-dimensional map. As an example of the ground detection method, there is a method using Random Sample Consensus (RANSAC), which is one of the algorithms for robust estimation. The relationship between the coefficient (ab c d) ^T of the plane and the position (x y z) ^T on the plane is expressed by the following equation (1).

In equation (1), "・" indicates the inner product. Since the plane obtained by using RANSAC is an infinite plane, the ground detection unit 121 calculates the range of the plane using a convex hull or the like. The ground detection unit 121 detects the plane having the largest range as the ground.

Since there are cases where the plane having the largest range is not necessarily the ground, the ground detection unit 121 may detect the ground by the following method. For example, the ground detection unit 121 may detect the ground (that is, a horizontal plane) from a plurality of planes based on a user input operation. Alternatively, the ground detection unit 121 may determine the direction of gravity from the acceleration measured by the inertial measurement unit (IMU), and may determine the plane whose normal line is close to gravity as the ground. Alternatively, the ground detection unit 121 may detect the ground using the direction of gravity measured by the IMU and the size of the area of the plane.

_{The external parameter calculation unit 122 calculates the external parameter T 1} from the relationship between the ground and the floor map 300 detected by the ground detection unit 121. First, the external parameter calculation unit 122 obtains the rotation R _{1 from the detected normal line ng of the ground and the normal line n f} of the floor map 300. _ng and n _f are represented by the following equations (2) and (3).

It is assumed that the floor map 300 is shown in the xyz Cartesian coordinate system as shown in FIG. External parameter calculation unit 122, a rotation _{R 1,} using the polarization angle theta _z for z-direction of _{n f} shown in declination theta _x and equation (3) with respect to the x-direction of _{n g} represented by the formula (2) To calculate. Assuming that the rotation matrices obtained by using the declinations θ _x and θ _{z are R x} and R _z , respectively, these are represented by the equation (4), and the rotation R ₁ is represented by the equation (5).

を計算する。 The external parameter calculation unit 122 obtains the rotation R _{1 by the equation (5), and then uses the vector x p} of one point on the plane.

To calculate.

When the h _y and translation amount is y-coordinate, corresponding to the height direction, the external parameter T ₁ from the rotation R ₁ and vector t _1, represented by the following equation (6).

In the equation (6), the vector t ₁ is a vector obtained by extracting only the rotation R ₁ and the vector x _{p in} the height direction, and is expressed by the following equation (7).

The external parameter input unit 123 accepts user input. Some external parameters are input by user input in the external parameter input unit 123. _{The external parameters input to the external parameter input unit 123 are, for example, the rotation R 2} around the y-axis in the coordinate system of the floor map 300 shown in FIG. 8 and the translation amount t ₂ in the xz plane. From these, the external parameter input unit 123 _{acquires the external parameter T 2} by the following equation (8). Further, assuming that the user input is the rotation angle θ _y , the rotation R ₂ is expressed by the following equation (9).

In FIG. 8, t ₂ is a translation amount with reference to the lower left point of the floor map 300 (that is, the origin).

The rotation angle θ around the y-axis may be acquired by using the geomagnetic sensor 25. The value of the angle θ is automatically input from the geomagnetic sensor 25. Alternatively, the angle θ may be input by the user with the detection value of the geomagnetic sensor 25 as the initial value.

The input method of the angle θ and the translation amount t ₂ may be either the operation of the Graphics User Interface (GUI) or the numerical input from the keyboard or the like. Further, the origin of the translation amount t ₂ may be a point different from the lower left point of the floor map shown in FIG.

The external parameter action unit 124 causes the external parameter calculated by the external parameter calculation unit 122 and the external parameter input from the external parameter input unit 123 to act on the three-dimensional map. The external parameter action performed on the point cloud managed by the three-dimensional map is shown in the following equation (10). Here, the vector p indicates one point in the point cloud before the action, and the vector p'indicates the point after the action.

<Range division 130>
When a plurality of objects having the same shape and the same pattern are lined up on the floor map, the estimation of the position and orientation of the target objects may fail. For example, in an environment where a plurality of objects B1 to B6 having the same shape and the same pattern are lined up, when the position and posture are estimated by scanning the object B1, the position and position of any of the objects B2 to B6 and Posture may be output. In order to avoid erroneous estimation, it is desirable to first specify the range in which the user is when estimating the position and posture. By designating the range, it is possible to estimate the detailed position and posture based on the three-dimensional map 400 of the object included in the range.

FIG. 9 is a diagram showing a range division process executed by the range division unit 130 shown in FIG. The range dividing unit 130 divides one floor represented by the floor map 300 into a plurality of ranges (for example, ranges # 1 to # 4). For example, the range dividing unit 130 statically divides one floor of a building into a plurality of ranges # 1 to # 4, and manages a three-dimensional map in each of the plurality of ranges # 1 to # 4. The range dividing unit 130 divides one floor map 300 into four ranges # 1 to # 4, for example, as shown in FIG. In this way, if one floor map 300 is divided into a plurality of ranges and the three-dimensional map is managed so that a plurality of objects having the same pattern and the same shape belong to different ranges, the target object can be obtained. , The possibility of falsely presuming that it is another object with the same shape and the same pattern is reduced. However, in this case, the possibility of erroneous estimation cannot be sufficiently reduced.

For this reason, it is desirable that the range dividing unit 130 dynamically divides the range. FIG. 10 is a functional block diagram schematically showing the configuration of the range dividing portion 130 shown in FIG. The range dividing unit 130 includes an image feature calculation unit 131 and a clustering unit 132.

The image feature calculation unit 131 calculates the features of the image based on the similarity of the images taken by the camera when generating the three-dimensional map. The image feature calculation unit 131 performs a process for range division using, for example, Bag of Words (BoW). The image feature calculation unit 131 vectorizes an image taken by each camera of the first three-dimensional map of the object using BoW. The image feature calculation unit 131 determines that the vectors are "similar" if they are close to each other.

The clustering unit 132 performs clustering based on the result of the image feature calculation unit 131 and divides the range. That is, the clustering unit 132 performs the range division process so that these vectors belong to different ranges.

<Range specification unit 140>
FIG. 11 is a diagram showing a screen at the time of a range designation operation using the range designation unit 140 of the three-dimensional map creation device 100 according to the first embodiment. The range designation unit 140 is determined by the range division unit 130 and designates one range from a plurality of ranges. The range designation unit 140 specifies, for example, one range selected by a user operation from the user operation unit. The user operation unit is a device connected to the range designation unit 140 or a part of the range designation unit 140. The user operation unit is, for example, a touch panel of a tablet PC as shown in FIG. When the user selects one range from the plurality of ranges displayed on the touch panel by tap operation or the like, the range designation unit 140 specifies the selected range.

<Position and orientation estimation unit 150>
The position / orientation estimation unit 150 estimates the position and orientation of the object using the three-dimensional map included in the range designated by the range designation unit 140. As an example of the technique for estimating the position and the posture, there is a method of combining BoW and Perceptive N Points (PnP).

<< 1-2 >> Operation FIG. 12A is a flowchart showing a three-dimensional map generation process, and FIG. 12B is a flowchart showing a position / orientation estimation process. First, as shown in FIG. 12A, the three-dimensional map generation unit 110 generates a small-scale first three-dimensional map for each object (steps S11 and S12). Next, the floor map registration unit 120 registers the first three-dimensional map in the floor map 300 (step S13). The processes of steps S11 to S13 are repeated for the number of objects, and as a result, a large-scale second three-dimensional map is generated. After the registration of all the objects is completed, the range dividing unit 130 divides the second three-dimensional map into a plurality of ranges (step S14).

Next, as shown in FIG. 12B, the position and posture are estimated using the three-dimensional map. First, the range designation unit 140 selects a designated range, which is an approximate range in which the user is present, based on the user operation (step S21). After that, the position / orientation estimation unit 150 estimates the position and orientation using the three-dimensional map within the designated range (step S22).

FIG. 13 is a flowchart showing a floor map registration process (step S13) executed by the floor map registration unit 120 of the three-dimensional map creation device 100. First, the ground detection unit 121 detects the ground (steps S131 to S135). For example, the ground detection unit 121 detects the ground by detecting a plurality of planes (steps S131 to S133) and then finding the maximum plane that can be regarded as the ground (steps S134, S135). After that, the external parameter calculation unit 122 calculates the movement amount using the relationship between the ground and the floor map 300, and the external parameter input unit 123 is based on the parallel movement amount input by the user operation and the rotation amount around the ground normal. The amount of movement is obtained (steps S136 and S137). Next, the external parameter action unit 124 causes the external parameters, which are the two movement amounts, to act on the three-dimensional map (step S138).

FIG. 14 is a flowchart showing a range division process executed by the range division unit 130 of the three-dimensional map creation device 100. First, the image feature calculation unit 131 calculates the image features using BoW or the like, and when the image features are similar to each other, adds this information to the rule. The index of the first loop (step S141) of the three-dimensional map is i (where i is an integer of 0 or more and N or less), and the index of the second loop (step S143) is j (where j is 0 or more and N). (The following integers), the image feature calculation unit 131 calculates the similarity of the three-dimensional map when i <j (steps S141 to S147). FIG. 15 is a diagram for explaining a case where the similarity is calculated and a case where the similarity is not calculated. That is, the similarity is calculated when it is white in FIG. 15, and the similarity is not calculated when it is in the shaded area.

FIG. 16 is a flowchart showing a similarity determination process executed by the range dividing unit 130 of the three-dimensional map creating device 100. As shown in FIG. 16, the image feature calculation unit 131 of the range dividing section 130 first calculates the distance between the two image features. Assuming that the image features here are vector v ₁ and vector v ₂ , the range dividing unit 130 uses, for example, the Euclidean distance represented by the following equation (11) as the distance between the two image features.

The image feature calculation unit 131 calculates the calculation of the distance between the two image features (step S1403), and repeats the process of obtaining the minimum distance (steps S1404 and S1405) N (j) times, which is the number of three-dimensional maps (step). ST1401), and further, the processing of steps S1401 to S1404 is repeated N (i) times, which is the number of three-dimensional maps (step ST1400). After obtaining the minimum distance of the image feature, the image feature calculation unit 131 determines that the minimum distance is less than α (true in step S1405), determines that it is similar, and ends the process, and if the minimum distance is α or more (step). In S1405, false), it is determined that they are dissimilar, and the process is terminated. Here, α is a preset value, and is, for example, a parameter set by a developer, a user, or the like.

After the rule is determined by the above process, the clustering unit 132 executes clustering based on the rule (steps S148 to S151 in FIG. 14). FIG. 14 shows an example in which clustering is performed using divided clustering, which is hierarchical clustering. First, the clustering unit 132 divides the range of the three-dimensional map to be divided. Here, the three-dimensional maps to be divided are C1 and C2. In the clustering unit 132, other 3D maps near the center position of the 3D map C1 belong to the C1 class, and other 3D maps near the center position of the 3D map C2 belong to the C2 class. , Perform range division. The clustering unit 132 repeats such a range division of the three-dimensional map to perform the final range division.

<< 1-3 >> Effect As described above, the three-dimensional map creating device 100 according to the first embodiment creates one or more small-scale first three-dimensional maps using SLAM, and creates a floor map. By arranging one or more first three-dimensional maps on the 300, a large-scale second three-dimensional map (for example, a three-dimensional map of the entire indoor environment) is created. Since the position error accumulated by SLAM when creating the first three-dimensional map is small, the position error in the second three-dimensional map is suppressed. Therefore, the three-dimensional map creating device 100 according to the first embodiment can produce a large-scale three-dimensional map having a small position error.

<< 2 >> Embodiment 2
<< 2-1 >> Configuration FIG. 17 is a functional block diagram schematically showing the configuration of the three-dimensional map creating device 101 according to the second embodiment. In FIG. 17, components that are the same as or correspond to the components shown in FIG. 4 are designated by the same reference numerals as those shown in FIG. The three-dimensional map creation device 101 according to the second embodiment is different from the three-dimensional map creation device 100 according to the first embodiment in that it has a content registration unit 160 and a content superimposition display unit 170. The HW configuration of the three-dimensional map creating device 101 is the same as that shown in FIG.

The content registration unit 160 registers the position of the content to be displayed on the image displaying each object by using the first three-dimensional map of each of the one or more objects. The content superimposition display unit 170 superimposes the registered content on the image taken by the camera 22 and displays it on the display 30. The content can include, for example, information indicating a figure such as a line or a plane and an object, information indicating a three-dimensional figure such as a cube or a sphere and an object, or a combination thereof.

<< 2-2 >> Operation FIG. 18 (A) is a flowchart showing a three-dimensional map generation process performed by the three-dimensional map creation device 101, and FIG. 18 (B) shows a content superimposed display by the three-dimensional map creation device 101. It is a flowchart which shows the process to perform for this. In FIGS. 18A and 18B, the same processing steps as those in FIGS. 12A and 12B are designated by the same reference numerals as those in the processing steps of FIGS. 12A and 12B. As shown in FIG. 18A, the three-dimensional map creation device 101 according to the second embodiment is a content registration process (step S15) for registering the position of the content to be displayed superimposed on the image displaying each object. This is different from the three-dimensional map creating device 100 according to the first embodiment. Further, as shown in FIG. 18B, the three-dimensional map creation device 101 according to the second embodiment displays the content registered in the content registration process by superimposing it on the image displaying each object. (Step S23) is different from the three-dimensional map creating device 100 according to the first embodiment.

<< 2-3 >> Effect As described above, the three-dimensional map creating device 101 according to the second embodiment creates one or more small-scale first three-dimensional maps using SLAM, and creates a floor map. By arranging one or more first three-dimensional maps on the 300, a large-scale second three-dimensional map (for example, a three-dimensional map of the entire indoor environment) is created. Since the position error accumulated by SLAM when creating the first three-dimensional map is small, the position error in the second three-dimensional map is suppressed. Therefore, the three-dimensional map creating device 101 according to the second embodiment can superimpose and display the contents at appropriate positions as shown in FIG. 2 (A).

Regarding points other than the above, the second embodiment is the same as the first embodiment.

<< 3 >> Embodiment 3
<< 3-1 >> Configuration FIG. 19 is a functional block diagram schematically showing the configuration of the three-dimensional map creating device 102 according to the third embodiment. In FIG. 19, components that are the same as or correspond to the components shown in FIG. 4 are designated by the same reference numerals as those shown in FIG. The three-dimensional map creation device 102 according to the third embodiment is different from the three-dimensional map creation device 100 according to the first embodiment in that it has a relative movement distance estimation unit 180. The HW configuration of the three-dimensional map creating device 102 is the same as that shown in FIG.

As described in the first embodiment, the position / orientation estimation unit 150 estimates the position and orientation of the object based on a small-scale first three-dimensional map in and around the object. However, in order to estimate the position and orientation of an object existing at a location away from the first three-dimensional map, it is necessary to obtain the relative movement distance from the location where the estimation of the position and orientation is successful. The three-dimensional map creation device 102 according to the third embodiment has a function of calculating the relative movement distance by providing the relative movement distance estimation unit 180.

<< 3-2 >> Operation FIG. 20A is a flowchart showing a three-dimensional map generation process performed by the three-dimensional map creation device 102, and FIG. 20B is a position / orientation estimation performed by the three-dimensional map creation device 102. It is a flowchart which shows the process. In FIGS. 20A and 20B, the same processing steps as those in FIGS. 12A and 12B are designated by the same reference numerals as those in the processing steps of FIGS. 12A and 12B. As shown in FIG. 20B, the three-dimensional map creating device 102 according to the third embodiment relates to the first embodiment in that it performs a process (steps S24, S25) for obtaining a relative moving distance. It is different from the three-dimensional map creating device 100.

For example, the following method can be used to estimate the relative travel distance. The first method is a method using SLAM. This is a method of obtaining the movement amount by using the image taken by the camera, the sensor data detected by the distance sensor 21, or both of them. This is, for example, a method of obtaining the movement amount by sequentially integrating the movement amounts between two frames arranged in the time direction.

The second method is dead reckoning using any one or more of the gyro sensor 23, the acceleration sensor 24, and the geomagnetic sensor 25 on the image taken by the camera and the sensor data detected by the distance sensor 21. It is a combined method. Dead reckoning is a method of obtaining a movement amount using a gyro sensor 23, an acceleration sensor 24, a geomagnetic sensor 25, or the like. In this method, the moving speed and the moving direction are obtained by integrating the acceleration and the gyro sensor 23, and the moving distance is obtained by integrating the speeds.

The third method is a method using pedestrian dead reckoning. The third method is a method of obtaining the stride length and the number of steps from the sensor data of the gyro sensor 23 and the acceleration sensor 24, and obtaining the moving distance from these.

<< 3-3 >> Effect As described above, the three-dimensional map creating device 102 according to the third embodiment creates one or more small-scale first three-dimensional maps using SLAM, and creates a floor map. By arranging one or more first three-dimensional maps on the 300, one large-scale second three-dimensional map (for example, a three-dimensional map of the entire indoor environment) is created. Since the position error accumulated by SLAM when creating the first three-dimensional map is small, the position error in the second three-dimensional map is reduced. Therefore, the three-dimensional map creating device 102 according to the third embodiment can superimpose and display the contents at appropriate positions as shown in FIG. 2 (A). Further, the position and the posture can be estimated even at a place away from the first three-dimensional map.

Regarding points other than the above, the third embodiment is the same as the first embodiment. It is also possible to provide the relative movement distance estimation unit 180 in the configuration of the second embodiment.

10 computers, 11 processors, 12 memories, 21 distance sensors, 22 cameras, 23 gyro sensors, 24 acceleration sensors, 25 geomagnetic sensors, 30 displays, 40 3D map DB, 100, 101, 102 3D map creation devices, 110 3 Dimension map generation unit, 120 floor map registration unit, 121 ground detection unit, 122 external parameter calculation unit, 123 external parameter input unit, 124 external parameter action unit, 130 range division unit, 140 range specification unit, 150 position / orientation estimation unit, 160 content registration unit, 170 content superimposition display unit, 180 relative movement distance estimation unit, 300 floor map.

Claims (9)

A 3D map generator that generates a first 3D map for each area including an object based on sensor data detected by a sensor moving on the floor.
By acquiring the floor map of the floor and arranging the one or more first three-dimensional maps generated by the three-dimensional map generation unit on the floor map, the one or more first three dimensions are obtained. A floor map registration unit that generates a second 3D map including a map,
A three-dimensional map making device characterized by having. The three-dimensional map creating device according to claim 1, further comprising a range dividing portion for dividing the second three-dimensional map into a plurality of ranges. A range specification unit that specifies any of the plurality of ranges as a specified range, and
Position and orientation for estimating the position and orientation of an object existing in the designated range using the first three-dimensional map arranged within the designated range of the one or more first three-dimensional maps. Estimator and
The three-dimensional map creating apparatus according to claim 2, further comprising. The floor map registration department
A ground detection unit that detects a plurality of planes and detects the ground corresponding to the floor from the plurality of planes.
An external parameter calculation unit that calculates a first external parameter based on the amount of movement of the sensor using the ground.
An external parameter input unit that generates a second external parameter based on the movement amount acquired by user input,
The invention according to any one of claims 1 to 3, further comprising an external parameter action unit that corrects the second three-dimensional map based on the first external parameter and the second external parameter. The described three-dimensional map making device. The range dividing portion is
An image feature calculation unit that calculates image features based on the similarity of features of the first three-dimensional map, and an image feature calculation unit.
A clustering unit that divides the range based on the similarity, and
The three-dimensional map making apparatus according to claim 2 or 3, wherein the three-dimensional map making apparatus is characterized by having. Using the first three-dimensional map for each area including the object, a content registration unit that registers the position of the content to be displayed overlaid on the image displaying each object, and the content registration unit.
The content is superimposed on the image taken by the camera and displayed on the display.
The three-dimensional map creating apparatus according to any one of claims 1 to 5, further comprising. The invention according to any one of claims 1 to 6, further comprising a relative movement distance estimation unit that calculates a relative movement distance to an object existing at a location away from the first three-dimensional map. 3D mapping device. Based on the sensor data detected by the sensor moving on the floor, the step of generating the first 3D map for each area including the object, and
A second 3 that includes the one or more first 3D maps by acquiring the floor map of the floor and arranging the generated one or more first 3D maps on the floor map. Steps to generate a dimensional map and
A three-dimensional map creation method characterized by having. On the computer
A process to generate a first 3D map for each area including an object based on the sensor data detected by the sensor moving on the floor.
A second 3 that includes the one or more first 3D maps by acquiring the floor map of the floor and arranging the generated one or more first 3D maps on the floor map. The process of generating a dimensional map and
A three-dimensional map creation program characterized by executing.

Images