Underwater attitude optimization method with multiple inertial navigation topology constraints

Abstract

The invention discloses a multi-inertial navigation topology constraint underwater attitude optimization method, relates to the technical field of underwater navigation, and improves attitude precision under an underwater electromagnetic signal denial condition. The scheme comprises the following steps: S1, establishing an augmented error state dynamic model, and increasing the speed, position and attitude between a single-axis RINS and a double-axis RINS as a new error state; s2, designing a measurement model of a filter, and constructing a complete observation equation; s3, the observability of the filter is analyzed, the motion required by the excitation error is judged, and the simplified filter is obtained for different scenes; and S4, carrying out multi-inertial navigation information fusion aiming at two conditions of dual RINS configuration and triple RINS configuration respectively.

Description

Technical Field

[0001] This invention relates to the field of underwater navigation technology, and in particular to an underwater attitude optimization method with multi-inertial navigation topology constraints. Background Technology

[0002] Long-endurance, high-precision autonomous maritime navigation places extremely high demands on inertial systems. Rotating inertial navigation systems (RINS), also known as indexed inertial measurement units (IMUs), are widely used in modern ships. RINS typically employ a single-axis or dual-axis rotation scheme to average the effects of gyro drift and accelerometer bias. Single-axis RINS averages gyro drift and accelerometer bias by rotating the IMU along the azimuth axis, but it cannot average gyro azimuth drift, leading to an increase in position error over time. In contrast, dual-axis RINS, by rotating the IMU along both the azimuth and roller axes, can average gyro drift and accelerometer bias in all directions, thereby improving positioning accuracy.

[0003] The primary error source for dual-axis RINS is gyro random walk error, which is proportional to the square root of the navigation time. While dual-axis RINS outperform single-axis RINS, they are also larger, more powerful, more complex, and more expensive. To meet the demands of modern ships for long-term, high-reliability navigation, redundant RINS configurations, including two or three sets, are typically employed. Dual-axis RINS usually serve as the primary RINS, with the single-axis RINS acting as a hot backup. However, single-axis RINS cannot average out the un-estimated residual azimuth gyro drift during the dock alignment phase, leading to reduced positioning accuracy.

[0004] In a three-axis RINS configuration, only one set of dual-axis RINS is designated as the primary RINS, while the other dual-axis and single-axis RINS serve as hot backup systems. To improve the performance of the single-axis RINS and ensure positioning accuracy even if the dual-axis RINS fails, navigation information fusion is required within the redundant RINS configuration. Furthermore, online performance evaluation between the dual-axis RINS is necessary to select the system with higher accuracy as the primary RINS.

[0005] In existing underwater navigation research, although some researchers have established redundant inertial navigation systems (INS), these methods still offer limited improvement in accuracy, and the constraints are not fully utilized. Therefore, to improve the attitude accuracy of underwater navigation systems, conserve research resources, and accelerate research efficiency, designing a multi-INS topology constraint method for underwater attitude optimization is particularly important. Inertial navigation system attitude suppression methods are relatively mature and widely used in low-dynamic environments such as vehicular applications. However, underwater navigation systems operate in more complex environments with more intense motion, and existing INS systems cannot fully meet the needs of underwater navigation. Therefore, there is an urgent need to design a multi-INS topology constraint underwater attitude optimization method. Summary of the Invention

[0006] In view of this, the present invention provides an underwater attitude optimization method with multi-inertial navigation topology constraints, which improves attitude accuracy under underwater electromagnetic signal rejection conditions.

[0007] To achieve the above objectives, the technical solution of the present invention includes the following steps:

[0008] S1. Establish an augmented error state dynamic model, adding velocity, position, and attitude between single-axis RINS and dual-axis RINS as new error states.

[0009] S2. Design the measurement model of the filter and construct the complete observation equation.

[0010] S3. Analyze the observability of the filter, determine the motion required for the excitation error, and obtain a simplified filter for different scenarios.

[0011] S4. Perform multi-inertial navigation information fusion for both dual RINS configuration and triple RINS configuration.

[0012] Furthermore, the specific steps of S1 are as follows:

[0013] S101. Establish an augmented error state dynamic model, including:

[0014] Considering that RINS evolved from strapdown INS, their error state dynamic models have the same form; taking single-axis RINS1 and dual-axis RINS as examples. k The position error difference, velocity error difference, and attitude error difference between (k=2,3) are used as new error states, and a dynamic model of the error states is given.

[0015]

[0016] in

[0017]

[0018]

[0019] Where δL 1k This represents the latitude error δL1 of uniaxial RINS1 and the latitude error of biaxial RINS1. k k = 2,3 latitude error δL k The difference, δλ 1k This represents the longitude error δλ1 of uniaxial RINS1 and biaxial RINS1. k Longitude error δλ k difference; Single-axis RINS1 speed error With dual-axis RINSk Velocity error Difference; is the attitude error of the single-axis RINS1 and the two-axis RINS k Attitude error Difference; is the earth's rotation speed in the northeast direction of the n coordinate system, is the rotation rate, is the angular rate of the navigation coordinate system relative to the inertial coordinate system i; similarly, represents the earth's rotation speed error, represents the transmission speed error, represents the difference of the angular velocity error of the navigation system; and the three-axis error vector of the gyroscope modeled as the sum of a constant drift and white noise, and the three-axis error vector of the accelerometer modeled as the sum of a constant bias and white noise; constant drift and white noise and and The vector of is the three-axis error vector of the accelerometer modeled as the sum of a constant bias and white noise; f n and v n are the specific force and velocity vectors expressed in the navigation frame; is the first derivative of δL 1k R N is the radius of curvature of the prime vertical, h is the altitude,​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​This is the Earth's rotational angular velocity vector. For single-axis RINS1 and dual-axis RINS k In the navigation system with k=2,3, ω ie difference, This is the measurement error of the Earth's rotation angular velocity using a single-axis RINS1. For dual-axis RINS k The measurement error of the Earth's rotation angular velocity at k=2,3 This represents the angular velocity vector of the single-axis RINS1 navigation system relative to the geocentric coordinate system. For dual-axis RINS k The angular velocity vector of the navigation frame with k=2,3 relative to the geocentric coordinate system. For single-axis RINS1 and dual-axis RINS k The difference in eastward velocities between k=2 and k=3, δL 1k For single-axis RINS1 and dual-axis RINS k The latitude difference between k=2 and k=3 The Earth's rotational angular velocity vector in the navigation system. Let be the angular velocity vector of the navigation system relative to the geocentric coordinate system. ε1 represents the angular velocity error output by the single-axis RINS1 gyroscope, and ε2 represents the zero-bias vector of the single-axis RINS1 gyroscope. This outputs the raw gyroscope data for a single-axis RINS1. For dual-axis RINS k The angular velocity error of the gyroscope output for k=2,3 is ε. k For dual-axis RINS k The zero bias vector of the gyroscope with k=2,3 For dual-axis RINS k Output the raw numbers of the gyroscope with k=2 or 3. The zero bias vector of the accelerometer for single-axis RINS1. This is the raw accelerometer output for a single-axis RINS1. For dual-axis RINS k , k = 2, 3 accelerometer zero bias vector For dual-axis RINS k The raw accelerometer data output for k=2,3.

[0020] S102, with RINS1 and RINS k The three-component drift vectors ε1 and ε of the gyroscope k Establish a redundant state vector, including:

[0021] With RINS1 and RINS k Accelerometer three-component bias vector and As the widening error state, the corresponding dynamic model of the error state is obtained as follows:

[0022]

[0023] The widened error state vector of the redundant RINS configuration is derived from Given; where δL, δλ, δv E ,δv N ,φ E ,φ N ,φ U ,ε, These represent latitude error, longitude error, eastward velocity error, northward velocity error, eastward attitude error, northward attitude error, celestial attitude error, gyro bias, and accelerometer bias, respectively. The subscripts 1 and k represent single-axis RINS1 and dual-axis RINS, respectively. k k = 2, 3, where the subscripts x, y, z represent the directions of the three sensitive axes of the gyroscope and accelerometer, respectively.

[0024] The process noise vector is written as:

[0025]

[0026] Where the subscript ε represents the physical quantity of the gyroscope, The physical quantity represented by the accelerometer.

[0027] The augmented error state dynamic model is as follows:

[0028]

[0029] Where x(t) is the augmented error state vector and w(t) is the process noise vector.

[0030] The components of the system matrix F(t) and the noise matrix G(t) are:

[0031]

[0032] The meanings of each parameter are the same as those in the variable descriptions in S101.

[0033] The process noise matrix is ​​represented as:

[0034]

[0035] in, This is the inertial navigation attitude matrix, where the subscripts 1 and k represent the single-axis RINS1 and dual-axis RINS1, respectively. k , k = 2, 3.

[0036] Furthermore, the specific implementation steps of S2 are as follows:

[0037] S201. Establish redundant inertial navigation spatial relationships, single-axis RINS1 and dual-axis RINS1. k For k=2 and 3, the output navigation information, including position and velocity, satisfies the following constraints:

[0038]

[0039] in, and These represent single-axis RINS1 and dual-axis RINS, respectively. k , and Represents their positional error, r n Indicates the actual location. For single-axis RINS1 and dual-axis RINS k The lever arm between; and Indicates single-axis RINS1 and dual-axis RINS k Speed ​​output, Indicate IMU series b1 and b k The relative angular velocity between them.

[0040] S202. Establish the lever arm model, specifically as follows:

[0041]

[0042] The corresponding component forms are given by the formula.

[0043]

[0044] This is a latitude estimate. This is an estimated value for longitude. This is an estimate of the eastward velocity. Here, i represents the estimated northbound velocity, with values ​​of 1 and k, representing single-axis RINS1 and dual-axis RINS1, respectively. k , k = 2, 3.

[0045] A measurement model for the augmented error state is given.

[0046] z(t)=H(t)x(t)+v(t)

[0047] In the formula:

[0048]

[0049] Where z(t) is the measurement vector, δL 1k ,δλ 1k , Let H(t) represent the latitude error, longitude error, eastward velocity error, and northward velocity error at time k, respectively; H(t) is the measurement matrix; and v(t) is the measurement noise vector.

[0050] Furthermore, the specific implementation steps of S3 are as follows:

[0051] S301. Separate the augmented error state by dividing the augmented error state x(t) into two parts.

[0052]

[0053] Where x(t) represents the error state, subscript 1 indicates RINS1, subscript 2 indicates RINS2; δL 1k ,δλ 1k , These represent the latitude error, longitude error, eastward velocity error, and northward velocity error at time k, respectively; φ is the misalignment angle; ε is the gyroscope zero bias; This indicates zero bias of the accelerometer; the subscripts indicate the east, north, and sky directions.

[0054] The system description has been rewritten as follows

[0055]

[0056] In the formula, x(t) represents the error state; y(t) or z(t) represents the output state; and F(t) is the state transition matrix. ij (t) represents the element at the corresponding position in the state transition matrix, describing the evolution of the system state over time; G(t) is the process noise correlation matrix; w(t) is the process noise vector; v(t) is the measurement noise vector.

[0057] S302. Establish an observability model, including:

[0058] The system matrix and measurement matrix are simplified as follows:

[0059]

[0060] Among them, F ij (t) represents the element at the corresponding position in the state transition matrix; A(t) is the state transition matrix, describing the evolution of the error state over time; C(t) is the output matrix; G is the magnitude of Earth's gravity; This is the projection of the Earth's rotational angular velocity in the n-frame. For n-system to b i The rotation matrix of the system, i takes the values ​​1 and k, representing the single-axis RINS1 and the dual-axis RINS1, respectively. k , k = 2, 3.

[0061] The stripping of the observability matrix O is used to analyze the observability of the equations, by

[0062]

[0063] Among them O j =[C T :(CA) T :(CA 2 ) T :…:(CA 12 ) T ] T j = 1, 2, ..., m, where the number of states of the reduced-order dynamic system is 13 and m is a block.

[0064] Get CA i The general form of i = 1, 2, ..., 12.

[0065]

[0066] Due to CA i Rows with i ≥ 3 are only CA and CA 2 A linear combination of rows gives:

[0067]

[0068] in

[0069]

[0070] For O(m) to be full rank, two sufficient conditions must be satisfied:

[0071] 1) Because The rank of m is 5, so the minimum segment of m is 3, making matrix O(m) full rank; 2) By checking equation (28), and Both must be changed to make the matrix O(m) full rank; furthermore, only when b1 and b k O(m) becomes full rank only when the relative attitude between systems changes in the horizontal and azimuth directions.

[0072] The azimuth frame of a single-axis RINS1 typically rotates its IMU back and forth between four orthogonal positions relative to the zero position: 0°, 90°, 270°, and 180°; a dual-axis RINS... k The azimuth and roll frames of the IMU rotate periodically through two orthogonal positions, 0° and 180°, relative to their own zero position; the b1 system and b k There are 4×2=8 relative attitudes between the systems, which satisfies sufficient conditions 1) and 2), making the reduced-order dynamic system completely observable at a certain moment.

[0073] The augmented error state dynamic system is fully observable.

[0074] Furthermore, the S4 configuration for dual RINS includes:

[0075] Single-axis RINS1 and dual-axis RINS2.

[0076] The error state of a single-axis RINS1 is represented as follows:

[0077]

[0078] x1(t) is the error state vector of a single-axis RINS1, containing gyroscope drift and accelerometer bias; δL1, δλ1, δv e ,δλ x ,δν t These represent the errors in longitude, latitude, heading, two horizontal angles, northward speed, and eastward speed, respectively.

[0079] The corresponding error state dynamic model is given

[0080]

[0081] In the formula:

[0082]

[0083] Where A1(t) is the state transition matrix, describing the evolution of the error state over time; B1(t) is the control input matrix, describing the influence of the control input on the error state; C1(t) is the output matrix; and x1(k) is the time t. k The error state prediction is set with initial values ​​of x1(0) = 0; u1(k) is the estimate of gyroscope drift and accelerometer bias, Δt is the discrete step length, and ε is the triaxial gyroscope zero bias. It is a three-way addition table with zero bias; The elements in the process noise vector w1(t) represent the noise from the gyroscope and accelerometer, respectively.

[0084] The designed Kalman filter can be used to estimate the gyroscope drift and accelerometer deviation of a single-axis RINS1, thereby predicting the deterministic position error.

[0085] The position error prediction model is

[0086] x1(k+1)=Φ1(k+1,k)x1(k)+Γ1(k)u1(k)

[0087] In the formula

[0088]

[0089] A1(t) is the state transition matrix, describing the evolution of the error state over time; B1(t) is the control input matrix, describing the influence of the control input on the error state; C1(t) is the output matrix.

[0090] The position accuracy is improved by subtracting the position error prediction from the position output of the single-axis RINS1:

[0091]

[0092] The superscripts “-” and “∧” represent the correction value and the predicted value, respectively.

[0093] Furthermore, the specific configuration of triple RINS in S4 is as follows:

[0094] Triple RINS includes single-axis RINS1, dual-axis RINS2, and RINS3.

[0095] Two parallel computational Kalman filters are established between single-axis RINS1 and dual-axis RINS2 and dual-axis RINS3 to estimate the error states; the common error states of the two Kalman filters are gyroscope drift and accelerometer bias in single-axis RINS1.

[0096] The calculated state error covariance is given by the following Riccati equation:

[0097]

[0098] Among them, P c F(t) is the state error covariance matrix (calculated value), reflecting the uncertainty estimate of the state error at time t; F(t) is the state transition matrix, describing the evolution of the system state over time; K c (t) represents the calculated gain; Q c The assumed process noise power spectrum intensity is given. Process noise reflects the noise level of the inertial sensor, including the white noise random drift of the dual-axis RINS gyroscope; G(t) is the process noise correlation matrix, which describes the effect of process noise on the state; R is the observation noise covariance matrix, which assumes the observation noise power spectrum intensity.

[0099] In practical applications, the relationship between the assumed value and the actual value is expressed as follows:

[0100] Q c = (1+α)Q

[0101] Where α represents a scalar and Q is the actual value.

[0102] The actual state error covariance is:

[0103]

[0104] Among them, P a (t)=E{[x a (t)-x(t)][x a (txt)] T} represents the actual state error covariance; other parameters are the same as those in the standard Riccati equation.

[0105] The equation for the difference between the actual and calculated covariance matrices is as follows:

[0106]

[0107] An explicit analytical solution is given.

[0108]

[0109] Where, ΔP(0)=P c (0)-P a (0) = 0, Φ c Let Φ be the state transition matrix, and Φ c (t,t)=I.,Q c ≥Q, α≥0, P a (t)=P c (t)-ΔP(t).

[0110] Beneficial effects:

[0111] This invention provides an underwater attitude optimization method with multi-inertial navigation topology constraints, comprising the following steps: S1, designing an augmented error state dynamic model, adding a single-axis RINS, and using the velocity, position, and attitude between dual-axis RINS as new error states, thus providing the error state dynamic model; S2, designing a filter measurement model and constructing a complete observation equation; S3, analyzing the observability of the filter, determining the motion required for the excitation error, and obtaining a simplified filter for different scenarios; S4, performing multi-inertial navigation information fusion in two cases, specifically targeting dual RINS configurations (single-axis RINS1 and dual-axis RINS2) and triple RINS configurations (single-axis RINS1, dual-axis RINS2, and RINS3). This method improves attitude accuracy under underwater electromagnetic signal rejection conditions. Attached Figure Description

[0112] Figure 1 This is a schematic diagram of redundant RINS configuration in an embodiment of the present invention.

[0113] Figure 2 A flowchart of an underwater attitude optimization method with multi-inertial navigation topology constraints provided in an embodiment of the present invention. Detailed Implementation

[0114] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0115] This invention provides an underwater attitude optimization method with multi-inertial navigation topology constraints, the process of which is as follows: Figure 2 As shown, the steps are as follows:

[0116] S1. Design an augmented error state dynamic model, adding a single-axis RINS and the velocity, position and attitude between the two-axis RINS as new error states, and give the error state dynamic model;

[0117] S2. Design the measurement model of the filter and construct the complete observation equation;

[0118] S3. Analyze the observability of the filter, determine the motion required for the excitation error, and obtain a simplified filter for different scenarios.

[0119] S4. In two cases, multi-inertial navigation information fusion is performed. The scenarios targeted by this invention are dual RINS configuration (single-axis RINS1 and dual-axis RINS2) and triple RINS configuration (single-axis RINS1, dual-axis RINS2, RINS3).

[0120] Furthermore, in step S1, the specific steps are as follows:

[0121] S101. Establish an augmented error state dynamic model, including:

[0122] Considering that RINS evolved from strapdown INS, their error state dynamic models share the same form. Using the position error difference, velocity error difference, and attitude error difference between single-axis RINS1 and dual-axis RINSk (k=2,3) as new error states, a dynamic model of the error states is presented.

[0123]

[0124] in

[0125]

[0126] Where, δL 1k This represents the latitude error δL1 of uniaxial RINS1 and the latitude error δL of biaxial RINSk (k=2,3). k The difference, δλ 1k This represents the longitude error δλ1 of uniaxial RINS1 and the longitude error δλ of biaxial RINSk. k difference. Single-axis RINS1 speed error Dual-axis RINSk speed error difference. is the attitude error of the single-axis RINS1 and the attitude error of the two-axis RINSk difference. is the earth rotation speed in the n coordinate system (north-east up), is the rotation rate, is the angular rate of the navigation coordinate system relative to the inertial coordinate system i; similarly, represents the earth rotation speed error, represents the transfer speed error, represents the difference of the navigation system angular velocity error. and The triple error vector of the gyroscope modeled as the sum of a constant drift and white noise, and The triple error vector of the accelerometer modeled as the sum of a constant bias and white noise. f n and v n are the specific force and velocity vectors represented in the navigation coordinate system. Since each RINS is installed adjacent to each other and the relative distance is usually less than 1 meter, the latitude, velocity and specific force roughly reflect the information of the same point. The constant drift and white noise and and The vector of is the triple error vector of the accelerometer modeled as the sum of a constant bias and white noise. f n and v n are the specific force and velocity vectors represented in the navigation frame. Since each RINS is installed adjacent, the relative distance is usually less than 1m, so the latitude, velocity and specific force roughly reflect the information from the same point; is the first derivative of δL 1k R N is the radius of curvature of the prime vertical, h is the height, is the northward velocity difference between the single-axis RINS1 and the two-axis RINS k , k = 2, 3, v E is the inertial navigation eastward velocity, L is the latitude, R E is the radius of curvature of the prime vertical, is the eastward velocity difference between the single-axis RINS1 and the two-axis RINS k , k = 2, 3, f n is the specific force vector in the navigation system, v n is the northward velocity vector, is the attitude matrix of the single-axis RINS1, is the attitude matrix of the two-axis RINS k , k = 2, 3, is the angular rate of the navigation coordinate system relative to the inertial system, used for rotation compensation in navigation calculations, is the difference between the single-axis RINS1 and the two-axis RINS kk=2,3 The difference, δL 1k For single-axis RINS1 and dual-axis RINS k The latitude difference between k=2 and 3, ω ie This is the Earth's rotational angular velocity vector. For single-axis RINS1 and dual-axis RINS k In the navigation system with k=2,3, ω ie difference, This is the measurement error of the Earth's rotation angular velocity using a single-axis RINS1. For dual-axis RINS k The measurement error of the Earth's rotation angular velocity at k=2,3 This represents the angular velocity vector of the single-axis RINS1 navigation system relative to the geocentric coordinate system. For dual-axis RINS k The angular velocity vector of the navigation frame with k=2,3 relative to the geocentric coordinate system. For single-axis RINS1 and dual-axis RINS k The difference in eastward velocities between k=2 and k=3, δL 1k For single-axis RINS1 and dual-axis RINS k The latitude difference between k=2 and k=3 The Earth's rotational angular velocity vector in the navigation system. Let be the angular velocity vector of the navigation system relative to the geocentric coordinate system. ε1 represents the angular velocity error output by the single-axis RINS1 gyroscope, and ε2 represents the zero-bias vector of the single-axis RINS1 gyroscope. This outputs the raw gyroscope data for a single-axis RINS1. For dual-axis RINS k The angular velocity error of the gyroscope output for k=2,3 is ε. k For dual-axis RINS k The zero bias vector of the gyroscope with k=2,3 For dual-axis RINS k Output the raw numbers of the gyroscope with k=2 or 3. The zero bias vector of the accelerometer for single-axis RINS1. This is the raw accelerometer output for a single-axis RINS1. For dual-axis RINS k , k = 2, 3 accelerometer zero bias vector For dual-axis RINS k The raw accelerometer data output for k=2,3.

[0127] S102, using the three-component gyroscope drift vectors ε1 and ε of RINS1 and RINSk. k Establish a redundant state vector, including:

[0128] The three-component bias vectors of the accelerometers RINS1 and RINSk and As the widening error state, the corresponding error state dynamic model is obtained as follows:

[0129]

[0130] The widened error state vector of the redundant RINS configuration is derived from Given; where δL, δλ, δv E ,δv N ,φ E ,φ N ,φ U ,ε, These represent latitude error, longitude error, eastward velocity error, northward velocity error, eastward attitude error, northward attitude error, celestial attitude error, gyro bias, and accelerometer bias, respectively. The subscripts 1 and k represent single-axis RINS1 and dual-axis RINS, respectively. k k = 2, 3, where the subscripts x, y, z represent the directions of the three sensitive axes of the gyroscope and accelerometer, respectively.

[0131] The process noise vector can be written as:

[0132]

[0133] The error state of the vertical channel is ignored due to its weak coupling with the horizontal channel. The augmented error state dynamic model is as follows:

[0134]

[0135] Where x(t) is the augmented error state vector and w(t) is the process noise vector.

[0136] The components of the system matrix F(t) and the noise matrix G(t) are:

[0137]

[0138]

[0139] The meanings of each parameter are the same as those in the variable descriptions in S101.

[0140] The process noise matrix is ​​represented as:

[0141]

[0142] in, This is the inertial navigation attitude matrix, where the subscripts 1 and k represent the single-axis RINS1 and dual-axis RINS1, respectively. k, k = 2, 3.

[0143] Furthermore, the specific implementation steps of step S2 are as follows:

[0144] S201. Establish redundant inertial navigation spatial relationships, including:

[0145] Figure 1 A triple RINS configuration is described. The diagram also applies to a dual RINS configuration after removing the dual-axis RINS3. The navigation information, including position and velocity, output by the single-axis RINS1 and the dual-axis RINSk (k=2,3) satisfies the following constraints.

[0146]

[0147] in, and These represent single-axis RINS1 and dual-axis RINS, respectively. k , and Represents their positional error, r n Indicates the actual location. For single-axis RINS1 and dual-axis RINS k The lever arm between; and This indicates the speed output of single-axis RINS1 and dual-axis RINSk. Indicate IMU series b1 and b k The relative angular velocity between them.

[0148] S202. Establish the lever arm model, including:

[0149] Since the lever arm is usually less than 1m and can be precisely calibrated in advance, its influence can be ignored.

[0150] Subtract the second equation from the first equation, ignoring the lever arm effect.

[0151]

[0152] Figure 1 Redundant RINS configuration diagram

[0153] The corresponding component forms are given by the formula.

[0154]

[0155] This is a latitude estimate. This is an estimated value for longitude. This is an estimate of the eastward velocity. Here, i represents the estimated northbound velocity, with values ​​of 1 and k, representing single-axis RINS1 and dual-axis RINS1, respectively.k , k = 2, 3.

[0156] A measurement model for the augmented error state is given.

[0157] z(t)=H(t)x(t)+v(t)

[0158] in

[0159]

[0160] Where z(t) is the measurement vector, δL 1k ,δλ 1k , Let represent the latitude error, longitude error, eastward velocity error, and northward velocity error at time k, respectively, and v(t) be the measurement noise vector. H(t) is the measurement matrix; v(t) is the measurement noise vector.

[0161] Furthermore, the specific implementation steps of step S3 are as follows:

[0162] S301, Separation augmentation error state, including:

[0163] The augmented error state x(t) is divided into two parts to obtain

[0164]

[0165] x(t) represents the error state, with subscript 1 indicating RINS1 and subscript 2 indicating RINS2; δL 1k ,δλ 1k , These represent the latitude error, longitude error, eastward velocity error, and northward velocity error at time k, respectively. φ is the misalignment angle; ε is the gyroscope zero bias. This indicates zero bias of the accelerometer; the subscripts indicate the east, north, and sky directions.

[0166] The system description can be rewritten as follows:

[0167]

[0168] In the formula, x(t) represents the error state; y(t) or z(t) represents the output state; and F(t) is the state transition matrix. ij (t) represents the element at the corresponding position in the state transition matrix, describing the evolution of the system state over time; G(t) is the process noise correlation matrix; w(t) is the process noise vector; v(t) is the measurement noise vector.

[0169] S302. Establish an observability model, including:

[0170] For large ships, the speed is lower and the maneuverability is poorer. and fn ≈-g n Applicable to most cases. The system matrix and measurement matrix can be approximately simplified to...

[0171]

[0172] Among them, F ij (t) represents the element at the corresponding position in the state transition matrix; A(t) is the state transition matrix, describing the evolution of the error state over time; C(t) is the output matrix; G is the magnitude of Earth's gravity; This is the projection of the Earth's rotational angular velocity in the n-frame. For n-system to b i The rotation matrix of the system, i takes the values ​​1 and k, representing the single-axis RINS1 and the dual-axis RINS1, respectively. k , k = 2, 3.

[0173] The stripping of the observability matrix can be used to analyze the observability of the equations, by

[0174]

[0175] in

[0176]

[0177] The reduced-order dynamic system has 13 states, and m represents the number of blocks.

[0178] CA can be obtained i The general form of (i = 1, 2, ..., 12)

[0179]

[0180] Due to CA i The rows with (i≥3) are only CA and CA. 2 For a linear combination of rows, the following equation is self-evident.

[0181]

[0182] in

[0183]

[0184] For O(m) to be full rank, two sufficient conditions must be satisfied:

[0185] 1) Because The rank of m is 5, so the minimum segment of m is 3, making matrix O(m) full rank. 2) By checking equation (28), it can be found that and Both must be changed to make the matrix O(m) full rank. Furthermore, only when b1 and b... kO(m) becomes full rank only when the relative attitude between systems changes in the horizontal and azimuth directions.

[0186] The azimuth frame of a single-axis RINS1 typically rotates its IMU back and forth between four orthogonal positions (0°, 90°, 270°, 180°) relative to its zero position (Tucker and Levinson, 2000). The azimuth and roll frames of a dual-axis RINSk periodically rotate the IMU between two orthogonal positions (0°, 180°) relative to its own zero position (Yuane et al., 2012). Therefore, the b1 series and b... k There are 4 × 2 = 8 relative attitudes between the systems, which satisfies sufficient conditions 1) and 2), making the reduced-order dynamic system completely observable at a certain moment. Therefore, the augmented error state dynamic system is also completely observable.

[0187] Furthermore, the specific implementation steps of step S4 are as follows:

[0188] S401. Analyze the configuration of Case 1, including:

[0189] Scenario 1: Dual RINS Configuration (Single-axis RINS1 and Dual-axis RINS2). Due to its higher reliability, the single-axis RINS1 is typically used as a hot backup system for the dual-axis RINS2. Based on the fusion of navigation information between the two systems, the azimuth gyroscope drift of the single-axis RINS1 can be estimated, and the resulting position error can be predicted and compensated. Furthermore, all inertial sensor biases estimated by the designed Kalman filter can be used to monitor the system's operating status, and the built-in test program can diagnose system faults. When the fault detection unit diagnoses a dual-axis RINS2 fault, it still predicts the position error caused by the constant azimuth gyroscope drift of the single-axis RINS1 and uses the compensated navigation information provided by the single-axis RINS1 as the output.

[0190] The error state of a single-axis RINS1 is represented as follows:

[0191]

[0192] x1(t) is the error state vector of a single-axis RINS1, containing gyroscope drift and accelerometer bias. δL1,δλ1, δv e ,δλ x ,δν t These represent the errors in longitude, latitude, heading, two horizontal angles, northward speed, and eastward speed, respectively.

[0193] The corresponding error state dynamic model is given

[0194]

[0195] in:

[0196]

[0197] Where A1(t) is the state transition matrix, describing the evolution of the error state over time. B1(t) is the control input matrix, describing the influence of the control input on the error state. C1(t) is the output matrix; x1(k) represents time t. k The error state prediction is set with initial values ​​of x1(0) = 0; u1(k) is the estimate of gyroscope drift and accelerometer bias, Δt is the discrete step length, and ε is the triaxial gyroscope zero bias. It is a three-way addition table with zero bias; The elements in the process noise vector w1(t) represent the noise of the gyroscope and accelerometer, respectively. Using the designed Kalman filter, the gyroscope drift and accelerometer bias of the single-axis RINS1 can be estimated, thereby predicting the deterministic position error.

[0198] The designed Kalman filter can be used to estimate the gyroscope drift and accelerometer deviation of a single-axis RINS1, thereby predicting the deterministic position error.

[0199] The position error prediction model is

[0200] x1(k+1)=Φ1(k+1,k)x1(k)+Γ1(k)u1(k)

[0201] in

[0202]

[0203] Where x1(k) is time t k The error state prediction is initialized to x1(0) = 0; u1(k) is the estimate of gyroscope drift and accelerometer deviation, and Δt is the discrete step size. A1(t) is the state transition matrix, describing the evolution of the error state over time. B1(t) is the control input matrix, describing the influence of the control input on the error state. C1(t) is the output matrix.

[0204] Position accuracy can be improved to some extent by subtracting the predicted position error from the position output of a single-axis RINS1.

[0205]

[0206] The superscripts “-” and “∧” represent the correction value and the predicted value, respectively.

[0207] S402. Analyze the configuration of scenario two, including:

[0208] Scenario 2: Triple RINS Configuration (Single-axis RINS1, Dual-axis RINS2 and 3). In some special applications requiring exceptionally high reliability, a triple RINS configuration is needed, comprising two sets of dual-axis RINS and a single-axis RINS. The only residual gyroscope error source is white noise random drift, also known as angular random walk, which is the primary factor determining the performance of a dual-axis RINS. The angular random walk coefficients of the gyroscopes in a dual-axis RINS are typically quite similar, resulting in similar performance across different systems. Therefore, the online performance of different systems may vary, potentially showing inconsistencies with previous test results during navigation. It is necessary to conduct online performance evaluations of the dual-axis RINS to provide reference information for ship operators.

[0209] Two parallel computational Kalman filters were established between the single-axis RINS1 and the dual-axis RINS2 and dual-axis RINS3 respectively to estimate the error states. The common error states of the two Kalman filters are gyroscope drift and accelerometer bias in the single-axis RINS1.

[0210] The calculated state error covariance is given by the following Riccati equation:

[0211]

[0212] Among them, P c (t) is the state error covariance matrix (calculated value), reflecting the uncertainty estimate of the state error at time t; F(t) is the state transition matrix, describing the evolution of the system state over time. K c (t) represents the calculated gain; Q c The assumed process noise power spectrum intensity is given. Process noise reflects the noise level of the inertial sensor, including the white noise random drift of the dual-axis RINS gyroscope; G(t) is the process noise correlation matrix, which describes the effect of process noise on the state; R is the observation noise covariance matrix, which assumes the observation noise power spectrum intensity.

[0213] Process noise reflects the noise level of an inertial sensor, including the white noise random drift of a dual-axis RINS gyroscope. In practical applications, it is difficult to obtain the accurate process noise power spectrum intensity; the relationship between its assumed and actual values ​​can be expressed as:

[0214] Q c = (1+α)Q

[0215] Where α represents a scalar and Q is the actual value.

[0216] Therefore, the actual state error covariance is:

[0217]

[0218] Among them, Pa (t)=E{[x a (t)-x(t)][x a (txt)] T} represents the actual state error covariance, x a (t) represents the actual state. The equation for the difference between the differential actual and calculated covariance matrices is as follows:

[0219]

[0220] An explicit analytical solution is given.

[0221]

[0222] Where ΔP(0)=P c (0)-P a (0) = 0, Φ c (t,t)=I.

[0223] In practice, Q c ≥Q, i.e., α≥0, is usually used for conservative considerations. The second term in equation (40) is a semi-positive term, P a (t)=P c (t)-ΔP(t), therefore, the greater the noise intensity of the actual process, the smaller the scalar α, and the greater the actual state error covariance; the lower the noise intensity of the actual process, the larger the scalar α, and the smaller the actual state error covariance.

[0224] Theoretically, the two Kalman filters should provide identical estimates of the gyroscope drift and accelerometer bias for the single-axis RINS1. However, as mentioned above, their estimation accuracy may differ due to uncertainties introduced by process noise. Note that the only different process noise in the two Kalman filters is the white noise random drift of the gyroscope and accelerometer in the dual-axis RINS. The performance of the dual-axis RINSk (k=2,3) can be evaluated using the time standard deviation of common error states in both Kalman filters instead of the ensemble standard deviation. When the gyroscope drift is stable or changes slowly, a smaller time standard deviation indicates lower process noise intensity, and the corresponding dual-axis RINS exhibits better performance. However, in cases of highly unstable gyroscope drift, a smaller time deviation for the single-axis RINS does not necessarily imply better performance for the dual-axis RINS. The time standard deviation of the azimuth gyroscope drift of the single-axis RINS1 is chosen as the criterion because of its high estimation accuracy. The corresponding calculation formula is:

[0225]

[0226] in, Let N be the standard deviation of the drift estimate of the single-axis RINS1 azimuth gyroscope by filter k, and N be the number of samples. This is the average value over time.

[0227] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for underwater attitude optimization with multi-inertial navigation topology constraints, characterized in that, Includes the following steps: S1. Establish an augmented error state dynamic model, adding velocity, position and attitude between single-axis RINS and dual-axis RINS as new error states; S2. Design the measurement model of the filter and construct the complete observation equation; S3. Analyze the observability of the filter, determine the motion required for the excitation error, and obtain a simplified filter for different scenarios. S4. Perform multi-inertial navigation information fusion for both dual RINS configuration and triple RINS configuration.

2. The underwater attitude optimization method with multi-inertial navigation topology constraints as described in claim 1, characterized in that, The specific steps of S1 are as follows: S101. Establish an augmented error state dynamic model, including: Considering that RINS evolved from strapdown INS, their error state dynamic models have the same form; taking single-axis RINS1 and dual-axis RINS as examples. k The position error difference, velocity error difference, and attitude error difference between (k=2,3) are used as new error states, and a dynamic model of the error states is given. in δL 1k =δL1-δL k ,sl 1k =sλ1-sλ k , where δL 1k represents the difference between the latitude error δL1 of the single-axis RINS1 and the latitude error δL k of the two-axis RINS, where k = 2, 3, δλ k represents the difference between the longitude error δλ1 of the single-axis RINS1 and the longitude error δλ 1k of the two-axis RINS; k k is the difference between the velocity error of the single-axis RINS1 and the velocity error of the two-axis RINS; k is the difference between the attitude error of the single-axis RINS1 and the attitude error of the two-axis RINS; k is the earth's rotation speed in the northeast direction of the n coordinate system, is the rotation rate, is the angular rate of the navigation coordinate system relative to the inertial coordinate system i; similarly, <s represents the earth's rotation speed error, represents the transmission speed error, represents the difference in the navigation system angular velocity error; and [[ID=Z3]] are the three-axis error vectors of the gyroscope modeled as the sum of a constant drift and white noise, and are the three-axis error vectors of the accelerometer modeled as the sum of a constant bias and white noise; the constant drift and white noise and and are the three-axis error vectors of the accelerometer modeled as the sum of a constant bias and white noise; f n and v n are the force and velocity vectors expressed in the navigation frame; is the first derivative of δL 1k , R N is the radius of the principal curvature of the meridian, h is the altitude, is the northward velocity difference between the single-axis RINS1 and the two-axis RINS k , where k = 2, 3, v E is the inertial navigation eastward velocity, L is the latitude, R E is the radius of the principal curvature of the prime vertical, is the eastward velocity difference between the single-axis RINS1 and the two-axis RINS k , where k = 2, 3, f n is the force vector in the navigation system, v n is the northward velocity vector, [[ID=SO]]is the attitude matrix of the single-axis RINS1, is the two-axis RINS​​​​ k The attitude matrix for k=2,3 This represents the angular rate of the navigation coordinate system relative to the inertial frame, used for rotation compensation in navigation calculations. For single-axis RINS1 and dual-axis RINS k k=2,3 The difference, δL 1k For single-axis RINS1 and dual-axis RINS k The latitude difference between k=2 and 3, ω ie This is the Earth's rotational angular velocity vector. For single-axis RINS1 and dual-axis RINS k In the navigation system with k=2,3, ω ie difference, This is the measurement error of the Earth's rotation angular velocity using a single-axis RINS1. For dual-axis RINS k The measurement error of the Earth's rotation angular velocity at k=2,3 This represents the angular velocity vector of the single-axis RINS1 navigation system relative to the geocentric coordinate system. For dual-axis RINS k The angular velocity vector of the navigation frame with k=2,3 relative to the geocentric coordinate system. For single-axis RINS1 and dual-axis RINS k The difference in eastward velocities between k=2 and k=3, δL 1k For single-axis RINS1 and dual-axis RINS k The latitude difference between k=2 and k=3 The Earth's rotational angular velocity vector in the navigation system. Let be the angular velocity vector of the navigation system relative to the Earth-centered Earth-fixed coordinate system. ε1 represents the angular velocity error output by the single-axis RINS1 gyroscope, and ε2 represents the zero-bias vector of the single-axis RINS1 gyroscope. This outputs the raw gyroscope data for a single-axis RINS1. For dual-axis RINS k The angular velocity error of the gyroscope output for k=2,3 is ε. k For dual-axis RINS k The zero bias vector of the gyroscope with k=2,3 For dual-axis RINS k Output the raw numbers of the gyroscope with k=2 or 3. The zero bias vector of the accelerometer for single-axis RINS1. This is the raw accelerometer output for a single-axis RINS1. For dual-axis RINS k , k = 2, 3 accelerometer zero bias vector For dual-axis RINS k The raw accelerometer data output for k=2,3; S102, with RINS1 and RINS k The three-component drift vectors ε1 and ε of the gyroscope k Establish a redundant state vector, including: With RINS1 and RINS k Accelerometer three-component bias vector and As the widening error state, the corresponding dynamic model of the error state is obtained as follows: The widened error state vector of the redundant RINS configuration is derived from Given; where δL, δλ, δv E ,δv N ,φ E ,φ N ,φ U ,ε, These represent latitude error, longitude error, eastward velocity error, northward velocity error, eastward attitude error, northward attitude error, celestial attitude error, gyro bias, and accelerometer bias, respectively. The subscripts 1 and k represent single-axis RINS1 and dual-axis RINS, respectively. k k = 2, 3, where the subscripts x, y, z represent the directions of the three sensitive axes of the gyroscope and accelerometer, respectively; The process noise vector is written as: Where the subscript ε represents the physical quantity of the gyroscope, The physical quantity representing the accelerometer; The augmented error state dynamic model is as follows: Where x(t) is the augmented error state vector and w(t) is the process noise vector; The components of the system matrix F(t) and the noise matrix G(t) are: The meanings of each parameter are the same as those in the variable descriptions in S101; The process noise matrix is ​​represented as: in, This is the inertial navigation attitude matrix, where the subscripts 1 and k represent the single-axis RINS1 and dual-axis RINS1, respectively. k , k = 2, 3.

3. The underwater attitude optimization method with multi-inertial navigation topology constraints as described in claim 1, characterized in that, The specific implementation steps of S2 are as follows: S201. Establish redundant inertial navigation spatial relationships, single-axis RINS1 and dual-axis RINS1. k For k=2 and 3, the output navigation information including position and velocity satisfies the following constraints: in, and These represent single-axis RINS1 and dual-axis RINS, respectively. k , and Represents their positional error, r n Indicates the actual location. For single-axis RINS1 and dual-axis RINS k The lever arm between; and Indicates single-axis RINS1 and dual-axis RINS k Speed ​​output, Indicate IMU series b1 and b k The relative angular velocity between them; S202. Establish the lever arm model, specifically as follows: The corresponding component forms are given by the formula. This is a latitude estimate. This is an estimated value for longitude. This is an estimate of the eastward velocity. Here, i represents the estimated northbound velocity, with values ​​of 1 and k, representing single-axis RINS1 and dual-axis RINS1, respectively. k k = 2, 3; A measurement model for the augmented error state is given. z(t)=H(t)x(t)+v(t) In the formula: Where z(t) is the measurement vector, δL 1k ,δλ 1k , Let H(t) represent the latitude error, longitude error, eastward velocity error, and northward velocity error at time k, respectively; H(t) is the measurement matrix; and v(t) is the measurement noise vector.

4. The underwater attitude optimization method with multi-inertial navigation topology constraints as described in claim 1, characterized in that, The specific implementation steps of S3 are as follows: S301. Separate the augmented error state by dividing the augmented error state x(t) into two parts. Where x(t) represents the error state, subscript 1 indicates RINS1, subscript 2 indicates RINS2; δL 1k ,δλ 1k , These represent the latitude error, longitude error, eastward velocity error, and northward velocity error at time k, respectively; φ is the misalignment angle; ε is the gyroscope zero bias; This indicates zero bias of the accelerometer; the subscripts indicate the directions: East, North, and Sky. The system description has been rewritten as follows In the formula, x(t) represents the error state; y(t) or z(t) represents the output state; and F(t) is the state transition matrix. ij (t) represents the element at the corresponding position in the state transition matrix, describing the evolution of the system state over time; G(t) is the process noise correlation matrix; w(t) is the process noise vector; v(t) is the measurement noise vector; S302. Establish an observability model, including: The system matrix and measurement matrix are simplified as follows: Among them, F ij (t) represents the element at the corresponding position in the state transition matrix; A(t) is the state transition matrix, describing the evolution of the error state over time; C(t) is the output matrix; G is the magnitude of Earth's gravity; This is the projection of the Earth's rotational angular velocity in the n-frame. For n-system to b i The rotation matrix of the system, i takes the values ​​1 and k, representing the single-axis RINS1 and the dual-axis RINS1, respectively. k k = 2, 3; The stripping of the observability matrix O is used to analyze the observability of the equations, by Among them O j =[C T :(CA) T :(CA 2 ) T :…:(CA 12 ) T ] T j = 1, 2, ..., m, where the number of states in the reduced-order dynamic system is 13, and m is a block; Get CA i The general form of i = 1, 2, ..., 12. Due to CA i Rows with i ≥ 3 are only CA and CA 2 A linear combination of rows gives: in For O(m) to be full rank, two sufficient conditions must be satisfied: 1) Because The rank of m is 5, so the minimum segment of m is 3, making matrix O(m) full rank; 2) By checking equation (28), and Both must be changed to make the matrix O(m) full rank; furthermore, only when b1 and b k O(m) becomes full rank only when the relative attitude between the systems changes in the horizontal and azimuth directions. The azimuth frame of a single-axis RINS1 typically rotates its IMU back and forth between four orthogonal positions relative to the zero position: 0°, 90°, 270°, and 180°; a dual-axis RINS... k The azimuth and roll frames of the IMU rotate periodically through two orthogonal positions, 0° and 180°, relative to their own zero position; the b1 system and b k There are 4×2=8 relative attitudes between the systems, which satisfies sufficient conditions 1) and 2), making the reduced-order dynamic system completely observable at a certain moment; The augmented error state dynamic system is fully observable.

5. The underwater attitude optimization method with multi-inertial navigation topology constraints as described in claim 1, characterized in that, S4 includes scenarios for dual RINS configurations, including: Single-axis RINS1 and dual-axis RINS2; The error state of a single-axis RINS1 is represented as follows: x1(t) is the error state vector of a single-axis RINS1, containing gyroscope drift and accelerometer bias; δL1, δλ1, δν n ,δv e ,δλ x ,δν t These represent the errors in longitude, latitude, heading, two horizontal angles, northward speed, and eastward speed, respectively. The corresponding error state dynamic model is given In the formula: Where A1(t) is the state transition matrix, describing the evolution of the error state over time; B1(t) is the control input matrix, describing the influence of the control input on the error state; C1(t) is the output matrix; and x1(k) is the time t. k The error state prediction is set with initial values ​​of x1(0) = 0; u1(k) is the estimate of gyroscope drift and accelerometer bias, Δt is the discrete step length, and ε is the triaxial gyroscope zero bias. It is a three-way addition table with zero bias; The elements in the process noise vector w1(t) represent the noise from the gyroscope and the accelerometer, respectively. The designed Kalman filter can be used to estimate the gyroscope drift and accelerometer deviation of a single-axis RINS1, thereby predicting the deterministic position error; The position error prediction model is x1(k+1)=Φ1(k+1,k)x1(k)+Γ1(k)u1(k) In the formula A1(t) is the state transition matrix, describing the evolution of the error state over time; B1(t) is the control input matrix, describing the influence of the control input on the error state; C1(t) is the output matrix. The position accuracy is improved by subtracting the position error prediction from the position output of the single-axis RINS1: The superscripts "-" and "^" represent the correction value and the predicted value, respectively.

6. The underwater attitude optimization method with multi-inertial navigation topology constraints as described in claim 5, characterized in that, The specific configuration of triple RINS in S4 is as follows: Triple RINS includes single-axis RINS1, dual-axis RINS2 and RINS3; Two parallel computational Kalman filters are established between single-axis RINS1 and dual-axis RINS2 and dual-axis RINS3 respectively to estimate the error state; the common error states of the two Kalman filters are gyroscope drift and accelerometer bias of single-axis RINS1; The calculated state error covariance is given by the following Riccati equation: Among them, P c F(t) is the state error covariance matrix (calculated value), reflecting the uncertainty estimate of the state error at time t; F(t) is the state transition matrix, describing the evolution of the system state over time; K c (t) represents the calculated gain; Q c The process noise power spectrum intensity is assumed, which reflects the noise level of the inertial sensor, including the white noise random drift of the dual-axis RINS gyroscope; G(t) is the process noise correlation matrix, which describes the effect of process noise on the state; R is the observation noise covariance matrix, which is assumed to be the observation noise power spectrum intensity. In practical applications, the relationship between the assumed value and the actual value is expressed as follows: Q c =(1+α)Q Where α represents a scalar; Q is the actual value; The actual state error covariance is: Among them, P a (t)=E{[x a (t)-x(t)][x a (txt)] T } represents the actual state error covariance; other parameters are the same as in the standard Riccati equation; The equation for the difference between the actual and calculated covariance matrices is as follows: An explicit analytical solution is given; in, Φ c Let Φ be the state transition matrix, and Φ c (t,t)=I.,Q c ≥Q, α≥0, P a (t)=P c (t)-ΔP(t).

Images