Position estimation device, position estimation method, and program

Abstract

To achieve accurate real-time self-localization and environmental mapping while keeping costs down. [Solution] The position estimation device 10 includes a data acquisition unit 11 that acquires odometry data of a moving object and 3D point cloud data of a target area; a first position estimation unit 11 that estimates a first candidate for the position of the moving object using the odometry data, 3D point cloud data and map data each time data is acquired; a second position estimation unit 13 that identifies a point cloud corresponding to the map data of the target area from the acquired 3D point cloud data and estimates a second candidate for the position of the moving object using the identified point cloud; a condition determination unit 13 that determines whether the first candidate satisfies the set conditions; and a position determination unit 14 that determines the position as the first candidate if the set conditions are met, and determines the position as the second candidate if the set conditions are not met.

Description

Technical Field

[0001] The present disclosure relates to a position estimation device and a position estimation method for estimating the self-position of a moving object, and further relates to a program for realizing these.

Background Art

[0002] SLAM (Simultaneous Localization and Mapping) is a technique for simultaneously performing self-position estimation of a moving object and environmental map construction (see, for example, Patent Document 1). According to SLAM, self-position estimation and environmental map construction can be performed without depending on GPS (Global Positioning System), and thus SLAM is utilized in fields such as autonomous mobile robots, drones, and automatic driving.

[0003] In general, SLAM is classified into three types: a scan matching type, a Bayesian filter type, and a graph-based type. Among these, in the scan matching type, scan data from LiDAR and an occupancy grid map are overlapped to perform self-position estimation and environmental map construction (see Non-Patent Document 1). Also, the scan matching type performs self-position estimation and environmental map construction in real time and is classified as online SLAM.

[0004] Also, the Bayesian filter type is classified as online SLAM, similar to the scan matching type. In the Bayesian filter type, a self-position estimation candidate is calculated from the odometry information of the moving object, and further, a particle filter is applied to the map data and the scan data from LiDAR to calculate the reliability of the calculated self-position estimation candidate. Then, the self-position estimation candidate with the highest reliability is estimated as the self-position (see Non-Patent Document 2).

[0005] On the other hand, in graph-based systems, SLAM is modeled using a graph structure, and self-localization and environment map construction are performed at specific timings. Unlike scan-matching and Bayesian filter systems, graph-based systems are classified as offline SLAM. In graph-based systems, scan data from LiDAR is used to search for corresponding similar point clouds in the map data, a transformation matrix (rotation and translation) that optimizes the geometric agreement between point clouds is calculated, and the self-localization is estimated from the obtained transformation matrix (see Non-Patent Document 3).

[0006] Of the SLAM methods mentioned above, scan-matching systems can achieve high accuracy in self-localization and environmental map construction, but they have the problem of high processing load and high cost when used on moving objects. On the other hand, Bayesian filter systems and graph-based systems can reduce processing load and lower costs compared to scan-matching systems. Therefore, in fields where cost is a concern, Bayesian filter systems or graph-based systems are mainly used. [Prior art documents] [Patent Documents]

[0007] [Patent Document 1] Japanese Patent Publication No. 2020-160594 [Non-patent literature]

[0008] [Non-Patent Document 1] Fernando Auat Cheein, Miguel Torres-Torritib, Joan R. Rosell-Poloc, “Usability Analysis of Scan Matching Techniques for Localization of Field Machinery in Avocado Groves”, Preprint submitted to Computers and Electronics in Agriculture, April 21, 2019 [Non-Patent Document 2] Naoki Akai, "Utilization of Bayesian Filter for High-Performance Self-Position Estimation", Transactions of the Robotics Society of Japan, Vol. 40, No. 10, pp. 882, 2022 [Non-Patent Document 3] Wolfgang Hess, Damon Kohler, Holger Rapp, Daniel Andor, "Real-time loop closure in 2D LIDAR SLAM", 2016 IEEE International Conference on Robotics and Automation (ICRA), Inter Net <URL: https: / / ieeexplore.ieee.org / document / <7487258> [Summary of the Invention] [Problems to be Solved by the Invention]

[0009] By the way, the scan matching system has the advantage of being able to realize online SLAM at a low cost, but there is a problem that the accuracy in self-position estimation and environmental map construction is not high.

[0010] On the other hand, in the graph-based system, since it is executed offline, the accuracy of self-position estimation and environmental map construction can be improved, but there is a problem that the application is limited because it cannot be executed in real time.

[0011] An example of the object of the present disclosure is to accurately achieve real-time self-position estimation and environmental map construction while suppressing costs. [Means for Solving the Problems]

[0012] To achieve the above object, a position estimation device according to an aspect of the present disclosure A data acquisition unit that acquires odometry data of a moving body and three-dimensional point cloud data of a target area output from a sensor mounted on the moving body, Each time the odometry data and the 3D point cloud data are acquired, a first position estimation unit estimates a first candidate for the position of the moving object using the odometry data, the 3D point cloud data, and the map data of the target area. A second position estimation unit, which, when the amount of data in the acquired 3D point cloud data satisfies the set conditions, identifies a point cloud corresponding to the map data of the target area from the acquired 3D point cloud data, and estimates a second candidate for the position of the moving object using the identified point cloud, A condition determination unit that determines whether the first candidate satisfies the set conditions, A position determination unit that determines the position of the moving body to the first candidate if the first candidate satisfies the setting conditions, and determines the position of the moving body to the second candidate if the first candidate does not satisfy the setting conditions, It is characterized by having the following features.

[0013] Furthermore, in order to achieve the above objective, the position estimation method in one aspect of this disclosure is: A data acquisition step involves acquiring odometry data of a moving object and 3D point cloud data of a target area output from a sensor mounted on the moving object. Each time the odometry data and the 3D point cloud data are acquired, a first position estimation step is performed, in which a first candidate for the position of the moving object is estimated using the odometry data, the 3D point cloud data, and the map data of the target area. If the amount of data in the acquired 3D point cloud data satisfies the set conditions, a second position estimation step is performed in which a point cloud corresponding to the map data of the target area is identified from the acquired 3D point cloud data, and a second candidate for the position of the moving object is estimated using the identified point cloud. A condition determination step to determine whether the first candidate satisfies the set conditions, A position determination step in which, if the first candidate satisfies the setting conditions, the position of the moving body is determined to be the first candidate, and if the first candidate does not satisfy the setting conditions, the position of the moving body is determined to be the second candidate, characterized by having

[0014] Furthermore, to achieve the above object, a program in one aspect of the present disclosure causes a computer to acquire odometry data of a moving object and three-dimensional point cloud data of a target area output from a sensor mounted on the moving object in a data acquisition step; in a first position estimation step, estimate a first candidate for the position of the moving object using the odometry data, the three-dimensional point cloud data, and map data of the target area each time the odometry data and the three-dimensional point cloud data are acquired; in a second position estimation step, when the amount of the acquired three-dimensional point cloud data satisfies a set condition, specify a point cloud corresponding to the map data of the target area from the acquired three-dimensional point cloud data, and estimate a second candidate for the position of the moving object using the specified point cloud; in a condition determination step, determine whether the first candidate satisfies the set condition; in a position determination step, when the first candidate satisfies the set condition, determine the position of the moving object as the first candidate, and when the first candidate does not satisfy the set condition, determine the position of the moving object as the second candidate; and execute, characterized by

Advantages of the Invention

[0015] As described above, according to the present disclosure, it is possible to accurately achieve real-time self-position estimation and environmental map construction while suppressing costs.

Brief Description of the Drawings

[0016] [Figure 1] FIG. 1 is a configuration diagram showing a schematic configuration of an example of a position estimation device. [Figure 2] FIG. 2 is a configuration diagram specifically showing a configuration of an example of a position estimation device. [Figure 3] FIG. 3 is a diagram showing an example of an environmental map. ​ [Figure 4] Figure 4 is a flowchart showing an example of the operation of the position estimation device 10. [Figure 5] Figure 5 is a block diagram showing an example of a computer that implements a position estimation device. [Modes for carrying out the invention]

[0017] (Embodiment) The position estimation device, position estimation method, and program in the embodiment will be described below with reference to Figures 1 to 4.

[0018] [Device configuration] First, we will explain the schematic configuration of an example of a position estimation device using Figure 1. Figure 1 is a schematic diagram showing the schematic configuration of an example of a position estimation device.

[0019] The position estimation device 10 shown in Figure 1 is a device for estimating the self-position of a moving object. As shown in Figure 1, the position estimation device 10 comprises a data acquisition unit 11, a first position estimation unit 12, a second position estimation unit 13, a condition determination unit 14, and a position determination unit 15.

[0020] The data acquisition unit 11 acquires odometry data of the moving object and 3D point cloud data of the target area output from sensors mounted on the moving object.

[0021] The first position estimation unit 12 estimates a first candidate for the position of the moving object using the odometry data, 3D point cloud data, and map data of the target area each time odometry data and 3D point cloud data are acquired.

[0022] The second position estimation unit 13 identifies a point cloud corresponding to the map data of the target area from the acquired 3D point cloud data when the amount of data in the acquired 3D point cloud data satisfies the set conditions. Then, the second position estimation unit 13 estimates a second candidate for the position of the moving object using the identified point cloud.

[0023] The condition determination unit 14 determines whether the first candidate estimated by the first position estimation unit 12 satisfies the set conditions.

[0024] If the position determination unit 14 determines that the first candidate position satisfies the set conditions, the position determination unit 15 determines the position of the moving object as the first candidate. On the other hand, if the position determination unit 14 determines that the first candidate position does not satisfy the set conditions, the position determination unit 15 determines the position of the moving object as the second candidate.

[0025] As described above, in the position estimation device 10, the first position estimation unit 12 performs online position estimation of the moving object, and the second position estimation unit 13 performs offline position estimation of the moving object. The position estimation device 10 then adopts the self-position obtained by online self-position estimation if the set conditions are met, for example, if the accuracy of self-position estimation meets the set criteria. On the other hand, if the accuracy of self-position estimation does not meet the set criteria, the position estimation device 10 adopts the self-position obtained by offline self-position estimation.

[0026] Thus, the position estimation device 10 can leverage the advantages of both online and offline self-position estimation while mitigating the disadvantages of each. The position estimation device 10 can accurately achieve real-time self-position estimation and environmental map construction while keeping costs down.

[0027] Next, we will specifically explain the configuration and function of the position estimation device 10 using Figure 2. Figure 2 is a configuration diagram that specifically shows the configuration of an example of a position estimation device.

[0028] As shown in Figure 2, in this embodiment, the position estimation device 10 is mounted on an autonomously mobile body 100 such as a robot. In addition to the position estimation device 10, the mobile body 100 includes a 3D sensor 20, a control device 30, a steering device 40, a powertrain 50, a sensor device 60, and a memory unit 70.

[0029] In this embodiment, the 3D sensor 20 is mounted on the moving object 100 so as to be able to sense the direction of its movement. The 3D sensor 20 measures the distance to each part of the object using the reflection time of light such as laser light. Using the measured distances to each part, the 3D sensor 20 generates 3D point cloud data of the object at a set frame rate and outputs the generated 3D point cloud data.

[0030] In the example shown in Figure 2, a rotating 3D LiDAR is used as the 3D sensor 20. Alternatively, multiple fixed-angle 3D LiDARs may be used as the 3D sensor 20. Furthermore, in addition to LiDAR, a TOF (Time of Flight) camera can also be used as the 3D sensor 20.

[0031] The control device 30 is built on a computer mounted on the mobile unit 100. The control device 30 controls the direction and speed of the mobile unit 100 using an environmental map, which will be described later. For example, the control device 30 uses the environmental map to set a route from the current location to the destination, determines the direction and speed of the mobile unit 100 so that it moves along the set route, and controls the powertrain 50 and steering device 40 accordingly.

[0032] The powertrain 50 consists of an electric motor for driving, a power transmission mechanism, etc. Tires, caterpillar tracks, etc. are connected to the powertrain 50. The powertrain 50 rotates the tires, caterpillar tracks, etc. in response to instructions from the control device 30.

[0033] The steering device 40 includes a mechanism for controlling the direction of the steering wheels of the moving body 100. The steering device 40 determines the direction of the steering wheels in response to instructions from the control device 30. The steering device 40 may also include a mechanism for controlling the direction of movement by controlling the torque of the left and right drive wheels.

[0034] In this embodiment, the position estimation device 10 is built on a computer mounted on the mobile body 100 using a program described later in this embodiment. Alternatively, the position estimation device 10 may be built using a device separate from the computer mounted on the mobile body 100 (for example, an electronic circuit).

[0035] The sensor device 60 is equipped with various sensors for detecting the distance traveled, direction of movement, and speed of the moving object 100. The sensor device 60 outputs information that identifies the distance traveled, direction of movement, and speed as odometry data. The storage unit 70 stores map data of the area (target area) in which the moving object is located.

[0036] In this embodiment, the data acquisition unit 11 acquires 3D point cloud data from the 3D sensor 20 on a frame-by-frame basis. The data acquisition unit 11 outputs the acquired 3D point cloud data to the first position estimation unit 12 and the second position estimation unit 13, and further stores the acquired 3D point cloud data in the storage unit 70. The data acquisition unit 11 also acquires odometry data output from the sensor device 60 and outputs the acquired odometry data to the first position estimation unit 12.

[0037] In this embodiment, the first position estimation unit 12 performs online SLAM. Each time the data acquisition unit 11 acquires 3D point cloud data and odometry data, the first position estimation unit 12 uses the odometry data, 3D point cloud data, and map data of the target area to construct an environmental map (first environmental map) around the mobile object 100 and estimates a first candidate for the position of the mobile object 100.

[0038] Specifically, the first position estimation unit 12 first determines the direction of movement of the moving object 100 and the distance traveled from the previous frame using odometry data. Then, using the determined direction of movement and distance traveled, the first position estimation unit 12 matches the 3D point cloud data of the previous frame with the 3D point cloud data of the current frame, and updates the environment map based on the matching result. If an environment map has not yet been created, the first position estimation unit 12 generates a new environment map using the 3D point cloud data of the current frame. Figure 3 shows an example of an environment map.

[0039] The first position estimation unit 12 then compares the updated environmental map (or newly generated environmental map) with the map data of the target area to identify the latest portion of the environmental map on the map data. The first position estimation unit 12 then identifies the current position of the mobile body 100 from the position of the latest portion of the environmental map on the map data, and designates the identified position as the first candidate.

[0040] Furthermore, in this embodiment, the first position estimation unit 12 calculates the likelihood of the estimated first candidate using noise generated during the creation of odometry data and 3D point cloud data. The likelihood is used as a judgment condition in the condition determination unit 14. Existing techniques can also be used to calculate the likelihood.

[0041] Specifically, the first position estimation unit 12 calculates the likelihood using a combination of multiple likelihood models (particle filters). Examples of likelihood models include a likelihood model representing Gaussian noise, a likelihood model representing obstacle countermeasure noise, a likelihood model representing the maximum distance noise of the 3D sensor 20, and a likelihood model representing random noise.

[0042] Of these, the likelihood model representing Gaussian noise outputs a high likelihood as the estimated position of the moving object 100 on the map data approaches a wall. The model representing obstacle countermeasure noise outputs a high likelihood if there is an obstacle not shown on the map data in front of the position reached by the laser beam emitted by the 3D sensor 20.

[0043] The likelihood model representing the maximum distance noise of the 3D sensor 20 outputs a high likelihood when the reflected light of the laser beam emitted from the 3D sensor 20 may not be received by the 3D sensor 20, and the moving object 100 is estimated to be located in the area corresponding to the maximum reach of the laser beam in that case. The likelihood model representing random noise outputs a constant likelihood across the entire observable range, taking into account the case where the moving object 100 is moved by a third party.

[0044] In this embodiment, the second position estimation unit 13 performs offline SLAM. When the amount of 3D point cloud data stored in the storage unit 70 satisfies a set condition (when the number of stored frames reaches a predetermined value), the second position estimation unit 13 constructs an environment map (second environment map) using the 3D point cloud data stored in the storage unit 70.

[0045] Specifically, the second position estimation unit 13 matches 3D point cloud data between consecutive frames in a time series and extracts matching feature points. Then, the second position estimation unit 13 constructs an environmental map by merging the 3D point cloud data between consecutive frames. This identifies the point cloud corresponding to the map data of the target area.

[0046] Next, the second position estimation unit 13 compares the constructed environmental map with the map data of the target area to identify the latest portion of the environmental map on the map data. Then, the second position estimation unit 13 identifies the current position of the mobile body 100 from the position on the map data of the latest portion of the environmental map, and designates the identified position as the second candidate.

[0047] The condition determination unit 14 determines that the set condition is met if the likelihood described above is equal to or greater than the threshold, and determines that the set condition is not met if the likelihood described above is less than the threshold.

[0048] In this embodiment, the position determination unit 15 determines the position of the moving body 100 as the first candidate if the likelihood described above is greater than or equal to a threshold (i.e., the first candidate satisfies the setting conditions). On the other hand, the position determination unit 15 determines the position of the moving body 100 as the second candidate if the likelihood described above is less than a threshold (i.e., the first candidate does not satisfy the setting conditions).

[0049] [Device operation] Next, an example of the operation of the position estimation device 10 will be explained using Figure 4. Figure 4 is a flowchart showing an example of the operation of the position estimation device 10. In the following explanation, Figures 1 to 3 will be referred to as appropriate. In this embodiment, the position estimation method is performed by operating the position estimation device 10. Therefore, in this embodiment, the explanation of the position estimation method will be replaced by the following explanation of the operation of the position estimation device 10.

[0050] As shown in Figure 4, first, the data acquisition unit 11 acquires the latest 3D point cloud data from the 3D sensor 20 on a frame-by-frame basis and acquires the latest odometry data from the sensor device 60 (step A1).

[0051] Next, the first position estimation unit 12 uses the 3D point cloud data and odometry data acquired in step A1 and the map data of the target area stored in the storage unit 70 to construct an environmental map of the area around the mobile object 100 and estimate a first candidate for the position of the mobile object 100 (step A2).

[0052] Next, the first position estimation unit 12 calculates the likelihood of the first candidate estimated in step A2 using noise generated during the creation of odometry data and 3D point cloud data (step A3).

[0053] Next, the second position estimation unit 13 determines whether the amount of 3D point cloud data acquired and stored in step A1 satisfies the set conditions (step A4). Specifically, the second position estimation unit 13 determines, for example, whether the number of 3D point cloud data frames stored in the storage unit 70 by the data acquisition unit 11 has reached a predetermined value.

[0054] If the result of step A4 is that the amount of 3D point cloud data does not meet the set conditions (step A4: No), then step A7, described below, will be executed.

[0055] If the result of the determination in step A4 indicates that the amount of 3D point cloud data meets the set conditions (step A4: Yes), the second position estimation unit 13 constructs an environmental map using the accumulated 3D point cloud data and uses this environmental map to estimate a second candidate for the position of the moving object 100 (step A5).

[0056] Next, the condition determination unit 14 determines whether the likelihood calculated in step A3 is equal to or greater than a threshold (step A6).

[0057] If the result of the determination in step A6 is that the likelihood calculated in step A3 is greater than or equal to the threshold (step A6: Yes), the position determination unit 15 determines the position of the moving body 100 to the first candidate estimated in step A2 (step A7).

[0058] On the other hand, if the result of the determination in step A6 is that the likelihood calculated in step A3 is not equal to or greater than the threshold (step A6: No), the position determination unit 15 determines the position of the moving body 100 to the second candidate estimated in step A5 (step A8).

[0059] After step A7 or step A8 is executed, the position determination unit 15 determines whether it has been instructed to terminate the process (step A9). If the result of the determination in step A9 is that it has not been instructed to terminate the process (step A9: No), step A1 is executed again. On the other hand, if the result of the determination in step A9 is that it has been instructed to terminate the process (step A9: Yes), the process in the position estimation device 10 is terminated.

[0060] [Effects in the embodiment] The position estimation device 10 estimates a first candidate for the position of the moving object 100 using online SLAM and a second candidate for the position of the moving object 100 using offline SLAM. The position estimation device 10 adopts the first candidate as the position of the moving object 100 when the likelihood is above a threshold, but adopts the second candidate as the position of the moving object 100 when the likelihood falls below the threshold, i.e., when the accuracy of position estimation by online SLAM decreases.

[0061] Therefore, the position estimation device 10 can leverage the advantages of both online SLAM and offline SLAM for self-position estimation while mitigating the disadvantages of each. The position estimation device 10 can accurately achieve real-time self-position estimation and environmental map construction while keeping costs down.

[0062] [Differentiation] The following describes some variations of the embodiment.

[0063] Variation 1: In the example described above, the first position estimation unit 12 calculates the likelihood for the determination condition. However, in the modified example 1, the first position estimation unit 12 calculates an index that shows the discrepancy between the 3D point cloud data and the constructed environmental map instead of the likelihood. In this case, the condition determination unit 14 determines that the set condition is met if the index is below a threshold, and determines that the set condition is not met if the index exceeds the threshold.

[0064] One method for calculating the index is to match 3D point cloud data with an environmental map, calculate the percentage of points in the 3D point cloud data that do not match the environmental map, and use that percentage as the index.

[0065] Variation 2: In Modification 2, the likelihood is not calculated by the first position estimation unit 12. Instead, in Modification 2, the condition determination unit 14 determines that the first candidate has been estimated by the first position estimation unit 12 as a set condition. That is, the condition determination unit 14 determines that the set condition is met as long as the first candidate has been estimated by the first position estimation unit 12, and determines that the set condition is not met if the first candidate has not been estimated by the first position estimation unit 12.

[0066] Online SLAM may fail to estimate the position depending on the situation (for example, when the moving object 100 moves back and forth in a straight line). Therefore, in Modification 2, if the first position estimation unit 12 has not estimated a first candidate, the second candidate is determined to be the position of the moving object 100.

[0067] [program] In this embodiment, the program is one that causes a computer to execute steps A1 to A9 shown in Figure 4. By installing and executing this program on a computer, a position estimation device and a position estimation method can be realized. In this case, the computer's processor functions as a data acquisition unit 11, a first position estimation unit 12, a second position estimation unit 13, a condition determination unit 14, and a position determination unit 15, and performs processing.

[0068] Furthermore, a computer mounted on the mobile unit 100 can be considered. Other computers include general-purpose PCs, smartphones, and tablet devices. In addition, an external computer connected to the computer mounted on the mobile unit 100 in a data communication manner can also be considered.

[0069] Furthermore, the program in the embodiment may be executed by a computer system constructed by multiple computers. In this case, for example, each computer's processor may function as one of the following: a data acquisition unit 11, a first position estimation unit 12, a second position estimation unit 13, a condition determination unit 14, and a position determination unit 15.

[0070] [Physical configuration] Here, a computer that implements the position estimation device 10 by executing the program in the embodiment will be described with reference to Figure 5. Figure 5 is a block diagram showing an example of a computer that implements the position estimation device.

[0071] As shown in Figure 5, the computer 110 comprises a CPU (Central Processing Unit) 111, main memory 112, storage device 113, input interface 114, display controller 115, data reader / writer 116, and communication interface 117. Each of these components is connected to the others via a bus 121, enabling data communication.

[0072] Furthermore, the computer 110 may include a GPU (Graphics Processing Unit) or an FPGA (Field-Programmable Gate Array) in addition to, or instead of, the CPU 111. In this embodiment, the GPU or FPGA can execute the program in the embodiment.

[0073] The CPU 111 loads the program in the embodiment, which consists of a set of codes stored in the storage device 113, into the main memory 112, and performs various calculations by executing each code in a predetermined order. The main memory 112 is typically a volatile storage device such as DRAM (Dynamic Random Access Memory).

[0074] Furthermore, the program in this embodiment is provided stored on a computer-readable recording medium 120. The program in this embodiment may also be distributed over the internet via a communication interface 117.

[0075] Specific examples of the storage device 113 include hard disk drives and semiconductor storage devices such as flash memory. The input interface 114 mediates data transmission between the CPU 111 and input devices 118 such as a keyboard and mouse. The display controller 115 is connected to the display device 119 and controls the display on the display device 119.

[0076] The data reader / writer 116 mediates data transmission between the CPU 111 and the recording medium 120, reads programs from the recording medium 120, and writes processing results from the computer 110 to the recording medium 120. The communication interface 117 mediates data transmission between the CPU 111 and other computers.

[0077] Furthermore, specific examples of the recording medium 120 include general-purpose semiconductor memory devices such as CF (Compact Flash®) and SD (Secure Digital), magnetic recording media such as Flexible Disks, or optical recording media such as CD-ROMs (Compact Disk Read Only Memory).

[0078] Furthermore, the position estimation device 10 in this embodiment can be implemented not by a computer with a program installed, but by using hardware corresponding to each part, such as electronic circuits. Moreover, the position estimation device 10 may be partially implemented by a program and the remaining part by hardware. In this embodiment, the computer is not limited to the computer shown in Figure 5.

[0079] Some or all of the embodiments described above can be expressed by (Appendix 1) to (Appendix 15) described below, but are not limited to the following descriptions.

[0080] (Note 1) A data acquisition unit acquires odometry data of a moving object and 3D point cloud data of a target area output from a sensor mounted on the moving object. Each time the odometry data and the 3D point cloud data are acquired, a first position estimation unit estimates a first candidate for the position of the moving object using the odometry data, the 3D point cloud data, and the map data of the target area. A second position estimation unit, which, when the amount of data in the acquired 3D point cloud data satisfies the set conditions, identifies a point cloud corresponding to the map data of the target area from the acquired 3D point cloud data, and estimates a second candidate for the position of the moving object using the identified point cloud, A condition determination unit that determines whether the first candidate satisfies the set conditions, A position determination unit that determines the position of the moving body to the first candidate if the first candidate satisfies the setting conditions, and determines the position of the moving body to the second candidate if the first candidate does not satisfy the setting conditions, A position estimation device characterized by being equipped with the following features.

[0081] (Note 2) The first position estimation unit estimates a first candidate for the position of the moving object by constructing a first environmental map around the moving object using the odometry data, the three-dimensional point cloud data, and the map data of the target area. The second position estimation unit constructs a second environmental map around the moving object using the acquired three-dimensional point cloud data, thereby identifying point clouds corresponding to the map data of the target area, and estimates a second candidate for the position of the moving object using the second environmental map. The position estimation device described in Appendix 1.

[0082] (Note 3) The first position estimation unit calculates the likelihood of the estimated first candidate using the noise generated during the generation of the odometry data and the 3D point cloud data. The condition determination unit determines that the setting condition is met when the likelihood is equal to or greater than the threshold. The position estimation device described in Appendix 2.

[0083] (Note 4) The first position estimation unit calculates an index that shows the discrepancy between the 3D point cloud data and the constructed environmental map, The condition determination unit determines that the set condition is met when the index is below the threshold. The position estimation device described in Appendix 2.

[0084] (Note 5) The condition determination unit determines that the first candidate has been estimated by the first position estimation unit as the setting condition. The position estimation device described in Appendix 2.

[0085] (Note 6) A data acquisition step involves acquiring odometry data of a moving object and 3D point cloud data of a target area output from a sensor mounted on the moving object. Each time the odometry data and the 3D point cloud data are acquired, a first position estimation step is performed, in which a first candidate for the position of the moving object is estimated using the odometry data, the 3D point cloud data, and the map data of the target area. If the amount of data in the acquired 3D point cloud data satisfies the set conditions, a second position estimation step is performed in which a point cloud corresponding to the map data of the target area is identified from the acquired 3D point cloud data, and a second candidate for the position of the moving object is estimated using the identified point cloud. A condition determination step to determine whether the first candidate satisfies the set conditions, A position determination step in which, if the first candidate satisfies the setting conditions, the position of the moving body is determined to be the first candidate, and if the first candidate does not satisfy the setting conditions, the position of the moving body is determined to be the second candidate, A method for estimating a position, characterized by having the following features.

[0086] (Note 7) In the first position estimation step, a first candidate for the position of the moving object is estimated by constructing a first environmental map around the moving object using the odometry data, the three-dimensional point cloud data, and the map data of the target area. In the second position estimation step, a second environmental map is constructed around the moving object using the acquired three-dimensional point cloud data to identify the point cloud corresponding to the map data of the target area, and a second candidate for the position of the moving object is estimated using the second environmental map. The position estimation method described in Appendix 6.

[0087] (Note 8) In the first position estimation step, the likelihood of the estimated first candidate is calculated using the noise generated during the generation of the odometry data and the 3D point cloud data. In the condition determination step, if the likelihood is greater than or equal to a threshold, it is determined that the setting condition is met. The position estimation method described in Appendix 7.

[0088] (Note 9) In the first position estimation step, an index is calculated that shows the discrepancy between the 3D point cloud data and the constructed environmental map. In the condition determination step, if the index is below a threshold, it is determined that the setting condition is met. The position estimation method described in Appendix 7.

[0089] (Note 10) In the condition determination step, it is determined that the first candidate has been estimated by the first position estimation step, and this is set as the condition. The position estimation method described in Appendix 7.

[0090] (Note 11) On the computer, A data acquisition step involves acquiring odometry data of a moving object and 3D point cloud data of a target area output from a sensor mounted on the moving object. Each time the odometry data and the 3D point cloud data are acquired, a first position estimation step is performed, in which a first candidate for the position of the moving object is estimated using the odometry data, the 3D point cloud data, and the map data of the target area. If the amount of data in the acquired 3D point cloud data satisfies the set conditions, a second position estimation step is performed in which a point cloud corresponding to the map data of the target area is identified from the acquired 3D point cloud data, and a second candidate for the position of the moving object is estimated using the identified point cloud. A condition determination step to determine whether the first candidate satisfies the set conditions, A position determination step in which, if the first candidate satisfies the setting conditions, the position of the moving body is determined to be the first candidate, and if the first candidate does not satisfy the setting conditions, the position of the moving body is determined to be the second candidate, A program that executes something.

[0091] (Note 12) In the first position estimation step, a first candidate for the position of the moving object is estimated by constructing a first environmental map around the moving object using the odometry data, the three-dimensional point cloud data, and the map data of the target area. In the second position estimation step, a second environmental map is constructed around the moving object using the acquired three-dimensional point cloud data to identify the point cloud corresponding to the map data of the target area, and a second candidate for the position of the moving object is estimated using the second environmental map. The program described in Appendix 11.

[0092] (Note 13) In the first position estimation step, the likelihood of the estimated first candidate is calculated using the noise generated during the generation of the odometry data and the 3D point cloud data. In the condition determination step, if the likelihood is greater than or equal to a threshold, it is determined that the setting condition is met. The program described in Appendix 12.

[0093] (Note 14) In the first position estimation step, an index is calculated that shows the discrepancy between the 3D point cloud data and the constructed environmental map. In the condition determination step, if the index is below a threshold, it is determined that the setting condition is met. The program described in Appendix 12.

[0094] (Note 15) In the condition determination step, it is determined that the first candidate has been estimated by the first position estimation step, and this is set as the condition. The program described in Appendix 12. [Industrial applicability]

[0095] As described above, this disclosure enables accurate real-time self-localization and environmental mapping while keeping costs down. This disclosure is useful in fields where SLAM is used. [Explanation of Symbols]

[0096] 10 Position estimation device 11 Data Acquisition Unit 12 First position estimation unit 13. Second position estimation unit 14 Condition judgment section 15 Positioning section 20 3D sensors 30 Control device 40 Steering gear 50 Powertrain 60 Sensor device 70 Memory section 100 Mobile Units 110 Computer 111 CPU 112 Main Memory 113 Storage device 114 Input Interface 115 Display Controller 116 Data Readers / Writers 117 Communication Interface 118 Input devices 119 Display device 120 recording media 121 Bus

Claims

1. A data acquisition unit acquires odometry data of a moving object and 3D point cloud data of a target area output from a sensor mounted on the moving object. Each time the odometry data and the three-dimensional point cloud data are acquired, a first position estimation unit estimates a first candidate for the position of the moving object using the odometry data, the three-dimensional point cloud data, and the map data of the target area. A second position estimation unit, which, when the amount of data in the acquired three-dimensional point cloud data satisfies the set conditions, identifies a point cloud corresponding to the map data of the target area from the acquired three-dimensional point cloud data, and estimates a second candidate for the position of the moving object using the identified point cloud, A condition determination unit that determines whether the first candidate satisfies the set conditions, A position determination unit that determines the position of the moving body to the first candidate if the first candidate satisfies the setting conditions, and determines the position of the moving body to the second candidate if the first candidate does not satisfy the setting conditions, A position estimation device characterized by being equipped with the following features.

2. The first position estimation unit estimates a first candidate for the position of the moving object by constructing a first environmental map around the moving object using the odometry data, the three-dimensional point cloud data, and the map data of the target area. The second position estimation unit constructs a second environmental map around the moving object using the acquired three-dimensional point cloud data, thereby identifying the point cloud corresponding to the map data of the target area, and estimates a second candidate for the position of the moving object using the second environmental map. The position estimation device according to claim 1.

3. The first position estimation unit calculates the likelihood of the estimated first candidate using the noise generated during the generation of the odometry data and the three-dimensional point cloud data. The condition determination unit determines that the setting condition is met when the likelihood is equal to or greater than the threshold. The position estimation device according to claim 2.

4. The first position estimation unit calculates an index that shows the discrepancy between the three-dimensional point cloud data and the constructed environmental map, The condition determination unit determines that the setting condition is met when the index is below the threshold. The position estimation device according to claim 2.

5. The condition determination unit determines that the first candidate has been estimated by the first position estimation unit as the setting condition. The position estimation device according to claim 2.

6. A data acquisition step involves acquiring odometry data of a moving object and 3D point cloud data of a target area output from a sensor mounted on the moving object. Each time the odometry data and the three-dimensional point cloud data are acquired, a first position estimation step is performed, in which a first candidate for the position of the moving object is estimated using the odometry data, the three-dimensional point cloud data, and the map data of the target area. If the amount of data in the acquired three-dimensional point cloud data satisfies the set conditions, a second position estimation step is performed in which a point cloud corresponding to the map data of the target area is identified from the acquired three-dimensional point cloud data, and a second candidate for the position of the moving object is estimated using the identified point cloud. A condition determination step to determine whether the first candidate satisfies the set conditions, A position determination step in which, if the first candidate satisfies the setting conditions, the position of the moving body is determined to be the first candidate, and if the first candidate does not satisfy the setting conditions, the position of the moving body is determined to be the second candidate, A method for estimating a position, characterized by having the following features.

7. In the first position estimation step, a first candidate for the position of the moving object is estimated by constructing a first environmental map around the moving object using the odometry data, the three-dimensional point cloud data, and the map data of the target area. In the second position estimation step, a second environmental map is constructed around the moving object using the acquired three-dimensional point cloud data to identify the point cloud corresponding to the map data of the target area, and a second candidate for the position of the moving object is estimated using the second environmental map. The position estimation method according to claim 6.

8. In the first position estimation step, the likelihood of the estimated first candidate is calculated using the noise generated during the generation of the odometry data and the 3D point cloud data. In the condition determination step, if the likelihood is greater than or equal to a threshold, it is determined that the setting condition is met. The position estimation method according to claim 7.

9. In the first position estimation step, an index is calculated that shows the discrepancy between the three-dimensional point cloud data and the constructed environmental map. In the condition determination step, if the index is below a threshold, it is determined that the setting condition is met. The position estimation method according to claim 7.

10. In the condition determination step, it is determined that the first candidate has been estimated by the first position estimation step, and this is set as the condition. The position estimation method according to claim 7.

11. On the computer, A data acquisition step involves acquiring odometry data of a moving object and 3D point cloud data of a target area output from a sensor mounted on the moving object. Each time the odometry data and the three-dimensional point cloud data are acquired, a first position estimation step is performed, in which a first candidate for the position of the moving object is estimated using the odometry data, the three-dimensional point cloud data, and the map data of the target area. If the amount of data in the acquired three-dimensional point cloud data satisfies the set conditions, a second position estimation step is performed in which a point cloud corresponding to the map data of the target area is identified from the acquired three-dimensional point cloud data, and a second candidate for the position of the moving object is estimated using the identified point cloud. A condition determination step to determine whether the first candidate satisfies the set conditions, A position determination step in which, if the first candidate satisfies the setting conditions, the position of the moving body is determined to be the first candidate, and if the first candidate does not satisfy the setting conditions, the position of the moving body is determined to be the second candidate, A program that executes something.

12. In the first position estimation step, a first candidate for the position of the moving object is estimated by constructing a first environmental map around the moving object using the odometry data, the three-dimensional point cloud data, and the map data of the target area. In the second position estimation step, a second environmental map is constructed around the moving object using the acquired three-dimensional point cloud data to identify the point cloud corresponding to the map data of the target area, and a second candidate for the position of the moving object is estimated using the second environmental map. The program according to claim 11.

13. In the first position estimation step, the likelihood of the estimated first candidate is calculated using the noise generated during the generation of the odometry data and the 3D point cloud data. In the condition determination step, if the likelihood is greater than or equal to a threshold, it is determined that the setting condition is met. The program according to claim 12.

14. In the first position estimation step, an index is calculated that shows the discrepancy between the three-dimensional point cloud data and the constructed environmental map. In the condition determination step, if the index is below a threshold, it is determined that the setting condition is met. The program according to claim 12.

15. In the condition determination step, it is determined that the first candidate has been estimated by the first position estimation step, and this is set as the condition. The program according to claim 12.

Images