Construction worker positioning and tracking algorithm for NLOS construction sites

Abstract

The present invention discloses a construction personnel positioning and tracking algorithm for NLOS construction sites, adopts a positioning system, the positioning system includes a tag worn by the construction personnel, at least three base stations with fixed known positions, a server for receiving ranging information and calculating tag position information, a switch for network data interaction between the base station and the server, and the base station is installed at the construction site of the construction site; according to the timestamp information of ultra-wideband signal transmission and reception acquired by the base station, ranging information is obtained, and the ranging information obtained by multiple base stations is sent to the server through the switch, and according to the ranging information, the weighted least squares algorithm is used to optimize the initial value required for Taylor algorithm operation, and then the Taylor algorithm is used to obtain the initial positioning result according to the optimized initial value, and the volumetric Kalman filtering algorithm is used to optimize the initial positioning result to obtain the final estimated position. The present invention can accurately locate the position of the construction personnel and improve the construction safety.

Description

Technical Field

[0001] The present invention relates to the field of construction safety at a construction site, and in particular to a construction personnel positioning and tracking algorithm for an NLOS construction site. Background Art

[0002] NLOS construction site is a construction scene that uses a management system to monitor the distribution and activity trajectory of on-site personnel in real time. Through the positioning system, the number of people and their locations on the site can be understood in real time, so that the on-site situation can be monitored, and possible and existing safety accidents can be alarmed and handled according to pre-set early warning rules, thereby greatly improving the safety of construction sites.

[0003] In NLOS construction sites, construction workers are active, so positioning and tracking them is extremely critical. Among the existing technical means, base stations and UWB tags are used together. The UWB tags are given to construction workers to carry with them. The location of the UWB tags is grasped in real time through data communication, thereby inferring the location of the construction workers.

[0004] However, during the positioning process, the tag communicates with the base station in real time through ultra-wideband signals, and after communication, the ranging information is obtained by acquiring the timestamp information of the ultra-wideband signal sent and received, and the distance between the tag and the corresponding base station is obtained through the algorithm. During the calculation process, the tracking algorithm uses the algorithm to calculate the location information of the construction personnel based on the communication data. Due to the different calculation methods, there is a large deviation between the calculated location information of the construction personnel and the actual location information of the construction personnel, resulting in a large deviation between the positioning result and the actual position, which greatly affects the positioning and tracking of the construction personnel. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a construction personnel positioning and tracking algorithm for NLOS construction sites, which can accurately calculate the actual position of the construction personnel and realize precise positioning and tracking of the construction personnel.

[0006] In order to solve the above technical problems, the present invention provides a construction worker positioning and tracking algorithm for NLOS construction sites, using a positioning system, the positioning system includes tags worn by construction workers, at least three base stations with fixed known positions, a server for receiving ranging information and calculating tag position information, and a switch for network data exchange between the base station and the server. The base station is installed at the construction site of the construction site, and the switch is used to power the base station; the positioning system includes the following steps:

[0007] The tag communicates with the base station in real time through ultra-wideband signals. After the communication, the base station obtains the timestamp information of the ultra-wideband signal sent and received, and the micro-processing unit on the base station processes it to obtain the ranging information, that is, the distance between the tag and the corresponding base station. The ranging information obtained by multiple base stations is sent to the server through the switch. After the server obtains all the ranging information about a certain tag, it uses the weighted least squares algorithm to optimize the initial values ​​required for the Taylor algorithm operation according to the ranging information, and then uses the Taylor algorithm based on the optimized initial values ​​to obtain the preliminary positioning result. Finally, the volumetric Kalman filter algorithm is used to optimize the preliminary positioning result to obtain the final estimated position.

[0008] Furthermore, the distance between the tag and the corresponding base station is calculated by the flight time T through 6 consecutive time stamps between the tag and the base station, and the distance d=T·C is calculated according to the flight time T, where C is the propagation speed of electromagnetic waves in the air.

[0009] Furthermore, the six timestamps are obtained by the following method: the tag sends a ranging request message to the base station, and records the sending timestamp T sp , the base station records the receiving timestamp T received from the tag rp After a certain delay, send a reply message to the tag and record the sending timestamp T sr , the tag records the current timestamp T when receiving the reply message rr After a delay, a distance measurement message is sent to the base station, and the sending timestamp T is recorded. sf , the base station records the receiving timestamp T rf , and get 6 timestamps.

[0010] Furthermore, the method for obtaining the initial value of the Taylor algorithm is as follows:

[0011] The three-dimensional coordinates (x i ,y i ,z i )(i=1,2…n) is projected onto a two-dimensional horizontal plane for calculation, and we get

[0012] Subtracting the first n-1 equations from the last equation in the above formula, we can get AX=B, where

[0013]

[0014]

[0015] X is the coordinate value of the label position;

[0016] Assume that the n-1-dimensional positioning error matrix e n Then n=AX-B, based on the least squares principle, let f(x) = |AX-B| 2 , take the derivative of f(x) and set the derivative to 0, and we can get the optimal estimate of X. The formula is as follows:

[0017]

[0018] Then different weights are added to the distance measured by each base station. The weighted weight matrix is ​​W. Based on the weighted least squares algorithm, the tag position estimate can be obtained:

[0019] When W = R -1 hour, The mean square error is the smallest, where R is the ranging error variance, and the elements of the weighted weight matrix are defined using the formula: In the formula, i represents the base station number corresponding to the tag ranging, j represents the tag number, and n is the total number of ranging;

[0020] Then calculate the estimated value error statistical parameter p k , p k Indicates the difference between the kth estimated value and the rest of the estimated values, and sets the outlier removal threshold p. When p k > p indicates that the estimated value of the label position of this group has a large error, so it is removed. After the elimination, there are t estimated label positions left. The estimated label coordinates are calculated based on the estimated label positions:

[0021] That is the initial value.

[0022] Furthermore, based on the obtained label, the coordinates (x0, y0) are estimated, and the actual coordinates of the label are assumed to be (x, y), then the positioning error equation is Among them (δ x ,δ y ) is the error between the initial value and the true value;

[0023] Establish the positioning function based on the distance formula: k i To locate the projection distance between the tag and each base station in the horizontal plane, expand the above equation at (x0, y0) by Taylor to obtain:

[0024] in

[0025] k i|0 (i=1,2...n) is the initial value of the input and the horizontal projection distance of each base station. Further, we can get:

[0026]

[0027] Then we can get ε i =h i -ξ i δ, where

[0028]

[0029] According to the least squares principle, the optimal estimate of δ can be obtained

[0030] By judging the relationship between the size of the positioning error and the preset threshold, it is inferred whether the algorithm needs further iterative calculation to reduce the positioning error. If the positioning error is greater than the preset threshold, the current calculation result is used as the new initial value for calculation until the positioning error is less than the preset threshold or the preset maximum number of iterations is reached. The estimated value of the label coordinate is (x0+δ x ,y0+δ y ).

[0031] Furthermore, the measurement noise variance is added as a parameter to be estimated into the system model of the volumetric Kalman filter algorithm, the tag coordinate estimation value is used as the observation value of the volumetric Kalman filter algorithm and the tag position is smoothed.

[0032] Furthermore, the system model is:

[0033] Where k represents the kth moment, z k and x k+1 are the measurement vector and state vector at time k, respectively, where the state vector contains the position parameters to be estimated and the measurement noise variance; h k (·) and f k (·) are the measurement function and the state transfer function respectively; w k and v k are process noise and measurement noise respectively, which satisfy zero-mean Gaussian distribution and are independent of each other.

[0034] Beneficial effects of the present invention:

[0035] The positioning and tracking algorithm first uses the weighted least squares algorithm to optimize the initial value of the Taylor algorithm. The use of the optimized initial value can greatly improve the positioning accuracy, and then the deviation between the preliminary positioning result obtained by Taylor and the actual position is greatly reduced. Finally, the volumetric Kalman filter algorithm is used to further optimize the preliminary positioning result to obtain the final estimated position, thereby realizing high-precision positioning and tracking functions for construction personnel. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is a schematic diagram of the system composition of the present invention;

[0037] Figure 2is a distance measurement schematic diagram of the present invention;

[0038] Figure 3 It is a schematic diagram of the two-dimensional projection of the three-dimensional distance of the present invention. DETAILED DESCRIPTION

[0039] The present invention is further described below in conjunction with the accompanying drawings and specific embodiments so that those skilled in the art can better understand the present invention and implement it, but the embodiments are not intended to limit the present invention.

[0040] Reference Figure 1 As shown, an embodiment of the construction personnel positioning and tracking algorithm for NLOS construction sites of the present invention adopts a positioning system, which includes tags worn by construction personnel, at least three base stations with fixed known positions, a server for receiving ranging information and calculating tag position information, and a switch for network data exchange between the base station and the server. The base station is installed at the construction site of the construction site, and the switch is used to power the base station, using PoE power supply;

[0041] Specifically, in Figure 1 In the figure, the mark in the middle of the three base stations is the tag position. The tag communicates with the base station in real time through ultra-wideband signals. After communication, the base station obtains the timestamp information of sending and receiving the ultra-wideband signal, and the micro-processing unit on the base station processes it to obtain the ranging information, that is, the distance between the tag and the corresponding base station. The ranging information obtained by multiple base stations is sent to the server through the switch. After the server obtains all the ranging information about a certain tag, it uses the weighted least squares algorithm to optimize the initial value required for the Taylor algorithm operation according to the ranging information, and then uses the Taylor algorithm according to the optimized initial value to obtain the preliminary positioning result. Finally, the volumetric Kalman filter algorithm is used to optimize the preliminary positioning result to obtain the final estimated position.

[0042] The distance between the base station and the tag is obtained by the DS-TWR ranging algorithm. Figure 2 As shown in the figure, the distance between the tag and the corresponding base station is calculated by the six consecutive timestamps between the tag and the base station to obtain the flight time T. Specifically, suppose that at a certain moment the tag sends a ranging request message to the base station and records the sending timestamp T at this time. sp , the base station records the receiving timestamp T received from the tag rp After a certain delay, send a reply message to the tag and record the sending timestamp T sr When the tag receives the reply message, it records the current timestamp T rr After a delay, a distance measurement message is sent to the base station, and the sending timestamp T is recorded. sf , the base station records the receiving timestamp T rfThe ranging process ends, and there are 6 timestamps in total. Based on this, the flight time T between the base station and the tag is obtained as:

[0043] T round1 =T rr -T sp ,T round2 =T rf -T sr

[0044] T reply1 =T sr -T rp ,T reply2 =T sf -T rr

[0045]

[0046] Then we can get the distance in three-dimensional space d=T·C, where C is the propagation speed of electromagnetic waves in air.

[0047] Through DS-TWR ranging, the distance between each base station and the tag can be obtained as d i (i=1,2,...,n).

[0048] Based on the above three-dimensional space distance, the method for obtaining the initial value of the Taylor algorithm is as follows:

[0049] Assume that there are n (n≥3) base stations in the position space, and the three-dimensional coordinates of each base station are set to (x i ,y i ,z i )(i=1,2…n), to simplify the analysis, the height of the label can be fixed, taking z=1.75, and projecting it to the two-dimensional horizontal plane for the next calculation. The projection diagram is shown in Figure 3 As shown, Δh is the height difference between the base station and the tag, d is the distance in three-dimensional space, and q is the projection distance in the two-dimensional horizontal plane.

[0050] From the projection formula we can get:

[0051]

[0052] Subtracting the first n-1 equations from the last equation in formula (1) yields AX=B, where

[0053]

[0054] Assume that the n-1-dimensional positioning error matrix e n , formula (2) is transformed into

[0055] e n =AX-B (3)

[0056] X is the coordinate value of the label position;

[0057] Based on the least squares principle, let f(x) = |AX-B| 2 , take the derivative of f(x) and set the derivative to 0, and we can get the optimal estimate of X. The formula is as follows:

[0058] Due to the uncertainty principle and the influence of the system error and random error of the actual measurement equipment, there is a certain error between the measured position of the base station node and its three-dimensional precise position in the actual space, and the classic least squares algorithm will introduce truncation error, which further affects the positioning accuracy. Therefore, the weighted least squares algorithm is used to deal with this problem. Different weights are added to the distance measured by each base station. The weighted weight matrix is ​​W, W = diag (w 1j ... w ij ... w nj ), based on the weighted least squares algorithm, we can get WAX = WB, and further get the tag position estimate

[0059] According to the relevant knowledge of advanced mathematics, we can know that when W=R -1 hour, The mean square error is the smallest, where R is the ranging error variance. Due to the equipment production system error and system operation error, the ranging error equation cannot be accurately obtained in real time. At this time, if the inaccurate ranging error variance is used, the formula W = R -1 Constructing a weighted matrix, it is verified by practical application that the error will increase, affecting the positioning accuracy of the system. Since the ranging accuracy is inversely proportional to the actual distance, the formula is used to define the elements of the weighted matrix. The formula is: In the formula, i represents the base station number corresponding to the tag ranging, j represents the tag number, and n is the total number of ranging;

[0060] After applying the above weighted least squares method to obtain the label position, assume that there are N (N≥2) estimated values ​​of the label position, and the statistical parameter of the estimated value error of each label position is

[0061] Among them, represents the estimated coordinates of the label position, p k It indicates the difference between the kth estimated value and the other estimated values. When a certain estimated value differs greatly from other estimated values, it means that its estimation error is large. Based on this, the outlier removal threshold p is set. When p k > p indicates that the estimated value of the label position of this group has a large error, so it is removed. After the elimination, there are t estimated values ​​of the label position left. The initial value used by the Taylor algorithm is calculated based on the estimated value of the label position:

[0062]

[0063] Then, the Taylor positioning algorithm is used to calculate based on the initial value. The coordinates (x0, y0) are estimated based on the obtained label. Assuming the true coordinates of the label are (x, y), the positioning error equation is: Among them (δ x ,δ y ) is the error between the initial value and the true value;

[0064] Establish the positioning function based on the distance formula: k i To locate the projection distance between the tag and each base station in the horizontal plane, expand the above equation at (x0, y0) by Taylor to obtain:

[0065] in

[0066] k i|0 (i=1,2…n) is the initial value of the input and the horizontal projection distance of each base station. Further, we can get:

[0067]

[0068] Then we can get ε i =h i -ξ i δ, where

[0069]

[0070] According to the least squares principle, the optimal estimate of δ can be obtained

[0071] By judging the relationship between the size of the positioning error and the preset threshold, it is inferred whether the algorithm needs further iterative calculation to reduce the positioning error. If the positioning error is greater than the preset threshold, the current calculation result is used as the new initial value for calculation until the positioning error is less than the preset threshold or the preset maximum number of iterations is reached. The estimated value of the label coordinate is (x0+δ x ,y0+δ y ).

[0072] In the NLOS environment of a construction site, when the tag moves, due to the influence of process noise and measurement noise, the tag position will jump and cannot meet the requirements of actual applications. Therefore, in the NLOS environment of a construction site, the measurement noise variance is added as a parameter to be estimated in the system model of the volumetric Kalman filter algorithm, and the tag coordinate estimation value is used as the observation value of the volumetric Kalman filter algorithm and the tag position is smoothed to further improve the system's positioning accuracy for construction personnel. The improved volumetric Kalman filter smoothing algorithm is proposed as follows:

[0073] First, establish the system model, the system model is:

[0074] Where k represents the kth moment, z k and x k+1 are the measurement vector and state vector at time k, respectively, where the state vector contains the position parameters to be estimated and the measurement noise variance; h k (·) and f k (·) are the measurement function and the state transfer function respectively; w k and v k are process noise and measurement noise respectively, which satisfy zero-mean Gaussian distribution and are independent of each other.

[0075] Then the improved CKF is used to further smooth the estimation results and improve the positioning accuracy. The corresponding improved CKF process is as follows:

[0076] ① Initialization

[0077] Assume that the initial estimate is The corresponding estimation error covariance is P0.

[0078] ②Time update

[0079] a. Estimated error covariance matrix and Cholesky decomposition

[0080]

[0081] b. Calculate the volume sampling points (i = 1, 2, ..., 2n x )

[0082]

[0083] When k = 0, S k-1|k-1 =S0,

[0084] in Represents the volume sampling rule.

[0085] c. Calculate the volume recursive value in one step (i=1,2,...,2n x )

[0086]

[0087] d. Calculate the predicted status

[0088]

[0089] e. Calculate the corresponding prediction error covariance matrix

[0090]

[0091] Where Q k-1 is the process noise covariance matrix.

[0092] Finally, perform the measurement update:

[0093] a. Decomposition of P k|k-1

[0094]

[0095] b. Calculate volume points (i = 1, 2, ..., 2n x )

[0096]

[0097] c. Calculate the measurement volume points (i = 1, 2, ..., 2n x )

[0098] Z i,k|k-1 =h(X i,k|k-1 )+v k

[0099] d. Calculate the predicted measurement value

[0100]

[0101] e. Calculate the innovation covariance matrix

[0102]

[0103] f. Calculate the state measurement covariance matrix

[0104]

[0105] g. Calculate the Kalman filter gain matrix

[0106]

[0107] h. Calculate updated estimate

[0108]

[0109] i. Calculate the corresponding estimation error covariance matrix

[0110]

[0111] Through the above CKF algorithm process, the coordinate position of the tag after smoothing can be obtained. and the corresponding estimation error covariance P k|k , and then the final positioning result can be obtained.

[0112] Through the positioning and tracking algorithm of the present application, the position of construction site workers can be accurately located, their positions can be obtained in real time, and timely warnings can be issued when they are in dangerous areas, thereby preventing the occurrence of dangerous situations in advance. The present invention first obtains preliminary positioning results through a weighted least squares algorithm, then inputs a Taylor algorithm positioning module to obtain a position calculation result, and finally uses an improved volumetric Kalman filter to obtain the final positioning result, thereby realizing the positioning and tracking functions of construction workers.

[0113] The above-described embodiments are only preferred embodiments for fully illustrating the present invention, and the protection scope of the present invention is not limited thereto. Equivalent substitutions or changes made by those skilled in the art based on the present invention are within the protection scope of the present invention. The protection scope of the present invention shall be subject to the claims.

Claims

1. A construction worker positioning and tracking algorithm for NLOS construction sites, characterized in that: A positioning system is used, which includes tags worn by construction workers, at least three base stations with fixed known positions, a server for receiving distance measurement information and calculating tag location information, and a switch for network data exchange between the base station and the server. The base station is installed at the construction site of the construction site, and the switch is used to power the base station; The following steps are involved: The tag communicates with the base station in real time through ultra-wideband signals. After the communication, the base station obtains the timestamp information of the ultra-wideband signal sent and received, and the micro-processing unit on the base station processes it to obtain the ranging information, that is, the distance between the tag and the corresponding base station. The ranging information obtained by multiple base stations is sent to the server through the switch. After the server obtains all the ranging information about a certain tag, it uses the weighted least squares algorithm to optimize the initial value required for the Taylor algorithm operation according to the ranging information, and then uses the Taylor algorithm to obtain the preliminary positioning result based on the optimized initial value. Finally, the volumetric Kalman filter algorithm is used to optimize the preliminary positioning result to obtain the final estimated position; Among them, the method for obtaining the initial value of the Taylor algorithm is as follows: The three-dimensional coordinates of the base station Projected onto a two-dimensional horizontal plane for calculation, we get ; In the above formula, Subtracting the last equation from the first one, we get ,in, ； X is the coordinate value of the label position; set up Dimensional positioning error matrix but , based on the least squares principle, let ,right Taking the derivative and setting it to 0, we get The optimal estimate is as follows: ； Then different weights are added to the distance measured by each base station, and the weighted weight matrix is: , based on the weighted least squares algorithm, the label position estimate can be obtained ; when hour, The mean square error is the smallest, where is the ranging error variance, and the elements of the weighted weight matrix are defined using the formula: , where It indicates the base station number corresponding to the tag ranging. It indicates the tag number. is the total number of distance measurements; Then calculate the estimated value error statistics parameters , Indicates The difference between the estimated value and the rest of the estimated values, and set the outlier removal threshold ,when This indicates that the estimated error of the label position of this group is large, so it is removed. After the elimination, the remaining label position estimate Calculate the estimated coordinates of the label based on the estimated label position: , which is the initial value; Estimate coordinates based on the obtained labels , let the real coordinates of the label be , then the positioning error equation is ,in is the error between the initial value and the true value; Establish the positioning function based on the distance formula: , To locate the projection distance between the tag and each base station in the horizontal plane, the above formula is used in Taylor expansion at: ,in , is the input initial value and the horizontal projection distance of each base station, and further, ， Then we can get ,in ; According to the least squares principle, we can get The best estimate of ; By judging the relationship between the size of the positioning error and the preset threshold, it is inferred whether the algorithm needs further iterative calculation to reduce the positioning error. If the positioning error is greater than the preset threshold, the current calculation result is used as the new initial value for calculation until the positioning error is less than the preset threshold or the preset maximum number of iterations is reached. The tag coordinate estimate is ; The measurement noise variance is added as a parameter to be estimated into the system model of the volumetric Kalman filter algorithm, the tag coordinate estimation value is used as the observation value of the volumetric Kalman filter algorithm and the tag position is smoothed; The system model is: ; in express time, and They are The measurement vector and state vector at the time instant, wherein the state vector includes the position parameters to be estimated and the measurement noise variance; and are the measurement function and the state transfer function respectively; and They are process noise and measurement noise respectively, satisfying zero-mean Gaussian distribution and are independent of each other; The improved CKF is used to further smooth the estimation results and improve the positioning accuracy. The corresponding improved CKF process is as follows: First initialize, set the initial estimate to , the corresponding estimation error covariance is ; Then the time is updated: a. Estimated error covariance matrix and Cholesky decomposition ; b. Calculate volume sampling points : ; when hour, ; in Represents volume sampling rules; c. Calculate the volume in one step : ; d. Calculate the predicted status: e. Calculate the corresponding prediction error covariance matrix: , in is the process noise covariance matrix; Finally, perform the measurement update: a. Decomposition : ; b. Calculate volume points : ; c. Calculate and measure volume points : ; d. Calculate the predicted measurement value: ; e. Calculate the innovation covariance matrix: ; f. Calculate the state measurement covariance matrix: ; g. Calculate the Kalman filter gain matrix: ; h. Calculate the updated estimate: ; i. Calculate the corresponding estimation error covariance matrix: ; Through the above CKF algorithm process, the coordinate position of the tag after smoothing can be obtained. and the corresponding estimation error covariance , and then the final positioning result can be obtained.

2. The construction worker positioning and tracking algorithm for NLOS construction sites as claimed in claim 1, characterized in that: The distance between the tag and the corresponding base station is calculated by the flight time T obtained by the 6 consecutive time stamps between the tag and the base station, and the distance is calculated based on the flight time T ,in is the propagation speed of electromagnetic waves in air.

3. The construction worker positioning and tracking algorithm for NLOS construction sites as claimed in claim 1, characterized in that: The 6 timestamps are obtained by the following method: the tag sends a ranging request message to the base station and records the sending timestamp at this time , the base station records the receiving timestamp from the tag After a certain delay, send a reply message to the tag and record the sending timestamp , the tag records the current timestamp when receiving the reply message After a delay, a distance measurement message is sent to the base station and the sending timestamp is recorded. , the base station records the receiving timestamp , and get 6 timestamps.

Images