Submersible tight combination single-beacon navigation method and system based on five-dimensional augmented error

Abstract

The application provides a submersible tight combination single-beacon navigation method and system based on five-dimensional augmented error, and relates to the technical field of navigation and positioning. The method comprises the following steps: obtaining DVL speed, compass heading angle and submersible depth information, and continuously recursively calculating the nominal position of the submersible; if there is no effective single-beacon distance observation, defining the compass error according to the periodic change of the heading angle, constructing a five-dimensional augmented error state model based on the DVL speed correction amount, the compass error and the two-dimensional position error, and calculating the prior covariance matrix; if there is, obtaining the single-beacon distance observation and calculating the distance measurement matrix, performing measurement updating on the augmented error state and the covariance, and taking the updating result as the posterior estimation; and based on the posterior estimation, compensating the position, the DVL speed and the compass error, and continuously outputting the corrected navigation result of the submersible. The application effectively suppresses the long-time underwater navigation cumulative drift and improves the navigation precision.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology, and in particular to a method and system for navigation of a submersible with a tightly packed single beacon based on five-dimensional augmented error. Background Technology

[0002] When underwater navigation, satellite navigation signal transmission is obstructed. Submersibles generally rely on inertial navigation, dead reckoning, and underwater acoustic positioning to obtain position information. The mainstream correction methods in the industry are to periodically surface with high-precision inertial navigation to obtain satellite positioning results for correction, or to deploy long / ultra-short baseline multi-node underwater acoustic arrays to constrain the cumulative positioning error, which can ensure basic navigation accuracy.

[0003] However, existing technologies have several drawbacks: high-precision inertial navigation systems are expensive, and periodic surfacing can interrupt continuous underwater operations; the deployment, calibration, and recovery processes for multi-node underwater acoustic systems are cumbersome, resulting in high logistics and equipment investment costs. While a lightweight architecture using a Doppler velocity log (DVL) meter, compass, depth gauge, and single acoustic beacon simplifies hardware deployment, it still presents challenges in error modeling: compass errors fluctuate periodically with the heading, and constant deviation models cannot fully characterize this disturbance, easily leading to trajectory drift; a single beacon only outputs scalar distance, making it difficult to distinguish between various coupled errors such as compass and DVL scales, and adding deviation states increases the filtering dimension and computational load, making it difficult to meet the requirements of low-complexity online solution. Summary of the Invention

[0004] To address the aforementioned issues, this invention proposes a submersible compact single beacon navigation method and system based on five-dimensional augmented error, which effectively suppresses long-term underwater dead reckoning drift and ensures continuous high-precision navigation for the submersible.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: In a first aspect, the present invention provides a submersible compact assembly single beacon navigation method based on five-dimensional augmented error, comprising: Obtain DVL velocity, compass heading angle and submersible depth information, perform coordinate transformation and continuously deduce the nominal position of the submersible; If no valid single beacon distance observation exists, a five-dimensional augmented error state model is constructed based on the DVL velocity correction, compass error, and two-dimensional position error. A state matrix is ​​constructed based on the model, and the prior covariance matrix is ​​calculated. The compass error is defined according to the periodic change of the heading angle. If it exists, obtain single beacon distance observations and calculate the distance measurement matrix; use the distance measurement matrix to update the augmented error state and its covariance, and use the updated result as the posterior estimate; The position, DVL velocity, and compass error are compensated based on posterior estimation, and the corrected submersible navigation results are continuously output.

[0006] Secondly, the present invention provides a submersible compact assembly single beacon navigation system based on five-dimensional augmented error, comprising: The nominal position calculation module is configured to acquire DVL velocity, compass heading angle and submersible depth information, perform coordinate transformation and continuously calculate the nominal position of the submersible. The no-beacon update module is configured to, if there is no valid single-beacon distance observation, construct a five-dimensional augmented error state model based on the DVL velocity correction, compass error, and two-dimensional position error, construct a state matrix based on the model, and calculate the prior covariance matrix; wherein, the compass error is defined according to the periodic change of the heading angle; There is a beacon update module, which is configured to, if it exists, acquire single beacon distance observations and calculate the distance measurement matrix; use the distance measurement matrix to update the augmentation error state and its covariance, and use the update result as a posterior estimate; The navigation module is configured to compensate for position, DVL velocity and compass error based on posterior estimation and continuously output the corrected submersible navigation results.

[0007] Thirdly, the present invention provides a submersible compact single beacon navigation device based on five-dimensional augmented error, including a navigation device, a single acoustic beacon, an acoustic ranging device, and a navigation processor; The navigation equipment includes a DVL (Depth Volume), a compass, and a depth sensor, which are used to collect submersible speed, compass heading angle, and depth information, respectively. The monoacoustic beacon is either a fixed beacon with a known location or a moving beacon obtained at the time of observation; The acoustic ranging device is used to obtain distance information between the submersible and the acoustic beacon; The navigation processor is used to execute the steps in the submersible compact single beacon navigation method based on five-dimensional augmented error as described in the first aspect.

[0008] Fourthly, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps in the submersible compact assembly single beacon navigation method based on five-dimensional augmented error described in the first aspect.

[0009] Fifthly, the present invention provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the submersible compact assembly single beacon navigation method based on five-dimensional augmented error described in the first aspect.

[0010] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention achieves underwater tight-integration navigation based on DVL, compass, depth gauge, and single acoustic beacon, eliminating the need for expensive high-precision inertial navigation systems and multi-baseline underwater acoustic arrays. This significantly reduces equipment procurement, deployment and calibration in water areas, and recovery and maintenance costs, and avoids the problem of interrupting underwater operations due to periodic surfacing corrections. It uses periodically correlated harmonic coefficients to fully characterize the compass error caused by periodic course fluctuations, overcoming the distortion defects of traditional constant bias models and accurately restoring the position drift caused by course disturbances. It constructs a five-dimensional fixed augmented error state, discarding the ocean current velocity deviation term that easily causes strong state coupling. Under the constraint of scarce single-dimensional distance observation information, it avoids the filter oscillation divergence caused by multiple error states competing to fit the residuals. It achieves simultaneous online estimation and closed-loop compensation of compass error, DVL scale error, and two-dimensional position error with low computational cost, balancing real-time solution and navigation accuracy.

[0011] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute a limitation thereof.

[0012] Figure 1 A main flowchart of a submersible compact assembly single beacon navigation method based on five-dimensional augmented error provided for an embodiment of the present invention; Figure 2 This is a schematic diagram comparing the radial position error of the trajectory and different error state models under the simulation scenario of the moving beacon provided in the embodiment of the present invention; wherein, (a) is a schematic diagram of the relative position of the simulated trajectory of the submersible and the moving beacon; (b) is a schematic diagram of the horizontal position error of different navigation models under the distance assistance of the moving beacon; Figure 3 This is a schematic diagram comparing the radial position error of the trajectory and different error state models under the actual measurement scenario of the fixed beacon provided in the embodiments of the present invention; wherein, (a) is a schematic diagram of the relative position of the submersible's measured trajectory and the fixed beacon; (b) is a schematic diagram of the horizontal position error of different navigation models under the fixed beacon distance assistance. Figure 4 This is a schematic diagram of a submersible compact assembly single beacon navigation device based on five-dimensional augmented error, provided as an embodiment of the present invention. Detailed Implementation

[0013] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0014] Example 1 like Figure 1As shown, this embodiment discloses a submersible compact assembly beacon navigation method based on five-dimensional augmented error, including the following steps: S1: Obtain DVL velocity, compass heading angle and submersible depth information, perform coordinate transformation and continuously calculate the nominal position of the submersible; S2: If there is no valid single beacon distance observation, construct a five-dimensional augmented error state model based on the DVL velocity correction, compass error, and two-dimensional position error. Construct a state matrix based on the model and calculate the prior covariance matrix; wherein, the compass error is defined according to the periodic change of the heading angle. S3: If it exists, obtain the single beacon distance observation and calculate the distance measurement matrix; use the distance measurement matrix to update the augmented error state and its covariance, and use the updated result as the posterior estimate; S4: Based on posterior estimation, position, DVL velocity and compass error are compensated, and the corrected submersible navigation results are continuously output.

[0015] Next, combined Figure 1 This embodiment provides a detailed description of a submersible compact assembly single beacon navigation method based on five-dimensional augmented error.

[0016] This embodiment uses two second harmonic coefficients to characterize the heading-related compass error, which together with the DVL velocity scale correction and the two-dimensional position error constitute a fixed five-dimensional augmented error state. The dynamic coupling relationship between each error state is established through velocity projection and position propagation. The single beacon distance residual is used for recursive estimation, and the estimation result is fed back to the dead reckoning process to realize online estimation and closed-loop compensation of the submersible navigation error.

[0017] The compact navigation system of a submersible includes navigation equipment, acoustic ranging equipment, individual acoustic beacons, and a navigation processor. The navigation equipment includes a DVL (Direct Velocity Measurement) system, a compass, and a depth sensor, which provide the submersible's speed, heading, and depth information, respectively. The acoustic ranging equipment acquires the distance between the submersible and the acoustic beacons. Individual acoustic beacons can be fixed beacons with known positions or moving beacons whose positions are available at the time of observation. The navigation processor performs nominal dead reckoning, five-dimensional augmented error state recursive estimation, single-beacon distance measurement updates, and closed-loop compensation, and outputs the corrected submersible navigation results.

[0018] (a) Acquire navigation data and perform nominal dead reckoning The motion state of the submersible is described using the submersible body coordinate system and the navigation coordinate system.

[0019] The submersible's location is indicated as follows: ; in, , and These represent the latitude, longitude, and altitude / depth of the submersible, respectively, and T represents the transpose operation.

[0020] The location of a single acoustic beacon is represented as follows: ; in, , and These represent the vertical positions corresponding to the latitude, longitude, and depth of a monoacoustic beacon, respectively.

[0021] The DVL outputs the submersible's velocity in either the body coordinate system or the DVL equipment coordinate system. Depending on the installation relationship between the DVL and the submersible, the DVL output velocity is converted to the submersible's navigation coordinate system.

[0022] Specifically, let the velocity in the submersible's body coordinate system be: ; in, , and These represent the longitudinal, lateral, and vertical velocities in the submersible's body coordinate system, respectively.

[0023] Using the heading angle output by the compass The horizontal velocity in the body coordinate system is further transformed to the navigation coordinate system to obtain the northward velocity. and eastward speed : ; in, and These are the northbound and eastbound speeds, respectively. This is the horizontal velocity transformation matrix from the body coordinate system to the navigation coordinate system. The specific form of the transformation matrix is ​​determined based on the zero point and positive direction of the heading angle, and remains consistent throughout the nominal dead reckoning, error state modeling, and closed-loop compensation processes.

[0024] The nominal position of the submersible is propagated continuously at horizontal velocity: ; ; in, , These represent the nominal latitude and nominal longitude of the submersible, respectively. and These are the meridian radius of curvature and the trochanter radius of curvature at the current latitude, respectively.

[0025] When single beacon distance observation is temporarily unavailable, the navigation processor continues to perform nominal dead reckoning using DVL velocity and compass heading.

[0026] Specifically, the navigation processor acquires DVL speed, compass heading angle, and submersible depth information based on the timestamps of the sensor data, and synchronizes the data with different update frequencies.

[0027] Based on navigation coordinate system speed and navigation update cycle The nominal location of the submersible: ; ; in, and The first The nominal latitude and nominal longitude at any given time and They represent the first The best estimated values ​​of posterior latitude and posterior longitude after observation correction at any given time; the superscript + represents the posterior state, and the superscript - represents the prior nominal position obtained through propagation by the motion model; and They represent the first The northbound and eastbound velocities at any given time; and The first The radius of curvature of the meridian and the radius of curvature of the zonal circle at the latitude of the time; Indicates the first The vertical position corresponding to the submersible's height / depth at any given time.

[0028] (ii) Constructing augmented error states and performing time prediction Considering that the compass error may change periodically with the heading angle of the submersible, it is difficult to fully describe the characteristics of its amplitude and sign changing with the heading using a single constant heading deviation.

[0029] Therefore, this embodiment uses a periodic basis function of the heading angle to describe the compass error. The navigation processor calculates the heading-related second harmonic basis function based on the current nominal heading angle, horizontal speed, and nominal position: ; ; in, For the first nominal heading angle at any moment For the first The second harmonic cosine basis function corresponding to the heading at a given time. For the first The second harmonic sinusoidal basis function corresponding to the time-to-heading coordinates.

[0030] To determine the model form of the heading-related compass error, the output heading of the submersible compass and the output heading of the reference heading device are obtained, and the heading residual between the two is calculated. The complete range of heading angle variation is divided into several heading intervals, and the heading residual samples are assigned to the corresponding intervals according to the compass heading angle.

[0031] Considering that the dwell time and sample size of the submersible may vary in different heading intervals, to avoid the heading intervals with a large number of samples having too much weight in the model fitting, the median of the heading residuals is extracted for each valid heading interval, and each valid heading interval is assigned the same fitting weight. The dispersion of the heading residuals within each heading interval is represented by the interquartile range.

[0032] A constant deviation model, a first-order harmonic model, a second-order harmonic model, and a third-order harmonic model were constructed respectively, and the candidate models were compared using the root mean square error of fitting, the root mean square error of cross-validation, and the Bayesian information criterion.

[0033] Under the data conditions of this embodiment, the root mean square error of fitting and the root mean square error of cross-validation for the constant deviation model are 2.8804° and 3.0405°, respectively; the corresponding results for the second harmonic model are 0.0334° and 0.0397°, respectively, and the second harmonic model has the lowest Bayesian information criterion. Therefore, the following equation is chosen to describe the relationship between compass error and heading angle: ; in, Output the heading angle for the compass. This represents the compass error corresponding to the current heading angle. and These are the second harmonic cosine coefficient and the sine coefficient, respectively.

[0034] Considering that existing DVL / compass integrated navigation methods typically use a single constant heading deviation to describe compass error, but the compass error of a submersible may undergo periodic changes in amplitude and sign with the heading angle, the constant deviation model cannot adequately describe this type of error. The unmodeled portion will result in continuous position drift through horizontal velocity projection and position integration. Therefore, this embodiment uses second harmonics. Describe the heading-related compass error and use the single beacon distance residual to calculate the second harmonic cosine coefficient. and sine coefficient Online estimation of augmented error states improves the ability to describe heading-related compass errors, enabling the compass error compensation to change in real time with the current heading angle, thereby reducing the transmission of heading error model mismatch to horizontal velocity and position.

[0035] The offline analysis described above is used to determine the functional form of the heading-related compass error. During the online navigation phase, there is no need to configure a reference heading device; instead, [the following is used:] and As an augmented error state, the single beacon distance residual is used for recursive estimation, and the compass error compensation is calculated in real time based on the current heading angle.

[0036] Furthermore, to jointly describe the DVL velocity scale error, heading-related compass error, and horizontal position error, a five-dimensional augmented error state is constructed: ; in, This is the DVL velocity scale correction amount. and These are the error parameters for the second harmonic compass. and These are latitude error and longitude error, respectively.

[0037] Under small error conditions, the horizontal velocity error caused by DVL velocity scale error and heading error can be expressed as: ; ; in, This is the DVL velocity scale correction.

[0038] Substituting the second harmonic compass error model into the equation, we can obtain the propagation relationship of the horizontal position error: ; ; in, , These represent the latitude error and longitude error of the submersible, respectively. .

[0039] DVL velocity scale error, compass error, and horizontal velocity deviation can all be represented as position drift through velocity integration. Given limited single-distance observation information, simply adding multiple horizontal velocity deviation states increases state dimension, computational complexity, and state correlation, leading to competing interpretations of the same distance residual by multiple states, affecting estimation stability and hindering low-complexity online implementation. Therefore, this embodiment configures five-dimensional states based on the main error sources and their propagation relationships, reducing redundant state coupling while retaining the ability to describe velocity scale error and heading-related errors.

[0040] Therefore, the five-dimensional augmented error state model can be uniformly represented as: ; in, The state matrix, This is the process noise vector. This represents the five-dimensional augmented error state. In the preferred embodiment, which ignores higher-order curvature coupling terms, the state matrix is ​​represented as: ; in, ; Within a relatively short filtering time, , and It can be regarded as a constant or slowly changing quantity; during continuous navigation, its changes can be described by a stochastic constant model or a random walk model.

[0041] The above model enables the dynamic coupling of the DVL velocity scale correction and the heading-related compass error parameters through the horizontal velocity error and the two-dimensional position error, thereby allowing the recursive estimation of the relevant error parameters using subsequent single-beacon distance observations.

[0042] Furthermore, the continuous state matrix is ​​updated based on the current speed, heading angle, position, and Earth curvature parameters. Furthermore, the continuous error state model is discretized. The discrete state transition matrix can be represented as: ; When the navigation update cycle is short, a first-order approximation can also be used: ; in, Represents the identity matrix.

[0043] The navigation processor uses the discrete state transition matrix to perform prior prediction of the error state: ; in, for The posterior error state estimate at time step 1.

[0044] Simultaneously perform covariance time prediction: ; in, The prior covariance matrix, for The posterior covariance matrix at time step 1. Let be the noise covariance matrix of the discrete process.

[0045] When there is no valid single beacon distance observation at the current moment, the navigation processor only executes steps S1 and S2, and continues to propagate the prior estimate as the output of the current moment.

[0046] (iii) Obtaining single beacon distance observations and constructing distance measurement residuals When a valid single-beacon range observation is obtained, the navigation processor receives the horizontal range or three-dimensional slant range directly output by the acoustic ranging device. When the input is a three-dimensional slant range, it is converted into a horizontal range based on the submersible depth and beacon depth; when the input is a horizontal range, it is directly used for subsequent distance residual construction.

[0047] When acoustic devices output three-dimensional slant range, and filters use horizontal distance as the measurement, the horizontal distance can be calculated using the straight-line propagation approximation: ; in, This represents the measured three-dimensional slope distance. This represents the converted horizontal distance. Indicates the first The vertical position corresponding to the submersible's height / depth at any given time. Indicates the first The vertical position corresponding to the depth of the monoacoustic beacon at any given moment.

[0048] Calculate the local horizontal position difference between the current nominal position of the submersible and the corresponding beacon position. Specifically: The northward position difference can be expressed as ; The eastward position difference can be expressed as ; in, , They represent the first The latitude and longitude of the monoacoustic beacon at any given time.

[0049] Therefore, the slant range is converted into a horizontal distance (when the acoustic equipment outputs the slant range) based on the submersible's depth and the beacon's depth, and this is used as the theoretical horizontal distance between the submersible and the beacon. ; Acoustic measured horizontal distance The difference between the distance and the theoretical horizontal distance is taken as the distance measurement residual: ; By linearizing the distance measurement function to the current nominal position, we obtain the distance measurement equation: ; in, This represents the five-dimensional augmented error state. To account for distance measurement noise, the distance measurement matrix is: ; When position error is defined as the difference between the true position and the nominal position, the position coefficients in the measurement matrix can be expressed as: ; ; If the opposite position error definition is used, the measurement matrix is ​​inverted accordingly.

[0050] The single beacon distance directly constrains the position error along the beacon's line-of-sight direction at the current observation time. The DVL velocity scale correction and the two heading-related compass error parameters do not appear directly in the measurement matrix, but rather have a cumulative effect on the position error through augmented error state propagation, thus indirectly constraining it during continuous state prediction and multiple distance measurement updates.

[0051] Therefore, this embodiment does not directly measure all five states using a single distance observation, but instead utilizes the dynamic coupling relationship between the five states to transfer the continuously obtained single beacon distance constraints to the DVL velocity scale error and heading-related compass error parameters.

[0052] (iv) Perform distance measurement updates The navigation processor calculates the innovation covariance based on the prior covariance matrix, the distance measurement matrix, and the distance measurement noise covariance: ; in, This is the distance measurement noise covariance matrix.

[0053] Kalman gain is calculated as ; New information on distance measurement ; Update the five-dimensional augmented error state using distance measurement innovations: ; Covariance update can be represented as ; To improve numerical stability, the Joseph form can also be used for covariance updates: ; Through continuous state prediction and distance measurement updates, the navigation processor obtains the DVL velocity scale correction, two heading-related compass error parameters, and a posterior estimate of the two-dimensional position error.

[0054] (v) Perform error injection, closed-loop compensation and navigation output. The navigation processor uses the posterior estimation results of the five-dimensional augmented error state for subsequent navigation calculations. Among them, the two-dimensional position error state is used for closed-loop correction of the submersible's nominal position, and the DVL velocity scale error parameters and the two heading-related compass error parameters are not directly fed back to the DVL velocity and compass heading.

[0055] When the position error is defined as the difference between the true position and the nominal position, the corrected nominal position is: ; ; After the position error is injected, the corresponding error status can be reset to zero: ; ; The velocity scale correction and second harmonic coefficient are not directly injected into the DVL velocity or compass heading. Instead, their posterior estimates are retained as initial values ​​for the next navigation cycle, continuing to participate in error state propagation, covariance prediction, and subsequent single-beacon distance measurement updates. These parameters, through their dynamic coupling with the position error, improve the accuracy of subsequent position error estimation.

[0056] After completing the closed-loop correction of the position error, the corrected nominal position is used as the initial position value for dead reckoning in the next navigation cycle. The navigation processor continuously outputs the submersible's position, speed, heading, and navigation trajectory.

[0057] Traditional long-baseline or ultra-short-baseline acoustic positioning systems typically require multiple beacons, array installation and calibration, and complete acoustic position calculation, resulting in a high complexity in terms of equipment quantity and engineering deployment. This embodiment only requires a fixed beacon with a known location or a moving beacon whose location can be obtained at the observation time. Only one scalar distance information is used at each observation time, and the complete acoustic position obtained from the calculation of multiple beacon arrays does not need to be used as filter input to constrain navigation error divergence.

[0058] Example 1 To verify the navigation correction effect of the five-dimensional augmented error state model described in this embodiment from both controlled simulation conditions and actual operating conditions, this embodiment sets up a moving beacon simulation scenario and a fixed beacon test scenario. The following three navigation methods are compared in both scenarios: 1. DVL / compass dead reckoning without using single beacon distance observations; 2. A four-dimensional error state model with constant heading deviation is adopted; 3. The five-dimensional augmented error state model using the second harmonic heading-related error parameters of this embodiment.

[0059] Among them, the four-dimensional error state is ; The five-dimensional augmentation error state used in this embodiment is: ; Each method uses the same submersible motion data, acoustic distance observations, initial state, and filtering parameters in the same verification scenario to ensure consistency in the comparison between different error state models.

[0060] (1) Simulation verification under mobile beacon conditions To verify the navigation correction effect of the five-dimensional augmented error state model described in this embodiment under moving beacon conditions, a simulated submersible track was constructed based on the actual submersible voyage's travel time, speed, and turning characteristics. The simulated travel time was approximately 8800 s, with an average speed of approximately 0.5 m / s. The track consisted of several straight segments and turning segments, forming an approximate triangular rectangle. A single moving acoustic beacon was set to move along a preset irregular trajectory, causing the relative position between the submersible and the moving beacon to continuously change, resulting in multi-moment single-distance observations with different line-of-sight directions. The track's scale, speed, and turning characteristics were basically consistent with the actual voyage.

[0061] The simulation included DVL velocity scale error, compass heading correlation error, depth measurement noise, and acoustic distance measurement noise. The compass error adopted the second harmonic form determined in Example 1, and the relevant parameters were set based on the fitting results of the measured compass residuals. The main sensor and acoustic distance parameters are shown in Table 1.

[0062] Table 1. Main parameter settings for the mobile beacon simulation scenario;

[0063] All comparison methods used the same submersible trajectory, moving beacon trajectory, sensor input, distance observation time, noise sequence, initial state, and filtering parameters to eliminate the influence of other factors on the comparison results.

[0064] Comparison of track and radial position errors, for example Figure 2 As shown, Figure 2 (a) shows the trajectories of the submersible and the moving beacon. The dots represent the starting point, the solid lines represent the submersible's trajectory, and the dashed lines represent the moving beacon's trajectory. The submersible moves south from the starting point and then turns, while the moving beacon moves southeast from near the starting point, forming an arc-shaped trajectory. The relative positions of the two change continuously over time. Figure 2Figure (b) shows the variation of horizontal position error over time for different navigation methods. The dead reckoning (DR) error accumulates rapidly with time, reaching approximately 68 m at about 5000 s, then gradually decreases, remaining at approximately 22 m at the end of the journey. The blue, red, and green dashed horizontal lines represent the mean horizontal position error reference lines for the DR scheme, the four-dimensional constant heading deviation model, and the five-dimensional model in this embodiment, respectively. After adding the moving beacon distance assist, both the four-dimensional constant heading deviation model and the five-dimensional model in this embodiment significantly suppressed the position error. Compared to the four-dimensional model, the error curve of the five-dimensional model in this embodiment is generally more stable, remaining at a lower level for most of the time, and exhibiting better error suppression capabilities in the later stages when error fluctuations are larger. Simulation results are shown in Table 2.

[0065] Table 2 Horizontal positioning error in the simulation scenario of moving beacons;

[0066] After introducing single-distance observations from moving beacons, both the four-dimensional model and the five-dimensional model in this embodiment significantly reduced the cumulative position error of DVL / compass dead reckoning, indicating that the continuous line-of-sight changes caused by moving beacons can provide effective external constraints for single-distance observations.

[0067] Compared to the four-dimensional constant heading deviation model, the root mean square error of the horizontal position in the five-dimensional model of this embodiment is reduced from 6.91 m to 4.37 m, a reduction of 36.8%; the maximum error is reduced from 21.14 m to 10.31 m. The results show that using the two second harmonic coefficients as error states for online estimation can more fully describe the characteristics of compass error changing with heading angle and reduce the error transmitted to horizontal position through velocity projection due to heading error model mismatch. This demonstrates that the heading-related error model provided in this embodiment can improve the error correction effect of single-beacon-assisted navigation.

[0068] (2) Field verification under fixed beacon conditions To verify the feasibility of this embodiment under actual submersible platform and actual acoustic observation conditions, a set of measured navigation data from fixed beacons was selected for navigation comparison. The submersible is equipped with DVL, compass, depth sensor, acoustic ranging equipment, and navigation processor, and fixed acoustic beacons with known locations and depths are set outside the submersible.

[0069] During underwater navigation, the DVL continuously provides speed information, the compass provides heading information, the depth sensor provides depth information, and the acoustic ranging equipment obtains single-beacon distance observations between the submersible and the fixed beacon.

[0070] The navigation filter uses only single-beacon distances and does not use the complete position results obtained from multi-beacon acoustic positioning systems or ultra-short baseline systems. The reference heading device and reference trajectory are only used for offline evaluation of navigation errors and are not used as inputs to the online navigation filter in this embodiment.

[0071] All methods used the same DVL velocity, compass heading, submersible depth, single beacon distance, initial state, and filtering parameters. Comparison of measured track and radial position error is shown below. Figure 3 As shown, Figure 3 (a) shows the measured trajectory of the submersible and its relative position to the fixed beacon in a fixed beacon scenario. The dots represent the starting point of the submersible's journey, the solid lines represent the measured trajectory, and the star marks indicate the position of the fixed beacon. The submersible travels south from the starting point, then turns east and continues moving; the fixed beacon is located southeast of the submersible's trajectory. Figure 3 Figure (b) shows the horizontal position errors of pure dead reckoning, the four-dimensional constant heading deviation model, and the five-dimensional model of this embodiment. The error of pure dead reckoning accumulates significantly over time; although the four-dimensional model introduces a fixed beacon distance constraint, the error still increases to approximately 45 m in the later stages. The blue, red, and green horizontal dashed lines represent the mean horizontal position error reference lines of the DR scheme, the four-dimensional constant heading deviation model, and the five-dimensional model of this embodiment, respectively. The five-dimensional model of this embodiment can further suppress error accumulation, and its overall horizontal position error is lower than that of the four-dimensional model, indicating that the five-dimensional model can more effectively utilize fixed beacon distance information to estimate and compensate for heading-related compass errors and navigation errors online. The positioning error results are shown in Table 3.

[0072] Table 3. Horizontal positioning error in actual fixed beacon scenarios;

[0073] In the selected fixed beacon measurement scenario, the root mean square error of the horizontal position calculated by pure DVL / compass dead reckoning is 33.86 m; after adopting the four-dimensional model with constant heading deviation, the root mean square error is 26.74 m; after adopting the five-dimensional augmented error state model of this embodiment, the root mean square error is further reduced to 14.83 m, which is 44.5% lower than that of the four-dimensional model.

[0074] The relative motion between the fixed beacon and the submersible is mainly generated by the submersible's own navigation, and the changes in line of sight direction it can provide are relatively limited. Using a single constant heading deviation is insufficient to fully describe the characteristics of compass error variation with heading angle during this voyage. However, establishing a heading-related compass error model using two second harmonic parameters can reduce position drift caused by model mismatch, allowing the single beacon distance residual to more effectively correct dead reckoning errors.

[0075] (3) Comparison of state dimensions To examine whether increasing the number of error states can achieve stable improvement, two horizontal velocity deviation states were added to the five-dimensional state to form a seven-dimensional comparison model.

[0076] In the mobile beacon simulation scenario, the root mean square error (RMSE) of the seven-dimensional model is 3.96 m, slightly lower than the 4.37 m of the five-dimensional model, but its maximum error is 19.85 m, significantly higher than the 10.31 m of the five-dimensional model. In the fixed beacon experimental scenario, the RMSE of the seven-dimensional model is 15.27 m, which is also not better than the 14.83 m of the five-dimensional model.

[0077] The above results indicate that adding horizontal velocity deviation states may improve the filter's ability to absorb unmodeled errors, but it also enhances the correlation between heading error, velocity deviation, and velocity scale error. Under the condition that a single beacon only provides scalar distance observations, increasing the number of states cannot guarantee stable multi-indicator performance improvement and will increase the computational scale of state propagation and covariance updates. Therefore, this embodiment preferably uses fixed five-dimensional augmented error states to balance error description capability, navigation result stability, and computational complexity.

[0078] The simulation and experimental results described above verify the effectiveness of this embodiment under different application conditions.

[0079] In the moving beacon simulation scenario, after introducing single beacon distance observation, the five-dimensional augmented error state model in this embodiment reduces the root mean square error of horizontal position from 46.38 m calculated by pure DVL / compass dead reckoning to 4.37 m, and further reduces it by 36.8% relative to the four-dimensional model with constant heading deviation. The maximum error is reduced from 21.14 m to 10.31 m, indicating that using second harmonic parameters to describe heading-related compass errors can reduce the impact of mismatch in position reckoning by the constant deviation model.

[0080] In the fixed beacon measurement scenario, the five-dimensional model in this embodiment reduces the root mean square error of the horizontal position from 33.86 m calculated by dead reckoning to 14.83 m, a reduction of 44.5% compared to the four-dimensional model. This indicates that the method can achieve online error estimation and closed-loop compensation under actual submersible sensor data and acoustic ranging conditions.

[0081] Furthermore, after adding two horizontal velocity deviation states to the five-dimensional state, the seven-dimensional model did not exhibit a stable overall performance advantage in both simulation and actual test scenarios. This indicates that the fixed five-dimensional state in this embodiment can achieve a good balance between the ability to describe heading-related errors, the stability of navigation results, and the complexity of filtering calculations.

[0082] The above results demonstrate that the present invention can effectively constrain DVL / compass dead reckoning errors using a single acoustic distance observation, and improve the horizontal navigation accuracy of submersibles while maintaining low state dimension and system complexity. The above values ​​are implementation results under specific platform, trajectory, acoustic geometry, and filtering parameter conditions, and do not constitute a limitation on the scope of application or navigation accuracy of the present invention.

[0083] This specific embodiment abandons the traditional high-cost inertial navigation and multi-node underwater acoustic arrays, and builds a minimalist hardware architecture that integrates DVL, compass, and single beacon, eliminating the need for buoyancy correction and reducing equipment and logistical investment; it uses harmonic coefficients to characterize periodic compass errors, solving the problem that traditional constant deviation models cannot match heading period disturbances and cause severe trajectory drift; it designs a five-dimensional augmented error model without redundant velocity deviation terms, avoiding strong coupling conflicts of multiple errors under single scalar distance observation, and completing the joint recursion and online correction of multiple types of coupled errors with low computational cost, balancing navigation accuracy, solution complexity, and hardware deployment cost.

[0084] Example 2 This embodiment provides a submersible compact assembly single beacon navigation system based on five-dimensional augmented error, including: The nominal position calculation module is configured to acquire DVL velocity, compass heading angle and submersible depth information, perform coordinate transformation and continuously calculate the nominal position of the submersible. The no-beacon update module is configured to, if there is no valid single-beacon distance observation, construct a five-dimensional augmented error state model based on the DVL velocity correction, compass error, and two-dimensional position error, construct a state matrix based on the model, and calculate the prior covariance matrix; wherein, the compass error is defined according to the periodic change of the heading angle; There is a beacon update module, which is configured to, if it exists, acquire single beacon distance observations and calculate the distance measurement matrix; use the distance measurement matrix to update the augmentation error state and its covariance, and use the update result as a posterior estimate; The navigation module is configured to compensate for position, DVL velocity and compass error based on posterior estimation and continuously output the corrected submersible navigation results.

[0085] Example 3 like Figure 4 As shown, this embodiment provides a submersible compact assembly single beacon navigation device based on five-dimensional augmented error, including: This includes navigation equipment, monoacoustic beacons, acoustic ranging devices, and navigation processors; The navigation equipment includes a DVL (Depth Volume), a compass, and a depth sensor, which are used to collect submersible speed, compass heading angle, and depth information, respectively. The monoacoustic beacon is either a fixed beacon with a known location or a moving beacon obtained at the time of observation; The acoustic ranging device is used to obtain distance information between the submersible and the acoustic beacon; The navigation processor is used to execute the steps in the submersible compact single beacon navigation method based on five-dimensional augmented error described in Embodiment 1.

[0086] Specifically, the device includes a navigation system, an acoustic ranging system, a single acoustic beacon, and a navigation processor. The navigation system includes a DVL (Depth-Vehicle Range), a compass, and a depth sensor, used to provide submersible speed, heading, and depth information, respectively. The acoustic ranging system is used to acquire distance information between the submersible and the acoustic beacon. The single acoustic beacon can be a fixed beacon with a known position or a moving beacon whose position is available at the time of observation. The navigation processor performs nominal dead reckoning, five-dimensional augmented error state recursive estimation, single beacon distance measurement update, and closed-loop compensation, and outputs the corrected submersible navigation results.

[0087] The navigation processor receives DVL velocity, compass heading, submersible depth, acoustic distance, and beacon position information at the corresponding observation time, and performs time synchronization based on the timestamps of various data. The navigation processor first continuously propagates the submersible's nominal position using DVL velocity, compass heading angle, and submersible depth; then it constructs a five-dimensional augmented error state consisting of DVL velocity scale corrections, two heading-related compass error parameters, and two-dimensional position errors, and performs time prediction of the error state and its covariance.

[0088] When a valid single-beacon range observation is obtained, the navigation processor calculates the theoretical distance based on the submersible's nominal position and the corresponding beacon position. It then compares the theoretical distance with the measured distance to generate a single-beacon range measurement residual, and updates the five-dimensional augmented error state accordingly. The position error obtained from the measurement update is fed back into the nominal dead reckoning process to correct the submersible's position, achieving closed-loop compensation. When no valid range observation is obtained, the navigation processor continues to perform nominal dead reckoning and error state time prediction.

[0089] Since this embodiment directly uses a single scalar distance residual for filtering and updating, without relying on the complete acoustic position calculated by a multi-beacon system, it forms a tightly coupled data processing relationship of "navigation data input - nominal dead reckoning - error state prediction - distance measurement update - closed-loop compensation - navigation result output", which effectively suppresses long-term underwater dead reckoning accumulation drift and ensures high-precision navigation of the submersible.

[0090] Example 4 This embodiment provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps in the submersible compact assembly single beacon navigation method based on five-dimensional augmented error as described in Embodiment 1 above.

[0091] Example 5 This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the steps in the submersible compact assembly single beacon navigation method based on five-dimensional augmented error as described in Embodiment 1 above.

[0092] The steps or modules involved in Embodiments 2 to 4 above correspond to those in Embodiment 1. For specific implementation details, please refer to the relevant description section of Embodiment 1. The term "computer-readable storage medium" should be understood as a single medium or multiple media including one or more instruction sets; it should also be understood as including any medium capable of storing, encoding, or carrying an instruction set for execution by a processor and enabling the processor to perform any of the methods in this invention.

[0093] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A submersible compact assembly single beacon navigation method based on five-dimensional augmented error, characterized in that, include: Obtain DVL velocity, compass heading angle and submersible depth information, perform coordinate transformation and continuously deduce the nominal position of the submersible; If no valid single beacon distance observation exists, a five-dimensional augmented error state model is constructed based on the DVL velocity correction, compass error, and two-dimensional position error. A state matrix is ​​constructed based on the model, and the prior covariance matrix is ​​calculated. The compass error is defined according to the periodic change of the heading angle. If it exists, obtain single beacon distance observations and calculate the distance measurement matrix; use the distance measurement matrix to update the augmented error state and its covariance, and use the updated result as the posterior estimate; The position, DVL velocity, and compass error are compensated based on posterior estimation, and the corrected submersible navigation results are continuously output.

2. The submersible compact assembly single beacon navigation method based on five-dimensional augmented error as described in claim 1, characterized in that, The process of acquiring DVL velocity, compass heading angle, and submersible depth information, performing coordinate transformation, and continuously recursively calculating the submersible's nominal position specifically includes: The submersible's location is represented based on its latitude, longitude, and depth information; Based on the installation relationship between the DVL and the submersible, the DVL velocity is converted to the submersible navigation coordinate system using the heading angle output by the compass, and the horizontal velocity including the northward velocity and the eastward velocity is obtained. The nominal position of the submersible is continuously recursively calculated based on its position using the horizontal velocity.

3. The submersible compact assembly single beacon navigation method based on five-dimensional augmented error as described in claim 1, characterized in that, The construction of the five-dimensional augmented error state model specifically includes: The periodic variation characteristics of the heading amplitude and sign are described using the second harmonic, the compass error is defined, and the cosine coefficient and sine coefficient of the second harmonic are obtained. A five-dimensional augmented error state model is constructed based on the DVL velocity correction, the cosine coefficient, the sine coefficient, the latitude error, and the longitude error.

4. The submersible compact assembly single beacon navigation method based on five-dimensional augmented error as described in claim 1, characterized in that, The process of constructing the state matrix and calculating the prior covariance matrix based on the model specifically includes: A state matrix is ​​constructed based on a five-dimensional augmented error state model. The state matrix is ​​updated according to the current velocity, heading angle, position, and Earth curvature parameters, and then discretized into a discrete state transition matrix. The prior prediction of error states is performed using the discrete state transition matrix, and the prior covariance matrix is ​​calculated.

5. The submersible compact assembly single beacon navigation method based on five-dimensional augmented error as described in claim 1, characterized in that, The process of acquiring single beacon distance observations and calculating the distance measurement matrix specifically includes: The measured horizontal distance between the submersible and the beacon was calculated based on effective single beacon distance observations, and the theoretical horizontal distance was calculated based on the northward and eastward position differences. The difference between the measured horizontal distance and the theoretical horizontal distance is taken as the distance measurement residual: The distance measurement residual is linearized to first order at the current nominal position of the submersible to obtain the distance measurement equation and the distance measurement matrix.

6. The submersible compact assembly single beacon navigation method based on five-dimensional augmented error as described in claim 1, characterized in that, The method of updating the augmented error state and its covariance using the distance measurement matrix and using the updated result as a posterior estimate specifically includes: Based on the prior covariance matrix, the distance measurement matrix, and the distance measurement noise covariance, calculate the innovation covariance; Calculate the Kalman gain matrix at the current time based on the prior covariance matrix, the distance measurement matrix, and the innovation covariance. The distance measurement information at the current moment is calculated based on the difference between the measured horizontal distance and the predicted distance measurement value calculated based on the current predicted state. Based on the Kalman gain matrix and the distance measurement information, the current error state prior estimate is corrected to obtain the updated error state. Based on the Kalman gain matrix and the distance measurement matrix, the prior covariance matrix is ​​corrected to obtain the updated error state covariance matrix; Through continuous state prediction and distance measurement updates, the posterior estimate of the DVL velocity scale correction, two heading-related compass error parameters, and two-dimensional position error is obtained.

7. A submersible compact assembly single beacon navigation system based on five-dimensional augmented error, characterized in that, include: The nominal position calculation module is configured to acquire DVL velocity, compass heading angle and submersible depth information, perform coordinate transformation and continuously calculate the nominal position of the submersible. The no-beacon update module is configured to, if there is no valid single-beacon distance observation, construct a five-dimensional augmented error state model based on the DVL velocity correction, compass error, and two-dimensional position error, construct a state matrix based on the model, and calculate the prior covariance matrix; wherein, the compass error is defined according to the periodic change of the heading angle; There is a beacon update module, which is configured to, if it exists, acquire single beacon distance observations and calculate the distance measurement matrix; use the distance measurement matrix to update the augmentation error state and its covariance, and use the update result as a posterior estimate; The navigation module is configured to compensate for position, DVL velocity and compass error based on posterior estimation and continuously output the corrected submersible navigation results.

8. A submersible compact assembly single beacon navigation device based on five-dimensional augmented error, characterized in that, This includes navigation equipment, monoacoustic beacons, acoustic ranging devices, and navigation processors; The navigation equipment includes a DVL (Depth Volume), a compass, and a depth sensor, which are used to collect submersible speed, compass heading angle, and depth information, respectively. The monoacoustic beacon is either a fixed beacon with a known location or a moving beacon obtained at the time of observation; The acoustic ranging device is used to obtain distance information between the submersible and the acoustic beacon; The navigation processor is used to perform the steps in the submersible compact single beacon navigation method based on five-dimensional augmented error as described in any one of claims 1-6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps in the submersible compact single beacon navigation method based on five-dimensional augmented error as described in any one of claims 1-6.

10. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps in the submersible compact single beacon navigation method based on five-dimensional augmented error as described in any one of claims 1-6.

Images