An optimal estimation method for ocean current velocity based on GNSS / SINS / DVL combination

Abstract

A method for optimally estimating ocean current velocity based on a GNSS / SINS / DVL combination is disclosed, comprising the following steps: (1) constructing a GNSS / SINS / DVL combined navigation motion model including ocean current velocity; (2) designing an objective function for optimizing ocean current velocity parameters; and (3) designing an optimal ocean current velocity estimation algorithm based on the GNSS / SINS / DVL combination. The present invention provides a method for estimating horizontal ocean current velocity based on GNSS / SINS / DVL output data based on optimization theory, effectively overcoming the shortcomings of traditional Kalman filter algorithms, such as reliance on initial parameters and the need for prior knowledge of instrument information, thereby improving estimation efficiency and accuracy.

Description

Technical Field

[0001] The present invention belongs to the field of navigation technology, and in particular relates to an optimal estimation method for ocean current velocity based on a GNSS / SINS / DVL combination. Background Art

[0002] In the current development of navigation, GNSS and INS are very effective in providing initial information such as position and velocity on land and in the air. However, underwater, GNSS signals cannot penetrate the water layer to provide auxiliary information to the vehicle in the water. Doppler velocimeters (DVLs) are more commonly used. They use the principle of acoustic velocity measurement to provide stable relative velocity information of the vehicle. However, under certain conditions, DVLs cannot accurately measure the relative velocity of the vehicle, and instead obtain the velocity relative to the water layer. In this case, the velocity of the ocean current cannot be ignored.

[0003] In certain fields, ocean current velocity information has a crucial impact on the effectiveness of certain equipment. While ocean currents don't directly influence climate, they do have a significant impact. Temperature and precipitation in many coastal areas are related to nearby ocean currents. Humanity also utilizes ocean currents in shipping, which plays a vital role in economic development. Therefore, estimating ocean current velocity is of paramount importance.

[0004] The patent with application number CN202111543405.6 (Ocean current velocity estimation method based on SINS and GNSS combination and DVL) proposes to use the Kalman filter (KF) method to estimate the ocean current velocity under the conditions of GNSS / SINS / DVL combination. This method requires prior knowledge of the error covariance matrix and requires good initial parameter values. The process inevitably involves a parameter adjustment process, which has certain limitations. In response to these problems, the present invention proposes to use an optimized alignment (OBA) method based on matrix decomposition technology and q-method to estimate the ocean current velocity. Under the same combination conditions, a mathematical method, namely the principle of optimal estimation, is used to transform the optimal problem into a multivariate function extreme value problem. Construct the objective function and find the partial derivative. When the two state updates are less than the set value or the number of iterative calculations is reached, the optimal ocean current velocity at the moment can be solved. Summary of the Invention

[0005] In order to solve the above problems, this application proposes an optimal estimation method for ocean current velocity based on the GNSS / SINS / DVL combination, which effectively overcomes the shortcomings of traditional algorithms that rely on initial parameters and require advance knowledge of instrument prior information, thereby improving the navigation positioning accuracy.

[0006] To achieve the above object, the technical solution adopted by the present invention is:

[0007] The present invention provides an optimal estimation method for ocean current velocity based on a GNSS / SINS / DVL combination, comprising:

[0008] (1) Construct a GNSS / SINS / DVL integrated navigation motion model that includes ocean current velocity;

[0009] The GNSS / SINS / DVL integrated navigation motion model including ocean current velocity described in step (1) is:

[0010]

[0011] The specific parameters in the formula have the following meanings: represents the attitude matrix from time t to time 0 in the b system, Represents the attitude matrix from time t to time 0 in the n system, is the projection of DVL output velocity in system b, Indicates the speed of ocean current. All other components are unknown and can be obtained directly from the output of the instrument, such as The other part can be calculated from the instrument output, such as and After shifting the integral, we get:

[0012]

[0013] The symbols are defined as:

[0014] Further select data at equal intervals, abbreviated as:

[0015]

[0016] Through the transformation relationship between quaternion and attitude matrix, and then using the multiplication property of quaternion, we get:

[0017]

[0018] (2) Designed the objective function for optimizing ocean current velocity parameters;

[0019] The objective function for optimizing the ocean current velocity parameters constructed in step (2) is:

[0020]

[0021] The parameters in the formula are as follows: Theoretically, when q and When the true value is taken, Γ=0 4×1 , and L approaches 0, at this time q and The problem of solving can be transformed into a parameter optimization problem;

[0022] (3) Designed an optimal estimation algorithm for ocean current velocity based on GNSS / SINS / DVL combination;

[0023] The ocean current velocity optimization estimation algorithm of the GNSS / SINS / DVL combination described in step (3) is to solve the parameter optimization problem in step (2) by transforming the parameter solving problem into the problem of finding the extreme value of a multivariate function.

[0024] As a further improvement of the present invention, the specific process of the ocean current velocity optimization estimation algorithm based on the GNSS / SINS / DVL combination described in step (3) is as follows:

[0025] For the current time t M :

[0026] Step 1: Calculate α M , β M and γ M value, and then according to and Use them M-τ The moment values ​​are used to calculate α, β and γ;

[0027] Step 2: Set k = 0 and start from a good estimate (x k ,u k )start;

[0028] Step 3: Calculate first and second order partial derivatives

[0029] Step 4: Solve the increment (Δx, Δu);

[0030] Step 5: Update (x k ,u k );

[0031] Step 6: Set k = k + 1 and go to step 4 until the convergence accuracy is reached or the maximum number of iterations is reached;

[0032] Step 7: Get the initial attitude quaternion q and ocean current speed

[0033] Step 8: Use the initial posture matrix to calculate the posture matrix at the current moment.

[0034] Beneficial Effects: This method, based on the optimal estimation of ocean current velocity using a GNSS / SINS / DVL combination, reconstructs the combined navigation motion model, including ocean current velocity, and uses the OBA algorithm to estimate the carrier attitude and ocean current velocity. Advantages of this method include good alignment, attitude error angles closer to zero and less volatile, low initial value requirements, greater robustness, and comparable estimation accuracy to the KF algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is the overall flow chart of the present invention;

[0036] Figure 2 This is a flow chart of a specific optimal estimation method of the present invention;

[0037] Figure 3 Comparison of the estimated values ​​of eastward ocean current velocity obtained by the present invention and the KF algorithm;

[0038] Figure 4 Comparison of the estimated values ​​of northward ocean current velocity obtained by the present invention and the KF algorithm. DETAILED DESCRIPTION

[0039] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. It should be understood that these embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention. After reading the present invention, modifications of various equivalent forms of the present invention made by those skilled in the art all fall within the scope defined by the claims attached to this application.

[0040] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0041] The present invention proposes an optimal ocean current velocity estimation method based on the GNSS / SINS / DVL combination, with the aim of providing an ocean current velocity estimation method that effectively overcomes the shortcomings of traditional algorithms that rely on initial parameters and require advance knowledge of instrument prior information, thereby improving estimation efficiency.

[0042] like Figure 1 As shown, the present invention proposes an optimal estimation method for ocean current velocity based on GNSS / SINS / DVL combination, and the implementation steps are as follows:

[0043] (1) Construct a GNSS / SINS / DVL integrated navigation motion model that includes ocean current velocity;

[0044] (2) Design the objective function for optimizing ocean current velocity parameters;

[0045] (3) Design an optimal estimation algorithm for ocean current velocity based on the GNSS / SINS / DVL combination.

[0046] When working in water tracking mode, it measures the projection value of the velocity relative to the ocean current under the system. The velocity has the following equation:

[0047]

[0048] According to the chain decomposition rule of the posture matrix, Decompose into In this form, formula (1) can be written as:

[0049]

[0050] Furthermore, multiply both sides of the above formula by After finishing, we can get:

[0051]

[0052] The specific parameters in the formula have the following meanings: represents the attitude matrix from time t to time 0 in the b system, Represents the attitude matrix from time t to time 0 in the n system, is the projection of DVL output velocity in system b, Indicates the speed of ocean current. All other components can be obtained directly from the instrument output or calculated from them. Equation (3) is the GNSS / SINS / DVL integrated navigation motion model including ocean current velocity.

[0053] Integrate (3) left and right to obtain:

[0054]

[0055] After integral discretization calculation, we can get:

[0056]

[0057] In the above formula, the gyroscope constant drift will accumulate over time. In order to reduce the influence of different time lengths on the parameter identification results, the integral term is selected with equal length:

[0058]

[0059] In the formula, the time interval τ is 1s. The above formula can be written as:

[0060]

[0061] Where, Shifting β to the right, we get:

[0062]

[0063] According to the transformation relationship between quaternion and attitude matrix and the multiplication property of quaternion, formula (8) can be converted to:

[0064]

[0065] Construct the following function:

[0066]

[0067] Because quaternions satisfy the conditions: q T q=1, introduce the Lagrange multiplier formula and construct the objective function:

[0068]

[0069] The parameters in the formula are as follows: u is the Lagrange multiplier, theoretically when q and When the true value is taken, Γ=0 4×1 , and L approaches 0, at this time q and The problem of solving can be transformed into a parameter optimization problem; the iterative method of parameter solution is as follows:

[0070]

[0071] Among them, Δx and Δu satisfy:

[0072]

[0073] As long as the coefficient matrix is ​​nonsingular, then x k Converge the square to the true value.

[0074] The ocean current velocity estimation algorithm is designed as follows Figure 2 As shown, for the current time t M :

[0075] Step 1: Calculate α M , β M and γ M value, and then according to and Use them M-τ The moment values ​​are used to calculate α, β and γ;

[0076] Step 2: Set k = 0 and start from a good estimate (x k ,u k )start;

[0077] Step 3: Calculate first and second order partial derivatives

[0078] Step 4: Solve the increment (Δx, Δu);

[0079] Step 5: Update (x k ,u k );

[0080] Step 6: Set k = k + 1 and go to step 4 until the convergence accuracy is reached or the maximum number of iterations is reached;

[0081] Step 7: Get the initial attitude quaternion q and ocean current speed

[0082] Step 8: Use the initial posture matrix to calculate the posture matrix at the current moment.

[0083] The beneficial effects of the present invention are verified by the following simulation:

[0084] Considering that the signal cannot be received underwater, a trajectory moving in an "8" shape on the horizontal plane was designed. Each side of the "8" shape is about 600 meters, and the total simulation time is close to 900 seconds.

[0085] The surface phase provides the vehicle's velocity and position information; the ideal angular velocity and acceleration information are obtained through the strapdown inertial navigation inversion algorithm, where the gyroscope constant drift is set to 0.02° / h and the random walk is The accelerometer constant drift is set to 200μg and random walk The update frequency is 100 Hz. The DVL's own scale factor error and the installation error angle between the DVL and the SINS are considered to have been well calibrated before the simulation test and are free of errors. The DVL output is the projection value of the vehicle's velocity relative to the water layer in the b-frame. In the simulation test, it is obtained by subtracting the ocean current velocity from the vehicle's ideal velocity and then projecting it into the b-frame using the ideal attitude matrix. It is specifically obtained by inversion according to the following formula:

[0086]

[0087] Where ω v is random noise, with an amplitude of 0.1 m / s. In the simulation, the ocean current value is set to [0.50.8] m / s. The attitude and velocity information of the carrier are set as follows: the carrier starts from due north, with a speed of 0 m / s and a speed of 1 m / s. 2 It accelerates for 5s to a speed of 5m / s and then moves at a constant speed. It then undergoes 7 90° right-angle turns to complete the movement process.

[0088] According to the above simulation trajectory, the ocean current velocity estimated by the traditional KF method is compared with the algorithm proposed in this paper to verify the effect of the optimization algorithm in estimating ocean currents under the conditions of GNSS / SINS / DVL integrated navigation.

[0089] The initial value of the KF method is set as follows:

[0090]

[0091] The initial value of the OBA method is set as follows:

[0092]

[0093] According to the above simulation conditions, the following simulation results were obtained: before 200 seconds, the OBA method has greater volatility than the KF method; after 200 seconds, the pitch angle, roll angle and heading angle errors all converge to near zero. Compared with the KF method, the attitude error angle using the OBA method is closer to zero and has less volatility. Figure 3 and Figure 4 The figures are respectively the effect diagrams of the eastward and northward ocean current velocity estimation of the two methods. It can be seen from the figure that after 200s, both methods converge to the set value and have high estimation accuracy. Like the attitude angle error, the estimated value of the ocean current velocity of the OBA method is closer to the true value.

[0094] Simulation results demonstrate that the OBA method achieves excellent alignment results by transforming the attitude solution and ocean current velocity estimation problems into optimization problems involving finding the extreme value of a multivariate function. The accuracy of alignment and ocean current velocity estimation is comparable to that of the KF algorithm. However, the initial value selection of the KF algorithm depends on prior information such as device performance, while the initial value of the OBA algorithm can be arbitrarily selected, resulting in a more robust OBA algorithm.

[0095] The above description is merely a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent variation based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A method for optimal estimation of ocean current velocity based on GNSS / SINS / DVL combination, characterized in that: include: (1) Constructing a GNSS / SINS / DVL integrated navigation motion model including ocean current velocity; The GNSS / SINS / DVL integrated navigation motion model including ocean current velocity described in step (1) is: ； The specific parameters in the formula have the following meanings: represents the attitude matrix from time t to time 0 in the b system, Represents the attitude matrix from time t to time 0 in the n system, is the projection of DVL output velocity in system b, Indicates the ocean current speed, only All other components are obtained directly from the output of the instrument, and the other part is calculated from the output of the instrument. After integration and transposition, we get: ； The symbols are defined as: , , ; Further select data at equal intervals, abbreviated as: ； Through the transformation relationship between quaternion and attitude matrix, and then using the multiplication property of quaternion, we get: ； (2) Designed the objective function for optimizing ocean current velocity parameters; The objective function for optimizing the ocean current velocity parameters constructed in step (2) is: ； The parameters in the formula are as follows: , Theoretically, when and When taking the true value ,at the same time Approaching 0, and The problem of solving can be transformed into a parameter optimization problem; (3) Designed an optimal estimation algorithm for ocean current velocity based on the GNSS / SINS / DVL combination; The ocean current velocity optimization estimation algorithm of the GNSS / SINS / DVL combination described in step (3) is to solve the parameter optimization problem in step (2) by transforming the parameter solution problem into the problem of finding the extreme value of a multivariate function.

2. The method for optimizing ocean current velocity estimation based on a GNSS / SINS / DVL combination according to claim 1, characterized in that: The specific process of the ocean current velocity optimization estimation algorithm based on the GNSS / SINS / DVL combination described in step (3) is as follows: For the current time : Step 1: Calculate , and value, and then according to and , use them Calculate the time value , and ; Step 2: Set k = 0 and use a good estimate start; Step 3: Calculate first and second order partial derivatives , , ; Step 4: Solve for the increment ; Step 5: Update ; Step 6: Set k=k+1 and go to step 4 until the convergence accuracy is reached or the maximum number of iterations is reached; Step 7: Get the initial attitude quaternion and ocean current speed ; Step 8: Use the initial posture matrix to calculate the posture matrix at the current moment.

Images