A multi-state estimation cooperative positioning method for AUV based on MEMS / acoustic ranging

Abstract

The application discloses an AUV multi-state estimation cooperative positioning method based on MEMS / acoustic ranging. Step 1: a multi-state estimation equation of an AUV cooperative system is established; step 2: a measurement equation of the AUV cooperative system is established, and linearization processing is carried out by adopting a first-order Taylor expansion; step 3: based on the multi-state estimation equation of step 1 and the measurement equation of step 2, a Kalman filtering algorithm is adopted to carry out filtering update on state information of the system. The cooperative positioning scheme provided by the application can greatly reduce the cost of the cooperative system, and meanwhile, the multi-state quantity estimation of the following AUV is realized through acoustic ranging information, so that the positioning precision is greatly improved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of autonomous underwater vehicles, and particularly relates to an AUV multi-state estimation cooperative positioning method based on MEMS / acoustic ranging. BACKGROUND

[0002] With the development of AUV technology, a cooperative system composed of multiple AUVs has attracted wide attention in the military and commercial fields. The AUV cooperative system can fully exert the overall resource advantage, reduce the difficulty of task execution, and improve the task completion efficiency. High-precision positioning capability in complex environments is the basis for the AUV cooperative system to complete tasks. In addition, the high cost-effectiveness requirement in the military application field promotes the miniaturization and low-cost development of the AUV cooperative system. With the maturity of MEMS technology, the high-precision cooperative positioning technology based on low-cost MEMS has important development prospects.

[0003] The cooperative positioning system currently studied by many scholars is mainly of the lead-follow type. A typical lead-follow system includes two lead AUVs capable of high-precision positioning and one follower AUV. The follower AUV is equipped with a compass providing a heading and a Doppler velocity log (DVL) providing a speed. Then, the distance between the lead AUV and the follower AUV and the position information of the lead AUV are shared through acoustic communication technology to improve the positioning accuracy of the follower AUV through information fusion. In this system, the compass and the DVL are relatively large in size and expensive. Meanwhile, the errors in the output heading and speed information will seriously affect the positioning performance of the follower AUV. SUMMARY

[0004] The application provides an AUV multi-state estimation cooperative positioning method based on MEMS / acoustic ranging, which aims to solve the problem that the compass and the DVL are relatively large in size and expensive in the prior art, and the errors in the output heading and speed information will seriously affect the positioning performance of the follower AUV.

[0005] The application achieves the above-mentioned technical effects through the following technical scheme.

[0006] An AUV multi-state estimation cooperative positioning method based on MEMS / acoustic ranging, which comprises the following steps:

[0007] Step 1: establishing an AUV multi-state estimation equation of the cooperative system;

[0008] Step 2: establishing an AUV measurement equation of the cooperative system, and performing linearization processing by using first-order Taylor expansion;

[0009] Step 3: Based on the multi-state estimation equation of step 1 and the measurement equation of step 2, the Kalman filtering algorithm is used to filter and update the state information of the system.

[0010] An AUV multi-state estimation cooperative positioning method based on MEMS / acoustic ranging, wherein step 1 is specifically using the three-direction attitude errors, velocity errors, position errors of the follower AUV and the three-axis gyro drift and three-axis acceleration zero offset of the MEMS as the estimation state quantity X of the follower AUV in the cooperative system.

[0011] The state quantity X is expressed as follows:

[0012] X = [ φ E φ N φ U δv E δv N δv U δL δλ δh ε E ε N ε U ▽ E ▽ N ▽ U ] T

[0013] Wherein, φ E , φ N , φ U respectively represent the east misalignment angle, the north misalignment angle, and the sky misalignment angle of the follower AUV, δv E , δv N , δv U respectively represent the east velocity error, the north velocity error, and the sky velocity error, δL, δλ, and δh respectively represent the latitude error, the longitude error, and the height error, ε E , ε N , ε U respectively represent the east gyro drift, the north gyro drift, and the sky gyro drift of the MEMS, ▽ E , ▽ N , ▽ U respectively represent the east accelerometer zero offset, the north accelerometer zero offset, and the sky accelerometer zero offset of the MEMS.

[0014] An AUV multi-state estimation cooperative positioning method based on MEMS / acoustic ranging, wherein the attitude error equation is:

[0015]

[0016] Wherein, φ = [ φ E φ N φ U ] T , denotes the angular velocity of the navigation frame relative to the inertial frame, denotes the projection of the angular velocity of the carrier frame relative to the inertial frame in the navigation frame.

[0017] wherein, denotes the differential of the variable N, and δN denotes the error amount of the variable N.

[0018] A multi-state estimation cooperative positioning method of an AUV based on MEMS / acoustic ranging, the velocity error equation is:

[0019]

[0020] wherein, v n = [v E v N v U ] T denotes the velocity, f b denotes the specific force output of the MEMS, denotes the conversion matrix of the carrier frame to the navigation frame, denotes the projection of the angular velocity of the earth frame relative to the inertial frame in the navigation frame, denotes the projection of the angular velocity of the navigation frame relative to the earth frame in the navigation frame, g n denotes the projection of the gravity vector in the navigation frame.

[0021] A multi-state estimation cooperative positioning method of an AUV based on MEMS / acoustic ranging, the position error equation is:

[0022]

[0023] wherein, p = [L λ h] T denotes the latitude, longitude, and height,

[0024]

[0025]

[0026] wherein, R M denotes the meridian radius, R N denotes the prime vertical radius

[0027] A multi-state estimation cooperative positioning method of an AUV based on MEMS / acoustic ranging, specifically, in the step 2, the pseudo-range measurement equation between the leading AUV and the following AUV is established by using the first-order Taylor expansion for linearization processing, and the relationship between the measurement and the estimation state is established by using the first-order Taylor expansion for linearization processing.

[0028] A multi-state estimation cooperative positioning method of AUV based on MEMS / acoustic ranging, the pseudo-range measurement equation between the leading AUV and the following AUV is:

[0029]

[0030] Wherein, ρ SINS represents the pseudo-range calculated according to the position of the following AUV inertial navigation solution and the position information of the leading AUV, ρ meas represents the ranging information output by the hydrophone, respectively represent the position coordinates of the leading AUV, the inertial navigation solution position coordinates of the following AUV and the true position coordinates of the following AUV in the navigation coordinate system with the initial position of the following AUV as the origin, the x-axis pointing east, the y-axis pointing north and the z-axis pointing sky.

[0031] A multi-state estimation cooperative positioning method of AUV based on MEMS / acoustic ranging, the pseudo-range measurement equation is linearized by first-order Taylor expansion, which is specifically,

[0032]

[0033] Wherein,

[0034]

[0035]

[0036]

[0037] Wherein, (L, λ, h) represents the latitude, longitude and height of the position of the following AUV, R N represents the radius of the prime vertical circle, and β represents the eccentricity of the ellipse;

[0038] The pseudo-range measurement equation is represented as:

[0039] Z=HX+w ρ

[0040] Wherein, w ρ represents the measurement noise;

[0041] The measurement equation of the cooperative system AUV is,

[0042]

[0043] A multi-state estimation cooperative positioning system of AUV based on MEMS / acoustic ranging, the system comprises a state estimation unit, a measurement unit and an updating unit

[0044] The state estimation unit: establishes the multi-state estimation equation of the cooperative system AUV;

[0045] The measurement unit: establish the cooperative system AUV measurement equation, and adopt the first order Taylor expansion to linearize processing;

[0046] The updating unit, based on the equation of state estimation unit and the equation of measurement unit, adopts Kalman filtering algorithm to filter and update the state information of the system.

[0047] The beneficial effects of the present application are:

[0048] The present application only relies on MEMS as the navigation source, and through the use of the water acoustic ranging information between the leading AUV and the following AUV, the attitude, speed, position of the following AUV and the estimation of the gyro drift and acceleration zero offset of the MEMS are realized, and through feedback correction, the positioning ability of the following AUV is greatly improved

[0049] The cooperative positioning scheme can greatly reduce the cost of the cooperative system, and through the water acoustic ranging information, the estimation of multiple state quantities of the following AUV is realized, and the positioning accuracy is greatly improved.

[0050] The present application is a cooperative system in which the following AUV only uses a micro-electro-mechanical (MEMS) gyroscope as a navigation source, and through the use of the water acoustic ranging information between the leading AUV and the following AUV, the estimation of the attitude, speed, position of the following AUV in three directions and the three-axis gyro drift and three-axis acceleration zero offset of the MEMS are realized, and through feedback correction, the positioning ability of the following AUV is greatly improved, and the development of the cooperative system to miniaturization and low cost is promoted. BRIEF DESCRIPTION OF DRAWINGS

[0051] Figure 1 It is the motion trajectory graph of the leading AUV and the following AUV in the simulation experiment of the present application.

[0052] Figure 2 It is the ranging error in the simulation experiment of the present application.

[0053] Figure 3 It is the heading, speed and corresponding error output by the compass and DVL in the simulation experiment of the present application.

[0054] Figure 4 It is the positioning error of different algorithms in the simulation experiment of the present application. DETAILED DESCRIPTION

[0055] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative effort belong to the scope of protection of the present application.

[0056] Meanwhile, in order to meet the miniaturization and low cost requirements of the AUV cooperative system, a new cooperative positioning scheme based on MEMS / underwater acoustic ranging is designed, and a new multi-state estimation cooperative positioning method based on ranging information is proposed. In the system, the leading AUV is equipped with a high-precision fiber-optic inertial navigation system and a DVL, and high-precision positioning is realized through information fusion. The following AUV uses MEMS for position estimation. Since the gyro performance of MEMS is poor, it will greatly affect the positioning performance of the following AUV, so underwater acoustic ranging equipment is used to measure the distance between the leading AUV and the following AUV and to quickly transfer information. In the cooperative positioning method proposed in the present application, the attitude, velocity and position of the following AUV in three directions, the three-axis gyro drift and three-axis accelerometer bias of the MEMS are selected as state variables, and the ranging information obtained through underwater acoustic ranging is selected as measurement. Through the conversion of the geographic coordinate system, the geocentric rectangular coordinate system and the navigation coordinate system, the measurement equation of the system is derived, the state variable estimation based on Kalman filtering is realized, and the positioning performance of the following AUV is greatly improved through feedback correction.

[0057] An AUV multi-state estimation cooperative positioning method based on MEMS / underwater acoustic ranging, the AUV multi-state estimation cooperative positioning method comprising the following steps:

[0058] Step 1: establishing an AUV multi-state estimation equation of the cooperative system;

[0059] Step 2: establishing an AUV measurement equation of the cooperative system, and performing linearization processing by using first-order Taylor expansion;

[0060] Step 3: based on the multi-state estimation equation of step 1 and the measurement equation of step 2, using a Kalman filtering algorithm to filter and update the state information of the system.

[0061] An AUV multi-state estimation cooperative positioning method based on MEMS / underwater acoustic ranging, the step 1 of establishing an AUV multi-state estimation equation of the cooperative system specifically comprises: based on the observable state variables, using the attitude error, velocity error and position error of the following AUV in three directions, and the three-axis gyro drift and three-axis accelerometer bias of the MEMS as the estimation state variables X of the following AUV in the cooperative system.

[0062] The estimated state quantity X includes 15 dimensions of attitude error, velocity error, position error, three-axis gyro drift of MEMS, three-axis acceleration zero offset, etc.

[0063] A multi-state estimation cooperative positioning method of AUV based on MEMS / acoustic ranging, the state quantity X is expressed as follows:

[0064] X = [ φ E φ N φ U δv E δv N δv U δL δ λ δ h ε E ε N ε U ▽ E ▽ N ▽ U ] T

[0065] Wherein, φ E , φ N , φ U respectively represent the east misalignment angle, the north misalignment angle, the sky misalignment angle of the following AUV, δv E , δv N , δv U respectively represent the east velocity error, the north velocity error, the sky velocity error, δL, δ λ, δ h respectively represent the latitude error, the longitude error, the height error, ε E , ε N , ε U respectively represent the east gyro drift, the north gyro drift, the sky gyro drift of MEMS, ▽ E , ▽ N , ▽ U respectively represent the east accelerometer zero offset, the north accelerometer zero offset, the sky accelerometer zero offset of MEMS.

[0066] A multi-state estimation cooperative positioning method of AUV based on MEMS / acoustic ranging, the attitude error equation is:

[0067]

[0068] Wherein, φ = [ φ E φ N φ U ] T , represents the angular velocity of the navigation coordinate system relative to the inertial coordinate system, represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system in the navigation coordinate system.

[0069] Wherein, The differential of variable N, and δN represents the error of variable N.

[0070] A multi-state estimation cooperative positioning method of AUV based on MEMS / acoustic ranging, the velocity error equation is:

[0071]

[0072] Wherein, v n = [v E v N v U ] T The velocity, f b The specific force output of MEMS, The transformation matrix from the carrier coordinate system to the navigation coordinate system, The projection of the angular velocity of the earth system relative to the inertial system in the navigation system, The projection of the angular velocity of the navigation system relative to the earth system in the navigation system, g n The projection of the gravity vector in the navigation system.

[0073] A multi-state estimation cooperative positioning method of AUV based on MEMS / acoustic ranging, the position error equation is:

[0074]

[0075] Wherein, p = [Lλh] T The latitude, longitude, and altitude,

[0076]

[0077]

[0078] Wherein, R M The meridian radius, R N The prime vertical circle radius

[0079] A multi-state estimation cooperative positioning method of AUV based on MEMS / acoustic ranging, the step 2 establishes the measurement equation of the cooperative system AUV, specifically, the pseudo-range measurement equation between the leading AUV and the following AUV is established by using the first-order Taylor expansion for linearization processing, and the relationship between the measurement and the estimation state is established by using the first-order Taylor expansion for linearization processing.

[0080] The distance obtained by the acoustic ranging equipment is used as the observation. The navigation coordinate system is established with the initial position of the following AUV as the origin, the x-axis points to the east, the y-axis points to the north, and the z-axis points to the sky. The position coordinates of the leading AUV are The position coordinates of the following AUV inertial navigation solution are Real position coordinates of the follower AUV ρ SINS represents the pseudo-range calculated according to the position calculated by the inertial navigation of the follower AUV and the position information of the leader AUV, ρ meas represents the ranging information output by the hydrophone. Without considering the influence of noise, it is:

[0081]

[0082]

[0083] An AUV multi-state estimation cooperative positioning method based on MEMS / acoustic ranging, the pseudo-range measurement equation between the leader AUV and the follower AUV is:

[0084]

[0085] wherein, ρ SINS represents the pseudo-range calculated according to the position calculated by the inertial navigation of the follower AUV and the position information of the leader AUV, ρ meas represents the ranging information output by the hydrophone,

[0086]

[0087] [δx n δy n δz n ] T is the position error output by the SINS respectively represent the position coordinates of the leader AUV, the position coordinates calculated by the inertial navigation of the follower AUV, and the real position coordinates of the follower AUV in the navigation coordinate system with the initial position of the follower AUV as the origin, the x-axis pointing east, the y-axis pointing north, and the z-axis pointing sky.

[0088] An AUV multi-state estimation cooperative positioning method based on MEMS / acoustic ranging, the pseudo-range measurement equation is linearized by using first-order Taylor expansion, which is specifically,

[0089] The first-order Taylor expansion is performed on ρ mems at to obtain:

[0090]

[0091] Let

[0092] The mutual conversion relationship between the geographic coordinates (λ, L, h) and the geocentric rectangular coordinates (x e , y e , z e ) at the same location is as follows:

[0093]

[0094] Wherein, β represents the eccentricity of the ellipse.

[0095] Two sides of the differential can be obtained:

[0096]

[0097] The conversion relationship between the geocentric rectangular coordinates (x e ,y e ,z e ) and the navigation system coordinates (x n ,y n ,z n ) is:

[0098]

[0099] Therefore, the pseudo-range measurement equation can be expressed as:

[0100]

[0101] Therefore, the pseudo-range measurement equation can be expressed as:

[0102] Z=HX+w ρ

[0103] Wherein, w ρ represents the measurement noise;

[0104] The measurement equation of the cooperative system AUV is,

[0105]

[0106] A multi-state estimation cooperative positioning system of AUV based on MEMS / acoustic ranging, the system comprises a state estimation unit, a measurement unit and an updating unit

[0107] The state estimation unit: the multi-state estimation equation of the cooperative system AUV is established;

[0108] The measurement unit: the measurement equation of the cooperative system AUV is established, and linearization processing is carried out by using the first-order Taylor expansion;

[0109] The updating unit, based on the equation of the state estimation unit and the equation of the measurement unit, the state information of the system is updated by using the Kalman filtering algorithm.

[0110] In order to verify the effectiveness of the application, a kind of multi-state estimation cooperative positioning method of AUV based on MEMS / acoustic ranging is simulated by using software.

[0111] The simulation conditions are as follows: there are two pilot AUVs and one follower AUV. The starting points of the two pilot AUVs are (200m, -200m) and (-200m, -200m) respectively, and the starting point of the follower AUV is (0m, 0m). The actual speed, heading, and trajectory of the three AUVs are shown in the attached figure. Figure 1 In the simulation, the gyro drift of the MEMS following the AUV is set to 5° / h, the accelerometer zero bias is set to 100μg, the underwater acoustic ranging noise is set to zero-mean white noise with a standard deviation of 3m and a Gaussian distribution, and the simulation time is set to 1800s. The ranging error in the simulation is shown in the attached figure. Figure 2 shown.

[0112] At the same time, in order to compare and verify the superiority of the method proposed in this invention, the simulation also analyzes the scheme of using the compass and DVL to locate the following AUV in the traditional multi-AUV cooperative system. In the simulation, the compass index is 1°, and the heading error is set to meet the normal distribution with a mean of 1° and a standard deviation of 1°. The DVL speed measurement index is 0.1%, and the speed error is set to meet the normal distribution with a mean of 0.003m / s and a standard deviation of 0.001m / s. The heading error and speed error in the simulation are shown in the attached figure. Figure 3 shown.

[0113] Attachment Figure 4 is the positioning error estimated using different collaborative positioning methods, where method one adopts the traditional collaborative scheme of following AUV equipped with a compass and DVL and uses the extended Kalman filter algorithm for collaborative positioning; method two adopts the collaborative scheme based on MEMS / underwater acoustic ranging proposed in this invention but uses the traditional dynamic model, with the state quantity being the position of the following AUV, and uses the extended Kalman filter for collaborative positioning; method three adopts the collaborative scheme based on MEMS / underwater acoustic ranging proposed in this invention and the AUV multi-state estimation collaborative positioning method proposed in this invention.

[0114] As can be seen from the figure, due to the heading error and speed error of the compass and DVL, the positioning error estimated by method one continues to increase and diverges over time; while the state quantity dimension in method two is relatively small, and the gyro drift and accelerometer bias of the MEMS cannot be estimated. Due to the existence of MEMS inertial navigation error, its positioning error continues to increase. Although the positioning error decreases when turning, the positioning error generally converges over time; method three is the method proposed in this invention, which uses underwater acoustic ranging information to realize the attitude, speed, position in three directions of the following AUV, as well as the three-axis gyro drift and acceleration bias of the MEMS. Its positioning effect is the best, and the positioning error does not diverge over time.

Claims

1. A multi-state estimation collaborative positioning method for AUV based on MEMS / underwater acoustic ranging, characterized by: The AUV multi-state estimation collaborative positioning method comprises the following steps: Step 1: Establish the cooperative system AUV multi-state estimation equation; Step 2: Establish the AUV measurement equation of the collaborative system and perform linearization using first-order Taylor expansion; Step 3: Based on the multi-state estimation equation in step 1 and the measurement equation in step 2, the Kalman filter algorithm is used to filter and update the system state information; The step 1 is specifically as follows: The attitude error, velocity error, position error in the three directions of the following AUV, as well as the three-axis gyroscope drift and three-axis acceleration zero bias of the MEMS are used as the estimated state quantities of the following AUV in the collaborative system. ; State quantity It is expressed as follows: in, 、 、 They represent the eastward misalignment angle, northward misalignment angle, and skyward misalignment angle of the following AUV respectively. 、 、 They represent the eastward velocity error, northward velocity error, and celestial velocity error respectively. 、 、 Represent latitude error, longitude error, and altitude error respectively. 、 、 They represent the eastward gyro drift, northward gyro drift, and skyward gyro drift of MEMS respectively. 、 、 They represent the MEMS accelerometer bias in the east, north, and sky directions, respectively. Specifically, step 2 includes establishing a pseudorange measurement equation between the lead AUV and the follower AUV by performing linearization processing using a first-order Taylor expansion, and establishing a relationship between the measured and estimated states by performing linearization processing using a first-order Taylor expansion; The pseudo-range measurement equation between the pilot AUV and the following AUV is: in, It represents the pseudo-range calculated based on the position calculated by the inertial navigation of the following AUV and the position information of the leading AUV. represents the ranging information output by the hydrophone, 、 、 They represent the initial position of the following AUV as the origin, x The axis points east, y The axis points north, z The position coordinates of the pilot AUV in the navigation coordinate system with the axis pointing to the sky, the position coordinates calculated by the inertial navigation of the following AUV, and the real position coordinates of the following AUV.

2. According to claim 1, a multi-state estimation collaborative positioning method for AUV based on MEMS / underwater acoustic ranging is characterized by: Attitude error equation: in, , represents the angular velocity of the navigation coordinate system relative to the inertial coordinate system, Represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system in the navigation coordinate system; in, Representing variables N The differential of Representing variables N The amount of error.

3. According to claim 2, a method for AUV multi-state estimation collaborative positioning based on MEMS / underwater acoustic ranging is characterized in that: Velocity error equation: in, Indicates speed, , Indicates the specific force output of MEMS, Represents the transformation matrix from the carrier coordinate system to the navigation coordinate system, It represents the projection of the angular velocity of the Earth system relative to the inertial system in the navigation system. It represents the projection of the angular velocity of the navigation system relative to the Earth system in the navigation system, Represents the projection of the gravity vector in the navigation system.

4. According to claim 3, a method for AUV multi-state estimation collaborative positioning based on MEMS / underwater acoustic ranging is characterized in that: Position error equation: in, Indicates latitude, longitude, and altitude. in, represents the meridian radius, Represents the radius of the Maoyou circle.

5. According to claim 1, a multi-state estimation collaborative positioning method for AUV based on MEMS / underwater acoustic ranging is characterized by: The pseudo-range measurement equation is linearized using the first-order Taylor expansion as follows: in, in, Indicates the latitude, longitude, and altitude of the AUV. represents the radius of the Maoyou circle, represents the eccentricity of the ellipse; The pseudorange measurement equation is expressed as: in, represents the measurement noise; The AUV measurement equation of the collaborative system is: 。 6. An AUV multi-state estimation collaborative positioning system based on MEMS / underwater acoustic ranging, characterized by: The system includes a state estimation unit, a measurement unit and an update unit; State estimation unit: establish the multi-state estimation equation of the collaborative system AUV; Measurement unit: Establish the AUV measurement equation of the collaborative system and use the first-order Taylor expansion for linearization; The updating unit uses the Kalman filter algorithm to filter and update the state information of the system based on the equations of the state estimation unit and the measurement unit; The state estimation unit is specifically: The attitude error, velocity error, position error in the three directions of the following AUV, as well as the three-axis gyroscope drift and three-axis acceleration zero bias of the MEMS are used as the estimated state quantities of the following AUV in the collaborative system. ; State quantity It is expressed as follows: in, 、 、 They represent the eastward misalignment angle, northward misalignment angle, and skyward misalignment angle of the following AUV respectively. 、 、 They represent the eastward velocity error, northward velocity error, and celestial velocity error respectively. 、 、 Represent latitude error, longitude error, and altitude error respectively. 、 、 They represent the eastward gyro drift, northward gyro drift, and skyward gyro drift of MEMS respectively. 、 、 They represent the MEMS accelerometer bias in the east, north, and sky directions, respectively. Specifically, the measurement unit uses a first-order Taylor expansion for linearization processing to establish a pseudo-range measurement equation between the lead AUV and the follower AUV, and uses a first-order Taylor expansion for linearization processing to establish a relationship between the measurement and the estimated state; The pseudo-range measurement equation between the pilot AUV and the following AUV is: in, It represents the pseudo-range calculated based on the position calculated by the inertial navigation of the following AUV and the position information of the leading AUV. represents the ranging information output by the hydrophone, 、 、 They represent the initial position of the following AUV as the origin, x The axis points east, y The axis points north, z The position coordinates of the pilot AUV in the navigation coordinate system with the axis pointing to the sky, the position coordinates calculated by the inertial navigation of the following AUV, and the real position coordinates of the following AUV.

Images