An unmanned ship cluster cooperative positioning method based on SOM network adaptive screening

Abstract

The application discloses a kind of based on SOM network self-adapting screening unmanned ship cluster cooperative positioning method, belong to navigation and positioning field.The method includes: the output of GNSS, INS, GMNS and other heterogeneous navigation sources is modeled as Gaussian probability model;With the ranging and direction finding information between ships, the positioning information of cooperative ship is converted into indirect estimation of target ship, forming a complete positioning information set;Position data is input into SOM neural network for clustering, based on node density, the threshold is adaptively determined using the maximum between-class variance algorithm, and high-confidence positioning information is selected;Based on information geometry theory, the filtered information is regarded as a point on the Riemannian manifold, and the weighted geometric center is calculated to obtain the final fusion result;Feedback fusion result to optimize the cooperative observation parameters of next iteration cycle.The application can adaptively remove outliers and uniformly process heterogeneous data, significantly improving the robustness, accuracy and real-time performance of large-scale unmanned ship cluster cooperative positioning in complex dynamic sea areas.

Description

Technical Field

[0001] This invention belongs to the field of navigation and positioning technology, specifically relating to a collaborative positioning method suitable for large-scale unmanned vessel swarms, and more specifically to a collaborative positioning method based on information geometry theory and using a self-organizing map (SOM) network adaptive filtering mechanism to optimize the navigation information of unmanned vessel swarms. Background Technology

[0002] Unmanned surface vehicle (USV) swarms have significant application value in fields such as marine surveying and environmental monitoring. The robustness and real-time performance of their collaborative navigation systems are key to achieving intelligent swarm control.

[0003] Existing systems typically integrate multi-source information, including Global Navigation Satellite System (GNSS), Inertial Navigation System (INS), Geomagnetic Matching Navigation System (GMNS), and ship-to-ship ranging and direction finding, for positioning. However, in complex waters such as ports and nearshore areas, GNSS signals are susceptible to interference from obstruction and multipath effects, and sensor data fluctuates dramatically due to wind and wave disturbances, leading to the accumulation of positioning errors. Existing methods have significant shortcomings in addressing the above scenarios: First, data filtering often relies on fixed thresholds set based on experience, making it difficult to adaptively remove "outliers" in dynamic environments. This leads to the accidental deletion of valid information or the omission of abnormal noise, severely reducing the robustness of the positioning system. Second, the heterogeneous navigation source data formats, calculation principles, and error characteristics are inconsistent, lacking a unified framework for probabilistic modeling and uncertainty representation. This makes it difficult to fully utilize the complementary advantages of each navigation source at the fusion level, resulting in a significant drop in system accuracy when a single source fails. Third, as the cluster size expands, the amount of data increases dramatically, placing a heavy computational burden on traditional centralized processing or complex detection algorithms, making it difficult to meet millisecond-level real-time requirements. Summary of the Invention

[0004] The purpose of this invention is to address the problems of poor cooperative positioning accuracy and low system stability in existing large-scale USV clusters operating in complex marine environments. These problems stem from inconsistent formats of multi-source heterogeneous navigation data, severe outlier interference, and the difficulty of balancing real-time performance and robustness with traditional screening methods. This invention proposes a large-scale USV cluster cooperative positioning method that integrates SOM network adaptive screening technology and information geometry theory. This method constructs a normalized information probability model of heterogeneous navigation sources, utilizes the topological mapping characteristics and maximum inter-class variance principle of the SOM neural network to adaptively screen outlier data, and combines information geometry theory to achieve high-precision fusion. This enables highly reliable and real-time cooperative positioning of USV clusters in dynamic and complex environments.

[0005] To achieve the above objectives, the technical solution provided by this invention is:

[0006] A collaborative localization method for unmanned surface vessels (USVs) swarms based on adaptive filtering using a Search Engine Optimization (SOM) network is provided, comprising the following steps:

[0007] Step 1, normalized information probability modeling of heterogeneous navigation sources: For GNSS, GMNS and INS carried by unmanned vessels, a unified information probability model is constructed respectively; for GNSS navigation source, the position correction is calculated by the least squares method and the positioning error covariance is calculated by the error propagation law; for GMNS navigation source and INS navigation source, an online adaptive statistical fitting method is adopted.

[0008] Step 2, position transformation of collaborative observation information: Using the ranging and direction finding information of the collaborative vessels in the unmanned vessel cluster relative to the vessel to be located, the navigation source positioning information of the collaborative vessels is transformed into an indirect position estimate and the corresponding covariance matrix of the vessel to be located. Combined with the positioning information of each navigation source of the vessel to be located, a complete set of positioning information pairs is generated.

[0009] Step 3, adaptive data filtering based on the SOM neural network, specifically includes:

[0010] Step 3.1, Data Preprocessing: Standardize the location estimation vectors in the set based on the location information;

[0011] Step 3.2, SOM network mapping: Input the standardized position vectors into the SOM neural network for training, and establish the mapping relationship between each input position vector and the two-dimensional grid node;

[0012] Step 3.3, Node density statistics: After the SOM network converges, count the number of input position vectors gathered by each grid node to obtain the density of each node;

[0013] Step 3.4, Adaptive Threshold Filtering: Based on the node density set obtained in Step 3.3, the optimal density filtering threshold is calculated using the maximum inter-class variance algorithm; then, according to the mapping relationship established in Step 3.2, the original positioning information pairs corresponding to nodes with node densities higher than the threshold are filtered out to form an optimized positioning information pair set.

[0014] Step 4, Information Geometry Multi-Source Fusion: Based on information geometry theory, the optimized positioning information pairs obtained in Step 3 are regarded as points on the Riemann manifold, and the weighted geometric center of their corresponding Gaussian probability density function is calculated to obtain the final fused position estimate and fused covariance matrix of the ship to be located.

[0015] Step 5, Feedback closed-loop optimization: Using the final fused position estimate obtained in this iteration, correct the initial value and variance of the cooperative ranging and direction finding information in the next iteration cycle to form a closed-loop optimization process.

[0016] Furthermore, information probability models for GMNS and INS navigation sources are constructed using the following methods:

[0017] Calculate the position estimate output by the navigation source at the previous moment. Final fusion position with the previous moment The differences between them yield observation error samples. ;

[0018] Using the exponentially weighted moving average rule, based on the deviation vector of the previous time step... Covariance Matrix Recursively update the deviation vector at the current time step. and covariance matrix The updated formula is as follows:

[0019]

[0020]

[0021] In the formula, and These are the corresponding forgetting factors for the bias term and the forgetting factor for the covariance matrix, respectively.

[0022] The current position estimate output by the navigation source is used as the mean. The updated version As the covariance matrix Construct a normalized probability model .

[0023] Furthermore, in step 2, the position transformation formula for the vessel to be located by the cooperating vessels is:

[0024]

[0025]

[0026] In the formula, For the first The next iteration is for ships to be located. The Collaborating vessels The Navigation source positioning results from an independent positioning source. For the first Next iteration of collaborative ships Location results The transformed information about the ship to be located Location information, For the first Next iteration of collaborative ships Relative to the vessel to be located The ranging and direction finding information, among which The distance measurement value, and These are the polar angle and azimuth angle in the direction finding information, respectively. For the first Next iteration of collaborative ships Location results The covariance matrix of the error, For the first The information about the ship to be located obtained from the next iteration transformation Location information The covariance matrix of the error, This represents the distance measurement variance.

[0027] Furthermore, in step 3.1, the standardization process specifically involves:

[0028] Calculate all location estimation vectors In all dimensions Sample mean with standard deviation ,in , Number of samples in the original navigation and positioning data:

[0029]

[0030]

[0031] according to Calculations are performed to obtain the standardized vector. ,in express In the Standardized feature values ​​in each dimension.

[0032] Furthermore, in step 3.2, the training process of the SOM neural network includes: for the input standardized vector By calculating its weight vector relative to each neuron node in the SOM network. The Euclidean distance between them is used to determine the best matching unit, and the index of the best matching unit is... Depend on The following formula is given; and the weight vectors of the best matching unit and its neighborhood nodes are iteratively updated according to the following formula:

[0033]

[0034] In the formula, To the number of training iterations, The learning rate decays over time. For index Best matching unit nodes and nodes Neighborhood functions between them.

[0035] Furthermore, the neighborhood function is a Gaussian neighborhood function, and is defined as follows:

[0036]

[0037] In the formula, and These represent the indexes as follows: Best matching unit nodes and nodes Geometric position within the 2D mesh of the SOM network is the neighborhood radius that decays over time.

[0038] Furthermore, in step 3.4, the optimal density screening threshold is calculated using the following formula:

[0039]

[0040] In the formula, To determine the optimal density screening threshold, For the set of node densities, This is a candidate threshold for traversing between the minimum and maximum node density values. and respectively with The proportion of background and foreground nodes classified by the threshold to the total number of nodes. and These represent the average density of background nodes and foreground nodes, respectively.

[0041] Furthermore, in step 4, the final fused location estimate and fused covariance matrix are calculated using the following formula:

[0042]

[0043]

[0044] In the formula, For the final fusion location estimation results, To finally fuse the covariance matrix, To optimize the number of information pairs in the location information pair set, the first... Each information pair is , These are the corresponding weighting factors.

[0045] The advantages of this invention are:

[0046] 1. This invention achieves deep probabilistic normalization and unified modeling of multi-source heterogeneous navigation data. Addressing the fundamental differences in data format, solution principles, and error characteristics among GNSS, GMNS, and INS navigation sources, this invention proposes a unified probabilistic modeling method combining the error propagation law and online adaptive statistical fitting. This method not only solves the problem of direct fusion of heterogeneous data but also captures and quantifies the matching ambiguity of GMNS and the cumulative drift of INS in real time, enabling the system to dynamically perceive the true reliability of each navigation source, laying a solid probabilistic foundation for subsequent high-precision fusion.

[0047] 2. This invention proposes an adaptive data filtering mechanism based on SOM networks and maximum inter-class variance (MOL), significantly improving the system's robustness and environmental adaptability. Addressing the problem of frequent outlier interference and time-varying statistical characteristics in complex and dynamic sea areas, this invention abandons the traditional filtering method relying on fixed thresholds. By utilizing the topology-preserving property of SOM networks to cluster positioning data and adaptively determining the optimal density threshold using the MOL algorithm, the system can automatically and accurately identify and eliminate outlier noise caused by multipath effects, sensor malfunctions, or severe disturbances based on real-time data distribution. This ensures the data quality of the input fusion stage, thereby significantly enhancing the positioning system's anti-interference capability in non-stationary environments.

[0048] 3. This invention balances the high precision and real-time performance requirements of large-scale unmanned surface vessel (USV) swarm collaborative positioning. Through pre-screening using a SOM network, this invention significantly reduces the data size entering complex fusion calculations while retaining rich geometric information from the effective data, effectively lowering the overall computational and communication load of the system. Furthermore, by introducing information geometric fusion theory, a weighted geometric average of the Gaussian distribution is directly applied to the Riemannian manifold, avoiding the linearization approximation errors of traditional filtering methods when dealing with nonlinear and non-Gaussian problems. This method is highly efficient and particularly suitable for large-scale USV swarm collaborative operation scenarios with stringent requirements for positioning accuracy, response speed, and system stability.

[0049] 4. This invention establishes a closed-loop optimization architecture, improving positioning convergence speed and long-term stability. By feeding the high-precision fusion results of the current iteration into the cooperative observation model of the next iteration cycle to correct ranging uncertainties and update direction-finding parameters, this invention constructs a self-optimizing closed-loop system. This mechanism can continuously correct cooperative observation errors, suppress error accumulation, thereby accelerating positioning convergence and ensuring the stability and reliability of the system during long-term operation. Attached Figure Description

[0050] The above and / or other features and advantages of the present invention will become more readily understood from the following description with reference to the accompanying drawings, in which:

[0051] Figure 1 This is a schematic diagram of the topology of the unmanned vessel swarm positioning system according to an embodiment of the present invention;

[0052] Figure 2 This is a flowchart illustrating the overall architecture and data processing of the unmanned vessel cluster collaborative positioning method based on SOM network adaptive filtering, according to an embodiment of the present invention.

[0053] Figure 3 This is a schematic diagram illustrating the training and mapping principle of the SOM neural network based on navigation and positioning data input used in this embodiment of the invention. Detailed Implementation

[0054] The present invention will now be described in detail with reference to the accompanying drawings and exemplary embodiments thereof. It should be noted that the following detailed description of the present invention is for illustrative purposes only and is not intended to limit the scope of the invention.

[0055] This invention is directed to, for example Figure 1 The diagram illustrates a large-scale unmanned surface vessel (USV) swarm collaborative positioning scenario. It depicts the collaborative operation of multiple USVs facing challenges such as satellite signal obstruction, weather interference from clouds, rain, and lightning, sea conditions, and obstacles like islands and reefs. The dashed circles represent local communication and sensing ranges, and the uniformly styled vessels within these circles represent USV nodes within the swarm. Each USV is equipped with independent heterogeneous navigation sources (such as GNSS, INS, GMNS, etc.) and communication equipment, enabling it to locate itself. The links between vessels represent the ability for USVs to perform distance and direction finding and communicate with each other. This invention aims to achieve high-precision, robust, and real-time positioning results for target vessels in complex and dynamic marine environments by comprehensively utilizing the multi-source navigation information of the target vessel itself and indirect observation information from collaborating vessels through an efficient collaborative positioning method.

[0056] Reference Figure 2The core process of the unmanned surface vessel (USV) swarm cooperative localization method based on SOM network adaptive filtering, as an exemplary embodiment of the present invention, includes five main steps: probabilistic modeling of normalized information from heterogeneous navigation sources, position transformation of cooperative observation information, adaptive data filtering based on SOM neural network, information geometric multi-source fusion, and feedback optimization. Each step will be described in detail below based on this framework.

[0057] Step S1: Probabilistic Modeling of Normalized Information from Heterogeneous Navigation Sources

[0058] To achieve effective fusion of multi-source information, the primary task is to uniformly represent the outputs of various navigation sources, which differ significantly in format, principle, and error characteristics, into a comparable probabilistic form. This invention employs targeted methods to construct unified Gaussian probability models for GNSS, GMNS, and INS respectively. , where the mean For location estimation, the covariance matrix The uncertainty of the estimate was quantified.

[0059] For GNSS navigation sources, positioning is based on satellite pseudorange measurements. Through linearization, a mathematical model is established to describe the relationship between the pseudorange measurement residuals and the position correction to be determined. This model can be concisely expressed as a matrix equation:

[0060]

[0061] In the formula, The actual pseudorange measurement and the approximate value calculated based on the approximate position. The difference, The coefficient matrix is ​​determined solely by the geometric configuration between the satellite and the receiver. This represents the offset of the approximate position and the correction amount for the receiver clock difference. To obtain... The optimal result is obtained by solving the matrix equation using the least squares method, and the solution can be given by the following formula:

[0062]

[0063] In the formula, This is the optimal correction for the approximate position. This formula calculates the optimal correction value for the initial approximate position by minimizing the sum of squares of measurement errors. Includes three-dimensional position correction. And receiver clock difference correction. Final positioning result. That is, an approximate value With 3D position correction sum.

[0064] After obtaining the USV's GNSS-based positioning results Next, the uncertainty of the ship's positioning results needs to be quantified, which is achieved by calculating the covariance matrix of the positioning error. The positioning error originates from the error in the original pseudorange measurement and propagates to the final positioning result through the solution process. Based on the assumption that the pseudorange errors of each satellite are independent and follow the same Gaussian distribution, its variance is... Based on the law of covariance propagation, the core formula describing the uncertainty of positioning results can ultimately be derived:

[0065]

[0066] This formula illustrates that the positioning error covariance matrix... It depends directly on the error level of the original pseudorange measurement and the satellite's geometry. Due to the final positioning result... It is an approximation Add three-dimensional position correction Obtain, and It is treated as a constant in a single iteration, therefore The 3×3 submatrix corresponding to the top left corner of the three-dimensional position components is... The covariance matrix is ​​used to describe the uncertainty of the positioning results.

[0067] Through the above derivation process, not only were the GNSS positioning results obtained, but the mean of the information probability model was also used. We also obtained the covariance matrix describing the uncertainty of this result. The covariance matrix of the information probability model Together, these two constitute the final probabilistic model of GNSS navigation source information. This achieves a normalized expression of GNSS navigation source parameter format. The core of this model lies in the fact that its mean is directly given by the positioning solution formula based on pseudorange measurement, while the covariance matrix is ​​derived from the original measurement noise through the error propagation law. This process strictly relies on a well-defined mathematical model, and the quantification of its uncertainty mainly stems from measurement errors within the system.

[0068] Unlike GNSS, which relies on external satellite signals for positioning, GMNS and INS employ drastically different computational logics. While GMNS is also an absolute positioning system, its performance is highly dependent on the external space environment; this positioning uncertainty cannot be easily quantified through internal error propagation. INS, as a completely independent relative position estimation system, experiences a continuous accumulation of positioning errors over time. Its information probability model requires a unique modeling logic adapted to the error accumulation patterns of the inertial measurement unit, and similarly, cannot be easily calculated and constructed using conventional error propagation methods. Therefore, their navigation source information probability models, especially the construction of the covariance matrix, require more adaptive approaches.

[0069] The positioning solution of GMNS is a sequential matching process based on the spatial distribution characteristics of the geomagnetic field and achieved through optimal estimation theory. This process begins with the normalization of navigation source parameters.

[0070] First, the raw geomagnetic measurements acquired by the system are three-dimensional vectors in the carrier coordinate system (hereinafter referred to as the b system). To eliminate the influence of carrier attitude changes on measurements, a navigation coordinate system fixed to the geographical location (hereinafter referred to as...) is established. (system), and with the help of the attitude transformation matrix provided by the inertial measurement unit. Project the measurement vector onto The transformation process can be described as follows:

[0071]

[0072] Obtaining the magnetic field vector in the geographic coordinate system Subsequently, to further eliminate interference from changes in heading angle, scalar characteristic parameters that do not change with the horizontal rotation of the carrier are typically extracted. Among numerous geomagnetic features, the total geomagnetic intensity... It is widely used as a core matching quantity due to its rotational invariance, and its calculation formula is as follows:

[0073]

[0074] In the formula, , and They are respectively The lower magnetic field vector The components are along the three axes of East, North, and Up. The core of the positioning lies in the real-time acquisition of measured geomagnetic feature sequences along the flight path. Compared with pre-mapped and stored geomagnetic reference maps A comparison is performed. This comparison process quantifies the measurement sequence and map data at a given candidate location by constructing a cost function. The degree of mismatch at each point. A commonly used cost function is the sum of squared differences (SSD), which is defined as follows:

[0075]

[0076] In the formula, Representatives with candidate positions The first calculated trajectory starting from the first point One point, Let be the total number of points on the track. Ultimately, the localization problem is transformed into an optimization problem, namely, finding the optimal location within a pre-defined search area determined by the initial position. Inside, find the cost function Geographical location that reaches the minimum value This location is the optimal location estimate for the system. This optimization process can be expressed as:

[0077]

[0078] Therefore, GMNS uses a series of rigorous mathematical steps, including coordinate transformation, feature extraction, cost function construction, and optimization search, to associate the dynamic parameters of the navigation source with static geographic information, thereby achieving a complete derivation and calculation from the initial location to the final accurate positioning result.

[0079] The sequential matching process described above can theoretically provide an optimal position estimate. However, for multi-source fusion localization, its output only contains a deterministic single-point localization result, lacking an estimation of the uncertainty of the result. This uncertainty mainly stems from three aspects: first, the direct influence of sensor noise and attitude error; second, the inherent accuracy of the geomagnetic reference map itself; and third, and most critically, the flat or repetitive features of the geomagnetic field in some areas can cause the minimum point of the matching cost function to become unclear, thus leading to significant matching ambiguity.

[0080] Because these error sources, especially matching ambiguities strongly correlated with spatial location, are difficult to accurately propagate using a fixed analytical model, this invention employs an online adaptive statistical fitting method. This method aims to dynamically evaluate the effectiveness and confidence of the spatial matching process based on the system's actual performance during operation, thereby constructing an information probabilistic model that can reflect environmental changes in real time.

[0081] At any discrete time The geomagnetic matching navigation system outputs an absolute position estimate by matching its sensor readings with pre-stored geomagnetic maps. This vector is a direct product of its spatial matching process and is therefore used as an information probability model. The expected value, that is:

[0082]

[0083] The covariance matrix of the model The update reflects the online identification of the core error mechanism of geomagnetic navigation. This process relies on the previous moment... The system will The final fusion position of moments Position estimation directly output from geomagnetic matching The difference between the two is defined as the observation error sample. :

[0084]

[0085] The algorithm quantifies the combined matching error near the current geographic location, caused by geomagnetic map resolution, map data errors, and local unmodeled magnetic anomalies. To separate regional systematic offsets and random fluctuations from this combined matching error, the algorithm recursively estimates the equivalent observation bias vector. This step aims to track and quantify the systematic and persistent positioning offsets caused by specific spatial environments. Its updates employ the Exponential Weighted Moving Average (EWMA) rule:

[0086]

[0087] In the formula, Forgetting factor of the bias term based on the GMNS model, and These are the deviation vectors from the previous time step and the current time step, respectively.

[0088] After identifying the systematic shift, the algorithm then analyzes the covariance matrix, which characterizes the random uncertainty of the observation process. The update is then performed. The goal of this step is to quantify the random matching error caused by both sensor noise and the fine structure of the geomagnetic field. Its recursive rule is based on a purely random error term that has been removed from systematic biases:

[0089]

[0090] In the formula, The forgetting factor is the covariance matrix based on the GMNS model. It will adaptively increase in regions with sparse or anomalous geomagnetic features, and decrease in regions with significant and stable geomagnetic features.

[0091] Through the above steps, Not only did it obtain the best position estimate for geomagnetic navigation, but it also... As the mean of the information probability model We also obtained the covariance matrix describing the uncertainty of this result. The covariance matrix of the information probability model Together, they constitute the basis of geomagnetic navigation. Probabilistic model of final navigation source information at time step This enables the normalized expression of geomagnetic navigation source parameter format.

[0092] The core calculation process of INS positioning can be concisely viewed as a double-integral dynamic problem, and its complete flow can be summarized by a set of tightly coupled mathematical equations. The entire navigation solution begins with the raw data acquired by the inertial measurement unit: the angular velocity of the carrier measured by the gyroscope. and the specific force measured by the accelerometer .

[0093] First, the positioning system must update the attitude matrix in real time by integrating the angular velocity. This matrix describes from Tie The rotational relationship of the system and the attitude update equation of the system describe the change of attitude over time, as shown in the following equation:

[0094]

[0095]

[0096] Assume the Euler angles of the initial attitude of the carrier are expressed as ,in Indicates the roll angle. Indicates pitch angle, Indicates the heading angle. It is the change in the carrier's attitude that causes the carrier to be relative to itself. The rotational angular rate of the system, and for The resulting antisymmetric matrix.

[0097] After obtaining the accurate attitude matrix, the system can then use the accelerometer... The ratio measured under the system Switch to System, and compensate for gravity. And the Coriolis effect, etc., thus obtaining the carrier in Absolute acceleration under the system The system's velocity update equation is shown below:

[0098]

[0099] This equation is the core dynamic equation of INS. Among them, It is the carrier in The speed of the tether; Convert the specific force measured by the accelerometer to Tie; Represents Coriolis acceleration and centripetal acceleration The combined effects are due to The correction term is introduced because the frame itself is a non-inertial frame, where The geocentric coordinate system (hereinafter referred to as the Earth-fixed coordinate system) is caused by the Earth's rotation. The angular rate of Earth's rotation relative to the geodetic coordinate system. express System relative to The Earth's rotational angular rate is generated by the motion of the carrier; This is the local gravitational acceleration vector. By integrating this equation over time and combining it with the initial velocity... This allows for the continuous calculation of the carrier's velocity at any given moment.

[0100] Finally, by performing two consecutive time integrations on the navigation equation and assigning an accurate initial position, the solution is achieved. and initial velocity This allows the current speed of the carrier to be calculated in real time. and location The position update equation for this system is derived as follows:

[0101]

[0102] The above position derivation reveals the essence of INS: it is an autonomous trajectory extrapolation process that extrapolates future position and attitude based on the initial state by continuously integrating the measured motion information.

[0103] Unlike navigation systems that output absolute position, INS is an integration-based system for relative position estimation. The challenge in modeling INS lies in the fact that inherent sensor biases and noise from the inertial measurement unit accumulate during the integration process of navigation calculations, resulting in drift errors with significant temporal cumulative effects. To accurately characterize this dynamic characteristic within the information probability model, INS also employs an adaptive statistical fitting framework based on internal feedback to evaluate the stability of the positioning results in the INS dead reckoning process.

[0104] The assessment of INS positioning uncertainty focuses on analyzing the correction amount of the fused update to the pure inertial navigation prediction. The system calculates process error samples. This provides a profound quantification of the integrated drift vector accumulated and ultimately calibrated by INS within one integration period, where... for The absolute position estimate calculated from the INS timeline. For the system in The final fusion position at any given moment. It contains persistent drift caused by gyroscope and accelerometer biases, as well as unpredictable fluctuations resulting from random noise integration. Using this sample, the system calculates the equivalent system bias vector of INS using an EWMA recursive rule similar to that described above for GMNS. and the covariance matrix characterizing the random uncertainty in its calculation process. Perform online updates.

[0105] The core difference from geomagnetic models is that... The dynamic changes directly reflect the real-time performance of the inertial measurement unit (IMU). It adaptively increases when the carrier experiences severe maneuvers or vibrations, and decreases during stable operation. Therefore, the covariance matrix ultimately produced by this model is not a static parameter describing absolute position uncertainty, but rather reflects the comprehensive drift stability of the IMU within an integral period under the current operating state. This provides a more accurate dynamic weighting basis for multi-source information fusion, reflecting the reliability of the internal calculation process.

[0106] The above derivation process not only yielded the dead reckoning results from INS, but also... As the mean of the information probability model We also obtained the covariance matrix describing the uncertainty of this result. The covariance matrix of the information probability model Together, they constitute INS's... Probabilistic model of final navigation source information at time step This achieves a normalized expression of the INS navigation source parameter format.

[0107] Thus far, based on the inherent characteristics and error mechanisms of the raw data from each navigation source, this invention has completed the transformation of several heterogeneous navigation sources into normalized information probability models through different targeted methods. The overall methodological framework of this transformation process is as follows: Figure 2 As shown, the transformation from raw observation data to a normalized information probability model is clearly illustrated. The full path.

[0108] Step S2: Location transformation of collaborative observation information

[0109] In the iterative process, the first Next, the vessel to be located In addition to its own navigation source positioning results ( In addition to indicating the serial number of the independent navigation source carried by the unmanned surface vessel, it can also obtain information from sources that can communicate with... The first step in communication, ranging, and direction finding Collaborating vessels Location results from various navigation sources and cooperating vessels Relative to the vessel to be located Distance and direction finding information .

[0110] For the vessel to be located In terms of its cooperating vessels The location information needs to be transformed using relative observation information to achieve information coordination within the USV cluster network. This invention employs... Local spherical coordinate system with origin Compared to The ranging and direction finding information is represented as follows: .in, The distance value represents the distance data measured by the ranging device, and its distance variance is... It is trained from a large amount of measured distance data and is continuously updated during use; Located at the origin place, Located at point place, Defined as polar angle, it is a point and points The angle between the line forming the line and the positive Z-axis satisfies... , Defined as the azimuth angle, it is the angle between the projection of the connecting line onto the XY plane and the positive X-axis direction, satisfying the following conditions: , and Together constitute Compared to The direction finding information.

[0111] Assuming the transformed information is collaborative vessels Treating positioning vessels Location estimation, expressed as ,in For the first Next iteration of collaborative ships Location results The transformed information about the ship to be located Location information, For the first The information about the ship to be located obtained from the next iteration transformation Location information The covariance matrix of the error. The position transformation equations of the cooperative vessels are shown below:

[0112]

[0113]

[0114] Through this relative observation in spherical coordinates, cooperative ships The positioning information can then be converted into the information of the vessel to be positioned through ranging and direction finding. Indirect estimation of location information. This transformation process also uses the error propagation law to quantify the uncertainty of relative observation into the covariance matrix. This completes the unification of all cooperative positioning information within the reference frame of the node to be located. Navigation source localization results of the self-navigation source and its covariance matrix , and all indirect location information obtained through transformation and its covariance matrix The aggregation, or collection of complete location information pairs, is then generated for subsequent processing. ,in This represents the number of samples in the original navigation and positioning data. This refers to the sample sequence number in the location data. , , For the position estimation vector, This step quantifies the uncertainty in its positioning. By unifying information from multiple platforms and sources into the target ship's reference frame, it greatly enriches the geometric and observational information available for positioning.

[0115] Step S3: Adaptive data filtering based on SOM neural network

[0116] Directly fusing the complete set of information generated in step S2, which may contain noise and outliers, will lead to problems such as high computational complexity, poor real-time performance, and poor system stability. For example... Figure 2 and Figure 3 As shown, this invention introduces a SOM neural network for adaptive filtering of location data. The SOM neural network aims to identify, filter, and eliminate location estimation vectors. The outliers are far from the mainstream clusters, so the selection process uses location estimation vectors. The input feature is the covariance matrix, which is an inherent property of the feature. It must be bound to its position vector and retained together in the fusion stage to ensure that the reliability information after quantization is not lost.

[0117] Before screening, to avoid the numerical scale differences between feature dimensions interfering with the SOM network topology learning process, the input position estimation vector needs to be evaluated before network training. Standardization is performed. Each sample in the original navigation and positioning data is represented as a three-dimensional coordinate vector. For its various dimensions Sample mean with standard deviation The calculation is performed, and the expression is:

[0118]

[0119]

[0120] Then, the original features are centered and standardized to obtain normalized feature values:

[0121]

[0122] This yields the standardized vector. In the above formula, Represents a standardized vector In the Standardized feature values ​​in each dimension. Using this as input to the SOM network helps avoid a specific dimension dominating Euclidean distance calculation, improving the network's topology preservation ability and the balance of cluster distribution in high-dimensional spaces. Each vector... Each pair has one and only one complete set of location information. It is bound to the corresponding entity.

[0123] Then, the SOM network is used for network mapping, which mainly consists of steps such as network initialization, competition for the winning node, weight update, and iterative convergence.

[0124] The initialization step begins with the definition of the SOM neural network structure. This invention defines a two-dimensional topology to organize neuron nodes, the number of which is related to the dimension and complexity of the input data. In a two-dimensional SOM mesh, a structure typically includes... A rectangular grid with nodes. and The values ​​are chosen to be as close as possible to enhance the network's clustering ability. The topological relationships between network nodes are defined using a neighborhood function. Next, a weight vector is defined for each node. Initialize weight vector Dimension and navigation source location vector Consistent, then The form is:

[0125]

[0126] In the formula, Weight vector In all dimensions The weighted components on.

[0127] Weights can be initialized randomly, typically distributed randomly within the input data range, to facilitate rapid convergence of the iterative process. The learning rate and neighborhood size are key parameters of the SOM network algorithm, gradually decreasing during training. The learning rate controls the magnitude of each weight update, while the neighborhood size determines which neighboring neurons participate in the weight update. Initially, a large learning rate and a large neighborhood radius can be set, which are then gradually decreased as training progresses.

[0128] In each iteration, the updated SOM normalized vector is... As input data, the Euclidean distance between the node and the weight vector of all nodes is calculated, and the node with the closest distance is found, which is the Best Matching Unit (BMU). The index of the BMU... The calculation formula is as follows:

[0129]

[0130] in, express and The Euclidean distance is calculated using the following formula:

[0131]

[0132] Find the index with the smallest distance. After the node, the node is considered to be related to the input vector. The most matching node, i.e. Mapped to index The choice of BMU is crucial to the SOM network algorithm, determining the direction of subsequent weight updates.

[0133] After determining the index as The nodes (hereinafter referred to as nodes) After a node is converted to a BMU, the weights of that node and its neighboring nodes need to be updated to be closer to the current weights. To adjust the update step size and convergence speed, the network learning rate is defined. , To determine the number of training iterations, and to ensure the network gradually converges, Typically decreases over time. This is the initial learning rate. Below are the SOM network nodes. Weight update calculation formula and learning rate Update the calculation formula:

[0134]

[0135]

[0136] In the formula, To control The time constant of the decay rate Represents a node With nodes The neighborhood relationship between nodes is defined by the neighborhood function. After each training iteration of the SOM network and finding the Base Mutual Element (BMU), not only is the weight vector of the BMU updated, but the weight vectors of other nodes in its neighborhood are also updated. This neighborhood is typically not uniformly distributed, but rather uses a function with decaying characteristics. This describes the different effects of distance. This method uses a family of Gaussian functions to define... This ensures that BMU nodes are more significantly affected by neighboring nodes during updates, specifically in the following form:

[0137]

[0138]

[0139] In the formula, and Representing nodes respectively and nodes Geometric position within the 2D mesh of the SOM network; It is the neighborhood radius, which usually decreases over time to shrink the neighborhood range, thereby achieving asymptotic convergence from global to local. It is the initial neighborhood width; To control The time constant of the decay rate.

[0140] The SOM network algorithm repeatedly performs the steps of node contention and weight update until the convergence condition is met. The convergence condition of the SOM algorithm is:

[0141]

[0142] When the number of iterations Reaching the preset maximum number of iterations Or the weight update variable is less than a preset threshold. When, it means The convergence condition has been met. and Updates have been discontinued.

[0143] After the SOM algorithm converges, the SOM network establishes a high-dimensional to low-dimensional node mapping for the navigation and positioning data. At this point, the data density at each node can directly reflect the representativeness of that node's contribution to the navigation and positioning results. For each node... Statistically analyze the aggregated input data points The number of nodes is defined as the number of nodes. Node density The formula for calculating node density is as follows:

[0144]

[0145] in, The indicator function is defined as follows:

[0146]

[0147] Finally, after the SOM network completes training, the density of the grid nodes is... This reflects the spatial distribution characteristics of the input positioning data. High-density nodes typically correspond to high-confidence true positioning values, while low-density nodes are often induced by outliers or discrete noise. Traditional screening strategies usually rely on empirically set fixed thresholds, which lack robustness in the face of complex and ever-changing marine environments: when environmental disturbances intensify and lead to a decrease in overall density, fixed thresholds may result in a significant loss of effective information; conversely, they may introduce excessive noise.

[0148] To address this problem, this invention employs an adaptive selection strategy based on Otsu's algorithm (OTSU), treating node selection as a binary classification problem targeting density features, aiming to find a globally optimal density selection threshold. This maximizes the statistical difference between the distinguished "high-density effective node set" and "low-density noise node set," thereby enabling adaptive perception of environmental changes.

[0149] The known SOM network contains The set of node densities of n neurons is represented as For any candidate threshold The node density set can be divided into two categories: one is the background class. This represents low-density noise and meets the requirements. Another type is the foreground type. This represents high-density, effective information and satisfies... .

[0150] set up and The percentages in the total number of nodes are respectively and Their corresponding average densities are respectively and Based on the OTSU principle, the objective function for measuring the discriminative power between these two types of nodes is... Defined as:

[0151]

[0152] The calculation formulas for each parameter are as follows:

[0153]

[0154]

[0155]

[0156]

[0157] In the formula, and At the threshold respectively The number of background and foreground nodes.

[0158] Optimal density screening threshold That is, when the inter-class variance function reaches its maximum value. value:

[0159]

[0160] Finally, select all that satisfy the condition. The nodes are added to the candidate set.

[0161] After obtaining the candidate node set, based on the established mapping relationship, the original positioning information is used to... The system retrieves and extracts location information pairs corresponding to all candidate nodes. These real observation data, belonging to high-density areas, directly constitute the input set for subsequent location data processing. Ultimately, a location data set containing... A set of high-quality location information pairs This provides input data with both high accuracy and high confidence for subsequent positioning fusion.

[0162] Step S4: Information Geometric Multi-Source Fusion

[0163] After selecting and obtaining a high-quality optimized set of positioning information pairs, this invention employs information geometry theory for fusion. For the vessel to be positioned, since the random errors of various navigation sources are all Gaussian white noise, it can be approximated that the positioning results of various navigation sources all satisfy a three-dimensional Gaussian distribution. According to information geometry theory, all non-singular Gaussian probability density functions constitute a Riemannian manifold. Therefore, this method treats each positioning information pair in the set as a point on the Riemannian manifold. At this point, the... Location information pairs The probability density function is expressed as a function , The goal of multi-source fusion is transformed into finding the weighted geometric center (i.e., Riemann mean) of these Gaussian distribution points on the manifold. The distribution corresponding to this center is the final fusion result. This method utilizes geodesic distances on the manifold (such as Fisher's information metric), which can more accurately reflect the intrinsic geometric structure between probability distributions compared to traditional Euclidean linear weighting.

[0164] The weighting factors corresponding to the fusion are represented as follows: The information density function after multi-source fusion is expressed as: Then the probability function of the navigation source information can be expressed as:

[0165]

[0166] In the formula, It is a constant term and .

[0167] Since the positioning information from the navigation sources all satisfy a three-dimensional Gaussian distribution and are mutually independent, we can obtain:

[0168]

[0169] Based on the above formula, the multi-source fusion positioning result of the vessel to be located is as follows:

[0170]

[0171] in, For the final fusion location estimation results, The final covariance matrix is ​​obtained by fusing the probability distributions directly on the manifold, avoiding the linear approximation in traditional filtering methods, and is more accurate and natural when dealing with nonlinear and non-Gaussian properties.

[0172] Step S5: Feedback closed-loop optimization

[0173] Finally, this invention feeds back the fusion results of this iteration to the system to improve the quality of collaborative observation information in the next iteration. Specifically, it feeds back the fused location estimation results. The known positioning results of cooperating vessels (which can be obtained by averaging their navigation sources). Compare and calculate the positioning residuals:

[0174]

[0175] In the formula, , and These are the collaborative fusion of the ship's position estimation results on the X, Y, and Z axes. Known positioning results with cooperating vessels The positioning residual between the two periods is used to recalculate the next iteration period (the period). Initial values ​​of ranging and direction finding information required for (times) :

[0176]

[0177]

[0178]

[0179] In the formula, For the first The distance measurement value of the cooperative vessel relative to the vessel to be located in the next iteration. and The first The next iteration coordinates the polar angle and azimuth angle of the vessel relative to the vessel to be located in the direction finding information. Since the ranging information is generally obtained from actual measurements, its value remains unchanged, and the ranging variance is also adaptively adjusted.

[0180] This feedback mechanism uses the current more accurate positioning results to correct the future relative observation model, effectively suppressing the propagation and accumulation of errors, enabling the entire cooperative positioning system to iteratively converge to a more stable and accurate state, which is particularly suitable for long-term, dynamically changing tasks.

[0181] Finally, it should be noted that the features mentioned and / or shown in the above description of exemplary embodiments of the present invention can be combined in the same or similar manner with one or more other embodiments, combined with or substituted for corresponding features in other embodiments. These combined or substituted technical solutions should also be considered to be included within the scope of protection of the present invention.

Claims

1. A collaborative localization method for unmanned surface vessels (USVs) based on adaptive filtering using a SOM network, characterized in that, Includes the following steps: Step 1, Normalized Information Probability Modeling of Heterogeneous Navigation Sources: For the GNSS navigation source, GMNS navigation source and INS navigation source carried by the unmanned vessel, a unified information probability model is constructed respectively; for the GNSS navigation source, the position correction is calculated using the least squares method and the positioning error covariance is calculated using the error propagation law; for the GMNS navigation source and INS navigation source, an online adaptive statistical fitting method is adopted. Step 2, position transformation of collaborative observation information: Using the ranging and direction finding information of the collaborative vessels in the unmanned vessel cluster relative to the vessel to be located, the navigation source positioning information of the collaborative vessels is transformed into an indirect position estimate and the corresponding covariance matrix of the vessel to be located. Combined with the positioning information of each navigation source of the vessel to be located, a complete set of positioning information pairs is generated. Step 3, adaptive data filtering based on the SOM network, specifically includes: Step 3.1, Data preprocessing: Standardize the location information in the set of location vectors; Step 3.2, SOM network mapping: Input the standardized position vectors into the SOM network for training and establish the mapping relationship between each position vector and the node; Step 3.3, Node density statistics: After the SOM network converges, count the number of location vectors gathered by each node to obtain the density of each node; Step 3.4, Adaptive threshold filtering: Based on the density of each node obtained in Step 3.3, the optimal density filtering threshold is calculated using the maximum inter-class variance algorithm; then, according to the mapping relationship established in Step 3.2, the set of location information pairs corresponding to nodes whose node density is higher than the optimal density filtering threshold is filtered out to form an optimized location information pair set. Step 4, Information Geometry Multi-Source Fusion: Based on information geometry theory, the optimized positioning information pairs obtained in Step 3 are regarded as points on the Riemann manifold, and the weighted geometric center of their corresponding Gaussian probability density function is calculated to obtain the final fused position estimate and the final fused covariance matrix of the ship to be located. Step 5, Feedback closed-loop optimization: Use the final fusion position estimate obtained in this iteration to correct the next iteration cycle, forming a closed-loop optimization process.

2. The unmanned surface vessel swarm cooperative positioning method according to claim 1, characterized in that, Information probability models for GMNS and INS navigation sources are constructed using the following methods: Calculate the position estimate output by the navigation source at the previous moment. Merging position with the previous moment The differences between them yield observation error samples. ; Using the exponentially weighted moving average rule, based on the deviation vector of the previous time step... Covariance Matrix Recursively update the deviation vector at the current time step. and covariance matrix The updated formula is as follows: In the formula, and These are the corresponding forgetting factors for the bias term and the forgetting factor for the covariance matrix, respectively. The current position estimate output by the navigation source is used as the mean. The updated version As the covariance matrix Constructing an information probability model .

3. The unmanned vessel swarm cooperative positioning method according to claim 1 or 2, characterized in that, In step 2, the navigation source positioning information of the cooperating vessel is transformed into an indirect position estimate and the corresponding covariance matrix of the vessel to be positioned, specifically as follows: In the formula, For the first The next iteration is for ships to be located. The Collaborating vessels The Navigation source positioning results from an independent positioning source. For the first Next iteration of collaborative ships Location results The transformed information about the ship to be located Location information, For the first Next iteration of collaborative ships Relative to the vessel to be located The ranging and direction finding information, among which The distance measurement value, and These are the polar angle and azimuth angle in the direction finding information, respectively. For the first Next iteration of collaborative ships Location results The covariance matrix of the error, For the first The information about the ship to be located obtained from the next iteration transformation Location information The covariance matrix of the error, This represents the distance measurement variance.

4. The unmanned vessel swarm cooperative positioning method according to claim 1 or 2, characterized in that, In step 3.1, the standardization process specifically involves: Calculate all position vectors In all dimensions Sample mean with standard deviation ,in , Number of samples in the original navigation and positioning data: according to Calculations are performed to obtain the standardized vector. ,in express In the Standardized feature values ​​in each dimension.

5. The unmanned surface vessel swarm cooperative positioning method according to claim 1 or 2, characterized in that, In step 3.2, the training process of the SOM network includes: for the input standardized vector By calculating its weight vector relative to each neuron node in the SOM network. The optimal matching unit is determined by the Euclidean distance between them, and the index of the optimal matching unit is... Depend on The following formula is given; and the weight vectors of the best matching unit and its neighborhood nodes are iteratively updated according to the following formula: In the formula, To the number of training iterations, The learning rate decays over time. For index Best matching unit nodes and nodes Neighborhood functions between them.

6. The unmanned surface vessel swarm cooperative positioning method according to claim 5, characterized in that, The neighborhood function is a Gaussian neighborhood function, and is defined as follows: In the formula, and These represent the indexes as follows: Best matching unit nodes and nodes Geometric position within the 2D mesh of the SOM network is the neighborhood radius that decays over time.

7. The unmanned surface vessel swarm cooperative positioning method according to claim 1 or 2, characterized in that, In step 3.4, the optimal density screening threshold is calculated using the following formula: In the formula, To determine the optimal density screening threshold, For the set of node densities, This is a candidate threshold for traversing between the minimum and maximum node density values. and respectively with The proportion of background and foreground nodes to the total number of nodes. and These are the average densities of the background nodes and the foreground nodes, respectively.

8. The unmanned vessel swarm cooperative positioning method according to claim 1 or 2, characterized in that, In step 4, the final fused location estimate and the final fused covariance matrix are calculated using the following formulas: In the formula, For the final fusion location estimation results, To finally fuse the covariance matrix, To optimize the number of information pairs in the location information pair set, the first... Each information pair is , These are the corresponding weighting factors.

Images