Simultaneous localization and mapping method based on rb-phd filter

Abstract

The application provides a simultaneous localization and mapping method based on RB-PHD filtering. The method only uses TOF to complete positioning of a target and correction of a VA position on a map, effectively avoiding angle estimation problems caused by irregular base station and terminal antenna arrays. Different from most current tracking algorithms based on known initial positions, the initial positioning algorithm provided by the application can construct a user motion range and find an initial position of the target, reducing the demand for prior information. In addition, the application provides an effective data association algorithm integrated into the RB-PHD framework, which can effectively filter out clutter in an observation set, reduce the calculation complexity of the algorithm, and improve the real-time performance of the algorithm. The application can be widely applied to indoor single station positioning scenes, and can still complete positioning of the target when the base station and terminal device antenna arrays are irregular, and can reduce hardware costs.

Description

Technical Field

[0001] This invention pertains to indoor positioning technology, specifically a simultaneous positioning and mapping method based on RB-PHD filtering. Background Technology

[0002] Although the Global Navigation Satellite System (GNSS) has matured, its signal is often weak or even unusable in indoor environments or areas with signal obstruction (such as underground parking lots or shopping malls). Therefore, indoor positioning and tracking technology has become a key solution to fill this gap and a popular research topic. In previous research, most indoor positioning algorithms acquired wireless parameters, such as Time of Flight (TOF), Angle of Arrival (AOA), and Angle of Departure (AOD), from multiple base stations (BS), and then processed these parameters to achieve target positioning. However, in practical applications, achieving high-precision multi-base station positioning systems typically increases hardware requirements and costs significantly, especially in large or complex environments. Furthermore, base stations in multi-base station systems usually require precise synchronization, and clock asynchrony between base stations can lead to significant errors in positioning calculations. Therefore, positioning technology based on a single base station is particularly necessary.

[0003] Multipath-assisted positioning technology offers an effective solution to the challenges of single-site localization. In recent years, localization technology based on multipath triangulation has developed rapidly. It constructs a multipath triangulation model by obtaining parameters such as AOA, TOF, and AOD from first-order reflection paths and line-of-sight (LOS) paths. This model can accurately estimate the position and orientation of a target, but it is limited to localization and cannot be used for tracking, thus having certain limitations. Virtual Anchor (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 equals the angle of reflection. Specular reflection can be modeled using VA, where the VA acts as a mirror image of the base station. The specular reflection path can be considered as a line-of-sight signal emitted by the virtual anchor, providing favorable conditions for single-site tracking. In this scenario, the user receives the line-of-sight signal, single reflection, multiple reflections, and scattering paths, but only the line-of-sight and single reflection paths are used for tracking; other paths are treated as noise.

[0004] This invention proposes a simultaneous localization and mapping (SLAM) method based on RB-PHD filtering, which utilizes Time-of-Flight (TOF) for target tracking and corrects the VA (Vehicle Aspect) position. In reality, due to the irregularities of terminal and base station antenna arrays, classic angle estimation algorithms such as MUSIC and ESPRIT are difficult to use in practical applications, making angle-based localization and tracking complex. In contrast, TOF does not have specific requirements for antenna arrays; therefore, this invention utilizes only TOF. This invention improves the Rao-Blackwellized Probability Hypothesis Density (RB-PHD) filter within the Random Finite Set (RFS) Bayesian framework. By utilizing an inaccurate planar map, it obtains the prior position of the VA, thereby achieving TOF-based SLAM and completing target tracking while correcting the VA. Furthermore, we propose an effective Data Association (DA) method, integrated into the RB-PHD filter, which effectively reduces computational complexity and clutter rate. Furthermore, many tracking algorithms require prior knowledge of the target's initial position, which is often difficult to obtain, limiting their practicality. In this invention, the initial position is crucial; an inaccurate initial position can lead to incorrect data association or tracking failure. Therefore, determining an accurate initial position is essential, and this invention proposes an algorithm to achieve this goal. Summary of the Invention

[0005] The purpose of this invention is to provide a simultaneous localization and mapping method based on RB-PHD filtering, which can effectively utilize TOF to locate the target and correct the VA position.

[0006] The simultaneous localization and mapping method based on RB-PHD filtering described in this invention includes the following steps:

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

[0008] Step 2: Construct the initial position of the target using an initial localization algorithm.

[0009] Step 3: Initialize the particle's state, PHD, and weights;

[0010] Step 4: Predict the target's state using the state transition equation and filter the observation set using a data association algorithm to remove clutter;

[0011] Step 5: Use Gaussian mixture technique to predict and update the PHD and weights of the particles, and complete the trimming and merging of Gaussian components. Then, perform a weighted summation of the PHD of the particles to obtain the updated VA position.

[0012] Step 6: Sum the states of the particles with weights to obtain the state and coordinates of the target.

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

[0014] Beneficial effects

[0015] The present invention, which uses Time-of-Flight (TOF) based on multipath signals for target localization, has the following advantages:

[0016] 1. The target is located by using the mirror reflection of the base station, thus avoiding the clock synchronization problem between multiple base stations;

[0017] 2. The initial position of the target can be obtained without prior knowledge;

[0018] 3. By using only TOF, the angle measurement problem caused by irregular antenna arrays of base stations and terminals is avoided.

[0019] 4. Data correlation reduces noise and computational complexity. Attached Figure Description

[0020] Figure 1 This is a flowchart of the present invention;

[0021] Figure 2 To construct a graph of the motion environment;

[0022] Figure 3 VA diagram Detailed Implementation Plan

[0023] The present invention will now be described in further detail with reference to the accompanying drawings:

[0024] Figure 1 The flowchart of this invention is as follows:

[0025] Step 1: Model the walls in the environment as follows Figure 2 The line segment shown is used, where the base station and VA are represented by points. Then, the base station x... BS The VA coordinates of the wall can be represented as:

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

[0027] Where P and t are the reflection matrix and translation vector of the reflection transformation with respect to the wall, respectively. The reflection matrix P = I⁻²uu H, where u is the normal vector of the wall. Assuming the second coordinates of the line segment modeled from the wall are (a, b, c), then we have: t = -2c[a,b] T The calculated VA coordinates are still two-dimensional coordinates, but the height of the VA is usually consistent with the height of the base station, so no further calculation is needed.

[0028] Step Two, as follows Figure 3 As shown, to more intuitively demonstrate the construction of the motion scene, a two-dimensional scene is used. The VAs in the figure are ordered sequentially according to the coordinates calculated in step one. The closed geometric shape formed by connecting the outermost VAs end-to-end can be considered the target's motion range. The grid is then divided into subgrids with a certain precision, thus obtaining the coordinates of each grid point. The Time-of-Flight (TOF) between each point within the grid and all VAs calculated in step one is then obtained. Here, l represents the l-th point within the grid. At this point, the simple Euclidean distance is no longer sufficient to measure the relationship between the observation set Z and... The differences between the two vectors are that their dimensions are not consistent, and the observations corresponding to the same VA are not necessarily in the same location. Therefore, Euclidean distance is no longer applicable, but 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 the vector; c is the cutoff point, a constant greater than 0, reflecting the degree of penalty for the different dimensions of the two vectors; p is the order, reflecting the sensitivity of the OSPA distance to outliers; Π N Let π be the set of all permutations and combinations of {1, 2, ..., N}. i For Π N The i-th element of the π-th permutation and combination; further defined as:

[0031]

[0032] Finally, the difference between the grid points and the actual observations is calculated sequentially, and the grid point with the smallest difference is the initial position of the target being tracked.

[0033] Step 3: After obtaining the initial position of the target, generate I particles according to a 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 represented as:

[0034]

[0035] Step 4: Based on the target's trajectory, the target's state transition equation can be written as:

[0036]

[0037] Where s k Let q be the state of the target at time k; T is the sampling period; q k The noise is a process noise that follows a Gaussian distribution. At this point, the state of each particle can be predicted by applying the state transition equation. At this point, based on the current state prediction, data association can be performed to filter out clutter from the observation set, leaving only observations that match the VA. The specific steps are as follows:

[0038] 4a. Extract the target's coordinates from the predicted state and calculate the distance between the target and each VA.

[0039] 4b. Find the observation set Z k In, with The observation closest to the first element is selected if the difference between the two is less than the threshold value d. max If the observation is found to match the VA, then it is considered a matching observation. The observation is then placed into a new observation set. Simultaneously from Z k Delete the observation. If Z k The difference between all elements and the first element is greater than the threshold value d. max Then continue iterating. In one element

[0040] 4c. Repeat the previous step until... The last element in the set yields the matched observation set.

[0041] Step 5: Obtain the observation set after data correlation. Next, the PHD and weights of the particles are predicted:

[0042]

[0043] Where γ i (x) represents the PHD of VA observed at the current time, and has Then expand the equation using Gaussian Mixture (GM):

[0044]

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

[0046]

[0047] in

[0048] Then update the particle's PHD and weights.

[0049]

[0050] Where λ is the average clutter quantity in the measurement; c(z) = λ / (4R) max π 4 ), R max The maximum perceptible range of the target; For the detection probability of VA, the detection probability of the VA detected at the current time should be 1, while the detection probability of the VA remaining from the previous time should be set to an adaptive detection probability, which is usually small.

[0051] Simultaneously use the measurement set Update particle weights:

[0052]

[0053] Here, δξ is a set integral, which is usually difficult to calculate, so two approximations are needed. The first approximation involves the following in the above equation: It can be converted into

[0054]

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

[0056]

[0057] In addition, to avoid numerical problems, equation (13) is transformed into logarithmic form:

[0058]

[0059] Using j(z) on the observation set Each observation in the dataset is labeled, and when a certain observation j = j(z):

[0060]

[0061] When j ≠ j(z):

[0062]

[0063] Since GM propagates Gaussian mixture components over time, the number of Gaussian components will increase indefinitely if left uncontrolled. Therefore, Gaussian components with lower weights should be pruned and discarded, and Gaussian components with similar means and covariances should be merged to reduce the number of Gaussian mixtures. Finally, the PHD of each particle is weighted and summed to obtain the updated VA's PHD.

[0064]

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

[0066] Step Six: After updating the particle weights, first take the exponent of the calculated particle weights: The updated target state can then be obtained by weighted summation of each particle:

[0067]

[0068] in This represents the updated target state, and the target's coordinates are included in its state.

[0069] Step 7: To ensure the system operates normally in the next moment, the particles need to be resampled, that is... If the target needs to be tracked again in the next moment after particle resampling, then continue to execute steps four through seven.

Claims

1. A simultaneous localization and mapping method based on RB-PHD filtering, characterized in that: a) Use Optimal Sub-pattern Assignment (OSA) distance to obtain the initial position of the target; b) Eliminate clutter received by the target at each time step through data correlation; c) Update the position of the virtual anchor (VA) node using the Rao-Blackwellized Probability Hypothesis Density (RB-PHD) filter; d) Obtain the updated target position by weighted summation of each updated particle; The method utilizes OSPA distance to obtain the initial position of a target, characterized by determining a relatively accurate initial position when the initial position of the tracked target is unknown. In a single-base station environment, wireless multipath signals undergo specular reflection on some reflectors during propagation, and the multipaths experiencing specular reflection can correspond to VAs. Given several VAs in the environment, the closed geometric shape constructed by connecting the outermost VAs end-to-end can be considered the range of the target's movement. Therefore, this closed shape can be divided into a grid with a certain precision to obtain the specific coordinates of each point within the grid, and then the Time-of-Flight (TOF) between each point within the grid and all VAs is calculated to obtain the set. ,in Represents the first in the grid 1 point; then compare with the actual observation using OSPA distance. The difference between them can be expressed as: (1) in yes The number of elements in the middle; yes The number of elements in the middle; The cutoff point is a constant greater than 0, reflecting the degree of penalty for the two vectors having different dimensions; The order reflects the sensitivity of the OSPA distance to outliers; for The set of all permutations and combinations, for The Middle The first permutation and combination One element; additionally defined: (2) Finally, the difference between the grid points and the actual observations is calculated sequentially, and the grid point with the smallest difference is the initial position of the target being tracked.

2. As described in claim 1, the method of eliminating clutter received by the tracking target at each moment through data association is characterized in that... To reduce computational complexity and improve positioning accuracy, the following steps are implemented. Since the observations received at each moment may not only include observations from the VA (the base station can also be considered a special VA), but may also include clutter such as higher-order reflections and scattering, this increases computational load and reduces positioning accuracy. a) Calculate the distance between the current tracking target and each VA, and obtain ; b) Find the observation set In, with The observation closest to the first element is selected if the difference between the two is less than the threshold value. If the observation is found to match the VA, it is considered not clutter. The observation is then added to a new observation set. At the same time from Remove from; c) Repeat the previous step until... The last element in the set yields the matched observation set. ; At this point, the new observation set It may not include all observations of VAs, because due to interference such as environmental noise, VAs that are far from the current location may not produce corresponding observations.

3. As described in claim 2, updating the position of VA using an RB-PHD filter, characterized in that... This can improve the accuracy of VA position and subsequent tracking accuracy. For the motion of the target to be tracked, a Constant Turn Rate and Velocity (CTRV) model can be used for modeling, which can simulate the linear and curvilinear motion of the target. .in , and These are the target's three-dimensional coordinates; The heading angle of the target; The linear velocity of the target; Let be the angular velocity of the target. Therefore, the state transition equation of the target is: (3) in For the goal of The state at any given moment; The sampling period; The noise is a process noise that follows a Gaussian distribution. ; The RB-PHD framework is used to describe the VA and the target to be tracked. The VA is modeled as a multi-objective Poisson process; therefore, at each time step, the PHD of the VA is transmitted, not its density. The system uses a total of... There are 3 particles, each with its own state, weight, and PHD. Therefore... A particle can be represented as: (4) in For the first The state of each particle; For the first The weight of each particle; For the first PHD of individual particles; The specific process of the algorithm is as follows: a) Initialize the particle state, weights, and PHD, and find the initial position of the target using the algorithm in Weight Requirement 1. Then generate the mean according to the Gaussian distribution. of There are n particles, each with a weight of n. PHD of particles, i.e. ; b) Particle PHD and Weight Prediction. The predicted target state can be obtained based on the state transition equation. Then, the observation set is filtered through the data association in claim 2 to obtain... Then, predict the PHD: (5) in The PhD representing the VA observed at the current moment, and has Then, expand the equation using Gaussian Mixture (GM): (6) (7) in Let be the number of Gaussians in PHD at the previous time step, and , and They are the first The weights, mean, and variance of each Gaussian component. It is the number of Gaussians observed in the PHD at the current moment, and it is also equal to the number of elements in the observation set after data correlation. These are the Gaussian mean and covariance of the corresponding observations, where the mean is the matched VA coordinates and the variance is a pre-set value reflecting the error of the VA coordinates. At this point, the above two equations can be combined into: (8) in ; c) Particle PHD and weight updates: (9) (10) in It is the average amount of clutter in the measurement; , The maximum perceptible range of the target; For the detection probability of VA, the detection probability of the VA detected at the current time should be 1, while the detection probability of the VA remaining from the previous time should be set to an adaptive detection probability, which is usually small. Simultaneously use the measurement set Update particle weights: (11) in Integrals are generally difficult to compute, so two approximations are needed. The first approximation involves the formula above. It can be converted into (12) The second approximation involves equation (10), which relates to Bayesian updates and can be transformed into a closed form: (13) In addition, to avoid numerical problems, equation (11) is transformed into logarithmic form: (14) use For the observation set Each observation in the data is labeled, and when a certain observation is... hour: (15) when hour: (16) in , and All were obtained through cubic Kalman filters (CKF); Since Gaussian mixing propagates Gaussian components over time, the number of Gaussian components will increase indefinitely if left uncontrolled. Therefore, Gaussian components with lower weights should be pruned and discarded, and Gaussian components with similar means and covariances should be merged to reduce the number of Gaussian mixtures. Finally, the PHD of each particle is weighted and summed to obtain the updated VA's PHD. (17) in Representing the The updated PHD at each time step, where the mean of each Gaussian component is the updated VA coordinate.

4. The updated target position obtained by weighted summation of particles as described in claim 3, characterized in that... After obtaining the updated target state and position, first take the exponent of the calculated particle weights: Then, by weighted summation of each particle, we obtain: (19) in This represents the updated target state, including the target's coordinates. Additionally, to ensure proper operation in the next time step, the particles need to be resampled. , Then, continue with the steps described above.

Images