Vision recognition positioning method, device and equipment of robot

Abstract

The application provides a visual identification positioning method, device and equipment of a robot, the method comprising: collecting multiple left and right target images through a stereo camera, double feature detection is performed on the images, point features and line features are extracted simultaneously, and complementary feature information is formed; then, the point and line features are subjected to matching optimization processing, and a high-quality feature matching result is obtained; key frame selection is performed based on the point and line fusion feature matching result, and redundant calculation is reduced; the point and line features in the key frame are subjected to three-dimensional reconstruction, and inertial measurement unit data is fused and optimized, and accurate robot position and attitude information is obtained. Through the point and line feature fusion mode, the application effectively solves the limitation of simply relying on point features in a texture missing environment, significantly improves the accuracy and robustness of robot visual positioning, and is suitable for robot autonomous navigation in a texture missing and rugged terrain environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of visual recognition, in particular to a robot visual recognition positioning method, device and equipment. BACKGROUND

[0002] With the rapid development of robot technology, visual inertial odometry (VIO) has become a key technology for robot autonomous navigation. Existing robot visual positioning methods are mainly based on point features for environment perception and position estimation, which realizes real-time positioning of robots by extracting corner features in images and combining inertial measurement unit data. These methods can achieve good positioning results in ideal environments with rich textures and sufficient feature points.

[0003] However, in environments with missing textures and rugged terrain, the existing point feature-based visual inertial odometry has obvious shortcomings. When there are large smooth surfaces and little texture information in the environment, the number of effective feature points is insufficient, resulting in a decrease in feature matching quality. At the same time, rugged terrain can cause significant shaking during camera movement, further increasing the difficulty of feature point matching and tracking, ultimately leading to reduced positioning accuracy or even positioning failure, which seriously restricts the practicality and reliability of robots in complex environments. SUMMARY

[0004] The main purpose of the present application is to solve the technical problem of the existing point feature-based visual inertial odometry in the environment with missing textures and rugged terrain, which leads to a decrease in positioning accuracy due to insufficient effective feature points and low feature matching quality.

[0005] The first aspect of the present application provides a robot visual recognition positioning method, which comprises:

[0006] Performing double feature detection processing on multiple frames of left and right target images continuously collected by a stereo camera installed on the robot to obtain a point feature set and a line feature set;

[0007] Performing feature matching optimization processing on the point feature set and the line feature set according to a preset filtering condition to obtain a point-line fusion feature matching result;

[0008] Performing key frame selection processing on the multiple frames of left and right target images according to the point-line fusion feature matching result to obtain an optimized key frame sequence;

[0009] Performing three-dimensional reconstruction processing on the point-line features in the optimized key frame sequence and fusion optimization processing with inertial measurement unit data to obtain a robot visual recognition positioning result.

[0010] Optionally, in the first implementation manner of the first aspect, the method further includes:

[0011] calculating pixel gradients of the multiple frames of left and right target images continuously captured by the stereo camera mounted on the robot, constructing a gradient matrix according to the pixel gradients, and calculating eigenvalues of the gradient matrix;

[0012] taking a minimum value in the eigenvalues as a corner response intensity, and screening pixel points with the corner response intensity greater than a preset intensity threshold to obtain the point feature set;

[0013] calculating gradient angles according to the pixel gradients, and obtaining an original line feature set through region growing and line fitting processing on pixels with the gradient angles greater than a preset similarity threshold;

[0014] determining a length threshold according to a product of the minimum value of the image resolution and a preset scaling factor, and filtering line segments in the original line feature set with lengths less than the length threshold to obtain the line feature set.

[0015] Optionally, in the second implementation manner of the first aspect, the method further includes:

[0016] calculating displacement vectors of the point feature set between consecutive frames through forward-backward optical flow consistency checking to obtain a point feature matching pair;

[0017] statistically encoding pixel gradient directions around the line feature set to generate a binary descriptor vector to obtain a line feature descriptor set;

[0018] calculating Hamming distances between binary descriptor vectors in the line feature descriptor set as similarities, performing elimination processing on binary descriptor pairs with the similarities lower than a preset matching threshold, and performing secondary screening in combination with a line segment endpoint position change constraint to obtain a line feature matching pair;

[0019] performing data fusion processing on the point feature matching pair and the line feature matching pair to obtain a point-line fusion feature matching result.

[0020] Optionally, in the third implementation manner of the first aspect, the method further includes:

[0021] Traverse the multiple frames of left and right target images, determine a current frame image and a last key frame image in the multiple frames of left and right target images, and count a point feature matching number and a line feature matching number between the current frame image and the last key frame image in the point-line fusion feature matching result;

[0022] Take the point feature matching number and the line feature matching number between the current frame image and the last key frame image as a total matching feature number, take the point feature matching number and the line feature matching number of the current frame image as a total current frame detection feature number, and calculate a ratio of the total matching feature number to the total current frame detection feature number to obtain a feature ratio;

[0023] According to the point feature position change and the line feature position change between the current frame image and the last key frame image, respectively calculate a point feature average disparity and a line feature average disparity, and perform weighted summation according to a preset weight coefficient to obtain a comprehensive average disparity;

[0024] Compare the feature ratio with a preset ratio threshold value for judgment, compare the comprehensive average disparity with a preset disparity threshold value for judgment, perform key frame marking processing on the current frame image with a feature ratio lower than the ratio threshold value or a comprehensive average disparity higher than the disparity threshold value, and obtain an optimized key frame sequence.

[0025] Optionally, in the fourth implementation manner of the first aspect of the present application, the step of calculating a point feature average disparity and a line feature average disparity according to the point feature position change and the line feature position change between the current frame image and the last key frame image, and performing weighted summation according to a preset weight coefficient to obtain a comprehensive average disparity comprises:

[0026] According to the point feature matching pair between the current frame image and the last key frame image, calculate a pixel displacement distance of each point feature on an image plane, perform arithmetic average calculation on the pixel displacement distances of all point features, and obtain a point feature average disparity;

[0027] According to the line feature matching pair between the current frame image and the last key frame image, respectively calculate a change amount of a line segment midpoint coordinate and a line segment direction angle, linearly combine the midpoint coordinate change amount and the angle change amount according to a preset proportion coefficient, and obtain a comprehensive change amount of each line feature;

[0028] Perform arithmetic average calculation on the comprehensive change amounts of the line features, and obtain a line feature average disparity;

[0029] Multiply the point feature average disparity by a preset point feature weight coefficient, multiply the line feature average disparity by a preset line feature weight coefficient, and add the two product results to obtain the comprehensive average disparity.

[0030] Optionally, in a fifth implementation form of the first aspect of the present application, the point line feature in the optimized key frame sequence is subjected to a three-dimensional reconstruction process and fusion optimization process with inertial measurement unit data to obtain a robot visual recognition positioning result, which includes:

[0031] The point feature in the optimized key frame sequence is subjected to singular value decomposition for triangulation calculation to solve three-dimensional coordinates of the point feature in a world coordinate system to obtain a point feature spatial position;

[0032] The line feature in the optimized key frame sequence is converted into a Plucker coordinate representation, and a spatial straight line equation of the line feature in the world coordinate system is determined by a normal vector and a direction vector of the Plucker coordinate to obtain a line feature spatial geometry;

[0033] A visual residual term is constructed according to the point feature spatial position and the line feature spatial geometry by calculating a re-projection error, an inertial residual term is constructed by calculating a pre-integration error according to inertial measurement unit data, and the visual residual term and the inertial residual term are combined to obtain an overall cost function;

[0034] The overall cost function is subjected to nonlinear minimization solving within a sliding window to iteratively optimize position and attitude parameters of the robot to obtain the robot visual recognition positioning result.

[0035] Optionally, in a sixth implementation form of the first aspect of the present application, the visual residual term is constructed according to the point feature spatial position and the line feature spatial geometry by calculating a re-projection error, the inertial residual term is constructed by calculating a pre-integration error according to inertial measurement unit data, and the visual residual term and the inertial residual term are combined to obtain an overall cost function, which includes:

[0036] The point feature spatial position is projected onto an image plane according to current robot pose parameters, and a Euclidean distance between the projected position and an actual observed position is calculated as a point feature re-projection error;

[0037] The line feature spatial geometry is projected onto the image plane according to the current robot pose parameters, and a point-to-line distance between a projected straight line and an actual observed line segment is calculated as a line feature re-projection error;

[0038] The point feature re-projection error and the line feature re-projection error are respectively subjected to square processing and weighted summation calculation according to a preset visual weight matrix to obtain the visual residual term;

[0039] An IMU predicted pose change amount is obtained by numerical integration calculation according to acceleration measurement values and angular velocity measurement values of the inertial measurement unit between adjacent key frames, and an inertial pre-integration error is obtained by difference calculation of the IMU predicted pose change amount and a visual estimated pose change amount;

[0040] The inertial pre-integration error is subjected to covariance matrix normalization processing, and is subjected to weighted calculation according to a preset inertial weight coefficient, to obtain the inertial residual term;

[0041] The visual residual term and the inertial residual term are subjected to linear combination calculation according to a preset fusion weight coefficient, and a regularization constraint term of robot prior pose information is added, to obtain a total cost function.

[0042] The second aspect of the present application provides a robot visual recognition positioning device, which comprises:

[0043] A feature detection module is configured to perform double feature detection processing on a plurality of left and right target images continuously collected by a stereo camera installed on a robot, to obtain a point feature set and a line feature set;

[0044] A feature matching module is configured to perform feature matching optimization processing on the point feature set and the line feature set according to a preset screening condition, to obtain a point-line fusion feature matching result;

[0045] A key frame selection module is configured to perform key frame selection processing on the plurality of left and right target images according to the point-line fusion feature matching result, to obtain an optimized key frame sequence;

[0046] A fusion positioning module is configured to perform three-dimensional reconstruction processing on point-line features in the optimized key frame sequence, and perform fusion optimization processing with inertial measurement unit data, to obtain a robot visual recognition positioning result.

[0047] The third aspect of the present application provides a robot visual recognition positioning device, which comprises a memory and at least one processor, the memory stores instructions, and the memory and the at least one processor are interconnected by a circuit; the at least one processor calls the instructions in the memory, so that the robot visual recognition positioning device executes the steps of the robot visual recognition positioning method described above.

[0048] The robot visual recognition positioning method, device and equipment described above collect a plurality of left and right target images by a stereo camera, perform double feature detection on the images, extract point features and line features at the same time, form complementary feature information, then perform matching optimization processing on the point-line features, obtain a high-quality feature matching result, perform key frame selection based on the point-line fusion feature matching result, reduce redundant calculation, perform three-dimensional reconstruction on the point-line features in the key frame, and perform fusion optimization with inertial measurement unit data, to obtain accurate robot position and attitude information. The present application effectively solves the limitation of relying solely on point features in a texture missing environment by means of point-line feature fusion, significantly improves the accuracy and robustness of robot visual positioning, and is suitable for robot autonomous navigation in a texture missing and rugged terrain environment.

[0049] Other features and advantages of the present application will be set forth in the descriptions that follow, and in part will be apparent from the description, or can be learned by practice of the application. The purposes and other advantages of the application will be realized and attained by the structure particularly pointed out in the written description and claims hereof as well as the appended drawings.

[0050] In order to make the above objectives, characteristics and advantages of the present application more apparent, the following preferred embodiments are specifically described with reference to the accompanying drawings. BRIEF DESCRIPTION OF DRAWINGS

[0051] Figure 1 A first embodiment schematic diagram of a robot vision recognition positioning method in the embodiments of the present application;

[0052] Figure 2 A first embodiment schematic diagram of a robot vision recognition positioning device in the embodiments of the present application;

[0053] Figure 3 A first embodiment schematic diagram of a robot vision recognition positioning device in the embodiments of the present application; DETAILED DESCRIPTION

[0054] In order to make the above objectives, characteristics and advantages of the present application more apparent, the following preferred embodiments are specifically described with reference to the accompanying drawings.

[0055] The terms "comprising" and "having" and any variations thereof in the embodiments of the present application are intended to cover not exclusively containing. For example, a process, method, system, product or device comprising a series of steps or units is not limited to the listed steps or units, but optionally further comprises other steps or units not listed, or optionally further comprises other steps or units inherent to the process, method, product or device.

[0056] In order to facilitate the understanding of the present embodiment, first, a robot vision recognition positioning method disclosed in the embodiments of the present application is introduced in detail. As shown in the figure, the method comprises the following steps: Figure 1

[0057] 101, a plurality of frames of left and right target images continuously collected by a stereo camera installed on the robot are subjected to double feature detection processing to obtain a point feature set and a line feature set;

[0058] ​In one embodiment of the present application, the double feature detection processing is performed on the multiple frames of left and right target images continuously captured by the stereo camera mounted on the robot, to obtain a point feature set and a line feature set, including: calculating the pixel gradient of the multiple frames of left and right target images continuously captured by the stereo camera mounted on the robot, constructing a gradient matrix according to the pixel gradient and calculating the eigenvalue of the gradient matrix; taking the minimum value in the eigenvalue as the corner point response intensity, and screening the pixel points with the corner point response intensity greater than a preset intensity threshold, to obtain the point feature set; calculating the gradient angle according to the pixel gradient, and performing region growing and straight line fitting processing on the pixels with the gradient angle greater than a preset similarity threshold, to obtain an original line feature set; determining a length threshold according to the product of the minimum value of the image resolution and a preset scaling factor, and filtering the line segments in the original line feature set with the length less than the length threshold, to obtain the line feature set.

[0059] Specifically, the double feature detection processing needs to calculate the pixel gradient of the multiple frames of left and right target images continuously captured by the stereo camera. The pixel gradient represents the intensity change rate of each pixel point in the image in the horizontal and vertical directions, and is obtained by convolution operation of the image with a Sobel operator or other gradient operator. Specifically, for each pixel point in the image, the partial derivatives of the pixel point in the x direction and the y direction are calculated respectively, to form gradient vectors Gx and Gy. These gradient information reflects the distribution of edges and textures in the image, and constitutes the basic data for subsequent feature detection. Based on the calculated pixel gradient, a gradient matrix of each pixel point is constructed by using the Shi-Tomasi corner detection algorithm. The Shi-Tomasi algorithm is an improved version of the Harris corner detection algorithm, which identifies corner features by analyzing the second moment matrix of the image. The gradient matrix is a 2x2 symmetric matrix, whose elements are composed of the product of gradient components. The matrix construction process involves the statistics of gradient information in the neighborhood, which accumulates gradient product items in a small window around the pixel point, to form a mathematical representation describing the local image structure. The Shi-Tomasi algorithm then performs eigenvalue decomposition on the gradient matrix, to obtain two eigenvalues λ1 and λ2, which describe the main direction and intensity change characteristics of the image structure at the pixel point.

[0060] Specifically, the minimum value of the two eigenvalues is taken as the corner response strength. This selection is based on the theoretical basis of the Shi-Tomasi algorithm, that is, both eigenvalues at the corner point should be relatively large, and the size of the minimum eigenvalue directly reflects the reliability of the point as a corner. Compared with the response function of the Harris operator using the product of the eigenvalues minus the sum of the squares, the Shi-Tomasi algorithm directly uses the minimum eigenvalue as the corner strength measure, avoiding the need to adjust the parameter k in the Harris operator, making the corner detection more stable and reliable. By setting a preset intensity threshold, pixel points with a corner response strength exceeding the threshold are selected, and these pixel points constitute the point feature set. The setting of the threshold needs to balance the relationship between the number and quality of the feature points. A too low threshold will introduce a large number of noise points, while a too high threshold may miss important corner information. The screening process also includes a non-maximum suppression step to ensure that the detected corner points have a reasonable spatial distribution and avoid excessive redundant feature points in the same area. Non-maximum suppression compares the response strength of each candidate corner point with other candidate points in its neighborhood, and only keeps the local maximum point, thereby avoiding the phenomenon of corner clustering.

[0061] Specifically, the LSD (Line Segment Detector) algorithm extracts line features from the angle information of pixel gradients. The LSD algorithm is a line segment detection algorithm that can detect sub-pixel accuracy straight line segments in an image. The core idea of this algorithm is to identify straight line structures by analyzing the direction consistency of image gradients. First, the LSD algorithm calculates the gradient angle of each pixel in the image. The gradient angle is obtained by calculating arctan(Gy / Gx), which represents the direction of the gradient vector at each pixel point. This angle information reveals the edge direction and the distribution of linear structures in the image. The LSD algorithm groups pixels based on the similarity of their gradient angles. When the gradient angle difference between adjacent pixels is less than a preset similarity threshold, these pixels are considered to belong to the same potential straight line segment. The algorithm uses a region growing strategy, starting from a seed pixel and gradually expanding to neighboring pixels with similar gradient angles to form connected pixel regions. Each region represents a possible linear structure in the image, and the pixels within the region have consistent edge direction characteristics. The advantage of the LSD algorithm is that it can automatically determine algorithm parameters without manual adjustment, while controlling the probability of false detection to ensure the reliability of the detection results. After region growing is completed, the LSD algorithm performs straight line fitting on each pixel region, using the least squares method to determine the parameters of the best fitting straight line. The straight line fitting process needs to handle the interference of noise pixels, and the LSD algorithm uses a statistical verification mechanism to ensure that the fitting result accurately reflects the true linear structure. The algorithm calculates the statistical significance of each detected line segment and only keeps the line segments that meet the significance requirements. The fitted straight line segments constitute the original line feature set, and each line segment is described by the start point, end point coordinates and direction parameters.

[0062] Specifically, the length threshold is determined according to the product of the minimum value of the image resolution and a preset scaling factor, and the setting of the threshold considers the influence of the image size on the line segment detection. An image with a larger resolution can detect more detailed line segments, and an image with a smaller resolution needs to adjust the threshold accordingly to avoid missing important linear features. The selection of the scaling factor value is based on experience and experimental results, ensuring that the filtered line segments are indeed noise or unimportant short line segments. The filtering process removes the line segments with a length less than the preset threshold by calculating the Euclidean distance length of each line segment in the original line feature set, and retains the line segments with sufficient length as effective line features. This filtering strategy is based on the consideration of geometric constraints. Short line segments often lack sufficient geometric information to support reliable feature matching and three-dimensional reconstruction, and are also easily affected by image noise. The line feature set obtained after length filtering contains the main linear structure information in the image. These line features provide important geometric constraints in the subsequent matching and positioning process. The entire double feature detection process combines the advantages of the Shi-Tomasi corner detection algorithm and the LSD line segment detection algorithm to provide more abundant and reliable visual information for robots in textureless environments. Point features provide accurate position constraints, while line features supplement geometric information in structured environments. The combination of the two significantly enhances the robustness and completeness of feature detection.

[0063] 102. performing feature matching optimization processing on the point feature set and the line feature set according to a preset screening condition to obtain a point-line fusion feature matching result;

[0064] In an embodiment of the present application, the feature matching optimization processing on the point feature set and the line feature set according to a preset screening condition to obtain a point-line fusion feature matching result includes: calculating the displacement vectors of the point feature set between consecutive frames through forward-backward optical flow consistency test to obtain point feature matching pairs; statistically encoding the pixel gradient directions around the line feature set to generate binary descriptor vectors to obtain a line feature descriptor set; calculating the Hamming distance between the binary descriptor vectors in the line feature descriptor set as a similarity, and performing rejection processing on the binary descriptor pairs with a similarity lower than a preset matching threshold, and performing secondary screening combined with a line segment endpoint position change constraint condition to obtain line feature matching pairs; and performing data fusion processing on the point feature matching pairs and the line feature matching pairs to obtain a point-line fusion feature matching result.

[0065] Specifically, the feature matching optimization process starts from the calculation of inter-frame displacement vectors of the point feature set, and adopts the KLT optical flow tracking algorithm to realize forward-backward optical flow consistency verification. The KLT (Kanade-Lucas-Tomasi) optical flow algorithm is a gradient-based feature tracking method, which assumes that the pixel intensity in the image remains unchanged in a short time, and the adjacent pixels have similar motion. Forward optical flow tracking starts from the feature points of the current frame, calculates the position of the feature points in the next frame by minimizing the brightness difference between image blocks, which involves the calculation of image gradient and iterative optimization solution. The algorithm selects a small window around the feature point, calculates the gradient information of the pixels in the window, and then solves the optimal displacement vector by Newton iteration method. Backward optical flow tracking starts from the predicted position of the next frame and tracks back to the original frame to calculate the reverse displacement vector. Forward-backward consistency verification evaluates the tracking quality by comparing the consistency of the displacement vectors obtained by forward tracking and backward tracking. When the tracking results of the two directions are within the same position with an error less than a preset threshold, it is considered that the tracking of the feature point is reliable. This bidirectional verification mechanism can effectively eliminate false matches caused by occlusion, illumination changes or image blur, ensuring the accuracy and stability of the point feature matching pair. The point feature matching pair obtained in this way contains the corresponding relationship of the reliably tracked feature points between consecutive frames, and each matching pair consists of the coordinate positions and displacement vectors of the corresponding feature points in two frames.

[0066] Specifically, the matching process of line features adopts the LBD (Line Band Descriptor) descriptor to describe the local appearance characteristics of line segment features. The LBD descriptor is a binary descriptor specially designed for straight line segments, which generates a compact binary code by analyzing the gradient direction distribution of the pixels around the line segment. In the specific implementation process, for each line segment in the line feature set, first, a band-shaped region perpendicular to the line segment direction is established on both sides of the line segment, and the width of the band-shaped region is adaptively determined according to the length of the line segment and the image resolution. Within the band-shaped region, the algorithm divides the region into multiple sub-regions, and the gradient direction distribution of each sub-region is obtained by histogram statistics. The gradient direction histogram records the distribution of the gradient direction of the pixels in the sub-region, reflecting the texture characteristics and edge structure around the line segment. Then, the algorithm quantizes the gradient direction histogram of each sub-region, maps the continuous gradient direction values to discrete direction intervals, and generates a binary code according to the relative relationship of the dominant gradient direction. This encoding method compresses the complex texture information around the line segment into a compact binary vector, which not only preserves the local feature information of the line segment, but also has strong noise resistance. By executing this descriptor generation process on all line segments in the line feature set, the corresponding line feature descriptor set is obtained, and each descriptor is represented by a fixed-length binary vector.

[0067] Specifically, the line feature matching process evaluates the feature similarity by computing the Hamming distance between binary descriptor vectors in the line feature descriptor set. The Hamming distance is a measure of the difference between two equal-length binary strings, calculated by counting the number of positions at which the corresponding bits are different. For LBD descriptors, this binary encoding process is very efficient, and the Hamming distance can be quickly calculated through bitwise operations. The algorithm iterates through all possible pairs of line feature descriptors, calculates the Hamming distance between them, and the smaller the distance value, the more similar the local appearance characteristics of the two line segments. By setting a pre-defined matching threshold, binary descriptor pairs with a similarity lower than the threshold are removed. The choice of this threshold needs to balance the relationship between matching accuracy and recall rate. A too high threshold will result in a large number of false matches, while a too low threshold will miss correct matches. After the initial descriptor matching is completed, the algorithm performs a secondary screening combined with the line segment endpoint position change constraint. This geometric constraint checks the endpoint position change of the matched line segment pair, including the displacement distance of the endpoint coordinates and the change ratio of the line segment length. When the geometric change of the two matched line segments between consecutive frames exceeds a reasonable range, the matching pair is considered to be a false match and is removed. The introduction of geometric constraints can further filter out false matches that are only similar in appearance but inconsistent in geometric position, significantly improving the accuracy and reliability of line feature matching. After the double filtering of descriptor similarity screening and geometric constraint verification, high-quality line feature matching pairs are obtained, which accurately reflect the corresponding relationship of line segment features between consecutive frames.

[0068] Specifically, the data fusion process integrates the point feature matching pairs and the line feature matching pairs into a unified point-line fusion feature matching result. The fusion process is not a simple data splicing, but needs to consider the complementarity and consistency of the two types of features. The point feature matching pairs provide accurate pixel-level position correspondence, which is expressed as discrete point-to-point mapping in the image, with high positioning accuracy but relatively sparse spatial distribution. The line feature matching pairs provide geometric constraint information of linear structure, which can complement the deficiency of point features in structured environments, especially in smooth areas with missing texture. The fusion algorithm verifies the mutual support degree of point-line feature matching through spatial consistency test. When the motion trend indicated by the point feature matching is consistent with the motion trend of the nearby line feature matching, the matching results of the two types of features enhance each other's credibility. Otherwise, if there is obvious inconsistency, the algorithm will reduce the weight of the corresponding matching pair or mark it as a suspicious match. The fusion process also includes spatial distribution optimization of the matching pairs, ensuring that the final feature matching result has good spatial distribution characteristics in the image, avoiding excessive concentration of features in certain local areas. Through this fusion strategy which comprehensively considers geometric consistency, spatial distribution and feature complementarity, the point-line fusion feature matching result contains rich visual correspondence information, maintaining the accuracy of point features and utilizing the structure constraint ability of line features.

[0069] 103. performing key frame selection processing on the multiple left and right target images according to the point-line fusion feature matching result, to obtain an optimized key frame sequence;

[0070] In an embodiment of the present application, the performing key frame selection processing on the multiple left and right target images according to the point-line fusion feature matching result, to obtain an optimized key frame sequence comprises: traversing the multiple left and right target images, determining a current frame image and a last key frame image in the multiple left and right target images, and counting the number of point feature matching and the number of line feature matching between the current frame image and the last key frame image in the point-line fusion feature matching result; taking the number of point feature matching and the number of line feature matching between the current frame image and the last key frame image as the total number of matching features, taking the number of point feature matching and the number of line feature matching of the current frame image as the total number of current frame detection features, and calculating the ratio of the total number of matching features to the total number of current frame detection features to obtain a feature ratio; calculating the average disparity of point features and the average disparity of line features according to the position change of point features and the position change of line features between the current frame image and the last key frame image, respectively, and performing weighted summation according to a preset weight coefficient to obtain a comprehensive average disparity; comparing the feature ratio with a preset ratio threshold value for judgment, comparing the comprehensive average disparity with a preset disparity threshold value for judgment, and performing key frame marking processing on the current frame image whose feature ratio is lower than the ratio threshold value or whose comprehensive average disparity is higher than the disparity threshold value, to obtain an optimized key frame sequence.

[0071] Specifically, the traversal process proceeds in the time order of the images, starting from the first frame and processing each frame image in turn. In this process, a key frame record needs to be maintained to store the image information that has been marked as a key frame. The current frame image refers to the target image being subjected to key frame judgment, while the last key frame image refers to the image that was last selected as a key frame before the current frame. This comparison strategy based on the last key frame ensures the continuity and consistency of key frame selection, avoiding the problem of information invalidation caused by comparison with a frame that is too far away. After determining the current frame image and the last key frame image, the algorithm begins to count the feature correspondence relationship between the two frames in the point-line fusion feature matching result. This counting process calculates the number of point feature matches and the number of line feature matches, respectively. The number of point feature matches represents the number of corner point features that successfully establish a correspondence relationship between the current frame and the last key frame, while the number of line feature matches represents the number of straight line segment features that successfully match between the two frames. The counting process needs to traverse all the feature matching pairs obtained previously, and determine whether they belong to the matching relationship between the current frame and the last key frame based on the frame information of the feature points or line segments in the matching pairs.

[0072] Specifically, the feature ratio calculation evaluates the feature matching quality of the current frame by analyzing the proportional relationship between the matching features and the detected features. The total number of matching features is obtained by adding the number of point feature matches and the number of line feature matches between the current frame image and the last key frame image. This value reflects the total number of feature correspondence relationships established between the two frames. At the same time, the total number of detected features in the current frame is obtained by adding the number of point features and the number of line features detected in the current frame image. This value represents the total amount of all features available for matching in the current frame. The total number of detected features in the current frame is derived from the output results of the feature detection module, including the corner point features detected by the Shi-Tomasi algorithm and the line segment features detected by the LSD algorithm. The ratio of the total number of matching features to the total number of detected features in the current frame is calculated to obtain the feature ratio. This ratio value reflects the proportion of features in the current frame that can establish reliable matching relationships with the last key frame. The higher the feature ratio, the smaller the angle change between the two frames, and most of the features remain visible and matchable. A lower feature ratio indicates that there is a significant change in angle, occlusion, or the appearance of new regions between the two frames, and the current frame contains a large amount of new visual information.

[0073] Specifically, the parallax calculation process respectively handles the position changes of point features and line features to quantify the degree of spatial changes between the current frame and the last key frame. The calculation of point feature average parallax is based on the pixel displacement distance of each feature point in the point feature matching pair between two frames. The parallax value of a single feature point is obtained by calculating the Euclidean distance of the matching point pair on the image plane, and then the parallax values of all point features are arithmetically averaged to obtain the point feature average parallax. This average value reflects the overall displacement amplitude of point features in the image caused by camera motion, and a larger point feature average parallax indicates that the camera has undergone significant position or attitude changes. The calculation of line feature average parallax is relatively complex and needs to consider the position change and direction change of the line segment. The algorithm respectively calculates the midpoint coordinate change and the line segment direction angle change of the matching line segment pair. The midpoint coordinate change is obtained by calculating the Euclidean distance of the corresponding line segment midpoint positions in two frames, while the direction angle change is obtained by calculating the included angle between the line segment direction vectors. The midpoint coordinate change and the angle change are linearly combined according to a preset proportion coefficient to obtain the comprehensive change of each line feature, and then the comprehensive changes of all line features are arithmetically averaged to obtain the line feature average parallax. This method of comprehensively considering position and direction changes can more comprehensively reflect the changes of line features in space. Finally, the point feature average parallax is multiplied by a preset point feature weight coefficient, the line feature average parallax is multiplied by a preset line feature weight coefficient, and the two product results are added to obtain the comprehensive average parallax. The setting of the weight coefficient reflects the relative importance of point features and line features in parallax evaluation, and this weight distribution considers the accuracy characteristics and environmental adaptability of the two feature types.

[0074] Specifically, the key frame determination process determines whether the current frame should be selected as a key frame through a double threshold comparison. The calculated feature ratio is compared with a preset ratio threshold to determine whether the current frame has the condition to become a key frame when the feature ratio is lower than the ratio threshold, indicating that there is less common feature between the current frame and the last key frame, and there is a large amount of new visual information. The setting of the feature ratio threshold needs to balance the information gain and the calculation efficiency of the key frame. A too low threshold will result in too dense key frames and increase the calculation burden, while a too high threshold will result in the loss of important visual information. At the same time, the comprehensive average disparity is compared with a preset disparity threshold to determine whether the current frame has the condition to become a key frame when the comprehensive average disparity is higher than the disparity threshold, indicating that there is a significant spatial change between the current frame and the last key frame, and the motion amplitude of the camera is large enough to provide new spatial constraint information. The setting of the disparity threshold considers the typical amplitude of the camera motion and the accuracy requirement of the three-dimensional reconstruction. A proper disparity change is beneficial to the accuracy and stability of the triangulation. When the current frame satisfies either of the conditions that the feature ratio is lower than the ratio threshold or the comprehensive average disparity is higher than the disparity threshold, the algorithm performs key frame marking processing on the frame. This "or" logic determination strategy ensures the flexibility of key frame selection, considering both the change of feature matching quality and the amplitude of spatial motion. Through the continuous traversal and determination process, an optimized key frame sequence is gradually constructed, which contains representative and information gain image frames, ensuring the integrity of visual information and controlling the calculation complexity.

[0075] Further, the calculating the point feature average disparity and the line feature average disparity according to the position change of the point feature and the position change of the line feature between the current frame image and the last key frame image, and obtaining the comprehensive average disparity by weighted summation according to preset weight coefficients comprises: calculating the pixel displacement distance of each point feature on the image plane according to the point feature matching pair between the current frame image and the last key frame image, and performing arithmetic average calculation on the pixel displacement distances of all point features to obtain the point feature average disparity; calculating the change amount of the line segment midpoint coordinate and the line segment direction angle according to the line feature matching pair between the current frame image and the last key frame image, linearly combining the midpoint coordinate change amount and the angle change amount according to a preset proportion coefficient to obtain the comprehensive change amount of each line feature; performing arithmetic average calculation on the comprehensive change amounts of the line features to obtain the line feature average disparity; multiplying the point feature average disparity by a preset point feature weight coefficient, multiplying the line feature average disparity by a preset line feature weight coefficient, and adding the two product results to obtain the comprehensive average disparity.

[0076] Specifically, the parallax calculation process calculates the pixel displacement distance of each point feature pair from the point feature matching pairs established between the current frame image and the last key frame image. For each point feature matching pair, the algorithm extracts the pixel coordinates of the corresponding feature points in the two frames, including the coordinate position in the last key frame and the coordinate position in the current frame. The pixel displacement distance is obtained by calculating the Euclidean distance between the two coordinate points, and the specific calculation process involves the square sum of the coordinate difference and the square root operation. The distance calculation in this two-dimensional plane can directly reflect the spatial displacement amplitude of the feature points in the image, and the larger the distance value, the more significant the position change of the feature points between the two frames. The pixel displacement distance of each point feature is a scalar value, with a unit of pixels, which directly reflects the change of the image projection position of the feature points caused by the camera motion or scene change. After obtaining the pixel displacement distances of all point feature matching pairs, the algorithm performs arithmetic average calculation on these distance values, and obtains the point feature average parallax by adding all distance values and dividing by the total number of matching pairs. This average value eliminates the influence of abnormal displacement of individual feature points and can reflect the overall feature point displacement trend, providing a reliable statistical indicator for the evaluation of camera motion amplitude. The size of the point feature average parallax is directly related to the degree of translation and rotation of the camera, and a larger average parallax indicates that the camera has undergone significant position or attitude change between the two frames.

[0077] Specifically, the parallax calculation of line features is more complex than that of point features, because line segment features contain both position information and direction information, and both aspects need to be considered. The algorithm first processes the change amount calculation of the line segment midpoint coordinates. For each line feature matching pair, the coordinates of the corresponding line segment endpoints in the two frames are extracted, and the midpoint position of the line segment is determined by calculating the average of the endpoint coordinates. The line segment midpoint represents the spatial position of the line segment in the image, and the change of the midpoint coordinates can reflect the overall translation motion of the line segment. The midpoint coordinate change amount is obtained by calculating the Euclidean distance between the midpoints of the corresponding line segments in the two frames. This calculation process is similar to the displacement distance calculation of point features, but it focuses on the overall position change of the line segment rather than the change of a single point. Subsequently, the algorithm calculates the change amount of the line segment direction angle, which is determined by the direction vector of the line segment. The direction vector can be calculated from the two endpoint coordinates of the line segment. The direction angle usually represents the angle between the line segment and a certain axis of the image coordinate system in radians or degrees, and the angle change amount is obtained by calculating the difference between the direction angles of the corresponding line segments in the two frames. This angle difference calculation needs to consider the periodic nature of the angle to ensure that the calculation result is within a reasonable angle range. The change of the line segment direction angle reflects the influence of the camera rotation motion on the projection direction of the linear feature, and a larger angle change indicates that the camera has undergone significant rotation motion.

[0078] Specifically, the midpoint coordinate change and the angle change are linearly combined by a preset proportionality coefficient to obtain the comprehensive change of each line feature. This linear combination strategy unifies the position change and the direction change into the same measurement space, so that the change quantities of different natures can be meaningfully compared and fused. The setting of the proportionality coefficient needs to consider the dimensional difference between the position change and the angle change and their relative importance in the disparity evaluation. The unit of the position change is pixel, while the unit of the angle change is radian or angle, and the numerical range and physical meaning of the two are different. Through appropriate setting of the proportionality coefficient, the angle change can be converted into an equivalent position change quantity, or the position change can be converted into an equivalent angle change quantity, so that the two changes can be compared at the same scale. The calculation process of the linear combination multiplies the midpoint coordinate change by a first proportionality coefficient, multiplies the angle change by a second proportionality coefficient, and then adds the two product results to obtain the comprehensive change of the line feature. This comprehensive change reflects the position change and the direction change of the line segment, and provides a more comprehensive description of the line feature change than a single indicator. After performing this comprehensive change calculation on all line feature matching pairs, the algorithm performs an arithmetic average calculation on the comprehensive changes of the line features, and obtains the line feature average disparity by adding all the comprehensive changes and then dividing by the total number of line feature matching pairs. This average value eliminates the interference of abnormal changes of individual line features, and provides a statistical description of the overall change trend of the line feature.

[0079] Specifically, the calculation of the comprehensive average disparity is realized by weighted fusion of the point feature average disparity and the line feature average disparity, the point feature average disparity is multiplied by a preset point feature weight coefficient to obtain a weighted contribution value of the point feature in the comprehensive disparity, and the point feature weight coefficient reflects the importance and reliability of the point feature in camera motion estimation. The point feature has high sensitivity and accuracy in the detection of translational motion due to its accurate pixel-level positioning capability, and thus plays an important role in the calculation of the disparity. Meanwhile, the line feature average disparity is multiplied by a preset line feature weight coefficient to obtain a weighted contribution value of the line feature in the comprehensive disparity, and the line feature weight coefficient reflects the geometric constraint capability of the line feature in a structured environment and the sensitivity of the line feature to rotational motion. The line feature has a unique advantage in detecting the rotational motion of the camera due to its directional information, especially in an environment with rich linear structures but lacking of texture. The two weighted contribution values are added to obtain the comprehensive average disparity, which comprehensively reflects the overall spatial change degree between the current frame and the last key frame. The setting of the weight coefficient is based on the performance of the point feature and the line feature in different environmental conditions, and the point feature can be given a higher weight in an environment with rich texture, and the weight proportion of the line feature can be increased in an environment with higher structuralization. The comprehensive average disparity is an important basis for key frame selection, and the value directly affects the selection frequency and quality of the key frame, and appropriate setting of the disparity threshold can ensure that the key frame contains sufficient new information and maintains a reasonable selection density.

[0080] 104. performing three-dimensional reconstruction processing on the point and line features in the optimized key frame sequence and fusion optimization processing with inertial measurement unit data to obtain a robot visual recognition positioning result.

[0081] In an embodiment of the present application, the performing three-dimensional reconstruction processing on the point and line features in the optimized key frame sequence and fusion optimization processing with inertial measurement unit data to obtain a robot visual recognition positioning result comprises: performing triangularization calculation on the point features in the optimized key frame sequence through singular value decomposition to solve three-dimensional coordinates of the point features in a world coordinate system to obtain a spatial position of the point features; converting the line features in the optimized key frame sequence into Plucker coordinates to determine a spatial straight line equation of the line features in the world coordinate system through a normal vector and a direction vector of the Plucker coordinates to obtain a spatial geometry of the line features; calculating a reprojection error to construct a visual residual term according to the spatial position of the point features and the spatial geometry of the line features, calculating a pre-integration error to construct an inertial residual term according to the inertial measurement unit data, and combining the visual residual term and the inertial residual term to obtain a total cost function; performing nonlinear minimization on the total cost function within a sliding window to iteratively optimize position and attitude parameters of the robot to obtain the robot visual recognition positioning result.

[0082] Specifically, the triangulation calculation of the point feature in the key frame sequence is optimized, and the singular value decomposition method is used to solve the three-dimensional coordinates of the point feature in the world coordinate system. The basic principle of triangulation calculation is to use the image projection of the same space point under multiple perspectives to reconstruct the three-dimensional position of the point. This process requires known camera intrinsic and extrinsic information. For each point feature in the optimized key frame sequence, the algorithm first collects the observation information of the feature in different key frames, including the pixel coordinates of the feature point in each frame image and the corresponding camera pose. The camera pose describes the position and direction of the camera in the world coordinate system, and is usually represented by a rotation matrix and a translation vector. Based on the camera projection model, a three-dimensional point in space is projected onto the image plane to form a pixel point. This projection process can be represented by a linear equation system. However, due to the existence of image noise and feature detection errors, these linear equation systems are often overdetermined and not completely consistent. The singular value decomposition (SVD) method can solve such overdetermined linear equation systems in the least squares sense, and find the solution that minimizes the residual by decomposing the coefficient matrix. The SVD algorithm decomposes the coefficient matrix into the product of three matrices, which contains the singular values and corresponding singular vectors of the matrix. The right singular vector corresponding to the smallest singular value provides the optimal solution of the equation system. The point feature space position calculated in this way is the three-dimensional coordinates of the feature point in the world coordinate system. These coordinate information constitutes a sparse three-dimensional point cloud representation of the environment.

[0083] In particular, the 3D reconstruction of line features employs the Plcker coordinate system to represent a spatial line, which can uniquely and compactly describe the geometry of a line in 3D space. The Plcker coordinates are a homogeneous coordinate system that represents a line in 3D space with six parameters, which include the components of a normal vector and a direction vector. The normal vector is perpendicular to the plane that contains the origin and the line, while the direction vector is along the direction of the line. Converting the line features in the optimized keyframe sequence to Plcker coordinate representation requires the use of the observation information of the line features under multiple viewpoints. For each spatial line, the algorithm collects the image projections of the line in different keyframes, which appear as line segment features in the images. The projection of a spatial line on the image plane is a straight line, and the equation of this projected line can be represented in the form of end-point coordinates or slope-intercept. Through the projection geometry of the camera, the constraint equation between the spatial line and its image projection can be established. With the projection constraints under multiple viewpoints, the algorithm can reconstruct the Plcker coordinate parameters of the spatial line. The calculation process of the Plcker coordinates involves solving a constrained optimization problem, which needs to satisfy both the intrinsic constraint conditions of the Plcker coordinates and the extrinsic constraint conditions of the multi-view projections. Through the normal vector and the direction vector of the Plcker coordinates, the spatial line equation of the line feature in the world coordinate system can be determined, which completely describes the position and direction of the line in 3D space. The obtained spatial geometric information of the line feature provides structured geometric constraints for the environment, which are particularly valuable in texture-lacking environments.

[0084] Specifically, the construction of visual-inertial residual terms is based on the computation of re-projection error and pre-integration error. Visual residual terms are constructed by computing the re-projection error of 3D features under the current estimated pose, which projects the spatial position of point features and the spatial geometry of line features back to the image plane, and then computes the difference between the projected position and the actual observed position. For point features, the re-projection error is the Euclidean distance between the projected point and the observed point, which reflects the accuracy of the current pose estimate. For line features, the re-projection error is obtained by computing the geometric distance between the projected straight line and the observed line segment, which includes the distance from a point to a line and the angular deviation between lines. The size of the re-projection error directly reflects the consistency between visual observations and the current pose estimate, and a smaller re-projection error indicates a more accurate pose estimate. Inertial residual terms are constructed based on the acceleration and angular velocity data provided by the inertial measurement unit, and the relative motion between adjacent keyframes is calculated through the IMU pre-integration technique. IMU pre-integration is a technique that integrates high-frequency inertial measurement data into low-frequency constraints, which can effectively handle the mismatch between the frequency of visual observations and the frequency of inertial measurements. The pre-integration process integrates all IMU measurements between adjacent keyframes into a relative motion constraint, including position change, velocity change, and attitude change. The pre-integration error is calculated by comparing the relative motion predicted by the IMU with the relative motion estimated by the vision, which reflects the consistency between inertial measurements and visual estimates. The visual residual terms and the inertial residual terms are combined to obtain the overall cost function, which considers the constraint information of visual observations and inertial measurements.

[0085] Specifically, the sliding window optimization technique can ensure positioning accuracy while maintaining computational efficiency. The sliding window maintains a fixed-size set of keyframes, and as new keyframes are added, the oldest keyframes are removed from the window. This mechanism ensures that the size of the optimization problem remains within a controllable range. Within the sliding window, the algorithm performs nonlinear minimization of the overall cost function. This process uses iterative optimization methods such as the Levenberg-Marquardt algorithm or the Gauss-Newton algorithm. The goal of nonlinear optimization is to find a set of robot position and attitude parameters that minimizes the value of the overall cost function, which is equivalent to minimizing the overall error of visual observations and inertial measurements. The iterative optimization process starts from an initial pose estimate and determines the direction and step size of parameter updates by calculating the gradient and Hessian matrix of the cost function. Each iteration updates the robot's position coordinates, attitude angles, and related velocity and bias parameters until the convergence conditions are met. Convergence conditions include indicators such as the change in the value of the cost function, the magnitude of parameter updates, and the size of the gradient. Various constraints need to be handled during optimization, including the orthogonality constraint of rotation matrices and the boundary constraint of IMU bias. Through this iterative optimization process, the algorithm gradually refines the robot's position and attitude estimates, and ultimately obtains the robot's visual recognition positioning results. This result includes the robot's accurate position coordinates and attitude information at each keyframe time, as well as the three-dimensional reconstruction results of feature points and feature lines in the environment, providing reliable spatial reference information for the robot's autonomous navigation and environmental understanding.

[0086] Further, the constructing a visual residual term according to the point feature space position and the line feature space geometry, constructing an inertial residual term according to inertial measurement unit data, and combining the visual residual term and the inertial residual term to obtain a total cost function comprises: projecting the point feature space position onto an image plane according to a current robot pose parameter, and calculating an Euclidean distance between a projected position and an actual observed position as a point feature re-projection error; projecting the line feature space geometry onto the image plane according to the current robot pose parameter, and calculating a point-to-line distance between a projected straight line and an actual observed line segment as a line feature re-projection error; squaring the point feature re-projection error and the line feature re-projection error respectively, and performing weighted summation calculation according to a preset visual weight matrix to obtain the visual residual term; performing numerical integration calculation according to acceleration measurement values and angular velocity measurement values of the inertial measurement unit between adjacent key frames to obtain an IMU predicted pose change amount, and performing difference calculation between the IMU predicted pose change amount and a visual estimated pose change amount to obtain an inertial pre-integration error; performing covariance matrix normalization processing on the inertial pre-integration error, and performing weighted calculation according to a preset inertial weight coefficient to obtain the inertial residual term; performing linear combination calculation according to a preset fusion weight coefficient on the visual residual term and the inertial residual term, and adding a regularization constraint term of robot prior pose information to obtain the total cost function.

[0087] Specifically, the re-projection error needs to utilize the current robot pose parameter and camera intrinsic parameters to perform three-dimensional to two-dimensional coordinate transformation. The process of projecting the point feature space position onto the image plane according to the current robot pose parameter involves a series of coordinate system transformations. Firstly, the three-dimensional point coordinates in the world coordinate system are transformed into the camera coordinate system. This transformation is realized by a rotation matrix and a translation vector. The rotation matrix describes the attitude direction of the camera, and the translation vector describes the spatial position of the camera. In the camera coordinate system, the three-dimensional point is mapped to the image plane through the perspective projection model. The perspective projection considers intrinsic factors such as the focal length, principal point coordinates and lens distortion of the camera. The pixel coordinates obtained by the projection represent the theoretical observed position of the three-dimensional point feature under the current pose estimation. Then, the Euclidean distance between the projected position and the actual observed position is calculated as the point feature re-projection error. The actual observed position comes from the feature point coordinates identified by the feature detection algorithm in the image. The Euclidean distance is calculated by squaring the difference between the projected coordinates and the observed coordinates in the x and y directions and then taking the square root. This distance value directly reflects the consistency degree of the current pose estimation and the actual observation. A smaller re-projection error indicates that the pose estimation is more accurate, while a larger error indicates that there is a pose estimation deviation or observation noise.

[0088] Specifically, the calculation of line feature reprojection error is relatively complex, because it needs to deal with the projection relationship of three-dimensional straight line on the image plane. The projection of line feature space geometry to the image plane according to the current robot pose parameters needs to consider the Plucker coordinate representation of the space straight line and the geometric characteristics of the perspective projection. The space straight line forms a projected straight line on the image plane through the perspective projection, and the equation of the projected straight line can be determined by the projection of two points on the straight line, or obtained by direct transformation of the Plucker coordinates. The projected straight line in the image is represented as a straight line equation with a certain slope and intercept, and its parameters are completely determined by the geometric properties of the space straight line and the pose parameters of the camera. The point-to-line distance of the projected straight line and the actual observed line segment is calculated as the line feature reprojection error, and the actual observed line segment is obtained from the line segment feature identified in the image by the line feature detection algorithm. The calculation of the point-to-line distance involves the calculation of the perpendicular distance from the line segment endpoints to the projected straight line, and since the line segment has a finite length, it also needs to consider whether the projection of the endpoints on the projected straight line falls within a reasonable range. The line feature reprojection error is usually represented by the distance from the midpoint of the line segment to the projected straight line, or the average of the distances from the endpoints of the line segment to the projected straight line. This error measurement method can comprehensively reflect the position and direction deviation of the line feature, and provides a quantitative indicator for the quality evaluation of the line feature constraint.

[0089] Specifically, the construction of the visual residual term is realized by mathematical processing of the point feature reprojection error and the line feature reprojection error. The point feature reprojection error and the line feature reprojection error are respectively squared, and the purpose of squaring is to eliminate the influence of the sign of the error, so that all errors contribute positively to the cost function. At the same time, the squaring operation can also amplify the influence of larger errors, so that the optimization process pays more attention to those observations with larger deviations. The weighted sum of the squared error values is calculated according to the preset visual weight matrix, which reflects the reliability and importance of different feature observations. The weight settings of point features and line features take into account their different characteristics in terms of positioning accuracy and environmental adaptability. Point features have higher accuracy in position constraints, while line features provide important geometric constraints in structured environments. The weight matrix can also be dynamically adjusted according to the detection quality of the features, the matching confidence and the observation conditions, to ensure that high-quality feature observations play a more important role in optimization. The weighted sum calculation adds up the weighted squared errors of all features to obtain the visual residual term, which comprehensively reflects the overall consistency of the current pose estimation with all visual observations.

[0090] Specifically, the construction of the inertial residual term is based on the IMU pre-integration technique and the comparative analysis of the pose change. According to the acceleration measurement value and the angular velocity measurement value of the inertial measurement unit between adjacent key frames, numerical integration is calculated, which integrates high-frequency IMU measurement data into the relative motion constraint between key frames. Numerical integration needs to consider the influence of the bias, noise characteristics of the IMU and the gravity field, and the integration process usually uses numerical methods such as midpoint method or fourth-order Runge-Kutta method to improve the accuracy. The results of IMU pre-integration include the relative position change, velocity change and attitude change between adjacent key frames, which constitute the pose change predicted by the IMU. At the same time, the pose change between adjacent key frames is calculated by a visual estimation method, which is based on visual feature matching and geometric constraint solving. The difference between the pose change predicted by the IMU and the pose change estimated by the vision is calculated to obtain the inertial pre-integration error, which reflects the consistency of inertial measurement and visual estimation in motion prediction. The pre-integration error includes position error, velocity error and attitude error components, each of which reflects the estimation bias of the corresponding motion parameter. The inertial pre-integration error is normalized by the covariance matrix, which describes the statistical characteristics of the IMU measurement noise and the correlation between different error components. Normalization processing converts the error to a standardized space through inverse matrix transformation of the covariance matrix, so that errors of different types and magnitudes can be compared and optimized in the same scale. The inertial residual term is obtained by weighting calculation according to the preset inertial weight coefficient, which reflects the importance and reliability of IMU measurement in the overall positioning.

[0091] Specifically, the construction of the overall cost function realizes the comprehensive optimization of multi-modal information by fusing the visual residual term and the inertial residual term. The visual residual term and the inertial residual term are linearly combined according to the preset fusion weight coefficient, and the setting of the fusion weight coefficient needs to balance the contribution of visual information and inertial information in different motion states and environmental conditions. The weight of the visual residual term can be increased in good visual conditions, while more reliance can be placed on inertial information in visual degradation environments. Linear combination ensures that the two information sources can work together and complement each other's shortcomings. In addition to the visual and inertial residual terms, the algorithm also adds a regularization constraint term of the prior pose information of the robot, which comes from the previous positioning results, motion model prediction or external positioning sources. The role of the regularization constraint term is to prevent the optimization process from deviating from the reasonable solution space, especially in the case of insufficient or ambiguous observation information, the prior constraint can guide the optimization to the more likely solution direction. The weight setting of the regularization term needs to balance the credibility of the prior information and the importance of the current observation, and too strong regularization will limit the algorithm's ability to adapt to new information, while too weak regularization may lead to unstable optimization results.

[0092] In the embodiment, a plurality of left and right target images are acquired by a stereo camera, double feature detection is performed on the images, point features and line features are extracted, and complementary feature information is formed; then, the point and line features are matched and optimized to obtain a high-quality feature matching result; key frames are selected based on the point and line fusion feature matching result to reduce redundant calculation; the point and line features in the key frames are reconstructed in three dimensions, and are fused and optimized with inertial measurement unit data to obtain accurate robot position and posture information. The robot vision recognition positioning method provided by the application effectively solves the limitation of relying on point features in a texture missing environment, significantly improves the accuracy and robustness of robot vision positioning, and is suitable for robot autonomous navigation in a texture missing and rugged terrain environment.

[0093] The robot vision recognition positioning method in the embodiment of the application is described above, and the robot vision recognition positioning device in the embodiment of the application is described below. The robot vision recognition positioning device is described with reference to Figure 2 An embodiment of the robot vision recognition positioning device in the embodiment of the application includes:

[0094] The feature detection module 201 is configured to perform double feature detection on a plurality of left and right target images continuously acquired by a stereo camera installed on the robot to obtain a point feature set and a line feature set.

[0095] The feature matching module 202 is configured to perform feature matching and optimization on the point feature set and the line feature set according to a preset screening condition to obtain a point and line fusion feature matching result.

[0096] The key frame selection module 203 is configured to perform key frame selection on the plurality of left and right target images according to the point and line fusion feature matching result to obtain an optimized key frame sequence.

[0097] The fusion positioning module 204 is configured to perform three-dimensional reconstruction on the point and line features in the optimized key frame sequence, and perform fusion and optimization with inertial measurement unit data to obtain a robot vision recognition positioning result.

[0098] In the embodiment of the present application, the visual recognition positioning device of the robot runs the visual recognition positioning method of the robot, the visual recognition positioning device of the robot collects multiple left and right target images through a stereo camera, performs double feature detection on the images, extracts point features and line features at the same time, and forms complementary feature information; then, the point and line features are matched and optimized to obtain a high-quality feature matching result; key frame selection is performed based on the point and line fusion feature matching result to reduce redundant calculation; the point and line features in the key frame are reconstructed in three dimensions, and are fused and optimized with inertial measurement unit data to obtain accurate robot position and attitude information. Through the point and line feature fusion mode, the present application effectively solves the limitation of relying on point features in a texture missing environment, significantly improves the accuracy and robustness of robot visual positioning, and is suitable for robot autonomous navigation in a texture missing and rugged terrain environment.

[0099] The above Figure 2 The visual recognition positioning device of the robot in the embodiment of the present application is described in detail from the perspective of a modular functional entity, and the visual recognition positioning device of the robot in the embodiment of the present application is described in detail from the perspective of hardware processing.

[0100] Figure 3 Fig. 1 is a structural schematic diagram of a visual recognition positioning device of a robot provided by the embodiment of the present application. The visual recognition positioning device 300 of the robot can have great differences due to different configurations or performances, and can include one or more processors (central processing units, CPU) 310 (for example, one or more processors) and a memory 320, one or more storage media 330 (for example, one or more mass storage device ends) storing application programs 333 or data 332. The memory 320 and the storage media 330 can be temporary storage or persistent storage. The programs stored in the storage media 330 can include one or more modules (not shown in the figure), and each module can include a series of instruction operations in the visual recognition positioning device 300 of the robot. Further, the processor 310 can be configured to communicate with the storage media 330, execute a series of instruction operations in the storage media 330 on the visual recognition positioning device 300 of the robot, so as to realize the steps of the visual recognition positioning method of the robot described above.

[0101] The visual recognition positioning device 300 of the robot can further include one or more power supplies 340, one or more wired or wireless network interfaces 350, one or more input and output interfaces 360, and / or one or more operating systems 331, such as Windows Serve, Mac OS X, Unix, Linux, FreeBSD, and the like. Those skilled in the art can understand that the visual recognition positioning device 300 of the robot can further include other components, which are not shown in the figure, for example, a bus line, a keyboard, a mouse, a display screen, and the like. Figure 3The visual identification positioning device structure of the robot shown does not constitute a limitation on the visual identification positioning device of the robot provided by the present application, and can include more or fewer components than shown, or combine certain components, or different component arrangements.

[0102] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described system or device, unit can refer to the corresponding processes in the foregoing method embodiments, which will not be described here.

[0103] The above-described embodiments are merely used to illustrate the technical solutions of the present application, rather than limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacements for some technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A visual recognition positioning method of a robot, characterized by, The visual recognition positioning method of the robot comprises the following steps: A stereo camera installed on a computer robot continuously collects pixel gradients of multiple left and right target images, a gradient matrix is constructed according to the pixel gradients, and eigenvalues of the gradient matrix are calculated; the minimum value in the eigenvalues is taken as a corner point response intensity, and pixel points with a response intensity greater than a preset intensity threshold are screened to obtain a point feature set; gradient angles are calculated according to the pixel gradients, and region growing and straight line fitting processing are performed on pixels with a gradient angle greater than a preset similarity threshold to obtain an original line feature set; a length threshold is determined according to a product of a minimum image resolution and a preset scaling factor, and line segments with a length less than the length threshold in the original line feature set are filtered to obtain a line feature set; Displacement vectors of the point feature set between consecutive frames are calculated through forward-backward optical flow consistency checking to obtain a point feature matching pair; a binary descriptor vector is generated by statistically encoding the gradient direction of the pixels around the line feature set to obtain a line feature descriptor set; a Hamming distance between the binary descriptor vectors in the line feature descriptor set is calculated as a similarity, binary descriptor pairs with a similarity lower than a preset matching threshold are removed, and a secondary screening is performed in combination with a line segment endpoint position change constraint to obtain a line feature matching pair; the point feature matching pair and the line feature matching pair are subjected to data fusion processing to obtain a point-line fusion feature matching result; The multiple left and right target images are traversed to determine a current frame image and a last key frame image in the multiple left and right target images, and the number of point feature matches and the number of line feature matches between the current frame image and the last key frame image in the point-line fusion feature matching result are counted; The number of point feature matches and the number of line feature matches between the current frame image and the last key frame image are taken as a total number of matching features, the number of point feature matches and the number of line feature matches of the current frame image are taken as a total number of current frame detection features, and a ratio of the total number of matching features to the total number of current frame detection features is calculated to obtain a feature ratio; Point feature average disparity and line feature average disparity are respectively calculated according to the point feature position change and the line feature position change between the current frame image and the last key frame image, and a comprehensive average disparity is obtained by weighted summation according to a preset weight coefficient; The feature ratio is compared with a preset ratio threshold, the comprehensive average disparity is compared with a preset disparity threshold, and key frame marking processing is performed on the current frame image with a feature ratio lower than the ratio threshold or a comprehensive average disparity higher than the disparity threshold to obtain an optimized key frame sequence; Point-line features in the optimized key frame sequence are subjected to three-dimensional reconstruction processing, and are fused and optimized with inertial measurement unit data to obtain a robot visual recognition positioning result.

2. The visual recognition positioning method of the robot according to claim 1, wherein, The point feature average disparity and the line feature average disparity are respectively calculated according to the point feature position change and the line feature position change between the current frame image and the last key frame image, and the comprehensive average disparity is obtained by weighted summation according to a preset weight coefficient. The pixel displacement distance of each point feature on the image plane is calculated according to the point feature matching pairs between the current frame image and the last key frame image, the pixel displacement distances of all point features are arithmetically averaged to obtain a point feature average disparity; The midpoint coordinate and the angle change of the line segment direction of each line segment are calculated according to the line feature matching pairs between the current frame image and the last key frame image, the midpoint coordinate change and the angle change are linearly combined according to a preset proportion coefficient to obtain a comprehensive change of each line feature; The comprehensive changes of the line features are arithmetically averaged to obtain a line feature average disparity; The point feature average disparity is multiplied by a preset point feature weight coefficient, the line feature average disparity is multiplied by a preset line feature weight coefficient, and the two product results are added to obtain the comprehensive average disparity.

3. The method of claim 1, wherein, The point and line features in the optimized key frame sequence are processed for three-dimensional reconstruction, and are fused and optimized with inertial measurement unit data to obtain a robot visual recognition positioning result, which includes: The point features in the optimized key frame sequence are processed for triangularization calculation through singular value decomposition to solve three-dimensional coordinates of the point features in a world coordinate system, thereby obtaining point feature spatial positions; The line features in the optimized key frame sequence are converted into Plücker coordinates, and a spatial straight line equation of the line features in the world coordinate system is determined through normal vectors and direction vectors of the Plücker coordinates, thereby obtaining line feature spatial geometries; A visual residual term is constructed according to the point feature spatial positions and the line feature spatial geometries by calculating re-projection errors, an inertial residual term is constructed according to pre-integration errors calculated from inertial measurement unit data, and a total cost function is obtained by combining the visual residual term and the inertial residual term; The total cost function is solved by non-linear minimization within a sliding window, and position and attitude parameters of the robot are iteratively optimized to obtain the robot visual recognition positioning result.

4. The visual recognition positioning method of the robot according to claim 3, wherein, The visual residual term is constructed according to the point feature spatial positions and the line feature spatial geometries by calculating re-projection errors, the inertial residual term is constructed according to pre-integration errors calculated from inertial measurement unit data, and the total cost function is obtained by combining the visual residual term and the inertial residual term, which includes: The point feature spatial positions are projected onto an image plane according to current robot pose parameters, and Euclidean distances between the projected positions and actual observed positions are calculated as point feature re-projection errors; The line feature spatial geometries are projected onto the image plane according to the current robot pose parameters, and point-to-line distances between the projected straight lines and actual observed line segments are calculated as line feature re-projection errors; The point feature re-projection errors and the line feature re-projection errors are respectively squared and weighted and summed according to a preset visual weight matrix to obtain the visual residual term; The IMU predicted pose change is obtained by numerical integration calculation of acceleration measurement values and angular velocity measurement values of the inertial measurement unit between adjacent key frames, and the inertial pre-integration error is obtained by difference calculation of the IMU predicted pose change and a visually estimated pose change. The inertial pre-integration error is subjected to covariance matrix normalization processing, and is subjected to weighted calculation according to a preset inertial weight coefficient, to obtain the inertial residual term; The visual residual term and the inertial residual term are subjected to linear combination calculation according to a preset fusion weight coefficient, and a regularization constraint term of robot prior pose information is added, to obtain an overall cost function.

5. A visual recognition positioning device for a robot, characterized in that, The visual recognition positioning device of the robot applies the visual recognition positioning method of the robot as claimed in any one of claims 1-4, and comprises: A feature detection module is configured to perform double feature detection processing on a plurality of left and right target images successively collected by a stereo camera installed on the robot, to obtain a point feature set and a line feature set; A feature matching module is configured to perform feature matching optimization processing on the point feature set and the line feature set according to a preset screening condition, to obtain a point-line fusion feature matching result; A key frame selection module is configured to perform key frame selection processing on the plurality of left and right target images according to the point-line fusion feature matching result, to obtain an optimized key frame sequence; A fusion positioning module is configured to perform three-dimensional reconstruction processing on point-line features in the optimized key frame sequence, and perform fusion optimization processing with inertial measurement unit data, to obtain a robot visual recognition positioning result.

6. A visual recognition positioning device for a robot, characterized in that The visual recognition positioning device of the robot comprises a memory and at least one processor, and the memory stores instructions; The at least one processor invokes the instructions in the memory, so that the visual recognition positioning device of the robot performs the steps of the visual recognition positioning method of the robot as claimed in any one of claims 1-4.

Images