A UWB three-dimensional dynamic target positioning method based on cooperative positioning

Abstract

The application discloses a kind of UWB three-dimensional dynamic target positioning method based on cooperative positioning, which comprises the following steps: setting three-dimensional layout of base station as fixed base station group, establishing space rectangular coordinate system, to carry out area division in the midpoint of each geometric boundary of positioning area;Based on unmanned aerial vehicle carrying auxiliary node, the positioning target and auxiliary node are positioned based on improved TL-LFL algorithm, the position of positioning target and auxiliary base station is initially determined, the switching of auxiliary node working state is maintained, the auxiliary node is moved to the vicinity of positioning target and keeps set distance, the auxiliary node and part of base station in fixed base station group constitute auxiliary base station group, and the positioning target is positioned based on auxiliary base station group.The application effectively improves the precision and stability of UWB positioning.

Description

Technical Field

[0001] This invention relates to the field of target localization technology, and specifically to a UWB three-dimensional dynamic target localization method based on cooperative localization. Background Technology

[0002] Currently, commonly used positioning technologies include GPS, Wi-Fi, Bluetooth, RFID, and ultrasound. GPS positioning technology uses satellite signals for location, offering wide coverage and high accuracy. However, it is affected by weather, buildings, and other factors, resulting in poor indoor positioning performance and high power consumption. Wi-Fi positioning technology uses the signal strength of wireless access points for location, offering low cost and utilization of existing network infrastructure. However, it suffers from limited accuracy, susceptibility to environmental interference, and poor network stability. Bluetooth positioning technology is a short-range wireless communication technology that uses the signal strength or direction of Bluetooth beacons for location. Its advantages include low power consumption, but its limited range makes it unsuitable for long-distance positioning. RFID (Radio Frequency Identification) positioning technology uses RFID tags and readers for data exchange and location. Its advantages include low cost, high accuracy, and no line-of-sight requirement. However, its disadvantages include short operating distance, susceptibility to interference from metal and other materials, and user privacy concerns. Ultrasonic positioning technology uses sound waves for ranging and positioning. Its advantages include high accuracy and resistance to electromagnetic interference. However, it is affected by air temperature and humidity, has slow propagation speed, and high power consumption.

[0003] Ultra Wide Band (UWB) is a novel wireless communication technology that solves major propagation problems that have plagued traditional wireless technologies for many years. Compared with traditional wireless positioning technologies, UWB positioning has advantages such as strong anti-interference performance, low cost, low power consumption, high security, and high positioning accuracy. However, the accuracy of UWB positioning is affected by a variety of factors, the most important of which are the measurement error of the UWB signal and the optimization effect of the positioning algorithm. The measurement error of the UWB signal includes signal noise, multipath effect, and non-line-of-sight effect, which can lead to inaccurate estimation of the arrival time or time difference of the UWB signal, thus affecting the calculation of distance or distance difference. To reduce the measurement error of the UWB signal, some signal processing methods can be used, such as filtering, correlation, synchronization, and calibration. The optimization effect of the positioning algorithm mainly depends on the establishment of the positioning model and the estimation of parameters. To improve the optimization effect of the positioning algorithm, some mathematical methods can be used, such as least squares, maximum likelihood, Bayesian, and Kalman filtering. Even with various signal processing or mathematical methods to improve the accuracy of UWB positioning, UWB positioning is still easily affected by distance. The wider the positioning range and the farther the signal travels, the greater the signal attenuation and noise interference, and the greater the error in UWB positioning accuracy. Summary of the Invention

[0004] To overcome the shortcomings and deficiencies of existing technologies, this invention provides a UWB 3D dynamic target localization method based on cooperative localization. This invention utilizes a dynamic weighted hybrid method to weight and measure the results of TOA verification and the improved TL-LFL algorithm (combining TOA localization and fingerprint localization). The improved TL-LFL algorithm divides indoor localization into three stages: offline data processing, localization data verification, and online localization. Each stage uses its own method to improve localization accuracy, making localization applications more widespread and effectively improving the accuracy and stability of UWB localization. The use of auxiliary nodes to dynamically control the UWB localization range effectively improves the accuracy of the UWB localization algorithm and dynamically controls the UWB localization range, enabling accurate and efficient tracking and localization of objects in 3D space.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] This invention provides a UWB three-dimensional dynamic target localization method based on cooperative localization, comprising the following steps:

[0007] Set up a three-dimensional layout of base stations as a fixed base station group, establish a spatial rectangular coordinate system, and divide the area by the midpoint of each geometric boundary of the positioning area;

[0008] Based on the auxiliary node carried by the UAV, the positioning target and the auxiliary node are located using the improved TL-LFL algorithm to initially determine the location of the positioning target and the auxiliary base station. The improved TL-LFL algorithm includes an offline data processing stage, a positioning data verification stage, and an online positioning stage.

[0009] The discrete data processing stage collects the location information of each UWB access point to form a fingerprint vector;

[0010] The positioning data verification stage obtains the preliminary positioning result of the target area based on TOA positioning, calculates the error vector of the direction of each base station pointing to the tag, accumulates the error vector to obtain the verification vector, and obtains the verified positioning result based on the verification vector.

[0011] In the online positioning stage, the fingerprint points in the dynamic intersection area formed by the error are divided into regions using the K-Means++ clustering method. Each cluster corresponds to a cluster center. The cluster center obtained by fingerprint positioning that is closest to the coordinates of the verified positioning result is selected as the fingerprint positioning result. The weight between the TOA positioning result and the fingerprint positioning result is set dynamically to obtain the positioning target position and the auxiliary node position.

[0012] Maintain the switching of auxiliary node working status, move the auxiliary node to the vicinity of the positioning target and maintain a set distance, the auxiliary node and some base stations in the fixed base station group form an auxiliary base station group, and locate the positioning target based on the auxiliary base station group.

[0013] As a preferred technical solution, a spatial rectangular coordinate system is established, and the region is divided using the midpoints of the geometric boundaries of the positioning area. Specifically, this includes:

[0014] The positioning area is a cubic region, with UWB base stations evenly distributed along the edges and vertices of the cube. A layer number marker is set as l. x , l y , l z The x, y, and z axes correspond to the x-axis, y-axis, and z-axis, respectively. The initial value is 0. Along the positive direction of each coordinate axis, the value increases by 1 for each UWB base station established. Therefore, the cube's length, width, and height are...

[0015] Let the coordinates of the base station farthest from the origin be... The coordinates of the center point of the cube are (x m y m , z m ),in

[0016] Let 0 < n i <l i i = x, y, z, n i If ∈Z, then the coordinate components of the base station at non-vertex locations are represented as:

[0017]

[0018] The cube is divided into regions based on the midpoints of its geometric boundaries.

[0019] As a preferred technical solution, the discrete data processing stage collects the positioning information of each UWB access point to form a fingerprint vector, specifically including:

[0020] The distance between the base station and the tag is obtained using a bilateral ranging method and is expressed as:

[0021] d = t p *c

[0022]

[0023] Where c is the speed of light, t pThe time of flight of the signal is represented by t1, which is the time from when the tag sends the Poll packet to when it sends the Reply packet to the receiving base station; t2, which is the time from when the base station sends the Reply packet to when it receives the tag's Finally packet; t3, which is the time from when the base station receives the tag's Poll packet to when it sends the Reply packet; and t4, which is the time from when the tag receives the base station's Reply packet to when it sends the Finally packet.

[0024] The positioning range in the spatial coordinate system is uniformly divided into several grids, with the center of each grid serving as the distance information sampling point. The coordinates of this sampling point and the ranging information from each base station constitute the location information, represented as follows:

[0025]

[0026] Among them, (x j y j , z j () represents the actual coordinates of the j-th sampling point. The ranging information from the i-th base station is received at the j-th sampling point.

[0027] As a preferred technical solution, the positioning data verification stage obtains the preliminary positioning result of the target area based on TOA positioning, calculates the error vector of the direction from each base station to the tag, accumulates the error vectors to obtain the verification vector, and obtains the verified positioning result based on the verification vector, specifically including:

[0028] Let the tag coordinates be (x, y, z), and the coordinates of the i-th base station be (x, y, z). i y i , z i Using each base station as the center of a sphere and the distance from the tag to the base station as the radius, the points where the n spheres intersect represent the tag coordinates. The coordinate relationship is expressed as:

[0029]

[0030] Subtracting the nth equation from the first n-1 equations in the system of equations yields the linearized equations:

[0031] AT=b

[0032] in:

[0033]

[0034]

[0035] T = (A T A) -1 A T b

[0036] Let S1 be the dynamically intersecting region formed by the error, and T be the TOA positioning result. Calculate the distance from point T = (x, y, z) within region S1 to each base station. Measuring each base station and tag Obtain the error vector of the direction from which each base station points to the tag. Specifically, it is expressed as follows:

[0037]

[0038]

[0039]

[0040]

[0041] Where n represents the number of base stations, and T1 represents the verified TOA positioning coordinates.

[0042] As a preferred technical solution, a dynamic weighted approach is used to set the weights between the TOA positioning results and the fingerprint positioning results to obtain the positioning target location and the auxiliary node location, specifically including:

[0043] The location result is expressed as follows:

[0044]

[0045] Where T1 represents the verified TOA positioning coordinates, T2 represents the fingerprint positioning result, and ρ1 and ρ2 represent the weight values, respectively.

[0046] As a preferred technical solution, the formula for calculating the weight value ρ1 is expressed as follows:

[0047]

[0048]

[0049] Where σ1 is the positional accuracy of the triangular UWB base station relative to the verified TOA positioning coordinates, and θ i Let be the azimuth angle between the i-th UWB base station and the verified TOA positioning coordinates.

[0050] As a preferred technical solution, the formula for calculating the weight value ρ2 is expressed as follows:

[0051]

[0052]

[0053] Where n is the number of base stations, C aσ2 represents the set of cluster fingerprint data points corresponding to the cluster center, σ2 is the accuracy of the fingerprint data point Δa in the dynamic intersection area formed by the error relative to the selected cluster center position, and T1 represents the TOA positioning coordinate point after verification.

[0054] As a preferred technical solution, maintaining the switching of the auxiliary node's working state specifically includes:

[0055] Auxiliary nodes are converted from tags to base stations, specifically including:

[0056] The microprocessor in the auxiliary node updates its operating mode and starts the receiving service to prepare to receive response data sent by the positioning target;

[0057] The auxiliary node waits for response data transmitted from the DW1000 via the SPI interface. At this time, the auxiliary node's SPI interface is in receive mode and waits to receive response data by polling.

[0058] The auxiliary node receives the response data sent by the positioning target and reads the response data from the SPI interface through the SPI data register;

[0059] The auxiliary node verifies whether the received response data frame is a Poll packet sent by the tag to ensure the accuracy of the frame sequence and complete the ranging process by checking the received response data frame.

[0060] Auxiliary nodes are converted from base stations into tags, specifically including:

[0061] The microprocessor in the auxiliary node updates the operating status, enables and initializes the tag signal in preparation for re-measuring the distance to other base stations;

[0062] The auxiliary node microprocessor uses the SPI interface to send the prepared data frame to the DW1000 via the MOSI line;

[0063] The auxiliary node remeasures the distance to other base stations and uses the new measurement results to update its own coordinate position.

[0064] As a preferred technical solution, the auxiliary node and some base stations in the fixed base station group constitute an auxiliary base station group. The positioning target is located based on the auxiliary base station group, specifically including:

[0065] The location of the target is represented as: M Q (m xq m yq m zq The auxiliary node position is represented as: M P (m xp m yp m zp );

[0066] Let the distance between the positioning target and the auxiliary node be Δr, and let Δr have three coordinate components. x , Δr y , Δr z ;

[0067] use As a judgment criterion, the target is located based on different situations, specifically as follows:

[0068] If we determine m xq <x m m yq <y m m zq <z m At this point, we take (0, 0, 0). The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0069] m xp =m xq +Δr x m yp =m yq +Δr y m zp =m zq +Δr z

[0070] A new spatial coordinate system is constructed using (0, 0, 0) as the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

[0071] If we determine m xq <x m m yq <y m m zq >z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0072] m xp =m xq +Δrx,m yp =m yq +Δr y m zp =m zq -Δr z

[0073] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. but This is the final positioning result of the target.

[0074] If we determine m xq <x m m yq >y m m zq <z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0075] m xp =m xq +Δr x m yp =m yq -Δr y m zp =m zq +Δr z

[0076] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. but This is the final positioning result of the target.

[0077] If we determine m xq <x m m yq >y m m zq >z m Take at this time The fixed base station at the location, and the auxiliary node M P(m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0078] m xp =m xq +Δr x m yp =m yq -Δr y m zp =m zq -Δr z

[0079] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. but This is the final positioning result of the target.

[0080] If we determine m xq >x m m yq <y m m zq <z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0081] m xp =m xq -Δr x m yp =m yq +Δr y m zp =m zq +Δr z

[0082] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

[0083] If we determine m xq >xm m yq <y m m zq >z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0084] m xp =m xq -Δr x m yp =m yq +Δr y m zp =m zq -Δr z

[0085] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

[0086] If we determine m xq >x m m yq >y m m zq <z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0087] m xp =m xq -Δr x m yp =m yq -Δr y m zp =m zq +Δr z

[0088] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

[0089] If we determine m xq >x m m yq >y m m zq >z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0090] m xp =m xq -Δr x m yp =m yq -Δr y m zp =m zq -Δr z

[0091] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

[0092] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0093] (1) This invention combines the advantages of the TL-LFL positioning algorithm and the dynamic weighted hybrid positioning method, and proposes a method that combines TOA positioning and fingerprint positioning, namely the improved TL-LFL algorithm. Indoor positioning is divided into three stages, including offline data processing stage, positioning data verification stage and online positioning stage. Each of the three stages uses its own method to improve positioning accuracy, making the application of positioning more extensive and effectively improving the accuracy and stability of UWB positioning.

[0094] (2) The auxiliary node of the present invention dynamically controls the UWB positioning range. Based on the working state switching function of the UWB node, the alternation between the two working states of "base station" and "tag" of the UWB node can be realized, so that the UWB auxiliary node can dynamically follow the positioning target and maintain a certain distance, thereby forming a positioning space range that is smaller than the original positioning coordinate system, so as to further reduce the transmission distance of UWB signal, reduce interference in the signal propagation process, and effectively improve the positioning accuracy and stability of the UWB positioning system.

[0095] (3) The UWB positioning technology of the present invention is a technology for ranging and positioning using ultra-wideband wireless signals. Compared with traditional wireless positioning technology, it has advantages such as high precision, high anti-interference, wide applicability, and high security. UWB positioning technology can be used indoors and outdoors without being limited by the environment. Its signal has strong penetration ability. It uses narrow pulses with a very short duration and has strong temporal and spatial resolution. The system has high multipath resolution. The whole system can make full use of the energy of the transmitted signal and can provide sub-centimeter-level positioning accuracy. It can achieve good positioning effect indoors and outdoors as well as in densely built-up areas. In addition, UWB positioning technology can provide the confidentiality of location information. The transmission power of UWB signal is low, only below 1mW, and it can spread the signal over a very wide frequency band. For general communication signals, UWB signal is similar to white noise and can be safely hidden. Attached Figure Description

[0096] Figure 1 This is a flowchart illustrating the UWB three-dimensional dynamic target localization method based on cooperative localization of the present invention.

[0097] Figure 2 This is a schematic diagram of the spatial coordinate system construction of the present invention;

[0098] Figure 3 This is a schematic diagram illustrating the localization process of the improved TL-LFL algorithm of this invention.

[0099] Figure 4 This is a flowchart illustrating the ranging process using the bilateral ranging method of this invention.

[0100] Figure 5 This is a schematic diagram illustrating the TOA positioning algorithm used in this invention. Detailed Implementation

[0101] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0102] Example

[0103] like Figure 1 As shown, this embodiment provides a UWB three-dimensional dynamic target localization method based on cooperative localization, including the following steps:

[0104] S1: Arrange the three-dimensional layout of the base stations as a fixed base station group, and divide the fixed base station group into areas on the host computer.

[0105] Specifically, a three-dimensional layout of base stations is arranged at the positioning site as a fixed base station group. The actual distance is measured and recorded. A spatial rectangular coordinate system is established on the host computer for the fixed base station group in millimeters (mm) at a scale of 1:1. The area is divided by the midpoint of each geometric boundary of the positioning area (taking a cube as an example).

[0106] like Figure 2 As shown, taking a cube as an example, the spatial coordinates are divided into eight octaves, and UWB base stations are evenly distributed along the edges and vertices of the cube. To determine the location of the target area when adjusting the positions of auxiliary nodes later, a layer number marker l is set. x , l y , l z The x, y, and z axes correspond to the x-axis, y-axis, and z-axis, respectively. The initial value is 0. Along the positive direction of each coordinate axis, the value increases by 1 for each UWB base station established. Therefore, the cube's length, width, and height are... (in the example l) x =l y =l z =3), taking the first base station as the reference point (0, 0, 0), the coordinates of the base station farthest from the origin are: The center point coordinates of the cube (x) m y m , z m ),in

[0107] Let 0 < n i <l i i = x, y, z, n i If ∈Z, then the coordinate components of the base station at non-vertex locations are represented as:

[0108]

[0109] S2: Use a small drone to carry an auxiliary node, and use a fixed base station group to locate the target and the auxiliary node using the improved TL-LFL algorithm to initially determine the location of the target and the auxiliary base station;

[0110] Specifically, this embodiment combines the advantages of the TL-LFL indoor positioning algorithm and dynamic weighted hybrid positioning to propose a method that combines TOA positioning with fingerprint positioning, namely, the improved TL-LFL positioning method. Figure 3As shown, the improved TL-LFL positioning method consists of three stages: offline data processing, positioning data verification, and online positioning. The specific implementation steps include:

[0111] S21: In the discrete data processing stage, relevant signal parameters (such as location information and distance information) of each UWB access point are collected at the reference point to form fingerprint vectors, and these vectors are stored to form a fingerprint database, as follows:

[0112] The offline database is established through sampling. The sampling phase requires three functions: selecting sampling points, collecting information, and processing it. The ranging information obtained from the sampling is stored in the database to enable data matching in the online phase. Specifically, for positioning information sampling, a bilateral ranging method is used to obtain the distance between the base station and the tag, where A represents the tag and B represents the base station. Figure 4 As shown, the ranging process includes the following steps:

[0113] 1) The tag sends a Poll packet, records the sending time t1, and opens reception;

[0114] 2) The base station turns on reception in advance, and after receiving the Poll packet, it records the arrival time t2 clearly;

[0115] 3) The base station sends a Reply packet at time t3 (t3 = t1 + t2), and waits for the data to be received after sending.

[0116] 4) The tag receives a Reply packet and records the time t4 at this moment;

[0117] 5) The base station sends the Finally packet at time t5 (t5 = t3 + t4);

[0118] 6) The base station receives the Finally packet, records the time t6, calculates the four time differences based on the above values, and then derives the signal's flight time:

[0119]

[0120] In the above formula, t1 represents the time from when the tag sends a Poll packet to when it receives a Reply packet from the base station; t2 is the time from when the base station sends a Reply packet to when it receives a Finally packet from the tag; t3 is the time from when the base station receives the tag's Poll packet to when it sends a Reply packet; and t4 is the time from when the tag receives the base station's Reply packet to when it sends a Finally packet.

[0121] The distance calculation formula is:

[0122] d = t p *c

[0123] In the formula, c is the speed of light. To ensure the validity of the data and verify the positioning accuracy of the algorithm, this embodiment first uses amplitude limiting filtering, and then uses sliding filtering to preprocess the time data of the TOA algorithm, determine a set of distance information corresponding to each sampling point, and save the data. The positioning range in the spatial coordinate system is evenly divided into several grids, and the center of each grid is used as the distance information sampling point. The actual coordinates of the sampling point and the ranging information of each base station after preprocessing together constitute a location information:

[0124]

[0125] Among them, F d The matrix F contains ranging information and physical location information received from different base stations at the sampling points. d The data within is discrete data; (x) j y j , z j () represents the actual coordinates of the j-th sampling point. The ranging information from the i-th base station is received at the j-th sampling point.

[0126] S22: During the positioning data verification stage, TOA positioning is used to obtain a rough target area, and the verification bit is used to eliminate errors caused by signal strength, indoor environment, electronic devices, etc., as follows:

[0127] The TOA (Time of Arrival) positioning algorithm is used to estimate and determine nearby reference points. The last point is set to verify the accuracy of the tag's value; this step filters and selects reference points that are far from the target node. Let the tag coordinates be (x, y, z), and the coordinates of the i-th base station be (x, y, z). i y i , z i With each base station as the center of a sphere and the distance from the tag to the base station as the radius, the points where the n spheres intersect are the tag coordinates.

[0128] In this embodiment, the coordinate relationship is expressed as:

[0129]

[0130] The system of equations is nonlinear. Subtracting the nth equation from the first n-1 equations yields the linearized equations:

[0131] AT=b

[0132] in:

[0133]

[0134]

[0135] T = (AT A) -1 A T b

[0136] In actual positioning, due to factors such as non-line-of-sight, noise, and multipath propagation, the measured distance will generally deviate from the actual value. Taking the two-dimensional plane containing three base stations as an example, for instance... Figure 5 As shown, where a, b, and c are positive integers, the dynamic intersection region formed by the error is denoted as S1, the TOA positioning result is denoted as T, and the distance measurement is as follows:

[0137]

[0138] in, It is a distance measurement vector with the direction of each base station pointing to the tag; The distance vector calculated for the coordinates of the base station and the tag; This is the error vector.

[0139] Specifically, after obtaining the TOA positioning area, the distance of point T = (x, y, z) within the S1 area to each base station is calculated. Measuring each base station and tag Then the error vector of the direction from which each base station points to the tag can be obtained. Let the verification vector be... The calculation formula is as follows:

[0140]

[0141]

[0142]

[0143]

[0144]

[0145] Where n is the number of base stations, T1 is the verified TOA positioning coordinates, σ1 is the position accuracy of the three-sided UWB base stations relative to the verified T1, and θ i Let be the azimuth angle between the i-th UWB base station and point T1.

[0146] S23: In the online positioning stage, combined with the fingerprint positioning algorithm, the K-Means++ algorithm is used to cluster fingerprint points within the area to find the cluster center that is closest to the online measured distance information. Finally, the final positioning coordinates are obtained by dynamically weighting the verified coordinates according to their dispersion. The method is as follows:

[0147] Based on the verified location data and combined with the fingerprint location algorithm, since there are points distributed far apart in the S1 region, if the coordinates are directly estimated by taking the average or by using a weighted method within the S1 region, the estimated coordinates will deviate significantly from the actual physical location. To address this, this embodiment uses the K-Means++ clustering method to divide the fingerprint points belonging to the S1 region, and uses dynamic weighting to set the weights between TOA location and fingerprint location.

[0148] Specifically, the K-means++ clustering method selects n cluster centers according to the following steps:

[0149] Step 1: Randomly select a sample from the dataset as the initial cluster center c1;

[0150] Step 2: First, calculate the shortest distance between each sample and the current cluster center (i.e., the distance to the nearest cluster center), denoted by D(x); then calculate the probability of each sample being selected as the next cluster center; finally, select the next cluster center using the roulette wheel method.

[0151] Step 3: Repeat Step 2 until a total of n cluster centers are selected;

[0152] Step 4: For each sample xi in the dataset, calculate its distance to the n cluster centers and assign it to the class corresponding to the cluster center with the smallest distance;

[0153] Step 5: For each category ci, recalculate its cluster center (i.e., the centroid of all samples belonging to that category);

[0154] Step 6: Repeat Step 4 and Step 5 until the position of the cluster center no longer changes.

[0155] Since there are relatively few points in the S1 region, the total number of clusters is set according to the characteristics of the S1 region. This reduces the amount of computation compared to performing weighted clustering calculations on all fingerprint points and makes the region division more accurate.

[0156] The K-Means++ algorithm divides several data points in region S1 into several clusters, each cluster corresponding to a cluster center. The cluster center obtained from fingerprint localization that is closest to the verification localization coordinates T1 is selected as the fingerprint localization result T2. The set of clustered fingerprint data points Δa corresponding to the cluster center is denoted as set C. a That is, Δa∈C a The positional precision of the fingerprint data point relative to the selected cluster center T2 is as follows:

[0157]

[0158] Where σ2 is the accuracy of the fingerprint data point Δa in region S1 relative to the selected cluster center T2.

[0159] The final coordinate result is estimated using a dynamic weighting method, as follows:

[0160] M is the location result to be calculated. The calculation formula is as follows:

[0161] M = (A T PA) -1 A T PL

[0162]

[0163] Where A is the coefficient matrix of the two error equations in the formula; L is the combination of fingerprint positioning result and ranging positioning result; P is the weighted combination of the two positioning results. The weight of each positioning result is the reciprocal of its accuracy. When calculating the positioning result, σ1 and σ2 are dynamically changing and will change with each calculation, which will affect the weight in P.

[0164] Final positioning result M1:

[0165]

[0166] Where ρ1 and ρ2 represent the weight values, respectively;

[0167] Then the target location M is determined. Q =(m xq m yq m zq ) and auxiliary node position M P =(m xp m yp m zp );

[0168] S3: Maintain the switching of the working state of the auxiliary node, move the auxiliary node to the vicinity of the positioning target and maintain the set distance, and then the auxiliary node and some base stations in the fixed base station group form an auxiliary base station group, forming a geometric space that includes the positioning target. Use the auxiliary base station group to locate the positioning target, thereby reducing the UWB positioning range and locating the positioning target more accurately.

[0169] Specifically, the steps for switching the working state of the auxiliary node are as follows:

[0170] S31: The transition between the "tag" and "base station" working states of the auxiliary node, specifically including:

[0171] 1) The auxiliary node is converted from a "tag" to a "base station";

[0172] Step 1: The microprocessor in the auxiliary node updates its working mode and immediately starts the receiving service to prepare to receive response data sent by the positioning target.

[0173] Step 2: The auxiliary node waits for response data transmitted from the DW1000 via the SPI interface. At this time, the auxiliary node's SPI interface is in receive mode and waits for the response data to be received by polling.

[0174] Step 3: The auxiliary node receives the response data sent by the positioning target and reads the response data from the SPI interface through the SPI data register.

[0175] Step 4: The auxiliary node checks the received response data frames to verify whether the frame is a Poll packet sent by the tag, in order to ensure the accuracy of the frame sequence and complete the ranging process.

[0176] 2) The auxiliary node is converted from a "base station" to a "tag";

[0177] Step 1: The microprocessor in the auxiliary node updates its operating status, enables and initializes the tag signal in preparation for re-measuring the distance to other base stations.

[0178] Step 2: The auxiliary node microprocessor uses the SPI interface to send the prepared data frame to the DW1000 via the MOSI (Master OutSlave In) line.

[0179] Step 3: The auxiliary node remeasures the distance between itself and other base stations and uses the new measurement results to update its own coordinate position.

[0180] Specifically, the methods for auxiliary node movement and cooperative localization are as follows:

[0181] S32. The auxiliary node moves to the vicinity of the positioning target and maintains a certain distance, forming an auxiliary base station group with the fixed base station to locate the positioning target;

[0182] The initial location M of the target is obtained based on the improved TL-LFL algorithm of S2. Q (m xq m yq m zq ) and auxiliary node position M P (m xp m yp m zp ), and the random error ΔM of the positioning results of both. Q and ΔM P Let the distance between the target and the auxiliary node be Δr, and let the three coordinate components (scalars) of Δr be Δr. x Δry Δr z Δr can be set according to the actual scenario, and is a fixed value where Δr > max{ΔM} Q ΔM P}

[0183] Combined with the variable l defined in step S1 i and n i ,as well as Figure 1 The reference scenario shown utilizes As a judgment indicator, the target M is located. Q (m xq m yq m zq It must exist. Therefore, we will discuss the following eight scenarios:

[0184] ①m xq <x m m yq <y m m zq <z m At this point, we take (0, 0, 0). The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0185] m xp =m xq +Δr x m yp =m yq +Δr y m zp =m zq +Δr z

[0186] A new spatial coordinate system is constructed using (0, 0, 0) as the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm from S2 is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

[0187] ②m xq <x m m yq <y m m zq >z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0188] m xp =m xq +Δr x m yp =m yq +Δr y m zp =m zq -Δr z

[0189] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm from step S2 is used in the new spatial coordinate system to obtain the position of the target. but This is the final positioning result of the target.

[0190] ③m xq <x m m yq >y m m zq <z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0191] m xp =m xq +Δr x m yp =m yq -Δr y m zp =m zq +Δr z

[0192] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm from step S2 is used in the new spatial coordinate system to obtain the position of the target. but This is the final positioning result of the target.

[0193] ④m xq <x m m yq >y m m z q>z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0194] m xp =m xq +Δr x m yp =m yq -Δr y m zp =m zq -Δr z

[0195] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The position of the target is obtained in the new spatial coordinate system using the improved TL-LFL algorithm from step S2. but This is the final positioning result of the target.

[0196] ⑤m xq >x m m yq <y m m zq <z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0197] m xp =m xq -Δr x m yp =m yq +Δr y m zp =m zq +Δr z

[0198] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The position of the target is obtained in the new spatial coordinate system using the improved TL-LFL algorithm from step S2. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

[0199] ⑥m xq >x m m yq <y m m zq >z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0200] m xp =m xq -Δr x m yp =m yq +Δr y m zp =m zq -Δr z

[0201] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The position of the target is obtained in the new spatial coordinate system using the improved TL-LFL algorithm from step S2. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

[0202] ⑦m xq >x m m yq >y m m zq <z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0203] m xp =m xq -Δr x m yp =m yq -Δr y m zp =m zq +Δr z

[0204] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The position of the target is obtained in the new spatial coordinate system using the improved TL-LFL algorithm from step S2. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

[0205] ⑧m xq >x m m yq >y m m zq >z m Take at this time The fixed base station at the location, and the auxiliary node M P (m xp m yp m zp To form an auxiliary base station group, the positions of the auxiliary nodes are adjusted in the original coordinate system as follows:

[0206] m xp =m xq -Δr x m yp =m yq -Δr y m zp =m zq -Δr z

[0207] and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The position of the target is obtained in the new spatial coordinate system using the improved TL-LFL algorithm from step S2. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

[0208] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A UWB three-dimensional dynamic target localization method based on cooperative localization, characterized in that, Includes the following steps: Set up a three-dimensional layout of base stations as a fixed base station group, establish a spatial rectangular coordinate system, and divide the area by the midpoint of each geometric boundary of the positioning area; Based on the auxiliary node carried by the UAV, the positioning target and the auxiliary node are located using the improved TL-LFL algorithm to initially determine the location of the positioning target and the auxiliary base station. The improved TL-LFL algorithm includes an offline data processing stage, a positioning data verification stage, and an online positioning stage. The offline data processing stage collects the location information of each UWB access point to form a fingerprint vector; The positioning data verification stage obtains the preliminary positioning result of the target area based on TOA positioning, calculates the error vector of the direction of each base station pointing to the tag, accumulates the error vector to obtain the verification vector, and obtains the verified positioning result based on the verification vector. In the online positioning stage, the fingerprint points in the dynamic intersection area formed by the error are divided into regions using the K-Means++ clustering method. Each cluster corresponds to a cluster center. The cluster center obtained by fingerprint positioning that is closest to the coordinates of the verified positioning result is selected as the fingerprint positioning result. The weight between the TOA positioning result and the fingerprint positioning result is set dynamically to obtain the positioning target position and the auxiliary node position. Maintain the switching of auxiliary node working status, move the auxiliary node to the vicinity of the positioning target and maintain a set distance, the auxiliary node and some base stations in the fixed base station group form an auxiliary base station group, and locate the positioning target based on the auxiliary base station group.

2. The UWB three-dimensional dynamic target localization method based on cooperative localization according to claim 1, characterized in that, Establish a spatial rectangular coordinate system, and divide the region using the midpoints of the geometric boundaries of the location area. Specifically, this includes: The positioning area is a cubic region, with UWB base stations evenly distributed along the edges and vertices of the cube, and layer number markers are set. , respectively corresponding axis, axis, The axes are initially set to 0. The value increases by 1 for each UWB base station along the positive direction of each coordinate axis. Therefore, the cube's length, width, and height are... , , ; Let the coordinates of the base station farthest from the origin be... The coordinates of the center point of the cube are ,in , , ; set up Then the coordinate components of the base station at non-vertex locations are represented as: , , ； The cube is divided into regions based on the midpoints of its geometric boundaries.

3. The UWB three-dimensional dynamic target localization method based on cooperative localization according to claim 1, characterized in that, The offline data processing stage collects the location information of each UWB access point to form a fingerprint vector, specifically including: The distance between the base station and the tag is obtained using a bilateral ranging method and is expressed as: ； ； in, At the speed of light, Indicates the signal's flight time. This indicates the time from when the tag sends a Poll packet to when it sends a Reply packet to the receiving base station. This indicates the time from when the base station sends a Reply packet to when the receiving tag receives a Final packet. This indicates the time from when the base station receives the Poll packet from the tag to when it sends the Reply packet. This indicates the time from when the tag receives the Reply packet from the base station to when it sends the Finally packet; The positioning range in the spatial coordinate system is uniformly divided into several grids, with the center of each grid serving as the distance information sampling point. The coordinates of this sampling point and the ranging information from each base station constitute the location information, represented as follows: ； in, For the first The actual coordinates of each sampling point For the first The sampling point receives the first... Ranging information for each base station.

4. The UWB three-dimensional dynamic target localization method based on cooperative localization according to claim 1, characterized in that, The TOA positioning result and fingerprint positioning result are dynamically weighted to obtain the positioning target location and auxiliary node location, specifically including: The location result is expressed as follows: ； in, This indicates the verified TOA location coordinates. This indicates the fingerprint location result. , These represent the weight values.

5. The UWB three-dimensional dynamic target localization method based on cooperative localization according to claim 4, characterized in that, weight value The calculation formula is expressed as: ； ； in, This refers to the positional accuracy of the three-sided UWB base station relative to the verified TOA positioning coordinates. Let be the azimuth angle between the i-th UWB base station and the verified TOA positioning coordinates.

6. The UWB three-dimensional dynamic target localization method based on cooperative localization according to claim 4, characterized in that, weight value The calculation formula is expressed as: ； ； in, For the number of base stations, This represents the set of cluster fingerprint data points corresponding to the cluster centers. Fingerprint data points within the dynamically intersecting region formed by errors Compared to the precision of selecting cluster center locations, This indicates the TOA location coordinates after verification.

7. The UWB three-dimensional dynamic target localization method based on cooperative localization according to claim 1, characterized in that, Maintaining the switching of auxiliary node working states specifically includes: Auxiliary nodes are converted from tags to base stations, specifically including: The microprocessor in the auxiliary node updates its operating mode and starts the receiving service to prepare to receive response data sent by the positioning target; The auxiliary node waits for response data transmitted from the DW1000 via the SPI interface. At this time, the auxiliary node's SPI interface is in receive mode and waits to receive response data by polling. The auxiliary node receives the response data sent by the positioning target and reads the response data from the SPI interface through the SPI data register; The auxiliary node verifies whether the received response data frame is a Poll packet sent by the tag to ensure the accuracy of the frame sequence and complete the ranging process by checking the received response data frame. Auxiliary nodes are converted from base stations into tags, specifically including: The microprocessor in the auxiliary node updates the operating status, enables and initializes the tag signal in preparation for remeasuring the distance to other base stations; The auxiliary node microprocessor uses the SPI interface to send the prepared data frame to the DW1000 via the MOSI line; The auxiliary node remeasures the distance to other base stations and uses the new measurement results to update its own coordinate position.

8. The UWB three-dimensional dynamic target localization method based on cooperative localization according to claim 1, characterized in that, Auxiliary nodes, together with some base stations in the fixed base station group, constitute an auxiliary base station group. The positioning target is located based on this auxiliary base station group, specifically including: The location of the target is represented as follows: The auxiliary node positions are represented as follows: ; Set the distance between the positioning target and the auxiliary node as follows: , The three coordinate components are ; use As a judgment criterion, the target is located based on different situations, specifically as follows: If the judgment Take at this time , , , Fixed base stations at specific locations, and auxiliary nodes To form an auxiliary base station group, the positions of the auxiliary nodes in the original coordinate system are adjusted as follows: ； and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target. If the judgment Take at this time , , , Fixed base stations at specific locations, and auxiliary nodes To form an auxiliary base station group, the positions of the auxiliary nodes in the original coordinate system are adjusted as follows: ； and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. ,but This is the final positioning result of the target. If the judgment Take at this time , , , Fixed base stations at specific locations, and auxiliary nodes To form an auxiliary base station group, the positions of the auxiliary nodes in the original coordinate system are adjusted as follows: ； and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. ,but This is the final positioning result of the target. If the judgment Take at this time , , , Fixed base stations at specific locations, and auxiliary nodes To form an auxiliary base station group, the positions of the auxiliary nodes in the original coordinate system are adjusted as follows: ； and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. ,but This is the final positioning result of the target. If the judgment Take at this time , , , Fixed base stations at specific locations, and auxiliary nodes To form an auxiliary base station group, the positions of the auxiliary nodes in the original coordinate system are adjusted as follows: ； and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target. If the judgment Take at this time , , , Fixed base stations at specific locations, and auxiliary nodes To form an auxiliary base station group, the positions of the auxiliary nodes in the original coordinate system are adjusted as follows: ； and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target. If the judgment Take at this time , , , Fixed base stations at specific locations, and auxiliary nodes To form an auxiliary base station group, the positions of the auxiliary nodes in the original coordinate system are adjusted as follows: ； and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target. If the judgment Take at this time , , , Fixed base stations at specific locations, and auxiliary nodes To form an auxiliary base station group, the positions of the auxiliary nodes in the original coordinate system are adjusted as follows: ； and with A new spatial coordinate system is constructed based on the reference point. The coordinate axes of the new spatial coordinate system are in the same direction as those of the original spatial coordinate system. The improved TL-LFL algorithm is used in the new spatial coordinate system to obtain the position of the target. Since the new spatial coordinate system coincides with the original spatial coordinate system, then This is the final positioning result of the target.

Images