Robot vision inertial navigation method and device and storage medium

Abstract

The invention discloses a robot vision inertial navigation method and device and a storage medium, and belongs to the technical field of mobile robot navigation, and the navigation method comprises the following steps: measuring motion information of a mobile robot by using an IMU sensor; collecting image data of the mobile robot by using a camera; inputting the feature points and the descriptors into a LightGlue network model to obtain a local feature point matching relation of the image; calibrating the offset of the gyroscope by optimizing and recovering visual scale information, and estimating the IMU coordinate system speed, the gravitational acceleration and the offset of the IMU; introducing a visual feature re-projection error, an IMU pre-integration error and marginalized residual constraint, and then integrally converting into a target function to obtain a nonlinear optimization result; further repositioning is carried out to obtain a repositioning result; and optimizing the global pose map to obtain a final global pose map optimization result, and carrying out navigation operation according to the global pose map optimization result. The method can improve the positioning precision and operation efficiency under view angle change and illumination change, and is high in practicality.

Description

Technical Field

[0001] The invention relates to a robot visual inertial navigation method, a device and a storage medium, and belongs to the technical field of mobile robot navigation. Background Art

[0002] In recent years, with the rapid development of science and technology, more and more high-tech products have been used in people's daily lives, bringing great convenience to people's production and life, especially the positioning and navigation function of mobile robots in unknown environments can provide people with great help. The precise positioning and accurate pose estimation of mobile robots play a very important role in the fields of unmanned driving and aerospace.

[0003] Since a single navigation system is difficult to meet the needs of actual navigation applications, positioning services in comprehensive environments require a positioning technology that is highly practical and stable. Research has found that the integration of visual navigation and inertial navigation technology can complement each other's advantages and gradually develop into a research hotspot in the current field of autonomous navigation and robotics.

[0004] The combination of inertial navigation and visual positioning has the advantages of strong concealment, strong anti-interference, and strong autonomy, and has significant advantages in complex environments such as elevated roads, tunnels, and urban canyons. However, the traditional visual inertial system has large pose estimation errors and long running time in environments with changing perspectives and lighting, resulting in low positioning accuracy and low operating efficiency, so it is not very practical. Summary of the invention

[0005] The technical problem to be solved by the present invention is to overcome the defects of the prior art and provide a robot visual inertial navigation method, which can improve the positioning accuracy and operating efficiency under the change of viewing angle and illumination, and has strong practicality.

[0006] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0007] In a first aspect, the present invention provides a robot visual inertial navigation method, comprising the following steps:

[0008] The IMU sensor is used to measure the motion information of the mobile robot, and the position, speed and posture data are obtained by IMU pre-integration processing of the motion information;

[0009] The image data of the mobile robot is collected by a camera and input into the Aliked network model to obtain feature points and the descriptors corresponding to each feature point.

[0010] Input feature points and descriptors into the LightGlue network model to obtain the matching relationship of local feature points of the image;

[0011] Perform IMU pre-integration calculation on the position information, velocity and attitude data to obtain the IMU pre-integration result, and use the matching relationship between feature points and local feature points, combined with the SFM (Structure from Motion) method to perform pure visual estimation on all image frames of the image data in the sliding window, calculate the position and 3D point coordinates of each frame, and obtain the visual estimation result. Then align the visual estimation result with the IMU pre-integration result, restore the visual scale information through optimization, calibrate the offset of the gyroscope, and estimate the IMU coordinate system velocity, gravity acceleration and IMU bias;

[0012] The visual scale information, gyroscope offset, IMU coordinate system velocity, gravity acceleration and IMU bias are introduced into the visual feature reprojection error, IMU pre-integration error and marginalized residual constraint, and then the whole is transformed into an objective function, and the objective function is nonlinearly optimized to obtain a nonlinear optimization result;

[0013] The tightly coupled relocalization module is combined with the monocular visual inertial odometer to further relocalize the nonlinear optimization results and obtain the relocalization results.

[0014] The relocalization result is optimized globally to obtain the final global pose graph optimization result, and navigation operations are performed based on the global pose graph optimization result.

[0015] The formula for IMU pre-integration in obtaining position, velocity and attitude data by IMU pre-integration processing motion information is:

[0016]

[0017]

[0018]

[0019] in, , , Respectively represent the discrete moments The estimated position, velocity and attitude at the time , , Respectively represent the discrete moments The estimated position, velocity and attitude at the time Indicates that at discrete moments Through quaternions The calculated rotation matrix is and Indicates that at discrete moments The linear acceleration data and angular velocity data measured by the IMU, and Represents the bias of IMU acceleration and angular velocity, Represents the time interval between discrete moments.

[0020] The method of collecting image data of the mobile robot using a camera and inputting the image data into the Aliked network model to obtain feature points and descriptors corresponding to each feature point includes:

[0021] The input image I is converted into multi-scale features F1, F2, F3, F4 using four encoding blocks through the feature encoder. The channel range of each encoding block is from c1 to c4. The first encoding block contains two convolutional layers to extract low-level image features F1; the second encoding block uses average pooling to downsample the image features F1; the third and fourth encoding blocks use average pooling to downsample the image features and use residual blocks with depth-separable convolution to extract image features;

[0022] Four ublocks are used to aggregate multi-scale features {F1, F2, F3, F4}. Each ublock contains a convolutional layer and an upsampling layer to align the dimensions and resolution of multi-scale features. The aligned features are connected by Thus, the aggregated features F for feature point and descriptor extraction are obtained;

[0023] SMH (score map head) uses aggregated features F to estimate the score map S, DKD (differentiable key point detection) uses non-maximum suppression on the score map to identify local maxima, and then determines the pixel-level feature points by setting the threshold of the local maximum score, and further refines the feature point positions by using softargmax on the local blocks, thereby extracting differentiable feature points , get the key point feature area;

[0024] M variability sample locations are estimated on the key point feature area through SDDH (Sparse Deformable Descriptor Head), M supporting features are sampled on the feature map according to the variability sample locations, the M supporting features are encoded, and aggregated through convM (convolutional module for M samples) to extract the descriptor.

[0025] The feature matcher of the LightGlue network model is composed of L identical layers stacked together. Each layer consists of self-attention units and cross-attention units to process the input feature set and predict a set of corresponding relationships at each layer.

[0026] The step of inputting the feature points and descriptors into the LightGlue network model to obtain the matching relationship of the local feature points of the image includes the following steps:

[0027] The feature points and descriptors are input into the LightGlue network model for prediction to obtain the prediction results. The prediction results are adaptively inferred whether further calculation is needed. If the model predicts that the matching results of the current stage meet the preset requirements, the subsequent calculation steps are stopped, otherwise the subsequent calculation steps are continued. The subsequent calculation steps are specifically: using feature points and descriptors for matching, and calculating the similarity between each feature point through self-attention and cross-attention units to generate a similarity score, and generating a matchability score based on the matching potential of each feature point pair, and then sorting and screening the matching results according to the similarity score and matchability score.

[0028] The method of using the feature point and local feature point matching relationship in combination with the SFM method to perform pure visual estimation on all image frames of the image data in the sliding window includes: using the feature point and local feature matching relationship in the sliding window to perform structure from motion recovery to estimate the camera pose and feature point position, specifically, first checking the feature correspondence between the latest frame and the previous frame, and if there are enough feature matches and sufficient parallax, using the five-point method to restore the rotation and scaled translation between the two frames; then scaling the translation, triangulating the feature points and PnP solving the pose;

[0029] The IMU pre-integration calculation of the position information, speed and attitude data and the alignment of the visual estimation result with the IMU pre-integration result, the optimization recovery of the visual scale information, the calibration of the gyroscope offset, and the estimation of the IMU coordinate system speed, gravity direction and IMU bias include: defining the IMU coordinate system speed, scale and gravity acceleration corresponding to each frame as:

[0030]

[0031] in, represents the state vector, Indicates The speed in the IMU coordinate system, represents the gravitational acceleration in the world coordinate system, represents the scale factor;

[0032] According to the pre-integration of position and velocity of two consecutive frames k and k+1, the expression can be obtained:

[0033]

[0034]

[0035]

[0036]

[0037] in, and Indicates Frame to The displacement increment and velocity increment of the frame, Indicates that from The rotation matrix from the IMU coordinate system to the world coordinate system, Indicates The position of the frame in the world coordinate system, Indicates that from Frame the posture from the IMU coordinate system to the world coordinate system, Represents the inverse quaternion transformation from the camera coordinate system to the IMU coordinate system. Represents the scaled The estimated value of the frame IMU position in the world coordinate system, Representation frame With frame the time interval between

[0038] According to the above four formulas, the linear constraint equation can be obtained:

[0039]

[0040]

[0041] in, represents the observed values ​​of displacement increment and velocity increment, Represents the translation vector from the camera coordinate system to the IMU coordinate system, represents the observation matrix, is the observation noise, represents the identity matrix;

[0042] Construct a least squares problem to solve the IMU coordinate system velocity, scale and gravitational acceleration corresponding to each frame:

[0043] .

[0044] The visual scale information, gyroscope offset, IMU coordinate system velocity, gravity acceleration and IMU bias are introduced into the visual feature reprojection error, IMU pre-integration error and marginalized residual constraint, and then the whole is converted into an objective function, and the objective function is nonlinearly optimized to obtain the state vector that needs to be optimized in the nonlinear optimization result:

[0045] ;

[0046] ;

[0047] ;

[0048] in, is the number of key frames, is the total number of landmark points observed in the sliding window, For the The IMU state captured by the frame image, is the camera extrinsic parameter, Indicates The inverse depth of landmark points, and Indicates IMU in the world coordinate system at frame time The position and velocity of Indicates Frame time IMU relative to the world coordinate system The posture, and represents the bias of the accelerometer and gyroscope, is the state vector that needs to be optimized in the entire system, For the The state of the IMU at the frame time;

[0049] The objective function is:

[0050] ;

[0051] in, , , They are the marginalized prior information term, the IMU measurement residual term, and the visual reprojection error term. represents the prior residual, The Jacobian matrix representing the prior information, represents the residual of IMU measurement, represents the weight of the IMU error, represents the residual of visual observation, Represents the covariance matrix of the visual observations.

[0052] The tightly coupled relocalization module is combined with the monocular visual inertial odometer to further relocalize the nonlinear optimization results, and the relocalization results include:

[0053] The DBoW2 bag-of-words position recognition method is used to perform loop detection on the nonlinear optimization results. After the temporal and spatial consistency test, DBoW2 returns the candidate frames for loop detection.

[0054] The loop window is further optimized by taking the pose of all loop frames as constants, using all IMU measurements, local visual measurements, and feature correspondence values ​​extracted from the loop to jointly optimize the sliding window. The objective function is:

[0055] ;

[0056] in, represents the loop frame residual term, Indicates a loop frame Middle The observed value of landmark points, Indicates a loop frame The pose relative to the world coordinate system, Indicates a loop frame The corresponding covariance matrix is, Indicates a loop frame The covariance matrix of the visual reprojection error in .

[0057] The relocalization result is subjected to global pose graph optimization to obtain a final global pose graph optimization result, and the navigation operation is performed according to the global pose graph optimization result, including:

[0058] Frame and frame The minimum marginal residual between is defined as:

[0059] ;

[0060] in, and Representation frame and The position vector in the world coordinate system, and Representation frame and The yaw angle, and are the estimated roll and pitch angles, Indicated by frame The attitude angle The inverse matrix of the rotation matrix is ​​formed, Representation frame Relative to frame The estimated location of Representation frame Relative to frame The estimated yaw angle increment of

[0061] The entire graph of sequence edges and loop edges is optimized by minimizing the following objective function:

[0062] ;

[0063] in, is the set of positions of all keyframes, is the set of yaw angles of all key frames, is the marginal residual, is the set of all sequence edges, is the set of all loop edges.

[0064] In a second aspect, the present invention provides a robot visual inertial navigation device, comprising a processor and a storage medium; the storage medium is used to store instructions; the processor is used to operate according to the instructions to execute the steps of any one of the methods.

[0065] In a third aspect, the present invention provides a computer-readable storage medium having a computer program / instruction stored thereon, which, when executed by a processor, implements the steps of the robot visual inertial navigation method.

[0066] Beneficial effects of the present invention: The present invention provides a robot visual inertial navigation method, device and storage medium, which uses a camera to collect image data of a mobile robot, and inputs the image data into an Aliked network model to obtain feature points and descriptors corresponding to each feature point, and inputs the feature points and descriptors into a LightGlue network model to obtain a matching relationship between local feature points of the image. Aliked feature extraction and LightGlue feature matching methods are used. Aliked can extract robust feature points and descriptors in scenes with changing illumination and viewing angles. By learning features of different scenes, the stability and adaptability of visual features can be improved. LightGlue can efficiently match feature points and consider global information to reduce erroneous matching, especially in the case of changes in viewing angle and illumination. More accurate matching pairs can be found. The combination of the two can improve the positioning accuracy and operating efficiency of the algorithm in scenes with changing illumination and viewing angles. In addition, an IMU sensor is added to provide effective constraints and compensation for camera pose solution, thereby eliminating cumulative errors and improving the stability of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 A schematic diagram of an algorithm flow of a robot visual inertial navigation method of the present invention;

[0068] Figure 2 This is the visual front-end flow chart of the present invention;

[0069] Figure 3 This is a structural diagram of the Aliked network model in the present invention;

[0070] Figure 4 This is a structural diagram of the LightGlue network model in the present invention. DETAILED DESCRIPTION

[0071] The present invention will be further described below in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solutions of the present invention and are not intended to limit the protection scope of the present invention.

[0072] Example 1

[0073] like Figure 1 As shown, the present invention discloses a robot visual inertial navigation method, comprising the following steps:

[0074] Step 1: Use the IMU sensor to measure the motion information of the mobile robot, and obtain the position, speed and posture data through IMU pre-integration processing of the motion information.

[0075] Step 2: Use the camera to collect image data of the mobile robot, and input the image data into the Aliked network model to obtain feature points and the descriptors corresponding to each feature point.

[0076] Step 3: Input the feature points and descriptors into the LightGlue network model to obtain the matching relationship of local feature points of the image.

[0077] Step 4: Perform an initial estimate of the position information, velocity and attitude data to obtain the IMU pre-integration result, and use the matching relationship between feature points and local feature points combined with the SFM method to perform pure visual estimation on all image frames of the image data in the sliding window, calculate the position and three-dimensional point coordinates of each frame, and obtain the visual estimation result. Then align the visual estimation result with the IMU pre-integration result, restore the visual scale information through optimization, calibrate the offset of the gyroscope, and estimate the IMU coordinate system velocity, gravity acceleration and IMU bias.

[0078] Step 5: After introducing visual feature reprojection error, IMU pre-integration error and marginalized residual constraint into visual scale information, gyroscope offset, IMU coordinate system velocity, gravity acceleration and IMU bias, the overall information is transformed into an objective function, and nonlinear optimization is performed on the objective function to obtain a nonlinear optimization result.

[0079] Step 6: Use a tightly coupled relocalization module combined with a monocular visual inertial odometer to further relocalize the nonlinear optimization result to obtain the relocalization result;

[0080] Step seven, perform global pose graph optimization on the relocalization result to obtain the final global pose graph optimization result, and perform navigation operations based on the global pose graph optimization result.

[0081] The present invention uses Aliked feature extraction and LightGlue feature matching methods to improve the positioning accuracy and operating efficiency of the algorithm in scenes with changing illumination and perspective. In addition, an IMU sensor is added to provide effective constraints and compensation for camera pose solution, thereby eliminating cumulative errors and improving the stability of the algorithm.

[0082] Example 2

[0083] The present invention discloses a robot visual inertial navigation method, comprising the following steps:

[0084] Step 1: Use the IMU sensor to measure the motion information of the mobile robot, and obtain the position, speed and posture data by IMU pre-integration processing of the motion information. Specifically, the formula of IMU pre-integration is:

[0085]

[0086]

[0087]

[0088] in, , , Respectively represent the discrete moments The estimated position, velocity and attitude at the time , , Respectively represent the discrete moments The estimated position, velocity and attitude at the time Indicates that at discrete moments Through quaternions The calculated rotation matrix is and Indicates that at discrete moments The linear acceleration data and angular velocity data measured by the IMU, and Represents the bias of IMU acceleration and angular velocity, Represents the time interval between discrete moments.

[0089] Step 2: Use the camera to collect image data of the mobile robot, and input the image data into the Aliked network model to obtain feature points and the descriptors corresponding to each feature point. Figure 3 The Aliked network model structure diagram in the present invention is as follows Figure 3 shown. Specifically,

[0090] The feature encoder uses four encoding blocks to convert the input image I into multi-scale features F1, F2, F3, F4. The channel of each encoding block ranges from c1 to c4. The first encoding block contains two convolutional layers to extract low-level image features F1; the second encoding block uses average pooling to downsample the image features F1; the third and fourth encoding blocks use average pooling to downsample the image features and use residual blocks with depth-separable convolution to extract image features.

[0091] Four ublocks are used to aggregate multi-scale features {F1, F2, F3, F4}. Each ublock contains a convolutional layer and an upsampling layer to align the dimensions and resolution of multi-scale features. The aligned features are connected by Thus, the aggregated features F for feature point and descriptor extraction are obtained;

[0092] SMH is used to estimate the score map S using aggregated features F. DKD uses non-maximum suppression on the score map to identify local maxima. The pixel-level feature points are determined by setting the threshold of the local maximum score. The feature point positions are further refined by using softargmax on the local blocks to extract differentiable , get the key point feature area;

[0093] M variability sample positions are estimated on the key point feature region through SDDH, M supporting features are sampled on the feature map according to the variability sample positions, the M supporting features are encoded, and aggregated through convM to extract the descriptor.

[0094] Step 3: Input the feature points and descriptors into the LightGlue network model to obtain the matching relationship of local feature points in the image. The structure diagram of the LightGlue network model is shown in the figure below: Figure 4 Specifically, the feature matcher of the LightGlue network model is composed of L identical layers stacked together, each of which consists of self-attention units and cross-attention units, which are used to process the input feature set and predict a set of corresponding relationships at each layer.

[0095] Inputting feature points and descriptors into the LightGlue network model to obtain the matching relationship of local feature points of the image includes the following steps: inputting feature points and descriptors into the LightGlue network model for prediction to obtain prediction results, adaptively reasoning whether further calculation is needed based on the prediction results, and stopping subsequent calculation steps if the model predicts that the matching results of the current stage meet the preset requirements, otherwise continuing subsequent calculation steps, wherein the subsequent calculation steps are specifically: using feature points and descriptors for matching, and calculating the similarity between each feature point through self-attention and cross-attention units to generate a similarity score, and generating a matchability score based on the matching potential of each feature point pair, and then sorting and screening the matching results based on the similarity score and the matchability score. The specific steps are as follows: Figure 2 shown.

[0096] Step 4: Perform IMU pre-integration calculation on the position information, velocity and attitude data to obtain the IMU pre-integration result, and use the feature points and local feature point matching relationship to combine the SFM method to perform pure visual estimation on all image frames of the image data in the sliding window, calculate the pose and three-dimensional point coordinates of each frame, and obtain the visual estimation result. Then align the visual estimation result with the IMU pre-integration result, optimize and restore the visual scale information, calibrate the offset of the gyroscope, and estimate the IMU coordinate system velocity, gravity acceleration and IMU bias. Specifically, the feature points and local feature matching relationship are used in the sliding window to perform structure recovery from motion to estimate the camera pose and feature point position. Specifically, the feature correspondence between the latest frame and the previous frame is checked first. If there are enough feature matches and sufficient parallax, the five-point method is used to restore the rotation and scaled translation between the two frames; then the translation is scaled, the feature points are triangulated and the PnP solution pose is calculated.

[0097] The IMU pre-integration calculation of the position information, speed and attitude data and the alignment of the visual estimation result with the IMU pre-integration result, the optimization recovery of the visual scale information, the calibration of the gyroscope offset, and the estimation of the IMU coordinate system speed, gravity direction and IMU bias include: defining the IMU coordinate system speed, scale and gravity acceleration corresponding to each frame as:

[0098]

[0099] in, represents the state vector, Indicates The speed in the IMU coordinate system, represents the gravitational acceleration in the world coordinate system, represents the scale factor;

[0100] According to the pre-integration of position and velocity of two consecutive frames k and k+1, the expression can be obtained:

[0101]

[0102]

[0103]

[0104]

[0105] in, and Indicates Frame to The displacement increment and velocity increment of the frame, Indicates that from The rotation matrix from the IMU coordinate system to the world coordinate system, Indicates The position of the frame in the world coordinate system, Indicates that from Frame the posture from the IMU coordinate system to the world coordinate system, Represents the inverse quaternion transformation from the camera coordinate system to the IMU coordinate system. Represents the scaled The estimated value of the frame IMU position in the world coordinate system, Representation frame With frame the time interval between

[0106] According to the above four formulas, the linear constraint equation can be obtained:

[0107]

[0108]

[0109] in, represents the observed values ​​of displacement increment and velocity increment, Represents the translation vector from the camera coordinate system to the IMU coordinate system, represents the observation matrix, is the observation noise, represents the identity matrix;

[0110] Construct a least squares problem to solve the IMU coordinate system velocity, scale and gravitational acceleration corresponding to each frame:

[0111] .

[0112] Step 5: After introducing visual feature reprojection error, IMU pre-integration error and marginalized residual constraint into visual scale information, gyroscope offset, IMU coordinate system velocity, gravity acceleration and IMU bias, the overall information is transformed into an objective function, and nonlinear optimization is performed on the objective function to obtain a nonlinear optimization result.

[0113] The state vector that needs to be optimized is:

[0114] ;

[0115] ;

[0116] ;

[0117] in, is the number of key frames, is the total number of landmark points observed in the sliding window, For the The IMU state captured by the frame image, is the camera extrinsic parameter, Indicates The inverse depth of landmark points, and Indicates IMU in the world coordinate system at frame time The position and velocity of Indicates Frame time IMU relative to the world coordinate system The posture, and represents the bias of the accelerometer and gyroscope, is the state vector that needs to be optimized in the entire system, For the The state of the IMU at the time of the frame.

[0118] The objective function is:

[0119] ;

[0120] in, , , They are the marginalized prior information term, the IMU measurement residual term, and the visual reprojection error term. represents the prior residual, The Jacobian matrix representing the prior information, represents the residual of IMU measurement, represents the weight of the IMU error, represents the residual of visual observation, Represents the covariance matrix of the visual observations.

[0121] Step 6: In order to further improve the positioning accuracy, a tightly coupled relocalization module is combined with a monocular visual inertial odometer to further relocalize the nonlinear optimization result and obtain the relocalization result. Specifically:

[0122] The DBoW2 bag-of-words position recognition method is used to perform loop detection on the nonlinear optimization results. After the temporal and spatial consistency test, DBoW2 returns the candidate frames for loop detection.

[0123] The loop window is further optimized by taking the pose of all loop frames as constants, using all IMU measurements, local visual measurements, and feature correspondence values ​​extracted from the loop to jointly optimize the sliding window. The objective function is:

[0124] ;

[0125] in, represents the loop frame residual term, Indicates a loop frame Middle The observed value of landmark points, Indicates a loop frame The pose relative to the world coordinate system, Indicates a loop frame The corresponding covariance matrix is, Indicates a loop frame The covariance matrix of the visual reprojection error in .

[0126] Step 7: After relocalization, in order to ensure that the past posture can be correctly mapped to the global map, the relocalization result is optimized for the global pose graph to obtain the final global pose graph optimization result, and navigation operations are performed based on the global pose graph optimization result. The trajectory drift problem caused by the accumulated error during long-term motion is solved by optimization, thereby ensuring the accuracy of the global trajectory. Specifically, the frame and frame The minimum marginal residual between is defined as:

[0127] ;

[0128] in, and Representation frame and The position vector in the world coordinate system, and Representation frame and The yaw angle, and are the estimated roll and pitch angles, Indicated by frame The attitude angle The inverse matrix of the rotation matrix is ​​formed, Representation frame Relative to frame The estimated location of Representation frame Relative to frame The estimated yaw angle increment of

[0129] The entire graph of sequence edges and loop edges is optimized by minimizing the following objective function:

[0130] ;

[0131] in, is the set of positions of all keyframes, is the set of yaw angles of all key frames, is the marginal residual, is the set of all sequence edges, is the set of all loop edges.

[0132] Finally, in practical applications of the present invention, the mobile robot communicates and interacts with positioning data through the robot operating system, and combines with other sensors to further improve the robustness and efficiency of the algorithm in complex environments.

[0133] Example 3

[0134] This embodiment discloses a pile foundation damage identification device, including a processor and a storage medium; the storage medium is used to store instructions; the processor is used to operate according to the instructions to execute the steps of the method described in Example 1 or 2.

[0135] Example 4

[0136] This embodiment introduces a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the steps of the method described in Embodiment 1 or 2 are executed.

[0137] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A robot visual inertial navigation method, characterized in that: The following steps are involved: The IMU sensor is used to measure the motion information of the mobile robot, and the position, speed and posture data are obtained by IMU pre-integration processing of the motion information; The image data of the mobile robot is collected by a camera and input into the Aliked network model to obtain feature points and the descriptors corresponding to each feature point. Input feature points and descriptors into the LightGlue network model to obtain the matching relationship of local feature points of the image; Perform IMU pre-integration calculation on the position information, velocity and attitude data to obtain the IMU pre-integration result, and use the matching relationship between feature points and local feature points, combined with the SFM method, to perform pure visual estimation on all image frames of the image data in the sliding window, calculate the position and 3D point coordinates of each frame, and obtain the visual estimation result. Then align the visual estimation result with the IMU pre-integration result, restore the visual scale information through optimization, calibrate the offset of the gyroscope, and estimate the IMU coordinate system velocity, gravity acceleration and IMU bias; The visual scale information, gyroscope offset, IMU coordinate system velocity, gravity acceleration and IMU bias are introduced into the visual feature reprojection error, IMU pre-integration error and marginalized residual constraint, and then the whole is transformed into an objective function, and the objective function is nonlinearly optimized to obtain a nonlinear optimization result; The tightly coupled relocalization module is combined with the monocular visual inertial odometer to further relocalize the nonlinear optimization results and obtain the relocalization results. The relocalization result is optimized globally to obtain the final global pose graph optimization result, and navigation operations are performed based on the global pose graph optimization result.

2. The robot visual inertial navigation method according to claim 1, characterized in that: The formula for IMU pre-integration in obtaining position, velocity and attitude data by IMU pre-integration processing motion information is: ， ， ， in, , , Respectively represent the discrete moments The estimated position, velocity and attitude at the time , , Respectively represent the discrete moments The estimated position, velocity and attitude at the time Indicates that at discrete moments Through quaternions The calculated rotation matrix is and Indicates that at discrete moments The linear acceleration data and angular velocity data measured by the IMU, and Represents the bias of IMU acceleration and angular velocity, Represents the time interval between discrete moments.

3. The robot visual inertial navigation method according to claim 2, characterized in that: The method of collecting image data of the mobile robot using a camera and inputting the image data into the Aliked network model to obtain feature points and descriptors corresponding to each feature point includes: The input image I is converted into multi-scale features F1, F2, F3, F4 using four encoding blocks through the feature encoder. The channel range of each encoding block is from c1 to c4. The first encoding block contains two convolutional layers to extract low-level image features F1; the second encoding block uses average pooling to downsample the image features F1; the third and fourth encoding blocks use average pooling to downsample the image features and use residual blocks with depth-separable convolution to extract image features; Four ublocks are used to aggregate multi-scale features {F1, F2, F3, F4}. Each ublock contains a convolutional layer and an upsampling layer to align the dimensions and resolution of multi-scale features. The aligned features are connected by Thus, the aggregated features F for feature point and descriptor extraction are obtained; SMH uses aggregated features F to estimate the score map S. DKD uses non-maximum suppression on the score map to identify local maxima. Then, the pixel-level feature points are determined by setting the threshold of the local maximum score. The feature point positions are further refined by using softargmax on the local blocks to extract differentiable feature points. , get the key point feature area; M variability sample positions are estimated on the key point feature region through SDDH, M supporting features are sampled on the feature map according to the variability sample positions, the M supporting features are encoded, and aggregated through convM to extract the descriptor.

4. The robot visual inertial navigation method according to claim 3, characterized in that: The feature matcher of the LightGlue network model is composed of L identical layers stacked together. Each layer consists of self-attention units and cross-attention units to process the input feature set and predict a set of corresponding relationships at each layer. The step of inputting the feature points and the descriptors into the LightGlue network model to obtain the matching relationship of the local feature points of the image includes the following steps: The feature points and descriptors are input into the LightGlue network model for prediction to obtain the prediction results. The prediction results are adaptively inferred whether further calculation is needed. If the model predicts that the matching results of the current stage meet the preset requirements, the subsequent calculation steps are stopped, otherwise the subsequent calculation steps are continued. The subsequent calculation steps are specifically: using feature points and descriptors for matching, and calculating the similarity between each feature point through self-attention and cross-attention units to generate a similarity score, and generating a matchability score based on the matching potential of each feature point pair, and then sorting and screening the matching results according to the similarity score and matchability score.

5. The robot visual inertial navigation method according to claim 4, characterized in that: The method of using the feature point and local feature point matching relationship in combination with the SFM method to perform pure visual estimation on all image frames of the image data in the sliding window includes: using the feature point and local feature matching relationship in the sliding window to perform structure from motion recovery to estimate the camera pose and feature point position, specifically, first checking the feature correspondence between the latest frame and the previous frame, and if there are enough feature matches and sufficient parallax, using the five-point method to restore the rotation and scaled translation between the two frames; then scaling the translation, triangulating the feature points and PnP solving the pose; The IMU pre-integration calculation of the position information, speed and attitude data and the alignment of the visual estimation result with the IMU pre-integration result, the optimization recovery of the visual scale information, the calibration of the gyroscope offset, and the estimation of the IMU coordinate system speed, gravity direction and IMU bias include: defining the IMU coordinate system speed, scale and gravity acceleration corresponding to each frame as: ， in, represents the state vector, Indicates The speed in the IMU coordinate system, represents the gravitational acceleration in the world coordinate system, represents the scale factor; According to the pre-integration of position and velocity of two consecutive frames k and k+1, the expression can be obtained: ， ， ， ， in, and Indicates Frame to The displacement increment and velocity increment of the frame, Indicates that from The rotation matrix from the IMU coordinate system to the world coordinate system, Indicates The position of the frame in the world coordinate system, Indicates that from Frame the posture from the IMU coordinate system to the world coordinate system, Represents the inverse quaternion transformation from the camera coordinate system to the IMU coordinate system. Represents the scaled The estimated value of the frame IMU position in the world coordinate system, Representation frame With frame the time interval between According to the above four formulas, the linear constraint equation can be obtained: ， ， in, represents the observed values ​​of displacement increment and velocity increment, Represents the translation vector from the camera coordinate system to the IMU coordinate system, represents the observation matrix, is the observation noise, represents the identity matrix; Construct a least squares problem to solve the IMU coordinate system velocity, scale and gravitational acceleration corresponding to each frame: 。 6. The robot visual inertial navigation method according to claim 5, characterized in that: The visual scale information, gyroscope offset, IMU coordinate system velocity, gravity acceleration and IMU bias are introduced into the visual feature reprojection error, IMU pre-integration error and marginalized residual constraint, and then the whole is converted into an objective function, and the objective function is nonlinearly optimized to obtain the state vector that needs to be optimized in the nonlinear optimization result: ； ； ； in, is the number of key frames, is the total number of landmark points observed in the sliding window, For the The IMU state captured by the frame image, is the camera extrinsic parameter, Indicates The inverse depth of landmark points, and Indicates IMU in the world coordinate system at frame time The position and velocity of Indicates Frame time IMU relative to the world coordinate system The posture, and represents the bias of the accelerometer and gyroscope, is the state vector that needs to be optimized in the entire system, For the The state of the IMU at the frame time; The objective function is: ； in, , , They are the marginalized prior information term, the IMU measurement residual term, and the visual reprojection error term. represents the prior residual, The Jacobian matrix representing the prior information, represents the residual error of IMU measurement, represents the weight of the IMU error, represents the residual of visual observation, Represents the covariance matrix of visual observations.

7. The robot visual inertial navigation method according to claim 6, characterized in that: The tightly coupled relocalization module is combined with the monocular visual inertial odometer to further relocalize the nonlinear optimization results, and the relocalization results include: The DBoW2 bag-of-words position recognition method is used to perform loop detection on the nonlinear optimization results. After the temporal and spatial consistency test, DBoW2 returns the candidate frames for loop detection. The loop window is further optimized by taking the pose of all loop frames as constants, using all IMU measurements, local visual measurements, and feature correspondence values ​​extracted from the loop to jointly optimize the sliding window. The objective function is: ； in, represents the loop frame residual term, Indicates a loop frame Middle The observed value of landmark points, Indicates a loop frame The pose relative to the world coordinate system, Indicates a loop frame The corresponding covariance matrix is, Indicates a loop frame The covariance matrix of the visual reprojection error in .

8. The robot visual inertial navigation method according to claim 7, characterized in that: The relocalization result is subjected to global pose graph optimization to obtain a final global pose graph optimization result, and the navigation operation is performed according to the global pose graph optimization result, including: Frame and frame The minimum marginal residual between is defined as: ； in, and Representation frame and The position vector in the world coordinate system, and Representation frame and The yaw angle, and are the estimated roll and pitch angles, Indicated by frame The attitude angle The inverse matrix of the rotation matrix is ​​formed, Representation frame Relative to frame The estimated location of Representation frame Relative to frame The estimated yaw angle increment of The entire graph of sequence edges and loop edges is optimized by minimizing the following objective function: ； in, is the set of positions of all keyframes, is the set of yaw angles of all key frames, is the marginal residual, is the set of all sequence edges, is the set of all loop edges.

9. A robot visual inertial navigation device, characterized in that: It comprises a processor and a storage medium; the storage medium is used to store instructions; the processor is used to operate according to the instructions to execute the steps of the method according to any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instruction is executed by a processor, the steps of the robot visual inertial navigation method described in any one of claims 1-8 are implemented.

Images