Multi-robot formation algorithm based on distributed asynchronous optimization for ROS platform

Abstract

The present invention discloses a multi-robot formation algorithm based on distributed asynchronous optimization for ROS platform. Each robot first determines the formation according to the target to be surrounded and establishes a distributed optimization model. Then, by comparing whether the current update progress of the robot is slower than that of other nodes, it is judged whether the event trigger is satisfied. If the condition is satisfied, the robot summarizes the information of the neighbors and updates its own parameters. If the condition is not satisfied, the robot does not update and communicate. In this way, nodes with fast update rates will not summarize the information of neighbor nodes for multiple times, thereby causing the constraint conditions to be unsatisfied. Under the premise of asynchronous and efficient communication based on ROS, the correct convergence of the algorithm is guaranteed, and the blocking behavior of the synchronous algorithm is avoided while maintaining the formation, so that the algorithm converges faster and the real-time performance of the robot system is higher.

Description

Technical Field

[0001] The present invention belongs to the field of automation and computer technology, and in particular relates to a multi-robot formation algorithm based on distributed asynchronous optimization for a ROS platform. Background Art

[0002] Multi-robot collaboration / formation has very important practical significance, for example: in the military / life, they can cooperate to encircle an object, and follow the target object and maintain formation; in addition, for aerial photography drones, they can take real-time photos of the target object from multiple angles. Therefore, it is of great significance to design a dynamic formation algorithm that can change according to the target to be encircled and its own needs.

[0003] Solving the optimization problem itself is not difficult, but traditional distributed optimization algorithms for solving constrained optimization problems are all based on synchronous communication, which is incompatible with the current mainstream ROS platform based on asynchronous communication robot system. Not only that, asynchronous communication also has many advantages. Compared with synchronous communication, asynchronous communication does not require blocking, is more efficient, is less prone to deadlock, and is more robust.

[0004] The communication method based on publishing topics / subscribing topics in the ROS platform is a natural asynchronous communication method. The agent publishes its own information through publishing topics. Once the published message is updated, it will overwrite the previous information. The agent receives information from neighbors through subscribing topics. The pace depends entirely on the agent itself. The received information may be ahead of time or delayed. It is not real-time and is a natural asynchronous environment. However, if traditional distributed optimization algorithms are deployed in this asynchronous environment, the information delay and the imbalance of updates will destroy the designed constraints. The error between the final convergence result and the theoretical optimal solution can reach 50% through experiments. Therefore, the existing optimization algorithms are not suitable for ROS-based platforms. Summary of the invention

[0005] The present invention aims at the problem that traditional algorithms in the prior art are not applicable to the ROS platform, and provides a multi-robot formation algorithm based on distributed asynchronous optimization for the ROS platform. Each robot first determines the formation according to the target to be surrounded and establishes a distributed optimization model; then, by comparing whether the current update progress of the robot is slower than that of other nodes, it is determined whether the event trigger is satisfied. If the condition is satisfied, the robot summarizes the information of its neighbors and updates its own parameters; if the condition is not satisfied, the robot does not update or communicate; in this way, nodes with fast update rates will not summarize neighbor node information multiple times, thereby causing constraint conditions to be unsatisfied. Under the premise of asynchronous and efficient communication based on ROS, the algorithm converges correctly, and the blocking behavior of the synchronous algorithm is avoided while maintaining the formation, so that the algorithm converges faster and the robot system has higher real-time performance.

[0006] In order to achieve the above object, the technical solution adopted by the present invention is: a multi-robot formation algorithm based on distributed asynchronous optimization for the ROS platform, applied to a system of i=1, 2, 3, ..., n robots, comprising the following steps:

[0007] S1: Determine the formation according to the position of the target to be surrounded and establish a distributed optimization model. The objective function of the distributed optimization model is to optimize the robot position x i To the best observation position i The distance is the smallest, that is:

[0008] minf i (x i )=||x i -s i || 2

[0009] Among them, x i ∈R 2 is the position coordinate of robot i; s i ∈R 2 is the best observation position of robot i;

[0010] The constraints of the distributed optimization model include at least: robot i needs to be in its safety area Ω i And the observation area S i The center of all robots' positions coincides with the target to be surrounded, that is:

[0011] s.tx i ∈Ω i

[0012] x i ∈S i

[0013]

[0014] Where d∈R 2 Indicates the location of the target to be contained;

[0015] S2: By comparing whether the current update progress of the robot is slower than that of other nodes, it is determined whether the event trigger is satisfied. If the number of updates of robot i is greater than that of other nodes, robot i needs to stop and wait for other agents. When the event trigger condition is satisfied, robot i performs a local update and proceeds to step S3. The event trigger condition of robot i is:

[0016]

[0017] Among them, k i is the number of local updates of robot i; k jis the number of local updates of robot j; τ is the event trigger condition parameter; N i is the neighbor set of robot i;

[0018] S3: Perform local updates based on the update conditions of robot i. The specific formula is:

[0019]

[0020]

[0021] k i =k i +1

[0022] Among them, η is the step size, a ij is the neighbor weight, is the gradient, v i is the Lagrange constant; I2 is the 2*2 identity matrix, is the Kronecker product, A=[a ij ] m*m is the adjacency matrix of the robot, when i≠j, a ij >0 if and only if robot i sends information to j, i = j,

[0023] S4: Determine whether the termination condition is met. If so, terminate the algorithm and output the final position x i If not satisfied, return to step S3 and repeat the steps, and the termination condition is set to the maximum number of iterations or the error accuracy.

[0024] Compared with the existing technology, this case makes up for the shortcomings of asynchronous communication based on ROS topics, adds event trigger conditions to the synchronous algorithm and designs the algorithm as an asynchronous algorithm. The error between the final convergence result and the theoretical optimal solution is reduced to 1%, allowing multiple robots to break away from centralized control and adopt true distributed control. Each robot runs its own independent program and converges to the optimal solution in an asynchronous communication environment. Under the premise of asynchronous and efficient communication based on ROS, this case ensures the correct convergence of the algorithm, avoids the blocking behavior of the synchronous algorithm while maintaining the formation, makes the algorithm converge faster, and makes the robot system more real-time. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 is a flowchart of the steps of the algorithm of the present invention;

[0026] Figure 2 This is the final effect diagram of the drone based on the ROS platform encircling the ground car in the test example of the present invention;

[0027] Figure 3It is a graph showing the robot position coordinates (i.e., equality constraints) obtained by different algorithms versus the number of iterations in the test example of the present invention. DETAILED DESCRIPTION

[0028] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0029] Example 1

[0030] A multi-robot formation algorithm based on distributed asynchronous optimization for ROS platform has the characteristics of each robot independently running its own program and asynchronous communication. For a system with n robots, the specific implementation steps for the i=1, 2, 3, ..., n robots are as follows: Figure 1 As shown:

[0031] S1: Determine the formation and establish a model based on the target to be encircled, establish the local loss function, constraint conditions, and virtual initial position. The specific steps are as follows:

[0032] The objective function and constraints of the i-th robot are shown as

[0033] minf i (x i )=||x i -s i || 2 (1)

[0034] s.tx i ∈Ω i (2)

[0035] x i ∈S i (3)

[0036]

[0037] where x i ∈R 2 is the position coordinate of robot i, which is also the parameter to be optimized, s i ∈R 2 is the best observed position of robot i; the objective function (1) is to optimize the robot position x i To the best observation position i The distance is the smallest.

[0038] Constraint (2) means that robot i needs to be in its safe area where z i ∈R 2The center of the safety range of robot i, and r2 is the observation area S i The distance from the center to any boundary, that is, half of the side length of the square area. Constraint (3) means that robot i needs to be within its observation area S i , x i ∈S i ={x i ∈R 2 ||x i -s i ∣<r2}, where r2 is the distance from the center of the observation area S to any boundary, that is, half of the side length of the square area. To meet the constraints, robot i needs to be in the intersection of two square areas X i , S i , S i The intersection of, and by solving the inequality, we can get Among them, Constraints (2) and (3) can be transformed into the linear inequality constraints in (6) and (7). Constraint (4) means that the center of all robot positions coincides with the target to be blocked, and d ∈ R 2 represents the position of the target to be blocked. The final optimization problem can be transformed into the following constrained optimization problem:

[0039] min f i (x i )=||x i -s i || 2 (5)

[0040]

[0041] S2: Determine whether the event trigger is satisfied by comparing whether the current update progress of the robot is slower than that of other nodes. The event trigger condition for robot i is:

[0042]

[0043] k i is the local update count of robot i. When the event trigger condition is satisfied, robot i performs a local update. If τ = 0, the event trigger of robot i means that robot i is the robot with the least update count among the current neighbors. The idea of this event trigger is: if the update count of robot i is already more than that of other robots, it means that robot i has aggregated the information of other robots multiple times. This multiple aggregation behavior will cause the position average value of the robot system to shift towards other robots, which will lead to the non - satisfaction of the constraints. Therefore, if the update count of robot i is more than that of other agents, robot i needs to stop and wait for other agents.

[0044] S3: Perform local updates based on the update conditions of robot i

[0045] Initialize robot i parameter x i =z i ,k i =0,v i =(0,0,0,0),M i =(1,-1), where When the event trigger condition is met, update according to the following formula:

[0046]

[0047] k i =k i +1 (12) Where I2 is the 2*2 identity matrix, is the Kronecker product, A=[a ij ] m*m is the adjacency matrix of the robot, when i≠j, a ij >0 if and only if robot i sends information to j, i = j, v i is the Lagrange multiplier. When the event trigger condition is triggered, robot i calculates the objective function gradient and a feasible gradient direction, and moves in the positive direction of the feasible gradient direction, then summarizes the neighbor's information, moves in the negative direction of the neighbor's feasible gradient, and finally updates its own Lagrange multiplier. The neighbor's information is stored in the buffer (if there is new information, the old information will be overwritten).

[0048] S4: Determine whether the termination condition is met. The termination condition includes the maximum number of iterations and the error accuracy. If one of them is met, the algorithm is terminated and the final position x is output. i If not satisfied, return to step 3

[0049] Implementation test case:

[0050] Taking the case of multiple robots surrounding a ground vehicle as an example, a multi-robot formation algorithm based on distributed asynchronous optimization for the ROS platform includes the following steps:

[0051] Step 1: Initialize parameters and build the robot model. In the experiment, the target coordinates are d = (15, -15), and three robots (n = 3) are used to encircle the target into a triangle. Initialize the robot's virtual initial position: z1 = (-65, -30), z2 = (10, 45), z3 = (100, -60), The best observation position of the robot is s1=(-20,-30), s2=(10,5), s3=(55,-50), r1=30, r2=20.

[0052] The adjacency matrix of the robot system A = [a ij ] 3×3

[0053]

[0054] Initialize other model parameters k1=k2=k3=,η=0.05,τ=1,v1=v2=v3=(0,0,0,0).

[0055] Through the above conditions and formula Calculated

[0056] For the i-th robot: the objective function is f i (x i )=||x i -s i || 2 , the constraints are

[0057] Step 2: Determine whether the event trigger is satisfied by comparing whether the robot's current update progress is slower than other nodes, as shown in the following formula:

[0058]

[0059] Among them, k i ,i=1,2,3 is the update times of robot i, which is also the times when the event trigger condition of robot i is triggered. If the update times of robot τ are more than other robots, it means that robot i has aggregated the information of other robots many times. This multiple aggregation behavior will cause the average position of the robot system to shift toward other robots, which will lead to the constraint condition not being satisfied. Therefore, if the update times of robot i are more than other agents, robot i needs to stop and wait for other agents.

[0060] τ is the event trigger condition parameter. The larger τ is, the greater the tolerance of the event trigger condition to delay. The larger τ is, the easier it is for robot i to trigger events, the more frequently the robot is updated, and the faster it converges. However, the error of the convergence result will be greater due to the greater tolerance to delay.

[0061] Therefore, if you pursue convergence speed, you can increase τ, and if you pursue convergence accuracy, you can reduce τ, τ∈N. Users can adjust τ to balance accuracy / speed.

[0062] Step 3: For the i-th robot, if the event triggering condition is not met, return to step 2. If the event triggering condition is met, update the agent. The specific update steps are as follows:

[0063] a. For the robot position

[0064] η is the step length, a ij is the neighbor weight, is the gradient, v i is the Lagrange constant.

[0065] When the event trigger condition is triggered, robot i calculates the objective function gradient and a feasible gradient direction, and moves in the negative direction of the feasible gradient direction. It then aggregates the information of its neighbors and moves in the positive direction of the feasible gradient of its neighbors, thereby keeping the center of the robot system the same as the target to be encircled.

[0066] b. For the robot Lagrange multiplier

[0067] c. Finally, update the number of times k i ,k i =k i +1.

[0068] After the local update is completed, the number of updates k i , local direction Send to neighbors.

[0069] Step 4: Determine whether the termination condition is met. The termination condition includes the maximum number of iterations and the error precision. If one of them is met, the algorithm is terminated and the final position x is output. i , if not satisfied, go back to step 3.

[0070] like Figure 2 As shown, Figure 2 This is the final effect diagram of the drone encircling the ground car based on the ROS platform. The star shape is the initial position of the drone, the solid circle shape is the final position of the drone, that is, the final output of the algorithm, the solid line hollow circle area is the safe position of the drone, the dotted line hollow circle center area is the observation area of ​​the drone, the "×" shape is the target to be encircled, s1, s2, s3 are the observation centers, Figure 2 The model and the entire optimization process are presented graphically.

[0071] In this test case, the synchronous algorithm syn, the asynchronous algorithm asy, and the algorithm asy-dc with an event trigger mechanism added to this case were tested respectively. The final robot position coordinates (i.e., the equality constraint condition) versus the number of iterations are shown in the following figure. Figure 3As shown, it can be seen that the synchronization algorithm can be well maintained at xy1+xy2+xy3=3×dy=-45. Ideally, the position coordinates should be the same as the synchronization algorithm, that is, -45; the final position sum of the algorithm based on ROS asynchronous communication without event triggering mechanism is xy1+xy2+xy3=-71.2, with an error of up to 60%, while the position sum of the asynchronous algorithm with event triggering compensation mechanism proposed in the present invention is xy1+xy2+xy3=-46.1, and the error is reduced to 2%. Therefore, under the premise of asynchronous and efficient communication based on ROS, the algorithm in this case ensures correct algorithm convergence, avoids the blocking behavior of the synchronization algorithm while maintaining the formation, makes the algorithm converge faster, and makes the robot system more real-time.

[0072] It should be noted that the above content only illustrates the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications all fall within the protection scope of the claims of the present invention.

Claims

1. A multi-robot formation algorithm based on distributed asynchronous optimization for the ROS platform, applied to a system with i=1, 2, 3, ..., n robots, characterized in that: The steps include: S1: Determine the formation according to the position of the target to be surrounded and establish a distributed optimization model. The objective function of the distributed optimization model is to optimize the robot position x i To the best observation position i The distance is the smallest, that is: minf i (x i )=||x i -s i || 2 Among them, x i ∈R 2 is the position coordinate of robot i; s i ∈R 2 is the best observation position of robot i; The constraints of the distributed optimization model include at least: robot i needs to be in its safety area Ω i And the observation area S i The center of all robots' positions coincides with the target to be surrounded, that is: s.t x i ∈Ω i x i ∈S i Where d∈R 2 Indicates the location of the target to be contained; S2: By comparing whether the current update progress of the robot is slower than that of other nodes, it is determined whether the event trigger is satisfied. If the number of updates of robot i is greater than that of other nodes, robot i needs to stop and wait for other agents. When the event trigger condition is satisfied, robot i performs a local update and proceeds to step S3. The event trigger condition of robot i is: Among them, k i is the number of local updates of robot i; k j is the number of local updates of robot j; τ is the event trigger condition parameter; N i is the neighbor set of robot i; S3: Perform local updates based on the update conditions of robot i. The specific formula is: k i =k i +1 Among them, η is the step size, a ij is the neighbor weight, is the gradient, v i is the Lagrange constant; I2 is the 2*2 identity matrix, is the Kronecker product, A=[a ij ] m*m is the adjacency matrix of the robot, when i≠j, a ij >0 if and only if robot i sends information to j, i = j, S4: Determine whether the termination condition is met. If so, terminate the algorithm and output the final position xi; if not, return to step S3 and repeat the steps. The termination condition is set to the maximum number of iterations or the error accuracy.

2. The multi-robot formation algorithm based on distributed asynchronous optimization for ROS platform as claimed in claim 1, characterized in that: In step S1, robot i needs to be in its safety area Ω i And the observation area S i Inside, x i ∈Ω i ={x i ∈R 2 ||x i -z i |≤r1}, x i ∈S i ={x i ∈R 2 ||x i -s i |<r2} Among them, z i ∈R 2 is the center of the safety range of robot i; r1 is the safety area Ω i The distance from the center to any boundary is half the length of the side of the square area; r2 is the observation area S i The distance from the center to any boundary is half the length of the side of the square region.

3. The multi-robot formation algorithm based on distributed asynchronous optimization for ROS platform as claimed in claim 2, characterized in that: In step S1, robot i needs to be in two square areas Ω i ,S i The intersection of , that is: in, 4. The multi-robot formation algorithm based on distributed asynchronous optimization for ROS platform as claimed in claim 2 or 3, characterized in that: In step S2, the speed or accuracy of model convergence can be adjusted by changing the size of τ. If the convergence speed is pursued, τ can be increased; if the convergence accuracy is pursued, τ can be reduced, τ∈n.

5. The multi-robot formation algorithm based on distributed asynchronous optimization for ROS platform as claimed in claim 4, characterized in that: In step S3, when the event triggering condition is met, robot i calculates the objective function gradient and a feasible gradient direction, and moves in the negative direction of the feasible gradient direction. Then, it summarizes the information of the neighbors and moves in the positive direction of the feasible gradient of the neighbors, so as to keep the center of the robot system the same as the target to be encircled.

Images