Vibration interference resistant optical fiber inertial navigation and vision fusion positioning system

Abstract

The invention relates to the technical field of high-precision positioning, and discloses an anti-vibration interference optical fiber inertial navigation and vision 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 fluctuation 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 interference fluctuation; the second identification module is used for identifying course drift of the inertial navigation data; and the positioning correction module constructs a course reference sequence based on the positioning image sequence, and performs course correction on the inertial navigation data with course drift. According to the invention, the positioning precision of the aircraft under vibration, shaking and complex maneuvering conditions is improved.

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-term 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 realize 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 velocity 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 to 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: 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: The data acquisition module is used to acquire inertial navigation data and positioning image sequences of the aircraft; The first identification module is used to identify the interference fluctuation of the inertial navigation data; The positioning compensation module is used to construct an attitude constraint curve, and to constrain and compensate the inertial navigation data with interference fluctuation based on 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 corrects the heading of the inertial navigation data with heading drift based on the heading reference sequence.

[0010] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the application, wherein: the inertial navigation data includes attitude angles, three-axis angular velocities and three-axis 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; 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: A first detection window with a length of N is set; N is a positive integer; the first detection window includes three-axis accelerations at the most recent N continuous time points; The variance of the three-axis 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.

[0011] 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 compensation module includes an attitude constraint unit; the attitude constraint unit is used to construct an attitude constraint curve, specifically including: 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 composed into a reference window; Based on the attitude angles in the reference window, an equation of the curve of the change of the attitude angle with time is fitted as the equation of the attitude constraint curve.

[0012] 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 compensation module further includes a constraint compensation unit; the constraint compensation unit is configured with a constraint compensation strategy for constraint compensation of the inertial navigation data with interference fluctuations; the constraint compensation strategy specifically includes: The 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 based on the equation of the attitude constraint curve; The attitude angle residual of the inertial navigation data at each time point with interference fluctuations is calculated; The constraint compensation unit is further configured with an attitude deviation threshold; for the inertial navigation data at any time point, if the corresponding attitude angle residual is greater than the attitude deviation threshold, the attitude angle is weightedly fused with the reference value of the attitude angle at the corresponding time point to obtain the constraint-compensated attitude angle.

[0013] As a preferred scheme of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system described in the application, wherein: the second identification module comprises a heading detection unit and a curvature identification unit; 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. The heading detection unit constructs the heading stability factor in the following manner: The heading detection unit constructs the heading stability factor in the following manner: The heading detection unit constructs the heading stability factor in the following manner: The heading detection unit constructs the heading stability factor in the following manner: The heading detection unit constructs the heading stability factor in the following manner:

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

[0015] 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 comprises a reference heading unit. The reference heading unit constructs a heading reference sequence based on the positioning image sequence of the aircraft, and specifically comprises: Respectively identify whether each frame of the positioning image sequence is stable; Mark any two adjacent frames of the positioning image that are stable as a group of target positioning images; mark the previous frame in any group of target positioning images as a reference frame, and mark the subsequent frame as a target frame; Respectively calculate the visual heading angle of each target frame at the corresponding time; interpolate the visual heading angles at different times to obtain the heading reference sequence; the heading reference sequence contains 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.

[0016] 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 reference heading unit calculates the visual heading angle of any frame of target frame at the corresponding time as follows: Respectively perform feature point detection between the target frame and the corresponding reference frame; perform feature point matching on 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, calculate the motion vector of each feature point in the target frame, and extract the main motion direction of the target frame based on the motion vectors of different feature points in the target frame; Project the main motion direction of the target frame into the inertial navigation coordinate system to obtain the visual heading angle at the corresponding time.

[0017] 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 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 comprises: For any frame of positioning image, calculate the absolute value of the edge intensity of each pixel point and take the average as the definition index of the corresponding frame of positioning image; the reference heading unit is also 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; The image detection strategy also includes: respectively performing feature point detection on each frame of positioning image, and recording the feature scale of each feature point in each frame of positioning image; for any frame of positioning image, calculate the variance of the feature scale of each feature point as the stability index of the corresponding frame of 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 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.

[0018] 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 comprises a heading correction unit; the heading correction unit is configured with a heading correction strategy for heading correction on the inertial navigation data with heading drift; the heading correction strategy specifically comprises: 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 through Kalman filtering to obtain the corrected heading angle.

[0019] Compared with the prior art, the application has the following beneficial effects: 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 environment where GNSS signal is interrupted or unavailable, the attitude positioning of the navigation system remains stable.

[0020] Through comprehensive judgment on the heading change mode and 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

[0021] In order to more clearly illustrate the technical scheme of the embodiments of the application, the drawings needed in the embodiment description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the application, and other drawings can be obtained by those skilled in the art without creative labor. Among them: Figure 1 The structure diagram of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system provided by the application; Figure 2 The function diagram of the anti-vibration interference optical fiber inertial navigation and visual fusion positioning system provided by the application. DETAILED DESCRIPTION

[0022] The technical scheme of the application will be described in detail below with the help of the drawings and specific embodiments. It should be understood that the specific features in the embodiments and the embodiments of the application are detailed descriptions of the technical scheme of the application, rather than limitations of the technical scheme of the application. In the case of no conflict, the technical features in the embodiments and the embodiments of the application can be combined with each other.

[0023] The embodiment introduces a kind of anti-vibration interference optical fiber inertial navigation and vision fusion positioning system, refer to Figure 1 The system includes data acquisition module, first identification module, positioning compensation module, second identification module, positioning correction module;The function of each module is as shown in Figure 2 Among them: Data acquisition module is used to collect the inertial navigation data and positioning image sequence of aircraft; The data acquisition module includes inertial navigation unit and vision navigation unit; The inertial navigation unit is used to collect the inertial navigation data of aircraft;The inertial navigation data includes the attitude angle of aircraft at each time, three-axis angular velocity, three-axis linear acceleration;The attitude angle includes pitch angle, roll angle and heading angle; In the embodiment, inertial navigation unit is configured with angular velocity measurement subunit, such as optical fiber gyroscope, for real-time detection of three-axis angular velocity of aircraft;It is also configured with linear acceleration measurement subunit, such as accelerometer, for real-time detection of three-axis linear acceleration of aircraft.Inertial navigation unit is also configured with inertial calculation subunit, which can calculate the attitude angle at each time according to the three-axis angular velocity and three-axis linear acceleration at each time, and the three-dimensional velocity and three-dimensional coordinates of aircraft at each time.

[0024] The vision navigation unit is used to collect the positioning image sequence of aircraft;The positioning image sequence includes positioning images at different times; In the embodiment, positioning image is collected by camera configured on vision navigation unit.The camera is fixedly installed on aircraft and moves synchronously with body attitude, to output positioning image periodically at set frame rate.

[0025] The first identification module is used to identify the interference fluctuation of the inertial navigation data; The first identification module includes first identification unit and second identification unit; The first identification unit is configured with first identification strategy, for identifying the interference fluctuation of inertial navigation data;The first identification strategy includes: Set the length of the first detection window as N;N is a positive integer;The first detection window includes the three-axis linear acceleration of the last continuous N time; Calculate the variance of three-axis linear acceleration at each time in the first detection window as the first fluctuation index;The first identification unit is also configured with 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 fluctuation.

[0026] The first fluctuation threshold value can be set by the person skilled in the art according to actual requirements. When calculating the variance of the three-axis linear acceleration at each time point in the first detection window, the variance of the linear acceleration in each direction can be calculated respectively, and the mean value or the maximum value can be taken. When the first fluctuation index is greater than the first fluctuation threshold value, it indicates that the aircraft is in a state of continuous disturbance.

[0027] The second identification unit is configured with a second identification strategy for identifying interference fluctuations in the inertial navigation data. The second identification strategy includes: A second detection window with a length of M is set, where M is a positive integer. The second detection window includes three-axis angular velocities at the most recent continuous M time points. 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 index. The first identification unit is further configured with a second fluctuation threshold value. If the second fluctuation index is greater than the second fluctuation threshold value, the inertial navigation data in the second detection window has interference fluctuations.

[0028] The second fluctuation threshold value can be set by the person skilled in the art according to actual requirements. 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 value or the maximum value can be taken. When calculating the high-frequency energy of the angular velocity in any direction, a frequency threshold value can be set according to actual requirements. The frequency components greater than the frequency threshold value are marked as high-frequency components, and the total energy of the high-frequency components is calculated. For example, the frequency threshold value can be set to 20 Hz. Angular velocity changes below 20 Hz are mainly caused by real maneuvers of the body, while changes above 20 Hz are mainly caused by motor vibration and structural elastic jitter, thereby distinguishing real attitude changes from jitter noise. When the second fluctuation index is greater than the second fluctuation threshold value, it indicates that the inertial navigation data of the aircraft is in a high-frequency vibration interference state.

[0029] The positioning compensation module is configured to construct an attitude constraint curve and constrain and compensate the inertial navigation data with interference fluctuations based on the attitude constraint curve. The positioning compensation module includes an attitude constraint unit and a constraint compensation unit. The attitude constraint unit is configured to construct an attitude constraint curve, specifically including: The first detection window or the second detection window before the K continuous time points corresponding to the inertial navigation data with interference fluctuations is extracted, and the attitude angles at the K continuous time points are combined to form a reference window. Based on the attitude angles in the reference window, an equation of the change curve of the attitude angle with time is fitted as an equation of the attitude constraint curve.

[0030] Optionally, the attitude constraint curve is fitted according to the attitude angles in the reference window by least square fitting. The variation curves of the pitch angle, the roll angle and the yaw angle can be fitted respectively, or the pitch angle, the roll angle and the yaw angle at each time can be combined into an attitude angle vector, and the least square residual curve fitting is performed uniformly. For example, the attitude angle vector at any time is denoted as [θ p, θ r, θ y], where θ p, θ r and θ y are the pitch angle, the roll angle and the yaw angle at the corresponding time respectively. For example, when the sampling period is 10 ms, the partial 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°]. In the unified fitting, 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, a second-order polynomial form is adopted. By minimizing the sum of squares of residuals between the attitude angle vectors at each time in the reference window and the fitted curve, the attitude constraint curve describing the smooth variation of the pitch angle, the roll angle and the yaw angle with time is obtained.

[0031] The attitude angles in the reference window are regarded as the real attitude variation trajectory in the normal state, and the attitude constraint curve fitted based thereon describes the smooth variation trend of the attitude angles with time of the aircraft in the stable state, which is used to provide a relatively smooth attitude reference during the inertial navigation exception.

[0032] The constraint compensation unit is configured with a constraint compensation strategy for performing constraint compensation on the inertial navigation data with disturbance fluctuation; the constraint compensation strategy specifically includes: calculating a reference value of the attitude angle at each time in the first detection window or the second detection window corresponding to the inertial navigation data with disturbance fluctuation based on the equation of the attitude constraint curve; calculating the attitude angle residual of the inertial navigation data at each time with disturbance fluctuation; The constraint compensation unit is further configured with an attitude deviation threshold value; for the inertial navigation data at any time, if the corresponding attitude angle residual is greater than the attitude deviation threshold value, the attitude angle is fused with the reference value of the attitude angle at the corresponding time by weighting to obtain the constrained and compensated attitude angle.

[0033] Optionally, the attitude angle residual at any moment is the length of the vector difference between the attitude angle vector at the moment and the reference value of the corresponding attitude angle. The manner of weighted fusion of the attitude angle and the reference value of the attitude angle at the corresponding moment is weighted summation, and the sum of the weight coefficients of the two is 1; the weight coefficient can be set according to the size of the attitude angle residual, for example, the greater the attitude angle residual, the greater the weight coefficient of the reference value. The skilled person can set the specific value of the attitude deviation threshold based on actual needs. For any moment, if the attitude angle residual is greater than the attitude deviation threshold, the attitude angle provided by the inertial navigation system has low reliability, and its reference value is used to constrain and compensate it, so that the compensated attitude angle changes along the attitude constraint curve and is not deviated by the instantaneous violent shaking.

[0034] The second identification module is configured to identify the heading drift of the inertial navigation data. The second identification module includes a heading detection unit and a curvature identification unit. 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. The heading detection unit constructs the heading stability factor in the following manner: The three-axis angular velocity at each moment is used to construct an angular velocity vector at each moment, and the three-axis linear acceleration at each moment is used to construct a linear acceleration vector at each moment. In this embodiment, the three-axis linear acceleration or the three-axis angular velocity at any moment includes components in three directions, and the angular velocity vector or the linear acceleration vector at each moment is represented by a corresponding three-dimensional vector.

[0035] The included angle between the angular velocity vector and the linear acceleration vector at each moment is calculated respectively, denoted as the acceleration included angle at each moment. The heading drift rate at each moment is calculated respectively; the heading drift rate at any moment is the difference between the acceleration included angle at the corresponding moment and the acceleration included angle at the adjacent previous moment. A drift detection window is set; the drift detection window includes the heading drift rates at the most recent m consecutive moments; m is a positive integer; the mean of the heading drift rates at each moment in the drift detection window is calculated as the heading stability factor of the drift detection window.

[0036] 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 included angle between the two. Under normal maneuvering, the two have a stable corresponding relationship; when the inertial navigation attitude solution drifts, the angular velocity direction will mismatch the acceleration direction, causing the heading stability factor to rise significantly.

[0037] The curvature identification unit constructs the heading curvature sequence in the following manner: The heading curvature at each time point is calculated respectively; 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; and the heading curvature sequence is a time sequence composed of the heading curvatures at each time point.

[0038] The curvature identification unit is configured with a drift identification strategy for identifying the heading drift of the inertial navigation data; and the drift identification strategy specifically includes: Whether the heading at any time point jumps is identified; the curvature identification unit is configured with a heading curvature threshold value; if the heading curvature at any time point is greater than the heading curvature threshold value, the heading at the corresponding time point jumps; The curvature identification 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 point in the drift detection window jumps, the inertial navigation data corresponding to the drift detection window has heading drift.

[0039] The skilled person in the art can set the specific values of the heading curvature threshold value and the heading drift threshold value based on actual needs. When the aircraft normally turns, the heading angle changes continuously and smoothly; when the inertial navigation data has heading drift, the heading angle abnormally jumps. The present 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.

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

[0041] The positioning correction module includes a reference heading unit and a heading correction unit. The reference heading unit constructs a heading reference sequence based on the positioning image sequence of the aircraft, and specifically includes: Whether each frame of the positioning image sequence is stable is identified respectively; Any two adjacent frames of the positioning image that are both 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 subsequent frame is marked as a target frame; The visual heading angle at the corresponding time point of each target frame is calculated respectively; the visual heading angles at different time points are interpolated to obtain a heading reference sequence; the heading reference sequence includes the visual heading angle at each time point, and the visual heading angle and the heading angle in the inertial navigation data correspond to each other in time.

[0042] The application only calculates the visual heading angle at the corresponding moment when both adjacent two frames of positioning images are stable, so as to ensure that the feature points in the positioning images are not shaken and blurred, the feature point consistency is high, and the direction information is reliable. A fitting method such as spline fitting is used to perform curve fitting on the visual heading angle, and the visual heading angle is interpolated with the inertial navigation time stamp as a reference, so that the visual heading angle completely corresponds to the heading angle in the inertial navigation data in time.

[0043] In the embodiment, the visual heading angle is only available at partial discrete time stamps when the visual heading angle is calculated only for stable image frames. For example, in the case that the camera frame rate is 20 Hz and the inertial navigation data output frequency is 100 Hz, the time stamp interval of the visual heading angle is 0.05 s, and the inertial navigation time stamp interval is 0.01 s. First, the discrete visual heading angle data is fitted by using 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 continuous curve is interpolated according to the time stamp of the inertial navigation data, so that the corresponding visual heading angle is obtained at each inertial navigation time stamp, and the visual heading angle strictly corresponds to the heading angle in the inertial navigation data 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°, so the interpolation heading angle is directly calculated to be 35.08° by using the spline curve.

[0044] The reference heading unit calculates the visual heading angle at the corresponding moment of any frame of target frame by the following method: Feature point detection is respectively 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; 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 of 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, a brute force matching method based on Hamming distance can be used, and cross-validation can be 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 as to obtain a stable and reliable feature point correspondence.

[0045] 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. 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).

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

[0047] 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, so a projection conversion is needed to be completed through the camera parameters to make them comparable in the same coordinate system. First, the image plane is projected to 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.

[0048] 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: For any frame of positioning image, the absolute values of the edge intensities of each pixel point are calculated respectively, and the mean value is taken as the definition index of the corresponding frame of positioning image; the reference heading unit is also 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.

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

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

[0051] 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 their feature scales is calculated as the stability index of the positioning image. The person skilled in the art can set the specific values of the first stability threshold and the second stability threshold based on actual needs. When the stability index is too small, it means 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 means 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 the disorder of multi-scale features of the image due to vibration.

[0052] The heading correction unit is configured with a heading correction strategy for correcting the inertial navigation data with heading drift; the heading correction strategy specifically comprises: 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.

[0053] 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 Kalman gain is used to correct the deviation between the inertial navigation heading angle and the visual heading angle to obtain an updated heading angle estimation. 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.

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

[0055] 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 memory, CD-ROM, optical memory, etc.) containing computer usable program code.

[0056] 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 under the inspiration of the present application without departing from the purpose and the protected range 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 first identification module is used to identify interference fluctuations in the inertial navigation data; 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 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 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 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 a 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, then there is interference fluctuation in the inertial navigation data in the first detection window.

3. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 2, characterized in that: 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.

4. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 3, 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.

5. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 4, 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.

6. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 5, 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.

7. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 6, 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.

8. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 7, 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.

9. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 8, 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.

10. The vibration-resistant fiber optic inertial navigation and vision fusion positioning system as described in claim 9, 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