Simultaneous localization and composition method based on RB-PHD filtering

Abstract

The invention provides a simultaneous positioning and composition method based on RB-PHD filtering. According to the method, the positioning of the target and the correction of the VA position on the map are completed only by using the TOF, and the angle estimation problem caused by the irregularity of the base station and the terminal antenna array is effectively avoided. Different from the condition that most of the current tracking algorithms are based on the known initial position, the initial positioning algorithm provided by the invention can utilize VA to construct the motion range of the user and find the initial position of the target, thereby reducing the demand for prior information. In addition, the invention provides an effective data association algorithm which is integrated into an RB-PHD framework, clutters in an observation set can be effectively filtered out, the calculation complexity of the algorithm is reduced, and the real-time performance of the algorithm is improved. The method can be widely applied to indoor single-station positioning scenes, target positioning can still be completed when a base station and a terminal device antenna array are irregular, and meanwhile hardware cost can be reduced.

Description

Technical Field

[0001] The invention belongs to indoor positioning technology, and is a simultaneous positioning and mapping method based on RB-PHD filtering. Background Art

[0002] Although the Global Navigation Satellite System (GNSS) has become mature, its signal is often weak or even unusable in indoor environments or areas where the signal is blocked (such as underground parking lots or shopping malls). Therefore, indoor positioning and tracking technology has become a key solution to fill this gap and has become a hot research topic. In previous studies, most indoor positioning algorithms obtain wireless parameters such as Time of Flight (TOF), Angle of Arrival (AOA) and Angle of Departure (AOD) through multiple base stations (BS), and then process these parameters to achieve target positioning. However, in actual application scenarios, implementing a high-precision multi-base station positioning system usually significantly increases hardware requirements and costs, especially in large or complex environments. In addition, the base stations in a multi-base station system usually need to be precisely synchronized, and the clock asynchrony between base stations may cause significant errors in positioning calculations. Therefore, positioning technology based on a single base station is particularly necessary.

[0003] Multipath-assisted technology provides an effective solution to the challenges of single-station positioning. In recent years, positioning technology based on multipath triangulation has developed rapidly. It constructs a multipath triangulation model by obtaining parameters such as AOA, TOF and AOD from the first-order reflection path and the line of sight (LOS) path. This model can accurately estimate the position and direction of the target, but it is limited to positioning and cannot be tracked, so it has certain limitations. The virtual base station (VirtualAnchor, VA) technology effectively solves this limitation. Multipath signals mainly come from specular reflection and scattering, where specular reflection is a special case where the angle of incidence is equal to the angle of reflection. Specular reflection can be modeled by VA, which acts as a mirror of the base station. The specular reflection path can be regarded as a line-of-sight signal emitted by the virtual anchor, which provides favorable conditions for single-base station tracking. In this case, the user receives line-of-sight signals, single reflections, multiple reflections, and scattering paths, but only uses line-of-sight and single reflection paths for tracking, and other paths are regarded as noise.

[0004] The present invention proposes a simultaneous positioning and mapping method based on RB-PHD filtering, which uses TOF to track the target and completes the correction of the VA position. In reality, due to the irregularity of the terminal and base station antenna arrays, classic angle estimation algorithms, such as MUSIC and ESPRIT, are difficult to use in practical applications, which makes positioning and tracking by angle complicated. In contrast, TOF does not have specific requirements for the antenna array, so the present invention only uses TOF. The present invention improves the (Rao-Blackwellized Probability Hypothesis Density, RB-PHD) filter within the Random Finite Set (RFS) Bayesian framework, and obtains the prior position of VA by using an inaccurate planar map, thereby realizing simultaneous positioning and mapping (Simultaneous Localization and Mapping, SLAM) based on TOF, and completing the tracking of the target while correcting VA. In addition, we propose an effective data association (DA) method, which is integrated into the RB-PHD filter, effectively reducing the computational complexity and clutter rate. In addition, many tracking algorithms require prior knowledge of the target's initial position, which is often difficult to obtain, limiting their practicality. In the present invention, the initial position is crucial, and an inaccurate initial position may lead to erroneous data association or tracking failure. Therefore, it is very necessary to determine the accurate initial position, and the present invention proposes an algorithm to achieve this goal. Summary of the invention

[0005] The object of the present invention is to provide a simultaneous positioning and mapping method based on RB-PHD filtering, which can effectively utilize TOF to complete the positioning of the target and the correction of the VA position.

[0006] The simultaneous positioning and mapping method based on RB-PHD filtering of the present invention comprises the following steps:

[0007] Step 1: Model the environment according to the plan and calculate the location of VA;

[0008] Step 2: Use the initial positioning algorithm to construct the initial position of the target.

[0009] Step 3: Initialize the particle state, PHD and weight;

[0010] Step 4: Use the state transfer equation to predict the state of the target and use the data association algorithm to filter the observation set to remove clutter;

[0011] Step 5: Use Gaussian mixture technology to predict and update the PHD and weight of the particles, complete the pruning and merging of Gaussian components, and then perform weighted summation of the PHD of the particles to obtain the updated VA position.

[0012] Step 6: Take the weighted sum of the particle states to obtain the state and coordinates of the target.

[0013] Step 7: Resample the particles to perform tracking at the next moment.

[0014] Beneficial Effects

[0015] The present invention is based on the TOF of multipath signals to perform target positioning, which has the following advantages:

[0016] 1. Use the mirror reflection of the base station to locate the target, avoiding the clock synchronization problem between multiple base stations;

[0017] 2. The initial position of the target can be obtained without being given in advance;

[0018] 3. Only using TOF avoids the angle measurement problem caused by the irregular antenna arrays of the base station and the terminal.

[0019] 4. Data association is used to reduce clutter and computational complexity. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 is a flow chart of the present invention;

[0021] Figure 2 Constructing graphs for sports environments;

[0022] Figure 3 VA schematic diagram Specific implementation plan

[0023] The present invention is further described in detail below in conjunction with the accompanying drawings:

[0024] Figure 1 It is a flow chart of the present invention, and the specific steps are as follows:

[0025] Step 1: Model the walls in the environment as follows Figure 2 The line segment shown in Figure 1 shows the base station and VA as points. Then the base station x BS The VA coordinates of the wall can be expressed as:

[0026] x VA =Px BS +t (1)

[0027] Where P and t are the reflection matrix and translation vector of the reflection transformation about the wall. Reflection matrix P = I-2uu H, where u is the normal vector of the wall. Assuming that the secondary coordinates of the line segment modeled by the wall are (a, b, c), then t=-2c[a,b] T The calculated VA coordinates are still two-dimensional coordinates, but usually the height of the VA is consistent with the height of the base station, so no calculation is required.

[0028] Step 2: Figure 3 As shown in the figure, in order to more intuitively demonstrate the construction of the motion scene, a two-dimensional scene is used for demonstration. The VAs in the figure are sorted according to the coordinates calculated in step 1. The closed geometric figure formed by the VAs in the outermost layer connected end to end can be considered as the motion range of the target. The grid is then divided into grids with a certain accuracy, so the coordinates of each grid point can be obtained, and then the TOF between each point in the grid and all the VAs calculated in step 1 is calculated. Where l represents the lth point in the grid. At this point, the simple Euclidean distance can no longer be used to measure the distance between the observation set Z and The difference between the two vector dimensions is not consistent, and the observations corresponding to the same VA are not necessarily in the same position, so the Euclidean distance is no longer applicable, but the optimal sub-pattern assignment (OSPA) distance can solve this problem:

[0029]

[0030] where M is the number of elements in Z; N is The number of elements in ; c is the cutoff point, which is a constant greater than 0, reflecting the different degrees of penalty for the dimensions of the two vectors; p is the order, reflecting the sensitivity of OSPA distance to outliers; Π N is the set of all permutations and combinations of {1,2,···,N}, π i For N The i-th element of the π-th permutation of ; additionally defined:

[0031]

[0032] Finally, the differences between the grid points and the actual observations are calculated in turn. The grid point with the smallest difference is the initial position of the tracking target.

[0033] Step 3: After obtaining the initial position of the target, generate I particles according to the Gaussian distribution of the position, where the weight of each particle is Then set the initial PHD of the particle to 0, that is Therefore, the I particles at the initial moment can be expressed as:

[0034]

[0035] Step 4: According to the target's motion trajectory, the target's state transfer equation can be written as:

[0036]

[0037] where s k is the state of the target at the kth moment; T is the sampling period; q k is process noise, which follows Gaussian distribution At this time, the prediction of the particle state can be obtained by passing each particle through the state transfer equation At this time, according to the current state prediction value, data association can be performed to filter out the clutter in the observation set, leaving only the observations that match the VA. The specific steps are as follows:

[0038] 4a. Extract the coordinates of the target from the predicted state and calculate the distance between the target and each VA to obtain

[0039] 4b. Find the observation set Z k In, with The closest observation to the first element in , if the difference between the two is less than the threshold value d max , then the observation is considered to be an observation that matches the VA. Then put the observation into the new observation set At the same time from Z k Delete the observation. If Z k The difference between all elements in and the first element is greater than the threshold value d max , then continue to traverse In an element

[0040] 4c. Repeat the previous step until The last element in the , you can get the matched observation set

[0041] Step 5: Get the observation set after data association Finally, the PHD and weight of the particles are predicted:

[0042]

[0043] where γ i (x) represents the PHD of the VA observed at the current moment, and Then expand the formula through Gaussian Mixture (GM):

[0044]

[0045] in is the number of Gaussians in the PHD at the previous moment, and and are the weight, mean and variance of the j-th Gaussian component respectively. is the number of Gaussians in the PHD observed at the current moment, which is also equal to the number of elements in the observation set after data association. are the corresponding Gaussian mean and variance, respectively, where the mean is the matched VA coordinates, and the variance is a preset value, reflecting the error of the VA coordinates. At this time, the above two equations can be combined into:

[0046]

[0047] in

[0048] Then update the PHD and weight of the particles.

[0049]

[0050] where λ is the average amount of clutter in the measurement; c(z) = λ / (4R max π 4 ), R max is the maximum perception range of the target; For the detection probability of VA, the detection probability corresponding to the VA detected at the current moment should be 1, and the detection probability of the VA remaining at the previous moment should set an adaptive detection probability, which is usually small.

[0051] At the same time, use the measurement set again Update the particle weights:

[0052]

[0053] Where δξ is a set integral, which is usually difficult to calculate, so two approximations are needed. The first approximation involves It can be converted into

[0054]

[0055] The second approximation involves equation (11), which involves Bayesian updating and can be transformed into a closed form:

[0056]

[0057] In addition, in order to avoid numerical problems, formula (13) is converted into logarithmic form:

[0058]

[0059] Use j(z) to observe the set Label each observation in, when an observation j=j(z):

[0060]

[0061] When j≠j(z):

[0062]

[0063] Since GM transmits Gaussian mixture components in time, if they are not controlled, the Gaussian components will increase indefinitely. Therefore, it is necessary to trim and discard the Gaussian components with lower weights, merge the Gaussian components with similar means and covariances to reduce the number of Gaussian mixtures, and finally weighted sum the PHD of each particle to obtain the updated PHD of VA:

[0064]

[0065] Where D k|k (x) represents the updated PHD at the kth moment, where the mean of each Gaussian component is the updated VA coordinate.

[0066] Step 6. After the particle weight is updated, first take the exponent of the calculated particle weight: Then, the target state can be updated by weighted summing of each particle:

[0067]

[0068] in Represents the updated target state, and the target's coordinate position is included in its state.

[0069] Step 7: In order to ensure the normal operation of the system at the next moment, the particles need to be resampled, that is, After resampling the particles, if the target needs to be tracked at the next moment, continue to execute steps 4 to 7.

Claims

1. A simultaneous positioning and mapping method based on RB-PHD filtering, characterized in that: a) Using the Optimal Sub-pattern Assignment (OSPA) distance to obtain the initial position of the target; b) Eliminate the clutter received by the target at each moment through data association; c) updating the position of the virtual anchor node (VA) through the Rao-Blackwellized Probability Hypothesis Density (RB-PHD) filter; d) The updated target position is obtained by weighted summing of each updated particle.

2. According to claim 1, the initial position of the target is obtained by using the OSPA distance, characterized in that When the initial position of the tracked target is unknown, a more accurate initial position is determined. In a single base station environment, wireless multipath signals will be mirror-reflected on some reflectors during propagation, and the multipath that undergoes mirror reflection can correspond to VA. There are several VAs in the environment, and the closed geometric figure constructed by connecting the edgemost VAs end to end can be considered as the range of target movement. Therefore, the closed figure can be divided into grids with a certain accuracy to obtain the specific coordinates of each point in the grid in the figure, and then the TOF between each point in the grid and all VAs is calculated to obtain the set Where l represents the lth point in the grid. Then the difference between the actual observation Z is compared by OSPA distance. The difference between the two can be expressed as: where M is the number of elements in Z; N is The number of elements in ; c is the cutoff point, which is a constant greater than 0, reflecting the different degrees of penalty for the dimensions of the two vectors; p is the order, reflecting the sensitivity of OSPA distance to outliers; Π N is the set of all permutations and combinations of {1,2,···,N}, π i For N The i-th element of the π-th permutation of ; additionally defined: Finally, the differences between the grid points and the actual observations are calculated in turn. The grid point with the smallest difference is the initial position of the tracking target.

3. According to claim 2, the clutter received at each moment of the tracking target is eliminated by data association, characterized in that Reduce computational complexity and improve positioning accuracy. Since the observations received at each moment cannot only include observations from VA (base stations can also be regarded as special VAs), they may also include clutter such as high-order reflections and scattering, which will increase the amount of computation and reduce positioning accuracy. Specifically, the following steps are included. a) Calculate the distance between the current tracking target and each VA, and get b) Find the observation set Z that matches The closest observation to the first element in , if the difference between the two is less than the threshold value d max , then the observation is considered to be an observation that matches the VA and is not a noise. Put the observation into a new observation set Also removed from Z. c) Repeat the previous step until The last element in the , you can get the matched observation set At this time, the new observation set It does not necessarily include all VA observations. Because of interference such as environmental noise, VAs far away from the current position may not generate corresponding observations.

4. The method of claim 3, wherein the position of the VA is updated by a RB-PHD filter, characterized in that It can improve the accuracy of VA position and subsequent tracking accuracy. For the movement of the target to be tracked, the Constant Turn Rate and Velocity (CTRV) model can be used to model the target's linear and curved motion: k =[x k ,y k ,z k ,α k ,ζ k ,ρ k ] T . Where x k ,y k and z k is the three-dimensional coordinate of the target; α k is the heading angle of the target; k is the linear velocity of the target; ρ k is the angular velocity of the target. Therefore, the state transfer equation of the target is: where s k is the state of the target at the kth moment; T is the sampling period; q k is the process noise, which obeys Gaussian distribution q k ~N(0,Q). The RB-PHD framework is used to describe VA and the target to be tracked. VA is modeled as a multi-target Poisson process, so what is transmitted at each moment is the PHD of VA, not its density. At the same time, a total of I particles are used in the system, each particle has its state, weight and PHD, so I particles can be expressed as: in is the state of the ith particle; is the weight of the i-th particle; is the PHD of the ith particle. The specific process of the algorithm is as follows: a) Initialize particle states, weights and PHD. The initial position s of the target is found by the algorithm in claim 1 0|0 , and then generate a mean of s according to the Gaussian distribution 0|0 I particles, where the weight of each particle is The PHD of a particle is b) Particle PHD and weight prediction. According to the state transfer equation, the predicted target state can be obtained Then, the observation set is filtered by the data association in claim 2 to obtain Then predict PHD: where γ i (x) represents the PHD of the VA observed at the current moment, and Then expand the formula through Gaussian Mixture (GM): in is the number of Gaussians in the PHD at the previous moment, and and are the weight, mean and variance of the j-th Gaussian component respectively. is the number of Gaussians in the PHD observed at the current moment, which is also equal to the number of elements in the observation set after data association. are the Gaussian mean and covariance of the corresponding observations, respectively, where the mean is the matched VA coordinates and the variance is a preset value, reflecting the error of the VA coordinates. At this point, the above two equations can be combined into: in c) Particle PHD and weight update: where λ is the average amount of clutter in the measurement; c(z) = λ / (4R max π 4 ), R max is the maximum perception range of the target; For the detection probability of VA, the detection probability corresponding to the VA detected at the current moment should be 1, and the detection probability of the VA remaining at the previous moment should set an adaptive detection probability, which is usually small. At the same time, use the measurement set again Update the particle weights: Where δξ is a set integral, which is usually difficult to calculate, so two approximations are needed. The first approximation involves It can be converted into The second approximation involves equation (10), which involves Bayesian updating and can be transformed into a closed form: In addition, in order to avoid numerical problems, formula (11) is converted into logarithmic form: Use j(z) to observe the set Label each observation in, when an observation j=j(z): When j≠j(z): in and All are obtained through the Cubature Kalman Filters (CKF). Since Gaussian mixture transmits Gaussian mixture components in time, if not controlled, the Gaussian components will increase indefinitely. Therefore, it is necessary to trim and discard the Gaussian components with lower weights, merge the Gaussian components with similar means and covariances to reduce the number of Gaussian mixtures, and finally weighted sum the PHD of each particle to obtain the PHD of the updated VA: Where D k|k (x) represents the updated PHD at the kth moment, where the mean of each Gaussian component is the updated VA coordinate.

5. The method of obtaining an updated target position by weighted summation of particles according to claim 4, characterized in that Get the updated target state and position. First, take the exponent of the calculated particle weight: Then, by taking the weighted sum of each particle, we get: in represents the updated target state, and the target's coordinate position is included in its state. In addition, in order to ensure normal operation at the next moment, the particles need to be resampled, that is, Then continue with the above steps.

Images