Information processing device, information processing device control method, and storage medium

Abstract

The present invention provides an information processing device, a method for controlling an information processing device, and a storage medium. The information processing device performs a first correction to correct, when a loop in a movement path of a sensor is detected, the position and orientation associated with a first measurement point used for estimating the position and orientation to a position and orientation based on a second measurement point that is present near the sensor when the loop is detected.

Description

Technical Field

[0001] The present disclosure relates to techniques for estimating the position and orientation of a sensor and generating an electronic map for use in the estimation. Background Art

[0002] Autonomous mobile objects such as automated guided vehicles (AGVs) are used in factories and distribution warehouses. Simultaneous Localization and Mapping (SLAM) technology, which uses cameras or laser rangefinders as sensors, is known as a method for estimating the position and orientation of such AGVs and creating electronic environmental map data for use in the estimation.

[0003] "Mur-Artal, R., & Tardos, JDORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras. IEEE Transactions on Robotics 33(5)1255-1262" discusses a technology in which a computer identifies a correspondence between a position in real space and a measurement point on environmental map data based on information about each measurement point acquired by a sensor in a process of generating environmental map data by moving a mobile body equipped with the sensor. The above-mentioned prior art document also discusses a loop closing technology for correcting either a position or an orientation in environmental map data based on the correspondence between the position in real space identified by the computer and the measurement point.

[0004] In the method discussed in "Mur-Artal, R., & Tardos, JDORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras. IEEE Transactions on Robotics 33(5)1255-1262," it takes a long time to correct the environment map data when closing the loop, which can lead to a decrease in the accuracy of position / orientation estimation. Specifically, if the sensor continues to move after the correction process begins, new measurement points may be generated that are affected by the errors accumulated before the correction process is completed. In this case, the position / orientation estimation results may change depending on whether the position and orientation are estimated based on the new measurement points or the existing measurement points. Summary of the Invention

[0005] The information processing device includes: an acquisition unit configured to acquire sensor information output from a sensor configured to move and obtained by measuring a surrounding environment; a generation unit configured to generate map data indicating a map based on a movement path of the sensor, the map data including measurement points associating the sensor information with a position and orientation of the sensor; an estimation unit configured to estimate the position and orientation of the sensor based on the measurement points and the sensor information acquired by the acquisition unit; and a detection unit configured to detect a first measurement point included in the map data based on the output of the sensor. a first correction unit configured to, when the first measurement point is detected by the detection unit, correct a position and orientation associated with a second measurement point used by the estimation unit to estimate a position and orientation and included in the map data, to a position and orientation based on the first measurement point; and a second correction unit configured to, when the first measurement point is detected by the detection unit, correct a position and orientation associated with a plurality of measurement points included in the map data, including a measurement point different from the second measurement point, the number of the plurality of measurement points being greater than the number of measurement points corrected by the first correction unit.

[0006] Further features of the present disclosure will become apparent from the following description of exemplary embodiments with reference to the attached drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0007] Figure 1 is a diagram illustrating an example of a movement path of a mobile object and a state of an environment map before correction processing according to one or more aspects of the present disclosure.

[0008] Figure 2 is a diagram illustrating an example of a state of an environment map without performing the first correction process according to one or more aspects of the present disclosure.

[0009] Figure 3 is a diagram illustrating an example of a state of an environment map after a first correction process is performed according to one or more aspects of the present disclosure.

[0010] Figure 4 is a diagram illustrating an example of a state of an environment map before performing a second correction process according to one or more aspects of the present disclosure.

[0011] Figure 5 is a diagram illustrating an example of a state of an environment map after a second correction process is performed according to one or more aspects of the present disclosure.

[0012] Figure 6is a block diagram illustrating a configuration of a mobile body system according to one or more aspects of the present disclosure.

[0013] Figure 7 is a block diagram illustrating a logical configuration of an information processing apparatus according to one or more aspects of the present disclosure.

[0014] Figure 8 is a block diagram illustrating details of a logical construction of a closed-loop processing unit according to one or more aspects of the present disclosure.

[0015] Figure 9 is a flowchart illustrating the flow of information processing according to one or more aspects of the present disclosure.

[0016] Figure 10 is a flow chart illustrating details of a closed-loop processing flow according to one or more aspects of the present disclosure.

[0017] Figure 11 is a flowchart illustrating details of a first correction processing flow according to one or more aspects of the present disclosure.

[0018] Figure 12 is a flowchart illustrating the flow of information processing according to one or more aspects of the present disclosure.

[0019] Figure 13 is a diagram illustrating an example of a state of an environment map before a first correction process is performed according to one or more aspects of the present disclosure.

[0020] Figure 14 is a flowchart illustrating details of a first correction processing flow according to one or more aspects of the present disclosure.

[0021] Figure 15 is a diagram illustrating an example of a state of an environment map after correction processing using rigid transformation is performed according to one or more aspects of the present disclosure.

[0022] Figure 16 is a diagram illustrating an example of a state of an environment map after feature point integration processing is performed according to one or more aspects of the present disclosure. DETAILED DESCRIPTION

[0023] An exemplary embodiment will be described below for preventing the generation of redundant measurement points caused by a long processing time for correcting map data when performing loop closing.

[0024] The exemplary embodiments will be described in detail below with reference to the accompanying drawings. The following exemplary embodiments are not intended to limit the scope of the claims of the present disclosure, and not all combinations of features described in the following exemplary embodiments are essential to the present disclosure.

[0025] A mobile body system, an environment map creating system, an information processing apparatus, an information processing method, and a computer program according to a first exemplary embodiment will be described in detail below with reference to the accompanying drawings.

[0026] The first exemplary embodiment illustrates an example in which a user operates a mobile object equipped with a sensor to move within an environment, generating environmental map data that can be used to estimate the mobile object's position and orientation and enable autonomous travel. While this exemplary embodiment illustrates an example in which a grayscale camera is used as a sensor, this exemplary embodiment is not limited to this example. For example, a compound eye camera can also be used as a sensor, and depth can be used as sensor information. Laser rangefinders, laser rangefinders, laser imaging detection and ranging (LIDAR), and the like can also be used as sensors.

[0027] The term "environment" used in the present exemplary embodiment refers to a three-dimensional space including the area in which the sensor moves and the surrounding area. The "position and orientation" used in the present exemplary embodiment is a value with six degrees of freedom as a combination of three-dimensional position information and orientation information with three degrees of freedom. In the present exemplary embodiment, the position and orientation in the three-dimensional space can be represented by a 4×4 affine matrix, and only rotation and parallel movement among the properties of affine transformation are used. If there are two affine matrices A and B that represent the position and orientation, respectively, an affine matrix d representing the relative position and orientation (position / orientation difference) between the affine matrix A and the affine matrix B can be obtained by adding the affine matrix B to the inverse matrix of the affine matrix A. Similarly, the position / orientation B can be obtained by adding the relative position and orientation d to the position / orientation A.

[0028] (Environmental map data)

[0029] The environment map data used in calculating position and orientation according to this exemplary embodiment includes one or more measurement points and a posture graph. Each measurement point includes sensor information (image data captured by the sensor) and position / orientation information, which serves as information for position / orientation measurement and information for loop detection.

[0030] A pose graph is a simple graph in which measurement points are shown as nodes and the relative position and orientation between two measurement points are shown as edges. In a pose graph, each measurement point is connected to one or more other measurement points via an edge. All measurement points on the environment map data are connected to the pose graph.

[0031] (Drift and closed loop)

[0032] The relationship between position and orientation errors (drift) that occur when generating environmental map data and closed-loop processing will now be described. Environmental map data is generated by repeatedly generating measurement points and estimating the current position and orientation using these measurement points. Therefore, position and orientation errors that occur when generating a measurement point lead to errors in subsequent position / orientation estimates using these measurement points, causing the errors to accumulate each time a measurement point is generated. This accumulation of errors is called "drift."

[0033] Figure 1 Schematically shows the relationship between the measurement points generated in the environment map data and the actual movement path of the moving object on the plane. Figure 1 In , the solid line represents the estimated position and orientation of the moving object obtained through the estimation result, while the dotted line represents the actual position and orientation path of the moving object. Figure 1 In the figure, each white circle represents a measurement point. The moving object generates measurement points B, C, …, E, F, G, and H while moving clockwise from measurement point A in the environment, and then returns to a position near measurement point A. Measurement point H' represents the true position and orientation at the time measurement point H was generated. The phenomenon in which position and orientation errors occurring at each measurement point accumulate and gradually deviate from the true position and orientation based on the amount of movement is called drift.

[0034] In order to correct drift, a process called "closed loop" is performed as follows. That is, when the mobile body repeatedly reaches (loops) a certain point on the environmental map data, the position and orientation of the mobile body and the elements on the environmental map data are corrected. In the closed loop process, when the sensor detects that the mobile body has returned to the mapped area (loop detection), the error accumulated by the search is corrected (loop correction). Figure 1 In the example shown, the position and orientation at each measuring point on the path from measuring point A to measuring point H are corrected (pose graph optimization), so that the position and orientation at measuring point H are corrected to substantially correspond to measuring point H' and to satisfy the relative position / orientation relationship between the corrected measuring point and the other measuring points.

[0035] However, pose graph optimization uses the positions and orientations of numerous measurement points on the environment map data as parameters. Consequently, it takes a considerable amount of time depending on the number of measurement points. Therefore, if the mobile object continues to move while pose graph optimization is performed after loop detection, new measurement points may be generated before pose graph optimization is complete.

[0036] Figure 2is a diagram showing the state of the environmental map data in the above-mentioned case. After measurement point H is generated, a loop is detected between measurement points A and H. The mobile body continues to travel through positions on the environmental map data that are far away from measurement points A and B, so that new measurement points I and J are generated before the posture graph optimization process is completed. After the posture graph optimization process is completed, the positions and orientations at measurement points I and J are corrected to measurement points I' and J', respectively. However, on the posture graph, measurement points I' and J' are not connected to measurement points A', B', and C' that exist near the corrected measurement points I' and J'. Therefore, if the mobile body moves further forward from measurement point J', other new measurement points can be generated near measurement point C'. If the mobile body travels near these measurement points again and calculates the position and orientation, the position / orientation estimation result may change depending on whether the position and orientation are calculated with reference to measurement points B' and C' or with reference to measurement points I' and J'.

[0037] To avoid this problem, in the present exemplary embodiment, at the timing when a loop is detected between the measurement points A and H, a local correction process that can be completed in a short period of time is performed on the measurement point H for measuring the position and orientation of the moving object. Figure 3 This diagram shows the state of the environmental map data at the end of the local correction process. The position and orientation at measurement point H are corrected to measurement point H'. In this exemplary embodiment, the moving object is still near measurement point H' at this point, and no new measurement point is generated.

[0038] Figure 4 is a diagram illustrating a state in which, after the local correction process ends and before the posture graph optimization process is completed, the mobile body continues to move forward on a route along measurement points A and B. At this time, in the present exemplary embodiment, the position and orientation of the mobile body can be calculated with reference to measurement points A and B, so that new measurement points corresponding to measurement points I and J are not generated.

[0039] At this time, since the drift of the position and orientation at the measurement point B is not corrected, the path of the mobile body obtained by the position / orientation estimation result after the loop is detected is different from the path of the mobile body obtained by the measurement point B. Figure 4 However, the amount of this error is smaller than the amount of error caused by the drift at the measurement point H. In addition, the relative position / orientation relationship between the path when the measurement point B was generated and the path after the loop is detected is maintained at this time.

[0040] Figure 5 This diagram shows the state after the posture graph optimization process is completed. The posture graph optimization process corrects the position and orientation at measurement points B through G to measurement points B' through G', which are located near the true motion path. At this time, the position and orientation of the moving object are also corrected based on measurement point B.

[0041] Therefore, in this exemplary embodiment, a local correction process that is completed in a short period of time is performed before the posture graph optimization. Therefore, even if a long time is spent on the posture graph optimization, it is possible to prevent the generation of redundant measurement points and prevent the accuracy of position / orientation estimation from decreasing when using environmental map data. According to this exemplary embodiment, in simultaneous localization and mapping (SLAM), when a loop is detected, before performing the posture graph optimization and before generating new measurement points, a local position / orientation correction process is performed only on the loop source, etc., and compared with the posture graph optimization process, the local position / orientation correction process is completed in a short period of time with a smaller processing amount. Therefore, it is possible to prevent the generation of measurement points that should not be generated based on errors in the environmental map data, and it is possible to prevent the accuracy of the position / orientation estimation results from decreasing due to the measurement points generated based on the errors.

[0042] (Structure of Mobile System, Environmental Map Creation System, and Information Processing Device)

[0043] Next, we will refer to Figure 6 The configuration example of a mobile body system 601 according to the present exemplary embodiment is described. The mobile body system 601 includes an environment map creation system 602 including a sensor 603 and an information processing device 604 , a communication unit 605 , a control device 606 , and a movement unit 607 .

[0044] Sensor 603 outputs sensor information obtained by measuring the surrounding environment. Sensor 603 is a camera fixed in front of mobile system 601 and configured to continuously acquire grayscale brightness images. To simplify the description, it is assumed that the camera's internal parameters (such as focal length and viewing angle) are known, and that the image is output with no distortion or with distortion corrected. Sensor 603 captures images 30 times per second, but can capture images at other frame rates.

[0045] The information processing device 604 estimates the position and orientation of the mobile system 601 based on the information input from the sensor 603 and generates environmental map data. During the autonomous travel of the mobile system 601, the information processing device 604 issues movement instructions to the control device 606. The configuration of the information processing device 604 will be described in detail below.

[0046] The communication unit 605 receives instructions from the user, such as instructions for movement or rotation of the mobile system 601 and instructions for starting or ending the environment map creation process to be performed by the information processing device 604. The communication unit 605 is, for example, a chip or an antenna for establishing communication based on the Institute of Electrical and Electronics Engineers (IEEE) 802.11 series. The communication method of the communication unit 605 is not limited to the IEEE 802.11 series. Other communication methods such as or wired communications.

[0047] The control device 606 controls the driving of the moving unit 607. The control device 606 drives the moving unit 607 based on instructions from the information processing device 604 and the communication unit 605 to move or rotate the moving body system 601. The moving unit 607 is a plurality of tires partially or entirely operated by power.

[0048] (Structure of Information Processing Device)

[0049] The information processing device 604 includes the functions of a general built-in personal computer (PC) device. The information processing device 604 includes a central processing unit (CPU) 611, a read-only memory (ROM) 612, a random access memory (RAM) 613, a storage unit 614 (such as a hard disk drive (HDD) or a solid-state drive (SSD)), a universal interface (I / F) 615 (such as a universal serial bus (USB)), and a system bus 616.

[0050] The CPU 611 executes an operating system (OS) and various computer programs stored in the ROM 612 and the storage unit 614 using the RAM 613 as a working memory, thereby calculating or processing information and controlling each unit via the system bus 616. For example, the programs to be executed by the CPU 611 include programs for executing the following processing.

[0051] The sensor 603 , the communication unit 605 , and the control device 606 are each connected to the information processing device 604 via a general-purpose I / F 615 .

[0052] Figure 6 An example is shown in which the information processing device 604 is incorporated into the mobile system 601. However, the configuration of the information processing device 604 is not limited to this example. The information processing device 604 may be configured as a device separate from the mobile system 601. In this case, the universal I / F 615 may function as a wireless or wired communication interface and may be configured to communicate with the communication unit 605 and communicate with the sensor 603 to exchange data. The information processing device 604 may constitute the entire mobile system 601.

[0053] (Logical Structure of Information Processing Device)

[0054] The following describes a logical configuration of the information processing apparatus 604 according to the present exemplary embodiment. The processing of each unit described below is executed as software by loading a computer program from the ROM 612 or the like into the RAM 613 and executing the loaded program by the CPU 611. Figure 760 is a block diagram showing the logical configuration of the environment map creation system 602 and the information processing device 604 . Figure 7 The various logic structures shown may be constructed as an application specific integrated circuit (ASIC), a field programmable gate array (FPGA), or the like.

[0055] The environment map data 701 is environment map data stored in the RAM 613 and currently created.

[0056] The sensor information acquisition unit 702 acquires an output from the sensor 603. Specifically, the sensor information acquisition unit 702 acquires an image captured by the sensor 603.

[0057] The position / orientation calculation unit 703 selects measurement points located near the sensor 603 from the measurement points included in the environment map data 701 , and estimates the position and orientation of the sensor 603 based on the image captured by the sensor 603 and the selected measurement points.

[0058] Measurement point generation unit 704 adds measurement points to environment map data 701 as needed based on the images captured by sensor 603 and the position / orientation information estimated by position / orientation calculation unit 703. For example, it is preferable to add measurement points when the distance between the position of sensor 603 in the environment map and an existing measurement point in the environment map is greater than or equal to a predetermined value, or when it is determined that it is difficult to estimate the position and orientation based on the existing measurement points. Furthermore, measurement point generation unit 704 adds the relative position and orientation between the generated measurement point and the measurement point used for position / orientation estimation when the measurement point was generated to the pose graph in environment map data 701.

[0059] The closed-loop processing unit 705 performs closed-loop processing. When the sensor detects that the mobile object has returned to an already mapped area (loop detection), the closed-loop processing unit 705 corrects the errors accumulated due to the search (loop correction). Specifically, when the closed-loop processing unit 705 determines that a newly generated measurement point is located near a previously generated measurement point, the closed-loop processing unit 705 calculates the relative position of the two measurement points and associates the measurement points on the pose graph. The closed-loop processing unit 705 then performs processing to correct the shape of at least the area forming the loop on the environment map. The structure of the closed-loop processing unit 705 will be described in detail below.

[0060] (Logical Structure of Closed-Loop Processing Unit)

[0061] Figure 8 is a block diagram showing a detailed logical configuration of the closed-loop processing unit 705 .

[0062] Loop detection unit 801 examines newly generated measurement points on environment map data 701 and determines whether a loop of measurement points has occurred. The term "loop," as used herein, refers to a state in which a newly generated measurement point and previously created measurement points that are not associated with each other on the pose graph are close to each other in real space. Specifically, the similarity between an image captured at a newly generated measurement point and images captured at previously created measurement points is first calculated using a known bag-of-words (BoW) model, and measurement points with a similarity greater than or equal to a certain level are extracted.

[0063] Afterwards, the loop detection unit 801 calculates the relative position and orientation between the newly generated measurement point and each created measurement point in descending order of similarity. Specifically, image feature points are extracted from the sensor information about the two measurement points, and the relative position and orientation between the images are estimated based on the distribution of the feature points. If the calculation of the relative position and orientation at any measurement point is successful, the loop detection unit 801 determines that a loop has occurred between the two measurement points. Hereinafter, the newly generated measurement point is referred to as the loop source, and the previously created measurement point is referred to as the loop destination. Before a loop is detected, the path between the loop source and the loop destination on the posture graph is referred to as the loop range.

[0064] First correction unit 802 corrects the position / orientation information regarding the elements in environment map data 701, referenced by position / orientation calculation unit 703, to the position and orientation based on the loop destination measurement point. Furthermore, first correction unit 802 adds the relative position and orientation between the loop source measurement point and the loop destination measurement point to the posture graph on environment map data 701.

[0065] The second correction unit 803 optimizes the position / orientation information of each measurement point shown in the pose graph on the environment map data 701 (pose graph optimization). The second correction unit 803 obtains the relative positions and orientations between the measurement points and performs pose graph optimization to minimize the error between the relative position / orientation information between the measurement points shown in the pose graph and the relative position / orientation information calculated based on the positions and orientations of the measurement points.

[0066] (Environmental map data creation process)

[0067] Next, we will refer to Figures 9 to 11 A creation method of environment map data related to an information processing method using the information processing apparatus 604 according to the present exemplary embodiment is described. Figure 9611 loads a program stored in the ROM 612 or the storage unit 614 and executes the program to calculate or process information and control each hardware module, thereby realizing Figures 9 to 11 Some or all of the steps shown in the flowcharts may be implemented by hardware modules such as ASIC or FPGA.

[0068] like Figure 9 As shown, in step S901, the information processing device 604 is initialized. The information processing device 604 constructs an empty environment map data 701 and generates a first measurement point ( Figure 1 The position of the first measurement point is set as the origin in the environment, and the orientation is set in a predetermined direction (e.g., the positive Y-axis direction). The information processing device 604 uses the position and orientation at the first measurement point to initialize the current position / orientation information estimated by the position / orientation calculation unit 703.

[0069] The subsequent processing is executed in parallel in three threads (ie, a position / orientation calculation thread, a map creation thread, and a map correction thread). Steps S902 to S904 correspond to the processing in the map creation thread.

[0070] In step S902, the position / orientation calculation unit 703 acquires an image from the sensor 603 and updates the current position / orientation information based on the image and the currently referenced measurement point. Specifically, the information processing device 604 uses the position / orientation information obtained by applying the position / orientation difference between the image associated with the measurement point and the most recent image captured at the current location to the position / orientation information associated with the currently referenced measurement point as the current position / orientation information. The position / orientation difference between the images is estimated based on the distribution of feature points in each image. To maintain the correspondence between the feature points in the images, the feature points are tracked between the most recent image and the previous image.

[0071] Instead of calculating the position / orientation difference of an image associated with a measurement point in each image, the position / orientation difference between the latest image and the previous image may be calculated, and the calculated position / orientation difference may be repeatedly applied to the current position / orientation information.

[0072] For the above-mentioned extraction of feature points on the image, the Features from Accelerated Segment Test (FAST) algorithm is used in the present exemplary embodiment. In order to estimate the position / orientation difference between the images, a bundle adjustment process is used to optimize only the position and orientation of the sensor 603 when the position of each feature point is fixed. The bundle adjustment process is a method for solving the problem of estimating the parameters of a geometric model based on the correspondence between image feature points extracted between multiple images, and is also used to numerically solve nonlinear optimization problems. In the bundle adjustment process, the three-dimensional coordinates of each feature point are calculated based on the correspondence between the feature points of the input images. The calculated three-dimensional coordinates of each feature point are reprojected onto the image plane, and the reprojection error calculated as the distance between the reprojection point and each feature point is repeatedly re-estimated, thereby estimating a more accurate value of the three-dimensional coordinates of each feature point.

[0073] The well-known Kanade-Lucas-Tomasi (KLT) algorithm is used to track feature points between images. The above algorithms for feature point extraction and feature point tracking can be replaced by other algorithms.

[0074] In step S903, the position / orientation calculation unit 703 selects a measurement point to be referenced in subsequent position / orientation calculations. Specifically, the information processing device 604 selects the measurement point with the highest feature point correspondence amount relative to the most recent image from the measurement points located near the current position and orientation on the environment map data 701.

[0075] The position / orientation calculation unit 703 determines whether to add a new measurement point. If so, the position / orientation calculation unit 703 issues an instruction to the measurement point generation unit 704 to generate the measurement point. Specifically, if the correspondence between the feature points of the measurement point selected in step S903 and the latest image is less than or equal to a threshold, or if the difference between the position and orientation at the measurement point and the current position and orientation is greater than or equal to a threshold, the information processing device 604 determines that a new measurement point is to be added. If it is determined that a new measurement point is to be added and the measurement point generation unit 704 does not have any unprocessed measurement point generation instructions, the information processing device 604 issues an instruction to generate a new measurement point in the map creation thread. In the instruction to generate the measurement point, the position / orientation calculation unit 703 passes information about the selected measurement point and the latest image to the measurement point generation unit 704.

[0076] In step S904, a determination is made as to whether the position / orientation calculation thread has terminated. If the environment map data creation process has terminated in response to a user instruction ("Yes" in step S904), the thread terminates. If the environment map data creation process has not terminated ("No" in step S904), the process returns to step S902 to repeat the process.

[0077] Steps S905 and S906 are processed in the map creation thread.

[0078] In step S905, the measurement point generation unit 704 generates a new measurement point based on the instruction issued by the position / orientation calculation unit 703 in step S903. If the measurement point generation unit 704 has no unprocessed instructions, it waits for a new instruction. The new measurement point includes information indicating the image acquired in step S902, as well as the position and orientation. The measurement point generation unit 704 acquires a captured image from the sensor 603 and calculates the position and orientation based on the captured image. The measurement point generation unit 704 then generates a measurement point that includes the acquired captured image and the calculated position and orientation.

[0079] Furthermore, the measurement point generation unit 704 adds relative position / orientation information between the new measurement point and the measurement point selected in step S903 to the posture graph.

[0080] In step S906, it is determined whether processing in the map creation thread has ended. If the position / orientation calculation thread has ended ("Yes" in step S906), the map creation thread terminates. If the position / orientation calculation thread has not ended ("No" in step S906), the process returns to step S905 to repeat the process.

[0081] Steps S907 and S908 are processes in the map correction thread.

[0082] In step S907, upon detecting a loop on the environment map data of the updated map creation thread, the information processing device 604 performs a closed loop process. The closed loop process will be described in detail below.

[0083] In step S908, it is determined whether the map correction thread has ended. If the position / orientation calculation thread and the map creation thread have ended ("Yes" in step S908), the map correction thread is terminated. If the position / orientation calculation thread and the map creation thread have not ended ("No" in step S908), the process returns to step S907 to repeat the process.

[0084] (Details of closed-loop processing)

[0085] Next, the closed-loop processing shown in step S907 will be described in detail. Figure 10 is a flowchart showing the flow of closed-loop processing according to the present exemplary embodiment.

[0086] like Figure 10 As shown, in step S1001, the loop detection unit 801 detects a loop and calculates the relative position and orientation between the loop source measurement point and the loop destination measurement point. In step S1002, it is determined whether a loop is detected. If a loop is detected ("Yes" in step S1002), the process proceeds to step S1003 after adding information about the relative position and orientation between the loop source and the loop destination to the posture graph. If a loop is not detected ("No" in step S1002), the loop closing process terminates.

[0087] In step S1003, the first correction unit 802 performs a first correction process to correct the position / orientation information of the measurement point currently referenced by the position / orientation calculation unit 703 to coordinates based on the loop destination measurement point. The first correction process is completed within a time period shorter than the interval used to generate measurement points in the map creation thread. In other words, the information processing device 604 does not generate any new measurement points during the period from the start of the first correction process to its completion. The first correction process will be described in detail below.

[0088] In step S1004, the second correction unit 803 performs a posture graph optimization process (second correction process) on the loop range on the environment map data 701 and the measurement points generated after the loop range. In step S1003, the information about the relative position / orientation between the loop source and the loop destination calculated in step S1001 is added to the posture graph, and the drift occurring at each measurement point is corrected through the posture graph optimization process. Therefore, the position and orientation at each measurement point can be corrected to a position close to the true position / orientation path of the sensor 603. If the position and orientation of the measurement point currently referenced by the position / orientation calculation unit 703 is corrected through the posture graph optimization process, the current position / orientation information about the sensor 603 is also corrected.

[0089] (Details of First Correction Processing)

[0090] Next, we will refer to Figure 11 The flowchart shown describes in detail the first correction process shown in step S1003. In this case, the position and orientation at each measurement point are corrected by a rigid transformation using the position / orientation difference.

[0091] First, consider the following situation: Figure 1As shown, the sensor 603 generates measurement points B, C, ..., E, F, G, and H while moving clockwise in the environment with measurement point A as the starting point, and then the sensor 603 returns to a position near measurement point A. In step S1001, a loop is detected in which measurement point H is set as the loop source and measurement point A is set as the loop destination, and the relative positions and orientations between the measurement points are calculated.

[0092] like Figure 11 As shown, in step S1101, it is determined whether correction processing using rigid transformation is performed. Rigid transformation indicates transformation of only parallel translation and rotation.

[0093] As described above, the first correction process is a process for correcting the position / orientation information of the measurement point currently referenced by position / orientation calculation unit 703 to coordinates based on the loop destination measurement point. Therefore, if position / orientation calculation unit 703 is already referencing the loop destination measurement point or a measurement point near the loop destination measurement point on the posture graph, the correction of the position and orientation at the measurement point using rigid transformation cannot be effectively performed. In this exemplary embodiment, the measurement point currently referenced by position / orientation calculation unit 703 is compared with the distance between the loop source measurement point and the loop destination measurement point on the posture graph. The following processing of steps S1102 to S1104 is performed only when the loop source measurement point is closer to the measurement point.

[0094] In step S1102, the first correction unit 802 first calculates the relative position and orientation. It then calculates the position and orientation of measurement point H' after the first correction based on the position and orientation of the loop destination measurement point and the calculated relative position and orientation. Furthermore, the position / orientation difference dH used in the first correction is calculated based on the position and orientation of measurement point H' after correction and the position and orientation of measurement point H before correction.

[0095] In step S1103, an element on the environment map data that has been corrected using a rigid transformation is selected. It is assumed here that the period from the issuance of the instruction to generate measurement point H (step S903) to the detection of the loop (step S1001) is sufficiently short, and that the sensor 603 is located near measurement point H. Therefore, the position / orientation calculation unit 703 still refers to measurement point H. In this case, measurement point H is selected as the measurement point to be corrected.

[0096] In step S1104, the first correction unit 802 adds the position / orientation difference dH calculated in step S1102 to the correction target position and orientation selected in step S1103, and updates the position and orientation. The current position / orientation information about the sensor 603 is also updated in the same manner.

[0097] This process can be implemented by integrating a simple 4×4 matrix and can be completed in a much shorter timeframe than the measurement point generation process in step S905 or the process of step S1004 including the posture graph optimization process. Therefore, after the closed-loop processing unit 705 detects a loop, before a new measurement point needs to be generated in the position / orientation calculation thread, the correction process is completed, and then the position / orientation calculation process with reference to measurement point A can be performed. Consequently, the generation of redundant measurement points I and J can be prevented.

[0098] Therefore, when a loop is detected, information processing device 604 performs a local correction process before performing a pose graph optimization process, which corrects the positions and orientations associated with a plurality of measurement points included in the environment map data. The first correction process, performed as a local correction process, corrects the positions and orientations of a smaller number of measurement points than the number corrected in the pose graph optimization process. In other words, when a loop is detected, information processing device 604 performs a first correction process to correct the positions and orientations of some measurement points. After completing the first correction process, information processing device 604 performs a second correction process for a larger number of measurement points than the number corrected in the first correction process. This prevents the generation of redundant measurement points.

[0099] This exemplary embodiment shows an example in which the first correction unit 802 performs correction processing using a rigid transformation. However, the correction processing is not limited to this example. The correction processing performed by the first correction unit 802 may be other correction processing as long as its processing amount is less than the processing amount in the correction processing (pose graph optimization processing) performed by the second correction unit 803. In addition, the correction processing performed by the first correction unit 802 may be other correction processing as long as the correction processing can be completed before generating a new measurement point even when the sensor 603 moves continuously after a loop is detected, and the position and orientation of the measurement point at or near the loop source measurement point can be corrected.

[0100] (Advantageous Effects of the Exemplary Embodiment)

[0101] As described above, according to this exemplary embodiment, even when environment map correction processing is performed during the creation of environment map data, environment map data can be generated that can estimate position and orientation with high accuracy. Furthermore, according to this exemplary embodiment, the first correction processing is completed before the second correction processing, thereby preventing the generation of redundant measurement points that would otherwise result from an increase in the time required to correct map data during closed-loop processing.

[0102] The mobile body system 601 moves to circulate through a plurality of coordinates while performing position / orientation estimation processing based on the environment map data generated by the information processing device 604 and the image captured by the sensor 603 , thereby autonomously traveling with high accuracy.

[0103] The second exemplary embodiment will now be described. The first exemplary embodiment described above illustrates an example in which the position / orientation calculation unit 703 performs position / orientation measurement processing with reference to a single measurement point in the environmental map data 701. This exemplary embodiment illustrates a method for correcting environmental map data, where three-dimensional position information regarding each feature point in the environment is stored as environmental map data, and the mobile system 601 estimates the position / orientation information based on the three-dimensional feature point distribution and sensor information. When creating such environmental map data, the relative positions and orientations between the measurement points are corrected as needed using a method known as bundle adjustment, enabling the generation of environmental map data with higher accuracy.

[0104] (Environmental map data)

[0105] The environment map data used for calculation of position and orientation in this exemplary embodiment includes a plurality of feature points in the environment, one or more measurement points, and a pose graph.

[0106] Each feature point in the environment includes three-dimensional position information. Each measurement point not only stores sensor information (images) and position / orientation information used for feature point detection and loop detection in the environment, but also stores observation information about each feature point in the environment. Observation information is a list of combinations of feature points in the environment, as included in the sensor information, and their two-dimensional coordinates on the image. The two-dimensional coordinates on the image are information corresponding to the orientation of each feature point in the environment as viewed from the sensor.

[0107] (Structure of Mobile System, Environmental Map Creation System, and Information Processing Device)

[0108] The physical and logical configurations of the mobile body system 601 according to the present exemplary embodiment are similar to those of the first exemplary embodiment, and thus descriptions thereof are omitted.

[0109] (Environmental map data creation process)

[0110] Will refer to Figures 12 to 16 A method for creating environment map data according to the present exemplary embodiment is described. Figure 12 : is a flowchart showing the flow of environment map data creation processing according to this exemplary embodiment. Figure 9 The steps in the flowchart shown are similar to the steps in Figure 12The same numbers are used in the flowchart shown, and detailed description thereof is omitted. Specifically, the processing of steps S903, S904, S906, and S908 is similar to that of the first exemplary embodiment, and thus description thereof is omitted.

[0111] like Figure 12 As shown, in step S1201, the information processing device 604 is initialized. The information processing device 604 constructs an empty environment map data 701 and generates a first measurement point ( Figure 1 The information processing device 604 uses the position and orientation of the first measurement point to initialize the current position / orientation information estimated by the position / orientation calculation unit 703. As in the first exemplary embodiment, the position of the first measurement point is set as the origin in the environment, and the orientation is set in a predetermined direction (e.g., the positive Y-axis direction).

[0112] To generate a set of feature points in the environment, images are first acquired from sensor 603 at two locations (i.e., at the position and orientation used to generate the first measurement point A and at a position / orientation A' slightly away from the first measurement point A). Next, based on the image feature points extracted from the images, a bundle adjustment process is used to estimate the three-dimensional coordinates of the image feature points that match between the images. Finally, the image feature points for which three-dimensional coordinates were successfully estimated are added to the environment map data as feature points in the environment, and the two-dimensional coordinates of the image feature points on the image of measurement point A corresponding to the feature points in the environment are added as observation information when viewed from measurement point A.

[0113] The position / orientation calculation thread according to the second exemplary embodiment will be described.

[0114] In step S1202, the position / orientation calculation unit 703 acquires an image from the sensor 603 and updates the current position / orientation information based on the image and feature points in the environment. The position / orientation calculation unit 703 reprojects the feature points in the environment, as viewed from the reference measurement point, onto the image and performs optimization processing to calculate the position and orientation so that the reprojection error between the reprojected points and the corresponding image feature points is minimized. In this case, feature points are tracked between the latest image and previous images to maintain the correspondence between the image and the feature points in the environment.

[0115] Furthermore, as in the first exemplary embodiment, steps S903 and S904 are performed.

[0116] Next, a map creation thread according to the second exemplary embodiment will be described.

[0117] In step S1203 , the measurement point generation unit 704 generates feature points and measurement points in the environment based on the instruction issued from the position / orientation calculation unit 703 in step S903 .

[0118] The measurement points to be generated include the image, position, and orientation acquired in step S1202, the feature point group in the environment tracked by the position / orientation calculation unit 703, and observation information regarding the newly generated feature point group in the environment, which will be described below. The feature points in the environment are generated using the image of the measurement point selected in step S1202 and the captured image at the measurement point to be generated.

[0119] First, the information processing device 604 extracts image feature points from the two images that are not associated with existing feature points in the environment. Next, the information processing device 604 estimates the three-dimensional coordinates of the corresponding image feature points 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 three-dimensional coordinates were successfully estimated to the environment map data as feature points in the environment. It 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. Furthermore, the measurement point generation unit 704 adds the relative position / orientation information between the new measurement point and the measurement point selected in step S903 to the pose graph.

[0120] In step S1204, the measurement point generation unit 704 performs bundle adjustment processing to correct the positions and orientations of the measurement points generated in step S1202 and the measurement point group that shares the feature points in the environment. Furthermore, the measurement point generation unit 704 adds or writes the relative position / orientation information between the measurement points obtained as a result of the bundle adjustment to the pose graph.

[0121] Next, a map correction thread according to the second exemplary embodiment will be described.

[0122] In step S1205, a loop on the environment map data updated in the map creation thread is detected, and a closed loop process is performed. Except for the details of the first correction process (step S1003), the details of the closed loop process are the same as those of the first exemplary embodiment. Figure 10 The details shown are similar. Therefore, only the details of the first correction process will now be described.

[0123] (Details of First Correction Processing)

[0124] Will refer to Figures 13 to 16The first correction process shown in step S1003 according to this exemplary embodiment will be described in detail. In this exemplary embodiment, in addition to correcting the position and orientation at each measurement point and feature point in the environment by rigid transformation using position / orientation differences, processing for integrating feature points in the environment and bundle adjustment processing using measurement points near the loop source and loop destination are also performed.

[0125] As in the first exemplary embodiment, the second exemplary embodiment also shows an example in which Figure 1 As shown, the sensor 603 generates measurement points B, C, ..., E, F, G, and H while moving clockwise in the environment starting from the measurement point A, and then returns to a position near the measurement point A.

[0126] When a loop is detected in which the measurement point H is set as the loop source and the measurement point A is set as the loop destination, the information processing device 604 calculates the relative position and orientation between the measurement points (step S1001 ).

[0127] Figure 13 is a diagram showing the state of the environment map near the sensor 603 when a loop is detected. Figure 13 In the figure, black points p, p', q, r, and s are feature points in the environment. Points p and q are observed from measurement point A. Point p' is observed from measurement points H and G and the latest image from sensor 603. Point r is observed from measurement point H and the latest image from sensor 603. Point s is observed from measurement point G. Points p and p' are registered in the environment map data 701 as different feature points in the environment, but they originate from the same feature point of an object in real space. Although there are actually numerous feature points in the environment that can be used for position / orientation estimation, only a few are shown here for ease of illustration.

[0128] Figure 14 : is a flowchart showing the flow of the first correction process according to the present exemplary embodiment. Figure 14 As shown, in step S1401, it is determined whether to perform correction processing using rigid transformation. In this exemplary embodiment, the information processing device 604 performs the processing of steps S1102, S1402, and S1403 as long as the feature points in the environment viewed from the loop destination measurement point are not included in the feature points in the environment referenced by the position / orientation calculation unit 703.

[0129] In step S1401, it is determined whether correction processing using rigid transformation is performed. If it is determined that correction processing using rigid transformation is performed ("Yes" in step S1401), the process proceeds to step S1102. Then, in step S1402, the information processing device 604 selects elements on the environment map data to be corrected by rigid transformation. First, feature points in the environment tracked by the position / orientation calculation unit 703 are extracted. Figure 13 In the example shown, points p' and r are extracted. Next, the measurement points where any feature point is observed are extracted as the measurement points to be corrected. Figure 13 In the example shown, measurement points H and G are extracted. Finally, feature points in the environment observed by any measurement point to be corrected are extracted as feature points in the environment to be corrected. Figure 13 In the example shown, points p', r, and s are characteristic points in the environment to be corrected. The current position and orientation information regarding sensor 603 is also corrected. Therefore, in this case, the position and orientation of each of measurement points H and G, the position of each of characteristic points p', r, and s in the environment, and the position and orientation of sensor 603 are corrected.

[0130] In step S1403, the first correction unit 802 adds the position / orientation difference dH calculated in step S1102 to each correction target selected in step S1102 and updates the position and orientation or position. This process is a rigid transformation and maintains the relative position / orientation relationship between the elements to be corrected.

[0131] Figure 15 The diagram shows the positional relationship between the measurement points obtained after the rigid transformation process. The position and orientation of each of the measurement points H and G are corrected to H' and G', respectively. At this point, the relative position / orientation relationship between the measurement point F and the element to be corrected is disrupted. However, since the measurement points and the position / orientation calculation unit 703 do not share feature points in the environment, the disruption of the relative position / orientation relationship does not adversely affect the position / orientation calculation.

[0132] In step S1404, the first correction unit 802 integrates the feature points in the environment whose positions were corrected in step S1403 and the feature points in the environment that exist near the corrected positions. First, the information processing device 604 extracts other points that are less than a certain distance away from each corrected position in the environment map from the feature points in the environment whose positions were corrected. Thereafter, the information processing device 604 compares the image features in the area of ​​the feature points in the observed environment on the image based on the observation information between the extracted feature points in the environment and the corrected feature points in the environment. Patch matching is used to compare the image features. If it is determined that the image features in the observed area are sufficiently similar to each other, it is determined that the image feature indicates reference to the same object in real space, and the two feature points in the environment are integrated. The method of comparing image features is not limited to this example. For example, directional FAST and rotational BRIEF (ORB) feature quantities can be used.

[0133] Figure 16 This diagram shows the state of the environment map near sensor 603 after the feature points are integrated. The position of point p' is corrected using a rigid transformation, and then points p' and p are integrated. The resulting point p includes observation information about points p' and p before integration. In other words, point p is observed from measurement points A, H', and G'.

[0134] In step S1405, the first correction unit 802 performs a bundle adjustment process to correct the position of the feature point group in the environment integrated in step S1404 and the position and orientation of the measurement point group from which any position of the feature point group is observed. Figure 13 In the example shown, feature point p in the environment and measurement points A, H', and G' are bundle-adjusted. In reality, integration processing and bundle adjustment are performed on many more feature points in the environment (not shown), and sufficient information is available for optimizing position and orientation through bundle adjustment. Furthermore, first correction unit 802 adds or writes the relative position / orientation information between the measurement points obtained as a result of bundle adjustment to the pose graph.

[0135] While using bundle adjustment to correct the position and orientation of each measurement point near the loop source and loop destination takes longer than using a rigid transformation, it can be completed in a shorter timeframe than the process of step S1004, which includes pose graph optimization. Therefore, after a loop is detected, the loop closure processing unit 705 can obtain the relative position and orientation of each measurement point near the loop source and loop destination with high accuracy in a short timeframe, resulting in improved accuracy in the position and orientation calculation by the position / orientation calculation unit 703. Even if new measurement points are generated before the second correction process is completed, the improved accuracy of the position / orientation calculation results in improved accuracy in the position / orientation information.

[0136] (Advantageous Effects of the Exemplary Embodiment)

[0137] According to the present exemplary embodiment described above, even when environment map correction processing is performed during creation of environment map data by a position / orientation calculation method that includes feature point information in the environment in the environment map data, environment map data that can achieve position / orientation estimation with high accuracy can be generated.

[0138] [Other exemplary embodiments]

[0139] The first exemplary embodiment and the second exemplary embodiment show an example in which a camera fixed in front of a moving body and configured to acquire a grayscale brightness image is used as the sensor 603. However, the type, number, and fixing method of the sensor 603 are not limited to this example. Any sensor can be used as the sensor 603 as long as the sensor can continuously acquire a brightness image or a depth image of the surrounding area from the moving body as digital data. Not only a grayscale camera but also a camera capable of acquiring a color image, a depth camera, a two-dimensional (2D) light detection and ranging (LiDAR) camera, or a three-dimensional (3D) LiDAR camera can be used. A stereo camera can also be used, or a plurality of cameras can be arranged in each direction of the moving body. In addition, the number of times the information ID is acquired per second is not limited to 30 times.

[0140] For example, when a stereo camera is used as the sensor 603, in step S1201, in the process of generating feature points in the environment, the information processing device 604 uses a stereo image pair to obtain the distance to each image feature point, rather than moving the sensor 603 to the position and orientation A'. The information processing device 604 can convert the distance into the three-dimensional coordinates of the image feature point.

[0141] Alternatively, when a sensor capable of obtaining distance information about each pixel of an image (such as a depth camera or 3D-LiDAR) is used as the sensor 603, the information processing device 604 calculates three-dimensional coordinates based on the position and orientation of the sensor and the viewing angle, as well as the distance from each pixel. In addition, the information processing device 604 can generate feature points in the environment corresponding to each pixel. In this configuration, a set of feature points in a denser environment than in the second exemplary embodiment can be obtained. In this case, the relative position and orientation between the measurement points are calculated using the known interactive closest point (ICP) algorithm. The calculated relative position and orientation can be used for loop detection processing and the first correction processing.

[0142] The first and second exemplary embodiments described above illustrate examples in which position and orientation in three-dimensional space are measured and environmental map data is created for position / orientation measurement. However, position / orientation measurement and environmental map data creation can also be performed on a two-dimensional plane along a surface on which a mobile object moves. For example, in a system in which a mobile object travels on the ground, a 2D-LiDAR for horizontally scanning data can be used as sensor 603, and environmental map data can be created on a two-dimensional plane with a set of feature points in a denser environment as described above.

[0143] In the first exemplary embodiment, an image is stored as information for loop detection at each measurement point. However, for example, when generating measurement points in step S905, eigenvalue vectors and image feature points may be calculated based on the BoW model, and these eigenvalue vectors and image feature points may be stored instead of the image. In this case, image similarity can be calculated based on the eigenvalue vectors pre-calculated by loop detection unit 801, and relative position and orientation can be calculated based on the pre-calculated feature points.

[0144] In the first exemplary embodiment, in step S901, the position of the first measurement point is set as the origin in the environment, and the orientation is set in a predetermined direction (e.g., the positive Y-axis direction). However, if the position and orientation of the first measurement point can be specified by another method, the values ​​obtained by this method can be used. For example, a marker detectable by sensor 603 or another unit can be used to calculate the relative position and orientation relative to sensor 603, and the origin and coordinate axes can be set based on the calculated relative position and orientation. Alternatively, the origin and coordinate axes can be set using the installation position and orientation of sensor 603 on mobile system 601 as compensation.

[0145] Markers similar to those described above can be used for loop detection by the loop detection unit 801. For example, by detecting that the same marker is observed from two measurement points, rather than calculating image similarity, the relative position and orientation between images can be calculated based on the relative position and orientation between the sensor and the marker in each image.

[0146] The second exemplary embodiment described above shows an example in which the determination as to whether to perform rigid transformation processing in step S1401 is performed by determining whether the position / orientation calculation unit 703 shares feature points in the environment with the loop destination measurement point. However, the method for determining whether to perform rigid transformation processing is not limited to this example. For example, the number of feature points in the environment shared between the position / orientation calculation unit 703 and the loop source measurement point can be compared with the number of feature points in the environment shared between the position / orientation calculation unit 703 and the loop destination measurement point, and then, if the former number is greater than the latter, it is determined that rigid transformation processing is to be performed. Alternatively, as in the first exemplary embodiment, whether to perform rigid transformation processing is determined based on the distance from each measurement point on the posture graph.

[0147] While the first exemplary embodiment illustrates an example in which a user externally operates a mobile object, the configuration of mobile object system 601 is not limited to this example. For example, a passenger-carrying mobile object that a user can ride and directly operate may be used. Alternatively, a mobile object including functionality for autonomous travel along a preset route may be used. In this case, autonomous travel can be achieved by generating control information for mobile object system 601 based on environmental map data 701 and position / orientation information calculated by position / orientation calculation unit 703, and driving mobile unit 607 via control device 606. Furthermore, mobile object system 601 can update environmental map data 701 based on sensor information acquired from sensor 603 during autonomous travel, according to the methods described in the first or second exemplary embodiments.

[0148] Although the exemplary embodiment shows a configuration in which the moving unit 607 is a wheel, a configuration may be adopted in which a plurality of propellers or the like are mounted on the moving body system 601 , the moving body system 601 flies in the air, and the sensor 603 observes in the direction of the ground.

[0149] The present disclosure may also be implemented by providing a program for implementing one or more functions according to the above exemplary embodiments to a system or device via a network or storage medium, and one or more processors in a computer of the system or device read out and execute the program. The present disclosure may also be implemented by a circuit (e.g., an ASIC) for implementing one or more functions according to the above exemplary embodiments.

[0150] A training model obtained by machine learning can be used instead of the position / orientation calculation unit 703 and the measurement point generation unit 704 included in the above-mentioned processing unit to perform processing. In this case, for example, multiple combinations of input data and output data of the processing unit are prepared as learning data, and knowledge is acquired through machine learning to generate a training model so that the training model outputs output data corresponding to the input data based on the acquired knowledge as a result. The training model can be constructed using, for example, a neural network model. The training model operates in collaboration with a CPU or a graphics processing unit (GPU) as a program for performing processing equivalent to the processing unit, thereby performing processing corresponding to the processing unit. The training model can be updated as needed after the predetermined processing.

[0151] According to exemplary embodiments of the present disclosure, it is possible to prevent generation of redundant measurement points due to an increase in time to correct map data during a closed-loop process.

[0152] Other embodiments

[0153] The embodiments of the present disclosure may also be implemented by reading and executing computer-executable instructions (e.g., one or more programs) recorded on a storage medium (also more fully referred to as a "non-transitory computer-readable storage medium") to perform one or more functions of the above-described embodiments, and / or a computer of a system or device including one or more circuits (e.g., an application-specific integrated circuit (ASIC)) for performing one or more functions of the above-described embodiments. Furthermore, the embodiments of the present disclosure may be implemented using a method in which the computer of the system or device, for example, reads and executes the computer-executable instructions from the storage medium to perform one or more functions of the above-described embodiments, and / or controls the one or more circuits to perform one or more functions of the above-described embodiments. The computer may include one or more processors (e.g., a central processing unit (CPU), a microprocessor unit (MPU)), and may include a network of separate computers or separate processors to read and execute the computer-executable instructions. The computer-executable instructions may be provided to the computer, for example, from a network or the storage medium. The storage medium may include, for example, a hard disk, a random access memory (RAM), a read-only memory (ROM), a memory of a distributed computing system, an optical disc (such as a compact disc (CD), a digital versatile disc (DVD), or a Blu-ray disc (BD) TM ), one or more of a flash memory device and a memory card, etc.

[0154] The embodiments of the present invention can also be implemented by the following method, that is, providing software (program) that performs the functions of the above-mentioned embodiments to a system or device through a network or various storage media, and the computer or central processing unit (CPU) or microprocessing unit (MPU) of the system or device reads and executes the program.

[0155] While the present disclosure has been described with reference to exemplary embodiments, the scope of the following claims is to be accorded the broadest interpretation so as to encompass all such modifications and equivalent structures and functions.

Claims

1. An information processing device, comprising: an acquisition unit configured to acquire sensor information obtained by measuring a surrounding environment, the sensor information being obtained using an output from a sensor configured to move; a generating unit configured to generate map data indicating a map based on a movement path of the sensor, the map data including measurement points associating the sensor information with a position and an orientation of the sensor; an estimating unit configured to estimate a position and an orientation of the sensor based on the measurement point and the sensor information acquired by the acquiring unit; a detection unit configured to detect a second measurement point based on an output of the sensor when revisiting the first measurement point; a first correction unit configured to, at a position where revisiting is detected, correct position and orientation information of one or more third measurement points used by the estimation unit to estimate the position and orientation by using a difference between position and orientation information of the second measurement point before alignment with the first measurement point and position and orientation information of the second measurement point after alignment with the first measurement point; as well as a second correction unit configured to, after the first correction unit corrects the position and orientation information of the third measurement point, correct the position and orientation information associated with a plurality of measurement points other than the third measurement point, the number of the plurality of measurement points being greater than the number of measurement points corrected by the first correction unit.

2. The information processing device according to claim 1, wherein In a case where a loop is detected by the detection unit, the generation unit does not generate a new measurement point until the correction by the first correction unit is completed.

3. The information processing device according to claim 1, wherein The first correction unit corrects the position and orientation using a rigid transformation.

4. The information processing device according to claim 1, wherein: The first correction unit calculates the corrected position and orientation of the second measurement point based on the relative position and orientation between the position and orientation of the second measurement point and the position and orientation of the first measurement point.

5. The information processing apparatus according to claim 4, wherein: The first correction unit adds a difference in position and orientation calculated based on the position and orientation at the second measurement point before correction and the position and orientation corresponding to the second measurement point after correction to the position and orientation at the third measurement point to correct the position and orientation at the third measurement point. The information processing device according to claim 1 , in, The map data generated by the generating unit includes position information about feature points in the surrounding environment, wherein the measurement point includes information about the feature point observed at the measurement point based on the sensor information, and The estimating unit estimates the position and orientation of the sensor based on the feature points.

7. The information processing device according to claim 6, in, The third measurement point is one or more measurement points at which the feature point used for estimation by the estimation unit is observed, and The first correction unit further corrects the position of the feature point.

8. The information processing apparatus according to claim 7, wherein: The first correction unit integrates the first feature point and the second feature point when a first feature point whose position is corrected and a second feature point included in the map data are located within a predetermined distance and a feature of the first feature point observed based on the sensor information is similar to a feature of the second feature point observed based on the sensor information.

9. The information processing apparatus according to claim 8, wherein: The first correction unit corrects the position and orientation by performing bundle adjustment on the integrated feature points and the measurement points observing the integrated feature points.

10. The information processing apparatus according to claim 1, wherein: The first correction unit performs correction according to the distance between the second measurement point and the first measurement point.

11. The information processing device according to claim 1, in, The map data includes a pose graph including relative positions and orientations between measurement points, and The second correction unit corrects the positions and orientations of the plurality of measurement points to minimize errors in relative positions and orientations calculated based on the relative positions and orientations between the measurement points shown in the posture graph and the positions and orientations of the plurality of measurement points.

12. The information processing device according to claim 1, in, The sensor is a camera, and The sensor information is an image.

13. The information processing apparatus according to claim 1, wherein: The detection unit detects a loop when an object observed based on the sensor information associated with the first measurement point is output from the sensor that has moved and is observed based on the sensor information acquired by the acquisition unit.

14. The information processing device according to claim 1, further comprising: the sensor; as well as Mobile unit.

15. The information processing apparatus according to claim 14, wherein: The moving unit moves based on a user's operation.

16. The information processing apparatus according to claim 14, wherein: The mobile unit moves along a preset route based on the position and orientation estimated by the estimation unit.

17. The information processing apparatus according to claim 14, wherein: The moving unit is a wheel or a propeller.

18. The information processing apparatus according to claim 1, wherein: The correction by the first correction unit is performed before the correction by the second correction unit is performed.

19. An information processing method, comprising: acquiring sensor information obtained from a sensor configured to measure a surrounding environment, the sensor information being obtained using an output from a sensor configured to move; generating map data indicating a map based on a movement path of the sensor, the map data including measurement points associating the sensor information with a position and orientation of the sensor; estimating a position and orientation of the sensor based on the measurement points and the acquired sensor information; When revisiting the first measurement point, detecting a second measurement point based on the output of the sensor; Correcting, at the position where the revisit is detected, position and orientation information of one or more third measurement points used to estimate the position and orientation by using a difference between the position and orientation information of the second measurement point before alignment with the first measurement point and the position and orientation information of the second measurement point after alignment with the first measurement point; as well as After correcting the position and orientation information of the third measurement point, correcting the position and orientation information associated with a plurality of measurement points other than the third measurement point, the number of the plurality of measurement points being greater than the number of the corrected third measurement points.

20. A non-transitory storage medium storing a program for causing a computer to execute an information processing method, the information processing method comprising: acquiring sensor information obtained from a sensor configured to measure a surrounding environment, the sensor information being obtained using an output from a sensor configured to move; generating map data indicating a map based on a movement path of the sensor, the map data including measurement points associating the sensor information with a position and orientation of the sensor; estimating a position and orientation of the sensor based on the measurement points and the acquired sensor information; When revisiting the first measurement point, detecting a second measurement point based on the output of the sensor; Correcting, at the position where the revisit is detected, position and orientation information of one or more third measurement points used to estimate the position and orientation by using a difference between the position and orientation information of the second measurement point before alignment with the first measurement point and the position and orientation information of the second measurement point after alignment with the first measurement point; as well as After correcting the position and orientation information of the third measurement point, correcting the position and orientation information associated with a plurality of measurement points other than the third measurement point, the number of the plurality of measurement points being greater than the corrected third measurement point.

Images