WO2024021642A1

WO2024021642A1 - Unmanned ship positioning method based on multi-sensor data fusion

Info

Publication number: WO2024021642A1
Application number: PCT/CN2023/083229
Authority: WO
Inventors: 袁明新; 赵泽钰; 王舜; 薛文博; 张亮; 申燚; 王雨欣; 吕增城; 王以龙; 刘维
Original assignee: 江苏科技大学
Priority date: 2022-07-26
Filing date: 2023-03-23
Publication date: 2024-02-01
Also published as: CN115342814A; CN115342814B

Abstract

An unmanned ship positioning method based on multi-sensor data fusion. The method comprises the steps of: firstly preprocessing data, which is collected by a multi-sensor positioning system of an unmanned ship; then, by means of a confidence distance check, performing confidence level determination on positioning data of a plurality of sensors of the unmanned ship, and assigning a corresponding confidence factor to the checked positioning data; checking and compensating for fault data by using a consistency check and variance weighting; subsequently, performing filtering processing on the checked and weighted positioning data of the sensors on the basis of basic particle filtering, so as to realize data enhancement; and finally, performing fusion filtering output processing on the positioning data of the plurality of sensors of the unmanned ship by using a new threshold layered particle filtering algorithm, so as to obtain precise positioning information of a navigation trajectory of the unmanned ship. The method improves the fault-tolerance performance of a multi-sensor positioning system of an unmanned ship and increases the degree of algorithm association thereof, ensures the reliability of positioning data of sensors, and achieves the aim of precisely positioning a navigation trajectory of the unmanned ship.

Description

An unmanned ship positioning method based on multi-sensor data fusion

Technical field

The invention belongs to the field of positioning and navigation technology, and relates to a fusion processing technology based on multi-sensor information collection, and in particular to an unmanned ship positioning method based on multi-sensor data fusion.

Background technique

The development and application of intelligent unmanned ship positioning systems supported by modern sensor information technology can not only promote the development of surface unmanned ships in the fields of offshore patrol, water quality monitoring and aquaculture, but also greatly reduce the workload of operators. Strength and improve work efficiency.

To achieve efficient and accurate work of unmanned ships, we must first rely on whether the obtained sensor positioning information can accurately perceive the position of the unmanned ship on the water surface, that is, whether the unmanned ship can use the obtained positioning information to ensure its work efficiency and Accuracy. With the development of automatic navigation technology for surface unmanned ships, the requirements for positioning accuracy and stability of unmanned ships are getting higher and higher. Due to the influence of the complex working environment on the water surface and shore, sensor information may be lost and positioning may be inaccurate. Therefore, there are major disadvantages in using a single sensor, but the fusion of multiple sensor data and the use of their complementary advantages will effectively overcome the above disadvantages and become the development trend of combined positioning systems. Therefore, the research and application of multi-sensor data fusion algorithms for unmanned ship positioning systems will help improve the fault-tolerant performance of the algorithm, thereby achieving efficient filtering of unmanned ship position data by the multi-sensor data fusion algorithm, and ultimately achieve the goal of unmanned ship positioning data processing. The purpose of accurately positioning the position of people and ships and improving production efficiency.

Contents of the invention

The purpose of the present invention is to overcome the major disadvantages of using a single sensor in the existing technology and provide an unmanned ship positioning method based on multi-sensor data fusion for multi-sensor data fusion positioning of the unmanned ship's navigation trajectory.

In order to achieve the above object, the present invention adopts the following technical solutions to achieve it:

An unmanned ship positioning method based on multi-sensor data fusion, first preprocessing the positioning data collected by the unmanned ship's multi-sensor positioning system; then determining the confidence distance of the unmanned ship positioning data and assigning the corresponding confidence factor; at the same time Perform fault detection and weighted compensation on the positioning data; then filter and enhance the positioning data; finally, use the data fusion algorithm to perform multi-sensor data fusion filtering output, thereby realizing the positioning of the unmanned ship's navigation trajectory. The specific steps are as follows:

Step 1. Data preprocessing;

Preprocess the positioning data of the unmanned ship collected by the multi-sensor positioning system of the unmanned ship.

Step 2. Data confidence determination and assignment;

The data confidence judgment and assignment includes making a confidence judgment based on the data collected by multiple sensors and assigning a confidence factor.

Step 3. Data failure inspection and compensation;

The data failure check and compensation includes weighted compensation processing for inconsistent data collected by the positioning system based on consistency check of multi-sensor data.

Step 4. Data enhancement;

The data enhancement includes preprocessing, checking and compensating multiple data based on basic particle filtering algorithm. Sensor data is filtered.

Step 5. Data fusion.

The data fusion includes designing a new threshold hierarchical particle filter algorithm by constructing a Gaussian mixture model and setting an adaptive threshold and a hierarchical sampling proportional capacity associated with a confidence factor, thereby positioning the navigation trajectory of the unmanned ship. The data is fused and filtered to output the navigation trajectory and positioning information of the unmanned ship.

Further, the data preprocessing described in step 1 is to preprocess the position data collected by the unmanned ship multi-sensor positioning system. The specific content and method steps are:

A) Based on the longitude and latitude information of the unmanned ship's navigation trajectory collected by the positioning system sensor, unify the spatial and temporal benchmarks;

B) Based on the unmanned ship's navigation of the river, build a communication environment that satisfies the effective connection of multiple nodes of the positioning system, so that the blind node on the unmanned ship is within a network of 4 signal receiving nodes with known transmit signal strength and position coordinates, and use this to calculate the collection The position coordinates of the unmanned ship when sailing on the river;

C) According to the Gauss-Krüger projection principle, the coordinates of the multi-sensor positioning system are transformed and unified.

Further, the data confidence determination and assignment in step 2 includes confidence determination and confidence factor assignment based on data collected by multiple sensors, wherein the specific content and method steps of the data confidence determination are:

1) Based on the measurement model p _i (x) of the unmanned ship positioning system measurement data, calculate the confidence distance _di between sensor data. The calculation formulas of p _i (x) _{and di} are as follows:

In the formula, x _i is the measurement value of the i-th sensor, μ is the true value of the measurement feature, θ _i is the information measurement accuracy of the i-th sensor, and σ _i is the information measurement error of the i-th sensor.

In the formula, x′ _i and x″ _i are the measurement values collected at time i by the multi-sensor of the unmanned ship positioning system respectively, τ′ _i and τ″ _i are the measurement variances of the corresponding sensors, is the mean variance of the measurement, Z is a random variable obeying the standard normal distribution, i=1,2,…,n.

2) Based on the Gaussian probability model, rewrite the d _i obtained in step 1) into a measure p _r (Z _i ) in the sense of probability between sensor measurement data, and set the sensor support confidence level to determine the credibility of different sensor information. Degree, the calculation formula of p _r (Z _i ) is as follows:

In the formula, Z _i is the measurement data of the unmanned ship multi-sensor positioning system, ε is the confidence level, and K is the sampling sample probability interval variable coefficient.

3) According to the size of the information measurement value in the sense of probability between the measured data, the determined positioning data is divided into different confidence intervals, so that the credible data moves closer to the area with higher confidence; at the same time, the set confidence level ε is consistent with the sensor Measured variance mean Correspondingly, it is used to indicate that the measurement data collected by the unmanned ship multi-sensor positioning system falls into the interval probability, so the variance of the measurement information is different, and the corresponding confidence interval of the unmanned ship positioning information is divided The class is also changing and can be adjusted according to the size of the sensor measurement variance, which improves the sampling reliability of the sensor measurement data.

Further, the specific content and method steps of the data confidence factor assignment in the data confidence determination assignment described in step 2 are:

(A) Construct the norm equation of the dynamic support factor β _i (k) and the Gaussian probability measurement model p _i (x) of the measurement information collected by multiple sensors at time k. The calculation formula of β _i (k) is as follows:

In the formula, ||.|| _F is the Frobenius norm, k=1,2,…,T, i=1,2,…,n.

(B) Calculate the multi-sensor dynamic support factor β _i (k) according to step (A), and calculate the system measurement error w _i . The calculation formula of w _i is as follows:
w _i =z _i -Aβ _i (k)

In the formula, A is the state transition matrix, z _i is the measurement value of the unmanned ship multi-sensor positioning system, i = 1, 2,...,n.

(C) Let the variance of the system measurement error w _i be use to assign corresponding confidence factors to the measurement information The confidence factor The calculation formula is as follows:

In the formula, It is the confidence factor of the measurement data of the unmanned ship multi-sensor positioning system, i=1,2,…,n.

Based on the calculated credibility of the unmanned ship's multi-sensor measurement data, a corresponding confidence factor is assigned to it, and the confidence factor is associated with the new threshold layered particle filter algorithm. The higher the confidence level of the measurement data, the corresponding fusion The degree of bias is also high, which improves the processing accuracy of the unmanned ship's multi-sensor data fusion algorithm.

Further, the data fault inspection and compensation described in step 3 includes performing weighted compensation processing on the inconsistent fault data collected by the positioning system based on the consistency inspection of multi-sensor data, wherein the specific content and method of the consistency inspection are, Use the following steps:

(1) Calculate the arithmetic average of the sensor measurement data obtained by the unmanned ship positioning system and obtain the arithmetic average described The calculation formula is as follows:

In the formula, x _i is the sensor measurement information, i=1,2,…,n.

(2) Use the difference between the arithmetic mean of the sensor measurement data obtained in step (1) and the subsequent sampling value x ^h of the positioning system.

(3) Set the system requirement error. If the difference between the arithmetic mean value of the sensor measurement data and the subsequent sampling value of the positioning system is less than the system requirement error, it is determined that the unmanned ship position data collected by the multi-sensor positioning system is consistent and is credible data. ; If the difference is greater than the system required error, variance weighted compensation needs to be performed on the sampling data to satisfy the basic particle Filtering requires sampling of data samples.

Furthermore, the method for weighted compensation processing of inconsistent fault data in the data fault inspection and compensation described in step 3 specifically adopts the following steps:

(I) Take the arithmetic mean of the multi-sensor measurement data of the unmanned ship as truth value is an unbiased estimate to represent the information measurement variance σ′ _i ² of the i-th sensor system when the unmanned ship multi-sensor positioning system measures the navigation trajectory of the unmanned ship ^at different locations in the same _space . The calculation formula is as follows:

(II) Based on the data set recorded by the unmanned ship multi-sensor positioning system for m measurements of the unmanned ship, record the j-th measurement data of the i-th sensor as x _ij , and replace x _ij with the sensor in step (I) The instrument measurement information x _i is used to obtain the variance of the data set information obtained through multiple measurements. described The calculation formula is as follows:

In the formula, i=1,2,…,n,j=1,2,…,m.

(III) Calculate the fusion weight κ _i of inconsistent fault data based on the variance of the position information data set collected by the unmanned ship multi-sensor positioning system calculated in step (II)). The calculation formula of κ _i is as follows:

(IV) According to the required fusion weight κ _i , for inconsistent fault data Perform variance weighted compensation to obtain sensor measurement data that meets the requirements of basic particle filter processing. described The calculation formula is as follows:

Further, the data enhancement in step 4 includes filtering the preprocessed, inspected, and compensated multi-sensor data based on a basic particle filter algorithm. The specific content and method steps of the data enhancement are:

(i) Establish a basic particle filter model of the unmanned ship multi-sensor positioning system, and substitute the unmanned ship position information after consistency testing and variance weighting into the system model. The status and measurement model of the sensor system are summarized by the following formula:

In the formula, x _k is the position prediction value of the sensor system at time k, x ^h is the subsequent sampling value of the sensor, The sensor measurement value after variance weighting, z _k is the unmanned ship position measurement value at time k, λ _k is the estimated noise, and ν _k is the measurement noise.

(ii) Particle set sample initialization, randomly sampling from the prior density p(x ₀ ) to generate an initialization particle set, The weight of all particles is 1/N,

(iii) Randomly select N particle samples from the importance density function.

(iv) Calculate the weight of sampling particles and update, as stated The calculation formula is as follows:

In the formula, ω ⁱ _k is the sampling particle weight, i = 1, 2,...N.

(v) Normalized importance weight.

(vi) Calculate the effective particle number N _eff in the basic particle filter algorithm, and compare it with the threshold N _th . If N _eff <N _th , resampling is performed. The calculation formula of N _eff is as follows:

(vii) Status output to obtain the filtered local estimate of the measurement information collected by the multi-sensor positioning system of the unmanned ship and covariance matrix described The calculation formula is as follows:

Particle propagation is to generate a new particle state x _k by sampling the state transition model p _z (x _k |x _k-1 , z _k ) of the unmanned ship multi-sensor positioning system, where x _k-1 is the weight of the previous step The particle state after sampling, z _k is the sensor system observation data. The basic particle filter algorithm is used to filter the measurement data of the unmanned ship's multi-sensor positioning system to achieve data enhancement, which reduces the interference of environmental noise on the measurement data and improves the fusion accuracy of the unmanned ship's multi-sensor measurement data.

Further, the data fusion described in step 5 includes designing a new threshold hierarchical particle filter algorithm by constructing a Gaussian mixture model and setting an adaptive threshold and a hierarchical sampling proportional capacity associated with a confidence factor, so as to detect unmanned The ship navigation trajectory positioning data is fused and filtered to output the unmanned ship navigation trajectory positioning information. The specific content and method steps of constructing the Gaussian mixture model are:

(a) Extract the multi-sensor data set processed by the basic particle filter algorithm and calculate the Sigma point set described The calculation formula is as follows:

In the formula, is the Sigma point set after traceless transformation, n _a is the dimension of the Sigma point, and λ is the scale parameter.

(b) Integrate the latest measurement information into the obtained Sigma sampling points, and analyze the system status and covariance To update, the The calculation formula is as follows:

In the formula, k _k is the Kalman gain, z _k is the measurement information, is the covariance of the weighted Sigma point set measurement.

(c) Use the status of the unmanned ship multi-sensor positioning system obtained in step (b) and covariance to obtain a proposed distribution closer to the target probability function and sample from the constructed proposal distribution.

(d) Based on the Gaussian mixture model, generate the posterior probability density function p(x _k |z _1:k ) with a time step of k. The calculation formula of p(x _k |z _1:k ) is as follows:

In the formula, N(x _k |m _i , _vi ) is the i-th component in the Gaussian mixture model, C(k) is the number of component units of the discrete sample, and ξ is the discrete point component weight.

(e) The discrete sampling points sampled in step (c) and their corresponding weights Integrate it into the Gaussian mixture component component unit of step (d), and use the constructed continuous posterior probability density function p(x _k |z _1:k ) to resample the discrete particles, said p(x _k |z _1: The calculation formula of _k ) is as follows:

in:

In the formula, p(k) is the covariance of the discrete particle filter distribution, is the mean value of the discrete particle filter distribution, h is the normalization constant, and n _x is the particle distribution dimension.

(f) Cluster analysis is used to merge the Gaussian mixture similar units in the continuous posterior probability density function p(x _k |z _1:k ) in step (e).

First, the posterior probability density function representing the state is represented by a continuous probability density function constructed from discrete particles. Then the constructed continuous posterior probability density function is approximated as a Gaussian mixture distribution, and particle samples are extracted from it to replace the heavy distribution. Sampling is used to maintain particle diversity, avoid sample shortage, and improve filter fusion accuracy.

Further, the specific content and method steps of setting the adaptive threshold in the data fusion described in step 5 are:

a) After selecting the importance sampling process, the particle X _c with the largest weight in the discrete particle sample set is used as the cluster center, and the Mahalanobis distance D _i between other particles i is calculated. The calculation formula of D _i is as follows:

In the formula, i=1,2,…,N, is the probability density of particle i, and S is the covariance matrix.

b) Calculate the number of effective particle samples _Ne in the clustering unit. The calculation formula of _Ne is as follows:

In the formula, N is the number of particle samples, is the particle probability density covariance.

c) Construct a threshold T. The calculation formula of T is as follows:

In the formula, T ₀ is the initial value of the threshold, k _e is the proportion coefficient, and R is the number of classifications.

d) Substitute the effective particle sample number N _e obtained in step b) into the threshold T to construct the adaptive threshold T _c . The calculation formula of T _c is as follows:

e) Compare D _i with the adaptive threshold T _c . If D _i is less than T _c , the particles are classified into component units related to their probability mass; if D _i is greater than T _c , the particle is skipped and other particles are clustered.

f) Select the particle with the largest weight from the remaining particle samples as the cluster center, and repeat step e) until the clustering ends.

g) Based on the clustered component units, substitute the constructed continuous probability density function of the particle set described The calculation formula of the construction is as follows:

In the formula, β _i is the probability mass of similar component unit component i, γ _i is the mean value of component unit component i, p _i is the covariance of component unit component i, i = 1, 2,...,n.

The larger the probability density of the particle, the smaller the D _i calculated in step a). Since the adaptive threshold T _c is proportional to the particle probability density covariance, the T _c compared with D _i in step e) can be adjusted accordingly according to the size of Di. Therefore, when clustering Gaussian mixtures, there is no need to continuously Adjusting the threshold to merge similar units reduces the number of repeated clusterings of the remaining particle samples and improves clustering efficiency. In addition, because the constructed adaptive threshold is associated with the particle probability mass, when merging similar units of the particle set, particles with similar weights can be clustered into one component unit based on the threshold level to ensure that the same The weight difference between the particle sets within the component unit is the smallest, so that the weighted point set mixed Gaussian distribution constructed by step (e) is closer to the posterior probability density function after merging similar units, improving the resampling process. Medium sampling accuracy.

Further, the confidence factor in the data fusion described in step 5 is associated with the hierarchical sampling proportion capacity, and then the unmanned ship navigation trajectory positioning data is fused and filtered, and the specific content and method steps of outputting the unmanned ship navigation trajectory positioning information are: :

(1) According to the layering theory, the continuous probability density function Divided into l layers, the probability density function of each layer is p(x), and according to its probability mass, the group layer is divided into a group of weight advantage layers and two groups of disadvantage layers, which are respectively defined as l _a and l _b ,, l _c .

(2) Set the proportional capacities of the number of particles in the _la , l _b , and l _c layers to: N/4, N/3, and N/3 respectively.

(3) Substitute the confidence factor into the weight optimization combination calculation.

(4) Perform weight optimization and combination on the particles whose weights are less than the mean value ω _k in the l _b and l _c layers, obtain the optimized particle weights ψ′ _k ⁱ , and conduct hierarchical sampling of the sample data. The ω _k , The calculation formula of ψ′ _k ⁱ is as follows:

(5) Obtain the multi-sensor data fusion sampling results obtained in step (4).

(6) Fusion and sampling results of the data obtained in step (5) are output in the form of a log file from the blind node carried by the unmanned ship.

(7) The PC-side coordinator node placed on the riverside is networked with the unmanned ship blind node to obtain the unmanned ship position information output by the blind node in step (7) in real time, thereby realizing the navigation trajectory of the unmanned ship. positioning.

The sampling samples of the constructed continuous probability density function of the weighted point set are stratified, and the proportional capacity of each sampling layer is set to ensure that the number of sampling particles in the stratification is reasonably distributed, and then the _la layer group is sampled, and the The particle weights in the l _b and l _c layer groups are optimized and combined to increase their probability quality. At the same time, the confidence factor of the measurement data collected by the unmanned ship multi-sensor positioning system is associated with the hierarchical sampling in the multi-sensor data algorithm, so that When using the new threshold hierarchical particle filtering algorithm to perform data fusion on multi-sensor measurement data of unmanned ships, the sensor measurement values with larger confidence factors are preferentially sampled and fused.

The invention has the following advantages and beneficial effects:

(1) The present invention uses positioning data collected by multiple sensors of the unmanned ship to complement information, effectively overcomes the problem of sensor signal loss caused by environmental signal shielding, and ensures the effectiveness of sensor data collection.

(2) This invention improves the fault tolerance performance of the data fusion algorithm by performing consistency checks on the unmanned ship position information collected by the multi-sensor positioning system and performing weighted compensation on the fault data while ensuring that the basic particle filter algorithm performs The confidence level of the sampled particle collection sample.

(3) The Monte Carlo method used in basic particle filtering is used to solve the integral operation in Bayesian estimation to eliminate environmental noise interference and ensure the processing accuracy of the sample data set, thereby improving the multi-sensor measurement of unmanned ships. Data reliability.

(4) In the importance sampling process of the data fusion algorithm, the present invention integrates the current measurement information into the particle set suggested distribution, making the suggested distribution closer to the true posterior probability density, and improving the algorithm estimation performance; at the same time, it performs the optimization of the Gaussian mixture unit An adaptive threshold is constructed in the cluster analysis, and discrete particle samples are merged into similar component units, which reduces the complexity of clustering operations and improves the real-time signal processing and computing efficiency of the system.

(5) The present invention stratifies the sampling samples of the constructed continuous probability density function of the weighted point set, and sets the proportional capacity of each sampling layer to ensure that the number of sampling particles in the stratification is reasonably distributed, and then the _la layer group take samples and The particle weights in the l _b and l _c layer groups are optimized and combined to increase their probability quality. At the same time, the confidence factor of the measurement data collected by the unmanned ship multi-sensor positioning system is associated with the hierarchical sampling in the multi-sensor data algorithm, so that When using the new threshold hierarchical particle filtering algorithm to fuse multi-sensor measurement data of unmanned ships, priority is given to sampling and fusion of sensor measurement values with large confidence factors, thereby increasing the reference value of the entire information sample, and ultimately improving the accuracy of the data fusion algorithm. Fusion positioning accuracy of unmanned ship position data.

Description of drawings

Figure 1 is a general flow chart of an unmanned ship positioning method based on multi-sensor data fusion of the present invention;

Figure 2 Schematic diagram of the construction and preprocessing of the multi-sensor platform for unmanned ships;

Figure 3 Confidence determination flow chart;

Figure 4 Consistency check and weighted compensation flow chart;

Figure 5 Basic particle filter data enhancement processing flow chart;

Figure 6 Posterior probability distribution diagram at each moment;

Figure 7 Flowchart of constructing Gaussian mixture model;

Figure 8: Sampling particle density distribution diagram when sampling time k=30;

Figure 9 is a flow chart for setting adaptive thresholds and stratified sampling proportional capacity associated with confidence factors;

Figure 10(a) shows the algorithm comparison root mean square test chart,

Figure 10(b) shows the algorithm comparison standard deviation test chart;

Figure 11 River test chart;

Figure 12 Experimental diagram of unmanned ship navigation trajectory test.

Detailed ways

In order to make the purpose and technical solutions of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the drawings of the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. Based on the described embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts fall within the scope of protection of the present invention.

As shown in Figure 1, an unmanned ship positioning method based on multi-sensor data fusion of the present invention first preprocesses the positioning data collected by the unmanned ship multi-sensor positioning system; and then performs confidence on the unmanned ship positioning data. The distance is determined and given the corresponding confidence factor; at the same time, fault detection and weighted compensation are performed on the positioning data; then the positioning data is filtered to achieve data enhancement; finally, a new threshold hierarchical particle filtering algorithm is used to perform multi-sensor data fusion filtering output to achieve Precise positioning of the navigation trajectory of the unmanned ship.

Specifically adopt the following steps:

(1) Data preprocessing;

(2) Data confidence determination and assignment;

(3) Data failure inspection compensation;

(4) Data enhancement;

(5)Data fusion.

As shown in Figure 2, the method for preprocessing the unmanned ship positioning data collected by the unmanned ship multi-sensor positioning system includes the following steps:

(A) Unify the spatial and temporal benchmarks based on the longitude and latitude information of the unmanned ship's navigation trajectory collected by the positioning system sensor;

(B) Based on the river navigation of the unmanned ship, construct a communication environment that satisfies the effective connection of multiple nodes of the positioning system, so that the blind node on the unmanned ship is within a network of 4 signal receiving nodes with known transmit signal strength and position coordinates, and calculate based on this Collect the position coordinates of the unmanned ship while sailing on the river;

(C) According to the Gauss-Krüger projection principle, the coordinates of the multi-sensor positioning system are transformed and unified.

Using the positioning data collected by multiple sensors of the unmanned ship for information complementation, it effectively overcomes the problem of sensor signal loss caused by environmental signal shielding and ensures the effectiveness of sensor data collection.

As shown in Figure 3, the method for determining confidence and assigning confidence factors based on data collected by multiple sensors includes the following steps:

1) Based on the measurement data collected by the positioning system, construct a multi-sensor measurement model p _i (x), and calculate the confidence distance _di between sensor data; the p _i (x) and _di are calculated as follows:

In the formula, x′ _i and x″ _i are the measurement values collected by multiple sensors at time i respectively, τ′ _i and τ″ _i are the measurement variances of the corresponding sensors, is the mean variance of the measurement, Z is a random variable obeying the standard normal distribution, i=1,2,…,n.

2) Based on the Gaussian probability model, rewrite the d _i obtained in step 1) into the metric p _r (Z _i ) in the sense of probability between sensor measurement data, and set the sensor support confidence level to determine the reliability of different sensor information. Reliability; the p _r (Z _i ) is calculated as follows:

In the formula, Z _i is the measurement data of multiple sensors, ε is the confidence level, and K is the sampling sample probability interval variable coefficient.

3) Determine the credibility of the location information collected by multiple sensors based on the sensor confidence distance, and divide the positioning data into different confidence intervals based on the credibility of the information;

4) Construct the norm equation of the dynamic support factor β _i (k) and the Gaussian probability measurement model p _i (x) of the measurement information collected by multiple sensors at time k; the β _i (k) is calculated as follows:

In the formula, ||.|| _F is the Frobenius norm, k=1,2,…,T, i=1,2,…,n.

5) Calculate the system measurement error w _i based on the multi-sensor dynamic support factor β _i (k) obtained in step 4); the w _i is calculated as follows:
w _i =z _i -Aβ _i (k)

6) Suppose the variance of the system measurement error w _i is use to assign corresponding confidence factors to the measurement information The confidence factor The calculation is as follows:

In the formula, is the confidence factor of sensor measurement data, i=1,2,…,n.

According to the size of the information measurement value in the sense of probability between the measured data, the determined positioning data is divided into different confidence intervals, so that the credible data moves closer to the higher confidence area; at the same time, the set confidence level ε is consistent with the sensor measurement variance mean Correspondingly, it is used to indicate that the measurement data collected by the unmanned ship multi-sensor positioning system falls into the interval probability, so the variance of the measurement information is different, and the corresponding confidence interval classification of the unmanned ship positioning information also changes. It can be adjusted according to the size of the sensor measurement variance, which improves the sampling reliability of the sensor measurement data.

As shown in Figure 4, the method of performing weighted compensation processing on inconsistent fault data collected by multiple sensors by performing consistency testing on the multi-sensor data includes the following steps:

(I) Make an arithmetic average of the sensor measurement data obtained by the unmanned ship positioning system and obtain the arithmetic average described The calculation is as follows:

In the formula, x _i is the sensor measurement information, i=1,2,…,n.

(II) Use the difference between the arithmetic mean of the sensor measurement data obtained in step (I) and the subsequent sampling value x ^h of the positioning system;

(III) Set the system requirement error. If the difference between the arithmetic mean value of the sensor measurement data and the subsequent sampling value of the positioning system is less than the system requirement error, it means that the unmanned ship position data collected by the multi-sensor positioning system is consistent and is credible data. ; If the difference is greater than the system requirement error, variance weighted compensation needs to be performed on the sampled data to meet the sampling requirements of the basic particle filter algorithm for data samples;

(IV) Take the arithmetic mean of multi-sensor measurement data as truth value is an unbiased estimate to represent the information measurement variance σ′ _i ² of the i-th sensor system when the unmanned ship multi-sensor positioning system measures the navigation trajectory of the unmanned ship ^at different locations in the same _space . The calculation is as follows:

(V) Based on the data set recorded by the unmanned ship multi-sensor positioning system for m measurements of the unmanned ship, record the j-th measurement data of the i-th sensor as x _ij , and replace x _ij with the sensor in step (IV) The instrument measurement information x _i is used to obtain the variance of the data set information obtained through multiple measurements. described The calculation is as follows:

In the formula, i=1,2,…,n,j=1,2,…,m.

(VI) Define the fusion weight κ _i of inconsistent fault data in step (III) according to the variance of the position information data set collected by multiple sensors calculated in step (V); the κ _i is calculated as follows:

(VII) According to the required fusion weight κ _i , for inconsistent fault data Perform variance weighted fusion to obtain sensor measurement data that meets the requirements of basic particle filter processing described The calculation is as follows:

By checking the consistency of the unmanned ship position information collected by the multi-sensor positioning system and performing weighted compensation on the fault data, while improving the fault tolerance performance of the data fusion algorithm, the particle set sample sampled by the basic particle filter algorithm is guaranteed. credibility.

As shown in Figure 5, the method of filtering multi-sensor data that has been preprocessed and inspected and compensated based on the basic particle filter algorithm adopts the following steps:

(i) Establish a basic particle filter model of the unmanned ship multi-sensor positioning system, and substitute the unmanned ship position information after consistency testing and variance weighting into the system model; the status and measurement model of the sensor system are summarized by the following formula:

In the formula, x _k is the position prediction value of the sensor system at time k, x ^h is the subsequent sampling value of the sensor, The sensor measurement value after variance weighting, z _k is the unmanned ship position measurement value at time k, λ _k is the estimated noise, and ν _k is the measurement noise;

(iii) Randomly select N particle samples from the importance density function;

(iv) Calculate the weight of sampling particles and update; stated The calculation is as follows:

In the formula, is the sampling particle weight, i=1,2,...N;

(v) Normalized importance weight;

(vi) Calculate the effective particle number N _eff in the basic particle filter algorithm, and compare it with the threshold N _th . If N _eff <N _th , resampling is performed; the N _eff is calculated as follows:

(vii) Status output to obtain the filtered local estimate of the measurement information collected by the multi-sensor positioning system of the unmanned ship and covariance matrix described The calculation is as follows:

Particle propagation is to generate a new particle state x _k by sampling the state transition model p _z (x _k |x _k-1 , z _k ) of the unmanned ship multi-sensor positioning system, where x _k-1 is the weight of the previous step The particle state after sampling, z _k is the sensor system observation data. The basic particle filter algorithm is used to filter the measurement data of the unmanned ship's multi-sensor positioning system to achieve data enhancement, which reduces the interference of environmental noise on the measurement data, ensures the processing accuracy of the sample data set, and thereby improves the accuracy of the unmanned ship's multi-sensor positioning system. Reliability of measurement data.

As shown in Figure 6 and Figure 7, the method of constructing a Gaussian mixture model adopts the following steps:

(a) Extract the multi-sensor data set processed by the basic particle filter algorithm and calculate the Sigma point set described The calculation is as follows:

In the formula, is the Sigma point set after traceless transformation, n _a is the dimension of the Sigma point, and λ is the scale parameter;

(b) Integrate the latest measurement information into the obtained Sigma sampling points, and analyze the system status and covariance to update; to The calculation is as follows:

In the formula, k _k is the Kalman gain, z _k is the measurement information, is the covariance measured by the weighted Sigma point set;

(c) Using the multi-sensor positioning system status obtained in step (b) and covariance to obtain a proposed distribution closer to the target probability function and sample from the constructed proposal distribution;

(d) Based on the Gaussian mixture model, generate the posterior probability density function p(x _k |z _1:k ) with time step k; the p(x _k |z _1:k ) is expressed as follows:

In the formula, N(x _k |m _i ,v _i ) is the i-th component in the Gaussian mixture model, C(k) is the number of component units of the discrete sample, and ξ is the discrete point component weight;

(e) The discrete sampling points sampled in step (c) and their corresponding weights Integrate into the Gaussian mixture component component unit of step (d), and use the constructed continuous posterior probability density function p(x _k |z _1:k ) to resample the discrete particles; the p(x _k |z _{1: k} ) is expressed as follows:

in:

In the formula, p(k) is the covariance of the discrete particle filter distribution, is the mean value of the discrete particle filter distribution, h is the normalization constant, and n _x is the particle distribution dimension;

In the importance sampling process of the new threshold hierarchical particle filter algorithm, the current measurement information is integrated into the particle set recommended distribution, making the recommended distribution closer to the true posterior probability density and improving the algorithm estimation performance. It can be seen from Figure 6 that the constructed weighted Gaussian mixture distribution function satisfies the continuous multimodal normal distribution at all times, indicating that its weighted point set mixture Gaussian distribution composed of similar component units approximates the posterior probability density function, and the function peak The distribution is concentrated, indicating that the particle sample effectively represents the probability distribution characteristics after resampling.

As shown in Figures 8 and 9, the adaptive threshold and the hierarchical sampling proportional capacity associated with the confidence factor are set, and then the unmanned ship navigation trajectory positioning data is fused and filtered, and the unmanned ship navigation trajectory positioning information is output. Method, the specific steps are as follows:

a) After selecting the importance sampling process, the particle X _c with the largest weight in the discrete particle sample set is used as the cluster center, and the Mahalanobis distance D _i between other particles i is calculated. The D _i is expressed as follows:

b) Calculate the number of effective particle samples _Ne in the clustering unit. The _{Ne is} calculated as follows:

c) Construct a threshold T, which is expressed as follows:

d) Substitute the effective particle sample number N _e obtained in step b) into the threshold _T to construct an adaptive threshold T _c , which is expressed as follows:

g) Based on the clustered component units, substitute the constructed continuous probability density function of the particle set described The structure is as follows:

In the formula, β _i is the probability mass of similar component unit component i, γ _i is the mean value of component unit component i, p _i is the covariance of component unit component i, i=1,2,…,n.

h) According to the hierarchical theory, the continuous probability density function Divided into l layers, the probability density function of each layer is p(x), and according to its probability mass, the group layer is divided into a group of weight advantage layers and two groups of disadvantage layers, which are respectively defined as l _a and l _b ,, l _c .

i) Set the proportional capacity of the number of particles in the la, l _b _and l _c layers respectively to: N/4, N/3, N/3.

j) Substitute the confidence factor into the weight optimization combination calculation.

k) Perform weight optimization and combination on the particles whose weights are less than the mean value ω _k in the l _b and l _c layers to obtain the optimized particle weights ψ′ _k ⁱ , and perform hierarchical sampling of the sample data. The ψ _k , ψ ′ _k ⁱ is calculated as follows:

l) Obtain the multi-sensor data fusion sampling results obtained in step (k).

m) Fusion the data sampling results obtained in step (l) and output them in the form of a log file from the blind node carried by the unmanned ship.

n) The PC-side coordinator node placed on the riverside is networked with the unmanned ship blind node to obtain the unmanned ship position information output by the blind node in step (u) in real time, thereby realizing the navigation trajectory of the unmanned ship. position.

The sampling samples of the constructed continuous probability density function of the weighted point set are stratified, and the proportional capacity of each sampling layer is set to ensure that the number of sampling particles in the stratification is reasonably distributed, and then the _la layer group is sampled, and the l _b , l _c in layer group The particle weights are optimized and combined to increase their probability quality. At the same time, the confidence factor of the measurement data collected by the unmanned ship multi-sensor positioning system is associated with the hierarchical sampling in the multi-sensor data algorithm, so that the new threshold hierarchical particles can be used. When the filtering algorithm performs data fusion on multi-sensor measurement data of unmanned ships, sensor measurement values with larger confidence factors are sampled and fused first, which increases the reference value of the entire information sample. It can be seen from Figure 8 that when time k = 30, the sampling points are concentrated at the maximum value of the continuous probability density function, which shows that by substituting the sensor data confidence factor into the stratified sampling calculation, when resampling the continuous probability density function , ensuring that the sampling points are concentrated at places with larger probability density function values, improving the reference value of the entire information sample, and thereby ensuring the fusion positioning accuracy of the measurement data by the new threshold hierarchical particle filtering algorithm.

In order to verify the optimization performance of the new threshold hierarchical particle filtering algorithm of the present invention, 30 independent tests were conducted on the computer simulation test system model around the root mean square error RMSE and standard deviation Std, and the test results were compared with the extended Kalman filter (Extended Kalman Filter,EKF) ^[1] , Unscented Kalman Filter (UKF) ^[2] , Unscented Particle Filter (UPF) ^[3] and Basic Particle Filter BPF ^[4] A comparison was made to verify the effectiveness of the improvement steps of the filtering algorithm. The simulation time step of the five algorithms is k=50, and the time interval Δt=1. The parameters of the algorithms to be compared are taken from the corresponding references.

Attached is a computer simulation test model:

Attached references:

[1] He Junyi, Li Nannan, An Weipeng. Research on improved extended Kalman filter algorithm [J]. Measurement and Control Technology, 2018, 37(12): 102-106.

[2] Li Xingjia, Li Jianfen, Zhu Min, et al. Research on positioning fusion and verification algorithm based on unscented Kalman filter [J]. Automotive Engineering, 2021, 43(6): 825-832.

[3] Wu Bin, Tian Qing. Indoor moving target positioning algorithm with improved unscented particle filtering [J]. Sensors and Microsystems, 2021, 40(3): 153-156,160.

[4] Gao Yi, Mao Yanhui, Yang Yi. Artificial fish swarm particle filtering algorithm [J]. Modern Electronic Technology, 2021, 44(16): 170-174.

As can be seen from Table 1, Figure 10(a), and Figure 10(b), first of all, whether it is the mean RMSE or the maximum value, the TLPF in this article is smaller than the other four algorithms, which illustrates the performance of the new threshold hierarchical particle filtering algorithm of this patent. The filtering accuracy is the highest. This is mainly because the article clusters discrete particles by constructing an adaptive threshold in the Gaussian mixture, and optimizes the weight combination of particles in the disadvantaged layer, which improves the diversity of particle samples. Judging from the mean and maximum values of Std, compared with the other four algorithms, the value of TLPF is also the smallest, which shows that the filtering stability of TLPF is also the best. This is mainly due to the importance function during the importance sampling process. The selection of , the latest measurement data is integrated into the importance function through unscented transformation, which improves the credibility of the particle samples. In the resampling process, the particles in the inferior layer are re-stratified and sampled, and more particle samples are substituted into the sampling process, thereby ensuring the diversity of the particle sample set.

In order to verify the optimization performance of the multi-sensor data fusion algorithm of the present invention, a comparative positioning test of multi-sensor system algorithms based on an unmanned ship platform was carried out. 100 sets of positioning data collected by the multi-sensor system in the experimental river were selected, and the root mean square was used to Error RMSE and standard deviation Std are used as performance indicators of experimental test results, and the test results are compared with extended Kalman filter, unscented Kalman filter, unscented particle filter and elementary particle filter. Figure 4 shows the performance of each algorithm on The results of filtering and fusion of human and ship navigation positioning trajectory data.

It can be seen from Table 2, Figure 11, and Figure 12 that when the unmanned ship sails in the test river, due to dense trees and long and narrow rivers, there is a certain environmental disturbance impact on the sensor signal transmission, which makes the filtering positioning results of each data fusion algorithm In some places, it seriously deviates from the true path of the unmanned ship, and the positioning results are prone to large jumps at the turns of the path, gradually deviating from the true trajectory of the unmanned ship, reducing the reliability of the positioning results. As time goes by, , the positioning result error is getting bigger and bigger. However, the positioning accuracy of the multi-sensor data fusion algorithm of the present invention is higher than the fusion positioning accuracy of any algorithm. This is mainly due to the fact that the present invention first performs a fault check on the multi-sensor measurement data of the unmanned ship through a consistency check and weights the inconsistent data. Compensation processing improves its credibility, so that the basic particle filter has more and more reliable sampling samples when performing data enhancement on data samples, which improves the processing accuracy and fault tolerance performance of the data fusion algorithm; secondly, through the confidence test, the The data collected by the multi-sensor positioning system of the man and ship are tested for confidence, and the corresponding positioning data is given a confidence factor based on the confidence distance, and is associated with the subsequent layered sampling operation steps of the sensor data fusion algorithm; finally, the new threshold layering is used The particle filter algorithm improves the efficiency of clustering merging by constructing a Gaussian mixed continuous probability density function and setting adaptive thresholds, improves the real-time performance of the data fusion algorithm in processing positioning data, and integrates the confidence factor into the correlation and hierarchical sampling weight calculations. , so that particle samples with higher confidence factors are sampled first, which improves the positioning accuracy of the data fusion algorithm. Compared with the other four data fusion algorithms, the average positioning error of the multi-sensor data fusion algorithm of the present invention is reduced by 47%, fully ensuring the positioning accuracy of the unmanned ship's navigation trajectory.

Table I:

Table II:

The above embodiments are only used to illustrate the technical solutions of the present invention but not to limit them. Although the present invention has been described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that the specific implementations of the present invention can still be modified. or equivalent substitutions. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention shall be covered by the scope of the claims of the present invention.

Claims

An unmanned ship positioning method based on multi-sensor data fusion, which is characterized by including the following steps:

(1) Preprocess the unmanned ship positioning data collected by the unmanned ship multi-sensor positioning system. The specific content and method steps are:

(A) Unify the spatial and temporal benchmarks based on the longitude and latitude information of the unmanned ship's navigation trajectory collected by the positioning system sensor;

(B) Based on the river navigation of the unmanned ship, construct a communication environment that satisfies the effective connection of multiple nodes of the positioning system, so that the blind node on the unmanned ship is within a network of 4 signal receiving nodes with known transmit signal strength and position coordinates, and calculate based on this Collect the position coordinates of the unmanned ship while sailing on the river;

(C) According to the Gauss-Krüger projection principle, the coordinates of the multi-sensor positioning system are transformed and unified;

(2) Confidence determination and confidence factor assignment based on data collected by multiple sensors;

(3) Based on the consistency check of multi-sensor data, weighted compensation processing is performed on the inconsistent fault data collected by the positioning system;

(4) Based on the basic particle filter algorithm, the multi-sensor data after preprocessing, inspection and compensation are filtered respectively;

(5) Design a new threshold hierarchical particle filter algorithm by constructing a Gaussian mixture model and setting an adaptive threshold and a hierarchical sampling proportional capacity associated with a confidence factor, and then fuse the unmanned ship navigation trajectory positioning data Filter and output the navigation trajectory positioning information of the unmanned ship.
An unmanned ship positioning method based on multi-sensor data fusion according to claim 1, characterized in that the specific content and method of confidence determination and confidence factor assignment based on data collected by multiple sensors in step (2) The steps are:

1) Based on the measurement data collected by the positioning system, construct a multi-sensor measurement model p i (x), and calculate the confidence distance di between sensor data; the p i (x) and di are calculated according to the following formulas:

In the formula, x i is the measurement value of the i-th sensor, μ is the true value of the measurement feature, θ i is the information measurement accuracy of the i-th sensor, and σ i is the information measurement error of the i-th sensor;

In the formula, x′ i and x″ i are the measurement values collected by multiple sensors at time i respectively, τ′ i and τ″ i are the measurement variances of the corresponding sensors, is the mean variance of the measurement, Z is a random variable obeying the standard normal distribution, i=1,2,…,n;

2) Based on the Gaussian probability model, rewrite the d i obtained in step 1) into the metric p r (Z i ) in the sense of probability between sensor measurement data, and set the sensor support confidence level to determine the reliability of different sensor information. Reliability; the p r (Z i ) is calculated according to the following formula:

In the formula, Z i is the measurement data of multiple sensors, ε is the confidence level, and K is the sampling sample probability interval variable coefficient;

3) Determine the credibility of the location information collected by multiple sensors based on the sensor confidence distance, and divide the positioning data into different confidence intervals based on the credibility of the information;

4) Construct the norm equation of the dynamic support factor β i (k) and the Gaussian probability measurement model p i (x) of the measurement information collected by multiple sensors at time k; the β i (k) is calculated as follows:

In the formula, ||.|| F is the Frobenius norm, k=1,2,…,T, i=1,2,…,n;

5) Calculate the system measurement error w i based on the multi-sensor dynamic support factor β i (k) obtained in step 4); the w i is calculated according to the following formula:
w i =z i -Aβ i (k)

In the formula, A is the state transition matrix, z i is the measurement value of the unmanned ship multi-sensor positioning system, i = 1, 2,...,n;

6) Suppose the variance of the system measurement error w i is use To assign corresponding confidence factors to the measurement data The confidence factor Calculate according to the following formula:

In the formula, is the confidence factor of sensor measurement data, i=1,2,…,n.
An unmanned ship positioning method based on multi-sensor data fusion according to claim 1, characterized in that step (3) is based on performing a consistency check on multi-sensor data to detect inconsistent faults collected by the positioning system. The specific content and method steps of weighted compensation processing of data are:

(I) Make an arithmetic average of the sensor measurement data obtained by the unmanned ship positioning system and obtain the arithmetic average described Calculate according to the following formula:

In the formula, x i is the sensor measurement information, i=1,2,…,n;

(II) Use the difference between the arithmetic mean of the sensor measurement data obtained in step (I) and the subsequent sampling value x h of the positioning system;

(III) Set the system requirement error. When the difference between the arithmetic mean value of the sensor measurement data and the subsequent sampling value of the positioning system is less than the system requirement error, it is determined that the unmanned ship position data collected by the multi-sensor positioning system is consistent and is credible. data; when the difference is greater than the system required error, variance weighted compensation needs to be performed on the sampled data to meet the sampling requirements of the basic particle filter for data samples;

(IV) Take the arithmetic mean of multi-sensor measurement data as truth value is an unbiased estimate to represent the information measurement variance of the i-th sensor system when the unmanned ship multi-sensor positioning system measures the unmanned ship's navigation trajectory at different locations in the same space. described Calculate according to the following formula:

(V) Based on the data set recorded by the unmanned ship multi-sensor positioning system for m measurements of the unmanned ship, record the j-th measurement data of the i-th sensor as x ij , and replace x ij with the sensor in step (IV) The instrument measurement information x i is used to obtain the variance of the data set information obtained through multiple measurements. described Calculate according to the following formula:

In the formula, i=1,2,…,n,j=1,2,…,m;

(VI) Define the fusion weight κ i of inconsistent fault data in step (III) according to the variance of the position information data set collected by multiple sensors calculated in step (V); the κ i is calculated as follows:

(VII) According to the required fusion weight κ i , for inconsistent data Perform weighted compensation to obtain sensor measurement data that meets the requirements of basic particle filtering processing described Calculate according to the following formula:
An unmanned ship positioning method based on multi-sensor data fusion according to claim 1, characterized in that the step (4) is based on a basic particle filter algorithm, and the pre-processed, inspected and compensated multi-sensor The specific content and method steps of data filtering are:

(i) Establish a basic particle filter model of the unmanned ship multi-sensor positioning system, and substitute the unmanned ship position information after consistency testing and variance weighting into the system model; the status and measurement model of the sensor system are summarized by the following formula:

In the formula, x k is the position prediction value of the sensor system at time k, x h is the subsequent sampling value of the sensor, The sensor measurement value after variance weighting, z k is the unmanned ship position measurement value at time k, λ k is the estimated noise, and ν k is the measurement noise;

(ii) Particle set sample initialization, randomly sampling from the prior density p(x 0 ) to generate an initialization particle set, The weight of all particles is 1/N,

(iii) Randomly select N particle samples from the importance density function;

(iv) Calculate the weight of sampling particles and update; stated Calculate according to the following formula:

In the formula, ω i k is the sampling particle weight, i = 1, 2,...N;

(v) Normalized importance weight;

(vi) Calculate the effective number of particles N eff in the basic particle filter algorithm, and compare it with the threshold N th . If N eff <N th , resampling is performed; the N eff is calculated as follows:

(vii) Status output to obtain the filtered local estimate of the measurement information collected by the multi-sensor positioning system of the unmanned ship and covariance matrix described Calculate according to the following formula:
An unmanned ship positioning method based on multi-sensor data fusion according to claim 1, characterized in that the specific content and method steps of constructing a Gaussian mixture model in step (5) are:

(a) Extract the multi-sensor data set processed by the basic particle filter algorithm and calculate the Sigma point set described Calculate according to the following formula:

In the formula, is the Sigma point set after traceless transformation, n a is the dimension of the Sigma point, and λ is the scale parameter;

(b) Integrate the latest measurement information into the obtained Sigma sampling points, and analyze the system status and covariance to update; to Calculate according to the following formula:

In the formula, k k is the Kalman gain, z k is the measurement information, is the covariance measured by the weighted Sigma point set;

(c) Using the multi-sensor positioning system status obtained in step (b) and covariance Construct a proposal distribution that is closer to the target probability function and sample from the constructed proposal distribution;

(d) Based on the Gaussian mixture model, generate the posterior probability density function p(x k |z 1:k ) with time step k; the p(x k |z 1:k ) is expressed as follows:

In the formula, N(x k |m i ,v i ) is the i-th component in the Gaussian mixture model, C(k) is the number of component units of the discrete sample, and ξ is the discrete point component weight;

(e) The discrete sampling points sampled in step (c) and their corresponding weights Integrate it into the Gaussian mixture component component unit of step (d), and use the constructed continuous posterior probability density function p(x k |z 1:k ) to resample the discrete particles; the p(x k |z 1: k ) is expressed as follows:

in:

In the formula, p(k) is the covariance of the discrete particle filter distribution, is the mean value of the discrete particle filter distribution, h is the normalization constant, and n x is the particle distribution dimension;

(f) Cluster analysis is used to merge the Gaussian mixture similar units in the continuous posterior probability density function p(x k |z 1:k ) in step (e).
An unmanned ship positioning method based on multi-sensor data fusion according to claim 1, characterized in that the specific content and method steps of setting the adaptive threshold in step (5) are:

(g) After selecting the importance sampling process, the particle X c with the largest weight in the discrete particle sample set is used as the cluster center, and the Mahalanobis distance D i between other particles i is calculated; the D i is expressed as follows:

In the formula, i=1,2,…,N, is the probability density of particle i, and S is the covariance matrix;

(h) Calculate the number of effective particle samples Ne in the clustering unit; the Ne is calculated according to the following formula:

In the formula, N is the number of particle samples, is the particle probability density covariance;

(i) Construct a threshold T; the T is expressed as follows:

In the formula, T 0 is the initial value of the threshold, k e is the proportion coefficient, and R is the number of classifications;

(j) Substitute the number of effective particle samples N e obtained in step (h) into the threshold T to construct the adaptive threshold T c ; the T c is expressed as follows:

(k) Compare D i with the adaptive threshold T c . When D i is less than T c , the particle is classified into the component unit related to its probability mass; when D i is greater than T c , the particle is skipped. Other particles are clustered;

(m) Select the particle with the largest weight from the remaining particle samples as the cluster center, and repeat step (k) until clustering Finish;

(n) Based on the clustered component units, substitute the constructed continuous probability density function of the particle set described Expressed as follows:

In the formula, β i is the probability mass of similar component unit component i, γ i is the mean value of component unit component i, p i is the covariance of component unit component i, i = 1, 2,...,n.
An unmanned ship positioning method based on multi-sensor data fusion according to claim 1, characterized in that the confidence factor in step (5) is associated with the hierarchical sampling proportion capacity, and then the unmanned ship navigation trajectory is positioned The specific content and method steps of data fusion and filtering to output unmanned ship navigation trajectory positioning information are:

(o) According to the hierarchical theory, the continuous probability density function Divided into l layers, the probability density function of each layer is p(x), and according to its probability mass, the group layer is divided into a group of weight advantage layers and two groups of disadvantage layers, which are respectively defined as l a and l b ,,l c ;

(p) Set the proportional capacities of the number of particles in the la, l b and l c layers respectively as: N/4, N/3, N/3;

(q) Substitute the confidence factor into the weight optimization combination calculation;

(r) For l b , the weight in the l c layer is less than the mean The particles are weighted and combined to obtain optimized particle weights. and conduct stratified sampling of the sample data; as described Calculate according to the following formula:

(s) Obtain the multi-sensor data fusion sampling result obtained in step (r);

(t) Fusion and sampling results of the data obtained in step (s) are output in the form of a log file from the blind node carried by the unmanned ship;

(u) The PC-side coordinator node placed on the riverside is networked with the unmanned ship blind node to obtain the unmanned ship position information output by the blind node in step (u) in real time, thereby realizing the navigation trajectory of the unmanned ship. positioning.