A cooperative localization method for multiple mobile nodes without anchor points

Abstract

This invention proposes a multi-mobile node cooperative localization method without anchor points. It uses autonomous sensors to obtain displacement information of each mobile node based on its previous position; uses ranging sensors to obtain distance information between the mobile nodes; and employs the least squares method to fuse the relative positions with distance-preserving relative positions to establish a cost function. The analytical solution of the optimal solution of the cost function is then calculated to obtain the optimized target position of each node at the current moment. The relative position at the next moment is inferred based on the optimized target position, and this process is iterated to perform multi-mobile node fusion localization.

Description

Technical Field

[0001] This invention relates to the field of multi-target positioning technology, and in particular to a method for collaborative positioning of multiple moving nodes without anchor points. Background Technology

[0002] With social development and continuous technological advancements, the demand for location-based services (LBS) is increasing. This is manifested in two ways: first, the requirement for positioning accuracy is rising; for example, 10-centimeter indoor positioning accuracy is a key indicator for 6G (6th generation mobile networks); second, the types of positioning are increasing due to the growing number of scenarios requiring positioning. Especially in recent years, with the continuous development of technologies such as artificial intelligence, sensors, and microelectronic devices, multi-node systems like swarm intelligence, wireless sensor networks, and the Industrial Internet have received widespread attention both domestically and internationally, and related technologies are developing rapidly. Among these, multi-node positioning technology, as a type of positioning, has become a crucial supporting technology for multi-node systems.

[0003] In multi-node positioning systems, nodes with known coordinates are called anchor nodes. These typically require pre-deployment and installation, or rely on infrastructure (GPS, BeiDou Navigation Satellite System, etc.) to sense their own position coordinates in real time. However, many multi-node positioning system applications cannot provide anchor nodes. Firstly, in many complex and special scenarios such as assault operations and fire rescue, anchor nodes are difficult to pre-deploy and install. Secondly, while GPS is often used as infrastructure to obtain the real-time position coordinates of anchor nodes, anchor nodes cannot be provided in scenarios with complex electromagnetic environments, strong satellite interference, or satellite-denied conditions such as military locations, important industrial areas, and complex terrain environments.

[0004] Therefore, anchorless positioning technology has become one of the important research problems in multi-node positioning systems and has received increasing attention. There are many existing anchorless positioning algorithms. Gradient diffusion-based anchorless positioning algorithms use mathematical calculations instead of measurements to obtain position information. This algorithm does not require measurement sensors and has a simple system structure, but lacks measurement information, resulting in low accuracy. Cluster-based distributed anchorless positioning technology uses a node density-based algorithm for clustering. This algorithm combines angle and distance measurements to calculate the coordinates of all nodes in the regional coordinate system. Optimal stochastic Newton-Raphson positioning algorithm and MDS-MAP algorithm obtain the relative topological structure of nodes through ranging information. This method can obtain the relative position coordinates of nodes at each moment, but there is no correlation between the relative positions of adjacent times. Methods that estimate node position information by obtaining node motion parameters through autonomous sensors (inertial sensors, encoders, etc.) provide position information that is correlated between adjacent times and has high positioning accuracy in short periods, but suffer from error accumulation and is prone to divergence over long periods.

[0005] This method considers the case where the distance between nodes is measurable. Addressing the time-series independence issue and the accumulation of positioning errors by autonomous sensors in the MDS-MAP algorithm, it fuses the displacement of nodes within a certain time period obtained from autonomous sensors with the ranging information obtained from external ranging sensors to obtain the node's position coordinates at a specific moment. Specifically, the positioning process is iterative, with each iteration divided into two stages: information measurement and position update. In the position update stage, displacement and distance information are fused. A prior estimate of the position is obtained from the displacement value. The MDS-MAP algorithm is then used to convert the distance information into relative coordinates of the nodes, i.e., the topology. A cost function is then established based on the topology and the prior estimate of the position, and fitted using the least squares method to obtain the node's position estimate at the current moment. Summary of the Invention

[0006] To address the aforementioned technical problems, this invention also proposes an anchor-free multi-moving-node cooperative positioning method, comprising:

[0007] Displacement information of each moving node is obtained using autonomous sensors, with the previous position as a reference.

[0008] Use a ranging sensor to obtain distance information between each mobile node;

[0009] Using the least squares method, the relative position and the distance-preserving relative position are merged to establish a cost function, and the analytical solution of the optimal solution of the cost function is calculated to obtain the optimized target position of each node at the current time.

[0010] The relative position at the next moment is inferred based on the optimized target position, and this process is iterated to perform multi-moving node fusion positioning.

[0011] Furthermore, the prior estimate of the node position at the current moment is calculated using the displacement information obtained from the autonomous sensor;

[0012] The relative topological structure of the node position at the current moment is calculated using the distance information obtained from the ranging sensor, and is represented by two-dimensional Cartesian coordinates;

[0013] A cost function is established using the aforementioned topology and prior estimates, and then fitted using the least squares method to obtain the position estimate of the node at the current time.

[0014] Furthermore, the process of calculating the prior estimate of the node position at the current moment using the displacement information obtained from the autonomous sensor is as follows:

[0015] Let the position coordinate estimation vector be P. k , For P k Posterior estimation:

[0016]

[0017]

[0018] in For P k The prior estimate, ΔP k The displacement vector is obtained using an autonomous sensor.

[0019] Furthermore, the process of calculating the relative topological structure of the node position at the current moment using the distance information obtained from the ranging sensor is as follows: Assume the ranging information forms the following matrix:

[0020]

[0021] Where d ij This represents the distance between node i and node j.

[0022] For D (2) The bicentering process yields matrix B, which contains elements b. ij With D (2) element d in ij 2 The following relationship exists:

[0023]

[0024] Perform singular value decomposition on B, B = VAV, where v is a unitary matrix;

[0025] The singular values ​​in the diagonal matrix A are arranged in descending order from the top left. Let VA be the singular values. 1 / 2The first two columns serve as the two-dimensional Cartesian coordinates X relative to the topology at time k. k .

[0026] Furthermore, the following cost function is established:

[0027]

[0028] in or Let p0 be the rotation factor, then p0 = [x0 y0] T θ is the translation factor, and θ is the rotation angle. n It is an n-dimensional column vector in which all elements are 1.

[0029] Furthermore, in order to obtain an estimate of the absolute position, we first find θ and p0 that minimize J, let:

[0030]

[0031]

[0032] Furthermore, assuming the error of the ranging sensor is much smaller than the error of the displacement measurement value in one iteration cycle, then:

[0033]

[0034] in k∈Z and Choose the θ that minimizes J.

[0035] Beneficial effects of the present invention

[0036] 1. This method can obtain the relative positions between multiple mobile nodes without using anchor nodes. In emergency scenarios, it can provide location services for collaboration between multiple mobile nodes without the need for infrastructure deployment.

[0037] 2. This method uses autonomous sensors and ranging sensors to perform fusion positioning of multiple mobile nodes. It can also perform independent positioning in satellite-denied and indoor scenarios, thus expanding the application scope of location services.

[0038] 3. This method, through fusion optimization, not only ensures the continuity of the trajectory under distance constraints when multiple nodes move, but also reduces the position drift caused by the accumulation of errors from autonomous sensors, thereby improving the availability of positioning. Attached Figure Description

[0039] To more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0040] Figure 1 This is a multi-target localization system in two-dimensional space, taking three nodes as an example.

[0041] Figure 2 A schematic diagram illustrating how measurements are updated based on distance information between nodes;

[0042] Figure 3 This is a schematic diagram of the algorithm flow for the iterative process;

[0043] Figure 4 This is a schematic diagram of the trajectory of a node in a simulation experiment of collaborative localization of 15 targets without anchor points.

[0044] Figure 5 A comparison chart of the mean squared errors obtained from 200 Monte Carlo experiments;

[0045] Figure 6 This is a comparison chart of the mean square error for different numbers of nodes in this invention. Detailed Implementation

[0046] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0047] In the accompanying drawings of specific embodiments of the present invention, in order to better and more clearly describe the working principle of each component in the system and show the connection relationship of each part in the device, only the relative positional relationship between each component is clearly distinguished. It does not constitute a limitation on the signal transmission direction, connection sequence, or size, dimension, and shape of each part within the component or structure.

[0048] This invention proposes an anchor-free positioning framework that integrates autonomous sensors and distance measurement. The framework uses an autonomous sensor positioning model to obtain the autonomously inferred relative position of each mobile node based on its previous position. It uses a ranging sensor to obtain distance information between the mobile nodes and calculates the distance-preserving relative position between nodes using a multi-target positioning model. Utilizing the least squares (LS) method, the autonomously inferred relative position and the distance-preserving position are fused to establish a cost function optimization problem for this position estimation problem, and an analytical solution to the optimal solution is given. Based on solving the optimization problem, an iterative method is proposed to achieve multi-target positioning. Specifically, the cost function for the current moment is established using the optimal solution from the previous moment, the sum of displacement information from the autonomous sensors, and the distance measurement information from the external sensors at the current moment as parameters. This yields the optimized target position of each node at the current moment, and the relative position for the next moment is inferred from this position. This process is iterated to perform multi-mobile node fusion positioning.

[0049] like Figure 1 The figure shows a multi-target localization system in two-dimensional space, using three nodes as an example. The hollow triangle represents the initial state of the three nodes, whose initial positions are known; the shaded triangle represents the state of the node at a later time step; the solid triangle represents the state of the node at a later time step; and the black directed solid line segment represents the displacement of each node within the corresponding time period, which can generally be measured by an autonomous sensor.

[0050] Assuming that the interaction information between nodes is the displacement value obtained by each node through the autonomous sensor in one iteration cycle, rather than the direct data of the autonomous sensor, this can reduce the information exchange in the cooperative localization process and thus ensure the robustness of the cooperative localization process.

[0051] For a positioning system with n nodes, consider the following vector:

[0052]

[0053]

[0054] in Let be the coordinates of node i at time k. Let be the displacement measurement value of node i at time k obtained by the autonomous sensor. Clearly, without considering the error of the autonomous sensor, theoretically, given the initial position of each node, the position of each node at any time can be obtained. However, in reality, the displacement measurement values ​​of the nodes have errors within each time interval, and the position estimation error gradually increases with time.

[0055] Because autonomous sensors lack external information for correction, errors accumulate over time, causing the multi-target localization results to diverge. To address this issue, this application proposes a general iterative process, updating the measurements obtained by the autonomous sensors with distance information between nodes in each iteration. That is, each iteration consists of two stages: information measurement and position update.

[0056] During the location update phase, such as Figure 2 As shown, the specific steps are as follows:

[0057] (1) Calculate the prior estimate of the node position at the current moment using the displacement information obtained by the autonomous sensor;

[0058] (2) The relative topological structure of the node position at the current moment is obtained by using the distance information obtained by the ranging sensor. The relative topological structure is represented by two-dimensional Cartesian coordinates.

[0059] (3) Establish a cost function based on the prior estimates of topology and location, and fit it using the least squares method to obtain the location estimate of the node at the current time.

[0060] The hollow triangle represents the true state of the three nodes at a certain moment; the shaded triangle represents the prior estimate of the position coordinates of the three nodes at that moment; the black triangle represents the relative topology obtained by using the MDS-MAP algorithm with distance information, where the distance information does not depend on the ranging method and many types of ranging techniques such as infrared ranging, UWB ranging, acoustic ranging, and Zigbee ranging can be used; the state of the solid triangle represents the posterior estimate of the estimated position obtained using this method.

[0061] The iterative process of this method, specifically the steps performed in one iteration, are described below:

[0062] 1. Obtain a priori estimate of the node position at the current moment using displacement information obtained from autonomous sensors;

[0063] Suppose the position coordinate estimation vector is as follows:

[0064]

[0065] For P k The posterior estimate is then:

[0066]

[0067] in For P k The prior estimate, ΔP k The displacement vector is obtained using an autonomous sensor.

[0068] 2. The relative topology of the node position at the current moment is obtained by using the distance information obtained by the ranging sensor. The relative topology is represented by two-dimensional Cartesian coordinates, that is, the relative position coordinates. It is worth noting that the relative position coordinates are a translational, rotational and mirrored version of the position coordinates with the position topology unchanged.

[0069] Suppose the ranging information is structured into the following matrix:

[0070]

[0071] Where d ij Let X be the distance between node i and node j. The MDS-MAP algorithm is used to obtain the two-dimensional Cartesian coordinates X of the relative topology of the nodes at time k. k :

[0072] X k =[a1 a2 L a] n ] T ;

[0073] in Let be the relative position coordinates of node i.

[0074] Where n is the number of nodes, 1≤i≤n, and the superscript r indicates that the coordinate value is a relative position coordinate, relative to the position coordinate x. i y i There is a distinction.

[0075] Specifically, for D (2) The bicentering process yields matrix B, which contains elements b. ij With D (2) element d in ij 2 The following relationship exists:

[0076]

[0077] And there are:

[0078]

[0079] Then, perform singular value decomposition (SVD) on B. SVD decomposes a matrix into two unitary matrices and a diagonal matrix of singular values, i.e., B = VAV, where v is a unitary matrix, and the singular values ​​in the diagonal matrix A are arranged in descending order from the top left. Then, in the case of two-dimensional space, VA... 1 / 2 The first two columns of the result are the two-dimensional Cartesian coordinates of the relative topology at time k, represented by X. k express.

[0080] 3. Establish a cost function based on the prior estimates of topology and location, and fit it using the least squares method to obtain the location estimate of the node at the current time.

[0081] Considering the translational, rotational, and mirror-image relationships between the relative position coordinates obtained from ranging information and the true coordinates, the following cost function is established using the idea of ​​least squares:

[0082]

[0083] in or Let p0 be the rotation factor, then p0 = [x0 y0] T This is the translation factor.

[0084] Relative position coordinates are a translational, rotational, and mirrored version of position coordinates. Therefore, relative position coordinates obtained solely through distance measurement cannot provide an estimate of position coordinates. Thus, the translation, rotation, and mirroring factors of relative position coordinates are estimated using prior estimates of position coordinates obtained from autonomous sensors.

[0085] θ is the rotation angle. After determining the mirror relationship, the overall relative coordinates are rotated by θ and then translated by p0 to coincide with the position coordinates.

[0086] 1 n Let J be an n-dimensional column vector with all elements equal to 1. To obtain an estimate of the absolute position, we first find θ and p0 that minimize J, i.e., let:

[0087]

[0088]

[0089] Assuming the error of the ranging sensor is much smaller than the error of the displacement measurement value over one iteration cycle, then:

[0090]

[0091] in k∈Z and Choose the θ that minimizes J.

[0092] The algorithm flowchart of the iterative process is shown below. Figure 3 As shown.

[0093] For the method described in this invention, a multi-mobile node cooperative localization simulation experiment with 15 mobile nodes was conducted in a two-dimensional space. The assumptions are as follows:

[0094] 1. Each node moves at a constant speed, and its trajectory is a rectangle;

[0095] 2. The errors in the displacement measurements of each iteration cycle are independent of each other;

[0096] 3. The noise model of the ranging sensor is the same for each node;

[0097] 4. The error of the distance measuring sensor is much smaller than the error of the displacement measurement value.

[0098] Assumption 4 is based on existing wireless positioning accuracy and autonomous sensor navigation accuracy. Simulation results are as follows: Figure 4 As shown, Figure 4 The diagram shows the trajectory of a node in a simulation experiment of cooperative localization of 15 targets without anchor points. In this experiment, each node moves in a rectangular motion at a constant speed. The dashed line trajectory is the actual motion trajectory, the thin solid line trajectory is the trajectory estimated by autonomous sensor navigation only, and the thick solid line trajectory is the trajectory estimated by our method.

[0099] Figure 5 The figure shows a comparison of the mean square error obtained from 200 Monte Carlo experiments. The solid line with triangles represents the mean square error variation curve of the autonomous sensor navigation alone, while the solid line with rectangles represents the mean square error variation curve of the proposed method. Observation of the trajectory diagrams and analysis of the results show that this method significantly improves the absolute position positioning accuracy of multiple moving nodes without anchor points.

[0100] To illustrate the conclusion that positioning accuracy increases with the number of nodes, we compared the mean square error for 5, 15, and 30 nodes. The experimental results are shown below. Figure 6 As shown.

[0101] Simulation results show that the positioning accuracy of this method increases with the number of nodes, and the absolute position positioning is less prone to divergence.

[0102] In the above embodiments, implementation can be achieved entirely or partially through software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented entirely or partially in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of this application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted through the computer-readable storage medium. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid-state disk (SSD)).

[0103] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in this application, and these modifications or substitutions should all be covered within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for collaborative localization of multiple mobile nodes without anchor points, characterized in that, include: Displacement information of each moving node is obtained using autonomous sensors, with the previous position as a reference. The prior estimate of the node position at the current moment is calculated using the displacement information obtained from the autonomous sensor; Use a ranging sensor to obtain distance information between each mobile node; The relative topological structure of the node position at the current moment is calculated using the distance information obtained from the ranging sensor, and is represented by two-dimensional Cartesian coordinates; A cost function is established using the relative topology and prior estimates, fitted using the least squares method, and the analytical solution of the optimal solution of the cost function is calculated to obtain the optimized target position of each node at the current time. The relative position at the next moment is inferred based on the optimized target position, and this process is iterated to perform multi-moving node fusion positioning.

2. The anchorless multi-moving node cooperative positioning method according to claim 1, characterized in that, The process of calculating the prior estimate of the node position at the current moment using the displacement information obtained from the autonomous sensor is as follows: Let the position coordinate estimation vector be P. k , For P k Posterior estimation: in For P k The prior estimate, ΔP k The displacement vector is obtained using an autonomous sensor.

3. The anchorless multi-moving node cooperative positioning method according to claim 1, characterized in that, The process of calculating the relative topological structure of the node position at the current moment using the distance information obtained from the ranging sensor is as follows: Assume the ranging information forms the following matrix: Where d ij This represents the distance between node i and node j. For D (2) The bicentering process yields matrix B, which contains elements b. ij With D (2) element d in ij 2 The following relationship exists: Perform singular value decomposition on B, B = VAV, where v is a unitary matrix; The singular values ​​in the diagonal matrix A are arranged in descending order from the top left. Let VA be the singular values. 1 / 2 The first two columns serve as the two-dimensional Cartesian coordinates X relative to the topology at time k. k .

4. The anchorless multi-moving node cooperative positioning method according to claim 1, characterized in that, Establish the following cost function: in or Let p0 be the rotation factor, then p0 = [x0 y0] T θ is the translation factor, and θ is the rotation angle. n It is an n-dimensional column vector in which all elements are 1.

5. The anchorless multi-moving node cooperative positioning method according to claim 4, characterized in that, To obtain an estimate of the absolute position, we first find θ and p0 that minimize J. Let:

6. The anchorless multi-moving node cooperative positioning method according to claim 5, characterized in that, Assuming the error of the ranging sensor is much smaller than the error of the displacement measurement value in one iteration cycle, then: in and Choose the θ that minimizes J.

Images