A method and system for filtering underwater vehicle positioning

Abstract

The present application belongs to the technical field of autonomous positioning of underwater vehicles, and specifically discloses a method and system for filtering the positioning of underwater vehicles. The underwater vehicle positioning filtering method provided by the present application obtains target samples by sampling the predicted state of the underwater vehicle at the next moment and the updated positioning data of the underwater vehicle, and judges whether the updated positioning data conforms to the predicted state distribution according to the number of clusters of the target samples, eliminates the interference of abnormal values ​​in the positioning data, and improves the positioning accuracy. At the same time, when it is determined that the updated positioning data conforms to the predicted distribution, the positioning data is dynamically adjusted in combination with the designed adjustment weight matrix, and the predicted state of the underwater vehicle at the next moment is updated, which effectively reduces the local data mutation caused by positioning hysteresis and improves the overall continuity of the filtering results.

Description

Technical Field

[0001] The present application belongs to the technical field of autonomous positioning of underwater vehicles, and more specifically, to a method and system for filtering positioning of underwater vehicles. Background Art

[0002] Underwater vehicles play a vital role in current marine surveys and underwater operations, such as resource exploration, environmental mapping, and maintenance and inspection of marine production structures. Underwater positioning is the key technology for vehicles to autonomously complete the above tasks. The ultra-short baseline positioning system is a sensor widely used for underwater target positioning. However, it is prone to positioning anomalies due to the multi-path effect, and is easily affected by environmental acoustic conditions and cannot feedback positioning data at a fixed frequency, and there is an obvious positioning hysteresis problem. The abnormal values ​​and positioning hysteresis problems of the ultra-short baseline positioning device will cause local jumps in the error input of the vehicle controller. When the positioning data jumps frequently, the controller will output oscillating control quantities, making the vehicle hull attitude unstable. When the positioning data has an abnormal value that is significantly different from the true value of the underwater vehicle position, the control quantity will cause the vehicle to have an erroneous movement trend and deviate from the target.

[0003] Therefore, how to solve the problems of positioning anomalies and positioning hysteresis existing in underwater vehicles in related technologies has become an issue that urgently needs to be solved. Summary of the invention

[0004] In view of the defects of the prior art, the purpose of the present application is to provide an underwater vehicle positioning filtering method and system, which aims to solve the problems of positioning outliers and positioning hysteresis existing in the related art for underwater vehicles.

[0005] To achieve the above objectives, in a first aspect, the present application provides an underwater vehicle positioning filtering method, comprising:

[0006] Determine a predicted state of the underwater vehicle at the next moment according to a position increment of the underwater vehicle at the next moment and a filtering result of the underwater vehicle at the previous moment, wherein the filtering result is used to characterize a predicted position vector of the underwater vehicle in a target coordinate system, and the target coordinate system is used to characterize a north coordinate and an east coordinate corresponding to the predicted position vector;

[0007] In the case where the positioning data of the underwater vehicle is updated, determining a measurement judgment result according to the updated positioning data and the predicted state, wherein the measurement judgment result is used to characterize the number of clusters of target samples, and the target samples are determined according to the updated positioning data and the predicted state;

[0008] In the case where it is determined that the number of clusters is greater than a preset value, taking the predicted state as a filtering result of the underwater vehicle at a next moment;

[0009] When it is determined that the number of clusters is equal to a preset value, the predicted state is updated according to the predicted state, the measurement state transfer matrix of the linear Kalman filter, the Kalman gain, the unit matrix, the updated positioning data and the adjustment weight matrix of the updated positioning data to obtain the filtering result of the underwater vehicle at the next moment, and the adjustment weight matrix is ​​determined according to the Euclidean distance between the predicted state and the updated positioning data and the preset exponential gradient coefficient of each element in the adjustment weight matrix.

[0010] In some embodiments, the method for obtaining the position increment of the underwater vehicle at the next moment includes:

[0011] Inputting the motion state quantity of the underwater vehicle at the previous moment and the control quantity of the underwater vehicle at the previous moment into the target prediction neural network to obtain the position increment of the underwater vehicle at the next moment;

[0012] The target prediction neural network is obtained by inputting the operating state quantity of the underwater vehicle at a historical moment, the control quantity of the underwater vehicle at a historical moment and the positioning data of the underwater vehicle at a historical moment into a preset prediction neural network for training.

[0013] In some embodiments, when the positioning data of the underwater vehicle is updated, determining the measurement judgment result according to the updated positioning data and the predicted state includes:

[0014] Determining the target sample according to the updated positioning data and the predicted state;

[0015] Based on a preset clustering algorithm, the target samples are clustered to obtain the measurement judgment result.

[0016] In some embodiments, determining the target sample according to the updated positioning data and the predicted state includes:

[0017] Taking the updated positioning data as the center, sampling is performed according to a normal distribution to obtain a measurement state sample set;

[0018] Taking the predicted state as the center, sampling is performed according to a normal distribution to obtain a predicted state sample set;

[0019] The target sample is determined according to the measurement state sample set and the prediction state sample set.

[0020] In some embodiments, when it is determined that the number of clusters is equal to a preset value, the predicted state is updated according to the predicted state, the measurement state transfer matrix of the linear Kalman filter, the Kalman gain, the unit matrix, the updated positioning data and the adjustment weight matrix of the updated positioning data to obtain the filtering result of the underwater vehicle at the next moment, including:

[0021] Determining a first adjustment coefficient according to the adjustment weight matrix and the measurement state transfer matrix;

[0022] Determining a second adjustment coefficient according to the first preset noise and the adjustment weight matrix;

[0023] determining the Kalman gain according to the covariance of the first adjustment coefficient, the second adjustment coefficient and the predicted state of the underwater vehicle at the next moment;

[0024] According to the adjustment weight matrix, the updated positioning data, the unit matrix, the predicted state and the measurement state transfer matrix, the updated positioning data is adjusted to obtain target positioning data;

[0025] Determining a third adjustment coefficient according to the Kalman gain and the measurement state transfer matrix;

[0026] The predicted state is updated according to the predicted state, the target positioning data, the measurement state transfer matrix and the third adjustment coefficient to obtain a filtering result of the underwater vehicle at the next moment.

[0027] In some embodiments, the method for obtaining the covariance of the predicted state of the underwater vehicle at the next moment includes:

[0028] The covariance of the predicted state is determined according to the covariance of the filtering result of the underwater vehicle at the previous moment, the second preset noise and the state transfer matrix of the linear Kalman filter.

[0029] In some embodiments, the motion state quantities include the heading angle, heading angular velocity, forward velocity and lateral velocity of the underwater vehicle, and the control quantity includes the steering rudder angle of the underwater vehicle.

[0030] In a second aspect, the present application provides an underwater vehicle positioning filtering system, comprising:

[0031] a first acquisition module, configured to determine a predicted state of the underwater vehicle at a next moment according to a position increment of the underwater vehicle at a next moment and a filtering result of the underwater vehicle at a previous moment, wherein the filtering result is used to characterize a predicted position vector of the underwater vehicle in a target coordinate system, and the target coordinate system is used to characterize a north coordinate and an east coordinate corresponding to the predicted position vector;

[0032] A second acquisition module is used to determine a measurement judgment result according to the updated positioning data and the predicted state when the positioning data of the underwater vehicle is updated, wherein the measurement judgment result is used to characterize the number of clusters of target samples, and the target samples are determined according to the updated positioning data and the predicted state;

[0033] A first positioning module, configured to use the predicted state as a filtering result of the underwater vehicle at a next moment when it is determined that the number of clusters is greater than a preset value;

[0034] The second positioning module is used to update the predicted state according to the predicted state, the measurement state transfer matrix of the linear Kalman filter, the Kalman gain, the unit matrix, the updated positioning data and the adjustment weight matrix of the updated positioning data when it is determined that the number of clusters is equal to the preset value, so as to obtain the filtering result of the underwater vehicle at the next moment, and the adjustment weight matrix is ​​determined according to the Euclidean distance between the predicted state and the updated positioning data and the preset exponential gradient coefficient of each element in the adjustment weight matrix.

[0035] In a third aspect, the present application provides an electronic device comprising: at least one memory for storing programs; and at least one processor for executing the programs stored in the memory. When the program stored in the memory is executed, the processor is used to execute the method described in the first aspect or any possible implementation of the first aspect.

[0036] In a fourth aspect, the present application provides a computer-readable storage medium, which stores a computer program. When the computer program runs on a processor, the processor executes the method described in the first aspect or any possible implementation of the first aspect.

[0037] In a fifth aspect, the present application provides a computer program product. When the computer program product runs on a processor, the processor executes the method described in the first aspect or any possible implementation of the first aspect.

[0038] It can be understood that the beneficial effects of the second to fifth aspects mentioned above can be found in the relevant description of the first aspect mentioned above, and will not be repeated here.

[0039] In general, the above technical solutions conceived by this application have the following beneficial effects compared with the prior art:

[0040] The underwater vehicle positioning filtering method and system provided by the present application obtains target samples by sampling the predicted state of the underwater vehicle at the next moment and the updated positioning data of the underwater vehicle, and determines whether the updated positioning data conforms to the predicted state distribution according to the number of clusters of the target samples, eliminates the interference of abnormal values ​​in the positioning data, and improves the positioning accuracy. At the same time, when it is determined that the updated positioning data conforms to the predicted distribution, the positioning data is dynamically adjusted in combination with the designed adjustment weight matrix, and the predicted state of the underwater vehicle at the next moment is updated, which effectively reduces the local data mutation caused by positioning hysteresis and improves the overall continuity of the filtering results. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 This is one of the flow charts of the underwater vehicle positioning filtering method provided in the embodiment of the present application;

[0042] Figure 2 It is a schematic diagram of the position state prediction neural network structure provided in an embodiment of the present application;

[0043] Figure 3 Schematic diagram of the ultra-short baseline positioning filter architecture provided in an embodiment of the present application;

[0044] Figure 4 This is the second flow chart of the underwater vehicle method provided in the embodiment of the present application;

[0045] Figure 5 is a schematic diagram of a typical underwater vehicle provided in an embodiment of the present application;

[0046] Figure 6 It is a curve diagram of ultra-short baseline positioning filtering data of an underwater vehicle provided in an embodiment of the present application;

[0047] Figure 7 It is a structural schematic diagram of an underwater vehicle positioning filtering system provided in an embodiment of the present application;

[0048] Figure 8 It is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0049] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0050] The term "and / or" in this article is a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. The symbol " / " in this article indicates that the associated objects are in an or relationship, for example, A / B means A or B.

[0051] The terms "first" and "second" in the specification and claims herein are used to distinguish different objects rather than to describe a specific order of the objects. For example, a first response message and a second response message are used to distinguish different response messages rather than to describe a specific order of the response messages.

[0052] In the embodiments of the present application, words such as "exemplary" or "for example" are used to indicate examples, illustrations or descriptions. Any embodiment or design described as "exemplary" or "for example" in the embodiments of the present application should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of words such as "exemplary" or "for example" is intended to present related concepts in a specific way.

[0053] In the description of the embodiments of the present application, unless otherwise specified, "multiple" means two or more than two. For example, multiple processing units refer to two or more processing units, etc.; multiple elements refer to two or more elements, etc.

[0054] The underwater vehicle positioning filtering method and system provided in the present application adopts a multi-layer perceptron to construct a target prediction neural network, and trains the network through a supervised learning method to convert the control quantity and motion state quantity of the underwater vehicle into an estimated result of the position state change of the underwater vehicle, and is integrated into the linear Kalman filter framework to improve the state prediction accuracy. Secondly, the target sample is obtained by sampling the predicted state and the updated positioning data, and a preset clustering algorithm (such as the K-means clustering algorithm) is used to determine whether the updated positioning data conforms to the predicted state distribution, eliminating the interference of positioning outliers. Finally, an exponential gradient weight is designed to dynamically adjust the gain coefficient of the positioning data during the Kalman filter measurement update process, which effectively reduces the local data jump caused by positioning hysteresis and improves the overall continuity of the filtering results.

[0055] The embodiments of the present application are described below in conjunction with the drawings in the embodiments of the present application.

[0056] See also Figure 1 The present application provides an underwater vehicle positioning filtering method, which may include: step 110, step 120, step 130 and step 140.

[0057] Step 110: determining a predicted state of the underwater vehicle at the next moment according to a position increment of the underwater vehicle at the next moment and a filtering result of the underwater vehicle at the previous moment, wherein the filtering result is used to characterize a predicted position vector of the underwater vehicle in a target coordinate system, and the target coordinate system is used to characterize a north coordinate and an east coordinate corresponding to the predicted position vector;

[0058] Step 120: when the positioning data of the underwater vehicle is updated, determining a measurement result according to the updated positioning data and the predicted state, wherein the measurement result is used to characterize the number of clusters of target samples, and the target samples are determined according to the updated positioning data and the predicted state;

[0059] Step 130: When it is determined that the number of clusters is greater than a preset value, the predicted state is used as a filtering result of the underwater vehicle at the next moment;

[0060] Step 140, when it is determined that the number of clusters is equal to a preset value, the predicted state is updated according to the predicted state, the measurement state transfer matrix of the linear Kalman filter, the Kalman gain, the unit matrix, the updated positioning data and the adjustment weight matrix of the updated positioning data to obtain the filtering result of the underwater vehicle at the next moment, and the adjustment weight matrix is ​​determined according to the Euclidean distance between the predicted state and the updated positioning data and the preset exponential gradient coefficient of each element in the adjustment weight matrix.

[0061] It should be noted that the executor of each step in the underwater vehicle positioning filtering method can be an underwater vehicle positioning filtering system or an underwater vehicle onboard industrial computer. The system can be implemented through software and / or hardware. The system can be integrated into an electronic device. The electronic device can be a terminal device (such as a smart phone, a personal computer, etc.), or a server (such as a local server or a cloud server, or a server cluster, etc.), or a processor, or a chip, etc.

[0062] In a specific implementation, the filtering result is obtained by improving the linear Kalman filter framework, which can be specifically used to characterize the predicted position vector of the underwater vehicle in the target coordinate system, wherein the target coordinate system can be specifically used to characterize the north coordinate and the east coordinate of the predicted position vector. Exemplarily, the target coordinate system can be a north-east inertial coordinate system.

[0063] In step 110, the acquired position increment of the underwater vehicle at the next moment and the filtering result of the underwater vehicle at the previous moment are input into the linear Kalman filter state prediction model, and the predicted state of the underwater vehicle at the next moment is obtained according to the output result of the linear Kalman filter state prediction model.

[0064] In step 120, the positioning data of the underwater vehicle may be the positioning data fed back by an ultra-short baseline positioning device. When the positioning data fed back by the ultra-short baseline positioning device is updated, the updated positioning data and the predicted state of the underwater vehicle at the next moment are sampled to obtain a target sample, and the target sample is clustered to obtain a measurement judgment result for characterizing the number of clusters in the target sample.

[0065] Specifically, when the positioning data fed back by the ultra-short baseline positioning device is updated, the updated positioning data and the predicted state of the underwater vehicle at the next moment are input into the measurement distribution discriminator to obtain the measurement judgment result. In the embodiment of the present application, the measurement distribution discriminator adopts the K-means measurement distribution discriminator. The updated positioning data can be regarded as the measurement state in the filtering process.

[0066] In step 130 and step 140, according to the cluster number of target samples in the measurement judgment result, it can be judged whether the updated positioning data conforms to the predicted state distribution of the underwater vehicle at the next moment, and whether to perform measurement update is judged accordingly. Specifically:

[0067] If the number of clusters in the target sample is greater than the preset value, it is determined that the updated positioning data does not conform to the predicted state distribution of the underwater vehicle at the next moment, that is, the updated positioning data fed back by the ultra-short baseline positioning device is considered to be an outlier, and the measurement update is not performed, and the predicted state of the underwater vehicle at the next moment is directly used as the filtering result of the underwater vehicle at the next moment.

[0068] If the number of clusters in the target sample is equal to the preset value, it is determined that the updated positioning data conforms to the predicted state distribution of the underwater vehicle at the next moment, and the measurement update is performed. According to the predicted state at the next moment, the measurement state transfer matrix of the linear Kalman filter, the Kalman gain, the unit matrix, the updated positioning data, and the adjustment weight matrix corresponding to the updated positioning data, the predicted state of the underwater vehicle at the next moment is updated to obtain the filtering result of the underwater vehicle at the next moment. In the embodiment of the present application, the preset value is 1.

[0069] The adjustment weight matrix can be obtained according to the Euclidean distance between the predicted state of the underwater vehicle at the next moment and the updated positioning data fed back by the ultra-short baseline positioning device and the preset exponential gradient coefficient of each element in the adjustment weight matrix.

[0070] It should be noted that in the embodiment of the present application, the state prediction of the underwater vehicle is performed at a fixed frequency.

[0071] The underwater vehicle positioning filtering method provided in the embodiment of the present application obtains a target sample by sampling the predicted state of the underwater vehicle at the next moment and the updated positioning data of the underwater vehicle, and determines whether the updated positioning data conforms to the predicted state distribution according to the number of clusters of the target sample, eliminates the interference of abnormal values ​​in the positioning data, and improves the positioning accuracy. At the same time, when it is determined that the updated positioning data conforms to the predicted distribution, the positioning data is dynamically adjusted in combination with the designed adjustment weight matrix, and the predicted state of the underwater vehicle at the next moment is updated, which effectively reduces the local data mutation caused by positioning hysteresis and improves the overall continuity of the filtering result.

[0072] Furthermore, in some embodiments, the method for obtaining the position increment of the underwater vehicle at the next moment may include:

[0073] Inputting the motion state quantity of the underwater vehicle at the previous moment and the control quantity of the underwater vehicle at the previous moment into the target prediction neural network to obtain the position increment of the underwater vehicle at the next moment;

[0074] The target prediction neural network is obtained by inputting the operating state quantity of the underwater vehicle at a historical moment, the control quantity of the underwater vehicle at a historical moment and the positioning data of the underwater vehicle at a historical moment into a preset prediction neural network for training.

[0075] In a specific implementation, the position increment of the underwater vehicle at the next moment is obtained by inputting the motion state quantity of the underwater vehicle at the previous moment and its control quantity at the previous moment into the target prediction neural network, and according to the output of the target prediction neural network.

[0076] The target prediction neural network is obtained by inputting the operating state of the underwater vehicle at a historical moment, its control amount at a historical moment, and its positioning data at a historical moment into a preset prediction neural network for training. Its positioning data at a historical moment can be obtained by the underwater vehicle's onboard inertial navigation system.

[0077] Please see further Figure 2 The preset prediction neural network is constructed based on a multi-layer perceptron and is obtained through offline supervised training based on the motion state quantity and control quantity of the underwater vehicle at historical moments and the positioning data obtained by the underwater vehicle's onboard inertial navigation system.

[0078] Furthermore, in some embodiments, the motion state quantity includes the heading angle, heading angular velocity, forward velocity and lateral velocity of the underwater vehicle, and the control quantity includes the steering rudder angle of the underwater vehicle.

[0079] In the embodiment of the present application, the motion state quantity includes the heading angle, heading angular velocity, forward velocity and lateral velocity of the underwater vehicle, and the control quantity includes the steering rudder angle of the underwater vehicle.

[0080] See also Figure 2 , and see further Figure 3 The state information sampling node collects the control quantity, motion state quantity and positioning data obtained by the underwater vehicle onboard inertial navigation system of the underwater vehicle according to the control frequency to construct a training sample set of the preset prediction neural network. Among them, the control quantity includes the steering rudder angle of the underwater vehicle, the motion state quantity includes the heading angle, heading angular velocity, forward velocity and lateral velocity of the underwater vehicle, and the positioning data obtained by the underwater vehicle onboard inertial navigation system includes the longitude and latitude of the vehicle.

[0081] Select a latitude and longitude origin, convert the positioning data of the underwater vehicle onboard inertial navigation system from latitude and longitude to north and east coordinates, and calculate the north coordinate change (Δp) of two adjacent sets of sampling data (i.e., the positioning data of the underwater vehicle onboard inertial navigation system) N ) and the eastward coordinate change (Δp E ). With the control quantity and motion state quantity as input and the north and east coordinate changes as output, the preset prediction neural network is trained by supervised learning to obtain the target prediction neural network. The trained target prediction neural network model is integrated in the linear Kalman filter to predict the complete position vector of the underwater vehicle.

[0082] In one example, the predicted state of the underwater vehicle at the next moment (moment k) can be obtained by inputting the control quantity at the previous moment (moment k-1) obtained in the underwater vehicle control cycle, the motion state quantity at the previous moment, and the filtering result at the previous moment into the linear Kalman filter state prediction model. Among them, the control quantity includes the steering rudder angle δ of the underwater vehicle r The motion state variables include the underwater vehicle heading angle ψ, heading angular velocity r, forward velocity u and lateral velocity v. The filtering result of the underwater vehicle at the previous moment is the predicted position vector of the underwater vehicle in the north-east inertial coordinate system. The predicted state of the underwater vehicle at the next moment can be expressed as:

[0083]

[0084] in, is the predicted state of the underwater vehicle at time k, or the prior predicted state, is the filtering result of the underwater vehicle at time k-1, A represents the state transfer matrix of the linear Kalman filter, For target prediction neural network, is the network parameter, ω k-1 ={ψ k-1 ,u k-1 ,v k-1 ,r k-1 ,δ k-1} T It is the input vector of the target prediction neural network, which consists of the motion state and control quantity of the underwater vehicle at time k-1.

[0085] Further, in some embodiments, when the positioning data of the underwater vehicle is updated, determining the measurement judgment result according to the updated positioning data and the predicted state may include:

[0086] Determining the target sample according to the updated positioning data and the predicted state;

[0087] Based on a preset clustering algorithm, the target samples are clustered to obtain the measurement judgment result.

[0088] Please see further Figure 3 , the target sample is obtained by sampling the updated positioning data and the predicted state of the underwater vehicle at the next moment through the predicted state and measured state sampling module.

[0089] Based on a preset clustering algorithm, the target samples are clustered to obtain a measurement judgment result for characterizing the number of clusters in the target samples, so as to judge the distribution difference between the updated positioning data and the predicted state of the underwater vehicle at the next moment. Exemplarily, the preset clustering algorithm may be a K-means clustering algorithm.

[0090] The underwater vehicle positioning filtering method provided in the embodiment of the present application adopts a multi-layer perceptron to construct a target prediction neural network, and trains the network through a supervised learning method to convert the motion state quantity and control quantity of the underwater vehicle into the position increment estimation of the underwater vehicle. Based on the linear Kalman filter architecture, the position increment is superimposed on the filtering result of the underwater vehicle at the previous moment to realize the position state prediction of the underwater vehicle. When the ultra-short baseline positioning device feeds back the positioning data update, the K-means clustering algorithm is used to determine whether the positioning data fed back by the ultra-short baseline positioning device is an outlier, eliminate the interference of outliers in the positioning data, and improve the positioning accuracy.

[0091] Furthermore, in some embodiments, determining the target sample according to the updated positioning data and the predicted state may include:

[0092] Taking the updated positioning data as the center, sampling is performed according to a normal distribution to obtain a measurement state sample set;

[0093] Taking the predicted state as the center, sampling is performed according to a normal distribution to obtain a predicted state sample set;

[0094] The target sample is determined according to the measurement state sample set and the prediction state sample set.

[0095] See also Figure 3 , respectively taking the predicted state of the underwater vehicle at the next moment and the updated positioning data as the center point, sampling is performed according to the normal distribution to obtain a predicted state sample set corresponding to the predicted state of the underwater vehicle at the next moment and a measured state sample set corresponding to the updated positioning data. The updated positioning data can be collected by an ultra-short baseline positioning device deployed at an ultra-short baseline positioning data collection node.

[0096]

[0097] Z k ~N(z k ,R)

[0098] The N(·) operator represents sampling according to the normal distribution, χ k is the predicted state sample set, is the measured state sample set, z k is the updated positioning data, that is, the positioning data at time k, and R is the state measurement process noise, that is, the first preset noise.

[0099] The predicted state sample set χ k After the measurement transfer transformation, the measurement state sample set Z k Merge to get the target sample.

[0100] S=H·χ k ∪Z k

[0101] Among them, H is the measurement state transfer matrix of the linear Kalman filter, and S is the merged sample set, that is, the target sample.

[0102] The target sample S is clustered using the K-means clustering algorithm. If the measurement judgment result obtained is a 2-class set, that is, the number of clusters included in the target sample is 2, then the updated positioning result fed back by the ultra-short baseline positioning device is judged to be an outlier and the subsequent measurement update step is skipped, and the predicted state of the underwater vehicle at the next moment is directly used as its filtering result at the next moment. If the measurement judgment result obtained is a 1-class set, that is, the number of clusters included in the target sample is 1, then it is considered that the updated positioning data conforms to the predicted state distribution of the underwater vehicle at the next moment. Combined with the linear Kalman filter measurement update module, the predicted state of the underwater vehicle at the next moment is updated according to the predicted state, the measurement state transfer matrix of the linear Kalman filter, the Kalman gain, the unit matrix, the updated positioning data and the adjusted weight matrix of the updated positioning data, and the filtering result of the underwater vehicle at the next moment is obtained.

[0103] Further, in some embodiments, when it is determined that the number of clusters is equal to a preset value, updating the predicted state according to the predicted state, the measurement state transfer matrix of the linear Kalman filter, the Kalman gain, the unit matrix, the updated positioning data and the adjustment weight matrix of the updated positioning data to obtain the filtering result of the underwater vehicle at the next moment may include:

[0104] Determining a first adjustment coefficient according to the adjustment weight matrix and the measurement state transfer matrix;

[0105] Determining a second adjustment coefficient according to the first preset noise and the adjustment weight matrix;

[0106] determining the Kalman gain according to the covariance of the first adjustment coefficient, the second adjustment coefficient and the predicted state of the underwater vehicle at the next moment;

[0107] According to the adjustment weight matrix, the updated positioning data, the unit matrix, the predicted state and the measurement state transfer matrix, the updated positioning data is adjusted to obtain target positioning data;

[0108] Determining a third adjustment coefficient according to the Kalman gain and the measurement state transfer matrix;

[0109] The predicted state is updated according to the predicted state, the target positioning data, the measurement state transfer matrix and the third adjustment coefficient to obtain a filtering result of the underwater vehicle at the next moment.

[0110] In the specific implementation, when it is determined that the number of clusters in the target sample is equal to the preset value, that is, when the number of clusters in the target sample is determined to be 1 based on the K-means clustering algorithm, the updated positioning data z obtained by the ultra-short baseline positioning device is combined. k The prior prediction state of the underwater vehicle at time k Perform measurement update.

[0111] After the linear Kalman filter measurement updater passes the measurement judgment (that is, the number of clusters in the target sample is 1), it dynamically adjusts the Kalman gain based on the exponential gradient weight calculation module to make the filtering result closer to the predicted state, enhance data smoothness, and suppress data jumps caused by positioning hysteresis of ultra-short baseline positioning equipment.

[0112] Specifically, the first adjustment coefficient is determined according to the adjustment weight matrix and the measurement state transfer matrix H of the linear Kalman filter, as shown in the following formula:

[0113] C=WH

[0114] Among them, C is the first adjustment coefficient, and W is the adjustment weight matrix.

[0115] According to the first preset noise R and the adjustment weight matrix W, the second adjustment coefficient is determined as shown in the following formula:

[0116] F=WRW

[0117] Wherein, F is the second adjustment coefficient.

[0118] According to the first adjustment coefficient C, the second adjustment coefficient F and the covariance of the predicted state of the underwater vehicle at time k Determine the Kalman gain as shown below:

[0119]

[0120] Among them, K k stands for Kalman gain.

[0121] According to the adjusted weight matrix W, the updated positioning data z k , the identity matrix I, the predicted state of the underwater vehicle at time k The updated positioning data is adjusted by the measurement state transfer matrix H of the linear Kalman filter to obtain the target positioning data, as shown in the following formula:

[0122]

[0123] Among them, z′ k Locate data for your target.

[0124] According to the Kalman gain K kThe third adjustment coefficient is determined by the measurement state transfer matrix H of the linear Kalman filter, as shown in the following formula:

[0125] G=K k H

[0126] Wherein, G is the third adjustment coefficient.

[0127] According to the predicted state of the underwater vehicle at time k Target positioning data z′ k , the measurement state transfer matrix H of the linear Kalman filter and the third adjustment coefficient G, the predicted state of the underwater vehicle at time k Update and obtain the filtering result of the underwater vehicle at time k, as shown in the following formula:

[0128]

[0129] in, is the filtering result of the underwater vehicle at time k, or called the posterior prediction state. The adjustment weight matrix W can be calculated by the dynamic exponential gradient function. Specifically, it can be calculated based on the predicted state of the underwater vehicle at time k. With the updated positioning data z k The Euclidean distance between them is determined by the preset exponential gradient coefficients that adjust the elements in the weight matrix as follows:

[0130]

[0131] Among them, η1, η2…η n To adjust each element to be determined in the weight matrix, the corresponding measurement state contains the state quantity. It can be calculated according to the following formula:

[0132]

[0133] Among them, d k is the predicted state of the underwater vehicle at time k With the updated positioning data z k The Euclidean distance between i To adjust the i-th (i=1,...,n) element η in the weight matrix i The preset exponential gradient coefficient, and δ i >0, is η at time k i The value of

[0134] Furthermore, in some embodiments, the method for obtaining the covariance of the predicted state of the underwater vehicle at the next moment may include:

[0135] The covariance of the predicted state is determined according to the covariance of the filtering result of the underwater vehicle at the previous moment, the second preset noise and the state transfer matrix of the linear Kalman filter.

[0136] In the specific implementation, the prior prediction state at time k is updated according to the Kalman filter process. The covariance of

[0137] P k =AP k-1 A T +Q

[0138] Where A is the state transfer matrix of the linear Kalman filter, P k-1 is the covariance of the filtering result of the underwater vehicle at time k-1, and Q is the state prediction process noise, that is, the second preset noise.

[0139] Please see further Figure 4 In practical applications, the underwater vehicle positioning filtering method provided in the embodiment of the present application includes the following steps:

[0140] S101. Construct a preset prediction neural network based on a multi-layer perceptron, collect the control quantity, motion state quantity and positioning data of the underwater vehicle's onboard inertial navigation system to construct a preset prediction neural network training sample set, and use a supervised learning method to train the preset prediction neural network to obtain a target prediction neural network.

[0141] S102, when the ultra-short baseline positioning device feeds back the positioning data of the underwater vehicle for the first time during the navigation of the underwater vehicle, the filter is initialized and filtering processing is started.

[0142] S103, collecting the control quantity and motion state quantity of the underwater vehicle at the previous moment, and inputting them into the trained target prediction neural network to obtain the predicted position increment, and combining with the linear Kalman filter state prediction model to obtain the predicted state of the underwater vehicle at the next moment;

[0143] S104, when the ultra-short baseline positioning device updates the positioning data, it takes the predicted state and the measured state as sampling points to obtain two sample sets (the predicted state sample set and the measured state sample set), and after merging the sample sets, obtains the target sample. The K-means clustering algorithm is used to cluster the target sample to obtain the centroid of the target sample and the clustering result.

[0144] S105. If the clustering result of the K-means clustering algorithm indicates that the target samples include two types of samples, the filter directly outputs the prior prediction state as the filtering result of the underwater vehicle at the next moment.

[0145] S106. If the clustering result of the K-means clustering algorithm represents that the target sample includes 1 type of sample, the filter calculates the Euclidean distance between the prior prediction state and the measurement state, substitutes it into the exponential gradient weight calculation module to obtain the adjusted weight matrix, and combines the measurement update process of the linear Kalman filter to obtain the posterior prediction state as the filtering result of the underwater vehicle at the next moment.

[0146] In order to verify the filtering effect of the underwater vehicle positioning filtering method provided in this application on the positioning data fed back by the ultra-short baseline positioning device, the following example is used: Figure 5 A typical underwater vehicle is shown, and the underwater vehicle positioning filtering method and system provided by the present application are integrated into the onboard industrial computer of the vehicle. The underwater vehicle is equipped with an ultra-short baseline positioning device and a high-precision inertial navigation system in the bow compartment, which is suitable for forming a physical prototype to verify the underwater vehicle positioning filtering method provided by the present application.

[0147] Please see further Figure 6 , Figure 6 This is a graph of ultra-short baseline positioning filter data provided by an embodiment of the present application. The circles in the graph are positioning data fed back by the ultra-short baseline positioning device recorded by the underwater vehicle during navigation, the dotted line is the positioning data of the underwater vehicle's onboard inertial navigation system, the solid line is the linear Kalman filter result, and the stars are the improved filtering results. There are obvious deviations and discontinuities between the original values ​​of the positioning data fed back by the ultra-short baseline positioning device and the positioning results of the high-precision inertial navigation system, and there are obvious positioning anomalies during local navigation. Figure 6 It can be seen that after filtering, the positioning data of the ultra-short baseline positioning device has higher smoothness and accuracy. Compared with the positioning results of the underwater vehicle onboard inertial navigation system, the average north error of the filtered track is 1.58m, the average east error is 2.79m, the mean square error of the north error is 0.85m, and the mean square error of the east error is 1.24m. The average north error of the linear Kalman filter is 2.52m, the average east error is 3.12m, the mean square error of the north error is 3.51m, and the mean square error of the east error is 3.09m. Compared with the linear Kalman filter, the underwater vehicle positioning filtering method provided in this application can effectively improve the accuracy and continuity of ultra-short baseline positioning data.

[0148] The underwater vehicle positioning filtering system provided by the present invention is described below. The underwater vehicle positioning filtering system described below and the underwater vehicle positioning filtering method described above can be referred to each other.

[0149] See also Figure 7The underwater vehicle positioning filtering system provided in the embodiment of the present application includes: a first acquisition module 710, a second acquisition module 720, a first positioning module 730 and a second positioning module 740.

[0150] A first acquisition module 710 is used to determine the predicted state of the underwater vehicle at the next moment according to the position increment of the underwater vehicle at the next moment and the filtering result of the underwater vehicle at the previous moment, wherein the filtering result is used to characterize the predicted position vector of the underwater vehicle in the target coordinate system, and the target coordinate system is used to characterize the north coordinate and the east coordinate corresponding to the predicted position vector;

[0151] A second acquisition module 720 is used to determine a measurement judgment result according to the updated positioning data and the predicted state when the positioning data of the underwater vehicle is updated, wherein the measurement judgment result is used to characterize the number of clusters of target samples, and the target samples are determined according to the updated positioning data and the predicted state;

[0152] A first positioning module 730 is configured to use the predicted state as a filtering result of the underwater vehicle at a next moment when it is determined that the number of clusters is greater than a preset value;

[0153] The second positioning module 740 is used to update the predicted state according to the predicted state, the measurement state transfer matrix of the linear Kalman filter, the Kalman gain, the unit matrix, the updated positioning data and the adjustment weight matrix of the updated positioning data when it is determined that the number of clusters is equal to the preset value, so as to obtain the filtering result of the underwater vehicle at the next moment, and the adjustment weight matrix is ​​determined according to the Euclidean distance between the predicted state and the updated positioning data and the preset exponential gradient coefficient of each element in the adjustment weight matrix.

[0154] The underwater vehicle positioning filtering system provided in the embodiment of the present application obtains target samples by sampling the predicted state of the underwater vehicle at the next moment and the updated positioning data of the underwater vehicle, and determines whether the updated positioning data conforms to the predicted state distribution according to the number of clusters of the target samples, eliminates the interference of abnormal values ​​in the positioning data, and improves the positioning accuracy. At the same time, when it is determined that the updated positioning data conforms to the predicted distribution, the positioning data is dynamically adjusted in combination with the designed adjustment weight matrix, and the predicted state of the underwater vehicle at the next moment is updated, which effectively reduces the local data mutation caused by positioning hysteresis and improves the overall continuity of the filtering results.

[0155] It can be understood that the detailed functional implementation of each of the above-mentioned units / modules can be found in the introduction of the aforementioned method embodiment, and will not be repeated here.

[0156] It should be understood that the above-mentioned system is used to execute the methods in the above-mentioned embodiments. The implementation principles and technical effects of the corresponding program modules in the system are similar to those described in the above-mentioned methods. The working process of the system can refer to the corresponding process in the above-mentioned method and will not be repeated here.

[0157] Based on the method in the above embodiment, the embodiment of the present application provides an electronic device, such as Figure 8 As shown, the electronic device may include: a processor (Processor) 810, a communication interface (CommunicationsInterface) 820, a memory (Memory) 830 and a communication bus 840, wherein the processor 810, the communication interface 820, and the memory 830 communicate with each other through the communication bus 840. The processor 810 can call the logic instructions in the memory 830 to execute the method in the above embodiment.

[0158] In addition, the logic instructions in the above-mentioned memory 830 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product, which is stored in a storage medium and includes a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present application.

[0159] Based on the method in the above embodiment, an embodiment of the present application provides a computer-readable storage medium, which stores a computer program. When the computer program runs on a processor, the processor executes the method in the above embodiment.

[0160] Based on the method in the above embodiment, an embodiment of the present application provides a computer program product. When the computer program product runs on a processor, the processor executes the method in the above embodiment.

[0161] It is understandable that the processor in the embodiment of the present application may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field programmable gate arrays (FPGA) or other programmable logic devices, transistor logic devices, hardware components or any combination thereof. The general-purpose processor may be a microprocessor or any conventional processor.

[0162] The method steps in the embodiments of the present application can be implemented by hardware or by a processor executing software instructions. The software instructions can be composed of corresponding software modules, and the software modules can be stored in random access memory (RAM), flash memory, read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), registers, hard disks, mobile hard disks, CD-ROMs, or any other form of storage medium known in the art. An exemplary storage medium is coupled to a processor so that the processor can read information from the storage medium and write information to the storage medium. Of course, the storage medium can also be a component of the processor. The processor and the storage medium can be located in an ASIC.

[0163] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware or any combination thereof. When implemented using software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the process or function described in the embodiment of the present application is generated in whole or in part. The computer may be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions may be stored in a computer-readable storage medium or transmitted through the computer-readable storage medium. The computer instructions may be transmitted from a website site, a computer, a server or a data center to another website site, a computer, a server or a data center by wired (e.g., coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium may be any available medium that a computer can access or a data storage device such as a server or a data center that includes one or more available media integrated. The available medium may be a magnetic medium (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium (e.g., a solid-state drive (SSD)), etc.

[0164] It should be understood that the various numerical numbers involved in the embodiments of the present application are only used for the convenience of description and are not used to limit the scope of the embodiments of the present application.

[0165] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present application and is not intended to limit the present application. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present application shall be included in the scope of protection of the present application.

Claims

1. A method for filtering underwater vehicle positioning, characterized in that: include: Determine a predicted state of the underwater vehicle at the next moment according to a position increment of the underwater vehicle at the next moment and a filtering result of the underwater vehicle at the previous moment, wherein the filtering result is used to characterize a predicted position vector of the underwater vehicle in a target coordinate system, and the target coordinate system is used to characterize a north coordinate and an east coordinate corresponding to the predicted position vector; When the positioning data of the underwater vehicle is updated, the target samples are determined according to the updated positioning data and the predicted state, and the target samples are clustered based on a preset clustering algorithm to obtain the measurement and judgment results, where the measurement and judgment results are used to characterize the number of clusters of the target samples; In the case where it is determined that the number of clusters is greater than a preset value, taking the predicted state as a filtering result of the underwater vehicle at a next moment; When it is determined that the number of clusters is equal to a preset value, the predicted state is updated according to the predicted state, the measurement state transfer matrix of the linear Kalman filter, the Kalman gain, the unit matrix, the updated positioning data, and the adjustment weight matrix of the updated positioning data to obtain the filtering result of the underwater vehicle at the next moment, including: Determining a first adjustment coefficient according to the adjustment weight matrix and the measurement state transfer matrix; Determining a second adjustment coefficient according to the first preset noise and the adjustment weight matrix; determining the Kalman gain according to the covariance of the first adjustment coefficient, the second adjustment coefficient and the predicted state of the underwater vehicle at the next moment; According to the adjustment weight matrix, the updated positioning data, the unit matrix, the predicted state and the measurement state transfer matrix, the updated positioning data is adjusted to obtain target positioning data; Determining a third adjustment coefficient according to the Kalman gain and the measurement state transfer matrix; The predicted state is updated according to the predicted state, the target positioning data, the measurement state transfer matrix and the third adjustment coefficient to obtain the filtering result of the underwater vehicle at the next moment, and the adjustment weight matrix is ​​determined according to the Euclidean distance between the predicted state and the updated positioning data and the preset exponential gradient coefficient of each element in the adjustment weight matrix.

2. The underwater vehicle positioning filtering method according to claim 1, characterized in that: The method for obtaining the position increment of the underwater vehicle at the next moment includes: Inputting the motion state quantity of the underwater vehicle at the previous moment and the control quantity of the underwater vehicle at the previous moment into the target prediction neural network to obtain the position increment of the underwater vehicle at the next moment; The target prediction neural network is obtained by inputting the operating state quantity of the underwater vehicle at a historical moment, the control quantity of the underwater vehicle at a historical moment and the positioning data of the underwater vehicle at a historical moment into a preset prediction neural network for training.

3. The underwater vehicle positioning filtering method according to claim 1, characterized in that: The determining the target sample according to the updated positioning data and the predicted state includes: Taking the updated positioning data as the center, sampling is performed according to a normal distribution to obtain a measurement state sample set; Taking the predicted state as the center, sampling is performed according to a normal distribution to obtain a predicted state sample set; The target sample is determined according to the measurement state sample set and the prediction state sample set.

4. The underwater vehicle positioning filtering method according to claim 1, characterized in that: The method for obtaining the covariance of the predicted state of the underwater vehicle at the next moment includes: The covariance of the predicted state is determined according to the covariance of the filtering result of the underwater vehicle at the previous moment, the second preset noise and the state transfer matrix of the linear Kalman filter.

5. The underwater vehicle positioning filtering method according to claim 2, characterized in that: The motion state quantities include the heading angle, heading angular velocity, forward velocity and lateral velocity of the underwater vehicle, and the control quantity includes the steering rudder angle of the underwater vehicle.

6. An underwater vehicle positioning filtering system for executing the method according to any one of claims 1 to 5, characterized in that: include: a first acquisition module, configured to determine a predicted state of the underwater vehicle at a next moment according to a position increment of the underwater vehicle at a next moment and a filtering result of the underwater vehicle at a previous moment, wherein the filtering result is used to characterize a predicted position vector of the underwater vehicle in a target coordinate system, and the target coordinate system is used to characterize a north coordinate and an east coordinate corresponding to the predicted position vector; A second acquisition module is used to determine a measurement and judgment result according to the updated positioning data and the predicted state when the positioning data of the underwater vehicle is updated, wherein the measurement and judgment result is used to characterize the number of clusters of the target sample; A first positioning module, configured to use the predicted state as a filtering result of the underwater vehicle at a next moment when it is determined that the number of clusters is greater than a preset value; The second positioning module is used to update the predicted state according to the predicted state, the measurement state transfer matrix of the linear Kalman filter, the Kalman gain, the unit matrix, the updated positioning data and the adjustment weight matrix of the updated positioning data when it is determined that the number of clusters is equal to the preset value, so as to obtain the filtering result of the underwater vehicle at the next moment, and the adjustment weight matrix is ​​determined according to the Euclidean distance between the predicted state and the updated positioning data and the preset exponential gradient coefficient of each element in the adjustment weight matrix.

7. An electronic device, characterized in that: include: at least one memory for storing a computer program; At least one processor is used to execute the program stored in the memory. When the program stored in the memory is executed, the processor is used to execute the method according to any one of claims 1 to 5.

8. A computer-readable storage medium storing a computer program, characterized in that: When the computer program runs on a processor, the processor is caused to execute the method according to any one of claims 1 to 5.

Images