Visual identification positioning method, device and equipment of robot

Abstract

The invention provides a visual identification positioning method, device and equipment for a robot, and the method comprises the steps: collecting a plurality of frames of left and right target images through a stereo camera, carrying out the dual feature detection of the images, extracting point features and line features, and forming complementary feature information; matching optimization processing is carried out on the point-line features, and a high-quality feature matching result is obtained; key frame selection is carried out based on a point-line fusion feature matching result, and redundancy calculation is reduced; and carrying out three-dimensional reconstruction on point-line features in the key frame, and carrying out fusion optimization with inertial measurement unit data to obtain accurate robot position and attitude information. Through a point-line feature fusion mode, the limitation of purely depending on point features in a texture missing environment is effectively solved, the precision and robustness of robot vision positioning are remarkably improved, and the method is suitable for robot autonomous navigation in texture missing and rugged terrain environments.

Description

Technical Field

[0001] The present invention relates to the field of visual recognition technology, and in particular to a robot visual recognition positioning method, device and equipment. Background Art

[0002] With the rapid development of robotics, visual inertial odometry (VIO) has become a key technology for autonomous robot navigation. Existing robot visual localization methods primarily use point features for environmental perception and position estimation. These methods extract corner features from images and combine them with inertial measurement unit data to achieve real-time robot localization. These methods achieve good localization results in ideal environments with rich textures and sufficient feature points.

[0003] However, existing point-feature-based visual-inertial odometry systems have significant shortcomings in environments with a lack of texture and rough terrain. When large areas of smooth surfaces and sparse texture information are present, the number of valid feature points is insufficient, resulting in reduced feature matching quality. Furthermore, rough terrain can cause significant vibrations during camera movement, further complicating feature point matching and tracking. This ultimately leads to reduced positioning accuracy or even failure, severely limiting the robot's practicality and reliability in complex environments. Summary of the Invention

[0004] The main purpose of this invention is to solve the technical problem that the existing point-feature-based visual inertial odometry has a reduced positioning accuracy due to insufficient effective feature points and low feature matching quality in texture-deficient and rugged terrain environments; A first aspect of the present invention provides a robot visual recognition and positioning method, the robot visual recognition and positioning method comprising: Perform dual feature detection on multiple frames of left and right target images continuously captured by the stereo camera installed on the robot to obtain a point feature set and a line feature set; Performing feature matching optimization processing on the point feature set and the line feature set according to preset screening conditions to obtain a point-line fusion feature matching result; Performing key frame selection processing on the multiple frames of left and right target images according to the point-line fusion feature matching results to obtain an optimized key frame sequence; The point and line features in the optimized key frame sequence are subjected to three-dimensional reconstruction processing, and are fused and optimized with the inertial measurement unit data to obtain the robot visual recognition and positioning result.

[0005] Optionally, in a first implementation of the first aspect of the present invention, performing dual feature detection processing on multiple frames of left and right target images continuously captured by a stereo camera installed on the robot to obtain a point feature set and a line feature set includes: Calculate the pixel gradients of multiple frames of left and right target images continuously captured by the stereo camera installed on the robot, construct a gradient matrix based on the pixel gradients, and calculate the eigenvalues ​​of the gradient matrix; Taking the minimum value of the characteristic values ​​as the corner point response strength, and screening the pixel points whose corner point response strength is greater than a preset strength threshold to obtain the point feature set; Calculating a gradient angle based on the pixel gradient, and performing region growing and line fitting on pixels whose gradient angles are greater than a preset similarity threshold to obtain an original line feature set; A length threshold is determined according to the product of the minimum image resolution and a preset scaling factor, and line segments in the original line feature set whose length is less than the length threshold are filtered to obtain a line feature set.

[0006] Optionally, in a second implementation of the first aspect of the present invention, performing feature matching optimization processing on the point feature set and the line feature set according to preset screening conditions to obtain a point-line fusion feature matching result includes: Calculate the displacement vector of the point feature set between consecutive frames through forward and backward optical flow consistency test to obtain point feature matching pairs; Performing statistical coding on pixel gradient directions around the line feature set to generate a binary descriptor vector, thereby obtaining a line feature descriptor set; Calculating the Hamming distance between binary descriptor vectors in the line feature descriptor set as similarity, eliminating binary descriptor pairs with similarity lower than a preset matching threshold, and performing secondary screening in combination with the line segment endpoint position change constraint to obtain line feature matching pairs; The point feature matching pairs and the line feature matching pairs are subjected to data fusion processing to obtain a point-line fusion feature matching result.

[0007] Optionally, in a third implementation of the first aspect of the present invention, 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 includes: 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 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; The number of point feature matches and line feature matches between the current frame image and the last key frame image is taken as the total number of matched features, the number of point feature matches and line feature matches of the current frame image is taken as the total number of detected features of the current frame, and the ratio of the total number of matched features to the total number of detected features of the current frame is calculated to obtain the feature ratio; Calculating the average disparity of point features and the average disparity of line features based on the position changes of point features and line features 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; The feature ratio is compared with a preset ratio threshold, the comprehensive average disparity is compared with a preset disparity threshold, and a key frame marking process is performed on the current frame image whose feature ratio is lower than the ratio threshold or the comprehensive average disparity is higher than the disparity threshold to obtain an optimized key frame sequence.

[0008] Optionally, in a fourth implementation of the first aspect of the present invention, the calculating the point feature average disparity and the line feature average disparity respectively based on 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 includes: Calculating the pixel displacement distance of each point feature on the image plane based on the point feature matching 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 changes in the midpoint coordinates and the direction angle of the line segment based on the line feature matching pairs between the current frame image and the last key frame image, and linearly combining the midpoint coordinate changes and the angle changes according to a preset proportional coefficient to obtain a comprehensive change in each line feature; Performing arithmetic average calculation on the comprehensive changes of the line features to obtain the average parallax of the line features; 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 multiplication results are added to obtain the comprehensive average disparity.

[0009] Optionally, in a fifth implementation of the first aspect of the present invention, performing three-dimensional reconstruction processing on the point and line features in the optimized key frame sequence and fusing and optimizing the data with the inertial measurement unit to obtain the robot visual recognition and positioning result includes: Performing triangulation calculation on the point features in the optimized key frame sequence by singular value decomposition, solving the three-dimensional coordinates of the point features in the world coordinate system, and obtaining the spatial position of the point features; Converting the line features in the optimized key frame sequence into Plücker coordinates, determining the spatial straight line equation of the line features in the world coordinate system through the normal vector and direction vector of the Plücker coordinates, and obtaining the spatial geometry of the line features; A visual residual term is constructed by calculating a reprojection error based on the point feature space position and the line feature space geometry, an inertial residual term is constructed by calculating a pre-integration error based on the inertial measurement unit data, and the visual residual term and the inertial residual term are combined to obtain an overall cost function; The overall cost function is nonlinearly minimized within a sliding window, and the position and posture parameters of the robot are iteratively optimized to obtain the robot visual recognition and positioning result.

[0010] Optionally, in a sixth implementation of the first aspect of the present invention, the step of calculating the reprojection error based on the point feature space position and the line feature space geometry to construct a visual residual term, calculating the pre-integration error based on the inertial measurement unit data to construct an inertial residual term, and combining the visual residual term and the inertial residual term to obtain an overall cost function includes: Projecting the spatial position of the point feature onto the image plane according to the current robot posture parameters, and calculating the Euclidean distance between the projected position and the actual observed position as the point feature reprojection error; Projecting the line feature space geometry onto the image plane according to the current robot pose parameters, and calculating the point-to-line distance between the projected line and the actual observed line segment as the line feature reprojection error; The point feature reprojection error and the line feature reprojection error are squared respectively, and weighted summed according to a preset visual weight matrix to obtain the visual residual term; Numerical integration is performed on the acceleration and angular velocity measurements of the inertial measurement unit between adjacent key frames to obtain the pose change predicted by the IMU, and the pose change predicted by the IMU is calculated as a difference with the pose change estimated by vision to obtain the inertial pre-integration error; Performing covariance matrix normalization processing on the inertia pre-integration error, and performing weighted calculation according to a preset inertia weight coefficient to obtain the inertia residual term; The visual residual term and the inertial residual term are linearly combined and calculated according to a preset fusion weight coefficient, and a regularization constraint term of the robot's prior posture information is added to obtain an overall cost function.

[0011] A second aspect of the present invention provides a robot visual recognition and positioning device, the robot visual recognition and positioning device comprising: The feature detection module is used to perform dual feature detection on multiple frames of left and right target images continuously captured by the stereo camera installed on the robot to obtain a point feature set and a line feature set; A feature matching module is used to perform feature matching optimization processing on the point feature set and the line feature set according to preset screening conditions to obtain a point-line fusion feature matching result; A key frame selection module is used to perform key frame selection processing on the multiple frames of left and right target images according to the point-line fusion feature matching results to obtain an optimized key frame sequence; The fusion positioning module is used to perform three-dimensional reconstruction processing on the point and line features in the optimized key frame sequence, and to perform fusion optimization processing with the inertial measurement unit data to obtain the robot visual recognition positioning result.

[0012] The third aspect of the present invention provides a robot visual recognition and positioning device, comprising: a memory and at least one processor, wherein instructions are stored in the memory, and the memory and the at least one processor are interconnected through a line; the at least one processor calls the instructions in the memory so that the robot's visual recognition and positioning device executes the steps of the above-mentioned robot's visual recognition and positioning method.

[0013] The above-mentioned robot visual recognition and positioning method, device, and equipment use a stereo camera to capture multiple frames of left and right target images, perform dual feature detection on the images, and simultaneously extract point features and line features to form complementary feature information. The point and line features are then matched and optimized to obtain high-quality feature matching results. Key frames are selected based on the point and line fusion feature matching results to reduce redundant calculations. The point and line features in the key frames are three-dimensionally reconstructed and optimized with inertial measurement unit data to obtain accurate robot position and posture information. By fusing point and line features, the present invention effectively solves the limitations of relying solely on point features in texture-deficient environments, significantly improving the accuracy and robustness of robot visual positioning, and is suitable for autonomous robot navigation in texture-deficient and rugged terrain environments.

[0014] Other features and advantages of the present invention will be described in the following description, and in part will become apparent from the description, or understood by practicing the present invention. The purposes and other advantages of the present invention are realized and obtained by the structures particularly pointed out in the description, claims and drawings.

[0015] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 Schematic diagram of a first embodiment of a robot visual recognition and positioning method according to an embodiment of the present invention; Figure 2 Schematic diagram of an embodiment of a visual recognition and positioning device for a robot in an embodiment of the present invention; Figure 3 Schematic diagram of an embodiment of a robot's visual recognition and positioning device in an embodiment of the present invention. DETAILED DESCRIPTION

[0017] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of them. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0018] The terms "including," "having," and any variations thereof, as used in the embodiments of the present invention are intended to cover non-exclusive inclusions. 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 may optionally include other steps or units not listed, or may optionally include other steps or units inherent to the process, method, product, or device.

[0019] To facilitate understanding of this embodiment, a robot visual recognition and positioning method disclosed in an embodiment of the present invention is first described in detail. Figure 1 As shown, this method includes the following steps: 101. Perform dual feature detection on multiple frames of left and right target images continuously captured by a stereo camera installed on the robot to obtain a point feature set and a line feature set; In one embodiment of the present invention, the dual feature detection processing of multiple frames of left and right target images continuously captured by the stereo camera installed on the robot to obtain a point feature set and a line feature set includes: calculating the pixel gradients of the multiple frames of left and right target images continuously captured by the stereo camera installed on the robot, constructing a gradient matrix based on the pixel gradients and calculating the eigenvalues ​​of the gradient matrix; taking the minimum value of the eigenvalues ​​as the corner point response intensity, and screening pixels whose corner point response intensity is greater than a preset intensity threshold to obtain the point feature set; calculating the gradient angle based on the pixel gradient, and obtaining the original line feature set by performing region growing and straight line fitting processing on pixels whose gradient angles are greater than a preset similarity threshold; determining the length threshold based on the product of the minimum image resolution and a preset scaling factor, and filtering the line segments in the original line feature set whose lengths are less than the length threshold to obtain the line feature set.

[0020] Specifically, dual feature detection requires calculating pixel gradients across multiple frames of left and right target images continuously captured by a stereo camera. Pixel gradients represent the rate of change of intensity at each pixel in the image in the horizontal and vertical directions and are obtained by convolving the image with the Sobel operator or other gradient operators. Specifically, for each pixel in the image, its partial derivatives in the x and y directions are calculated, forming gradient vectors Gx and Gy. This gradient information reflects the distribution of edges and textures in the image and forms the basis for subsequent feature detection. Based on the calculated pixel gradients, the Shi-Tomasi corner detection algorithm is used to construct a gradient matrix for each pixel. The Shi-Tomasi algorithm is an improved version of the Harris corner detection algorithm, which identifies corner features by analyzing the image's second-order moment matrix. The gradient matrix is ​​a 2×2 symmetric matrix whose elements consist of the product of gradient components. The matrix construction process involves statistically analyzing gradient information within a neighborhood. By accumulating gradient product terms within a small window around the pixel, a mathematical representation describing the local image structure is formed. 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.

[0021] Specifically, the minimum of the two eigenvalues ​​is taken as the corner response strength. This choice is based on the theoretical basis of the Shi-Tomasi algorithm, which states that both eigenvalues ​​at a corner point should be relatively large, and the size of the minimum eigenvalue directly reflects the reliability of the point as a corner point. Compared to the Harris operator, which uses the product of the eigenvalues ​​minus the sum of squares as the response function, the Shi-Tomasi algorithm directly uses the minimum eigenvalue as the corner strength metric, avoiding the need to adjust the parameter k in the Harris operator and making corner detection more stable and reliable. By setting a preset intensity threshold, pixels with corner response strength exceeding the threshold are selected, and these pixels constitute the point feature set. Setting the threshold requires a balance between the number and quality of 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 are reasonably distributed spatially and avoid generating too many redundant feature points in the same area. Non-maximum suppression compares the response strength of each candidate corner point with that of other candidate points in its neighborhood, retaining only those with local maxima, thereby preventing corner point clustering.

[0022] Specifically, the Line Segment Detector (LSD) algorithm extracts line features from the angle information of pixel gradients. The LSD algorithm is a line segment detection algorithm capable of detecting line segments in an image with sub-pixel accuracy. Its core concept is to identify linear structures by analyzing the directional 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) and represents the direction of the gradient vector at each pixel. This angular information reveals the direction of edges 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 line segment. The algorithm uses a region growing strategy, starting from a seed pixel and gradually expanding to neighboring pixels with similar gradient angles, forming connected pixel regions. Each region represents a possible linear structure in the image, and the pixels within the region have consistent edge directional characteristics. The advantage of the LSD algorithm is that it automatically determines algorithm parameters, eliminating the need for manual adjustment. It also manages the probability of false detections, ensuring the reliability of detection results. After region growing is complete, the LSD algorithm performs line fitting on each pixel region, using the least squares method to determine the parameters of the best-fit line. This line fitting process requires addressing the interference of noisy pixels. The LSD algorithm uses a statistical validation mechanism to ensure that the fitting results accurately reflect the true linear structure. The algorithm calculates the statistical significance of each detected line segment and retains only those that meet the significance requirements. The fitted line segments constitute the original line feature set, each described by the start and end coordinates and direction parameters.

[0023] Specifically, the length threshold is determined based on the product of the minimum image resolution and a preset scaling factor. The setting of this threshold takes into account the impact of image size on line segment detection. Images with higher resolutions can detect more detailed line segments, while images with smaller resolutions require the threshold to be adjusted accordingly to avoid missing important linear features. The scaling factor value is selected based on experience and experimental results to ensure that the filtered line segments are indeed noise or unimportant short line segments. The filtering process calculates the Euclidean distance length of each line segment in the original line feature set, removes line segments with a length less than a preset threshold, and retains line segments with sufficient length as valid line features. This filtering strategy is based on geometric constraints. Too short line segments often lack sufficient geometric information to support reliable feature matching and 3D 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 processes. The entire dual feature detection process combines the advantages of the Shi-Tomasi corner detection algorithm and the LSD line segment detection algorithm to provide the robot with richer and more reliable visual information in texture-deficient environments. Point features provide precise position constraints, while line features supplement the geometric information in structured environments. The combination of the two significantly enhances the robustness and integrity of feature detection.

[0024] 102. Perform feature matching optimization processing on the point feature set and the line feature set according to preset screening conditions to obtain a point-line fusion feature matching result; In one embodiment of the present invention, the feature matching optimization processing of the point feature set and the line feature set according to preset screening conditions to obtain a point-line fusion feature matching result includes: calculating the displacement vector of the point feature set between consecutive frames through forward and backward optical flow consistency check to obtain a point feature matching pair; statistically encoding the pixel gradient direction around the line feature set to generate a binary descriptor vector to obtain a line feature descriptor set; calculating the Hamming distance between the binary descriptor vectors in the line feature descriptor set as the similarity, eliminating the binary descriptor pairs with a similarity lower than a preset matching threshold, and performing secondary screening in combination with the line segment endpoint position change constraint condition to obtain a line feature matching pair; 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.

[0025] Specifically, the feature matching optimization process begins with the calculation of the displacement vectors between consecutive frames of a point feature set. The KLT (Kanade-Lucas-Tomasi) optical flow tracking algorithm is then used to perform forward and backward optical flow consistency checks. The KLT (Kanade-Lucas-Tomasi) optical flow algorithm is a gradient-based feature tracking method that assumes that pixel intensities in an image remain constant over short periods of time and that adjacent pixels have similar motion. Forward optical flow tracking begins with a feature point in the current frame and calculates its position in the next frame by minimizing the brightness difference between image patches. This process involves the calculation of image gradients and iterative optimization. The algorithm selects a small window around the feature point, calculates the gradient information of the pixels within this window, and then uses the Newton iteration method to find the optimal displacement vector. Backward optical flow tracking starts from the predicted position in the next frame and tracks back to the original frame to calculate the reverse displacement vector. The forward and backward consistency check evaluates tracking quality by comparing the consistency of the displacement vectors obtained by forward and backward tracking. When the error between the tracking results in both directions at the same location is less than a preset threshold, the tracking of the feature point is considered reliable. This bidirectional verification mechanism effectively eliminates false matches caused by occlusion, illumination changes, or image blur, ensuring the accuracy and stability of point feature matching pairs. The resulting point feature matching pairs contain reliable correspondences between consecutive frames, with each matching pair consisting of the coordinate position and displacement vector of the corresponding feature point in the two frames.

[0026] Specifically, the line feature matching process uses the Line Band Descriptor (LBD) descriptor to describe the local appearance characteristics of line segment features. The LBD descriptor is a binary descriptor specifically designed for line segments. It generates a compact binary code by analyzing the gradient direction distribution of pixels surrounding the line segment. In its implementation, for each line segment in the line feature set, a strip region perpendicular to the line segment direction is first established on both sides. The width of this strip region is adaptively determined based on the line segment length and image resolution. Within this strip region, the algorithm divides the region into multiple subregions. The gradient direction distribution of each subregion is obtained using histogram statistics. The gradient direction histogram records the distribution of pixel gradient directions within the subregion, reflecting the texture characteristics and edge structure surrounding the line segment. The algorithm then quantizes the gradient direction histogram of each subregion, mapping the continuous gradient direction values ​​to discrete direction intervals. A binary code is then generated based on the relative relationship of the dominant gradient directions. This encoding method compresses the complex texture information surrounding the line segment into a compact binary vector, preserving the local feature information of the line segment while providing strong noise immunity. By executing this descriptor generation process on all line segments in the line feature set, a corresponding line feature descriptor set is obtained, where each descriptor is represented by a binary vector of fixed length.

[0027] Specifically, the line feature matching process evaluates feature similarity by calculating the Hamming distance between binary descriptor vectors in the line feature descriptor set. The Hamming distance measures the difference between two binary strings of equal length and is calculated by counting the number of bits that differ at corresponding positions in the two binary vectors. For binary encoding like LBD descriptors, the Hamming distance calculation process is very efficient and can be quickly performed using bitwise operations. The algorithm traverses all possible pairs of line feature descriptors and calculates the Hamming distance between them. The smaller the distance, the more similar the local appearance characteristics of the two line segments are. Binary descriptor pairs with similarity below the threshold are eliminated by setting a preset matching threshold. The selection of this threshold requires a balance between matching accuracy and recall. 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 based on the line segment endpoint position change constraint. This geometric constraint check calculates the endpoint position change of the matching line segment pairs, including the displacement distance of the endpoint coordinates and the change ratio of the line segment length. When the geometric variations between two matching line segments between consecutive frames exceed a reasonable range, the matching pair is considered a false match and is rejected. The introduction of geometric constraints can further filter out false matches that are similar in appearance but inconsistent in geometric position, significantly improving the accuracy and reliability of line feature matching. After dual filtering through descriptor similarity screening and geometric constraint verification, high-quality line feature matching pairs are obtained. These matching pairs accurately reflect the correspondence between line segment features between consecutive frames.

[0028] Specifically, data fusion combines point feature matching pairs and line feature matching pairs into a unified point-line fusion feature matching result. This fusion process is not a simple data splicing process; instead, it considers the complementarity and consistency of the two feature types. Point feature matching pairs provide precise pixel-level positional correspondences. These correspondences appear as discrete point-to-point mappings within the image, resulting in high positioning accuracy but relatively sparse spatial distribution. Line feature matching pairs provide geometric constraints on linear structures, which can complement the shortcomings of point features in structured environments, particularly in smooth areas with missing texture. The fusion algorithm verifies the mutual support between point and line feature matching through a spatial consistency check. When the motion trend indicated by a point feature matching matches aligns with that of a nearby line feature matching, the matching results of the two enhance each other's credibility. Conversely, if there is a significant inconsistency, the algorithm downgrades the weight of the corresponding matching pair or marks it as a suspicious match. The fusion process also involves optimizing the spatial distribution of matching pairs to ensure that the final feature matching result has a well-defined spatial distribution within the image and avoid excessive concentration of features in certain localized areas. Through this fusion strategy that comprehensively considers geometric consistency, spatial distribution and feature complementarity, the point-line fusion feature matching results obtained contain rich visual correspondence information, which not only maintains the accuracy of point features but also utilizes the structural constraint capability of line features.

[0029] 103. Perform 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; In one embodiment of the present invention, the key frame selection processing is performed on the multiple frames of left and right target images according to the point-line fusion feature matching results to obtain an optimized key frame sequence, which includes: traversing the multiple frames of left and right target images, determining the current frame image and the last key frame image in the multiple frames of left and right target images, and counting 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 results; taking the number of point feature matches and the number of line feature matches between the current frame image and the last key frame image as the total number of matching features, and counting the number of point feature matches and the number of line feature matches between the current frame image and the last key frame image As the total number of detected features of the current frame, and calculate the ratio of the total number of matching features to the total number of detected features of the current frame to obtain the feature ratio; according to the point feature position change and line feature position change between the current frame image and the last key frame image, calculate the point feature average disparity and the line feature average disparity respectively, and perform weighted summation according to the preset weight coefficient to obtain the comprehensive average disparity; compare and judge the feature ratio with the preset ratio threshold, compare and judge the comprehensive average disparity with the preset disparity threshold, and perform key frame marking processing on the current frame image whose feature ratio is lower than the ratio threshold or the comprehensive average disparity is higher than the disparity threshold to obtain an optimized key frame sequence.

[0030] Specifically, the traversal process proceeds in chronological order, processing each frame sequentially, starting with the first one. During this process, a keyframe record is maintained to store information about images marked as keyframes. The current frame is the target image undergoing keyframe determination, while the last keyframe is the image most recently selected as a keyframe before the current frame. This last-keyframe-based comparison strategy ensures continuity and consistency in keyframe selection, avoiding information loss caused by comparing with frames too old. After determining the current and last keyframes, the algorithm begins counting feature correspondences between the two frames in the point-line fusion feature matching results. This counting process calculates the number of point feature matches and line feature matches, respectively. The point feature match count represents the number of corner features that have successfully established a correspondence between the current and last keyframes, while the line feature match count represents the number of line segment features that have successfully matched between the two frames. This counting process traverses all previously obtained feature match pairs and determines whether the feature points or line segments in the matching pairs belong to the matching frame information.

[0031] Specifically, feature ratio calculation evaluates the feature matching quality of the current frame by analyzing the ratio of matched features to detected features. The total number of matched features is calculated by adding the number of point feature matches and line feature matches between the current frame and the last keyframe, reflecting the total number of feature correspondences established between the two frames. Furthermore, the total number of detected features in the current frame is calculated by adding the number of detected point features and line features in the current frame, representing the total number of features available for matching in the current frame. The total number of detected features in the current frame is derived from the output of the previous feature detection module, including corner features detected by the Shi-Tomasi algorithm and line segment features detected by the LSD algorithm. The feature ratio is calculated by calculating the ratio of the total number of matched features to the total number of detected features in the current frame. This ratio reflects the proportion of features in the current frame that can be reliably matched with the last keyframe. The feature ratio directly reflects the similarity between the current frame and the last keyframe. A higher feature ratio indicates minimal viewpoint change between the two frames, with most features remaining visible and matchable. A lower feature ratio indicates significant viewpoint change, occlusion, or the appearance of new areas between the two frames, indicating that the current frame contains a large amount of new visual information.

[0032] Specifically, the disparity calculation process processes the positional changes of point features and line features separately to quantify the degree of spatial change between the current frame and the last keyframe. The average disparity of point features is calculated based on the pixel displacement distance between the two frames for each feature point in a matching point feature pair. The disparity value of each feature point is obtained by calculating the Euclidean distance of the matching point pairs on the image plane. The average disparity of all point features is then arithmetic averaged. This average value reflects the overall displacement of the point features in the image due to camera motion. A large average disparity of point features indicates a significant change in the camera's position or attitude. The calculation of the average disparity of line features is more complex and requires comprehensive consideration of both the positional and directional changes of the line segments. The algorithm separately calculates the change in the midpoint coordinates and the change in the directional angle of the matching line segment pairs. The midpoint coordinate change is obtained by calculating the Euclidean distance between the corresponding line segment midpoints in the two frames, while the directional angle change is obtained by calculating the angle between the line segment directional vectors. The midpoint coordinate change and the angle change are linearly combined according to the preset proportional coefficient to obtain the comprehensive change of each line feature, and then the comprehensive change of all line features is arithmetic averaged to obtain the average disparity of the line features. This method of comprehensively considering the changes in position and direction can more comprehensively reflect the changes in line features in space. Finally, the average disparity of the point features is multiplied by the preset point feature weight coefficient, the average disparity of the line features is multiplied by the preset line feature weight coefficient, and the two products are added together to obtain the comprehensive average disparity. The setting of the weight coefficient reflects the relative importance of point features and line features in disparity evaluation. This weight distribution takes into account the accuracy characteristics and environmental adaptability of the two feature types.

[0033] Specifically, the keyframe determination process uses a dual threshold comparison to determine whether the current frame should be selected as a keyframe. The calculated feature ratio is compared with a preset ratio threshold. When the feature ratio is below the ratio threshold, it indicates that the current frame shares few features with the last keyframe and contains a significant amount of new visual information, thus qualifying the current frame as a keyframe. Setting the feature ratio threshold requires a balance between keyframe information gain and computational efficiency. A too low threshold results in an overly dense keyframe, increasing the computational burden, while a too high threshold can lead to the loss of important visual information. Simultaneously, the comprehensive average disparity is compared with a preset disparity threshold. When the comprehensive average disparity exceeds the disparity threshold, it indicates significant spatial variation between the current frame and the last keyframe, indicating sufficient camera motion, allowing the current frame to provide new spatial constraint information. The disparity threshold is set based on the typical amplitude of camera motion and the accuracy requirements of 3D reconstruction. Appropriate disparity variation is beneficial to the accuracy and stability of triangulation. If the current frame meets either the feature ratio below the ratio threshold or the comprehensive average disparity above the disparity threshold, the algorithm marks the frame as a keyframe. This logical OR strategy ensures flexibility in keyframe selection, accounting for both variations in feature matching quality and the magnitude of spatial motion. Through continuous iteration and judgment, an optimized keyframe sequence is gradually constructed. This sequence contains image frames that are representative and have high information gain, ensuring the integrity of visual information while controlling computational complexity.

[0034] Furthermore, the point feature average disparity and line feature average disparity are calculated respectively based on the point feature position change and line feature position change between the current frame image and the last key frame image, and the weighted summation is performed according to the preset weight coefficient to obtain the comprehensive average disparity, including: calculating the pixel displacement distance of each point feature on the image plane according to the point feature matching pairs between the current frame image and the last key frame image, and performing arithmetic averaging on the pixel displacement distances of all point features to obtain the point feature average disparity; calculating the changes in the midpoint coordinates and the direction angle of the line segment according to the line feature matching pairs between the current frame image and the last key frame image, linearly combining the midpoint coordinate changes and the angle changes according to a preset proportional coefficient to obtain the comprehensive changes of each line feature; performing arithmetic averaging on the comprehensive changes of each line feature to obtain the line feature average disparity; multiplying the point feature average disparity with the preset point feature weight coefficient, multiplying the line feature average disparity with the preset line feature weight coefficient, and adding the two multiplication results to obtain the comprehensive average disparity.

[0035] Specifically, disparity calculation involves analyzing each point feature correspondence established between the current frame and the last keyframe image. For each point feature match, the algorithm extracts the pixel coordinates of the corresponding feature point in both frames, including the coordinate position in the last keyframe and the coordinate position in the current frame. The pixel displacement distance is calculated by taking the Euclidean distance between the two coordinate points. This calculation involves taking the square root of the sum of the squares of the coordinate differences and then performing a square root operation. This two-dimensional distance calculation directly reflects the magnitude of the spatial displacement of the feature point in the image. A larger distance indicates a more significant positional change between the two frames. The pixel displacement distance for each point feature is a scalar value, measured in pixels, that directly reflects the change in the feature point's image projection position caused by camera motion or scene changes. After obtaining the pixel displacement distances for all point feature match pairs, the algorithm calculates the arithmetic average of these distance values. The sum is then divided by the total number of match pairs to obtain the average disparity of the point features. This average eliminates the influence of individual feature point displacements, reflects the overall displacement trend, and provides a reliable statistical indicator for assessing the magnitude of camera motion. The size of the point feature average disparity is directly related to the degree of translation and rotation of the camera. A larger average disparity indicates that the camera has undergone significant position or posture changes between two frames.

[0036] Specifically, disparity calculation for line features is more complex than for point features because line segment features contain both positional and directional information, requiring comprehensive consideration of changes in both aspects. The algorithm first calculates the change in the coordinates of the line segment's midpoint. For each matching pair of line features, the coordinates of the corresponding line segment's endpoints in both frames are extracted. The midpoint of the line segment is determined by averaging these coordinates. The midpoint represents the segment's spatial position within the image, and changes in its coordinates reflect the overall translational motion of the segment. The change in midpoint coordinates is calculated by calculating the Euclidean distance between the corresponding line segment midpoints in the two frames. This calculation is similar to the displacement distance calculation for point features, but focuses on the change in the overall position of the line segment rather than the change in a single point. The algorithm then calculates the change in the segment's orientation angle. The segment's orientation angle is determined by the segment's orientation vector, which can be calculated from the coordinates of its two endpoints. The orientation angle is typically expressed in radians or degrees, representing the angle between the line segment and an axis of the image coordinate system. The angle change is calculated by taking the difference between the corresponding segment's orientation angles in the two frames. This angle difference calculation needs to take into account the periodicity of the angle to ensure that the calculation result is within a reasonable angle range. The change in the line segment direction angle reflects the impact of the camera rotation on the projection direction of the linear feature. A large angle change indicates that the camera has undergone significant rotation.

[0037] Specifically, the midpoint coordinate change and the angular change are linearly combined using preset scaling factors to obtain the comprehensive change for each line feature. This linear combination strategy unifies positional and directional changes into the same metric space, enabling meaningful comparison and fusion of changes of different natures. The scaling factor must consider the dimensional differences between positional and angular changes and their relative importance in disparity assessment. Positional change is measured in pixels, while angular change is measured in radians or degrees, with different numerical ranges and physical meanings. By appropriately setting the scaling factor, angular changes can be converted to equivalent positional changes, and vice versa, allowing comparison of both types of changes at the same scale. The linear combination calculation multiplies the midpoint coordinate change by the first scaling factor and the angular change by the second scaling factor. The two products are then added together to obtain the comprehensive change for the line feature. This comprehensive change reflects both the positional and directional changes of a line segment, providing a more comprehensive description of line feature changes than a single metric alone. After performing this comprehensive change calculation for all matching pairs of line features, the algorithm takes the arithmetic average of the comprehensive changes for each line feature. This sum is divided by the total number of matching pairs to obtain the average disparity of the line features. This average eliminates the interference of abnormal changes in individual line features and provides a statistical description of the overall trend of linear feature changes.

[0038] Specifically, the comprehensive average disparity is calculated by weighted fusion of the average disparity of point features and the average disparity of line features. The average disparity of point features is multiplied by a preset point feature weight coefficient to obtain the weighted contribution of the point feature to the comprehensive disparity. The point feature weight coefficient reflects the importance and reliability of the point feature in camera motion estimation. Due to their precise pixel-level positioning capabilities, point features have high sensitivity and accuracy in detecting translational motion, and therefore play a key role in disparity calculation. Simultaneously, the average disparity of line features is multiplied by a preset line feature weight coefficient to obtain the weighted contribution of line features to the comprehensive disparity. The line feature weight coefficient reflects the line feature's ability to impose geometric constraints in structured environments and its sensitivity to rotational motion. Due to their directional information, line features have a unique advantage in detecting camera rotational motion, especially in environments with a lack of texture but rich linear structure. The two weighted contributions are summed to obtain the comprehensive average disparity, which comprehensively reflects the overall spatial change between the current frame and the last keyframe. The weighting coefficients are set based on the performance of point and line features under different environmental conditions. Point features can be given a higher weight in texture-rich environments, while line features can be given a higher weight in highly structured environments. The comprehensive average disparity is an important factor in keyframe selection, and its value directly affects the frequency and quality of keyframe selection. Setting an appropriate disparity threshold ensures that keyframes contain sufficient new information while maintaining a reasonable selection density.

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

[0040] In one embodiment of the present invention, the three-dimensional reconstruction processing of the point and line features in the optimized key frame sequence and the fusion optimization processing with the inertial measurement unit data to obtain the robot visual recognition and positioning result includes: triangulating the point features in the optimized key frame sequence through singular value decomposition, solving the three-dimensional coordinates of the point features in the world coordinate system, and obtaining the point feature spatial position; converting the line features in the optimized key frame sequence into Plücker coordinate representation, determining the spatial straight line equation of the line features in the world coordinate system through the normal vector and direction vector of the Plücker coordinates, and obtaining the line feature spatial geometry; calculating the reprojection error according to the point feature spatial position and the line feature spatial geometry to construct the visual residual term, calculating the pre-integration error according to the inertial measurement unit data to construct the inertial residual term, and combining the visual residual term and the inertial residual term to obtain the overall cost function; performing nonlinear minimization on the overall cost function in a sliding window, iteratively optimizing the position and posture parameters of the robot, and obtaining the robot visual recognition and positioning result.

[0041] Specifically, the algorithm optimizes the triangulation of point features in the keyframe sequence, using the singular value decomposition method to determine the 3D coordinates of the point features in the world coordinate system. The basic principle of triangulation is to reconstruct the 3D position of the point using image projections of the same spatial point from multiple viewpoints. This process requires known camera intrinsic and extrinsic parameters. For each point feature in the optimized keyframe sequence, the algorithm first collects observation information of the feature across different keyframes, including the pixel coordinates of the feature point in each frame and the corresponding camera pose. The camera pose describes the position and orientation of the camera in the world coordinate system and is typically represented by a rotation matrix and a translation vector. Based on the camera's projection model, 3D points in space are projected onto the image plane to form pixels. This projection process can be represented by a system of linear equations. However, due to image noise and feature detection errors, these linear equations are often overdetermined and not completely consistent. The singular value decomposition (SVD) method can solve this overdetermined linear system in the least squares sense, finding the solution that minimizes the residual error by decomposing the coefficient matrix. The SVD algorithm decomposes the coefficient matrix into the product of three matrices, which contain the matrix's singular values ​​and corresponding singular vectors. The right singular vector corresponding to the smallest singular value provides the optimal solution to the system of equations. The spatial position of the point feature calculated in this way is the 3D coordinate of the feature point in the world coordinate system. These coordinates form a sparse 3D point cloud representation of the environment.

[0042] Specifically, 3D reconstruction of line features uses the Plücker coordinate system to represent spatial lines. This representation uniquely and compactly describes the geometry of lines in 3D space. Plücker coordinates are a homogeneous coordinate system that represents a line in 3D space using six parameters: the normal vector and the components of the direction vector. The normal vector is perpendicular to the plane containing the origin and the line, while the direction vector lies along the line. Converting line features from the optimized keyframe sequence into Plücker coordinates requires utilizing observation information of the line features from multiple viewpoints. For each spatial line, the algorithm collects image projections of the line from different keyframes. These projections represent line segment features in the image. The projection of the spatial line onto the image plane is a straight line, and the equation of this projected line can be expressed in terms of the line's endpoint coordinates or slope-intercept form. The projection geometry of the camera can be used to establish constraint equations between the spatial line and its image projection. Leveraging the projection constraints from multiple viewpoints, the algorithm reconstructs the Plücker coordinate parameters of the spatial line. The calculation of Plücker coordinates involves solving a constrained optimization problem that requires satisfying both the intrinsic constraints of the Plücker coordinates and the extrinsic constraints of multi-view projection. The normal and direction vectors of the Plücker coordinates can be used to determine the spatial line equation of the line feature in the world coordinate system. This equation fully describes the position and orientation of the line in three-dimensional space. The resulting spatial geometric information of the line feature provides structured geometric constraints for the environment, which are particularly valuable in texture-deficient environments.

[0043] Specifically, the visual-inertial residual term is constructed based on the calculation of reprojection error and pre-integration error. The visual residual term is constructed by calculating the reprojection error of 3D features at the current estimated pose. This process geometrically projects the spatial positions of point features and line features back onto the image plane and then calculates the difference between the projected positions and the actual observed positions. For point features, the reprojection error is the Euclidean distance between the projected point and the observed point, reflecting the accuracy of the current pose estimate. For line features, the reprojection error is calculated by calculating the geometric distance between the projected line and the observed line segment. The specific calculation methods include the point-to-line distance and the angular deviation between the lines. The magnitude of the reprojection error directly reflects the consistency between the visual observation and the current pose estimate; a smaller reprojection error indicates a more accurate pose estimate. The inertial residual term is constructed based on the acceleration and angular velocity data provided by the inertial measurement unit (IMU). The relative motion between adjacent keyframes is calculated using IMU pre-integration technology. IMU pre-integration is a technique that integrates high-frequency inertial measurement data into low-frequency constraints. This method effectively addresses the mismatch between the visual observation frequency and the inertial measurement frequency. The pre-integration process integrates all IMU measurements between adjacent keyframes into a relative motion constraint, including position, velocity, and attitude changes. The pre-integration error is calculated by comparing the relative motion predicted by the IMU with the relative motion estimated by the visual method. This error reflects the consistency between the inertial and visual estimates. The visual and inertial residual terms are combined to form an overall cost function that comprehensively considers the constraints from both visual observations and inertial measurements.

[0044] Specifically, the sliding window optimization technique ensures both computational efficiency and positioning accuracy. The sliding window maintains a fixed-size set of keyframes. As new keyframes are added, the oldest keyframes are removed from the window. This mechanism ensures that the size of the optimization problem remains manageable. Within the sliding window, the algorithm performs a nonlinear minimization of the overall cost function. This process employs iterative optimization methods such as the Levenberg-Marquardt or Gauss-Newton algorithms. The goal of nonlinear optimization is to find a set of robot position and attitude parameters that minimizes the overall cost function, which is equivalent to minimizing the combined error between visual observation and inertial measurement. The iterative optimization process begins with an initial pose estimate and determines the direction and step size of parameter updates by calculating the gradient of the cost function and the Hessian matrix. Each iteration updates the robot's position coordinates, attitude angles, and related velocity and bias parameters until convergence criteria are met. Convergence criteria include the magnitude of the change in the cost function, the magnitude of the parameter update, and the magnitude of the gradient. Various constraints must also be addressed during the optimization process, including orthogonality constraints on the rotation matrix and bounds on the IMU biases. Through this iterative optimization process, the algorithm gradually refines the robot's position and pose estimates, ultimately producing a visual recognition and positioning result. This result includes the robot's precise position coordinates and pose information at each key frame, as well as 3D reconstructions of feature points and lines in the environment, providing reliable spatial reference information for the robot's autonomous navigation and environmental understanding.

[0045] Furthermore, the reprojection error is calculated based on the point feature space position and the line feature space geometry to construct a visual residual term, the pre-integration error is calculated based on the inertial measurement unit data to construct an inertial residual term, and the visual residual term and the inertial residual term are combined to obtain an overall cost function, which includes: projecting the point feature space position onto the image plane according to the current robot posture parameters, and calculating the Euclidean distance between the projection position and the actual observation position as the point feature reprojection error; projecting the line feature space geometry onto the image plane according to the current robot posture parameters, and calculating the point-to-line distance between the projection line and the actual observation line segment as the line feature reprojection error; and squaring the point feature reprojection error and the line feature reprojection error respectively. , and perform weighted summation calculation according to the preset visual weight matrix to obtain the visual residual term; perform numerical integration calculation according to the acceleration measurement value and angular velocity measurement value of the inertial measurement unit between adjacent key frames to obtain the IMU-predicted posture change, perform difference calculation on the IMU-predicted posture change and the visually estimated posture change to obtain the inertial pre-integration error; perform covariance matrix normalization on the inertial pre-integration error, and perform weighted calculation according to the preset inertia weight coefficient to obtain the inertial residual term; perform linear combination calculation on the visual residual term and the inertial residual term according to the preset fusion weight coefficient, and add the regularization constraint term of the robot's prior posture information to obtain the overall cost function.

[0046] Specifically, the reprojection error requires a 3D-to-2D coordinate transformation using the current robot pose parameters and camera intrinsics. Projecting the spatial position of a point feature onto the image plane based on the current robot pose parameters involves a series of coordinate system transformations. First, the 3D point coordinates in the world coordinate system are transformed into the camera coordinate system. This transformation is achieved using a rotation matrix and a translation vector. The rotation matrix describes the camera's orientation, while the translation vector describes the camera's spatial position. In the camera coordinate system, the 3D point is projected onto the image plane using a perspective projection model. Perspective projection takes into account intrinsic parameters such as the camera's focal length, principal point coordinates, and lens distortion. The pixel coordinates calculated from the projection represent the theoretical observed position of the 3D point feature at the current pose estimate. The Euclidean distance between the projected position and the actual observed position is then calculated as the point feature reprojection error. The actual observed position is derived from the coordinates of the feature points identified in the image by the feature detection algorithm. The Euclidean distance is calculated by taking the square root of the sum of the differences in the x and y directions between the projected and observed coordinates. This distance directly reflects the degree of consistency between the current pose estimate and the actual observation. A smaller reprojection error indicates that the pose estimation is more accurate, while a larger error indicates the presence of pose estimation bias or observation noise.

[0047] Specifically, the calculation of line feature reprojection error is relatively complex because it requires processing the projection relationship of the 3D line onto the image plane. Projecting the line feature's spatial geometry onto the image plane based on the current robot pose parameters requires considering the Plücker coordinate representation of the spatial line and the geometric properties of perspective projection. A spatial line is projected onto the image plane through perspective projection. The equation of this projected line can be determined by projecting two points onto the line or by directly transforming the Plücker coordinates. The projected line appears in the image as a line equation with a specific slope and intercept, whose parameters are completely determined by the geometric properties of the spatial line and the camera pose parameters. The line feature reprojection error is calculated as the point-to-line distance between the projected line and the actual observed line segment. The actual observed line segment is derived from the line segment features identified in the image by the line feature detection algorithm. The calculation of the point-to-line distance involves calculating the perpendicular distance between the line segment endpoints and the projected line. Since line segments have finite length, it is also necessary to consider whether the projection of the endpoints onto the projected line falls within a reasonable range. The line feature reprojection error is typically expressed as the distance between the line segment midpoint and the projected line, or as the average of the distances between the line segment endpoints and the projected line. This error measurement method can comprehensively reflect the position and direction deviation of line features, and provide a quantitative indicator for the quality evaluation of line feature constraints.

[0048] Specifically, the visual residual term is constructed by mathematically processing the reprojection errors of point and line features. These reprojection errors are squared separately. This squaring operation aims to eliminate the influence of the error sign, ensuring that all errors contribute positively to the cost function. It also amplifies the impact of larger errors, allowing the optimization process to prioritize observations with larger deviations. The squared errors are then weighted and summed according to a preset visual weight matrix, which reflects the reliability and importance of different feature observations. The weights assigned to point and line features take into account their different characteristics in terms of localization accuracy and environmental adaptability. Point features offer higher precision in positional constraints, while line features provide important geometric constraints in structured environments. The weight matrix can also be dynamically adjusted based on feature detection quality, matching confidence, and observation conditions, ensuring that high-quality feature observations play a more prominent role in the optimization process. The weighted summation of the weighted squared errors of all features yields the visual residual term, which comprehensively reflects the overall consistency between the current pose estimate and all visual observations.

[0049] Specifically, the inertial residual term is constructed based on a comparative analysis of IMU pre-integration techniques and pose variations. Numerical integration is performed on the acceleration and angular velocity measurements of the inertial measurement unit (IMU) between adjacent keyframes. This process integrates high-frequency IMU measurements into relative motion constraints between keyframes. Numerical integration must account for IMU bias, noise characteristics, and the influence of the gravity field. Numerical methods such as the midpoint method or the fourth-order Runge-Kutta method are typically used to improve accuracy. The results of IMU pre-integration include the relative position, velocity, and attitude changes between adjacent keyframes. These changes constitute the IMU-predicted pose variation. Simultaneously, a visual estimation method is used to calculate the pose variation between adjacent keyframes. This estimation is based on visual feature matching and solving geometric constraints. The difference between the IMU-predicted pose variation and the visually estimated pose variation is calculated to produce the inertial pre-integration error, which reflects the consistency of motion prediction between the inertial measurement and the visual estimation. The pre-integration error consists of position, velocity, and attitude error components, each of which reflects the estimated deviation of the corresponding motion parameter. The inertial pre-integration error is normalized using the covariance matrix. The covariance matrix describes the statistical characteristics of IMU measurement noise and the correlation between different error components. Normalization converts the error to a standardized space through the inverse matrix transformation of the covariance matrix, allowing errors of different types and magnitudes to be compared and optimized on the same scale. The inertial residual term is weighted according to the preset inertia weight coefficient, which reflects the importance and reliability of the IMU measurement in the overall positioning.

[0050] Specifically, the overall cost function is constructed by fusing visual and inertial residual terms to achieve comprehensive optimization of multimodal information. These two terms are linearly combined using preset fusion weights. The fusion weights balance the contributions of visual and inertial information under different motion states and environmental conditions. The visual residual term is weighted more heavily in good visual conditions, while in visually degraded environments, inertial information can be relied upon more heavily. This linear combination ensures that the two information sources work together and complement each other's shortcomings. In addition to the visual and inertial residual terms, the algorithm also adds regularization constraints based on the robot's prior pose information. This prior information comes from previous localization results, motion model predictions, or external localization sources. The regularization constraints prevent the optimization process from deviating from the reasonable solution space. In particular, when observation information is insufficient or ambiguous, the prior constraints can guide the optimization towards more likely solutions. The weight of the regularization term must balance the credibility of the prior information with the importance of the current observation. Excessive regularization can limit the algorithm's ability to adapt to new information, while too weak regularization can lead to unstable optimization results.

[0051] In this embodiment, a stereo camera captures multiple frames of left and right target images, performs dual feature detection on the images, and simultaneously extracts point and line features to form complementary feature information. The point and line features are then matched and optimized to obtain high-quality feature matching results. Keyframes are selected based on the point-line fusion feature matching results to reduce redundant calculations. The point and line features in the keyframes are three-dimensionally reconstructed and fused and optimized with inertial measurement unit data to obtain accurate robot position and posture information. By fusing point and line features, the present invention effectively addresses the limitations of relying solely on point features in texture-deficient environments, significantly improving the accuracy and robustness of robot visual positioning. It is suitable for autonomous robot navigation in texture-deficient and rugged terrain environments.

[0052] The above describes the visual recognition and positioning method of the robot in the embodiment of the present invention. The following describes the visual recognition and positioning device of the robot in the embodiment of the present invention. Figure 2 In one embodiment of the present invention, a robot visual recognition and positioning device includes: The feature detection module 201 is used to perform dual feature detection processing on multiple frames of left and right target images continuously captured by the stereo camera installed on the robot to obtain a point feature set and a line feature set; A feature matching module 202 is configured to perform feature matching optimization processing on the point feature set and the line feature set according to preset screening conditions to obtain a point-line fusion feature matching result; A key frame selection module 203 is configured to perform 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; The fusion positioning module 204 is used to perform three-dimensional reconstruction processing on the point and line features in the optimized key frame sequence, and perform fusion optimization processing with the inertial measurement unit data to obtain the robot visual recognition positioning result.

[0053] In an embodiment of the present invention, the robot's visual recognition and positioning device runs the above-mentioned robot's visual recognition and positioning method, and the robot's visual recognition and positioning device collects multiple frames of left and right target images through a stereo camera, performs dual feature detection on the images, and simultaneously extracts point features and line features to form complementary feature information; then performs matching optimization processing on the point and line features to obtain high-quality feature matching results; performs key frame selection based on the point and line fusion feature matching results to reduce redundant calculations; performs three-dimensional reconstruction on the point and line features in the key frames, and fuses and optimizes them with the inertial measurement unit data to obtain accurate robot position and posture information. The present invention effectively solves the limitations of relying solely on point features in texture-deficient environments by fusing point and line features, significantly improves the accuracy and robustness of robot visual positioning, and is suitable for robot autonomous navigation in texture-deficient and rugged terrain environments.

[0054] above Figure 2 The visual recognition and positioning device of the robot in the embodiment of the present invention is described in detail from the perspective of modular functional entities. The visual recognition and positioning device of the robot in the embodiment of the present invention is described in detail from the perspective of hardware processing.

[0055] Figure 3 This is a schematic diagram of the structure of a robot visual recognition and positioning device provided by an embodiment of the present invention. The robot visual recognition and positioning device 300 may vary significantly depending on configuration or performance. It may include one or more processors (central processing units, CPUs) 310 (e.g., one or more processors), memory 320, and one or more storage media 330 (e.g., one or more mass storage devices) storing application programs 333 or data 332. The memory 320 and storage medium 330 may be either transient or persistent storage. The program stored in the storage medium 330 may include one or more modules (not shown), each of which may include a series of instructions for operating on the robot visual recognition and positioning device 300. Furthermore, the processor 310 may be configured to communicate with the storage medium 330, executing the series of instructions stored in the storage medium 330 on the robot visual recognition and positioning device 300 to implement the steps of the aforementioned robot visual recognition and positioning method.

[0056] The robot's visual recognition and positioning device 300 may also 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 Server, Mac OS X, Unix, Linux, FreeBSD, etc. It will be understood by those skilled in the art that Figure 3The structure of the robot's visual recognition and positioning device shown does not constitute a limitation on the robot's visual recognition and positioning device provided by the present invention, and may include more or fewer components than shown in the figure, or a combination of certain components, or a different arrangement of components.

[0057] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described systems, devices, and units can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0058] As described above, the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that the technical solutions described in the above embodiments can still be modified, or some of the technical features thereof can be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A robot visual recognition and positioning method, characterized in that: The robot's visual recognition and positioning method includes: Perform dual feature detection on multiple frames of left and right target images continuously captured by the stereo camera installed on the robot to obtain a point feature set and a line feature set; Performing feature matching optimization processing on the point feature set and the line feature set according to preset screening conditions to obtain a point-line fusion feature matching result; Performing key frame selection processing on the multiple frames of left and right target images according to the point-line fusion feature matching results to obtain an optimized key frame sequence; The point and line features in the optimized key frame sequence are subjected to three-dimensional reconstruction processing, and are fused and optimized with the inertial measurement unit data to obtain the robot visual recognition and positioning result.

2. The robot visual recognition and positioning method according to claim 1, characterized in that: The dual feature detection process is performed on multiple frames of left and right target images continuously captured by the stereo camera installed on the robot to obtain a point feature set and a line feature set, including: Calculate the pixel gradients of multiple frames of left and right target images continuously captured by the stereo camera installed on the robot, construct a gradient matrix based on the pixel gradients, and calculate the eigenvalues ​​of the gradient matrix; Taking the minimum value of the characteristic values ​​as the corner point response strength, and screening the pixel points whose corner point response strength is greater than a preset strength threshold to obtain the point feature set; Calculating a gradient angle based on the pixel gradient, and performing region growing and line fitting on pixels whose gradient angles are greater than a preset similarity threshold to obtain an original line feature set; A length threshold is determined according to the product of the minimum image resolution and a preset scaling factor, and line segments in the original line feature set whose length is less than the length threshold are filtered to obtain a line feature set.

3. The robot visual recognition and positioning method according to claim 1, characterized in that: The performing feature matching optimization processing on the point feature set and the line feature set according to the preset screening conditions to obtain the point-line fusion feature matching result includes: Calculate the displacement vector of the point feature set between consecutive frames through forward and backward optical flow consistency test to obtain point feature matching pairs; Performing statistical coding on pixel gradient directions around the line feature set to generate a binary descriptor vector, thereby obtaining a line feature descriptor set; Calculating the Hamming distance between binary descriptor vectors in the line feature descriptor set as similarity, eliminating binary descriptor pairs with similarity lower than a preset matching threshold, and performing secondary screening in combination with the line segment endpoint position change constraint to obtain line feature matching pairs; The point feature matching pairs and the line feature matching pairs are subjected to data fusion processing to obtain a point-line fusion feature matching result.

4. The robot visual recognition and positioning method according to claim 1, characterized in that: The 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 includes: 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 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; The number of point feature matches and line feature matches between the current frame image and the last key frame image is taken as the total number of matched features, the number of point feature matches and line feature matches of the current frame image is taken as the total number of detected features of the current frame, and the ratio of the total number of matched features to the total number of detected features of the current frame is calculated to obtain the feature ratio; Calculating the average disparity of point features and the average disparity of line features based on the position changes of point features and line features 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; The feature ratio is compared with a preset ratio threshold, the comprehensive average disparity is compared with a preset disparity threshold, and a key frame marking process is performed on the current frame image whose feature ratio is lower than the ratio threshold or the comprehensive average disparity is higher than the disparity threshold to obtain an optimized key frame sequence.

5. The robot visual recognition and positioning method according to claim 4, characterized in that: The step of calculating the point feature average disparity and the line feature average disparity respectively based on 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 the comprehensive average disparity includes: Calculating the pixel displacement distance of each point feature on the image plane based on the point feature matching 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 changes in the midpoint coordinates and the direction angle of the line segment based on the line feature matching pairs between the current frame image and the last key frame image, and linearly combining the midpoint coordinate changes and the angle changes according to a preset proportional coefficient to obtain a comprehensive change in each line feature; Performing arithmetic average calculation on the comprehensive changes of the line features to obtain the average parallax of the line features; 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 multiplication results are added to obtain the comprehensive average disparity.

6. The robot visual recognition and positioning method according to claim 1, characterized in that: The three-dimensional reconstruction of the point and line features in the optimized key frame sequence and the fusion optimization processing with the inertial measurement unit data to obtain the robot visual recognition and positioning results include: Performing triangulation calculation on the point features in the optimized key frame sequence by singular value decomposition, solving the three-dimensional coordinates of the point features in the world coordinate system, and obtaining the spatial position of the point features; Converting the line features in the optimized key frame sequence into Plücker coordinates, determining the spatial straight line equation of the line features in the world coordinate system through the normal vector and direction vector of the Plücker coordinates, and obtaining the spatial geometry of the line features; A visual residual term is constructed by calculating a reprojection error based on the point feature space position and the line feature space geometry, an inertial residual term is constructed by calculating a pre-integration error based on the inertial measurement unit data, and the visual residual term and the inertial residual term are combined to obtain an overall cost function; The overall cost function is nonlinearly minimized within a sliding window, and the position and posture parameters of the robot are iteratively optimized to obtain the robot visual recognition and positioning result.

7. The robot visual recognition and positioning method according to claim 6, characterized in that: The visual residual term is constructed by calculating the reprojection error according to the point feature space position and the line feature space geometry, the inertial residual term is constructed by calculating the pre-integration error according to the inertial measurement unit data, and the overall cost function is obtained by combining the visual residual term and the inertial residual term. Projecting the spatial position of the point feature onto the image plane according to the current robot posture parameters, and calculating the Euclidean distance between the projected position and the actual observed position as the point feature reprojection error; Projecting the line feature space geometry onto the image plane according to the current robot pose parameters, and calculating the point-to-line distance between the projected line and the actual observed line segment as the line feature reprojection error; The point feature reprojection error and the line feature reprojection error are squared respectively, and weighted summed according to a preset visual weight matrix to obtain the visual residual term; Numerical integration is performed on the acceleration and angular velocity measurements of the inertial measurement unit between adjacent key frames to obtain the pose change predicted by the IMU, and the pose change predicted by the IMU is calculated as a difference with the pose change estimated by vision to obtain the inertial pre-integration error; Performing covariance matrix normalization processing on the inertia pre-integration error, and performing weighted calculation according to a preset inertia weight coefficient to obtain the inertia residual term; The visual residual term and the inertial residual term are linearly combined and calculated according to a preset fusion weight coefficient, and a regularization constraint term of the robot's prior posture information is added to obtain an overall cost function.

8. A robot visual recognition and positioning device, characterized in that: The robot's visual recognition and positioning device includes: The feature detection module is used to perform dual feature detection on multiple frames of left and right target images continuously captured by the stereo camera installed on the robot to obtain a point feature set and a line feature set; A feature matching module is used to perform feature matching optimization processing on the point feature set and the line feature set according to preset screening conditions to obtain a point-line fusion feature matching result; A key frame selection module is used to perform key frame selection processing on the multiple frames of left and right target images according to the point-line fusion feature matching results to obtain an optimized key frame sequence; The fusion positioning module is used to perform three-dimensional reconstruction processing on the point and line features in the optimized key frame sequence, and to perform fusion optimization processing with the inertial measurement unit data to obtain the robot visual recognition positioning result.

9. A robot visual recognition and positioning device, characterized in that: The robot's visual recognition and positioning device includes: a memory and at least one processor, wherein the memory stores instructions; The at least one processor calls the instructions in the memory to enable the robot's visual recognition and positioning device to perform the steps of the robot's visual recognition and positioning method as described in any one of claims 1 to 7.

Images