UWB positioning method and system with mobile anchor nodes in tunnel

Abstract

The invention discloses a UWB positioning method and system with mobile anchor nodes in a tunnel, and solves the problems of many positioning base stations, high cost, inconvenient installation in the tunnel and the like in the existing UWB positioning technology. In order to reduce the problem that multiple anchor nodes and multiple DS-TWR distance measurement are needed in positioning, a PDOA method is added in the anchor nodes to measure the angle between two nodes, so that the coordinate position of the measured node is determined according to the distance and the angle, and positioning is realized; in order to further improve the positioning precision, a combined positioning method of mobile anchor node assisted positioning is adopted; in order to solve the positioning real-time performance of the same to-be-tested label node in a time period between two UWB positioning, an IMU sensing module is added in the to-be-tested label node, and the UWB positioning information and IMU information are fused to reduce the positioning error fluctuation and improve the real-time performance. Compared with a traditional positioning system based on multiple UWB base stations, the number of the positioning base stations is effectively reduced, and the construction cost of the positioning system is reduced.

Description

Technical Field

[0001] The present invention belongs to the technical field of UWB positioning, and particularly relates to a UWB positioning method and system with mobile anchor nodes in a tunnel. Background Art

[0002] Tunnel construction is the most important link in the construction of urban subways, highways, high-speed railways, etc. With the increasing diversity of tunnel construction regions and environments, tunnel construction poses higher requirements for the safety of construction personnel and equipment. In tunnel construction, the positioning of construction personnel and mobile equipment is very important for strengthening safety management. However, in tunnel interiors, construction personnel and equipment cannot use the GNSS global navigation satellite system. Unlike ground positioning where the GNSS global navigation satellite system can be used for positioning and navigation, an autonomous positioning system has to be developed. The UWB positioning method is an important indoor positioning method. It is convenient to deploy and can achieve centimeter-level positioning accuracy, which can meet the positioning requirements in tunnel construction. In UWB positioning, double side-two way ranging (DS-TWR) is a basic positioning method, but it requires 4 fixed anchor nodes. After the tag node to be measured communicates and ranges with the 4 fixed anchor nodes, the coordinate position of the tag node is calculated through a ranging positioning algorithm. In distance-based positioning methods, one positioning requires 4 DS-TWR ranging operations, with a long positioning period, a relatively small number of tag node positions that can be achieved per unit time, and high energy consumption for a single positioning. Summary of the Invention

[0003] The object of the present invention is to overcome the deficiencies of the prior art and propose a UWB positioning method and system with mobile anchor nodes in a tunnel, so as to solve the problems existing in the existing UWB positioning technology, such as a large number of positioning base stations, high costs, and inconvenient installation in tunnels. In order to reduce the problems of multiple anchor nodes and multiple DS-TWR ranging required in positioning, the present invention measures the angle between two nodes by adding the method of phase difference of arrival (PDOA) to the anchor nodes, so as to determine the coordinate position of the measured node according to the distance and angle, and achieve positioning. This DS-TWR plus PDOA positioning method only requires one multifunctional anchor node, reduces the number of communications, reduces the energy consumption of the tag nodes, and increases the coverage density of the tag nodes. However, generally, the PDOA angle measurement method has an error of ±5°. When the distance between the tag node and the positioning anchor node is relatively far, the positioning error of the tag node is relatively large. In order to further improve the positioning accuracy, a combined positioning method with mobile anchor node assisted positioning is adopted; in order to solve the real-time positioning of the tag node to be measured during the time period between two UWB positionings of the same tag node to be measured, an IMU sensing module is added to the tag node to be measured, and the positioning error fluctuation and real-time performance are reduced by the method of fusing UWB positioning information and IMU information.

[0004] The object of the present invention is achieved by the following technical solutions: a UWB positioning method with mobile anchor nodes in a tunnel. First, three types of communication nodes, namely fixed anchor nodes, mobile anchor nodes, and tag nodes to be measured, are set. The fixed anchor nodes are installed on both sides or the top of the tunnel, the mobile anchor nodes are installed on moving engineering vehicles, and the tag nodes to be measured are installed on moving objects to be positioned; among them, the fixed anchor nodes are integrated with DS-TWR modules, PDOA modules, and WIFI modules; the mobile anchor nodes are integrated with DS-TWR modules, PDOA modules, WIFI modules, and sensor modules. The mobile anchor nodes communicate with millimeter-wave radars and image sensors installed on the engineering vehicles to obtain real-time positions; the tag nodes to be measured are integrated with DS-TWR modules and IMU sensing modules, and the IMU sensing module is used to extend the positioning period of the tag nodes to be measured themselves and maintain the positioning accuracy.

[0005] The tag node to be measured communicates with the fixed anchor node through DS-TWR ranging to measure the angle between the tag node to be measured and the fixed anchor node by PDOA, so as to calculate the coordinate position of the tag node to be measured according to the measured distance and angle. At the same time, if the mobile anchor node is within the UWB communication range, the mobile anchor node is used for assisted positioning. The mobile anchor node does not communicate with the tag node to be measured for ranging. By listening to the DS-TWR ranging communication between the tag node to be measured and the fixed anchor node, it calculates the incident angle of the PDOA module of the tag node to be measured relative to the mobile anchor node, and converts this incident angle into the angle in the coordinate system of the mobile anchor node itself and provides it to the fixed anchor node. The fixed anchor node realizes the precise positioning of the tag node to be measured according to the angle data. If the mobile anchor node is not within the UWB communication range, the mobile anchor node is not used for assisted positioning.

[0006] Further, the method includes the following steps:

[0007] a. The tag node to be measured sends a Poll frame. The DS-TWR module and PDOA module of the fixed anchor node receive the Poll frame. If the mobile anchor node is also within the UWB communication range, it also receives the Poll frame for PDOA measurement of the mobile anchor node. The Poll frame carries the time when the tag node to be measured sends the frame. The fixed anchor node receives the Poll frame and records the reception time of the Poll frame. After a fixed time T replyB later, the fixed anchor node sends a Resp frame;

[0008] b. The tag node to be measured receives the Resp frame for DS-TWR ranging. The Resp frame includes the reception time of the Poll frame and the transmission time of the Resp frame. The mobile anchor node within the UWB signal coverage range also receives the Resp frame for measuring the angle between the mobile anchor node and the fixed anchor node by the PDOA method;

[0009] c. After a fixed time delay T replyA by the tag node to be measured, it sends a Final frame. The content of the Final frame includes the time when the tag node to be measured receives the Resp frame and the time when the Final frame is sent. The fixed anchor node receives the Final frame and records the reception time of this frame, and measures the angle of the tag node to be measured by using the PDOA module. The mobile anchor node receives the Final frame and records the reception time of this frame, and measures the angle of the tag node to be measured by using the PDOA module;

[0010] d. The fixed anchor node calculates the distance and PDOA angle between the tag node to be measured and the fixed anchor node based on the transceiver times of each frame and the DS-TWR algorithm. Meanwhile, the mobile anchor node sends the measured PDOA angles of the tag node to be measured and the fixed anchor node, as well as its own position, to the fixed anchor node through the WIFI module. The fixed anchor node completes the integrated positioning of the tag node to be measured based on these angle and position data and notifies the tag node to be measured by sending an Ack frame.

[0011] e. The tag node to be measured receives the Ack frame data. The mobile anchor nodes within the UWB signal coverage range also receive the Ack frame data.

[0012] Furthermore, the DS-TWR ranging communication between the tag node to be measured and the fixed anchor node includes:

[0013] The DS-TWR distance calculated after the exchange time of Poll frame, Resp frame, Final frame, and Ack frame between the tag node to be measured and the fixed anchor node is shown in Formula (1):

[0014]

[0015] where ρ i is the distance between the i-th tag node to be measured and the fixed anchor node, T i is the time of flight of UWB electromagnetic wave between the i-th tag node to be measured and the fixed anchor node; C is the speed of light in air, which is a constant; T roundA is the time difference between the moment when the tag node to be measured sends the Poll frame and the moment when it receives the Resp frame, T roundB is the time difference between the moment when the fixed anchor node sends the Resp frame and the moment when it receives the Final frame, T replayA is the time difference between the moment when the tag node to be measured receives the Resp frame and the moment when it sends the Final frame, T replyB is the time difference between the moment when the fixed anchor node receives the Poll frame and the moment when it sends the Resp frame.

[0016] Furthermore, to measure the angle between the tag node to be measured and the fixed anchor node for PDOA, and thus calculate the coordinate position of the tag node to be measured based on the measured distance and angle, includes:

[0017] During the DS-TWR ranging communication process between the tag node to be measured and the fixed anchor node, the PDOA modules of the fixed anchor node and the mobile anchor node only need the UWB signal of the tag node to be measured or the fixed anchor node to calculate the incident angle β of the measured signal source relative to its PDOA module:

[0018] Using two antennas separated by a distance d, denoted as antenna A and antenna B, there is a phase difference in the electromagnetic waves received from the same signal source. The phase difference α between the electromagnetic waves received by antenna A and antenna B has a corresponding relationship with the incident angle β of the signal source relative to the two antennas, as shown in formula (2):

[0019]

[0020] where λ is the wavelength of the UWB electromagnetic wave;

[0021] After obtaining the distance and angle of the tag node to be measured relative to itself by the fixed anchor node, through coordinate transformation, the global coordinates (x i , y i ) of the tag node to be measured are calculated, as shown in formula (3):

[0022]

[0023] where θ0 is the angle of the tag node to be measured relative to the fixed anchor node, which is determined by the sum of β and the angle of the installation position of the fixed anchor node. When the fixed anchor node is installed at the top of the tunnel, it is 0°, then θ0 = β; when the fixed anchor node is installed on the left side of the tunnel, it is 90°, then θ0 = β + 90; when the fixed anchor node is installed on the right side of the tunnel, it is -90°, then θ0 = β - 90; is the declination angle of the fixed anchor node in the global coordinate system, which is determined when installing the fixed anchor node and establishing the global coordinate; using this method of combining distance measurement and angle measurement to achieve the positioning of the tag node to be measured using a single fixed anchor node.

[0024] Furthermore, when the mobile anchor node is within the UWB communication range, using the mobile anchor node for assisted positioning includes:

[0025] Within the UWB communication range, when the fixed anchor node is positioning the tag node to be measured, it can receive the angle data of an indefinite number of mobile anchor nodes. According to the number of mobile anchor nodes, it is divided into three cases: the fixed anchor node can receive the angle data of 1 mobile anchor node, the fixed anchor node can receive the angle data of 2 mobile anchor nodes, and the fixed anchor node can receive the angle data of more than 2 mobile anchor nodes; among them, when the fixed anchor node can receive the angle data of more than 2 mobile anchor nodes, it is necessary to select the angle data of the 2 mobile anchor nodes with the strongest signal intensity received by the tag node to be measured, that is, it is equivalent to the case where the fixed anchor node can receive the angle data of 2 mobile anchor nodes and then perform calculations.

[0026] Furthermore, the situation where the fixed anchor node can receive the angle data of 1 mobile anchor node includes:

[0027] When the fixed anchor node can receive the angle data of one mobile anchor node MA1 during the positioning of the tag node to be measured, that is, one mobile anchor node MA1 participates in the positioning of the tag node Tag to be measured by the fixed anchor node FA; the antenna coordinates of the mobile anchor node are determined by its installation position and millimeter-wave radar positioning, the global coordinates of the fixed anchor node FA are (x0, y0), and the global coordinates of the mobile anchor node MA1 are (x1, y1). After a DS-TWR communication process, both FA and MA1 obtain a PDOA angle θ of the tag node Tag to be measured relative to them. i Calculate the angle of the tag node to be measured relative to FA as α0, and calculate the angle of the tag node to be measured relative to MA1 as α1. In the global coordinate system, the angle of the tag node to be measured relative to the mobile anchor node where is the declination angle of the fixed anchor node and the mobile anchor node in the global coordinate system, as shown in the following formula (4):

[0028]

[0029] A set of non-homogeneous linear equations is obtained, that is, AX = b, as shown in the following formula (5):

[0030]

[0031] The additional angle measurement data will further reduce the error of single-base station positioning. The least squares method is used to solve it to minimize the error, as shown in the following formula (6):

[0032]

[0033] Integrate the angle measurement information from other mobile anchor nodes other than the fixed anchor node, and improve the positioning accuracy of the fixed anchor node for the tag node by fitting the optimal solution. Finally, the fixed anchor node returns the finally calculated global coordinates to the tag node to be measured through the Ack frame.

[0034] Furthermore, the angle data of two mobile anchor nodes that the fixed anchor node can receive includes:

[0035] When the fixed anchor node can receive the angle data of two mobile anchor nodes MA1 and MA2 during the positioning of the tag node to be measured, that is, two mobile anchor nodes MA1 and MA2 participate in the positioning of the tag node Tag to be measured by the fixed anchor node FA; the antenna coordinates of the mobile anchor node are determined by its installation position and millimeter-wave radar positioning, the global coordinates of the fixed anchor node are (x0, y0), the global coordinates of the mobile anchor node MA1 are (x1, y1), and the global coordinates of the mobile anchor node MA2 are (x2, y2). After a DS-TWR communication process, FA, MA1, and MA2 all obtain a PDOA angle θ of the tag node Tag to be measured relative to them.i , calculate the angle of the tag node to be measured relative to FA as α0, calculate the angle of the tag node to be measured relative to MA1 as α1, and calculate the angle of the tag node to be measured relative to MA2 as α2. In the global coordinate system, the angle of the tag node to be measured relative to the mobile anchor node is in is the deflection angle of the fixed anchor node and the mobile anchor node in the global coordinate system, as shown in the following formula (7):

[0036]

[0037] A set of non-homogeneous linear equations is obtained, namely AX = b, as shown in the following formula (8):

[0038]

[0039] The additional angle measurement data will further reduce the error of single base station positioning. The least square method is used to solve and minimize the error, as shown in the following formula (9):

[0040]

[0041] The angle measurement information from other mobile anchor nodes other than the fixed anchor nodes is integrated to improve the positioning accuracy of the fixed anchor nodes for the tag nodes by fitting the optimal solution. Finally, the fixed anchor nodes return the final calculated global coordinates to the measured tag nodes through Ack frames.

[0042] Furthermore, using the IMU sensor module to extend the positioning period of the tag node to be tested and maintain the positioning accuracy includes:

[0043] After the tag node is powered on, the position P obtained after the 0th time, that is, the initial UWB positioning UWB (0), with P UWB (0) As the tag node position, before the first UWB positioning k = m, the displacement of the tag node to be measured obtained by the IMU sensor module is used to correct the position of the tag node to be measured:

[0044]

[0045] In formula (10), k and m are natural numbers, m*T IMU =T UWB , where T IMU is the displacement update period of the tag node IMU, T UWB is the UWB positioning update period of the tag node and the fixed anchor node, m is the number of IMU position update periods in the UWB positioning period; k represents the current time kT IMU ;

[0046] After the UWB positioning of the tag node to be measured and the fixed anchor node is completed for the \(i\) -th time at time \(k = im\), the obtained position is \(P\) UWB (im), and meanwhile, the IMU sensing module of the tag node to be measured has a displacement \(\Delta P\) IMU (im) from time \((i - 1)m\) to time \(im\). Combining the position of UWB and the IMU displacement, the position \(P(im)\) of the tag node is obtained according to formula (11):

[0047]

[0048] In formula (11), \(i\) is the number of UWB positioning times, \(\Delta P\) UWB (im|(i - 1)m) is the displacement of the tag node in two UWB positionings at time \((i - 1)m\) and time \(im\), and \(\Delta P\) IMU (im|(i - 1)m) is the cumulative IMU displacement of the tag node from time \((i - 1)m\) to time \(im\), that is, \(\Delta P\) UWB (im|(i - 1)m)=P UWB (im)-P UWB ((i - 1)m) and \(\Delta P\) IMU (im|(i - 1)m)=P IMU (im)-P IMU ((i - 1)m); \(\alpha\) UWB is the weighting coefficient for synthesizing the UWB displacement, and \(\alpha\) IMU is the weighting coefficient for synthesizing the IMU position. There is \(\alpha\) UWB >0, \(\alpha\) IMU >0 and \(\alpha\) UWB +\(\alpha\) IMU =1; when \(|\Delta P\) UWB (im|(i - 1)m)|\) and \(|\Delta P\) IMU (im|(i - 1)m)|\) differ greatly, the position of the tag node to be measured is determined by the UWB positioning. When \(|\Delta P\) UWB (im|(i - 1)m)|\) and \(|\Delta P\) IMU (im|(i - 1)m)|\) do not differ much, that is, \(|\Delta P\) UWB (im|(i - 1)m)|\(\leq1.2|\Delta P\) IMU (im|(i - 1)m)|\), the position of the tag node to be measured is determined by synthesizing the UWB displacement and the IMU displacement;

[0049] Between two UWB positioning moments, i.e., k ∈ ((i - 1)m, im), learn the comparison between the displacements of the (i - 2)-th and (i - 1)-th UWB positioning and the IMU displacements at the corresponding moments, and correct the position of the to-be-measured tag node through the IMU's IMU periodic displacement, where m is the number of IMU position update periods contained in the UWB positioning period, i is the number of UWB positioning times since power-on, and i = 0 at the first time; Formulas (12) and (13) are the positioning calculations of the tag node between two UWB positioning moments:

[0050]

[0051] P(k) = P((i - 1)m) + β(i)△P IMU (k|(i - 1)m), (i - 1)m < k ≤ im, i ∈ N, i > 1. (13);

[0052] In Formulas (12) and (13), △P UWB ((i - 1)m|(i - 2)m) is the displacement between the (i - 2)-th UWB positioning P UWB ((i - 2)m) and the (i - 1)-th positioning P UWB ((i - 1)m); △P((i - 1)m|(i - 2)m) is the displacement between the (i - 2)-th comprehensive positioning P((i - 2)m) and the (i - 1)-th comprehensive positioning P((i - 1)m) of the to-be-measured tag node; △P IMU ((i - 1)m|(i - 2)m) is the displacement between the (i - 2)-th positioning P IMU ((i - 2)m) and the (i - 1)-th positioning P IMU ((i - 1)m) of the IMU;

[0053] In Formula (12), the initial value of β(i) is 1 (i = 0, 1). When updating β(i), judge whether the condition |△P UWB ((i - 1)m|(i - 1)m)| ≤ 1.2|△P IMU (im|(i - 1)m)| holds. If it holds, update it; otherwise, use the previous value β(i - 1); Through Formula (13), the to-be-measured tag node also obtains position update data during two UWB positionings, and the correction amount △P IMU (k|(i - 1)m) is adjusted by the β(i) coefficient to reduce the cumulative error generated by the IMU; At the same time, through the validity condition generated by comparing the UWB positioning displacement data with the IMU displacement data, the tag node positioning data is corrected at the UWB positioning moment according to Formula (11) and corrected between two UWB positioning moments according to Formula (13).

[0054] A UWB positioning system with mobile anchor nodes in a tunnel, which is used to implement the above-mentioned UWB positioning method with mobile anchor nodes in the tunnel. The system includes fixed anchor nodes, mobile anchor nodes and mobile anchor nodes:

[0055] The fixed anchor node is integrated with a DS-TWR module, a PDOA module, a WIFI module, an MCU circuit module and a UPS power supply circuit module;

[0056] The mobile anchor node is integrated with a DS-TWR module, a PDOA module, a WIFI module, a sensor module, an MCU circuit module and a UPS power supply circuit module;

[0057] The tag node to be measured is integrated with a DS-TWR module, an IMU sensing module, an MCU circuit module and a battery circuit module.

[0058] Furthermore, the sensor module is communicatively connected to a millimeter-wave radar, an image sensor and a 3D vision device.

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

[0060] 1. The present invention combines the DS-TWR and PDOA technologies of UWB, can complete the positioning of the tag node based on a single anchor node, and PDOA does not require additional communication messages, only needs to listen to the data packet during the DS-TWR process, without increasing the communication burden. Compared with the traditional multi-UWB base station positioning system, the number of positioning base stations is effectively reduced, and the construction cost of the positioning system is reduced.

[0061] 2. The present invention proposes a method of adding mobile anchor nodes to assist in positioning. The mobile anchor node listens to the DS-TWR ranging communication signal between the tag node and the fixed anchor node, and applies the PDOA technology to provide the fixed anchor node with the angle information between the tag node and the mobile anchor node, helping the fixed anchor node to more accurately position the tag node.

[0062] 3. The tag node extends the DS-TWR ranging period in the UWB positioning process of the tag node through the UWB and IMU positioning data fusion technology, improving the deployment density of the tag node and the real-time performance of positioning.

[0063] 4. The UWB and IMU integrated positioning algorithm designed by the present invention has low computing power requirements and is suitable for implementation on a low-end single-chip microcomputer of the tag node; under the condition of maintaining the same positioning accuracy, the integrated positioning algorithm reduces the number of UWB positioning times per unit time, reduces power consumption, and can also ensure the real-time performance of positioning, providing real-time and continuous position data for the tag node. Description of the Drawings

[0064] Figure 1It is a positioning activity diagram of a tag node with a fixed anchor node and a mobile anchor node.

[0065] Figure 2 It is a schematic diagram of positioning between a tag node and a fixed anchor node based on DS-TWR and PDOA.

[0066] Figure 3 It is a schematic diagram of tag node positioning assisted by a mobile anchor node.

[0067] Figure 4 It is a schematic diagram of positioning assisted by different numbers of mobile anchor nodes in practice.

[0068] Figure 5 It is a schematic diagram of the structures of a fixed anchor node, a mobile anchor node, and a tag node. Detailed implementation manners

[0069] Example 1

[0070] The UWB positioning method with a mobile anchor node in a tunnel provided in this example first sets three types of communication nodes: a fixed anchor node, a mobile anchor node, and a tag node to be measured. The fixed anchor nodes are installed on both sides or the top of the tunnel, the mobile anchor nodes are installed on moving engineering vehicles, and the tag nodes to be measured are installed on the helmets worn by personnel and on moving objects that need to be positioned. Among them, the fixed anchor node is integrated with a DS-TWR module, a PDOA module, and a WIFI module; the mobile anchor node is integrated with a DS-TWR module, a PDOA module, a WIFI module, and a sensor module. The mobile anchor node communicates with the millimeter-wave radar and image sensor installed on the engineering vehicle to obtain the real-time position. The tag node to be measured is integrated with a DS-TWR module and an IMU sensing module, and the IMU sensing module is used to extend the positioning period of the tag node to be measured itself and maintain the positioning accuracy. The tag node to be measured performs DS-TWR ranging communication with the fixed anchor node, measures the angle between the tag node to be measured and the fixed anchor node through PDOA, and thus calculates the coordinate position of the tag node to be measured according to the measured distance and angle. At the same time, if the mobile anchor node is within the UWB communication range, the mobile anchor node is used for assisted positioning. The mobile anchor node does not perform ranging communication with the tag node to be measured. By listening to the DS-TWR ranging communication between the tag node to be measured and the fixed anchor node, it calculates the incident angle of the PDOA module of the tag node to be measured relative to the mobile anchor node, and converts this incident angle into an angle relative to the coordinate system of the mobile anchor node itself and provides it to the fixed anchor node. The fixed anchor node realizes the precise positioning of the tag node to be measured according to the angle data. If the mobile anchor node is not within the UWB communication range, the mobile anchor node is not used for assisted positioning.

[0071] See Figure 1As shown in the figure, the communication process between the fixed anchor node, the mobile anchor node and the tag node to be measured includes the following steps: Step311: The tag node to be measured sends a Poll frame; Step321: The DS-TWR module and the PDOA module of the fixed anchor node receive the Poll frame; Step331: If the mobile anchor node is also within the UWB communication range and also receives the Poll frame, it is used for PDOA measurement of the mobile anchor node; Step322: The Poll frame carries the time when the tag node to be measured sends the frame. The fixed anchor node receives the Poll frame and records the Poll frame reception time. After a fixed time TreplyB, the fixed anchor node sends a Resp frame; Step312: The tag node to be measured receives the Resp frame for DS-TWR ranging. The Resp frame includes the reception time of the Poll frame and the transmission time of the Resp frame; Step332: The mobile anchor node within the UWB signal coverage range also receives the Resp frame for measuring the angle between the mobile anchor node and the fixed anchor node by the PDOA method; Step313: After a fixed time delay TreplyA, the tag node to be measured sends a Final frame. The content of the Final frame includes the time when the tag node to be measured receives the Resp frame and the time when the Final frame is sent; Step323: The fixed anchor node receives the Final frame and records the reception time of the frame, and uses the PDOA module to measure the angle of the tag node to be measured; Step333: The mobile anchor node receives the Final frame and records the reception time of the frame, and uses the PDOA module to measure the angle of the tag node to be measured; Step324: The fixed anchor node calculates the distance and the PDOA angle between the tag node to be measured and the fixed anchor node based on the transceiver times of each frame and according to the DS-TWR algorithm. At the same time, the mobile anchor node sends the measured PDOA angles of the tag node to be measured and the fixed anchor node and its own position to the fixed anchor node through the WIFI module. The fixed anchor node completes the comprehensive positioning of the tag node to be measured based on these angle and position data and notifies the tag node to be measured by sending an Ack frame; Step314: The tag node to be measured receives the Ack frame data; Step334: The mobile anchor node within the UWB signal coverage range also receives the Ack frame data. All mobile anchor nodes can monitor the DS-TWR communication process between the tag node and the fixed anchor node, calculate the incident angle of the tag node relative to the PDOA module of the mobile anchor node and convert it into the angle θ in the coordinate system of the mobile anchor node itself, and provide it to the fixed anchor node. The fixed anchor node locates the tag node more accurately based on the new angle data of the mobile anchor node for the tag node. i , which is provided to the fixed anchor node. The fixed anchor node locates the tag node more accurately based on the new angle data of the mobile anchor node for the tag node.

[0072] The DS-TWR distance calculated after the exchange time of the Poll frame, Resp frame, Final frame and Ack frame between the tag node to be measured and the fixed anchor node is shown in formula (1):

[0073]

[0074] where ρ i is the distance between the i-th tag node to be measured and the fixed anchor node, and T i is the flight time of the UWB electromagnetic wave between the i-th tag node to be measured and the fixed anchor node; C is the speed of light in air, which is a constant; T roundA is the time difference between the moment when the tag node to be measured sends a Poll frame and the moment when it receives a Resp frame, and T roundB is the time difference between the moment when the fixed anchor node sends a Resp frame and the moment when it receives a Final frame, and T replayA is the time difference between the moment when the tag node to be measured receives a Resp frame and the moment when it sends a Final frame, and T replyB is the time difference between the moment when the fixed anchor node receives a Poll frame and the moment when it sends a Resp frame.

[0075] During the DS-TWR ranging communication process between the tag node to be measured and the fixed anchor node, the PDOA modules of the fixed anchor node and the mobile anchor node only need the UWB signal of the tag node to be measured or the fixed anchor node, and calculate the incident angle β of the measured signal source relative to its PDOA module:

[0076] See Figure 2 As shown, using two antennas separated by a distance d, denoted as antenna A and antenna B, there is a phase difference in the electromagnetic waves received from the same signal source. The phase difference α between the electromagnetic waves received by antenna A and antenna B has a corresponding relationship with the incident angle β of the signal source relative to the two antennas, as shown in formula (2):

[0077]

[0078] where λ is the wavelength of the UWB electromagnetic wave;

[0079] See Figure 3 As shown, after the fixed anchor node obtains the distance and angle of the tag node to be measured relative to itself, using coordinate transformation, the global coordinates (x i , y i ) of the tag node to be measured are calculated, as shown in formula (3):

[0080]

[0081] where θ0 is the angle of the tag node to be measured relative to the fixed anchor node, which is determined by the sum of β and the angle of the fixed anchor node installation position. When the fixed anchor node is installed at the top of the tunnel, it is 0°, then θ0 = β; when the fixed anchor node is installed on the left side of the tunnel, it is 90°, then θ0 = β + 90; when the fixed anchor node is installed on the right side of the tunnel, it is -90°, then θ0 = β - 90; is the deflection angle of the fixed anchor node in the global coordinate system, which is determined when the fixed anchor node is installed and the global coordinate is established; the ranging and angle measurement combination method is used to realize the positioning of the tag node to be measured using a single fixed anchor node.

[0082] See Figure 3 As shown, when the fixed anchor node can receive the angle data of two mobile anchor nodes MA1 and MA2 during the positioning of the tag node to be measured, that is, the mobile anchor nodes MA1 and MA2 are installed at the front and rear of an engineering vehicle respectively to participate in the positioning of the tag node Tag by the fixed anchor node FA; the antenna coordinates of the mobile anchor node are determined by its installation position and millimeter-wave radar positioning, the global coordinates of the fixed anchor node are (x0, y0), the global coordinates of the mobile anchor node located at the front of the vehicle are (x1, y1), and the global coordinates of the mobile anchor node located at the rear of the vehicle are (x2, y2). After a DS-TWR communication process, FA, MA1, and MA2 all obtain a PDOA angle θ of the tag node to be measured relative to them i , calculate the angle of the tag node to be measured relative to FA as α0, calculate the angle of the tag node to be measured relative to MA1 as α1, calculate the angle of the tag node to be measured relative to MA2 as α2. In the global coordinate system, the angle of the tag node to be measured relative to the mobile anchor node where is the deflection angle of the fixed anchor node and the mobile anchor node in the global coordinate system, as shown in the following formula (4):

[0083]

[0084] A set of non-homogeneous linear equations is obtained, that is, AX = b, as shown in the following formula (5):

[0085]

[0086] The additional angle measurement data will further reduce the error of single-base station positioning. The least squares method is used to solve it and minimize the error, as shown in the following formula (6):

[0087]

[0088] In practice, when the fixed anchor node is positioning the tag node, it may receive angle data from an indefinite number of mobile anchor nodes, such as Figure 4 shown. Figure 4 (a) is the case where no mobile anchor node participates in the auxiliary positioning. At this time, formula (4) only contains two equations (4a) and (4b), and the coordinate solution of the tag node is equivalent to formula (3); Figure 4(b) There is a mobile anchor node and an auxiliary fixed anchor node for positioning. At this time, Equation (4) reduces one dimension accordingly and includes three equations: (4a), (4b), and (4c). The least squares method is also used to fit the optimal solution.

[0089]

[0090] Figure 4 (c) The situation shown is equivalent Figure 3 ; as Figure 4 (d) As shown in (d), when there are two or more engineering vehicles, the fixed anchor node can receive angle data from 4 or more mobile anchor nodes, and 6 or more equations can be listed. Limited by the computing power and resource constraints of the embedded device, the calculation and solution of higher dimensions require a large increase in computing power but have little obvious effect on further improving the accuracy. In practice, when there are more than 2 mobile anchor node data, only 2 of them are selected, and then calculated according to Equations (4), (5), and (6). The selection is based on the received signal strength of the UWB signal of the tag node received by the mobile anchor node, and the angle data of the two mobile anchor nodes with the largest received signal strength of the tag node are selected. By synthesizing the angle measurement information from other mobile anchor nodes other than the fixed anchor node, the positioning accuracy of the fixed anchor node for the tag node is improved by fitting the optimal solution. Finally, the fixed anchor node returns the finally calculated global coordinates to the measured tag node through the Ack frame.

[0091] The UWB positioning process of the tag node is initiated by the tag node to be measured itself, and its positioning period or frequency is determined by itself. A fixed anchor node needs to exchange 4 communication data packets to locate a tag node to be measured. At the same time, it may also need to communicate with surrounding mobile anchor nodes to obtain the auxiliary positioning angle data of the tag node to be measured. Therefore, a fixed anchor node needs more communication resources (including UWB communication and WIFI communication) and computing resources to locate a tag node to be measured. The tag node is powered by a battery. When the positioning period is long, it can save the electrical energy of the tag node and increase the density of tag nodes located in the same area. However, too long a positioning period will reduce the positioning accuracy of the mobile tag node. In order to extend the positioning period of the tag node to be measured and maintain its positioning accuracy, an IMU (Inertial Measurement Unit) is installed in the sensor module of M135 in the tag node to be measured. The IMU provides position updates of the tag node between two UWB positionings. During the UWB positioning process, each positioning is independent of each other and there is no cumulative positioning error; while the IMU is related to historical data. The IMU obtains the velocity through numerical integration based on its accelerometer, and then integrates the velocity to obtain the displacement. Since the UWB positioning period is generally in the range of several seconds to dozens of seconds, the cumulative displacement error of the IMU cannot be ignored. By using the relationship between the two UWB positioning data and the IMU measurement data, the cumulative error of the IMU can be reduced. Conversely, the IMU can also correct possible mutations in UWB ranging.

[0092] Formula (8) is the position P obtained after the tag node is powered on, for the 0th time, that is, after the initial UWB positioning. UWB (0), taking P UWB (0) as the position of the tag node, before the first UWB positioning k = m, the position of the tag node to be measured is corrected by the displacement of the tag node to be measured obtained through the IMU sensing module:

[0093]

[0094] In formula (8), k and m are natural numbers, m*T IMU = T UWB , where T IMU is the displacement update period of the IMU of the tag node, T UWB is the UWB positioning update period of the tag node and the fixed anchor node, and m is the number of IMU position update periods contained in the UWB positioning period; k represents the current moment kT IMU ;

[0095] When the tag node to be measured and the fixed anchor node complete the i-th UWB positioning at the moment k = im, the obtained position P UWB(im), and the IMU sensing module of the to-be-measured tag node generates a displacement ΔP from the (i - 1)m moment to the im moment IMU (im), combining the position of UWB and the displacement of IMU, the position P(im) of the tag node is obtained according to formula (9):

[0096]

[0097] In formula (9), i is the number of UWB positioning times, and ΔP UWB (im|(i - 1)m) is the displacement of the tag node between two UWB positionings at the (i - 1)m moment and the im moment, and ΔP IMU (im|(i - 1)m) is the cumulative IMU displacement of the tag node from the (i - 1)m moment to the im moment, that is, there is ΔP UWB (im|(i - 1)m) = P UWB (im) - P UWB ((i - 1)m) and ΔP IMU (im|(i - 1)m) = P IMU (im) - P IMU ((i - 1)m); α UWB is the weighting coefficient for integrating the UWB displacement, and α IMU is the weighting coefficient for integrating the IMU position, and there is α UWB > 0, α IMU > 0 and α UWB + α IMU = 1; when |ΔP UWB (im|(i - 1)m)| and |ΔP IMU (im|(i - 1)m)| differ greatly, the position of the to-be-measured tag node is determined by UWB positioning. When |ΔP UWB (im|(i - 1)m)| and |ΔP IMU (im|(i - 1)m)| do not differ greatly, that is, there is |ΔP UWB (im|(i - 1)m)| ≤ 1.2|ΔP IMU (im|(i - 1)m)|, the position of the to-be-measured tag node is determined by integrating the UWB displacement and the IMU displacement;

[0098] Between two UWB positioning moments, that is, k ∈ ((i - 1)m, im), compare the displacements of the (i - 2)nd and (i - 1)st UWB positionings with the corresponding IMU displacements at the corresponding moments, and correct the position of the to-be-measured tag node through the IMU's IMU periodic displacement. Here, m is the number of IMU position update cycles contained in the UWB positioning cycle, i is the number of UWB positioning times since power-on, and i = 0 at the first time; Formulas (10) and (11) are the positioning calculations of the tag node between two UWB positioning moments:

[0099]

[0100] P(k) = P((i - 1)m) + β(i)△P IMU (k|(i - 1)m), (i - 1)m < k ≤ im, i ∈ N, i > 1. (11);

[0101] In formulas (10) and (11), △P UWB ((i - 1)m|(i - 2)m) is the displacement from the (i - 2)-th UWB positioning P UWB ((i - 2)m) to the (i - 1)-th positioning P UWB ((i - 1)m); △P((i - 1)m|(i - 2)m) is the displacement from the (i - 2)-th comprehensive positioning P((i - 2)m) of the tag node to be measured to the (i - 1)-th comprehensive positioning P((i - 1)m); △P IMU ((i - 1)m|(i - 2)m) is the displacement from the (i - 2)-th positioning P IMU ((i - 2)m) to the (i - 1)-th positioning P IMU ((i - 1)m);

[0102] In formula (10), the initial value of β(i) is 1 (i = 0, 1). When updating β(i), it is judged whether the condition |△P UWB ((i - 1)m|(i - 1)m)| ≤ 1.2|△P IMU (im|(i - 1)m)| holds. If it holds, it is updated; otherwise, the previous value β(i - 1) is adopted; through formula (11), the tag node to be measured also obtains position update data during two UWB positionings, and the correction amount △P IMU (k|(i - 1)m) is adjusted by the β(i) coefficient to reduce the cumulative error generated by the IMU; at the same time, through the validity condition generated by comparing the UWB positioning displacement data with the IMU displacement data, the tag node positioning data is corrected at the UWB positioning moment according to formula (9) and corrected between two UWB positioning moments according to formula (11), increasing the smoothness and robustness of the positioning process. Compared with existing methods such as Kalman filtering for combined UWB and IMU positioning, the comprehensive positioning algorithm of formulas (8) - (11) has low computing power requirements for the tag node and can be implemented using a low-grade single-chip microcomputer. The tag node positioning algorithm that fuses IMU displacement data and UWB positioning data improves the positioning accuracy, extends the UWB positioning period, and reduces the energy consumption of the tag node.

[0103] Example 2

[0104] See Figure 5As shown in the figure, the UWB positioning system with mobile anchor nodes in the tunnel provided in this embodiment is used to implement the UWB positioning method with mobile anchor nodes in the tunnel described in Embodiment 1. The system includes fixed anchor nodes, mobile anchor nodes, and mobile tag nodes;

[0105] Figure 5 (a) is the structure of the fixed anchor node, which consists of the MCU circuit of M110, the UWB transceiver module of M111, the PDOA module of M112, the WIFI module of M113, and the UPS power supply circuit of M114. The UWB transceiver module of M111 realizes the DS-TWR ranging communication function; the PDOA module of M112 realizes measuring the angle between the UWB signal transmitting node and the current measuring node; the WIFI module of M113 realizes WIFI communication with other anchor nodes and mobile anchor nodes; the UPS power supply circuit module of M114 obtains energy from an external AC power supply and converts it into the DC power required by the fixed anchor node, and also charges the rechargeable battery to provide continuous power supply for the fixed anchor node when there is no external AC power. The MCU circuit of M110 uses STM32H745 as the core, has an EEPROM to save the system working parameters, and is connected to other modules through SPI and UART interfaces.

[0106] Figure 5 (b) is the structure of the mobile anchor node, which consists of the MCU circuit of M120, the UWB transceiver module of M121, the PDOA module of M122, the WIFI module of M123, the UPS power supply circuit of M124, and the sensor module of M125. Except for adding the M125 sensor module, the functions of other modules are the same as those of the fixed anchor node. The M125 sensor module has an IMU function and realizes interfaces with millimeter-wave radars, image sensors, and 3D vision devices on the engineering vehicle, and can give accurate real-time positioning information of the mobile anchor node.

[0107] Figure 5 (c) is the structure of the tag node to be measured, which consists of the MCU circuit of M130, the UWB transceiver module of M131, the sensor module of M135, and the battery circuit module of M134. The MCU circuit of M130 uses STM32L072, which is responsible for completing the energy management, DS-TWR ranging communication, and IMU positioning calculation of the entire tag node; the UWB transceiver module of M131 is the same as that of the fixed anchor node, using a DW1000 chip to realize the transceiver of UWB signals in DS-TWR ranging; the sensor module of M135 consists of an IMU chip ICM-42688 and its peripheral circuits; the battery circuit module of M134 converts the battery voltage into various power supply voltages required by the tag node, measures the dynamic values of the battery voltage and current, and thus estimates the remaining battery energy, which is transmitted as a status to the fixed anchor node and the control center to remind the user to charge in time.

[0108] Example 3

[0109] This embodiment discloses a non-transitory computer-readable medium storing instructions, which, when executed by a processor, perform the steps of the UWB positioning method with a mobile anchor node in a tunnel according to Embodiment 1.

[0110] The non-transitory computer-readable medium in this embodiment may be a medium such as a magnetic disk, an optical disc, a computer memory, a read-only memory (ROM), a random access memory (RAM), a USB flash drive, or a portable hard drive.

[0111] Example 4

[0112] This embodiment discloses a computing device, including a processor and a memory for storing a program executable by the processor. When the processor executes the program stored in the memory, the UWB positioning method with a mobile anchor node in a tunnel according to Embodiment 1 is implemented.

[0113] The computing device described in this embodiment may be a desktop computer, a laptop computer, a smart phone, a PDA handheld terminal, a tablet computer, a programmable logic controller (PLC), or other terminal devices with processor functions.

[0114] The above-described embodiments are only the preferred embodiments of the present invention, and do not limit the scope of implementation of the present invention. Therefore, any changes made according to the shape and principle of the present invention should be covered within the protection scope of the present invention.

Claims

1. A UWB positioning method with a mobile anchor node in a tunnel, characterized in that: The method is to first set three types of communication nodes, namely, fixed anchor nodes, mobile anchor nodes and tag nodes to be tested, install the fixed anchor nodes on both sides of the tunnel or on the top of the tunnel, install the mobile anchor nodes on a mobile engineering vehicle, and install the tag nodes to be tested on the mobile object to be positioned; wherein, the fixed anchor nodes are integrated with a DS-TWR module, a PDOA module and a WIFI module; the mobile anchor nodes are integrated with a DS-TWR module, a PDOA module, a WIFI module and a sensor module, and the mobile anchor nodes communicate with a millimeter-wave radar and an image sensor installed on the engineering vehicle to obtain a real-time position; the tag nodes to be tested are integrated with a DS-TWR module and an IMU sensor module, and the IMU sensor module is used to extend the positioning period of the tag nodes to be tested and maintain the positioning accuracy; The tag node to be measured performs DS-TWR ranging communication with the fixed anchor node, and performs PDOA to measure the angle between the tag node to be measured and the fixed anchor node, so as to calculate the coordinate position of the tag node to be measured according to the measured distance and angle; at the same time, if the mobile anchor node is within the UWB communication range, the mobile anchor node is used for auxiliary positioning, and the mobile anchor node does not perform ranging communication with the tag node to be measured. By monitoring the DS-TWR ranging communication between the tag node to be measured and the fixed anchor node, the incident angle of the tag node to be measured relative to the PDOA module of the mobile anchor node is calculated, and the incident angle is converted into an angle relative to the mobile anchor node's own coordinate system and provided to the fixed anchor node. The fixed anchor node realizes the precise positioning of the tag node to be measured based on the angle data; if the mobile anchor node is not within the UWB communication range, the mobile anchor node is not used for auxiliary positioning.

2. The UWB positioning method with a mobile anchor node in a tunnel according to claim 1, characterized in that: The method comprises the following steps: a. The tag node to be tested sends a Poll frame; the DS-TWR module and PDOA module of the fixed anchor node receive the Poll frame; if the mobile anchor node is also within the UWB communication range, it also receives the Poll frame for PDOA measurement of the mobile anchor node; the Poll frame carries the time when the tag node to be tested sends the frame, the fixed anchor node receives the Poll frame and records the Poll frame reception time, and after a fixed period of time T replyB After that, the fixed anchor node sends a Resp frame; b. The tag node to be measured receives the Resp frame for DS-TWR ranging; the Resp frame includes the receiving time of the Poll frame and the sending time of the Resp frame. The mobile anchor node within the coverage of the UWB signal also receives the Resp frame for the PDOA method to measure the angle between the mobile anchor node and the fixed anchor node. c. The tag node to be tested undergoes a fixed time delay T replyA After that, a Final frame is sent, and the content of the Final frame includes the time when the tag node to be tested receives the Resp frame and the time when the Final frame is sent; the fixed anchor node receives the Final frame and records the reception time of the frame, and uses the PDOA module to measure the angle of the tag node to be tested; the mobile anchor node receives the Final frame and records the reception time of the frame, and uses the PDOA module to measure the angle of the tag node to be tested; d. The fixed anchor node calculates the distance and PDOA angle between the tag node to be tested and the fixed anchor node through the sending and receiving time of each frame and the DS-TWR algorithm. At the same time, the mobile anchor node sends the measured PDOA angles of the tag node to be tested and the fixed anchor node and its own position to the fixed anchor node through the WIFI module. The fixed anchor node completes the comprehensive positioning of the tag node to be tested based on these angles and position data, and informs the tag node to be tested by sending an Ack frame; e. The tag node to be tested receives the Ack frame data; the mobile anchor node within the coverage of the UWB signal also receives the Ack frame data.

3. The UWB positioning method with a mobile anchor node in a tunnel according to claim 2, characterized in that: The DS-TWR ranging communication between the tag node to be tested and the fixed anchor node includes: The DS-TWR distance calculated after the tested tag node and the fixed anchor node exchange Poll frames, Resp frames, Final frames and Ack frames is as shown in formula (1): Where ρ i is the distance between the i-th label node to be tested and the fixed anchor node, T i is the flight time of the UWB electromagnetic wave between the i-th tag node to be tested and the fixed anchor node; C is the speed of light in the air, which is a constant; T roundA T is the time difference between the time when the tag node to be tested sends the Poll frame and the time when it receives the Resp frame. roundB T is the time difference between the fixed anchor node sending the Resp frame and receiving the Final frame. replayA T is the time difference between the tag node receiving the Resp frame and sending the Final frame. replyB It is the time difference between the fixed anchor node receiving the Poll frame and sending the Resp frame.

4. The UWB positioning method with a mobile anchor node in a tunnel according to claim 3, characterized in that: Performing PDOA to measure the angle between the tag node to be measured and the fixed anchor node, thereby calculating the coordinate position of the tag node to be measured based on the measured distance and angle includes: During the DS-TWR ranging communication between the tag node to be tested and the fixed anchor node, the PDOA modules of the fixed anchor node and the mobile anchor node only need the UWB signal of the tag node to be tested or the fixed anchor node to calculate the incident angle β of the measured signal source relative to its PDOA module: Using two antennas separated by a distance of d, denoted as antenna A and antenna B, the electromagnetic waves received from the same signal source have a phase difference. The phase difference α between the electromagnetic waves received by antenna A and antenna B corresponds to the incident angle β of the signal source relative to the two antennas, as shown in formula (2): Where λ is the wavelength of the UWB electromagnetic wave; After the anchor node obtains the distance and angle of the tag node to be measured relative to itself, it uses coordinate transformation to calculate the global coordinates (x i ,y i ), as shown in formula (3): Where θ0 is the angle of the tag node to be tested relative to the fixed anchor node, which is determined by the sum of β and the angle of the fixed anchor node installation position. When the fixed anchor node is installed on the top of the tunnel, it is 0°, then θ0 = β; when the fixed anchor node is installed on the left side of the tunnel, it is 90°, then θ0 = β + 90; when the fixed anchor node is installed on the right side of the tunnel, it is -90°, then θ0 = β - 90; It is the deflection angle of the fixed anchor node in the global coordinate system, which is determined when the fixed anchor node is installed and the global coordinate system is established. The method combining the distance measurement and the angle measurement is used to realize the positioning of the tag node to be measured using a single fixed anchor node.

5. The UWB positioning method with a mobile anchor node in a tunnel according to claim 1, characterized in that: If the mobile anchor node is within the UWB communication range, the auxiliary positioning using the mobile anchor node includes: Within the UWB communication range, the fixed anchor node can receive angle data of an indefinite amount of mobile anchor nodes when locating the tag node to be measured. According to the number of mobile anchor nodes, it is divided into three cases: the fixed anchor node can receive angle data of 1 mobile anchor node, the fixed anchor node can receive angle data of 2 mobile anchor nodes, and the fixed anchor node can receive angle data of more than 2 mobile anchor nodes; among them, when the fixed anchor node can receive angle data of more than 2 mobile anchor nodes, it is necessary to select the angle data of the two mobile anchor nodes with the largest signal strength of the tag node to be measured, which is equivalent to the case where the fixed anchor node can receive angle data of 2 mobile anchor nodes and then calculate.

6. The UWB positioning method with a mobile anchor node in a tunnel according to claim 5, characterized in that: The angle data that a fixed anchor node can receive from a mobile anchor node includes: When the fixed anchor node is locating the tag node to be measured, it can receive the angle data of one mobile anchor node MA1, that is, one mobile anchor node MA1 participates in the positioning of the tag node Tag to be measured by the fixed anchor node FA; the antenna coordinates of the mobile anchor node are determined by its installation position and millimeter wave radar positioning. The global coordinates of the fixed anchor node FA are (x0, y0), and the global coordinates of the mobile anchor node MA1 are (x1, y1). After a DS-TWR communication process, FA and MA1 both obtain a PDOA angle θ of the tag node to be measured relative to them. i , calculate the angle of the tag node to be measured relative to FA as α0, calculate the angle of the tag node to be measured relative to MA1 as α1, and in the global coordinate system, the angle of the tag node to be measured relative to the mobile anchor node in is the deflection angle of the fixed anchor node and the mobile anchor node in the global coordinate system, as shown in the following formula (4): A set of non-homogeneous linear equations is obtained, namely AX = b, as shown in the following formula (5): The additional angle measurement data will further reduce the error of single base station positioning. The least square method is used to solve and minimize the error, as shown in the following formula (6): The angle measurement information from other mobile anchor nodes other than the fixed anchor nodes is integrated to improve the positioning accuracy of the fixed anchor nodes for the tag nodes by fitting the optimal solution. Finally, the fixed anchor nodes return the final calculated global coordinates to the measured tag nodes through Ack frames.

7. The UWB positioning method with a mobile anchor node in a tunnel according to claim 5, characterized in that: The fixed anchor node can receive the angle data of the two mobile anchor nodes including: When the fixed anchor node locates the tag node to be measured, it can receive the angle data of the two mobile anchor nodes MA1 and MA2, that is, the two mobile anchor nodes MA1 and MA2 participate in the positioning of the tag node Tag to be measured by the fixed anchor node FA; the antenna coordinates of the mobile anchor node are determined by its installation position and millimeter wave radar positioning. The global coordinates of the fixed anchor node are (x0, y0), the global coordinates of the mobile anchor node MA1 are (x1, y1), and the global coordinates of the mobile anchor node MA2 are (x2, y2). After a DS-TWR communication process, FA, MA1 and MA2 all obtain a PDOA angle θ of the tag node Tag to be measured relative to them. i , calculate the angle of the tag node to be measured relative to FA as α0, calculate the angle of the tag node to be measured relative to MA1 as α1, and calculate the angle of the tag node to be measured relative to MA2 as α2. In the global coordinate system, the angle of the tag node to be measured relative to the mobile anchor node is in is the deflection angle of the fixed anchor node and the mobile anchor node in the global coordinate system, as shown in the following formula (7): A set of non-homogeneous linear equations is obtained, namely AX = b, as shown in the following formula (8): The additional angle measurement data will further reduce the error of single base station positioning. The least square method is used to solve and minimize the error, as shown in the following formula (9): The angle measurement information from other mobile anchor nodes other than the fixed anchor nodes is integrated to improve the positioning accuracy of the fixed anchor nodes for the tag nodes by fitting the optimal solution. Finally, the fixed anchor nodes return the final calculated global coordinates to the measured tag nodes through Ack frames.

8. The UWB positioning method with a mobile anchor node in a tunnel according to claim 1, characterized in that: Using the IMU sensor module to extend the positioning cycle of the tag node to be tested and maintain the positioning accuracy includes: After the tag node is powered on, the position P obtained after the 0th time, that is, the initial UWB positioning UWB (0), with P UWB (0) As the tag node position, before the first UWB positioning k = m, the displacement of the tag node to be measured obtained by the IMU sensor module is used to correct the position of the tag node to be measured: In formula (10), k and m are natural numbers, m*T IMU =T UWB , where T IMU is the displacement update period of the tag node IMU, T UWB is the UWB positioning update period of the tag node and the fixed anchor node, m is the number of IMU position update periods in the UWB positioning period; k represents the current time kT IMU ; When the tag node to be tested and the fixed anchor node complete the i-th UWB positioning at time k = im, the obtained position P UWB (im), and at the same time, the IMU sensor module of the tag node to be tested generates a displacement △P from time (i-1)m to time im IMU (im), combining the UWB position and IMU displacement, the position of the tag node P(im) is obtained according to formula (11): In formula (11), i is the number of UWB positioning times, △P UWB (im|(i-1)m) is the displacement of the tag node at the UWB positioning at time (i-1)m and time im, △P IMU (im|(i-1)m) is the IMU displacement accumulated by the label node from time (i-1)m to time im, that is, △P UWB (im|(i-1)m)=P UWB (im)-P UWB ((i-1)m) and △P IMU (im|(i-1)m)=P IMU (im)-P IMU ((i-1)m); α UWB is the weighting coefficient of the comprehensive UWB displacement, α IMU is the weighted coefficient of the comprehensive IMU position, with α UWB >0,α IMU >0 and α UWB +α IMU =1; when |△P UWB (im|(i-1)m)| and |△P IMU When the difference between (im|(i-1)m)| is large, the position of the tag node to be tested is determined by UWB positioning. UWB (im|(i-1)m)| and |△P IMU When (im|(i-1)m)| is not much different, we have |△P UWB (im|(i-1)m)|≤1.2|△P IMU When (im|(i-1)m)|, the position of the tag node to be measured is determined by the UWB displacement and IMU displacement; Between two UWB positioning moments, i.e., k∈((i-1)m,im), the (i-2)th and (i-1)th UWB positioning displacements are compared with the IMU displacements at the corresponding moments, and the position of the tag node to be measured is corrected by the IMU periodic displacement of the IMU, where m is the number of IMU position update cycles in the UWB positioning cycle, i is the number of UWB positioning times since power-on, and i=0 for the first time; formulas (12) and (13) are the positioning calculations of the tag node between two UWB positioning moments: P(k)=P((i-1)m)+β(i)△P IMU (k|(i-1)m),(i-1)m<k≤im,i∈N,i>1.(13); In formulas (12) and (13), △P UWB ((i-1)m|(i-2)m) is the (i-2)th UWB positioning P UWB ((i-2)m) to (i-1)th positioning P UWB ((i-1)m); △P((i-1)m|(i-2)m) is the displacement between the (i-2)th comprehensive positioning P((i-2)m) and the (i-1)th comprehensive positioning P((i-1)m) of the tag node to be tested; △P IMU ((i-1)m|(i-2)m) is the (i-2)th IMU positioning P IMU ((i-2)m) to (i-1)th positioning P IMU Displacement between ((i-1)m); In formula (12), β(i) is initialized to 1 (i = 0, 1). When updating β(i), we judge |△P UWB ((i-1)m|(i-1)m)|≤1.2|△P IMU (im|(i-1)m)| is true, if true then update, otherwise use the last value β(i-1); Formula (13) enables the tag node to obtain the position update data during the two UWB positioning periods, and the correction value △P IMU (k|(i-1)m) is adjusted by the β(i) coefficient to reduce the cumulative error generated by the IMU. At the same time, the validity condition is generated by comparing the UWB positioning displacement data with the IMU displacement data. The tag node positioning data is corrected at the UWB positioning moment according to formula (11) and corrected between two UWB positioning moments according to formula (13).

9. A UWB positioning system with a mobile anchor node in a tunnel, characterized in that: Used to implement the UWB positioning method with a mobile anchor node in a tunnel as described in any one of claims 1 to 8, the system includes a fixed anchor node, a mobile anchor node and a mobile anchor node; The fixed anchor node integrates a DS-TWR module, a PDOA module, a WIFI module, an MCU circuit module and a UPS power circuit module; The mobile anchor node integrates a DS-TWR module, a PDOA module, a WIFI module, a sensor module, an MCU circuit module and a UPS power circuit module; The tag node to be tested integrates a DS-TWR module, an IMU sensor module, an MCU circuit module and a battery circuit module.

10. A UWB positioning system with a mobile anchor node in a tunnel according to claim 9, characterized in that: The sensor module is communicatively connected with a millimeter wave radar, an image sensor and a 3D vision device.

Images