JP7740683B2 - Position estimation device, position estimation method, and program - Google Patents

Description

The present invention relates to a position estimation device and a position estimation method for estimating the position of a moving object, as well as a program for implementing these.

In recent years, advances in sensing technology have drawn attention to SLAM (Simultaneous Localization and Mapping). SLAM is a technology in which a mobile object equipped with sensors simultaneously estimates its own position and creates an environmental map (see, for example, Patent Document 1 and Non-Patent Document 1). With SLAM, mobile objects such as autonomous robots do not need to move randomly, but can move along an autonomous driving map obtained from an environmental map, thereby improving their movement efficiency.

In Patent Document 1 and Non-Patent Document 1, a camera is used as the sensor. Specifically, in Patent Document 1 and Non-Patent Document 1, the mobile object acquires image data output from the camera for each frame, searches for feature points in the latest frame that correspond to feature points extracted from previous frames, and extracts pairs of feature points made up of corresponding feature points.

The mobile body then uses the set of feature points to calculate the camera matrix for the latest frame, and for each set of feature points, calculates the three-dimensional coordinates of the feature points using the camera matrix and the two-dimensional coordinates of the feature points in the frame. After calculating the camera matrix, the mobile body calculates the camera position from the camera matrix and performs self-location estimation. Furthermore, the mobile body uses the feature points whose three-dimensional coordinates have been calculated to generate or update an environmental map composed of a three-dimensional point cloud (hereinafter referred to as a "three-dimensional environmental map").

The mobile object then generates a two-dimensional environmental map by converting the three-dimensional environmental map into two dimensions. However, with camera-based SLAM, the point cloud that makes up the three-dimensional environmental map is a feature point on the image, so there is a problem in that it is difficult to increase the density of the point cloud. As a result, there are few distinctive features in the autonomous driving map, making it difficult for the mobile object to drive autonomously with high accuracy using this autonomous driving map.

On the other hand, the above-mentioned problems can be solved by using a depth sensor, such as LiDAR (light detection and ranging). There are two types of LiDAR: 3D LiDAR, which measures depth in three-dimensional space, and 2D LiDAR, which measures depth on a specific plane. However, 3D LiDAR is generally used in SLAM.

3D LiDAR can measure depth for each pixel in a captured image, making it possible to calculate the 3D coordinates of points that are not characteristic, thereby increasing the density of the point cloud in the 3D environmental map. As a result, the map for autonomous driving will have more characteristic areas, which is thought to enable highly accurate autonomous driving by mobile objects. 3D LiDAR also makes it possible to accurately estimate one's own position.

Japanese Patent Application Laid-Open No. 2020-76714

Raul Mur-Artal, J. M. M. Montiel, Juan D. Tardos, “ORB-SLAM: A Versatile and Accurate Monocular SLAM System”, [online], IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 5, OCTOBER 2015, [Retrieved 2020/7/29], Internet <URL: https://ieeexplore.ieee.org/document/7219438?arnumber=7219438>

However, 3D LiDAR has problems such as high cost and a large processing load on the system. Using 2D LiDAR, on the other hand, solves the high cost problem, but it also has the problem of not being able to create a 3D environmental map. Furthermore, 2D LiDAR has difficulty extracting distinctive features in environments with little unevenness, and using 2D LiDAR results in a decrease in the accuracy of self-location estimation.

One example of the disclosure of the present invention is to provide a position estimation device, a position estimation method, and a program that can improve the accuracy of self-position estimation while suppressing increases in costs.

In order to achieve the above object, a position estimation device according to one aspect of the present disclosure includes:
a first position estimation unit that acquires image data output from a camera mounted on a moving object on a frame-by-frame basis, extracts, for each frame, feature points corresponding to each other in the image data of the frame, and estimates three-dimensional coordinates of the moving object in the frame based on the extracted feature points;
a second position estimation unit that acquires depth data continuously output from a two-dimensional depth sensor mounted on the moving body and estimates two-dimensional coordinates of the moving body using the acquired depth data;
a correction processing unit that determines whether a set condition for the estimated two-dimensional coordinates of the moving object is satisfied, and if the set condition is satisfied, corrects the three-dimensional coordinates of the moving object using the two-dimensional coordinates of the moving object;
The present invention is characterized in that it is provided with:

In order to achieve the above object, a position estimation method according to one aspect of the present disclosure includes:
a first position estimation step of acquiring image data output from a camera mounted on a moving object on a frame-by-frame basis, extracting, for each frame, feature points corresponding to each other in the image data of the frame, and estimating three-dimensional coordinates of the moving object in the frame based on the extracted feature points;
a second position estimation step of acquiring depth data continuously output from a two-dimensional depth sensor mounted on the moving body and estimating two-dimensional coordinates of the moving body using the acquired depth data;
a correction processing step of determining whether a set condition for the estimated two-dimensional coordinates of the moving body is satisfied, and if the set condition is satisfied, correcting the three-dimensional coordinates of the moving body using the two-dimensional coordinates of the moving body;
The present invention is characterized by having the following:

Furthermore, in order to achieve the above object, a program according to one aspect of the present disclosure includes:
On the computer,
a first position estimation step of acquiring image data output from a camera mounted on a moving object on a frame-by-frame basis, extracting, for each frame, feature points corresponding to each other in the image data of the frame, and estimating three-dimensional coordinates of the moving object in the frame based on the extracted feature points;
a second position estimation step of acquiring depth data continuously output from a two-dimensional depth sensor mounted on the moving body and estimating two-dimensional coordinates of the moving body using the acquired depth data;
a correction processing step of determining whether a set condition for the estimated two-dimensional coordinates of the moving body is satisfied, and if the set condition is satisfied, correcting the three-dimensional coordinates of the moving body using the two-dimensional coordinates of the moving body;
The method is characterized in that:

As described above, the present disclosure makes it possible to improve the accuracy of self-location estimation while suppressing increases in costs.

FIG. 1 is a diagram showing a schematic configuration of a position estimation device according to an embodiment. FIG. 2 is a diagram specifically illustrating an example of the configuration of the position estimation device according to the embodiment. FIG. 3 is a diagram illustrating the processing in the first position estimation unit. FIG. 4( a) is a diagram showing an example of depth data output from a two-dimensional depth sensor, and FIG. 4( b) is a diagram showing an example of a two-dimensional autonomous driving map (two-dimensional environmental map) that has been created in advance. FIG. 5 is a diagram showing the processing in the second position estimation unit, and FIG. 5(a) and FIG. 5(b) show the flow of a series of processing. FIG. 6 is a diagram showing a process of determining the set conditions in the correction processing unit. FIG. 7 is a diagram for explaining the processing in the correction processing unit, and FIG. 7(a) and FIG. 7(b) show the flow of a series of processing. FIG. 8 is a flowchart showing the operation of the position estimation device during the process of estimating the two-dimensional coordinates of a moving object according to the embodiment. FIG. 9 is a flowchart showing the operation of the position estimation device during the process of generating or updating a three-dimensional environmental map in this embodiment. FIG. 10 is a flowchart showing the operation of the position estimation device during the estimation and correction process of the position of a moving object according to the embodiment. FIG. 11 is a block diagram illustrating an example of a computer that realizes the position estimation device according to the embodiment.

(Embodiment)
Hereinafter, a position estimation device, a position estimation method, and a program according to an embodiment will be described with reference to FIGS.

[Device configuration]
First, the schematic configuration of a position estimation device according to an embodiment will be described with reference to Fig. 1. Fig. 1 is a diagram showing the schematic configuration of a position estimation device according to an embodiment.

1, a position estimation device 10 is a device for estimating the position of a moving object. As shown in FIG. 1, the position estimation device 10 includes a first position estimation unit 11, a second position estimation unit 12, and a correction processing unit 13.

The first position estimation unit 11 acquires image data output from a camera mounted on a moving object on a frame-by-frame basis, and extracts feature points from the image data of each frame that correspond to those of other frames. Then, the first position estimation unit 11 estimates the three-dimensional coordinates of the moving object in each frame based on the extracted feature points.

The second position estimation unit 12 acquires depth data continuously output from a two-dimensional depth sensor mounted on the moving body, and uses the acquired depth data to estimate the two-dimensional coordinates of the moving body.

The correction processing unit 13 determines whether the set conditions for the estimated two-dimensional coordinates of the moving body are satisfied, and if the set conditions are satisfied, corrects the three-dimensional coordinates of the moving body using the two-dimensional coordinates of the moving body.

In this way, the position estimation device 10 corrects the three-dimensional coordinates estimated from the image using depth data from the two-dimensional depth sensor, thereby improving the accuracy of self-position estimation. Furthermore, since the position estimation device 10 does not require the use of a three-dimensional depth sensor such as a three-dimensional LiDAR, costs are kept low compared to when such a sensor is used.

Next, the configuration and functions of the position estimation device in the embodiment will be described in detail using Figures 2 to 7. Figure 2 is a block diagram specifically illustrating an example of the configuration of the position estimation device in the embodiment. Figure 3 is a diagram explaining the processing in the first position estimation unit. Figure 4(a) is a diagram showing an example of depth data output from a two-dimensional depth sensor, and Figure 4(b) is a diagram showing an example of a pre-created two-dimensional autonomous driving map (two-dimensional environmental map). Figure 5 is a diagram showing the processing in the second position estimation unit, and Figures 5(a) and 5(b) show the flow of the series of processes. Figure 6 is a diagram showing the process of determining setting conditions in the correction processing unit. Figure 7 is a diagram explaining the processing in the correction processing unit, and Figures 7(a) and 7(b) show the flow of the series of processes.

As shown in FIG. 2, in this embodiment, the position estimation device 10 is mounted on a moving body 100. In addition to the position estimation device 10, the moving body 100 is equipped with a camera 20, a two-dimensional depth sensor 30, a control device 40, and a drive device 50.

Camera 20 is an imaging device equipped with a solid-state imaging element. Camera 20 is attached so that it can capture images of the area around moving body 100, and outputs image data of a two-dimensional image of the area around moving body 100.

The two-dimensional depth sensor 30 measures the distance (depth) to an object on a two-dimensional plane centered on the sensor, and continuously outputs information specifying the measured distance as depth data. A specific example of the two-dimensional depth sensor 30 is a two-dimensional LiDAR. The two-dimensional LiDAR continuously emits laser light to scan the surrounding area on a two-dimensional plane centered on the sensor, and measures the time it takes for the laser light reflected by the object to enter the area, thereby measuring the distance to an object on the two-dimensional plane and outputting depth data. An example of depth data output by the two-dimensional depth sensor 30 is shown in Figure 4(a).

The control device 40 is a device including a processor and memory, and controls the mobile object 100. For example, the control device 40 determines the direction of travel and speed of travel of the mobile object 100 based on the three-dimensional coordinates of the mobile object 100 estimated by the position estimation device 10. The control device 40 also controls the drive device 50 in accordance with the determined direction of travel and speed of travel, causing the mobile object 100 to move along a predetermined route.

The drive unit 50 includes at least a prime mover such as an electric motor, a transmission mechanism that transmits the power of the prime mover to the wheels, and a steering mechanism. The steering mechanism is configured to change the direction of movement of the mobile body 100, and may, for example, include a steering wheel and an actuator for moving the wheel. The steering mechanism may also be a mechanism that controls the torque of the left and right drive wheels.

In this embodiment, as shown in FIG. 2, the position estimation device 10 includes a first position estimation unit 11, a second position estimation unit 12, a correction processing unit 13, and a memory unit 14.

In this embodiment, the first position estimation unit 11 acquires image data output from the camera 20 on a frame-by-frame basis. Then, for each frame, the first position estimation unit 11 extracts feature points that correspond to each other from the image data of each frame, and sets pairs of corresponding feature points.

Specifically, in the example of Figure 3, the first position estimation unit 11 acquires frame 22a at time [t] and frame 22b at time [t+1]. Furthermore, between time [t] and time [t+1], the moving object 100 moves, and the position of the camera 20 changes from 21a to 21b. Since the first position estimation unit 11 extracted feature points 23a to 27a in frame 22a, it extracts corresponding feature points 23b to 27b in frame 22b. As a result, the following sets of feature points are set: (23a, 23b), (24a, 24b), (25a, 25b), (26a, 26b), and (27a, 27b).

Then, the first position estimation unit 11 calculates the three-dimensional coordinates of the feature points using the set of corresponding feature points. Specifically, the first position estimation unit 11 calculates the camera pose of the camera 20 in the current frame using the combination of corresponding feature points. Here, the camera pose is composed of a rotation matrix R _t and a translation vector t _t . The camera pose can be calculated using a known method shown in the following reference document.

(Reference 1)
David Nister, “An Efficient Solution to the Five-Point Relative Pose Problem”, Published 2004, DOI:10.1109/TPAMI.2004.17, Corpus ID:886598, <URL:https://pdfs.semanticscholar.org/c288/7c83751d2c36c63139e68d46516ba3038909.pdf>

Furthermore, the first position estimation unit 11 calculates the three-dimensional coordinates (point cloud data) of corresponding feature points by performing triangulation using the camera posture and the two-dimensional coordinates of corresponding feature points in each of the current and past frames.

Then, the first position estimation unit 11 uses the feature points whose three-dimensional coordinates have been calculated to generate a three-dimensional environmental map composed of point cloud data of the feature points. Furthermore, if an environmental map has already been generated, the first position estimation unit 11 updates the environmental map using the feature points whose three-dimensional coordinates have been newly calculated. Specifically, the first position estimation unit 11 repeatedly executes the process of calculating the three-dimensional coordinates of the feature points in accordance with the movement of the mobile body 100. Then, the first position estimation unit 11 constructs three-dimensional point cloud data representing objects around the mobile body using the repeatedly calculated feature points, and generates or updates the three-dimensional environmental map.

In addition, the first position estimation unit 11 uses the two-dimensional coordinates of the feature points in the current frame and the calculated three-dimensional coordinates of the feature points to estimate the three-dimensional coordinates of the camera 20 in the current frame, i.e., the three-dimensional coordinates of the moving body 100 on the three-dimensional environmental map.

Specifically, estimating the three-dimensional coordinates of the camera 20 in the current frame corresponds to solving the PNP problem. Therefore, the first position estimation unit 11 calculates a perspective projection matrix using, for example, the DLT (Direct Linear Transformation) method, and decomposes the obtained perspective projection matrix into a rotation matrix _Rt and a translation vector _tt , thereby calculating the three-dimensional coordinates of the camera 20. The first position estimation unit 11 stores the generated three-dimensional environmental map and information on the estimated three-dimensional coordinates in the storage unit 14.

In this embodiment, the second position estimation unit 12 acquires depth data, for example, as shown in FIG. 4(a), from the two-dimensional depth sensor 30 and stores the acquired depth data in the storage unit 14. The second position estimation unit 12 then uses the acquired depth data to generate two-dimensional point cloud data around the moving body 100.

The second position estimation unit 12 also estimates the two-dimensional coordinates of the mobile unit 100 using the generated two-dimensional point cloud data and the two-dimensional autonomous driving map shown in FIG. 4(b). The two-dimensional autonomous driving map is a map created in advance and is also called a two-dimensional environmental map. Specifically, as shown in FIG. 5(a), the second position estimation unit 12 compares the two-dimensional point cloud data with the two-dimensional autonomous driving map and matches them as shown in FIG. 5(b). Then, the second position estimation unit 12 applies the position of the mobile unit 100 in the two-dimensional point cloud data to the two-dimensional autonomous driving map to estimate the two-dimensional coordinates of the mobile unit 100. The second position estimation unit 12 stores information on the estimated two-dimensional coordinates in the memory unit 14.

In this embodiment, the correction processing unit 13 calculates the variation in the depth data output by the two-dimensional depth sensor 30 at one time, and determines whether the calculated variation is equal to or greater than a threshold, which is a set condition, and determines whether the set condition is met, i.e., whether the variation is equal to or greater than the threshold. This is because if the variation is less than the threshold, the area around the moving object is likely to be a flat wall, etc., and in this case, the accuracy of the estimation of the two-dimensional coordinates of the moving object by the second position estimation unit 12 may be reduced. On the other hand, if the variation is equal to or greater than the threshold, there are irregularities around the moving object, and in this case, the accuracy of the estimation of the two-dimensional coordinates of the moving object by the second position estimation unit 12 is ensured.

Specifically, as shown in FIG. 6, the correction processing unit 13 identifies the start point and end point in the depth data output at one time, and then sets the axis passing through the start point and end point as the x-axis and the axis perpendicular to the x-axis as the y-axis. The correction processing unit 13 then calculates dx/dy for every two adjacent points, and then integrates the calculated dx/dy, treating the resulting integrated value (Σdx/dy) as the variation. Thereafter, if the integrated value (Σdx/dy) representing the variation is equal to or greater than the threshold value α, the correction processing unit 13 determines that the set condition is met; otherwise, it determines that the set condition is not met.

If the correction processing unit 13 determines that the set conditions are met, it calculates the difference between the two-dimensional coordinates of the moving body 100 and the three-dimensional coordinates of the moving body 100, and corrects the three-dimensional coordinates of the moving body 100 and the three-dimensional environmental map based on the calculated difference.

Specifically, as shown in FIG. 7(a), the correction processing unit 13 first projects the coordinates of the moving body 100 on the three-dimensional environmental map onto two-dimensional coordinates on the two-dimensional autonomous driving map, and identifies the two-dimensional coordinates of the moving body 100 (indicated by "■" in the figure). Then, the correction processing unit 13 calculates the difference between the two-dimensional coordinates of the moving body 100 (indicated by "x" in the figure) and the coordinates of the moving body projected from the three-dimensional coordinates (indicated by "■" in the figure).

Next, the correction processing unit 13 corrects the coordinates of each point cloud that makes up the three-dimensional environmental map based on the difference in the two-dimensional coordinates represented by the two-dimensional autonomous driving map, and also corrects the three-dimensional coordinates of the moving body 100. The corrected coordinates become the final coordinates of the moving body in the current frame.

[Device operation]
Next, the operation of the position estimation device 10 in the embodiment will be described with reference to Figures 8 to 10. In the following description, Figures 1 to 7 will be referenced as appropriate. In the embodiment, a position estimation method is implemented by operating the position estimation device 10. Therefore, the description of the position estimation method in the embodiment will be replaced by the following description of the operation of the position estimation device 10.

First, the process of estimating the two-dimensional coordinates of a moving object will be described using Figure 8. Figure 8 is a flow diagram showing the operation of a position estimation device during the process of estimating the two-dimensional coordinates of a moving object in this embodiment.

As shown in FIG. 8, first, the second position estimation unit 12 acquires depth data (see FIG. 4(a)) from the two-dimensional depth sensor 30 and uses the acquired depth data to generate two-dimensional point cloud data around the moving object 100 (step A1). The two-dimensional depth sensor 30 outputs depth data at set intervals. In step A1, the second position estimation unit 12 acquires the depth data output in one go.

Next, as shown in Figures 5(a) and 5(b), the second position estimation unit 12 compares the two-dimensional point cloud data generated in step A1 with a two-dimensional autonomous driving map created in advance and matches the two (step A2).

Then, the second position estimation unit 12 applies the position of the moving body 100 in the two-dimensional point cloud data to the two-dimensional autonomous driving map to estimate the two-dimensional coordinates of the moving body 100 (step A3).

After that, when the two-dimensional depth sensor 30 outputs new depth data, steps A1 to A3 are executed again. In other words, steps A1 to A3 are executed each time the two-dimensional depth sensor 30 outputs depth data.

Next, the process of generating or updating a three-dimensional environmental map will be described using Figure 9. Figure 9 is a flow diagram showing the operation of a position estimation device during the process of generating or updating a three-dimensional environmental map in this embodiment.

As shown in FIG. 9, first, the first position estimation unit 11 acquires one frame of image data output from the camera 20 (step B1). Next, the first position estimation unit 11 extracts corresponding feature points from the image data of the frame (current frame) acquired in step B1 with another frame, and sets pairs of corresponding feature points (step B2).

Next, the first position estimation unit 11 calculates the three-dimensional coordinates of each feature point using the set of feature points set in step B2 (step B3). Furthermore, the first position estimation unit 11 updates the three-dimensional environmental map using the calculated three-dimensional coordinates of each feature point (step B4). If a three-dimensional environmental map has not yet been generated, the first position estimation unit 11 generates one using the calculated three-dimensional coordinates of each feature point. Steps B1 to B4 are repeatedly executed, and the three-dimensional environmental map expands each time.

When the camera 20 outputs new image data, steps B1 to B4 are executed again. In other words, steps B1 to B4 are executed each time the camera 20 outputs image data.

Next, the estimation and correction process of the three-dimensional coordinates of a moving object will be described using Figure 10. Figure 10 is a flow diagram showing the operation of the position estimation device during the estimation and correction process of the position of a moving object in this embodiment.

First, the first position estimation unit 11 acquires the two-dimensional coordinates of each feature point in the current frame acquired in step B1 shown in FIG. 9, and then identifies each feature point whose three-dimensional coordinates were calculated in step B3 for a previous frame acquired before the current frame, and sets the two-dimensional coordinates of each acquired feature point and the three-dimensional coordinates of each identified feature point as a set of data (step C1).

Next, the first position estimation unit 11 uses the set of data set in step C1 to estimate the three-dimensional coordinates of the camera 20 in the current frame, i.e., the three-dimensional coordinates on the three-dimensional environmental map of the moving body 100 (step C2). Specifically, in step C2, the first position estimation unit 11 applies the set of data to the DLT method to obtain a perspective projection matrix, and calculates the three-dimensional coordinates of the camera 20 by decomposing the obtained perspective projection matrix into a rotation matrix _Rt and a translation vector _tt .

Next, the correction processing unit 13 determines whether the set conditions for the two-dimensional coordinates of the moving body 100 are met (step C3). Specifically, in step C3, the correction processing unit 13 calculates the variation in the depth data output by the two-dimensional depth sensor 30 at one time, and determines whether the calculated variation is equal to or greater than a threshold value.

If the result of the determination in step C3 is that the set conditions are met, the correction processing unit 13 calculates the difference between the two-dimensional coordinates of the moving body 100 estimated in step A3 shown in Figure 8 and the three-dimensional coordinates of the moving body 100 estimated in step C2 (step C4).

Next, the correction processing unit 13 corrects the three-dimensional coordinates of the moving body 100 and the three-dimensional environmental map based on the difference calculated in step C4 (step C5).

Then, the correction processing unit 13 outputs the corrected three-dimensional coordinates of the moving body 100 to the control device 40 (step C6).

Furthermore, if the result of the determination in step C3 indicates that the set conditions are not met, the correction processing unit 13 outputs the three-dimensional coordinates estimated in step C2 to the control device 40 (step C6).

When step C6 is executed, the control device 40 determines the direction of travel and speed of travel of the mobile object 100 based on the output three-dimensional coordinates. The control device 40 also controls the drive device 50 in accordance with the determined direction of travel and speed of travel, causing the mobile object 100 to move along a predetermined route.

Steps C1 to C6 are repeatedly executed while the moving body 100 is moving. In this embodiment, steps C1 to C6 are executed independently of the execution cycle of steps B1 to B4.

As described above, according to the embodiment, the three-dimensional coordinates of the moving body 100 estimated from the image are corrected using depth data from the two-dimensional depth sensor 30 if the set conditions are met. This significantly improves the accuracy of the moving body's position estimation. Furthermore, since the embodiment does not require the use of a costly three-dimensional depth sensor, such as a three-dimensional LiDAR, costs are kept down compared to when such a sensor is used.

[program]
The program in the embodiment may be a program that causes a computer to execute steps A1 to A3 shown in Fig. 8, steps B1 to B4 shown in Fig. 9, and steps C1 to C6 shown in Fig. 10. By installing and executing this program on a computer, the position estimation device 10 and position estimation method in the embodiment can be realized. In this case, the processor of the computer functions as the first position estimation unit 11, the second position estimation unit 12, and the correction processing unit 13 and performs processing. In addition, the storage device of the computer functions as the storage unit 14.

In addition to general-purpose PCs, examples of computers include smartphones and tablet terminal devices. Furthermore, the computer may also function as the control device 40 of the mobile object 100. In this case, the position estimation device 10 will be built on the operating system of the control device 40 of the mobile object 100.

Furthermore, the program in the embodiment may be executed by a computer system constructed by multiple computers. In this case, for example, each computer may function as either the first position estimation unit 11, the second position estimation unit 12, or the correction processing unit 13.

[Physical configuration]
A computer that implements the position estimation device 10 by executing a program according to the embodiment will now be described with reference to Fig. 11. Fig. 11 is a block diagram showing an example of a computer that implements the position estimation device according to the embodiment.

As shown in FIG. 10, the computer 110 includes a CPU (Central Processing Unit) 111, a main memory 112, a storage device 113, an input interface 114, a display controller 115, a data reader/writer 116, and a communication interface 117. These components are connected to each other via a bus 121 so that they can communicate data with each other.

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

The CPU 111 deploys the program in this embodiment, which is composed of a group of codes stored in the storage device 113, into the main memory 112 and executes each code in a predetermined order to perform various calculations. The main memory 112 is typically a volatile storage device such as DRAM (Dynamic Random Access Memory).

Furthermore, the program in this embodiment is provided in a state stored on a computer-readable recording medium 120. Note that the program in this embodiment may also be distributed over the Internet connected via the communication interface 117.

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

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

Specific examples of the recording medium 120 include general-purpose semiconductor storage devices such as CF (Compact Flash (registered trademark)) and SD (Secure Digital), magnetic recording media such as flexible disks, and optical recording media such as CD-ROMs (Compact Disk Read Only Memory).

Note that the position estimation device 10 in this embodiment can also be realized by using hardware corresponding to each component, rather than a computer with a program installed. Furthermore, the position estimation device 10 may be partially realized by a program and the remaining components by hardware. Examples of hardware include electronic circuits.

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

(Appendix 1)
a first position estimation unit that acquires image data output from a camera mounted on a moving object on a frame-by-frame basis, extracts, for each frame, feature points corresponding to each other in the image data of the frame, and estimates three-dimensional coordinates of the moving object in the frame based on the extracted feature points;
a second position estimation unit that acquires depth data continuously output from a two-dimensional depth sensor mounted on the moving body and estimates two-dimensional coordinates of the moving body using the acquired depth data;
a correction processing unit that determines whether a set condition for the estimated two-dimensional coordinates of the moving object is satisfied, and if the set condition is satisfied, corrects the three-dimensional coordinates of the moving object using the two-dimensional coordinates of the moving object;
Equipped with
A position estimation device characterized by:

(Appendix 2)
2. The position estimation device of claim 1,
the first position estimation unit calculates three-dimensional coordinates of the feature points using sets of corresponding feature points, and generates or updates a three-dimensional environmental map composed of point cloud data using the feature points whose three-dimensional coordinates have been calculated;
A position estimation device characterized by:

(Appendix 3)
3. The position estimation device according to claim 1,
the correction processing unit calculates a difference between the two-dimensional coordinates of the moving body and the three-dimensional coordinates of the moving body, and corrects the three-dimensional coordinates of the moving body and the three-dimensional environmental map based on the calculated difference.
A position estimation device characterized by:

(Appendix 4)
4. A position estimation device according to any one of claims 1 to 3,
the second position estimation unit generates two-dimensional point cloud data of the periphery of the moving body using the acquired depth data, and estimates two-dimensional coordinates of the moving body using the generated two-dimensional point cloud data and a two-dimensional autonomous driving map created in advance;
A position estimation device characterized by:

(Appendix 5)
5. A position estimation device according to any one of claims 1 to 4,
the correction processing unit calculates a variation in the depth data output by the two-dimensional depth sensor at one time, and determines that the set condition is satisfied when the calculated variation is equal to or greater than a threshold value;
A position estimation device characterized by:

(Appendix 6)
a first position estimation step of acquiring image data output from a camera mounted on a moving object on a frame-by-frame basis, extracting, for each frame, feature points corresponding to each other in the image data of the frame, and estimating three-dimensional coordinates of the moving object in the frame based on the extracted feature points;
a second position estimation step of acquiring depth data continuously output from a two-dimensional depth sensor mounted on the moving body and estimating two-dimensional coordinates of the moving body using the acquired depth data;
a correction processing step of determining whether a set condition for the estimated two-dimensional coordinates of the moving body is satisfied, and if the set condition is satisfied, correcting the three-dimensional coordinates of the moving body using the two-dimensional coordinates of the moving body;
having
A position estimation method comprising:

(Appendix 7)
7. The location estimation method according to claim 6, further comprising:
In the first position estimation step, three-dimensional coordinates of the feature points are calculated using sets of corresponding feature points, and a three-dimensional environmental map composed of point cloud data is generated or updated using the feature points whose three-dimensional coordinates have been calculated.
A position estimation method comprising:

(Appendix 8)
8. The location estimation method according to claim 6, further comprising:
In the correction processing step, a difference between the two-dimensional coordinates of the moving body and the three-dimensional coordinates of the moving body is calculated, and the three-dimensional coordinates of the moving body and the three-dimensional environmental map are corrected based on the calculated difference.
A position estimation method comprising:

(Appendix 9)
9. A location estimation method according to any one of Supplementary Notes 6 to 8, comprising:
In the second position estimation step, two-dimensional point cloud data of the periphery of the moving body is generated using the acquired depth data, and two-dimensional coordinates of the moving body are estimated using the generated two-dimensional point cloud data and a two-dimensional autonomous driving map that has been created in advance.
A position estimation method comprising:

(Appendix 10)
10. A location estimation method according to any one of Supplementary Notes 6 to 9, comprising:
In the correction processing step, a variation in the depth data output at one time by the two-dimensional depth sensor is calculated, and if the calculated variation is equal to or greater than a threshold value, it is determined that the set condition is satisfied.
A position estimation method comprising:

(Appendix 11)
On the computer,
a first position estimation step of acquiring image data output from a camera mounted on a moving object on a frame-by-frame basis, extracting, for each frame, feature points corresponding to each other in the image data of the frame, and estimating three-dimensional coordinates of the moving object in the frame based on the extracted feature points;
a second position estimation step of acquiring depth data continuously output from a two-dimensional depth sensor mounted on the moving body and estimating two-dimensional coordinates of the moving body using the acquired depth data;
a correction processing step of determining whether a set condition for the estimated two-dimensional coordinates of the moving body is satisfied, and if the set condition is satisfied, correcting the three-dimensional coordinates of the moving body using the two-dimensional coordinates of the moving body;
A program that executes.

(Appendix 12)
12. The program of claim 11,
In the first position estimation step, three-dimensional coordinates of the feature points are calculated using sets of corresponding feature points, and a three-dimensional environmental map composed of point cloud data is generated or updated using the feature points whose three-dimensional coordinates have been calculated.
A program characterized by:

(Appendix 13)
13. The program according to claim 11 or 12,
In the correction processing step, a difference between the two-dimensional coordinates of the moving body and the three-dimensional coordinates of the moving body is calculated, and the three-dimensional coordinates of the moving body and the three-dimensional environmental map are corrected based on the calculated difference.
A program characterized by:

(Appendix 14)
14. The program according to any one of Supplementary Notes 11 to 13,
In the second position estimation step, two-dimensional point cloud data of the periphery of the moving body is generated using the acquired depth data, and two-dimensional coordinates of the moving body are estimated using the generated two-dimensional point cloud data and a two-dimensional autonomous driving map that has been created in advance.
A program characterized by:

(Appendix 15)
15. The program according to any one of Supplementary Notes 11 to 14,
In the correction processing step, a variation in the depth data output at one time by the two-dimensional depth sensor is calculated, and if the calculated variation is equal to or greater than a threshold value, it is determined that the set condition is satisfied.
A program characterized by:

As described above, the present disclosure makes it possible to improve the accuracy of self-position estimation while suppressing increases in costs. The present invention is useful for autonomous driving systems for mobile objects.

REFERENCE SIGNS LIST 10 Position estimation device 11 First position estimation unit 12 Second position estimation unit 13 Correction processing unit 14 Storage unit 20 Camera 30 Two-dimensional depth sensor 40 Control device 50 Driving device 100 Mobile object 110 Computer 111 CPU
112 Main memory 113 Storage device 114 Input interface 115 Display controller 116 Data reader/writer 117 Communication interface 118 Input device 119 Display device 120 Recording medium 121 Bus

Claims (7)

a first position estimation unit that acquires image data output from a camera mounted on a moving object on a frame-by-frame basis, extracts, for each frame, feature points corresponding to each other in the image data of the frame, and estimates three-dimensional coordinates of the moving object in the frame based on the extracted feature points;
a second position estimation unit that acquires depth data continuously output from a two-dimensional depth sensor mounted on the moving body and estimates two-dimensional coordinates of the moving body using the acquired depth data;
a correction processing unit that calculates a variation in the depth data, determines whether a setting condition is satisfied when the calculated variation is equal to or greater than a threshold, and corrects the three-dimensional coordinates of the moving body using the two-dimensional coordinates of the moving body when the setting condition is satisfied;
Equipped with
A position estimation device characterized by: 2. The position estimation device according to claim 1,
the first position estimation unit calculates three-dimensional coordinates of the feature points using the sets of corresponding feature points, and generates or updates a three-dimensional environmental map composed of point cloud data using the feature points whose three-dimensional coordinates have been calculated;
A position estimation device characterized by: 3. The position estimation device according to claim 2 ,
the correction processing unit calculates a difference between the two-dimensional coordinates of the moving body and the three-dimensional coordinates of the moving body, and corrects the three-dimensional coordinates of the moving body and the three-dimensional environmental map based on the calculated difference.
A position estimation device characterized by: The position estimation device according to any one of claims 1 to 3,
the second position estimation unit generates two-dimensional point cloud data of the periphery of the moving body using the acquired depth data, and estimates two-dimensional coordinates of the moving body using the generated two-dimensional point cloud data and a two-dimensional autonomous driving map created in advance;
A position estimation device characterized by: 5. A position estimation device according to claim 1,
the correction processing unit calculates a variation in the depth data output by the two-dimensional depth sensor at one time, and determines that the set condition is satisfied when the calculated variation is equal to or greater than a threshold value;
A position estimation device characterized by: a first position estimation step of acquiring image data output from a camera mounted on a moving object on a frame-by-frame basis, extracting, for each frame, feature points corresponding to each other in the image data of the frame, and estimating three-dimensional coordinates of the moving object in the frame based on the extracted feature points;
a second position estimation step of acquiring depth data continuously output from a two-dimensional depth sensor mounted on the moving body and estimating two-dimensional coordinates of the moving body using the acquired depth data;
a correction processing step of calculating a variation in the depth data, determining whether a setting condition is satisfied when the calculated variation is equal to or greater than a threshold , and correcting the three-dimensional coordinates of the moving body using the two-dimensional coordinates of the moving body if the setting condition is satisfied;
having
A position estimation method comprising: On the computer,
a first position estimation step of acquiring image data output from a camera mounted on a moving object on a frame-by-frame basis, extracting, for each frame, feature points corresponding to each other in the image data of the frame, and estimating three-dimensional coordinates of the moving object in the frame based on the extracted feature points;
a second position estimation step of acquiring depth data continuously output from a two-dimensional depth sensor mounted on the moving body and estimating two-dimensional coordinates of the moving body using the acquired depth data;
a correction processing step of calculating a variation in the depth data, determining whether a setting condition is satisfied when the calculated variation is equal to or greater than a threshold , and correcting the three-dimensional coordinates of the moving body using the two-dimensional coordinates of the moving body if the setting condition is satisfied;
A program that executes.