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

Abstract

The present invention relates to a chain - type two - dimensional positioning system based on ultra - wideband signals and its implementation method, belonging to the field of wireless positioning technology. By transmitting UWB signals in a chain - type manner, only one terminal node is required to achieve high - precision two - dimensional positioning of multiple link nodes in a tunnel environment. The system utilizes the known position of the terminal node, the tunnel geometry, and the distance measurement data between adjacent nodes, combines non - linear least - squares optimization algorithms, constraint optimization and graph optimization techniques, as well as error correction and filtering methods, overcomes the limitations of traditional positioning technologies in complex environments, has the advantages of high precision, low cost, and flexible deployment, and is applicable to multiple application fields such as tunnel monitoring, intelligent transportation, and industrial automation.

Description

Technical Field

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

[0002] In complex environments such as tunnel construction, achieving high-precision worker positioning is a key challenge, especially in enclosed or semi-enclosed spaces such as inside tunnels. Existing positioning technologies mainly rely on the GPS system, which works well in open spaces, but its effectiveness is severely 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 the RFID system can provide positioning services at specific points, but it relies on a dense layout of tags and readers, which may be impractical in terms of 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 positions of workers. Accurate real-time position information is crucial for ensuring worker safety, optimizing resource allocation, preventing potential collisions between equipment and personnel, and enabling effective response in case of emergencies. For example, in case of a fire or structural collapse, being able to quickly and accurately locate the position of each worker will greatly improve the rescue efficiency and the survival rate of workers.

[0003] Therefore, there is an urgent need in the field of tunnel construction for a new technical solution that can both 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 tunnels, and at the same time, it should have sufficient flexibility and scalability to adapt to changing working environments and safety requirements. Summary of the Invention

[0004] In view of this, the purpose of the present invention is to provide a chain-type two-dimensional positioning system based on ultra-wideband signals and an implementation method thereof, so as to solve the problem of worker positioning in the tunnel construction environment, especially aiming at the limitations and deficiencies faced by traditional positioning technologies in complex and changeable underground environments.

[0005] To achieve the above purpose, the present invention provides the following technical solutions:

[0006] A chain-type two-dimensional positioning system based on ultra-wideband signals, the system includes:

[0007] A master node, installed at the tunnel entrance or key positions, for sending and receiving ultra-wideband signals;

[0008] Slave nodes, 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 slave nodes.

[0009] A control unit, connected to the master node and the slave nodes, is used to receive ranging data and perform positioning calculations, and output node position information.

[0010] Furthermore, the control unit uses a non-linear least squares optimization algorithm for positioning calculations.

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

[0012] Furthermore, the control unit uses an error correction and filtering method to optimize the positioning results.

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

[0014] A chain-type two-dimensional positioning method based on ultra-wideband signals includes the following steps:

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

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

[0017] Step 3: The control unit uses a non-linear least squares optimization algorithm or a graph optimization algorithm to optimize the positioning results.

[0018] Step 4: The control unit uses an error correction and filtering method to optimize the positioning results.

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

[0020] Furthermore, 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.

[0021] Furthermore, in Step 3, the control unit performs constraint optimization on the positioning results according to 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 for repositioning calculations.

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

[0025] (1) The present invention was subjected to simulation tests. The simulation conditions were as follows: a 1.5-km tunnel, using the UWB's TWR technology for ranging and positioning. The simulation results are as Figure 3 shown. The results indicate that the present invention can locate devices in a tunnel several kilometers long using only a single terminal. The error results are as Figure 4 shown. It can be seen that in a tunnel with a width of 20 meters, the average horizontal accuracy is approximately 4 meters, meeting the positioning requirements for the approximate positions of targets such as personnel / objects in the tunnel scenario. The present invention can present the target chain topology results in an ultra-long tunnel, improving the safety operation level in the tunnel.

[0026] (2) Compared with the existing method, the existing invention CN110618434B provides a tunnel interior positioning system based on lidar and its positioning method, which locates by arranging road signs on both sides of the tunnel. However, the lidar is costly, and multiple installations are required along the way, resulting in a multiple-fold increase in cost. In contrast, the present invention proposes an effective chain positioning algorithm using only one terminal, achieving low-cost high-precision positioning.

[0027] In summary, the present invention proposes a low-cost positioning solution that utilizes only a single terminal node in a tunnel, solving the problem of inability to locate using satellite signals in the tunnel.

[0028] Other advantages, objectives, and features of the present invention will, to some extent, be elaborated in the subsequent specification, and to some extent, will be obvious to those skilled in the art based on an examination of the following text, or can be learned from the practice of the present invention. The objectives and other advantages of the present invention can be achieved and obtained through the following specification. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] To make the objectives, technical solutions, and advantages of the present invention clearer, the present invention will be described in detail and preferably with reference to the accompanying drawings, where:

[0030] Figure 1 is the implementation flowchart of the present invention;

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

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

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

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

[0035] Among them, the drawings are only for illustrative purposes, showing only schematic diagrams, not physical diagrams, and should not be construed as limiting the present invention; to better illustrate the embodiments of the present invention, some components in the drawings will be omitted, enlarged, or reduced, which does not represent the dimensions of the actual product; for those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0036] In the drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components; in the description of the present invention, it should be understood that if there are terms such as "upper", "lower", "left", "right", "front", "rear", etc. indicating the orientation or positional relationship, they are based on the orientation or positional relationship shown in the drawings. This 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 operated in a specific orientation. Therefore, the terms describing the positional relationship in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those of ordinary skill in the art, the specific meanings of the above terms can be understood according to specific circumstances.

[0037] Please refer to Figure 1 , the present invention includes the following steps:

[0038] Step 1: System composition and initialization settings. Node configuration: Install ultra-wideband (UWB) devices on the safety helmets of each worker as mobile sub-nodes; install fixed UWB terminals at the entrance or key positions of the tunnel as main nodes. The main node and the sub-nodes are collectively referred to as nodes. Assume that there is a fixed main node O at the entrance of the tunnel, and other sub-nodes, such as Figure 2 shown. Use the TWR technology of UWB to measure the distance to each other, and take the shortest distance as the main reference distance (with the smallest error). And use statistical methods to eliminate abnormal errors. Assume that the ranging error ∈ i,i+1 obeys a Gaussian distribution:

[0039]

[0040] represents 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, which can be a straight line or a curve with a known curvature. The position of the terminal node is known as (x0, y0). The position of the first child node is estimated based on the distance measurement d 0,1 between adjacent nodes, and to improve the robustness of the algorithm and enhance the optimization effect, a random small offset angle θ1 is introduced:

[0043]

[0044] where the subscript 1 of represents the first child node, and the superscript (0) represents the initial value of the iteration. Since there will be an iterative process in the subsequent algorithm, the superscript for the first iteration will be (1), and so on. θ1 is a random value, usually taking a small value (e.g., |θ1| ≤ 5°) to ensure that the node is within the tunnel width range. For the i-th link node (i ≥ 2), based on the position of the previous node and the ranging value d i-1,i its initial position is estimated:

[0045]

[0046] where θ i is the random offset angle of the i-th node relative to the previous node, also taking a small value to ensure that the node position is within the tunnel width range. To ensure that the initial positions of all link nodes are within the tunnel width range, if the lateral offset of a certain node exceeds the tunnel boundary (i.e., where w is the tunnel width), then it is adjusted to the boundary position:

[0047]

[0048] where is the sign function.

[0049] Step 3: Nonlinear least squares optimization

[0050] The Nonlinear Least Squares (NLS) method 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 differences between the actual distances and the measured distances between all link nodes:

[0051]

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

[0053]

[0054] The elements of the Jacobian matrix are:

[0055]

[0056] Step 4: Introduction of tunnel terrain constraints and optimization of the positioning model. To improve the positioning accuracy, the tunnel terrain data is combined as an additional constraint condition to ensure that the positions of the link nodes conform 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 offset δ i of each node satisfies:

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

[0059]

[0060] where w is the tunnel width. Then, a penalty term for terrain constraints is added to the non-linear least squares objective function to form a comprehensive optimization model:

[0061]

[0062] where λ is a weight factor used to balance the importance of ranging error and terrain constraints. To more effectively balance the ranging error and terrain constraints, during the non-linear least squares optimization process, we propose to dynamically adjust the weight factor λ according to the ranging error. The specific method is as follows:

[0063]

[0064] where ν > 1 is an adjustment factor used to control the adjustment amplitude of the weight factor.

[0065] In the simulation, it is judged according to the relative change rate of the objective function: It can be considered valid. Where k represents the k-th iteration. τ is an empirical value, 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 the parameters: Update the node coordinates according to the iteration formula of the Levenberg-Marquardt (LM) algorithm:

[0071]

[0072] where r is the current residual vector, μ is the damping factor, and I is the identity matrix.

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

[0074] 6) Termination condition: Stop the optimization when the change amount of the residual is lower than the preset threshold or the maximum number of iterations is reached.

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

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

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

[0078]

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

[0080]

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

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

[0083] where λ is the coefficient controlling the weight of the penalty term. Then iterative optimization is carried out, 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 the Ceres Solver;

[0085] 2) Build the problem: Build an 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, and Ceres will update the node positions iteratively to minimize the total loss;

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

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

[0090] Step 6: Dynamic Monitoring and Relocalization. Real-time monitor the ranging data between nodes, and calculate the change between the current position and the previous position. Set a threshold value. When the change in the node position exceeds the threshold, it is determined as abnormal. Once an abnormal node position is detected, trigger the relocalization algorithm to re-estimate the two-dimensional coordinates of the abnormal node. Combine the current ranging data and historical data for global optimization to ensure that the overall positioning accuracy of the system is not affected by local anomalies.

[0091] Step 7: Multi-sensor Data Fusion. During relocalization, to further improve the positioning accuracy, combine the data of other sensors such as the Inertial Measurement Unit (IMU) to further enhance the accuracy and robustness of the positioning system. First, read the acceleration and angular velocity data from the IMU and perform preprocessing to remove noise and drift; then use the IMU data for state prediction. The state can be represented as the position and orientation of the node, and the update formula is:

[0092]

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

[0094] where p is the position, v is the velocity, a is the acceleration, θ is the orientation, ω 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 according to the graph optimization results. The initial positions and states of the particles come from the output results of Step 4;

[0096] 2) Prediction: Each particle performs state prediction according to the IMU data, updates the position and orientation, and the prediction formula is the same as the IMU fusion process.

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

[0098]

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

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

[0101] 5) State Estimation: Calculate the final state of the particles through weighted averaging as the fused positioning result:

[0102]

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

[0104] Figure 3 It is a simulation result graph of the present invention; Figure 4 It 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 solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the purpose and scope of the present technical solution, and they should all be covered within the scope of the claims of the present invention.

Claims

1. A chain - type two - dimensional positioning system based on ultra - wideband signals, characterized in that: The system includes: A master node, installed at the tunnel entrance or a critical location, for transmitting and receiving ultra-wideband signals; Slave nodes, installed on workers' safety helmets or other mobile devices, for receiving the ultra-wideband signals transmitted by the master node and ranging with adjacent slave nodes; A control unit, connected to the master node and the slave nodes, for receiving the ranging data and performing positioning calculations to output node position information; The control unit performs positioning calculations using a non-linear least squares optimization algorithm; The control unit performs positioning calculations using a graph optimization algorithm; The control unit optimizes the positioning result using an error correction and filtering method; The slave node further includes an inertial measurement unit for providing node motion state information; A chain two-dimensional positioning method based on the chain two-dimensional positioning system includes the following steps: Step 1: Initialize the nodes, including the master node and the slave nodes. The master node sends ultra-wideband signals, and the slave nodes receive the signals and measure distances with adjacent slave nodes. Assume there is a fixed master node O at the tunnel entrance, which measures distances with other slave nodes using the TWR technology of UWB, and takes the shortest distance as the reference distance. And use statistical methods to eliminate abnormal errors. Assume the ranging error ∈ i,i+1 obeys a Gaussian distribution: Indicates the measured distance, d i,i+1 represents the true distance, σ 2 represents the variance of the Gaussian distribution; It is a standard Gaussian distribution symbol; the RANSAC algorithm is used to eliminate abnormal ranging data to ensure the accuracy of the optimization process; Step 2: Initial position estimation; assume that the known tunnel geometry is a straight line or a curve with a known curvature; the position of the terminal node is known as (x0, y0); the position of the first child node is estimated based on the distance measurement d between adjacent nodes 0,1 and, to improve the robustness of the algorithm and enhance the optimization effect, a random small offset angle θ1 is introduced: Among them, The subscript 1 of indicates the first child node, and the superscript (0) indicates the initial value of the iteration. For the first iteration, the superscript is (1), and so on; θ1 is a random value, |θ1| ≤ 5°, to ensure that the node is within the tunnel width range; for the i-th link node, i ≥ 2, according to the position of the previous node and the ranging value d i-1,i estimate its initial position: where, θ i is the random offset angle of the i-th node relative to the previous node, |θ i | ≤ 5°, to ensure that the node position is within the tunnel width range; to ensure that the initial positions of all link nodes are within the tunnel width range, if the lateral offset of a certain node exceeds the tunnel boundary, that is where w is the tunnel width, then adjust it to the boundary position: Among them, is sign function of; Step 3: Apply non-linear least squares optimization (NLS) to optimize the node positions to minimize the ranging error; the optimization objective function is defined as the sum of the squares of the differences between the actual distances and the measured distances between all link nodes: where N is the total number of link nodes, (x i , y i ) is the two-dimensional coordinate of the i-th link node, and d i,i+1 is the ranging value between the i-th and the (i + 1)-th nodes; the Levenberg-Marquardt algorithm is used for non-linear least squares optimization; a Jacobian matrix J of the objective function J(x, y) with respect to the optimization variables x and y is constructed, that is, the partial derivative of each ranging error with respect to each coordinate variable; for each ranging error term, there is: The elements of the Jacobian matrix are: Step 4: Introduce tunnel terrain constraints and optimize the positioning model; assume the tunnel centerline is defined as a known function y = f(x); The lateral offset δ of each node i Satisfies: y i = f(x i ) + δ i where w is the tunnel width; add a penalty term for terrain constraints to the non-linear least squares objective function to form a comprehensive optimization model: where λ is a weight factor used to balance the importance of the ranging error and the terrain constraints; during the non-linear least squares optimization process, a method for dynamically adjusting the weight factor λ according to the ranging error is proposed as follows: where ν > 1 is an adjustment factor used to control the adjustment amplitude of the weight factor; In the simulation, it is judged according to the relative change rate of the objective function: then it is considered valid; where k represents the k-th iteration; τ is an empirical value set to 0.01; The optimization iteration steps are as follows: 1) Initialization: Use the initial position estimate as the optimization starting point (x (0) , y (0) ); 2) Calculate the residual: Calculate the current residual value of the objective function J′(x, y); 3) Calculate the Jacobian matrix: Calculate the Jacobian matrix J according to the current node positions; 4) Update the parameters: Update the node coordinates according to the iteration formula of the Levenberg-Marquardt (LM) algorithm: where r is the current residual vector, μ is the damping factor, and I is the identity matrix; 5) Adjust the damping factor: Dynamically adjust μ according to the effectiveness of the optimization step size to ensure the convergence and stability of the algorithm; 6) Termination condition: Stop the optimization when the change in the residual is lower than a preset threshold or the maximum number of iterations is reached; Step 5: Establish a graph optimization model; transform the chain localization problem into a graph optimization problem, and achieve a globally consistent node position estimation by constructing a graph structure of nodes and edges; a node is defined as the two-dimensional position (x i , y i ) of each node in the graph; an edge is defined as the ranging relationship d i,i+1 between adjacent nodes in the graph, and a terrain constraint edge is introduced; then use Ceres Solver for graph optimization; the error model is positioned as the Euclidean distance model, that is, the measurement error of the edge is defined as: e i,i+1 = ||p i -p i+1 || -d i,i+1 where p i and p i+1 are the current estimated positions of node i and node j respectively; the loss function is the distance error: calculate the sum of the squared errors of all edges: The penalty term is used to constrain the position change of the nodes and is expressed as: Among them, is the position of node k in the previous iteration; the final optimization goal is: minimize Loss total = Loss dist + λ·Loss penalty where λ is the coefficient controlling the weight of the penalty term; then iterative optimization is performed, and the specific process is as follows: 1) Pass in the initial solution: Use the optimization result of the LM algorithm as the initial solution of the Ceres Solver; pass the initial positions of all nodes to the Ceres Solver; 2) Construct the problem: Construct an optimization problem in the Ceres Solver and add nodes and edges to the optimizer; 3) Set the optimization options: Include the number of iterations, convergence criteria, and solver options; set the maximum number of iterations to 500 and the convergence criterion to an error change less than 0.001; 4) Call the optimization function: Execute the optimization process, and the Ceres Solver will update the node positions iteratively to minimize the total loss; 5) Convergence judgment: Set the convergence standard to determine whether the optimization ends; if the convergence condition is met, the optimization ends; 6) Output results: After the optimization is completed, Ceres Solver will return the final position of each node; the output results include: the optimized node coordinates, the statistical information of the optimization process: the total number of iterations and the final loss value; 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; when an abnormal node position 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; Step 7: Multi-sensor data fusion; During relocalization, read acceleration and angular velocity data from the inertial measurement unit (IMU) and preprocess to remove noise and drift; then use the IMU data for state prediction; the state is represented as the position and orientation of the node, and the update formula is: θ new = θ old + ω·Δt 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; the particle filter is combined with the IMU and graph optimization results. The specific steps are as follows: 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; 2) Prediction: Each particle predicts its state based on the IMU data, updates its position and orientation, and the prediction formula is the same as the IMU fusion process; 3) Update: Weight calculation: Calculate the weight of each particle based on the current particle position and the actual measurement value; the weight is determined based on the measurement error: where z is the measured value, h(p i ) is the expected measurement in the particle state, and σ is the standard deviation of the measurement noise; 4) Resampling: Resample the particles according to their weights, retain high-weight particles, and remove low-weight particles to prevent particle degradation; 5) State estimation: The final state of the particle is calculated by weighted average as the positioning result after fusion: Combining IMU data and particle filtering can improve the real-time and accuracy of positioning; the dynamic information provided by IMU helps correct position changes, and particle filtering enhances robustness to nonlinearity and noise.

Images