Position and Orientation Estimation Method, Position and Orientation Estimation Device, and Program

Abstract

This position and attitude estimation device acquires three-dimensional point cloud data at each clock time measured every time a first period of time elapses and position data at each clock time measured every time a second period of time, which is longer than the first period of time, elapses. The position and attitude estimation device estimates a local position in a local coordinate system and a local attitude in the local coordinate system. The position and attitude estimation device determines an estimated absolute position and an estimated absolute attitude in an absolute coordinate system every time the position data is acquired. The position and attitude estimation device generates provisional three-dimensional point cloud data in the absolute coordinate system every time the position data is acquired. The position and attitude estimation device generates composite data by integrating the provisional three-dimensional point cloud data and map point cloud data generated from three-dimensional point cloud data measured in the past, and corrects the estimated absolute position and the estimated absolute attitude so as to give a high degree of agreement between the composite data and the map point cloud data.

Description

Technical Field

[0001] The disclosed technology relates to a position and orientation estimation method, a position and orientation estimation device, and a program.

Background Art

[0002] Conventionally, a technology for estimating the position of a moving object using LiDAR (Light Detection and Ranging) is known (for example, Non-Patent Document 1 and Non-Patent Document 2). The technologies disclosed in Non-Patent Document 1 and Non-Patent Document 2 estimate the position and orientation of a moving object in a local coordinate system based on data obtained by LiDAR.

[0003] In addition, a technology for estimating the absolute position and absolute orientation of a moving object when the moving object equipped with GPS (Global Positioning System), LiDAR, and IMU (Inertial Measurement Unit) moves is known (for example, Non-Patent Document 3).

Prior Art Documents

Non-Patent Documents

[0004]

Non-Patent Document 1

Non-Patent Document 2

[0005] The technologies disclosed in Non-Patent Document 1 and Non-Patent Document 2 are technologies for estimating the position and orientation of a moving object in a local coordinate system based only on data obtained by LiDAR. Therefore, the technology disclosed in Non-Patent Document 1 cannot estimate the position and orientation of the moving object in an absolute coordinate system with a predetermined position on the earth as the origin.

[0006] On the other hand, when using the technology disclosed in Non-Patent Document 3, it is necessary to perform accurate calibration between LiDAR and IMU. Specifically, the difference in the installation angles of the two sensors, LiDAR and IMU, needs to be known. Therefore, even if the technology disclosed in Non-Patent Document 3 is used in a state where calibration between LiDAR and IMU has not been performed, the estimation accuracy of the position and orientation of the moving body in the absolute coordinate system is low.

[0007] The disclosed technology has been made in view of the above points, and aims to accurately estimate the position and orientation of the moving body in the absolute coordinate system based on the position data obtained by the position measuring device mounted on the moving body and the three-dimensional point cloud data obtained by the measuring device mounted on the moving body.

Means for Solving the Problems

[0008] A first aspect of the present disclosure is to obtain three-dimensional point cloud data at each time measured every time a first time elapses by a measuring device mounted on a moving body, and position data at each time measured every time a second time longer than the first time elapses by a position measuring device mounted on the moving body. Every time the three-dimensional point cloud data is obtained as the first time elapses, based on the obtained three-dimensional point cloud data, a local position representing the position of the moving body in a local coordinate system with the position at the start of movement of the moving body as the origin, and a local attitude representing the attitude of the moving body in the local coordinate system are estimated. Every time the position data is obtained as the second time elapses, an estimated absolute position, which is the position of the moving body in an absolute coordinate system with a predetermined position on the earth as the origin, is estimated based on the local position of the moving body, and an estimated absolute attitude, which is the attitude of the moving body in the absolute coordinate system, is estimated based on the local attitude of the moving body. Every time the position data is obtained as the second time elapses, for each of the three-dimensional point cloud data at each time measured every time the first time elapses, provisional three-dimensional point cloud data at each time in the absolute coordinate system is generated. For each of the provisional three-dimensional point cloud data at each time, composite data is generated by integrating the provisional three-dimensional point cloud data and map point cloud data generated from three-dimensional point cloud data measured in the past. By correcting the estimated absolute position and the estimated absolute attitude so that the degree of coincidence between the composite data and the map point cloud data becomes high, a corrected absolute position with the estimated absolute position corrected and a corrected absolute attitude with the estimated absolute attitude corrected are generated. This is a position and attitude estimation method executed by a computer for the above process.

[0009] A second aspect of the present disclosure includes an acquisition unit that acquires three-dimensional point cloud data at each time measured every time a first time elapses by a measuring device mounted on a moving body, and position data at each time measured every time a second time longer than the first time elapses by a position measuring device mounted on the moving body; a first estimation unit that, every time the three-dimensional point cloud data is acquired as the first time elapses, estimates a local position representing the position of the moving body in a local coordinate system with the position at the start of movement of the moving body as the origin, and a local attitude representing the attitude of the moving body in the local coordinate system; a second estimation unit that, every time the position data is acquired as the second time elapses, estimates an estimated absolute position that is the position of the moving body in an absolute coordinate system with a predetermined position on the earth as the origin based on the local position of the moving body, and estimates an estimated absolute attitude that is the attitude of the moving body in the absolute coordinate system based on the local attitude of the moving body; and a correction unit that, every time the position data is acquired as the second time elapses, for each of the three-dimensional point cloud data at each time measured every time the first time elapses, generates provisional three-dimensional point cloud data at each time in the absolute coordinate system, generates composite data by integrating the provisional three-dimensional point cloud data at each time with map point cloud data generated from the three-dimensional point cloud data measured in the past, and generates a corrected absolute position with the estimated absolute position corrected and a corrected absolute attitude with the estimated absolute attitude corrected by correcting the estimated absolute position and the estimated absolute attitude so that the degree of coincidence between the composite data and the map point cloud data becomes high. The position and attitude estimation device is provided with the above components.

[0010] A third aspect of the present disclosure is to obtain three-dimensional point cloud data at each time measured every time a first hour elapses by a measuring device mounted on a moving body, and position data at each time measured every time a second hour longer than the first hour elapses by a position measuring device mounted on the moving body. Every time the three-dimensional point cloud data is obtained as the first hour elapses, based on the obtained three-dimensional point cloud data, a local position representing the position of the moving body in a local coordinate system with the position at the start of movement of the moving body as the origin, and a local posture representing the posture of the moving body in the local coordinate system are estimated. Every time the position data is obtained as the second hour elapses, an estimated absolute position, which is the position of the moving body in an absolute coordinate system with a predetermined position on the earth as the origin, is estimated based on the local position of the moving body. An estimated absolute posture, which is the posture of the moving body in the absolute coordinate system, is estimated based on the local posture of the moving body. Every time the position data is obtained as the second hour elapses, for each of the three-dimensional point cloud data at each time measured every time the first hour elapses, provisional three-dimensional point cloud data at each time in the absolute coordinate system is generated. For each of the provisional three-dimensional point cloud data at each time, composite data is generated by integrating the provisional three-dimensional point cloud data and map point cloud data generated from three-dimensional point cloud data measured in the past. By correcting the estimated absolute position and the estimated absolute posture so that the degree of coincidence between the composite data and the map point cloud data becomes high, a corrected absolute position with the estimated absolute position corrected and a corrected absolute posture with the estimated absolute posture corrected are generated. It is a program for causing a computer to execute a process.

Effect of the Invention

[0011] According to the disclosed technology, based on the position data obtained by the position measuring device mounted on the moving body and the three-dimensional point cloud data obtained by the measuring device mounted on the moving body, the position and posture of the moving body in the absolute coordinate system can be accurately estimated.

Brief Description of the Drawings

[0012]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Embodiments for Carrying Out the Invention

[0013] Hereinafter, an example of an embodiment of the disclosed technology will be described with reference to the drawings. In each drawing, the same or equivalent constituent elements and parts are given the same reference numerals. Also, the dimensional ratios in the drawings are exaggerated for convenience of explanation and may be different from the actual ratios.

[0014] FIG. 1 is a block diagram showing the hardware configuration of the position and orientation estimation device 10. As shown in FIG. 1, the position and orientation estimation device 10 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 configuration is connected so as to be communicable with each other via a bus 19.

[0015] The CPU 11 is a central processing unit that executes various programs and controls each part. That is, the CPU 11 reads a program from the ROM 12 or the storage 14 and executes the program using the RAM 13 as a working area. The CPU 11 performs the control of each of the above configurations and various arithmetic processes according to the program stored in the ROM 12 or the storage 14. In the present embodiment, a program for estimating the position and orientation of the moving body is stored in the ROM 12 or the storage 14.

[0016] The ROM 12 stores various programs and various data. The RAM 13 temporarily stores a program or data as a working area. The storage 14 is composed of a storage device such as an HDD (Hard Disk Drive) or an SSD (Solid State Drive), and stores various programs including an 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 adopt a touch panel method and function as the input unit 15.

[0019] The communication interface 17 is an interface for communicating with other devices such as a mobile terminal. For the communication, for example, a standard for wired communication such as Ethernet (registered trademark) or FDDI, or a standard for wireless communication such as 4G, 5G, or Wi-Fi (registered trademark) is used.

[0020] Next, the functional configuration of the position and orientation estimation device 10 will be described.

[0021] FIG. 2 is a block diagram showing an example of the functional configuration of the position and orientation estimation device 10.

[0022] As shown in FIG. 2, the position and orientation estimation device 10 includes, as functional components, a position data storage unit 100, a three-dimensional point cloud data storage unit 102, a local coordinate data storage unit 104, an absolute coordinate data storage unit 106, a correction data storage unit 108, a map point cloud data storage unit 110, an acquisition unit 120, an initial estimation unit 122, a first estimation unit 124, a second estimation unit 126, a correction unit 128, and a map data generation unit 130. Each functional component 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.

[0023] Next, the terms appearing in this embodiment will be described below.

[0024] The absolute coordinate system is a coordinate system with a predetermined position on the earth as the origin. By projecting the position and orientation of the moving object with respect to the absolute coordinate system, the position and orientation of the moving object on the earth are specified. For example, the orientation of the moving object is represented by Euler angles in the case of a plane coordinate system of the World Geodetic System. Note that the Euler angles are the roll angle, pitch angle, and yaw angle of the moving object in each coordinate system. Note that the absolute coordinate system may be any coordinate system, for example, a plane rectangular coordinate system.

[0025] The device coordinate system is a coordinate system with the position of the measurement device mounted on the moving object as the origin. In this embodiment, the measurement device mounted on the moving object is a LiDAR. Therefore, in this embodiment, the coordinate system with the position of the LiDAR at each time as the origin is the device coordinate system.

[0026] The local coordinate system is a coordinate system with the position at the start of movement of the moving object as the origin, and with the roll angle, pitch angle, and yaw angle representing the orientation at the start of movement of the moving object as the initial orientation (for example, 0 degrees).

[0027] GPS is an example of a position measurement device mounted on a moving object, and measures the latitude, longitude, and altitude of the moving object.

[0028] LiDAR is an example of a measuring device mounted on a moving object. LiDAR is a device that acquires three-dimensional point cloud data of the surroundings from the time it irradiates laser light externally until the laser light hits an object and bounces back.

[0029] IMU is a sensor unit equipped with an accelerometer, an angular accelerometer, and a compass.

[0030] Figures 3 and 4 show diagrams for explaining the absolute coordinate system, the device coordinate system, and the local coordinate system. In Figure 3, the absolute coordinate system A, the device coordinate system D, the local coordinate system L, the earth E, and the moving object M are schematically shown. In the example of Figure 3, the origin of the absolute coordinate system A is the center of gravity of the earth E. For example, consider the case where the moving object M is an airplane. In this case, for example, in the absolute coordinate system A, when the moving object is horizontal facing north, the Euler angle is set to 0 degrees. On the other hand, for example, in the local coordinate system L, when the moving object is horizontal facing east, the Euler angle is set to 0 degrees. On the other hand, in the device coordinate system D, the position of the moving object at each time is set as the origin, and the posture of the moving object at each time is set as the initial posture (for example, 0 degrees). Note that the absolute coordinate system A in Figure 3 is U A , V A , W A with its coordinate axes.

[0031] Also, as shown in Figure 4, while the absolute coordinate system A and the local coordinate system L are fixed, the device coordinate system D moves according to the movement of the moving object M. The device coordinate system D at time t1 in Figure 4 is different from the device coordinate system D at time tN. Therefore, when calculating the position and posture of the moving object M at each time in the absolute coordinate system A, it is necessary to perform coordinate transformation so as to project the position and posture of the moving object M in the local coordinate system L and the device coordinate system D onto the absolute coordinate system A. Note that in this embodiment, the coordinate transformation Cov from the local coordinate system L to the absolute coordinate system A is executed. The local coordinate system L in Figure 4 is U L , V L , W L with its coordinate axes, and the device coordinate system D is U D , V D , WD Let the coordinate axes be based on it.

[0032] The time interval at which the position data of the moving object is measured by GPS is longer than the time interval at which the three-dimensional point cloud data around the moving object is measured by LiDAR. For example, the time interval at which the position data of the moving object is measured by GPS is 1 second, and the time interval at which the three-dimensional point cloud data around the moving object is measured by LiDAR is 0.1 second. Hereinafter, the time interval at which the three-dimensional point cloud data around the moving object is measured by LiDAR is referred to as the first time, and the time interval at which the position data of the moving object is measured by GPS is referred to as the second time.

[0033] In the three-dimensional point cloud data measured by LiDAR, an accumulation of errors called drift occurs. Therefore, when calculating the position and orientation of the moving object in the absolute coordinate system using the three-dimensional point cloud data at each time, this drift has an adverse effect, and the position and orientation of the moving object in the absolute coordinate system cannot be accurately estimated.

[0034] Therefore, in the present embodiment, every time the position data of the moving object is acquired by GPS, the position and orientation of the moving object in the absolute coordinate system are estimated based on the position data, thereby reducing the influence of drift and accurately estimating the position and orientation of the moving object in the absolute coordinate system. Further, in the present embodiment, map point cloud data corresponding to the movement of the moving object is generated based on the estimated position and orientation of the moving object in the absolute coordinate system. This map point cloud data is useful when obtaining the position of an object on the earth.

[0035] Hereinafter, a specific description will be given. In the following, it is assumed that the mounting position of LiDAR on the moving object and the mounting position of GPS on the moving object are close to each other, and the difference in the positions of the two devices can be ignored. Such an aspect can be realized, for example, by mounting GPS on the upper surface of LiDAR mounted on the moving object. Further, in the present embodiment, the position of the moving object is represented by three-dimensional parameters, and the orientation of the moving object is represented by three-dimensional parameters of roll angle, pitch angle, and yaw angle.

[0036] In the position data storage unit 100, position data at each time measured by the GPS mounted on the moving body is stored. As described above, the position data of the moving body is measured by the GPS every time the second time elapses. In the present embodiment, the position data stored in the position data storage unit 100 is not the latitude, longitude, and altitude themselves, but data converted into an absolute coordinate system such as the World Geodetic System.

[0037] In the three-dimensional point cloud data storage unit 102, three-dimensional point cloud data at each time measured by the LiDAR mounted on the moving body is stored. As described above, the three-dimensional point cloud data around the moving body is measured by the LiDAR every time the first time elapses.

[0038] Also, hereinafter, the time is expressed as X_Y.

[0039] X is a variable that is set to 0 at the time when the position data is first measured by the GPS, and is incremented by 1 every time the position data is measured by the GPS.

[0040] Y is a variable that is set to 0 at the time when the three-dimensional point cloud data is first obtained by the LiDAR after the most recent position data is obtained, and is incremented by 1 every time the three-dimensional point cloud data is obtained by the LiDAR.

[0041] For example, when the measurement of the position data by the GPS is 1 Hz and the measurement of the three-dimensional point cloud data by the LiDAR is 10 Hz, the times X and Y increase as follows.

[0042] 0_0, 0_1, 0_2, ···, 0_9, 1_0, 1_1, 1_2, ···, 1_9, 2_0, 2_1, ···

[0043] Hereinafter, it is assumed that the three-dimensional point cloud data is measured N + 1 times by the LiDAR within the time interval during which the position data is measured by the GPS. That is, the times X and Y increase as follows.

[0044] 0_0, 0_1, 0_2, ···, 0_N−1, 0_N, 1_0, 1_1, ···

[0045] The local coordinate data storage unit 104 stores a local position representing the position of the moving object at each time in the local coordinate system and a local orientation representing the orientation of the moving object at each time in the local coordinate system. Note that the local position and the local orientation are estimated by a first estimation unit 124 described later.

[0046] The absolute coordinate data storage unit 106 stores an estimated absolute position that is the position of the moving object at each time in the absolute coordinate system and an estimated absolute orientation that is the orientation of the moving object at each time in the absolute coordinate system. Note that the estimated absolute position and the estimated absolute orientation are estimated by a second estimation unit 126 described later.

[0047] The correction data storage unit 108 stores a corrected absolute position that is data obtained by correcting the estimated absolute position and a corrected absolute orientation that is data obtained by correcting the estimated absolute orientation. Note that the corrected absolute position and the corrected absolute orientation are generated by a correction unit 128 described later.

[0048] The map point cloud data storage unit 110 stores map point cloud data generated from the three-dimensional point cloud data at each time. The map point cloud data is generated by integrating the three-dimensional point cloud data at each time collected according to the movement of the moving object.

[0049] When new position data (hereinafter referred to as the current position data) is stored in the position data storage unit 100, the acquisition unit 120 acquires the three-dimensional point cloud data measured at each time from the storage of the previous position data to the storage of the current position data, the current position data, and the previous position data.

[0050] The initial estimation unit 122 estimates the translation from the local position of the moving object in the local coordinate system to the estimated absolute position of the moving object in the absolute coordinate system and estimates the rotation from the local orientation of the moving object in the local coordinate system to the estimated absolute orientation of the moving object in the absolute coordinate system.

[0051] Specifically, the initial estimation unit 122 estimates the rotation and translation for performing geometric transformation from the local coordinate system to the absolute coordinate system, and estimates the position and orientation of the moving object in the absolute coordinate system at time 0_0.

[0052] More specifically, first, the initial estimation unit 122 estimates the translation element of the geometric transformation from the local coordinate system to the absolute coordinate system by setting the position represented by the position data measured by GPS at time 0_0 as the position of the moving object in the absolute coordinate system at time 0_0.

[0053] Next, the initial estimation unit 122 estimates the position and orientation of the moving object in the local coordinate system from only the three-dimensional point cloud data measured by LiDAR using a known method such as the above Non-Patent Document 1 or the above Non-Patent Document 2. Specifically, the initial estimation unit 122 calculates the average of the postures of the moving object from time 0_0 to time 1_0 based on the postures of the moving object at each time in the local coordinate system obtained from time 0_1 to time 1_0. Then, the initial estimation unit 122 sets the average of the postures of the moving object from time 0_0 to time 1_0 as the initial posture of the moving object in the local coordinate system.

[0054] Next, the initial estimation unit 122 sets the posture obtained by assuming that the moving object moves with a constant posture from time 0_0 to time 1_0, which is obtained by the difference between the position data measured by GPS at time 0_0 and the position data measured by GPS at time 1_0, as the initial posture in the absolute coordinate system. Specifically, the vector representing the movement from the position represented by the position data at time 0_0 to the position represented by the position data at time 1_0 is set as the initial posture of the moving object in the absolute coordinate system.

[0055] Then, the initial estimation unit 122 estimates the rotation element of the geometric transformation from the local coordinate system to the absolute coordinate system on the assumption that the initial posture of the moving body in the local coordinate system is equal to the initial posture of the moving body in the absolute coordinate system. Thereby, it is specified at what angle the posture of 0 degrees in the local coordinate system is in the absolute coordinate system. Note that the translation element and the rotation element representing the coordinate transformation estimated by the initial estimation unit 112 are used in the coordinate transformation in the first estimation unit 124, the second estimation unit 126, and the correction unit 128 described later.

[0056] Each time the three-dimensional point cloud data is acquired with the elapse of the first time and the three-dimensional point cloud data is stored in the three-dimensional point cloud data storage unit 102, the first estimation unit 124 uses a known method such as the non-patent document 1 or the non-patent document 2, etc., and based on the three-dimensional point cloud data acquired by the acquisition unit 120, estimates a local position representing the position of the moving body in the local coordinate system and a local posture representing the posture of the moving body in the local coordinate system.

[0057] Each time the position data is acquired with the elapse of the second time and the position data is stored in the position data storage unit 100, the second estimation unit 126 estimates an estimated absolute position which is the position of the moving body in the absolute coordinate system based on the local position of the moving body estimated by the first estimation unit 124. Further, each time the position data is acquired with the elapse of the second time and the position data is stored in the position data storage unit 100, the second estimation unit 126 estimates an estimated absolute posture which is the posture of the moving body in the absolute coordinate system based on the local posture of the moving body estimated by the first estimation unit 124.

[0058] In this way, each time the time X_0 when the position data is acquired with the elapse of the second time arrives, the second estimation unit 126 estimates the estimated absolute position and the estimated absolute posture. By the process executed by the second estimation unit 126, the position data measured by the GPS and the three-dimensional point cloud data measured by the LiDAR are integrated, and the estimated absolute position and the estimated absolute posture are estimated. Thereby, the drift occurring in the LiDAR is reduced, and the estimated absolute position and the estimated absolute posture in the absolute coordinate system are accurately estimated.

[0059] Specifically, every time position data is acquired as the second time elapses, the second estimation unit 126 sets the local position of the moving object estimated by the first estimation unit 124 as the estimated absolute position. For this reason, the estimated absolute position at time X_0 corresponds to the GPS position data obtained at time X_0.

[0060] Further, every time position data is acquired as the second time elapses, the second estimation unit 126 estimates the estimated absolute attitude, which is the attitude in the current absolute coordinate system, by applying a rotation representing the difference between the local attitude of the moving object estimated this time by the first estimation unit 124 and the local attitude of the moving object estimated last time by the first estimation unit 124 to the corrected absolute attitude of the moving object obtained at the time of the previous position data acquisition. Specifically, the second estimation unit 126 estimates the estimated absolute attitude at time X_0 by applying a rotation corresponding to the difference between the local attitude at time X-1_0 and the local attitude at time X_0 estimated by the first estimation unit 124 to the corrected absolute attitude at time X-1_0. Note that the corrected absolute attitude and the corrected absolute position are generated by a correction unit 128 described later.

[0061] Further, every time the first time elapses, the second estimation unit 126 estimates the estimated absolute position and the estimated absolute attitude at each time based on the local position of the moving object estimated by the first estimation unit 124 and the local attitude of the moving object at each time at each time. Specifically, the second estimation unit 126 estimates the estimated absolute position and the estimated absolute attitude at each time between the time X-1_0 when the previous position data was acquired and the time X_0 when the current position data was acquired. For example, the second estimation unit 126 estimates the estimated absolute position and the estimated absolute attitude at time X-1_N by applying a rotation and a translation representing the difference between the local position and the local attitude estimated by the first estimation unit 124 from time X-1_N to X-1_0 to the corrected absolute position and the corrected absolute attitude at time X-1_0.

[0062] Each time position data is acquired as the second hour elapses, the correction unit 128 applies the translation and rotation estimated by the initial estimation unit 122 to the three-dimensional point cloud data at each time measured every time the first hour elapses, thereby generating provisional three-dimensional point cloud data in the absolute coordinate system. Then, for each of the provisional three-dimensional point cloud data at each time, the correction unit 128 generates composite data by integrating the provisional three-dimensional point cloud data at that time and the map point cloud data generated from the three-dimensional point cloud data measured in the past. Then, the correction unit 128 corrects the estimated absolute position and the estimated absolute orientation so that the degree of coincidence between the composite data generated for each of the provisional three-dimensional point cloud data at each time and the map point cloud data becomes high, thereby generating a corrected absolute position with the estimated absolute position corrected and a corrected absolute orientation with the estimated absolute orientation corrected.

[0063] Specifically, each time position data is acquired as the second hour elapses, the correction unit 128 generates composite data, which is three-dimensional point cloud data obtained by synthesizing the provisional three-dimensional point cloud data at each time and the map point cloud data generated up to the previous time. Then, for each of the three-dimensional point cloud data at each time measured every time the first hour elapses, the correction unit 128 calculates the degree of coincidence between the composite data and the map point cloud data using the ICP (Iterative Closest Point) algorithm, and corrects the estimated absolute position and the estimated absolute orientation so that the degree of coincidence between the composite data and the map point cloud data becomes high, thereby generating a corrected absolute position and a corrected absolute orientation.

[0064] For example, the correction unit 128 corrects the estimated absolute position and the estimated absolute orientation using the estimated absolute position and the estimated absolute orientation at times X-1_1, ···, X-1_N, X_0, the three-dimensional point cloud data at each time, and the map point cloud data, thereby generating a corrected absolute position and a corrected absolute orientation.

[0065] Specifically, first, the correction unit 128 applies translation from the local position to the estimated absolute position and rotation from the local orientation to the estimated absolute orientation to the three-dimensional point cloud data at times X-1_1, ···, X-1_N, ···, X_0 to generate provisional three-dimensional point cloud data at each time. Then, for each of the provisional three-dimensional point cloud data at each time, the correction unit 128 integrates the provisional three-dimensional point cloud data at that time with the map point cloud data stored in the map point cloud data storage unit 110 to generate each of the composite data.

[0066] Next, the correction unit 128 uses the ICP (Iterative Closest Point) algorithm with the estimated absolute position and estimated absolute orientation at each time as initial values to generate the corrected absolute position of the moving object and the corrected absolute orientation of the moving object when the three-dimensional point cloud data was measured so that the overlap between the composite data and the map point cloud data for the three-dimensional point cloud data at each time becomes large. This is performed for the three-dimensional point cloud data at all times. Furthermore, this is repeatedly performed a plurality of times.

[0067] FIG. 5 shows a diagram for explaining the outline of the processing of the second estimation unit 126 and the correction unit 128.

[0068] As shown in FIG. 5, consider the case where position data is measured at time X_0 when the second time has elapsed since the position data was measured at time X-1_0. In this case, the second estimation unit 126 estimates the estimated absolute position of the moving object and the estimated absolute orientation of the moving object in the absolute coordinate system A at time X_0 from the local position and local orientation in the local coordinate system L when the position data was measured at time X_0.

[0069] Next, the second estimation unit 126 estimates the estimated absolute position and the estimated absolute orientation in the absolute coordinate system A at each time X-1_1, ···, X-1_N at which the three-dimensional point cloud data is measured from time X-1_0 to time X_0. At this time, the second estimation unit 126 estimates the estimated absolute position and the estimated absolute orientation in the absolute coordinate system A at each time X-1_1, ···, X-1_N at which the three-dimensional point cloud data is measured, based on the rotation and translation representing the changes in the local position and the local orientation in the local coordinate system L. For example, the second estimation unit 126 estimates the estimated absolute position and the estimated absolute orientation in the absolute coordinate system A at time X-1_1 by applying the rotation and translation representing the difference between the local position and the local orientation at time X-1_0 and the local position and the local orientation at time X-1_1 in the local coordinate system L to the corrected absolute position and the corrected absolute orientation in the absolute coordinate system A at time X-1_0. Also, for example, the second estimation unit 126 estimates the estimated absolute position and the estimated absolute orientation in the absolute coordinate system A at time X-1_2 by applying the rotation and translation representing the difference between the local position and the local orientation at time X-1_1 and the local position and the local orientation at time X-1_2 in the local coordinate system L to the estimated absolute position and the estimated absolute orientation in the absolute coordinate system A at time X-1_1.

[0070] Next, the correction unit 128 generates composite data by synthesizing each of the provisional three-dimensional point cloud data in the absolute coordinate system measured at each time up to time X_0 and the map point cloud data generated from the three-dimensional point cloud data measured at each time up to time X-1_0. Then, the correction unit 128 corrects the estimated absolute position and the estimated absolute orientation in the absolute coordinate system A at each time X-1_1, ···, X-1_N at which the three-dimensional point cloud data is measured so that the degree of coincidence between the composite data and the map point cloud data becomes high, thereby generating the corrected absolute position and the corrected absolute orientation. At this time, the correction unit 128 generates the corrected absolute position and the corrected absolute orientation by correcting the estimated absolute position and the estimated absolute orientation at the time when the three-dimensional point cloud data is acquired for each of the three-dimensional point cloud data at each time.

[0071] Every time the first hour elapses, for each of the three-dimensional point cloud data at each time measured, the translation and rotation estimated by the initial estimation unit 122 are applied, thereby generating a series of three-dimensional point cloud data in the current second time interval. Next, the map data generation unit 130 generates each of the three-dimensional point cloud data at each time in the absolute coordinate system as if the three-dimensional point cloud data were measured from the moving body with the corrected absolute position and corrected absolute orientation in the absolute coordinate system. Next, the map data generation unit 130 generates new map point cloud data by integrating the series of three-dimensional point cloud data at each time generated in the absolute coordinate system and the map point cloud data. Then, the map data generation unit 130 updates the map point cloud data by storing the new map point cloud data in the map point cloud data storage unit 110.

[0072] Next, the operation of the position and orientation estimation device 10 will be described.

[0073] FIG. 6 and FIG. 7 are flowcharts showing the flow of processing by the position and orientation estimation device 10. The processing is performed by the CPU 11 reading a program from the ROM 12 or the storage 14, expanding it in the RAM 13, and executing it.

[0074] When position data is stored in the position data storage unit 100 and three-dimensional point cloud data is stored in the three-dimensional point cloud data storage unit 102, the position and orientation estimation device 10 executes the flowchart shown in FIG. 6.

[0075] In step S100, when new position data is stored in the position data storage unit 100, the CPU 11, as the acquisition unit 120, acquires the three-dimensional point cloud data measured at each time from the storage of the previous position data to the storage of the current position data, the current position data, and the previous position data. Note that the acquisition unit 120 acquires the three-dimensional point cloud data from the three-dimensional point cloud data storage unit 102 and acquires the position data from the position data storage unit 100.

[0076] In step S102, as the initial estimation unit 122, the CPU 11 estimates the translation from the local position of the moving object in the local coordinate system to the estimated absolute position of the moving object in the absolute coordinate system, and estimates the rotation from the local attitude of the moving object in the local coordinate system to the estimated absolute attitude of the moving object in the absolute coordinate system.

[0077] In step S104, as the initial estimation unit 122, the CPU 11 estimates the initial estimated absolute position of the moving object and the initial estimated absolute attitude of the moving object based on the translation and rotation estimated in the above step S100.

[0078] The initial absolute position and the initial absolute attitude are estimated according to the flowchart of FIG. 6. These initial absolute estimated positions and initial absolute estimated attitudes are used in the flowchart of FIG. 7 described later. Each time the 3D point cloud data is stored in the 3D point cloud data storage unit 102, the position and attitude estimation device 10 executes the flowchart shown in FIG. 7. At this time, the acquisition unit 120 acquires the position data and the 3D point cloud data.

[0079] In step S200, as the first estimation unit 124, every time the 3D point cloud data is acquired due to the elapse of the first time and the 3D point cloud data is stored in the 3D point cloud data storage unit 102, based on the 3D point cloud data acquired by the acquisition unit 120 using a known method such as the above Non-Patent Document 1 or the above Non-Patent Document 2, the local position representing the position of the moving object in the local coordinate system and the local attitude representing the attitude of the moving object in the local coordinate system are estimated. Then, the first estimation unit 124 stores the local position and the local attitude in the local coordinate data storage unit 102.

[0080] In step S202, as the second estimation unit 126, the CPU 11 determines whether new position data has been stored in the position data storage unit 100. If new position data has been stored in the position data storage unit 100, the process proceeds to step S204. If new position data has not been stored in the position data storage unit 100, the process returns to step S200.

[0081] In step S204, the CPU 11, as the second estimation unit 126, estimates an estimated absolute position, which is the position of the moving object in the absolute coordinate system, based on the local position of the moving object estimated in step S200. Further, the CPU 11, as the second estimation unit 126, estimates an estimated absolute orientation, which is the orientation of the moving object in the absolute coordinate system, based on the local orientation of the moving object estimated in step S200. Then, the second estimation unit 126 stores the estimated absolute position and the estimated absolute orientation in the absolute coordinate data storage unit 106.

[0082] In step S205, the CPU 11, as the correction unit 128, applies the translation and rotation estimated by the initial estimation unit 122 to the three-dimensional point cloud data at each time measured every time the first period of time elapses, thereby generating provisional three-dimensional point cloud data in the absolute coordinate system. Then, the correction unit 128 generates composite data, which is three-dimensional point cloud data obtained by combining the provisional three-dimensional point cloud data and the map point cloud data generated up to the previous time and stored in the map point cloud data storage unit 110.

[0083] In step S206, the CPU 11, as the correction unit 128, corrects the estimated absolute position and the estimated absolute orientation so that the degree of coincidence between the composite data generated in step S205 and the map point cloud data is high for each of the three-dimensional point cloud data at each time, thereby generating a corrected absolute position with the estimated absolute position corrected and a corrected absolute orientation with the estimated absolute orientation corrected.

[0084] In step S208, the CPU 11, as the correction unit 128, determines whether the process of step S206 has been completed for the three-dimensional point cloud data at each time. If the process of step S206 has been completed for the three-dimensional point cloud data at each time, the process proceeds to step S210. On the other hand, if there is three-dimensional point cloud data for which the process of step S206 has not been completed, the process returns to step S206.

[0085] In step S210, the CPU 11, acting as the correction unit 128, determines whether the process of step S206 has been repeated a predetermined number of times. If the process of step S206 has been repeated the predetermined number of times, the process proceeds to step S212. On the other hand, if the process of step S206 has not been repeated the predetermined number of times, the process returns to step S206. By repeating the process of step S206 the predetermined number of times, the corrected absolute position and the corrected absolute orientation are accurately estimated.

[0086] In step S212, the CPU 11, acting as the correction unit 128, stores the corrected absolute position and the corrected absolute orientation obtained in step S206 in the correction data storage unit 108.

[0087] In step S214, the CPU 11, acting as the map data generation unit 130, generates a series of 3D point cloud data for the current second time interval by applying the translation and rotation estimated by the initial estimation unit 122 to each of the 3D point cloud data at each time. Next, the map data generation unit 130 generates each of the 3D point cloud data at each time in the absolute coordinate system as if the 3D point cloud data were measured from the moving body at the corrected absolute position and the corrected absolute orientation in the absolute coordinate system. Next, the map data generation unit 130 generates new map point cloud data by integrating the series of 3D point cloud data at each time generated in the absolute coordinate system and the map point cloud data. Then, the map data generation unit 130 updates the map point cloud data by storing the new map point cloud data in the map point cloud data storage unit 110.

[0088] As described above, the position and orientation estimation device according to the embodiment acquires three-dimensional point cloud data at each time measured by a measuring device mounted on a moving body every time a first period of time elapses, and position data at each time measured by a position measuring device mounted on the moving body every time a second period of time longer than the first period of time elapses. Each time the three-dimensional point cloud data is acquired as the first period of time elapses, the position and orientation estimation device estimates, based on the acquired three-dimensional point cloud data, a local position representing the position of the moving body in a local coordinate system with the position at the start of movement of the moving body as the origin, and a local orientation representing the orientation of the moving body in the local coordinate system. Each time the position data is acquired as the second period of time elapses, the position and orientation estimation device estimates an estimated absolute position, which is the position of the moving body in an absolute coordinate system with a predetermined position on the earth as the origin, based on the local position of the moving body, and estimates an estimated absolute orientation, which is the orientation of the moving body in the absolute coordinate system, based on the local orientation of the moving body. Each time the position data is acquired as the second period of time elapses, for each of the three-dimensional point cloud data at each time measured every time the first period of time elapses, the position and orientation estimation device generates provisional three-dimensional point cloud data at each time in the absolute coordinate system, and for each of the provisional three-dimensional point cloud data at each time, generates composite data obtained by integrating the provisional three-dimensional point cloud data and map point cloud data generated from the three-dimensional point cloud data measured in the past. Then, the position and orientation estimation device corrects the estimated absolute position and the estimated absolute orientation so that the degree of coincidence between the composite data and the map point cloud data becomes high, thereby generating a corrected absolute position in which the estimated absolute position is corrected and a corrected absolute orientation in which the estimated absolute orientation is corrected. As a result, based on the position data obtained by the position measuring device mounted on the moving body and the three-dimensional point cloud data obtained by the measuring device mounted on the moving body, the position and orientation of the moving body in the absolute coordinate system can be accurately estimated.

[0089] Further, by correcting the estimated absolute position and the estimated absolute orientation every time the position data is acquired, the drift occurring in the LiDAR is reduced, and the position and orientation of the moving body in the absolute coordinate system are accurately estimated.

[0090] Moreover, according to the present embodiment, the problem that the absolute position and absolute orientation of the moving body cannot be obtained without using GPS is also solved. Further, according to the present embodiment, the problem that calibration between LiDAR and IMU is required when an IMU is mounted on the moving body is also solved. Therefore, according to the present embodiment, calibration between the IMU and the LiDAR becomes unnecessary, and the absolute position and absolute orientation of the moving body can be estimated only by the GPS and the LiDAR.

[0091] Moreover, according to the present embodiment, as if the three-dimensional point cloud data is measured from the moving body of the corrected absolute position and the corrected absolute orientation, each of the three-dimensional point cloud data at each time in the absolute coordinate system is generated, and a new map point cloud data can be generated by integrating the series of the three-dimensional point cloud data at each time generated in the absolute coordinate system and the map point cloud data.

[0092] Note that, in the above embodiment, various processes executed by the CPU by reading software (program) may be executed by various processors other than the CPU. Examples of the processor in this case include a PLD (Programmable Logic Device) whose circuit configuration can be changed after manufacture, such as an FPGA (Field-Programmable Gate Array), and a dedicated electric circuit which is a processor having a circuit configuration dedicated to executing specific processes, such as an ASIC (Application Specific Integrated Circuit). Further, various processes may be executed by one of these various processors, or may be executed by a combination of two or more processors of the same type or different types (for example, a plurality of FPGAs, and a combination of a CPU and an FPGA, etc.). Moreover, the hardware structure of these various processors is, more specifically, an electric circuit combining circuit elements such as semiconductor elements.

[0093] Also, in each of the above embodiments, although the mode in which the program is pre-stored (installed) in the storage 14 has been described, the present invention is not limited thereto. The program may be provided in a form stored in a non-transitory storage medium such as a CD-ROM (Compact Disk Read Only Memory), a DVD-ROM (Digital Versatile Disk Read Only Memory), and a USB (Universal Serial Bus) memory. Further, the program may be in a form downloaded from an external device via a network.

[0094] Regarding the above embodiments, the following additional remarks are disclosed.

[0095] (Supplementary Note 1) A memory, At least one processor connected to the memory, comprising, The processor acquires three-dimensional point cloud data at each time measured every time a first hour elapses by a measuring device mounted on a moving body, and position data at each time measured every time a second hour longer than the first hour elapses by a position measuring device mounted on the moving body; each time the three-dimensional point cloud data is acquired due to the elapse of the first hour, based on the acquired three-dimensional point cloud data, a local position representing the position of the moving body in a local coordinate system with the position at the start of movement of the moving body as the origin, and a local posture representing the posture of the moving body in the local coordinate system are estimated; each time the position data is acquired due to the elapse of the second hour, based on the local position of the moving body, an estimated absolute position which is the position of the moving body in an absolute coordinate system with a predetermined position on the earth as the origin is estimated, and based on the local posture of the moving body, an estimated absolute posture which is the posture of the moving body in the absolute coordinate system is estimated; Each time the position data is acquired as the second time elapses, for each of the three-dimensional point cloud data at each time measured each time the first time elapses, provisional three-dimensional point cloud data at each time in the absolute coordinate system is generated. For each of the provisional three-dimensional point cloud data at each time, synthetic data is generated by integrating the provisional three-dimensional point cloud data and map point cloud data generated from three-dimensional point cloud data measured in the past. By correcting the estimated absolute position and the estimated absolute orientation so that the degree of coincidence between the synthetic data and the map point cloud data becomes high, a corrected absolute position with the estimated absolute position corrected and a corrected absolute orientation with the estimated absolute orientation corrected are generated. A position and orientation estimation device configured as described above.

[0096] (Appended Claim 2) A non-transitory storage medium storing a program executable by a computer to execute a position and orientation estimation process, The position and orientation estimation process includes: Acquiring three-dimensional point cloud data at each time measured each time a first time elapses by a measuring device mounted on a moving body, and position data at each time measured each time a second time longer than the first time elapses by a position measuring device mounted on the moving body. Each time the three-dimensional point cloud data is acquired as the first time elapses, based on the acquired three-dimensional point cloud data, a local position representing the position of the moving body in a local coordinate system with the position at the start of movement of the moving body as the origin, and a local orientation representing the orientation of the moving body in the local coordinate system are estimated. Each time the position data is acquired as the second time elapses, based on the local position of the moving body, an estimated absolute position which is the position of the moving body in an absolute coordinate system with a predetermined position on the earth as the origin is estimated, and based on the local orientation of the moving body, an estimated absolute orientation which is the orientation of the moving body in the absolute coordinate system is estimated. Each time the position data is acquired as the second time elapses, for each of the three-dimensional point cloud data at each time measured each time the first time elapses, provisional three-dimensional point cloud data at each time in the absolute coordinate system is generated, and for each of the provisional three-dimensional point cloud data at each time, synthetic data obtained by integrating the provisional three-dimensional point cloud data and map point cloud data generated from three-dimensional point cloud data measured in the past is generated, and the estimated absolute position and the estimated absolute orientation are corrected so that the degree of coincidence between the synthetic data and the map point cloud data becomes high, thereby generating a corrected absolute position with the estimated absolute position corrected and a corrected absolute orientation with the estimated absolute orientation corrected. Non-transitory storage medium.

Explanation of Signs

[0097] 100 Position data storage unit 102 Three-dimensional point cloud data storage unit 104 Local coordinate data storage unit 106 Absolute coordinate data storage unit 108 Correction data storage unit 110 Map point cloud data storage unit 120 Acquisition unit 122 Initial estimation unit 124 First estimation unit 126 Second estimation unit 128 Correction unit 130 Map data generation unit

Claims

1. For each passage of the first time measured by a measuring device mounted on a moving body, acquire three-dimensional point cloud data at each time and position data at each time measured by a position measuring device mounted on the moving body for each passage of a second time longer than the first time, Each time the three-dimensional point cloud data is acquired due to the passage of the first time, based on the acquired three-dimensional point cloud data, estimate a local position representing the position of the moving body in a local coordinate system with the position at the start of movement of the moving body as the origin, and a local orientation representing the orientation of the moving body in the local coordinate system, Each time the position data is acquired due to the passage of the second time, based on the local position of the moving body, estimate an estimated absolute position which is the position of the moving body in an absolute coordinate system with a predetermined position on the earth as the origin at each time when the first time has elapsed, and based on the local orientation of the moving body, estimate an estimated absolute orientation which is the orientation of the moving body in the absolute coordinate system at each time when the first time has elapsed, Each time the position data is acquired due to the passage of the second time, for each of the three-dimensional point cloud data at each time measured every time the first time has elapsed, based on the estimated absolute position and the estimated absolute orientation at each time when the first time has elapsed, generate temporary three-dimensional point cloud data at each time in the absolute coordinate system, and for each of the temporary three-dimensional point cloud data at each time, generate composite data obtained by integrating the temporary three-dimensional point cloud data and map point cloud data generated from three-dimensional point cloud data measured in the past, and correct the estimated absolute position and the estimated absolute orientation so that the degree of coincidence between the composite data and the map point cloud data becomes high, thereby generating a corrected absolute position in which the estimated absolute position at each time when the first time has elapsed is corrected, and a corrected absolute orientation in which the estimated absolute orientation at each time when the first time has elapsed is corrected, A position and orientation estimation method executed by a computer for the processing.

2. Generate each of the three-dimensional point cloud data at each time in the absolute coordinate system as if the three-dimensional point cloud data were measured from the moving body of the corrected absolute position and the corrected absolute attitude, and generate new map point cloud data by integrating the series of three-dimensional point cloud data at each time generated in the absolute coordinate system and the map point cloud data. The position and attitude estimation method according to claim 1.

3. When estimating the estimated absolute position and the estimated absolute attitude, every time the position data is acquired due to the elapse of the second time, set the estimated local position of the moving body as the estimated absolute position which is the position in the absolute coordinate system, and apply a rotation representing the difference between the currently estimated local attitude of the moving body and the previously estimated local attitude of the moving body to the corrected absolute attitude of the moving body obtained based on the previous position data, thereby estimating the estimated absolute attitude which is the attitude in the current absolute coordinate system. The position and attitude estimation method according to claim 1 or claim 2.

4. When estimating the estimated absolute position and the estimated absolute attitude, use the ICP (Iterative Closest Point) algorithm to calculate the degree of coincidence between the temporary three-dimensional point cloud data and the three-dimensional point cloud data, and correct the estimated absolute position and the estimated absolute attitude so that the degree of coincidence becomes high. The position and attitude estimation method according to any one of claims 1 to 3.

5. An acquisition unit that acquires three-dimensional point cloud data at each time measured every time the first time elapses by a measurement device mounted on the moving body, and position data at each time measured every time the second time longer than the first time elapses by a position measurement device mounted on the moving body. Every time the three-dimensional point cloud data is acquired as the first time elapses, based on the acquired three-dimensional point cloud data, a first estimator that estimates a local position representing the position of the moving body in a local coordinate system with the position at the start of movement of the moving body as the origin, and a local attitude representing the attitude of the moving body in the local coordinate system; Every time the position data is acquired as the second time elapses, based on the local position of the moving body, an estimated absolute position that is the position of the moving body in an absolute coordinate system with a predetermined position on the earth as the origin at each time when the first time elapses is estimated, and based on the local attitude of the moving body, an estimated absolute attitude that is the attitude of the moving body in the absolute coordinate system at each time when the first time elapses is estimated, a second estimator; Every time the position data is acquired as the second time elapses, for each of the three-dimensional point cloud data at each time measured every time the first time elapses, based on the estimated absolute position and the estimated absolute attitude at each time when the first time elapses, temporary three-dimensional point cloud data at each time in the absolute coordinate system is generated, and for each of the temporary three-dimensional point cloud data at each time, composite data obtained by integrating the temporary three-dimensional point cloud data and map point cloud data generated from three-dimensional point cloud data measured in the past is generated, and the estimated absolute position and the estimated absolute attitude are corrected so that the degree of coincidence between the composite data and the map point cloud data becomes high, thereby generating a corrected absolute position in which the estimated absolute position at each time when the first time elapses is corrected, and a corrected absolute attitude in which the estimated absolute attitude at each time when the first time elapses is corrected, a correction unit; A position and attitude estimation device comprising:

6. Three-dimensional point cloud data at each time measured every time a first time elapses by a measuring device mounted on a moving body, and position data at each time measured every time a second time longer than the first time elapses by a position measuring device mounted on the moving body are acquired; Every time the three-dimensional point cloud data is acquired as the first time elapses, based on the acquired three-dimensional point cloud data, a local position representing the position of the moving body in a local coordinate system with the position at the start of movement of the moving body as the origin, and a local posture representing the posture of the moving body in the local coordinate system are estimated. Every time the position data is acquired as the second time elapses, based on the local position of the moving body, an estimated absolute position, which is the position of the moving body in an absolute coordinate system with a predetermined position on the earth as the origin at each time when the first time elapses, is estimated. Based on the local posture of the moving body, an estimated absolute posture, which is the posture of the moving body in the absolute coordinate system at each time when the first time elapses, is estimated. Every time the position data is acquired as the second time elapses, for each of the three-dimensional point cloud data at each time measured every time the first time elapses, based on the estimated absolute position and the estimated absolute posture at each time when the first time elapses, temporary three-dimensional point cloud data at each time in the absolute coordinate system is generated. For each of the temporary three-dimensional point cloud data at each time, composite data is generated by integrating the temporary three-dimensional point cloud data and map point cloud data generated from the three-dimensional point cloud data measured in the past. By correcting the estimated absolute position and the estimated absolute posture so that the degree of coincidence between the composite data and the map point cloud data becomes high, a corrected absolute position in which the estimated absolute position at each time when the first time elapses is corrected, and a corrected absolute posture in which the estimated absolute posture at each time when the first time elapses is corrected are generated. A program for causing a computer to execute the processing.

Images