SINS (Strapdown Inertial Navigation System) dynamic alignment method for unmanned underwater vehicle

Abstract

The invention provides an SINS (Strapdown Inertial Navigation System) dynamic alignment method for an underwater unmanned vehicle, and belongs to the technical field of underwater navigation. The method is different from an existing optimization alignment method based on a specific force equation, the GNSS and DVL speed space constraint relation is utilized, an optimization model based on speed observation is constructed, the problem that under the weak maneuver condition, due to large zero offset of MEMS-IMU, acceleration measurement cannot correctly reflect the maneuver state of a carrier is solved, and therefore the alignment efficiency is improved; on the other hand, on the basis of the constructed optimization model, a correction strategy capable of jointly estimating MEMS-IMU gyroscope zero offset and alignment process b-system attitude errors is redesigned, the problem of error accumulation in observed quantity and a b-system attitude matrix is solved, and the alignment precision is further improved.

Description

Technical Field

[0001] The invention belongs to the technical field of underwater navigation, and in particular relates to a SINS dynamic alignment method for an underwater unmanned vehicle. Background Art

[0002] Unmanned underwater vehicles (UUVs) have been widely used in fields such as marine science, resource exploration, and national defense due to their numerous advantages, including autonomy, stealth, environmental adaptability, and high cost-effectiveness. Currently, UUVs typically combine a strapdown inertial navigation system (SINS) based on a micro-electromechanical system (MEMS-IMU), a Doppler velocity log (DVL), and information from the global navigation satellite system (GNSS), using data fusion methods to achieve continuous positioning in complex scenarios both on the surface and underwater. The positioning accuracy of the integrated navigation system during the navigation phase depends on the alignment accuracy of the SINS, while the alignment speed is also related to the SINS's rapid response capability. Furthermore, the operating environment and operating characteristics of UUVs preclude static alignment. Therefore, achieving efficient SINS alignment under dynamic conditions is key to improving the accuracy of UUV integrated navigation systems.

[0003] To address the SINS dynamic alignment problem, the solution adopted by related technologies uses an optimized alignment method based on Kalman filtering. First, based on the inertial specific force equation, it uses inertial sensor information and external sensor information to construct vector observations in continuous time, thereby converting the SINS alignment problem into an optimization-based least squares constraint problem and solving it using the Davenport Q method. Secondly, to avoid the problem of MEMS-IMU zero bias errors gradually accumulating in the constructed observations and the carrier attitude conversion matrix caused by the open-loop calculation method, it combines the Kalman filtering method to construct a closed-loop correction strategy, reducing the impact of MEMS-IMU deviation accumulation on alignment accuracy. However, the alignment method based on the inertial specific force equation is essentially based on acceleration observations, while unmanned underwater vehicles generally have small accelerations and weak maneuvering characteristics. In addition, the large zero bias error of the MEMS-IMU will cause the constructed observations to be inconsistent with the actual maneuvering conditions, thereby affecting the alignment efficiency and even causing alignment failure.

[0004] Therefore, it is necessary to provide a SINS dynamic alignment method for underwater unmanned vehicles to solve the above problems. Summary of the Invention

[0005] The present invention provides a SINS dynamic alignment method for underwater unmanned vehicles (UUVs). By constructing an optimization model based on velocity observations, this method avoids the problem of weak maneuvering conditions affecting acceleration observations. Furthermore, based on this constructed optimization model, a redesigned correction strategy is developed to jointly estimate the MEMS-IMU gyro bias and attitude errors during the alignment process. This solves the problem of error accumulation in the observed values and the vehicle attitude conversion matrix, further improving alignment accuracy and effectively resolving at least one of the technical issues discussed in the background art.

[0006] In order to solve the above-mentioned technical problems, the present invention is achieved as follows:

[0007] A SINS dynamic alignment method for an underwater unmanned vehicle comprises the following steps:

[0008] Step S1: Based on the attitude conversion matrix from the n-frame to the b-frame, a constraint model is constructed between the MEMS-IMU, DVL, and DNSS measurement velocities. The constraint model is solved to obtain the attitude conversion matrix from the n-frame to the b-frame at the initial moment, where the b-frame represents the MEMS-IMU coordinate system and the carrier coordinate system, and the n-frame represents the navigation coordinate system.

[0009] Step S2, performing Kalman filtering with the gyro bias error and the B-frame attitude error of the MEMS-IMU as state quantities to estimate the state quantities;

[0010] Step S3, compensate the gyro bias and the B-system attitude matrix, calculate the actual gyro output and the B-system attitude conversion matrix after error correction, and use the matrix chain multiplication rule to decompose the attitude conversion matrix from the N-system to the B-system at any time t into the product of the B-system attitude conversion matrix, the N-system attitude conversion matrix, and the initial N-system to the B-system attitude conversion matrix. The attitude conversion matrix from the N-system to the B-system at any time t is calculated to complete the dynamic alignment of the SINS.

[0011] As a preferred improvement, the constraint model is expressed as:

[0012]

[0013] Where, β v , α v represents the observed quantity; represents the attitude transformation matrix from system n to system b at the initial moment; where:

[0014]

[0015] Where, Represents the attitude transformation matrix from the n-system at the initial moment to the n-system at time t; Indicates GNSS measurement speed; represents the attitude transformation matrix from the initial b system to the b system at time t; Indicates DVL measurement speed; represents the angular velocity of system b relative to system i in system b; × represents an antisymmetric matrix; It represents the arm vector pointing from the GNSS center to the DVL center in the b frame. It represents the arm vector pointing from the center of MEMS-IMU to the center of DVL in the b frame. It represents the arm vector pointing from the center of MEMS-IMU to the center of GNSS in the b-frame, where the i-frame represents the inertial coordinate system.

[0016] As a preferred improvement, the derivation process of the constraint model includes the following steps:

[0017] Step S11, defining the relationship between the DVL measurement speed and the MEMS-IMU measurement speed, expressed as:

[0018]

[0019] Where, Represents the attitude transformation matrix from the n system to the b system; Indicates the MEMS-IMU measurement speed; represents the angular velocity of the b system relative to the e system in terms of the b system, where the e system represents the Earth-centered Earth-fixed coordinate system;

[0020] Step S12, defining the relationship between the GNSS measurement speed and the MEMS-IMU measurement speed, expressed as:

[0021]

[0022] Step S13, Approximate substitution The relationship between the GNSS measurement speed and the DVL measurement speed is obtained by using the MEMS-IMU measurement speed as an intermediate medium, which is expressed as:

[0023]

[0024] Step S14, using the matrix chain multiplication rule to decompose the attitude conversion matrix from the n-frame to the b-frame at any time t, expressed as:

[0025]

[0026] Where, Represents the attitude transformation matrix from the n-system at time t to the n-system at the initial time, and are transposed matrices of each other; and It can be calculated according to the following two formulas:

[0027]

[0028] Where, express The first derivative of ; It represents the angular velocity of the n system relative to the i system in the n system; express The first derivative of ;

[0029] Step S15: Substituting the expression into the relationship between DVL measurement and GNSS measurement speed, we get:

[0030]

[0031] make:

[0032]

[0033]

[0034] The constraint model is obtained:

[0035] As a preferred improvement, the constraint model is solved by the Davenport Q method to obtain The value of .

[0036] As a preferred improvement, the state quantity X of the Kalman filter is expressed as:

[0037]

[0038] Where, represents the attitude error of the b system; δε b represents the gyro bias error; T represents the transposed matrix;

[0039] The continuous-time linear state equation is expressed as:

[0040]

[0041] Where; represents the derivative of the state quantity X; F represents the continuous-time state transfer matrix; w represents the state noise; where:

[0042]

[0043] Where, represents the attitude error noise of the b system; η gn represents the gyro bias error noise; Represents the gyro output value after error correction; I3 represents the 3×3 unit matrix; 03 represents the 3×3 zero matrix.

[0044] As a preferred improvement, the attitude error The state equation is expressed as:

[0045]

[0046] The state equation of gyro bias error is expressed as:

[0047]

[0048] As a preferred improvement, the measurement equation of Kalman filtering is expressed as:

[0049] Z k =H k X k +V k ;

[0050] Where H k is the observation matrix; V k is the measurement noise; X k Represents the state quantity during the k-th update process, where:

[0051]

[0052] As a preferred improvement, step S3 specifically includes the following steps:

[0053] Gyro bias error δε obtained using Kalman filtering b and the attitude error of the b system Calculate the actual gyro output at the current moment And the B-frame attitude transformation matrix after error compensation The calculation method is as follows:

[0054]

[0055] Where,

[0056]

[0057] Then the attitude transformation matrix from system n to system b at any time t is Calculated by the following formula:

[0058]

[0059] The beneficial effects of the present invention are:

[0060] (1) Different from the existing optimization alignment method based on the specific force equation, the present invention utilizes the spatial constraint relationship between GNSS and DVL velocity to construct an optimization model based on velocity observation. This avoids the problem that the acceleration measurement cannot correctly reflect the maneuvering state of the carrier due to the large zero bias of the MEMS-IMU under weak maneuvering conditions, thereby improving the alignment efficiency.

[0061] (2) Based on the speed-based optimization model, a Kalman filter correction method is designed to jointly estimate the IMU zero bias error and the B-system attitude error to avoid the error accumulation affecting the alignment effect of the optimization model and further improve the speed and accuracy of dynamic alignment. DETAILED DESCRIPTION

[0062] The following will be combined with the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the embodiments described are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0063] This embodiment provides a SINS dynamic alignment method for an underwater unmanned vehicle. The underwater unmanned vehicle is equipped with a MEMS-IMU, a DVL, and a GNSS integrated navigation system. To more clearly illustrate the content of this embodiment, the coordinate systems are defined as follows:

[0064] System b represents the MEMS-IMU coordinate system. In this invention, it is assumed that the installation error between the MEMS-IMU and the carrier (underwater unmanned vehicle) has been compensated, so system b also represents the carrier coordinate system.

[0065] n system represents the navigation coordinate system;

[0066] The e system represents the Earth-centered Earth-fixed coordinate system;

[0067] The i system represents the inertial coordinate system.

[0068] The SINS dynamic alignment method for an underwater unmanned vehicle provided in this embodiment includes the following steps:

[0069] Step S1: Based on the attitude conversion matrix from the n-system to the b-system, a constraint model between the MEMS-IMU, DVL, and DNSS measurement velocities is constructed, and the attitude conversion matrix from the n-system to the b-system at the initial moment is obtained by solving the constraint model.

[0070] The constraint model is expressed as:

[0071]

[0072] Where, β v , α v represents the observed quantity; represents the attitude transformation matrix from system n to system b at the initial moment; where:

[0073]

[0074] Where, Represents the attitude transformation matrix from the n-system at the initial moment to the n-system at time t; Indicates GNSS measurement speed; represents the attitude transformation matrix from the initial b system to the b system at time t; Indicates DVL measurement speed; represents the angular velocity of system b relative to system i in system b; × represents an antisymmetric matrix; It represents the arm vector pointing from the GNSS center to the DVL center in the b frame. It represents the arm vector pointing from the center of MEMS-IMU to the center of DVL in the b frame. It represents the arm vector pointing from the center of MEMS-IMU to the center of GNSS in the b frame.

[0075] The derivation process of the speed constraint model is:

[0076] The relationship between DVL measurement speed and MEMS-IMU measurement speed is as follows:

[0077]

[0078] Where, Represents the attitude transformation matrix from the n system to the b system; Indicates the MEMS-IMU measurement speed; It represents the angular velocity of the b system relative to the e system in the b system.

[0079] The relationship between GNSS measurement speed and MEMS-IMU measurement speed is as follows:

[0080]

[0081] Since the measurement deviation of MEMS-IMU is large, it is usually not sensitive to the angular velocity of the earth's rotation. Approximate substitution The relationship between the GNSS measurement speed and the DVL measurement speed is obtained by using the MEMS-IMU measurement speed as an intermediate medium, which is expressed as:

[0082]

[0083] The essence of the SINS alignment problem is to determine the attitude transformation matrix from the n-frame at time t to the b-frame at time t. Using the matrix chain multiplication rule Decompose it into:

[0084]

[0085] Where, Represents the attitude transformation matrix from the n-system at time t to the n-system at the initial time, and are transposed matrices of each other;

[0086] and It can be calculated according to the following two formulas:

[0087]

[0088] Where, express The first derivative of ; It represents the angular velocity of the n system relative to the i system in the n system; express The first derivative of ;

[0089] Will Substituting the expression into the relationship between GNSS measurement speed and DVL measurement speed, we get:

[0090]

[0091] make:

[0092]

[0093] The constraint model can be obtained:

[0094] The constraint model is solved by the Davenport Q method, and we get The value of .

[0095] In step S2, Kalman filtering is performed using the gyro bias error and the B-frame attitude error of the MEMS-IMU as state quantities to estimate the state quantities.

[0096] Taking into account the gyro bias error, the actual gyro output at time t is With the ideal gyro output The relationship between them is expressed as:

[0097]

[0098] Where, ε b (t), η b (t) represent the actual gyro bias and random noise at time t, respectively.

[0099] Using the gyro bias estimated at time t-1 Compensate the actual gyro output to get the approximate value of the ideal gyro output at time t Expressed as:

[0100]

[0101] Gyro bias corrected by Kalman filter Compared with the actual gyro bias ε b (t) is expressed as:

[0102]

[0103] Where δε b Indicates the gyro bias error;

[0104] Then the approximate value of the ideal gyro output at time t is It can also be expressed as:

[0105]

[0106] Will include attitude error The b series is defined as System, using the matrix chain multiplication rule to Decompose it into:

[0107]

[0108] Where, represents the b system from the initial moment to the moment t The attitude transformation matrix of the system; Represents time t The attitude transformation matrix from system b to system b at time t;

[0109] During the Kalman filter update process, Update as follows:

[0110]

[0111] Where, represents the attitude transformation matrix from the initial b frame to the b frame at time t-1 after Kalman filter correction; represents the ideal attitude transformation matrix from system b at time t-1 to system b at time t; Use an approximation of the ideal gyro output Update as follows:

[0112]

[0113] Where I3 represents the 3×3 identity matrix; represents the equivalent rotation vector corresponding to the b-system rotation, T s Indicates the sampling interval.

[0114] Assumed attitude error For a small amount, It can be expressed as:

[0115]

[0116] and The relationship between them can be expressed as follows:

[0117]

[0118] right Taking the derivative of both sides of the equation gives:

[0119]

[0120] Will as well as Substituting the above formula and combining it with the attitude differential equation, we can get:

[0121]

[0122] The aforementioned Substituting the expression into the equation and ignoring the second-order small quantity, we can obtain:

[0123]

[0124] According to the matrix cross multiplication rule, the above formula can be expressed as:

[0125]

[0126] Both sides of the above equation are antisymmetric matrices. At the same time, take the vectors that constitute the antisymmetric matrix and change η b Considered as process noise Indicates that the attitude error is obtained The state equation is as follows:

[0127]

[0128] The gyro bias error can be modeled as the following random walk process:

[0129]

[0130] For state quantity The continuous-time linear state equation is expressed as follows

[0131]

[0132] Where; represents the derivative of the state quantity X; F represents the continuous-time state transfer matrix; w represents the state noise; where:

[0133]

[0134] Where, represents the attitude error noise of the b system; η gn Represents the gyro bias error noise; I3 represents the 3×3 identity matrix; 03 represents the 3×3 zero matrix.

[0135] The DVL measurement speed and GNSS measurement speed including errors are expressed as:

[0136]

[0137] Where, represents the DVL measurement speed including error; It represents the b-axis velocity measurement error of DVL after compensating the installation error angle; Indicates the GNSS measurement speed including errors; Indicates the n-frame velocity measurement error of GNSS.

[0138] For the observation α v , considering attitude error, gyro bias error and DVL measurement speed error, the actual calculated and Bring in In the above equation, we get the observation quantity containing error It is expressed as follows:

[0139]

[0140] Will and Substitute into the above formula and ignore the random noise η b And the second-order small quantity:

[0141]

[0142] Approximation of the ideal gyro output replace Obtained recursively after filtering and correction of the previous epoch replace The above formula can be expressed as:

[0143]

[0144] If the initial constant posture transformation matrix after filtering correction is recorded as And the observation quantity β including GNSS velocity error is Since n changes little in a short time, it can be calculated by open loop. replace The quantity measurement can be expressed as:

[0145]

[0146] The measurement equation can be further obtained:

[0147] Z k =H k X k +V k ;

[0148] Where H k is the observation matrix; V k is the measurement noise; X k Represents the state quantity during the k-th update process, where:

[0149]

[0150] Step S3, compensate the gyro bias and the B-system attitude matrix, calculate the actual gyro output and the B-system attitude conversion matrix after error correction, and use the matrix chain multiplication rule to decompose the attitude conversion matrix from the N-system to the B-system at any time t into the product of the B-system attitude conversion matrix, the N-system attitude conversion matrix, and the initial N-system to the B-system attitude conversion matrix. The attitude conversion matrix from the N-system to the B-system at any time t is calculated to complete the dynamic alignment of the SINS.

[0151] Gyro bias error δε obtained using Kalman filtering b and the attitude error of the b system Calculate the actual gyro output at the current moment And the B-frame attitude transformation matrix after error compensation The calculation method is as follows:

[0152]

[0153] Where,

[0154]

[0155] Then the attitude transformation matrix from system n to system b at any time t is Calculated by the following formula:

[0156]

[0157] During the Kalman filtering process, time update and measurement update are performed. The updating process may adopt conventional techniques in this field, and will not be described in detail in this embodiment.

[0158] The above describes the embodiments of the present invention, but the present invention is not limited to the above specific implementation methods. The above specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the present invention and the claims, all of which are protected by the present invention.

Claims

1. A SINS dynamic alignment method for underwater unmanned vehicles, characterized in that: The steps include: Step S1: Based on the attitude conversion matrix from the n-frame to the b-frame, a constraint model is constructed between the MEMS-IMU, DVL, and DNSS measurement velocities. The constraint model is solved to obtain the attitude conversion matrix from the n-frame to the b-frame at the initial moment, where the b-frame represents the MEMS-IMU coordinate system and the carrier coordinate system, and the n-frame represents the navigation coordinate system. Step S2, performing Kalman filtering with the gyro bias error and the B-frame attitude error of the MEMS-IMU as state quantities to estimate the state quantities; Step S3, compensate the gyro bias and the B-system attitude matrix, calculate the actual gyro output and the B-system attitude conversion matrix after error correction, and use the matrix chain multiplication rule to decompose the attitude conversion matrix from the N-system to the B-system at any time t into the product of the B-system attitude conversion matrix, the N-system attitude conversion matrix, and the initial N-system to the B-system attitude conversion matrix. The attitude conversion matrix from the N-system to the B-system at any time t is calculated to complete the dynamic alignment of the SINS.

2. The SINS dynamic alignment method for underwater unmanned vehicles according to claim 1, characterized in that: The constraint model is expressed as: Where, β v , α v represents the observed quantity; represents the attitude transformation matrix from system n to system b at the initial moment; where: Where, Represents the attitude transformation matrix from the n-system at the initial moment to the n-system at time t; Indicates GNSS measurement speed; represents the attitude transformation matrix from the initial b system to the b system at time t; Indicates DVL measurement speed; represents the angular velocity of system b relative to system i in system b; × represents an antisymmetric matrix; It represents the arm vector pointing from the GNSS center to the DVL center in the b frame. It represents the arm vector pointing from the center of MEMS-IMU to the center of DVL in the b frame. It represents the arm vector pointing from the center of MEMS-IMU to the center of GNSS in the b-frame, where the i-frame represents the inertial coordinate system.

3. The SINS dynamic alignment method for underwater unmanned vehicles according to claim 2, characterized in that: The derivation process of the constraint model includes the following steps: Step S11, defining the relationship between the DVL measurement speed and the MEMS-IMU measurement speed, expressed as: Where, Represents the attitude transformation matrix from the n system to the b system; Indicates the MEMS-IMU measurement speed; represents the angular velocity of the b system relative to the e system in terms of the b system, where the e system represents the Earth-centered Earth-fixed coordinate system; Step S12, defining the relationship between the GNSS measurement speed and the MEMS-IMU measurement speed, expressed as: Step S13, Approximate substitution The relationship between the GNSS measurement speed and the DVL measurement speed is obtained by using the MEMS-IMU measurement speed as an intermediate medium, which is expressed as: Step S14, using the matrix chain multiplication rule to decompose the attitude conversion matrix from the n-frame to the b-frame at any time t, expressed as: Where, Represents the attitude transformation matrix from the n-system at time t to the n-system at the initial time, and are transposed matrices of each other; and It can be calculated according to the following two formulas: Where, express The first derivative of ; It represents the angular velocity of the n system relative to the i system in the n system; express The first derivative of ; Step S15: Substituting the expression into the relationship between DVL measurement and GNSS measurement speed, we get: make: The constraint model is obtained:

4. The SINS dynamic alignment method for underwater unmanned vehicles according to claim 3, characterized in that: The constraint model is solved by the Davenport Q method and obtained as The value of .

5. The SINS dynamic alignment method for underwater unmanned vehicles according to claim 3, characterized in that: The state quantity X of the Kalman filter is expressed as: Where, represents the attitude error of the b system; δε b represents the gyro bias error; T represents the transposed matrix; The continuous-time linear state equation is expressed as: Where; represents the derivative of the state quantity X; F represents the continuous-time state transfer matrix; w represents the state noise; where: Where, represents the attitude error noise of system b; η gn represents the gyro bias error noise; Represents the gyro output value after error correction; I3 represents the 3×3 unit matrix; 03 represents the 3×3 zero matrix.

6. The SINS dynamic alignment method for underwater unmanned vehicles according to claim 5, characterized in that: Attitude error The state equation is expressed as: The state equation of gyro bias error is expressed as:

7. The SINS dynamic alignment method for underwater unmanned vehicles according to claim 6, characterized in that: The measurement equation of Kalman filter is expressed as: Z k =H k X k +V k ; Where H k is the observation matrix; V k is the measurement noise; X k Represents the state quantity in the k-th update process, where:

8. The SINS dynamic alignment method for underwater unmanned vehicles according to claim 7, characterized in that: Step S3 specifically includes the following steps: Gyro bias error δε obtained using Kalman filtering b and the attitude error of the b system Calculate the actual gyro output at the current moment And the B-frame attitude transformation matrix after error compensation The calculation method is as follows: Where, Then the attitude transformation matrix from system n to system b at any time t is Calculated by the following formula:

Images