Fiber-optic inertial navigation system fused with vision for anti-jamming

Abstract

The application relates to the technical field of high-precision positioning, and discloses an anti-vibration interference optical fiber inertial navigation and visual fusion positioning system, which comprises a data acquisition module, a first identification module, a positioning compensation module, a second identification module and a positioning correction module; wherein: the data acquisition module is used for acquiring inertial navigation data and a positioning image sequence of an aircraft; the first identification module is used for identifying interference fluctuations of the inertial navigation data; the positioning compensation module is used for constructing an attitude constraint curve and performing constraint compensation on the inertial navigation data with the interference fluctuations; the second identification module is used for identifying heading drift of the inertial navigation data; and the positioning correction module is used for constructing a heading reference sequence based on the positioning image sequence and performing heading correction on the inertial navigation data with the heading drift. The application improves the positioning precision of the aircraft under vibration, shaking and complex maneuvering conditions.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of high-precision positioning, and in particular to an anti-vibration interference optical fiber inertial navigation and visual fusion positioning system. BACKGROUND

[0002] With the wide application and deployment of unmanned aerial vehicles, higher requirements are put forward for their anti-interference, especially anti-strong vibration interference, autonomous positioning capabilities. Optical fiber inertial navigation systems have the characteristics of short-time high precision, and are often used as an important compensation means when GNSS signals fail. Visual navigation can enhance the system redundancy in complex environments. However, in strong vibration or transient high dynamic flight states, both types of systems have weak anti-interference points, and fusion calculation is prone to problems such as attitude drift, heading fluctuation, and image blur, resulting in navigation failure or sudden drop in precision.

[0003] Inertial navigation attitude drift is easily amplified by high-frequency vibration, causing short-time abnormalities in navigation output; visual images are prone to blur and defocus under high dynamic vibration, causing unstable key frame selection and affecting the continuity of navigation and positioning; in the state of yaw or turning, the heading estimation is prone to distortion, and the inertial navigation direction deviates from the actual direction; existing fusion algorithms lack anti-vibration adaptability and cannot guarantee the stability of the fusion navigation output when the consistency of multi-source data decreases.

[0004] A navigation and positioning method and system based on an inertial navigation system are disclosed in Chinese Patent No. CN119916422B. The method includes: obtaining multi-source navigation raw data, including inertial navigation data, satellite navigation data, and atmospheric information data; performing feature extraction through deep learning to obtain multi-modal feature data; performing quality assessment on satellite navigation data to determine whether the satellite navigation signal is valid. When the satellite navigation signal is valid, a federated filtering algorithm is used for combined navigation calculation, and an atmospheric information error model is established; when the satellite navigation signal is invalid, the error model is used to compensate the atmospheric information, and the compensated atmospheric information is fused with the inertial navigation data to achieve high-precision navigation and ensure the flight safety of unmanned aerial vehicles in complex environments.

[0005] A kind of airplane airborne GPS and inertial navigation system combination positioning method disclosed in Chinese patent with authorization announcement No.CN104570033B, including: constructing inertial navigation angular velocity error model, through GPS correction inertial navigation angular velocity error;That is, construct gyro output angular velocity error model, through static or horizontal straight line motion correction inertial navigation angular velocity zero drift error, through Kalman filter linear fitting correction inertial navigation angular velocity linear error, finally realize the error correction of inertial navigation angular velocity;Acceleration error model of accelerometer output is constructed, through correction inertial navigation acceleration zero drift error, through Kalman filter fitting correction inertial navigation acceleration linear error, to realize the error correction of inertial navigation acceleration.The scheme can effectively combine GPS and INS technology by establishing gyro output angular velocity error model, acceleration error model of accelerometer output, and through GPS dynamic correction INS three-axis angular speed and three-axis acceleration.

[0006] The above technical solutions all have the problems raised in the background art of the present application: in the yaw or turning state, the heading estimation is easy to be distorted, and the inertial navigation direction deviates from the actual direction.

[0007] The information disclosed in this BACKGROUND section is only for the purpose of increasing the understanding of the background of the present application and should not be taken as admitting that such information in the prior art, known by those of ordinary skill in the art. SUMMARY

[0008] The technical problem to be solved by the present application is to overcome the defects of the prior art and provide an anti-vibration interference optical fiber inertial navigation and visual fusion positioning system to improve the positioning accuracy of the aircraft under vibration, shaking and complex maneuvering conditions.

[0009] To solve the above technical problems, the present application provides the following technical solutions:

[0010] An anti-vibration interference optical fiber inertial navigation and visual fusion positioning system, comprising a data acquisition module, a first identification module, a positioning compensation module, a second identification module and a positioning correction module;Wherein:

[0011] The data acquisition module is used to acquire inertial navigation data and positioning image sequences of the aircraft.

[0012] The first identification module is used to identify the interference fluctuation of the inertial navigation data.

[0013] The positioning compensation module is used to construct an attitude constraint curve and compensate the inertial navigation data with interference fluctuation based on the attitude constraint curve.

[0014] The second identification module is used to identify the heading drift of the inertial navigation data.

[0015] The positioning correction module constructs a heading reference sequence based on the positioning image sequence, and performs heading correction on the inertial navigation data with heading drift based on the heading reference sequence.

[0016] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the application, the inertial navigation data includes attitude angles, three-axis angular velocities, and three-axis linear accelerations of the aircraft at each time point; the attitude angles include a pitch angle, a roll angle, and a heading angle; the positioning image sequence includes positioning images at different time points;

[0017] The first identification module is configured with a first identification strategy for identifying interference fluctuations in the inertial navigation data; the first identification strategy includes:

[0018] A first detection window with a length of N is set; N is a positive integer; the first detection window includes three-axis linear accelerations at the most recent N continuous time points;

[0019] The variance of the three-axis linear acceleration at each time point in the first detection window is calculated as a first fluctuation index; the first identification unit is further configured with a first fluctuation threshold; if the first fluctuation index is greater than the first fluctuation threshold, the inertial navigation data in the first detection window has interference fluctuations.

[0020] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the application, the positioning compensation module includes an attitude constraint unit; the attitude constraint unit is configured to construct an attitude constraint curve, specifically including:

[0021] The attitude angles at the K continuous time points before the first detection window or the second detection window corresponding to the inertial navigation data with interference fluctuations are extracted, and a reference window is formed;

[0022] Based on the attitude angles in the reference window, an equation of the change curve of the attitude angle with time is fitted as the equation of the attitude constraint curve.

[0023] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the application, the positioning compensation module further includes a constraint compensation unit; the constraint compensation unit is configured with a constraint compensation strategy for performing constraint compensation on the inertial navigation data with interference fluctuations; the constraint compensation strategy specifically includes:

[0024] Based on the equation of the attitude constraint curve, a reference value of the attitude angle at each time point in the first detection window or the second detection window corresponding to the inertial navigation data with interference fluctuations is calculated;

[0025] The attitude angle residual error of the inertial navigation data at each time point with interference fluctuations is calculated.

[0026] The constraint compensation unit is also configured with a pose deviation threshold value; for the inertial navigation data at any moment, if the corresponding pose angle residual error is greater than the pose deviation threshold value, the pose angle is fused with the reference value of the pose angle at the corresponding moment to obtain the pose angle after constraint compensation.

[0027] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the present application, wherein: the second identification module comprises a heading detection unit and a curvature identification unit;

[0028] The heading detection unit is configured to construct a heading stability factor; and the curvature identification unit is configured to construct a heading curvature sequence and identify the heading drift of the inertial navigation data based on the heading stability factor and the heading curvature sequence.

[0029] The heading detection unit constructs the heading stability factor in the following manner:

[0030] The heading detection unit constructs the heading stability factor in the following manner:

[0031] The heading detection unit constructs the heading stability factor in the following manner:

[0032] The heading detection unit constructs the heading stability factor in the following manner:

[0033] The heading detection unit constructs the heading stability factor in the following manner:

[0034] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the present application, wherein: the curvature identification unit constructs the heading curvature sequence in the following manner:

[0035] The heading detection unit constructs the heading stability factor in the following manner:

[0036] The heading detection unit constructs the heading stability factor in the following manner:

[0037] The curvature recognition unit is further configured with a heading drift threshold value; if the heading stability factor of the drift detection window is greater than the heading drift threshold value, and the heading of at least one time point in the drift detection window jumps, then the inertial navigation data corresponding to the drift detection window has heading drift.

[0038] The curvature recognition unit is further configured with a heading drift threshold value; if the heading stability factor of the drift detection window is greater than the heading drift threshold value, and the heading of at least one time point in the drift detection window jumps, then the inertial navigation data corresponding to the drift detection window has heading drift.

[0039] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the present application, the positioning correction module comprises a reference heading unit.

[0040] The reference heading unit constructs a heading reference sequence based on the positioning image sequence of the aircraft, and specifically comprises:

[0041] Each frame of the positioning image sequence is identified respectively whether it is stable or not;

[0042] Any two adjacent frames of the positioning image sequence that are both stable are marked as a group of target positioning images; the former frame in any group of target positioning images is marked as a reference frame, and the latter frame is marked as a target frame;

[0043] The visual heading angle of each target frame is calculated respectively; the visual heading angles of different time points are interpolated to obtain a heading reference sequence; the heading reference sequence contains the visual heading angle of each time point, and the visual heading angle and the heading angle in the inertial navigation data correspond to each other in time.

[0044] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the present application, the reference heading unit calculates the visual heading angle of the target frame at any time point by the following method:

[0045] Feature point detection is performed between the target frame and the corresponding reference frame respectively; feature point matching is performed between the target frame and the corresponding reference frame to obtain the coordinates of each feature point between the target frame and the corresponding reference frame;

[0046] Based on the coordinates of each feature point between the target frame and the corresponding reference frame, the motion vector of each feature point in the target frame is calculated, and the main motion direction of the target frame is extracted based on the motion vectors of different feature points in the target frame;

[0047] The main motion direction of the target frame is projected into the inertial navigation coordinate system to obtain the visual heading angle at the corresponding time point.

[0048] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the application, wherein: the reference heading unit is configured with an image detection strategy for identifying whether any frame of positioning image is stable; the image detection strategy specifically includes:

[0049] For any frame of positioning image, the absolute value of the edge intensity of each pixel point is calculated respectively, and the average value is taken as the definition index of the corresponding frame of positioning image; the reference heading unit is further configured with a definition threshold; if the definition index of any frame of positioning image is greater than the definition threshold, the corresponding frame of positioning image is stable, otherwise the corresponding frame of positioning image is unstable;

[0050] The image detection strategy further includes: feature point detection is performed on each frame of positioning image respectively, and the feature scale of each feature point in each frame of positioning image is recorded; for any frame of positioning image, the variance of the feature scale of each feature point is calculated as the stability index of the corresponding frame of positioning image; the reference heading unit is further configured with a first stability threshold and a second stability threshold; if the stability index of any frame of positioning image is greater than the first stability threshold and less than the second stability threshold, the corresponding frame of positioning image is stable, otherwise the corresponding frame of positioning image is unstable.

[0051] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the application, wherein: the positioning correction module further includes a heading correction unit; the heading correction unit is configured with a heading correction strategy for performing heading correction on the inertial navigation data with heading drift; the heading correction strategy specifically includes:

[0052] For any inertial navigation data with heading drift, the heading angle in the inertial navigation data and the visual heading angle corresponding to the timestamp in the heading reference sequence are fused through Kalman filtering to obtain the corrected heading angle.

[0053] Compared with the prior art, the application has the following beneficial effects:

[0054] The application can identify and effectively correct in time when high-frequency jitter, peak disturbance or short-time abnormality occurs in inertial navigation, so that the attitude change output by the inertial navigation remains continuous, smooth and consistent with the real motion trend of the body; in the case of GNSS signal interruption or unavailability, the attitude positioning of the navigation system remains stable.

[0055] Through comprehensive judgment of the heading change mode and the stability characteristics, the application can accurately identify and correct abnormal jumps caused by interference, and reduce the cumulative influence of heading drift in subsequent navigation. By judging the stability and trend of data from different sources, the correction process can adapt to different working conditions, and more smooth and reliable navigation output is realized. BRIEF DESCRIPTION OF DRAWINGS

[0056] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed to be used in the embodiment description will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without any creative effort on the basis of these drawings. Among them:

[0057] Figure 1 The structural schematic diagram of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system provided by the present application is shown in the figure.

[0058] Figure 2 The functional schematic diagram of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system provided by the present application is shown in the figure. DETAILED DESCRIPTION

[0059] The technical solutions of the present application will be described in detail below by means of the drawings and specific embodiments. It should be understood that the embodiments of the present application and the specific features in the embodiments are detailed descriptions of the technical solutions of the present application, but not limitations of the technical solutions of the present application. The technical features in the embodiments of the present application and the embodiments can be combined with each other without conflict.

[0060] The present embodiment introduces an anti-vibration interference optical fiber inertial navigation and visual fusion positioning system. Referring to Figure 1 , the system comprises a data acquisition module, a first identification module, a positioning compensation module, a second identification module, and a positioning correction module. The functions of each module are shown in Figure 2 . Among them:

[0061] The data acquisition module is used to acquire the inertial navigation data and the positioning image sequence of the aircraft.

[0062] The data acquisition module comprises an inertial navigation unit and a visual navigation unit.

[0063] The inertial navigation unit is used to acquire the inertial navigation data of the aircraft. The inertial navigation data comprises the attitude angle, the three-axis angular velocity, and the three-axis linear acceleration of the aircraft at each time. The attitude angle comprises the pitch angle, the roll angle, and the heading angle.

[0064] In the present embodiment, the inertial navigation unit is configured with an angular velocity measurement subunit, such as an optical fiber gyroscope, for real-time detection of the three-axis angular velocity of the aircraft. It is also configured with a linear acceleration measurement subunit, such as an accelerometer, for real-time detection of the three-axis linear acceleration of the aircraft. The inertial navigation unit is also configured with an inertial calculation subunit, which can calculate the attitude angle at each time, as well as the three-dimensional velocity and three-dimensional coordinates of the aircraft at each time, according to the three-axis angular velocity and the three-axis linear acceleration at consecutive times.

[0065] The visual navigation unit is configured to collect a sequence of positioning images of the aerial vehicle; the sequence of positioning images comprises positioning images at different time instants;

[0066] In this embodiment, the positioning images are collected by a camera configured on the visual navigation unit. The camera is fixedly installed on the aerial vehicle and moves synchronously with the body attitude of the aerial vehicle to periodically output the positioning images at a set frame rate.

[0067] The first identification module is configured to identify interference fluctuations in the inertial navigation data;

[0068] The first identification module comprises a first identification unit and a second identification unit;

[0069] The first identification unit is configured with a first identification strategy for identifying interference fluctuations in the inertial navigation data; the first identification strategy comprises:

[0070] A first detection window of length N is set; N is a positive integer; the first detection window comprises three-axis linear accelerations at the most recent N consecutive time instants;

[0071] The variance of the three-axis linear acceleration at each time instant in the first detection window is calculated as a first fluctuation indicator; the first identification unit is further configured with a first fluctuation threshold value; if the first fluctuation indicator is greater than the first fluctuation threshold value, the inertial navigation data in the first detection window has interference fluctuations.

[0072] The specific value of the first fluctuation threshold value can be set by a person skilled in the art based on actual needs. When calculating the variance of the three-axis linear acceleration at each time instant in the first detection window, the variance of the linear acceleration in each direction can be calculated respectively, and the mean or maximum value can be taken. When the first fluctuation indicator is greater than the first fluctuation threshold value, it indicates that the aerial vehicle is in a state of continuous disturbance.

[0073] The second identification unit is configured with a second identification strategy for identifying interference fluctuations in the inertial navigation data; the second identification strategy comprises:

[0074] A second detection window of length M is set; M is a positive integer; the second detection window comprises three-axis angular velocities at the most recent M consecutive time instants;

[0075] The three-axis angular velocity data in the second detection window is subjected to frequency domain transformation to obtain a corresponding frequency spectrum; the high-frequency energy of the three-axis angular velocity data in the second detection window is calculated based on the frequency spectrum as a second fluctuation indicator; the first identification unit is further configured with a second fluctuation threshold value; if the second fluctuation indicator is greater than the second fluctuation threshold value, the inertial navigation data in the second detection window has interference fluctuations.

[0076] The skilled person in the art can set the specific value of the second fluctuation threshold based on actual needs. When calculating the high-frequency energy of the three-axis angular velocity data in the second detection window, the high-frequency energy of the angular velocity in each direction can be calculated respectively, and the mean or maximum value can be taken. When calculating the high-frequency energy of the angular velocity in any direction, the frequency threshold can be set based on actual needs, the frequency components greater than the frequency threshold are marked as high-frequency components, and the total energy of the high-frequency components is calculated. For example, the frequency threshold can be set to 20 Hz, and the angular velocity change below 20 Hz is mainly caused by the real maneuver of the body, while the change above 20 Hz is mainly caused by motor vibration and structural elastic jitter, thereby distinguishing the real attitude change from the jitter noise. When the second fluctuation index is greater than the second fluctuation threshold, it indicates that the inertial navigation data of the aircraft is in a high-frequency vibration interference state.

[0077] The positioning compensation module is configured to construct an attitude constraint curve, and constrain and compensate the inertial navigation data with interference fluctuation based on the attitude constraint curve.

[0078] The positioning compensation module comprises an attitude constraint unit and a constraint compensation unit.

[0079] The attitude constraint unit is configured to construct an attitude constraint curve, and specifically comprises:

[0080] extracting the attitude angles at the K continuous time points before the first detection window or the second detection window corresponding to the inertial navigation data with interference fluctuation, and composing a reference window;

[0081] fitting an equation of the change curve of the attitude angle with time based on the attitude angles in the reference window as the equation of the attitude constraint curve.

[0082] Optionally, the attitude constraint curve of the reference window is fitted by least square fitting. The change curves of the pitch angle, the roll angle and the heading angle can be fitted respectively, or the pitch angle, the roll angle and the heading angle at each time point can be composed into an attitude angle vector, and the least square fitting is performed uniformly. For example, the attitude angle vector at any time point is denoted as [θ_p, θ_r, θ_y], where θ_p, θ_r and θ_y are the pitch angle, the roll angle and the heading angle at the corresponding time point, respectively. For example, when the sampling period is 10 ms, the attitude angle vectors in the reference window can be [2.1°, 0.5°, 35.0°], [2.2°, 0.6°, 35.1°], [2.8°, 0.9°, 35.6°]. When fitting uniformly, the above attitude angle vectors are regarded as three-dimensional time functions with time as the independent variable, and the curve fitting is performed by the least square method, for example, in the form of a second-order polynomial. By minimizing the sum of squares of residuals between the attitude angle vectors at each time point in the reference window and the fitted curve, the attitude constraint curve describing the smooth change of the pitch angle, the roll angle and the heading angle with time is obtained.

[0083] The attitude angle in the reference window is regarded as a real attitude change trajectory in a normal state, and a posture constraint curve fitted based on the same describes a smooth change trend of the attitude angle of the aircraft over time in a stable state, which is used to provide a relatively smooth attitude reference during an inertial navigation anomaly.

[0084] The constraint compensation unit is configured with a constraint compensation strategy for constraint compensation of the inertial navigation data with disturbance fluctuation; the constraint compensation strategy specifically includes:

[0085] The constraint compensation unit is configured with a constraint compensation strategy for constraint compensation of the inertial navigation data with disturbance fluctuation; the constraint compensation strategy specifically includes:

[0086] The constraint compensation unit is configured with a constraint compensation strategy for constraint compensation of the inertial navigation data with disturbance fluctuation; the constraint compensation strategy specifically includes:

[0087] The constraint compensation unit is configured with a constraint compensation strategy for constraint compensation of the inertial navigation data with disturbance fluctuation; the constraint compensation strategy specifically includes:

[0088] The constraint compensation unit is configured with a constraint compensation strategy for constraint compensation of the inertial navigation data with disturbance fluctuation; the constraint compensation strategy specifically includes:

[0089] The second identification module is configured to identify a heading drift of the inertial navigation data.

[0090] The second identification module includes a heading detection unit and a curvature identification unit.

[0091] The heading detection unit is configured to construct a heading stability factor, and the curvature identification unit is configured to construct a heading curvature sequence and identify a heading drift of the inertial navigation data based on the heading stability factor and the heading curvature sequence.

[0092] The heading detection unit constructs the heading stability factor in the following manner:

[0093] construct an angular velocity vector at each time instant based on the three-axis angular velocity at each time instant; and construct a linear acceleration vector at each time instant based on the three-axis linear acceleration at each time instant;

[0094] In this embodiment, the three-axis linear acceleration or the three-axis angular velocity at any time instant comprises components in three directions, and the angular velocity vector or the linear acceleration vector at each time instant is represented by a corresponding three-dimensional vector.

[0095] calculate an angle between the angular velocity vector and the linear acceleration vector at each time instant, denoted as an acceleration angle at each time instant;

[0096] calculate a heading drift rate at each time instant; the heading drift rate at any time instant is the difference between the acceleration angle at the corresponding time instant and the acceleration angle at the adjacent previous time instant;

[0097] set a drift detection window; the drift detection window comprises the heading drift rates at the most recent consecutive m time instants; m is a positive integer; and calculate the mean of the heading drift rates at each time instant in the drift detection window as a heading stability factor of the drift detection window.

[0098] The linear acceleration vector and the angular velocity vector respectively reflect the linear motion trend and the rotation motion trend of the aircraft, and the application reflects the consistency between the linear motion trend and the rotation motion trend of the aircraft body through the angle between the two. When the aircraft is normally maneuvered, the two have a stable corresponding relationship; when the inertial navigation attitude solution drifts, the direction of the angular velocity will be mismatched with the direction of the acceleration, causing the heading stability factor to significantly increase.

[0099] The curvature recognition unit constructs a heading curvature sequence in the following manner:

[0100] calculate a heading curvature at each time instant; the heading curvature at any time instant is the difference between the heading angle at the corresponding time instant and the heading angle at the adjacent previous time instant; and the heading curvature sequence is a time sequence composed of the heading curvatures at each time instant.

[0101] The curvature recognition unit is configured with a drift recognition strategy for recognizing the heading drift of the inertial navigation data; and the drift recognition strategy specifically comprises:

[0102] recognize whether the heading at any time instant jumps; the curvature recognition unit is configured with a heading curvature threshold value; if the heading curvature at any time instant is greater than the heading curvature threshold value, the heading at the corresponding time instant jumps;

[0103] The curvature recognition unit is further configured with a heading drift threshold value; if the heading stability factor of the drift detection window is greater than the heading drift threshold value, and the heading at at least one time instant in the drift detection window jumps, the inertial navigation data corresponding to the drift detection window has a heading drift.

[0104] The specific values of the heading curvature threshold and the heading drift threshold can be set by the skilled person in the art based on actual needs. The heading angle changes continuously and smoothly when the aircraft is normally turning; the heading angle abnormally jumps when the inertial navigation data drifts. The application uses the heading curvature as a supplementary detection means to prevent misjudgment of normal large maneuver actions of the aircraft such as rapid turning as abnormal drift.

[0105] The positioning correction module constructs a heading reference sequence based on the positioning image sequence, and performs heading correction on the inertial navigation data with heading drift based on the heading reference sequence.

[0106] The positioning correction module comprises a reference heading unit and a heading correction unit.

[0107] The reference heading unit constructs a heading reference sequence based on the positioning image sequence of the aircraft, and specifically comprises:

[0108] It is determined whether each frame of the positioning image sequence is stable respectively.

[0109] Any two adjacent frames of the positioning image sequence that are stable are marked as a group of target positioning images; the previous frame in any group of target positioning images is marked as a reference frame, and the next frame is marked as a target frame.

[0110] The visual heading angle at the corresponding time of each target frame is calculated respectively; the visual heading angles at different times are interpolated to obtain a heading reference sequence; the heading reference sequence comprises the visual heading angle at each time, and the visual heading angle and the heading angle in the inertial navigation data correspond to each other in time.

[0111] The application only calculates the visual heading angle at the corresponding time when the two adjacent frames of the positioning image are stable, thereby ensuring that the feature points in the positioning image are not shaken and blurred, the feature points have high consistency, and the direction information is reliable. The visual heading angle is curve-fitted by using a fitting method such as spline fitting, and the visual heading angle is interpolated with the inertial navigation time stamp as a reference, so that the time stamp of the visual heading angle completely corresponds to the time stamp of the heading angle in the inertial navigation data.

[0112] In this embodiment, the visual heading angle is only available at partial discrete timestamps when the visual heading angle is calculated only for the stable image frames. For example, when the camera frame rate is 20 Hz and the inertial navigation data output frequency is 100 Hz, the interval of the visual heading angle corresponding timestamps is 0.05 s, while the interval of the inertial navigation data timestamps is 0.01 s. Firstly, the discrete visual heading angle data is fitted by a spline to construct a continuous visual heading angle curve, for example, a cubic spline, so that the curve is continuous and the first and second derivatives are smooth at the visual heading angle sampling points; then, the interpolation calculation is performed on the continuous curve with the timestamps of the inertial navigation data as the reference, so that the corresponding visual heading angle is obtained at each inertial navigation data timestamp, and the visual heading angle and the heading angle in the inertial navigation data are strictly one-to-one corresponding in time. For example, the visual heading angle collected at 0.05 s is 34.8°, the visual heading angle collected at 0.1 s is 35.0°, and the visual heading angle collected at 0.15 s is 35.2°, and the interpolation heading angle is directly calculated to be 35.08° by the spline curve.

[0113] The reference heading unit calculates the visual heading angle at the corresponding time of any frame target frame by the following method:

[0114] Feature point detection is performed between the target frame and the corresponding reference frame respectively; feature point matching is performed between the target frame and the corresponding reference frame to obtain the coordinates of each feature point between the target frame and the corresponding reference frame;

[0115] In the feature point detection between the target frame and the corresponding reference frame, the ORB feature detection algorithm can be used. The specific settings are as follows: the maximum number of feature points is 1000, the FAST threshold is 20, the number of image pyramid layers is 8, and the scaling ratio of each layer is 1.2; in each frame image, the corner points are first detected by the FAST algorithm, and then the ORB descriptor is calculated for each corner point. In the feature point matching stage, the brute force matching method based on Hamming distance can be used, and cross-validation is set to improve the matching reliability. After the matching is completed, the false matches can be further removed by a pre-set distance threshold, for example, only the matching pairs with a Hamming distance less than 40 are retained, so that the stable and reliable feature point correspondence relationship is obtained.

[0116] Based on the coordinates of each feature point between the target frame and the corresponding reference frame, the motion vector of each feature point in the target frame is calculated, and the main motion direction of the target frame is extracted based on the motion vectors of different feature points in the target frame;

[0117] In this embodiment, the coordinate difference of each feature point between the target frame and the reference frame is calculated to obtain the motion vector of each feature point; the mean vector of all motion vectors is calculated, and the direction recorded by the mean vector is the main motion direction. For example, for any successfully matched feature point, if its pixel coordinates in the reference frame are (x_r, y_r) and its pixel coordinates in the target frame are (x_t, y_t), the motion vector of the feature point in the target frame is (x_t-x_r, y_t-y_r). For example, when the coordinates of a feature point in the reference frame are (320, 240) and the coordinates of the feature point in the target frame are (330, 242), the corresponding motion vector is (10, 2).

[0118] The main motion direction of the target frame is projected into the inertial navigation coordinate system to obtain the visual heading angle at the corresponding moment.

[0119] The main motion direction is a vector in the image coordinate system, while the heading angle in the inertial navigation data is defined in the inertial navigation coordinate system, and needs to be converted through the camera parameters to make them comparable in the same coordinate system. First, the image plane is projected into the camera coordinate system plane through the calibrated camera intrinsic parameters, so that the direction vector of the main motion direction in the image coordinate system is converted into a unit vector in the camera coordinate system. Then, the extrinsic parameters of the camera are used, i.e., according to the relative attitude of the camera and the aircraft, the direction vector in the camera coordinate system is converted into a direction vector in the inertial navigation coordinate system. The direction vector of the main motion direction in the inertial navigation coordinate system is projected onto the horizontal plane, and the included angle between it and the forward axis of the aircraft body is calculated, i.e., the visual heading angle calculated based on the positioning image is obtained.

[0120] The reference heading unit is configured with an image detection strategy for identifying whether any frame of positioning image is stable; the image detection strategy specifically includes:

[0121] For any frame of positioning image, the absolute value of the edge intensity of each pixel point is calculated respectively, and the mean value is taken as the sharpness index of the corresponding frame of positioning image; the reference heading unit is also configured with a sharpness threshold; if the sharpness index of any frame of positioning image is greater than the sharpness threshold, the corresponding frame of positioning image is stable, otherwise the corresponding frame of positioning image is unstable.

[0122] The person skilled in the art can set the specific value of the clarity threshold based on actual needs. Optionally, the pixel gradient of each pixel point is extracted based on a Laplace operator as the edge strength thereof. For example, when calculating the image clarity index, a 3*3 Laplace operator can be used to perform convolution operation on the positioning image to extract the edge response of each pixel point. For example, the Laplace operator can use a discrete template [0-10; -14-1; 0-10] with positive center weight and negative neighborhood weight to process the gray-scale image. The absolute value of the Laplace response value of each pixel point is taken as the edge strength of the pixel point, and the average of the edge strengths of all pixel points in the entire frame image is taken to obtain the clarity index of the corresponding frame positioning image, which is used to determine whether the frame image is stable.

[0123] The image detection strategy further comprises: performing feature point detection on each frame positioning image respectively, and recording the feature scale of each feature point in each frame positioning image; for any frame positioning image, calculating the variance of the feature scale of each feature point as the stability index of the corresponding frame positioning image; the heading reference unit is further configured with a first stability threshold and a second stability threshold; if the stability index of any frame positioning image is greater than the first stability threshold and less than the second stability threshold, the corresponding frame positioning image is stable, otherwise the corresponding frame positioning image is unstable.

[0124] Optionally, the method of performing feature point detection on any frame positioning image and recording the feature scale of each feature point is as follows: an L-layer image pyramid of the positioning image is constructed, L can take a value of 4-8, and each layer of image is scaled by a fixed ratio; for each layer of image in the image pyramid, a FAST feature detection algorithm is executed, each detected feature point and its response value are recorded, and the layer number of each feature point is marked as the feature scale of the corresponding feature point; a number of feature points with the largest response value are selected, and the variance of the feature scale thereof is calculated as the stability index of the positioning image. The person skilled in the art can set the specific value of the first stability threshold and the second stability threshold based on actual needs. When the stability index is too small, it indicates that the feature points with large response values are concentrated in a certain layer of the pyramid. In a vibration scene, the positioning image is prone to local blur or detail loss, resulting in that significant features can only be detected at a single scale layer, and the positioning image lacks stable significant features at multiple scales. When the stability index is too large, it indicates that the significant feature points are too scattered in each layer of the pyramid, lacking consistent stable scales. Vibration can cause scale distortion in local regions of the positioning image, such as partial blur, which is a manifestation of multi-scale feature disorder of the image due to vibration.

[0125] The heading correction unit is configured with a heading correction strategy for correcting the heading of the inertial navigation data with heading drift; the heading correction strategy specifically comprises:

[0126] For any inertial navigation data with heading drift, the heading angle in the inertial navigation data is fused with the visual heading angle corresponding to the time stamp in the heading reference sequence through Kalman filtering to obtain a corrected heading angle.

[0127] In the embodiment, one-dimensional Kalman filtering is used to fuse the heading angle of inertial navigation and the visual heading angle. The heading angle output by the inertial navigation system is taken as a predicted state, and the visual heading angle corresponding to the time stamp is taken as an observation. The Kalman filtering takes the recursive result of the heading angle of the inertial navigation as a priori estimation, and calculates the Kalman gain according to the prediction covariance of the inertial navigation and the observation noise covariance of the visual heading angle. At each time, the deviation between the inertial navigation heading angle and the visual heading angle is corrected by using the Kalman gain to obtain an updated heading angle estimation value. Through the recursive updating mode of the Kalman filtering, the smooth correction of the heading angle is realized, and the accumulation of the inertial navigation heading drift in time is inhibited.

[0128] The inertial navigation data compensated and corrected through the embodiment can be packaged into a standardized data structure, sent to the flight control computer or the upper control system of the aircraft through the bus interface, and used in real time by the flight control, autonomous navigation and other modules.

[0129] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system or a computer program product. Therefore, the present application can adopt a complete hardware embodiment, a complete software embodiment or an embodiment combining software and hardware aspects. Moreover, the present application can adopt a computer program product implemented on one or more computer usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer usable program codes.

[0130] The embodiments of the present application are described above in combination with the drawings, but the present application is not limited to the above specific embodiments, and the above specific embodiments are only illustrative but not limiting. Those skilled in the art can make many forms without departing from the purpose and the protected scope of the present application under the inspiration of the present application, and these are all within the protection of the present application.

Claims

1. A vibration-resistant fiber optic inertial navigation and vision fusion positioning system, characterized in that: It includes a data acquisition module, a first identification module, a positioning compensation module, a second identification module, and a positioning correction module; wherein: The data acquisition module is used to collect the aircraft's inertial navigation data and positioning image sequences; The inertial navigation data includes the aircraft's attitude angles, three-axis angular velocities, and three-axis accelerations at each moment; the attitude angles include pitch angle, roll angle, and yaw angle; the positioning image sequence includes positioning images at different moments. The first identification module is used to identify interference fluctuations in the inertial navigation data; The first identification module is configured with a first identification strategy for identifying interference fluctuations in inertial navigation data; the first identification strategy includes: A first detection window of length N is set; N is a positive integer; the first detection window includes the triaxial accelerations of the most recent N consecutive time moments; The variance of the three-axis acceleration at each moment in the first detection window is calculated as the first fluctuation index; the first identification unit is also configured with a first fluctuation threshold; if the first fluctuation index is greater than the first fluctuation threshold, the inertial navigation data in the first detection window has interference fluctuations. The positioning compensation module is used to construct attitude constraint curves and perform constraint compensation on inertial navigation data with interference fluctuations based on the attitude constraint curves. The positioning compensation module includes an attitude constraint unit; the attitude constraint unit is used to construct an attitude constraint curve, specifically including: Extract the attitude angles of the first or second detection window corresponding to the inertial navigation data with interference fluctuations, and form a reference window; Based on the attitude angles in the reference window, the equation of the curve of attitude angle change over time is fitted and used as the equation of the attitude constraint curve. The second identification module is used to identify the heading drift of the inertial navigation data; The positioning correction module constructs a heading reference sequence based on the positioning image sequence, and performs heading correction on the inertial navigation data with heading drift based on the heading reference sequence.

2. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 1, characterized in that: The positioning compensation module further includes a constraint compensation unit; the constraint compensation unit is configured with a constraint compensation strategy for constraining and compensating inertial navigation data subject to interference fluctuations; the constraint compensation strategy specifically includes: The reference value of the attitude angle at each moment in the first or second detection window corresponding to the inertial navigation data with interference fluctuations is calculated based on the equation of the attitude constraint curve. Calculate the attitude angle residuals of the inertial navigation data at each moment where disturbance fluctuations occur; The constraint compensation unit is also equipped with an attitude deviation threshold. For inertial navigation data at any time, if the corresponding attitude angle residual is greater than the attitude deviation threshold, the attitude angle is weighted and fused with the reference value of the attitude angle at the corresponding time to obtain the attitude angle after constraint compensation.

3. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 2, characterized in that: The second identification module includes a heading detection unit and a curvature identification unit; The heading detection unit is used to construct a heading stability factor; the curvature recognition unit is used to construct a heading curvature sequence and identify heading drift of inertial navigation data based on the heading stability factor and the heading curvature sequence. The heading detection unit constructs the heading stability factor in the following way: The angular velocity vector at each moment is constructed based on the three-axis angular velocities; the linear acceleration vector at each moment is constructed based on the three-axis axial accelerations. Calculate the angle between the angular velocity vector and the linear acceleration vector at each moment, and record it as the acceleration angle at each moment; Calculate the heading drift rate at each time step; the heading drift rate at any time step is the difference between the acceleration angle at the corresponding time step and the acceleration angle at the adjacent previous time step. Set a drift detection window; the drift detection window contains the heading drift rate of the most recent m consecutive moments; m is a positive integer; calculate the mean of the heading drift rate at each moment in the drift detection window, as the heading stability factor of the drift detection window.

4. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 3, characterized in that: The curvature recognition unit constructs the heading curvature sequence in the following way: Calculate the heading curvature at each time point; the heading curvature at any time point is the difference between the heading angle at the corresponding time point and the heading angle at the adjacent previous time point; the heading curvature sequence is a time series composed of the heading curvature at each time point. The curvature recognition unit is configured with a drift recognition strategy to identify heading drift in inertial navigation data; The drift detection strategy specifically includes: The system identifies whether the heading has changed at any given moment; the curvature recognition unit is configured with a heading curvature threshold; if the heading curvature at any given moment is greater than the heading curvature threshold, then the heading has changed at the corresponding moment. The curvature recognition unit is also configured with a heading drift threshold; if the heading stability factor of the drift detection window is greater than the heading drift threshold, and the heading changes at least once in the drift detection window, then the inertial navigation data corresponding to the drift detection window has heading drift.

5. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 4, characterized in that: The positioning correction module includes a reference heading unit; The reference heading unit constructs a heading reference sequence based on the aircraft's positioning image sequence, specifically including: Each frame of the positioning image sequence is identified as stable. Mark any two adjacent and stable positioning images in the positioning image as a set of target positioning images; mark the first frame in any set of target positioning images as the reference frame and the second frame as the target frame; The visual heading angle at each target frame corresponding to the time is calculated; the visual heading angles at different times are interpolated to obtain a heading reference sequence; the heading reference sequence contains the visual heading angle at each time, and the visual heading angle corresponds one-to-one with the heading angle in the inertial navigation data in time.

6. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 5, characterized in that: The method by which the reference heading unit calculates the visual heading angle at any given time for any target frame is as follows: Feature point detection is performed between the target frame and the corresponding reference frame; feature point matching is performed between the target frame and the corresponding reference frame to obtain the coordinates of each feature point between the target frame and the corresponding reference frame. Based on the coordinates of each feature point between the target frame and the corresponding reference frame, the motion vector of each feature point in the target frame is calculated, and the main motion direction of the target frame is extracted based on the motion vectors of different feature points in the target frame. Project the main motion direction of the target frame onto the inertial navigation coordinate system to obtain the visual heading angle at the corresponding moment.

7. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 6, characterized in that: The reference heading unit is configured with an image detection strategy to identify whether any frame of the positioning image is stable; the image detection strategy specifically includes: For any frame of the positioning image, the absolute value of the edge intensity of each pixel is calculated and the average value is used as the sharpness index of the corresponding frame of the positioning image; the reference heading unit is also configured with a sharpness threshold; if the sharpness index of any frame of the positioning image is greater than the sharpness threshold, the corresponding frame of the positioning image is stable, otherwise the corresponding frame of the positioning image is unstable. The image detection strategy further includes: performing feature point detection on each frame of the positioning image and recording the feature scale of each feature point in each frame of the positioning image; for any frame of the positioning image, calculating the variance of the feature scale of each feature point as a stability index of the corresponding frame of the positioning image; the reference heading unit is also configured with a first stability threshold and a second stability threshold; if the stability index of any frame of the positioning image is greater than the first stability threshold and less than the second stability threshold, then the corresponding frame of the positioning image is stable, otherwise the corresponding frame of the positioning image is unstable.

8. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 7, characterized in that: The positioning correction module further includes a heading correction unit; the heading correction unit is configured with a heading correction strategy for correcting the heading of inertial navigation data exhibiting heading drift; the heading correction strategy specifically includes: For any inertial navigation data with heading drift, the heading angle in the inertial navigation data is fused with the visual heading angle corresponding to the timestamp in the heading reference sequence by Kalman filtering to obtain the corrected heading angle.

Images