Single-pilot AUV underwater moving base alignment method based on acoustic distance measurement

By using the combination of acoustic ranging and inertial navigation systems in single-pilot AUVs, cubic spline interpolation and Kalman filtering are performed, the problem of underwater moving base alignment under low refresh rate acoustic ranging is solved, and the AUVs are significantly improved. Positioning accuracy.

CN118913313APending Publication Date: 2024-11-08HARBIN ENG UNIV
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202410983883.6
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-22
Publication Date
2024-11-08

AI Technical Summary

Technical Problem

Existing methods cannot achieve alignment of underwater moving bases when providing positioning information using acoustic ranging at low refresh rate.

Method used

Using a single-pilot AUV underwater moving base alignment method based on acoustic ranging, cubic spline interpolation is performed through the relative distance and position information between the main AUV and the slave AUV, combined with the data output from the inertial navigation system, Kalman filtering is used to perform online Align to improve alignment performance.

Benefits of technology

It effectively improves alignment performance, enhances positioning accuracy from AUV, and does not require external information as a reference.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118913313A_ABST
    Figure CN118913313A_ABST
Patent Text Reader

Abstract

The invention discloses a single-pilot AUV underwater moving base alignment method based on acoustic distance measurement, and belongs to the technical field of underwater navigation positioning. According to the invention, the problem that the existing method cannot realize the alignment of the underwater moving base when acoustic ranging is utilized to provide positioning information is solved. The master AUV sends the position of the master AUV and the relative distance between the master AUV and the slave AUV to the slave AUV, the number of effective position information of the acoustic ranging slave AUV is increased through a cubic spline interpolation method, the difference value of the position information of the slave AUV output by an inertial navigation system and the interpolated position information of the slave AUV is used as observed quantity, online alignment is carried out through Kalman filtering, and the position information of the slave AUV is obtained. And the alignment performance can be further improved. Compared with the existing method, the method does not need the external information of the single-pilot AUV as a reference. The method can be applied to single-pilot AUV underwater moving base alignment based on acoustic ranging.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of underwater navigation and positioning, and in particular relates to an underwater dynamic base alignment method of a single pilot AUV (Autonomous Underwater Vehicle) based on acoustic ranging. Background Art

[0002] Due to the complexity and uncertainty of the underwater environment, a single underwater vehicle is difficult to meet the demand. Multiple underwater vehicles are larger in scale, more precise, more intelligent, and can better complete complex tasks. Therefore, the collaborative operation mode of multiple underwater vehicles is increasingly being used in the execution of underwater tasks.

[0003] Collaborative navigation of multiple underwater vehicles refers to improving the navigation capability of AUVs through information sharing between different AUVs. This method has broad application prospects in the fields of marine scientific investigation, seabed resource exploration, marine environmental monitoring, etc. It can be divided into master-slave, parallel and hierarchical types according to the formation form. Currently, the single pilot collaborative navigation mode in the master-slave structure is more commonly used. The master AUV is equipped with high-precision navigation equipment, and the slave AUV is equipped with low-precision navigation equipment. The information advantage of the master AUV is used to improve the navigation accuracy of the slave AUV, thereby effectively reducing the operating cost while ensuring a high overall navigation capability.

[0004] Underwater vehicles are usually equipped with a variety of navigation systems. The inertial navigation system (INS) has become one of the most important navigation systems for underwater vehicles because of its advantages of being less susceptible to external interference and good real-time performance. The main sensors of the INS are gyroscopes and accelerometers. The gyroscope collects the carrier's angular velocity information, and the accelerometer collects the carrier's acceleration information. The carrier's velocity, position, and attitude information are obtained through integral calculation.

[0005] Although inertial navigation systems are highly favored in underwater environments due to their autonomy and good concealment, they are limited by the basic principles of Newton's laws of motion. Before the system works, it must determine the initial attitude information to establish an accurate navigation reference. The accuracy of the initial attitude information directly affects the accuracy of subsequent navigation solutions and the overall performance of the system.

[0006] Initial alignment is a key step before inertial navigation. It uses the error law of inertial navigation and external information to estimate and eliminate the initial attitude error of the navigation system through filtering algorithms. Single-pilot AUV collaborative navigation initial alignment refers to the use of the information advantage of the master AUV to align the slave AUV online when multiple AUVs are operating underwater to provide accurate initial attitude information.

[0007] The invention patent with publication number CN114777812A proposes "a method for alignment and attitude estimation of an underwater integrated navigation system on the move". This method is based on the Mahalanobis distance and uses a variational Bayesian robust adaptive Kalman filter to perform on-the-move attitude estimation of the SINS / DVL integrated navigation system of the AUV. The document "VL-assisted unmanned underwater vehicle retrospective non-latitude motion alignment method" proposes that the underwater vehicle travels freely with the assistance of DVL, and the navigation system can complete alignment in a short time without any initial position information. The document "A Robust FOAM Initial Alignment Method for SINSAssisted by DVL" proposes a robust fast attitude matrix initial alignment method based on the independence test of the vector error distribution, which is suitable for the situation where the DVL velocity information has abnormal values. The paper “Initial Alignment and Position Aiding Time Delay Compensation for SDINS of Deep Sea Underwater Vehicles” proposes a simple and effective initial alignment scheme for deep-sea underwater robots by combining the position-assisted time delay compensation method in order to solve the problem that the speed information provided by DVL and the position information provided by USBL in deep-sea alignment have a long time delay.

[0008] In summary, the existing methods either use different methods to align under the DVL auxiliary information, or improve the DVL auxiliary information through different methods before alignment. However, when the positioning information is provided by acoustic ranging with a low refresh rate, the existing methods still cannot achieve the alignment of the underwater dynamic base. Summary of the invention

[0009] The purpose of the present invention is to solve the problem that the existing method cannot achieve the alignment of the underwater dynamic base when acoustic ranging is used to provide positioning information, and proposes a single pilot AUV underwater dynamic base alignment method based on acoustic ranging.

[0010] The technical solution adopted by the present invention to solve the above technical problems is: a method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging, the method specifically comprising the following steps:

[0011] Step 1: The master AUV and the slave AUV navigate according to the designed path;

[0012] Step 2: When k=1, collect data output from the AUV inertial component, and the collected data specifically includes acceleration information output by the accelerometer and angular velocity information output by the MEMS gyroscope;

[0013] The acceleration information from the AUV at time k is recorded as The superscript T indicates transpose. and They represent the components of the specific force measured by the AUV accelerometer at time k on the three coordinate axes of the carrier system, all in m / s 2 ; The angular velocity information from the AUV at time k is recorded as and They represent the components of the angular velocity measured by the MEMS gyroscope at time k on the three coordinate axes of the carrier system, all in rad / s;

[0014] Step 3, initialization time k = 2, number of iterations l = 1;

[0015] Step 4: Collect the data output from the AUV inertial component at time k, and use the data output from the AUV inertial component collected at time k-1 to update the inertial navigation information of the AUV at time k, that is, obtain the position, velocity and attitude information of the AUV at time k output by the inertial navigation system;

[0016] Step 5: Use acoustic ranging to measure the position information of the master AUV at time k and the relative distance between the master AUV and the slave AUV;

[0017] Step 6: Determine whether k=l·ΔT is satisfied, where ΔT is a set threshold;

[0018] If it is satisfied, the relative distance between the master AUV and the slave AUV from time (l-1)·ΔT+1 to time l·ΔT is sent to the slave AUV, and step seven is executed;

[0019] If not satisfied, set k=k+1 and return to step 4;

[0020] Step 7: Send the master AUV position information and the relative distance between the master AUV and the slave AUV measured in step 5 to the slave AUV, and use the received information and the attitude information of the slave AUV at each moment to solve the position information of the slave AUV;

[0021] Step 8: Filter the slave AUV position information obtained in step 7 to obtain the filtered slave AUV position information;

[0022] Step 9: interpolate the slave AUV position information and the corresponding time selected in step 8 as known data nodes to obtain the slave AUV position information at each time after interpolation;

[0023] Step 10: For any moment between the moment (l-1)·ΔT+1 and the moment l·ΔT, the difference between the position information of the slave AUV output by the inertial navigation system at the current moment and the position information of the slave AUV after interpolation is obtained, and the difference of the position information of the slave AUV obtained is used as the observation value;

[0024] The AUV position error, velocity error, attitude error, gyroscope error and accelerometer error are combined into state quantities, and Kalman filtering is performed based on the state quantities and observation quantities to obtain the estimated results of the state quantities at each moment;

[0025] Then, the attitude information of the slave AUV at each moment is updated according to the state estimation result, and the alignment result of the slave AUV, i.e., the updated attitude information of the slave AUV, is output;

[0026] Step 11: Determine whether the alignment process is completed;

[0027] If not finished, set l=l+1, k=(l-1)ΔT+1, and return to step 4;

[0028] If it is finished, the whole alignment process is completed and the attitude information from the AUV is fed back to the inertial navigation system.

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

[0030] The master AUV of the present invention sends its own position and the relative distance between the master and slave AUVs to the slave AUVs, increases the number of effective position information of the slave AUVs for acoustic ranging by using the cubic spline interpolation method, takes the difference between the position information of the slave AUVs output by the inertial navigation system and the position information of the slave AUVs after interpolation as the observed quantity, and uses the Kalman filter to perform alignment online, which can further improve the alignment performance. Moreover, compared with the existing methods, the present invention does not require the external information of a single pilot AUV as a reference. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 It is a flow chart of a method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging according to the present invention;

[0032] Figure 2 This is the master and slave AUV alignment path trajectory diagram of the present invention;

[0033] Figure 3a This is a graph of cubic spline interpolation results from AUV latitude in the present invention;

[0034] Figure 3b for Figure 3a A magnified image of

[0035] Figure 4a This is a graph of the interpolation result from the AUV longitude cubic spline in the present invention;

[0036] Figure 4b for Figure 4a A magnified image of

[0037] Figure 5 This is the AUV alignment result diagram without interpolation in the present invention;

[0038] Figure 6 This is the interpolated AUV alignment result diagram in the present invention;

[0039] Figure 7a It is a trajectory diagram after feedback of alignment results from AUV in the present invention;

[0040] Figure 7b for Figure 7a A magnified image of

[0041] Figure 8a This is a positioning error diagram after the alignment result is fed back from the AUV in the present invention;

[0042] Figure 8b This is a positioning accuracy diagram after the alignment result is fed back from the AUV in the present invention. DETAILED DESCRIPTION

[0043] Specific implementation method 1: Combination Figure 1 The present embodiment is described as follows. The present embodiment describes a method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging, and the method specifically comprises the following steps:

[0044] Step 1: The master AUV and the slave AUV navigate according to the designed path;

[0045] Step 2: When k=1, collect data output from the AUV inertial component, and the collected data specifically includes acceleration information output by the accelerometer and angular velocity information output by the MEMS gyroscope;

[0046] The acceleration information from the AUV at time k is recorded as The superscript T indicates transpose. and They represent the components of the specific force measured by the AUV accelerometer at time k on the three coordinate axes of the carrier system, all in m / s 2 ; The angular velocity information from the AUV at time k is recorded as and They represent the components of the angular velocity measured by the MEMS gyroscope at time k on the three coordinate axes of the carrier system, all in rad / s;

[0047] Step 3, initialization time k = 2, number of iterations l = 1;

[0048] Step 4: Collect the data output from the AUV inertial component at time k, and use the data output from the AUV inertial component collected at time k-1 to update the inertial navigation information of the AUV at time k, that is, obtain the position, velocity and attitude information of the AUV at time k output by the inertial navigation system;

[0049] Step 5: Use acoustic ranging to measure the position information of the master AUV at time k and the relative distance between the master AUV and the slave AUV;

[0050] Step 6: Determine whether k=l·ΔT is satisfied, where ΔT is a set threshold;

[0051] If it is satisfied, the relative distance between the master AUV and the slave AUV from time (l-1)·ΔT+1 to time l·ΔT is sent to the slave AUV, and step seven is executed;

[0052] If not satisfied, set k=k+1 and return to step 4;

[0053] Step 7: Send the master AUV position information and the relative distance between the master AUV and the slave AUV measured in step 5 to the slave AUV, and use the received information and the attitude information of the slave AUV at each moment to solve the position information of the slave AUV;

[0054] Step 8: Filter the slave AUV position information obtained in step 7 to obtain the filtered slave AUV position information;

[0055] Step 9: interpolate the slave AUV position information and the corresponding time selected in step 8 as known data nodes to obtain the slave AUV position information at each time after interpolation;

[0056] Step 10: For any moment between the moment (l-1)·ΔT+1 and the moment l·ΔT, the difference between the position information of the slave AUV output by the inertial navigation system at the current moment and the position information of the slave AUV after interpolation is obtained, and the difference of the position information of the slave AUV obtained is used as the observation value;

[0057] The AUV position error, velocity error, attitude error, gyroscope error and accelerometer error are combined into state quantities, and Kalman filtering is performed based on the state quantities and observation quantities to obtain the estimated results of the state quantities at each moment;

[0058] Then, the attitude information of the slave AUV at each moment is updated according to the state estimation result, and the alignment result of the slave AUV, i.e., the updated attitude information of the slave AUV, is output;

[0059] Step 11: Determine whether the alignment process is completed;

[0060] If not finished, set l=l+1, k=(l-1)ΔT+1, and return to step 4;

[0061] If it is finished, the whole alignment process is completed and the attitude information from the AUV is fed back to the inertial navigation system.

[0062] Specific implementation method 2: This implementation method is different from the specific implementation method 1 in that, in step 1, the path design satisfies the following principles:

[0063] Principle (1): The master AUV and the slave AUV cannot move in the same direction and at the same speed parallel to any coordinate axis of the carrier system;

[0064] Principle (2): At any two adjacent ranging moments, the relative position information of the master AUV and the slave AUV changes.

[0065] The other steps and parameters are the same as those in the first embodiment.

[0066] Specific implementation method three: This implementation method is different from specific implementation methods one or two in that, in step four, the inertial navigation information of the AUV at time k is updated using the output data of the AUV inertial component collected at time k-1, specifically:

[0067] Step 4. Calculate the speed of the AUV at time k

[0068] The attitude transformation matrix from the loading system to the navigation system at time k-1 The acceleration information at time k-1 Perform projection transformation:

[0069]

[0070] in, It represents the acceleration force information from the AUV navigation system at time k-1. It represents the components of the acceleration force information from the AUV at time k-1 on each coordinate axis of the navigation system;

[0071] The speed change rate of the AUV at time k for:

[0072]

[0073] in, is the speed of the AUV in the navigation system at time k-1, ν E(k-1) , ν N(k-1) , ν U(k-1) is the speed of the AUV at time k-1 The components of each coordinate axis of the navigation system, namely ν E(k-1) is the eastward speed of the AUV at time k-1; ν N(k-1) is the northward velocity of the AUV at time k-1; ν U(k-1) is the celestial velocity of the AUV at time k-1; g n is the projection of the gravity acceleration vector in the navigation system, g n =[00-g] T, 0, 0, -g is the component of the gravity acceleration vector in the directions of the coordinate axes of the navigation system; for The antisymmetric matrix of represents the projection of the earth's rotation speed in the navigation system at time k-1; It represents the projection of the angular velocity of the navigation system relative to the earth system at time k-1 on the carrier system;

[0074]

[0075] Among them, ω ie is the Earth's rotation speed; R is the Earth's radius; L k-1 is the latitude from AUV at time k-1;

[0076] The speed of the AUV in the navigation system at time k for:

[0077]

[0078] Among them, Δt is the time interval between adjacent moments;

[0079] Step 4.2: Calculate the position information from the AUV

[0080]

[0081] Among them, L k is the latitude of the AUV at time k; k is the longitude from the AUV at time k; k-1 is the longitude from the AUV at time k-1;

[0082] Step 4.3: Calculate the attitude information from the AUV at time k

[0083]

[0084] Among them, C ij Represents the attitude transformation matrix from the load system to the navigation system at time k The element at row i and column j in . is the rolling angle of the AUV at time k; θ k is the pitch angle of the AUV at time k; ψ k is the heading angle of the AUV at time k.

[0085] The other steps and parameters are the same as those in the first or second embodiment.

[0086] In the present invention, the carrier system is recorded as b system, the x-axis of the carrier system points from the horizontal axis of the AUV to the right side of the AUV, the y-axis points from the vertical axis of the AUV to the front of the AUV, and the z-axis points from the top of the AUV. The navigation system is recorded as n system, and the three coordinate axes of the navigation system point to the east, north, and sky directions respectively. The earth system is recorded as e system, the origin of the earth system is located at the center of the earth, the x-axis is along the intersection of the Greenwich meridian plane and the earth's equatorial plane, and the z-axis is along the direction of the earth's polar axis.

[0087] Specific implementation method 4: This implementation method is different from the specific implementation methods 1 to 3 in that the posture conversion matrix The calculation method is:

[0088] When k = 1, The calculation method is:

[0089]

[0090] in, is the AUV rolling angle at the initial moment; θ 1 is the pitch angle of the AUV at the initial moment; ψ 1 is the heading angle of the AUV at the initial moment;

[0091] When k>1, Calculated as follows:

[0092] The projection of the angular velocity of the carrier system relative to the navigation system at time k on the carrier system is:

[0093]

[0094] in, It represents the projection of the angular velocity of the carrier system relative to the navigation system at time k-1 on the carrier system; is the posture transformation matrix The transposed matrix of represents the angular velocity information from the AUV at time k-1;

[0095] Attitude quaternion Q at time k-1 k-1 for:

[0096]

[0097] Among them, q 1 ,q 2 ,q 3 and q 4 is a real number, are mutually orthogonal unit vectors;

[0098] Change rate of attitude quaternion at time k for:

[0099]

[0100] in, yes The x-axis component of the carrier system; yes The component in the y-axis direction of the load system; yes The component in the z-axis direction of the load system; Yes 1 The first derivative of ; Yes 2 The first derivative of ; Yes 3 The first derivative of ; Yes 4 The first derivative of ;

[0101] The attitude quaternion Q at time k k for:

[0102]

[0103] Q k Recorded as q 1 ′,q 2 ′,q 3 ′ and q 4 ′ is a real number;

[0104] The attitude conversion matrix from the loading system to the navigation system at time k for:

[0105]

[0106] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0107] Specific implementation method 5: This implementation method is different from the specific implementation methods 1 to 4 in that the specific process of step 7 is as follows:

[0108] The position information of the AUV at time k satisfies:

[0109]

[0110] in, is the position information of the main AUV in the navigation system at time k, is the position information of the AUV in the navigation system at time k, r k is the relative distance between the master AUV and the slave AUV at time k;

[0111] The position information from the AUV at time k-1 satisfies:

[0112]

[0113] in, is the position information of the main AUV in the navigation system at time k-1, is the position information of the AUV in the navigation system at time k-1, r k-1 is the relative distance between the master AUV and the slave AUV at time k-1;

[0114] The position information from the AUV at time k and the position information from the AUV at time k-1 satisfy:

[0115]

[0116] Among them, v k-1 is the forward composite velocity from the AUV at time k-1 measured by the DVL, is the displacement change of the AUV in the x-axis direction of the navigation system from time k-1 to time k, is the displacement change of the AUV in the y-axis direction of the navigation system from time k-1 to time k;

[0117] According to formula (15), formula (16) and formula (17), calculate the position of the AUV at time k Will Converted to the navigation system, we get the latitude information L' from the AUV at time k k and longitude information λ' k .

[0118] The other steps and parameters are the same as those in Specific Embodiments 1 to 4.

[0119] Specific implementation method 6: This implementation method is different from the specific implementation methods 1 to 5 in that the specific process of step 8 is as follows:

[0120] If the AUV position information at time k and the AUV position information at time k-1 satisfy equation (18), the AUV position information at time k is considered invalid:

[0121]

[0122] If the AUV position information at time k and the AUV position information at time k-1 do not satisfy equation (18), the AUV position information at time k is regarded as the valid position information.

[0123] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 5.

[0124] Specific implementation method 7: This implementation method is different from any one of specific implementation methods 1 to 6 in that, in step 9, the AUV position information and the corresponding time selected in step 8 are used as known data nodes for interpolation, specifically:

[0125] Interpolate the time period between the i-th data node and the i+1-th data node to obtain the interpolation function:

[0126] S i (x) = a i +b i (xx i )+c i (xx i ) 2 +d i (xx i ) 3 (19)

[0127] Among them, S i (x) is the interpolation function, x is the independent variable of the interpolation function, x i is the time corresponding to the known i-th data node, a i , b i , c i and d i is the interpolation function coefficient;

[0128]

[0129] Among them, y i is the known position information of the slave AUV corresponding to the i-th data node, y i+1 is the known position information of the slave AUV corresponding to the i+1th data node, w i is the step length, w i =x i+1 -x i , x i+1 is the time corresponding to the known i+1th data node, m i+1 is the interpolation function at x i+1 The first-order derivative value at m i is the interpolation function at x i The first-order derivative value at satisfies:

[0130]

[0131] Among them, w i+1 =x i+2 -x i+1 , x i+2 is the time corresponding to the known i+2th data node, m i+2 is the interpolation function at x i+2The first derivative value at y i+2 It is the known position information of the slave AUV corresponding to the i+2th data node.

[0132] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0133] Specific implementation eight: This implementation differs from any one of specific implementations one to seven in that the interpolation function satisfies the following conditions (1) to (4) at the same time;

[0134] Condition (1): Interpolation condition

[0135] S i (x j )=y j ,j=0,1,…,n (22)

[0136] Among them, y j is the time x j The corresponding interpolation position information, x j is any time between the time corresponding to the known i-th data node and the time corresponding to the known i+1-th data node;

[0137] y j is the interpolation node x j The corresponding function value;

[0138] Condition (2): Continuity condition

[0139]

[0140] Condition (3): First-order derivative continuity condition

[0141]

[0142] Condition (4): Second-order derivative continuity condition

[0143]

[0144] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.

[0145] Specific implementation method 9: This implementation method is different from any one of specific implementation methods 1 to 8 in that the AUV position error, velocity error, attitude error, gyroscope error and accelerometer error are combined into a state quantity, and Kalman filtering is performed based on the state quantity and the observed quantity to obtain the state quantity estimation result at each moment;

[0146] State one-step prediction:

[0147]

[0148] in, is the estimated value of the state quantity at time k-1, φ k-1 =[φ E(k-1) φ N(k-1) φ U(k-1) ] T is the attitude error at time k-1, δν k-1 =[δν E(k-1) δν N(k-1) δν U(k-1) ] T is the velocity error at time k-1, δp k-1 =[δL k-1 δλ k-1 δh k-1 ] T is the position error at time k-1, ε k-1 =[ε E(k-1) ε N(k-1) ε U(k-1) ] T is the gyro drift error in the navigation system at time k-1, is the accelerometer error in the navigation system at time k-1; is the one-step prediction of the state quantity, Φ k,k-1 is the intermediate variable matrix;

[0149]

[0150] Where:

[0151] Among them, I 15×15 is a 15×15 identity matrix, R M is the principal radius of curvature of the meridian, in m, R N is the main curvature radius of the east-west circle, in meters; h k-1 is the height of the AUV at time k-1; f E(k-1) is the eastward accelerometer measurement value at time k-1, in m / s 2 ;f N(k-1) is the north accelerometer measurement value at time k-1, in m / s 2 ;f U(k-1) is the celestial accelerometer measurement value at time k-1, in m / s 2 ;

[0152] State Estimation:

[0153]

[0154] in, is the estimated value of the state quantity at time k, H k is the system measurement matrix at time k, Zk is the observed quantity at time k, K k is the filter gain matrix at time k;

[0155] Filter gain:

[0156]

[0157] Among them, P k,k-1 is the one-step prediction mean square error matrix, R k is the variance matrix of the measurement noise sequence at time k, which is assumed to be a positive definite matrix;

[0158] One-step forecast mean square error:

[0159]

[0160] Among them, P k-1 is the mean square error matrix at time k-1, Q k ' -1 is the variance matrix of the system noise sequence, assumed to be a non-negative definite matrix;

[0161] Estimate the mean squared error:

[0162] P k =(IK k H k ) k,k-1 (31)

[0163] Where P k is the mean square error matrix at time k; I is the unit matrix;

[0164] According to the state quantity estimation result, φ k Update the attitude information of the slave AUV at each moment, specifically:

[0165]

[0166] in, is the updated attitude transformation matrix from the loading system to the navigation system at time k;

[0167] The attitude of the AUV is:

[0168]

[0169] Among them, C i ' j express The element in row i and column j in is the updated AUV roll angle at time k, θ k ′ is the updated pitch angle of the AUV at time k, ψ k ′ is the updated heading angle of the AUV at time k.

[0170] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0171] Specific implementation method 10: This implementation method is different from any one of specific implementation methods 1 to 9 in that the method for obtaining the AUV observation quantity at time k is:

[0172] The AUV attitude error model is:

[0173]

[0174] Among them, ω ie is the angular velocity of the Earth's rotation, in rad / s;

[0175] The AUV speed error model is:

[0176]

[0177]

[0178] The AUV position error model is:

[0179]

[0180] The error model from the AUV inertial measurement unit is:

[0181]

[0182] Among them, ε k is the gyro drift error in the navigation system, is the accelerometer error in the navigation system; the rate of change of the AUV filter state at time k for:

[0183]

[0184] The AUV filtering system equation at time k is:

[0185]

[0186] That is X k =Φ k,k-1 X k-1 ; ε k ′ is the gyro drift error under the load system at time k, is the accelerometer error under the load system at time k, ε k ' -1 is the gyro drift error under the k-1 time loading system, is the accelerometer error under the load system at time k-1, ε k ′=[ε xkε yk ε zk ] T ,

[0187] The filtered observation equation from AUV at time k is:

[0188] Z k =[L k -L k ″ +1 λ k -λ k ″ +1 0] T =HX k =[I 3×3 0 3×12 ]X k (47)

[0189] Among them, L k ″ +1 is the latitude information from the AUV after interpolation, λ k ″ +1 is the longitude information from the AUV after interpolation.

[0190] The other steps and parameters are the same as those in Specific Embodiments 1 to 9.

[0191] Experimental Section

[0192] The underwater dynamic base alignment method based on acoustic ranging for a single pilot AUV proposed in the present invention is verified based on the sea trial test data of Sea A, and the alignment time is 20 minutes.

[0193] Input the interpolation time interval ΔT=1 min and the interpolation frequency f=1 Hz in step 1.

[0194] Select the corresponding test data according to the path design principle in step 2.

[0195] The path trajectory of the master-slave AUV alignment process in the experiment is as follows Figure 2 shown.

[0196] The system initialization parameters in the Northeast Sky coordinate system are as follows:

[0197] Main AUV

[0198] Initial position: latitude 19.1494°, longitude 108.0433°;

[0199] Initial speed: eastward speed 2.1671m / s, northward speed 1.5137m / s, celestial speed 0.0340m / s;

[0200] Initial attitude: pitch angle -0.8405°, roll angle -1.4008°, heading angle -30.3452°;

[0201] From AUV

[0202] Initial position: latitude 19.1489°, longitude 108.0432°;

[0203] Initial speed: eastward speed 2.1674m / s, northward speed 1.5137m / s, celestial speed 0.0340m / s;

[0204] Initial attitude: pitch angle -0.8410°, roll angle -1.4003°, heading angle -30.3467°;

[0205] Sampling time interval: T = 0.005s;

[0206] The gyroscope used in the experiment is a fiber optic gyroscope with a three-axis zero bias of 0.01° / h.

[0207] Kalman filter initial parameters:

[0208] Initial state quantity: X 0 =[0 1×3 0 1×3 0 1×3 0 1×3 0 1×3 ];

[0209] Among them, 0 1×3 Represents a zero matrix with 1 row and 3 columns.

[0210] Initial system noise matrix Q:

[0211]

[0212] Initial mean square error matrix P 0 :

[0213]

[0214] Where a = 30 μg (g is the local gravity acceleration, unit is m / s 2 ), b = 0.01° / h, c = 1 / Re (Re is the radius of the earth, unit is m), d = 0.1m / s, e = 0.5°, 0 3×3 represents a 3-row 3-column zero matrix, I 3×3 Represents the identity matrix with 3 rows and 3 columns.

[0215] Measurement noise variance matrix:

[0216] The results from the AUV alignment test are shown in Table 1.

[0217] Table 1 Results from AUV alignment tests

[0218]

[0219] Where D is the total distance travelled.

[0220] The alignment results of the AUV in the above experiment were obtained by using the method of the present invention. Figure 3a and Figure 3b This is the cubic spline interpolation result from the AUV latitude. Figure 4a and Figure 4b This is the result of cubic spline interpolation of AUV longitude. It can be seen from the figure that the refresh rate of acoustic ranging and positioning information is approximately 1 / 5Hz. Figure 5 This is the alignment result diagram without interpolation from AUV positioning information. Figure 6 This is the alignment result diagram after interpolation of the AUV positioning information. The above alignment results are fed back to the navigation system for 3h dead reckoning, and the positioning results from the AUV are compared with its GPS positioning results. Figure 7a and Figure 7b This is the trajectory diagram after the alignment result is fed back from the AUV. The trajectory after the interpolated positioning information is closer to the GPS trajectory than the trajectory after the alignment without interpolated positioning information. Figure 8a and Figure 8b This is the positioning result diagram after the alignment result is fed back from the AUV. The positioning accuracy after alignment of the positioning information without interpolation is 19.27‰D, and the positioning accuracy after alignment of the positioning information after interpolation is 4.07‰D. The latter is 15.2‰D higher than the former, and the positioning accuracy of the AUV is significantly improved. The above test results show that the present invention can effectively make up for the phenomenon that the alignment effect is poor due to the low refresh rate of the acoustic ranging positioning information.

[0221] The above calculation examples of the present invention are only used to explain the calculation model and calculation process of the present invention in detail, and are not intended to limit the implementation methods of the present invention. For ordinary technicians in the relevant field, other different forms of changes or modifications can be made based on the above description. It is impossible to list all the implementation methods here. All obvious changes or modifications derived from the technical solution of the present invention are still within the scope of protection of the present invention.

Claims

1. A method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging, characterized in that: The method specifically comprises the following steps: Step 1: The master AUV and the slave AUV navigate according to the designed path; Step 2: When k=1, collect data output from the AUV inertial component, and the collected data specifically includes acceleration information output by the accelerometer and angular velocity information output by the MEMS gyroscope; The acceleration information from the AUV at time k is recorded as The superscript T indicates transpose. and They represent the components of the specific force measured by the AUV accelerometer at time k on the three coordinate axes of the carrier system, all in m / s 2 ; The angular velocity information from the AUV at time k is recorded as and They represent the components of the angular velocity measured by the MEMS gyroscope at time k on the three coordinate axes of the carrier system, all in rad / s; Step 3, initialization time k = 2, number of iterations l = 1; Step 4: Collect the data output from the AUV inertial component at time k, and use the data output from the AUV inertial component collected at time k-1 to update the inertial navigation information of the AUV at time k, that is, obtain the position, velocity and attitude information of the AUV at time k output by the inertial navigation system; Step 5: Use acoustic ranging to measure the position information of the master AUV at time k and the relative distance between the master AUV and the slave AUV; Step 6: Determine whether k=l·ΔT is satisfied, where ΔT is a set threshold; If it is satisfied, the relative distance between the master AUV and the slave AUV from time (l-1)·ΔT+1 to time l·ΔT is sent to the slave AUV, and step seven is executed; If not satisfied, set k=k+1 and return to step 4; Step 7: Send the master AUV position information and the relative distance between the master AUV and the slave AUV measured in step 5 to the slave AUV, and use the received information and the attitude information of the slave AUV at each moment to solve the position information of the slave AUV; Step 8: Filter the slave AUV position information obtained in step 7 to obtain the filtered slave AUV position information; Step 9: interpolate the slave AUV position information and the corresponding time selected in step 8 as known data nodes to obtain the slave AUV position information at each time after interpolation; Step 10: For any moment between the moment (l-1)·ΔT+1 and the moment l·ΔT, the difference between the position information of the slave AUV output by the inertial navigation system at the current moment and the position information of the slave AUV after interpolation is obtained, and the difference of the position information of the slave AUV obtained is used as the observation value; The AUV position error, velocity error, attitude error, gyroscope error and accelerometer error are combined into state quantities, and Kalman filtering is performed based on the state quantities and observation quantities to obtain the estimated results of the state quantities at each moment; Then, the attitude information of the slave AUV at each moment is updated according to the state estimation result, and the alignment result of the slave AUV, i.e., the updated attitude information of the slave AUV, is output; Step 11: Determine whether the alignment process is completed; If not finished, set l=l+1, k=(l-1)ΔT+1, and return to step 4; If it is finished, the whole alignment process is completed and the attitude information from the AUV is fed back to the inertial navigation system.

2. The method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging according to claim 1, characterized in that: In step 1, the path design meets the following principles: Principle (1): The master AUV and the slave AUV cannot move in the same direction and at the same speed parallel to any coordinate axis of the carrier system; Principle (2): At any two adjacent ranging moments, the relative position information of the master AUV and the slave AUV changes.

3. The method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging according to claim 2, characterized in that: In step 4, the inertial navigation information of the AUV at time k is updated using the output data of the AUV inertial component collected at time k-1, specifically: Step 4. Calculate the speed of the AUV at time k The attitude transformation matrix from the load system to the navigation system at time k-1 The acceleration information at time k-1 Perform projection transformation: in, It represents the acceleration force information from the AUV navigation system at time k-1. It represents the components of the acceleration force information from the AUV at time k-1 on each coordinate axis of the navigation system; The speed change rate of the AUV at time k for: in, is the speed of the AUV in the navigation system at time k-1, ν E(k-1) , ν N(k-1) , ν U(k-1) is the speed of the AUV at time k-1 The components of each coordinate axis of the navigation system; g n is the projection of the gravity acceleration vector in the navigation system, g n =[0 0 -g] T , 0, 0, -g is the component of the gravity acceleration vector in the directions of the coordinate axes of the navigation system; The antisymmetric matrix of represents the projection of the earth's rotation speed in the navigation system at time k-1; It represents the projection of the angular velocity of the navigation system relative to the earth system at time k-1 on the carrier system; Among them, ω ie is the Earth's rotation speed; R is the Earth's radius; L k-1 is the latitude from AUV at time k-1; The speed of the AUV in the navigation system at time k for: Among them, Δt is the time interval between adjacent moments; Step 4.2: Calculate the position information from the AUV Among them, L k is the latitude of the AUV at time k; k is the longitude from the AUV at time k; k-1 is the longitude from the AUV at time k-1; Step 4.3: Calculate the attitude information from the AUV at time k Among them, C ij Represents the attitude transformation matrix from the load system to the navigation system at time k The element in row i and column j in is the rolling angle of the AUV at time k; θ k is the pitch angle of the AUV at time k; ψ k is the heading angle of the AUV at time k.

4. The method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging according to claim 3 is characterized in that: The attitude transformation matrix The calculation method is: When k = 1, The calculation method is: in, is the AUV roll angle at the initial moment; θ1 is the AUV pitch angle at the initial moment; ψ1 is the AUV heading angle at the initial moment; When k>1, Calculated as follows: The projection of the angular velocity of the carrier system relative to the navigation system at time k on the carrier system is: in, It represents the projection of the angular velocity of the carrier system relative to the navigation system at time k-1 on the carrier system; is the posture transformation matrix The transposed matrix of represents the angular velocity information from the AUV at time k-1; Attitude quaternion Q at time k-1 k-1 for: Among them, q1, q2, q3 and q4 are real numbers, are mutually orthogonal unit vectors; Rate of change of attitude quaternion at time k for: in, yes The x-axis component of the carrier system; yes The component in the y-axis direction of the load system; yes The component in the z-axis direction of the load system; is the first derivative of q1; is the first derivative of q2; is the first derivative of q3; is the first derivative of q4; The attitude quaternion Q at time k k for: Q k Recorded as q1′, q2′, q3′ and q4′ are real numbers; The attitude conversion matrix from the loading system to the navigation system at time k for:

5. The method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging according to claim 4, characterized in that: The specific process of step seven is as follows: The position information from the AUV at time k satisfies: in, is the position information of the main AUV in the navigation system at time k, is the position information of the AUV in the navigation system at time k, r k is the relative distance between the master AUV and the slave AUV at time k; The position information from the AUV at time k-1 satisfies: in, is the position information of the main AUV in the navigation system at time k-1, is the position information of the AUV in the navigation system at time k-1, r k-1 is the relative distance between the master AUV and the slave AUV at time k-1; The position information from the AUV at time k and the position information from the AUV at time k-1 satisfy: Among them, v k-1 is the forward composite velocity from the AUV at time k-1 measured by the DVL, is the displacement change of the AUV in the x-axis direction of the navigation system from time k-1 to time k, is the displacement change of the AUV in the y-axis direction of the navigation system from time k-1 to time k; According to formula (15), formula (16) and formula (17), calculate the position of the AUV at time k Will Converted to the navigation system, we get the latitude information L' from the AUV at time k k and longitude information λ' k .

6. The method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging according to claim 5, characterized in that: The specific process of step eight is as follows: If the AUV position information at time k and the AUV position information at time k-1 satisfy equation (18), the AUV position information at time k is considered invalid: If the AUV position information at time k and the AUV position information at time k-1 do not satisfy equation (18), the AUV position information at time k is regarded as the valid position information.

7. The method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging according to claim 6, characterized in that: In step nine, the AUV position information and the corresponding time selected in step eight are interpolated as known data nodes, specifically: Interpolate the time period between the i-th data node and the i+1-th data node to obtain the interpolation function: S i (x)=a i +b i (x-x i )+c i (x-x i ) 2 +d i (x-x i ) 3 (19) Among them, S i (x) is the interpolation function, x is the independent variable of the interpolation function, x i is the time corresponding to the known i-th data node, a i , b i , c i and d i is the interpolation function coefficient; Among them, y i is the known position information of the slave AUV corresponding to the i-th data node, y i+1 is the known position information of the slave AUV corresponding to the i+1th data node, w i is the step length, w i =x i+1 -x i , x i+1 is the time corresponding to the known i+1th data node, m i+1 is the interpolation function at x i+1 The first-order derivative value at m i is the interpolation function at x i The first-order derivative value at satisfies: Among them, w i+1 =x i+2 -x i+1 , x i+2 is the time corresponding to the known i+2th data node, m i+2 is the interpolation function at x i+2 The first derivative value at y i+2 It is the known position information of the slave AUV corresponding to the i+2th data node.

8. The method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging according to claim 7, characterized in that: The interpolation function satisfies the following conditions (1) to (4) at the same time; Condition (1): Interpolation condition S i (x j )=y j ,j=0,1,…,n (22) Among them, y j is the time x j The corresponding interpolation position information, x j is any time between the time corresponding to the known i-th data node and the time corresponding to the known i+1-th data node; y j is the interpolation node x j The corresponding function value; Condition (2): Continuity condition Condition (3): First-order derivative continuity condition Condition (4): Second-order derivative continuity condition 9. The method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging according to claim 8, characterized in that: The AUV position error, velocity error, attitude error, gyroscope error and accelerometer error are combined into a state quantity, and Kalman filtering is performed according to the state quantity and the observation quantity to obtain the state quantity estimation result at each moment; State one-step prediction: in, is the estimated value of the state quantity at time k-1, φ k-1 =[φ E(k-1) φ N(k-1) φ U(k-1) ] T is the attitude error at time k-1, δν k-1 =[δν E(k-1) δν N(k-1) δν U(k-1) ] T is the velocity error at time k-1, δp k-1 =[δL k-1 δλ k-1 δh k-1 ] T is the position error at time k-1, ε k-1 =[ε E(k-1) ε N(k-1) ε U(k-1) ] T is the gyro drift error in the navigation system at time k-1, ▽ k-1 =[▽ E(k-1) ▽ N(k-1) ▽ U(k-1) ] T is the accelerometer error in the navigation system at time k-1; is the one-step prediction of the state quantity, Φ k,k-1 is the intermediate variable matrix; Where: Among them, I 15×15 is a 15×15 identity matrix, R M is the principal radius of curvature of the meridian, in m, R N is the main curvature radius of the east-west circle, in meters; h k-1 is the height of the AUV at time k-1; f E(k-1) is the eastward accelerometer measurement value at time k-1, in m / s 2 ;f N(k-1) is the north accelerometer measurement value at time k-1, in m / s 2 ;f U(k-1) is the celestial accelerometer measurement value at time k-1, in m / s 2 ; State Estimation: in, is the estimated value of the state quantity at time k, H k is the system measurement matrix at time k, Z k is the observed quantity at time k, K k is the filter gain matrix at time k; Filter gain: Among them, P k,k-1 is the one-step prediction mean square error matrix, R k is the variance matrix of the noise sequence measured at time k; One-step forecast mean square error: Among them, P k-1 is the mean square error matrix at time k-1, Q′ k-1 is the variance matrix of the system noise sequence; Estimate the mean squared error: P k =(I-K k H k )P k,k-1 (31) Where P k is the mean square error matrix at time k; I is the unit matrix; According to the state quantity estimation result, φ k Update the attitude information of the slave AUV at each moment, specifically: in, is the updated attitude transformation matrix from the loading system to the navigation system at time k; The attitude of the AUV is: Among them, C i ' j express The element in row i and column j in is the updated AUV roll angle at time k, θ k ′ is the updated pitch angle of the AUV at time k, ψ k ′ is the updated heading angle of the AUV at time k.

10. The method for aligning a single pilot AUV underwater dynamic base based on acoustic ranging according to claim 9, characterized in that: The method of obtaining the AUV observation at time k is: The AUV attitude error model is: Among them, ω ie is the angular velocity of the Earth's rotation, in rad / s; The AUV speed error model is: The AUV position error model is: The error model from the AUV inertial measurement unit is: Among them, ε k is the gyro drift error in the navigation system, ▽ k is the accelerometer error in the navigation system; The rate of change of the AUV filtered state at time k for: The filtering system equation from AUV at time k is: That is X k =Φ k,k-1 X k-1 ; ε′ k is the gyro drift error under the load system at time k, ▽′ k is the accelerometer error under the load system at time k, ε′ k-1 is the gyro drift error under the load system at time k-1, ▽′ k-1 is the accelerometer error under the load system at time k-1; The filtered observation equation from AUV at time k is: Z k =[L k -L″ k+1 l k -l″ k+1 0] T =HX k =[I 3×3 0 3×12 ]X k (47) Among them, L k+1 is the latitude information from the AUV after interpolation, λ″ k+1 is the longitude information from the AUV after interpolation.