CN108594169B - Multi-robot distributed cooperative positioning method suitable for time-varying communication topology - Google Patents

Multi-robot distributed cooperative positioning method suitable for time-varying communication topology Download PDF

Info

Publication number
CN108594169B
CN108594169B CN201810211742.7A CN201810211742A CN108594169B CN 108594169 B CN108594169 B CN 108594169B CN 201810211742 A CN201810211742 A CN 201810211742A CN 108594169 B CN108594169 B CN 108594169B
Authority
CN
China
Prior art keywords
robot
information
cooperative positioning
positioning information
robots
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810211742.7A
Other languages
Chinese (zh)
Other versions
CN108594169A (en
Inventor
王雷钢
崔建岭
张杨
戴幻尧
孔德培
赵琳锋
贾锐
石川
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
UNIT 63892 OF PLA
Original Assignee
UNIT 63892 OF PLA
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by UNIT 63892 OF PLA filed Critical UNIT 63892 OF PLA
Priority to CN201810211742.7A priority Critical patent/CN108594169B/en
Publication of CN108594169A publication Critical patent/CN108594169A/en
Application granted granted Critical
Publication of CN108594169B publication Critical patent/CN108594169B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0284—Relative positioning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04L—TRANSMISSION OF DIGITAL INFORMATION, e.g. TELEGRAPHIC COMMUNICATION
    • H04L41/00—Arrangements for maintenance, administration or management of data switching networks, e.g. of packet switching networks
    • H04L41/12—Discovery or management of network topologies
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/025—Services making use of location information using location based information parameters

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Optimization (AREA)
  • Remote Sensing (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Signal Processing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Algebra (AREA)
  • General Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Computing Systems (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention belongs to the technical field of self-positioning of robots, and discloses a multi-robot distributed cooperative positioning method suitable for a time-varying serial communication topology, which is based on a UKF frame and is used for converting distributed cooperative positioning into distributed U transformation. The invention avoids the requirement of single robot positioning on the information of all other robots, and the single robot can estimate the position of the single robot and update the position relation with other robots only by depending on randomly available adjacent robot information. The adaptability of cooperative positioning to dynamic time-varying communication topology is improved, and the problem that communication topology conditions are limited when multiple robots perform tasks such as post-disaster search and rescue, large-scale monitoring and the like can be solved.

Description

Multi-robot distributed cooperative positioning method suitable for time-varying communication topology
Technical Field
The invention belongs to the technical field of robot self-positioning, and particularly relates to a multi-robot distributed cooperative positioning method applied to a time-varying communication topology.
Background
Cooperative positioning is a method for improving the positioning accuracy of multiple robots by using relative information between the multiple robots. For this reason, there are two kinds of implementation architectures of centralized type and distributed type for cooperative positioning, the former is that all robots transmit information to a central processing unit, which has problems of large communication consumption and poor reliability, and the latter is that cooperative positioning is completed in each robot, which is a commonly used architecture. In distributed cooperative positioning, sharing of information circulation among robots is a prerequisite, however, in some special application environments, free information circulation is difficult to provide, for example, in unknown environment exploration, the distance between the robots exceeds a communication path and free communication cannot be achieved, for example, in a countermeasure environment, various intentional or unintentional interferences can block communication links among nodes, and the like, so that the communication topology (describing communication relationship) between the robots is uncertain, and as environmental conditions change, if the situation cannot be adapted to, the cooperative positioning efficiency will be greatly reduced.
Kalman Filtering (KF) is one of the main technologies for solving the problem of cooperative positioning at present, a distributed cooperative positioning algorithm based on the KF is designed in two directions, one is to improve positioning precision and focuses on a theoretical analysis level, and it is often assumed that a robot can freely obtain required information and the two can communicate without constraint; another approach is to use communication conditions as design constraints, but generally assume that the constraints are known and fixed (i.e. communication topology is fixed), that is, for each individual, from whom information is sent to whom the information is specified in advance, algorithm design based on this premise is harsh on communication conditions, and once the information cannot be satisfied in practical application, the cooperative positioning task is faced with the possibility of failure. Therefore, the cooperative positioning method designed based on the two directions is limited in practical application, especially in the case of time-varying communication topology.
Disclosure of Invention
In order to overcome the defects of the prior art, the invention provides a multi-robot distributed cooperative positioning method suitable for a time-varying communication topology.
In order to achieve the purpose, the invention adopts the following technical scheme:
a multi-robot distributed cooperative positioning method suitable for time-varying communication topology adopts unscented Kalman filtering UKF as the technical basis of distributed cooperative positioning, utilizes a group of sigma points to approximate probability density distribution of variables, and aims at obeying normal divisionThe variables of the cloth are equivalently described by using a plurality of sampling points to form an estimated value of the variables and a variance of the estimated value, and the process is called U transformation; here, it is assumed that at time k, the overall position of N robots is estimated as:
Figure BDA0001597382710000011
Figure BDA0001597382710000012
the position estimate of the robot i is represented, and the variance corresponding to the overall position estimate is:
Figure BDA0001597382710000013
Figure BDA0001597382710000014
then the estimated U transforms to:
Figure RE-GDA0001700676640000021
xi in the formula(0)(k)…ξ(4N)(k) The sigma point is the sigma point, and lambda is an adjustable quantity; p (k) is a symmetric matrix,
Figure BDA0001597382710000022
Figure BDA0001597382710000023
to represent
Figure BDA0001597382710000024
The p-th column of (1);
ensuring that a single robot obtains all block elements Pij(k) To construct P (k) to obtain
Figure BDA0001597382710000025
So that a single robot i obtains local sigma points:
Figure BDA0001597382710000026
based on the Cholesky score looking upSolving the problem of distributed acquisition of covariance matrix, and ordering: p ═ AATEach block of element A in AijThe method can be based on solving:
Figure BDA0001597382710000027
Figure BDA0001597382710000028
the formula is as follows: 1) each block element AijIs dependent only on Aij(i > j) and Pij(i > j), rather than all the element blocks, avoids the need for information exchange by all robots; 2) recursion from top to bottom and combining the characteristics with the communication sequence;
decomposing Block element A with a look-Up Cholesky matrixijThe recursive characteristic from top to bottom is that the distributed covariance matrix decomposition is combined with the communication conditions, so that the matrix decomposition is changed from 'demand improvement' for communication to automatic compliance, and the problem of high dependence of distributed cooperative positioning on communication topological conditions is avoided; each robot can complete the position updating of the robot by means of the obtained local information quantity; the method comprises two steps of position prediction and position posterior estimation, wherein output results of the two steps are mutually input;
when no robot in the robot group measures other teammates, each robot continuously predicts the position of the robot according to a certain step length, and how to perform distributed position prediction in each robot is well solved;
when one robot measures teammates and measures the relative information of the teammates, the position prediction values of all the robots are updated by the measurement, and after the posterior estimation is completed, the position prediction is continued.
A distributed cooperative positioning method of multiple robots suitable for time-varying communication topology is characterized in that the working process of a single robot in cooperative positioning is carried out in four steps; aiming at a plurality of robots provided with linear velocity measuring equipment, relative distance measuring equipment and wireless communication equipment, the number of the robot is i, which is abbreviated as Ri; the specific implementation steps are as follows:
the method comprises the following steps: starting a cooperative positioning task;
after the task starts, the robot continuously predicts the position of the robot according to the delta step length; meanwhile, the robot monitors teammates in real time, and whether the information related to the positions and the field angles and sent by the monitoring friends can be monitored or not can be monitored;
at a certain moment, if the robot finds a teammate, executing a second step, and if the robot receives the positioning information, executing a third step; otherwise, the position of the team is continuously estimated, and the team friends are continuously detected and the external information is continuously intercepted. If a cooperative positioning ending instruction is received, stopping team member monitoring;
step two: finding teammates and generating cooperative positioning information;
suppose at kmRi finds teammates at any time, and if a plurality of teammates are found at the same time based on the relative measurement value between the teammates and the finder, only one team closest to the finder is processed to calculate and generate positioning information Si(km) Positioning information Si(km) Then re-entering the step one;
step three: updating the position of the team friend according to the received information of the team friend;
suppose at krAt the moment, Rj receives the information of the adjacent robot, and the Rj firstly judges whether the information is cooperative positioning information or not, and if so, the information is utilized to perform cooperative positioning;
before using the cooperative positioning information, preprocessing is performed: first, whether S has been received is judgedi(km) If the positioning information is received, giving up, otherwise, continuously utilizing the following steps to judge whether the positioning information is effective:
km+δ＞kr \*MERGEFORMAT(4)
wherein k isrIndicating the moment of reception.
If not, abandoning and not transmitting to the adjacent robot, otherwise, Rj utilizes the positioning information Si(km) Updating the position, and then re-entering the step one;
step four: the cooperative positioning task is finished;
after the robot receives the cooperative positioning termination command, the robot does not monitor teammates any more.
3. The multi-robot distributed cooperative positioning method adapted to the time-varying communication topology as claimed in claim 2, wherein: the forming process of the cooperative positioning information is described in detail below by taking an example that the robot Ri observes Rj at the time k:
(1) forming local sigma points
According to
Figure BDA0001597382710000031
Calculation of AiiLet Ai＝[Aii]According to the formula, the robot i correspondence is calculated
Figure BDA0001597382710000032
Figure BDA0001597382710000033
Wherein
Figure BDA0001597382710000034
Which represents a prediction estimate of the location of the mobile device,
Figure BDA0001597382710000035
the variance of the estimate is represented as a function of,
Figure BDA0001597382710000036
represents the covariance between the different estimates;
(2) interacting with the monitored Rj
Request communication with the measured Rj, AiiThe transmission and Rj, Rj calculate A according to the formulaji,Ajj(ii) a Let Aj＝[Aji,Ajj]According to the formula, calculating the correspondence
Figure BDA0001597382710000037
And transmitting the data to Ri;
(3) generating a cooperative positioning information element
Receiving Rj
Figure BDA0001597382710000041
Then according to the calculation
Figure BDA0001597382710000042
And expected measured values
Figure BDA0001597382710000043
Figure BDA0001597382710000044
Figure 1
Wherein
Figure BDA0001597382710000046
Representing superimposed weight values
As to calculate the variance of the expected measurements:
Figure 2
in the formula
Figure BDA0001597382710000048
The overlay weight value is represented. RijRepresenting the relative measurement noise variance;
the covariance between the expected estimate and the expected position estimate is measured as calculated:
Figure BDA0001597382710000049
|neighii represents the number of robots from the measuring robot to the middle of the robot i in the communication topology, and for the robot that finds the teammate: | neighbori|＝1；
(4) Ri updates self-position and local covariance
Using obtained a posteriori estimates
Figure BDA00015973827100000410
By performing an update of the local covariance,
Figure BDA00015973827100000411
Figure BDA00015973827100000412
wherein
Figure BDA00015973827100000413
A posteriori estimate of the position is represented,
Figure BDA00015973827100000414
representing the corresponding error covariance;
(5) the Ri number i, the expected measurement error, the corresponding variance and the like form the cooperative positioning information Si(km)：
Figure BDA00015973827100000415
Will S1(k) Packaging and sending to a neighboring robot;
the cooperative positioning information forming process is compiled into an independent program and is implanted into each robot, and once the robot measures teammates, the program is executed.
A multi-robot distributed cooperative positioning method suitable for a time-varying communication topology is characterized by comprising the following steps: the cooperative positioning information utilization process comprises the following specific implementation steps:
after the robot receives the teammate positioning information, the process of updating the predicted position by using the information; crC represents a robot number set included in the positioning information,
Figure BDA0001597382710000051
receiving positioning information S from Rj by Rij(km)；
(1) Forming local sigma points
Calculating A according to formulaiThen according to the calculation
Figure BDA0001597382710000052
(2) Updating a predicted estimated position
Using formula to calculate
Figure BDA0001597382710000053
Updating the position by utilizing the formula;
(3) updating local covariance
Statistics Sj(k) The robot number set C included in (1) is carried out for each robot j of g e C by using a formula
Figure BDA0001597382710000054
Updating of (1); to pair
Figure BDA0001597382710000055
Figure BDA0001597382710000056
Keeping the same;
(4) reforming positioning information
Packing receiver numbers i, Ai、
Figure BDA0001597382710000057
Sj(k) Information, reforming positioning information Si(k)：
Figure BDA0001597382710000058
Will Si(k) Continuing to send to the neighboring robot;
the cooperative positioning information utilization process is compiled into an independent program which is implanted in each robot, and once the robot receives effective positioning information of teammates, the program is executed.
A distributed cooperative positioning method of multiple robots, which is suitable for time-varying communication topology, predicts the position estimation updating process of each robot under different communication topology conditions; take four robots as an example (C)r1,2,3, 4) and assume that at some point robot R1 finds R2;
(a) the corresponding communication topology is: 1 → 2 → 3 → 4;
(b) the corresponding communication topology is: 1 → 2 → 4 → 3;
representing relative measurement generated by one robot, and enabling all robots to realize self position updating through different communication topologies; in (a), (b), there is an interactive communication between both robots 1,2, in order to form positioning information S1(k) Therefore, it should be ensured that the measuring party and the measured party are in communication first, otherwise, the relative measurement information z is obtained12Will be worthless and discarded.
Due to the adoption of the technical scheme, the invention has the following advantages:
a multi-robot distributed cooperative positioning method suitable for a time-varying communication topology utilizes the recursion characteristic that Cholesky matrix decomposition is upward seen from top to bottom, and combines distributed covariance matrix decomposition with communication conditions, so that matrix decomposition is changed from 'demand improvement' for communication into automatic compliance, and the problem of high dependence of distributed cooperative positioning on the communication topology conditions is avoided. Each robot can complete the position update of the robot by means of the available local information quantity.
The method of the invention overcomes the problem of the attachment of the existing method to the fixed communication topology. The positioning information formed by each robot is not specific to a certain robot, and is regarded as a sender, the robot does not need to determine which robot the positioning information is to be received by, and is regarded as a receiver, and the robot can also process the positioning information of any sender. The method has strong modularity and good actual operation effect, supports independent operation in each robot, and improves the adaptability of multi-robot distributed cooperative positioning to severe communication environment.
Description of the drawings:
fig. 1 is a schematic flow chart of the cooperative positioning based on the UKF, which includes position prediction and position posterior estimation, and shows the overall working process of the cooperative positioning.
Fig. 2 is an overall work flow diagram of a single robot, which shows trigger conditions, end conditions, and the like for executing different processing algorithms when a single robot executes a cooperative positioning task.
Fig. 3 is a flow chart for forming positioning information, which shows how a single robot exchanges information with a measured robot to form positioning information after measuring to a teammate.
Fig. 4 is a flowchart of cooperative positioning information utilization, which shows how a robot updates its own predicted position estimate using cooperative positioning information of teammates after receiving the information.
Fig. 5(a) is a corresponding 1 → 2 → 3 → 4 communication topology, and fig. 5(b) is a corresponding 1 → 2 → 4 → 3 communication topology. And under different communication topological conditions, the four robots are used for showing different position variable updating processes.
The specific implementation mode is as follows:
embodiments of the present invention will be described below with reference to the accompanying drawings.
As shown in fig. 1,2,3,4, 5(a), 5(b), the present invention adopts Unscented Kalman Filtering (UKF) as the basis of the distributed cooperative localization, which is a technique of approximating a probability density distribution of variables by a set of sigma points, and for variables that follow a normal distribution, i.e., using some sampling points to equivalently describe the estimated values of the variables and the variances of the estimates, the process is called U-transform. Here, it is assumed that at time k, the overall position of N robots is estimated as:
Figure BDA0001597382710000061
Figure BDA0001597382710000062
the position estimate of the robot i is represented, and the variance corresponding to the overall position estimate is:
Figure BDA0001597382710000063
then the estimated U transforms to:
Figure RE-GDA0001700676640000063
xi in the formula(0)(k)…ξ(4N)(k) I.e., sigma point, and λ is an adjustable quantity. P (k) is a symmetric matrix,
Figure BDA0001597382710000065
Figure BDA0001597382710000066
to represent
Figure BDA0001597382710000067
Column p. Under a distributed architecture, when communication conditions are limited, it is difficult to guarantee that a single robot obtains all block elements Pij(k) By constructing P (k), it is difficult to obtain
Figure BDA0001597382710000068
So that a single robot i obtains local sigma points:
Figure BDA0001597382710000069
the invention solves the distributed acquisition problem of the covariance matrix based on the upward-looking Cholesky decomposition, and leads: p ═ AATEach block of element A in AijThe method can be based on solving:
Figure BDA0001597382710000071
Figure BDA0001597382710000072
it can be seen that the above formula has two features: 1) each block element AijIs dependent only on Aij(i > j) and Pij(i > j) instead of all element blocks, which avoids the need for information exchange by all robots, 2) block element AijThis feature may be considered in conjunction with the communication order, as may be inferred from the top-down.
Fig. 1 depicts, from an overall point of view, the principle process of cooperative positioning (irrespective of communication implementation), which includes two steps of position prediction and position posterior estimation, with the two output results being inputs to each other. When no robot in the robot group measures other teammates, each robot continuously predicts the position of the robot according to a certain step length, and how to perform distributed position prediction in each robot is well solved; when one robot measures teammates and measures relative information of the teammates, the position prediction values of all the robots are updated by the measurement, after the posterior estimation updating is completed, the position prediction is continued, and how to implement the position posterior estimation in a distributed mode is the key point of the invention.
Fig. 2 shows the workflow of a single robot (each robot being identical) in a cooperative positioning, which is performed in four steps. The present invention is directed to a plurality of robots equipped with a linear velocity measuring device, a relative distance measuring device (e.g., a laser radar), and a wireless communication device, and is abbreviated as Ri for a robot numbered i.
The method comprises the following steps: initiating a cooperative positioning task
After the task starts, the robot continuously predicts the position of the robot according to the delta step length. Meanwhile, the robot monitors teammates in real time (whether information related to the positions and the angles of view can be monitored) and listens to information sent by friends.
At a certain moment, if the robot finds a teammate, executing a second step, and if the robot receives the positioning information, executing a third step; otherwise, the position of the team is continuously estimated, and the team friends are continuously detected and the external information is continuously intercepted. And if the cooperative positioning ending instruction is received, stopping team member monitoring.
Step two: finding teammates and generating cooperative positioning information
Suppose at kmAt the moment, Ri finds the teammate, and only processes and if more than one is found at the same time, based on its relative measurement with the found personThe nearest one of the discoverers), the location information S is computationally generatedi(km) The specific method is shown in the cooperative positioning information forming flow, as shown in fig. 3, and then step one is re-entered.
Step three: updating self position by the received team friend information
Suppose at krAt the moment, Rj receives the adjacent robot information, and Rj firstly judges whether the information is cooperative positioning information or not, and if so, the information is utilized to perform cooperative positioning.
Before using the cooperative positioning information, preprocessing is performed: first, whether S has been received is judgedi(km) If the positioning information is received, giving up, otherwise, continuously utilizing the following steps to judge whether the positioning information is effective:
km+δ＞kr \*MERGEFORMAT(4)
wherein k isrIndicating the moment of reception.
If not, abandoning and not transmitting to the adjacent robot, otherwise, Rj utilizes Si(km) And (5) updating the position, wherein a specific method is a cooperative positioning information utilization process, as shown in fig. 4, and then the step one is re-entered.
Step four: cooperative positioning task termination
After the robot receives the cooperative positioning termination command, the robot does not monitor teammates any more.
Fig. 3 shows a forming process of the cooperative positioning information, which is described in detail below by taking an example that the robot Ri observes Rj at time k:
(1) forming local sigma points
According to
Figure BDA0001597382710000081
Calculation of AiiLet Ai＝[Aii]According to the formula, the robot i correspondence is calculated
Figure BDA0001597382710000082
Figure BDA0001597382710000083
Wherein
Figure BDA0001597382710000084
Which represents a prediction estimate of the location of the mobile device,
Figure BDA0001597382710000085
the variance of the estimate is represented as a function of,
Figure BDA0001597382710000086
representing the covariance between the different estimates.
(2) Interacting with the monitored Rj
Request communication with the measured Rj, AiiThe transmission and Rj, Rj calculate A according to the formulaji,Ajj. Let Aj＝[Aji,Ajj]According to the formula, calculating the correspondence
Figure BDA0001597382710000087
And sends it with Ri.
(3) Generating a cooperative positioning information element
Receiving Rj
Figure BDA0001597382710000088
Then according to the calculation
Figure BDA0001597382710000089
And expected measured values
Figure BDA00015973827100000810
Figure BDA00015973827100000811
Figure BDA00015973827100000812
Wherein
Figure BDA00015973827100000813
Representing superimposed weight values
As to calculate the variance of the expected measurements:
Figure 3
in the formula
Figure BDA0001597382710000092
The overlay weight value is represented. RijRepresenting the relative measurement noise variance.
The covariance between the expected estimate and the expected position estimate is measured as calculated:
Figure BDA0001597382710000093
|neighii represents the number of robots from the measuring robot to the middle of the robot i in the communication topology, and for the robot that finds the teammate: | neighbori|＝1。
(4) Ri updates self-position and local covariance
Using obtained a posteriori estimates
Figure BDA0001597382710000094
By performing an update of the local covariance,
Figure BDA0001597382710000095
Figure BDA0001597382710000096
wherein
Figure BDA0001597382710000097
A posteriori estimate of the position is represented,
Figure BDA0001597382710000098
to representThe corresponding error covariance.
(5) The Ri number i, the expected measurement error, the corresponding variance and the like form the cooperative positioning information Si(km)：
Figure BDA0001597382710000099
Will S1(k) The package is sent to the neighboring robot.
The cooperative positioning information forming flow shown in fig. 3 is compiled into an independent program, the program is implanted in each robot, and once the robot measures teammates, the program is executed.
Fig. 4 illustrates a process of updating the predicted position using teammate location information when the robot receives the information. In the figure CrC represents a robot number set included in the positioning information,
Figure BDA00015973827100000910
next, S from Rj is received as Rij(km) This flow will be described in detail as an example.
(1) Forming local sigma points
Calculating A according to formulaiThen according to the calculation
Figure BDA00015973827100000911
(2) Updating a predicted estimated position
Using formula to calculate
Figure BDA00015973827100000912
Updating the position by utilizing the formula;
(3) updating local covariance
Statistics Sj(k) The robot number set C included in (1) is carried out for each robot j of g e C by using a formula
Figure BDA0001597382710000101
Updating of (1); to pair
Figure BDA0001597382710000102
Figure BDA0001597382710000105
Keeping the same;
(4) reforming positioning information
Packing receiver numbers i, Ai、
Figure BDA0001597382710000103
Sj(k) Information, reforming positioning information Si(k)：
Figure BDA0001597382710000104
Will Si(k) Continue to transmit to the neighboring robot.
The cooperative positioning information utilization flow shown in fig. 4 is compiled into an independent program, the independent program is implanted into each robot, and once the robot receives effective positioning information of teammates, the program is executed.
FIG. 5 illustrates a predicted position estimate update process for each robot under different communication topology conditions for the present invention. Take four robots as an example (C)r＝{1,2,3,4}), and assume that at some point robot R1 finds R2. The corresponding communication topology of fig. 5(a) is: 1 → 2 → 3 → 4, and the communication topology corresponding to fig. 5(b) is: 1 → 2 → 4 → 3. This means that relative measurements made by one robot can be updated for the location of all robots by different communication topologies. In both 5(a), 5(b), there is an interactive communication between the robots 1,2, in order to form the positioning information S1(k) Therefore, it should be ensured that the measuring party and the measured party can communicate with each other first, otherwise, the relative measurement information z is obtained12Will be worthless and discarded.

Claims (4)

1. A multi-robot distributed cooperative positioning method suitable for a time-varying communication topology is characterized by comprising the following steps: the unscented Kalman filter UKF is adopted as the technical basis of distributed cooperative positioning, and a group of s is utilizedThe igma points are used for approximately representing the probability density distribution of the variables, and for the variables which are subjected to normal distribution, the estimated values of the variables and the variance of the estimated values are equivalently described by using a plurality of sampling points, and the process is called U transformation; here, it is assumed that at time k, the overall position of N robots is estimated as:
Figure FDA0002814290070000011
Figure FDA0002814290070000012
indicating robot RiThe variance corresponding to the overall position estimation is:
Figure FDA0002814290070000013
Figure FDA0002814290070000014
then the estimated U transforms to:
Figure FDA0002814290070000015
xi in the formula(0)(k)Lξ(4N)(k) The sigma point is the sigma point, and lambda is an adjustable quantity; p (k) is a symmetric matrix,
Figure FDA0002814290070000016
Figure FDA0002814290070000017
to represent
Figure FDA0002814290070000018
P-th column of (a), T represents a matrix transposition;
ensuring that a single robot obtains all block elements Pij(k) To construct P (k) to obtain
Figure FDA0002814290070000019
So that the single robot RiObtaining a local sigma point:
Figure FDA00028142900700000110
solving the distributed acquisition problem of covariance matrix based on looking up Cholesky decomposition, let: p ═ AATEach block of element A in AijThe solution can be found from equation (2) (3):
Figure FDA00028142900700000111
Figure FDA00028142900700000112
in the above formula: 1) each block element AijIs dependent only on Aij(i > j) and Pij(i > j), rather than all the element blocks, avoids the need for information exchange by all robots; 2) recursion from top to bottom and combining the characteristics with the communication sequence;
decomposing Block element A with a look-Up Cholesky matrixijThe recursive characteristic from top to bottom is that the distributed covariance matrix decomposition is combined with the communication conditions, so that the matrix decomposition is changed from 'demand improvement' for communication to automatic compliance, and the problem of high dependence of distributed cooperative positioning on communication topological conditions is avoided; each robot can complete the position updating of the robot by means of the obtained local information quantity; the method comprises two steps of position prediction and position posterior estimation, wherein output results of the two steps are mutually input;
when no robot in the robot group measures other teammates, each robot continuously predicts the position of the robot according to a certain step length;
when one robot measures teammates and measures relative information of the teammates, the position prediction values of all the robots are updated by the measurement, and after the posterior estimation is completed, the position prediction is continued;
the work flow of a single robot in the cooperative positioning is carried out in four steps; aiming at a plurality of robots provided with linear velocity measuring equipment, relative distance measuring equipment and wireless communication equipment, the robot numbered i is abbreviated as Ri(ii) a The specific implementation steps are as follows:
the method comprises the following steps: starting a cooperative positioning task;
after the task starts, the robot continuously predicts the position of the robot according to the delta step length; meanwhile, the robot monitors teammates in real time, and whether the information related to the positions and the field angles and sent by the monitoring friends can be monitored or not can be monitored;
at a certain moment, if the robot finds a teammate, executing a second step, and if the robot receives the positioning information, executing a third step; otherwise, continuing to estimate the position of the team and monitoring team friends and external information, and stopping monitoring the team friends if a cooperative positioning ending instruction is received;
step two: finding teammates and generating cooperative positioning information;
suppose at kmTime of day, RiFinding teammates, processing only one nearest to the discoverer if a plurality of teammates are found at the same time according to the relative measurement value between the teammates and the discoveree, and calculating and generating positioning information Si(km) Positioning information Si(km) Then re-entering the step one;
step three: updating the position of the team friend according to the received information of the team friend;
suppose at krTime of day, RjReceive neighboring robot RiInformation, RjFirstly, judging whether the information is cooperative positioning information or not, and if so, utilizing the cooperative positioning information to perform cooperative positioning;
before using the cooperative positioning information, preprocessing is performed: first, whether S has been received is judgedi(km) If the positioning information is received, abandoning, otherwise, continuously using the formula (4) to judge whether the positioning information is valid:
km+δ＞kr (4)
wherein k isrIndicating a reception time;
if a formula(4) If not, abandoning and not transmitting to the adjacent robot, otherwise, RjUsing positioning information Si(km) Updating the position, and then re-entering the step one;
step four: the cooperative positioning task is finished;
after the robot receives the cooperative positioning termination command, the robot does not monitor teammates any more.
2. The multi-robot distributed cooperative positioning method adapted to the time-varying communication topology as claimed in claim 1, wherein: the formation process of the cooperative positioning information is that the robot R takes the k timeiR is observedjFor a detailed description:
(1) forming local sigma points
According to
Figure FDA0002814290070000041
Calculation of AiiLet Ai＝[Aii]Calculating robot R according to equation (5)iCorrespond to
Figure FDA0002814290070000042
Figure FDA0002814290070000043
Wherein
Figure FDA0002814290070000044
Representing the position prediction estimate, the variance of the prediction estimate
Figure FDA0002814290070000045
Representing the covariance between different prediction estimates;
(2) with the monitored RjInteraction
Requesting and measured robot RjCommunication, will AiiSending and Rj；RjFirst according to the publicFormula (2)
(3) Calculation of Aji,Ajj(ii) a Let Aj＝[Aji,Ajj]Then, the correspondence is calculated according to the formula (5)
Figure FDA0002814290070000046
And send it with Ri；
(3) Generating a cooperative positioning information element
Receive RjIs/are as follows
Figure FDA0002814290070000047
Then, the calculation is performed according to the formulas (6) and (7)
Figure FDA0002814290070000048
And expected measured values
Figure FDA0002814290070000049
Figure FDA00028142900700000410
Figure FDA00028142900700000411
The variance of the expected measurements is calculated according to equation (8):
Figure FDA00028142900700000412
in the formula
Figure FDA00028142900700000413
Indicating a superimposed weight value, RijRepresenting the relative measurement noise variance;
the covariance between the measured expected estimate and the expected position estimate is calculated according to equation (9):
Figure FDA00028142900700000414
|neighii denotes in the communication topology from the measuring robot to the robot RiThe number of robots in the middle, for robots that find teammates: | neighbori|＝1；
(4) Robot RiUpdating self-position and local covariance
Obtaining a posteriori estimate using equation (10)
Figure FDA0002814290070000051
The local covariance is updated using equation (11),
Figure FDA0002814290070000052
Figure FDA0002814290070000053
wherein
Figure FDA0002814290070000054
A posteriori estimate of the position is represented,
Figure FDA0002814290070000055
error covariance representing a posteriori estimation;
(5) from RiThe number i, the expected measurement error and the corresponding variance form the cooperative positioning information Si(km)：
Figure FDA0002814290070000056
Will Si(k) Packaging and sending to a neighboring robot;
the cooperative positioning information forming process is compiled into an independent program and is implanted into each robot, and once the robot measures teammates, the program is executed.
3. The multi-robot distributed cooperative positioning method adapted to the time-varying communication topology as claimed in claim 2, wherein: the cooperative positioning information utilization process comprises the following specific implementation steps:
after the robot receives the teammate positioning information, the process of updating the predicted position by using the information; crC represents a robot number set included in the positioning information,
Figure FDA0002814290070000057
with RiReceive a message from RjPositioning information Sj(km)；
(1) Forming local sigma points
Calculating A according to the formulas (2) and (3)iThen calculated according to the formula (5)
Figure FDA0002814290070000058
(2) Updating a predicted estimated position
Calculated by equation (9)
Figure FDA0002814290070000061
Then, updating the position by using a formula (10);
(3) updating local covariance
Statistics Sj(k) The number set C of the robot contained in (1) is used for each robot R of g epsilon CjBy using the formula (11)
Figure FDA0002814290070000062
Updating of (1); to pair
Figure FDA0002814290070000063
Figure FDA0002814290070000064
Keeping the same;
(4) reforming positioning information
Packing receiver numbers j, Ai、
Figure FDA0002814290070000065
Sj(k) Information, reforming positioning information Sj(k)：
Figure FDA0002814290070000066
Will Sj(k) Continuing to send to the neighboring robot;
the cooperative positioning information utilization process is compiled into an independent program which is implanted in each robot, and once the robot receives effective positioning information of teammates, the program is executed.
4. The multi-robot distributed cooperative positioning method adapted to the time-varying communication topology as claimed in claim 1, wherein: under different communication topological conditions, each robot predicts a position estimation updating process; with four robots (C)r1,2,3, 4) and assume that at some point in time robot R is present1Discovery of R2；
(a) The corresponding communication topology is: 1 → 2 → 3 → 4;
(b) the corresponding communication topology is: 1 → 2 → 4 → 3;
represents a robot (R)1) The generated relative measurement can enable all the robots to realize self position updating through different communication topological structures; in (a), (b), there is an interactive communication between both robots 1,2, in order to form positioning information S1(k) Therefore, it should be ensured that the measuring party and the measured party are in communication first, otherwise, the relative measurement information z is obtained12Will be worthless and discarded.
CN201810211742.7A 2018-03-15 2018-03-15 Multi-robot distributed cooperative positioning method suitable for time-varying communication topology Active CN108594169B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810211742.7A CN108594169B (en) 2018-03-15 2018-03-15 Multi-robot distributed cooperative positioning method suitable for time-varying communication topology

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810211742.7A CN108594169B (en) 2018-03-15 2018-03-15 Multi-robot distributed cooperative positioning method suitable for time-varying communication topology

Publications (2)

Publication Number Publication Date
CN108594169A CN108594169A (en) 2018-09-28
CN108594169B true CN108594169B (en) 2021-02-09

Family

ID=63626341

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810211742.7A Active CN108594169B (en) 2018-03-15 2018-03-15 Multi-robot distributed cooperative positioning method suitable for time-varying communication topology

Country Status (1)

Country Link
CN (1) CN108594169B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110587606B (en) * 2019-09-18 2020-11-20 中国人民解放军国防科技大学 Open scene-oriented multi-robot autonomous collaborative search and rescue method
CN112445244B (en) * 2020-11-09 2022-03-04 中国科学院沈阳自动化研究所 Target searching method for multiple autonomous underwater robots

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN100461058C (en) * 2006-03-07 2009-02-11 北京大学 Automatic positioning method for intelligent robot under complex environment
CN103135117B (en) * 2013-02-05 2014-12-03 中国人民解放军国防科学技术大学 Distributed multi-robot synergetic location algorithm
CN104330083A (en) * 2014-10-27 2015-02-04 南京理工大学 Multi-robot cooperative positioning algorithm based on square root unscented kalman filter
CN106482736B (en) * 2016-07-11 2019-04-09 安徽工程大学 A kind of multirobot co-located algorithm based on square root volume Kalman filtering

Also Published As

Publication number Publication date
CN108594169A (en) 2018-09-28

Similar Documents

Publication Publication Date Title
CN108896047B (en) Distributed sensor network collaborative fusion and sensor position correction method
CN110989352B (en) Group robot collaborative search method based on Monte Carlo tree search algorithm
Nerurkar et al. Asynchronous multi-centralized cooperative localization
CN103648108B (en) Sensor network distributed consistency object state estimation method
CN111522341A (en) Multi-time-varying formation tracking control method and system for network heterogeneous robot system
Wanasinghe et al. Decentralized cooperative localization for heterogeneous multi-robot system using split covariance intersection filter
Wanasinghe et al. Distributed leader-assistive localization method for a heterogeneous multirobotic system
Bakkali et al. Kalman filter-based localization for Internet of Things LoRaWAN™ end points
Petitti et al. Consensus-based distributed estimation for target tracking in heterogeneous sensor networks
Leung et al. Distributed and decentralized cooperative simultaneous localization and mapping for dynamic and sparse robot networks
CN109782269B (en) Distributed multi-platform cooperative active target tracking method
CN107592671B (en) Networked multi-agent active variable topology autonomous cooperative positioning method
CN110958639A (en) Target state estimation method and system
CN108594169B (en) Multi-robot distributed cooperative positioning method suitable for time-varying communication topology
Walls et al. Experimental comparison of synchronous-clock cooperative acoustic navigation algorithms
WO2020221050A1 (en) Centralized cooperative localization system and method based on time-space domain joint processing
CN113411213A (en) Ad hoc network topology control method and cooperative monitoring method based on Internet of things
Liu et al. Measurement dissemination-based distributed Bayesian filter using the latest-in-and-full-out exchange protocol for networked unmanned vehicles
Alonso-Román et al. Adaptive consensus-based distributed Kalman filter for WSNs with random link failures
Reinhardt et al. Generalized covariance intersection based on noise decomposition
CN110807478B (en) Cooperative target tracking method under condition of observing intermittent loss
CN111683377B (en) Real-time reliable relay deployment method for power distribution network
Xie et al. Quantitative uncertainty-based incremental localization and anchor selection in wireless sensor networks
Indelman et al. Graph-based distributed cooperative navigation
Reinhardt et al. Distributed Kalman filtering in the presence of packet delays and losses

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant