A real-time target tracking method and medium based on adaptive particle filtering

Abstract

The application discloses a real-time target tracking method and medium based on adaptive particle filtering, and the method comprises the following steps: presetting an initial particle number, a target particle number and an initial state of the particle; predicting a current state based on a previous state of the particle; calculating a current time weight of the particle according to a position in the current predicted state, a sensor position and an observation value; calculating a current effective particle number according to a normalized weight of the current particle; comparing the current effective particle number with a preset number, adjusting the particle number according to a comparison result, performing particle resampling according to the normalized weight and the adjusted particle number, and obtaining a new particle set; using an average state of the new particle set as a target state estimation and output; and repeating the above steps to continue tracking the target state. The application uses the effective particle number which can represent a state posterior probability distribution under a current observation value, avoids waste of calculation resources on redundant particles, and can guarantee tracking accuracy of the particle filtering on the real-time target.

Description

Technical Field

[0001] This invention belongs to the field of nonlinear filtering and electronic technology, specifically relating to a real-time target tracking method and medium based on adaptive particle filtering. Background Technology

[0002] Particle filtering, also known as the sequential Monte Carlo method, is a Bayesian filtering method based on Monte Carlo simulation approximation and belongs to the category of nonlinear filtering methods. Its basic idea is to recursively estimate the posterior density of random events using a set of weighted samples (particles), thereby estimating the system state from a noisy or incomplete observation sequence.

[0003] The particle filter is a generalized method of the Kalman filter. While the Kalman filter is based on a linear state space and Gaussian noise, the particle filter's state space model can be nonlinear, and the noise distribution can be of any form. Compared to other filtering algorithms, the particle filter exhibits better stability and performance when dealing with nonlinear, non-Gaussian dynamic state-space models. Therefore, particle filtering is widely used in various fields, such as target tracking and localization. [1,2,3] .

[0004] The standard particle filter algorithm consists of four steps: sampling, weight estimation, resampling, and output estimation, as shown in Code 1. It can be seen that the particle filter algorithm uses a fixed number of particles at all filtering stages. That is, in the traditional particle filter algorithm for target tracking and localization, the number of particles is determined during the algorithm initialization phase and remains unchanged throughout the tracking and localization phase. Furthermore, the filtering effect, i.e., the localization accuracy, of the particle filter is highly dependent on the number of particles selected. In short, the more particles used, the higher the localization accuracy. However, using a large number of particles will bring a very large computational burden to the particle filter, resulting in a very slow computation speed, thus limiting its application in real-time systems. [4] .

[0005]

[0006]

[0007] Furthermore, regarding the characteristics of the particle filter algorithm, the number of weighted particles required to represent the posterior probability density distribution changes in real time during each iteration, depending on the changes in the observations in the particle filter system. Therefore, using a fixed number of particles can lead to unnecessary waste of computational resources in some iterations. Moreover, the filtering effect of particle filtering does not increase indefinitely with the number of particles; when the number of particles increases to a certain extent, its positioning accuracy converges to a fixed value. The more particles used for state estimation, the higher the computational complexity, thus affecting the real-time performance of robot tracking and localization.

[0008] To implement particle filtering algorithms for real-time target tracking, adaptive particle filtering methods have been proposed. [4,5,6,7] That is, the number of particles used at each moment is dynamically changed according to the observation value to estimate the state, thereby reducing the average number of particles used and improving the speed of the particle filtering algorithm. There are two main types of existing adaptive particle filtering methods: one is the adaptive particle filtering method based on the likelihood function proposed by Koller et al. [6], which generates a sample set only when the sum of the non-normalized likelihood values ​​exceeds a pre-specified threshold. Although this method is easy to implement in terms of algorithm, it enhances the correlation between particles, affects the parallel computing characteristics of the particle filtering algorithm, and makes it difficult to implement the particle filtering algorithm in parallel in hardware; the other is the adaptive particle filtering method based on KLD sampling proposed by Fox et al. [5], which is based on the error between the approximate value of the sample and the true posterior value, which comes from the adaptively changing number of particles. This method has a better approximation value and uses fewer particles than the previous adaptive method, but the adaptive method based on KLD sampling requires a pre-discrete approximation of the proposal distribution, and the calculation of the KLD distance between probabilities is too complicated, which brings great challenges to hardware implementation and acceleration. In addition, Bolic et al. proposed a simplified adaptive particle filtering method [7]. This method randomly changes the number of particles used in each step, but cannot guarantee the effectiveness and positioning accuracy of the algorithm.

[0009] In general, existing adaptive particle filtering algorithms can reduce the average number of particles used compared to standard filtering methods, but the additional computation they introduce affects the parallel execution of particle filtering [6] and brings additional complex computational costs. [5] Or the effectiveness of the algorithm cannot be guaranteed. [7] Therefore, the speed improvement they bring is very limited, which restricts their application in real-time target tracking systems.

[0010] References:

[0011] [1] ZL 201910432983.9 An Integrated Target Detection Method for Adaptive Particle Filter Ground Wave Radar

[0012] [2] ZL 201310357510.X A target tracking algorithm based on adaptive particle filtering and sparse representation

[0013] [3]TCChau, W.Luk, PYCheung, A.Eele, and J.Maciejowski, "Adaptivesequential monte carlo approach for real-time applications," in 22ndInternational Conference on Field Programmable Logic andApplications (FPL). IEEE, 2012, pp.527–530.

[0014] [4] C.Kwok, D.Fox, and M.Meila, "Real-time particle filters," Advances inneural informationprocessing systems, vol.15, 2002.

[0015] [5]D.Fox, "Adapting the sample size in particle filters through kld-sampling," The international Journal of robotics research, vol.22, no.12, pp.985–1003, 2003.

[0016] [6]D.Koller andR.Fratkina, "Using learning for approximation instochastic processes." in ICML, 1998, pp.287–29.

[0017] [7] M.Bolic, S.Hong, and PMDjuric, “Performance and complexity analysis of adaptive particle filtering for tracking applications,” in ConferenceRecord of the Thirty-Sixth Asilomar Conference on Signals, Systems and Computers, 2002., vol.1.IEEE, 2002, pp.853–857

[0018] [8] L.Martino, V.Elvira, and F.Louzada, "Effective sample size forimportance sampling based on discrepancy measures," Signal Processing, vol.131, pp.386–401, 2017. Summary of the Invention

[0019] This invention provides a real-time target tracking method and medium based on adaptive particle filtering. In each tracking step, the most effective number of particles that can characterize the posterior probability distribution of the current observation value is calculated and used according to the weighted particle set. This can avoid wasting computational resources on redundant particles and ensure the tracking accuracy of particle filtering for real-time targets.

[0020] To achieve the above technical objectives, the present invention adopts the following technical solution:

[0021] A real-time target tracking method based on adaptive particle filtering includes:

[0022] Step 1: Set the initial number of particles N (t=0) for particle filtering and the number of particle targets during the target tracking process;

[0023] Step 2: Initialize the state of all particles at time t=0 for target tracking, with equal weights of 1. The state includes position and velocity.

[0024] Step 3: Based on the state of the particle at time t-1, predict the state of the particle at time t according to the state transition equation;

[0025] Step 4: Calculate the weight of the particle at time t based on the predicted position of the particle at time t, the position of the sensor, and the sensor's observations.

[0026] Step 5: Calculate the effective number of particles at time t based on the normalized weights of all particles at time t.

[0027] Step 6: Compare the calculated effective particle count at time t with the target particle count, and adjust the particle count at time t to N based on the comparison result. * (t);

[0028] Step 7: Based on the normalized weights of all particles at time t and the adjusted number of particles N * (t) Perform particle resampling to obtain the particle set at time t, and all particles have the same weight;

[0029] Step 8: Estimate the target state using the average state of the new particle set and output the target state at time t.

[0030] Step 9, update the number of particles at the next time step t+1 to N(t+1) = N * (t), update time t = t + 1, then return to step 3 based on the new particle set, and continue step 3-9 to perform state tracking of the target at subsequent time points.

[0031] Furthermore, the weight calculation method for particle at time t in step 4 is as follows:

[0032]

[0033] In the formula, To calculate the weight of the i-th particle at time t; y j,t Let be the target signal intensity value received by the j-th sensor at time t, which is used as the observation value for target tracking particle filtering; S is the number of sensors set. s represents the predicted position of the i-th particle at time t; j Let P be the position of the j-th sensor; P0 be the power of the target's transmitted signal; α > 1 refers to the path loss exponent of the signal; ||·|| is the L2 norm calculation.

[0034] Furthermore, the method for calculating the number of effective particles in step 5 is as follows:

[0035]

[0036]

[0037] In the formula, ESS(t) represents the effective number of particles at time t. The weight of the i-th particle at time t, calculated in step 4. Let N(t) be the normalized weight of the i-th particle at time t, and N(t) be the number of particles at time t.

[0038] Furthermore, the step 6, which involves adjusting the number of particles at time t based on the comparison results, specifically involves:

[0039] If the calculated number of effective particles at time t is greater than the upper limit of the set particle target number, then

[0040] Adjust the number of particles at time t to N * (t)=N(t)-M; where N(t),N * (t) represents the number of particles before and after adjustment, and M is the adjustment value for the number of particles in the particle set.

[0041] If the calculated number of effective particles at time t is less than the lower limit of the set particle target number, then the number of particles at time t is adjusted to N. * (t)=N(t)+M;

[0042] Otherwise, keep the number of particles at time t constant.

[0043] Furthermore, the estimation of the target's state using the average state of the new particle set, as described in step 8, is expressed by the following formula:

[0044]

[0045] In the formula, For the predicted state of the i-th particle in the new particle set at time t, N * (t) represents the adjusted number of particles at time t, X t To estimate the target state.

[0046] Furthermore, the prior density of the initialized target tracking particle set follows a Gaussian distribution:

[0047] p(x0) = N(x0; 0; D0);

[0048] Where D0 is a diagonal matrix: σ r and σ v These represent the prior variances of position and velocity, respectively.

[0049] Furthermore, the state transition equation used for particle state prediction in step 3 is in the form of:

[0050]

[0051] in, Let A be the transition matrix, where the element T is the time interval between system observations; It is the state of the i-th particle at time t-1; u t It is a Gaussian vector In this implementation, I is the identity matrix; It is the predicted state of the i-th particle at time t.

[0052] A computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the real-time target tracking method based on adaptive particle filtering as described in any of the above technical solutions.

[0053] Compared with the prior art, the present invention has the following beneficial effects:

[0054] 1. This invention selects an effective number of particles that can characterize the posterior probability distribution of the current observation value, and adjusts the number of particles in the particle set in real time based on the effective number of particles. This can ensure the target tracking accuracy while reducing the number of particles, thereby improving the calculation speed, and is suitable for real-time systems.

[0055] 2. The adaptive particle filtering method used in the target tracking of this invention only needs to calculate the number of effective samples when determining the number of particles, which avoids the complex computation of existing adaptive particle filtering methods and is more suitable for real-time processing and hardware implementation.

[0056] 3. The adaptive particle filtering method used in the target tracking of this invention can adjust the number of particles only in the resampling step, without affecting the parallel computation of the three steps of particle filtering: sampling, weight calculation and state estimation. Therefore, it has more advantages in hardware parallel implementation and is more suitable for hardware computing systems. Attached Figure Description

[0057] Figure 1 This is a schematic diagram of the basic process of the present invention.

[0058] Figure 2 This is a schematic diagram of particle initialization and sensor distribution.

[0059] Figure 3 This is a schematic diagram of particle filtering used for robot tracking.

[0060] Figure 4 A comparison diagram of the robot's motion path and the trajectory tracked using this invention. Detailed Implementation

[0061] The embodiments of the present invention will be described in detail below. These embodiments are based on the technical solutions of the present invention and provide detailed implementation methods and specific operation processes to further explain the technical solutions of the present invention.

[0062] This invention proposes an adaptive particle filtering method based on the effective sample size (ESS) for real-time target tracking. This method uses an improved adaptive particle filtering technique to track targets in real time. In each tracking step, the effective particle sample size is calculated based on the weighted particle set and compared with a pre-set target particle threshold value, thus forming a decision condition for changing the number of particles. This allows for the adaptive selection of the number of particles used, either increasing or decreasing the number accordingly. Then, target state estimation is performed, ultimately obtaining the target's tracking trajectory. This invention adaptively selects the most effective particle size to represent the posterior probability distribution of the state under the current observation value, avoiding the waste of computational resources on redundant particles and ensuring the real-time tracking accuracy of the particle filter. Furthermore, updating the particle set size only requires calculating the effective particle size, not the distance between probabilities, simplifying the computation. This step is only completed after resampling, without affecting the parallel execution of the sampling, weight calculation, and state estimation steps, making it easy to implement in hardware. This invention not only improves target tracking performance by adaptively increasing or decreasing the number of particles, but also ensures the optimal number of particles, thereby reducing the average number of particles in the filtering process, improving the algorithm's running speed, and making it suitable for real-time target tracking systems.

[0063] The following embodiments use an indoor robot as the target and employ an adaptive particle filter method based on the number of effective samples to track the indoor robot in real time. (Refer to...) Figure 1 As shown, it includes the following steps.

[0064] Step 1, Initialize the parameters of the adaptive particle filter:

[0065] 1) Initial number of particles N ini , which is the number of particles used at the initial moment when the adaptive particle filter algorithm is used for robot tracking;

[0066] 2) Number of target particles N f That is, the number of particles that the adaptive particle filter algorithm is expected to use during the target tracking and localization process;

[0067] 3) Threshold δ, set threshold (1±δ)N f This is used to compare with the effective particle sample count (ESS) of each iteration step to determine whether the number of particles needs to be increased or decreased; the target particle count N is used to determine this. f The threshold δ can be used to determine the number of particle targets during the target tracking process as [(1-δ)N]. f , (1+δ)N f ];

[0068] 4) Particle set number adjustment value M, that is, when the condition for changing the number of particles is met, M particles are added or removed in each iteration.

[0069] The specific parameters can be selected as follows: 1) Determine the appropriate number of target particles N based on the accuracy and speed requirements of target tracking. f ;2) Initial number of particles N ini The value of affects the number of particles used in the initial stage of robot localization and tracking, therefore the initial number of particles N ini The quantity should be set to the target particle number N. f 3) The threshold δ is set between 0.1 and 0.3; 4) The parameter M can be set to an appropriate value according to the corresponding target particle number and initial particle number.

[0070] Step 2, Particle state initialization:

[0071] 1) The indoor robot moves in a two-dimensional space, therefore the state variables of the tracking and positioning particle, i.e., the system, contain two pieces of information: the robot's position and velocity. The target's position and velocity at time t are expressed as: r t =[r 1,t ,r 2,t ] T v t =[v 1,t ,v 2,t ] T Joint process It represents the state of the particles in the system, where a vector consisting of four values ​​represents the position and velocity along the X and Y axes, respectively.

[0072] At time t=0, the prior density of the states follows a Gaussian distribution:

[0073] p(x0) = N(x0; 0; D0);

[0074] Where D0 is a diagonal matrix: generally σ r and σ v These represent the prior variances of position and velocity, respectively.

[0075] 2) Assign the same weight to particles at the initial time step.

[0076] The particle initialization and sensor distribution in this embodiment are as follows: Figure 2 As shown.

[0077] Step 3: At time t = 1, predict the state of the particle at time t based on the state of the particle at time t-1 and according to the state transition equation. The state transition equation has the following form:

[0078]

[0079] in, T is the time interval between system observations; u t It is a Gaussian vector The implementation (I is the identity matrix).

[0080] Step 4: Calculate the weight of the particle at time t=1 based on the predicted position of the particle, the position of the sensor, and the sensor's observation value, and normalize the weight.

[0081] To track the robot's trajectory, this embodiment uses S sensors to measure the signal strength y received from the robot. j,t Furthermore, the received signal strength is related to the distance between the sensor and the robot. Based on the robot state represented by each particle and the position of the sensor, the observation value y obtained by the j-th sensor (j∈{1,...,S}) is established. j,t The expression is as follows:

[0082]

[0083] Where, vector s j =[s 1,j ,s 2,j ] T Represents the position of the j-th sensor in the X and Y axes; ||·|| represents the L2 norm calculation; g j,t It is a noise variable that follows a Gaussian density. P0 represents the power of the target's transmitted signal; α>1 is the path loss exponent, used to set how the received power decreases with distance between the target and the sensor. Noise variable g j,t The observations are independent across different sensors and at different times. At time t, all sensor observations are collected in an S×1 vector y. t =[y 1,t ,...,y S,t ] T ;

[0084] Based on the relationship between particle positions and sensor observations expressed in the above expression, the weights of each particle at the current moment and the normalization formulas are as follows:

[0085]

[0086]

[0087] In the formula, The weight of the i-th particle at time t, calculated in step 4 of this paper. Let N(t) be the normalized weight of the i-th particle at time t, and N(t) be the number of particles at time t.

[0088] Step 5: Calculate the effective number of particles at time t=1, ESS(t), based on the normalized weights of all particles at time t=1. The calculation formula is as follows:

[0089]

[0090] Step 6, compare the calculated effective particle number ESS(t) with the particle target number [(1-δ)N] f , (1+δ)N f Comparison:

[0091] If the calculated effective particle count ESS(t) at time t is greater than the upper limit (1+δ)N of the set particle target count. f This indicates that the robot's position is relatively certain, and the current number of particles is sufficient to represent the posterior probability distribution of the target position. Therefore, to improve computation speed, the number of particles is appropriately reduced, and the number of particles at time t is adjusted to N. * (t)=N(t)-M; where N(t),N * (t) represents the number of particles before and after adjustment, and M is the adjustment value for the number of particles in the particle set.

[0092] If the calculated effective particle count ESS(t) at time t is less than the lower limit (1-δ)N of the set particle target count f This indicates that the robot's position has high uncertainty, requiring a larger number of particles to accurately represent the posterior probability distribution of the target position. Therefore, the number of particles at time t is adjusted to N. * (t) = N(t) + M, to ensure tracking accuracy;

[0093] Otherwise, it means the current weighted number of particles is appropriate, and the number of particles at time t should remain unchanged, i.e., N * (t) = N(t).

[0094] Step 7: Based on the normalized weights of all particles at time t and the adjusted number of particles N * (t) Perform particle resampling to obtain a new particle set at time t, and the weights of all particles are 1 / N(t+1).

[0095] Step 8: After resampling, the robot's position can be estimated using the average state of the particle set. The calculation formula is as follows:

[0096]

[0097] Step 9: For all times after t=1, update the number of particles at the next time t+1 as N(t+1) = N* (t), update time t = t + 1, then return to step 3 based on the new particle set, and continue steps 3-9 to perform state tracking on the target at subsequent times, so as to obtain the robot's position and tracking trajectory at each time.

[0098] Step 10: After the robot stops moving, compare the tracking path obtained by the algorithm with the robot's actual trajectory. The path comparison image, position error, and the average number of particles used throughout the tracking phase are used as the final output. Figure 3 , 4 As shown.

[0099] The above embodiments are preferred embodiments of this application. Those skilled in the art can make various changes or improvements based on them. Without departing from the overall concept of this application, these changes or improvements should fall within the scope of protection claimed in this application.

Claims

1. A real-time target tracking method based on adaptive particle filtering, characterized in that, include: Step 1, set the initial number of particles for particle filter and the number of particles for target tracking process; 1) the number of initial particles the number of initial particles used at the initial time when the adaptive particle filter algorithm is used for robot tracking 2) target particle number i.e. the number of particles that the adaptive particle filter algorithm anticipates using in the target tracking and localization process; 3) threshold value , set threshold value , for comparison with the effective particle sample number ESS of each iteration step, to determine whether the number of particles needs to be increased or decreased; the target particle number and the threshold value can determine the target particle number in the target tracking process as ; 4) particle set number adjustment value i.e. when the particle set number change condition is met, M particles are added or removed at each iteration; Step 2, initialize target tracking all particles the state at time t, with equal weights of 1, where the state includes position and velocity; let ; Step 3, particle-based the state at the time instant and predict the state of the particle the state at the time instant and predict the state of the particle Step 4, the particle is updated according to the predicted position of the time point and the position of the sensor and the observation value of the sensor Step 5, the weight of the particle at the time point is calculated Step 6, the weight of the particle at the time point is calculated Step 5, based on all particles Normalized weight calculation at time step The number of effective particles at any given time; Step 6, calculate the results The effective number of particles at any given time is compared with the number of target particles, and adjustments are made based on the comparison results. The number of particles at time is ; If the calculated result Effective particle count at time 1 Greater than the upper limit of the set particle target number ,Adjustment The number of particles at time is ;in To adjust the number of particles before and after, This is an adjustment value for the number of particles; If the calculated result Effective particle count at time 1 Less than the lower limit of the set particle target number ,Adjustment The number of particles at time is ; Otherwise, keep The number of particles remains constant at time t, that is, ; Step 7, based on all particles Normalized weights and adjusted particle count at time step Perform particle resampling to obtain A new set of particles at each moment, and all particles have the same weight; Step 8: Estimate the target's state using the average state of the new particle set, and output the current state. The target state at any given moment; Step 9, Update the next time step The number of particles is Update time Then, based on the new particle set, return to step 3 and continue with steps 3-9 to track the state of the target at subsequent moments.

2. The real-time target tracking method based on adaptive particle filtering according to claim 1, characterized in that, Particles in step 4 The weights at each time point are calculated as follows: ； In the formula, To calculate the first Particles The weight of each moment; For the first One sensor The received target signal strength value at any given time is used as the observation value of the target tracking particle filter; The number of sensors to be set; For the first Particles Predicted location at any given time; For the first The location of each sensor; The power of the signal transmitted to the target; The path loss index refers to the signal loss. This refers to the L2 norm calculation.

3. The real-time target tracking method based on adaptive particle filtering according to claim 1, characterized in that, The method for calculating the number of effective particles in step 5 is as follows: ； ； In the formula, for The number of effective particles at time 10:00 The first one calculated in step 4 Particles Weight of time, For the first Particles Normalized weights at time points, for The number of particles at any given time.

4. The real-time target tracking method based on adaptive particle filtering according to claim 1, characterized in that, The estimation of the target's state using the average state of the new particle set in step 8 is expressed by the following formula: ； In the formula, For the new particle set Particles Predicted state at any given time for The number of particles after time adjustment. To estimate the target state.

5. The real-time target tracking method based on adaptive particle filtering according to claim 1, characterized in that, The initial target tracking particle set has a prior density of states that follows a Gaussian distribution: ； in, Diagonal matrix: , , and These represent the prior variances of position and velocity, respectively.

6. The real-time target tracking method based on adaptive particle filtering according to claim 1, characterized in that, The state transition equation used for particle state prediction in step 3 is in the form of: ； in, Here is the transition matrix, the transition matrix elements in It is the time interval between system observations; It is the first Particles The state at any given moment; It is a Gaussian vector The implementation, It is the identity matrix; It is predicted that the first Particles The state at any given moment.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1 to 6.

Images