Chain type two-dimensional positioning system based on ultra-wideband signal and implementation method thereof

Abstract

The invention relates to a chained two-dimensional positioning system based on an ultra-wideband signal and an implementation method thereof, and belongs to the technical field of wireless positioning. The UWB signals are transmitted in a chained manner, and high-precision two-dimensional positioning of a plurality of link nodes can be realized in a tunnel environment by only one terminal node. The system utilizes known terminal node positions, tunnel geometrical shapes and distance measurement data between adjacent nodes, combines a nonlinear least square optimization algorithm, constraint optimization and graph optimization technologies and an error correction and filtering method, overcomes the limitation of a traditional positioning technology in a complex environment, and improves the positioning accuracy. The method has the advantages of high precision, low cost and flexible deployment, and is suitable for multiple application fields such as tunnel monitoring, intelligent transportation and industrial automation.

Description

Technical Field

[0001] The invention belongs to the technical field of wireless positioning, and relates to a chained two-dimensional positioning system based on ultra-wideband signals and a realization method thereof. Background Art

[0002] Achieving high-precision worker positioning in complex environments such as tunnel construction is a key challenge, especially in closed or semi-enclosed spaces such as tunnels. Existing positioning technologies mainly rely on GPS systems, which work well in open spaces, but their effectiveness is greatly limited in tunnels due to the lack of direct antenna line of sight. In addition, network-based positioning systems such as Wi-Fi and RFID can provide indoor coverage to a certain extent, but the positioning accuracy of Wi-Fi is often affected by environmental interference in complex tunnel structures, and the positioning accuracy is unstable; while RFID systems can provide positioning services at specific points, they rely on dense tag and reader layouts, which may be impractical in cost and implementation, especially in long-distance and multi-level tunnel projects. Due to the limitations of these technologies, tunnel construction safety management faces major challenges, especially in accurately monitoring the real-time location of workers. Accurate real-time location information is essential to ensure worker safety, optimize resource allocation, prevent potential collisions between equipment and personnel, and respond effectively in emergency situations. For example, in the event of a fire or structural collapse, being able to quickly and accurately locate the location of each worker will greatly improve rescue efficiency and worker survival rate.

[0003] Therefore, the field of tunnel construction urgently needs a new technical solution that can provide high-precision positioning information and adapt to the special environment of tunnel construction. This solution needs to be able to operate stably under various complex working conditions in the tunnel, and at the same time have sufficient flexibility and scalability to adapt to the ever-changing working environment and safety requirements. Summary of the invention

[0004] In view of this, the purpose of the present invention is to provide a chain two-dimensional positioning system based on ultra-wideband signals and its implementation method to solve the problem of worker positioning in a tunnel construction environment, especially the limitations and shortcomings of traditional positioning technology in complex and changeable underground environments.

[0005] In order to achieve the above object, the present invention provides the following technical solutions:

[0006] A chained two-dimensional positioning system based on ultra-wideband signals, the system comprising:

[0007] The master node is installed at the tunnel entrance or key locations to send and receive UWB signals;

[0008] Subnodes, installed on workers’ safety helmets or other mobile devices, are used to receive ultra-wideband signals sent by the master node and measure distances with adjacent subnodes;

[0009] The control unit connects the main node and the sub-nodes, is used to receive ranging data and perform positioning calculations, and output node location information.

[0010] Furthermore, the control unit uses a nonlinear least squares optimization algorithm to perform positioning calculations.

[0011] Furthermore, the control unit uses a graph optimization algorithm to perform positioning calculations.

[0012] Furthermore, the control unit optimizes the positioning result by using error correction and filtering methods.

[0013] Furthermore, the subnode also includes an inertial measurement unit for providing node motion state information.

[0014] A chained two-dimensional positioning method based on an ultra-wideband signal comprises the following steps:

[0015] Step 1: Initialize nodes, including master nodes and sub-nodes. The master node sends ultra-wideband signals, and the sub-nodes receive the signals and perform ranging with adjacent sub-nodes.

[0016] Step 2: The control unit receives the ranging data, performs positioning calculations, and outputs the node location information;

[0017] Step 3: The control unit optimizes the positioning result using a nonlinear least squares optimization algorithm or a graph optimization algorithm;

[0018] Step 4: The control unit optimizes the positioning result using error correction and filtering methods;

[0019] Step 5: Determine whether the node position is abnormal. If so, relocate it.

[0020] Furthermore, in step 2, the control unit adopts multi-sensor data fusion technology to fuse ultra-wideband signal data and inertial measurement unit data to improve positioning accuracy.

[0021] Furthermore, in step 3, the control unit performs constrained optimization on the positioning result according to the tunnel terrain data.

[0022] Furthermore, in step 4, the control unit uses a method of dynamically adjusting the weight factor to balance the importance of ranging error and tunnel terrain constraints.

[0023] Furthermore, in step 5, the control unit uses a particle filter algorithm to perform relocation calculation.

[0024] The beneficial effects of the present invention are:

[0025] (1) The present invention was simulated and tested under the following conditions: a 1.5 km tunnel, using UWB TWR technology for ranging and positioning. The simulation results are shown in Figure 2. Figure 3 As shown in the figure, the results show that the present invention can locate the equipment in the tunnel of several kilometers with only a single terminal. The error results are as follows Figure 4 As shown, it can be seen that in a tunnel with a width of 20 meters, the average horizontal accuracy is about 4 meters, which meets the positioning requirements for the approximate position of targets such as personnel / objects in the tunnel scene. The present invention can realize the presentation of target chain topology results in ultra-long tunnels and improve the level of safe operation in tunnels.

[0026] (2) Compared with the existing methods, the existing invention CN110618434B provides a laser radar-based tunnel positioning system and positioning method thereof, which is achieved by laying road signs on both sides of the tunnel and positioning. However, the laser radar is expensive and requires multiple installations along the way, which increases the cost exponentially. The present invention, however, proposes an effective chain positioning algorithm through only one terminal, achieving low-cost high-precision positioning.

[0027] In summary, the present invention proposes a low-cost positioning solution using only a single terminal node in a tunnel, which solves the problem that satellite signals cannot be positioned in a tunnel.

[0028] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0030] Figure 1 It is a flow chart for realizing the present invention;

[0031] Figure 2 It is a schematic diagram of the transfer positioning of the present invention;

[0032] Figure 3 It is the simulation result diagram of the present invention;

[0033] Figure 4 This is the simulation error result of the present invention. DETAILED DESCRIPTION

[0034] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0035] Among them, the drawings are only used for illustrative explanations, and they only represent schematic diagrams rather than actual pictures, and should not be understood as limitations on the present invention. In order to better illustrate the embodiments of the present invention, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0036] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right", "front", "rear", etc. indicate the orientation or position relationship, they are based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the terms describing the position relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0037] See also Figure 1 , the present invention comprises the following steps:

[0038] Step 1: System composition and initialization settings. Node configuration: Install an ultra-wideband (UWB) device on each worker's helmet as a mobile sub-node; install a fixed UWB terminal at the entrance of the tunnel or at a key location as a master node. The master node and the sub-node are collectively referred to as nodes. Assume that there is a fixed master node O at the entrance of the tunnel, and other sub-nodes, such as Figure 2 As shown. The distance between each other is measured using UWB TWR technology, and the shortest distance is used as the main reference distance (with the smallest error). Statistical methods are used to eliminate abnormal errors, assuming that the ranging error ∈ i,i+1 Obey Gaussian distribution:

[0039]

[0040] Indicates the measured distance, d i,i+1 represents the true distance, σ2 Represents the variance of the Gaussian distribution. is the standard Gaussian distribution symbol.

[0041] Therefore, the RANSAC algorithm is used to eliminate abnormal ranging data to ensure the accuracy of the optimization process.

[0042] Step 2: Initial position estimation. Assume that the tunnel geometry is known, i.e. it can be a straight line or a curve with known curvature. The terminal node position is known as (x0, y0). The position of the first child node is The distance between adjacent nodes is measured as d 0,1 Estimate, and introduce a random small offset angle θ1 to improve the robustness of the algorithm and enhance the optimization effect:

[0043]

[0044] in, The subscript 1 represents the first child node, and the superscript (0) represents the initial value of the iteration. Because the algorithm will have an iterative process later, the superscript of the first iteration is (1), and so on. θ1 is a random value, usually small (such as |θ1| ≤ 5°) to ensure that the node is within the tunnel width. For the i-th link node (i ≥ 2), according to the position of the previous node and the distance value d i-1,i Estimate its initial position:

[0045]

[0046] Among them, θ i is the random offset angle of the ith node relative to the previous node, which is also small to ensure that the node position is within the tunnel width. To ensure that the initial position of all link nodes is within the tunnel width, if the lateral offset of a node is Beyond the tunnel boundary (i.e. Where w is the tunnel width), adjust it to the boundary position:

[0047]

[0048] in, for The symbol function of .

[0049] Step 3: Nonlinear Least Squares Optimization

[0050] Nonlinear Least Squares (NLS) is used to optimize the node positions to minimize the ranging error. The optimization objective function is defined as the sum of the squares of the difference between the actual distance and the measured distance between all link nodes:

[0051]

[0052] Where N is the total number of link nodes, (x i ,y i ) is the two-dimensional coordinate of the i-th link node, d i,i+1 is the distance measurement value between the i-th and i+1-th nodes. Then the Levenberg-Marquard algorithm is used for nonlinear least squares optimization. The Jacobian matrix J of the objective function J(x,y) relative to the optimization variables x and y is constructed, that is, the partial derivative of each distance measurement error with respect to each coordinate variable. For each distance measurement error term, there is:

[0053]

[0054] The elements of the Jacobian matrix are:

[0055]

[0056] Step 4: Tunnel terrain constraint introduction and positioning model optimization. In order to improve positioning accuracy, the tunnel terrain data is combined as an additional constraint to ensure that the link node position conforms to the geometry of the tunnel. Assume that the tunnel centerline is defined as a known function y = f(x), such as a straight line y = 0 or a circular arc curve

[0057] The lateral displacement of each node is δ i satisfy:

[0058] y i =f(x i )+δ i

[0059]

[0060] Where w is the tunnel width. Then the penalty term of terrain constraints is added to the nonlinear least squares objective function to form a comprehensive optimization model:

[0061]

[0062] Among them, λ is a weight factor used to balance the importance of ranging error and terrain constraints. In order to more effectively balance ranging error and terrain constraints, we propose to dynamically adjust the weight factor λ according to the ranging error in the nonlinear least squares optimization process. The specific method is as follows:

[0063]

[0064] Among them, ν>1 is an adjustment factor, which is used to control the adjustment range of the weight factor.

[0065] In the simulation, the relative rate of change of the objective function is used to determine: It can be considered effective. k represents the kth iteration. τ is an empirical value, which is set to 0.01.

[0066] Therefore, the optimization iteration steps are as follows:

[0067] 1) Initialization: Use the initial position estimate as the optimization starting point (x (0) ,y (0) );

[0068] 2) Calculate the residual: Calculate the current residual value of the objective function J′(x,y);

[0069] 3) Calculate the Jacobian matrix: Calculate the Jacobian matrix J according to the current node position;

[0070] 4) Update parameters: Update node coordinates according to the iterative formula of the Levenberg-Marquardt algorithm (LM):

[0071]

[0072] Among them, r is the current residual vector, μ is the damping factor, and I is the unit matrix.

[0073] 5) Adjust the damping factor: Dynamically adjust μ according to the effectiveness of the optimization step to ensure the convergence and stability of the algorithm.

[0074] 6) Termination condition: When the residual change is lower than the preset threshold or reaches the maximum number of iterations, the optimization is stopped.

[0075] Step 5: Establish a graph optimization model. To further improve the positioning accuracy, the chain positioning problem is transformed into a graph optimization problem. By building a graph structure of nodes and edges, a globally consistent node position estimation is achieved. A node is defined as each node in the graph representing a two-dimensional position of a link node (x i ,y i ); each edge in the graph represents the distance measurement relationship between adjacent nodes d i,i+1 , and introduce terrain constraint edges. Then we use Ceres Solver to optimize the graph. The error model is positioned as the Euclidean distance model, that is, the edge measurement error is defined as:

[0076] e i,i+1 =||p i -p i+1 ||-d i,i+1

[0077] Among them, p i and p i+1are the current estimated positions of node i and node j respectively. The loss function is the distance error: the sum of squared errors of all edges is calculated:

[0078]

[0079] The penalty term is used to constrain the position change of the node, expressed as:

[0080]

[0081] in, is the position of node k in the last iteration. The final optimization goal is:

[0082] Minimize Loss total =Loss dist +λ·Loss penalty

[0083] Among them, λ is the coefficient that controls the weight of the penalty term. Then iterative optimization is performed, and the specific process is as follows:

[0084] 1) Initial solution input: Use the optimization result of the LM algorithm in step 4 as the initial solution of Ceres. Pass the initial positions of all nodes to Ceres Solver;

[0085] 2) Build the problem: Build the optimization problem in Ceres and add nodes and edges to the optimizer;

[0086] 3) Set optimization options: including the number of iterations, convergence criteria, and solver options. In this paper, the maximum number of iterations is set to 500, and the convergence criterion is that the error change is less than 0.001.

[0087] 4) Call the optimization function: Execute the optimization process, Ceres will iteratively update the node position to minimize the total loss;

[0088] 5) Convergence judgment: Set the convergence standard (such as the change rate of the loss function or the change of the node position is less than the threshold) to determine whether the optimization ends. If the convergence condition is met, the optimization ends;

[0089] 6) Output results. After the optimization is completed, Ceres Solver will return the final position of each node. These positions should more accurately reflect the actual tunnel environment. The output results include: the optimized node coordinates, statistical information of the optimization process: for example, the total number of iterations, the final loss value, etc.

[0090] Step 6: Dynamic monitoring and relocation Real-time monitoring of the distance measurement data between nodes, calculate the change between the current position and the previous position. Set a threshold, and when the node position change exceeds the threshold, it is judged as abnormal. Once the node position abnormality is detected, the relocation algorithm is triggered to re-estimate the two-dimensional coordinates of the abnormal node. Combine the current distance measurement data and historical data to perform global optimization to ensure that the overall positioning accuracy of the system is not affected by local abnormalities.

[0091] Step 7: Multi-sensor data fusion. During relocalization, in order to further improve the positioning accuracy, the data of other sensors such as the inertial measurement unit (IMU) are combined to further improve the accuracy and robustness of the positioning system. First, the acceleration and angular velocity data are read from the IMU and preprocessed to remove noise and drift; then the IMU data is used for state prediction. The state can be expressed as the position and orientation of the node, and the update formula is:

[0092]

[0093] θ new =θ old +ω·Δt

[0094] Among them, p is the position, v is the velocity, a is the acceleration, θ is the direction, ω is the angular velocity, and Δt is the time interval. Then, we combine the particle filter with the IMU and graph optimization results. The specific steps are as follows:

[0095] 1) Particle initialization: Generate a set of particles based on the graph optimization results. The initial position and state of the particles come from the output results of step 4;

[0096] 2) Prediction: Each particle predicts its state based on the IMU data and updates its position and orientation. The prediction formula is the same as the IMU fusion process.

[0097] 3) Update: Weight calculation: Calculate the weight of each particle based on the current particle position and the actual measurement value (such as UWB positioning result). The weight can be determined based on the measurement error:

[0098]

[0099] Among them, z is the measured value, h(p i ) is the expected measurement under the particle state, and σ is the standard deviation of the measurement noise.

[0100] 4) Resampling: Resample the particles according to their weights, retain high-weight particles, and remove low-weight particles to prevent particle degradation.

[0101] 5) State estimation: The final state of the particle is calculated by weighted average as the positioning result after fusion:

[0102]

[0103] Combining IMU data and particle filtering can effectively improve the real-time and accuracy of positioning. The dynamic information provided by IMU helps correct position changes, while particle filtering enhances robustness to nonlinearity and noise.

[0104] Figure 3 It is the simulation result diagram of the present invention; Figure 4 This is the simulation error result of the present invention.

[0105] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A chained two-dimensional positioning system based on ultra-wideband signals, characterized in that: The system includes: The master node is installed at the tunnel entrance or key locations to send and receive UWB signals; Subnodes, installed on workers’ safety helmets or other mobile devices, are used to receive ultra-wideband signals sent by the master node and measure distances with adjacent subnodes; The control unit connects the main node and the sub-node, is used to receive the ranging data and perform positioning calculations, and output the node location information.

2. The chained two-dimensional positioning system based on ultra-wideband signals according to claim 1, characterized in that: The control unit uses a nonlinear least squares optimization algorithm to perform positioning calculations.

3. The chained two-dimensional positioning system based on ultra-wideband signals according to claim 1, characterized in that: The control unit uses a graph optimization algorithm to perform positioning calculations.

4. The chained two-dimensional positioning system based on ultra-wideband signals according to claim 1, characterized in that: The control unit optimizes the positioning result by using error correction and filtering methods.

5. The chained two-dimensional positioning system based on ultra-wideband signals according to claim 1, characterized in that: The subnode also includes an inertial measurement unit for providing node motion state information.

6. A chained two-dimensional positioning method based on ultra-wideband signals, characterized in that: The following steps are involved: Step 1: Initialize nodes, including master nodes and sub-nodes. The master node sends ultra-wideband signals, and the sub-nodes receive the signals and perform ranging with adjacent sub-nodes. Step 2: The control unit receives the ranging data, performs positioning calculations, and outputs the node location information; Step 3: The control unit optimizes the positioning result using a nonlinear least squares optimization algorithm or a graph optimization algorithm; Step 4: The control unit optimizes the positioning result using error correction and filtering methods; Step 5: Determine whether the node position is abnormal. If so, relocate it.

7. The chained two-dimensional positioning method based on ultra-wideband signals according to claim 6, characterized in that: In step 2, the control unit uses multi-sensor data fusion technology to fuse ultra-wideband signal data and inertial measurement unit data to improve positioning accuracy.

8. The chained two-dimensional positioning method based on ultra-wideband signals according to claim 6, characterized in that: In step 3, the control unit performs constrained optimization on the positioning result according to the tunnel terrain data.

9. The chained two-dimensional positioning method based on ultra-wideband signals according to claim 6, characterized in that: In step 4, the control unit uses a method of dynamically adjusting the weight factor to balance the importance of ranging error and tunnel terrain constraints.

10. The chained two-dimensional positioning method based on ultra-wideband signals according to claim 6, characterized in that: In step 5, the control unit uses a particle filter algorithm to perform relocation calculation.

Images