Distributed dynamic relative positioning method and system, electronic device and storage medium

Abstract

The application provides a distributed dynamic relative positioning method, system, electronic equipment and storage medium, the method comprises the following steps: obtaining continuous distance measurement information and angle measurement information between a to-be-positioned node and neighbor nodes of the to-be-positioned node, and inertial navigation measurement information of the to-be-positioned node through real-time observation; determining initial time distance measurement information and initial time angle measurement information between the to-be-positioned node and the neighbor nodes in the continuous distance measurement information and the angle measurement information, and initializing a relative pose of a node network based on the initial time distance measurement information and the initial time angle measurement information to obtain an initial relative pose estimation; and performing real-time positioning on the to-be-positioned node by using a multi-source information fusion technology based on the initial relative pose estimation, the continuous distance measurement information and the angle measurement information obtained through continuous observation, and the inertial navigation measurement information. High three-dimensional relative positioning accuracy and robustness are achieved.

Description

Technical Field

[0001] This invention relates to the field of positioning technology, and in particular to a distributed dynamic relative positioning method, system, electronic device, and storage medium. Background Technology

[0002] Real-time, high-precision location information is a common need in the development of modern information society. Positioning technology has important applications in scenarios such as intelligent navigation, warehousing and logistics, smart factories, and emergency rescue.

[0003] According to relevant technologies, satellite-based navigation systems are the most common positioning systems, but they are generally suitable for open outdoor environments. In complex environments such as urban canyons or indoors where satellite signals are blocked or denied, positioning performance and stability will be severely degraded. Ground-based wireless network positioning systems can provide better high-precision positioning services, but they are easily limited by geographical environment and deployment costs. Therefore, current positioning methods are often constrained by environmental and infrastructure limitations, resulting in low positioning accuracy.

[0004] Therefore, finding a high-precision and robust relative positioning method has become a research hotspot. Summary of the Invention

[0005] This invention provides a distributed dynamic relative positioning method, system, electronic device, and storage medium to solve the defects of existing technologies, such as mismatch of coordinate references between distributed nodes, limited relative positioning performance, and limited efficiency of multi-source information fusion, and achieves higher three-dimensional relative positioning accuracy and robustness.

[0006] This invention provides a distributed dynamic relative positioning method. It involves real-time observation of continuous distance and angle measurements between a node to be positioned and its neighboring nodes, as well as inertial navigation measurement information of the node to be positioned. The neighboring nodes represent nodes capable of communicating with the node to be positioned. Initial distance and angle measurements between the node to be positioned and its neighboring nodes are determined from the continuous distance and angle measurements. Based on these initial distance and angle measurements, the relative pose of the node network is initialized to obtain an initial relative pose estimate, where the node network is determined according to the node to be positioned. Based on the initial relative pose estimate, the continuously observed distance and angle measurements, and the inertial navigation measurement information, multi-source information fusion technology is used to perform real-time positioning of the node to be positioned.

[0007] According to a distributed dynamic relative positioning method provided by the present invention, the step of initializing the relative pose of a node network based on the initial distance measurement information and the initial angle measurement information to obtain an initial relative pose estimate specifically includes: selecting an initial node from a plurality of nodes to be positioned based on the initial distance measurement information and the initial angle measurement information, and constructing a well-conditioned initial reference coordinate system based on the initial node; estimating the relative pose of the remaining nodes in the initial reference coordinate system to obtain the relative pose estimate of the remaining nodes, wherein the remaining nodes are the nodes other than the initial node among the plurality of nodes to be positioned; and obtaining the relative pose estimate of the initial node. The algorithm calculates the relative pose estimation of the initial node and the relative pose estimation of the remaining nodes, and then calculates the location confidence of each node to be located. The algorithm treats the initial node and the remaining nodes as network nodes and iteratively performs steps to estimate the relative pose of the network nodes, obtain relative pose estimates, and calculate the location confidence of each network node based on the relative pose estimates, until the obtained global average location confidence converges. The global average location confidence is determined based on the location confidence of each network node. Based on the relative pose estimates of the network nodes under the condition of global average location confidence convergence, the relative pose of the node network is initialized to obtain an initial relative pose estimate.

[0008] According to a distributed dynamic relative positioning method provided by the present invention, the step of selecting an initial node from a plurality of nodes to be positioned based on the initial time distance measurement information and the initial time angle measurement information specifically includes: obtaining the polyhedral geometric structure constructed by the nodes to be positioned through geometric relationships based on the initial time distance measurement information and the initial time angle measurement information; determining the degree of structural ill-conditioning of the polyhedral geometric structure according to the geometric condition number of the polyhedral geometric structure; determining the polyhedral geometric structure whose degree of structural ill-conditioning is less than a degree threshold as a benign polyhedral geometric structure; and selecting an initial node from a plurality of nodes to be positioned based on the benign polyhedral combination structure.

[0009] According to a distributed dynamic relative positioning method provided by the present invention, the method involves real-time positioning of a node to be positioned using multi-source information fusion technology, based on the initial relative pose estimation, distance and angle measurement information obtained from continuous observations, and inertial navigation measurement information. Specifically, this includes: when the distance and angle measurement information obtained from continuous observations are collected, real-time positioning of the node to be positioned is performed based on the initial relative pose estimation and complete observation data using multi-source information fusion technology. The complete observation data includes the distance measurement information at the current moment, the angle measurement information at the current moment, and the inertial navigation measurement information from the previous moment; when the inertial navigation measurement information is collected, trajectory estimation is performed based on the inertial navigation measurement information to achieve real-time positioning of the node to be positioned.

[0010] According to a distributed dynamic relative positioning method provided by the present invention, the step of performing real-time positioning of the node to be positioned based on the initial relative pose estimation and complete observation data using multi-source information fusion technology specifically includes: performing real-time positioning of the node to be positioned based on the initial relative pose estimation and complete observation data using multi-source information fusion technology through filtering estimation processing.

[0011] According to a distributed dynamic relative positioning method provided by the present invention, the method involves real-time positioning of a node to be positioned based on an initial relative pose estimation and complete observation data, using multi-source information fusion technology and filtering estimation processing. Specifically, the method includes: initializing a coordinate transformation parameter table based on the initial relative pose estimation, wherein the coordinate transformation parameter table is used to achieve benchmark alignment of the local reference coordinate systems among the nodes to be positioned, and the local reference coordinate system is used to characterize the reference coordinate system for positioning from the perspective of each node to be positioned; receiving the state estimates of each neighboring node of the node to be positioned; achieving benchmark alignment of the local reference coordinate systems of the state estimates of the neighboring nodes according to the coordinate transformation parameter table and the state estimates of the neighboring nodes, obtaining benchmark-aligned state estimates of the neighboring nodes; and performing real-time filtered self-positioning of the node to be positioned based on the benchmark-aligned state estimates of the neighboring nodes and the complete observation data.

[0012] According to a distributed dynamic relative positioning method provided by the present invention, after performing real-time filtered self-localization on the node to be located, the method further includes: correcting the state estimates of each neighboring node of the node to be located based on the positioning result of the filtered self-localization, complete observation information, and angle measurement information of each neighboring node, to obtain corrected state estimates; updating the coordinate transformation parameter table based on the positioning result of the filtered self-localization and the corrected state estimates of each neighboring node, to obtain an updated coordinate transformation parameter table; aligning the local reference coordinate system of the state estimates of the neighboring nodes with the updated reference alignment based on the updated coordinate transformation parameter table and the state estimates of each neighboring node, to obtain updated reference-aligned state estimates of the neighboring nodes; the real-time filtered self-localization of the node to be located based on the reference-aligned state estimates of the neighboring nodes and the complete observation data specifically includes: performing real-time filtered self-localization of the node to be located based on the updated reference-aligned state estimates of the neighboring nodes and the complete observation data.

[0013] This invention also provides a distributed dynamic relative positioning system, the system comprising: an observation module, configured to observe in real time continuous distance and angle measurement information between a node to be positioned and its neighboring nodes, as well as inertial navigation measurement information of the node to be positioned, wherein the neighboring nodes are used to characterize nodes capable of communicating with the node to be positioned itself; a processing module, configured to determine initial time-based distance and angle measurement information between the node to be positioned and its neighboring nodes from the continuous distance and angle measurement information, and initialize the relative pose of the node network based on the initial time-based distance and angle measurement information to obtain an initial relative pose estimate, wherein the node network is determined according to the node to be positioned; and a positioning module, configured to perform real-time positioning of the node to be positioned using multi-source information fusion technology based on the initial relative pose estimate, the continuously observed distance and angle measurement information, and the inertial navigation measurement information.

[0014] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the distributed dynamic relative positioning method as described above.

[0015] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the distributed dynamic relative positioning method as described above.

[0016] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the distributed dynamic relative positioning method as described above.

[0017] The distributed dynamic relative positioning method, system, electronic device, and storage medium provided by this invention obtain continuous distance and angle measurement information between the node to be positioned and its neighboring nodes, as well as the inertial navigation measurement information of the node to be positioned, in real time. From the continuous distance and angle measurement information, the initial distance and angle measurement information between the node to be positioned and its neighboring nodes are determined. Based on the initial distance and angle measurement information, the relative pose of the node network is initialized to obtain an initial relative pose estimate. Based on the initial relative pose estimate, the continuously observed distance and angle measurement information, and the inertial navigation measurement information, multi-source information fusion technology is used to perform real-time positioning of the node to be positioned. This achieves high-precision, robust distributed dynamic relative positioning in three-dimensional space without relying on infrastructure. Attached Figure Description

[0018] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0019] Figure 1 This is a flowchart illustrating the distributed dynamic relative positioning method provided by the present invention.

[0020] Figure 2 This is a schematic diagram of the process provided by the present invention for initializing the relative pose of the node network based on the initial distance measurement information and the initial angle measurement information to obtain the initial relative pose estimation.

[0021] Figure 3 This is a schematic diagram of the process provided by the present invention for selecting an initial node from multiple nodes to be located based on initial distance measurement information and initial angle measurement information.

[0022] Figure 4 This is a schematic diagram of the process provided by the present invention for real-time positioning of the node to be located based on initial relative pose estimation and complete observation data, using multi-source information fusion technology and filtering estimation processing.

[0023] Figure 5 This is a schematic diagram of the distributed dynamic relative positioning system provided by the present invention. Figure 6This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

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

[0025] The distributed dynamic relative positioning method provided by this invention utilizes distance measurements (corresponding to distance measurement information) and angle measurements (corresponding to angle measurement information) observed by an antenna array, supplemented by acceleration and angular velocity measurements from an inertial sensor (corresponding to inertial navigation measurement information), to perform multi-source information fusion, efficiently achieving high-precision and robust distributed dynamic relative positioning in three-dimensional space without relying on infrastructure. The distributed dynamic relative positioning method constructs a well-conditioned initial reference coordinate system based on distance and angle measurement information, efficiently determines the initial relative pose estimation of the node network through iterative positioning using dynamically expanded virtual anchor points, and further proposes a multi-source information fusion algorithm based on dynamic alignment and updating of coordinate references. Combined with a particle filtering and trajectory estimation framework, it evaluates the confidence level of velocity estimation in particle states based on radial relative velocity fitting, and utilizes the maintenance and updating of coordinate transformation parameter tables to achieve real-time alignment of geometric references and translational velocity references of different local reference coordinate systems, thereby obtaining a high-precision and robust spatiotemporal cooperative relative positioning estimate.

[0026] The distributed dynamic relative positioning method provided by this invention can effectively solve the problem of coordinate reference mismatch between nodes by relying solely on cooperation between nodes, without relying on infrastructure, and achieve filtered estimation under a unified coordinate reference. The designed multi-source information fusion framework can efficiently mine spatiotemporal cooperation gains, improving the accuracy and robustness of relative positioning under the requirements of flexible and low-cost system deployment.

[0027] Figure 1 This is a flowchart illustrating the distributed dynamic relative positioning method provided by the present invention.

[0028] The following will combine Figure 1 The process of the distributed dynamic relative positioning method provided by this invention will be described.

[0029] In an exemplary embodiment of the present invention, combined with Figure 1 As can be seen, the distributed dynamic relative positioning method may include steps 110 to 130, which will be described in detail below.

[0030] In step 110, continuous distance and angle measurement information between the node to be located and its neighboring nodes, as well as the inertial navigation measurement information of the node to be located, are obtained in real time.

[0031] In one embodiment, continuous distance and angle measurements between the node to be located and its neighboring nodes can be obtained in real time based on an antenna array. Additionally, inertial navigation measurement information of the node to be located can be obtained in real time based on an inertial sensor. This inertial navigation measurement information can be considered as information obtained by observing the state of the node to be located, which may include acceleration, angular velocity, attitude angle, etc.

[0032] Neighbor nodes can be used to represent nodes that can communicate with the node to be located itself.

[0033] In one embodiment, consider a three-dimensional space composed of A relative positioning network (corresponding node network) consisting of nodes to be located can be denoted as The corresponding node index set can be denoted as . Consider discrete time intervals. The problem of positioning, the node to be located At any moment The state variables can be represented by formula (1): (1).

[0034] Each item represents the position of the node. ,speed acceleration Attitude angle angular velocity .in, For RPY (roll-pitch-yaw) angles, These represent the yaw, pitch, and roll angles about the Z, Y, and X axes, respectively. Node To the node The distance is ,in, Let the L2 norm of the vector be represented, and the relative azimuth and relative elevation angles be represented by formulas (2) and (3), respectively: in, Represents the arctangent function in the four quadrants. Indicates at node Nodes in local reference coordinate system coordinates Convert the attitude angles into the corresponding rotation matrices, satisfying formula (4): (4).

[0035] Node to be located The neighbor node index set is denoted as .node The relevant measurements can be obtained by solving the antenna array signals, where the relevant measurements are as shown in formula (5): (5).

[0036] in, , , These are the distance, azimuth, and elevation angle measurements obtained at time k for all neighboring nodes. The measurement noise is assumed to follow a zero-mean Gaussian distribution, and the noise variance of the distance measurement can be considered inversely proportional to the square of the distance. Furthermore, acceleration and angular velocity measurements can be obtained through its own inertial sensor, as shown in formula (6): (6).

[0037] And the node state variables satisfy formula (7): (7).

[0038] in, This indicates the acceleration that can be obtained from its own inertial sensor; This indicates the angular velocity that can be obtained from its own inertial sensor. It is zero-mean Gaussian noise.

[0039] In step 120, the initial distance measurement information and initial angle measurement information between the node to be located and its neighboring nodes are determined from the continuous distance measurement information and angle measurement information. Based on the initial distance measurement information and initial angle measurement information, the relative pose of the node network is initialized to obtain the initial relative pose estimate.

[0040] In one embodiment, initial distance and angle measurements between the node to be located and its neighboring nodes can be determined from continuous distance and angle measurements. Based on these initial distance and angle measurements, the relative pose of the node network is initialized to obtain an initial relative pose estimate. The relative pose may include relative position and attitude angle. The node network can be determined based on the node to be located. This embodiment is used for the initialization of a dynamic relative positioning system, utilizing only spatial cooperation for relative positioning to obtain a sufficiently accurate initial relative pose estimate of the network with high efficiency, serving as the initialization for subsequent dynamic positioning algorithms.

[0041] In step 130, based on the initial relative pose estimation, distance measurement information and angle measurement information obtained from continuous observation, and inertial navigation measurement information, the node to be located is located in real time using multi-source information fusion technology.

[0042] In one embodiment, the node to be located can be located in real time using multi-source information fusion technology based on initial relative pose estimation, distance measurement information and angle measurement information obtained from continuous observation, and inertial navigation measurement information.

[0043] Since different sensors update at different frequencies, with inertial sensors typically measuring at a higher frequency than antenna arrays, a multi-source sensor fusion architecture combining particle filtering and trajectory extrapolation can be used. During particle filtering, the most recent inertial sensor measurements can be considered as part of the current measurement. Specific implementation examples are described below.

[0044] The distributed dynamic relative positioning method provided by this invention obtains continuous distance and angle measurements between the node to be positioned and its neighboring nodes, as well as the inertial navigation measurement information of the node to be positioned, in real time. From the continuous distance and angle measurements, the initial distance and angle measurements between the node to be positioned and its neighboring nodes are determined. Based on these initial distance and angle measurements, the relative pose of the node network is initialized to obtain an initial relative pose estimate. Based on the initial relative pose estimate, the continuously observed distance and angle measurements, and the inertial navigation measurement information, multi-source information fusion technology is used to perform real-time positioning of the node to be positioned. This achieves high-precision, robust distributed dynamic relative positioning in three-dimensional space without relying on infrastructure.

[0045] Figure 2 This is a schematic diagram of the process for initializing the relative pose of a node network based on initial distance measurement information and initial angle measurement information provided by the present invention.

[0046] The following will combine Figure 2 The process of initializing the relative pose of the node network and obtaining the initial relative pose estimate based on the initial distance measurement information and the initial angle measurement information is explained.

[0047] In an exemplary embodiment of the present invention, combined with Figure 2 As can be seen, the initial relative pose estimation of the node network based on the initial distance measurement information and the initial angle measurement information can include steps 210 to 250, which will be described in detail below.

[0048] In step 210, based on the initial distance measurement information and the initial angle measurement information, an initial node is selected from multiple nodes to be located, and a well-formed initial reference coordinate system is constructed based on the initial node.

[0049] In one embodiment, an initial node can be selected from multiple nodes to be located based on initial distance measurement information and initial angle measurement information. Furthermore, a well-defined initial reference coordinate system is then constructed based on the initial node.

[0050] Figure 3 This is a schematic diagram of the process provided by the present invention for selecting an initial node from multiple nodes to be located based on initial distance measurement information and initial angle measurement information.

[0051] The following will combine Figure 3 The process of selecting the initial node from multiple nodes to be located based on the initial distance measurement information and the initial angle measurement information is explained.

[0052] In an exemplary embodiment of the present invention, combined with Figure 3 As can be seen, selecting the initial node from multiple nodes to be located based on the initial distance measurement information and the initial angle measurement information can include steps 310 to 340, which will be described in detail below.

[0053] In step 310, based on the initial distance measurement information and the initial angle measurement information, the polyhedral geometric structure of the node to be located is obtained through geometric relationships; In step 320, the degree of structural ill-conditioning of the polyhedral geometry is determined based on the geometric condition number of the polyhedral geometry. In step 330, polyhedral geometric structures whose degree of ill-conditioning is less than the degree threshold are identified as benign polyhedral geometric structures. In step 340, based on the well-formed polyhedral combination structure, an initial node is selected from multiple nodes to be located.

[0054] It should be noted that, for the sake of brevity, the subscript k representing time is omitted in this embodiment.

[0055] In one embodiment, the index set elements of the node to be located can be rearranged from high to low according to the number of neighboring nodes of each node to obtain an ordered index list. From the list Selecting the first four fully connected nodes, the index set can be denoted as... Furthermore, the geometric structure of the tetrahedron (corresponding to the geometric structure of the polyhedron) can be obtained through geometric relationships by measuring the distance between them (which can correspond to the distance measurement information and the angle measurement information at the initial time), and the relative coordinates can be calculated. The corresponding geometric condition number is given by formula (8): (8).

[0056] in, The matrix condition number operator satisfies formula (9): (9).

[0057] in, For the node 𝑖 pointing to The matrix formed by the three column vectors of the remaining three nodes, with For example, . Characterized The ability of the basis pairs to represent three-dimensional space (corresponding to the degree of structural ill-conditioning); the smaller the value, the better the geometric structure. This can be set. (Corresponding degree threshold), when When the geometry is in a good state, it can be considered well-formed; when... Then Nodes in Replace with list The next node in the sequence yields the indicator set. Calculate the corresponding geometric condition number. Repeat the above steps until a suitable set of indicators for well-formed nodes is found. If the list If no set of criteria is found after the traversal, the threshold can be relaxed. Then repeat the above steps. The final result is... The four nodes in the system are marked as virtual anchor points (which can correspond to the initial nodes), and the coordinates obtained by measuring the distance and angle between them will form the initial reference coordinate system.

[0058] In step 220, the relative poses of the remaining nodes are estimated in the initial reference coordinate system to obtain the relative pose estimates of the remaining nodes.

[0059] In step 230, the relative pose estimate of the initial node is obtained, and the positioning confidence of each node to be located is calculated based on the relative pose estimate of the initial node and the relative pose estimate of the remaining nodes.

[0060] In step 240, the initial node and the remaining nodes are taken as network nodes, and the relative pose of the network nodes is estimated iteratively to obtain the relative pose estimate. Based on the relative pose estimate, the location confidence of each network node is calculated until the global average location confidence converges.

[0061] In step 250, the relative pose of the node network is initialized based on the relative pose estimation of the network nodes under the condition of convergence of global average positioning confidence, and the initial relative pose estimation is obtained.

[0062] In one embodiment, the relative poses of the remaining nodes can be estimated in an initial reference coordinate system to obtain relative pose estimates for the remaining nodes, and the relative pose estimates of the initial nodes can be obtained. Based on the relative pose estimates of the initial nodes and the relative pose estimates of the remaining nodes, the positioning confidence of each node to be located is calculated. Further, the initial nodes and the remaining nodes are then treated as network nodes, and the steps of estimating the relative poses of the network nodes to obtain relative pose estimates, and calculating the positioning confidence of each network node based on the relative pose estimates, are iteratively performed until the obtained global average positioning confidence converges. The global average positioning confidence is determined based on the positioning confidence of each network node.

[0063] Furthermore, based on the relative pose estimation of network nodes under the condition of convergence of global average positioning confidence, the relative pose of the node network is initialized to obtain the initial relative pose estimation.

[0064] In one embodiment, the remaining nodes can be other nodes among a plurality of nodes to be located, excluding the initial node.

[0065] In another embodiment, the relative poses of the remaining nodes can be estimated and the positioning confidence of each node can be evaluated. In application, the positioning confidence of the initial node can be set to 1, and the positioning confidence of the remaining nodes to be located can be set to 0. The remaining nodes are then sequentially located one by one without loss of generality, with the nodes to be located... For example, let the number of its neighboring virtual anchor points be . Organize and label the corresponding indicator sets as follows: The coordinates of each virtual anchor point obtained in the preceding estimation are as follows: The distances and angles between them and the node to be located are denoted as... The corresponding noise variances are respectively To facilitate linear representation, let nodes be defined. The pose to be estimated is Based on the measurement model and geometric relationships, the following system of linear equations (10) can be obtained: (10).

[0066] And can be decomposed into , The coefficients of the distance information dominant component are given by the following formulas (11)-(12): (11).

[0067] (12).

[0068] The coefficient of the dominant component of angle information is , ,in, .

[0069] .

[0070] For the sake of brevity, the trigonometric functions in the above formula are... It has been abbreviated to In practical solutions, the distance and angle related terms in the above equations are replaced by observed measurements, which contain corresponding noise. Considering the heteroscedasticity of the measurement noise, weighted least squares can be used to solve the above equations, and the result is shown in formula (13): (13).

[0071] The weight matrix is ​​given by formula (14): (14).

[0072] The parameters are as follows: in Convert the vector to a diagonal matrix. In actual calculations, the matrix... Parameters in The previous round of estimates can be used. Replacement. Final node The pose estimation is given by formula (15): (15).

[0073] Then it's necessary to determine whether the location estimate of the newly added, already located node is reliable, i.e., whether it's suitable for expansion as a virtual anchor point for locating the remaining nodes. When a node... When the pose estimate is updated, its positioning confidence is updated as follows: .

[0074] in The location confidence of its neighboring nodes is given, and the residual terms are as follows: .

[0075] Set confidence threshold ,like If the position estimate of the node is considered sufficiently reliable, its state is set as a virtual anchor point; if If the location estimate of a node is deemed unreliable, its state is set to silent node. During the sequential localization process, only virtual anchors are allowed to participate in the localization and state update processes of their neighboring nodes.

[0076] Furthermore, iterative estimation converges to the global average local confidence level. The list is iterated repeatedly. In several rounds, each node in the selection list is traversed and located sequentially. For each selected node to be located, based on the coordinates of its neighboring virtual anchors and the corresponding distance and angle measurements, a new relative pose estimate is performed using the method described above. Its positioning confidence is then updated, and it is determined whether it can be extended to a virtual anchor. After completing one round of iterative positioning, the global average positioning confidence is calculated. And determine whether it converges. When After convergence or reaching the maximum number of iterations, the initial relative pose estimate of the network is obtained, as shown in formula (16): (16).

[0077] In another embodiment, the above process can be repeated across multiple consecutive time frames, selecting the result with the highest global average positioning confidence as the initial relative pose estimate. This operation is generally performed when the network measurement refresh rate is sufficiently high relative to the node movement speed, because in this case, it can be assumed that the network relative pose does not change significantly over a relatively short period of time. Selecting the state estimate with good average performance as the initial value for the dynamic algorithm can improve the stability of the dynamic positioning system initialization.

[0078] In yet another exemplary embodiment of the present invention, the preceding text continues... Figure 1 The above embodiment is used as an example for illustration. Based on the initial relative pose estimation, distance measurement information and angle measurement information obtained from continuous observation, and inertial navigation measurement information, the real-time positioning of the node to be positioned (corresponding to step 130) can be achieved using multi-source information fusion technology in the following way: With the collection of distance and angle measurement information obtained from continuous observations, the node to be located is located in real time based on the initial relative pose estimation and complete observation data using multi-source information fusion technology. The complete observation data includes the distance measurement information at the current moment, the angle measurement information at the current moment, and the inertial navigation measurement information at the previous moment. Once inertial navigation measurement information is collected, trajectory estimation is performed based on the inertial navigation measurement information to achieve real-time positioning of the node to be positioned.

[0079] In one embodiment, observation data can be collected, wherein the observation data can be considered to be obtained through observation, such as distance measurement information and angle measurement information obtained through observation via an antenna array, and inertial navigation measurement information obtained through observation via an inertial sensor.

[0080] During application, the sensor source of the current observation data can be determined. If the source of the observation data is an antenna array, that is, distance measurement information and angle measurement information obtained from continuous observation are collected, the node to be located is located in real time using multi-source information fusion technology based on the initial relative pose estimation and complete observation data. The complete observation data includes the distance measurement information at the current moment, the angle measurement information at the current moment, and the inertial navigation measurement information at the previous moment.

[0081] It should be noted that when the current time is other than the initial time, the initial relative pose estimate can be the state estimate from the previous time. This state estimate can include the relative pose estimate, acceleration, angular velocity, and velocity. After initialization, the multi-source information fusion localization algorithm is executed at each time step. The algorithm's input can be the state estimate from the previous time step and the observation data from the current time step. The second time step (the time after the initial time step) uses the first time step (corresponding to the initial time step), i.e., the "initial relative pose estimate." The third time step should use the relative pose estimate calculated at the second time step, and so on.

[0082] In another embodiment, if the source of the observation data is an inertial sensor, that is, when inertial navigation measurement information is collected, the trajectory is calculated based on the inertial navigation measurement information to achieve real-time positioning of the node to be located.

[0083] In another embodiment, if angular velocity and acceleration measurement data from the inertial sensor are collected, the trajectory is calculated using the inertial navigation observation data to obtain a relative position estimate. First, the particle state of the particle filter is updated using the inertial sensor measurement as the control variable. The corresponding state transition equation is expressed as follows (17): (17).

[0084] in, State transition function of particle filtering The format remains the same, except that the acceleration and angular velocity state variables in the expression are measured using inertial sensors. Replacement. For each particle Update it to formula (18): (18).

[0085] The particle weights remain unchanged, i.e. Then, the node state estimate is updated according to the state transition equation, i.e., formula (19): (19).

[0086] The resulting cumulative error can be corrected by the next particle filter estimation. The trajectory estimation of each neighboring node is updated using formula (20): (20).

[0087] in This involves estimating the neighbor states from the previous time step, and then obtaining the corresponding neighbor pose states. Then the node The relative pose estimation within the neighborhood is given by formula (21): (twenty one).

[0088] In this embodiment, since different sensors have different update frequencies, the measurement frequency of inertial sensors is generally higher than that of antenna arrays. Therefore, a multi-source sensor fusion can be achieved by combining particle filtering and trajectory extrapolation. In particle filtering, the latest inertial sensor measurement in the past can be regarded as part of the measurement at the current moment.

[0089] In another exemplary embodiment of the present invention, based on the initial relative pose estimation and complete observation data, the node to be located is located in real time using multi-source information fusion technology, which can be achieved in the following manner: Based on the initial relative pose estimation and complete observation data, the node to be located is located in real time by using multi-source information fusion technology and filtering estimation processing.

[0090] Figure 4 This is a schematic diagram of the process provided by the present invention for real-time positioning of the node to be located based on initial relative pose estimation and complete observation data, using multi-source information fusion technology and filtering estimation processing.

[0091] The following will combine Figure 4 This paper describes the process of real-time localization of the node to be located by using multi-source information fusion technology and filtering estimation based on initial relative pose estimation and complete observation data.

[0092] In an exemplary embodiment of the present invention, combined with Figure 4 As can be seen, based on the initial relative pose estimation and complete observation data, the real-time positioning of the node to be located can be achieved by using multi-source information fusion technology and filtering estimation processing, which may include steps 410 to 440. Each step will be described below.

[0093] In step 410, the coordinate transformation parameter table is initialized based on the initial relative pose estimation.

[0094] In one embodiment, a coordinate transformation parameter table can be initialized based on the initial relative pose estimation. This table is used to achieve reference alignment of the local reference coordinate systems between the nodes to be located, where the local reference coordinate systems characterize the reference coordinate systems used for positioning from the perspective of each node.

[0095] In another embodiment, the coordinate transformation parameter table and the particle filter can be initialized based on the initial relative pose estimation. To achieve distributed relative positioning without absolute position information, considering that the local reference coordinate system for relative positioning of each node changes over time, it is necessary to dynamically align the coordinate references of each node, realizing the real-time conversion and unification of the geometric reference and translational velocity reference of the local reference coordinate system. For this purpose, each node will use and maintain the coordinate transformation parameter table to achieve state transformation between inertial frames. (Note: The last sentence appears to be incomplete and possibly refers to a node.) At any moment The local reference coordinate system is ,exist Define nodes below. At any moment The homogeneous pose state is given by formula (22): (twenty two).

[0096] Among them, subscript Indicates the index of the local reference coordinate system, superscript This indicates the node number. Each node will maintain a coordinate transformation parameter table. Without loss of generality, consider the nodes... The coordinate transformation parameter table is shown in formula (23): (twenty three).

[0097] Among them, from node Switch to node The transformation matrix of the local reference coordinate system can be expressed as formula (24): (twenty four).

[0098] At the initial moment, node To the node The transformation matrix will be initialized as shown in equation (25): (25).

[0099] in, They are nodes At the node Local reference coordinate system Below and at the node Local reference coordinate system The homogeneous pose state matrix is ​​given. For the particle filtering process, the nodes... Maintenance and updates The state of each particle and weight Based on the initial relative pose estimation of the node network, Particles According to prior probability The distribution sampling is used for initialization, where... They are nodes The initial state estimate and the corresponding covariance matrix, for The position and attitude angle terms can be obtained from the initial relative pose estimation in step 120. The remaining state variables (velocity, acceleration, angular velocity) are initialized to 0 without prior knowledge, and initialized to their corresponding values ​​with prior knowledge (e.g., when inertial sensor measurements have been obtained). All weight coefficients are initialized to... .

[0100] In step 420, the state estimates of each neighboring node of the node to be located are received.

[0101] In step 430, based on the coordinate transformation parameter table and the state estimates of each neighboring node, the local reference coordinate system of the state estimates of the neighboring nodes is aligned to obtain the state estimates of the neighboring nodes after alignment.

[0102] In step 440, based on the state estimates after benchmark alignment with neighboring nodes and complete observation data, the node to be located is subjected to real-time filtered self-localization.

[0103] The state estimates of each neighboring node can include at least the relative pose of each neighboring node, and may also include velocity, acceleration, angular velocity, etc.

[0104] In another embodiment, the node to be located can be self-localized in real time by using the benchmark-aligned state estimates of neighboring nodes, complete observation data, and the radial relative velocities of neighboring nodes. Specifically, the radial relative velocities with each neighboring node can be estimated based on the time series of distance measurement information.

[0105] In another embodiment, state estimates of each neighboring node of the node to be located can be received. These state estimates can be obtained through broadcasts from the neighboring nodes of the node to be located.

[0106] Furthermore, based on the coordinate transformation parameter table and the state estimates of each neighboring node, the local reference coordinate system of the state estimates of the neighboring nodes is aligned to obtain the state estimates of the neighboring nodes after alignment. Then, based on the state estimates of the neighboring nodes after alignment, complete observation data, and the radial relative velocities of the neighboring nodes, the node to be located is subjected to real-time filtering and self-localization.

[0107] In another embodiment, state estimates of each neighbor can be received. Node Receive the state estimates of each neighbor from the previous time step, and obtain the pose state estimates of each neighbor node in its local reference coordinate system. .

[0108] In another embodiment, the local reference coordinate system alignment of the state estimates of neighboring nodes can be achieved based on the coordinate transformation parameter table and the state estimates of each neighboring node. Combined with... The transformation parameters at time can be based on Time Node Received latest neighbor state estimate From the local reference coordinate system To the local reference coordinate system Perform a transformation to obtain a local reference coordinate system. The pose states of the neighbors are as shown in formula (26): (26).

[0109] in, This is the coordinate datum alignment function. Specifically, it is first derived from... predict , record arrive The time interval is ,but The coefficient matrix is ​​given by formula (27): (27).

[0110] Depend on achievable Then perform a rotation matrix. The corresponding rotations are then performed, and the position and velocity are then analyzed separately. and The translation is expressed by formula (28). (28).

[0111] Therefore, in the reference frame The neighbor pose states after local reference coordinate system alignment are obtained. At this point, the node... All information has been converted to its local reference coordinate system. For the sake of brevity, the subscripts will be used thereafter without ambiguity. Omit.

[0112] Furthermore, based on the distance measurement sequence of the antenna array, the radial relative velocity with each neighboring node is estimated. The distance measurements between each node have been obtained through the antenna array measurements, with each node... and Taking the estimation of radial relative velocity between them as an example, the storage of a certain window length within... Distance measurement sequence With the corresponding measurement timestamp By fitting the rate of change of the distance measurement sequence over time, the radial relative velocity can be estimated. In non-high dynamic scenarios, i.e., when the refresh frequency of the distance measurement is sufficiently high relative to the node's movement speed, a first-order model can be used for fitting, and the radial relative velocity is estimated by formula (29): (29).

[0113] in, .

[0114] When the refresh frequency of distance measurement is low or the node movement speed is high, a second-order model can be used for more accurate fitting, and the coefficients in the above formula can be replaced with formula (30): (30).

[0115] in, This represents the Hadamard product. In actual implementation, The expanded expression has a certain recursive relationship, which can efficiently realize iterative calculation. Therefore, a radial relative velocity term can be introduced into the observation equation during the filtering process. This enables the evaluation of the confidence level of the velocity dimension state estimation and reflects it in the particle weight update.

[0116] In another embodiment, the node to be located can be self-localized in real time by using the benchmark-aligned state estimates of neighboring nodes and complete observation data. For example, the state transition equation and the observation equation are as follows: (31) (31).

[0117] in, Represents state variables. For complete observations, and State transition noise and observation noise are respectively modeled as zero-mean Gaussian noise, with covariance matrices of respectively. and The state transition equation can be determined based on the physical motion model, and the state transition function... It is a linear function that satisfies formula (32): (32).

[0118] If the actual motion model has additional constraints, such as the velocity direction being consistent with the attitude angle direction, then adjustments should be made accordingly. The expression can be expressed as follows: Observation function. Based on the actual observation models of each sensor, the state information of each neighboring node is determined using the pose state quantities obtained after aligning the local reference coordinate system as described above. The relevant items have been given in the foregoing embodiments. The terms in the formula satisfy formula (33): (33).

[0119] in, Radial velocity noise modeled with a zero-mean Gaussian.

[0120] In the particle filtering process, based on Particle state at time 1 and weight , in turn for the first Sequential importance sampling is performed on each particle, that is, according to Generate sampling particles And update the corresponding particle weights to formula (34): (34).

[0121] Among them, the residual term is .

[0122] Then the weights After normalization, we get ,in, .

[0123] Calculate the number of effective particles When the value is below a certain threshold, the particles can be resampled to avoid weight degradation. Finally, the updated particle state is obtained. and weight The corresponding particle filter estimation result is given by formula (35): (35).

[0124] This embodiment enables real-time filtering and self-localization estimation of the node to be located.

[0125] In yet another exemplary embodiment of the present invention, continuing with the previously described embodiments, the distributed dynamic relative positioning method may further include the following steps after real-time filtering and self-localization of the node to be located: Based on the localization results of the filtered self-localization, complete observation information, and angle measurement information of each neighboring node, the state estimates of each neighboring node of the node to be located are corrected to obtain the corrected state estimates. And based on the localization results of the filtered self-localization and the corrected state estimates of each neighboring node, the coordinate transformation parameter table is updated to obtain the updated coordinate transformation parameter table. Based on the updated coordinate transformation parameter table and the state estimates of each neighboring node, the local reference coordinate system of the state estimates of the neighboring nodes is aligned to obtain the updated state estimates of the neighboring nodes after the reference alignment. The real-time filtering and self-localization of the node to be located, based on the benchmark-aligned state estimates of neighboring nodes and complete observation data, can be achieved in the following way: Based on the updated benchmark-aligned state estimates from neighboring nodes and complete observation data, the node to be located is subjected to real-time filtering and self-localization.

[0126] In one embodiment, the state estimates of each neighboring node of the node to be located can be corrected based on the location results of the filtered self-localization, complete observation information, and angle measurement information of each neighboring node, thereby obtaining the corrected state estimates. Furthermore, the coordinate transformation parameter table can be updated based on the location results of the filtered self-localization and the corrected state estimates of each neighboring node, thereby obtaining the updated coordinate transformation parameter table.

[0127] Furthermore, at the next time step, based on the updated coordinate transformation parameter table and the state estimates of each neighboring node, the local reference coordinate system of the neighboring node's state estimates can be aligned to obtain the updated, aligned state estimates of the neighboring nodes. Then, based on the updated, aligned state estimates of the neighboring nodes and complete observation data, real-time filtering self-localization is performed on the node to be located. This improves the accuracy of the final real-time filtering self-localization of the node to be located.

[0128] In another embodiment, the neighbor pose state estimation in the local reference coordinate system can be corrected based on the self-localization result, complete observation information, and angle measurement information of each neighbor node, and the corresponding coordinate transformation parameter table can be updated. Since the rotation term in the transformation parameters is related to the attitude angle of each neighbor node, it cannot be corrected by the angle measurement of node n alone. Therefore, it is necessary to introduce the angle measurement information of the neighbor nodes and define the measurement term as shown in formula (36): (36).

[0129] in, The angle measurement information for each neighboring node can be obtained through broadcasting by each neighboring node. At this time, the view node... state variables Given information, the neighbor pose state The term to be corrected can be estimated using extended Kalman filtering with minimum mean square error. The neighbor pose states are then rewritten in vector form. ,in The corresponding observation equation can be represented by formula (37): (37).

[0130] in, The observation noise is zero-mean Gaussian, and the covariance matrix is... Observation function The terms can be obtained from the observation model given above, and its linear approximation Jacobian matrix is... Satisfying formula (38): (38).

[0131] Measurement residuals satisfy formula (39): (39).

[0132] The correction factor satisfies formula (40): (40).

[0133] in, Let be the covariance matrix of the neighbor pose state estimation, satisfying ,in The correction value for the neighbor pose state satisfies formula (41): (41).

[0134] Thus, nodes can be obtained. The relative pose estimation within the neighborhood is shown in Equation (42): (42).

[0135] Depend on Obtain the homogeneous pose state matrix. Then the corresponding coordinate transformation parameters can be updated in formula (43): (43).

[0136] This step doesn't have to be performed after every particle filter. It can be executed at a reasonable cycle, taking into account the system's positioning update frequency and computational resources, and fine-tuning the coordinate reference alignment and update function to match the corresponding cycle. Note that this step doesn't involve correcting the pose states of neighbors. The neighbor pose state before correction is used. That's all.

[0137] In yet another embodiment, each node will display its latest state estimate. and angle measurement values The data is broadcast to neighboring nodes, which then serve as state estimates and angle measurements for the next round of receiving data from the neighboring nodes of the node to be located.

[0138] It is worth noting that the distributed nature of the proposed method is that the above operations are performed in parallel on each node to be located. That is, each node to be located estimates its own relative pose and that of its neighboring nodes with itself as the center, and exchanges information through communication between nodes, thereby achieving efficient distributed relative positioning.

[0139] As described above, the distributed dynamic relative positioning method provided by this invention obtains continuous distance and angle measurement information between the node to be positioned and its neighboring nodes, as well as the inertial navigation measurement information of the node to be positioned, in real time. From the continuous distance and angle measurement information, the initial distance and angle measurement information between the node to be positioned and its neighboring nodes are determined. Based on the initial distance and angle measurement information, the relative pose of the node network is initialized to obtain an initial relative pose estimate. Based on the initial relative pose estimate, the continuously observed distance and angle measurement information, and the inertial navigation measurement information, multi-source information fusion technology is used to perform real-time positioning of the node to be positioned. This achieves high-precision, robust distributed dynamic relative positioning in three-dimensional space without relying on infrastructure.

[0140] Based on the same concept, the present invention also provides a distributed dynamic relative positioning system.

[0141] The distributed dynamic relative positioning system provided by the present invention is described below. The distributed dynamic relative positioning system described below can be referred to in correspondence with the distributed dynamic relative positioning method described above.

[0142] Figure 5 This is a schematic diagram of the distributed dynamic relative positioning system provided by the present invention.

[0143] In an exemplary embodiment of the present invention, combined with Figure 5As can be seen, the distributed dynamic relative positioning system may include an observation module 510, a processing module 520, and a positioning module 530. The following sections will introduce each module in turn.

[0144] The observation module 510 can be configured to observe in real time the continuous distance measurement information and angle measurement information between the node to be located and its neighboring nodes, as well as the inertial navigation measurement information of the node to be located, wherein the neighboring nodes are used to characterize nodes that can communicate with the node to be located itself. The processing module 520 can be configured to determine the initial time distance measurement information and the initial time angle measurement information between the node to be located and the neighboring node from continuous distance measurement information and angle measurement information, and initialize the relative pose of the node network based on the initial time distance measurement information and the initial time angle measurement information to obtain an initial relative pose estimate, wherein the node network is determined according to the node to be located; The positioning module 530 can be configured to perform real-time positioning of the node to be positioned based on the initial relative pose estimation, distance measurement information and angle measurement information obtained from continuous observation, and inertial navigation measurement information, using multi-source information fusion technology.

[0145] In an exemplary embodiment of the present invention, the processing module 520 can initialize the relative pose of the node network based on the initial time distance measurement information and the initial time angle measurement information to obtain an initial relative pose estimate: Based on the initial distance measurement information and the initial angle measurement information, an initial node is selected from among the multiple nodes to be located, and a well-formed initial reference coordinate system is constructed based on the initial node; The relative poses of the remaining nodes are estimated in the initial reference coordinate system to obtain the relative pose estimates of the remaining nodes, wherein the remaining nodes are the other nodes among the plurality of nodes to be located, excluding the initial node; The relative pose estimate of the initial node is obtained, and the positioning confidence of each node to be located is calculated based on the relative pose estimate of the initial node and the relative pose estimate of the remaining nodes. The initial node and the remaining nodes are taken as network nodes, and the relative pose of the network nodes is estimated iteratively to obtain the relative pose estimate, and the location confidence of each network node is calculated based on the relative pose estimate, until the obtained global average location confidence converges, wherein the global average location confidence is determined according to the location confidence of each network node. Based on the relative pose estimation of the network nodes under the condition of convergence of global average local confidence, the relative pose of the node network is initialized to obtain the initial relative pose estimation.

[0146] In an exemplary embodiment of the present invention, the processing module 520 may select an initial node from a plurality of nodes to be located based on the initial time distance measurement information and the initial time angle measurement information in the following manner: Based on the initial distance measurement information and the initial angle measurement information, the polyhedral geometric structure of the node to be located is obtained through geometric relationships; The degree of structural ill-conditioning of the polyhedral geometry is determined based on the geometric condition number of the polyhedral geometry. Polyhedral geometric structures whose degree of ill-conditioning is less than the degree threshold are identified as benign polyhedral geometric structures. Based on the well-formed polyhedral combination structure, an initial node is selected from multiple nodes to be located.

[0147] In an exemplary embodiment of the present invention, the positioning module 530 can perform real-time positioning of the node to be positioned based on the initial relative pose estimation, distance measurement information and angle measurement information obtained from continuous observation, and inertial navigation measurement information, using multi-source information fusion technology: With the collection of distance and angle measurement information obtained from continuous observations, the node to be located is located in real time based on the initial relative pose estimation and complete observation data using multi-source information fusion technology. The complete observation data includes the distance measurement information at the current moment, the angle measurement information at the current moment, and the inertial navigation measurement information at the previous moment. Once inertial navigation measurement information is collected, trajectory estimation is performed based on the inertial navigation measurement information to achieve real-time positioning of the node to be located.

[0148] In an exemplary embodiment of the present invention, the positioning module 530 can perform real-time positioning of the node to be positioned based on the initial relative pose estimation and complete observation data, using multi-source information fusion technology: Based on the initial relative pose estimation and complete observation data, the node to be located is located in real time by using multi-source information fusion technology and filtering estimation processing.

[0149] In an exemplary embodiment of the present invention, the positioning module 530 can perform real-time positioning of the node to be positioned based on the initial relative pose estimation and complete observation data, using multi-source information fusion technology and filtering estimation processing: Based on the initial relative pose estimation, a coordinate transformation parameter table is initialized, wherein the coordinate transformation parameter table is used to realize the benchmark alignment of the local reference coordinate system between each node to be located, and the local reference coordinate system is used to characterize the reference coordinate system for positioning under the view of each node to be located. Receive the state estimates of each neighboring node of the node to be located; Based on the coordinate transformation parameter table and the state estimates of each neighboring node, the local reference coordinate system of the state estimates of the neighboring nodes is aligned to obtain the state estimates of the neighboring nodes after alignment. Based on the state estimates after benchmark alignment with neighboring nodes and the complete observation data, the node to be located is subjected to real-time filtering and self-localization.

[0150] In an exemplary embodiment of the present invention, the positioning module 530 may further be configured to: Based on the localization results of the filtered self-localization, complete observation information, and angle measurement information of each neighboring node, the state estimates of each neighboring node of the node to be located are corrected to obtain the corrected state estimates. And based on the localization results of the filtered self-localization and the corrected state estimates of each neighboring node, the coordinate transformation parameter table is updated to obtain the updated coordinate transformation parameter table; Based on the updated coordinate transformation parameter table and the state estimates of each neighboring node, the local reference coordinate system of the state estimates of the neighboring nodes is aligned to obtain the updated reference-aligned state estimates of the neighboring nodes.

[0151] The positioning module 530 can also perform real-time filtered self-localization of the node to be located based on the benchmark-aligned state estimate of neighboring nodes and the complete observation data in the following manner: Based on the updated benchmark-aligned state estimates of neighboring nodes and the complete observation data, the node to be located is subjected to real-time filtering and self-localization.

[0152] Figure 6 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 6As shown, the electronic device may include: a processor 610, a communication interface 620, a memory 630, and a communication bus 640, wherein the processor 610, the communication interface 620, and the memory 630 communicate with each other through the communication bus 640. The processor 610 can call logic instructions in the memory 630 to execute a distributed dynamic relative positioning method. This method includes: real-time observation of continuous distance and angle measurement information between the node to be positioned and its neighboring nodes, as well as inertial navigation measurement information of the node to be positioned, wherein the neighboring nodes represent nodes capable of communicating with the node to be positioned itself; determining initial time-based distance and angle measurement information between the node to be positioned and its neighboring nodes from the continuous distance and angle measurement information, and initializing the relative pose of the node network based on the initial time-based distance and angle measurement information to obtain an initial relative pose estimate, wherein the node network is determined according to the node to be positioned; and real-time positioning of the node to be positioned using multi-source information fusion technology based on the initial relative pose estimate, the continuously observed distance and angle measurement information, and the inertial navigation measurement information.

[0153] Furthermore, the logical instructions in the aforementioned memory 630 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0154] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the distributed dynamic relative positioning method provided by the above methods. The method includes: real-time observation of continuous distance measurement information and angle measurement information between the node to be positioned and its neighboring nodes, as well as inertial navigation measurement information of the node to be positioned, wherein the neighboring nodes are used to characterize nodes that can communicate with the node to be positioned itself; determining the initial time-based distance measurement information and initial time-based angle measurement information between the node to be positioned and its neighboring nodes from the continuous distance measurement information and angle measurement information, and initializing the relative pose of the node network based on the initial time-based distance measurement information and the initial time-based angle measurement information to obtain an initial relative pose estimate, wherein the node network is determined according to the node to be positioned; and real-time positioning of the node to be positioned using multi-source information fusion technology based on the initial relative pose estimate, the continuously observed distance measurement information and angle measurement information, and the inertial navigation measurement information.

[0155] In another aspect, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements a distributed dynamic relative positioning method provided by the above methods. This method includes: real-time observation of continuous distance and angle measurement information between a node to be positioned and its neighboring nodes, as well as inertial navigation measurement information of the node to be positioned, wherein the neighboring nodes are used to characterize nodes capable of communicating with the node to be positioned itself; determining initial-time distance and angle measurement information between the node to be positioned and its neighboring nodes from the continuous distance and angle measurement information, and initializing the relative pose of the node network based on the initial-time distance and angle measurement information to obtain an initial relative pose estimate, wherein the node network is determined according to the node to be positioned; and real-time positioning of the node to be positioned using multi-source information fusion technology based on the initial relative pose estimate, the continuously observed distance and angle measurement information, and the inertial navigation measurement information.

[0156] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0157] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0158] It is further understood that although the operations are described in a specific order in the accompanying drawings in the embodiments of the present invention, this should not be construed as requiring these operations to be performed in the specific order or serial order shown, or requiring all the operations shown to obtain the desired result. In certain environments, multitasking and parallel processing may be advantageous.

[0159] 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 foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A distributed dynamic relative positioning method, characterized in that, The method includes: Real-time observation obtains continuous distance and angle measurement information between the node to be located and its neighboring nodes, as well as the inertial navigation measurement information of the node to be located. The neighboring nodes are used to characterize nodes that can communicate with the node to be located itself. The initial distance measurement information and initial angle measurement information between the node to be located and the neighboring node are determined from continuous distance measurement information and angle measurement information. Based on the initial distance measurement information and the initial angle measurement information, the relative pose of the node network is initialized to obtain the initial relative pose estimate. The node network is determined according to the node to be located. With the collection of distance and angle measurement information obtained from continuous observations, the node to be located is located in real time based on the initial relative pose estimation and complete observation data using multi-source information fusion technology. The complete observation data includes the distance measurement information at the current moment, the angle measurement information at the current moment, and the inertial navigation measurement information at the previous moment. Having collected inertial navigation measurement information, a trajectory is calculated based on this information to achieve real-time positioning of the node to be located. The inertial navigation measurement information includes observations of the node's state, including acceleration and angular velocity. Based on the initial relative pose estimation, a coordinate transformation parameter table is initialized, wherein the coordinate transformation parameter table is used to realize the benchmark alignment of the local reference coordinate system between each node to be located, and the local reference coordinate system is used to characterize the reference coordinate system for positioning under the view of each node to be located. Receive the state estimates of each neighboring node of the node to be located; Based on the coordinate transformation parameter table and the state estimates of each neighboring node, the local reference coordinate system of the state estimates of the neighboring nodes is aligned to obtain the state estimates of the neighboring nodes after alignment. Based on the state estimates after benchmark alignment with neighboring nodes and the complete observation data, the node to be located is subjected to real-time filtering and self-localization.

2. The distributed dynamic relative positioning method according to claim 1, characterized in that, The initial relative pose estimation of the node network, based on the initial distance measurement information and the initial angle measurement information, specifically includes: Based on the initial distance measurement information and the initial angle measurement information, an initial node is selected from among the multiple nodes to be located, and a well-formed initial reference coordinate system is constructed based on the initial node; The relative poses of the remaining nodes are estimated in the initial reference coordinate system to obtain the relative pose estimates of the remaining nodes, wherein the remaining nodes are the other nodes among the plurality of nodes to be located, excluding the initial node; The relative pose estimate of the initial node is obtained, and the positioning confidence of each node to be located is calculated based on the relative pose estimate of the initial node and the relative pose estimate of the remaining nodes. The initial node and the remaining nodes are taken as network nodes, and the relative pose of the network nodes is estimated iteratively to obtain the relative pose estimate, and the location confidence of each network node is calculated based on the relative pose estimate, until the obtained global average location confidence converges, wherein the global average location confidence is determined according to the location confidence of each network node. Based on the relative pose estimation of the network nodes under the condition of convergence of global average local confidence, the relative pose of the node network is initialized to obtain the initial relative pose estimation.

3. The distributed dynamic relative positioning method according to claim 2, characterized in that, The step of selecting an initial node from among multiple nodes to be located based on the initial time distance measurement information and the initial time angle measurement information specifically includes: Based on the initial distance measurement information and the initial angle measurement information, the polyhedral geometric structure of the node to be located is obtained through geometric relationships; The degree of structural ill-conditioning of the polyhedral geometry is determined based on the geometric condition number of the polyhedral geometry. Polyhedral geometric structures whose degree of ill-conditioning is less than the degree threshold are identified as benign polyhedral geometric structures. Based on the well-formed polyhedral combination structure, an initial node is selected from multiple nodes to be located.

4. The distributed dynamic relative positioning method according to claim 1, characterized in that, The real-time localization of the node to be located, based on the initial relative pose estimation and complete observation data and utilizing multi-source information fusion technology, specifically includes: Based on the initial relative pose estimation and complete observation data, the node to be located is located in real time by using multi-source information fusion technology and filtering estimation processing.

5. The distributed dynamic relative positioning method according to claim 1, characterized in that, After performing real-time filtering and self-localization on the node to be located, the method further includes: Based on the localization results of the filtered self-localization, complete observation information, and angle measurement information of each neighboring node, the state estimates of each neighboring node of the node to be located are corrected to obtain the corrected state estimates. And based on the location results of the filtered self-localization and the corrected state estimates of each neighboring node, the coordinate transformation parameter table is updated to obtain the updated coordinate transformation parameter table; Based on the updated coordinate transformation parameter table and the state estimates of each neighboring node, the local reference coordinate system of the state estimates of the neighboring nodes is aligned to obtain the updated reference-aligned state estimates of the neighboring nodes. The real-time filtering and self-localization of the node to be located, based on the benchmark-aligned state estimate of neighboring nodes and the complete observation data, specifically includes: Based on the updated benchmark-aligned state estimates of neighboring nodes and the complete observation data, the node to be located is subjected to real-time filtering and self-localization.

6. A distributed dynamic relative positioning system, characterized in that, The system is used to implement the distributed dynamic relative positioning method according to any one of claims 1 to 5, and the system comprises: The observation module is used to observe in real time the continuous distance and angle measurement information between the node to be located and its neighboring nodes, as well as the inertial navigation measurement information of the node to be located. The neighboring nodes are used to represent nodes that can communicate with the node to be located itself. The processing module is used to determine the initial time distance measurement information and the initial time angle measurement information between the node to be located and the neighboring node from continuous distance measurement information and angle measurement information, and to initialize the relative pose of the node network based on the initial time distance measurement information and the initial time angle measurement information to obtain an initial relative pose estimate, wherein the node network is determined according to the node to be located; The positioning module is used to perform real-time positioning of the node to be positioned based on the initial relative pose estimation, distance measurement information and angle measurement information obtained from continuous observation, and inertial navigation measurement information, using multi-source information fusion technology.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the distributed dynamic relative positioning method as described in any one of claims 1 to 5.

8. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the distributed dynamic relative positioning method as described in any one of claims 1 to 5.

Images