Method, system and storage medium for analyzing and improving UWB ranging error
Technical Field
The present invention relates to the field of UWB ranging technologies, and in particular, to a method, a system, and a storage medium for analyzing and improving UWB ranging errors.
Background
In the Ultra Wide Band (UWB) based ranging positioning algorithm, it is mainly classified into a Time difference of Arrival positioning method (TIME DIFFERENCE of Arrival, TDOA) and a Time of Arrival ranging method (TOA). TDOA positioning requires that all positioning base stations maintain strict time synchronization and is difficult to implement in engineering. Thus, UWB-based positioning methods typically employ TOA ranging algorithms. Single-pass ranging, double-pass ranging, single-side two-way ranging, and double-side two-way ranging are all the most commonly used ranging protocols for TOA estimation. Among these, the most widely used industry is the Double-sided Two-way ranging algorithm (Double-sided Two-WAY RANGING). The basic principle of the algorithm is that a positioning tag and a plurality of base stations perform ranging operation (shown in fig. 1), then through triangular positioning, the real-time coordinates of the positioning tag can be calculated by calculating the length of the triangle edge to measure the angle formed by the base stations and the positioning tag point on the assumption that the positioning tag point and the base station coordinate point can form a triangle, as shown in fig. 2. The essence is to calculate the distance by measuring the Time of Flight (TOF) of the signal between the transceiving nodes multiplied by the speed of light. Meanwhile, the distance measuring two sides eliminate the distance measuring error caused by different clock errors of the two sides through the interaction of the two signals. However, in the practical application process, after the ranging error caused by the clock error of the ranging equipment is eliminated, the ranging effect is still not ideal, and the positioning accuracy range is about 40cm in the environment without the obstacle.
Through extensive testing, the main sources of error in the UWB ranging process are 1) clock drift, where different time stamps need to be recorded by the transmission of the signal. In practical situations, the existence of objective factors (such as the material technology of the device, the ambient temperature, etc.) can cause the clock frequency of the transceiver device to be slightly different, i.e. the phenomenon of clock drift, so that the time information of the signal obtained by the receiving device is inaccurate. 2) The error of the receiving and transmitting time of the read signal is that the device has an error of a white noise distribution state when the device reads the time stamp of the signal transmission and reception, and the error value is about (3E-10) seconds. If the time for a device to transmit a signal is t seconds, the signal transmission time actually read by the device may be any value between t- (3E-10) seconds and t+ (3E-10) seconds. The equipment needs at least one information interaction for one ranging, namely, respectively reads and writes 2 times. Thus, in the worst case, the error value is (3E-10) 4*C =0.36 meters. Because the existing time drift correction algorithm is based on that the equipment successfully reads the correct receiving and transmitting time, the time error generated by the read signal can cause the deviation of the clock drift correction process.
According to the analysis, the original bilateral two-way ranging algorithm can only reduce errors caused by clock drift, and deviation errors caused by errors of read signal receiving and transmitting time and deviation correction errors caused by the time deviation cannot be eliminated.
Disclosure of Invention
In view of the above, the present invention aims to provide an analysis and improvement method for UWB ranging errors, which can effectively reduce errors generated in the ranging process, thereby improving positioning accuracy.
To achieve the above object, a first aspect of the present invention provides a method for analyzing and improving UWB ranging error, comprising the steps of:
s101, carrying out n rounds of ranging signal interaction between a first ranging device and a second ranging device, wherein each round of ranging signal interaction starts by sending a message to the second ranging device by the first ranging device, and ends by receiving a message replied by the second ranging device by the first ranging device;
S102, respectively recording time points of sending and receiving messages of the first ranging equipment and the second ranging equipment;
S103, calculating a clock drift rate ratio based on the recorded time points;
S104, sequentially calculating the ranging result of each round in n rounds of signal interaction based on the clock drift rate ratio;
S105, calculating a final ranging result based on the n-round ranging result.
Further, in each round of signal interaction, both the message returned by the second ranging device received by the first ranging device and the message sent by the first ranging device received by the second ranging device are filtered by the filter.
Further, it is recorded that the first ranging device transmits messages at time points of τas1, τas2, τasn, and receives messages at time points of τar1, τar2, τArn, the second ranging device transmits messages at time points of τbs1, τbs2, τ Bsn, and receives messages at time points of τbr1, τbr2, τbrn, calculates a clock drift rate ratio based on the recorded time points, whose calculation formula is shown in formula (1),
W=(τAsn-τAs1)/(τBrn-τBr1) (1)
Wherein W is the clock drift rate ratio, and n is the number of signal interaction rounds.
Further, the calculating the result of each round of ranging in n rounds of signal interaction based on the clock drift rate ratio specifically includes:
calculating the time t roundAn elapsed from the first ranging device sending the message to the second ranging device receiving the message;
Calculating a time t replyBn from the second ranging device receiving a message sent by the first ranging device to the second ranging device replying to the message;
calculating the signal flight duration of the corresponding turn according to t roundAn、treplyBn and the clock drift rate ratio, wherein n represents the signal interaction turn;
and calculating the ranging result of the corresponding round according to the signal flight duration and the signal transmission speed.
Further, the final ranging result is calculated based on the n-round ranging result, and specifically includes the following steps:
Judging whether the number n of the signal interaction rounds is an odd number, if n is an odd number, calculating a final ranging result L by a formula (2):
L=L(n+1)/2 (2);
if n is even, the final ranging result is calculated by equation (3):
A second aspect of the present invention provides an analysis and improvement system for UWB ranging errors, the system comprising a first ranging device, a second ranging device, a recording module, a first computing module, a second computing module, and a third computing module, wherein:
The first ranging equipment is used for carrying out n rounds of signal interaction with the second ranging equipment, each round of signal interaction starts with the transmission of a message from the first ranging equipment to the second ranging equipment, and ends with the reception of a message replied by the second ranging equipment by the first ranging equipment;
The recording module is used for respectively recording the time points of the first distance measuring equipment and the second distance measuring equipment for sending and receiving the message;
The first calculation module is used for calculating a clock drift rate ratio based on the time points recorded by the recording module;
the second calculation module is used for sequentially calculating the ranging result of each round in n rounds of signal interaction based on the clock drift rate ratio;
The third calculation module is used for calculating a final ranging result based on the n-round ranging result.
A third aspect of the invention provides a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the method of the first aspect described above.
Compared with the prior art, the invention has the beneficial effects that:
1. The invention reduces the error of time drift caused by the system by carrying out multi-round ranging signal interaction between the ranging devices;
2. the method provided by the invention has the advantages that the clock drift rate ratio is calculated, the multi-round ranging result is further calculated on the basis of the clock drift rate ratio, and the final ranging result is calculated according to the multi-round ranging result, so that errors existing in the process of reading the receiving and transmitting signals by equipment are eliminated, and the positioning accuracy of the method provided by the invention is far superior to that of the traditional double-side double-distance ranging algorithm.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for the description of the embodiments will be briefly described below, and it is apparent that the drawings in the following description are only preferred embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a schematic diagram of a prior art positioning tag and multiple base station ranging networking.
Fig. 2 is a schematic diagram of the TOA positioning principle in the prior art.
Fig. 3 is a schematic overall flow chart of an analysis and improvement method of UWB ranging error according to an embodiment of the present invention.
Fig. 4 is a schematic diagram of an improved method for UWB ranging error according to an embodiment of the present invention.
Fig. 5 is a schematic diagram of an overall structure of an analysis and improvement system for UWB ranging error according to an embodiment of the present invention.
Fig. 6 is a schematic diagram of a ranging result of a one-way ranging algorithm in the prior art.
Fig. 7 is a schematic diagram of a ranging result of a two-way ranging algorithm in the prior art.
Fig. 8 is a schematic diagram of a result of ten interactive ranging according to an improved method provided by an embodiment of the present invention.
Fig. 9 is a schematic diagram of a hundred-time interactive ranging result of an improved method according to an embodiment of the present invention.
In the figure, 1a first distance measuring device, 2 a second distance measuring device, 3 a recording module, 4 a first calculating module, 5 a second calculating module and 6a third calculating module.
Detailed Description
The principles and features of the present invention are described below with reference to the drawings, the illustrated embodiments are provided for the purpose of illustrating the invention and are not to be construed as limiting the scope of the invention.
Referring to fig. 3 and 4, the present embodiment provides a method for analyzing and improving UWB ranging errors, the method comprising the steps of:
S101, the first ranging device and the second ranging device conduct n rounds of ranging signal interaction, each round of ranging signal interaction starts by sending a message to the second ranging device by the first ranging device, and ends by receiving a message replied by the second ranging device by the first ranging device.
The first ranging device may send a message to the second ranging device first, and the second ranging device may reply to the first ranging device immediately after receiving the message, and may cycle through n rounds. The messages received by the first ranging device and the second ranging device are filtered by a filter, so that excessive noise interference is filtered.
S102, respectively recording the time points of the first ranging device and the second ranging device for sending and receiving the message.
S103, calculating the clock drift rate ratio based on the recorded time points.
S104, sequentially calculating the ranging result of each round in n rounds of signal interaction based on the clock drift rate ratio.
S105, calculating a final ranging result based on the n-round ranging result.
Specifically, in step S102, the time point of each message sent by the first ranging device is recorded As1, as2, and then, as, and the time point of each message received by the first ranging device is recorded As τar1, ar2, and then, as. And sequentially recording the time points of the second ranging device sending the message each time as tau Bs1, tau Bs2, & gt, tau Bsn, and sequentially recording the time points of the second ranging device receiving the message each time as tau Br1, tau Br2, & gt, tau Brn. In step S103, a clock drift rate ratio is calculated based on the recorded time points, the calculation formula of which is shown in (1),
W=(τAsn-τAs1)/(τBrn-τBr1) (1)
Wherein W is the clock drift rate ratio, and n is the number of signal interaction rounds.
In step S104, the result of each ranging in n ranging signal interactions is sequentially calculated based on the clock drift rate ratio, and specifically includes the following steps:
The time t roundAn elapsed from the first ranging device sending the message to the second ranging device receiving the message is calculated as follows:
troundAn=τArn-τAsn
Where n represents a round, τ Arn is the point in time when the first ranging device receives the message for that round, and τ Asn is the point in time when the first ranging device sends the message for that round.
Calculating a time t replyBn from the second ranging device receiving a message sent by the first ranging device to the second ranging device replying to the message, as shown in the following equation:
treplyBn=τBsn-τBrn
where n represents the round, τ Brn is the point in time when the second ranging device receives the message for that round, and τ Bsn is the point in time when the second ranging device sends the message for that round.
The signal flight duration of the corresponding turn is calculated from T roundAn、treplyBn and the clock drift rate ratio, and the expression of the signal flight duration T tof is as follows:
Ttof=(troundAn–treplyBn*W)/2
And calculating the ranging result of the corresponding round according to the signal flight duration and the signal transmission speed, wherein the expression of the ranging result L n of the nth round is shown as follows:
Ln=Ttof*C
Wherein C represents the signal transmission speed, and in this embodiment, C takes the light speed value.
In step S105, the calculating a final ranging result based on the n-round ranging result specifically includes the following steps:
Judging whether the number n of the signal interaction rounds is an odd number, if n is an odd number, calculating a final ranging result L by a formula (2):
L=L(n+1)/2 (2)
When the number of the interactive rounds of the ranging signals is odd, the median of the multi-round ranging results is taken as the final ranging result.
If n is even, the final ranging result is calculated by equation (3):
as an alternative implementation manner, during the process of executing the method described in the foregoing embodiment, it is further detected whether the moving speed of the positioning tag itself exceeds a preset threshold, if the moving speed exceeds the preset threshold, a prompt message is sent and the execution method is terminated, because if the positioning tag itself is in a high-speed moving state, the ranging process is meaningless.
In a specific embodiment of the invention, the method is tested in a practical environment, wherein the test result is shown in table 1, when the number of distance measurement signal interactions is 10, each distance measurement takes 25 milliseconds, the positioning accuracy is within 0.05 meter, and the method is suitable for positioning the robot when the robot moves. When the interaction times of the ranging signals are 100 times, each ranging time takes 250 milliseconds, the positioning precision is within 0.03 meter, and the method is suitable for positioning the robot when the robot is stationary.
Table 1 algorithm test results provided in this example
Number of signal interactions n |
Time consuming for each distance measurement |
Precision of |
Suitable scene |
10 |
25ms |
0.05m |
Use when robot moves |
100 |
250ms |
0.03m |
Use when robot is stationary |
Referring to fig. 6 to fig. 9, the ranging results of the one-way ranging algorithm in fig. 6 and the two-way ranging algorithm in fig. 7 have larger fluctuation and larger error, and it can be seen from fig. 8 and fig. 9 that the ranging results of the method provided by the embodiment are more stable and smaller than the two algorithms, and meanwhile, the ranging results of the hundred interactions are more stable than the ranging results of the ten interactions, and it can also be seen from table 2 that the mean square deviations of the ten interactions and the hundred interactions of the improved algorithm provided by the embodiment are 0.00857 and 0.0044878 respectively, which are far smaller than the mean square deviations of the ranging results of the one-way ranging algorithm and the two-way ranging algorithm, so that the ranging results have smaller fluctuation and smaller error.
Table 2 measurement distance result value
The method provided by the embodiment successfully eliminates most of ranging errors caused by different clock errors of two sides by the interaction of multiple signals of the two sides, improves the positioning accuracy from 0.4 meter to 0.03 meter, and has larger improvement compared with the traditional double-side double-distance algorithm.
Based on the same inventive concept as the previous method embodiment, another embodiment of the present invention provides a UWB ranging error analysis and improvement system, referring to fig. 5, comprising a first ranging device 1, a second ranging device 2, a recording module 3, a first calculation module 4, a second calculation module 5, and a third calculation module 6, wherein:
The first ranging device 1 is configured to perform n rounds of signal interaction with the second ranging device 2, where each round of signal interaction starts with the first ranging device 1 sending a message to the second ranging device 2 and ends with the first ranging device 1 receiving a message replied by the second ranging device 2.
The recording module 3 is configured to record the time points when the first ranging device 1 and the second ranging device 2 send and receive messages, respectively.
The first calculating module 4 is configured to calculate a clock drift rate ratio based on the time points recorded by the recording module.
The second calculating module 5 is used for sequentially calculating the ranging result of each round in n rounds of signal interaction based on the clock drift rate ratio.
The third calculation module 6 is configured to calculate a final ranging result based on the results of the n rounds of ranging.
The system is used for implementing the method described in the foregoing method embodiment, and the working principle and technical effects of the system can refer to the foregoing method embodiment, which is not described herein again.
Another embodiment of the present invention also provides a computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, implements the method described in the foregoing method embodiments.
The foregoing description of the preferred embodiments of the invention is not intended to limit the invention to the precise form disclosed, and any such modifications, equivalents, and alternatives falling within the spirit and scope of the invention are intended to be included within the scope of the invention.