An indoor positioning and tracking method based on UWB, vision and PDR fusion

Abstract

The application relates to an indoor positioning tracking method based on UWB, vision and PDR fusion, comprising the following steps: arranging an ultra-wideband (UWB) anchor point, recording the position (x i ,y i ) of the UWB anchor point; correcting a monitoring camera in an environment, and obtaining a projection matrix of the monitoring camera; a server pre-processes data, obtains UWB position estimation and vision position estimation, generates pedestrian dead reckoning (PDR) through inertial measurement unit (IMU) data; the pre-processing result is fused through particle filtering based on vision and UWB, positioning tracking of pedestrians is carried out, and the label of the UWB is returned as the pedestrian ID to the pedestrian to be positioned. The application improves the precision of the UWB sub-module, designs particle filtering fusion vision and UWB, realizes robust positioning, and can still provide positioning when a certain sensor fails; the advantages of the vision and the UWB are complementary to each other, data is tightly coupled, and errors can be eliminated in the bud; the application has wide application range, is easy to deploy, and has high positioning precision.

Description

Technical Field

[0001] This invention relates to the field of indoor positioning technology, and in particular to an indoor positioning and tracking method based on the fusion of UWB, vision, and PDR. Background Technology

[0002] In recent years, indoor positioning technology has developed rapidly, with widespread applications in large shopping malls and medical tracking. Various sensors are being explored for indoor positioning, such as ultra-wideband (UWB), Bluetooth, surveillance cameras, and built-in sensors in mobile phones. Fusion of this multi-source data to improve positioning accuracy has become a current research hotspot.

[0003] Among these, ultra-wideband (UWB) positioning offers high accuracy but suffers from significant non-line-of-sight (NLS) errors. Vision-based positioning, while also offering high accuracy, is susceptible to line-of-sight occlusion. In other words, using UWB alone results in NLS errors, and changes in ambient temperature and humidity weaken ranging accuracy. Furthermore, relying solely on vision-based positioning is affected by visual conditions such as smoke, brightness, and lighting. Therefore, it is necessary to propose a fusion positioning method. Summary of the Invention

[0004] The purpose of this invention is to provide an indoor positioning and tracking method based on the fusion of UWB, vision, and PDR that achieves robust positioning by fusing particle filtering with vision and ultra-wideband fusion, realizing the complementary advantages of vision and ultra-wideband, achieving tight data coupling, and high positioning accuracy.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: an indoor positioning and tracking method based on the fusion of UWB, vision, and PDR, the method comprising the following sequential steps:

[0006] (1) Offline phase: Deploy UWB anchor points and record the location (x) of the UWB anchor points. i ,y i );

[0007] (2) Calibration phase: Calibrate the surveillance cameras in the environment and obtain the projection matrix of the surveillance cameras;

[0008] (3) Online preprocessing stage: The pedestrian to be located sends ranging information and inertial measurement unit (IMU) data to the server, while the monitoring camera transmits video stream to the server; the server preprocesses the data, obtains UWB position estimation and visual position estimation, and generates pedestrian trajectory to calculate PDR through inertial measurement unit (IMU) data;

[0009] (4) Location and tracking stage: Based on the preprocessing results, the location and tracking of pedestrians are carried out by fusing the preprocessing results through particle filtering based on vision and UWB.

[0010] (5) During online location tracking, return the UWB tag as the pedestrian ID to the pedestrian to be located.

[0011] Step (1) specifically refers to: deploying UWB anchor points in the environment, ensuring that the UWB anchor points are not located on the same straight line, and recording the position (x) of each UWB anchor point. i ,y i At the same time, it corrects the ranging error of each ultra-wideband (UWB).

[0012] Step (2) specifically refers to: calibrating each surveillance camera in the environment, obtaining the intrinsic and extrinsic parameter matrices of the surveillance cameras after calibration, and obtaining a correspondence between world coordinates and the pixel plane:

[0013]

[0014] in, Using pixel coordinates, K is the intrinsic parameter matrix, and KP is the extrinsic parameter matrix. Using the world coordinate system, z c Scale factor;

[0015] Multiply the intrinsic parameter matrix by the extrinsic parameter matrix to obtain the projection matrix.

[0016] Step (3) specifically includes the following steps: The preprocessing includes processing the video stream data and processing the ultra-wideband (UWB) data:

[0017] (3a) Processing the video stream data refers to: using the YOLOv5 algorithm to detect pedestrians in the video frames transmitted to the server, taking the bottom midpoint of the detection box as the pixel position (u,v) of the pedestrian, and then obtaining the real-world position (x) of the pedestrian through projection. w ,y w (i.e., visual position estimation);

[0018] (3b) The processing of ultra-wideband (UWB) data refers to: using the adaptive variance-weighted least squares method, where the location of the UWB anchor point (x) is set. i ,y i The label position of the pedestrian to be located is (x, y), and the distance between the pedestrian label and the UWB anchor point is: Therefore, the following relationship holds:

[0019]

[0020] Using the least squares algorithm, we have the following location estimation model: Aε=b

[0021] in:

[0022]

[0023]

[0024]

[0025] Initial least squares estimate: ε = (A T A) -1 A T b, Define the distance from the least squares estimation point at the current time t to the UWB anchor point as m. t The distance from the current label point t to the UWB anchor point is d. t Construct a sample S of the absolute value of the difference between the estimated location and the distance to the UWB anchor point i and the measured distance. i :{|m1-d1|,|m2-d2|,……,|m t -d t |, the sample S at current time t i The variance of is σ i Construct an adaptive variance weight matrix for the current time t:

[0026]

[0027] We obtain a weighted position estimate: For ultra-wideband location estimation;

[0028] When performing the next time step positioning, the weight matrix is ​​recalculated;

[0029] (3c) For the 9-axis data of the inertial measurement unit (IMU), the direction is calculated. h k Step size d k Calculate a displacement at the current moment:

[0030] x k =x k-1 +d k cos(h k )

[0031] y k =y k-1 +d k sin(h k )

[0032] Among them, (x k y k (x) represents the current position, (x) k-1 y k-1 ) represents the position at the previous time step; d kcos(h k ) represents the displacement of two positions, i.e., the pedestrian trajectory estimation PDR.

[0033] Step (4) includes the following steps:

[0034] (4a) Robust localization and tracking is achieved by fusing vision-based and UWB-based particle filtering. The first step is to initialize the particles and generate random positions (x). i ,y i ), random direction h i random weights w i The set of particles, denoted as P = {P1, P2, P3, ..., P...} N}, where P i = < x i ,y i ,h i ,w i >;

[0035] (4b) The movement of the particle is driven by the PDR input from the pedestrian to be located, and the distance between the particle and the nearest visual point is:

[0036]

[0037] The visual results are as follows: Let C represent the visual localization result of the a-th pedestrian, and let C represent the number of pedestrians in the visual localization.

[0038] The distance between the particle and the nearest ultrawideband (UWB) point is:

[0039]

[0040] The results for Ultra Wideband (UWB) are as follows: Let D represent the ultrawideband positioning result for the a-th pedestrian, where D is the number of pedestrians in the ultrawideband positioning result.

[0041] The particle weight update strategy is as follows:

[0042]

[0043]

[0044] In the formula, To update the particle weights, For the updated particle weights, For ultrawideband Gaussian kernel variance factor, The visual Gaussian kernel variance factor. The distance from the particle point to the nearest visual point. The distance from the particle point to the nearest ultrawideband positioning point;

[0045] (4c) Particle resampling strategy: First, normalize the weights of all particles:

[0046]

[0047] In the formula, w i The weights of the particles before normalization. Here, n represents the normalized weight of the particles, and n is the number of particles.

[0048] Randomly generate a number m ∈ [0, 1) such that ∑ 1≤j＜n w j ≤m∑ 1≤j＜n+1 w j Generate a random position that satisfies |x n ,y n )-X v |+|x n ,y n )-X u |≤2D, the new particle is X n X n = < x n ,y n ,h n ,w n >;

[0049] Repeat steps (4a) to (4c) until the required number of resampled particles are generated, then proceed to step 4(d).

[0050] (4d) In the final position estimation stage, the position of the pedestrian is estimated using the position of weighted particles, i.e., localization and tracking:

[0051]

[0052] In the formula, (x n ,y n ) represents the particle position, W n Let N be the particle weight, N be the number of particles, and (x,y) be the pedestrian's position.

[0053] Step (5) specifically refers to: based on the continuous iteration and movement of the particles, and based on the weighted update of the visual position and the ultra-wideband UWB position, the particles converge to the vicinity of the visual position and the ultra-wideband UWB position, and the ultra-wideband UWB label is returned to the pedestrian to be located to assign a visual positioning result ID, thereby determining the identity of the pedestrian to be located.

[0054] As can be seen from the above technical solution, the beneficial effects of the present invention are as follows: First, the present invention improves the accuracy of the ultra-wideband submodule, and the design of particle filter fusion vision and ultra-wideband can achieve robust positioning, still providing positioning even when a certain sensor fails; Second, the present invention achieves complementary advantages between vision and ultra-wideband, realizing tight data coupling, and can nip errors in the bud; Third, the present invention has a wide range of applications, is easy to deploy, has high positioning accuracy, can achieve centimeter-level positioning, uses low-cost sensors, and is easy to promote; Fourth, the present invention has strong resistance to environmental interference and can work in smoke, low light, and complex obstacle environments; while having high accuracy, the present invention has low algorithm complexity, enabling real-time pedestrian positioning and tracking. Attached Figure Description

[0055] Figure 1 Calibration diagram for surveillance cameras;

[0056] Figure 2 This is a schematic diagram of particle filtering rules;

[0057] Figure 3 This is a schematic diagram of particle weights. Detailed Implementation

[0058] An indoor positioning and tracking method based on the fusion of UWB, vision, and PDR, comprising the following sequential steps:

[0059] (1) Offline phase: Deploy UWB anchor points and record the location (x) of the UWB anchor points. i ,y i );

[0060] (2) Calibration phase: Calibrate the surveillance cameras in the environment and obtain the projection matrix of the surveillance cameras;

[0061] (3) Online preprocessing stage: The pedestrian to be located sends ranging information and inertial measurement unit (IMU) data to the server, while the monitoring camera transmits video stream to the server; the server preprocesses the data, obtains UWB position estimation and visual position estimation, and generates pedestrian trajectory to calculate PDR through inertial measurement unit (IMU) data;

[0062] (4) Location and tracking stage: Based on the preprocessing results, the location and tracking of pedestrians are carried out by fusing the preprocessing results through particle filtering based on vision and UWB.

[0063] (5) During online location tracking, return the UWB tag as the pedestrian ID to the pedestrian to be located.

[0064] like Figure 1As shown, step (1) specifically refers to: deploying UWB anchor points in the environment, wherein the UWB anchor points are not located on the same straight line, and recording the position (x) of each UWB anchor point. i ,y i This process also corrects the ranging error of each UWB (Ultra-Wideband) satellite. The goal is to keep the UWB anchor points as close as possible to the same plane but not on the same straight line to eliminate multipath effects.

[0065] Step (2) specifically refers to: calibrating each surveillance camera in the environment, obtaining the intrinsic and extrinsic parameter matrices of the surveillance cameras after calibration, and obtaining a correspondence between world coordinates and the pixel plane:

[0066]

[0067] in, Using pixel coordinates, K is the intrinsic parameter matrix, and KP is the extrinsic parameter matrix. Using the world coordinate system, z c Scale factor;

[0068] Multiply the intrinsic parameter matrix by the extrinsic parameter matrix to obtain the projection matrix.

[0069] Step (3) specifically includes the following steps: The preprocessing includes processing the video stream data and processing the ultra-wideband (UWB) data:

[0070] (3a) Processing the video stream data refers to: using the YOLOv5 algorithm to detect pedestrians in the video frames transmitted to the server, taking the bottom midpoint of the detection box as the pixel position (u,v) of the pedestrian, and then obtaining the real-world position (x) of the pedestrian through projection. w ,y w (i.e., visual position estimation); the lightweight computational advantage of the YOLOv5 algorithm can ensure real-time video detection and provide stable detection boxes.

[0071] (3b) The processing of ultra-wideband (UWB) data refers to: using the adaptive variance-weighted least squares method, where the location of the UWB anchor point (x) is set. i ,y i The label position of the pedestrian to be located is (x, y), and the distance between the pedestrian label and the UWB anchor point is: Therefore, the following relationship holds:

[0072]

[0073] Using the least squares algorithm, we have the following location estimation model: Aε=b

[0074] in:

[0075]

[0076]

[0077]

[0078] Initial least squares estimate: ε = (A T A) -1 A T b, Define the distance from the least squares estimation point at the current time t to the UWB anchor point as m. t The distance from the current label point t to the UWB anchor point is d. t Construct a sample S of the absolute value of the difference between the estimated location and the distance to the UWB anchor point i and the measured distance. i :{|m1-d1|,|m2-d2|,……,|m t -d t |}, the sample S at time t i The variance of is σ i Construct an adaptive variance weight matrix for the current time t:

[0079]

[0080] We obtain a weighted position estimate: For ultra-wideband location estimation;

[0081] When performing the next time step positioning, the weight matrix is ​​recalculated;

[0082] (3c) For the 9-axis data of the inertial measurement unit (IMU), the direction h is calculated. k Step size d k Calculate a displacement at the current moment:

[0083] x k =x k-1 +d k cos(h k )

[0084] y k =y k-1 +d k sin(h k )

[0085] Among them, (x k y k (x) represents the current position, (x) k-1 y k-1 ) represents the position at the previous time step; d k cos(hk ) represents the displacement of two positions, i.e., the pedestrian trajectory estimation PDR.

[0086] Step (4) includes the following steps:

[0087] (4a) Robust localization and tracking is achieved by fusing vision-based and UWB-based particle filtering. The first step is to initialize the particles and generate random positions (x). i ,y i ), random direction h i random weights w i The set of particles, denoted as P = {P1, P2, P3, ..., P...} N}, where P i = <x i ,y i ,h i ,w i >;

[0088] (4b) Particle movement is driven by the PDR input from the pedestrian to be located, and the particles are constrained by the environmental geometry. A particle entering an obstacle area indicates its demise, such as... Figure 2 As shown, the distance between the particle and the nearest visual point is:

[0089]

[0090] The visual results are as follows: Let C represent the visual localization result of the a-th pedestrian, and let C represent the number of pedestrians in the visual localization.

[0091] The distance between the particle and the nearest ultrawideband (UWB) point is:

[0092]

[0093] The results for Ultra Wideband (UWB) are as follows: Let D represent the ultrawideband positioning result for the a-th pedestrian, where D is the number of pedestrians in the ultrawideband positioning result.

[0094] like Figure 3 As shown, the particle weight update strategy is as follows:

[0095]

[0096]

[0097] In the formula, To update the particle weights, For the updated particle weights, For ultrawideband Gaussian kernel variance factor, The visual Gaussian kernel variance factor. The distance from the particle point to the nearest visual point. The distance from the particle point to the nearest ultrawideband positioning point;

[0098] (4c) Particle resampling strategy: First, normalize the weights of all particles:

[0099]

[0100] In the formula, w i The weights of the particles before normalization. Here, n represents the normalized weight of the particles, and n is the number of particles.

[0101] Randomly generate a number m ∈ [0, 1) such that ∑ 1≤j＜n w j ≤m∑ 1≤j＜n+1 w j Generate a random position that satisfies |x n ,y n )-X v |+|x n ,y n )-X u |≤2D, the new particle is X n X n = < x n ,y n ,h n ,w n >;

[0102] Repeat steps (4a) to (4c) until the required number of resampled particles are generated, then proceed to step 4(d).

[0103] (4d) In the final position estimation stage, the position of the pedestrian is estimated using the position of weighted particles, i.e., localization and tracking:

[0104]

[0105] In the formula, nx n ,y n ) represents the particle position, W n Let N be the particle weight, N be the number of particles, and (x,y) be the pedestrian's position.

[0106] Step (5) specifically refers to: based on the continuous iteration and movement of the particles, and based on the weighted update of the visual position and the ultra-wideband UWB position, the particles converge to the vicinity of the visual position and the ultra-wideband UWB position, and the ultra-wideband UWB label is returned to the pedestrian to be located to assign a visual positioning result ID, thereby determining the identity of the pedestrian to be located.

[0107] In summary, this invention improves the accuracy of the ultra-wideband submodule, and its particle filter fusion of vision and ultra-wideband enables robust positioning, providing location even when one sensor fails. This invention achieves complementary advantages between vision and ultra-wideband, realizing tight data coupling and nipping errors in the bud. This invention has a wide range of applications, is easy to deploy, and offers high positioning accuracy, achieving centimeter-level positioning. The sensors used are inexpensive and easy to promote. This invention is highly resistant to environmental interference and can operate in smoke, low-light, and complex obstacle environments. While achieving high accuracy, this invention also has low algorithm complexity, enabling real-time pedestrian positioning and tracking.

Claims

1. A method for indoor positioning and tracking based on UWB, vision and PDR fusion, characterized in that: The method includes the following steps in sequence: (1) Offline stage: arranging an ultra-wideband (UWB) anchor point, recording the position of the ultra-wideband (UWB) anchor point , ; (2) Calibration phase: Calibrate the surveillance cameras in the environment and obtain the projection matrix of the surveillance cameras; (3) Online preprocessing stage: The pedestrian to be located sends ranging information and inertial measurement unit (IMU) data to the server, while the monitoring camera transmits video stream to the server; The server preprocesses the data to obtain UWB position estimates and visual position estimates, and generates pedestrian trajectory estimation (PDR) by using inertial measurement unit (IMU) data. (4) Location and tracking stage: Based on the preprocessing results, the location and tracking of pedestrians are carried out by fusing the preprocessing results through particle filtering based on vision and UWB. (5) During online location tracking, return the UWB tag as the pedestrian ID to the pedestrian to be located; Step (3) specifically includes the following steps: The preprocessing includes processing the video stream data and processing the ultra-wideband (UWB) data: (3a) Processing the video stream data means: using YOLOv5 algorithm to detect pedestrians on the video frames transmitted to the server, and taking the lower midpoint of the detection box as the pixel position of the pedestrian , and obtaining the real-world position of the pedestrian through projection , , that is, visual position estimation; (3b) For the processing of the ultra-wideband UWB data, an adaptive variance weighted least square method is adopted, the position of an ultra-wideband UWB anchor point is set as position , , the tag position of a pedestrian to be positioned is position , the distance between the tag of the pedestrian to be positioned and the ultra-wideband UWB anchor point is: , therefore, the following relationship is established: ， By least square algorithm, there is a position estimation model as follows: , in: ， ， ， Initial least squares estimate: Let the distance from the least squares estimation point at the current time t to the UWB anchor point be defined as... The distance from the current label point t to the UWB anchor point is Construct estimated location and UWB anchor point The sample of the absolute value of the difference between the distance and the measured distance. The sample at time t The variance of is Construct an adaptive variance weight matrix for the current time t: ， A weighted position estimate is obtained: , for ultra-wideband position estimation; When performing the next time step positioning, the weight matrix is ​​recalculated; (3c) For 9-axis data of the inertial measurement unit IMU, the direction , step , a displacement at the current time is calculated: ， ， in,( , () represents the current position. , () represents the position at the previous time step; The PDR is calculated based on the displacement of two positions, i.e., the pedestrian trajectory. The characteristic is that step (4) includes the following steps: (4a) Robust localization tracking with vision and UWB based particle filter fusion, first is the initialization of particles, generating a set of particles with random positions , random directions , random weights , denoted as , where ; (4b) The movement of the particle is driven by the PDR input from the pedestrian to be located, and the distance between the particle and the nearest visual point is: ， Wherein, the visual results are as follows: ; represents the visual positioning result of the a th pedestrian, and C represents the number of pedestrians in visual positioning. The distance between the particle and the nearest ultrawideband (UWB) point is: ， Wherein, the ultra-wideband UWB result is as follows: ; The ultra-wideband positioning result of the a th pedestrian is represented, and D is the number of pedestrians of the ultra-wideband positioning result. The particle weight update strategy is as follows: ， ， wherein, is the updated particle weight, is the updated particle weight, is the ultra-wideband Gaussian kernel variance factor, is the visual Gaussian kernel variance factor, is the distance of the particle point to the nearest visual point, is the distance of the particle point to the nearest ultra-wideband localization point; (4c) Particle resampling strategy: First, normalize the weights of all particles: ， wherein is the weight of the particle before normalization, is the weight of the particle after normalization, n is the number of particles; Randomly generate a number , so that it satisfies ; generate a random position that satisfies , the new particle is , ; Repeat steps (4a) to (4c) until the required number of resampled particles are generated, then proceed to step 4(d). (4d) In the final position estimation stage, the position of the pedestrian is estimated using the position of weighted particles, i.e., localization and tracking: ， wherein, is the particle position, is the particle weight, N is the number of particles, is the pedestrian position.

2. The indoor positioning tracking method based on UWB, vision and PDR fusion according to claim 1, characterized in that: The step (1) is specifically to arrange the ultra-wideband (UWB) anchor points in the environment, the UWB anchor points are not located on the same straight line, record the position of each UWB anchor point, and correct the ranging error of each UWB at the same time. , ​ 3.The UWB, vision and PDR fusion-based indoor positioning and tracking method according to claim 1, characterized in that: Step (2) specifically refers to: calibrating each surveillance camera in the environment, obtaining the intrinsic and extrinsic parameter matrices of the surveillance cameras after calibration, and obtaining a correspondence between world coordinates and the pixel plane: ， wherein, is a pixel coordinate system, is an intrinsic matrix, is an extrinsic matrix, is a world coordinate system, is a scale factor; Multiply the intrinsic parameter matrix by the extrinsic parameter matrix to obtain the projection matrix. 4.The UWB, vision and PDR fusion-based indoor positioning and tracking method according to claim 1, characterized in that: The step (5) specifically refers to: based on the continuous iteration and movement of the particles, and based on the weighted update of the visual position and the ultra-wideband UWB position, the particles converge to the vicinity of the visual position and the ultra-wideband UWB position, and the label of the ultra-wideband UWB is returned to the pedestrian to be located to assign a visual positioning result ID, thereby determining the identity of the pedestrian to be located.

Images