Patrol device space positioning method and equipment based on RGB-D image, and medium
Through the space positioning method of the patrol based on RGB-D images, the feature points and depth information are extracted using RGB-D data, combined with the incremental beam adjustment method, the problem of low positioning accuracy of the patrol in extraterrestrial celestial environment is solved, and the problem of low positioning accuracy of the patrol in the extraterrestrial celestial environment is realized. High-precision pose estimation and trajectory reconstruction in weak texture environments.
Patent Information
- Application Number
- CN202410797957.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-06-20
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2044-06-20
AI Technical Summary
The prior art has low positioning accuracy of patrol devices in extraterrestrial celestial environments, especially in weak texture environments, and the calculation is complicated and cumbersome.
Using the paratrooper spatial positioning method based on RGB-D images, the three-dimensional coordinates are calculated by extracting feature points from RGB-D data, and the tracking matching results and depth map information of adjacent frame images are used, combined with the incremental beam of secondary optimization Method adjustment method optimizes the pose estimation results and improves positioning accuracy and robustness.
In the deep space detection GNSS denial scenario, the position pose is estimated through the RGB-D camera data carried by the patrol itself, which reduces the three-dimensional data solution pressure and calculation error, improves positioning accuracy and robustness, and is suitable for extraterrestrial weaknesses. Texture environment.
Smart Images

Figure CN118941627A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of space positioning technology, and in particular to a patroller space positioning method, device and medium based on RGB-D images. Background Art
[0002] The large-scale patrol exploration of extraterrestrial bodies can effectively ensure the continuous output of scientific research results of deep space in-situ exploration, and its autonomous navigation capability is an important guarantee for its safe and efficient roaming. In the environment of extraterrestrial bodies such as the moon and Mars, there is no Global Navigation Satellite System (GNSS) system that has been widely used on Earth, so it is impossible to obtain accurate patrol position information through satellites.
[0003] After searching, the Chinese invention patent application CN109631876A discloses a patrol probe positioning method based on a single camera navigation image, which uses the offset in the zero coordinate system, the body coordinate system and the patrol coordinate system for positioning. However, the above scheme involves coordinate conversion in multiple coordinate systems, and the calculation is extremely cumbersome. In addition, the above scheme is difficult to apply to the weak texture environment of extraterrestrial bodies, and the positioning accuracy is not high.
[0004] Therefore, it is urgent to design a patrol space positioning method with high positioning accuracy. Summary of the invention
[0005] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and provide a patroller spatial positioning method, device and medium based on RGB-D images with high positioning accuracy.
[0006] The purpose of the present invention can be achieved by the following technical solutions:
[0007] According to a first aspect of the present invention, a method for spatial positioning of a small patrol vehicle based on RGB-D images is provided, the method comprising:
[0008] Get sequence RGB images and depth map information from RGB-D data;
[0009] Extract feature points from RGB images, evenly distribute the feature points, and calculate the three-dimensional coordinates of the feature points based on the camera model and depth map information;
[0010] Feature points are extracted from adjacent frame images, and tracking and matching results of feature points extracted from adjacent frames are calculated based on descriptors and grayscale information. False matches are eliminated from the tracking and matching results, and the distribution of tracking point pairs is processed uniformly.
[0011] The relative pose transformation results are calculated based on the three-dimensional coordinates of the feature points and the tracking and matching results of adjacent frame images, and the pose estimation results of each image are optimized by the incremental bundle adjustment method based on quadratic optimization.
[0012] Preferably, the extracting feature points from the RGB image specifically comprises: extracting feature points from the RGB image using an ORB feature extraction algorithm.
[0013] Preferably, the uniform distribution processing of the feature points is specifically: uniform distribution processing of the feature points is performed by using a quadtree method.
[0014] Preferably, the camera model is a pinhole camera model.
[0015] Preferably, the tracking and matching results of feature points extracted from adjacent frames are calculated based on descriptors and grayscale information, specifically: the tracking and matching results of feature points extracted from adjacent frames are calculated using a nearest neighbor matching method based on descriptors and an optical flow method based on grayscale.
[0016] Preferably, the performing of mismatch elimination and uniform distribution processing of tracking point pairs on the tracking results specifically comprises: performing random sampling consistency processing on the tracking results to eliminate mismatches, and performing uniform distribution processing based on the matching error score.
[0017] Preferably, the relative pose transformation result is calculated based on the three-dimensional coordinates of the feature points and the tracking and matching results of the adjacent frame images, specifically: the relative pose transformation result is calculated using the EPnP algorithm based on the three-dimensional coordinates of the feature points and the tracking results of the feature points of the adjacent images.
[0018] Preferably, the pose estimation results of each image are optimized by using an incremental bundle adjustment method based on quadratic optimization, specifically:
[0019] 1) Perform bundle adjustment on the pose calculation results of the current frame;
[0020] 2) Perform bundle adjustment on the corresponding pose results outside the first frame image in the fixed-size optimization window and the corresponding feature 3D point observation coordinates to optimize the pose estimation results.
[0021] According to a second aspect of the present invention, there is provided an electronic device, comprising a memory and a processor, wherein a computer program is stored in the memory, and when the processor executes the program, any one of the methods described above is implemented.
[0022] According to a third aspect of the present invention, there is provided a computer-readable storage medium having a computer program stored thereon, wherein when the program is executed by a processor, any one of the methods described above is implemented.
[0023] Compared with the prior art, the present invention has the following beneficial effects:
[0024] 1) In the deep space exploration GNSS denial scenario, the present invention estimates the position and trajectory of the rover through the RGB-D camera sensor data carried by the rover itself. Compared with the binocular visual odometer, the method of the present invention can effectively reduce the solution pressure and calculation error of three-dimensional data. At the same time, considering the situation that multiple frames of image data have common observations, the position and posture calculation results are optimized through the incremental bundle adjustment method, which improves the positioning accuracy and robustness, achieves good positioning in the weak texture environment of extraterrestrial bodies, and better meets the needs of deep space exploration missions.
[0025] 2) The incremental bundle adjustment with a fixed optimization window size is used to optimize the pose results, making full use of the common observations between multiple frames. The first optimization only optimizes the exterior orientation elements corresponding to the current image, and the second optimization optimizes the exterior orientation elements and three-dimensional coordinates of the observation points outside the first frame image in the incremental window, thereby improving the positioning robustness and accuracy. BRIEF DESCRIPTION OF THE DRAWINGS
[0026] Figure 1 It is a technical flow chart of the present invention;
[0027] Figure 2 The RGB image and depth map image of the patrol device used in the present invention; wherein (a) and (b) correspond to the RGB image and the depth map image respectively;
[0028] Figure 3 It is a schematic diagram of the feature extraction result of the RGB image of the present invention;
[0029] Figure 4 It is a schematic diagram of the tracking result of feature points of adjacent frame images of the present invention;
[0030] Figure 5 It is a flow chart of the incremental bundle adjustment method based on secondary optimization in the present invention;
[0031] Figure 6 This is a schematic diagram of the pose calculation results of the RGB-D data used in the present invention. DETAILED DESCRIPTION
[0032] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.
[0033] Example
[0034] like Figure 1 As shown, this embodiment provides a small patrol space positioning method based on RGB-D images, the method comprising:
[0035] S1, obtain sequence RGB image and depth map information from RGB-D data;
[0036] S2, extract feature points from the RGB image, perform uniform distribution processing on the feature points, and calculate the three-dimensional coordinates of the feature points based on the camera model and the depth map information, specifically:
[0037] In this embodiment, an ORB (oriented fast and rotated BRIEF) feature extraction algorithm is used to extract feature points from an RGB image, which mainly includes FAST corner point extraction and BRIEF descriptor extraction with increased rotation invariance. Then, the extracted feature points are subjected to uniform distribution processing, and the feature points in each node are retained and eliminated by a quadtree method, so as to ensure a sufficient number of feature points when the feature points are distributed as evenly as possible. The three-dimensional coordinates of the feature points are calculated by a pinhole camera model based on the ORB feature points in the RGB image combined with the depth map information, as shown in the following formula:
[0038]
[0039] Among them, u and v are the two-dimensional coordinates of the image feature points, Z is the corresponding depth map information, and K is the camera sensor intrinsic parameter matrix.
[0040] S3, extracting feature points from adjacent frame images, calculating tracking and matching results of feature points extracted from adjacent frames based on descriptors and grayscale information, eliminating mismatches from the tracking and matching results, and performing uniform distribution processing on tracking point pairs;
[0041] For adjacent RGB image frames, the ORB feature points in each image are first extracted respectively, and then the ORB feature points of adjacent image frames are tracked and paired by nearest neighbor matching based on the BREIF descriptor. In addition, the grayscale-based optical flow method is used to track the ORB feature points in the image, and the matching error scores are recorded. Finally, the initial tracking results of all feature points in adjacent image frames are obtained, and then the random sampling consistency (RANSAC) is used to remove the possible erroneous matching results for the initial tracking results; the RGB image is divided into regions, and the optimal feature tracking point pairs in each region of the image are retained based on the tracking error scores. According to the empirical value, the image can be divided into small 4*4 regions, and 30-40 feature tracking point pairs are retained for each region. This method can effectively reduce the error of subsequent pose calculation and improve the positioning accuracy and robustness of the algorithm.
[0042] S4. Calculate the relative pose transformation results according to the three-dimensional coordinates of the feature points and the tracking and matching results of the adjacent frame images, and optimize the pose estimation results of each image by an incremental bundle adjustment method based on quadratic optimization.
[0043] Combine the three-dimensional coordinates of the feature points and the tracking results to calculate the pose transformation results of the adjacent frames, and use the EPnP algorithm in the 2D-3D method to solve it. First, select at least 4 pairs of matching points for initial EPnP motion estimation, and use the initial estimation results to calculate the reprojection errors of all matching points. If the error is less than the set threshold, it is considered to be an inlier point. If the number of inliers is greater than 90%, it is considered that the correct relative pose is obtained, and then use all inliers to perform EPnP motion estimation again;
[0044] In the case of multiple frames of image data in visual positioning, large-scale bundle adjustment can be performed on all pose results, but the amount of calculation and time consumption will be very huge. Therefore, for the situation that multiple frames of RGB images have common observation in visual positioning, the incremental bundle adjustment with a fixed optimization window size will be used to optimize the pose results. Specifically, the incremental bundle adjustment method based on secondary optimization in this embodiment optimizes the pose results of the image. This method makes full use of the common observation between multiple frames. The first optimization only optimizes the exterior orientation elements corresponding to the current image. The second optimization is to optimize the exterior orientation elements corresponding to the first frame of the image in the incremental window and the three-dimensional coordinates of the observation point, thereby improving the robustness and accuracy of the positioning algorithm. The specific steps are as follows. First, it is assumed that the fixed optimization window size is n (n>2), and the current moment is k. When new image data is added to the system, proceed in steps:
[0045] 1) First, determine whether there are n frames of images in the window. If the number of images in the optimization window is less than the threshold n, the image is added. If it is equal to n, the image at the moment farthest from the current image frame in the optimization window is first eliminated, and then the current image is added to the window.
[0046] 2) All image poses and 3D positions of object points in the fixed window except the image pose at time k are optimized by bundle adjustment using the images and object points that have a common view relationship with time k.
[0047] 3) After optimization, the image at time k is used to extract feature points and combined with the depth map to calculate and generate new object points, which are added to the map. At this time, if the new object point is close enough to the original point in the map, it will be merged with the original point (that is, no new point is added, only one observation of the existing point is added).
[0048] 4) Then only the pose of the first frame in the window and the position of the object point generated by the first frame are fixed, and the bundle adjustment optimization is performed on all other image poses and object points in the window to obtain the final optimization result.
[0049] Among them, 2) is to obtain the optimized pose value in advance, so as to reduce the number of iterations in step 3. When performing 1)-4), this method uses some strategies to reduce the number of convergence times of optimization:
[0050] Before each bundle adjustment (steps 2 and 3), the back-projection error of all object points in the window is calculated. Points with a value greater than the set threshold are considered to be mismatched and are removed from the window.
[0051] In the process of 2) and 3), an iteration threshold a is set (10 times is taken in the experiment of this method). When the number of iterations is equal to a, the iteration has not converged, and the result of this iteration is not accepted. This is because when the iteration converges after a large number of times, it often converges to an erroneous result.
[0052] like Figure 2 The figure shows the RGB-D data used. The RGB image mainly contains the texture information of the surrounding environment, which can clearly reflect the distribution of surrounding stones, terrain changes, material conditions and some terrain conditions in the distance; while the depth map can reflect the depth information within a certain range and reflect the surrounding terrain conditions.
[0053] like Figure 3 As shown in the figure, feature points are extracted from RGB images. An RGB image with a size of 1024*1024 pixels is input and ORB feature points in the image are extracted, mainly various corner points, such as the edges or protrusions of stones, places where the ground material changes greatly, the tops of distant mountains, etc., which can make full use of the environmental texture information contained in the image.
[0054] like Figure 4 As shown, the ORB feature points extracted from adjacent frame images need to construct corresponding matching relationships, so as to establish the relative posture relationship between adjacent frame images in the actual three-dimensional space. In this embodiment, the nearest neighbor matching method based on feature descriptors and the optical flow method based on grayscale information are used to calculate the tracking results of adjacent frame feature points, and the tracking results are optimized.
[0055] like Figure 5The figure shows a flowchart of the incremental bundle adjustment method based on secondary optimization, where the circle represents the image pose at each moment, the ellipse represents the observation value of the object point, the black solid line represents the fixed value, and the red dotted line is the element adjusted in the bundle adjustment. This method makes full use of the common observation between multiple frames. The first optimization only optimizes the exterior orientation elements corresponding to the current image, and the second optimization optimizes the exterior orientation elements corresponding to the first frame image and the three-dimensional coordinates of the observation point in the incremental window.
[0056] The EPnP algorithm in the 2D-3D method is used to solve the three-dimensional coordinates of the feature points and the tracking results to obtain the pose calculation results of adjacent frame images. At the same time, considering the situation that multiple frames of RGB images have common observations in visual positioning, an incremental bundle adjustment method based on quadratic optimization is used to optimize the pose results of the images, such as Figure 6 As shown, it is a schematic diagram of the trajectory depicted by the final all-pose calculation results of the RGB-D data of this embodiment.
[0057] The electronic device of the present invention includes a central processing unit (CPU), which can perform various appropriate actions and processes according to computer program instructions stored in a read-only memory (ROM) or loaded from a storage unit into a random access memory (RAM). In the RAM, various programs and data required for device operation can also be stored. The CPU, ROM and RAM are connected to each other via a bus. An input / output (I / O) interface is also connected to the bus.
[0058] Multiple components in the device are connected to the I / O interface, including: input units, such as keyboards, mice, etc.; output units, such as various types of displays, speakers, etc.; storage units, such as disks, optical disks, etc.; and communication units, such as network cards, modems, wireless communication transceivers, etc. The communication unit allows the device to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunication networks.
[0059] The processing unit performs the various methods and processes described above, such as methods S1 to S4. For example, in some embodiments, methods S1 to S4 may be implemented as a computer software program, which is tangibly contained in a machine-readable medium, such as a storage unit. In some embodiments, part or all of the computer program may be loaded and / or installed on the device via a ROM and / or a communication unit. When the computer program is loaded into the RAM and executed by the CPU, one or more steps of methods S1 to S4 described above may be performed. Alternatively, in other embodiments, the CPU may be configured to execute methods S1 to S4 in any other appropriate manner (e.g., by means of firmware).
[0060] The functions described above herein may be performed at least in part by one or more hardware logic components. For example, without limitation, exemplary types of hardware logic components that may be used include: field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chip systems (SOCs), load programmable logic devices (CPLDs), and the like.
[0061] The program code for implementing the method of the present invention can be written in any combination of one or more programming languages. These program codes can be provided to a processor or controller of a general-purpose computer, a special-purpose computer or other programmable data processing device, so that the program code, when executed by the processor or controller, enables the functions / operations specified in the flow chart and / or block diagram to be implemented. The program code can be executed entirely on the machine, partially on the machine, partially on the machine as a stand-alone software package and partially on a remote machine, or entirely on a remote machine or server.
[0062] In the context of the present invention, a machine-readable medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, device, or equipment. A machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or equipment, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
[0063] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of various equivalent modifications or replacements within the technical scope disclosed by the present invention, and these modifications or replacements should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.
Claims
1. A patrol space positioning method based on RGB-D images, characterized in that: The method includes: Get sequence RGB images and depth map information from RGB-D data; Extract feature points from RGB images, evenly distribute the feature points, and calculate the three-dimensional coordinates of the feature points based on the camera model and depth map information; Feature points are extracted from adjacent frame images, and tracking and matching results of feature points extracted from adjacent frames are calculated based on descriptors and grayscale information. False matches are eliminated from the tracking and matching results, and the distribution of tracking point pairs is processed uniformly. The relative pose transformation results are calculated based on the three-dimensional coordinates of the feature points and the tracking and matching results of adjacent frame images, and the pose estimation results of each image are optimized by the incremental bundle adjustment method based on quadratic optimization.
2. According to the RGB-D image-based patrol vehicle spatial positioning method of claim 1, it is characterized in that: The extracting feature points from the RGB image specifically includes: extracting feature points from the RGB image using an ORB feature extraction algorithm.
3. The method for spatial positioning of a patrol vehicle based on RGB-D images according to claim 1, characterized in that: The processing of evenly distributing the feature points specifically includes: using a quadtree method to evenly distribute the feature points.
4. The method for spatial positioning of a patrol vehicle based on RGB-D images according to claim 1, characterized in that: The camera model is a pinhole camera model.
5. The method for spatial positioning of a patrol vehicle based on RGB-D images according to claim 1, characterized in that: The tracking and matching results of feature points extracted from adjacent frames are calculated based on descriptors and grayscale information, specifically: the tracking and matching results of feature points extracted from adjacent frames are calculated using a nearest neighbor matching method based on descriptors and an optical flow method based on grayscale.
6. The method for spatial positioning of a patrol vehicle based on RGB-D images according to claim 5, characterized in that: The performing of mismatch elimination on the tracking results and uniform distribution processing of the tracking point pairs specifically includes: performing random sampling consistency processing on the tracking results to eliminate mismatches, and performing uniform distribution processing based on the matching error score.
7. The method for spatial positioning of a patrol vehicle based on RGB-D images according to claim 1, characterized in that: The relative pose transformation result is calculated based on the three-dimensional coordinates of the feature points and the tracking and matching results of the adjacent frame images, specifically: the relative pose transformation result is calculated using the EPnP algorithm based on the three-dimensional coordinates of the feature points and the tracking results of the feature points of the adjacent images.
8. The method for spatial positioning of a patrol vehicle based on RGB-D images according to claim 1, characterized in that: The pose estimation results of each image are optimized by the incremental bundle adjustment method based on quadratic optimization, specifically: 1) Perform bundle adjustment on the pose calculation results of the current frame; 2) Perform bundle adjustment on the corresponding pose results outside the first frame image in the fixed-size optimization window and the corresponding feature 3D point observation coordinates to optimize the pose estimation results.
9. An electronic device comprising a memory and a processor, wherein a computer program is stored in the memory, wherein: When the processor executes the program, the method according to any one of claims 1 to 8 is implemented.
10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the method according to any one of claims 1 to 8 is implemented.
Citation Information
Patent Citations
Robot scene self-adaptive pose estimation method based on RGB-D camera
CN110223348A
Visual SLAM method and device based on key frame optimization
CN115131420A
Visual synchronous localization and mapping method and system
CN116152337A
Positioning device, estimation method, and non-transitory computer-readable medium
US20230245341A1
Cited By
Three-dimensional scene reconstruction and real-time positioning method and system based on 3D Gaussian spattering
CN119672225A
