Course constraint-based MSCKF visual odometer method

Abstract

According to the MSCKF visual odometer method based on the course constraint, the problem that the estimation precision is reduced due to inconsistent course observability is solved through course constraint information, the system robustness is improved, and the MSCKF visual odometer method based on the course constraint has wide application prospects in multiple fields in the future. The problem that the estimation precision is reduced due to inconsistent course observability is solved, and the robustness of long-time operation of the MSCKF system is enhanced.

Description

A Heading-Constrained MSCKF Visual Odometry Method Technical Field

[0001] This invention belongs to the field of visual-assisted navigation technology, specifically relating to an MSCKF visual odometry method based on heading constraints. Background Technology

[0002] In recent years, aircraft navigation in low- and medium-altitude airspace has primarily relied on INS / GNSS integrated navigation. However, in some complex and challenging environments, satellite denial may occur. If relying solely on the INS system, positioning errors accumulate over time, rendering it ineffective in providing navigation. Visual-assisted navigation technology, on the other hand, can utilize visual observation information to suppress the error divergence rate of the INS system, thereby improving navigation performance in satellite-denied environments. This represents an important future development direction for airborne navigation.

[0003] Common visual-assisted navigation technologies can be divided into those with and without reference maps. Visual-assisted navigation with reference maps typically refers to scene matching techniques. This technique requires pre-loading a satellite image database and matching templates between real-time observed visual images and reference maps in the database to obtain the current position information of the vehicle. However, the drawback of this technique is the need for pre-prepared databases, which is not feasible in most unknown environments. Visual-assisted navigation without reference maps utilizes inter-frame information to construct constraint relationships and generate visual measurements. Navigation performance is improved through filtering or optimization methods. Compared to optimization methods, filtering-based visual-assisted navigation schemes without reference maps have lower computational complexity, better real-time performance, and broad application prospects.

[0004] Multi-state constrained Kalman filter (MSCKF) is a classic visual odometry method based on EKF. Traditional EKF-SLAM adds feature points to the state vector and estimates them together with the IMU state. However, when the environment is rich and the number of feature points is huge, the dimensionality of the state vector becomes very large. MSCKF differs from this approach by adding the camera poses at different times to the state vector instead of feature points. The feature points are observed by the camera at multiple times, thus forming geometric constraints between multiple camera states. This optimizes the global position of the feature points. Measurement residuals are constructed by reprojecting them onto the camera plane, thereby updating the filter and suppressing the rate of increase in system error.

[0005] The MSCKF method has been widely applied in many fields, such as autonomous vehicles, drone autonomous navigation, and augmented reality (AR). The well-known Google Tango uses MSCKF technology to achieve GPS-free autonomous navigation and augmented reality experiences through MEMS inertial sensors and cameras on a mobile phone.

[0006] However, the traditional MSCKF technique still has some problems. The unobservable dimension of the VIO system is 4 degrees of freedom, while the unobservable dimension of MSCKF becomes 3 degrees of freedom. The heading angle is mistakenly considered observable, which leads to inconsistency and underestimation of the covariance matrix corresponding to the heading angle. Over time, this will lead to a decrease in estimation accuracy. Summary of the Invention

[0007] The purpose of this invention is to address the inconsistency caused by the incorrect estimation of heading angle observability in traditional MSCKF systems. It proposes an MSCKF visual odometry method based on heading constraints, which improves the estimation accuracy reduction caused by inconsistencies in heading observability through heading constraint information, thereby enhancing system robustness and showing broad application prospects in multiple fields. This method improves the estimation accuracy reduction problem caused by inconsistencies in heading observability and enhances the long-term robustness of the MSCKF system.

[0008] The technical solution of this invention: This invention proposes an MSCKF visual odometry method under heading constraints, applicable to one or more of the following: inertial navigation system (INS), camera, and dual-antenna direction finding, magnetic sensor, or polarized light direction finding device for providing heading constraints; the method includes the following steps: Step 1: Define a filter state vector, composed of INS state error and camera state error; Step 2: Perform system initialization, initializing INS state, camera state, system covariance matrix, system noise matrix, and feature tracking list; Step 3: INS acquires angular velocity and acceleration information at its operating frequency, and camera acquires image information at its operating frequency; Step 4: Perform pure inertial calculation on the INS angular velocity w and acceleration a acquired in Step 3; Step 5: According to the MSCKF algorithm, write the INS error equation to obtain the filter state equation, and discretize the state equation to obtain the filter time update equation; Step 6: Determine whether the measurement update time has been reached based on whether a visual image has been received. If it has, proceed to Step 7; otherwise... Then return to step 3; Step 7: According to the MSCKF algorithm, amplify the camera pose error into the system state error, calculate the Jacobian matrix of the newly added camera pose error to the original state error, and calculate the amplified system covariance matrix; Step 8: Perform feature tracking on the image information received in step 3, traverse the ownership logic of all feature points, find the lost feature points and add them to the list to be optimized, and update the feature tracking list for the remaining feature points; Step 9: Traverse the list to be optimized in step 8, use the tracking trajectory of each feature point to triangulate the feature point and calculate the position coordinates of the feature point, thereby constructing the visual measurement residual vector; Step 10: Construct a visual feature point measurement model; Step 11: Construct a heading constraint information measurement update model; Step 12: Merge the visual feature point measurement model in step 10 and the heading constraint measurement model in step 11 to obtain the total filter measurement model and the observation noise covariance matrix, respectively; Step 13: According to the MSCKF algorithm, execute the filter measurement update, using the state correction amount Correct the inertial navigation state.

[0009] In one possible embodiment, in step 4, the inertial calculation method is not limited to one of single sample or fourth-order Runge-Kutta.

[0010] In one possible embodiment, in step 8, the feature tracking method includes one of LK optical flow feature tracking and ORB feature descriptor tracking.

[0011] In one possible embodiment, the specific process of determining the attribution logic of each feature point in step 8 includes: a) whether the feature point belongs to the feature tracking list. If it does not belong to the feature tracking list, it indicates that it is a new feature point, and a new track is added to the feature tracking list; otherwise, proceed to criterion b); b) if the feature point belongs to the feature tracking list, it indicates that the feature point is not a new feature point. Determine whether it has been lost. If it has not been lost, it indicates that the feature point is continuously tracked, and the observation coordinates of the track corresponding to the feature point in the feature tracking list are updated; otherwise, proceed to criterion c); c) if the feature point belongs to the feature tracking list and has been lost, it indicates that the feature point can be used for measurement updates, and the track of the feature point is added to the list to be optimized for feature point triangulation calculation.

[0012] In one possible embodiment, step 10 specifically includes the following steps: 10-1) Constructing a residual model of a single feature point against a single camera observation frame using the feature point location coordinates obtained in step 9 according to the MSCKF algorithm; 10-2) Merging all camera observation frames of the single feature point from step 10-1) according to the MSCKF algorithm to obtain a residual model of the single feature point against all camera observation frames; 10-3) Linearizing the single feature point residual model obtained in step 10-2) according to the MSCKF algorithm to obtain a single feature point measurement model; 10-4) According to the MSCKF algorithm... 10-3) Null-space projection is performed on the single feature point measurement model obtained in step 10-3) to remove interference terms related to the feature point position in the measurement model; 10-5) The measurement models of all feature points in the current frame are merged according to the MSCKF algorithm; 10-6) QR decomposition is performed on the visual correlation measurement model of the current frame obtained in step 10-5) according to the MSCKF algorithm to reduce the computational load; In a possible embodiment, in step 11, the following steps are specifically performed: 11-1) Based on the heading constraint information, the constraint equation between the heading error angle and the platform misalignment angle is constructed, as shown in the following equation:

[0013] Among them, the inertial navigation attitude Euler angles are The misalignment angle of the three-axis platform is , This represents the heading angle error; the heading constraint information is not limited to methods such as dual-antenna direction finding, magnetic sensor direction finding, or polarized light direction finding; 11-2) Constructing a residual model using the heading constraint information.

[0014] in It is the heading constraint information residual term. It is heading constraint information. This is the inertial navigation heading angle estimated by the filter; 11-3) Using the heading constraint equation in step 11-1), calculate the Jacobian matrix of the heading constraint information residual term with respect to the original state error of the filter, as shown in the following formula: ;11-4) Using the Jacobian matrix obtained in step 11-3), construct the measurement matrix under the heading constraint information as shown in the following formula:

[0015] in It is the Jacobian matrix of the error between the heading constraint information residual term and the original state of the filter. It is the heading constraint information observation noise vector; 11-5) The heading constraint information observation noise vector in step 11-4). Construct the observation noise covariance matrix .

[0016] In one possible embodiment, when a camera frame no longer observes any feature points, the camera state is removed from the state vector, and the row and column corresponding to the camera pose are deleted from the system covariance matrix.

[0017] The beneficial effects of this invention are as follows: by introducing heading constraint information, such as that from a magnetic heading sensor, the heading error in the state vector is corrected by direct heading constraint, which improves the long-term estimation accuracy degradation problem caused by inconsistent heading observability in the traditional MSCKF method and enhances the robustness of the MSCKF system. Attached Figure Description

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

[0019] Figure 1 is a flowchart of the MSCKF method under heading constraints. Detailed Implementation

[0020] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0021] The features and illustrative embodiments of various aspects of the present invention will now be described in detail. Numerous specific details are set forth in the following detailed description to provide a thorough understanding of the invention. However, it will be apparent to those skilled in the art that the invention may be practiced without requiring some of these specific details. The following description of embodiments is merely intended to provide a better understanding of the invention by illustrating examples of the invention. The invention is by no means limited to any specific setups and methods set forth below, but covers any improvements, substitutions, and modifications to structures, methods, and devices without departing from the spirit of the invention. Well-known structures and techniques are not shown in the drawings and the following description to avoid unnecessarily obscuring the invention.

[0022] In the description of this invention, it should be noted that the directions or positional relationships indicated by terms such as "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer" are based on the directions or positional relationships shown in the accompanying drawings and are only for the convenience of describing and simplifying the invention, and should not be construed as limiting the invention. Furthermore, the use of ordinal numbers (e.g., "first and second," etc.) is for distinguishing objects and is not limited to this order, and should not be construed as indicating or implying relative importance.

[0023] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly, encompassing both direct connection and indirect connection via an intermediate medium. Those skilled in the art can understand the specific meaning of these terms in this invention based on the specific circumstances.

[0024] It should be noted that, unless otherwise specified, the embodiments of the present invention and the features thereof can be combined with each other, and the various embodiments can be referenced and cited in each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0025] The present invention will be further described in detail below with reference to the embodiments and accompanying drawings, but the embodiments of the present invention are not limited thereto.

[0026] As shown in Figure 1, this invention proposes a multi-state constrained Kalman filter method (hereinafter referred to as the MSCKF method) based on heading constraint information. This method performs the following steps: 1) Define the filter state, which consists of the inertial navigation system's state error and the camera's state error, as shown in the following equation. This represents the state error of the inertial navigation system. This represents the camera's state error, with a dimension of 15 + 6n, where n represents the number of camera frames. The specific steps are as follows:

[0027] 1-1) The state error of the inertial navigation system is defined as shown in the following equation. Wherein This indicates the misalignment angle of the three-axis platform under inertial navigation. This indicates the gyroscope's zero-bias error. This indicates the inertial navigation system's northeast-sky velocity error in the geographic frame. This indicates the zero-point error of the accelerometer. This represents the position error of the inertial navigation system in the WGS84 system.

[0028]

[0029] 1-2) The state error of the camera is defined as shown in the following formula. Wherein This indicates the camera's three-axis platform misalignment angle error. This represents the position error of the camera in the local world coordinate system (Anchor system), where n represents the number of camera frames. The Anchor system is defined as the northeast-sky coordinate system with the origin as the starting point.

[0030]

[0031] 2) Perform system initialization, specifically the following steps: 2-1) Initialize the inertial navigation state, providing initial values ​​for the attitude, velocity, position, gyroscope zero bias, and accelerator zero position of the inertial navigation system.

[0032] 2-2) Initialize the camera state. Since no camera frames are added at the initial moment, the initial camera state is empty.

[0033] 2-3) Initialize the covariance matrix of the filter system, which has the following form, where The covariance matrix is ​​related to the inertial navigation state error. This is the correlation covariance matrix between the inertial navigation system state error and the camera state error. Let be the covariance matrix involving camera state errors. Initially, no camera frames are added. Equivalent to .

[0034]

[0035] 2-4) Initialize the filter system noise matrix. The inertial navigation system noise vector is... ,in The white noise is measured by the gyroscope and accelerometer, respectively. The zero bias of the gyroscope and the zero position of the accelerometer are modeled as random walk processes. These are the random walk errors of the gyroscope and accelerometer zero bias, respectively. The corresponding diagonal matrix is ​​the initial system noise covariance matrix. .

[0036] 2-5) Initialize the feature tracking list. The feature tracking list contains tracks for all feature points. Each feature point track contains the feature point's ID and observation coordinates (u, v), where u and v represent the feature point's horizontal and vertical coordinates in the pixel plane, respectively. Initially, no feature points are observed, so the initial feature tracking list is empty.

[0037] 3) Sensor data acquisition. The inertial navigation system acquires three-axis angular velocity and three-axis acceleration information from the gyroscope and accelerometer at its operating frequency, while the camera acquires visual image information at its operating frequency.

[0038] 4) Pure inertial calculation. Following the MSCKF algorithm, inertial calculation is performed on the inertial navigation angular velocity and acceleration information received in step 3). The inertial calculation method is not limited to single-sample, fourth-order Runge-Kutta, or other methods. Assume that the position, velocity, and attitude of the inertial navigation system at time k are respectively... ,go through After the sampling period, the inertial navigation position, velocity, and attitude obtained after inertial calculation are as follows: , , .

[0039] 5) Filter time update, specifically the following steps: 5-1) According to the MSCKF algorithm, write the inertial navigation error state equation in matrix form as shown below. Where F matrix is ​​the state transition matrix and G matrix is ​​the process noise matrix.

[0040]

[0041] 5-2) Linearization of State Equations. The linearization of the system state equations in step 5-1) yields the discrete-form state transition matrix and system process noise matrix, as shown in the following equations: The sampling period is This is the initial system process noise matrix.

[0042]

[0043]

[0044] 5-3) Covariance Matrix Time Update. Since the filter time update is only related to the inertial navigation, according to the system covariance matrix form shown in step 2-3), only the covariance matrix related to the inertial navigation is updated, while the covariance matrix involving the camera remains unchanged, as shown in the following formula.

[0045]

[0046] 6) Determine if the measurement update time has arrived based on whether an image has been received. If so, proceed to step 7; otherwise, return to step 3.

[0047] 7) Camera State Augmentation, specifically the following steps: 7-1) Augmenting the State Vector. For each frame received, the filter augments a new camera pose error into the state vector shown in step 1).

[0048] 7-2) Calculate the pose of the new camera. According to the MSCKF algorithm, since the camera and inertial navigation system are rigidly connected, the extrinsic calibration parameters between the inertial navigation coordinate system and the camera coordinate system can be used. and the pose information of the inertial navigation system in the Anchor frame. , Calculate the pose of the new camera in the Anchor system. and .in Let be the rotation quaternion of the inertial navigation frame relative to the camera frame. This represents the displacement of the camera system relative to the inertial navigation system. This represents the attitude matrix of the inertial navigation system in the Anchor frame. The calibration method described is not limited to extrinsic parameter calibration methods such as Kalibr.

[0049]

[0050] 7-3) Expanding the covariance matrix. The expanded covariance matrix is ​​calculated using the MSCKF algorithm, as shown in the following formula. Where the J matrix is ​​the Jacobian matrix of the new camera pose error versus the original state error.

[0051]

[0052] 8) Feature tracking, specifically the following steps: 8-1) Perform feature tracking on the image data acquired in step 3) according to the MSCKF algorithm, and output the feature point ID number and two-dimensional observation coordinates (u,v) of the current frame image. The feature tracking method is not limited to LK optical flow feature tracking, ORB feature descriptor tracking, etc.

[0053] 8-2) Traverse all feature points and determine the ownership logic of each feature point: a) Whether the feature point belongs to the feature tracking list. If it does not belong to the feature tracking list, it means that it is a new feature point and a new track is added to the feature tracking list; otherwise, jump to criterion (b).

[0054] b) If the feature point belongs to the feature tracking list, it means that the feature point is not a new feature point. To determine whether it has been lost, if the feature point belonged to the feature tracking list in the previous frame, but does not belong to the feature tracking list in the current frame, it means that the feature point has been lost.

[0055] If the feature point is not lost, it means that the feature point is being continuously tracked, and the observation coordinates of the track corresponding to the feature point in the feature tracking list are updated; otherwise, proceed to criterion (c). c) If the feature point belongs to the feature tracking list and has been lost, it means that the feature point can be used for measurement updates, and the track of the feature point is added to the list to be optimized for feature point triangulation calculation.

[0056] 9) Feature point triangulation: Following the MSCKF algorithm, nonlinear optimization calculations are performed using the feature point track information from the list of features to be optimized in step 8-2). The feature points are optimized by minimizing the reprojection error within the track. Position in the local world coordinate system Anchor The nonlinear optimization is not limited to the LM algorithm, Gauss-Newton iterative algorithm, etc.

[0057] 10) Construct a visual feature point measurement model, specifically by performing the following steps: 10-1) Construct a residual model of a single feature point to a single camera frame according to the MSCKF algorithm, as shown in the following formula. For feature points For camera frames The residual model, For feature points In camera frame Direct observation coordinates, For feature points In camera frame The estimated coordinates.

[0058]

[0059] Through step 9) The calculation yields the following formula, where and camera frames The position and orientation matrix in the local world coordinate system Anchor.

[0060] ,

[0061] 10-2) Following the MSCKF algorithm, construct the residual model of a single feature point for all camera frames that observed that feature point, as shown in the following equation. Feature points The residual model for all camera frames that observed the feature point, where m is the number of camera frames that observed the feature point.

[0062]

[0063] 10-3) Residual model linearization: Following the MSCKF algorithm, the single-feature-point residual model in step 10-2) is linearized to obtain the single-feature-point measurement model. For feature points residual For filter state error Jacobian matrix, For feature points residual For feature points Position error in Anchor system The Jacobian matrix. For feature points Measurement noise.

[0064]

[0065] 10-4) Null space projection of the measurement model. Following the MSCKF algorithm, this is achieved by multiplying both sides of the measurement model simultaneously. left null space The location of the feature points of the interference term Removed from the measurement model. For feature points The residual after null space projection Feature points Measurement matrix after null space projection For feature points Measurement noise after zero-space projection.

[0066]

[0067] 10-5) Merge all feature point measurement models. According to the MSCKF algorithm, traverse all feature point track information in the list to be optimized in step 8-2), stack the measurement equations of all feature points, and obtain the total visual measurement model of the current frame as shown in the following formula.

[0068]

[0069] 10-6) Perform QR decomposition on the visual total measurement model in step 10-5) according to the MSCKF algorithm to reduce the amount of computation.

[0070]

[0071] Only the non-zero terms are retained to obtain the final measurement model, as shown in the following formula.

[0072]

[0073] 11) Construction of the heading constraint information measurement model, specifically the following steps: 11-1) Construction of the heading constraint equation, as shown in the following formula. Where the inertial navigation attitude Euler angles are... The misalignment angle of the three-axis platform is , This represents the heading angle error. The heading constraint information can be used to constrain the platform's misalignment angle error. This heading constraint information is not limited to methods such as dual-antenna direction finding, magnetic sensors, or polarized light direction finding.

[0074]

[0075] 11-2) Construction of the residual model for heading constraint information. ,in It is the heading constraint information residual term. It is heading constraint information. It is the inertial navigation heading angle estimated by the filter.

[0076] 11-3) Measurement equations under heading constraints are shown in the following equation, where It is a measurement matrix under heading constraints. It is the observation noise vector of heading constraint information.

[0077] The matrix is ​​the Jacobian matrix of the heading angle error versus the original state error of the filter, with dimensions 1×(15+6n), where n is the number of camera frames. The specific definition is as follows:

[0078] 11-4) Calculation of the observation noise covariance matrix of heading constraint information, in step 11-3) The corresponding covariance matrix is .

[0079] 12) Combine the feature point measurement model in step 10-6) and the heading constraint measurement model in step 11-3) to obtain the total filter measurement model, as shown in the following formula.

[0080]

[0081] 13) Merging of observation noise covariance matrices. Specifically, the following steps are performed: 13-1) The observation noise covariance matrix of a single visual feature point for all camera frames that observed that feature point is shown in the following formula. Wherein… Let be the camera observation noise covariance vector, where diag represents the diagonal matrix and m represents the number of camera frames observed for this feature point.

[0082]

[0083] 13-2) After zero-space projection ,in This is the left null space in step 10-4).

[0084] 13-3) Merge all visual feature points to obtain the total noise covariance matrix of the current frame's visual observations, as shown in the following formula. Where k is the total number of feature points in the list to be optimized in step 8-2). This represents the total number of camera frames in which the current feature point has been observed.

[0085]

[0086] 13-4) After QR decomposition, ,in The Q matrix is ​​obtained from the QR decomposition in step 10-6).

[0087] 13-5) Observation noise covariance matrix after incorporating heading constraint information ,in It is the observation noise covariance matrix corresponding to the heading constraint calculated in step 11-4).

[0088] 14) Update the filter measurements according to the MSCKF algorithm, as shown in the following equation. Where H is the merged measurement matrix, R is the merged measurement noise covariance matrix, and r is the merged residual vector. This is achieved through state correction. Correcting the inertial navigation state .

[0089] 15) State Management. When a camera frame no longer observes any feature points, the camera state is removed from the state vector, and the row and column corresponding to the camera pose are deleted from the system covariance matrix.

[0090] The above detailed embodiments are a description of the present invention. It should not be considered that the specific embodiments of the present invention are limited to these descriptions. For those skilled in the art, several simple deductions and substitutions can be made without departing from the concept of the present invention, and all of these should be considered to fall within the protection scope of the present invention.

Claims

1. A visual odometry method for MSCKF under heading constraints, characterized in that, It can be applied to one or more of the following: inertial navigation, cameras, and dual-antenna direction finding, magnetic sensors, or polarized light direction finding devices used to provide heading constraints.

2. The MSCKF visual odometry method under heading constraints according to claim 1, characterized in that, The process includes the following steps: Step 1: Define the filter state vector, which consists of the inertial navigation state error and the camera state error; Step 2: Perform system initialization, which initializes the inertial navigation state, camera state, system covariance matrix, system noise matrix, and feature tracking list. Step 3: The inertial navigation system (INS) acquires angular velocity and acceleration information at its operating frequency, and the camera acquires image information at its operating frequency. Step 4: Pure inertial calculations are performed on the INS angular velocity *w* and acceleration *a* acquired in Step 3. Step 5: According to the MSCKF algorithm, the INS error equation is written to obtain the filter state equation. The state equation is discretized to obtain the filter time update equation. Step 6: The measurement update time is determined based on whether a visual image has been received. If it has, Step 7 is executed; otherwise, Step 3 is returned. Step 7: According to the MSCKF algorithm, the camera pose error is amplified into the system state error. The Jacobian matrix of the newly added camera pose error relative to the original state error is calculated, and the amplified system covariance matrix is ​​calculated. Step 8: Feature tracking is performed on the image information received in Step 3. The attribution logic of all feature points is traversed, and the lost feature points are added to the optimization list. The remaining feature points are updated in the feature tracking list. Step 9: Traverse the list of features to be optimized in Step 8, and use the tracking trajectory of each feature point to triangulate the feature point and calculate its position coordinates to construct the visual measurement residual vector; Step 10: Construct a visual feature point measurement model; Step 11: Construct a heading constraint information measurement update model; Step 12: Merge the visual feature point measurement model from Step 10 and the heading constraint measurement model from Step 11 to obtain the total filter measurement model and the observation noise covariance matrix, respectively; Step 13: Perform filter measurement update according to the MSCKF algorithm, using the state correction quantity. Correct the inertial navigation state.

3. The MSCKF visual odometry method under heading constraints according to claim 1, characterized in that, In step 4, the inertial calculation method is not limited to one of single sample or fourth-order Runge-Kutta.

4. The MSCKF visual odometry method under heading constraints according to claim 1, characterized in that, In step 8, the feature tracking method includes one of LK optical flow feature tracking and ORB feature descriptor tracking.

5. The MSCKF visual odometry method under heading constraints according to claim 1, characterized in that, In step 8, the specific process of determining the attribution logic of each feature point includes: a) whether the feature point belongs to the feature tracking list. If it does not belong to the feature tracking list, it is a new feature point, and a new track is added to the feature tracking list; otherwise, proceed to criterion b); b) if the feature point belongs to the feature tracking list, it is not a new feature point. Determine whether it has been lost. If it has not been lost, it is continuously tracked. Update the observation coordinates of the track corresponding to the feature point in the feature tracking list; otherwise, proceed to criterion c); c) if the feature point belongs to the feature tracking list and has been lost, it is usable for measurement updates. Add the track of the feature point to the list to be optimized for feature point triangulation calculation.

6. The MSCKF visual odometry method under heading constraints according to claim 1, characterized in that, Step 10 specifically includes the following steps: 10-1) Using the feature point location coordinates obtained in step 9, construct a residual model of a single feature point against a single camera observation frame according to the MSCKF algorithm; 10-2) Merge all camera observation frames of the single feature point from step 10-1) according to the MSCKF algorithm to obtain a residual model of the single feature point against all camera observation frames; 10-3) Linearize the single feature point residual model obtained in step 10-2) according to the MSCKF algorithm to obtain a single feature point measurement model; 10-4) Perform null-space projection on the single feature point measurement model obtained in step 10-3) according to the MSCKF algorithm to remove interference terms related to the feature point location in the measurement model; 10-5) Merge the measurement models of all feature points in the current frame according to the MSCKF algorithm; 10-6) Perform QR decomposition on the visual correlation measurement model of the current frame obtained in step 10-5) according to the MSCKF algorithm to reduce the computational load.

7. The MSCKF visual odometry method under heading constraints according to claim 1, characterized in that, In step 11, the following steps are specifically performed: 11-1) Based on the heading constraint information, construct the constraint equation between the heading error angle and the platform misalignment angle, as shown in the following equation: Among them, the inertial navigation attitude Euler angles are The misalignment angle of the three-axis platform is ， This represents the heading angle error; the heading constraint information is not limited to methods such as dual-antenna direction finding, magnetic sensor direction finding, or polarized light direction finding; 11-2) Constructing a residual model using the heading constraint information. in It is the heading constraint information residual term. It is heading constraint information. This is the inertial navigation heading angle estimated by the filter; 11-3) Using the heading constraint equation in step 11-1), calculate the Jacobian matrix of the heading constraint information residual term with respect to the original state error of the filter, as shown in the following formula: ;11-4) Using the Jacobian matrix obtained in step 11-3), construct the measurement matrix under the heading constraint information as shown in the following formula: in It is the Jacobian matrix of the error between the heading constraint information residual term and the original state of the filter. It is the heading constraint information observation noise vector; 11-5) The heading constraint information observation noise vector in step 11-4). Construct the observation noise covariance matrix 。 8. The MSCKF visual odometry method under heading constraints according to claim 1, characterized in that, When a camera frame no longer observes any feature points, the camera state is removed from the state vector, and the row and column corresponding to the camera pose are deleted from the system covariance matrix.

Images