Information processing device, information processing method, and program

Abstract

To generate accurate map data through loop close processing even when position on a real space between map elements which have detected a loop (that a mobile body has arrived at the vicinity of a certain spot a plurality of times) is close.SOLUTION: A map element group is selected which includes one or more map elements which have sensor information having measured environment partially common to that of sensor information included in a map element used when detecting that a mobile body has arrived at the vicinity of a certain spot a plurality of times, and which are on a movement route at each time, the sensor information included in the selected map element group is used to acquire one or more pairs of relative position postures of the map element group, the relative position posture of the sensor is acquired between the map elements corresponding to the movement route on which the mobile body has arrived at the vicinity of the certain spot a plurality of times, and the acquired two types of relative position postures are used to correct position posture information of the sensor included in the map element corresponding to the movement route on which the mobile body has arrived at the vicinity of the certain spot a plurality of times.SELECTED DRAWING: Figure 4

Description

Technical Field

[0001] The present invention relates to a technique for creating three-dimensional map information of an environment in which a moving object moves.

Background Art

[0002] In factories and logistics warehouses, autonomous moving objects such as Automated Guided Vehicles (AGVs) are used. Further, as a method for estimating the position and orientation of such an unmanned transport vehicle and creating electronic map data used for the estimation, a SLAM (Simultaneous Localization and Mapping) technique using a camera or a laser range scanner (laser range finder, Laser Imaging Detection and Ranging (LIDAR)) as a sensor is known. For position and orientation estimation, a three-dimensional map of the environment using image features detected from image information and three-dimensional position information of the image features calculated therefrom as map elements is used.

[0003] In Non-Patent Document 1, in the process of generating map data by moving a moving object equipped with a sensor, the correspondence of measurement points at positions close in real space is recognized from the information of each measurement point acquired by the sensor, and based on the result, the position and orientation deviation in the map data is corrected. The so-called loop closure technique is disclosed.

Prior Art Documents

Non-Patent Documents

[0004]

Non-Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0005] However, in the method described in Non-Patent Document 1, the relative position and orientation of map elements between the measurement points where a loop is detected are calculated only between the measurement points with close positions in the real space. Therefore, the accuracy is not sufficient, and there is a limit to the accuracy when performing position and orientation estimation using map data (three-dimensional map information) on which loop closure processing has been performed.

[0006] The present invention has been made in view of the above problems, and an object thereof is to generate map data capable of realizing highly accurate position and orientation estimation.

Means for Solving the Problems

[0007] In order to solve the above problems, an information processing apparatus according to the present invention has the following configuration. Sensor information obtained by measuring the environment with a sensor mounted on a moving body and position and orientation information of the sensor estimated from the sensor information are used as map elements, and acquisition means for acquiring three-dimensional map information composed of a plurality of the map elements corresponding to the movement path of the moving body, detection means for detecting that the moving body has reached a vicinity of a certain point a plurality of times based on the sensor information included in the map elements, and when the detection means detects that the moving body has reached a vicinity of a certain point a plurality of times, the Two map element each of having sensor information that partially shares the environment being measured with the sensor information included in the map element, and When each of the map elements in the vicinity of each of the two map elements is used as its own movement route, each including one or more map elements on the movement path of while including one or more map elements other than the two map elementsSelection means for selecting a group of map elements, first position and orientation acquisition means for acquiring the relative position and orientation of one or more sets of the group of map elements using the sensor information included in the group of map elements selected by the selection means, second position and orientation acquisition means for acquiring the relative position and orientation of the sensors between adjacent map elements corresponding to a movement path that the moving body has reached a plurality of times in the vicinity of a certain point, and correction means for correcting the position and orientation information of the sensors included in the map elements corresponding to the movement path that the moving body has reached a plurality of times in the vicinity of a certain point using the relative position and orientation acquired by the first and second position and orientation acquisition means.

Advantages of the Invention

[0008] According to the present invention, it is possible to generate map data (three-dimensional map information) capable of realizing highly accurate position and orientation estimation.

Brief Description of the Drawings

[0009]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Figure 8

Figure 9

Figure 10

Figure 11

Figure 12

Mode for Carrying Out the Invention

[0010] Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. Note that the following embodiments do not limit the scope of the claims of the present invention, and not all combinations of the features described in the following embodiments are essential for constituting the present invention.

[0011] [First Embodiment] Hereinafter, an information processing apparatus, an information processing method, and a computer program according to a first embodiment of the present invention will be described in detail with reference to the drawings.

[0012] In the first embodiment, an example will be described in which map data (hereinafter, referred to as "map data" as a term synonymous with "three-dimensional map information") that can be used for position and orientation estimation and autonomous driving in the environment by a moving body equipped with sensors is corrected on an information processing apparatus independent of the moving body. In this embodiment, a grayscale camera is used as an imaging device as the sensor.

[0013] Here, the environment is the three-dimensional space of the area where the sensor has moved and its surroundings, and the position and orientation is a six-degree-of-freedom value obtained by combining three-dimensional position information and three-degree-of-freedom orientation information. The position and orientation in the three-dimensional space can be represented by a 4×4 affine matrix, and among the properties of the affine transformation, only rotation and translation are used. When there are two affine matrices A and B indicating the position and orientation, an affine matrix d indicating the relative position and orientation (position and orientation difference, relative position and orientation) between A and B can be obtained by multiplying B by the inverse matrix of A. Similarly, the position and orientation B can be obtained by integrating the relative position and orientation d with the position and orientation A. Note that "relative position and orientation" and "relative position and orientation" are synonymous terms.

[0014] (Map data) The map data (3D map information) to be corrected in this embodiment is composed of one or more measurement points (hereinafter, "measurement points" are used as terms synonymous with "map elements" and "key frames"). Each measurement point has sensor information (image data captured by a sensor) as information for position and orientation measurement and loop detection, and the position and orientation information of the sensor. When creating the map data, each measurement point is arranged at a predetermined interval along the movement path. The interval between the measurement points is set to a distance at which a necessary and sufficient baseline length can be obtained when performing stereo measurement or bundle adjustment using the sensor information of both sensors for features in the environment that are commonly observed by the sensors of adjacent measurement points. Features in the environment are, for example, stationary objects having features observable by sensors such as walls and ceilings. This depends on the distance to the features in the environment and the amount of features, but in this embodiment, it is assumed that the length of the necessary and sufficient baseline is 1 m, and one measurement point is arranged every approximately 1 m that the movement path advances.

[0015] (Bundle adjustment) The bundle adjustment process is used when solving the problem of estimating the parameters of a geometric model from the correspondence relationships of image feature points extracted between multiple images, and is a method for numerically solving a non-linear optimization problem. In the bundle adjustment process, based on the correspondence relationships of each feature point between the input images, the 3D coordinates of each feature point are calculated. Then, the 3D coordinates of each calculated feature point are reprojected onto the image plane, and the reprojection error calculated as the distance between the reprojected point and the feature point is calculated. By repeatedly re-estimating so that this reprojection error becomes smaller, more accurate 3D coordinate values of the feature points and the position and orientation of the images (measurement points) are estimated.

[0016] (Drift and loop closure) Here, the relationship between the position and orientation error (drift) that occurs during the generation of map data and the loop closure process will be described. The generation of map data is repeatedly performed by generating measurement points and estimating the current position and orientation using the measurement points. Therefore, the position and orientation error that occurs during the generation of measurement points becomes the error in the subsequent position and orientation estimation using that measurement point, and the error accumulates each time the generation of measurement points is repeated. This accumulated error is called drift.

[0017] Figure 1 is a diagram schematically showing the relationship between the measurement points generated in the map data and the true movement path of the moving object on a plane. The solid line in the figure is the trajectory of the position and orientation estimation result, and the dotted line is the trajectory of the true position and orientation of the moving object. The white circles in the figure are measurement points. The moving object starts from measurement point A and generates measurement points B to K while moving clockwise in the environment. When generating measurement point I, it passes near measurement point C in the real space. That is, it reaches a point that it has reached in the past again. However, due to the influence of the accumulated position and orientation error (drift) for each arrangement of measurement points, the measurement points at or near measurement point I are arranged in a relative position and orientation including an error when viewed from the measurement points at or near measurement point C. A' to K' in Figure 2 are the position and orientation of each measurement point when there is no drift and each measurement point is ideally arranged.

[0018] To correct the accumulated error, when the moving object revisits a certain point on the map data (loop of the movement path), the process of correcting the position and orientation of the moving object and the elements on the map data is loop closure. In the examples of Figures 1 and 2, a loop is detected between measurement points C and I, and the position and orientation of the corrected measurement point I approximately becomes I'. The position and orientation of the measurement points other than measurement points C and I are corrected so that the consistency with the relative position and orientation of each measurement point can be achieved.

[0019] Here, in the conventional method, the relative position and orientation between measurement points C and I are calculated based on the correspondence of features observed by the sensor information of both sides and stereo measurement. Further, when it is determined that the positions of measurement points C and I in the real space are sufficiently close from the calculated relative position and orientation, a loop is detected. However, if the distance between measurement points C and I in the real space is too close at this time, the baseline length between measurement points C and I cannot be obtained sufficiently, and the accuracy of the relative position and orientation between measurement points C and I may decrease. In this case, the correction results of the position and orientation of each measurement point other than measurement points C and I within the loop also become inaccurate.

[0020] (Configuration of the information processing device) FIG. 3 is a diagram showing an example of the configuration of the information processing device 301 according to the present embodiment. The information processing device 301 has the functions of a general embedded PC device, and is composed of a CPU 311, a ROM 312, a RAM 313, a storage unit 314 such as an HDD or an SSD, a communication unit 315, and a system bus 316.

[0021] The CPU 311 uses the RAM 313 as a work memory, executes an operating system (OS) and various computer programs stored in the ROM 312, the storage unit 314, etc., and controls each unit via the system bus 316. For example, the programs executed by the CPU 311 include programs for executing the processes described later.

[0022] Further, the information processing device 301 is connected to the mobile body 302 through the communication unit 315. The mobile body 302 has sensors and moving means, and has a function of creating map data while traveling in the environment based on a user's instruction and transmitting it to the information processing device 301. The information processing device 301 stores the map data received from the mobile body 302 in the storage unit 314. The mobile body is, for example, an AMR (autonomous mobile robot), an AGV (automatic guides vehicle), an autonomous driving vehicle, a vacuum cleaner robot, a delivery robot, a drone, or the like.

[0023] (Logical configuration of the information processing device) Next, the logical configuration of the information processing apparatus according to this embodiment will be described. The processing of each part shown below is implemented as software by reading a computer program from the ROM 312 or the like onto the RAM 313 and then executing the program by the CPU 311. FIG. 4 is a block diagram showing the logical configuration of the information processing apparatus 301.

[0024] The map acquisition unit 401 acquires map data from the storage unit 314 and expands it on the RAM 313.

[0025] The loop detection unit 402 compares two measurement points on the map data and detects a loop in the movement path of the sensor. Specifically, first, the similarity of the sensor information (images) of the two measurement points is calculated using a known Bag-of-Words (BoW) model, and it is determined whether the similarity is equal to or greater than a certain value. Note that although the expression "loop" is used, it detects not only the case where the moving object makes a circular movement, but also the case where the moving object reaches (revisits) near the same point multiple times in general. The movement path during this period may include straight movement, curvilinear movement, or turning movement, and may also include a mixture of forward, backward, and lateral movement. Also, when the moving object revisits near the same point, it is not only the case where the moving direction of the moving object is opposite, but also the case where the extension lines of the movement paths to which the respective measurement points belong intersect, such as the measurement points C and I in FIG. 1.

[0026] After that, the loop detection unit 402 extracts image feature points from the images of both measurement points, and estimates the relative position and orientation between the measurement points (hereinafter, in this embodiment, "between measurement points" is used as a term synonymous with "between map elements") from the distribution of the corresponding feature points on both images. When it is determined that the relative position and orientation have been successfully calculated with a certain level of accuracy or higher, the loop detection unit 402 determines that a loop has occurred between the two measurement points. In the example of FIG. 1, a loop is detected between the measurement points C and I.

[0027] However, due to the reasons described above, the accuracy of the relative position and orientation between the measurement points calculated by the loop detection unit 402 here may not be sufficient for performing high-precision map correction processing. Therefore, the information processing apparatus has the following configuration for calculating the relative position and orientation of the measurement points around the loop detection location with high precision.

[0028] The selection unit 403 selects a plurality of measurement points to be used by the first relative position and orientation acquisition unit 404 from the two measurement points detected by the loop detection unit 402 and the measurement points in the vicinity thereof on the movement path. At this time, the selection unit 403 selects at least one or more measurement points from each movement path (each movement path to which the two measurement points detecting the loop belong) that reaches the point forming the loop. And the measurement points are selected so that at least one measurement point other than the measurement points detected by the loop detection unit 402 is included. Taking FIG. 1 as an example, when a loop is detected between measurement points C and I, one or more are selected from the movement path of measurement points A to E, and one or more are selected from the movement path of measurement points F to K, and a plurality of measurement points are selected such that at least one measurement point other than measurement points C and I is included therein.

[0029] Based on the sensor information of the plurality of measurement points selected by the selection unit 403, the first relative position and orientation acquisition unit 404 calculates the relative position and orientation between the measurement points for one or more sets among the plurality of measurement points selected by the selection unit 403. At this time, at least one or more sets of measurement points are sets of measurement points selected from different movement paths that reach the point forming the loop.

[0030] The method for calculating the relative position and orientation of each set is the same as that performed between measurement points by the conventional method, and uses the association of feature points on the image observed by both sensor information and stereo measurement. As an example, a known FAST (Features from Accelerated Segment Test) algorithm is used for the extraction of the feature points on the image described above.

[0031] The second relative position and orientation acquisition unit 405 calculates the relative position and orientation between two measurement points based on the position and orientation information of any two measurement points on the map data. The relative position and orientation between adjacent measurement points on the entire movement path on the map are calculated.

[0032] The position and orientation correction unit 406 corrects the position and orientation information of each measurement point based on the relative position and orientation between the measurement points acquired by the first relative position and orientation acquisition unit 404 and the second relative position and orientation acquisition unit 405, and the position and orientation of one or more reference measurement points. This correction is performed so that the error between the acquired relative position and orientation information and the relative position and orientation between the measurement points after correction becomes small.

[0033] (Map data correction process) Next, a method for correcting map data according to the information processing method using the information processing apparatus of the present embodiment will be described with reference to the flowcharts of FIGS. 5 to 7. Hereinafter, it is assumed that the flowchart is realized by the CPU executing a control program.

[0034] In step S501, the map acquisition unit 401 acquires map data from the storage unit 314 and expands it on the RAM 313. Hereinafter, the following description will be made assuming that the map data in the state shown in FIG. 1 is read.

[0035] Step S502 is loop control for repeatedly performing the processes from S503 to S506 hereinafter for all pairs of two measurement points on the map data. Regarding the processes from S503 to S506 hereinafter, the case where the process is performed for the pair of measurement points C and I in the iteration will be described as an example.

[0036] In step S503, the loop detection unit 402 determines whether or not the pair of measurement points of interest forms a loop. Here, the sensor information of measurement points C and I is compared and the relative position and orientation are calculated to detect that a loop has occurred between the two measurement points. The relative position and orientation calculated here may have low accuracy and are not used in the subsequent processes.

[0037] Step S504 is a branch based on whether a loop was detected in step S503. If a loop was detected, the subsequent processes of S505 and S506 are performed. If no loop was detected, the process returns to S502 to process the next set of measurement points. Here, since a loop is detected between measurement points C and I, the process proceeds to S505.

[0038] In step S505, the selection unit 403 selects a group of measurement points for calculating the high-precision relative position and orientation between measurement points from both sides of the movement path forming the loop. FIG. 6 is an example of the measurement point selection result in the present embodiment.

[0039] First, the selection unit 403 selects the measurement points where the loop was detected and the measurement points adjacent to them in each movement path. In the example of FIG. 6, the measurement points C, I where the loop was detected and the measurement points B, D, H, J adjacent to them are selected. After that, for each movement path, it is determined whether the longest distance between the selected measurement points exceeds a predetermined distance. The predetermined distance is twice the length of the necessary and sufficient baseline length (here 2 m). If it does not meet the predetermined distance, additional measurement points adjacent to the already selected measurement points are selected until the predetermined distance is exceeded. In the example of FIG. 6, on the movement path on the C side of the measurement point, since the distance between measurement points B and D is less than 2 m, measurement point A is additionally selected. Since the distance between measurement points A and D exceeds 2 m, the selection on the movement path on the C side of the measurement point is completed. Similarly, on the movement path on the C side of the measurement point, measurement point G is additionally selected. Finally, the selection unit 403 selects measurement points A to D and G to J to complete this process. Thereby, a group of measurement points is selected such that the maximum distance between the measurement points selected by the selection unit 403 is equal to or greater than the distance value of the baseline length. Also, these groups of measurement points share a part of the measured environment. Note that as long as measurement points with a distance exceeding the baseline length can be selected, measurement points E and K are not selected. This is because if the number of selected measurement points increases more than necessary, the subsequent processing load will increase.

[0040] At this time, the true relative position and orientation between measurement points C and I are unknown. However, since the distance between measurement points A and D exceeds 2 m, it is guaranteed that a baseline length of 1 m or more can be obtained between any one of measurement points A to D and measurement point I regardless of the true position and orientation of measurement point I. Similarly, it is guaranteed that a baseline length of 1 m or more can be obtained between measurement point C and any one of measurement points G to J.

[0041] Note that when selecting measurement points, one or both of the measurement points C and I where a loop has been detected may be excluded from the selection. For example, for the sake of reducing the processing load, only measurement points A, D, G, and J that can satisfy a predetermined distance in each movement path may be left as the selection result, and measurement points B, C, I, and J may be excluded from the selection. Alternatively, only measurement point C may be selected from the movement path on the measurement point C side, and measurement points G and J that can satisfy a predetermined distance may be selected from the movement path on the measurement point I side. Even in this case, either between measurement points C and G or between C and J can obtain the required baseline length. In addition to the method of selecting measurement points one by one, it is also possible to collectively select measurement points within a range that is equal to or greater than a predetermined distance and equal to or less than another predetermined distance.

[0042] In step S506, the first relative position and orientation acquisition unit 404 calculates the relative position and orientation between a plurality of measurement points (map element groups) selected in step S505. FIG. 7 is a diagram showing how the relative position and orientation of the selected range are calculated. Here, six sets of relative position and orientation between measurement point C and measurement points G, H, J, and between measurement points A, B, D and measurement point I are calculated.

[0043] The processes of step S505 and step S506 are performed for all sets in which a loop is detected in step S503 among the sets of measurement points repeated in step S502. Thereafter, the relative position and orientation between measurement points calculated in step S506 is a non-overlapping set of the relative position and orientation calculated in S506 for all iterations. If a plurality of loops are detected in a series of processes and a set of measurement points for which the relative position and orientation have been calculated in the past appears in step S506, the calculation for that set is omitted.

[0044] Step S507 is a branch based on whether step S503 has detected a loop one or more times. If a loop is detected, the subsequent processes of S508 and S509 are performed. If no loop is detected, the process returns to S502 to process the next set of measurement points. Here, since a loop is detected between measurement points C and I, it proceeds to S508.

[0045] In step S508, the second relative position and orientation acquisition unit 405 calculates the relative position and orientation between adjacent measurement points on all movement paths on the map. In the example of FIG. 6, the relative position and orientation between measurement points A and B, between B and C, and so on, up to between measurement points J and K are calculated. Each relative position and orientation is calculated by obtaining the position and orientation information of each measurement point from the map data as an affine matrix and multiplying the inverse matrix of one by the other.

[0046] In step S509, the position and orientation correction unit 406 corrects the position and orientation information of each measurement point based on the relative position and orientation obtained in steps S506 and S508. Specifically, first, the position and orientation correction unit 406 obtains those among the relative position and orientation between measurement points calculated in step S506 where the distance between the measurement points has a sufficient length as the baseline length. Subsequently, the position and orientation correction unit 406 obtains the relative position and orientation between all measurement points calculated in step S507. Thereafter, the position and orientation correction unit 406 uses the position and orientation of each measurement point other than the measurement point serving as the reference for position and orientation calculation as parameters, and minimizes the error between the relative position and orientation between the corrected measurement points and the relative position and orientation obtained in steps S506 and S507. To minimize the error of the relative position and orientation, an optimization process using a method such as the known Newton method or Levenberg - Marquardt method is performed. Here, measurement point A generated at the starting point of the movement path on the map is used as the reference for position and orientation, and the position and orientation of measurement point A is fixed with the value obtained from the map data.

[0047] By performing the above - described processes, the relative position and orientation between measurement points around the loop detection location can be calculated with high precision, and using this, the position and orientation of each measurement point can be corrected with high precision.

[0048] (Effects of the Invention in this Embodiment) As described above, by the process described in this embodiment, even when loop detection is performed between measurement points that are close in distance in the real space, map data capable of realizing highly accurate position and orientation estimation can be generated.

[0049] [Second Embodiment] In the first embodiment, an example was described in which map data created by a moving body equipped with a sensor is corrected on an information processing device independent of the moving body. In this embodiment, an example will be described in which a moving body equipped with an information processing device creates map data while moving in the environment, and corrects the map in parallel with the generation of measurement points. Also, an example of using a pose graph that records information on feature points in the environment and relative position and orientation information between measurement points together with the measurement points as map data will be described. Hereinafter, "measurement point" is used as a term synonymous with "map element" and "key frame".

[0050] (Map Data) The environmental map data used for calculating the position and orientation in this embodiment includes a plurality of feature points in the environment, one or more measurement points, and a pose graph. Each feature point in the environment holds three-dimensional position information. Each measurement point holds observation information of the feature points in the environment in addition to sensor information and position and orientation information. Observation information is a list of pairs of feature points in the environment captured by sensor information (image) and their two-dimensional coordinates on the image. The two-dimensional coordinates on the image are information corresponding to the orientation of the feature points in the environment as seen from the sensor. When creating such map data, by correcting the relative position and orientation between measurement points at any time by bundle adjustment using a plurality of measurement points and the feature point data in the environment that they observe, more accurate environmental map data can be generated.

[0051] The pose graph is a simple graph that describes measurement points as nodes and the relative position and orientation between two measurement points as edges. Each measurement point is connected by an edge to one or more other measurement points. Also, all measurement points on the map data are in a connected state on the pose graph. In this embodiment, the relative position and orientation recorded in the pose graph are values estimated by the above-described bundle adjustment.

[0052] (Configuration of the moving body) FIG. 8 is a diagram showing an example of the configuration of a mobile system 801 according to this embodiment. The mobile system 801 includes a map creation system 802 including a sensor 803 and an information processing device 301, a communication unit 804, a control device 805, and a moving unit 806. Since the physical configuration of the information processing device 301 is the same as that described with reference to FIG. 3 in the first embodiment, the description here is omitted.

[0053] The sensor 803 outputs sensor information obtained by measuring the surrounding environment. The sensor 803 is a stereo camera capable of continuously acquiring a grayscale luminance image fixed in the front direction of the moving body. For simplicity of explanation, it is assumed that the internal parameters such as the focal length and the field angle of the camera are known, and the image is output in a state where there is no distortion or the distortion has been corrected. Also, it is assumed that the sensor 803 performs shooting 30 times per second, but other frame rates may be used. Also, since the sensor 803 is fixed on the mobile system 801, the position and orientation values of both can be easily converted.

[0054] The control device 805 controls the driving of the moving unit 806. The control device 805 drives the moving unit 806 based on an instruction from an external user through the communication unit 804, and moves and turns the mobile system 801. The moving unit 806 is a plurality of tires partially or entirely interlocked with power.

[0055] Since the physical configuration and logical configuration of the information processing device 301 are the same as those described with reference to FIGS. 3 and 4 in the first embodiment, the description here is omitted.

[0056] (Logical Configuration of Information Processing Apparatus) Next, with reference to FIG. 9, the logical configuration of the information processing apparatus according to the present embodiment will be described. Since the loop detection unit 402 to the position and orientation correction unit 406 are the same as those in the first embodiment, the description thereof will be omitted here.

[0057] The map acquisition unit 901 in the present embodiment includes a sensor information acquisition unit 902, a position and orientation estimation unit 903, and a map creation unit 904.

[0058] The sensor information acquisition unit 902 continuously acquires sensor information (stereo image) from the sensor 803.

[0059] Based on the stereo image acquired by the sensor information acquisition unit 902, the position and orientation estimation unit 903 selects a measurement point near the sensor 803 from the measurement points in the map data. Also, based on the feature points on the image and the feature points in the environment on the map data observed by the selected measurement points, or the distribution of the feature points tracked between the image and the image of the previous frame, the position and orientation of the sensor 803 are estimated.

[0060] As necessary, the map creation unit 904 uses the stereo image acquired by the sensor information acquisition unit 902 and the position and orientation information estimated by the position and orientation calculation unit 903 to add measurement points to the map data. The case where it is necessary to add measurement points is, for example, when it is determined that the sensor 803 and the existing measurement points in the map data are separated by a predetermined distance or more and it becomes difficult to estimate the position and orientation based on the existing measurement points. Here, the predetermined distance is the distance at which a necessary and sufficient baseline length can be obtained between measurement points, similar to the first embodiment, and in the following description, it is also assumed to be set to 1 m as in the first embodiment.

[0061] Also, when adding measurement points, the map creation unit 904 generates new feature point data in the environment based on the image feature points on the stereo image that have not yet been associated with the feature point data in the environment on the map data.

[0062] In addition, the map creation unit 904 adds the relative position and orientation between the generated measurement points and the measurement points used for the position and orientation estimation at the time of generation to the pose graph on the environmental map data 701. The relative position and orientation used here are calculated by bundle adjustment using the generated measurement points, the measurement points used for the position and orientation estimation at the time of generation, and the feature point data in the environment that they commonly observe. Further, based on the calculated relative positions and orientations between the measurement points and between the measurement points and the environment, the position and orientation of the generated measurement points and the position information of the feature points in each environment are updated with reference to the position and orientation of the measurement points used for the position and orientation estimation at the time of generation.

[0063] (Map Data Creation Process) Hereinafter, a method for creating map data using the mobile system 801 will be described. FIG. 10 is a flowchart showing the flow of the environmental map data creation process in the present embodiment.

[0064] In S1001, the information processing device 301 performs initialization. The information processing device 301 constructs empty map data on the RAM 313 and generates the first measurement point (measurement point A in FIG. 1) thereon. The position of the first measurement point is set as the origin in the environment, and the orientation is set to a predetermined direction (for example, the positive Y-axis direction). Also, the current position and orientation of the position and orientation calculation unit 903 are initialized with the position and orientation of the first measurement point.

[0065] The subsequent processes are executed in parallel by three threads: a position and orientation calculation thread, a map creation thread, and a map correction thread. S1002 to 1003 are processes by the map creation thread.

[0066] In S1002, the position and orientation calculation unit 903 acquires an image from the sensor 803, and updates the current position and orientation information based on the image and the measurement point closest to the current position and orientation on the map data. Specifically, the information processing device 604 uses, as the current position and orientation information, the position and orientation information associated with the measurement point being referred to, to which is applied the position and orientation difference between the image associated with the measurement point and the latest image that is the captured image at the current position. The position and orientation difference between the images is estimated from the distribution of feature points on both images. In order to maintain the association of the feature points between the images, feature point tracking is performed between the latest image and the image before it. Instead of calculating the position and orientation difference between the image associated with the measurement point in each image, it is also possible to repeatedly calculate the position and orientation difference between the latest image and the previous image and apply it to the current position and orientation information.

[0067] For the extraction of the feature points on the above-mentioned image, in this embodiment, the FAST (Features from Accelerated Segment Test) algorithm is used. Also, for the tracking of the feature points between the images, the known Kanade - Lucas - Tomasi (KLT) algorithm is used. Note that algorithms other than those described above for the extraction of the feature points and the tracking of the feature points may also be used.

[0068] Step S1003 is a branch for determining whether to end the processing of the position and orientation calculation thread. When ending the creation of the map data according to a user's instruction or the like, the thread is ended. Otherwise, the process returns to S1002 and repeats.

[0069] Steps S1004 to 1005 are processing by the map creation thread.

[0070] In step S1004, the map creation unit 904 determines whether it is necessary to add a new measurement point, and if necessary, generates a new measurement point. The new measurement point includes the image and its position and orientation information that S1002 last acquired.

[0071] In addition, the map creation unit 904 extracts image feature points that are not associated with feature points in the existing environment from images of both the new measurement point and the measurement point that S1003 last used for position and orientation calculation. Then, the map creation unit 904 estimates the three-dimensional coordinates of the image feature points that are corresponding between the images based on the relative position and orientation between the images. Finally, the information processing device 604 adds the image feature points for which the estimation of the three-dimensional coordinates has been successful to the map data as feature points in the environment, and also adds the two-dimensional coordinates of the image feature points on each image corresponding to the feature points in the environment as observation information from each measurement point.

[0072] In addition, the measurement point generation unit 904 performs bundle adjustment processing between the new measurement point and the measurement point that S1003 last used for position and orientation calculation, and adds the calculated relative position and orientation information to the pose graph. At this time, the measurement points on the movement path other than the measurement point that S1003 last used for position and orientation calculation may be included in the bundle adjustment processing. Also at that time, the relative position and orientation information calculated for the set of measurement points including those measurement points may be added to the pose graph. Further, when updating the position and orientation of each measurement point and the position information of the feature points in the environment based on the bundle adjustment result, measurement points other than the measurement points used for position and orientation calculation may be used as the reference for the position and orientation.

[0073] Step S1005 is a branch for determining whether to end the processing of the map creation thread. If the position and orientation calculation thread has already ended, the thread is ended. Otherwise, the processing of S1004 is repeated.

[0074] Steps S1006 to S1007 are processing by the map correction thread.

[0075] In step S1006, the information processing device 301 performs correction processing on the map data updated by the map creation thread. Details of the map correction processing will be described later.

[0076] Step S1007 is a branch to determine whether to end the processing of the map correction thread. If the position and orientation calculation thread and the map creation thread have already ended, the thread is ended. Otherwise, the processing of S1006 is repeated.

[0077] (Details of Map Correction Processing) Regarding the details of the map correction processing shown in S1006 in this embodiment, it will be described in detail with reference to the drawings.

[0078] Step S1101 is a determination of whether a new measurement point has been added by step S1004. If there is a measurement point added after the last determination, the processing from S1102 to S1106 is performed. If not, the map correction processing is ended. Here, each measurement point is generated as shown in FIG. 1, and the case where measurement points I, J, and K are added between the last determination and this determination will be taken as an example for explanation.

[0079] Step S1102 is a loop control for repeatedly performing the subsequent processing from S503 to S506 for all pairs of the newly detected measurement point in step S1101 and the other measurement points on the map data. Regarding the following processing from S503 to S1104, the case where the processing is performed for the pair of measurement points C and I during the iteration will be taken as an example for explanation.

[0080] Steps S502 and S503 are the same as those in the first embodiment, so the description thereof is omitted.

[0081] In step S1103, the selection unit 403 selects a group of measurement points for calculating the high-precision relative position and orientation between measurement points from both sides of the moving path forming the loop. In the present embodiment, the selection unit 403 selects the detected measurement points C and I of the loop and the measurement points that observe a certain number or more of common feature points in the environment on each moving path. Here, in order to reduce the processing load of subsequent processing, an upper limit is set for the number of measurement points to be selected. If there are measurement points exceeding the upper limit number as candidates on each moving path, the measurement points up to the upper limit number are selected in order from those with a large number of common feature points in the environment. Here, the upper limit number of measurement points for each path is set to 4, and as a result, as shown in FIG. 6, measurement points A to D are selected from the C side of the measurement points, and measurement points G to J are selected from the I side of the measurement points.

[0082] In step S1104, the selection unit 403 integrates the feature points in the environment observed by each measurement point selected in step S1103 with the feature points in the environment observed by the measurement points on the other path side.

[0083] First, the selection unit 403 compares the image features between the feature points in the environment observed by the measurement points (in the example of FIG. 6, A to D) on one path and the feature points in the environment observed by the measurement points (in the example of FIG. 6, G to J) on the other path. At this time, the selection unit 403 compares the image features of the parts where each measurement point observes the feature points in the environment on the image based on each observation information. Patch matching is used for the comparison of image features. When it is determined that the image features of both observation parts are sufficiently similar, it is determined that they refer to the same object in the real space, and the two feature points in the environment are integrated. Note that the method for comparing image features is not limited to this, and for example, ORB feature amounts may be used.

[0084] Alternatively, the selection unit 403 may perform integration of the feature points in the environment between the two detected measurement points (in the example of FIG. 6, C and I) prior to the selection of the measurement points, and select each measurement point that observes the integrated feature points in the environment.

[0085] In step S1105, the first relative position and orientation acquisition unit 404 performs bundle adjustment processing on each measurement point selected in step S1103 and the feature points observed by those measurement points, and calculates the relative position and orientation between each measurement point. FIG. 12 is a diagram showing the state of the relative position and orientation between each measurement point by the bundle adjustment processing.

[0086] Step S1106 is a branch based on whether step S503 has detected a loop one or more times. If a loop is detected, the subsequent processes of S1107 and S1108 are executed. If no loop is detected, the map correction process is terminated.

[0087] In step S1107, the second relative position and orientation acquisition unit 405 acquires, from the pose graph on the map data, the relative position and orientation between adjacent measurement points on all the movement paths on the map that have not been calculated in step S1105. In the example of FIG. 6, since the relative position and orientation on the paths of measurement points A to D and the paths of measurement points G to J have been calculated in step S1105, the relative position and orientation between each measurement point from measurement point D to G and between measurement points J and K other than these are acquired from the pose graph.

[0088] In step S1108, the position and orientation correction unit 406 corrects the position and orientation information of each measurement point based on the relative position and orientation acquired in steps S1105 and S1107. Here, similar to the first embodiment, the position and orientation of each measurement point other than the measurement point serving as the reference for position and orientation calculation are used as parameters, and the error between the relative position and orientation between each measurement point after correction and the relative position and orientation acquired in steps S1105 and S1107 is minimized. To minimize the error, optimization processing using known methods such as the Newton method or the Levenberg-Marquardt method is performed.

[0089] (Effect of the invention in this embodiment) By implementing the processes described in this embodiment above, even when creating and correcting map data in parallel, it is possible to generate environmental map data capable of realizing highly accurate position and orientation estimation.

[0090] [Other Embodiments] In the first and second embodiments, a grayscale camera is used as the sensor, but the type, number, and fixing method of the sensor are not limited to this. The sensor may be any device that can continuously acquire luminance images and depth images around the moving object as digital data. In addition to grayscale cameras, cameras that acquire color images, depth cameras, 2D-LiDAR, 3D-LiDAR, etc. can be used. Also, a plurality of cameras may be arranged so as to face each direction of the moving object. Also, the number of information acquisitions per second is not limited to 30 times.

[0091] In the first and second embodiments, the detection of the loop of the moving path is performed based on the similarity of images, but the loop detection method is not limited to this. For example, markers that are distinguishable on the sensor information may be used, and the loop may be detected between measurement points where common markers are detected.

[0092] Also, in the first embodiment, the measurement points are selected based on the adjacent state on the moving path, and in the second embodiment, based on the feature points in the environment that are commonly observed. However, the method for selecting the measurement points by the selection unit 403 is not limited to this. For example, the measurement points with the image similarity to the measurement points where the loop is detected, or the measurement points where the markers used at the time of loop detection are commonly detected may be selected.

[0093] Also, in the second embodiment, a moving object operated by a user from the outside is taken as an example for explanation, but the form of the moving object system is not limited to this. For example, it may be a manned moving object that the user can board and directly operate. Alternatively, it may be a moving object equipped with a function of autonomously traveling on a pre-set route. In this case, the control information of the moving object system 801 may be generated based on the map data and the position and orientation information calculated by the position and orientation calculation unit 903, and the moving unit 806 may be driven through the control device 805 to realize autonomous driving.

[0094] Also, although the moving unit 806 is assumed to be wheels, it may be configured to be equipped with a plurality of propellers or the like, and the moving object system 801 may fly in the air and the sensor 803 may observe the ground surface direction.

[0095] The present invention can also be realized by executing the following processes. That is, software (program) that realizes the functions of the above-described embodiments is supplied to a system or device via a network or various storage media, and a computer (or CPU, MPU, etc.) of the system or device reads and executes the program. Further, the program may be recorded on a computer-readable recording medium and provided.

Explanation of Reference Numerals

[0096] 301 Information processing apparatus 302 Mobile body 401 Map acquisition unit 402 Loop detection unit 403 Selection unit 404 First relative position and attitude acquisition unit 405 Second relative position and attitude acquisition unit 406 Position and attitude correction unit

Claims

1. an acquisition means for acquiring three-dimensional map information, which is composed of a plurality of map elements corresponding to a moving route of the mobile body, and which defines sensor information obtained by measuring an environment using a sensor mounted on the mobile body and position and orientation information of the sensor estimated from the sensor information as map elements; a detection means for detecting that the moving object has reached a vicinity of a certain point a plurality of times based on the sensor information included in the map element; a selection means for selecting, as a map element group, map elements having sensor information that partially overlaps with the sensor information included in each of two map elements used when the detection means detects that the moving object has reached the vicinity of a certain point a plurality of times, the map elements including at least one map element on each of the movement routes when the map elements in the vicinity of each of the two map elements are set as the movement routes, and including at least one map element other than the two map elements; a first position and orientation acquisition means for acquiring a relative position and orientation of a set of one or more map elements in a map element group selected by the selection means, using the sensor information included in the map element group; a second position and orientation acquisition means for acquiring a relative position and orientation of the sensor between adjacent map elements, the second position and orientation acquisition means being associated with a moving route along which the moving object has reached a vicinity of a certain point a plurality of times; and a correction means for correcting position and orientation information of the sensor included in a map element corresponding to a movement route along which the moving body reached the vicinity of a certain point a plurality of times, using the relative position and orientation acquired by the first and second position and orientation acquisition means.

2. 2. The information processing apparatus according to claim 1, wherein the selection means additionally selects map elements adjacent to the selected map elements on the moving path of the moving object so that the maximum distance between the selected map elements is equal to or greater than a predetermined value.

3. the three-dimensional map information includes environmental feature point data that records correspondence between three-dimensional position information of one or more environmental feature points observed by the sensor and each map element of a group of map elements having three-dimensional position information of the environmental feature points observed by the sensor; the selection means performs correspondence between feature points in the environment observed in map elements along a moving route each time the moving object reaches a vicinity of a certain point a plurality of times, 3. The information processing device according to claim 1, wherein the first position and orientation acquisition means calculates the relative position and orientation of each map element of the selected group of map elements so as to reduce a reprojection error between the sensor information of each map element of the selected group of map elements and the feature points in the environment observed at the map elements.

4. The information processing device according to claim 3, characterized in that the selection means selects a map element in which common feature points are observed as with a map element used when it is detected that the moving object has arrived near a certain point multiple times, based on the three-dimensional position information of each map element.

5. 4. The information processing apparatus according to claim 3, wherein said selection means selects a map element observing a characteristic point in the environment for which the association has been performed.

6. 4. The information processing device according to claim 3, wherein the selection means selects, for each map element used when it is detected that a moving object has arrived near a certain point multiple times, a map element that observes a common characteristic point in the environment as the map element used.

7. The three-dimensional map information includes a pose graph that records relative positions and orientations between map elements; 2. The information processing apparatus according to claim 1, wherein said second position and orientation acquisition means acquires the relative positions and orientations between map elements from the pose graph.

8. 2 . The information processing apparatus according to claim 1 , wherein the second position and orientation acquisition means calculates relative positions and orientations between a plurality of map elements in the three-dimensional map information based on position and orientation information of the map elements.

9. 2. The information processing apparatus according to claim 1, wherein the correction means uses, from among the relative positions and orientations between the map elements acquired by the first position and orientation acquisition means, those in which the distance between the map elements is equal to or greater than a predetermined value.

10. 10. The information processing apparatus according to claim 1, wherein the sensor is an imaging device, and the sensor information is an image.

11. an acquisition step of acquiring three-dimensional map information composed of a plurality of map elements corresponding to a moving route of the mobile body, the map elements being made up of sensor information obtained by measuring an environment using a sensor mounted on the mobile body and position and orientation information of the sensor estimated from the sensor information; a detection step of detecting that the moving object has reached a vicinity of a certain point a plurality of times based on the sensor information included in the map element; a selection step of selecting, as a map element group, map elements having sensor information that partially overlaps with the sensor information included in each of two map elements used when it is detected in the detection step that the moving object has reached the vicinity of a certain point a plurality of times, the map elements including at least one map element on each of the movement routes when the map elements in the vicinity of each of the two map elements are set as the movement routes, and including at least one map element other than the two map elements; a first position and orientation acquisition step of acquiring a relative position and orientation of a set of one or more map elements in the map element group from which the map elements have been selected in the selection step, using the sensor information included in the map element group; a second position and orientation acquisition step of acquiring a relative position and orientation of the sensor between adjacent map elements corresponding to a movement path on which the moving object has reached a vicinity of a certain point a plurality of times; and a correction step of correcting position and orientation information of the sensor included in a map element corresponding to a movement path along which the moving body reached the vicinity of a certain point a plurality of times, using the relative position and orientation acquired in the first and second position and orientation acquisition steps.

12. A program for causing a computer to function as each of the means of the information processing device according to any one of claims 1 to 10.

Images