Distributed cooperative positioning method considering communication interruption and related device

Abstract

The invention belongs to the technical field of distributed cooperative positioning, and discloses a distributed cooperative positioning method considering communication interruption and a related device. The distributed cooperative positioning method comprises the steps that relative measurement of an agent # imgabs0 # to all neighbor agents is acquired, and the relative measurement of the agent # imgabs0 # to all neighbor agents is carried out by utilizing an intermediate positioning result of the agent # imgabs1 #, an intermediate positioning result of a neighbor agent which is interrupted in communication with the agent # imgabs2 # at the moment k and an intermediate positioning result sent by a neighbor agent which can still communicate with the agent # imgabs3 # at the moment k; and calculating a final positioning result of the agent # imgabs4 # at the moment k and a final positioning result of the neighbor agent which is interrupted with the agent # imgabs5 # at the moment k. According to the technical scheme disclosed by the invention, an accurate distributed cooperative positioning result can be obtained under the condition of considering communication interruption.

Description

Technical Field

[0001] The present invention belongs to the technical field of distributed collaborative positioning, and in particular relates to a distributed collaborative positioning method and related devices taking communication interruption into consideration. Background Art

[0002] Currently, it's increasingly common for multiple intelligent agents to be networked together to perform tasks. Sensors, robots, drones, underwater vehicles, and more can all form networks, maximizing their potential compared to individual devices. Precise positioning of intelligent agents is crucial for mission accomplishment. Existing, established positioning technologies, such as GPS and inertial navigation, have limitations. For example, GPS's usability declines dramatically, or even becomes completely unusable, in many challenging scenarios, such as indoors, in dense forests, deep sea, beneath polar ice caps, and on battlefields. High-precision inertial navigation components (INS) are expensive, while standard INS components suffer from significant cumulative errors.

[0003] In view of the above situation, collaborative localization technology is currently attracting widespread attention. It uses the useful information hidden in the relative measurements between intelligent agents in the network to improve positioning accuracy. Specifically, for example, the existing distributed collaborative localization algorithm based on the extended Kalman filter (EKF) and belief propagation (BP) algorithm is a representative example. It calculates the marginal posterior probability density of the intelligent agent state based on the BP algorithm and uses the EKF to calculate the marginal posterior probability density under the Gaussian distribution assumption, which can achieve good positioning results.

[0004] Existing collaborative localization algorithms typically assume that relative measurements must be received simultaneously with the intermediate positioning results of neighboring agents in order to improve positioning accuracy. This approach fails to account for situations where relative measurements persist despite communication interruptions. In practice, communication disruptions or even interruptions are common, such as those caused by electronic jamming in battlefield environments. In the event of a communication interruption, agents executing traditional collaborative localization algorithms are unable to utilize relative measurements between themselves and their neighbors to improve their positioning due to the inability to receive intermediate positioning results from neighboring agents. If communication between an agent and all its neighbors is interrupted, traditional collaborative localization algorithms degenerate into dead reckoning, leading to rapidly increasing positioning errors and reduced mission success rates, even impacting equipment survival. Examples include situations where autonomous underwater vehicles misalign seafloor topography data, distorted locations of precious glacier samples collected, or even lost in ocean trenches. Furthermore, drone swarms deviate from their planned flight paths and even collide with each other. Consequently, there is an urgent need for a distributed collaborative localization solution that accounts for communication interruptions. Summary of the Invention

[0005] The present invention aims to provide a distributed collaborative positioning method and related apparatus that takes communication interruption into account, so as to solve one or more of the above-mentioned technical problems. The technical solution disclosed in the present invention can obtain accurate distributed collaborative positioning results while taking communication interruption into account.

[0006] In order to achieve the above object, the present invention adopts the following technical solutions: In a first aspect, the present invention provides a distributed collaborative positioning method considering communication interruption, comprising the following steps: Based on a network composed of multiple agents, obtain k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- Positioning result at moment 1; based on k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- The positioning result at time 1 is used k Momentary Agent Internal measurement, intelligent agent The motion evolution model of k Moment and Agent The identification model of neighboring agents with communication interruption uses linear minimum mean square error estimation to calculate the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with communication interruption; The agent The intermediate positioning results are sent to k Moment and Agent Neighbor agents that can still communicate and obtain k Moment and Agent Intermediate positioning results sent by neighboring agents that are still able to communicate; Get the agent The relative measurement of all neighboring agents, using the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication, k Moment and Agent The intermediate positioning results sent by neighboring agents that can still communicate are calculated k Momentary Agent The final positioning result,k Moment and Agent The final positioning results of neighboring agents with interrupted communication.

[0007] A further improvement of the technical solution of the present invention is that the k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- The positioning result at time 1 is used k Momentary Agent Internal measurement, intelligent agent The motion evolution model of k Moment and Agent The identification model of neighboring agents with communication interruption uses linear minimum mean square error estimation to calculate the agent The intermediate positioning results, k Moment and Agent In the step of intermediate positioning results of neighboring agents with communication interruption, First construct the stacking motion model and the stacking internal measurement model, and then simultaneously calculate the agent based on the stacking motion model and the stacking internal measurement model The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication; k Moment and Agent The identification model of neighboring agents with interrupted communication is: ; Where: Representing an agent Maintained and intelligent Neighbor Agents with Lost Communication status, , Representing an agent In the horizontal position, Representing an agent Position in the vertical direction; express exist Moment and The state increment between moments is assumed to obey the mean , the variance is Gaussian distribution of different moments There is no correlation between irrelevant; Agent-based The motion evolution model ofk Moment and Agent The stacking motion model constructed by the identification model of the neighboring agents with interrupted communication is: ; Where, Representing an agent Status The state of a neighboring agent that loses communication with the agent it maintains The stacked state of the composition, , Representing an agent The speed in the horizontal direction, Representing an agent The speed in the vertical direction, express Moment and Agent The set of neighboring agents with disrupted communication; , represents the block diagonal matrix generating function, Representing an agent The state transition matrix, represents a unit matrix of appropriate size; , Representing an agent The noise gain matrix, represents a vector or matrix of zeros of appropriate size; Representing an agent The process noise is assumed to have a mean of 0 and a variance of Gaussian white noise; ; The constructed stack internal measurement model is: ; Where, , ; , ; Representing an agent Internal measurements obtained The measurement noise is assumed to have a mean of 0 and a variance of Gaussian white noise; Simultaneous calculation of intelligent agents based on stack motion model and stack internal measurement model The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with communication interruption ; Where, Express An intermediate estimate of It represents the covariance of the estimation error and is calculated as: ; Where, , and From Momentary Agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- Positioning result at moment 1 , , , For intelligent agents Increment the state of neighboring agents Variance estimates; ; ; .

[0008] A further improvement of the technical solution of the present invention is that the intelligent body is calculated based on the stacking motion model and the stacking internal measurement model at the same time. The intermediate positioning results, k Moment and Agent During the process of intermediate positioning results of neighboring agents with communication interruption, it is estimated The steps for calculating the mean and variance of include: First, establish the innovation exponential smoothing state space model of the horizontal position and vertical position of the neighboring agent, and then establish The state space model of and get and ;in, The horizontal position of the established neighboring agents The innovation exponential smoothing state space model is: ; Where: , and are the horizontal component and trend component of the neighbor agent’s horizontal position, respectively; is the error term, assuming its mean is 0 and its variance is Gaussian white noise; and are the coefficients in the model; Estimate initial values ​​of model states using maximum likelihood and model parameters 、 and ; 、 and It is obtained by solving the following optimization problem: ; Where: , From the neighbors Intermediate positioning results sent before communication interruption The horizontal position estimate in yes Time Neighbor Agent An intermediate estimate of its own state, is the covariance of the estimation errors; Agent at any moment With neighbors When a communication interruption occurs, ; Similarly, 、 、 and , thus establishing The state space model of is: ; Where, ; ; is the error term, assuming its mean is 0 and its variance is Gaussian white noise, , for Constant Neighbors Intermediate estimate results sent The covariance of the position estimation errors in ; ; ; based on The state space model predicts and get and , the calculation formula is as follows: ; ; Where, .

[0009] A further improvement of the technical solution of the present invention is that the acquisition of the intelligent agent The relative measurement of all neighboring agents, using the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication, k Moment and Agent The intermediate positioning results sent by neighboring agents that can still communicate are calculated k Momentary Agent The final positioning result, k Moment and Agent In the step of the final positioning result of the neighboring agent with interrupted communication, First, a stacked linearized measurement model is established, and then the stacked linearized measurement model is simultaneously calculated. k Momentary Agent The final positioning result, k Moment and Agent The final positioning result of the neighboring agent with interrupted communication; The stacked linearized measurement model is: ; Where, , express Momentary Agent Obtained neighbor agents The relative measurement of express Always be able to communicate with the agent The set of neighboring agents that communicate; , express Momentary Agent Neighbor Agents The measurement function of ; , , , ; , , ; ; , Representing an agent Neighbor Agents The measurement noise of the relative measurement is assumed to be 0 and the variance is Gaussian white noise; Then, based on the stacked linearized measurement model, Kalman filtering is used to simultaneously calculate k Momentary Agent The final positioning result, k Moment and Agent Final positioning results of neighboring agents with interrupted communication , where Express Estimates, It represents the covariance of the estimation error and is calculated as follows: ; Where, , , , .

[0010] A second aspect of the present invention provides a distributed collaborative positioning system taking communication interruption into consideration, comprising: The initial positioning result acquisition module is used to obtain the initial positioning result based on a network composed of multiple intelligent agents. k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- Positioning result at moment 1; The first intermediate positioning result acquisition module is used to obtain the k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- The positioning result at time 1 is used k Momentary Agent Internal measurement, intelligent agent The motion evolution model of k Moment and Agent The identification model of neighboring agents with communication interruption uses linear minimum mean square error estimation to calculate the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with communication interruption; The second intermediate positioning result acquisition module is used to The intermediate positioning results are sent to k Moment and Agent Neighbor agents that can still communicate and obtain k Moment and Agent Intermediate positioning results sent by neighboring agents that are still able to communicate; The final positioning result acquisition module is used to obtain the intelligent agent The relative measurement of all neighboring agents, using the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication, k Moment and Agent The intermediate positioning results sent by neighboring agents that can still communicate are calculated k Momentary Agent The final positioning result, k Moment and Agent The final positioning results of neighboring agents with interrupted communication.

[0011] A further improvement of the technical solution of the present invention is that the k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- The positioning result at time 1 is used k Momentary Agent Internal measurement, intelligent agent The motion evolution model of k Moment and Agent The identification model of neighboring agents with communication interruption uses linear minimum mean square error estimation to calculate the agent The intermediate positioning results, k Moment and Agent In the step of intermediate positioning results of neighboring agents with communication interruption, First construct the stacking motion model and the stacking internal measurement model, and then simultaneously calculate the agent based on the stacking motion model and the stacking internal measurement model The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication; k Moment and Agent The identification model of neighboring agents with interrupted communication is: ; Where: Representing an agent Maintained and intelligent Neighbor Agents with Lost Communication status, , Representing an agent In the horizontal position, Representing an agent Position in the vertical direction; express exist Moment and The state increment between moments is assumed to obey the mean , the variance is Gaussian distribution of different moments There is no correlation between irrelevant; Agent-based The motion evolution model of k Moment and Agent The stacking motion model constructed by the identification model of the neighboring agents with interrupted communication is: ; Where, Representing an agent Status The state of a neighboring agent that loses communication with the agent it maintains The stacked state of the composition, , Representing an agent The speed in the horizontal direction, Representing an agent The speed in the vertical direction, express Moment and Agent The set of neighboring agents with disrupted communication; , represents the block diagonal matrix generating function, Representing an agent The state transition matrix, represents a unit matrix of appropriate size; , Representing an agent The noise gain matrix, represents a vector or matrix of zeros of appropriate size; Representing an agent The process noise is assumed to have a mean of 0 and a variance of Gaussian white noise; ; The constructed stack internal measurement model is: ; Where, , ; , ; Representing an agent Internal measurements obtained The measurement noise is assumed to have a mean of 0 and a variance of Gaussian white noise; Simultaneous calculation of intelligent agents based on stack motion model and stack internal measurement model The intermediate positioning results,k Moment and Agent Intermediate positioning results of neighboring agents with communication interruption ; Where, Express An intermediate estimate of It represents the covariance of the estimation error and is calculated as: ; Where, , and From Momentary Agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- Positioning result at moment 1 , , , For intelligent agents Increment the state of neighboring agents Variance estimates; ; ; .

[0012] A further improvement of the technical solution of the present invention is that the intelligent body is calculated based on the stacking motion model and the stacking internal measurement model at the same time. The intermediate positioning results, k Moment and Agent During the process of intermediate positioning results of neighboring agents with communication interruption, it is estimated The steps for calculating the mean and variance of include: First, establish the innovation exponential smoothing state space model of the horizontal position and vertical position of the neighboring agent, and then establish The state space model of and get and ;in, The horizontal position of the established neighboring agents The state space model of innovation exponential smoothing is: ; Where: , and are the horizontal component and trend component of the neighbor agent’s horizontal position, respectively; is the error term, assuming its mean is 0 and its variance is Gaussian white noise; and are the coefficients in the model; Estimate initial values ​​of model states using maximum likelihood and model parameters 、 and ; 、 and It is obtained by solving the following optimization problem: ; Where: , From the neighbors Intermediate positioning results sent before communication interruption The horizontal position estimate in yes Time Neighbor Agent An intermediate estimate of its own state, is the covariance of the estimation errors; Agent at any moment With neighbors When a communication interruption occurs, ; Similarly, 、 、 and , thus establishing The state space model of is: ; Where, ; ; is the error term, assuming its mean is 0 and its variance is Gaussian white noise, , for Constant Neighbors Intermediate estimate results sent The covariance of the position estimation errors in ; ; ; based on The state space model predicts and get and , the calculation formula is as follows: ; ; Where, .

[0013] A further improvement of the technical solution of the present invention is that the acquisition of the intelligent agent The relative measurement of all neighboring agents, using the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication, k Moment and Agent The intermediate positioning results sent by neighboring agents that can still communicate are calculated k Momentary Agent The final positioning result, k Moment and Agent In the step of the final positioning result of the neighboring agent with interrupted communication, First, a stacked linearized measurement model is established, and then the stacked linearized measurement model is simultaneously calculated. k Momentary Agent The final positioning result, k Moment and Agent The final positioning result of the neighboring agent with interrupted communication; The stacked linearized measurement model is: ; Where, , express Momentary Agent Obtained neighbor agents The relative measurement of express Always be able to communicate with the agent The set of neighboring agents that communicate; , express Momentary Agent Neighbor Agents The measurement function of ; , , , ; , , ; ; , Representing an agent Neighbor Agents The measurement noise of the relative measurement is assumed to be 0 and the variance is Gaussian white noise; Then, based on the stacked linearized measurement model, Kalman filtering is used to simultaneously calculate k Momentary Agent The final positioning result, k Moment and Agent Final positioning results of neighboring agents with interrupted communication , where Express Estimates, It represents the covariance of the estimation error and is calculated as follows: ; Where, , , , .

[0014] In a third aspect of the present invention, an electronic device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the distributed collaborative positioning method taking into account communication interruption as described in any one of the first aspects of the present invention is implemented.

[0015] According to a fourth aspect of the present invention, a non-transitory computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the distributed collaborative positioning method considering communication interruption as described in any one of the first aspects of the present invention is implemented.

[0016] Compared with the prior art, the present invention has the following beneficial effects: The present invention specifically discloses a distributed collaborative positioning method that takes communication interruptions into account, which can obtain accurate distributed collaborative positioning results when communication interruptions are taken into account. Specifically, the technical solution of the present invention reconstructs a factor graph for scenarios where communication interruptions occur during collaborative positioning, so that the intelligent agent can still use the identification model of the neighboring intelligent agent and the relative measurement of the neighboring intelligent agent to continuously improve positioning during communication interruptions. Compared with the traditional collaborative positioning algorithm that cannot coordinate when communication is interrupted, and even degenerates to the track calculation algorithm, the technical solution of the present invention can continue to use relative measurement to improve positioning during communication interruptions, thereby improving the accuracy of coordinated positioning and effectively preventing the rapid growth of positioning errors.

[0017] In the preferred technical solution of the present invention, when the motion evolution model of the neighboring intelligent agent is unknown, an identification model is constructed based on the time series model using the historical intermediate positioning results sent by the neighboring intelligent agent, further ensuring that the intelligent agent can still obtain accurate positioning results in the scenario of communication interruption. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below; obviously, the drawings described below are some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0019] Figure 1 is a collaborative positioning factor graph in a communication interruption scenario in an embodiment of the present invention; Figure 2 is a flow chart of a distributed collaborative positioning method considering communication interruption in a specific embodiment of the present invention; Figure 3 is a schematic diagram of a grid topology simulation scenario in a specific embodiment of the present invention; Figure 4 is a schematic diagram of a chain topology simulation scenario in a specific embodiment of the present invention; Figure 5 1 is a schematic diagram of the RMSE of the absolute position of agent 5 in a grid topology scenario in a specific embodiment of the present invention; Figure 6 This is a schematic diagram of the RMSE of the absolute position of agent 5 in a chain topology scenario in a specific embodiment of the present invention; Figure 7 This is a schematic diagram of a distributed collaborative positioning system taking communication interruption into consideration in an embodiment of the present invention. DETAILED DESCRIPTION

[0020] In order to make the purpose, technical solutions and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention; it is obvious that the described embodiments and technical solutions are only part of the embodiments of the present invention, not all of the embodiments.

[0021] All other embodiments obtained by persons of ordinary skill in the art based on the technical solutions disclosed in the embodiments of the present invention without creative effort shall fall within the scope of protection of the present invention. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units that are not explicitly listed or that are inherent to these processes, methods, products, or apparatuses.

[0022] See also Figure 1 and Figure 2 The embodiment of the present invention provides a distributed collaborative positioning method considering communication interruption. The entire calculation process is as follows: Figure 1The BP algorithm is executed on the factor graph constructed for the communication interruption scenario shown in the figure, which specifically includes the following steps: In a network composed of multiple agents, Momentary Agent The state of maintenance is defined as , where express Momentary Agent The state is defined as , and Represents the intelligent agent Position and velocity in the horizontal and vertical directions; express Momentary Agent Maintained Neighbor Agents The state is defined as ; express Moment and Agent The set of neighboring agents where communication interruption occurs; in addition, express Moment and Agent A set of neighboring agents that communicate normally.

[0023] Momentary Agent Getting internal measurements After that, the agent can be calculated The intermediate positioning results and the agent Intermediate positioning results of neighboring agents with communication interruption , where Representing an agent Status The state of the neighboring agents it maintains The stacked state of the composition, Yes An intermediate estimate of is the covariance of the corresponding estimation error. Get relative metrics on neighboring agents , the intermediate positioning results sent by the neighboring intelligent agent with normal communication After that, we can calculate Momentary Agent The final positioning results and the agent Final positioning results of neighboring agents with interrupted communication ,in is the intermediate estimate of the neighboring agent’s own state, is the covariance of the corresponding estimation error, Express Estimates, represents the covariance of the estimation errors.

[0024] Step 1, Momentary Agent Getting internal measurements After that, the computing agent The intermediate positioning results and k Moment and Agent Intermediate positioning results of neighboring agents with communication interruption ;in, Agent Need to be based on neighboring agents The intermediate estimation results sent in history are used to establish the state of neighboring agents Identification model of , where express exist Moment and The state increment between moments is assumed to obey the mean , the variance is Gaussian distribution of different moments There is no correlation between Unrelated, thus constructing Next, the agent Construct a stack internal measurement model. Then use Positioning results at the moment 、 Internal measurement of time and the corresponding stack internal measurement model, The stacked motion model uses linear minimum mean square error estimation to calculate the intermediate positioning results of itself and neighboring agents with which communication is interrupted.

[0025] First, we need to establish the innovation exponential smoothing state space model of the horizontal position and vertical position of the neighboring agent, and then establish The state space model of the established state space model is used to predict the identification model. and get and , where For intelligent agents Increment the state of neighboring agents Variance Estimates.

[0026] Select the innovation exponential smoothing state space model with additive error, linear trend, and no seasonal component for the horizontal or vertical position of the neighbor agent. For example, the established model is as follows: ; Where, , and are the horizontal component and trend component of the neighbor agent’s horizontal position, respectively; is the error term, assuming its mean is 0 and its variance is Gaussian white noise; and are the coefficients in the model.

[0027] The initial values ​​of the model states need to be estimated using the maximum likelihood method and model parameters 、 and Specifically, the joint probability density of the horizontal position at each historical moment is the product of the probability density of the new information at each moment, and then the neighboring agents are used to calculate the joint probability density of the horizontal position at each historical moment. The horizontal position estimate of the intermediate positioning result sent replaces the true horizontal position. 、 、 、 The log-likelihood function is: ; Where, , From neighboring agents Intermediate positioning results sent before communication interruption Horizontal position estimation in ; Agent at any moment With neighbors The moment when the communication interruption occurred.

[0028] right Find the partial derivative and set it equal to 0, and we get The maximum likelihood estimate of is: ; Substitute the above formula into the expression of the log-likelihood function and take the log-likelihood function times, ignoring the constant term, the objective function to be optimized is: ; Therefore, we use the maximum likelihood method to estimate 、 、 The optimization problem is defined as: ; The same logic applies 、 、 and , thus establishing The state space model of is: ; Where, ; ; is the error term, assuming its mean is 0 and its variance is Gaussian white noise, because the position estimate sent by the neighboring agent is used when estimating the error term variance of the innovation exponential smoothing state space model. In order to consider the uncertainty of the position estimate, Time and The diagonal matrix composed of the maximum likelihood estimate of The sum of the covariance matrices of the position estimation errors in the intermediate positioning results of neighboring agents at the moment, that is, , where Indicates that the elements in the brackets form a focus array. for Constant Neighbors The intermediate positioning results sent The covariance of the position estimation errors in ; ; .

[0029] Based on this model, the prediction and get and , the calculation formula is as follows: ; ; Where, .

[0030] Constructed The stacking motion model is: ; Where, , represents the block diagonal matrix generating function, Representing an agent The state transition matrix, represents a unit matrix of appropriate size; , Representing an agent The noise gain matrix, represents a vector or matrix of zeros of appropriate size; Representing an agent The process noise is assumed to have a mean of 0 and a variance of Gaussian white noise; .

[0031] The constructed stack internal measurement model is: ; Where, , ; , ; represents the measurement noise, which has a covariance of Zero-mean Gaussian white noise.

[0032] Compute the agent using linear minimum mean square error estimation The intermediate positioning results and the agent Intermediate positioning results of neighboring agents with communication interruption The calculation formula is: ; Where, , , ; ; ; .

[0033] Step 2, Agent Get its relative measure to neighboring agents , the intermediate positioning results sent by the neighboring intelligent agent with normal communication , and calculate k Momentary Agent The final positioning result, k Moment and Agent Final positioning results of neighboring agents with interrupted communication .

[0034] The model definition for a single relative measurement is as follows:

[0035] Where: is the measurement function; is the measurement noise, and the covariance is Zero-mean Gaussian white noise.

[0036] The calculation of the final positioning result is equivalent to performing the update step of the Kalman filter based on the stacked linearized measurement model. Here we use and Perform a first-order Taylor expansion of the relative measurement model for the working point, and then transform the agent The linearized equations of all available relative measurements are stacked together to obtain the stacked linearized measurement model as follows: ; Where: , express Momentary Agent Obtained neighbor agents Relative measurement of ; ; , where , ; ; , , ; ; ; Based on the stacked linearized measurement model, Kalman filtering is used to simultaneously calculate k Momentary Agent The final positioning result, k Moment and Agent Final positioning results of neighboring agents with interrupted communication , the calculation formula is: ; Where: .

[0037] The distributed collaborative positioning process considering communication interruption proposed by the present invention is as follows: Figure 2 shown.

[0038] The proposed distributed collaborative localization algorithm considering communication interruptions was tested in two dynamic scenarios and compared with the results of a scenario where no communication interruption occurred and a scenario where the relative measurements related to the neighbors with communication interruption were discarded after a communication interruption occurred. The agents performed approximately uniform linear motion, and the motion evolution model was: ; Where: ; Process noise The covariance is Zero-mean Gaussian white noise.

[0039] Measurement noise of internal measurements The covariance is The relative measurement can provide the position and orientation measurement of the neighboring agent relative to itself, that is, ; And measure the noise The covariance is The difference between the two scenarios is that the communication and measurement topologies between agents are different. Figure 3 As shown in the figure, 9 agents form a grid topology; scene 2 is as follows Figure 4 As shown in the figure, nine agents form a grid topology. The black dots in the figure represent the agents, and the numbers to the upper right of the black dots indicate the agent numbers. The dashed lines indicate that when communication is stable, two connected agents are neighbors and can measure and communicate with each other.

[0040] The simulation process is divided into three stages. In the first stage, the agents Figure 3 and Figure 4 The initial position shown in the figure starts to move, the communication is stable, and each agent executes the distributed collaborative positioning algorithm to improve its own positioning using the motion evolution model, internal measurements and relative measurements; When , the simulation enters the second stage, the communication between agent 5 and its neighboring agents is interrupted. In scenario 1, the communication between agent 5 and agents 2, 4, 6, and 8 is interrupted. In scenario 2, the communication between agent 5 and agents 4 and 6 is interrupted. When , the simulation enters the third stage, and the communication between agent 5 and its neighboring agents is restored to normal. The distributed collaborative localization algorithm considering communication interruption proposed in this paper mainly plays a role in the second stage.

[0041] In addition, the initial state of the agent ,in It consists of the initial position and velocity. The initial position is as follows: Figure 3 and Figure 4 As shown, the initial velocity is , In each scenario, 1000 Monte Carlo simulations were performed, and the simulation results were as follows: Figure 5 and Figure 6As shown in the figure. We use the root mean squared error (RMSE) of the agent's absolute position to measure the algorithm's positioning performance. Absolute position refers to the agent's position in a fixed external coordinate system. In the figure, DR describes the effect of using a dead reckoning algorithm, where agents do not collaborate and each agent uses only a motion evolution model and internal measurements for positioning. GiveUp describes the effect of abandoning the use of relative measurements associated with neighbors experiencing communication interruptions during the second phase of the simulation to improve positioning. For agent 5, this is equivalent to using the dead reckoning algorithm in the second phase. Normal describes the positioning effect when no communication interruptions occur throughout the simulation. ETS describes the positioning effect using the distributed collaborative localization algorithm proposed in this paper that considers communication interruptions. It can be seen that the collaborative localization algorithm described in this paper that considers communication interruptions performs consistently with the original algorithm when communication is stable. Its positioning performance during communication interruptions is superior to that of abandoning the use of relative measurements associated with neighbors experiencing communication interruptions, demonstrating that the algorithm successfully utilizes these relative measurements to improve positioning.

[0042] The following are device embodiments of the present invention, which can be used to perform the method embodiments of the present invention. For details not disclosed in the device embodiments, please refer to the method embodiments of the present invention.

[0043] See also Figure 7 In an embodiment of the present invention, a distributed collaborative positioning system considering communication interruption is provided, including: The initial positioning result acquisition module is used to obtain the initial positioning result based on a network composed of multiple intelligent agents. k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- Positioning result at moment 1; The first intermediate positioning result acquisition module is used to obtain the k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- The positioning result at time 1 is used k Momentary Agent Internal measurement, intelligent agent The motion evolution model of k Moment and Agent The identification model of neighboring agents with communication interruption uses linear minimum mean square error estimation to calculate the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with communication interruption; The second intermediate positioning result acquisition module is used to The intermediate positioning results are sent to k Moment and Agent Neighbor agents that can still communicate and obtain k Moment and Agent Intermediate positioning results sent by neighboring agents that are still able to communicate; The final positioning result acquisition module is used to obtain the intelligent agent The relative measurement of all neighboring agents, using the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication, k Moment and Agent The intermediate positioning results sent by neighboring agents that can still communicate are calculated k Momentary Agent The final positioning result, k Moment and Agent The final positioning results of neighboring agents with interrupted communication.

[0044] In one embodiment of the present invention, a computer device is provided, comprising a processor and a memory, wherein the memory is configured to store a computer program, the computer program including program instructions, and the processor is configured to execute the program instructions stored in the computer storage medium. The processor may be a central processing unit (CPU), or may also be another general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic device, discrete gate or transistor logic device, discrete hardware component, etc. The processor is the computing core and control core of the terminal and is suitable for implementing one or more instructions, specifically loading and executing one or more instructions in the computer storage medium to implement a corresponding method flow or corresponding function. The processor described in this embodiment of the present invention can be used to perform operations of a distributed collaborative positioning method that takes into account communication interruptions.

[0045] In one embodiment of the present invention, a storage medium is provided, specifically a computer-readable storage medium (Memory). The computer-readable storage medium is a memory device in a computer device, used to store programs and data. It is understood that the computer-readable storage medium herein may include both built-in storage media in the computer device and, of course, extended storage media supported by the computer device. The computer-readable storage medium provides storage space that stores the terminal's operating system. Furthermore, the storage space also stores one or more instructions suitable for being loaded and executed by a processor. These instructions may be one or more computer programs (including program code). It should be noted that the computer-readable storage medium herein may be high-speed Random Access Memory (RAM) or non-volatile memory, such as at least one disk drive. The processor may load and execute the one or more instructions stored in the computer-readable storage medium to implement the corresponding steps of the distributed collaborative location method considering communication interruptions in the above-mentioned embodiment.

[0046] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, optical storage, etc.) containing computer-usable program code.

[0047] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0048] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0049] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0050] 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 it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered by the scope of protection of the claims of the present invention.

Claims

1. A distributed collaborative positioning method considering communication interruption, characterized in that: The following steps are involved: Based on a network composed of multiple agents, obtain k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- Positioning result at moment 1; based on k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- The positioning result at time 1 is used k Momentary Agent Internal measurement, intelligent agent The motion evolution model of k Moment and Agent The identification model of neighboring agents with communication interruption uses linear minimum mean square error estimation to calculate the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with communication interruption; The agent The intermediate positioning results are sent to k Moment and Agent Neighbor agents that can still communicate and obtain k Moment and Agent Intermediate positioning results sent by neighboring agents that are still able to communicate; Get the agent The relative measurement of all neighboring agents, using the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication, k Moment and Agent The intermediate positioning results sent by neighboring agents that can still communicate are calculated k Momentary Agent The final positioning result, k Moment and Agent The final positioning results of neighboring agents with interrupted communication.

2. A distributed collaborative positioning method considering communication interruption according to claim 1, characterized in that: The based k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- The positioning result at time 1 is used k Momentary Agent Internal measurement, intelligent agent The motion evolution model of k Moment and Agent The identification model of neighboring agents with communication interruption uses linear minimum mean square error estimation to calculate the agent The intermediate positioning results, k Moment and Agent In the step of intermediate positioning results of neighboring agents with communication interruption, First construct the stacking motion model and the stacking internal measurement model, and then simultaneously calculate the agent based on the stacking motion model and the stacking internal measurement model The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication; k Moment and Agent The identification model of neighboring agents with interrupted communication is: ； Where: Representing an agent Maintained and intelligent Neighbor Agents with Lost Communication status, , Representing an agent In the horizontal position, Representing an agent Position in the vertical direction; express exist Moment and The state increment between moments is assumed to obey the mean , the variance is Gaussian distribution of different moments There is no correlation between irrelevant; Agent-based The motion evolution model of k Moment and Agent The stacking motion model constructed by the identification model of the neighboring agents with interrupted communication is: ； Where, Representing an agent Status The state of a neighboring agent that loses communication with the agent it maintains The stacked state of the composition, , Representing an agent The speed in the horizontal direction, Representing an agent The speed in the vertical direction, express Moment and Agent The set of neighboring agents with disrupted communication; , represents the block diagonal matrix generating function, Representing an agent The state transition matrix, represents a unit matrix of appropriate size; , Representing an agent The noise gain matrix, represents a vector or matrix of zeros of appropriate size; Representing an agent The process noise is assumed to have a mean of 0 and a variance of Gaussian white noise; ; The constructed stack internal measurement model is: ； Where, , ; , ; Representing an agent Internal measurements obtained The measurement noise is assumed to have a mean of 0 and a variance of Gaussian white noise; Simultaneous calculation of intelligent agents based on stack motion model and stack internal measurement model The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with communication interruption ; Where, Express An intermediate estimate of It represents the covariance of the estimation error and is calculated as: ； Where, , and From Momentary Agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- Positioning result at moment 1 , , , For intelligent agents Increment the state of neighboring agents Variance estimates; ； ； 。 3. The distributed collaborative positioning method considering communication interruption according to claim 2, characterized in that: The intelligent agent is calculated based on the stacked motion model and the stacked internal measurement model. The intermediate positioning results, k Moment and Agent During the process of intermediate positioning results of neighboring agents with communication interruption, it is estimated The steps for calculating the mean and variance of include: First, establish the innovation exponential smoothing state space model of the horizontal position and vertical position of the neighboring agent, and then establish The state space model of and get and ;in, The horizontal position of the established neighboring agents The innovation exponential smoothing state space model is: ； Where: , and are the horizontal component and trend component of the neighbor agent’s horizontal position, respectively; is the error term, assuming that its mean is 0 and its variance is Gaussian white noise; and are the coefficients in the model; Estimate initial values ​​of model states using maximum likelihood and model parameters 、 and ; 、 and It is obtained by solving the following optimization problem: ； Where: , From the neighbors Intermediate positioning results sent before communication interruption The horizontal position estimate in yes Time Neighbor Agent An intermediate estimate of its own state, is the covariance of the estimation errors; Agent at any moment With neighbors When a communication interruption occurs, ; Similarly, 、 、 and , thus establishing The state space model of is: ； Where, ; ; is the error term, assuming that its mean is 0 and its variance is Gaussian white noise, , for Constant Neighbors Intermediate estimate results sent The covariance of the position estimation errors in ; ; ; based on The state space model predicts and get and , the calculation formula is as follows: ； ； Where, .

4. The distributed collaborative positioning method considering communication interruption according to claim 3, characterized in that: The acquisition of intelligent agent The relative measurement of all neighboring agents, using the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication, k Moment and Agent The intermediate positioning results sent by neighboring agents that can still communicate are calculated k Momentary Agent The final positioning result, k Moment and Agent In the step of the final positioning result of the neighboring agent with interrupted communication, First, a stacked linearized measurement model is established, and then the stacked linearized measurement model is simultaneously calculated. k Momentary Agent The final positioning result, k Moment and Agent The final positioning result of the neighboring agent with interrupted communication; The stacked linearized measurement model is: ； Where, , express Momentary Agent Obtained neighbor agents The relative measurement of express Always be able to communicate with the agent The set of neighboring agents that communicate; , express Momentary Agent Neighbor Agents The measurement function of ; , , , ; , , ; ; , Representing an agent Neighbor Agents The measurement noise of the relative measurement is assumed to be 0 and the variance is Gaussian white noise; Then, based on the stacked linearized measurement model, Kalman filtering is used to simultaneously calculate k Momentary Agent The final positioning result, k Moment and Agent Final positioning results of neighboring agents with interrupted communication , where Express Estimates, It represents the covariance of the estimation error and is calculated as follows: ； Where, , , , .

5. A distributed collaborative positioning system considering communication interruption, characterized in that: include: The initial positioning result acquisition module is used to obtain the initial positioning result based on a network composed of multiple intelligent agents. k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- Positioning result at moment 1; The first intermediate positioning result acquisition module is used to obtain the k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- The positioning result at time 1 is used k Momentary Agent Internal measurement, intelligent agent The motion evolution model of k Moment and Agent The identification model of neighboring agents with communication interruption uses linear minimum mean square error estimation to calculate the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with communication interruption; The second intermediate positioning result acquisition module is used to The intermediate positioning results are sent to k Moment and Agent Neighbor agents that can still communicate and obtain k Moment and Agent Intermediate positioning results sent by neighboring agents that are still able to communicate; The final positioning result acquisition module is used to obtain the intelligent agent The relative measurement of all neighboring agents, using the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication, k Moment and Agent The intermediate positioning results sent by neighboring agents that can still communicate are calculated k Momentary Agent The final positioning result, k Moment and Agent The final positioning results of neighboring agents with interrupted communication.

6. A distributed collaborative positioning system considering communication interruption according to claim 5, characterized in that: The based k- 1-moment agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- The positioning result at time 1 is used k Momentary Agent Internal measurement, intelligent agent The motion evolution model of k Moment and Agent The identification model of neighboring agents with communication interruption uses linear minimum mean square error estimation to calculate the agent The intermediate positioning results, k Moment and Agent In the step of intermediate positioning results of neighboring agents with communication interruption, First construct the stacking motion model and the stacking internal measurement model, and then simultaneously calculate the agent based on the stacking motion model and the stacking internal measurement model The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication; k Moment and Agent The identification model of neighboring agents with interrupted communication is: ； Where: Representing an agent Maintained and intelligent Neighbor Agents with Lost Communication status, , Representing an agent In the horizontal position, Representing an agent Position in the vertical direction; express exist Moment and The state increment between moments is assumed to obey the mean , the variance is Gaussian distribution of different moments There is no correlation between irrelevant; Agent-based The motion evolution model of k Moment and Agent The stacking motion model constructed by the identification model of the neighboring agents with interrupted communication is: ； Where, Representing an agent Status The state of a neighboring agent that loses communication with the agent it maintains The stacked state of the composition, , Representing an agent The speed in the horizontal direction, Representing an agent The speed in the vertical direction, express Moment and Agent The set of neighboring agents with disrupted communication; , represents the block diagonal matrix generating function, Representing an agent The state transition matrix, represents a unit matrix of appropriate size; , Representing an agent The noise gain matrix, represents a vector or matrix of zeros of appropriate size; Representing an agent The process noise is assumed to have a mean of 0 and a variance of Gaussian white noise; ; The constructed stack internal measurement model is: ； Where, , ; , ; Representing an agent Internal measurements obtained The measurement noise is assumed to have a mean of 0 and a variance of Gaussian white noise; Simultaneous calculation of intelligent agents based on stack motion model and stack internal measurement model The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with communication interruption ; Where, Express An intermediate estimate of It represents the covariance of the estimation error and is calculated as: ； Where, , and From Momentary Agent The positioning results, k Moment and Agent Neighboring agents whose communication is interrupted k- Positioning result at moment 1 , , , For intelligent agents Increment the state of neighboring agents Variance estimates; ； ； 。 7. The distributed collaborative positioning system considering communication interruption according to claim 6, characterized in that: The intelligent agent is calculated based on the stacked motion model and the stacked internal measurement model. The intermediate positioning results, k Moment and Agent During the process of intermediate positioning results of neighboring agents with communication interruption, it is estimated The steps for calculating the mean and variance of include: First, establish the innovation exponential smoothing state space model of the horizontal position and vertical position of the neighboring agent, and then establish The state space model of and get and ;in, The horizontal position of the established neighboring agents The innovation exponential smoothing state space model is: ； Where: , and are the horizontal component and trend component of the neighbor agent’s horizontal position, respectively; is the error term, assuming that its mean is 0 and its variance is Gaussian white noise; and are the coefficients in the model; Estimate initial values ​​of model states using maximum likelihood and model parameters 、 and ; 、 and It is obtained by solving the following optimization problem: ； Where: , From the neighbors Intermediate positioning results sent before communication interruption The horizontal position estimate in yes Time Neighbor Agent An intermediate estimate of its own state, is the covariance of the estimation errors; Agent at any moment With neighbors When a communication interruption occurs, ; Similarly, 、 、 and , thus establishing The state space model of is: ； Where, ; ; is the error term, assuming that its mean is 0 and its variance is Gaussian white noise, , for Constant Neighbors Intermediate estimate results sent The covariance of the position estimation errors in ; ; ; based on The state space model predicts and get and , the calculation formula is as follows: ； ； Where, .

8. The distributed collaborative positioning system considering communication interruption according to claim 7, characterized in that: The acquisition of intelligent agent The relative measurement of all neighboring agents, using the agent The intermediate positioning results, k Moment and Agent Intermediate positioning results of neighboring agents with interrupted communication, k Moment and Agent The intermediate positioning results sent by neighboring agents that can still communicate are calculated k Momentary Agent The final positioning result, k Moment and Agent In the step of the final positioning result of the neighboring agent with interrupted communication, First, a stacked linearized measurement model is established, and then the stacked linearized measurement model is simultaneously calculated. k Momentary Agent The final positioning result, k Moment and Agent The final positioning result of the neighboring agent with interrupted communication; The stacked linearized measurement model is: ； Where, , express Momentary Agent Obtained neighbor agents The relative measurement of express Always be able to communicate with the agent The set of neighboring agents that communicate; , express Momentary Agent Neighbor Agents The measurement function of ; , , , ; , , ; ; , Representing an agent Neighbor Agents The measurement noise of the relative measurement is assumed to be 0 and the variance is Gaussian white noise; Then, based on the stacked linearized measurement model, Kalman filtering is used to simultaneously calculate k Momentary Agent The final positioning result, k Moment and Agent Final positioning results of neighboring agents with interrupted communication , where Express Estimates, It represents the covariance of the estimation error and is calculated as follows: ； Where, , , , .

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the distributed collaborative positioning method considering communication interruption according to any one of claims 1 to 4 is implemented.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the distributed collaborative positioning method considering communication interruption according to any one of claims 1 to 4 is implemented.

Images