Water surface unmanned vehicle trajectory tracking method and system based on distributed fusion strategy

Abstract

The application discloses a kind of water unmanned vehicle trajectory tracking method and system based on distributed fusion strategy, belong to automatic control field.First, the state estimation problem of water unmanned vehicle based on multiple radars is constructed;Then, by means of radar measurement vector, the mean and variance of random variable after transformation by nonlinear radar measurement model are approximated using unscented transformation, and a local nonlinear filter is designed under the least mean square error criterion;Further, the distributed fusion estimation value is obtained by fusing local estimates based on distributed fusion strategy, and the real-time tracking of unmanned vehicle motion trajectory is realized.The simulation results show that the application realizes the real-time tracking of water unmanned vehicle trajectory.

Description

TECHNICAL FIELD

[0001] The application relates to a water surface unmanned ship trajectory tracking method and system based on a distributed fusion strategy, which is used for improving the tracking effect of the water surface unmanned ship and belongs to the automatic control field. BACKGROUND

[0002] Trajectory tracking is to make a target ship reach a predetermined position within a specified time, so that the ship can complete the navigation task according to the trajectory predetermined by people, thereby enhancing the safety and efficiency of the ship. Due to the change of the self-load and the disturbance of the navigation environment, the motion model of the unmanned ship is highly nonlinear and difficult to determine, and many factors in the model directly affect the accuracy of the designed filter. Therefore, accurate estimation of the mathematical model of the unmanned ship is of great significance to the study of the motion control of the unmanned ship.

[0003] With the development of unmanned systems, the water surface unmanned ship has been widely concerned in the military and civilian fields, and is commonly applied to scenes such as environmental monitoring, marine search and rescue, resource exploration and the like. In recent years, the trajectory tracking problem of the water surface unmanned ship has also been studied a lot. For example, the research on the unmanned ship trajectory tracking algorithm based on deep reinforcement learning [Xia Jia-wei, Zhu Xu-fang, Luo Ya-song, Wu Zhaodong. Research on unmanned ship trajectory tracking algorithm based on deep reinforcement learning [J / OL]. Journal of Huazhong University of Science and Technology (Natural Science Edition). 2022: 1-8.] considers the time constraint existing in the water surface unmanned ship trajectory tracking control problem, and good tracking effect is achieved. However, most of the existing researches do not consider the multi-radar observation water surface unmanned ship model, and only establish an optimization problem for the single-radar observation model. Compared with the single-radar observation model, the multi-radar observation model has the following advantages: (1) there is a certain redundancy between the measurement information of multiple radars, when some radars cannot be used or are disturbed, or a target is not in the coverage range, generally there will be a radar that can provide information; (2) through the action range of multiple overlapping radars, the spatial coverage range is expanded, a radar can detect places that other radars cannot detect; (3) the detection probability is improved through the cooperation of multiple radars, a radar can detect targets or events that other radars cannot consider in a certain time period; (4) multiple radars confirm the same target or event, which can improve the credibility. SUMMARY

[0004] The application aims to solve the problems existing in the prior art, and provides a water surface unmanned ship trajectory tracking method and system based on a distributed fusion strategy, which introduces a multi-radar observation distributed fusion estimation model to improve the tracking performance of the water surface unmanned ship model trajectory.

[0005] Technical solution: To achieve the above-mentioned purposes, the application provides a water surface unmanned ship trajectory tracking method based on a distributed fusion strategy, which comprises the following steps:

[0006] Step 1, constructing a water surface unmanned ship state estimation problem based on multiple radars;

[0007] Step 2, relying on the radar measurement vector, using unscented transformation to approximate the mean and variance of the random variable after the nonlinear radar measurement model transformation, designing a local nonlinear filter under the minimum mean square error criterion to obtain the local optimal estimation of the unmanned ship state vector of each radar;

[0008] Step 3, fusing the local estimation based on the distributed fusion strategy to obtain the distributed fusion estimation value of the unmanned ship state vector, and realizing real-time tracking of the unmanned ship motion trajectory.

[0009] As preferred, the system equation used in the estimation problem constructed in step 1 is:

[0010] x(k)=f(x(k-1))+B(k-1)ω(k-1);

[0011] y i (k)=h i (x(k))+υ i (k),i=1,…,L;

[0012] Wherein x(k-1) and x(k) are the state vectors of the water surface unmanned ship at the k-1 and k moments respectively, y i 9k) represents the measurement vector of the i-th radar at the k moment, and L is the number of radars; f represents the mapping from the state vector x(k-1) at the k-1 moment to the state vector x(k) at the k moment without considering noise, h i (k) represents the mapping from the state vector x(k) at the k moment to the i-th radar measurement vector y i (k) without considering noise, ω(k-1) represents the zero-mean process noise at the k-1 moment, v i (k) represents the zero-mean measurement noise of the i-th radar at the k moment; the system sampling period is T, and the matrix B has the following form:

[0013]

[0014] As preferred, in step 2, it specifically comprises:

[0015] Step 21, initializing the distributed fusion estimation method, including the state estimation value and the covariance matrix

[0016] Step 22, obtaining the measurement vector y i(k) ;

[0017] Step 23, predict the USV state vector at time k and the covariance matrix of the state vector prediction error

[0018] Step 24, predict the measurement vector of the radar at time k the covariance matrix of the measurement vector prediction error and the cross-covariance matrix

[0019] Step 25, construct the optimization problem adopted by the local state estimation, including:

[0020] Based on the minimum local variance criterion, the following optimization problem is established and solved to obtain the local optimal estimation

[0021]

[0022] where K i (k) is the gain matrix of the i-th radar at time k, is the error covariance matrix of the state vector estimate value estimated by the i-th radar at time k;

[0023] Step 26, obtain the local optimal estimation of the state vector according to the following nonlinear filtering iterative form:

[0024]

[0025] As preferred, in step 3, specifically includes:

[0026] Step 31, given the local estimation of the first radar and the second radar and obtain the local fusion estimation based on the distributed fusion strategy

[0027]

[0028] where M1(k) is the fusion gain matrix of the local estimation 1, and M2(k) is the fusion gain matrix of the local estimation 2;

[0029] Step 32, given the local fusion estimation and the local estimation of the third radar obtain the local fusion estimation based on the distributed fusion strategy

[0030]

[0031] Where L1(k) is the fusion gain matrix of local fusion estimation 1, and M3(k) is the fusion gain matrix of local estimation 3;

[0032] Step 33: Following the method in step 32, sequentially fuse all local estimates. Obtain distributed fusion estimation

[0033]

[0034] As a preferred option, in step 31...

[0035]

[0036] Where λ∈[0,1], P1 Δ (k) represents the local fusion estimation The covariance matrix, These are the error covariance matrices of local estimates 1 and 2, respectively.

[0037] Preferably, in step 32...

[0038]

[0039] Where λ∈[0,1], Indicates local fusion estimation The covariance matrix, These are the error covariance matrices of local estimate 3.

[0040] Preferably, the state vector of the unmanned surface vessel includes the position and velocity of the center of mass of the unmanned surface vessel in the world coordinate system.

[0041] Based on the same inventive concept, this invention provides a surface unmanned vessel trajectory tracking system based on a distributed fusion strategy, comprising:

[0042] The problem construction module is used to construct state estimation problems for unmanned surface vessels based on multiple radars;

[0043] In addition, a trajectory tracking module is used to approximate the mean and variance of random variables after transformation by a nonlinear radar measurement model by using unscented transformation based on radar measurement vectors, and to design local nonlinear filters under the minimum mean square error criterion to obtain the local optimal estimate of the unmanned surface vessel state vector for each radar; and to obtain the distributed fusion estimate of the unmanned surface vessel state vector by fusing local estimates based on a distributed fusion strategy, so as to realize the real-time tracking of the unmanned surface vessel's motion trajectory.

[0044] Based on the same inventive concept, the present invention provides a computer system including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the computer program is loaded onto the processor, it implements the steps of the unmanned surface vessel trajectory tracking method.

[0045] Beneficial effects: Compared with the single-radar observation model, the multi-radar observation model proposed in this invention not only improves reliability but also expands the spatial and temporal coverage, enabling better observation of the position of unmanned surface vessels (USVs). The local nonlinear filter designed in this invention has high computational accuracy for nonlinearly distributed statistics, effectively overcoming the shortcomings of low estimation accuracy and poor stability of Kalman filtering. The distributed fusion estimation algorithm designed in this invention reduces computational complexity compared with the weighted fusion estimation algorithm, and its weight coefficients are random constants. The state estimation method based on the distributed fusion strategy designed in this invention can track the motion trajectory of USVs in real time. Simulation results show that this invention achieves real-time tracking of USV trajectories. Attached Figure Description

[0046] Figure 1 This is a general flowchart of the method in an embodiment of the present invention.

[0047] Figure 2 This is a flowchart of the distributed fusion estimation method in an embodiment of the present invention.

[0048] Figure 3 This is a trajectory tracking curve of the unmanned surface vessel estimated in this embodiment. Detailed Implementation

[0049] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0050] Depend on Figure 1As shown in the embodiments of the present invention, the unmanned surface vessel (USV) trajectory tracking method first constructs a USV state estimation problem based on multiple radars. Specifically, it establishes the system state equation based on the USV's kinematic equations and obtains system state information data based on real-time differential positioning, using this information data as the state, thus transforming the USV trajectory tracking problem into a state estimation problem. Then, relying on radar measurement vectors, it uses unscented transformation to approximate the mean and variance of random variables after transformation by a nonlinear radar measurement model. Under the minimum mean square error criterion, a local nonlinear filter is designed to obtain a locally optimal estimate of the USV state vector for each radar. Furthermore, based on a distributed fusion strategy, the local estimates are fused to obtain a distributed fusion estimate of the USV state vector, achieving real-time tracking of the USV's trajectory. Specifically, this involves: designing a local nonlinear filter based on the minimum mean square error criterion; and further, using a distributed fusion strategy to fuse the local estimates to obtain a distributed fusion estimate.

[0051] The following uses the trajectory tracking of an unmanned surface vessel model as an example to illustrate the detailed implementation process of this invention, including the following specific steps:

[0052] S1 is constructed considering the state estimation problem of unmanned surface vessels.

[0053] For an unmanned surface vessel system, the state equation can be written in the following form:

[0054]

[0055]

[0056] Where T is the system sampling period, s x (k), s y (k) represents the position of the center of mass of the unmanned surface vessel in the x and y directions in the world coordinate system at time k, and u(k) and v(k) represent the velocities of the center of mass of the unmanned surface vessel in the x and y directions in the world coordinate system at time k.

[0057] The state vector of the unmanned surface vessel is selected as x=[s x ,u,s y ,v] T ,Right now:

[0058] x(k)=[s x (k),u(k),s y (k),v(k)] T ;

[0059] The system equations used in the state estimation problem can then be written as:

[0060] x(k)=f(x(k-1))+B(k-1)ω(k-1);

[0061] Where x(k-1) and x(k) are the state vectors of the unmanned surface vessel at times k-1 and k, respectively, and f represents the mapping from the state vector x(k-1) at time k-1 to the state vector x(k) at time k, without considering noise, and satisfies:

[0062] f = @(x)[s x (k)+Tu(k); u(k); s y (k)+Tv(k); v(k)];

[0063] Where @(x) indicates that f is a function of x, ω(k-1) represents the zero-mean process noise at time k-1 with variance Q(k), and the noise matrix B has the following form:

[0064]

[0065] The measurement equations are constructed based on the measurement principles of radar:

[0066] y i (k)=h i (x(k))+υ i (k) = , i = 1, ..., L;

[0067] y i (k) represents the measurement value of the i-th radar at time k, and L is the number of radars; h i This represents the state vector x(k) at time k to the i-th radar measurement y when noise is not considered. i A nonlinear mapping of (k) that satisfies:

[0068]

[0069] Where @(x) represents h i It is a function of x, [s x (k),s y [(k)] is the centroid coordinate of the unmanned surface vessel in the world coordinate system at time k, [s x,i (k),s y,i [(k)] is the centroid coordinate of the i-th radar in the world coordinate system at time k. i (k) represents the zero-mean measurement noise of the i-th radar at time k, with variance R. i (k).

[0070] S2 uses a distributed fusion estimation method to track the trajectory of the unmanned surface vessel model.

[0071] Design of local nonlinear filters:

[0072] S21, Initialize the distributed fusion estimation method;

[0073] Select initial state estimate The value should be close to the actual initial state of the unmanned surface vessel, and the covariance matrix of the initial state estimate error should be given.

[0074] S22, acquire radar measurement values;

[0075] The measurement value y of the i-th radar i (k) Obtained through radar observations;

[0076] S23, the covariance matrix of the predicted state vector and the prediction error of the state vector;

[0077] First, generate a set of σ sample points for the i-th radar at time k-1.

[0078]

[0079] in, It is the state vector estimated by the i-th radar at time k-1. Let be the covariance matrix of the error of the state vector estimate, n be the dimension of the state vector, and α be a constant used to reduce the total prediction error. and Represents the τ-th column of the square root of the matrix;

[0080] Then calculate the corresponding weights for the σ sample points:

[0081]

[0082]

[0083] Where the subscript m is the mean, c is the covariance, the superscript is the nth σ sample point, the selection of a controls the distribution of the sample points, and b is a non-negative weighting coefficient.

[0084] Then, the predicted value of each σ point is mapped using the state equation of the unmanned surface vessel system.

[0085] Based on the predicted value of point σ Predict the state vector of the i-th radar at time k.

[0086] Then, the covariance matrix of the error of the state vector prediction value is calculated.

[0087]

[0088] S24, predict radar measurements, the covariance matrix of measurement prediction errors, and the cross-covariance matrix; regenerate σ points.

[0089]

[0090] in, and Represents the τ-th column of the square root of the matrix;

[0091] Then calculate each σ point. Predicted measurement value

[0092]

[0093] Calculate the predicted measurement value of the i-th radar at time k.

[0094]

[0095] Then, the covariance matrix of the measurement prediction error is calculated.

[0096]

[0097] Then, calculate and cross-covariance matrix

[0098]

[0099] S25, the optimization problem used to construct the local state estimation;

[0100] Based on the minimum variance criterion, the following optimization problem is established and solved to obtain a local optimum estimate.

[0101] Among them, K i (k) is the gain matrix of the i-th radar at time k;

[0102] S26, obtain the local optimal estimate of the state vector based on the nonlinear filtering iterative form;

[0103] After applying the matrix inversion lemma and appropriately rearranging the optimal solution, the following iterative formula for the nonlinear filter can be obtained:

[0104]

[0105] Then, the covariance matrix of the local state estimation error is:

[0106]

[0107] Design of a distributed fusion estimator:

[0108] Step 27, given a local estimate and Local fusion estimation is obtained based on a distributed fusion strategy.

[0109]

[0110]

[0111] Where λ∈[0,1], M1(k) is the fusion gain matrix of local estimate 1, M2(k) is the fusion gain matrix of local estimate 2, and P1 Δ (k) represents the local fusion estimation The covariance matrix;

[0112] Step 28, given the local fusion estimate and local estimation Local fusion estimation is obtained based on a distributed fusion strategy.

[0113]

[0114] Where L1(k) is the fusion gain matrix of local fusion estimation 1, and M3(k) is the fusion gain matrix of local estimation 3. Indicates local fusion estimation The covariance matrix;

[0115] Step 29: Repeat steps 27 and 28 to fuse all local estimates sequentially. Obtain distributed fusion estimation

[0116]

[0117] In this embodiment, MATLAB 2021b is used as simulation software to compare the trajectory tracking method of the unmanned surface vessel of the present invention with the actual motion trajectory of the unmanned surface vessel.

[0118] The simulation uses zero-mean Gaussian noise as the measurement noise, with a covariance of:

[0119] R1(k)=R2(k)=R3(k)=0.1;

[0120] The noise affecting the state vector of the unmanned surface vessel is zero-mean Gaussian noise, and its covariance matrix is:

[0121]

[0122] The parameters of the distributed fusion estimation method are set as follows:

[0123] Given n = 4, α = -1, a = 0.01, b = 2, λ = 0.3, note that the local estimation algorithm hinges on the selection of parameters α, a, and b. The sampling period T = 1. The initial estimate of the state vector for the distributed fusion estimation is set as follows: Simulation duration: 80 seconds;

[0124] Figure 3 This represents the trajectory of an unmanned surface vessel in the world coordinate system and the trajectory estimated by a distributed fusion estimation method.

[0125] Figure 3 The black solid line marked as the actual trajectory represents the motion trajectory of the unmanned surface vessel in the world coordinate system as observed by the high-precision GPS carried by the unmanned surface vessel itself. The gray solid line marked as distributed fusion estimation represents the real-time motion trajectory of the unmanned surface vessel estimated by the distributed fusion estimation method proposed in this invention. It can be seen that the unmanned surface vessel trajectory tracking method of this invention can achieve excellent tracking results.

[0126] Based on the same inventive concept, the present invention discloses a trajectory tracking system for unmanned surface vessels, including: a problem construction module, used to construct a state estimation problem for unmanned surface vessels based on multiple radars;

[0127] In addition, the trajectory tracking module is used to approximate the mean and variance of random variables after transformation by a nonlinear radar measurement model by using unscented transformation based on radar measurement vectors. Under the minimum mean square error criterion, a local nonlinear filter is designed to obtain the local optimal estimate of the unmanned surface vessel state vector for each radar. Furthermore, based on a distributed fusion strategy, the local estimates are fused to obtain the distributed fusion estimate of the unmanned surface vessel state vector, thereby realizing real-time tracking of the unmanned surface vessel's motion trajectory.

[0128] The specific working processes of each module described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here. The division of modules is only a logical functional division, and there may be other division methods in actual implementation. For example, multiple modules may be combined or integrated into another system.

[0129] Based on the same inventive concept, the present invention provides a computer system including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the computer program is loaded onto the processor, it implements the steps of the unmanned surface vessel trajectory tracking method.

[0130] Those skilled in the art will understand that the technical solution of this invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer system (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in the embodiments of this invention. The storage medium includes various media capable of storing computer programs, such as a USB flash drive, portable hard drive, read-only memory (ROM), random access memory (RAM), magnetic disk, or optical disk.

[0131] The above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for tracking the trajectory of unmanned surface vessels based on a distributed fusion strategy, characterized in that, Includes the following steps: Step 1: Construct a state estimation problem for unmanned surface vessels based on multiple radars; Step 2: Based on the radar measurement vectors, an unscented transformation is used to approximate the mean and variance of the random variables after transformation by the nonlinear radar measurement model. A local nonlinear filter is designed under the minimum mean square error criterion to obtain a locally optimal estimate of the unmanned surface vessel's state vector for each radar. Specifically, this includes: Step 21: Initialize the distributed fusion estimation method, including state estimates. and its error covariance matrix ; Step 22, obtain the first Measurement vectors of each radar ; Step 23, predict the first The state vector of the unmanned surface vessel at time t. The covariance matrix of the state vector prediction error ; Step 24, predict the first Measurement vector of time radar The covariance matrix of the measurement vector prediction error and cross-covariance matrix ; Step 25, construct the optimization problem used for local state estimation, including: Based on the minimum local variance criterion, the following optimization problem is established and solved to obtain the local optimal estimate. : ； ； in, It is the first Time of the first Gain matrix of each radar It is the first Time of the first The error covariance matrix of the state vector estimates obtained by the radar; Step 26: Obtain the local optimal estimate of the state vector according to the following nonlinear filtering iterative form: ； ； ； Step 3: Based on the distributed fusion strategy, the local estimation is fused to obtain the distributed fusion estimate of the unmanned surface vessel's state vector, thereby realizing real-time tracking of the unmanned surface vessel's trajectory.

2. The method for tracking the trajectory of unmanned surface vessels based on a distributed fusion strategy according to claim 1, characterized in that, The system equations used in the estimation problem constructed in step 1 are as follows: ； ； in and They are the first , The state vector of the unmanned surface vessel at any given time. Indicates the first Time of the first The measurement vector of each radar, Number of radars; When noise is not considered, Time-state vector arrive Time-state vector The mapping, When noise is not considered, Time-state vector To the radar measurement vectors The mapping, express Zero-mean process noise at time step express Time of the first Zero-mean measurement noise of each radar; Assume the system sampling period is ,matrix It has the following form: 。 3. The method for tracking the trajectory of unmanned surface vessels based on a distributed fusion strategy according to claim 1, characterized in that, Step 3 specifically includes: Step 31, given the local estimates of the first radar and the second radar and Local fusion estimation is obtained based on a distributed fusion strategy. ; ； in, It is the fusion gain matrix of local estimate 1. It is the fusion gain matrix of local estimate 2; Step 32, given the local fusion estimate Local estimation of the third radar Local fusion estimation is obtained based on a distributed fusion strategy. ; ； in, It is the fusion gain matrix of local fusion estimation 1. It is the fusion gain matrix of local estimate 3; Step 33: Following the method in step 32, sequentially fuse all local estimates. , Given the number of radars, a distributed fusion estimate is obtained. ; 。 4. The method for tracking the trajectory of unmanned surface vessels based on a distributed fusion strategy according to claim 3, characterized in that, In step 31, ； ； ； in, , Indicates local fusion estimation The covariance matrix, , These are the error covariance matrices of local estimates 1 and 2, respectively.

5. The method for tracking the trajectory of unmanned surface vessels based on a distributed fusion strategy according to claim 3, characterized in that, In step 32, ； ； ； in, Indicates local fusion estimation The covariance matrix, Indicates local fusion estimation The covariance matrix, These are the error covariance matrices of local estimate 3.

6. The method for tracking the trajectory of unmanned surface vessels based on a distributed fusion strategy according to claim 1, characterized in that, The state vector of the unmanned surface vessel includes the position and velocity of the center of mass of the unmanned surface vessel in the world coordinate system.

7. The method for tracking the trajectory of unmanned surface vessels based on a distributed fusion strategy according to claim 6, characterized in that, For an unmanned surface vessel system, the kinematic equations can be written in the following form: ； ； ； ； in, The system sampling period is , The first Unmanned surface vessel in the world coordinate system The position of the centroid in the direction, , The first Unmanned surface vessel in the world coordinate system The velocity of the center of mass in the direction.

8. A surface unmanned surface vessel trajectory tracking system based on a distributed fusion strategy, used to implement the surface unmanned surface vessel trajectory tracking method based on a distributed fusion strategy according to any one of claims 1-7, characterized in that, include: The problem construction module is used to construct state estimation problems for unmanned surface vessels based on multiple radars; In addition, a trajectory tracking module is used to approximate the mean and variance of random variables after transformation by a nonlinear radar measurement model by using unscented transformation based on radar measurement vectors, and to design local nonlinear filters under the minimum mean square error criterion to obtain the local optimal estimate of the unmanned surface vessel state vector for each radar; and to obtain the distributed fusion estimate of the unmanned surface vessel state vector by fusing local estimates based on a distributed fusion strategy, so as to realize the real-time tracking of the unmanned surface vessel's motion trajectory.

9. A computer system comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the computer program is loaded into the processor, it implements the steps of the unmanned surface vessel trajectory tracking method based on a distributed fusion strategy according to any one of claims 1-7.

Images