Map creation device, method, and program

Abstract

To enable map data to be generated with good accuracy while correcting errors of the position and attitude used in inertial navigation and maintaining accuracy.SOLUTION: A map creation device includes: an acquisition unit that acquires a point cloud that corresponds to the time of day for each scan by a ranging sensor that is provided to a mobile entity and acquires position-attitude information that corresponds to the time of day by an inertial navigation method; a creation unit that creates map data by converting into coordinates and plotting the point cloud using the position-attitude information at the time of day corresponding to the time of day of the acquired point cloud; and a matching unit that matches the created map data and the point cloud with regard to the plotted time by a prescribed matching method to find the true position-attitude, calculates a difference between the true position- attitude and the position-attitude information, and estimates errors of the acceleration and angular speed that are used in calculating position-attitude information by an inertial navigation method. The map creation device outputs the estimated errors in order for correcting errors of the acceleration and angular speed that are used in calculating position-attitude information.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present disclosure relates to a map creation device and a program. [Background technology]

[0002] Conventionally, there are techniques related to generating map data.

[0003] For example, a 3D map generation system is disclosed (see Patent Document 1). In Patent Document 1, a 2D grid map is generated using a data set obtained by scanning the surroundings with a 3D laser, and the 2D grid map is aligned with a predetermined 3D coordinate system, and predetermined matching and correction are performed to generate a 3D map. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Japanese Patent Application Publication No. 2017-198517 Summary of the Invention [Problem to be solved by the invention]

[0005] In Patent Document 1, alignment is performed by matching each point converted into a three-dimensional coordinate system according to a time-series data set. However, if the matching result is inaccurate, the inaccurate points are added to the map, which may result in a deterioration in the accuracy of the map. Furthermore, the data set used for the map with inaccuracy is also used for the next matching, which may further deteriorate the accuracy of the map. Furthermore, matching is difficult in an environment with few features, which may make it impossible to generate a map.

[0006] The present disclosure has been made in consideration of the above circumstances, and aims to provide a map creation device and program that can generate map data with high accuracy while correcting errors in position and attitude used in inertial navigation to maintain accuracy. [Means for solving the problem]

[0007] In order to achieve the above object, the map creation device of the present disclosure includes: an acquisition unit that acquires a point cloud according to time for each scan by a ranging sensor provided on a moving body, and acquires position and attitude information according to time by inertial navigation; a creation unit that creates map data by coordinate transforming and plotting the point cloud using the position and attitude information for a time corresponding to the time of the acquired point cloud; and a matching unit that matches the created map data with the point cloud using a predetermined matching method for the plotted time to obtain a true position and attitude, calculates a difference between the true position and attitude and the position and attitude information, and estimates errors in acceleration and angular velocity used in calculating the position and attitude information in inertial navigation, and is configured to output the estimated errors in acceleration and angular velocity acquired from the inertial sensor provided on the moving body in order to correct errors in the acceleration and angular velocity used in calculating the position and attitude information.

[0008] The map creation device may further include an inertial navigation processing unit and a correction unit, wherein the matching unit outputs the estimated error to the correction unit when the estimated error is equal to or greater than a threshold, the correction unit corrects errors in the acceleration and angular velocity acquired from the inertial sensor when the estimated error is equal to or greater than a threshold, and the inertial navigation processing unit calculates the position and attitude information using inertial navigation from the acceleration and angular velocity acquired from the inertial sensor.

[0009] The map creation device also determines whether or not it has received the position and orientation information at the time corresponding to the time of the acquired point cloud, and if it has received it creates the map data, but if it has not received it does not generate the map data and repeats acquiring the point cloud and the position and orientation information until it receives the corresponding position and orientation information.

[0010] The program disclosed herein causes a computer to execute the following processes: acquire a point cloud according to time for each scan by a ranging sensor provided on a moving body; acquire position and attitude information according to time using inertial navigation; create map data by coordinate-transforming and plotting the point cloud using the position and attitude information for a time corresponding to the time of the acquired point cloud; obtain a true position and attitude for the plotted time by matching the created map data with the point cloud using a predetermined matching method; calculate a difference between the true position and attitude and the position and attitude information; estimate errors in acceleration and angular velocity used in calculating the position and attitude information in inertial navigation; and output the estimated acceleration and angular velocity acquired from an inertial sensor provided on the moving body in order to correct the errors in the acceleration and angular velocity used in calculating the position and attitude information. [Effects of the Invention]

[0011] According to the map creation device and program of the present disclosure, it is possible to generate map data with high accuracy while maintaining accuracy by correcting errors in the position and attitude used in inertial navigation. [Brief explanation of the drawings]

[0012] [Figure 1] FIG. 2 is a block diagram showing the hardware configuration of the map creation device. [Figure 2] 1 is a block diagram illustrating a configuration of a map creation device according to an embodiment of the present disclosure. [Figure 3] 10 is a flowchart showing the flow of a position and orientation estimation processing routine. [Figure 4] 10 is a flowchart showing the flow of a map generation processing routine. DETAILED DESCRIPTION OF THE INVENTION

[0013] Hereinafter, embodiments of the present disclosure will be described in detail with reference to the drawings.

[0014] Fig. 1 is a block diagram showing the hardware configuration of a map creation device 100. As shown in Fig. 2, the map creation device 100 is a computer that includes a CPU (Central Processing Unit) 11, a ROM (Read Only Memory) 12, a RAM (Random Access Memory) 13, a storage 14, an input unit 15, a display unit 16, and a communication interface (I / F) 17. Each component is connected to each other via a bus 19 so as to be able to communicate with each other.

[0015] The CPU 11 is a central processing unit that executes various programs and controls each part. That is, the CPU 11 reads programs from the ROM 12 or the storage 14 and executes the programs using the RAM 13 as a work area. The CPU 11 controls the above components and performs various arithmetic processing in accordance with the programs stored in the ROM 12 or the storage 14. In this embodiment, the programs are stored in the ROM 12 or the storage 14.

[0016] The ROM 12 stores various programs and various data. The RAM 13 temporarily stores programs or data as a working area. The storage 14 is configured with a storage device such as an HDD (Hard Disk Drive) or an SSD (Solid State Drive) and stores various programs including the operating system and various data.

[0017] The input unit 15 includes a pointing device such as a mouse and a keyboard, and is used to perform various inputs.

[0018] The display unit 16 is, for example, a liquid crystal display, and displays various information. The display unit 16 may function as the input unit 15 by adopting a touch panel system.

[0019] The communication interface 17 is an interface for communicating with other devices such as terminals, and uses standards such as Ethernet (registered trademark), FDDI, and Wi-Fi (registered trademark).

[0020] Fig. 2 is a block diagram showing the configuration of a map creation device according to an embodiment of the present disclosure. As shown in Fig. 2, the map creation device 100 includes an inertial navigation processing unit 102, an acquisition unit 110, a creation unit 116, a matching unit 118, and a correction unit 120. The acquisition unit 110 includes a point cloud acquisition unit 112 and a position and orientation information acquisition unit 114.

[0021] The map creation device 100 also acquires information from a ranging sensor 90 and an inertial sensor 92. The ranging sensor 90 and the inertial sensor 92 are provided on a mobile object, such as a robot, that moves through the space for which map creation is to be performed, and are connected to the map creation device 100 via a network (not shown). The ranging sensor 90 uses a measuring device such as a LiDAR (Light Detection and Ranging). The inertial sensor 92 uses any IMU (Inertial Measurement Unit) that can measure at least the acceleration and angular velocity of the mobile object. The map creation device 100 acquires a point cloud according to time from the ranging sensor 90, and acquires acceleration and angular velocity according to time from the inertial sensor 92.

[0022] An overview of this embodiment will be described. One issue is that the inertial sensor 92 will contain errors due to the effects of heat and other factors when operated for a long period of time, making it impossible to accurately determine the position and orientation by simple integration. For this reason, in this embodiment, the input point cloud is matched to the generated map data to determine the true position and orientation, which is then fed back to the inertial navigation. This makes it possible to correct errors in the acceleration and angular velocity acquired by the inertial sensor 92. Accurate map data can be created by looping the process of creating a map using this inertial navigation and estimating the position and orientation by point cloud matching using the created map.

[0023] Next, a description will be given of each functional configuration of the map creation device 100. Each functional configuration is realized by the CPU 11 reading out a program stored in the ROM 12 or the storage 14, expanding it in the RAM 13, and executing it.

[0024] The point cloud acquisition unit 112 acquires a point cloud according to time for each scan by the distance measurement sensor 90. A point cloud according to time is a point cloud associated with time information that records the time of scanning.

[0025] The inertial navigation processing unit 102 calculates the position and attitude of the moving object by integration using inertial navigation based on the received acceleration and angular velocity, and outputs the calculated position and attitude information according to time. The received acceleration and angular velocity are those output from the inertial sensor 92 or those corrected by the correction unit 120. The position and attitude information according to time refers to position and attitude information associated with time information recording the time at which the acceleration and angular velocity were detected. Note that the inertial navigation processing unit 102 may not be provided inside the map creation device 100 but may be provided outside the map creation device 100, and the position and attitude information may be acquired from an external device. As will be described later, the processing of the inertial navigation processing unit 102 is executed in the processing of a position and attitude estimation processing routine, separate from the map creation processing routine.

[0026] The position and attitude information acquisition unit 114 acquires position and attitude information corresponding to the time from the inertial navigation processing unit 102. In order to generate map data, which will be described later, it is necessary to acquire position and attitude information at a time corresponding to the time of the point cloud acquired by the point cloud acquisition unit 112. Therefore, acquisition of the point cloud and acquisition of the position and attitude information are repeated until position and attitude information at a time corresponding to the time of the acquired point cloud can be acquired.

[0027] The creation unit 116 performs coordinate transformation on the point cloud using position and orientation information corresponding to the time of the acquired point cloud. The creation unit 116 creates map data by plotting the coordinate-transformed point cloud in a three-dimensional space for map creation. The coordinate transformation can be performed by determining a base point for scanning by the ranging sensor 90 from the position and orientation of the base point of the mobile object, and converting the position of the scanned point cloud according to the base point into three-dimensional spatial coordinates. Since it is assumed that the accuracy of the acceleration and angular velocity used to determine the position and orientation information is maintained, map data can be created without matching the point cloud. That is, in this embodiment, matching is not used directly to generate map data, but is used for correction as described below. This avoids the previously recognized difficulty of map generation due to matching.

[0028] The matching unit 118 matches the created map data with the point cloud using a predetermined matching method for the plotted time to obtain the true position and attitude, calculates the difference between the true position and attitude and the position and attitude information, and estimates errors in acceleration and angular velocity used to calculate the position and attitude information in inertial navigation. ICP matching or NDT matching is used for matching. The matching method involves matching the coordinate-transformed point cloud with the point cloud of the created map data. The matching unit 118 determines whether the error is equal to or greater than a threshold, and if so, outputs the error to the correction unit 120.

[0029] The correction unit 120 uses the estimated error to correct errors in the acceleration and angular velocity acquired from the inertial sensor 92. The correction unit 120 performs correction when the error is equal to or greater than a threshold. That is, when the true position and attitude obtained as a result of matching by the matching unit 118 deviates from the position and attitude information acquired from the inertial navigation processing unit 102, the correction unit 120 performs correction assuming that there is an error in the acceleration and angular velocity detected from the inertial sensor 92. Note that the correction unit 120 may be provided externally, like the inertial navigation processing unit 102.

[0030] Next, the operation of the map creation device 100 according to the embodiment of the present disclosure will be described.

[0031] Fig. 3 is a flowchart showing the flow of the position and attitude estimation processing routine. Fig. 4 is a flowchart showing the flow of the map generation processing routine. The CPU 11 reads out a program from the ROM 12 or the storage 14, expands it in the RAM 13, and executes it, thereby performing the position and attitude estimation processing routine and the map generation processing routine. Note that if the inertial navigation processing unit 102 and the correction unit 120 are provided outside the map creation device 100, the position and attitude estimation processing routine may be executed externally.

[0032] The position and orientation estimation processing routine will now be described. In step S100, the CPU 11 acquires acceleration and angular velocity from the inertial sensor 92. The acceleration and angular velocity are associated with detected time information.

[0033] In step S102, the CPU 11 determines whether an error has been received from the map generation processing routine. The error is received by input (B) from the map generation processing routine. If an error has not been received, the process proceeds to step S104, and if an error has been received, the process proceeds to step S106.

[0034] In step S104, the CPU 11 outputs the acceleration and angular velocity acquired in step S100 to the inertial navigation processing unit .

[0035] In step S106, the CPU 11, functioning as the correction unit 120, corrects the acceleration and angular velocity acquired in step S100 based on the error, and outputs the corrected acceleration and angular velocity to the inertial navigation processing unit .

[0036] In step S108, the CPU 11, functioning as the inertial navigation processing unit 102, calculates the position and attitude of the moving body by integration using the inertial navigation using the received acceleration and angular velocity, and outputs position and attitude information according to time. Here, the position and attitude information is output to the map generation processing routine as output (A). The above-described position and attitude estimation processing routine is repeated every time the inertial sensor 92 detects something.

[0037] In step S110, the CPU 11 determines whether to end the position and orientation estimation processing routine. If it is determined that the processing should be ended, the processing ends. If it is determined that the processing should not be ended, the processing returns to step S100 and is repeated. Here, the processing is determined to be ended when an end command is received from outside or when the map data generation processing routine is ended.

[0038] The map generation processing routine will be described below. In step S200, the CPU 11 functions as the point cloud acquisition unit 112 to acquire a point cloud according to time for each scan by the distance measurement sensor 90.

[0039] In step S202, the CPU 11, functioning as the position and attitude information acquisition unit 114, acquires position and attitude information according to time from the inertial navigation processing unit 102. The position and attitude information is acquired by input of (A) from the position and attitude estimation processing routine. If there is no input of (A), the position and attitude information is not acquired.

[0040] In step S204, the CPU 11 determines whether or not it has received the position and orientation information at the time corresponding to the time of the acquired point cloud as the position and orientation information acquisition unit 114. If it has received the position and orientation information, it proceeds to step S206, and if it has not received the position and orientation information, it returns to step S200 and repeats the process.

[0041] In step S206, the CPU 11, functioning as the creation unit 116, performs coordinate transformation on the point cloud using the position and orientation information at the time corresponding to the time of the acquired point cloud.

[0042] In step S208, the CPU 11 functions as the creation unit 116 to create map data by plotting the coordinate-converted point cloud in a three-dimensional space for map creation.

[0043] In step S210, the CPU 11 functions as the matching unit 118 to match the created map data with the point cloud for the plotted time using a predetermined matching method to obtain the true position and orientation.

[0044] In step S212, the CPU 11, functioning as the matching unit 118, calculates the difference between the true position and attitude information and estimates errors in the acceleration and angular velocity used to calculate the position and attitude information in inertial navigation.

[0045] In step S214, the CPU 11, functioning as the matching unit 118, determines whether the error is equal to or greater than a threshold. If the error is equal to or greater than the threshold, the error is output. Here, the error is output to the position and orientation estimation processing routine as output (B). If the error is not equal to or greater than the threshold, the process proceeds to step S216.

[0046] In step S216, the CPU 11 determines whether or not to end the map generation processing routine. If it is determined that the processing should be ended, the processing ends. If it is determined that the processing should not be ended, the processing returns to step S200 and is repeated. Here, the processing is determined to be ended when an end command is received from the outside or when map data for a predetermined search range or conditions is generated.

[0047] As described above, the map creation device 100 according to this embodiment can generate accurate map data while maintaining accuracy by correcting errors in the position and attitude used in inertial navigation.

[0048] The present disclosure is not limited to the above-described embodiment, and various modifications and applications are possible without departing from the spirit and scope of the present invention.

[0049] In the above embodiment, the processes (position and orientation estimation process routine and map generation process routine) executed by the CPU after reading the software (program) may be executed by various processors other than the CPU. Examples of such processors include programmable logic devices (PLDs) such as field-programmable gate arrays (FPGAs), whose circuit configuration can be changed after manufacture, and dedicated electrical circuits such as application-specific integrated circuits (ASICs), which are processors having a circuit configuration specifically designed to execute specific processes. Furthermore, the processes may be executed by one of these various processors, or by a combination of two or more processors of the same or different types (e.g., multiple FPGAs, or a combination of a CPU and an FPGA). Furthermore, the hardware structure of these various processors is, more specifically, an electrical circuit that combines circuit elements such as semiconductor devices.

[0050] In the above embodiment, the program for executing the process is pre-stored (installed) in a ROM or storage device, but the present invention is not limited to this. The program may be provided in a form recorded on a non-transitory recording medium such as a CD-ROM (Compact Disk Read Only Memory), a DVD-ROM (Digital Versatile Disk Read Only Memory), or a USB (Universal Serial Bus) memory. The program may also be downloaded from an external device via a network. [Explanation of symbols]

[0051] 90 Distance sensor 92 Inertial Sensor 100 Map-making device 102 Inertial Navigation Processing Unit 110 Acquisition Department 112 Point cloud acquisition part 114 Position and orientation information acquisition unit 116 Creation Department 118 Matching Department 120 Correction unit

Claims

1. an acquisition unit that acquires a point cloud according to time for each scan by a distance measurement sensor provided in the moving object and acquires position and orientation information according to time by inertial navigation; a creating unit that creates map data by converting the coordinates of the point cloud using the position and orientation information at a time corresponding to the time of the acquired point cloud and plotting the coordinates; a matching unit that, for the plotted time, matches the created map data with the point cloud using a predetermined matching method to obtain a true position and attitude, calculates a difference between the true position and attitude and the position and attitude information, and estimates errors in acceleration and angular velocity used in calculating the position and attitude information in inertial navigation, outputting the acceleration and angular velocity acquired from an inertial sensor provided on the moving body, the estimated errors being used to correct errors in the acceleration and angular velocity used to calculate the position and orientation information; Mapping device.

2. further including an inertial navigation processing unit and a correction unit; the matching unit outputs the estimated error to the correction unit when the estimated error is equal to or greater than a threshold; the correction unit corrects errors in the acceleration and angular velocity acquired from the inertial sensor when the estimated errors are equal to or greater than a threshold; the inertial navigation processing unit calculates the position and attitude information using inertial navigation from the acceleration and angular velocity acquired from the inertial sensor; The map creation device according to claim 1 .

3. 3. The map creation device according to claim 1, wherein the map creation device determines whether the position and orientation information at a time corresponding to the time of the acquired point cloud has been received, and if the position and orientation information has been received, creates the map data, but if the position and orientation information has not been received, does not generate the map data, and repeatedly acquires the point cloud and the position and orientation information until the corresponding position and orientation information is received.

4. On the computer, A point cloud is acquired according to time for each scan by a ranging sensor equipped on the moving object, and position and orientation information according to time is acquired by inertial navigation. creating map data by converting the coordinates of the point cloud using the position and orientation information at a time corresponding to the time of the acquired point cloud and plotting the coordinates; For the plotted time, a true position and attitude is obtained by matching the created map data with the point cloud using a predetermined matching method, a difference between the true position and attitude and the position and attitude information is calculated, and errors in acceleration and angular velocity used in calculating the position and attitude information in inertial navigation are estimated; outputting the acceleration and angular velocity acquired from an inertial sensor provided on the moving body, the estimated errors being used to correct errors in the acceleration and angular velocity used to calculate the position and orientation information; A program that executes a process.

Images