A DVL error parameter estimation method based on particle swarm optimization

Abstract

A DVL error parameter estimation method based on particle swarm optimization relates to the field of navigation system technology. The present invention aims to solve the problems of high cost and low accuracy in existing DVL error parameter estimation methods. The present invention includes: obtaining the velocity V of the vehicle in the DVL measurement coordinate system. d (k) Obtain the vehicle attitude angle, and use the vehicle attitude angle and the vehicle's velocity in the DVL measurement coordinate system to perform attitude, velocity, and position calculations to obtain the vehicle's dead-reckoning trajectory; combine the vehicle's longitude, latitude, and depth information at all times to obtain the vehicle's measured trajectory, and obtain the error between the vehicle's measured trajectory and the dead-reckoning trajectory; use the DVL error parameter as the parameter to be optimized, and use the error between the vehicle's measured trajectory and the dead-reckoning trajectory to construct a fitness function. Using the fitness function as the optimization indicator, perform PSO iterations and ultimately complete DVL calibration. The present invention is used to estimate DVL error parameters.

Description

Technical Field

[0001] The present invention relates to the technical field of navigation systems, and in particular to a DVL error parameter estimation method based on particle swarm optimization. Background Art

[0002] With the increasing utilization and development of marine resources, seabed surveying, pipeline inspection, and deep-sea operations have placed new demands on underwater positioning and navigation technologies. Underwater integrated navigation systems based on multi-sensor data fusion are an important research direction for achieving high-precision underwater navigation. As a key sensor device for underwater navigation, the acoustic Doppler Velocity Log (DVL) emits sound waves to the seabed and analyzes the Doppler frequency shift of the echo to calculate the velocity of the underwater vehicle relative to the seabed. However, due to limitations in sensor structure and machining accuracy, as well as assembly errors between the measurement coordinate system and the vehicle coordinate system, measurement errors are primarily composed of scale factor error and installation angle error. Accurately estimating these two error parameters can significantly improve underwater navigation accuracy.

[0003] In engineering applications, to control underwater equipment costs, a compass is used instead of an inertial navigation system, combined with a DVL and an Ultra Short Base Line (USBL) navigation system. However, due to the poor operational stability of the USBL under actual operating conditions and the significant fluctuations in sensor data, it is impossible to obtain valid vehicle velocity data through position differentials, thus failing to provide effective velocity observation conditions for DVL calibration. Therefore, a DVL error parameter calibration technique based on a compass / DVL / USBL combination is urgently needed for miniaturized, low-cost, and deep-sea applications.

[0004] Currently, traditional DVL calibration methods primarily employ fixed navigation trajectories, such as straight or circular, using GPS-measured velocity information as a reference. After the navigation is complete, the installation deflection angle and scale factor are calculated. However, this method places high demands on the navigation trajectory, increasing the difficulty of vehicle control. Currently proposed SINS / GNSS-based DVL calibration methods, while not requiring a specific navigation trajectory, still rely on comparing combined navigation velocity data with DVL-measured velocity data. This method places high demands on the vehicle's sensor payload, increasing costs. Furthermore, the calibration algorithm based on velocity observations is significantly affected by DVL measurement noise, limiting calibration accuracy. "An Online Calibration Method for DVL Error Parameters Based on Integrated Navigation" proposes applying Kalman filtering to DVL error parameters using dead reckoning and LBL measurements to obtain DVL calibration parameters and navigation data. However, this method only estimates scale factor error and z-axis installation angle error, and its estimation accuracy is low, resulting in limitations. Therefore, current DVL error parameter estimation methods suffer from high cost and low accuracy, making them unsuitable for deep underwater environments. Summary of the Invention

[0005] The purpose of the present invention is to solve the problems of high cost and low precision in the existing DVL error parameter estimation method, and propose a DVL error parameter estimation method based on particle swarm optimization.

[0006] A DVL error parameter estimation method based on particle swarm optimization has the following specific process:

[0007] Step 1: Use DVL to obtain the velocity V of the vehicle in the DVL measurement coordinate system d (k) obtaining the vehicle attitude angle using a compass, and performing attitude calculation, velocity calculation, and position calculation in sequence using the vehicle attitude angle and the vehicle velocity in the DVL measurement coordinate system to obtain the vehicle's dead reckoning trajectory;

[0008] The attitude calculation is specifically as follows: using the compass to obtain the vehicle attitude angle, and using the vehicle attitude angle to obtain the body coordinate system OX b Y b Z b To the Northeast Navigation Coordinate System OX n Y n Z n The direction cosine matrix

[0009] The speed calculation is specifically as follows: the speed V of the vehicle in the DVL measurement coordinate system d (k) Converted to the velocity V in the body coordinate system b (k), and then the velocity V in the body coordinate system b (k) with Multiply to obtain the carrier's velocity V in the northeast sky navigation coordinate system n (k);

[0010] The position solution is specifically as follows: using V n (k) obtaining a dead reckoning increment of the carrier at the current moment, and using the dead reckoning increment and the dead reckoning position of the carrier at the previous moment to obtain the dead reckoning position of the carrier at the current moment, wherein the dead reckoning positions of the carrier at all moments constitute the dead reckoning trajectory of the carrier;

[0011] Step 2: Use USBL and a depth gauge to obtain the longitude, latitude, and depth information of the vehicle. Combine the longitude, latitude, and depth information of the vehicle at all times to obtain the measured trajectory of the vehicle, and then obtain the error between the measured trajectory of the vehicle and the dead reckoning trajectory;

[0012] Step 3: Using the DVL error parameter as the parameter to be optimized, the error between the vehicle's measured trajectory and the dead reckoning trajectory is used to construct a fitness function. The fitness function is used as the optimization indicator to perform PSO iteration and finally complete the calibration of the DVL.

[0013] The error parameters of the DVL include: scale factor error, x-axis installation error angle, y-axis installation error angle, and z-axis installation error angle.

[0014] Furthermore, the dead reckoning trajectory is obtained by:

[0015] Step 1: Abstract the four DVL error parameter values ​​to be optimized into a particle with a dimension of 4. Multiple particles constitute a particle swarm, and then initialize the particle swarm optimizer and the particle swarm.

[0016] The four DVL error parameters to be optimized include: scale factor error, x-axis installation error angle, y-axis installation error angle, and z-axis installation error angle;

[0017] The particle swarm optimizer and particle swarm are initialized as follows:

[0018] Step 1: Set the particle dimension dim, particle size size, maximum number of iterations T, and fitness value variance threshold σ of the particle swarm optimizer. t , individual learning factor c1, social learning factor c2, initial inertia coefficient ω ini , terminal inertia coefficient ω end , speed lower limit v min , speed limit v max , Position lower limit x min 、Position upper limit x max Initialize;

[0019] Step 112: Set the initial velocity v of the particlei (0), initial position x i (0), initial individual optimal value pfitness i , initial individual optimal solution pbest i , and label all particles, then set the initial group optimal value bestf = pfitness1 and the initial group optimal solution gbest = pbest1 based on the initial individual optimal solution and initial individual optimal value of the particles;

[0020] Where i is the particle number;

[0021] Step 1 and 2: number all particles in the particle swarm and set the current particle number i to 0;

[0022] Step 13: Add 1 to the current particle number;

[0023] Step 14: According to the position x of the current particle i in the current iteration process t i (t), obtain the error parameters of DVL, and use the error parameters of DVL to obtain the DVL error parameter matrix S;

[0024] x i (t)=(x i1 x i2 x i3 x i4 )

[0025] =(δk θ dx θ dy θ dz )

[0026]

[0027] Where δk is the scale factor error of DVL, θ dx ,θ dy ,θ dz They are the installation angle errors of DVL in the x-axis, y-axis and z-axis directions, x i1 、x i2 、x i3 、x i4 is the parameter of each dimension of particle i;

[0028] Step 15: Get the initial position of the carrier:

[0029] P DR (0)=[L DR (0) λ DR (0) h DR (0)] T

[0030] Among them, LDR (0) is the initial latitude of the carrier, λ DR (0) is the initial longitude of the vehicle, h DR (0) is the initial depth of the carrier;

[0031] Step 16: Update the longitude, latitude and altitude of the navigation system at the last moment to obtain the updated vehicle position P at the last moment. DR (k-1):

[0032] P DR (k-1)=[L DR (k-1) λ DR (k-1) h DR (k-1)] T

[0033] Where k is the current moment;

[0034] Step 17: Use the updated carrier position to obtain the earth curvature R m (k), R n (k), as follows:

[0035]

[0036]

[0037]

[0038] Where e is the eccentricity of the Earth, f' is the ellipticity of the Earth, and R e is the radius of the Earth;

[0039] Step 18: Use the compass to obtain the attitude angle of the vehicle Use the carrier attitude angle to obtain the body coordinate system OX b Y b Z b To the Northeast Navigation Coordinate System OX n Y n Z n The direction cosine matrix As follows:

[0040]

[0041] in, is the heading angle, θ is the pitch angle, and γ is the roll angle;

[0042] Step 19: Use DVL to obtain the velocity of the vehicle in the DVL measurement coordinate system And use V d (k) and S obtained in step 14 are used to obtain the current speed of the vehicle in the body coordinate system As follows:

[0043] V b (k) = S -1 ·V d (k)

[0044] Among them, S- 1 is the inverse matrix of S, is the x-direction velocity of the vehicle in the DVL measurement coordinate system, is the y-direction velocity of the vehicle in the DVL measurement coordinate system, is the z-direction velocity of the carrier in the DVL measurement coordinate system, is the x-direction velocity of the vehicle in the body coordinate system, is the y-direction velocity of the vehicle in the body coordinate system, is the z-direction velocity of the vehicle in the body coordinate system;

[0045] Step 10: Use the data obtained in step 18 and V obtained in step 19 b (k) Get the carrier speed in the current Northeast Sky navigation coordinate system As follows:

[0046]

[0047] in, They represent the eastward velocity, northward velocity and celestial velocity of the vehicle in the northeast sky navigation coordinate system respectively;

[0048] Step 11: Use the V obtained in step 10 n (k) The updated carrier position P at the last moment obtained in step 16 DR (k-1) and the earth curvature R obtained in step 17 m (k), R n (k) Get the current position increment ΔP DR (k)=[ΔL DR (k) Δλ DR (k) Δh DR (k)] T ;

[0049] Where, ΔL DR (k) is the dead centre latitude increment, Δλ DR (k) is the dead longitude increment, Δh DR (k) is the dead depth increment;

[0050] Step 12: According to the updated last moment carrier position P DR (k-1) and the current position increment ΔPDR (k)=[ΔL DR (k) Δλ DR (k) Δh DR (k)] T Perform dead reckoning on the carrier to obtain the current carrier position P DR (k);

[0051] Step 13: Determine whether the carrier's dead reckoning has been completed at all times. If the carrier's position at all times has not been obtained, return to step 16 to enter the dead reckoning update, complete the carrier's dead reckoning at the next time, and continue until the carrier's position at all times is obtained. If the carrier's position at all times has been obtained, the carrier's position at all times constitutes the carrier's dead reckoning trajectory, and then execute step 2.

[0052] Furthermore, the current position increment ΔP DR (k)=[ΔL DR (k) Δλ DR (k) Δh DR (k)] T Obtained by the following formula:

[0053] ΔP DR (k) = T s ·T V (P DR (k-1))·V n (k)

[0054]

[0055] Among them, T s is the system period of dead reckoning, T V (P DR (k-1)) is the position increment conversion matrix based on the previous moment. Further, the current moment carrier position P DR (k), is obtained by the following formula:

[0056] P DR (k)=ΔP DR (k)+P DR (k-1).

[0057] Furthermore, the error between the measured trajectory and the dead reckoning trajectory of the vehicle in step 2 includes the following steps:

[0058] Step 2. Use USBL and depth meter to obtain the longitude, latitude and depth information of the vehicle, combine the longitude, latitude and depth information of the vehicle at all times, and obtain the measurement trajectory P of the vehicle. Mea (k)=[L USBL (k) λUSBL (k) h DEP (k)] T ,k=1,2,…,N:

[0059] Among them, L USBL (k) is the carrier latitude measured by USBL, λ USBL (k) is the longitude of the carrier measured by USBL, h DEP (k) is the depth of the vehicle measured by the depth gauge;

[0060] Step 2: Use dead reckoning trajectory P DR (k)=[L DR (k) λ DR (k) h DR (k)] T ,k=1,2,…,N and P Mea (k)=[L USBL (k) λ USBL (k) h DEP (k)] T , k=1,2,…,N, and obtain the error between the measured trajectory and the dead reckoning trajectory of the vehicle ε(k)=[ε L (k) ε λ (k) ε h (k)] T ,k=1,2,…,N, as follows:

[0061] ε L (k) = R e ·[L USBL (k)-L DR (k)]

[0062] ε λ (k) = R e ·cos(L USBL (k))·[λ USBL (k)-λ DR (k)]

[0063] ε h (k) = h DEP (k)-h DR (k)

[0064] Among them, ε L (k) is the latitude error, ε λ (k) is the longitude error, ε h (k) is the depth error and N is the total time length.

[0065] Furthermore, in step 3, the error parameters of the DVL are used as the parameters to be optimized, the error between the measured trajectory and the dead reckoning trajectory of the vehicle is used to construct a fitness function, and the fitness function is used as the optimization indicator to perform PSO iteration and finally complete the calibration of the DVL, including the following steps:

[0066] Step 3. Obtain the fitness value of the current particle fitness(x according to ε(k) obtained in step 2 i );

[0067] Step 32: Determine whether all particles in the particle swarm have completed fitness value calculation. If all particles have completed fitness value calculation, proceed to step 33. If not, return to step 13.

[0068] Step 3. Set the current particle fitness value fitness(x i ) and the current particle individual optimal value pfitness i For comparison, when fitness(x i )≤pfitness i When , the optimal value pfitness of the current particle is updated i =fitness(x i ), and at the same time update the individual optimal solution of the current particle to the current particle position pbest i =x i ; If fitness(x i )>pfitness i , then it will not be updated;

[0069] Step 3 and 4: Set the optimal value of the particle swarm individual pfitness = [pfitness1 pfitness2 …pfitness size ] the minimum value min[pfitness1 pfitness2 … pfitness size ] is compared with the current particle swarm's optimal value bestf. If min[pfitness1 pfitness2 … pfitness size ]≤bestf, then update the optimal value of the current particle swarm bestf=min[pfitness1 pfitness2 … pfitness size ], gbest = x i ;If min[pfitness1 pfitness2 … pfitness size ]>bestf is not updated;

[0070] Step 35: Obtain the current particle swarm fitness value variance σ based on the current particle fitness value 2 ;

[0071] Step 36: Get the current number of iterations t. If the current number of iterations is greater than or equal to the maximum number of iterations T, then the optimization ends. If the current number of iterations is less than the maximum number of iterations T, and σ 2 Less than or equal to the threshold σ t , the optimization ends; if the current number of iterations is less than the maximum number of iterations T, and σ 2 Greater than σ t , then execute step 37;

[0072] Step 37: Use the current iteration number to obtain the inertia factor ω(t) of the current iteration;

[0073] Step 38: Use the inertia factor ω(t) of the current iteration to update the velocity v of all particles in the particle swarm. i (t+1):

[0074] v i (t+1)=ω(t)×v i (t)+c1×rand×(pbest i -x i (t))+c2×rand×(gbest-x i (t))

[0075] Where i = 1, 2, ..., size is the particle number; v i (t), v i (t+1) represents the current speed and updated speed of particle i respectively; c1 and c2 represent the individual optimal learning factor and the global optimal learning factor respectively; rand represents a random number between 0 and 1; pbest i represents the individual optimal solution of particle i in the first t iterations; gbest represents the group optimal solution of the particle swarm in the first t iterations; x i (t) represents the current position of particle i;

[0076] Step 39: Use the v obtained in step 38 i (t+1) Process the particle velocity out of bounds:

[0077] When v i (t+1)>v max When v i (t+1)=v max

[0078] When v i (t+1)<v min When v i (t+1)=vmin

[0079] Among them, v min is the lower speed limit, v max It is the speed limit;

[0080] Step 30: Use the v obtained in step 39 i (t+1) Update the positions of all particles in the particle swarm:

[0081] x i (t+1)=x i (t)+v i (t+1)

[0082] Among them, x i (t), x i (t+1) represents the current position and updated position of particle i respectively;

[0083] Step 31: Use all updated particle positions obtained in step 30 to perform particle position out-of-bounds processing:

[0084] When x i (t+1)>x max When x i (t+1)=x max

[0085] When x i (t+1)<x min When x i (t+1)=x min

[0086] Among them, x min is the lower limit of the position, x max is the upper limit of position;

[0087] Step 32: After the particle position out-of-bounds processing is completed, return to step 2 to perform the t+1th particle swarm iteration.

[0088] Furthermore, in step 3, the fitness value of the current particle is obtained according to ε(k) obtained in step 2. i ), as follows:

[0089]

[0090] Furthermore, in the steps 3 and 5, the current particle swarm fitness value variance σ is obtained according to the fitness value of the current particle. 2 , as follows:

[0091]

[0092]

[0093] Δf max =max{|fitness(x i )-f avg |,i=1,2,…,size}

[0094] Among them, f avg is the average fitness value of the current particle swarm, f is the adjustment factor, Δf max is an intermediate variable.

[0095] Preferably,

[0096] Δf max =max{|fitness(x i )-f avg |,i=1,2,…,size}

[0097] Where Δf max is an intermediate variable.

[0098] Furthermore, in step 37, the inertia factor ω(t) of the current iteration is obtained by using the current iteration number, as shown in the following formula:

[0099]

[0100] Among them, ω ini is the initial inertia coefficient, ω end is the terminal inertia coefficient.

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

[0102] The present invention can complete the calibration of the DVL without a fixed navigation trajectory and complex calibration steps, has low control requirements for the underwater vehicle, and can perform calibration work during the normal driving process of the underwater vehicle; the present invention adopts a calibration method based on position observation, which greatly reduces the influence of DVL measurement noise on the calibration results, thereby improving the calibration accuracy; the present invention only uses a compass as an attitude measurement sensor, which reduces the requirements for the sensor load of the underwater vehicle and reduces the calibration cost. The present invention has great advantages in a miniaturized and low-cost use environment; the present invention uses USBL as a position measurement sensor, so that the underwater vehicle can calibrate the DVL in waters of greater depths, and there is no need to frequently surface to obtain position calibration, which is conducive to use in environments with high concealment requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0103] Figure 1 This is a system block diagram of the present invention;

[0104] Figure 2 Flowchart of the present invention;

[0105] Figure 3 It is the definition diagram of the carrier coordinate system and attitude angle;

[0106] Figure 4 is the simulation trajectory diagram;

[0107] Figure 5 This is the curve of the optimal value of the group changing with the number of iterations in the simulation experiment;

[0108] Figure 6 is the scale factor error estimation curve of DVL in the simulation experiment;

[0109] Figure 7 is the estimated curve of the x-axis installation angle error of the DVL in the simulation test;

[0110] Figure 8 is the estimated curve of the y-axis installation angle error of the DVL in the simulation test;

[0111] Figure 9 is the estimated curve of the z-axis installation angle error of the DVL in the simulation test;

[0112] Figure 10 Global relative positioning error curves of dead reckoning before and after compensating the estimated values ​​of the DVL error parameters. DETAILED DESCRIPTION

[0113] Specific implementation method 1: Figure 1-2 As shown, the specific process of the DVL error parameter estimation method based on particle swarm optimization in this embodiment is as follows:

[0114] First, the four DVL error parameter values ​​to be optimized are abstracted into a particle with a dimension of 4. A particle swarm is formed by multiple such particles. Then, the particle swarm optimizer and the particle swarm are initialized:

[0115] (1.1) Particle Swarm Optimizer Initialization

[0116] During the initialization of the particle swarm optimizer, the four DVL error parameters to be optimized are abstracted into a particle with a dimension of 4. The corresponding relationship of each dimension of the particle is shown in the following table:

[0117] Table 1

[0118]

[0119] Based on the above abstraction, the initial parameters of the particle swarm optimizer are defined. The specific parameters are shown in the following table:

[0120] Table 2

[0121]

[0122]

[0123] The space between the lower limit and the upper limit of position is the value space of each particle, and the space between the lower limit and the upper limit of speed is the step size of particle iteration.

[0124] (1.2) Particle swarm initialization

[0125] Initialize the initial position, initial velocity and initial individual optimal value of each particle as shown in the following table:

[0126] Table 3

[0127]

[0128] After all particles are initialized, they are numbered, and then the group optimal value and group optimal solution are further set as the individual optimal value of particle number 1 and its corresponding particle position, as shown in the following table:

[0129] Table 4

[0130]

[0131] 2. Obtaining fitness values ​​based on position observations:

[0132] (2.1) Set the sequence number i of the current particle to 0;

[0133] (2.2) Increase the current particle number by 1;

[0134] (2.3) DVL error parameter matrix solution:

[0135] According to the position x of the current particle i in the current iteration process t i (t) Obtain the error parameters of DVL. The specific corresponding relationships are as follows:

[0136] x i (t)=(x i1 x i2 x i3 x i4 )

[0137] =(δk θ dx θ dy θ dz )

[0138] Where δk is the scale factor error of DVL, θ dx ,θ dy ,θ dz They are the installation angle errors of DVL in the x-axis, y-axis and z-axis directions, x i1 、x i2 、xi3 、x i4 is the parameter of each dimension of particle i.

[0139] Obtain the DVL error parameter matrix S according to the error parameters of DVL:

[0140]

[0141] (2.4) Dead reckoning initialization is to determine the initial position of the vehicle:

[0142] P DR (0)=[L DR (0) λ DR (0) h DR (0)] T

[0143] Among them, L DR (0) is the initial latitude of the carrier, λ DR (0) is the initial longitude of the vehicle, h DR (0) is the initial depth of the carrier;

[0144] Here, you can directly obtain the current values ​​of USBL and depth gauge.

[0145] (2.5) Update the longitude, latitude and altitude of the navigation system at the last moment:

[0146] P DR (k-1)=[L DR (k-1) λ DR (k-1) h DR (k-1)] T

[0147] Where k is the current moment;

[0148] (2.6) Calculate the radius of curvature of the Earth at the current moment as follows:

[0149]

[0150]

[0151] Among them, the eccentricity of the earth The Earth's ellipticity f' = 1 / 298.257, the Earth's radius R e =6378137m.

[0152] (2.7) Attitude calculation: Based on the vehicle attitude angle data measured by the compass The current slave body coordinate system OX can be calculated b Y b Z bNortheast celestial navigation coordinate system OX n Y n Z n The direction cosine matrix for:

[0153]

[0154] in, is the heading angle, θ is the pitch angle, and γ is the roll angle;

[0155] Figure 3 This is the definition diagram of the carrier coordinate system and attitude angle. The heading angle when the carrier rotates clockwise is Take a positive value. The pitch angle θ takes a positive value when the carrier raises its head, and the roll angle γ takes a positive value when the carrier sinks on the starboard side.

[0156] (2.8) Velocity calculation: Obtain the velocity of the vehicle in the DVL measurement coordinate system based on the DVL. Calculate the current carrier's velocity in the body coordinate system as follows:

[0157] V b (k) = S -1 ·V d (k)

[0158] Among them, S -1 is the inverse matrix of S, is the x-direction velocity of the vehicle in the DVL measurement coordinate system, is the y-direction velocity of the vehicle in the DVL measurement coordinate system, is the z-direction velocity of the carrier in the DVL measurement coordinate system, is the x-direction velocity of the vehicle in the body coordinate system, is the y-direction velocity of the vehicle in the body coordinate system, is the z-direction velocity of the vehicle in the body coordinate system;

[0159] (2.9) Get the speed of the vehicle in the current Northeast Sky navigation coordinate system as follows:

[0160]

[0161] in They respectively represent the current eastward speed, northward speed and celestial speed of the vehicle in the northeast sky navigation coordinate system.

[0162] (2.10) Position calculation: Calculate the current position increment ΔP based on the position of the vehicle and the speed value in the Northeast Navigation Coordinate System DR (k)=[ΔL DR(k) Δλ DR (k) Δh DR (k)] T as follows:

[0163] ΔP DR (k) = T s ·T V (P DR (k-1))·V n (k)

[0164] Where, ΔL DR (k) is the dead centre latitude increment, Δλ DR (k) is the dead longitude increment, Δh DR (k) is the dead depth increment, T s is the system period of dead reckoning, T V (P DR (k-1)) is the position increment transformation matrix based on the previous moment, which is defined as:

[0165]

[0166] (2.11) According to the position P at the last moment DR (k-1) and the current position increment ΔP DR (k) Calculate the current position P DR (k)=[L DR (k) λ DR (k) h DR (k)] T as follows:

[0167] P DR (k)=ΔP DR (k)+P DR (k-1)

[0168] (2.12) Determine whether dead reckoning is completed at all times:

[0169] If the entire dead reckoning trajectory is not completed, return to step (2.5) and continue to enter the dead position update to perform dead reckoning at the next moment;

[0170] After the overall dead reckoning trajectory is completed, proceed to step (2.13) to perform error resolution based on the measured trajectory and the dead reckoning trajectory.

[0171] (2.13) According to the carrier measurement trajectory P composed of the longitude and latitude measured by USBL at all times and the depth measured by depth meter at all times Mea (k)=[L USBL (k) λ USBL (k) h DEP (k)]T , k = 1, 2, ..., N, and the dead reckoning trajectory P DR (k)=[L DR (k) λ DR (k) h DR (k)] T , k=1,2,…,N, calculate the position error ε(k)=[ε L (k) ε λ (k) ε h (k)] T , k=1,2,…,N is as follows:

[0172] ε L (k) = R e ·[L USBL (k)-L DR (k)]

[0173] ε λ (k) = R e ·cos(L USBL (k))·[λ USBL (k)-λ DR (k)]

[0174] ε h (k) = h DEP (k)-h DR (k)

[0175] Among them, ε L (k) is the latitude error, ε λ (k) is the longitude error, ε h (k) is the depth error, N is the total time length, L USBL (k) is the carrier latitude measured by USBL, λ USBL (k) is the longitude of the carrier measured by USBL, h DEP (k) is the depth of the vehicle measured by the depth gauge;

[0176] (2.14) Calculate the fitness value of the current particle fitness(x i )as follows:

[0177]

[0178] (2.15) Determine whether all particles in the particle swarm have completed the calculation of fitness values:

[0179] If the current particle swarm fails to traverse all particles, it returns to step (2.2) and enters the fitness value calculation process of the next particle;

[0180] When all the current particle swarms have obtained fitness values, they enter the particle swarm iteration process.

[0181] 3. Particle Swarm Iteration:

[0182] (3.1) Update of optimal position of individual particle swarm:

[0183] According to the fitness value of the current particle fitness(x i ), the individual optimal value pfitness of each particle i Make a judgment:

[0184] When the particle's current fitness value is less than or equal to its individual optimal value, that is, fitness(x i )≤pfitness i When the individual optimal value is updated to the fitness value pfitness i =fitness(x i ), and update the individual optimal solution of the particle to the current particle position pbest i =x i ; When the current fitness value of a particle is greater than its individual optimal value, it will not be updated.

[0185] (3.2) Particle swarm optimal position update:

[0186] According to the current individual optimal value of the particle swarm pfitness = [pfitness1 pfitness2 … pfitness size ]Judge the optimal value bestf of the current particle swarm:

[0187] When the minimum value of the individual optimal values ​​of the entire particle swarm is less than or equal to the current group optimal value, that is, min[pfitness1 pfitness2 … pfitness size ]≤bestf, update the group optimal value to the individual optimal value bestf=min[pfitness1 pfitness2 … pfitness size ], and update the group optimal solution to the corresponding particle position gbest = x i ; When the optimal value of the particle swarm is less than the minimum value of the individual optimal value, it will not be updated.

[0188] (3.3) Get the variance of group fitness value:

[0189] The current particle swarm fitness value variance is calculated based on the current particle swarm fitness value as follows:

[0190]

[0191] Among them, f avg is the average fitness value of the current particle swarm, and f is the adjustment factor, which is calculated as follows:

[0192]

[0193] Among them, the intermediate variable Δf max =max{|fitness(x i )-f avg |,i=1,2,…,size}

[0194] (3.4) Particle swarm iteration condition judgment:

[0195] The particle swarm optimizer is judged whether to continue iterative optimization based on the current number of iterations and the change in the optimal value of the group:

[0196] When the current number of iterations is greater than or equal to the maximum number of iterations T, the optimization ends;

[0197] The current number of iterations is less than the maximum number of iterations T, and the variance of the group fitness value σ 2 Less than or equal to the threshold σ t When , the optimization ends;

[0198] The current number of iterations is less than the maximum number of iterations T, and the variance of the group fitness value σ 2 Greater than σ t , continue with step (3.5) to enter the particle update process.

[0199] (3.5) According to the current number of iterations t and the initial inertia coefficient ω ini , terminal inertia coefficient ω end The inertia factor ω(t) of the current iteration is calculated as follows:

[0200]

[0201] (3.6) Update the speed of all particles in the particle swarm according to the current data as follows:

[0202] v i (t+1)=ω(t)×v i (t)+c1×rand×(pbest i -x i (t))+c2×rand×(gbest-x i (t))

[0203] Where i = 1, 2, ..., size represents the particle number; v i (t), v i(t+1) represents the current speed and updated speed of particle i; ω(t) represents the inertia factor under the current iteration; c1 and c2 represent the individual optimal learning factor and the global optimal learning factor respectively; rand represents a random number between 0 and 1; pbest i represents the individual optimal solution of particle i in the first t iterations; gbest represents the group optimal solution of the particle swarm in the first t iterations; x i (t) represents the current position of particle i;

[0204] (3.7) According to the updated velocity v of each particle i (t+1) and the upper and lower speed limits v max 、v min The relationship between the particle velocity out of bounds is as follows:

[0205] When v i (t+1)>v max When v i (t+1)=v max ;

[0206] When v i (t+1)<v min When v i (t+1)=v min .

[0207] (3.8) Update the positions of all particles in the particle swarm according to the current data as follows:

[0208] x i (t+1)=x i (t)+v i (t+1)

[0209] Where: i=1,2,…,size represents the particle number; v i (t+1) represents the updated velocity of particle i; x i (t), x i (t+1) represents the current position and updated position of particle i respectively;

[0210] (3.9) Particle position out-of-bounds processing:

[0211] According to the updated position x of each particle i (t+1) and the upper and lower limits of the position x min 、x max The relationship between the particle position out of bounds is as follows:

[0212] When x i (t+1)>x max When x i (t+1)=xmax ;

[0213] When x i (t+1)<x min When x i (t+1)=x min .

[0214] After completing the particle position update, return to (2.1) and continue to enter the t+1th particle swarm iteration process.

[0215] Example:

[0216] In order to verify the beneficial effects of the present invention, the present invention has carried out simulation experiments, and the simulation trajectory is as follows Figure 4 As shown in the table below, the initial conditions are as follows:

[0217] Table 5

[0218]

[0219]

[0220] Figure 5 This is the curve of the change of the optimal value of the group with the number of iterations in the simulation experiment. In the figure, after the number of iterations reaches 40, the fitness value gradually converges and remains unchanged. Figure 6-Figure 9 These are the estimated curves of the DVL scale factor error and the axis installation angle error in the simulation test. The error parameters all converge to near the true value after a short period of oscillation.

[0221] The specific estimation results are shown in the table below. The estimation results of each error parameter are close to the true value, which proves that the present invention can estimate the DVL error parameters with high accuracy.

[0222] Table 6 DVL error parameters of simulation test

[0223]

[0224] Figure 10 It is the global relative positioning error of dead reckoning before and after compensation of the estimated value of DVL error parameter. The calculation results show that after calibration, the global relative positioning error of dead reckoning is greatly reduced from 1.44% of the range to 0.09% of the range.

[0225] Through theoretical analysis and simulation experiments, it can be concluded that the present invention can calibrate the scale factor error and three installation angle errors of the DVL with high precision, and at the same time can compensate for the errors to provide the vehicle with high-precision navigation position information. It can be widely used in underwater vehicles with DVL calibration requirements. Compared with the traditional DVL calibration method, the present invention does not require a fixed navigation track, does not require complicated calibration steps, has low control requirements for the underwater vehicle, and calibration work can be performed during the normal driving process of the underwater vehicle; compared with the DVL error parameter calibration algorithm based on speed observation, the present invention adopts a calibration method based on position observation, which can greatly reduce the impact of DVL measurement noise on the calibration results, thereby improving the calibration accuracy; compared with the DVL error parameter calibration algorithm based on SINS, the present invention only uses a compass as an attitude measurement sensor, and has lower sensor load requirements for the underwater vehicle, which has great advantages in miniaturized and low-cost use environments; compared with the DVL error parameter calibration algorithm based on GNSS, the present invention uses USBL as a position measurement sensor, allowing the underwater vehicle to calibrate the DVL in deeper waters without having to frequently surface to obtain position calibration, which is more beneficial for use environments with high concealment requirements.

Claims

1. A DVL error parameter estimation method based on particle swarm optimization, characterized by The specific process of the method is: Step 1: Use DVL to obtain the velocity V of the vehicle in the DVL measurement coordinate system d (k) obtaining the vehicle attitude angle using a compass, and performing attitude calculation, velocity calculation, and position calculation in sequence using the vehicle attitude angle and the vehicle velocity in the DVL measurement coordinate system to obtain the vehicle's dead reckoning trajectory; The attitude calculation is specifically as follows: using the compass to obtain the vehicle attitude angle, and using the vehicle attitude angle to obtain the body coordinate system OX b Y b Z b To the Northeast Navigation Coordinate System OX n Y n Z n The direction cosine matrix The speed calculation is specifically as follows: the speed V of the vehicle in the DVL measurement coordinate system d (k) Converted to the velocity V in the body coordinate system b (k), and then the velocity V of the carrier in the body coordinate system b (k) with Multiply to obtain the carrier's velocity V in the northeast sky navigation coordinate system n (k); The position solution is specifically as follows: using V n (k) obtaining a dead reckoning increment of the carrier at the current moment, and using the dead reckoning increment and the dead reckoning position of the carrier at the previous moment to obtain the dead reckoning position of the carrier at the current moment, wherein the dead reckoning positions of the carrier at all moments constitute the dead reckoning trajectory of the carrier; Step 2: Use USBL and a depth gauge to obtain the longitude, latitude, and depth information of the vehicle. Combine the longitude, latitude, and depth information of the vehicle at all times to obtain the measured trajectory of the vehicle, and then obtain the error between the measured trajectory of the vehicle and the dead reckoning trajectory; Step 3: Using the DVL error parameter as the parameter to be optimized, the error between the vehicle's measured trajectory and the dead reckoning trajectory is used to construct a fitness function. The fitness function is used as the optimization indicator to perform PSO iteration and finally complete the calibration of the DVL. The error parameters of the DVL include: scale factor error, x-axis installation error angle, y-axis installation error angle, and z-axis installation error angle.

2. The DVL error parameter estimation method based on particle swarm optimization according to claim 1, characterized in that: The dead reckoning trajectory is obtained by: Step 1: Abstract the four DVL error parameter values ​​to be optimized into a particle with a dimension of 4. Multiple particles constitute a particle swarm, and then initialize the particle swarm optimizer and the particle swarm. The four DVL error parameters to be optimized include: scale factor error, x-axis installation error angle, y-axis installation error angle, and z-axis installation error angle; The particle swarm optimizer and particle swarm are initialized as follows: Step 1: Set the particle dimension dim, particle size size, maximum number of iterations T, and fitness value variance threshold σ of the particle swarm optimizer. t , individual learning factor c1, social learning factor c2, initial inertia coefficient ω ini , terminal inertia coefficient ω end , speed lower limit v min , speed limit v max , Position lower limit x min 、Position upper limit x max Initialize; Step 112: Set the initial velocity v of the particle i (0), initial position x i (0), initial individual optimal value pfitness i , initial individual optimal solution pbest i , and label all particles, then set the initial group optimal value bestf = pfitness1 and the initial group optimal solution gbest = pbest1 based on the initial individual optimal solution and initial individual optimal value of the particles; Where i is the particle number; Step 1 and 2: number all particles in the particle swarm and set the current particle number i to 0; Step 13: Add 1 to the current particle number; Step 14: According to the position x of the current particle i in the current iteration process t i (t), obtain the error parameters of DVL, and use the error parameters of DVL to obtain the DVL error parameter matrix S; x i (t)=(x i1 x i2 x i3 x i4 ) =(δk θ dx i dy i dz ) Where δk is the scale factor error of DVL, θ dx ,θ dy ,θ dz They are the installation angle errors of DVL in the x-axis, y-axis and z-axis directions, x i1 、x i2 、x i3 、x i4 is the parameter of each dimension of particle i; Step 15: Get the initial position of the carrier: P DR (0)=[L DR (0) l DR (0) hours DR (0)] T Among them, L DR (0) is the initial latitude of the carrier, λ DR (0) is the initial longitude of the vehicle, h DR (0) is the initial depth of the carrier; Step 16: Update the longitude, latitude and altitude of the navigation system at the last moment to obtain the updated vehicle position P at the last moment. DR (k-1): P DR (k-1)=[L DR (k-1) λ DR (k-1) h DR (k-1)] T Where k is the current moment; Step 17: Use the updated last moment carrier position to obtain the earth curvature R m (k), R n (k), as follows: Where e is the eccentricity of the Earth, f' is the ellipticity of the Earth, and R e is the radius of the Earth; Step 18: Use the compass to obtain the attitude angle of the vehicle Use the carrier attitude angle to obtain the body coordinate system OX b Y b Z b To the Northeast Navigation Coordinate System OX n Y n Z n The direction cosine matrix As follows: in, is the heading angle, θ is the pitch angle, and γ is the roll angle; Step 19: Use DVL to obtain the velocity of the vehicle in the DVL measurement coordinate system And use V d (k) and S obtained in step 14 are used to obtain the current speed of the vehicle in the body coordinate system As follows: V b (k)=S -1 ·V d (k) Among them, S -1 is the inverse matrix of S, is the x-direction velocity of the vehicle in the DVL measurement coordinate system, is the y-direction velocity of the vehicle in the DVL measurement coordinate system, is the z-direction velocity of the carrier in the DVL measurement coordinate system, is the x-direction velocity of the vehicle in the body coordinate system, is the y-direction velocity of the vehicle in the body coordinate system, is the z-direction velocity of the vehicle in the body coordinate system; Step 10: Use the data obtained in step 18 and V obtained in step 19 b (k) Get the carrier speed in the current Northeast Sky navigation coordinate system As follows: in, They represent the eastward velocity, northward velocity and celestial velocity of the vehicle in the northeast sky navigation coordinate system respectively; Step 11: Use the V obtained in step 10 n (k) The updated carrier position P at the last moment obtained in step 16 DR (k-1) and the earth curvature R obtained in step 17 m (k), R n (k) Get the current position increment ΔP DR (k)=[ΔL DR (k) Δλ DR (k) Δh DR (k)] T ; Where, ΔL DR (k) is the dead centre latitude increment, Δλ DR (k) is the dead longitude increment, Δh DR (k) is the dead depth increment; Step 12: According to the updated last moment carrier position P DR (k-1) and the current position increment ΔP DR (k)=[ΔL DR (k) Δλ DR (k) Δh DR (k)] T Perform dead reckoning on the carrier to obtain the current carrier position P DR (k); Step 13: Determine whether the carrier's dead reckoning has been completed at all times. If the carrier's position at all times has not been obtained, return to step 16 to enter the dead reckoning update, complete the carrier's dead reckoning at the next time, and continue until the carrier's position at all times is obtained. If the carrier's position at all times has been obtained, the carrier's position at all times constitutes the carrier's dead reckoning trajectory, and then execute step 2.

3. The DVL error parameter estimation method based on particle swarm optimization according to claim 2, characterized in that: The current position increment ΔP DR (k)=[ΔL DR (k) Δλ DR (k) Δh DR (k)] T Obtained by the following formula: ΔP DR (k)=T s ·T V (P DR (k-1))·V n (k) Among them, T s is the system period of dead reckoning, T V (P DR (k-1)) is the position increment transformation matrix based on the previous moment.

4. The DVL error parameter estimation method based on particle swarm optimization according to claim 3, characterized in that: The current carrier position P DR (k), is obtained by the following formula: P DR (k)=ΔP DR (k)+P DR (k-1)。 5. The DVL error parameter estimation method based on particle swarm optimization according to claim 4, characterized in that: The error between the measured trajectory and the dead reckoning trajectory of the vehicle in step 2 includes the following steps: Step 2. Use USBL and depth meter to obtain the longitude, latitude and depth information of the vehicle, combine the longitude, latitude and depth information of the vehicle at all times, and obtain the measurement trajectory P of the vehicle. Mea (k)=[L USBL (k) λ USBL (k) h DEP (k)] T ,k=1,2,…,N; Among them, L USBL (k) is the latitude of the carrier measured by USBL, λ USBL (k) is the longitude of the carrier measured by USBL, h DEP (k) is the depth of the vehicle measured by the depth gauge; Step 2: Use dead reckoning trajectory P DR (k)=[L DR (k) λ DR (k) h DR (k)] T ,k=1,2,…,N and P Mea (k)=[L USBL (k) λ USBL (k) h DEP (k)] T , k=1,2,…,N, and obtain the error between the measured trajectory and the dead reckoning trajectory of the vehicle ε(k)=[ε L (k) ε λ (k) ε h (k)] T ,k=1,2,…,N, as follows: e L (k)=R e ·[L USBL (k)-L DR (k)] e λ (k)=R e ·cos(L USBL (k))·[λ USBL (k)-l DR (k)] ε h (k)=h DEP (k)-h DR (k) Among them, ε L (k) is the latitude error, ε λ (k) is the longitude error, ε h (k) is the depth error and N is the total time length.

6. The DVL error parameter estimation method based on particle swarm optimization according to claim 5, characterized in that: In step 3, the error parameters of the DVL are used as the parameters to be optimized, and the error between the measured trajectory and the dead reckoning trajectory of the vehicle is used to construct a fitness function. The fitness function is used as the optimization indicator to perform PSO iteration and finally complete the calibration of the DVL. The following steps are included: Step 3. Obtain the fitness value of the current particle fitness(x according to ε(k) obtained in step 2 i ); Step 32: Determine whether all particles in the particle swarm have completed fitness value calculation. If all particles have completed fitness value calculation, proceed to step 33. If not, return to step 13. Step 3. Set the current particle fitness value fitness(x i ) and the current particle individual optimal value pfitness i For comparison, when fitness(x i )≤pfitness i When , the optimal value pfitness of the current particle is updated i =fitness(x i ), and at the same time update the individual optimal solution of the current particle to the current particle position pbest i =x i ; If fitness(x i )>pfitness i , then it will not be updated; Step 3 and 4: Set the optimal value of the particle swarm individual pfitness = [pfitness1 pfitness2 … pfitness size ] the minimum value min[pfitness1 pfitness2 … pfitness size ] is compared with the current particle swarm's optimal value bestf. If min[pfitness1 pfitness2 … pfitness size ]≤bestf, then update the optimal value of the current particle swarm to bestf=min[pfitness1 pfitness2 … pfitness size ], gbest = x i ;If min[pfitness1 pfitness2 … pfitness size ]>bestf is not updated; Step 35: Obtain the current particle swarm fitness value variance σ based on the current particle fitness value 2 ; Step 36: Get the current number of iterations t. If the current number of iterations is greater than or equal to the maximum number of iterations T, then the optimization ends. If the current number of iterations is less than the maximum number of iterations T, and σ 2 Less than or equal to the threshold σ t , the optimization ends; if the current number of iterations is less than the maximum number of iterations T, and σ 2 Greater than σ t , then execute step 37; Step 37: Use the current iteration number to obtain the inertia factor ω(t) of the current iteration; Step 38: Use the inertia factor ω(t) of the current iteration to update the velocity v of all particles in the particle swarm. i (t+1): v i (t+1)=ω(t)×v i (t)+c1×rand×(pbest i -x i (t))+c2×rand×(gbest-x i (t)) Where i = 1, 2, ..., size is the particle number; v i (t), v i (t+1) represents the current speed and updated speed of particle i respectively; c1 and c2 represent the individual optimal learning factor and the global optimal learning factor respectively; rand represents a random number between 0 and 1; pbest i represents the individual optimal solution of particle i in the first t iterations; gbest represents the group optimal solution of the particle swarm in the first t iterations; x i (t) represents the current position of particle i; Step 39: Use the v obtained in step 38 i (t+1) Process the particle velocity out of bounds: When v i (t+1)>v max When v i (t+1)=v max When v i (t+1)<v min When v i (t+1)=v min Among them, v min is the lower speed limit, v max It is the speed limit; Step 30: Use the v obtained in step 39 i (t+1) Update the positions of all particles in the particle swarm: x i (t+1)=x i (t)+v i (t+1) Among them, x i (t), x i (t+1) represents the current position and updated position of particle i respectively; Step 31: Use all updated particle positions obtained in step 30 to perform particle position out-of-bounds processing: When x i (t+1)>x max When x i (t+1)=x max When x i (t+1)<x min When x i (t+1)=x min Among them, x min is the lower limit of the position, x max is the upper limit of position; Step 32: After the particle position out-of-bounds processing is completed, return to step 2 to perform the t+1th particle swarm iteration.

7. The DVL error parameter estimation method based on particle swarm optimization according to claim 6, characterized in that: In step 3, the fitness value of the current particle is obtained according to ε(k) obtained in step 2. i ), as follows:

8. The DVL error parameter estimation method based on particle swarm optimization according to claim 7, characterized in that: The fitness value variance σ of the current particle swarm is obtained according to the fitness value of the current particle in steps 3 and 5. 2 , as follows: Among them, f avg is the average fitness value of the current particle swarm, and f is the adjustment factor.

9. The DVL error parameter estimation method based on particle swarm optimization according to claim 8, characterized in that: Δf max =max{|fitness(x i )-f avg |,i=1,2,…,size} Where Δf max is an intermediate variable.

10. The DVL error parameter estimation method based on particle swarm optimization according to claim 9, characterized in that: In step 37, the inertia factor ω(t) of the current iteration is obtained by using the current iteration number, as shown in the following formula: Among them, ω ini is the initial inertia coefficient, ω end is the terminal inertia coefficient.

Images