Integrated navigation method and system based on heterogeneous feature data association

Abstract

The invention relates to the technical field of computer vision and robots, and provides an integrated navigation method and system based on heterogeneous feature data association, and the method comprises the following steps: preprocessing visual sensor data to obtain a camera inter-frame rotation variation and a camera inter-frame translation variation; the method comprises the steps of preprocessing observation data to obtain acceleration variation and angular velocity variation, calculating a motion scale factor according to rotation variation and translation variation between frames of a camera, constructing motion constraints according to the motion scale factor, the acceleration variation and the angular velocity variation, and optimizing a key frame selection mechanism according to the motion constraints. And constructing a bimodal tight coupling objective function according to the key frame, and performing nonlinear optimization on the bimodal tight coupling objective function to obtain an optimized camera pose and map point. According to the method, the coverage capability of robust features is remarkably improved, the adaptability and stability of the system in a complex environment are enhanced, and accurate estimation of key state variables is realized.

Description

Technical Field

[0001] This invention relates to the fields of computer vision and robotics, and in particular to a combined navigation method and system based on heterogeneous feature data association. Background Technology

[0002] Simultaneous localization and mapping (SLAM) is a core technology for solving the navigation problem of intelligent unmanned systems in unknown environments. Visual SLAM captures image information through visual sensors to achieve high-precision environmental perception and 3D reconstruction. However, in practical applications, the effectiveness of a single visual sensor is significantly limited when faced with line-of-sight occlusion. The strong complementarity between inertial and visual sensors enables visual-inertial SLAM (VI-SLAM) to exhibit strong environmental adaptability and operational stability in complex scenarios.

[0003] While VI-SLAM can provide stable positional information in complex environments, it still faces performance bottlenecks when the system encounters highly dynamic motion modes, such as rapid vehicle movement or abrupt angle changes. On one hand, scene capture is difficult in high-motion modes, and the system is prone to missing keyframes, resulting in inaccurate environmental features input to the backend. On the other hand, abrupt changes in viewpoint introduce complex noise into the system, causing pose estimation to be significantly affected by outliers. Therefore, constructing an algorithmic architecture with motion state awareness capabilities is crucial for improving the reliability and applicability of VI-SLAM systems in highly dynamic motion modes.

[0004] Existing methods mainly revolve around two dimensions: front-end feature tracking and back-end optimization algorithms. At the front end, this manifests as keyframe selection strategies based on deep learning, camera geometry, or hybrid methods. Deep learning struggles to meet real-time processing requirements, while traditional geometric and hybrid methods mostly rely on fixed thresholds for rapid keyframe selection, lacking collaborative optimization with cross-modal data, thus limiting navigation and positioning accuracy. At the back end, this manifests as optimization methods based on Kalman filtering and its variants, as well as nonlinear optimization based on correlation entropy. Kalman filtering-based optimization methods face complex computational tasks when processing multi-source data. While nonlinear optimization based on correlation entropy can suppress outlier interference, a single kernel function struggles to adapt to the statistical characteristics of complex noise, resulting in limited suppression of different types of outliers, and room for improvement in pose estimation accuracy. Summary of the Invention

[0005] This invention aims to at least solve one of the technical problems existing in related technologies. To this end, this invention provides a combined navigation method and system based on heterogeneous feature data association. It utilizes visual data and inertial information to construct motion constraints, enabling the system to adaptively allocate dual-modal confidence scores according to the vehicle's motion state. This avoids the system losing keyframes during high-dynamic motion, improves the system's robustness in complex environments, and by constructing a tightly coupled residual model based on heterogeneous kernel functions, it suppresses the influence of different outliers or noise on nonlinear optimization, achieving accurate estimation of key state variables.

[0006] This invention provides a combined navigation method based on heterogeneous feature data association, comprising: 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.

[0007] Furthermore, in step S1, the preprocessing of the visual sensor data to obtain the inter-frame rotation and translation changes 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.

[0008] Furthermore, 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.

[0009] Furthermore, 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. Let be the trace of the matrix.

[0010] Furthermore, the calculation expression for motion constraints is as follows: 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, This is the angular velocity threshold.

[0011] Furthermore, the keyframe selection mechanism optimized 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.

[0012] Furthermore, 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.

[0013] Furthermore, the calculation expression for the dual-modal tightly coupled objective function is as follows: in, The optimal target 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.

[0014] Furthermore, 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.

[0015] The present invention also provides a combined navigation system based on heterogeneous feature data association, 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.

[0016] The above-described one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects: Motion constraints are constructed using visual data and inertial information, enabling the system to adaptively allocate dual-modal confidence based on the motion state of the carrier. This avoids the loss of keyframes in highly dynamic motion and improves the system's robustness in complex environments. By constructing a tightly coupled residual model based on heterogeneous kernel functions, the influence of different outliers or noise on nonlinear optimization is suppressed, thus achieving accurate estimation of key state variables.

[0017] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0018] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0019] Figure 1 This is a flowchart illustrating a combined navigation method based on heterogeneous feature data association provided by the present invention.

[0020] Figure 2 This is a schematic diagram of the structure of a combined navigation system based on heterogeneous feature data association provided by the present invention.

[0021] Figure label: 101. Data Processing Module; 102. Calculation Module; 103. Motion Constraint Construction Module; 104. Selection Module; 105. Objective Function Construction Module; 106. Optimization Module. Detailed Implementation

[0022] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention. The following embodiments are used to illustrate this invention but cannot be used to limit the scope of this invention.

[0023] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0024] The following is combined with Figures 1 to 2 This invention describes a combined navigation method and system based on heterogeneous feature data association.

[0025] like Figure 1 As shown, a combined navigation method based on heterogeneous feature data association includes: 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. Preprocessing visual sensor data to obtain inter-frame rotation and translation changes 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. 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: in, For gradient magnitude, The gradient component in the horizontal direction. This represents the gradient component in the vertical direction; 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. .

[0026] 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; The extracted Regions of Interest (ROIs) are used as spatial weight masks in quadtree filtering to improve the feature density of salient regions. The specific filtering process is as follows: when a quadtree node cannot be further divided, if multiple feature points exist within a node, the feature point with the largest response value is retained first. For other feature points within the node, it is further checked whether they are located within an ROI; if so, they are also retained. This filtering strategy not only effectively avoids the uneven distribution of feature points but also yields more robust, high-quality feature points.

[0027] S114: Calculate the feature descriptors of feature points in the retained feature point set using the BRIEF (Binary Robust Independent Elementary Features) method; S115: Perform image matching between frames based on feature descriptors to obtain matching features; Image matching between frames is performed based on feature descriptors. That is, by calculating the distance between descriptors, for each descriptor in one image, the nearest descriptor in another image is found.

[0028] S116: Calculate the inter-frame rotation and translation changes of the camera based on the matching features; The camera motion is initially estimated based on the matching features, and the rotation and translation changes between camera frames are obtained.

[0029] Preprocessing the observed data to obtain changes in acceleration and angular velocity includes: S121: Perform pre-integration processing on the observed data to obtain a pre-integral model for position, velocity, and rotation. The calculation expression is as follows: in, For the current frame, For the previous frame, This represents the change in position from the previous frame to the current frame. This represents the change in velocity from the previous frame to the current frame. This represents the change in position and rotation from the previous frame to the current frame. For the current moment, For the previous moment, This is the rotation matrix of the previous frame at the previous time step. For acceleration observations, For accelerometer bias, For accelerometer noise, This is the angular velocity matrix. It is the angular velocity vector. For gyroscope bias This is gyroscope noise. This represents the rotation amount from the previous frame at the previous moment; By solving Obtaining the antisymmetric matrix .

[0030] 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. Since the sampling frequency of image frames is much lower than that of the IMU, the time interval of a single image frame will cover multiple IMU sampling points. The pre-integration results are adapted to the discrete-time dimension of the image frame, aligning the motion information of the IMU with the time of the image frame. The update formulas for the average acceleration and average angular velocity of each image frame are as follows: in, For the acceleration at the current moment, The acceleration of the previous moment. The total time for pre-integration, Let be a rotation matrix. For accelerometer bias, For the time difference between frames, Let ω be the angular velocity at the current moment. The angular velocity at the previous moment. For gyroscope bias; 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. The changes in acceleration and angular velocity between the current frame and the previous frame are: in, 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. S2: Calculate the motion scaling factor based on the camera's inter-frame rotation and translation changes. The calculation expression 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; S3: Construct motion constraints based on the motion scaling factor, the change in acceleration, and the change in angular velocity. The calculation expression is as follows: 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; S4: Optimize the keyframe selection mechanism based on motion constraints to obtain keyframes for fast linear motion and rotational motion. 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.

[0031] 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. S51: Construct visual reprojection error and inertial residual based on keyframes of fast linear motion and rotational motion; The expression for calculating visual reprojection error is: in, For the first Frame number Visual reprojection error of a 3D point For the first Frame number The pixel coordinates of a 3D point projected onto the image plane For projection function, For the first The Lie group corresponding to the frame pose. For the first A three-dimensional point; Pre-integrate the IMU observation data of keyframes to obtain updated models of position, velocity and rotation. Obtain inertial motion prediction values ​​between adjacent keyframes based on the updated models. Construct inertial residuals based on the inertial motion prediction values ​​between adjacent keyframes and the observation values ​​between adjacent keyframes. S52: Construct a cost function based on heterogeneous kernel function optimization according to the visual reprojection error. The calculation expression 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 It is a heavy-tailed factor. The number of key frames for which pose optimization is needed. This represents the number of map points observed.

[0032] S53: Construct a dual-modal tightly coupled objective function based on the cost function optimized by heterogeneous kernel functions and the inertial residual. The calculation expression is as follows: in, The optimal target state variable. For inertial residuals, To find the function The minimum value, Let covariance matrix be the variance matrix. Let be the norm squared with covariance weighting.

[0033] 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.

[0034] S6: Perform nonlinear optimization on the dual-modal tightly coupled objective function to obtain the optimized camera pose and map points; 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; The expression for calculating the Jacobian matrix is: 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; 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.

[0035] like Figure 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: 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. The calculation module 102 calculates the motion scaling factor based on the inter-frame rotation and translation changes of the camera. The motion constraint construction module 103 constructs motion constraints based on the motion scaling factor, the change in acceleration, and the change in angular velocity. The selection module 104 optimizes the keyframe selection mechanism based on motion constraints to obtain keyframes for fast linear motion and rotational motion. The objective function construction module 105 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. The optimization module 106 performs nonlinear optimization on the dual-modal tightly coupled objective function to obtain the optimized camera pose and map points.

[0036] Through the collaborative work of the above modules, motion constraints are constructed using visual data and inertial information, enabling the system to adaptively allocate dual-modal confidence based on the motion state of the carrier. This avoids the loss of keyframes in highly dynamic motions and improves the robustness of the system in complex environments. By constructing a tightly coupled residual model based on heterogeneous kernel functions, the influence of different outliers or noise on nonlinear optimization is suppressed, thus achieving accurate estimation of key state variables.

[0037] The simulation environment for the experimental integrated navigation method based on heterogeneous feature data association was: CPU Intel i9, Ubuntu 20.04. Performance testing was conducted using the EuRoC dataset, and quantitative analysis was performed using root mean square error (RMSE) and standard deviation (SD). Lower RMSE and SD values ​​are preferred. Table 1 shows the comparison results between the method of this invention and ORB-SLAM3.

[0038] Table 1 Performance Comparison Test

[0039] As shown in Table 1, under test sequences of varying complexity, the RMSE and SD values ​​of this invention are lower than those of ORB-SLAM3, specifically a 42% reduction in RMSE and a 46% reduction in SD. Specifically, when the carrier moves rapidly and significantly in challenging environments, such as MH05, V202, and V203, the advantages of this invention are more pronounced, effectively suppressing outliers while incorporating more real-world data. Furthermore, the quadtree optimization mechanism based on heterogeneous features provides more accurate information for backend processing, enabling the system to exhibit high robustness and accuracy in complex environments.

[0040] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

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 expression for calculating 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 target 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