Underwater vehicle cooperative navigation and positioning method based on companion acoustic beacon assistance

Abstract

The present application relates to a method for cooperative navigation and positioning of underwater vehicles based on USBL aided navigation. The method comprises: the USV is equipped with RTK GPS and USBL positioning system, and the UUV is synchronously tracked and accompanied; the error equation of the strapdown inertial navigation system is established; the UUV obtains Doppler velocity information; the UUV obtains the USBL positioning information of the USV, and the measurement equation of the integrated navigation system is established; the adaptive variance limited sequential Kalman filtering algorithm is used for data fusion; and the cooperative positioning experiment platform is built, and the verification is carried out in the lake state experiment. The USBL positioning device is fixed on the USV for accompanying aided positioning, the UUV is equipped with SINS / DVL / USBL for integrated navigation, and the operation accuracy of the UUV navigation system is significantly improved.

Description

Technical Field

[0001] This invention belongs to the technical field of underwater vehicle navigation and positioning methods, and particularly relates to a cooperative navigation and positioning method for underwater vehicles based on accompanying acoustic beacons. Background Technology

[0002] Unmanned Underwater Vehicles (UUVs) are devices that navigate underwater without human drivers, relying on remote or autonomous control. They can replace divers in performing dangerous tasks such as exploration, rescue, mine clearance, and cable laying, possessing significant advantages and playing an important role in future ocean exploration. Therefore, research on related technologies for UUVs has great application prospects and strategic value.

[0003] In UUV underwater navigation, the combination of acoustic navigation and inertial navigation is a relatively effective method. Acoustic navigation helps to eliminate the cumulative error of inertial navigation and achieve high-precision underwater navigation and positioning.

[0004] Doppler velocity logs (DVL) are currently one of the most important navigational methods for measuring the speed of maritime vessels. Based on the Doppler effect, they can provide accurate velocity measurements over long periods of time and complement strapdown inertial navigation systems, thus finding wide application in various fields. In this device, by emitting waves (usually microwaves or lasers) and receiving the reflected waves, changes in frequency can be measured to calculate the velocity of an object.

[0005] Ultra-Short Baseline (USBL) positioning is an underwater acoustic positioning technology widely used in offshore oil exploration and development, marine salvage, and other fields. It is primarily used to determine the precise location of ROVs, AUVs, divers, and other underwater vehicles. An USBL system consists of a transmitting transducer, a transponder, and a receiving array. The transmitting transducer and receiving array are mounted on a ship, while the transponder is fixed to the UUV. The transmitting transducer emits an acoustic pulse, which the transponder receives and transmits back. The receiving array receives this pulse, measures the phase difference between array directions, and calculates the distance R from the underwater device to the array based on the arrival time of the sound wave. This allows the calculation of the UUV's position in the array coordinate system.

[0006] Currently, the main methods that can be used for high-precision underwater navigation and positioning of UUVs are acoustic methods, including:

[0007] (1) Acoustic positioning method based on inertial navigation and Doppler. Inertial navigation systems (INS) calculate the motion state of underwater targets, including position, velocity, and attitude, by measuring information such as acceleration and angular velocity. Doppler sensors emit acoustic signals, which propagate underwater. When the acoustic waves interact with the underwater target and return to the sensor, the relative velocity information of the underwater target is obtained by measuring the Doppler frequency shift. The velocity information provided by the INS and the velocity information measured by the Doppler frequency shift are fused to obtain a more accurate underwater target velocity. The position information provided by the INS is combined with the fused velocity information to calculate the position of the underwater target. The fused velocity and position information are combined with time to deduce the displacement of the underwater target, thus achieving target positioning. However, based on the principle of integrated navigation, this method cannot eliminate the initial position error, and the position error will exhibit uncertain position divergence as time accumulates.

[0008] (2) Long-baseline acoustic positioning method based on multiple fixed beacons. This method requires the pre-positioning of 3-4 beacons on the seabed and the determination of their accurate positions. During use, the revised beacon positions are stored in the UUV. By measuring the distance between the UUV and each beacon in real time, the system calculates the position of the UUV relative to each beacon. However, this method is limited in that the UUV can only be positioned within a limited area covered by the beacons, and the calibration and deployment of the beacons is relatively cumbersome. Summary of the Invention

[0009] To overcome the aforementioned shortcomings of existing methods, this invention proposes a novel cooperative navigation and positioning method for underwater vehicles based on accompanying acoustic beacon assistance. This method fixes a USBL positioning device on a USV (Unmanned Surface Vehicle) for accompanying assisted positioning. The UUV is equipped with SINS / DVL / USBL OEM for combined navigation, which significantly improves the accuracy of the UUV navigation system.

[0010] Specifically, this invention provides a cooperative navigation and positioning method for underwater vehicles based on accompanying acoustic beacon assistance, the method comprising:

[0011] S1. Confirm the location of USBL and USBL OEM to ensure that they meet the transmitter directionality and coverage of USBL;

[0012] S2. Place the UUV below the USV, with a distance of D between the USBL and USBL OEM devices, and an angle θ between the line connecting the two devices and the vertical direction of the USBL installation, while satisfying the following acoustic constraints:

[0013]

[0014] S3. Under this constraint, the USV performs real-time tracking and accompaniment based on the UUV's position, using a line-of-sight-based position tracking PID algorithm;

[0015] S4. Mount the USBL at the center of the USV. Convert the relative position vector of the UUV returned by the USBL to the local navigation coordinate system as (x... P y P z P ),at this time:

[0016]

[0017] S5. When the USV tracks the UUV on the water surface, the tracking point is set as the projection point of the UUV on the water surface, so that the relative distance between the two is kept within the positioning constraint range.

[0018] S6. Let If the heading is the center of the USV where the UUV is located, then:

[0019]

[0020] S7. Calculate the heading angle error between the two. Tracking distance error Δp(t):

[0021]

[0022] In the formula, the USV's own heading angle is p xymin This is the minimum safe distance for horizontal distance;

[0023] S8. Maintain the relative position of the USV by adjusting its rudder angle control output α(t) and speed output β(t). The control commands are as follows:

[0024]

[0025] In the formula, the proportionality coefficient is k p The integral coefficient is k i =k p T / T i The differential coefficient is k d =k p T / T d The integral period and the differential period are T, respectively. i ,T d T is the system sampling period.

[0026] Furthermore, step S1 of the underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance of the present invention includes:

[0027] S11. The complete error state vector of a strapdown inertial navigation system is expressed as follows:

[0028]

[0029] In the formula, Let δv be the attitude error vector, and δv = [δv E δv N δv U ] represents the velocity error vector, δp=[δL δλ δh] represents the position error vector, ε=[ε x ε y ε z [ ] represents the gyroscope drift error vector. This is the accelerometer drift error vector;

[0030] S12. The state equation of the integrated navigation system based on SINS / DVL / USBL consists of the error state of the inertial navigation system. The error state equation of the inertial navigation system is expressed as:

[0031]

[0032] In the formula, F(t) is the state transition matrix. Where F a ∈R 9×9 The system matrix represents the error parameters of the strapdown inertial navigation system, and it is a 9th-order square matrix; G(t) is the system noise matrix, which is taken as the identity matrix here; W(t) = [0 1×9 ω gx ω gy ω gz ω ax ω ax ω ax ] T Represents the noise of the modeled inertial system, where [ω gx ω gy ω gz ] T Represents random white noise from the gyroscope, [ω ax ω ay ω az ] T This represents random white noise from a triaxial accelerometer.

[0033] Furthermore, step S2 of the underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance of the present invention includes:

[0034] S21. Use the real-time high-precision velocity information of the carrier obtained by DVL to correct inertial navigation errors;

[0035] S22. DVL measures the three-axis velocity components of the vehicle relative to the vehicle coordinate system. The velocity vector output by DVL is transformed to the geographic coordinate system, and then the corresponding velocity information output by SINS is subtracted from it to obtain the measurement value.

[0036]

[0037] In the formula, v Ei v Ni v Ui These represent the eastward, northward, and upward velocities in the inertial navigation geographic coordinate system, respectively; v Ed v Nd v Ud These represent the eastward, northward, and celestial velocities in the geographic coordinate system after the Doppler velocimeter output transformation.

[0038] S23. can be further expressed as:

[0039]

[0040] In the formula, δv Ei δv Ni δv Ui These represent the velocity measurement errors in the east, north, and celestial directions under the inertial navigation geographic coordinate system; δv Ed δv Nd δv Ud These represent the eastward, northward, and celestial velocity errors in the geographic coordinate system after the Doppler velocimeter output transformation, respectively, and are assumed to be zero-mean Gaussian white noise.

[0041] S24. The measurement equations for a SINS / DVL-based integrated navigation system can be further expressed as:

[0042] Z d =H d X+V d ;

[0043] In the formula, H d For the measurement matrix, H d =[0 3×3 I 3×3 0 3×9 ], V d =[δv ED δv ND δv UD [ ] represents the measurement noise of the system, which is zero-mean Gaussian white noise.

[0044] Furthermore, step S3 of the underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance of the present invention includes:

[0045] S31.λ usv L usvh usv These represent the longitude, latitude, and altitude of the UUV, respectively. Therefore, the location of the UUV in the Earth's rectangular coordinate system is:

[0046]

[0047] In the formula, [x uuv y uuv z uuv [R] represents the position in the Earth's rectangular coordinate system. N For the Earth's radius, e Represents eccentricity. This is the transformation matrix for the corresponding coordinate system;

[0048] S32. Convert the UUV location coordinates back to the geographic coordinate system:

[0049]

[0050]

[0051]

[0052] S33. When the USBL connects to the external GNSS of the surface vessel, the USBL calculates the UUV's position information, including longitude, latitude, and altitude; the difference is calculated with the SINS equipment installed on the UUV itself, and this difference is added as new measurement information to the SINS / DVL integrated navigation system mentioned in S22 above; the measurement value Z du As shown in the following formula:

[0053]

[0054] S34. The measurement equations for a USV / UUV cooperative navigation system based on the SINS / DVL / USBL combination are as follows:

[0055] Z du =H du X+V du ;

[0056] In the formula, H du =[0 6×3 I 6×6 0 6×6 ], V du =[δv ED δv ND δv UD δL u δλ u δh u Let V be the measurement noise of the system. du Gaussian white noise with zero mean;

[0057] S35. Obtain the state-space expression of the SINS / DVL / USBL cooperative navigation system:

[0058]

[0059] In the formula, X(t) is the state equation of the navigation system, F(t) is the state transition matrix, G(t) is the system noise driving matrix, Z(t) is the difference between the measurement of the navigation system, and is the difference between the SINS output and USBL and DVL; W(t) and V(t) are the system noise and measurement noise, respectively.

[0060] Furthermore, step S4 of the underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance of the present invention includes:

[0061] S41. The variance-constrained sequential Sage-Husa adaptive filtering algorithm is used to filter the SINS / DVL / USBL cooperative navigation system. The state-space equation of the navigation system is:

[0062]

[0063] S42. In the above formula, X k Let Φ be the system state at time k. k|k-1 Γ is the state transition matrix; k|k-1 Z is the system noise input matrix; k The measurement value at time k is the state vector X. k The function; H k For measurement matrix; W k and V k These are system noise and measurement noise, respectively; and W k and V k Satisfy the following formula:

[0064]

[0065] In the formula, Q k Let R be the noise covariance matrix of the system, which is a non-negative definite matrix; k Let be the measurement noise covariance matrix, and be a positive definite matrix;

[0066] S43. The steps and formulas for Kalman filtering are as follows:

[0067] (1) One-step prediction of the state:

[0068]

[0069] (2) One-step prediction of mean squared error:

[0070]

[0071] (3) Calculation of Kalman gain matrix:

[0072]

[0073] (4) Status update:

[0074]

[0075] (5) Update of the state covariance matrix:

[0076] P k =(IK k H k )P k|k-1 ;

[0077] S44. Assume the measurement noise variance matrix R k Unknown, R is given below. k Adaptive estimation method:

[0078] (1) In the Kalman filtering process, the formula for the measurement prediction error (innovation) is:

[0079]

[0080] (2) Taking the variance of both sides of the above equation, we get:

[0081]

[0082] (3) After rearranging the terms, the expression for the variance of the measurement noise is:

[0083]

[0084] In the formula, It is theoretically the total average of the set of random sequences;

[0085] S45. In the application of adaptive filtering algorithms, time averaging is used instead. To ensure that the algorithm maintains an appropriate level of adaptability, R... k The weighted recursive estimation method can be constructed as follows:

[0086]

[0087]

[0088] In the formula, β0=1, 0<b<1 is the fading factor;

[0089] S46. Use sequential filtering to... By limiting the size of the diagonal elements, and using a sequential filtering method to update the i-th scalar sequential measurement, the scalar measurement equation is:

[0090]

[0091]

[0092] Reuse the lower bound condition To ensure It is positive, and the upper limit condition is used at the same time. Rapidly reduce measurement Credibility:

[0093]

[0094] Furthermore, step S5 of the underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance of the present invention includes:

[0095] The S51.USV is equipped with an S2C R 18 / 34H USBL positioning system and an RTK GPS terminal; the UUV is equipped with an inertial navigation system, DVL, and an S2C R18 / 34H OEM transponder.

[0096] S52. Transplant the SINS / DVL / USBL OEM-based integrated navigation system to the UUV industrial control computer system;

[0097] S53. Perform navigation tasks and verify the designed USV / UUV cooperative navigation system and filtering algorithm in experiments.

[0098] In summary, since USBL is installed on a mobile USV equipped with a high-precision RTK GPS and travels alongside the UUV, it enables underwater navigation and positioning of the UUV over a wide range. Attached Figure Description

[0099] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of the embodiments of the present invention will be briefly introduced below. Obviously, the following drawings are only some embodiments recorded in the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0100] Figure 1 This is a schematic diagram of the overall architecture of the underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance of the present invention.

[0101] Figure 2 This is the directional pattern of the USBL transmitter.

[0102] Figure 3 A schematic diagram showing the installation and deployment locations of USV and UUV.

[0103] Figure 4 This is a horizontal projection diagram of USV and UUV.

[0104] Figure 5 This is a flowchart of the Kalman filter process.

[0105] Figure 6 Image showing the USV platform.

[0106] Figure 7 Image showing the UUV platform.

[0107] Figure 8 This is a UUV motion trajectory diagram.

[0108] Figure 9 This is a UUV position error diagram. Detailed Implementation

[0109] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below in conjunction with specific embodiments and corresponding drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. This invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of this invention.

[0110] At the same time, it should be understood that the scope of protection of the present invention is not limited to the specific embodiments described below; it should also be understood that the terminology used in the embodiments of the present invention is for describing specific embodiments and not for limiting the scope of protection of the present invention.

[0111] Example: Cooperative navigation and positioning method for underwater vehicles based on accompanying acoustic beacon assistance

[0112] like Figure 1 As shown, this method includes the following steps:

[0113] The positioning between the S1.EvoLogics S2C R 18 / 34H USBL and the S2C R 18 / 34H OEM needs to meet the transmitter directivity of the USBL, such as... Figure 2 As shown, the installation and placement positions of the two are as follows: Figure 3 As shown;

[0114] S2. At this point, the UUV should be below the USV, the distance between the USBL and the OEM device is D, and the angle between the line connecting the two devices and the vertical direction of the USBL installation is θ. To obtain good positioning accuracy and communication effect, the acoustic constraint condition should be met as follows:

[0115]

[0116] S3. Under this constraint, in order to ensure that the UUV continuously obtains high-precision positioning and communication effects when performing tasks, the USV follows the UUV in real time according to the UUV's position; in order to realize the USV equipped with USBL mobile cooperative positioning, a position tracking PID algorithm based on line of sight is proposed.

[0117] S4. The USBL is mounted at the center of the USV. Ignoring installation errors, the USBL coordinate system and the USV carrier coordinate system are approximately coincident. At this point, the relative position vector of the UUV returned by the USBL is converted to the local navigation coordinate system using a coordinate transformation method. P y P z P );

[0118]

[0119] S5. During the UUV mission execution phase, the underwater depth is typically constant, and the main motion is planar. When the USV tracks the UUV on the water surface, the tracking point is set as the UUV's projection point on the water surface, ensuring that the relative distance between them remains within the positioning constraints. At this time, their positions in the horizontal plane are as follows: Figure 4 As shown;

[0120] S6. Heading towards the USV center where the UUV is located:

[0121]

[0122] S7. Calculate the heading angle error between the two. Tracking distance error Δp(t):

[0123]

[0124] In the formula, the USV's own heading angle is p xymin This is the minimum safe distance for horizontal distance;

[0125] S8. Maintain the relative position of the USV by adjusting its rudder angle control output α(t) and speed output β(t). The control commands are as follows:

[0126]

[0127] In the formula, the proportionality coefficient is k p The integral coefficient is k i =k p T / T i The differential coefficient is k d =k p T / T d The integral period and the differential period are T, respectively. i ,Td T is the system sampling period.

[0128] Furthermore, step S1 of this method includes:

[0129] S11. Establishment of the error equations for a strapdown inertial navigation system. The state error vector equations generally include position errors (longitude, latitude, and altitude); velocity errors (eastward, northward, and azimuth); and errors in three attitude angles (pitch, roll, and yaw); as well as accelerometer drift and gyroscope drift errors. Therefore, the complete error state vector of the inertial navigation system is expressed as:

[0130]

[0131] In the formula, Let δv be the attitude error vector, and δv = [δv E δv N δv U ] represents the velocity error vector, δp=[δL δλ δh] represents the position error vector, ε=[ε x ε y ε z [ ] represents the gyroscope drift error vector. This is the accelerometer drift error vector;

[0132] S12. The state equation of the UUV autonomous navigation system based on SINS / DVL consists of the error state of the inertial navigation system. It integrates the platform error angle equation, position error equation, velocity error equation, and the error equation of the inertial instrument. Therefore, the error state equation of the inertial navigation system can be expressed as:

[0133]

[0134] In the formula, F(t) represents the state transition matrix. Where F a ∈R 9×9 The system matrix representing the error parameters of the strapdown inertial navigation system is a 9th-order square matrix. Based on the error analysis of the strapdown inertial navigation system, the equations for its velocity, attitude, and position errors can be obtained:

[0135] (1) Velocity error equation:

[0136]

[0137] (2) Attitude error equation:

[0138]

[0139] (3) Position error equation:

[0140]

[0141] S13. For convenience, F a , will F a Divide into 3×3 block matrices, let Each block matrix is ​​represented as follows:

[0142]

[0143]

[0144]

[0145]

[0146]

[0147]

[0148]

[0149]

[0150] In the formula, F 31 Take the zero matrix; similarly, F b It can be written in the following matrix form, i.e. F c ∈R 6×6 Let F be the state matrix corresponding to the error states of the gyroscope and accelerometer. Here, the error between the two is considered as white noise. c Take the zero matrix, 0∈R 9×6 G(t) is the system noise matrix, which is taken as the identity matrix here; W(t) = [0 1×9 ω gx ω gy ω gz ω ax ω ax ω ax ] T Denotes the noise of the modeled inertial system, where [ω] gx ω gy ω gz ] T Represents random white noise from the gyroscope, [ω ax ω ay ω az ] T This represents random white noise from a triaxial accelerometer.

[0151] Furthermore, step S2 of this method includes:

[0152] S21. Establish measurement equations using the real-time high-precision velocity information of the carrier obtained by DVL, and correct inertial navigation errors;

[0153] S22. DVL measures the three-axis velocity components of the vehicle relative to the vehicle coordinate system. The velocity vector output by DVL is transformed to the geographic coordinate system, and then the corresponding velocity information output by SINS is subtracted from it to obtain the measurement value.

[0154]

[0155] In the formula, v Ei v Ni v Ui These represent the eastward, northward, and upward velocities in the inertial navigation geographic coordinate system, respectively; v Ed v Nd v Ud These represent the eastward, northward, and celestial velocities in the geographic coordinate system after the Doppler velocimeter output transformation.

[0156] S23. can be further expressed as:

[0157]

[0158] In the formula, δv Ei δv Ni δv Ui These represent the velocity measurement errors in the east, north, and sky directions under the inertial navigation geographic coordinate system, δv Ed δv Nd δv Ud These are the eastward, northward, and celestial velocity errors in the geographic coordinate system after the Doppler velocimeter output is transformed, respectively, and are assumed to be zero-mean Gaussian white noise.

[0159] The measurement equations for the S24.SINS / DVL integrated navigation system can be further expressed as:

[0160] Z d =H d X+V d ;

[0161] In the formula, H d For the measurement matrix, H d =[0 3×3 I 3×3 0 3×9 ], V d =[δv ED δv ND δv UD [ ] represents the measurement noise of the system, which is zero-mean Gaussian white noise.

[0162] Furthermore, step S3 of this method includes:

[0163] S31. For the ultra-short baseline positioning coordinate system (u-frame), the obtained position vector of the transponder is p. xyz USBL carrier coordinate system (b usv The U-frame and the U-frame are considered to coincide. The transponder position vector under the U-short baseline positioning system is transformed to the USV carrier coordinate system using a coordinate transformation matrix. The final transformation matrix obtained from the geographic coordinate system (n-frame) to the geophysical coordinate system (e-frame) is as follows:

[0164]

[0165] S32.λ usv L usv h usv These represent the longitude, latitude, and altitude of the UUV, respectively. Therefore, the location of the UUV in the Earth's rectangular coordinate system is:

[0166]

[0167] In the formula, [x uuv y uuv z uuv [R] represents the position in the Earth's rectangular coordinate system. N For the Earth's radius, e Represents eccentricity. This is the transformation matrix for the corresponding coordinate system;

[0168] S33. Convert the UUV location coordinates back to the geographic coordinate system:

[0169]

[0170]

[0171]

[0172] S34. When the USBL connects to the external GNSS of the surface vessel, the USBL calculates the UUV's position information, including longitude, latitude, and altitude; the difference is calculated with the SINS equipment installed on the UUV itself, and this difference is added as new measurement information to the SINS / DVL integrated navigation system mentioned in S22 above; the measurement value Z du The following formula can be selected:

[0173]

[0174] S34. The measurement equations for a USV / UUV cooperative navigation system based on the SINS / DVL / USBL combination are as follows:

[0175] Zdu =H du X+V du ;

[0176] In the formula, H du =[0 6×3 I 6×6 0 6×6 ], V du =[δv ED δv ND δv UD δL u δλ u δh u Let V be the measurement noise of the system. du Gaussian white noise with zero mean;

[0177] S35. Obtain the state-space expression of the SINS / DVL / USBL cooperative navigation system:

[0178]

[0179] In the formula, X(t) is the state equation of the navigation system, F(t) is the state transition matrix, G(t) is the system noise driving matrix, Z(t) is the difference between the measurement of the navigation system, and is the difference between the SINS output and USBL and DVL; W(t) and V(t) are the system noise and measurement noise, respectively.

[0180] Furthermore, step S4 of this method includes:

[0181] S41. A variance-constrained sequential Sage-Husa adaptive filtering algorithm is used to filter the SINS / DVL / USBL cooperative navigation system; the state-space equation of the navigation system is:

[0182]

[0183] S42. In the above formula, X k Let Φ be the system state at time k. k|k-1 Γ is the state transition matrix; k|k-1 Z is the system noise input matrix; k The measurement value at time k is the state vector X. k The function; H k For measurement matrix; W k and V k These are system noise and measurement noise, respectively; and W k and V k Satisfy the following formula:

[0184]

[0185] In the formula, Q k Let R be the noise covariance matrix of the system, which is a non-negative definite matrix; k Let be the measurement noise covariance matrix, and be a positive definite matrix;

[0186] S43. The steps and formulas for Kalman filtering are as follows:

[0187] (1) One-step prediction of the state:

[0188]

[0189] (2) One-step prediction of mean squared error:

[0190]

[0191] (3) Calculation of Kalman gain matrix:

[0192]

[0193] (4) Status update:

[0194]

[0195] (5) Update of the state covariance matrix:

[0196] P k =(IK k H k )P k|k-1 ;

[0197] S44. Given an initial state X(0), initial covariance P0, and initial measurements, the system state and state error covariance can be updated according to steps (1) and (2) in S43. Then, the system's filter gain is calculated using step (3). Finally, steps (4) and (5) are used for further state estimation and covariance prediction. In practical applications, when measurement Z occurs... k In cases where observation is not possible, the Kalman filter can be directly updated according to steps (1) and (2) to ensure the system's accuracy over a short period of time. The algorithm flow is as follows: Figure 5 As shown;

[0198] S45. Assume the measurement noise variance matrix R k Unknown, R is given below. k Adaptive estimation method:

[0199] (1) In the Kalman filtering process, the formula for the measurement prediction error (innovation) is:

[0200]

[0201] (2) Taking the variance of both sides of the above equation, we get:

[0202]

[0203] (3) After rearranging the terms, the expression for the variance of the measurement noise is:

[0204]

[0205] In the formula, It is theoretically the total average of a set of random sequences;

[0206] S46. In the application of adaptive filtering algorithms, time averaging can be used instead of R. k The weighted recursive estimation method can be constructed as follows:

[0207]

[0208] Among them, the initial value A larger variance matrix can be chosen based on the actual situation;

[0209] S47. According to the above formula, when k→∞, 1 / k→0, which means that after a long period of filtering, the adaptive capability will gradually weaken until it loses its adaptive adjustment effect.

[0210] S48. To ensure the algorithm maintains an appropriate level of adaptability, the equal weighting formula can be replaced with an exponentially diminishing memory weighted average:

[0211]

[0212]

[0213] In the formula, β0=1, 0<b<1 is called the fading factor;

[0214] S49. Using sequential filtering method to... Limiting the size of the diagonal elements can effectively avoid this problem; assuming Given a diagonal matrix, when using the sequential filtering method to update the i-th scalar sequential measurement, the scalar measurement equation is:

[0215]

[0216]

[0217] Reuse the lower bound condition To ensure It is positive, and the upper limit condition is used at the same time. Rapidly reduce measurement Credibility ( It can also be used to determine whether the measurement is abnormal. If it is considered abnormal, the measurement update is abandoned. This actually includes the idea of ​​fault detection and isolation.

[0218]

[0219] Through the above processing, the measurement noise can be reduced. Always limited to Within this range, better adaptive capability and filtering reliability can be achieved.

[0220] Furthermore, step S5 of the method of the present invention includes:

[0221] The S51.USV is equipped with an S2C R 18 / 34H USBL positioning system and an RTK GPS terminal. The UUV is equipped with inertial navigation, DVL, and an S2C R18 / 34H OEM transponder, such as... Figure 6 , Figure 7 As shown;

[0222] S52. Transplant the combined navigation system based on SINS / DVL / USBL OEM to the UUV control system;

[0223] S53. Perform the navigation task and verify the designed USV / UUV cooperative navigation system and filtering algorithm in the experiment. The final UUV trajectory and navigation error are as follows: Figure 8 , Figure 9 As shown.

[0224] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to obtain equivalent embodiments without departing from the scope of the technical solution of the present invention. Any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the technical solution of the present invention should be included within the scope of protection of the claims of the present invention.

Claims

1. A cooperative navigation and positioning method for underwater vehicles based on accompanying acoustic beacons, characterized in that, The method includes: S1. Confirm the location of USBL and USBL OEM to ensure that they meet the transmitter directionality and coverage of USBL; S2. Place the UUV below the USV, with a distance of D between the USBL and USBL OEM devices, and an angle θ between the line connecting the two devices and the vertical direction of the USBL installation, while satisfying the following acoustic constraints: S3. Under this constraint, the USV performs real-time tracking and accompaniment based on the UUV's position, using a line-of-sight-based position tracking PID algorithm; S4. Mount the USBL at the center of the USV. Convert the relative position vector of the UUV returned by the USBL to the local navigation coordinate system as (x... P y P z P ),at this time: S5. When the USV tracks the UUV on the water surface, the tracking point is set as the projection point of the UUV on the water surface, so that the relative distance between the two is kept within the positioning constraint range. S6. Let If the heading is the center of the USV where the UUV is located, then: S7. Calculate the heading angle error between the two. Tracking distance error Δp(t): In the formula, the USV's own heading angle is p xymin This is the minimum safe distance for horizontal distance; S8. Maintain the relative position of the USV by adjusting its rudder angle control output α(t) and speed output β(t). The control commands are as follows: In the formula, the proportionality coefficient is k p The integral coefficient is k i =k p T / T i The differential coefficient is k d =k p T / T d The integral period and the differential period are T, respectively. i ,T d T is the system sampling period.

2. The underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance according to claim 1, characterized in that, Step S1 includes: S11. The complete error state vector of a strapdown inertial navigation system is expressed as follows: In the formula, Let δv be the attitude error vector, and δv = [δv E δv N δv U ] is the velocity error vector, δp=[δLδλ δh] is the position error vector, ε=[ε x ε y ε z [ ] represents the gyroscope drift error vector. This is the accelerometer drift error vector; S12. The state equation of the integrated navigation system based on SINS / DVL / USBL consists of the error state of the inertial navigation system. The error state equation of the inertial navigation system is expressed as: In the formula, F(t) is the state transition matrix. Where F a ∈R 9×9 The system matrix represents the error parameters of the strapdown inertial navigation system, and it is a 9th-order square matrix; G(t) is the system noise matrix, which is taken as the identity matrix here; W(t) = [0 1×9 ω gx ω gy ω gz ω ax ω ax ω ax ] T Represents the noise of the modeled inertial system, where [ω gx ω gy ω gz ] T Represents random white noise from the gyroscope, [ω ax ω ay ω az ] T This represents random white noise from a triaxial accelerometer.

3. The underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance according to claim 2, characterized in that, Step S2 includes: S21. Use the real-time high-precision velocity information of the carrier obtained by DVL to correct inertial navigation errors; S22. DVL measures the three-axis velocity components of the vehicle relative to the vehicle coordinate system. The velocity vector output by DVL is transformed to the geographic coordinate system, and then the corresponding velocity information output by SINS is subtracted from it to obtain the measurement value. In the formula, v Ei v Ni v Ui These represent the eastward, northward, and upward velocities in the inertial navigation geographic coordinate system, respectively; v Ed v Nd v Ud These represent the eastward, northward, and celestial velocities in the geographic coordinate system after the Doppler velocimeter output transformation. S23. can be further expressed as: In the formula, δv Ei δv Ni δv Ui These represent the velocity measurement errors in the east, north, and celestial directions under the inertial navigation geographic coordinate system; δv Ed δv Nd δv Ud These represent the eastward, northward, and celestial velocity errors in the geographic coordinate system after the Doppler velocimeter output transformation, respectively, and are assumed to be zero-mean Gaussian white noise. S24. The measurement equations for a SINS / DVL-based integrated navigation system can be further expressed as: Z d =H d X+V d ; In the formula, H d For the measurement matrix, H d =[0 3×3 I 3×3 0 3×9 ], V d =[δv ED δv ND δv UD [ ] represents the measurement noise of the system, which is zero-mean Gaussian white noise.

4. The underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance according to claim 3, characterized in that, Step S3 includes: S31.λ usv L usv h usv These represent the longitude, latitude, and altitude of the UUV, respectively. Therefore, the location of the UUV in the Earth's rectangular coordinate system is: In the formula, [x uuv y uuv z uuv [R] represents the position in the Earth's rectangular coordinate system. N Where is the Earth's radius, and 'e' represents the eccentricity. This is the transformation matrix for the corresponding coordinate system; S32. Convert the UUV location coordinates back to the geographic coordinate system: S33. When the USBL connects to the external GNSS of the surface vessel, the USBL calculates the UUV's position information, including longitude, latitude, and altitude; the difference is calculated with the SINS equipment installed on the UUV itself, and this difference is added as new measurement information to the SINS / DVL integrated navigation system in S22; the measurement value Z du As shown in the following formula: S34. The measurement equations for a USV / UUV cooperative navigation system based on the SINS / DVL / USBL combination are as follows: Z du =H du X+V du ; In the formula, H du =[0 6×3 I 6×6 0 6×6 ], V du =[δv ED δv ND δv UD δL u δλ u δh u Let V be the measurement noise of the system. du Gaussian white noise with zero mean; S35. Obtain the state-space expression of the SINS / DVL / USBL cooperative navigation system: In the formula, X(t) is the state equation of the navigation system, F(t) is the state transition matrix, G(t) is the system noise driving matrix, Z(t) is the difference between the measurement of the navigation system, and is the difference between the SINS output and USBL and DVL; W(t) and V(t) are the system noise and measurement noise, respectively.

5. The underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance according to claim 4, characterized in that, Step S4 includes: S41. The variance-constrained sequential Sage-Husa adaptive filtering algorithm is used to filter the SINS / DVL / USBL cooperative navigation system. The state-space equation of the navigation system is: S42. In the above formula, X k Let Φ be the system state at time k. k|k-1 Γ is the state transition matrix; k|k-1 Z is the system noise input matrix; k The measurement value at time k is the state vector X. k The function; H k For measurement matrix; W k and V k These are system noise and measurement noise, respectively; and W k and V k Satisfy the following formula: In the formula, Q k Let R be the noise covariance matrix of the system, which is a non-negative definite matrix; k Let be the measurement noise covariance matrix, and be a positive definite matrix; S43. The steps and formulas for Kalman filtering are as follows: (1) One-step prediction of the state: (2) One-step prediction of mean squared error: (3) Calculation of Kalman gain matrix: (4) Status update: (5) Update of the state covariance matrix: P k =(I-K k H k )P k|k-1 ; S44. Assume the measurement noise variance matrix R k Unknown, R is given below. k Adaptive estimation method: (1) In the Kalman filtering process, the measurement prediction error formula is: (2) Taking the variance of both sides of the above equation, we get: (3) After rearranging the terms, the expression for the variance of the measurement noise is: In the formula, It is theoretically the total average of a set of random sequences; S45. In the application of adaptive filtering algorithms, time averaging is used instead. To ensure that the algorithm maintains an appropriate level of adaptability, R... k The weighted recursive estimation method is constructed as follows: In the formula, β0=1, 0<b<1 is the fading factor; S46. Use sequential filtering to... By limiting the size of the diagonal elements, and using a sequential filtering method to update the i-th scalar sequential measurement, the scalar measurement equation is: Reuse the lower bound condition To ensure It is positive, and the upper limit condition is used at the same time. Rapidly reduce measurement Credibility:

6. The underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance according to claim 5, characterized in that, Step S5 includes: The S51.USV is equipped with an S2C R 18 / 34H USBL positioning system and an RTK GPS terminal; the UUV is equipped with an inertial navigation system, DVL, and an S2C R 18 / 34H OEM transponder. S52. Transplant the SINS / DVL / USBL OEM-based integrated navigation system to the UUV industrial control computer system; S53. Perform navigation tasks and verify the designed USV / UUV cooperative navigation system and filtering algorithm in experiments.

7. A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the underwater vehicle cooperative navigation and positioning method based on accompanying acoustic beacon assistance as described in any one of claims 1-6.

Images