Position estimation device, position estimation method, and program

Abstract

To improve accuracy of self-position estimation while suppressing cost increase.SOLUTION: A position estimation device 10 comprises: a first position estimation unit 11 for acquiring image data output from a camera mounted on a mobile on a frame basis, extracting for each frame a feature point correspondent with another frame from the image data of the frame and estimating three-dimensional coordinates of the mobile in the frame based on the extracted feature point; a second position estimation unit 12 for acquiring depth data continuously output from a two-dimensional depth sensor mounted on the mobile and estimating two-dimensional coordinates of the mobile using the acquired depth data; and a correction processing unit 13 for determining whether or not preset conditions regarding the estimated two-dimensional coordinates of the mobile are satisfied and, in a case where the preset conditions are satisfied, correcting the three-dimensional coordinates of the mobile using the two-dimensional coordinates of the mobile.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present invention relates to a position estimation device and a position estimation method for estimating the position of a moving object, and further to a program for implementing these. [Background technology]

[0002] In recent years, advances in sensing technology have led to the development of SLAM (Simultaneous Localization and Mapping). SLAM is a technology in which a mobile object equipped with a sensor simultaneously estimates its own position and creates an environmental map (see, for example, Patent Document 1 and Non-Patent Document 1). With SLAM, a mobile object such as an autonomously traveling robot does not need to move randomly, but can move along an autonomous traveling map obtained from an environmental map, thereby improving the efficiency of its movement.

[0003] 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, a moving body acquires image data output from a camera for each frame, searches for feature points in the latest frame that correspond to feature points extracted from past frames, and extracts pairs of feature points each made up of corresponding feature points.

[0004] The moving body then calculates a camera matrix for the latest frame using the set of feature points, 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 moving body calculates the camera position from the camera matrix and performs self-location estimation. Furthermore, the moving body generates or updates an environmental map composed of a three-dimensional point cloud (hereinafter referred to as a "three-dimensional environmental map") using the feature points whose three-dimensional coordinates have been calculated.

[0005] The mobile unit then generates a 2D environmental map by converting the 3D environmental map into 2D. However, with camera-based SLAM, the point cloud that makes up the 3D environmental map is a feature point on the image, so it is difficult to increase the density of the point cloud. As a result, the autonomous driving map has few distinctive features, making it difficult for the mobile unit to drive autonomously with high accuracy.

[0006] On the other hand, if a depth sensor such as LiDAR (light detection and ranging) is used as the sensor, the above problems can be solved. 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.

[0007] 3D LiDAR can measure depth for each pixel in a captured image, allowing it to calculate the 3D coordinates of points that are not characteristic points, 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 devices. 3D LiDAR also makes it possible to accurately estimate the vehicle's position. [Prior art documents] [Patent documents]

[0008] [Patent Document 1] Japanese Patent Publication No. 2020-76714 [Non-patent literature]

[0009] [Non-Patent Document 1] Raul Mur-Artal, JMM 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"> Summary of the Invention [Problem to be solved by the invention]

[0010] However, 3D LiDAR has problems such as high cost and a large processing load on the system. On the other hand, using 2D LiDAR solves the high cost problem, but it has the problem of not being able to create a 3D environmental map. In addition, it is difficult for 2D LiDAR to extract characteristic features in environments with little unevenness, and when using 2D LiDAR, the accuracy of self-localization decreases.

[0011] An 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 an increase in costs. [Means for solving the problem]

[0012] 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 body is satisfied, and if the set condition is satisfied, corrects the three-dimensional coordinates of the moving body using the two-dimensional coordinates of the moving body; The present invention is characterized in that it is provided with:

[0013] 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:

[0014] 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: [Effects of the Invention]

[0015] As described above, according to the present disclosure, it is possible to improve the accuracy of self-location estimation while suppressing increases in costs. [Brief explanation of the drawings]

[0016] [Figure 1] FIG. 1 is a diagram showing a schematic configuration of a position estimation device according to an embodiment. [Figure 2] FIG. 2 is a diagram specifically illustrating an example of the configuration of the position estimation device according to the embodiment. [Figure 3] FIG. 3 is a diagram illustrating the processing in the first position estimation unit. [Figure 4] 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. [Figure 5] 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. [Figure 6] FIG. 6 is a diagram showing a process of determining the setting conditions in the correction processing unit. [Figure 7] 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. [Figure 8] FIG. 8 is a flowchart showing the operation of the position estimation device during the estimation process of the two-dimensional coordinates of a moving object according to the embodiment. [Figure 9] 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 the embodiment. [Figure 10] 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 in the embodiment. [Figure 11] FIG. 11 is a block diagram illustrating an example of a computer that realizes the position estimation device according to the embodiment. DETAILED DESCRIPTION OF THE INVENTION

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

[0018] [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.

[0019] The position estimation device 10 in the embodiment shown in FIG. 1 is a 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.

[0020] 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, for each frame, feature points corresponding to each other in the image data of that frame. Then, the first position estimation unit 11 estimates, for each frame, the three-dimensional coordinates of the moving object in that frame based on the extracted feature points.

[0021] The second position estimation unit 12 acquires depth data continuously output from a two-dimensional depth sensor mounted on the moving body, and estimates the two-dimensional coordinates of the moving body using the acquired depth data.

[0022] 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.

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

[0024] Next, the configuration and functions of the position estimation device in the embodiment will be specifically described with reference to Figs. 2 to 7. Fig. 2 is a configuration diagram specifically showing an example of the configuration of the position estimation device in the embodiment. Fig. 3 is a diagram explaining 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 map for autonomous driving (two-dimensional environmental map) created in advance. Fig. 5 is a diagram showing the processing in the second position estimation unit, and Figs. 5(a) and 5(b) show the flow of a series of processes. Fig. 6 is a diagram showing the process of determining setting conditions in the correction processing unit. Fig. 7 is a diagram explaining the processing in the correction processing unit, and Figs. 7(a) and 7(b) show the flow of a series of processes.

[0025] 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 driving device 50.

[0026] The camera 20 is an imaging device equipped with a solid-state imaging element. The camera 20 is attached so as to be able to capture an image of the periphery of the moving body 100, and outputs image data of a two-dimensional image of the periphery of the moving body 100.

[0027] 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 surroundings 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 sensor, thereby measuring the distance to an object on the two-dimensional plane and outputting the depth data. An example of the depth data output by the two-dimensional depth sensor 30 is shown in FIG. 4(a).

[0028] The control device 40 is a device including a processor and a memory, and controls the moving body 100. For example, the control device 40 determines the traveling direction and traveling speed of the moving body 100 based on the three-dimensional coordinates of the moving body 100 estimated by the position estimation device 10. Furthermore, the control device 40 controls the driving device 50 in accordance with the determined traveling direction and traveling speed, and moves the moving body 100 along a preset route.

[0029] 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 may be configured to change the direction of movement of the mobile body 100, and may include, for example, steering wheels and an actuator for moving the steering wheels. The steering mechanism may also be a mechanism that controls the torque of the left and right drive wheels.

[0030] 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 storage unit 14.

[0031] In the embodiment, first position estimation unit 11 acquires image data output from camera 20 on a frame-by-frame basis. Then, for each frame, 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.

[0032] 3, the first position estimation unit 11 acquires frame 22a at time [t] and acquires frame 22b at time [t+1]. 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 extracts feature points 23a to 27a in frame 22a, it extracts corresponding feature points 23b to 27b in frame 22b. As a result, feature point pairs (23a, 23b), (24a, 24b), (25a, 25b), (26a, 26b), and (27a, 27b) are set.

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

[0034] (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>

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

[0036] Thereafter, the first position estimation unit 11 generates a three-dimensional environmental map composed of point cloud data of the feature points using the feature points whose three-dimensional coordinates have been calculated. 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 a process of calculating the three-dimensional coordinates of the feature points in accordance with the movement of the moving body 100. Then, the first position estimation unit 11 constructs three-dimensional point cloud data representing objects around the moving body using the repeatedly calculated feature points, and generates or updates the three-dimensional environmental map.

[0037] In addition, the first position estimation unit 11 uses the two-dimensional coordinates of the feature points in the current frame and the three-dimensional coordinates of the calculated 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.

[0038] 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 obtains a perspective projection matrix using, for example, a DLT method (Direct Linear Transformation method), and converts the obtained perspective projection matrix into a rotation matrix R t and the translation vector t t and calculates 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 .

[0039] 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. Then, the second position estimation unit 12 generates two-dimensional point cloud data around the moving object 100 using the acquired depth data.

[0040] Furthermore, the second position estimation unit 12 estimates the two-dimensional coordinates of the moving body 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 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. The second position estimation unit 12 stores information on the estimated two-dimensional coordinates in the storage unit 14.

[0041] 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 satisfied, 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 considered 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.

[0042] Specifically, as shown in Fig. 6, the correction processing unit 13 identifies the start point and the end point in the depth data output at one time, and further sets the axis passing through the start point and the 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, further integrates the calculated dx / dy, and regards the obtained integrated value (Σdx / dy) as the variation. Thereafter, if the integrated value (Σdx / dy) representing the variation is equal to or greater than a threshold value α, the correction processing unit 13 determines that the set condition is satisfied; otherwise, it determines that the set condition is not satisfied.

[0043] If the correction processing unit 13 determines that the set conditions are satisfied, 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.

[0044] 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 of 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).

[0045] Next, the correction processing unit 13 corrects the coordinates of each of the points constituting 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.

[0046] [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 referred to 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.

[0047] First, the process of estimating the two-dimensional coordinates of a moving body will be described with reference to Fig. 8. Fig. 8 is a flow chart showing the operation of the position estimation device during the process of estimating the two-dimensional coordinates of a moving body in this embodiment.

[0048] 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 generates two-dimensional point cloud data around the moving object 100 using the acquired depth data (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 at one time.

[0049] 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).

[0050] 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, and estimates the two-dimensional coordinates of the moving body 100 (step A3).

[0051] Thereafter, steps A1 to A3 are executed again whenever two-dimensional depth sensor 30 outputs new depth data. That is, steps A1 to A3 are executed every time two-dimensional depth sensor 30 outputs new depth data.

[0052] Next, the process of generating or updating a three-dimensional environmental map will be described with reference to Fig. 9. Fig. 9 is a flow diagram showing the operation of the position estimation device during the process of generating or updating a three-dimensional environmental map in the embodiment.

[0053] 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 and another frame, and sets pairs of corresponding feature points (step B2).

[0054] 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). Note that if the three-dimensional environmental map has not yet been generated, the first position estimation unit 11 generates the three-dimensional environmental map using the calculated three-dimensional coordinates of each feature point. Steps B1 to B4 are repeatedly executed, and the three-dimensional environmental map is expanded each time.

[0055] When camera 20 outputs new image data, steps B1 to B4 are executed again. That is, steps B1 to B4 are executed every time camera 20 outputs image data.

[0056] Next, the estimation and correction processing of the three-dimensional coordinates of a moving body will be described with reference to Fig. 10. Fig. 10 is a flow diagram showing the operation of the position estimation device during the estimation and correction processing of the position of a moving body in this embodiment.

[0057] 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 further 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 acquired two-dimensional coordinates of each feature point and the identified three-dimensional coordinates of each feature point as one set of data (step C1).

[0058] 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 of the moving body 100 on the three-dimensional environmental map (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 converts the obtained perspective projection matrix into a rotation matrix R t and the translation vector t t The three-dimensional coordinates of the camera 20 are calculated by decomposing the image data into the following two points:

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

[0060] If the result of the judgment 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).

[0061] 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).

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

[0063] If the result of the determination in step C3 is that the set conditions are not satisfied, the correction processing unit 13 outputs the three-dimensional coordinates estimated in step C2 to the control device 40 (step C6).

[0064] When step C6 is executed, the control device 40 determines the traveling direction and traveling speed of the moving body 100 based on the output three-dimensional coordinates. The control device 40 also controls the driving device 50 in accordance with the determined traveling direction and traveling speed to move the moving body 100 along a preset route.

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

[0066] As described above, according to the embodiment, the three-dimensional coordinates of the moving object 100 estimated from an 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 position estimation of the moving object. Furthermore, the embodiment does not require the use of a costly three-dimensional depth sensor, such as a three-dimensional LiDAR, and therefore reduces costs compared to using such a sensor.

[0067] [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 in a computer, the position estimation device 10 and the position estimation method in the embodiment can be realized. In this case, the processor of the computer functions as first position estimation unit 11, second position estimation unit 12, and correction processing unit 13 and performs processing. In addition, the storage device of the computer functions as storage unit 14.

[0068] The computer may be a general-purpose PC, a smartphone, or a tablet terminal. Furthermore, the computer may function as the control device 40 of the moving object 100. In this case, the position estimation device 10 is built on the operating system of the control device 40 of the moving object 100.

[0069] The program in the embodiment may be executed by a computer system constructed by a plurality of computers. In this case, for example, each computer may function as one of the first position estimation unit 11, the second position estimation unit 12, and the correction processing unit 13.

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

[0071] 10, a 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 as to be able to communicate data with each other.

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

[0073] The CPU 111 loads a program in the embodiment, which is composed of a group of codes and 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 a DRAM (Dynamic Random Access Memory).

[0074] The program in the embodiment is provided in a state stored in a computer-readable recording medium 120. The program in the embodiment may be distributed over the Internet connected via the communication interface 117.

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

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

[0077] 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, or optical recording media such as CD-ROMs (Compact Disk Read Only Memory).

[0078] The position estimation device 10 in this embodiment can be realized by using hardware corresponding to each part, rather than a computer on which a program is installed. Furthermore, the position estimation device 10 may be partially realized by a program and the remaining part by hardware. An example of hardware is an electronic circuit.

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

[0080] (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 body is satisfied, and if the set condition is satisfied, corrects the three-dimensional coordinates of the moving body using the two-dimensional coordinates of the moving body; Equipped with A position estimation device characterized by:

[0081] (Appendix 2) 10. The location estimation device of 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:

[0082] (Appendix 3) 3. The position estimation device according to claim 1, further comprising: 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:

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

[0084] (Appendix 5) A position estimation device according to any one of Supplementary Notes 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:

[0085] (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:

[0086] (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 the corresponding sets of 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:

[0087] (Appendix 8) 8. The location estimation method according to claim 6 or 7, 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:

[0088] (Appendix 9) A location estimation method according to any one of Supplementary Notes 6 to 8, 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 created in advance. A position estimation method comprising:

[0089] (Appendix 10) A position estimation method according to any one of Supplementary Notes 6 to 9, 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, it is determined that the set condition is satisfied. A position estimation method comprising:

[0090] (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.

[0091] (Appendix 12) 12. The program of claim 11, In the first position estimation step, three-dimensional coordinates of the feature points are calculated using the corresponding sets of 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:

[0092] (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:

[0093] (Appendix 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 created in advance. A program characterized by:

[0094] (Appendix 15) A 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, it is determined that the set condition is satisfied. A program characterized by: [Industrial Applicability]

[0095] As described above, according to the present disclosure, it is possible to improve the accuracy of self-location estimation while suppressing an increase in costs. The present invention is useful for an autonomous driving system for a moving body. [Explanation of symbols]

[0096] 10 Position estimation device 11 First position estimation unit 12 Second position estimation unit 13 Correction processing section 14 Storage section 20 Camera 30 2D depth sensor 40 Control device 50 Drive unit 100 Mobile 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 Devices 119 Display Device 120 Recording Media 121 Bus< / url:>

Claims

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

4. 4. The position estimation device according to claim 1, the second position estimation unit generates two-dimensional point cloud data around 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 that has been created in advance; A position estimation device characterized by:

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:

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 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:

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

Images