A non-line-of-sight positioning error correction method and device

Abstract

The present invention discloses a non-line-of-sight positioning error correction method and device. The present invention first uses the distance measurement value between a mobile node and multiple base stations to make a difference with the predicted value to obtain a residual; then assigns a weight to each distance measurement value according to the residual, and then solves the positioning equation group by weighted least squares method to obtain the coordinates of the mobile node; finally, Kalman filtering is performed to obtain the filtered mobile node coordinates and the next round of mobile node coordinate prediction values. The method only uses the distance measurement result, does not require the receiver to record the waveform of the signal, can avoid the problem that the signal characteristics change with the environment, has the characteristics of low calculation cost and low hardware requirements, and can be applied to indoor wireless positioning and other occasions.

Description

Technical Field

[0001] The present invention relates to wireless indoor positioning technology, and in particular to a non-line-of-sight error correction method and device for wireless indoor positioning. Background Art

[0002] The indoor wireless positioning system consists of a base station and a mobile node. The location of the base station is known. When the distance from the mobile node to multiple base stations is measured, the positioning equation can be constructed and the location of the mobile node can be obtained by solving it. To measure the distance from the mobile node to the base station, the two need to send signals to each other and record the transmission time and reception time of the signal, and then estimate the signal flight time (TOF), and multiply it by the speed of the electromagnetic wave to get the distance.

[0003] However, in indoor environments, the signal transmission path between nodes may be blocked by obstacles, and the signal passes through the obstacles at a lower speed or is reflected by other obstacles to reach the receiver. This is called the Non Line of Sight (NLOS) phenomenon. NLOS will cause the estimated value of TOF to be too large, and then the estimated value of distance to be too large, which seriously affects the positioning performance. Therefore, detecting and correcting NLOS can effectively improve the positioning accuracy.

[0004] The current NLOS detection and correction methods are mainly based on the characteristics of the received signal. Under NLOS and Line of Sight (LOS) conditions, the received signal has different parameters such as signal strength and signal duration. By collecting data and constructing a classifier, it is possible to determine whether the path is NLOS. The classifier can also further predict the size of the ranging error caused by NLOS, thereby correcting the error.

[0005] The main problems with current NLOS detection and correction are: (1) The receiver needs to accurately record the signal waveform in order to extract features, which is not possible in low-cost positioning devices; (2) The received signal features are greatly affected by the environment and the characteristics of the receiver itself, and the classifier may fail when the scene changes. Summary of the invention

[0006] In view of the shortcomings of the prior art, the present invention proposes a non-line-of-sight positioning error correction method and device.

[0007] To achieve the above technical objectives, the technical solution of the present invention is: a non-line-of-sight positioning error correction method, the method comprising the following steps:

[0008] S101: construct and initialize the Kalman filter;

[0009] S102: performing state prediction through a Kalman filter to predict the coordinates of the mobile node;

[0010] S103: Calculate the predicted distances from the mobile node to each base station based on the predicted coordinates obtained in step S102 according to the Pythagorean theorem;

[0011] S104: measuring the distance between the mobile node and each base station to obtain a distance measurement value;

[0012] S105: Subtract the distance measurement value obtained in step S104 from the distance prediction value obtained in step S103 to obtain a distance residual;

[0013] S106: Calculate the weight of each distance measurement value according to the distance residual obtained in step S105;

[0014] S107: constructing a positioning equation group according to the coordinates of each base station and the distance measurement value in step S104;

[0015] S108: using the weights of the distance measurement values ​​obtained in step S106 to weight the positioning equation group constructed in step S107 to obtain an optimization objective function, and solving the optimization objective function using the weighted least squares method to obtain the measurement value of the mobile node coordinates;

[0016] S109: The coordinate measurement value obtained in step S108 is used as the observation quantity, and error prediction, Kalman gain addition, state correction and error correction are performed through the Kalman filter to obtain a filtered state vector; the state quantity contains the mobile node coordinates, that is, the corrected mobile node positioning result.

[0017] Furthermore, in step S103, the coordinates of N base stations are known, and the coordinates of the i-th base station are denoted by (x a (i), y a (i)), the predicted coordinates of the mobile node given by step S102 The predicted distance from the mobile node to each base station is calculated based on the Pythagorean theorem. The formula is as follows:

[0018]

[0019] Furthermore, in step S104, the time-of-flight method is preferably used to measure the distance from the mobile node to each base station.

[0020] Furthermore, the calculation formula of the distance residual res(i) in step S105 is as follows:

[0021]

[0022] Among them, d m (i) is the distance measurement value obtained in step S104.

[0023] Furthermore, in step S106, the formula for calculating the weight is as follows:

[0024]

[0025] Among them, r is the judgment threshold, which is 2 to 3 times the standard deviation of the ranging error under line-of-sight conditions.

[0026] Furthermore, when the distance residual is positive, the distance measurement value is too large, it is judged as non-line-of-sight and assigned a weight less than 1; otherwise, it is judged as line-of-sight and assigned the highest weight of 1; the larger the distance residual, the larger the non-line-of-sight error, and the lower the weight.

[0027] Furthermore, the formula of the positioning equation group is as follows:

[0028]

[0029] Among them, (x, y) is the unknown quantity, that is, the coordinates of the mobile node.

[0030] Furthermore, the formula of the target optimization function in step S108 is as follows:

[0031]

[0032] Furthermore, the method further includes: repeating the above steps S102-S109, correcting the positioning error in each round, and obtaining a more accurate positioning result.

[0033] The present invention proposes a non-line-of-sight positioning error correction device, comprising one or more processors, for use in the above-mentioned non-line-of-sight positioning error correction method.

[0034] The present invention proposes a computer-readable storage medium on which a program is stored. When the program is executed by a processor, it is used to implement the above-mentioned non-line-of-sight positioning error correction method.

[0035] The beneficial effects of the present invention are as follows: the method of the present invention only utilizes the ranging results between the mobile node and the base station, does not require the receiver to record the waveform of the signal, can avoid the problem that the signal characteristics change with the environment, has the characteristics of low computational cost, low hardware requirements, and a wide range of applications, and is particularly suitable for indoor wireless positioning and other occasions. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 This is a schematic diagram of the positioning system;

[0037] Figure 2 This is a flow chart of the non-line-of-sight positioning error correction method proposed by the present invention;

[0038] Figure 3 Schematic diagram of the activation function used in the present invention;

[0039] Figure 4 A schematic diagram of the positioning error correction result using the method proposed by the present invention;

[0040] Figure 5 Schematic diagram of the device of the present invention. DETAILED DESCRIPTION

[0041] Exemplary embodiments will be described in detail herein, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present invention. Instead, they are merely examples of devices and methods consistent with some aspects of the present invention as detailed in the appended claims.

[0042] The terms used in the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The singular forms "a", "the" and "the" used in the present invention and the appended claims are also intended to include plural forms unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0043] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. In the absence of conflict, the features of the following embodiments and implementations can be combined with each other.

[0044] Indoor wireless positioning system consists of base stations and mobile nodes, such as Figure 1 As shown. The location of the base station (circle) is known, and the mobile node (triangle) moves in the venue. When the distance from the mobile node to multiple base stations is measured, the positioning equation can be constructed and solved to obtain the location of the mobile node. To measure the distance from the mobile node to the base station, it is necessary to let the two send signals to each other, and record the transmission time and reception time of the signal, and then estimate the signal flight time (Time of Flight, TOF), and multiply it by the speed of the electromagnetic wave to get the distance. In an indoor environment, the path from the mobile node to the base station (dashed line) may be blocked by obstacles (black squares), which is called the non-line of sight (NLOS) phenomenon. NLOS will cause the estimated value of TOF to be too large, and then the estimated value of the distance will be too large, which will seriously affect the positioning performance.

[0045] The least squares method is often used to solve overdetermined linear equations. The weighted least squares method assigns a weight to each equation based on the least squares method. In the positioning system, when the mobile node estimates the distance to multiple base stations, a set of equations can be constructed to calculate the coordinates of the mobile node using the weighted least squares method, and the weight can be determined based on the ranging error.

[0046] Kalman filtering is used to solve the state estimation of discrete-time linear systems. With the rapid development of computer technology and in-depth research on Kalman filters, Kalman filtering technology has now been widely used in signal and data processing in various fields. Kalman filters are represented by the following five equations:

[0047] Status prediction:

[0048] Error prediction:

[0049] Kalman gain:

[0050] Status correction:

[0051] Error correction:

[0052] Among them, the subscript k represents the current calculation result, the subscript (k-1) represents the previous calculation result; X is the state vector, and They represent the estimated value of the previous round, the predicted value of this round, and the corrected value respectively. P is the mean square error matrix, P k-1 , and P k They represent the estimated value of the previous round, the predicted value of this round and the corrected value respectively. A is the state transfer matrix; Q is the process noise matrix; R is the measurement noise matrix; C is the measurement relationship matrix; K k is the Kalman gain; Y k is the measurement vector.

[0053] Weighted least squares and Kalman filters are now widely used in wireless positioning systems. When the ranging error is a random variable with a Gaussian distribution, the weighted least squares and Kalman filters can achieve the best positioning effect. However, when there is an NLOS path, the above methods cannot eliminate the error caused by NLOS.

[0054] The present invention proposes a non-line-of-sight positioning error correction method based on residual and weighted least squares method. Figure 2 The method flow chart of the present invention specifically comprises the following steps:

[0055] In step S101, a Kalman filter is constructed to filter the position of the mobile node. The state vector is X k =[x k y k v xk v yk ] T , where x k and k represents the x and y coordinates of the mobile node, v xk and v yk Represents the speed in the x and y directions. Measurement relationship matrix C = [IO], state transfer matrix Where I is a diagonal matrix and Δt is the time interval between two positionings. Process noise matrix Where a is the estimated value of acceleration; the measurement noise matrix in and is the variance of the positioning result under LOS condition. k The initial value is X0 = [x0 y0 0 0] T , where (x0, y0) is the initial positioning result obtained using the least squares method; the initial value of the mean square error matrix P is

[0056] In step S102, the state prediction is performed by using the Kalman filter, which is used to predict the state vector of the mobile node obtained in the previous round. Predict the status of the mobile node in this round The first two items are the predicted coordinates of the mobile node

[0057] In step S103, the coordinates of N base stations are known, and the coordinates of the i-th base station are denoted by (x a (i), y a (i)) Using the predicted coordinates in step 102, the predicted distances from the mobile node to each base station are calculated according to the Pythagorean theorem. The formula is as follows:

[0058]

[0059] In step S104, the distance from the mobile node to each base station is measured based on the time of flight (TOF) method to obtain a distance measurement value d m (i), i=1, 2, ...N.

[0060] In step S105, the distance measurement value obtained in step S104 is subtracted from the distance prediction value obtained in step S103 to obtain the distance residual res(i):

[0061]

[0062] In step S106, according to the distance residual res(i) obtained in step S105, the weight w(i) of each distance measurement value is calculated using the following activation function:

[0063]

[0064] Among them, r is the judgment threshold. The function of this activation function is: when the distance residual is positive and large, it means that the distance measurement value is too large, so it is judged as NLOS and assigned a weight less than 1, and the larger the residual value, the larger the NLOS error, and the lower the weight; conversely, it is judged as LOS and assigned the highest weight 1. r can be 2 to 3 times the standard deviation σ of the distance measurement error under the line of sight (LOS) condition. According to the normal distribution law, under the LOS condition, the distance measurement error will be within the range of (-2σ, +2σ) in more than 95% of the cases, so the weight is 1.

[0065] In step S107, a positioning equation group is constructed according to the coordinates of each base station and the distance measurement value in step S104, and the formula is as follows:

[0066]

[0067] Among them, (x, y) is the unknown quantity, that is, the coordinates of the mobile node.

[0068] In step S108, the weights of the distance values ​​obtained in step S106 are used to weight the positioning equations constructed in step S107 to obtain an optimization objective function, wherein the i-th equation is assigned a weight w(i), and the optimization objective function obtained is as follows:

[0069]

[0070] Then the least square method is used to solve the optimization objective function and the measured value of the mobile node coordinates (x m ,y m ).

[0071] In step S109, the coordinate measurement value obtained in step S108 is used as the observation value, and error prediction, Kalman gain addition, state correction and error correction are performed through the Kalman filter, wherein the vector Y in the state correction k =[x m y m ] T , that is, the coordinate measurement values ​​obtained in step S107 constitute a vector. The filtered state vector is calculated The first two Represents the mobile node coordinates as the corrected mobile node positioning result.

[0072] In step S110, the above steps S102-S109 are repeated to correct the positioning error of each round and obtain a more accurate positioning result.

[0073] Example 1

[0074] The method of the present invention has been proved by experiments to be able to effectively correct the positioning error caused by non-line-of-sight. The experimental scenario is ultra-wideband indoor positioning. Figure 1 As shown in the figure, the four base stations form a rectangle with a length of 9.45m and a width of 5.40m. The mobile node moves along the straight line of y=1.20m toward the positive direction of the x-axis. The obstacle is placed at the coordinates (5m, 1.8m). According to the results of the ultra-wideband ranging experiment, the standard deviation of the ranging error under LOS conditions is σ=0.02m. Therefore, the parameter value in the activation function formula (4) is: r=2σ=0.04m, and the function graph is as follows Figure 3 As shown. Kalman filter parameters are Δt = 0.1, a = 1m / s 2 ,

[0075] The experimental results are as follows Figure 4 As shown in the figure. Due to the obstruction of obstacles, in a section at 6m, the positioning result before correction obviously deviates from the true value. However, using the correction method proposed in the present invention, the positioning result is closer to the true value. From a numerical point of view, without correction, the positioning error caused by NLOS is 0.30m. After using the correction method proposed in the present invention, the positioning error is reduced to 0.05m.

[0076] Corresponding to the above-mentioned embodiment of the non-line-of-sight positioning error correction method, the present invention also provides an embodiment of a non-line-of-sight positioning error correction device.

[0077] See also Figure 5 A non-line-of-sight positioning error correction device provided in an embodiment of the present invention includes one or more processors for implementing the non-line-of-sight positioning error correction method in the above embodiment.

[0078] The embodiments of the non-line-of-sight positioning error correction device of the present invention can be applied to any device with data processing capabilities, and the device with data processing capabilities can be a device or apparatus such as a computer. The device embodiment can be implemented through software, or through hardware or a combination of software and hardware. Taking software implementation as an example, as a device in a logical sense, it is formed by the processor of any device with data processing capabilities in which it is located reading the corresponding computer program instructions in the non-volatile memory into the internal memory for execution. From the hardware level, if Figure 5As shown in the figure, it is a hardware structure diagram of any device with data processing capability where the non-line-of-sight positioning error correction device of the present invention is located. Figure 5 In addition to the processor, memory, network interface, and non-volatile memory shown, any device with data processing capabilities in which the apparatus in the embodiments is located may also include other hardware, generally based on the actual functions of the device with data processing capabilities, which will not be described in detail.

[0079] The implementation process of the functions and effects of each unit in the above-mentioned device is specifically described in the implementation process of the corresponding steps in the above-mentioned method, and will not be repeated here.

[0080] For the device embodiment, since it basically corresponds to the method embodiment, the relevant parts can refer to the partial description of the method embodiment. The device embodiment described above is only schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of the present invention. Ordinary technicians in this field can understand and implement it without paying creative work.

[0081] An embodiment of the present invention further provides a computer-readable storage medium having a program stored thereon. When the program is executed by a processor, the non-line-of-sight positioning error correction method in the above embodiment is implemented.

[0082] The computer-readable storage medium may be an internal storage unit of any device with data processing capability described in any of the aforementioned embodiments, such as a hard disk or a memory. The computer-readable storage medium may also be any device with data processing capability, such as a plug-in hard disk, a smart media card (SMC), an SD card, a flash card, etc. equipped on the device. Furthermore, the computer-readable storage medium may also include both an internal storage unit of any device with data processing capability and an external storage device. The computer-readable storage medium is used to store the computer program and other programs and data required by any device with data processing capability, and may also be used to temporarily store data that has been output or is to be output.

[0083] The above are only specific implementation methods of the present invention, which cannot be used to limit the scope of the present invention. Equivalent changes made by ordinary technicians in this technical field based on this creation, as well as changes known to technicians in this field, should still fall within the scope of the present invention.

Claims

1. A non-line-of-sight positioning error correction method, characterized in that: The method comprises the following steps: S101: construct and initialize the Kalman filter; S102: performing state prediction through a Kalman filter to predict the coordinates of the mobile node; S103: Calculate the predicted distances from the mobile node to each base station based on the predicted coordinates obtained in step S102 according to the Pythagorean theorem; S104: measuring the distance between the mobile node and each base station to obtain a distance measurement value; S105: Subtract the distance measurement value obtained in step S104 from the distance prediction value obtained in step S103 to obtain a distance residual; S106: Calculate the weight of each distance measurement value according to the distance residual obtained in step S105; S107: constructing a positioning equation group according to the coordinates of each base station and the distance measurement value in step S104; S108: using the weights of the distance measurement values ​​obtained in step S106 to weight the positioning equation group constructed in step S107 to obtain an optimization objective function, and solving the optimization objective function using the weighted least squares method to obtain the measurement value of the mobile node coordinates; S109: The coordinate measurement value obtained in step S108 is used as the observation value, and error prediction, Kalman gain addition, state correction and error correction are performed through the Kalman filter to obtain a filtered state vector; the state vector contains the mobile node coordinates, that is, the corrected mobile node positioning result.

2. The non-line-of-sight positioning error correction method according to claim 1, characterized in that: In step S103, the coordinates of N base stations are known, and the coordinates of the i-th base station are denoted by (x a (i),y a (i)), the predicted coordinates of the mobile node given by step S102 The predicted distance from the mobile node to each base station is calculated based on the Pythagorean theorem. The formula is as follows:

3. The non-line-of-sight positioning error correction method according to claim 1, characterized in that: In step S104, the distance from the mobile node to each base station is measured using a time-of-flight method.

4. The non-line-of-sight positioning error correction method according to claim 1, characterized in that: The calculation formula of the distance residual res(i) in step S105 is as follows: Among them, d m (i) is the distance measurement value obtained in step S104.

5. The non-line-of-sight positioning error correction method according to claim 1, characterized in that: In step S106, the formula for calculating the weight is as follows: Among them, r is the judgment threshold, which is 2 to 3 times the standard deviation of the ranging error under line-of-sight conditions. When the distance residual is positive, the ranging value is too large, it is judged as non-line-of-sight and assigned a weight less than 1; otherwise, it is judged as line-of-sight and assigned the highest weight of 1; the larger the distance residual, the larger the non-line-of-sight error, and the lower the weight.

6. The non-line-of-sight positioning error correction method according to claim 1, characterized in that: The formula of the positioning equation group is as follows: Among them, (x, y) is the unknown quantity, that is, the coordinates of the mobile node.

7. The non-line-of-sight positioning error correction method according to claim 1, characterized in that: The formula of the target optimization function in step S108 is as follows:

8. The non-line-of-sight positioning error correction method according to claim 1, characterized in that: Also includes: Repeat the above steps S102-S109 to correct the positioning error in each round to obtain a more accurate positioning result.

9. A non-line-of-sight positioning error correction device, characterized in that: It includes one or more processors for implementing the non-line-of-sight positioning error correction method described in any one of claims 1-8.

10. A computer-readable storage medium having a program stored thereon, characterized in that: When the program is executed by a processor, it is used to implement the non-line-of-sight positioning error correction method described in any one of claims 1-8.

Images