A DVL position alignment method

Abstract

The present invention discloses a DVL position alignment method, comprising establishing a measurement model and acquiring sensor data; employing attitude decomposition to process position integrals, and using end-point position approximation as a substitute to achieve coarse alignment of the DVL moving base position; and performing fine alignment of the DVL moving base position through iterative position alignment; and completing the position alignment if the position alignment time reaches a set value M, otherwise repeating the steps until the set value M is reached. The present invention solves the problem of being unable to obtain an accurate position during the combined alignment process of a DVL and an inertial navigation system, and can achieve both attitude alignment and position alignment. After the real-time position alignment is completed, post-processing position alignment can be used to further optimize the position accuracy.

Description

Technical Field

[0001] The present invention relates to the field of integrated navigation systems for underwater vehicles, and particularly to a DVL position alignment method. Background Art

[0002] Underwater vehicles are important carriers for exploring underwater terrain and underwater organisms. Currently, underwater navigation applicable to underwater vehicles mainly adopts a scheme with inertial navigation as the main and acoustic navigation as the auxiliary. In the scheme for achieving high-precision autonomous navigation, DVL (Acoustic Doppler Velocimeter) has the advantages of strong autonomy and high velocity measurement accuracy. In order for the system to achieve autonomous navigation, initial alignment is an essential stage. Most of the existing methods focus on attitude alignment, but ignore the defect that the position of DVL is uncertain, which leads to large errors in subsequent integrated navigation. Therefore, it is of great significance to propose a DVL position alignment method that can achieve both attitude alignment and position alignment. Summary of the Invention

[0003] The purpose of the present invention is to provide a DVL position alignment method, which can achieve attitude alignment and position alignment at the same time, and achieve post-processing position fine alignment, so as to improve the position estimation accuracy of the underwater autonomous navigation system.

[0004] The technical solution for achieving the purpose of the present invention is as follows:

[0005] A DVL position alignment method includes the following steps:

[0006] Step 1: Establish a measurement model and obtain sensor data;

[0007] Step 2: Use attitude decomposition to perform position integration processing, and use the end position for approximate substitution to achieve rough alignment of the DVL moving base position;

[0008] Step 3: Perform fine alignment of the DVL moving base position through iterative position alignment;

[0009] Step 4: The position alignment time length is M. If k≥M, output the position alignment result and complete the position alignment process. If k<M, indicating that the position alignment is not completed, repeat the above steps 1 to 3 until the position alignment ends.

[0010] Compared with the prior art, the advantages of the present invention are as follows:

[0011] (1) The present invention uses attitude decomposition to perform step-by-step calculation on the position integration formula, and uses the end position for approximate substitution to achieve real-time position estimation, having the advantage of real-time position alignment;

[0012] (2) The present invention uses iterative position alignment calculation to achieve post-processing position fine alignment;

[0013] (3) The present invention adopts position alignment, which can realize attitude alignment and position estimation at the same time, improve the position estimation accuracy of the underwater autonomous navigation system, and improve the navigation performance of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 It is a position alignment structure diagram.

[0015] Figure 2 This is the real-time position alignment result diagram.

[0016] Figure 3 This is the post-processing position alignment result diagram. DETAILED DESCRIPTION

[0017] The present invention will be further described in detail below with reference to the accompanying drawings and implementation examples:

[0018] At present, real-time position alignment does not use iterative calculation, but directly uses the attitude alignment result and the current calculated position plus the initial position calculation, so the position error is not accumulated, and as the alignment accuracy improves, the position estimation accuracy will also improve. However, due to the use of approximate calculation, the position error will be large at the beginning, and it will not be able to converge to a better result in the end. Based on this, the present invention proposes a DVL position alignment method, combined with Figure 1 , a two-step method of real-time decomposition position rough estimation and post-processing position fine alignment is adopted to achieve high-precision DVL position self-alignment without external assistance. Specifically, it includes the following steps:

[0019] Step 1: Get sensor data;

[0020] From the inertial sensor measurement model, we can know that:

[0021]

[0022] Where, represents the output acceleration of the mid-latitude accelerometer; f b represents the true acceleration at mid-latitudes; b a Indicates acceleration zero bias; η a represents the accelerometer measurement noise; Indicates the angular velocity output by the mid-latitude gyroscope; represents the true angular velocity at mid-latitudes; b g Indicates the gyroscope bias; η g represents the gyroscope measurement noise;

[0023] The gyroscope measurement constant drift error is b g =[0.02 0.02 0.02] T° / h, the random walk error measured by the gyroscope is The output frequency is 200Hz; the accelerometer measurement constant drift error is b a =[500 500 500] T μg, the random walk error of the accelerometer is The output frequency is 200Hz;

[0024] The sampling period of the DVL system is 1s. Ignoring the DVL proportional factor error and installation error angle error, its measurement model can be expressed as:

[0025]

[0026] Where, Indicates DVL measurement speed; v b Indicates the true velocity of the carrier system; η b represents the measurement noise;

[0027] Step 2: Perform rough alignment of the DVL dynamic base position;

[0028] The above measurement model can be used to obtain sensor outputs, thus achieving attitude alignment. However, during the combination process, DVL cannot provide position information. Therefore, position alignment is required. From the position update equation, we can know:

[0029]

[0030] Where, represents position differential; R c represents the transformation matrix; v n Indicates the navigation system speed; R M R represents the radius of curvature of the circle; N Indicates the radius of curvature of the meridian; h indicates the altitude information; L indicates the latitude;

[0031] Rearranging the above formula and applying the direction cosine chain rule yields:

[0032]

[0033] Where, Represents the direction cosine matrix of the navigation system relative to the initial navigation system; Represents position differential; R c represents the transformation matrix; v n Indicates the navigation system speed; Indicates the initial alignment to obtain the direction cosine matrix; represents the direction cosine matrix of the carrier system relative to the initial carrier system; v b Indicates the true velocity of the carrier system;

[0034] Integrate both sides of the above equation:

[0035]

[0036] Where, Represents the direction cosine matrix of the navigation system at time τ relative to the initial navigation system; Represents position differential; R c represents the transformation matrix; t m Indicates the current moment; p n (0) indicates the initial position; Indicates the navigation system at time τ relative to t k Direction cosine matrix of the time navigation system; Represents the mapping of the angular velocity of the navigation system relative to the inertial system in the navigation system;

[0037] Calculate the integral term of the above formula and replace it with the end point position:

[0038]

[0039] Where R c represents the transformation matrix; Indicates the navigation system at time τ relative to t k Direction cosine matrix of the time navigation system; Represents the mapping of the angular velocity of the navigation system relative to the inertial system in the navigation system; Δt D Indicates DVL sampling time;

[0040] Therefore, the position integral formula can be reformulated as:

[0041]

[0042] Where, Represents the direction cosine matrix of the navigation system at time τ relative to the initial navigation system; Represents position differential; R c represents the transformation matrix; p n (t m ) represents t m Time position; p n (0) indicates the initial position;

[0043] The integral of the term related to the carrier system velocity can be expressed as:

[0044]

[0045] Where, Indicates the initial alignment to obtain the direction cosine matrix; represents the direction cosine matrix of the load system relative to the initial load system at time τ; v b (τ) represents the velocity of the loading system at time τ; It represents the mapping of the rotational angular velocity of the carrier system relative to the inertial system in the carrier system;

[0046] Use the velocity extrapolation algorithm to calculate the above formula:

[0047]

[0048] Where, It represents the mapping of the angular velocity of the carrier system relative to the inertial system in the carrier system; v b (τ) represents the velocity of the loading system at time τ; Δt D Indicates DVL sampling time; v b (t k ) represents t k Time load system speed; v b (t k-1 ) represents t k+1 Time load system speed;

[0049] At this point, the rough alignment of the DVL dynamic base position can be expressed as:

[0050]

[0051] Where p n (t m ) represents t m Time position; p n (0) indicates the initial position; R c represents the transformation matrix; Indicates the initial alignment to obtain the direction cosine matrix; v b (t k ) represents t k Time load system speed; v b (t k-1 ) represents t k-1 Time loading system speed; Δt D Indicates DVL sampling time; It represents the mapping of the rotational angular velocity of the carrier system relative to the inertial system in the carrier system;

[0052] Step 3: Perform precise alignment of the DVL dynamic base position;

[0053] The above rough position alignment is obtained by approximating the end-point position, ignoring the effects of position changes. Therefore, it can only be used as an approximate estimate in real-time applications. When the initial carrier alignment is completed, the DVL position alignment can be accurately obtained through post-processing. To this end, the above integral is calculated as follows:

[0054]

[0055] Where, Represents the direction cosine matrix of the navigation system at time τ relative to the initial navigation system; represents position differential; R c represents the transformation matrix; p n (t k ) represents t k Time position; p n (t k+1 ) represents t k+1 Time position; Indicates t k The direction cosine matrix of the moment navigation system relative to the initial navigation system; Indicates t k+1 The direction cosine matrix of the moment navigation system relative to the initial navigation system; Represents the mapping of the angular velocity of the navigation system relative to the inertial system in the navigation system;

[0056] The integral on the right side of the equation can be expressed as:

[0057]

[0058] Where, Indicates the initial alignment to obtain the direction cosine matrix; represents the direction cosine matrix of the load system relative to the initial load system at time τ; v b (τ) represents the velocity of the loading system at time τ; v b (t k ) represents t k Time load system speed; v b (t k+1 ) represents t k+1 Time loading system speed; Δt D Indicates DVL sampling time; Indicates t k The direction cosine matrix of the load system relative to the initial load system at time t; Indicates t k+1 The direction cosine matrix of the load system relative to the initial load system at time t;

[0059] The post-processing DVL position fine alignment can be expressed as:

[0060]

[0061] Where p n (t k+1 ) represents t k+1 Time position; p n (t k ) represents t k Time position; R c represents the transformation matrix; Indicates the initial navigation system relative to tk+1 Direction cosine matrix of the time navigation system; v b (t k ) represents t k Time load system speed; v b (t k+1 ) represents t k+1 Time loading system speed; Δt D Indicates DVL sampling time; Indicates t k The direction cosine matrix of the load system relative to the initial load system at time t; Indicates t k+1 The direction cosine matrix of the load system relative to the initial load system at time t;

[0062] The method is verified, and the gyroscope measurement constant drift error is b g =[0.02 0.02 0.02] T° / h, the random walk error measured by the gyroscope is The output frequency is 200Hz; the accelerometer measurement constant drift error is b a =[500 500 500] T μg, the random walk error of the accelerometer is The output frequency is 200Hz; the DVL system sampling period is 1s. The calibration time is set to 600s, and the result is Figure 2 Real-time position alignment result diagram, Figure 3 The post-processing position alignment result diagram is compared with Figure 2 and Figure 3 It can be seen that the real-time position alignment error is 59.9069m, while the post-processing position alignment error is 17.9817m. The total alignment process path length is 5.85km. This shows that the present invention can implement iterative position calculation, and the final position alignment accuracy is also better than the real-time position alignment, achieving the purpose of improving the position alignment of the underwater autonomous navigation system.

Claims

1. A DVL position alignment method, characterized in that: Including steps: Step 1: Establish a measurement model and obtain sensor data; Step 2: Use attitude decomposition to perform position integration processing, and use the end point position approximation to achieve rough alignment of the DVL moving base position; Specifically include: Step 2-1, determining a location update model based on sensor data; Step 2-2: Apply the direction cosine chain rule and integral operation to the position update model, and replace it with the end point position to determine a new position integral model; Step 2-3: Based on the new position integral model, a velocity extrapolation algorithm is used to determine the position coarse alignment model and obtain the coarse alignment position; The location update model is: Where, Represents position differential; R c represents the transformation matrix; v n Indicates the navigation system speed; R M R represents the radius of curvature of the y-axis circle; N Indicates the radius of curvature of the meridian; h indicates the altitude information; L indicates the latitude; Step 3: Perform precise alignment of the DVL dynamic base position through iterative position alignment; The model for the DVL dynamic base position fine alignment is: Where p n (t k+1 ) represents t k+1 Time position; p n (t k ) represents t k Time position; R c represents the transformation matrix; Indicates the initial navigation system relative to t k+1 Direction cosine matrix of the time navigation system; v b (t k ) represents t k Time load system speed; v b (t k+1 ) represents t k+1 Time loading system speed; Δt D Indicates DVL sampling time; Indicates t k The direction cosine matrix of the load system relative to the initial load system at time t; Indicates t k+1 The direction cosine matrix of the load system relative to the initial load system at time t; Step 4: If the position alignment time reaches the set value M, the position alignment is completed; otherwise, repeat steps 1 to 3 until the set value M is reached.

2. The DVL position alignment method according to claim 1, wherein: The measurement model ignores the DVL scale factor error and the installation error angle error, specifically: Where, Indicates DVL measurement speed; v b Indicates the true velocity of the carrier system; η b represents the measurement noise.

3. The DVL position alignment method according to claim 1, wherein: The step 2-2 specifically includes: The direction cosine chain method is used to update the position model: Where, Represents the direction cosine matrix of the navigation system relative to the initial navigation system; Represents position differential; R c represents the transformation matrix; v n Indicates the navigation system speed; Indicates the initial alignment to obtain the direction cosine matrix; represents the direction cosine matrix of the carrier system relative to the initial carrier system; v b Indicates the true velocity of the carrier system; Performing integral operations on both sides of the above equation yields: Where, Represents the direction cosine matrix of the navigation system at time τ relative to the initial navigation system; Represents position differential; R c represents the transformation matrix; t m Indicates the current moment; p n (0) indicates the initial position; Indicates the navigation system at time τ relative to t k Direction cosine matrix of the time navigation system; Represents the mapping of the angular velocity of the navigation system relative to the inertial system in the navigation system; Calculate the integral term of the above formula and replace it with the end point position to obtain: Where R c represents the transformation matrix; Indicates the navigation system at time τ relative to t k Direction cosine matrix of the time navigation system; Represents the mapping of the angular velocity of the navigation system relative to the inertial system in the navigation system; Δt D Indicates DVL sampling time; The new position integral model is determined as: Where, Represents the direction cosine matrix of the navigation system at time τ relative to the initial navigation system; Represents position differential; R c represents the transformation matrix; p n (t m ) represents t m Time position; p n (0) indicates the initial position.

4. The DVL position alignment method according to claim 3, wherein: The method of using the velocity extrapolation algorithm to determine the position coarse alignment model specifically includes: Determine the integral of the term related to the carrier system velocity: Where, Indicates the initial alignment to obtain the direction cosine matrix; represents the direction cosine matrix of the load system relative to the initial load system at time τ; v b (τ) represents the velocity of the loading system at time τ; It represents the mapping of the rotational angular velocity of the carrier system relative to the inertial system in the carrier system; Using the velocity extrapolation algorithm, we get: Where, It represents the mapping of the angular velocity of the carrier system relative to the inertial system in the carrier system; v b (τ) represents the velocity of the loading system at time τ; Δt D Indicates DVL sampling time; v b (t k ) represents t k Time load system speed; v b (t k-1 ) represents t k+1 Time load system speed; The rough alignment model is determined as follows: Where p n (t m ) represents t m Time position; p n (0) indicates the initial position; R c represents the transformation matrix; Indicates the initial alignment to obtain the direction cosine matrix; v b (t k ) represents t k Time load system speed; v b (t k-1 ) represents t k-1 Time loading system speed; Δt D Indicates DVL sampling time; It represents the mapping of the angular velocity of the carrier system relative to the inertial system in the carrier system.

5. The DVL position alignment system according to the method of claim 1, characterized in that: It includes a data acquisition unit, a position coarse alignment unit and a position fine alignment unit, wherein the data acquisition unit is used to establish a measurement model and obtain sensor data; the position coarse alignment unit uses posture decomposition to perform position integration processing, and uses the end point position approximation replacement to achieve the DVL dynamic base position coarse alignment; the position fine alignment unit performs the DVL dynamic base position fine alignment through iterative position alignment.

6. A DVL position alignment device, characterized in that: include: A memory, a processor and a computer program stored in the memory, wherein when the processor executes the computer program, the DVL position alignment method according to any one of claims 1 to 4 is implemented.

7. A computer storage medium, characterized in that The computer storage medium stores an executable program, and the executable program is executed by a processor to implement the steps of the DVL position alignment method according to any one of claims 1 to 4.

Images