An underwater navigation method based on SINS and active transponder

Abstract

The application belongs to the technical field of underwater navigation, and provides an underwater navigation method based on SINS and effective transponders. The SINS is used to navigate the AUV and continuously correct the navigation result of the SINS. The correction steps include: when the number of effective transponders is greater than one, determining the first pseudo-range vector and the second pseudo-range vector of the AUV and each effective transponder based on the SINS solution result and the sound line tracking algorithm; when the number of effective transponders is one, determining the third pseudo-range vector and the fourth pseudo-range vector of the AUV and the single effective transponder at each discrete time based on the SINS solution result and the sound line tracking algorithm; and correcting the navigation result of the SINS based on the Kalman filtering result of the first pseudo-range vector and the second pseudo-range vector, or based on the Kalman filtering result of the third pseudo-range vector and the fourth pseudo-range vector. The technical scheme of the application can reasonably determine the number of effective transponders participating in the integrated navigation, and perform the corresponding integrated navigation strategy accordingly.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of underwater navigation, and particularly relates to an underwater navigation method based on SINS and effective transponders. BACKGROUND

[0002] With the increasing number of tasks such as ocean resource development, deep-sea scientific research and seabed exploration, the application demand of AUV (Autonomous Underwater Vehicle) in the field of ocean exploration and environmental monitoring is rapidly growing. In the process of executing long-term tasks, AUV must rely on a high-precision navigation and positioning system to ensure that it can complete the task safely and stably according to the predetermined track.

[0003] Due to the serious attenuation of electromagnetic waves in seawater, the global satellite navigation system signal cannot be directly used for underwater positioning. Therefore, the commonly used underwater navigation method at present is the combination of Strapdown Inertial Navigation System (SINS) and underwater acoustic ranging method. Among them, SINS calculates the attitude, velocity and position of AUV in real time by integrating the measurement results of gyroscopes and accelerometers, and has the advantages of complete autonomy and independence from external signals. However, its inherent defect is that the error continues to accumulate over time, resulting in a gradual decline in positioning accuracy in long-term tasks. The positioning method based on underwater acoustic ranging calculates the position of AUV by measuring the sound wave propagation time between AUV and multiple transponders of Long Baseline positioning system (LBL) and other underwater positioning systems. By Kalman filtering the calculation results of SINS and underwater acoustic positioning systems such as LBL, the navigation error of SINS system can be estimated, and the attitude, velocity, position and other information output by SINS can be corrected according to the estimation results.

[0004] During the navigation of AUV, due to the influence of underwater topography or AUV structure shielding, hydrological conditions or noise interference, and underwater acoustic channel attenuation, not all underwater transponders can obtain response signals at any time. In addition, due to the distance between underwater transponders in LBL system may reach several kilometers or even tens of kilometers, there may be a great difference in the distribution level of errors when calculating underwater transponders at different distances from AUV. Therefore, it is necessary to consider the effectiveness of transponders, and according to the change of the number of effective transponders, different Kalman filtering strategies are designed. SUMMARY

[0005] The application provides an underwater navigation method based on SINS and effective transponders, uses SINS to navigate the AUV, and continuously corrects the navigation result of the SINS during the navigation process, the correction of the navigation result of the SINS comprising cyclically performing the following steps:

[0006] S1, judging the number of effective transponders If Step S2 is performed, otherwise step S1 is returned to be executed;

[0007] S2, if , the first pseudo-range vector of the AUV and the plurality of effective transponders is determined based on the solving result of the SINS, and the second pseudo-range vector of the AUV and the plurality of effective transponders is determined based on the ray tracing algorithm; if , the third pseudo-range vector of the AUV and the single effective transponder at a plurality of discrete time points is determined based on the solving result of the SINS, and the fourth pseudo-range vector of the AUV and the single effective transponder at a plurality of discrete time points is determined based on the ray tracing algorithm;

[0008] S3, the navigation result of the SINS is corrected based on the Kalman filtering result of the first pseudo-range vector and the second pseudo-range vector, or based on the Kalman filtering result of the third pseudo-range vector and the fourth pseudo-range vector.

[0009] Further, the number of effective transponders is judged by the following steps:

[0010] S11, the AUV actively emits an inquiry signal through a sound source, and receives an underwater acoustic signal through a hydrophone and identifies a response signal therein;

[0011] S12, the number of underwater transponders with a response delay less than an upper limit of the response delay is counted;

[0012] S13, if the number of underwater transponders with a response delay less than an upper limit of the response delay is 0, then and the judgment is ended, otherwise step S14 is executed;

[0013] S14, if the number of underwater transponders with a response delay less than an upper limit of the response delay is 1, then , and the underwater transponder with the response delay less than the upper limit of the response delay is marked as an effective transponder, and then the judgment is ended, otherwise step S15 is executed;

[0014] S15, the underwater transponder with the response delay less than the upper limit of the response delay is marked as a candidate transponder;

[0015] S16, marking the underwater transponder with the minimum response time delay among all the candidate transponders as an effective transponder, and counting the response time delay difference between the other candidate transponders and the effective transponder;

[0016] S17, marking the candidate transponder with the response time delay difference less than the upper limit of the response time delay difference as an effective transponder;

[0017] S18, determining the number of effective transponders based on the marking results of steps S16 and S17, and ending the judgment.

[0018] Preferably, the upper limit of the response time delay and the upper limit of the response time delay difference are determined based on the degree of variation of the sound speed profile of the water area where the AUV is located.

[0019] Further, when , the vector elements of the first pseudo-range vector are the pseudo-ranges between the AUV and each effective transponder calculated by the SINS, and the vector elements of the second pseudo-range vector are the pseudo-ranges between the AUV and each effective transponder calculated by the ray tracing algorithm; when , the vector elements of the third pseudo-range vector are the pseudo-ranges between the AUV and a single effective transponder at each discrete time calculated by the SINS, and the vector elements of the fourth pseudo-range vector are the pseudo-ranges between the AUV and a single effective transponder at each discrete time calculated by the ray tracing algorithm.

[0020] Further, in step S3, the navigation results of the SINS are corrected based on the Kalman filtering results of the first pseudo-range vector and the second pseudo-range vector, specifically: taking the difference between the second pseudo-range vector and the first pseudo-range vector as the observation, taking the error vector of the SINS as the state quantity, obtaining the optimal estimation value of the error vector of the SINS through Kalman filtering, and correcting the navigation results of the SINS based on the estimation results.

[0021] Further, in step S3, the navigation results of the SINS are corrected based on the Kalman filtering results of the third pseudo-range vector and the fourth pseudo-range vector, specifically: taking the difference between the fourth pseudo-range vector and the third pseudo-range vector as the observation, taking the error vector of the SINS as the state quantity, obtaining the optimal estimation value of the error vector of the SINS through Kalman filtering, and correcting the navigation results of the SINS based on the estimation results.

[0022] Further, the error vector of the SINS includes: position error, velocity error, attitude error, gyro zero bias vector, accelerometer zero bias vector, gyro scale factor error, accelerometer scale factor error, lever arm vector error, and installation angle error.

[0023] Preferably, when The number of discrete time instants used to construct the measurement is determined based on the perpendicular distance between a single active transponder and the projection of the AUV's track on the water bottom, wherein the track projection is the projection of the extension of the current heading direction obtained from the SINS on the water bottom.

[0024] Preferably, the ray tracing algorithm is a ray tracing algorithm considering ray bending.

[0025] Embodiments of the present application provide an underwater navigation method based on SINS and active transponders, which can realize minimum prior assumptions, constrained quality control, and on-line self-calibration of parameters, ensuring that the mapping of time delay-pseudorange remains good physical consistency under different active transponder conditions; in the case of sparse measurement, only a single transponder, or asynchronous measurement, the SINS calculation can still be effectively constrained, improving the consistency of long-term track, and providing an engineering solution with precision and robustness under the condition of limited number of devices and deployment cost. BRIEF DESCRIPTION OF DRAWINGS

[0026] Figure 1 A schematic diagram of the deployment of a long baseline underwater acoustic positioning system according to some embodiments;

[0027] Figure 2 A flowchart of the process of correcting the SINS navigation result in the underwater navigation method based on SINS and active transponders according to embodiments of the present application;

[0028] Figure 3 A specific flowchart of step S1 according to embodiments of the present application;

[0029] Figure 4 A schematic diagram of the principle of the ray tracing algorithm considering ray bending;

[0030] Figure 5 A schematic diagram of the principle of calculating the pseudorange in the case of a single active transponder according to embodiments of the present application;

[0031] Figure 6 A top view of an AUV passing through a single active transponder according to embodiments of the present application. DETAILED DESCRIPTION

[0032] The present application will be further described below based on the preferred embodiments and with reference to the accompanying drawings.

[0033] In the description in the embodiments of the present application, it should be noted that if the terms "upper", "lower", "inner", "outer" and the like indicating the orientation or position relationship are based on the orientation or position relationship shown in the drawings, or the orientation or position relationship in which the product of the embodiments of the present application is usually placed, and are only for the convenience of describing the present application and simplifying the description, and therefore cannot be understood as indicating or implying that the device or element referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as limiting the present application. In addition, in the description of the present application, in order to distinguish different units, the first, second and the like are used in the specification, but these are not limited by the order of manufacture, and cannot be understood as indicating or implying relative importance, and the name thereof may be different in the detailed description and the claims of the present application. In addition, in order to facilitate understanding, the various components on the drawing are enlarged or reduced, but this practice is not intended to limit the protection scope of the present application.

[0034] The words in the specification are used to illustrate the embodiments of the present application, but are not intended to limit the present application. It should be noted that, unless otherwise explicitly specified and limited, if the terms "provided", "connected", "connected" appear, they should be understood in a broad sense, for example, they can be fixedly connected, or can be detachably connected, or integrally connected; can be mechanically connected, can be directly connected, or indirectly connected through an intermediate medium, can be the communication inside two elements. For those skilled in the art, the specific meaning of the above terms in the present application can be specifically understood.

[0035] In order to clearly set forth the technical solutions of the present application, first, the existing underwater integrated navigation technology and the existing problems are introduced.

[0036] When AUV (Autonomous Underwater Vehicle, autonomous underwater vehicle) is navigating underwater, due to the serious attenuation of electromagnetic waves in water, it cannot use global satellite navigation system signals for navigation and positioning. At present, the commonly used underwater navigation and positioning scheme generally takes the SINS (Strapdown Inertial Navigation System, strapdown inertial navigation system) carried on the AUV as the core navigation device, measures the angular motion and linear motion of the carrier by using its inertial sensor (gyroscope and accelerometer), and then calculates the attitude, velocity and position information of the AUV in the navigation coordinate system (such as a three-dimensional coordinate system) in real time through strapdown solving algorithm.

[0037] Although SINS can autonomously determine the position, velocity and attitude of the AUV without relying on external information sources, the errors of inertial devices such as gyroscopes and accelerometers will accumulate over time, resulting in a decrease in navigation accuracy. Therefore, the output of the SINS needs to be corrected by combining other positioning means. This combination of two or more navigation and positioning means is generally referred to as integrated underwater navigation. LBL (Long Baseline positioning system) is suitable for various underwater integrated navigation scenarios due to its wide range of deployment.

[0038] Figure 1 In one specific embodiment, the deployment of LBL is shown as follows: Figure 1 As shown, LBL is composed of a plurality of underwater transponders (including the transponders , transponder and other transponders in the figure, in addition to several other transponders not shown in the figure) deployed in advance on the seabed. The spacing between each underwater transponder can be between a few kilometers and tens of kilometers. An active sound source and a hydrophone are provided on the AUV. During navigation, the active sound source continuously emits inquiry signals. When the AUV enters the working range of the LBL system, the underwater transponders that receive the inquiry signals will send response signals. The hydrophone provided on the AUV can obtain a set of distance information (also referred to as pseudo-range) between the AUV and the underwater transponders based on the propagation time of the inquiry signals and the response signals using the sound ray tracing algorithm. At the same time, the SINS carried by the AUV can also obtain another set of pseudo-range information between the AUV and the underwater transponders based on the position calculated by the SINS and the accurate position of the underwater transponders determined in advance. By Kalman filtering of the two sets of pseudo-range information from different sources, the errors of the SINS (such as gyroscope bias and accelerometer bias) can be iteratively estimated, and the attitude, velocity and position of the SINS can be corrected and compensated using the error estimation results, thereby obtaining high-precision navigation results.

[0039] In the integrated navigation process, in order to improve the estimation accuracy, it is generally necessary to increase the number of underwater transponders participating in the underwater acoustic positioning calculation to strengthen the constraints, and to improve the accuracy of the underwater acoustic positioning calculation to ensure that sufficient observation information is provided and the positioning accuracy is less sensitive to distance measurement errors. However, the communication of the AUV with each underwater transponder may be affected by various factors when the AUV navigates underwater, resulting in the inability of the underwater transponder to provide effective response signals.

[0040] For example, due to complex underwater topography, hills, canyons, shipwrecks, etc., may obstruct the propagation of sound signals, causing the signals of some underwater transponders to fail to reach the hydrophones of the AUV. Or, when the AUV is in certain attitudes, its hull, equipment compartment, and other structures may block the response signals from certain directions. Furthermore, due to underwater acoustic channel attenuation and interference from ship and biological noise, long-distance response signals may not be received due to excessive attenuation or excessive noise. Additionally, the sound velocity distribution in water is often uneven due to the influence of hydrological factors such as temperature, salinity, and pressure, causing underwater acoustic signals to bend and resulting in sound shadow zones in certain areas. Moreover, the transponder itself may experience insufficient power or even stop working due to reasons such as battery depletion, electronic component damage, or watertight failure.

[0041] In addition to the aforementioned phenomenon of not being able to receive response signals from certain underwater transponders, for an LBL consisting of multiple underwater transponders spaced several kilometers or even tens of kilometers apart, even if some response signals can be received, they are not suitable for participation in integrated navigation.

[0042] For example, in Figure 1 In the illustrated embodiment, it is assumed that the underwater transponder has been received after the AUV enters the operating range of the LBL. and underwater transponder The response signal is calculated from the time an interrogation signal is emitted by an active sound source on the AUV to the time its hydrophone receives the underwater transponder. , Response delay of the response signal , Take the minimum value (e.g.) ) is denoted as And define the response delay for other receiveable response signals (such as...) )and The difference is expressed as the response delay difference, denoted as . .

[0043] Obviously, The underwater transponder closest to the AUV among all underwater transponders that can respond normally to an AUV. The response delay is the shortest when the ray tracking algorithm described later is executed to calculate the pseudorange between the AUV and the underwater transponder, and correspondingly, its calculation accuracy is the highest. This reflects the underwater transponder The distance to the AUV compared to the nearest underwater transponder The degree of deviation from the distance of the AUV, The larger the size, the better the underwater transponder obtained through the ray tracing algorithm. The accuracy of the pseudorange calculation results for AUVs is comparable to that for underwater transponders. The greater the difference in accuracy between the pseudorange calculation results, for example, when AUVs and underwater transponders... The distance is within a few hundred meters, and the underwater transponder When the distance is several kilometers or even more than ten kilometers, The value may be much greater than 1, which results in a large difference in the accuracy of the position calculation results of different underwater transponders in the LBL underwater acoustic positioning process.

[0044] Because the pseudorange calculated by the ray-tracking algorithm needs to be substituted as an observation into the iterative update process of the state variables during the subsequent Kalman filtering process, this calculation result error (generally expressed as the observation variance) Significant inconsistencies in the representation will have a significant impact on the Kalman filter estimation results.

[0045] In addition, if A larger value indicates that the AUV is close to the nearest underwater transponder. There is still a significant distance to go. In some shallow sea areas, the sound speed profile changes drastically, and there may be complex ocean currents in various directions, which will lead to large errors in the solution results of the ray tracking algorithm, and may even make it unsuitable as an observation to participate in the integrated navigation process.

[0046] Based on the analysis of the above problems, this application provides an underwater navigation method based on SINS and effective transponders. This method differs from the existing methods for determining effective underwater transponders in various SINS / LBL integrated navigation methods (for example, in Chinese invention patent CN111964684A, the calculated slant range is compared with the sound wave propagation distance, and when the slant range is less than the sound wave propagation distance, the hydrophone is determined to be a usable hydrophone). This method proposes a new way to determine the number of effective transponders and adopts different integrated navigation strategies based on different numbers of effective transponders.

[0047] Specifically, the method includes performing the following operations during the AUV's flight:

[0048] Operation 1: Use SINS to navigate the AUV;

[0049] Operation two involves continuously correcting the SINS navigation results during the navigation process, including, for example... Figure 2 As shown, correcting the navigation results of SINS involves repeatedly performing the following steps:

[0050] S1, determine the number of valid transponders. ,if If so, proceed to step S2; otherwise, return to step S1.

[0051] S2, if If the number of the effective transponders is more than one, then the first pseudo-range vector between the AUV and the effective transponders is determined based on the solving result of the SINS, and the second pseudo-range vector between the AUV and the effective transponders is determined based on the ray tracing algorithm; if the number of the effective transponders is one, then the third pseudo-range vector between the AUV and the single effective transponder at multiple discrete time instants is determined based on the solving result of the SINS, and the fourth pseudo-range vector between the AUV and the single effective transponder at multiple discrete time instants is determined based on the ray tracing algorithm. If the number of the effective transponders is more than one, then the first pseudo-range vector between the AUV and the effective transponders is determined based on the solving result of the SINS, and the second pseudo-range vector between the AUV and the effective transponders is determined based on the ray tracing algorithm; if the number of the effective transponders is one, then the third pseudo-range vector between the AUV and the single effective transponder at multiple discrete time instants is determined based on the solving result of the SINS, and the fourth pseudo-range vector between the AUV and the single effective transponder at multiple discrete time instants is determined based on the ray tracing algorithm.

[0052] S3, correcting the navigation result of the SINS based on the Kalman filtering result of the first pseudo-range vector and the second pseudo-range vector, or based on the Kalman filtering result of the third pseudo-range vector and the fourth pseudo-range vector.

[0053] Specifically, before the AUV enters the navigation state, the SINS can be firstly initialized, including calibrating the initial position, velocity, attitude and specific force of the AUV, and then performing initial alignment of the SINS strapdown inertial navigation system, and then in the process of AUV navigation, the SINS uses the inertial sensor to measure the angular velocity and acceleration of the AUV in real time, and outputs the position, velocity and attitude information through the strapdown solving algorithm to navigate the AUV, and at the same time, in the process of using the SINS to navigate the AUV, steps S1 to S3 are also continuously and cyclically executed to adopt different Kalman filtering processing according to different numbers of effective transponders, so as to correct the navigation result of the SINS.

[0054] The specific implementation modes of steps S1 to S3 are described in detail below.

[0055] <Judging the number of effective transponders>

[0056] In the embodiments of the present application, step S1 is used to judge the number of effective transponders, if there is no effective transponder, it indicates that the current each underwater transponder is not suitable for combined navigation, at this time, the position, velocity and attitude information independently output by the SINS is directly used to navigate the AUV, and after a preset time step (such as 1 second), step S1 is returned to continue to judge the number of effective transponders, until the effective transponder appears, and the combined navigation process of steps S2 and S3 is entered.

[0057] In some specific embodiments, as shown in Figure 3 step S1 further includes the following steps:

[0058] S11, the AUV actively emits an inquiry signal through a sound source, and receives an underwater acoustic signal through a hydrophone and identifies the response signal in the underwater acoustic signal;

[0059] ​S12, count the number of underwater transponders whose response delay is less than the upper limit of the response delay;

[0060] S13, if the number of underwater transponders with a response delay less than the upper limit of the response delay is 0, then let The judgment ends there; otherwise, step S14 is executed.

[0061] S14, if the number of underwater transponders with a response delay less than the upper limit of the response delay is 1, then let The underwater transponder whose response delay is less than the upper limit of the response delay is marked as a valid transponder, and then the judgment ends; otherwise, step S15 is executed.

[0062] S15, mark underwater transponders with a response delay less than the upper limit of the response delay as alternative transponders;

[0063] S16, mark the underwater transponder with the smallest response delay among all candidate transponders as the effective transponder, and calculate the difference in response delay between the other candidate transponders and the effective transponder;

[0064] S17, mark the candidate transponders whose response delay difference is less than the upper limit of the response delay difference as valid transponders;

[0065] S18. Based on the marking results of steps S16 and S17, determine the number of valid responders and end the judgment.

[0066] Specifically, in step S11, the AUV actively transmits an interrogation signal through a sound source installed on its hull and continuously receives underwater acoustic signals through a hydrophone installed on its hull. Since each underwater transponder can transmit a corresponding response signal in a frequency division or code division manner after receiving the interrogation signal, it is possible to identify whether a response signal exists, as well as the transponder number and response delay corresponding to the response signal, from the underwater acoustic signals received by the hydrophone.

[0067] Next, in step S12, the response delay is statistically less than the upper limit of the response delay (in terms of...). The number of underwater transponders (represented by the number of transponders) is as analyzed above. When the underwater temperature and salinity distribution fluctuate significantly with depth, or when there are internal ocean currents, the sound speed profile of the underwater acoustic propagation channel changes drastically, and the sound ray propagation calculation results often have large errors, making them unsuitable for integrated navigation. Therefore, in the embodiments of this application, historical sound speed profile data of the water area that the AUV is expected to pass through can be obtained in advance, and then the upper limit of the response delay can be determined according to the degree of drastic change in the sound speed profile of the water area where the AUV is located. (The more drastic the change in the sound velocity profile,) The smaller, the larger. If the response delay of an underwater transponder exceeds , then it is directly identified as an invalid transponder.

[0068] Obviously, in step S13, if no transponder signal is identified in the underwater acoustic signal received by the hydrophone, or the response time delay of the identified transponder signal exceeds , then the number of valid transponders is determined , at this time, the valid transponder determination process is exited, and the AUV will directly navigate based on the attitude, speed and position information output by the SINS, and then at a preset time interval , return to step S1 to re-execute the search and determination process of the valid transponder. If the determination result of step S13 is , then go to step S14.

[0069] If only one underwater transponder has a response time delay that does not exceed , then in step S14, the underwater transponder is marked as a valid transponder, and then the determination process is exited, and in subsequent steps S2, S3, the navigation result of the SINS will be corrected using the single valid transponder; if two or more underwater transponders have response time delays that do not exceed , then go to the further screening process of steps S15 to S17.

[0070] Specifically, in the screening process, first, mark each underwater transponder with a response time delay less than as a candidate transponder (step S15), then select the underwater transponder with the smallest response time delay , mark it as a valid transponder, and count the response time delay difference between the other candidate transponders and the valid transponder (step S16), for example, for any candidate transponder , the response time delay difference can be expressed as , then compare the of each candidate transponder with the upper limit of the response time delay difference (denoted as ), and mark those candidate transponders that satisfy as valid transponders (step S17), and finally, based on the screening results of steps S16, S17, the marking of all valid transponders can be completed (step S18).

[0071] The upper limit of the response time delay difference can be adjusted according to the minimum response time delay , for example, , is a proportionality coefficient, i.e. only when the response signal of a candidate transponder lags behind the response signal of the nearest transponder by a time period not exceeding of the minimum response time delay the alternative transponder is determined as an effective transponder. In some preferred embodiments, may be determined according to the severity of the sound velocity profile of the water area where the AUV is located. If the sound velocity profile changes severely, the may be set to be small, for example, less than or equal to 2, and the may be set to be large, so as to increase the number of effective transponders.

[0072] Obviously, even if the screening process of steps S15 to S17 is entered, in addition to the first marked effective transponder with the minimum response time , other alternative transponders may still not meet the determination conditions for effective transponders (both and two determination conditions must be met), therefore, the statistical result of step S18 may be or , if , in subsequent steps S2, S3, the navigation result of the SINS will be corrected by using a single effective transponder; if , in subsequent steps S2, S3, the navigation result of the SINS will be corrected by using multiple effective transponders.

[0073] <Calculation of pseudo-range vector for multiple effective transponders / single effective transponder cases>

[0074] In the embodiments of the present application, step S2 uses different ways to obtain the pseudo-range vector calculated by the SINS and the pseudo-range vector obtained by the ray tracing algorithm based on the response signal of the effective transponder, according to the different number of effective transponders.

[0075] Specifically, if , the first pseudo-range vector between the AUV and the effective transponder is determined based on the calculation result of the SINS, and the second pseudo-range vector between the AUV and the effective transponder is determined based on the ray tracing algorithm, wherein the specific operation steps for calculating the first pseudo-range vector are as follows:

[0076] At any time point , the position information of the AUV calculated by the SINS, including latitude , longitude and depth , is converted from the geodetic coordinate system to the geocentric rectangular coordinate system, so as to obtain the position vectors of the AUV in the X-axis, Y-axis and Z-axis directions at the time point: ​

[0077] (1),

[0078] Meanwhile, the position of each active transponder determined in step S1, including the latitude , longitude and depth determined in advance, is converted from the geodetic coordinate system to the geocentric rectangular coordinate system, so as to obtain the position vector of each active transponder in the X-axis, Y-axis and Z-axis directions:

[0079] (2),

[0080] According to the formula (1) and formula (2), the first pseudo-range vector shown in formula (3) can be obtained:

[0081] (3),

[0082] wherein, represents the 2-norm of the vector, that is, the length of the vector.

[0083] In the embodiment of the present application, the second pseudo-range vector is obtained by the ray tracing algorithm according to the response time delay of each active transponder . As analyzed in the foregoing, for the underwater acoustic propagation channel with a sharp change in sound velocity profile, such as shallow sea, the result of the conventional straight-line propagation model has a large error, therefore, preferably, the ray tracing algorithm considering the bending of the sound ray is adopted to determine the second pseudo-range vector of the AUV and each active transponder, Figure 4 shows the principle of ray tracing, as shown in Figure 4 , for any one active transponder , based on the measured data or historical data at the time of deployment, the sound velocity profile of the water area where the active transponder is located can be obtained, the sound velocity profile is divided into several layers in the depth dimension, and a layered medium model is established according to the change trend in the layer. It is easy to know that the propagation time of the sound ray in the first layer of the sound velocity profile is:

[0084] (4),

[0085] wherein, is the sound velocity gradient in the layer, is the sound velocity of each layer, is the angle of incidence of the sound ray, obviously, the sum of the round-trip propagation time of the sound ray in each layer should be equal to the measured response time delay, therefore, the ​​​The search is performed, and the sum of the round-trip propagation time is obtained by substituting the values ​​into the formula (preferably, the position change of the AUV under the response delay can also be considered, that is, the angle of the sound ray return is compensated based on the position change of the AUV). The angle that is closest to the measured response delay data is the true sound ray incident angle.

[0086] After determining the incident angle of the sound ray, the vertical and horizontal distances of the sound ray are calculated using an intra-layer superposition method, thus determining the propagation path of the sound ray and obtaining the AUV and the effective transponder. The distance, take that distance as The second pseudorange at time (i.e., the time when the response signal is received) By performing the above steps on all valid transponders and constructing the results into a vector, the second pseudorange vector can be obtained. :

[0087] (5).

[0088] if This indicates that only one valid transponder is suitable for integrated navigation. In this case, the sound tracking algorithm is used at time... Only one pseudorange can be obtained, resulting in insufficient constraint information. To enable integrated navigation using pseudorange information even with a single active transponder, in the embodiments of this application, when... At that time, based on the SINS solution results, the third pseudorange vector of the AUV and a single effective transponder at multiple discrete moments is determined, and the fourth pseudorange vector of the AUV and a single effective transponder at multiple discrete moments is determined based on the ray tracking algorithm. The specific steps for obtaining the third pseudorange vector are as follows:

[0089] At any point when correction is needed ,like Figure 5 As shown, the AUV obtained by SINS calculation at time... as well as Previous At any given moment (i.e.) common The location information (at each moment) is transformed from the geodetic coordinate system to the geocentric rectangular coordinate system, thus obtaining the total position of the AUV in the X, Y, and Z axes. Position vector at time 1 :

[0090] (6),

[0091] At the same time, the single valid transponder determined in step S1 (hereinafter referred to as...) The position of the transponder is transformed from the geodetic coordinate system to the geocentric rectangular coordinate system, thus obtaining the position vectors of a single effective transponder in the X, Y, and Z axes. :

[0092] (7),

[0093] Based on equations (6) and (7), the third pseudorange vector shown in equation (8) can be obtained. :

[0094] (8).

[0095] Fourth pseudo-range vector Based on the hydrophone on the AUV to common The response delay of a single effector obtained at a given time. Similarly, the result is obtained using the aforementioned ray tracing algorithm that considers ray bending:

[0096] (9).

[0097] The third pseudorange vector is constructed using the steps described above. and the fourth pseudo-range vector It can construct a multi-dimensional pseudorange vector by utilizing the pseudorange information of the single transponder received at multiple historical moments during AUV operation, even when only a single effective transponder exists. This forms a high-dimensional geometric constraint similar to that of multiple transponders, thereby enabling effective correction of SINS navigation results and suppressing inertial navigation drift.

[0098] <Combined Filtering Correction for Multiple Effective Transponders and Single Effective Transponder Cases>

[0099] After constructing the pseudorange vectors for different numbers of effective transponders in step S2, step S3 adopts corresponding combined filtering correction strategies for multiple effective transponders and single effective transponders to correct the SINS navigation results.

[0100] Among them, when At that time, with the second pseudorange vector With the first pseudorange vector The difference is the observed quantity Using the error vector of SINS As a state variable, the optimal estimate of the SINS error vector is obtained through Kalman filtering, and the navigation results of SINS are corrected based on the estimation results; when The difference between the fourth pseudo-range vector and the third pseudo-range vector is taken as an observation, and the error vector of the SINS is taken as a state variable. The optimal estimation value of the error vector of the SINS is obtained through Kalman filtering, and the navigation result of the SINS is corrected based on the estimation result.

[0101] Combined with (3), (5), (8) and (9), the observation shown in (10) can be obtained.

[0102] (10).

[0103] In the process of solving the position and attitude of the AUV by the SINS, the lever vector and the installation angle between the transducer (including the sound source and the hydrophone) and the center of the SINS are fixed in the carrier coordinate system, but due to assembly errors and slight bending, the actual lever vector and the beam pointing direction may be different from the nominal value. In order to suppress the systematic deviation caused by the installation error, in some preferred embodiments of the present application, the state variable for Kalman filtering includes the lever vector error and the installation angle error between the transducer and the center of the SINS in addition to the error term commonly used in integrated navigation Kalman filtering.

[0104] (11),

[0105] wherein each item in the vector is a three-dimensional vector, representing the position error, the velocity error, the attitude error, the gyro zero bias vector, the accelerometer zero bias vector, the gyro scale factor error, the accelerometer scale factor error, the lever vector error and the installation angle error, respectively.

[0106] The difference between the pseudo-range obtained by the SINS and the pseudo-range obtained by the ray tracing algorithm is taken as an observation to perform Kalman filtering with the error of the SINS as a state variable, so as to obtain the estimation value of the error of the navigation result of the SINS, and the specific steps of using the same for navigation correction are known to those skilled in the art. In some specific embodiments, the above Kalman filtering can be performed through the following steps:

[0107] First, the state equation shown in (12) is established:

[0108] (12),

[0109] wherein is a system state transition matrix, is a system noise distribution matrix, is a system noise vector,​ , The expression is as follows:

[0110] ,

[0111] where, is the third order identity matrix, is the direction cosine matrix from b-frame (body coordinate system) to n-frame (navigation coordinate system), is the projection of accelerometer specific force error in b-frame, is the angular velocity vector, and are the cross product matrices of and respectively, is the projection of gyro angular velocity error in b-frame, , , , , and are the correlation times of the first order Markov process, denotes a diagonal matrix with the elements in the brackets as the diagonal elements.

[0112] In addition, in the formula:

[0113] ,

[0114] ,

[0115] ,

[0116] ,

[0117] ,

[0118] where, , are the meridian radius and the prime vertical radius of the position of the AUV respectively, is the height of the AUV from the ellipsoid surface to the outside of the ellipsoid, is the latitude, , , are the velocity of the AUV relative to the earth (ground speed) respectively, is the gravitational acceleration, is the angular rate of the earth rotation.

[0119] The system noise distribution matrix is a 27x24 matrix:

[0120] ,

[0121] The system noise vector is a 24x1 dimensional vector:

[0122] ,

[0123] where each term is a three-dimensional random noise vector, corresponding to the random noise of the accelerometer, the gyroscope, the gyroscope bias vector, the accelerometer bias vector, the gyroscope scale factor error, the accelerometer scale factor error, the lever arm vector error and the installation angle error, respectively.

[0124] Then, the measurement equation shown in equation (13) is established:

[0125] (13),

[0126] wherein, is an observation matrix, is a measurement noise vector, considering , the dimension of , is , and , therefore, the dimension of can be uniformly expressed as , for any th element in it, the pseudo-range obtained by SINS can be expressed as:

[0127] (14).

[0128] It can be understood that when , , , represent the three-dimensional coordinates of each valid transponder , , , represent the three-dimensional position coordinates of the AUV at time obtained by SINS; when , , , represent the three-dimensional position coordinates of the AUV at each time obtained by SINS, , , then

[0129] Further, the Taylor first-order expansion of the AUV true position is carried out, and the second-order and higher-order terms are ignored:

[0130] (15),

[0131] wherein, , , are the three-dimensional position errors corresponding to the i-th element of the observation, , , The expression of is:

[0132] ,

[0133] Combined with (14), (15) and considering the observation noise, the observation innovation of the i-th element of can be expressed as:

[0134] (16),

[0135] wherein, is a Gaussian white noise.

[0136] Considering that the position information output by the SINS is usually in the navigation coordinate system, such as the North-East-Down (NED) coordinate system, there is the following conversion relationship between the NED coordinate system and the geodetic coordinate system:

[0137] (17),

[0138] wherein, , , are the longitude, latitude and depth errors of the i-th element of in the North-East-Down coordinate system, are the latitude and longitude of the position of the AUV.

[0139] Finally, the expression form of the observation matrix can be obtained as:

[0140] (18),

[0141] wherein, is a 3-column unit matrix, is a 3-column matrix, and the three elements of any i-th row of are: .

[0142] .

[0143] ​​​​​​​​After the above state equation and observation equation are constructed, they can be discretized:

[0144] (19),

[0145] wherein, , are the discrete state vectors at time and time , respectively, is the observation vector at time , is the system noise vector at time , is the observation noise vector at time , is the state transition matrix from time to time , is the system noise input matrix at time , is the observation matrix at time .

[0146] Then, the following Kalman filter estimation process is iteratively performed:

[0147] a obtaining the prior estimation of the state vector at time :

[0148] ,

[0149] b updating the prior state covariance matrix at time :

[0150] ,

[0151] c updating the Kalman gain at time :

[0152] ,

[0153] d obtaining the posterior estimation of the state vector :

[0154] ,

[0155] e updating the posterior state covariance matrix at time :

[0156] .

[0157] The above steps are iteratively performed to obtain the posterior estimation value ​i.e. after estimating the errors of SINS, such as position, attitude, velocity, etc., the corrected navigation results can be obtained by using the SINS output results.

[0158] In some preferred embodiments, when the number of discrete time instants used to construct the observation is determined based on the vertical distance between the single active transponder and the projection of the AUV's track on the water bottom, where the projection is the projection of the extension of the current heading direction of the AUV on the water bottom.

[0159] Specifically, as Figure 6 shown, when only one active transponder is involved in the correction process of the SINS navigation results, if the vertical distance between the projection of the AUV's track and the active transponder (i.e. H in Figure 6 ) is too close, it is obviously not conducive to add sufficient vertical geometric constraints in the Kalman filtering process, therefore, in the case of , the value of H can be estimated based on the current navigation results of the AUV, and then the discrete time instants used to construct the observation is increased with the decrease of H, until it is increased to a preset upper limit of the discrete time instants (such as or more) to ensure that sufficient geometric constraints are provided in the Kalman filtering process.

[0160] The specific embodiments of the present application are described in detail above, and for those skilled in the art, without departing from the principles of the present application, some improvements and modifications can be made to the present application, and these improvements and modifications also belong to the protection scope of the present application.

Claims

1. An underwater navigation method based on SINS and active transponder, using SINS to navigate AUV, and continuously correcting the navigation results of SINS during navigation, characterized in that, The navigation result of the SINS is corrected, including cyclically performing the following steps: S1, determine the number of active transponders if then execute step S2, otherwise return to execute step S1; S2, if then determining a first pseudo-range vector between the AUV and the plurality of active transponders based on the SINS solution and a second pseudo-range vector between the AUV and the plurality of active transponders based on the ray-tracing algorithm; if then determining a third pseudo-range vector between the AUV and the single active transponder at a plurality of discrete time instants based on the SINS solution and a fourth pseudo-range vector between the AUV and the single active transponder at the plurality of discrete time instants based on the ray-tracing algorithm; S3, based on the Kalman filtering result of the first pseudo-range vector and the second pseudo-range vector, or based on the Kalman filtering result of the third pseudo-range vector and the fourth pseudo-range vector, correcting the navigation result of the SINS; The number of valid transponders is determined by the following steps: S11, the AUV actively emits an inquiry signal through a sound source, and receives an underwater acoustic signal through a hydrophone and identifies a response signal in the signal; S12, the number of underwater transponders with a response delay less than the upper limit of the response delay is counted; S13, if the number of underwater transponders whose response delay is less than the upper limit of the response delay is 0, then let and end the judgment, otherwise execute step S14; S14, if the number of underwater transponders whose reply time is less than the upper limit of the reply time is 1, then let and mark the underwater transponder whose reply time is less than the upper limit of the reply time as a valid transponder, and then end the judgment, otherwise execute step S15; S15, the underwater transponders with a response delay less than the upper limit of the response delay are marked as candidate transponders; S16, the underwater transponder with the smallest response delay among all candidate transponders is marked as a valid transponder, and the response delay difference between other candidate transponders and the valid transponder is counted; S17, the candidate transponders with a response delay difference less than the upper limit of the response delay difference are marked as valid transponders; S18, the number of valid transponders is determined based on the marking results of steps S16 and S17, and the determination is ended; When the number of discrete time instances used to construct the observation is determined based on the perpendicular distance of a single active transponder to the projection of the AUV's track on the water bottom, where the track projection is the projection on the water bottom of the extension of the current heading direction as determined by the SINS.

2. The underwater navigation method based on SINS and valid transponders according to claim 1, characterized in that: The upper limit of the response delay and the upper limit of the response delay difference are determined based on the degree of change of the sound speed profile of the water area where the AUV is located.

3. The underwater navigation method based on SINS and valid transponders according to claim 1, characterized in that: When the vector elements of the first pseudo-range vector are pseudo-distances between the AUV and the effective transponders calculated by the SINS, and the vector elements of the second pseudo-range vector are pseudo-distances between the AUV and the effective transponders calculated by the acoustic ray tracing algorithm. When the vector elements of the third pseudo-range vector are pseudo-ranges of the AUV and the single effective transponder at each discrete time obtained by the SINS, and the vector elements of the fourth pseudo-range vector are pseudo-ranges of the AUV and the single effective transponder at each discrete time obtained by the acoustic ray tracing algorithm.

4. The SINS and active transponder based underwater navigation method of claim 3, wherein, In step S3, the navigation result of the SINS is corrected based on the Kalman filtering result of the first pseudo-range vector and the second pseudo-range vector, specifically: The difference between the second pseudo-range vector and the first pseudo-range vector is taken as the observation, the error vector of the SINS is taken as the state quantity, the optimal estimation value of the error vector of the SINS is obtained through Kalman filtering, and the navigation result of the SINS is corrected based on the estimation result.

5. The SINS and active transponder based underwater navigation method of claim 3, wherein, In step S3, the navigation result of the SINS is corrected based on the Kalman filtering result of the third pseudo-range vector and the fourth pseudo-range vector, specifically: The difference between the fourth pseudo-range vector and the third pseudo-range vector is taken as the observation, the error vector of the SINS is taken as the state quantity, the optimal estimation value of the error vector of the SINS is obtained through Kalman filtering, and the navigation result of the SINS is corrected based on the estimation result.

6. The SINS and active transponder based underwater navigation method of claim 4 or claim 5, wherein, The error vector of the SINS includes: Position error, velocity error, attitude error, gyro zero bias vector, accelerometer zero bias vector, gyro scale factor error, accelerometer scale factor error, lever arm vector error, and installation angle error.

7. The SINS and active transponder based underwater navigation method of claim 1, wherein, The ray tracing algorithm is a ray tracing algorithm considering ray bending.

Images