Three-dimensional underwater multi-target tracking method based on improved smc-phd algorithm

Abstract

The application discloses a three-dimensional underwater multi-target tracking method based on an improved SMC-PHD algorithm, and the method comprises the following steps: converting the two-dimensional sonar detection result of the multi-target into a three-dimensional coordinate system, taking the coordinate value of the multi-target in the three-dimensional coordinate system as a measurement value of the improved SMC-PHD algorithm to perform filtering calculation, and obtaining a tracking result of the target, wherein the improved SMC-PHD algorithm uses a minimum sampling variance sampling method in resampling of an original SMC-PHD algorithm, and the improved SMC-PHD algorithm further adds judgment on state estimation results of continuous multiple frames of images and adjustment according to the judgment. The application can perform three-dimensional leak-free tracking on underwater multi-targets, and the obtained tracking trajectory is high in precision and free of breakage.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of underwater target tracking method, and particularly relates to an underwater target tracking method based on sonar images. BACKGROUND

[0002] Underwater target detection and tracking has always been a hot topic in the field of underwater acoustics. At present, the common detection and tracking methods mainly include two kinds: detection before tracking (DBT) and tracking before detection (TBD), wherein the detection before tracking technology has the advantages of fast speed, short time consumption and higher tracking accuracy, and is widely used in practical applications.

[0003] However, when the detection before tracking technology is used to track a three-dimensional target, the pitch angle information and position coordinate information of the related points need to be obtained according to the two-dimensional detection results to generate three-dimensional information for tracking, and the center of mass of the target needs to be accurately determined in the two-dimensional detection results, so that the accuracy of the detection algorithm is required to be higher, and the dependence on the detection algorithm is strong. The detection algorithms in the prior art, such as frame difference method, background reduction method, FCM and feature matching, each have different advantages and disadvantages, have different application requirements for the motion speed of the target or the underwater environment, and are difficult to be widely applied to detection and tracking under different conditions.

[0004] On the other hand, in multi-target detection tracking, the prior art mainly adopts a method of combining a single-target tracking method with a data association method to realize inter-frame target association, such as a multi-hypothesis tracking filter (MHT) and a joint probabilistic data association (JPDA) method, but with the increase in the number of tracked targets, the calculation complexity and difficulty of these methods significantly increase. While some new detection tracking algorithms such as a Gaussian mixture probability hypothesis density filter (GM-PHD) and a sequential Monte Carlo probability hypothesis density filter (SMC-PHD) (B.-N. Vo, S. Singh, and A. Doucet, "Sequential Monte Carlo Implementation of the PHD Filter for Multi-target Tracking," in Proceedings of the Sixth International Conference of Information Fusion, vol. 2, Jul. 2003, pp. 792-799.) can greatly reduce the calculation complexity, the SMC-PHD algorithm has not been actually developed and applied to three-dimensional underwater multi-target tracking; the GM-PHD algorithm has only shown great application potential in two-dimensional underwater multi-target tracking, and when directly applied to three-dimensional multi-target tracking, simulation results show that there is a large error between the tracking trajectory obtained by the GM-PHD algorithm and the actual trajectory of the target, and the tracking trajectory is broken. SUMMARY

[0005] In view of the defects of the prior art, the purpose of the present application is to provide a method for three-dimensional underwater multi-target tracking using an improved sequential Monte Carlo probability hypothesis density filter algorithm (SMC-PHD), which further introduces a threshold calculation process in the SMC-PHD algorithm and can be widely applied to three-dimensional underwater multi-target tracking to obtain a high-precision continuous non-broken target tracking trajectory.

[0006] The technical scheme of the present application is as follows:

[0007] The three-dimensional underwater multi-target tracking method based on the improved SMC-PHD algorithm comprises the following steps:

[0008] S1 obtains three-dimensional sonar data returned from a three-dimensional forward-looking sonar system, converts the three-dimensional sonar data into a two-dimensional sonar image through beamforming technology, pre-processes the two-dimensional sonar image, and obtains a pre-processed two-dimensional sonar image;

[0009] S2 detects the target in the pre-processed two-dimensional sonar image by a target detection algorithm to obtain two-dimensional measurement information of the target and the pitch angle β of the target, the two-dimensional measurement information including x-axis and y-axis coordinate values O x,k , O y,k of the target in a two-dimensional rectangular coordinate system.

[0010] S3 constructs a three-dimensional rectangular coordinate system with the two-dimensional rectangular coordinate system in which the target is located as an xy plane, and the direction of the vertical line of the vertical projection of the pitch angle β of the target to the xy plane as a z axis, keeps the horizontal and vertical coordinate values O x,k , O y,k of the target in the three-dimensional rectangular coordinate system unchanged, and calculates a vertical coordinate value O z,k of the target.

[0011] S4 takes the coordinate values O x,k , O y,z and O z,k of the target in the three-dimensional rectangular coordinate system as measurement values of an improved SMC-PHD algorithm, performs filtering calculation, and obtains three-dimensional coordinate values of the target in multiple pre-processed two-dimensional sonar images to obtain a three-dimensional tracking result of the target.

[0012] The improved SMC-PHD algorithm is a minimum sampling variance sampling method used in resampling of an original SMC-PHD algorithm, that is, MSV resampling, and a judgment on state estimation results of the continuous multiple pre-processed two-dimensional sonar images is added after target state estimation of the original SMC-PHD algorithm, and the weight value and the state value obtained by resampling are adjusted according to the judgment result.

[0013] According to some preferred embodiments of the present application, the target detection algorithm is selected from a K-means clustering detection algorithm and / or a CA-CFAR target detection algorithm.

[0014] According to some preferred embodiments of the present application, the vertical coordinate value O z,k is calculated by the following calculation model:

[0015] wherein

[0016] or

[0017] wherein λ represents a horizontal angle between the x and y direction coordinate values.

[0018] According to some preferred embodiments of the present application, the MSV resampling includes:

[0019] (1) obtaining a normalized particle weight before resampling at any k moment The number of particles B pre-allocated for each target, the state value of any i-th particle and the particle label value where the normalized particle weight is calculated as follows:

[0020]

[0021] where, denotes the weight value of the i-th particle updated through the update stage at time k, as follows:

[0022]

[0023]

[0024] where, denotes the predicted state value of the i-th particle at time k The detection probability of zk, which denotes the target measurement value at time k, Z k denotes the target measurement value at time k, B' denotes the number of particles corresponding to the target before resampling, denotes the likelihood function between the measurement value and the predicted value at time k, κ k (z k ) denotes the intensity of clutter at time k, C k (z k ) is a calculation parameter, J k denotes the number of new particles generated at time k, L k-1 denotes the total number of existing particles at time k-1;

[0025] (2) Determine the number of times the i-th particle is resampled at time k Residual weight and the total number of times B that the B' particles are resampled k|k , as follows:

[0026]

[0027]

[0028]

[0029] where floor() denotes the floor function;

[0030] (3) Sort the residual weights of the i-th particles at time k from small to large, and store the B-B k|k residual weights and the corresponding particle label values in the set L_rank in which the last sorted, i.e., the highest weight, is stored;

[0031] (4) For the B-B k|k higher remaining weights According to its corresponding particle label value, its state information is copied to the resampled particle state information, which includes the state value, label value and weight value of the particle;

[0032] (5) According to step (4), the state information of B resampled particles is obtained, respectively label value and weight value

[0033] (6) The weight values of the B resampled particles are averaged, and the average value is used as the weight of the resampled particle, thereby obtaining the resampled particle weight, state value and label

[0034] According to some preferred embodiments of the present application, the judgment of the state estimation results of the preprocessed two-dimensional sonar images of the continuous multiple frames after the target state estimation of the original SMC-PHD algorithm is added, and the weight value and state value obtained by resampling are adjusted according to the judgment result, which includes:

[0035] (1) The target state estimation is performed on the continuous multiple frames, and it is judged whether the predicted state value of any i-th particle at k time matches the target measurement value successfully, and the number of continuous frames without successful matching NOC is obtained;

[0036] (2) If the number of continuous frames without successful matching NOC < N Th , the predicted state value and the predicted weight value of the particle are extracted, and the predicted state value and the predicted weight value are used as the weight value and state value of the resampled particle, that is If NOC > NTh, the resampled particle state value and weight value are directly obtained by MSV resampling where N Th represents the continuous loss threshold;

[0037] (3) According to the obtained resampled particle state value and weight value , the target state value is obtained through the following target state estimation model:

[0038]

[0039] wherein, i represents particle serial number, B represents pre-allocated particle number B of each target;

[0040] (4) the number of frames of the target continuously unmatched successfully NOC is increased by 1, and becomes NOC+1, the target tracking at k moment is ended, and k+1 moment is entered.

[0041] According to some preferred embodiments of the present application, the continuous loss threshold N Th is obtained by the following calculation model:

[0042]

[0043] wherein, A F represents the total number of frames of the real target motion, i.e. how many frames the real target moves in total, which can be obtained through the sonar image; FT is the number of times of the target trajectory breaking in the tracking result of the original SMC-PHD algorithm, which can be obtained through visual observation; M AR represents the multi-target missed detection rate as follows:

[0044]

[0045] wherein, TP is the number of real targets detected, and FN is the number of real targets not detected

[0046] The minimum sampling variance resampling adopted by the present application can maximize the ability of the posterior distribution, and the optimal particle state value corresponding to the target is obtained, so as to improve the tracking precision; in further preferred embodiments, the present application sets a continuous loss frame number threshold, which can significantly reduce the number of times of the target tracking trajectory breaking and the missed tracking rate. The present application can obtain the tracking result of the three-dimensional underwater multi-target with unbroken trajectory, zero missed tracking rate and high precision. BRIEF DESCRIPTION OF DRAWINGS

[0047] Figure 1 is the flow chart of the three-dimensional underwater multi-target tracking method in the specific embodiment.

[0048] Figure 2 is the target bearing chart in the three-dimensional Cartesian coordinate system in the specific embodiment.

[0049] Figure 3 is the particle state chart in the three-dimensional space in the specific embodiment, wherein (a) is a normal particle state chart, and (b) is a particle state chart of target missed detection and error estimation.

[0050] Figure 4 is the real trajectory chart of the movement of four targets in the simulation experiment of the specific embodiment.

[0051] Figure 5The following is a comparison of the tracking results of three tracking algorithms when the actual detection probability is set to 0.9 in a specific implementation. (a) is the tracking result of the GM-PHD algorithm; (b) is the tracking result of the SMC-PHD algorithm; and (c) is a comparison of the tracking results of the TH-SMC-PHD algorithm.

[0052] Figure 6 The following is a statistical comparison chart of the OSPA distance of the three tracking algorithms over time when the true detection probability is set to 0.9 in a specific implementation method. (a) is a line graph and (b) is a box plot.

[0053] Figure 7 The following is a comparison of the tracking results of three tracking algorithms when the actual detection probability is set to 0.8 in a specific implementation. (a) is the tracking result of the GM-PHD algorithm; (b) is the tracking result of the SMC-PHD algorithm; and (c) is a comparison of the tracking results of the TH-SMC-PHD algorithm.

[0054] Figure 8 The following is a statistical comparison chart of the OSPA distance of the three tracking algorithms over time when the true detection probability is set to 0.8 in a specific implementation method. (a) is a line graph and (b) is a box plot. Detailed Implementation

[0055] The present invention will now be described in detail with reference to embodiments and accompanying drawings. However, it should be understood that the embodiments and drawings are for illustrative purposes only and do not constitute any limitation on the scope of protection of the present invention. All reasonable modifications and combinations included within the inventive spirit of the present invention fall within the scope of protection of the present invention.

[0056] The following section first introduces the existing SMC-PHD algorithm. The SMC-PHD algorithm includes target birth, prediction, update, resampling, and state estimation stages, each of which specifically includes:

[0057] (1) Target birth:

[0058] The states and measurements of multiple targets are represented by two random finite sets:

[0059] and

[0060] Where m represents the number of target states, n represents the number of measurements, and the target state set any element Z represents the state value of the q-th target at time k, and the target measurement set. k any element This represents the measurement value of the j-th target at time k.

[0061] wherein xq(k) is the state value of the qth target at time k the corresponding particle state x k,i Specifically,

[0062] x k,i = [O x,k ,O y,k ,O z,k ,v x,k ,v y,k ,v z,k ] T ;

[0063] wherein v x,k , v y,k , v y,k respectively represent the velocity components of the qth target at time k in the x, y, z directions of the three-dimensional coordinate system, and (O x,k ,O y,k ,O z,k ) represents the coordinates of the qth target in the three-dimensional coordinate system.

[0064] the measurement value of the jth target at time k is:

[0065] wherein (O' x,k ,O' y,k ,O' z,k ) represents the coordinates of the measurement value of the jth target at time k in the three-dimensional coordinate system.

[0066] In the target birth stage, i.e. when k = 1, the state values of a predetermined number of newborn particles are obtained from the measurement value and the measurement noise matrix, thereby generating newborn particles. For example, the state values of a predetermined number of newborn particles can be generated from the measurement value and the measurement noise matrix according to a specific distribution function; and the newborn particles are labeled. Newborn particles obtained from the same measurement value are labeled with the same label. For example, the label values of the particles of a target are sequentially labeled in ascending order, more specifically, for example, the label values of 100 particles of a target at time k are labeled as 1,..., 100 respectively; in order to ensure that the state values of the newborn particles corresponding to the target are more accurate, the state values of the newborn particles can also be obtained without using a distribution function, but directly set as:

[0067] (2) Prediction:

[0068] After the target is born, the state and weight values of the J k newborn particles generated at time k and the L k-1 existing particles at time k-1 are predicted respectively by the following prediction model;

[0069] ​

[0070]

[0071] where, and denote the predicted state value and the predicted weight value of the i-th particle at time k, respectively; denotes the weight value of the i-th particle at time k-1, and p k (·|Z k ) denote the importance density function of the i-th particle at time k, denotes the importance density function of the i-th previously existing particle, denotes the importance density function of the i-th newly born particle; is the survival probability of the state of the i-th particle at time k-1, denotes the state transition density of the i-th particle from time k-1 to time k, denotes the birth intensity function of the random finite set (RFS).

[0072] Further, the predicted intensity v k|k-1 (x k ) of the particles corresponding to the target at time k can be obtained as follows:

[0073]

[0074] where, is the Dirac function.

[0075] In the above prediction process, the predicted label value of the particle at time k is kept the same as the label value at time k-1.

[0076] (3) Update:

[0077] The predicted intensity function and the weight value of the particle at time k obtained in the prediction are updated by the following update model:

[0078]

[0079]

[0080]

[0081] where, v k (x k ) denotes the updated posterior predictive intensity function, denotes the updated weight value at time k, This represents the predicted state value of the i-th particle at time k. The detection probability, z k κ represents the target measurement value at time k. k (z k () represents the intensity of the clutter at time k. C represents the likelihood function between the measured value and the predicted value at time k. It can calculate the magnitude of the likelihood between the measured value and the predicted value at time k. k (z k ) are the calculation parameters.

[0082] During the update process, when calculating the likelihood values ​​between the measured state and the predicted state, an M×n dimensional empty matrix can be pre-set, where M is the number of particles stored at time k-1 and n is the number of measured values ​​at time k. Then, a probability metric is calculated using a two-dimensional Gaussian function and the covariance matrix of the measured target, and input into the M×n matrix. The size of the similarity matrix is ​​used to determine which particle values ​​are similar to the measured values, and then the updated weights can be calculated. Simultaneously, an empty cluster is created, resulting in n+1 clusters. The weight of the (n+1)th cluster is... The calculation process involves identifying particles with weights exceeding a threshold, classifying them as untracked. Information on these untracked particles is then stored to prevent trajectory breakage. Subsequently, the likelihood values ​​of the measured and predicted values ​​are used to determine the tracking behavior. Particle tags at time k renew.

[0083] (3) Resampling:

[0084] The particle degradation problem is solved by using hierarchical resampling and other resampling methods, and new state values ​​are obtained through the particle resampling process. Tag value and weight B is the number of particles expected to be allocated to each target;

[0085] (4) Target state estimation:

[0086] When the particle weights satisfy Then, the target state value can be calculated using the following formula;

[0087]

[0088] Among them, w Th B is the weight threshold, where B is the number of particles expected to be assigned to each target.

[0089] The inventors discovered that the above SMC-PHD algorithm is prone to problems when performing 3D target tracking. Figure 3 (b) illustrates the cases of missed detections and incorrect estimates of particle states. Figure 3(a) is a normal particle state map as a contrast). Therefore, the SMC-PHD algorithm is further improved in the present application.

[0090] The improved SMC-PHD algorithm of the present application uses MSV (minimum sampling variance) resampling to replace the traditional stratified or polynomial resampling method in the resampling process of the above SMC-PHD algorithm, which can minimize the sampling variance and prevent particle degradation, thereby improving the accuracy of target tracking.

[0091] The minimum sampling variance is an optimal resampling method that can obtain the minimum sampling variance under the premise of meeting the asymptotic unbiasedness. The sampling variance SV is the average value of the difference between the number of particle resampling and the expected value, as shown in the following formula:

[0092]

[0093] wherein B represents the pre-allocated number of particles for each target, represents the number of times the i-th particle at time k is resampled, and B' represents the number of particles corresponding to a certain target before resampling, represents the normalized weight value.

[0094] More specifically, the MSV resampling process in the improved SMC-PHD algorithm of the present application is as follows:

[0095] (1) Obtain the normalized particle weight before resampling at any time k Pre-allocated number of particles B, particle state value and particle label value wherein the normalized particle weight is calculated as follows:

[0096]

[0097] (2) Determine the number of times the i-th particle at time k is resampled Residual weight and the total number of times B that B' particles are resampled k|k …, as follows:

[0098]

[0099]

[0100]

[0101] wherein floor() represents the floor function.

[0102] (3) Resample the residual weight of the i-th particle at time k Sort from small to large, store the last B-B k|k higher residual weights in set L_rank and the corresponding particle numbers;

[0103] (4) For the B-B k|k higher residual weights stored in set L_rank According to the corresponding particle numbers, the state information of the corresponding particles is copied into the resampled particle state information. Each weight value is copied times where i = 1,...,B'.

[0104] (5) According to step (4), the state information of B resampled particles is obtained, respectively: state distribution label value weight value

[0105] (6) The weight values of the B resampled particles are averaged to obtain:

[0106] Finally, the resampled particle weight state value and label

[0107] After completing the above MSV resampling and performing target state estimation according to the original SMC-PHD algorithm, the present application further introduces a continuous loss threshold N Th to reduce the number of broken target tracking trajectories and the missing tracking rate. The specific implementation includes:

[0108] (1) Perform target state estimation on consecutive multiple frames of images, judge whether the predicted state value of any i-th particle at time k matches the target measurement value , and obtain the number of consecutive frames NOC that do not match successfully;

[0109] (2) If the number of consecutive frames NOC that do not match successfully is less than N Th , extract the predicted state value and the predicted weight value of the particle, and use the predicted state value and the predicted weight value as the resampled particle weight value and state value Even if If NOC>N Th , directly obtain the resampled particle state value and weight values

[0110] (3) the target state value is obtained by a target state estimation model of the original SMC-PHD algorithm as follows:

[0111]

[0112] (4) the target continuous unmatched frame number NOC is increased by 1, becoming NOC+1, and the target tracking at the k moment is ended, and the k+1 moment is entered;

[0113] (5) in some more specific embodiments, N Th can be calculated by the following formula:

[0114]

[0115] wherein A F is the total frame number of the real target motion, that is, how many frames the real target moves in total, and the total frame number of the motion can be obtained through the sonar image; FT is the number of target trajectory breaks in the tracking result of the original SMC-PHD algorithm, which can be obtained by visual observation; M AR is the multi-target missed detection rate, which is the probability that the real target is not detected after being detected by the target detection algorithm, and the calculation method is as follows:

[0116]

[0117] wherein TP is the number of real targets detected, and FN is the number of real targets not detected.

[0118] On the basis of the improved SMC-PHD algorithm, reference is made to the attached Figure 1 In some specific embodiments, the three-dimensional underwater multi-target tracking method based on the improved SMC-PHD of the present application comprises the following steps:

[0119] S1 obtains three-dimensional sonar data returned from a three-dimensional forward-looking sonar system, converts the three-dimensional sonar data into a two-dimensional sonar image through beamforming technology, and performs preprocessing such as downsampling, image noise reduction, image enhancement, etc. on the two-dimensional sonar image to obtain a preprocessed two-dimensional sonar image;

[0120] S2 performs target detection on the obtained preprocessed two-dimensional sonar image through a K-means clustering or CA-CFAR target detection algorithm to obtain two-dimensional measurement information and a pitch angle β of the target, wherein the two-dimensional measurement information includes x and y axis coordinate values O x,k , O y,k of the target in a two-dimensional rectangular coordinate system;

[0121] S3 reference is made to the attached Figure 2, and the vertical line direction of the vertical projection of the pitch angle β to the xy plane is the z axis to construct a three-dimensional rectangular coordinate system, and the coordinate value of the target in the three-dimensional rectangular coordinate system is (O 'x,k , O' y,k , O 'z,k ), wherein the vertical coordinate O' z，k is calculated through the following calculation model:

[0122] Then

[0123] Or Then

[0124] Wherein, λ represents the horizontal angle between the x and y direction coordinate values.

[0125] S4 takes the coordinate value O' x,k , O' y,z , O' z,k of the target obtained by S3 in the three-dimensional rectangular coordinate system as the measurement value of the improved SMC-PHD algorithm, performs filtering calculation, obtains the three-dimensional coordinate value of the target in the preprocessed two-dimensional sonar image, and obtains the three-dimensional tracking result.

[0126] Compared with the prior art, the minimum sampling variance resampling can maximize the posterior distribution capability, obtain the optimal particle state value corresponding to the target, and thus improve the tracking precision; due to the setting of the continuous loss frame number threshold, the number of target tracking trajectory breaks and the missing tracking rate are reduced, and the three-dimensional multi-target can realize the tracking result of unbroken trajectory, zero missing tracking rate and high precision.

[0127] Embodiment 1

[0128] According to the above specific embodiment, simulation experiments are carried out, two detection conditions are set in the experiment, the true detection probabilities are 0.9 and 0.8 respectively, four tracking targets are included, the tracking targets are all in the space [0 1000]×[0 1000]×[0 1000] and do variable speed motion, a plurality of random three-dimensional interference waves are added in the space range, and the simulation time is between 1 and 201s; the motion information of the four tracking targets is as shown in the following table;

[0129]

[0130] After the target motion is completed, noise with a standard deviation σ v =0.5m / s 2 is added in the measurement value, the clutter number obeys the Poisson distribution, and the average value is 10.

[0131] The motion model and the measurement model of the target are set as follows:

[0132] x k,3D = F k,3D x k-1,3D + w k,3D ;

[0133] z k,3D = H k,3D x k,3D + λ k,3D ;

[0134] wherein, F k,3D and H k,3D are respectively a state transition matrix and a measurement matrix in a three-dimensional filtering algorithm.System noise w k,3D and measurement noise λ k,3D are both Gaussian white noise, and covariance matrices are Q k,3D and R k,3D .

[0135] In the two detection conditions, the weight threshold is w Th = 0.1, the survival probability of the target is p S,k = 0.99, and the state transition matrix F k,3D and the measurement matrix H k,3D are as follows:

[0136]

[0137] wherein, t = 1 is a time interval;

[0138]

[0139] The improved SMC-PHD algorithm adopted in the application is compared with the tracking results of the existing GM-PHD algorithm and SMC-PHD algorithm, wherein the resampling step of the SMC-PHD algorithm uses hierarchical resampling; in the GM-PHD algorithm, the pruning threshold is set as P Th = 5 * 10 -5 , the merging threshold is set as U = 10, the maximum allowed Gaussian component is set as J max = 50, the state transition matrix F k,3D and the measurement matrix H k,3D are the same as those of the TH-SMC-PHD algorithm, and the remaining parameters are set as follows:

[0140]

[0141]

[0142] The detection probability in the algorithm is set to be the same as the real detection probability.

[0143] The following three metrics were used to evaluate the tracking results:

[0144] Indicator Meaning Missed tracking rate The ratio of the number of missed tracked targets to the number of real targets Average OSPA distance The average OSPA distance of the estimated positions and real positions of all matched targets Number of trajectory breaks The sum of the number of trajectory breaks of real targets

[0145] The parameter settings for OSPA are as follows:

[0146] Distance sensitivity parameter p = 1 and association sensitivity parameter c = 10.

[0147] When the true detection probability is p D,k When '=0.9, set N Th =11, the tracking trajectories of different algorithms obtained in the experiment are shown in the appendix. Figure 5 As shown in the figures, (a), (b), and (c) are the tracking trajectories of the GM-PHD, SMC-PHD, and improved SMC-PHD algorithms, respectively.

[0148] Based on the tracking results above, the metrics for each tracking algorithm are calculated, as shown in the table below:

[0149]

[0150] The OSPA distance data comparison is attached. Figure 6 As shown, where, Figure 6 (a) is a line graph showing the relationship between OSPA distance and time for the three tracking algorithms. Figure 6 (b) shows the OSPA distance box plots for the three tracking algorithms.

[0151] When the true detection probability is p D,k When ′=0.8, set N Th =14, the tracking trajectories of different algorithms obtained in the experiment are shown in the appendix. Figure 7 As shown in the figures, (a), (b), and (c) are the tracking trajectories of the GM-PHD, SMC-PHD, and improved SMC-PHD algorithms, respectively. Their detection performance is slightly worse than that of the actual detection probability of 0.9.

[0152] Based on the tracking results above, the metrics for each tracking algorithm are calculated, as shown in the table below:

[0153]

[0154] The OSPA distance data comparison is attached. Figure 8 As shown, where, Figure 8 (a) is a line graph showing the relationship between OSPA distance and time for the three tracking algorithms. Figure 8 (b) shows the OSPA distance box plots for the three tracking algorithms.

[0155] According to the simulation results of the two detection probabilities, it can be concluded that the improved SMC-PHD algorithm can realize zero-miss tracking and target trajectory without breaking in the three-dimensional simulation scene. Among the three tracking algorithms, the improved SMC-PHD is the most ideal, the threshold and MSV resampling are adopted, which avoids the missed tracking, and finally improves the tracking accuracy, while the other two algorithms are broken due to missed detection and interference; for the SMC-PHD and GM-PHD algorithms, the target trajectory is broken at the intersection point of the target at the same time and the speed mutation, which shows that due to the incorrect association of prediction and measurement, the target trajectory is broken.

[0156] The above examples are only preferred embodiments of the present application, and the protection scope of the present application is not limited to the above examples. Any technical solutions falling within the concept of the present application shall fall within the protection scope of the present application. It should be pointed out that, for ordinary skilled persons in the art, improvements and refinements without departing from the principles of the present application shall also be considered as the protection scope of the present application.

Claims

1. A three-dimensional underwater multi-target tracking method based on an improved SMC-PHD algorithm, characterized in that, It comprises: S1 obtaining three-dimensional sonar data returned from a three-dimensional forward-looking sonar system, converting the three-dimensional sonar data into a two-dimensional sonar image through a beamforming technique, and pre-processing the two-dimensional sonar image to obtain a pre-processed two-dimensional sonar image; S2 carries out target detection on the preprocessed two-dimensional sonar image through a target detection algorithm to obtain two-dimensional measurement information of the target and a pitch angle β of the target, the two-dimensional measurement information including x-axis and y-axis coordinate values O' x and O' y of the target in a two-dimensional rectangular coordinate system x,k , O' y,k ; S3 takes the two-dimensional rectangular coordinate system where the target is located as the xy plane, and constructs a three-dimensional rectangular coordinate system with the direction of the vertical line vertically projected from the pitch angle β of the xy plane as the z axis, and keeps the horizontal and vertical coordinate values of the target in the three-dimensional rectangular coordinate system as O' x,k , O' y,k , and calculates the vertical coordinate value O' z,k ; S4 takes the coordinate value O' of the target in the three-dimensional orthogonal coordinate system as the target x,k , O' y,z , and O' z,k As the measurement value of the improved SMC-PHD algorithm, the filtering calculation is performed to obtain the three-dimensional coordinate value of the target in the preprocessed two-dimensional sonar image, and the three-dimensional tracking result is obtained. Wherein, the improved SMC-PHD algorithm is to use minimum sampling variance sampling method in the resampling of the original SMC-PHD algorithm, that is, to perform MSV resampling, and to add judgment on the state estimation results of the pre-processed two-dimensional sonar image of continuous multiple frames after the target state estimation of the original SMC-PHD algorithm, and to adjust the weight value and state value obtained by resampling according to the judgment result; Wherein, the MSV resampling comprises: (1) Obtain the normalized particle weights before resampling at any time k The number of particles B pre-allocated for each target, the state value of any i-th particle and the particle label value i=1,...,B', wherein the normalized particle weight is calculated as follows: wherein, represents the weight value of the i-th particle updated through the update stage at the k-th moment, as follows: wherein, represents the predicted state value of the i-th particle at the k-th time the detection probability of the i-th particle, z k represents the target measurement value at the k-th time, Z k represents the target measurement value at the k-th time, B' represents the number of particles corresponding to the target before resampling, represents the likelihood function between the measurement value and the predicted value at the k-th time, K k (z k ) represents the intensity of the clutter at the k-th time, C k (z k ) is a calculation parameter, J k represents the number of new particles generated at the k-th time, L k-1 represents the total number of existing particles at the k-1-th time; (2) determine the number of times the ith particle is resampled at time k the remaining weights and the total number of times B' particles are resampled, B k|k as follows: Wherein, floor() represents rounding down; (3) the residual weight of the i-th particle at time k Sort from small to large, store the B-B with the highest weight k|k residual weight p = 1, …, B-B k|k and its corresponding particle label value in the set L_rank; (4) For each B-B k|k higher remaining weight p = 1,..., B-B k|k , according to its corresponding particle label value, its state information, including the state value, label value and weight value of the particle, is copied to the resampled particle state information. (5) According to step (4), the state information of the B resampled particles is obtained, respectively as the state value tag value and weight value j = 1,..., B; (6) Weight values of the B resampled particles The average is taken, and the average value thereof is used as the weight of the resampled particle, whereby the weight and state value of the resampled particle are obtained and the label 2. The three-dimensional underwater multi-target tracking method according to claim 1, characterized in that, The target detection algorithm is selected from a K-means clustering detection algorithm and / or a CA-CFAR target detection algorithm.

3. The three-dimensional underwater multi-target tracking method according to claim 1, characterized in that, the ordinate value O' z,k is calculated by the following calculation model: wherein or Wherein, λ represents the horizontal angle between the x and y direction coordinate values.

4. The three-dimensional underwater multi-target tracking method according to claim 1, characterized in that, The judgment on the state estimation results of the pre-processed two-dimensional sonar image of continuous multiple frames after the target state estimation of the original SMC-PHD algorithm comprises: (1) Estimate the target state of multiple consecutive frames of images and determine the predicted state value of any i-th particle at time k. Its target measurement value Whether a match was successful, and the number of consecutive frames that failed to match (NOC); (2) If the number of consecutive unmatched frames NOC < N Th , the predicted state value of the particle is extracted and its predicted weight value , and the predicted state value and its predicted weight value are taken as the resampled particle weight value and state value . Even if NOC > N Th , the resampled particle state value and weight value are directly obtained by MSV resampling. Here, N Th represents the threshold of consecutive loss. (3) The target state value is obtained from the resampled particle state values and the weight values The target state value is obtained by a target state estimation model as follows: Wherein, i represents the particle serial number, and B represents the pre-allocated particle number B of each target; (4) increasing the number of frames NOC of the target that are continuously unmatched successfully by 1, becoming NOC+1, and ending the target tracking at the k moment, and entering the k+1 moment.

5. The three-dimensional underwater multi-target tracking method according to claim 4, characterized in that, the continuous loss threshold N Th is obtained by the following calculation model: where A F represents the total number of frames of real target motion, i.e. how many frames the real target moves in total, which can be obtained through the sonar image; FT is the number of times of track breaking in the tracking results of the original SMC-PHD algorithm, which can be obtained through visual observation; M AR represents the multi-target missed detection rate as follows: Wherein, TP is the number of real targets that are detected, and FN is the number of real targets that are not detected.

Images