JP7750150B2 - Map creation device, method, and program - Google Patents

Description

This disclosure relates to a map creation device and a program.

There is currently technology for generating map data.

For example, a 3D map generation system has been 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.

Japanese Patent Application Laid-Open No. 2017-198517

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 results are off, the off-center points are added to the map, which could result in a deterioration in the accuracy of the map. Furthermore, the data set used for the map with the reduced accuracy is also used for the next matching, which could further reduce the accuracy of the map. Furthermore, matching is difficult in environments with few features, which could make it impossible to generate a map.

This disclosure was made in consideration of the above circumstances, and aims to provide a map creation device and program that can generate highly accurate map data while maintaining accuracy by correcting errors in the position and attitude used in inertial navigation.

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 the mobile body and acquires position and orientation information according to time using inertial navigation; a creation unit that creates map data by coordinate transforming and plotting the point cloud using the position and orientation information for the 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 to obtain a true position and orientation for the plotted time, calculates the difference between the true position and orientation and the position and orientation information, and estimates errors in acceleration and angular velocity used in calculating the position and orientation 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 mobile body to correct errors in the acceleration and angular velocity used in calculating the position and orientation information.

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 the error 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.

The map creation device also determines whether 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; 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.

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 equipped on a moving object; acquire position and orientation information according to time using inertial navigation; create map data by transforming the point cloud into coordinates and plotting it using the position and orientation information for the time corresponding to the time of the acquired point cloud; match the created map data with the point cloud using a predetermined matching method to obtain a true position and orientation for the plotted time; calculate the difference between the true position and orientation and the position and orientation information; estimate errors in acceleration and angular velocity used in calculating the position and orientation information using inertial navigation; and output the estimated acceleration and angular velocity errors acquired from an inertial sensor equipped on the moving object to correct the errors in the acceleration and angular velocity used in calculating the position and orientation information.

The map creation device and program disclosed herein can generate highly accurate map data while maintaining accuracy by correcting errors in the position and attitude used in inertial navigation.

FIG. 2 is a block diagram showing the hardware configuration of the map creation device. 1 is a block diagram illustrating a configuration of a map creation device according to an embodiment of the present disclosure. 10 is a flowchart showing the flow of a position and orientation estimation processing routine. 10 is a flowchart showing the flow of a map generation processing routine.

Embodiments of the present disclosure will be described in detail below with reference to the drawings.

Figure 1 is a block diagram showing the hardware configuration of the map creation device 100. As shown in Figure 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, 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 that they can communicate with each other.

The CPU 11 is a central processing unit that executes various programs and controls each component. That is, the CPU 11 reads programs from the ROM 12 or 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 storage 14. In this embodiment, the programs are stored in the ROM 12 or storage 14.

ROM 12 stores various programs and data. RAM 13 temporarily stores programs or data as a working area. Storage 14 is composed of a storage device such as an HDD (Hard Disk Drive) or SSD (Solid State Drive), and stores various programs, including the operating system, and various data.

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

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

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).

Figure 2 is a block diagram showing the configuration of a map creation device according to an embodiment of the present disclosure. As shown in Figure 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.

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 mapping 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 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.

An overview of this embodiment will be explained below. One issue is that when the inertial sensor 92 is operated for a long period of time, it will contain errors due to the effects of heat, etc., and it is anticipated that simple integration will not be able to accurately determine the position and orientation. 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.

Next, we will explain each functional component of the map creation device 100. Each functional component is realized when the CPU 11 reads a program stored in the ROM 12 or storage 14, expands it in the RAM 13, and executes it.

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

The inertial navigation processing unit 102 calculates the position and attitude of the moving body 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 that records 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 externally, and the position and attitude information may be obtained from an external source. Furthermore, as will be described later, the processing of the inertial navigation processing unit 102 is executed as part of the position and attitude estimation processing routine, separate from the map generation processing routine.

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

The creation unit 116 performs coordinate transformation on the point cloud using position and orientation information for the time 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 the base point for scanning by the distance measurement 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 space 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. In other words, in this embodiment, matching is not used directly to generate map data, but rather for correction, as described below. This makes it possible to avoid the difficulty of map creation due to matching, which has been an issue.

The matching unit 118 uses a predetermined matching method to match the created map data with the point cloud for the plotted time to determine the true position and attitude, calculates the difference between the true position and attitude and the position and attitude information, and estimates the acceleration and angular velocity errors 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 greater than or equal to a threshold, and if so, outputs the error to the correction unit 120.

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 value. In other words, if 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 assumes that there is an error in the acceleration and angular velocity detected by the inertial sensor 92 and performs correction. Note that the correction unit 120 may be provided externally, like the inertial navigation processing unit 102.

Next, we will explain the operation of the map creation device 100 according to an embodiment of the present disclosure.

Figure 3 is a flowchart showing the flow of the position and attitude estimation processing routine. Figure 4 is a flowchart showing the flow of the map generation processing routine. The CPU 11 reads a program from the ROM 12 or storage 14, expands it into 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 external to the map creation device 100, the position and attitude estimation processing routine can be executed externally.

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.

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; if an error has been received, the process proceeds to step S106.

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

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 102.

In step S108, the CPU 11, as the inertial navigation processing unit 102, calculates the position and attitude of the moving body by integration using 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 position and attitude estimation processing routine is repeated each time the inertial sensor 92 detects something.

In step S110, the CPU 11 determines whether or not to terminate the position and orientation estimation processing routine. If it determines that the processing should be terminated, it terminates the processing; if it determines that the processing should not be terminated, it returns to step S100 and repeats the processing. Here, it determines that the processing should be terminated when an end command is received from outside or when the map data generation processing routine has ended.

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

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.

In step S204, the CPU 11, functioning as the position and orientation information acquisition unit 114, determines whether position and orientation information for the time corresponding to the time of the acquired point cloud has been received. If so, the process proceeds to step S206; if not, the process returns to step S200 and repeats the process.

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

In step S208, the CPU 11, as the creation unit 116, creates map data by plotting the coordinate-transformed point cloud in a three-dimensional space for map creation.

In step S210, the CPU 11, as the matching unit 118, matches the created map data with the point cloud using a predetermined matching method to determine the true position and orientation for the plotted time.

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

In step S214, the CPU 11, functioning as the matching unit 118, determines whether the error is greater than or equal to a threshold. If the error is greater than or equal to 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 greater than or equal to the threshold, the process proceeds to step S216.

In step S216, the CPU 11 determines whether to terminate the map generation processing routine. If it determines that it should terminate, the processing terminates; if it determines that it should not terminate, the processing returns to step S200 and is repeated. Here, it determines that the processing has terminated when an external termination command is received or when map data for a predetermined search range or conditions has been generated.

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

Note that this disclosure is not limited to the above-described embodiments, and various modifications and applications are possible without departing from the spirit and scope of the present invention.

In the above embodiments, the processes (position and orientation estimation processing routine and map generation processing routine) executed by the CPU after loading the software (program) may be executed by various processors other than the CPU. Examples of processors in this case 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 with circuit configurations 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.

In addition, while the above embodiment describes a configuration in which the program for executing the processing is pre-stored (installed) in ROM or storage, this is not limited to this. The program may also 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.

90 Distance measurement sensor 92 Inertial sensor 100 Map creation device 102 Inertial navigation processing unit 110 Acquisition unit 112 Point cloud acquisition unit 114 Position and orientation information acquisition unit 116 Creation unit 118 Matching unit 120 Correction unit

Claims (4)

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. 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 . The map creation device of claim 1 or 2 determines whether the position and orientation information has been received at a time corresponding to the time of the acquired point cloud, and if so, creates the map data; if not, 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. 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.