A combined navigation method and system based on heterogeneous feature data association

Abstract

The application relates to the fields of computer vision and robot technology, and provides a combined navigation method and system based on heterogeneous feature data association, which comprises the following steps: pre-processing visual sensor data to obtain a camera interframe rotation change amount and a translation change amount, pre-processing observation data to obtain an acceleration change amount and an angular velocity change amount, calculating a motion scale factor according to the camera interframe rotation change amount and the translation change amount, constructing a motion constraint according to the motion scale factor, the acceleration change amount and the angular velocity change amount, optimizing a key frame selection mechanism according to the motion constraint, constructing a double-mode tight coupling target function according to the key frame, performing nonlinear optimization on the double-mode tight coupling target function, and obtaining an optimized camera pose and a map point. The application significantly improves the coverage capacity of robust features, enhances the adaptability and stability of the system in a complex environment, and realizes accurate estimation of key state variables.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of computer vision and robotics, and in particular to a combined navigation method and system based on heterogeneous feature data association. BACKGROUND

[0002] Simultaneous Localization and Mapping (SLAM) is the core technology to solve the navigation problem of intelligent unmanned systems in unknown environments. Visual SLAM captures image information through visual sensors to achieve high-precision environment perception and three-dimensional reconstruction. However, in practical applications, a single visual sensor is significantly restricted in effectiveness when facing line-of-sight occlusion. The strong complementarity of inertial sensors and visual sensors makes visual-inertial SLAM (VI-SLAM) exhibit strong environmental adaptability and operational stability in complex scenarios.

[0003] Although VI-SLAM can provide stable position information in complex environments, it still has performance bottlenecks when the system faces high dynamic motion patterns, such as the carrier being in rapid motion or experiencing sharp angle changes. On the one hand, the system has difficulty capturing scenes in high motion modes, and the system is prone to miss selecting key frames, resulting in inaccurate environment features input to the backend. On the other hand, sharp changes in viewing angles can bring complex noise to the system, causing the pose estimation to be greatly affected by outliers. Therefore, constructing an algorithm architecture with motion state perception capability is crucial to improving the reliability and applicability of VI-SLAM systems in high dynamic motion patterns.

[0004] Existing methods mainly focus on two dimensions: front-end feature tracking or back-end optimization algorithms. In the front-end, the key frame selection strategies based on deep learning, camera geometry, or hybrid methods are specific. Deep learning is difficult to meet real-time processing requirements, and traditional geometric and hybrid methods mostly select key frames quickly based on fixed thresholds, but lack of collaborative optimization with cross-modal data, resulting in limited navigation and positioning accuracy. In the back-end, optimization methods based on Kalman filter and its variants, and nonlinear optimization based on correlation entropy are specific. The optimization method based on Kalman filter will face complex calculation tasks when processing multi-source data. Although the nonlinear optimization based on correlation entropy can suppress outlier interference, a single kernel function is difficult to adapt to the statistical properties of complex noise, resulting in limited suppression effect on different types of outliers, and the pose estimation accuracy still has room for improvement. SUMMARY

[0005] The present application aims to at least solve one of the technical problems existing in the related art. To this end, the present application provides a combined navigation method and system based on heterogeneous feature data association, which uses visual data and inertial information to construct motion constraints, so that the system can complete adaptive allocation of dual-mode confidence according to the motion state of the carrier, avoid losing key frames in high dynamic motion, improve the robustness of the system in complex environments, and realize accurate estimation of key state variables by constructing a tightly coupled residual model based on heterogeneous kernel functions to suppress the influence of different abnormal values or noise on nonlinear optimization.

[0006] The present application provides a combined navigation method based on heterogeneous feature data association, comprising:

[0007] S1: acquiring visual sensor data and observation data of an inertial measurement unit, preprocessing the visual sensor data to obtain camera inter-frame rotation change and translation change, and preprocessing the observation data to obtain acceleration change and angular velocity change;

[0008] S2: calculating a motion scale factor according to the camera inter-frame rotation change and the translation change;

[0009] S3: constructing a motion constraint according to the motion scale factor, the acceleration change, and the angular velocity change;

[0010] S4: optimizing a key frame selection mechanism according to the motion constraint to obtain fast straight line motion key frames and rotational motion key frames;

[0011] S5: constructing visual re-projection errors and inertial residuals according to the fast straight line motion key frames and the rotational motion key frames, constructing a cost function based on heterogeneous kernel function optimization according to the visual re-projection errors, and constructing a dual-mode tightly coupled objective function according to the cost function based on heterogeneous kernel function optimization and the inertial residuals;

[0012] S6: performing nonlinear optimization on the dual-mode tightly coupled objective function to obtain optimized camera poses and map points.

[0013] Further, in the S1 step, preprocessing the visual sensor data to obtain the camera inter-frame rotation change and the translation change comprises:

[0014] S111: performing image feature extraction on the visual sensor data to obtain a preliminary feature point set;

[0015] S112: calculating the gradient amplitude of each pixel point in the image, calculating the global mean according to the gradient amplitude of each pixel point, selecting pixel points with gradient amplitude higher than the global mean to form a region of interest;

[0016] S113: Introduce the region of interest as a spatial weight mask into the quad tree, filter the preliminary feature point set through the quad tree, and obtain a reserved feature point set;

[0017] S114: Calculate the feature descriptor of the feature points in the reserved feature point set through the BRIEF method;

[0018] S115: Perform image matching on the inter-frame images according to the feature descriptor, and obtain matching features;

[0019] S116: Calculate the inter-frame rotation change and translation change of the camera according to the matching features.

[0020] Further, in the S1 step, the pre-processing of the observation data to obtain the acceleration change and the angular velocity change includes:

[0021] S121: Perform pre-integration processing on the observation data to obtain a pre-integrated model about position, velocity, and rotation;

[0022] S122: Convert the motion data observed by the inertial measurement unit at continuous time into average acceleration and average angular velocity corresponding to each frame of image according to the pre-integration result of the pre-integrated model about position, velocity, and rotation;

[0023] S123: Calculate the acceleration change between the current frame and the previous frame according to the average acceleration, and calculate the angular velocity change between the current frame and the previous frame according to the average angular velocity.

[0024] Further, the calculation expression of the motion scale factor is:

[0025]

[0026] wherein, is the motion scale factor, is the translation change, is the rotation angle, is the inter-frame rotation change, is the trace of a matrix.

[0027] Further, the calculation expression of the motion constraint is:

[0028]

[0029] wherein, is the dynamic motion, is a natural constant, is the motion scale factor, is the acceleration change between the current frame and the previous frame, is the angular velocity change between the current frame and the previous frame, is the motion adaptive threshold, For acceleration threshold, This is the angular velocity threshold.

[0030] Furthermore, the keyframe selection mechanism optimized based on motion constraints includes:

[0031] When moving rapidly in a straight line, the motion scaling factor is 0, and the motion constraint degenerates into focusing on the IMU acceleration change. If so, select the keyframe;

[0032] During rotational motion, the motion scaling factor increases with the rotation angle. The motion constraint focuses on the change in IMU angular velocity while also considering visual sensor data. If so, then select the keyframe.

[0033] Furthermore, the calculation expression for the cost function based on heterogeneous kernel function optimization is as follows:

[0034]

[0035] in, The cost function is optimized based on heterogeneous kernel functions. The mixing coefficient, For the size of the core, For the first Frame number Visual reprojection error of a 3D point The heavy-tailed factor, The number of key frames for which pose optimization is needed. The number of map points observed. To find the norm.

[0036] Furthermore, the calculation expression for the dual-modal tightly coupled objective function is as follows:

[0037]

[0038] in, The optimal objective state variable. To find the function The minimum value, For the first Frame number Visual reprojection error of a 3D point The cost function is optimized based on heterogeneous kernel functions. The number of key frames for which pose optimization is needed. The number of map points observed. For inertial residuals, Let covariance matrix be the variance matrix. Let be the norm squared with covariance weighting.

[0039] Further, the S6 step comprises:

[0040] S61: Obtain the camera pose and the map point Jacobian matrix by derivation of the cost function, and determine the gradient direction of the optimization iteration according to the camera pose and the map point Jacobian matrix;

[0041] S62: According to the gradient direction of the optimization iteration, the double-mode tightly coupled target function is nonlinearly optimized by the Levenberg-Marquardt method to obtain the optimized camera pose and the map point.

[0042] The application also provides a combined navigation system based on heterogeneous feature data association, comprising:

[0043] A data processing module, which acquires visual sensor data and observation data of an inertial measurement unit, pre-processes the visual sensor data to obtain camera inter-frame rotation change and translation change, and pre-processes the observation data to obtain acceleration change and angular velocity change;

[0044] A calculation module, which calculates a motion scale factor according to the camera inter-frame rotation change and the translation change;

[0045] A motion constraint construction module, which constructs a motion constraint according to the motion scale factor, the acceleration change and the angular velocity change;

[0046] A selection module, which optimizes a key frame selection mechanism according to the motion constraint to obtain fast linear motion key frames and rotational motion key frames;

[0047] A target function construction module, which constructs visual re-projection errors and inertial residuals according to the fast linear motion key frames and the rotational motion key frames, constructs a cost function based on heterogeneous kernel function optimization according to the visual re-projection errors, and constructs a double-mode tightly coupled target function according to the cost function based on heterogeneous kernel function optimization and the inertial residuals;

[0048] An optimization module, which nonlinearly optimizes the double-mode tightly coupled target function to obtain an optimized camera pose and a map point.

[0049] The above one or more technical solutions in the embodiments of the application have at least one of the following technical effects:

[0050] Motion constraints are constructed by using visual data and inertial information, so that the system can complete adaptive distribution of double-mode confidence according to the motion state of a carrier, avoid losing key frames in high dynamic motion, improve the robustness of the system in complex environments, suppress the influence of different abnormal values or noises on nonlinear optimization by constructing a tightly coupled residual model based on heterogeneous kernel functions, and realize accurate estimation of key state variables.

[0051] Additional aspects and advantages of the present application will be made apparent from the following description. BRIEF DESCRIPTION OF DRAWINGS

[0052] In order to more clearly illustrate the technical solutions of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings in the following description are some embodiments of the present application, and those skilled in the art can obtain other drawings according to these drawings without any creative effort.

[0053] Fig. 1 is a flow diagram of a combination navigation method based on heterogeneous feature data association provided by the present application.

[0054] Fig. 2 is a structural diagram of a combination navigation system based on heterogeneous feature data association provided by the present application.

[0055] Reference signs:

[0056] 101, data processing module; 102, calculation module; 103, motion constraint construction module; 104, selection module; 105, target function construction module; 106, optimization module. DETAILED DESCRIPTION

[0057] In order to make the objectives, technical solutions and advantages of the present application clearer, the technical solutions in the present application will be clearly and completely described below. Obviously, the described embodiments are some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without any creative effort belong to the protection scope of the present application. The following embodiments are used to illustrate the present application, but cannot be used to limit the scope of the present application.

[0058] In the description of the present specification, the description of the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in combination with the embodiment or example are contained in at least one embodiment or example of the embodiments of the present application. In the present specification, the illustrative description of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any appropriate manner in any one or more embodiments or examples. In addition, those skilled in the art can combine and combine the different embodiments or examples described in the present specification and the features of the different embodiments or examples without contradiction.

[0059] The following is combined Figs. 1-2 This invention describes a combined navigation method and system based on heterogeneous feature data association.

[0060] like Fig. 1 As shown, a combined navigation method based on heterogeneous feature data association includes:

[0061] S1: Acquire visual sensor data and inertial measurement unit observation data, preprocess the visual sensor data to obtain the camera frame rotation change and translation change, and preprocess the observation data to obtain the acceleration change and angular velocity change.

[0062] Preprocessing visual sensor data to obtain inter-frame rotation and translation changes includes:

[0063] S111: Extract image features from visual sensor data to obtain a preliminary feature point set;

[0064] S112: Calculate the gradient magnitude of each pixel in the image, calculate the global mean based on the gradient magnitude of each pixel, and select pixels with gradient magnitudes higher than the global mean to form regions of interest.

[0065] To effectively reduce feature point redundancy and improve feature point quality, further screening of the initial feature points is necessary. Given that edge and texture-rich regions and their neighborhoods exhibit significant feature performance and strong robustness, regions with gradient magnitudes higher than the global mean are marked as Regions of Interest (ROIs). The expression for calculating the gradient magnitude is as follows:

[0066]

[0067] in, For gradient magnitude, The gradient component in the horizontal direction. This represents the gradient component in the vertical direction;

[0068] For a pixel in an image, the gradient component in the horizontal direction of that pixel can be obtained by convolving the pixel values ​​in its surrounding neighborhood with a horizontal convolution kernel. By convolving the pixel values ​​in its surrounding neighborhood with the vertical convolution kernel, the gradient component in the vertical direction of that pixel can be obtained. .

[0069] S113: Introduce the region of interest as a spatial weight mask into the quadtree, and use the quadtree to filter the initial feature point set to obtain the retained feature point set;

[0070] The extracted ROI is introduced as a spatial weight mask into the quadtree screening to improve the feature density of the salient feature region. The specific screening process is as follows: when the quadtree node cannot be further divided, if there are multiple feature points in the node, the feature point with the largest response value is first retained. For other feature points in the node, it is further detected whether the feature points are located in the ROI, and if so, the feature points are also retained. This screening strategy can not only effectively avoid the unevenness of the distribution of feature points, but also obtain more high-quality feature points with strong robustness.

[0071] S114: Calculate the feature descriptor of the feature point in the retained feature point set by the BRIEF (Binary Robust Independent Elementary Features) method;

[0072] S115: Perform image matching on the inter-frame images according to the feature descriptor to obtain matching features;

[0073] The image matching on the inter-frame images according to the feature descriptor is to find the nearest descriptor in another image for each descriptor in one image by calculating the distance between the descriptors.

[0074] S116: Calculate the inter-frame rotation change and translation change of the camera according to the matching features;

[0075] The motion of the camera is preliminarily estimated according to the matching features to obtain the inter-frame rotation change and translation change of the camera.

[0076] The pre-processing of the observation data to obtain the acceleration change and angular velocity change includes:

[0077] S121: Perform pre-integration processing on the observation data to obtain a pre-integration model about position, velocity and rotation, and the calculation expression is:

[0078]

[0079] wherein, is the current frame, is the previous frame, is the position change from the previous frame to the current frame, is the velocity change from the previous frame to the current frame, is the position change and rotation change from the previous frame to the current frame, is the current time, is the previous time, is the rotation matrix of the previous time and the previous frame, is the acceleration observation value, is the accelerometer bias, is the accelerometer noise, is the angular velocity matrix, is the angular velocity vector, is the gyroscope bias, is the gyroscope noise, is the rotation amount of the previous frame at the previous time;

[0080] is obtained by solving the anti-symmetric matrix of . .

[0081] S122: convert the motion data of the inertial measurement unit observation in continuous time into the average acceleration and average angular velocity corresponding to each frame of image according to the pre-integration result of the pre-integration model about position, velocity and rotation;

[0082] Since the sampling frequency of the image frame is much lower than the IMU, the time interval of a single image frame will cover multiple IMU sampling points. The pre-integration result is adapted to the discrete time dimension of the image frame, so that the motion information of the IMU and the time of the image frame are aligned. The update formula of the average acceleration and average angular velocity of each frame of image is:

[0083]

[0084] wherein, is the acceleration at the current time, is the acceleration at the previous time, is the total time of pre-integration, is the rotation matrix, is the accelerometer bias, is the inter-frame time difference, is the angular velocity at the current time, is the angular velocity at the previous time, is the gyroscope bias;

[0085] S123: calculate the acceleration change amount of the current frame and the previous frame according to the average acceleration, and calculate the angular velocity change amount of the current frame and the previous frame according to the average angular velocity;

[0086] The acceleration change amount and the angular velocity change amount of the current frame and the previous frame are:

[0087]

[0088] wherein, is the acceleration change amount of the current frame and the previous frame, is the angular velocity change amount of the current frame and the previous frame;

[0089] S2: calculate the motion scale factor according to the rotation change amount and the translation change amount between camera frames, and the calculation expression is:

[0090]

[0091] wherein, is a motion scale factor, is a translation change amount, is a rotation angle, is an inter-frame rotation change amount, is a trace of a matrix, is a norm;

[0092] S3: constructing a motion constraint according to the motion scale factor, the acceleration change amount and the angular velocity change amount, and the calculation expression is:

[0093]

[0094] wherein, is a dynamic motion amount, is a natural constant, is a motion scale factor, is an acceleration change amount of a current frame and a previous frame, is an angular velocity change amount of the current frame and the previous frame, is a motion adaptive threshold, is an acceleration threshold, is an angular velocity threshold;

[0095] S4: optimizing a key frame selection mechanism according to the motion constraint, to obtain fast linear motion key frames and rotation motion key frames;

[0096] The motion constraint optimization key frame selection mechanism comprises:

[0097] When the fast linear motion, the motion scale factor is 0, and the motion constraint degenerates into paying attention to the IMU acceleration change, if , the key frame is selected.

[0098] When the rotation motion, the motion scale factor increases with the increase of the rotation angle, and the motion constraint is to pay attention to the IMU angular velocity change, while taking into account the visual sensor data, if , the key frame is selected.

[0099] S5: constructing a visual re-projection error and an inertial residual according to the fast linear motion key frame and the rotation motion key frame, constructing a cost function based on heterogeneous kernel function optimization according to the visual re-projection error, and constructing a dual-mode tight coupling objective function according to the cost function based on the heterogeneous kernel function optimization and the inertial residual;

[0100] S51: constructing a visual re-projection error and an inertial residual according to the fast linear motion key frame and the rotation motion key frame;

[0101] The calculation expression of the visual re-projection error is:

[0102]

[0103] wherein, is the pose of the i-th frame, is the visual re-projection error of the i-th 3D point in the j-th frame, is the pixel coordinate of the i-th 3D point projected onto the image plane in the j-th frame, is a projection function, is the Lie group corresponding to the pose of the i-th frame, is the i-th 3D point; is the i-th 3D point; is the i-th 3D point;

[0104] pre-integrating the IMU observation data of the key frame to obtain an update model of position, velocity and rotation, obtaining an inertial motion prediction value between adjacent key frames according to the update model, and constructing an inertial residual according to the inertial motion prediction value between adjacent key frames and the observation value between adjacent key frames;

[0105] S52: constructing a cost function based on heterogeneous kernel function optimization according to the visual re-projection error, and the calculation expression is:

[0106]

[0107] wherein, is the cost function based on heterogeneous kernel function optimization, is a mixing coefficient, is a kernel size, is the pose of the i-th frame, is the visual re-projection error of the i-th 3D point in the j-th frame, is a heavy tail factor, is the number of key frames whose poses need to be optimized, is the number of observed map points. S53: constructing a dual-modal tight coupling target function according to the cost function based on heterogeneous kernel function optimization and the inertial residual, and the calculation expression is:

[0108]

[0109]

[0110] wherein, is the optimal target state quantity, is the inertial residual, is the minimum value of the function is a covariance matrix, is a norm square weighted with covariance.

[0111] ​​​​​By dynamically adjusting the constraint relationships between nodes in the graph optimization using a dual-modal tightly coupled objective function, the system's dependence on unreliable observation data is reduced. This invention enables the system to form a dual-insurance mechanism of front-end prevention and back-end correction in highly dynamic motion modes, thereby improving the system's reliability and accuracy.

[0112] S6: Perform nonlinear optimization on the dual-modal tightly coupled objective function to obtain the optimized camera pose and map points;

[0113] S61: By taking the derivative of the cost function, obtain the camera pose and the Jacobian matrix of the map points, and determine the gradient direction of the optimization iteration based on the camera pose and the Jacobian matrix of the map points;

[0114] The expression for calculating the Jacobian matrix is:

[0115]

[0116]

[0117]

[0118] in, For Jacobian matrices, For disturbance quantity, For the first A three-dimensional point, For the first Frame number Visual reprojection error of a 3D point For Lie algebra, For the camera intrinsic parameters in the x-direction, For the camera intrinsic parameters in the y-direction, These are the coordinates of a point in the camera coordinate system. It is a rotation matrix;

[0119] S62: Based on the gradient direction of the optimization iteration, the bimodal tightly coupled objective function is nonlinearly optimized using the Levenberg-Marquardt method to obtain the optimized camera pose and map points.

[0120] like Fig. 2 As shown, a combined navigation system based on heterogeneous feature data association, used to execute the aforementioned combined navigation method based on heterogeneous feature data association, includes:

[0121] The data processing module 101 acquires visual sensor data and inertial measurement unit observation data, preprocesses the visual sensor data to obtain the camera frame rotation change and translation change, and preprocesses the observation data to obtain the acceleration change and angular velocity change.

[0122] The computing module 102 calculates a motion scale factor according to the inter-frame rotation change and the translation change of the camera;

[0123] The motion constraint construction module 103 constructs a motion constraint according to the motion scale factor, the acceleration change and the angular velocity change;

[0124] The selection module 104 optimizes a key frame selection mechanism according to the motion constraint, and obtains fast linear motion key frames and rotational motion key frames;

[0125] The target function construction module 105 constructs a visual re-projection error and an inertial residual according to the fast linear motion key frames and the rotational motion key frames, constructs a cost function based on heterogeneous kernel function optimization according to the visual re-projection error, and constructs a dual-modal tight coupling target function according to the cost function based on heterogeneous kernel function optimization and the inertial residual;

[0126] The optimization module 106 performs nonlinear optimization on the dual-modal tight coupling target function, and obtains an optimized camera pose and a map point.

[0127] Through the cooperative work of the above modules, the motion constraint is constructed by using visual data and inertial information, so that the system completes the adaptive allocation of dual-modal confidence according to the motion state of the carrier, avoids the loss of key frames in high dynamic motion, improves the robustness of the system in complex environments, suppresses the influence of different abnormal values or noises on nonlinear optimization by constructing a tight coupling residual model based on heterogeneous kernel function, and realizes accurate estimation of key state variables.

[0128] The simulation environment of the combined navigation method experiment based on heterogeneous feature data association is: CPU Intel i9, Ubuntu 20.04. The EuRoC dataset is used for performance testing, and the root mean square error (RMSE) and the standard deviation (S.D.) are used for data quantitative analysis. The smaller the RMSE value is, the better, and the smaller the S.D. value is, the better. Table 1 is the comparison result of the method and ORB-SLAM3.

[0129] Table 1 Performance comparison test

[0130]

[0131] As can be seen from Table 1, under different test sequences with different complexity, the RMSE value and S.D. value of the application are reduced compared with ORB-SLAM3, specifically, the RMSE value is reduced by an average of 42%, and the S.D. value is reduced by an average of 46%. Specifically, when the carrier moves rapidly and greatly in a challenging environment, such as MH05, V202, V203, etc., the advantage of the application is more prominent. It considers more real data while effectively suppressing outliers. In addition, the four-tree optimization mechanism based on heterogeneous features provides more accurate information for the back-end processing, making the system have high robustness and accuracy in complex environments.

[0132] Finally, it should be noted that: the above examples are only used to illustrate the technical solutions of the present application, and not to limit it; although the present application has been described in detail with reference to the foregoing examples, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing examples, or make equivalent replacement for part of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of each embodiment of the present application.

Claims

1. A combined navigation method based on heterogeneous feature data association, characterized in that, include: S1: Acquire visual sensor data and inertial measurement unit observation data, preprocess the visual sensor data to obtain the camera frame rotation change and translation change, and preprocess the observation data to obtain the acceleration change and angular velocity change. S2: Calculate the motion scaling factor based on the inter-frame rotation and translation changes of the camera; S3: Construct motion constraints based on the motion scaling factor, the change in acceleration, and the change in angular velocity; S4: Optimize the keyframe selection mechanism based on motion constraints to obtain keyframes for fast linear motion and rotational motion. S5: Construct visual reprojection error and inertial residual based on keyframes of fast linear motion and rotational motion; construct a cost function based on heterogeneous kernel function optimization based on visual reprojection error; and construct a dual-modal tightly coupled objective function based on cost function based on heterogeneous kernel function optimization and inertial residual. S6: Perform nonlinear optimization on the dual-modal tightly coupled objective function to obtain the optimized camera pose and map points.

2. The integrated navigation method based on heterogeneous feature data association according to claim 1, characterized in that, In step S1, the preprocessing of the visual sensor data to obtain the inter-frame rotation and translation changes of the camera includes: S111: Extract image features from visual sensor data to obtain a preliminary feature point set; S112: Calculate the gradient magnitude of each pixel in the image, calculate the global mean based on the gradient magnitude of each pixel, and select pixels with gradient magnitudes higher than the global mean to form regions of interest. S113: Introduce the region of interest as a spatial weight mask into the quadtree, and use the quadtree to filter the initial feature point set to obtain the retained feature point set; S114: Calculate the feature descriptors of the feature points in the retained feature point set using the BRIEF method; S115: Perform image matching between frames based on feature descriptors to obtain matching features; S116: Calculate the inter-frame rotation and translation changes of the camera based on the matching features.

3. The integrated navigation method based on heterogeneous feature data association according to claim 1, characterized in that, In step S1, the preprocessing of the observed data to obtain the changes in acceleration and angular velocity includes: S121: Perform pre-integration processing on the observed data to obtain a pre-integrated model of position, velocity, and rotation; S122: Based on the pre-integration results of the pre-integration model for position, velocity, and rotation, convert the motion data observed by the inertial measurement unit over continuous time into the average acceleration and average angular velocity corresponding to each frame of the image. S123: Calculate the change in acceleration between the current frame and the previous frame based on the average acceleration, and calculate the change in angular velocity between the current frame and the previous frame based on the average angular velocity.

4. The integrated navigation method based on heterogeneous feature data association according to claim 1, characterized in that, The expression for calculating the motion scaling factor is: in, For motion scaling factor, This represents the amount of translation change. The rotation angle is... This represents the amount of rotation change between frames. The trace of the matrix, To find the norm.

5. The integrated navigation method based on heterogeneous feature data association according to claim 1, characterized in that, The calculation expression for motion constraints is: in, For dynamic exercise volume, It is a natural constant. For motion scaling factor, This represents the change in acceleration between the current frame and the previous frame. This represents the change in angular velocity between the current frame and the previous frame. For motion-adaptive threshold, For acceleration threshold, Angular velocity threshold To find the norm.

6. The integrated navigation method based on heterogeneous feature data association according to claim 5, characterized in that, The keyframe selection optimization mechanism based on motion constraints includes: When moving rapidly in a straight line, the motion scaling factor is 0, and the motion constraint degenerates into focusing on the IMU acceleration change. If so, select the keyframe; During rotational motion, the motion scaling factor increases with the rotation angle. The motion constraint focuses on the change in IMU angular velocity while also considering visual sensor data. If so, then select the keyframe.

7. The integrated navigation method based on heterogeneous feature data association according to claim 1, characterized in that, The calculation expression for the cost function based on heterogeneous kernel function optimization is as follows: in, The cost function is optimized based on heterogeneous kernel functions. The mixing coefficient, For the size of the core, For the first Frame number Visual reprojection error of a 3D point The heavy-tailed factor, The number of key frames for which pose optimization is needed. The number of map points observed. To find the norm.

8. The integrated navigation method based on heterogeneous feature data association according to claim 1, characterized in that, The calculation expression for the dual-modal tightly coupled objective function is as follows: in, The optimal objective state variable. To find the function The minimum value, For the first Frame number Visual reprojection error of a 3D point The cost function is optimized based on heterogeneous kernel functions. The number of key frames for which pose optimization is needed. The number of map points observed. For inertial residuals, Let covariance matrix be the variance matrix. Let be the norm squared with covariance weighting.

9. The integrated navigation method based on heterogeneous feature data association according to claim 1, characterized in that, Step S6 includes: S61: By taking the derivative of the cost function, obtain the camera pose and the Jacobian matrix of the map points, and determine the gradient direction of the optimization iteration based on the camera pose and the Jacobian matrix of the map points; S62: Based on the gradient direction of the optimization iteration, the bimodal tightly coupled objective function is nonlinearly optimized using the Levenberg-Marquardt method to obtain the optimized camera pose and map points.

10. A combined navigation system based on heterogeneous feature data association, characterized in that, To perform a combined navigation method based on heterogeneous feature data association as described in any one of claims 1 to 9, comprising: The data processing module acquires visual sensor data and inertial measurement unit observation data, preprocesses the visual sensor data to obtain the camera frame rotation change and translation change, and preprocesses the observation data to obtain the acceleration change and angular velocity change. The calculation module calculates the motion scaling factor based on the rotation and translation changes between camera frames; A motion constraint construction module, which constructs motion constraints based on motion scaling factor, acceleration change, and angular velocity change; The selection module optimizes the keyframe selection mechanism based on motion constraints to obtain keyframes for fast linear motion and rotational motion. The objective function construction module constructs visual reprojection error and inertial residual based on fast linear motion keyframes and rotational motion keyframes, constructs a cost function based on heterogeneous kernel function optimization based on visual reprojection error, and constructs a dual-modal tightly coupled objective function based on the cost function based on heterogeneous kernel function optimization and inertial residual. An optimization module performs nonlinear optimization on a dual-modal tightly coupled objective function to obtain optimized camera pose and map points.

Images