An inertial navigation comprehensive correction method based on normal vector position model

Abstract

The application relates to an inertial navigation comprehensive correction method based on a normal vector position model, which comprises the following steps: receiving a real-time rotation angular velocity vector and a specific force vector, binding initial navigation parameters, obtaining an initial normal vector position vector, an initial quaternion position vector and an initial direction cosine matrix, obtaining an initial velocity and an initial attitude matrix; obtaining a current velocity, a current attitude matrix and a current quaternion position vector; obtaining a current damping velocity and a current damping quaternion position vector, calculating a normal vector integral term; judging whether comprehensive correction is needed, if yes, adjusting an external reference position based on an observation number; if the observation number is greater than 1, adjusting a horizontal position to the external reference position, obtaining a drift angle and an azimuth gyro drift, correcting an attitude matrix output according to the drift angle, and using the azimuth gyro drift for navigation calculation; recording a time of successful observation and setting the normal vector integral term to zero; repeating the foregoing steps until navigation is completed.

Description

Technical Field

[0001] This invention relates to the field of inertial navigation technology, and in particular to an inertial navigation integrated correction method based on a normal vector position model. Background Technology

[0002] Inertial navigation, characterized by its good continuity, high autonomy, and strong concealment, is the most fundamental navigation method for ships navigating in polar regions. However, due to the convergence of meridians in high-latitude regions, the commonly used north-pointing inertial navigation mechanics are not suitable for polar areas. Furthermore, when ships enter or leave polar regions, it is necessary to switch between traditional navigation algorithms and polar-area algorithms, a process that can affect the continuity and consistency of the internal processes of the algorithms.

[0003] For long-endurance inertial navigation systems, the main source of error in inertial indicated position and heading angle is the uncompensated drift error of the gyroscope. Besides the error caused by the propagation of periodic oscillations, gyroscope drift also generates errors that accumulate over time. To suppress the accumulated errors of the inertial navigation system, for vessels navigating underwater for extended periods, occasional surfacing can be used to correct the inertial navigation system errors using external reference information such as satellite navigation or celestial navigation. To avoid frequent surfacing, comprehensive correction of the inertial navigation system based on sparse observation information is of significant practical importance. For currently widely used optical gyroscope marine inertial navigation systems, single-axis rotation modulation inertial navigation systems only need to consider the azimuth gyroscope constant drift error, while in dual-axis rotation modulation optical gyroscope marine inertial navigation systems, although most of the azimuth gyroscope constant drift errors are suppressed, there will still be residual equivalent azimuth gyroscope constant drift errors.

[0004] Traditional inertial navigation integrated correction techniques project the relationship equation between drift angle and gyroscope drift onto the o-frame (qep coordinate system). The existence of an analytical solution to the relationship equation between drift angle and gyroscope drift in the o-frame relies on the assumption that the angular velocity of the carrier's displacement on the Earth's surface is much smaller than the Earth's rotational angular velocity, and also assumes that the gyroscope drift angular velocity error ε... o It is approximately a constant value, because And because

[0005]

[0006] Due to the direction cosine matrix It is only related to latitude L, and the gyroscope is fixed to the carrier, ε b It can be approximated as a constant, therefore ε o The condition for approximating a constant value is It is approximately a constant value. That is, for traditional strapdown inertial navigation systems, the traditional integrated correction method requires the carrier to travel at low speed and at approximately equal latitudes, and the carrier's attitude to remain unchanged, which greatly limits its practicality.

[0007] On the other hand, the equation relating the position error vectors of latitude L and longitude λ to the drift angle is:

[0008]

[0009] Since this coefficient matrix is ​​related to the tangent of latitude L, the coefficient matrix of the observation equation of the traditional comprehensive correction method is ill-conditioned in high-latitude regions, meaning it is not applicable to polar regions.

[0010] Similarly, when the latitude L t =90°, meaning when the carrier is located at the equator (the pole of the horizontal Earth coordinate system), the polar correction method in the horizontal coordinate system is also inapplicable and does not have global applicability.

[0011] In addition, since the e-axis of the O-system points eastward, the direction of the e-axis will change rapidly near the poles, which will affect the traditional synthesis correction to some extent.

[0012] In summary, traditional inertial navigation integrated correction technology has the following drawbacks:

[0013] Traditional inertial navigation integrated correction technology is arranged in the QEP coordinate system, which requires the carrier to travel at low speed and at approximately equal latitudes, thus greatly limiting its practicality.

[0014] The relationship between position error and drift angle in traditional integrated correction methods is implemented in the traditional local horizontal navigation coordinate system, which is not applicable to polar regions;

[0015] The inertial navigation integrated correction method applicable to polar regions in the horizontal coordinate system and grid coordinate system also lacks global applicability;

[0016] In applications that traverse polar regions, the need to switch from traditional geographic coordinate system algorithms leads to changes in the integration process. For comprehensive correction algorithms, this switching process affects the continuity and consistency of the internal algorithms and also greatly increases the algorithm's complexity. Summary of the Invention

[0017] The purpose of this invention is to provide an inertial navigation integrated correction method based on a normal vector position model.

[0018] To achieve the above-mentioned objectives, this invention provides an inertial navigation integrated correction method based on a normal vector position model, comprising:

[0019] S1. Receive the real-time rotational angular velocity vector and specific force vector from the IMU inertial measurement unit, and load the initial navigation parameters, wherein the initial navigation parameters include: initial latitude and longitude, initial indicated altitude h(T0), and initial navigation speed v. n (T0), Initial navigation attitude matrix Based on the initial navigation parameters, obtain the vehicle's initial normal vector position vector η(T0) and initial quadruplet position vector P. η (T0), Initial direction cosine matrix And obtain the initial velocity in the Earth-centered Earth-fixed coordinate system. and initial attitude matrix

[0020] S2. Based on the previous velocity in the geocentric-fixed coordinate system at the previous moment. Previous attitude matrix Previous Quadruple Position Vector P η (T i-1 The measurements from the gyroscope and accelerometer in the IMU inertial measurement unit and the azimuth gyroscope drift ε U (T0), the current velocity at the current moment is obtained using the recursive formula of pure inertial navigation solution. Current attitude matrix Current quadruplet position vector P η (T i );

[0021] S3. Obtain the current damped velocity at the current moment using the recursive formula for horizontal damping based on the external reference velocity. And the current damped quaternion position vector P after damping at the current moment η_Damp (T i ), and calculate the normal vector integral term INT. η (T i );

[0022] S4. Determine if comprehensive calibration is needed and if the position from the external measuring device has been successfully received. If calibration is not needed, proceed to S2; if comprehensive calibration is needed, successfully receive the real-time latitude and longitude from the external measuring device and obtain the external reference position P. ηr (T mk When k is the number of observations, the external reference position P is selected for readjustment. ηr (T mk In this method, if the number of observations k equals 1, the output horizontal position is readjusted to the external reference position P. ηr (T mk Record the time T of each successful observation. mk And the normal vector integral term INT η (T mk Set to zero;

[0023] S5. If the number of observations k is greater than 1, readjust the output horizontal position to the external reference position P. ηr (T mk According to the normal vector position error vector δη(T)mk The time interval ΔT between the previous observation and the previous observation. mk and the integral term of the normal vector INT η (T mk The drift angle is obtained according to the comprehensive correction formula. And azimuth gyroscope drift According to the drift angle Correct the attitude matrix output to And azimuth gyroscope drift This will be used for subsequent navigation calculations using pure inertial navigation, and finally, the time T of a successful observation will be recorded. mk And the normal vector integral term INT η (T mk Set to zero;

[0024] S6. Repeat steps S2 to S5 until navigation ends.

[0025] According to one aspect of the invention, in step S1, the initial normal vector position vector η(T0) and the initial quaternion position vector P of the vehicle are obtained based on the initial navigation parameters. η (T0), Initial direction cosine matrix The steps include:

[0026] Based on the initial latitude and longitude, calculate the initial normal vector position vector η(T0) and the initial quaternion position vector P of the vehicle. η (T0);

[0027] Calculate the initial direction cosine matrix based on the initial normal vector and position vector η(T0).

[0028] According to one aspect of the invention, in step S1, the initial normal vector position vector η(T0) and the initial quaternion position vector P of the vehicle are calculated based on the initial latitude and longitude. η In step (T0), the initial normal vector position vector η(T0) is calculated using the following formula:

[0029]

[0030] Where L(T0) and λ(T0) are the initial geographical latitude and longitude, respectively, and η x (T0), η y (T0) and η z (T0) represent the three components of the initial normal vector position vector η(T0) in the x, y, and z directions, respectively;

[0031] The initial quadruplet position vector P η The calculation method for (T0) is expressed as follows:

[0032] P η (T0)=[η x (T0)η y (T0)η z (T0)h(T0)] T

[0033] Where h(T0) is the initial indicated height.

[0034] According to one aspect of the invention, in step S1, the initial direction cosine matrix is ​​calculated based on the initial normal vector position vector η(T0). In the step, the initial direction cosine matrix Let be the direction cosine matrix between the geocentric and geofixed coordinate system and the local horizontal north-pointing geographic coordinate system. Its calculation method is expressed as follows:

[0035]

[0036] According to one aspect of the present invention, in step S1, the initial velocity in the geocentric-geostatic coordinate system is obtained. and initial attitude matrix In the steps, based on the initial navigation speed v n (T0), the initial navigation attitude matrix The initial velocity is obtained and the initial attitude matrix Wherein, the initial velocity The calculation formula is expressed as:

[0037]

[0038] The initial attitude matrix The calculation formula is expressed as:

[0039]

[0040] According to one aspect of the present invention, in step S2, the current velocity at the current moment is obtained using a recursive formula derived from pure inertial navigation calculation. Current attitude matrix Current quadruplet position vector P η (T i The steps include:

[0041] The current velocity at the current moment is obtained using the velocity recursion formula in the Earth-centered Earth-fixed coordinate system. It is represented as:

[0042]

[0043] Where t is the sampling interval, Let f be the Earth's rotational angular velocity vector. b (T i The force vector is the specific force vector obtained in real time by the accelerometer in the IMU inertial measurement unit. This is the projection of the gravity vector from the previous moment onto the Earth-centered, Earth-fixed coordinate system. for skew-symmetric matrix;

[0044] Based on the determined current speed Update the position vector P of the previous quadruple. η (T i-1 ), to obtain the current quadruple position vector P η (T i The recursive formula for the position of a quadruple is expressed as:

[0045]

[0046] in, R E R is the radius of the circle. N The radius of the meridian circle;

[0047] The radius R of the Maoyou circle E and the meridian radius R N Represented as:

[0048]

[0049] Where R0 = 6378137 is the Earth's radius, and e = 0.081819191 is the Earth's first eccentricity;

[0050] Based on the current quadruple position vector P η (T i The attitude is updated using the real-time rotational angular velocity vector measured by the gyroscope in the IMU inertial measurement unit, i.e., the current attitude matrix is ​​obtained. The recursive formula for the attitude matrix is:

[0051]

[0052]

[0053] Where I3 is a 3-order identity matrix. ε is the rotational angular velocity vector obtained in real time by the gyroscope. U (T mk ) represents the azimuth gyroscope drift obtained after the k-th correction, with an initial value of 0.

[0054] According to one aspect of the present invention, in step S3, the current damped velocity after damping is obtained at the current moment using a recursive formula for horizontal damping based on the external reference velocity. And the current damped quaternion position vector P after damping at the current moment η_Damp (T i ), and calculate the normal vector integral term INT. η (T i In step (), the recursive formula for the horizontal damping is:

[0055]

[0056] Among them, v H (T i () represents the horizontal velocity vector at the current moment;

[0057]

[0058] Among them, v Hr (T i ) represents the external reference horizontal velocity vector at the current moment. H_damp (T i () represents the horizontal velocity vector after damping at the current moment;

[0059] The current damping velocity is obtained based on the recursive formula for the horizontal damping. It is represented as:

[0060]

[0061] Wherein, η(T) i ) represents the current normal vector position vector, v h (T i ) represents the current vertical indicated velocity, and num and dum are both damping coefficients of the phase lag-lead series correction network, i.e.:

[0062] num(1)=1.0,num(2)=-1.99997367841154,num(3)=0.999975215058425

[0063] dum(1)=1.0, dum(2)=-1.99875923183544, dum(3)=0.998760768482328;

[0064] Based on the recursive formula for the horizontal damping, a recursive formula for the position of the normal vector is constructed, which is expressed as:

[0065]

[0066] The current damping quadruplet position vector P is obtained based on the recursive formula for the normal vector position. η_Damp (T i ), which is represented as:

[0067]

[0068] Wherein, h(T) i () indicates the current indicated altitude;

[0069] The normal vector integral term INT η (T i The recursive formula for ) is:

[0070]

[0071] According to one aspect of the present invention, in step S5, the comprehensive correction formula is:

[0072]

[0073] in,

[0074]

[0075] The comprehensive correction formula then simplifies to:

[0076]

[0077] Where, β ε For the time constants related to the Markov process.

[0078] According to one aspect of the invention, in steps S4 and S5, the output horizontal position is readjusted to the external reference position η. r (T mk In the step of ), the position readjustment formula used for horizontal position readjustment is expressed as:

[0079]

[0080] According to the drift angle Correct the attitude output to In the steps described, the formula for calculating attitude output correction is expressed as:

[0081]

[0082] Where [×] represents the corresponding skew-symmetric matrix.

[0083] According to one aspect of the invention, in step S5, the normal vector position error vector δη(T) mk ) is represented as:

[0084]

[0085] Wherein, η(T) mk ) represents the normal vector position vector before the position is readjusted at the k-th correction time.

[0086] According to one aspect of the present invention, taking polar inertial navigation of ships as an example, a comprehensive correction method is designed and implemented based on a normal vector position model under horizontal damping conditions. This method avoids the complexity and discontinuity caused by algorithm switching when entering and exiting polar regions, and has global applicability; it avoids restrictions on the ship's trajectory and operating state, and has strong practicality; it accurately estimates the azimuth gyroscope constant drift, effectively suppressing navigation errors accumulated over long voyages, and improving navigation and positioning accuracy. Attached Figure Description

[0087] Figure 1 This is a schematic diagram illustrating the steps of an inertial navigation integrated correction method according to an embodiment of the present invention;

[0088] Figure 2 This is a flowchart illustrating an inertial navigation integrated correction method according to an embodiment of the present invention. Detailed Implementation

[0089] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly described below. Obviously, the drawings described below are merely some embodiments of the present invention, and those skilled in the art can obtain other drawings based on these drawings without creative effort.

[0090] In describing embodiments of the present invention, the terms "longitudinal," "lateral," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer" express orientations or positional relationships based on the orientations or positional relationships shown in the relevant drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the above terms should not be construed as limitations on the present invention.

[0091] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. The embodiments cannot be described in detail here, but the embodiments of the present invention are not limited to the following embodiments.

[0092] Combination Figure 1 and Figure 2 As shown, according to one embodiment of the present invention, an inertial navigation integrated correction method based on a normal vector position model includes:

[0093] S1. Receive the real-time rotational angular velocity vector and specific force vector from the IMU inertial measurement unit, and load the initial navigation parameters, which include: initial latitude and longitude, initial indicated altitude h(T0), and initial navigation speed v. n (T0) (which is the initial velocity in the northeast-north sky coordinate system), initial navigation attitude matrix Furthermore, based on the initial navigation parameters, the initial normal vector position vector η(T0) and the initial quaternion position vector P of the vehicle are obtained. η (T0), Initial direction cosine matrix And obtain the initial velocity in the Earth-centered Earth-fixed coordinate system. and initial attitude matrix

[0094] S2. Based on the previous velocity in the geocentric-fixed coordinate system at the previous moment. Previous attitude matrix Previous Quadruple Position Vector P η (T i-1 ), and the measurements from the gyroscopes and accelerometers in the IMU (i.e., the real-time rotational angular velocity and specific force vector output by the IMU) and the azimuth gyroscope drift ε. U (T0), the current velocity at the current moment is obtained using the recursive formula of pure inertial navigation solution. Current attitude matrix Current quadruplet position vector P η (T i );

[0095] S3. Obtain the current damped velocity at the current moment using the recursive formula for horizontal damping based on the external reference velocity. And the current damped quaternion position vector P after damping at the current moment η_Damp (T i ), and calculate the normal vector integral term INT. η (T i The external reference speed can be obtained using external speed measuring devices, such as water speed gauges, air speed gauges, and Doppler speedometers.

[0096] S4. Determine if comprehensive calibration is needed and if the position from the external measuring device has been successfully received. If calibration is not needed, proceed to S2; if comprehensive calibration is needed, successfully receive the real-time latitude and longitude from the external measuring device and obtain the external reference position P. ηr (T mk When k is the number of observations, the external reference position P is selected for readjustment. ηr (T mk In this method, if the number of observations k equals 1, the output horizontal position is readjusted to the external reference position P.ηr (T mk Record the time T of each successful observation. mk And the normal vector integral term INT η (T mk Set to zero; where external measuring equipment refers to equipment used for measuring external locations, such as satellite measuring equipment; external reference position P ηr (T mk The latitude and longitude are obtained from the real-time measurement equipment, and the formula is consistent with the initial four-tuple position vector calculation formula. In addition, the output horizontal position is the inertial indication position, that is, the position obtained by the inertial system through pure inertial navigation calculation. Resetting to the external reference position means setting the external reference position as the inertial indication position.

[0097] S5. If the number of observations k is greater than 1, readjust the output horizontal position to the external reference position P. ηr (T mk According to the normal vector position error vector δη(T) mk The time interval ΔT between the previous observation and the previous observation. mk and the integral term of the normal vector INT η (T mk The drift angle is obtained according to the comprehensive correction formula. And azimuth gyroscope drift According to the drift angle Correct the attitude matrix output to And azimuth gyroscope drift This will be used for subsequent navigation calculations using pure inertial navigation, and finally, the time T of a successful observation will be recorded. mk And the normal vector integral term INT η (T mk Set to zero;

[0098] S6. Repeat steps S2 to S5 until navigation ends.

[0099] According to one embodiment of the present invention, in step S1, the initial normal vector position vector η(T0) and the initial quaternion position vector P of the vehicle are obtained based on the initial navigation parameters. η (T0), Initial direction cosine matrix The steps include:

[0100] The initial normal vector position vector η(T0) and the initial quaternion position vector P of the vehicle are calculated based on the initial latitude and longitude. η (T0);

[0101] Calculate the initial direction cosine matrix based on the initial normal vector and position vector η(T0).

[0102] According to one embodiment of the present invention, in step S1, the initial normal vector position vector η(T0) and the initial quaternion position vector P of the vehicle are calculated based on the initial latitude and longitude. η In step (T0), the formula for calculating the initial normal vector position vector η(T0) is expressed as:

[0103]

[0104] Where L(T0) and λ(T0) are the initial geographical latitude and longitude, respectively, and η x (T0), η y (T0) and η z (T0) represent the three components of the initial normal vector position vector η(T0) in the x, y, and z directions, respectively;

[0105] Initial Quadruple Position Vector P η The calculation method for (T0) is expressed as follows:

[0106] P η (T0)=[η x (T0)η y (T0)η z (T0)h(T0)] T

[0107] Where h(T0) is the initial indicated height.

[0108] According to one embodiment of the present invention, in step S1, the initial direction cosine matrix is ​​calculated based on the initial normal vector position vector η(T0). In the steps, the initial direction cosine matrix Let be the direction cosine matrix between the geocentric and geofixed coordinate system and the local horizontal north-pointing geographic coordinate system. Its calculation method is expressed as follows:

[0109]

[0110] According to one embodiment of the present invention, in step S1, the initial velocity in the geocentric-geostatic coordinate system is obtained. and initial attitude matrix In the steps, based on the initial navigation speed v n (T0), Initial navigation attitude matrix Obtain the initial velocity and initial attitude matrix Among them, the initial velocity The calculation formula is expressed as:

[0111]

[0112] Initial attitude matrix The calculation formula is expressed as:

[0113]

[0114] According to one embodiment of the present invention, in step S2, the current velocity at the current moment is obtained using a recursive formula derived from pure inertial navigation calculation. Current attitude matrix Current quadruplet position vector P η (T i The steps include:

[0115] The current velocity at the current moment is obtained using the velocity recursion formula in the geocentric-fixed coordinate system. (T i ), which is represented as:

[0116]

[0117] Where t is the sampling interval, Let f be the Earth's rotational angular velocity vector. b (T i The force vector is the specific force vector obtained in real time by the accelerometer in the IMU (Inertial Measurement Unit). This is the projection of the gravity vector from the previous moment onto the Earth-centered, Earth-fixed coordinate system. for skew-symmetric matrix;

[0118] Based on the determined current speed Before updating, the position vector P of the quadruplet was updated. η (T i-1 ), to obtain the current quadruple position vector P η (T i The recursive formula for the position of a quadruple is expressed as:

[0119]

[0120] in, R E R is the radius of the circle. N The radius of the meridian circle;

[0121] Radius R of the Mao-You circle E and the radius R of the meridian N Represented as:

[0122]

[0123] Where R0 = 6378137 is the Earth's radius, and e = 0.081819191 is the Earth's first eccentricity;

[0124] Based on the current quadruple position vector P η (Ti The attitude is updated by using the real-time rotational angular velocity vector measured by the gyroscope in the IMU (Inertial Measurement Unit), i.e., obtaining the current attitude matrix. The recursive formula for the attitude matrix is:

[0125]

[0126]

[0127] Where I3 is a 3-order identity matrix. ε is the rotational angular velocity vector obtained in real time by the gyroscope. U (T mk ) represents the azimuth gyroscope drift obtained after the k-th correction, with an initial value of 0.

[0128] According to one embodiment of the present invention, in step S3, the current damped velocity after damping is obtained at the current moment using a recursive formula for horizontal damping based on the external reference velocity. And the current damped quaternion position vector P after damping at the current moment η_Damp (T i ), and calculate the normal vector integral term INT. η (T i In the steps of ), the recursive formula for horizontal damping is:

[0129]

[0130] Among them, v H (T i ) represents the horizontal velocity vector at the current moment.

[0131]

[0132] Among them, v Hr (T i ) represents the external reference horizontal velocity vector at the current moment. H_damp (T i ) represents the horizontal velocity vector after damping at the current moment.

[0133] The current damping velocity is obtained based on the recursive formula for horizontal damping. It is represented as:

[0134]

[0135] Wherein, η(T) i ) represents the current normal vector position vector, v h (T i ) represents the current vertical indicated velocity, and num and dum are both damping coefficients of the phase lag-lead series correction network, i.e.:

[0136] num(1)=1.0,num(2)=-1.99997367841154,num(3)=0.999975215058425

[0137] dum(1)=1.0, dum(2)=-1.99875923183544, dum(3)=0.998760768482328;

[0138] Based on the recursive formula for horizontal damping, a recursive formula for the position of the normal vector is constructed, which is expressed as:

[0139]

[0140] The current damping quadrupole position vector P is obtained based on the recursive formula of the normal vector position. η_Damp (T i ), which is represented as:

[0141]

[0142] Wherein, h(T) i () indicates the current indicated altitude;

[0143] Normal vector integral term INT η (T i The recursive formula for ) is:

[0144]

[0145] According to one embodiment of the present invention, in step S5, the comprehensive correction formula is:

[0146]

[0147] in,

[0148]

[0149] The comprehensive correction formula then simplifies to:

[0150]

[0151] Where, β ε For the time constants related to the Markov process.

[0152] According to one embodiment of the present invention, in steps S4 and S5, the output horizontal position is readjusted to the external reference position η. r (T mk In the step of ), the position readjustment formula used for horizontal position readjustment is expressed as:

[0153]

[0154] According to the drift angle Correct the attitude output to In the steps described, the formula for calculating attitude output correction is expressed as:

[0155]

[0156] Where [×] represents the corresponding skew-symmetric matrix.

[0157] According to one embodiment of the present invention, in step S5, the normal vector position error vector δη(T) mk ) is represented as:

[0158]

[0159] Wherein, η(T) mk ) represents the normal vector position vector before the position is readjusted at the k-th correction time.

[0160] According to the present invention, the relationship between the damped normal vector position error vector δη and the drift angle error vector in the e-frame is:

[0161]

[0162] Since this coefficient matrix [η×] is only related to the normal vector, and not to the latitude L or transverse latitude L t The tangent value is no longer relevant, therefore the comprehensive correction method of the present invention is applicable globally.

[0163] Furthermore, the differential equation for the drift angle in the e-system is:

[0164]

[0165] Since the horizontal gyroscope drift effect of the rotation-modulated inertial navigation system has been sufficiently suppressed, this invention only considers the residual equivalent azimuth gyroscope drift as the main error factor. Because the normal vector direction is the local perpendicular direction, therefore:

[0166]

[0167] Where, ε U For equivalent azimuth gyroscope drift, w g Let ε be the random walk error of the gyroscope angle. Combining this with the above formula, let ε... U Modeling it as a slowly changing Markov process, we can obtain:

[0168]

[0169] Where, β ε Let w be the time constant related to the Markov process.ε It is related Gaussian white noise.

[0170] Furthermore, let the filtering state of the integrated correction system be:

[0171]

[0172] When T k，k-1 =t k -t k-1 When the value is small, the gyroscope drift and normal vector are approximately constant over a short period. Therefore, the filter time update equation can be obtained as follows:

[0173] x k =F(t) k ,t k-1 )x k-1 +w k-1

[0174] in,

[0175]

[0176]

[0177]

[0178]

[0179]

[0180] Among them, w gdk-1 w εdk-1 They are respectively with w g w ε The corresponding discretized noise.

[0181] Because the above derivation process does not include the slow speed of the carrier movement and the gyroscope drift angular velocity error ε 0 The assumption of approximating constant values ​​avoids the limitations imposed by traditional methods on the ship's trajectory and operating status.

[0182] Furthermore, if we select the output normal vector η of the marine inertial navigation system... INS Normal vector η of satellite navigation reference position GNSS The difference z GNSS As an observation, the observation equation can be expressed as:

[0183]

[0184] Where, ν GNSS Gaussian white noise is used to represent the observed positioning normal vector for satellite navigation information.

[0185] Therefore, the observation matrix is:

[0186] H GNSS (t k )=[[η(t k )×] 0 3×1 ]

[0187] In summary, the integrated correction algorithm of this invention avoids the complexity and discontinuity caused by algorithm switching when entering and leaving polar regions, and has global applicability; it avoids restrictions on the ship's motion trajectory and operating status, and has strong practicality; it accurately estimates the constant drift of the azimuth gyroscope, effectively suppresses navigation errors accumulated over long voyages, and improves navigation and positioning accuracy.

[0188] In addition, the coordinate system involved is defined as follows:

[0189] i-frame: Geocentric inertial coordinate system;

[0190] e-system: Geocentric Earth-Fixed Coordinate System;

[0191] n-system: local navigation coordinate system; in this paper, the Northeastern Sky (ENU) geographic coordinate system is selected.

[0192] b-frame: Carrier coordinate system;

[0193] o-frame: qep coordinate system. The origin of the o-frame is located at the location of the carrier. The q-axis is parallel to the intersection of the equatorial plane and the local meridian, pointing outward from the Earth's axis. The e-axis is tangent to the iso-latitude circles and points eastward. The p-axis is parallel to the polar axis, forming a right-handed coordinate system.

[0194] The above description is merely an example of a specific solution of the present invention. For any devices and structures not described in detail herein, it should be understood that they are implemented using common devices and methods already available in the art.

[0195] The above description is merely one embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the invention by those skilled in the art. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An inertial navigation integrated correction method based on a normal vector position model, characterized in that, include: S1. Receive the real-time rotational angular velocity vector and specific force vector from the IMU inertial measurement unit, and load the initial navigation parameters, wherein the initial navigation parameters include: initial latitude and longitude, and initial indicated altitude. Initial navigation speed Initial navigation attitude matrix ; and, based on the initial navigation parameters, obtain the initial normal vector position vector of the vehicle. Initial quadruplet position vector Initial direction cosine matrix And obtain the initial velocity in the Earth-centered Earth-fixed coordinate system. and initial attitude matrix ; S2. Based on the previous velocity in the geocentric-ground-fixed coordinate system at the previous moment. Previous attitude matrix ,Previous quadruplet position vector The measurements from the gyroscope and accelerometer in the IMU inertial measurement unit and the azimuth gyroscope drift. The current velocity at the current moment is obtained using a recursive formula derived from pure inertial navigation calculation. Current attitude matrix Current quadruplet position vector ; S3. Obtain the current damped velocity at the current moment using the recursive formula for horizontal damping based on the external reference velocity. and the current damped quaternion position vector after damping at the current moment And calculate the integral term of the normal vector. Among them, the normal vector integral term The recursive formula is: ； S4. Determine if comprehensive calibration is needed and if the position from the external measuring device has been successfully received. If calibration is not needed, proceed to S2; if comprehensive calibration is needed, successfully receive the real-time latitude and longitude from the external measuring device and obtain the external reference position. Then, based on the number of observations k Select the external reference position for readjustment The method in which the number of observations k If the value is 1, the output horizontal position will be readjusted to the external reference position. Record the time T of the successful observation. mk And the normal vector integral term Set to zero; S5. If the number of observations k If the value is greater than 1, the horizontal position of the output will be readjusted to the external reference position. According to the normal vector position error vector The time interval ΔT between the previous observation and the previous observation mk and normal vector integral term The drift angle is obtained according to the comprehensive correction formula. And azimuth gyroscope drift According to the drift angle Correct the attitude matrix output to And azimuth gyroscope drift This will be used for subsequent navigation calculations using pure inertial navigation, and finally, the time T of a successful observation will be recorded. mk And the normal vector integral term Set to zero; S6. Repeat steps S2 to S5 until navigation ends; The comprehensive correction formula is as follows: in, The comprehensive correction formula then simplifies to: in, β ε For the time constants related to the Markov process.

2. The inertial navigation integrated correction method according to claim 1, characterized in that, In step S1, the initial normal vector position vector of the vehicle is obtained based on the initial navigation parameters. Initial quadruplet position vector Initial direction cosine matrix The steps include: Calculate the initial normal vector and position vector of the vehicle based on the initial latitude and longitude. and the initial quadruplet position vector ; Based on the initial normal vector position vector Calculate the initial direction cosine matrix .

3. The inertial navigation integrated correction method according to claim 2, characterized in that, In step S1, the initial normal position vector of the vehicle is calculated based on the initial latitude and longitude. and the initial quadruplet position vector In the step, the initial normal vector position vector The calculation formula is expressed as: in, L (T0) λ (T0) represents the initial latitude and longitude. , and These are the initial normal vector and the position vector, respectively. exist x , y and z The three components of direction; The initial quadruplet position vector The calculation method is expressed as follows: in, h (T0) is the initial indicated height.

4. The inertial navigation integrated correction method according to claim 3, characterized in that, In step S1, based on the initial normal vector position vector Calculate the initial direction cosine matrix In the step, the initial direction cosine matrix Let be the direction cosine matrix between the geocentric and geofixed coordinate system and the local horizontal north-pointing geographic coordinate system. Its calculation method is expressed as follows: 。 5. The inertial navigation integrated correction method according to claim 4, characterized in that, In step S1, the initial velocity in the Earth-centered Earth-fixed coordinate system is obtained. and initial attitude matrix In the steps, based on the initial navigation speed The initial navigation attitude matrix The initial velocity is obtained and the initial attitude matrix ; wherein, the initial velocity The calculation formula is expressed as: The initial attitude matrix The calculation formula is expressed as: 。 6. The inertial navigation integrated correction method according to claim 5, characterized in that, In step S2, the current velocity at the current moment is obtained using the recursive formula of pure inertial navigation solution. Current attitude matrix Current quadruplet position vector The steps include: The current velocity at the current moment is obtained using the velocity recursion formula in the Earth-centered Earth-fixed coordinate system. It is represented as: in, t The sampling interval is... This is the Earth's rotational angular velocity vector. The specific force vector is obtained in real time by the accelerometer in the IMU inertial measurement unit. This is the projection of the gravity vector from the previous moment onto the Earth-centered, Earth-fixed coordinate system. for skew-symmetric matrix; Based on the determined current speed Update the position vector of the previous quadruplet To obtain the current quadruple position vector That is, the recursive formula for the position of the quadruple is expressed as: in, , , The radius of the circle is the area between the east and west. The radius of the meridian circle; The radius of the Maoyou circle and the radius of the meridian circle Represented as: in, R 0 = 6378137 is the Earth's radius. e =0.081819191 is the first eccentricity of the Earth; Based on the current quadruplet position vector The attitude is updated by using the real-time rotational angular velocity vector measured by the gyroscope in the IMU (Inertial Measurement Unit), i.e., the current attitude matrix is ​​obtained. Then the recursive formula for the attitude matrix is: in, It is a 3-order identity matrix. The angular velocity vector obtained in real time by the gyroscope. For the first k The azimuth gyroscope drift obtained after the second correction has an initial value of 0.

7. The inertial navigation integrated correction method according to claim 6, characterized in that, In step S3, the current damped velocity after damping is obtained at the current moment using the recursive formula for horizontal damping based on the external reference velocity. and the current damped quaternion position vector after damping at the current moment And calculate the integral term of the normal vector. In the following steps, the recursive formula for the horizontal damping is: in, The horizontal velocity vector at the current moment; in, Let the external reference horizontal velocity vector be at the current moment. The horizontal velocity vector after damping at the current moment; The current damping velocity is obtained based on the recursive formula for the horizontal damping. It is represented as: in, This represents the current position vector of the normal vector. This represents the current vertical indicated velocity. num and dum are both damping coefficients of the phase lag-lead series correction network, i.e.: num(1)=1.0, num(2)=-1.99997367841154, num(3)=0.999975215058425 dum(1)=1.0, dum(2)=-1.99875923183544, dum(3)=0.998760768482328; Based on the recursive formula for the horizontal damping, a recursive formula for the position of the normal vector is constructed, which is expressed as: The current damping quadrupole position vector is obtained based on the recursive formula for the normal vector position. It is represented as: in, This indicates the current altitude.

8. The inertial navigation integrated correction method according to claim 7, characterized in that, In steps S4 and S5, the output horizontal position is readjusted to the external reference position. In the steps, the position readjustment formula used for horizontal position readjustment is expressed as: According to the drift angle Correct the attitude output to In the steps described, the formula for calculating attitude output correction is expressed as: Where [×] represents the corresponding skew-symmetric matrix.

9. The inertial navigation integrated correction method according to claim 8, characterized in that, In step S5, the normal vector position error vector Represented as: in, Indicates the first k The normal vector position vector before the position is readjusted at the next correction time.

Images