A Robot Formation Control Method Based on the Formation Integrity Evaluation Index

Abstract

The present invention provides a robot formation control method based on a formation integrity evaluation index, which relates to the field of robot formation motion control. This method calculates the formation integrity evaluation index by computing the error of the average center distance of the formation of actual robots and the ideal formation, the error of the sum of the distances from the robots to the average center, and the angle error of the robots, and according to the weights of each error in the actual formation application scenario. On the premise of retaining the position constraint of the virtual structure method, speed constraints are integrated, and the speed constraint with a small amount of computation is preferentially used as the formation motion control method. By calculating the formation integrity evaluation index of the robot formation in real time, when the formation motion error reaches the threshold, the formation control method switches to the position constraint to eliminate the motion error, avoiding the use of the position constraint throughout the motion process. It can reduce the number of robot path planning times and the amount of computation during the formation motion while ensuring the stability of the formation shape, and better meet the application requirements in the actual environment.

Description

Technical Field

[0001] The present invention relates to the technical field of robot formation control, and particularly to a robot formation control method based on a formation integrity evaluation index. Background Art

[0002] The formation control of robots mainly adopts the leader-follower method, the behavior-based method, and the virtual structure method to ensure the integrity of the formation during movement. The robot system formed by the virtual structure method has a clear formation structure, which is more convenient to control the robot formation to execute specific tasks, and can better analyze the formation movement behavior during the task planning stage. Regarding multiple robots as a rigid whole to implement the virtual structure method, the robots perform the same actions, and macroscopically, it appears that a "whole robot" is moving. The inside of the "whole robot" maintains rigidity, that is, the relative position remains unchanged. The virtual structure method can be regarded as a leader-follower method in which the robot follows a corresponding virtual leader. Restricted by the characteristics of the virtual structure method itself, the formation needs to always maintain the rigid structure of the formation during operation, and the computational complexity is relatively large.

[0003] The robots formed by the virtual structure method use position constraints throughout the process. Each robot runs a complete target navigation algorithm for path planning, which ensures the stability of the formation. Therefore, a large amount of position constraint information is required. As the number of robots in the formation increases, the constraint information that the robots need to consider increases accordingly, resulting in an increase in the amount of computation, making it difficult to meet the real-time requirements, and there are certain limitations in practical applications. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a robot formation control method based on a formation integrity evaluation index in view of the deficiencies of the above-mentioned prior art. The formation integrity of the robot formation is comprehensively evaluated from factors such as the average error of the formation, the error of the sum of the distances from the formation robots to the average center, and the angle error. And according to this evaluation index, the speed constraint and the position constraint are switched, reducing the overall amount of computation on the premise of ensuring the stability of the formation structure.

[0005] To solve the above technical problem, the technical solution adopted by the present invention is:

[0006] A robot formation control method based on a formation integrity evaluation index includes the following steps:

[0007] Step 1: Calculate the actual average center coordinates according to the position coordinates of the robots in the actual formation, and at the same time calculate the average center coordinates under the ideal formation; calculate the Euclidean distance between the two average center coordinates to obtain the average center error;

[0008] Step 2: Calculate the Euclidean distance from the actual coordinates of each robot in the formation to the actual average center coordinate respectively, and sum them up; at the same time, calculate the Euclidean distance from the ideal coordinates of each robot in the ideal formation to the ideal average center coordinate, and sum them up; take the difference between the two distance sums to obtain the distance sum error.

[0009] Step 3: Calculate the difference between the actual yaw axis angle and the ideal yaw axis angle of each robot respectively, and finally sum up each difference to obtain the angle error.

[0010] Step 4: Sum up the average center error, the distance sum error, and the angle error according to different weights to obtain the formation integrity evaluation index.

[0011] Step 5: Judge whether the robot uses position constraint or speed constraint for robot formation control according to the formation integrity evaluation index calculated in real time and a preset threshold, so as to realize the formation control method based on the formation integrity evaluation index.

[0012] Further, the average center error E in step 1 c is defined as the Euclidean distance between the two average centers, that is, the average center error.

[0013] In the map coordinate system, only considering the position coordinates of the robots, each robot forms an n-sided polygon, and the vertex coordinates are (x i , y i ), i = 1, 2... n. The coordinates of the average center C of this n-sided polygon are (x c , y c ), where

[0014] The actual positions and ideal positions of the robots in the formation respectively form two polygons, and the average centers of the two polygons are C and C'; the average center error E c is shown in formula (1):

[0015]

[0016] Among them, (x′ i , y′ i ) are the vertex coordinates of the n-sided polygon of the ideal formation shape.

[0017] Further, the sum L of the Euclidean distances from the coordinates of each robot to the average center coordinate in step 2 is shown in formula (2);

[0018]

[0019] The sum of the distances from the formation robots to the average center and the error is E LAs shown in Equation (3), where L and L’ are respectively the sums of the distances from the coordinates of each robot in the actual formation and the ideal formation to the average center coordinates C and C’;

[0020] E L = |L - L′| (3).

[0021] Furthermore, in Step 3, the heading of the robot is described by the yaw angle. The difference e between the actual yaw angle ∈[-π, π] of the robot and the ideal yaw angle is as shown in Equation (4); i The angular error

[0022]

[0023] is as shown in Equation (5);

[0024]

[0025] Furthermore, the formation integrity evaluation index P in Step 4 is as shown in Equation (6), where P is a non - negative number. The smaller P is, the closer the actual formation is to the ideal formation;

[0026]

[0027] where the weight Q = [q1, q2, q3] T ; q1, q2, and q3 are all non - negative numbers, corresponding to the sensitivity of the average center error E C , the error of the sum of the average center distances E L , and the angular error respectively.

[0028] Furthermore, the specific values of q1, q2, and q3 are scaled proportionally; the proportional relationship of q1, q2, and q3 is determined by the operating environment and the task requirements of the formation execution, and their specific values are set within a corresponding range for convenient later data processing.

[0029] Furthermore, for the task environment, the maximum values of E C , E L , are determined, then q1 ≤ P / E C_max , q2 ≤ P / E L_max , Then the proportional relationship of q1, q2, q3 is

[0030] Further, in step 5, during the movement after the formation of the formation, the speed constraint is preferentially used as the constraint condition for the formation. In this open-loop control system, due to the existence of the cumulative error of the robot, the formation cannot maintain a stable structure for a long time. The formation integrity evaluation index is introduced to quantify the looseness of the robot formation. According to the robot operating environment and the requirements of the formation task, a suitable threshold is set, and the position constraint condition is switched in a timely manner. Each robot moves to the corresponding position in the ideal formation through path planning, which can eliminate the cumulative error, and then switch back to the speed constraint condition.

[0031] Further, for the speed constraint condition, during the formation's movement, one of the robots acts as the leader for path planning to implement the complete target point navigation algorithm for the robot, while the other robots in the formation use the speed constraint to maintain the movement of the robot formation. The actual position of the robot subject to the speed constraint is only related to the initial pose of the robot and subsequent speed control instructions, and has nothing to do with the positions of other robots inside the formation. Without global map feedback, the formation maintenance at this time is open-loop control. Therefore, in the ideal situation without considering the robot movement error, the robot formation can still ensure the stability of the robot formation only through the speed constraint after the formation is formed.

[0032] Further, for the position constraint condition, the robots in the formation are regarded as followers in the leader-follower method, and each robot has a corresponding virtual leader. The movement of the virtual leader can be free from the influence of the robot's own hardware and actual friction factors, and all virtual leaders can achieve ideal movement. Therefore, the formation formed by the virtual leaders is absolutely stable. The actual robot, as a follower, only needs to set its own movement target pose as the real-time pose of the virtual leader and perform path planning. With the global map as the pose information feedback, under the position constraint condition, the formation maintenance is closed-loop control. By continuously performing path planning for each robot, it can ensure that the formation of the formation is always complete. In this way, the robot formation is a rigid whole for the outside world, with a stable internal structure, realizing the robot formation.

[0033] The beneficial effects of adopting the above technical solutions are as follows: The robot formation control method based on the formation integrity evaluation index provided by the present invention integrates the speed constraint on the premise of retaining the position constraint of the virtual structure method, and autonomously switches between the two constraint conditions through the formation integrity evaluation index. Finally, it can reduce the number of path planning times of the robot on the premise of ensuring the stability of the formation, reduce the computational amount during the formation movement, can additionally increase the functional tasks of the robot, and better meet the requirements in the actual environment. Description of the Drawings

[0034] Figure 1Flowchart of the robot formation control method based on the formation integrity evaluation index provided by the embodiments of the present invention;

[0035] Figure 2 Schematic diagram of the average center provided by the embodiments of the present invention. Detailed implementation manners

[0036] The following combines the accompanying drawings and embodiments to further describe in detail the specific implementation manners of the present invention. The following embodiments are used to illustrate the present invention, but are not used to limit the scope of the present invention.

[0037] As Figure 1 shown, the method of this embodiment is described as follows.

[0038] Step 1: Calculate the average center error. As Figure 2 shown, the dotted line is the ideal formation shape of the robots, and the corresponding average center is C'. The solid line is the actual formation shape of the robots, and the corresponding average center is C.

[0039] In the map coordinate system, only considering the position coordinates of the robots, each robot can form an n-sided polygon, and the vertex coordinates are (x i , y i ), where i = 1, 2... n. The coordinates of the average center C of this n-sided polygon are (x c , y c ), where

[0040] The average center error E c is defined as the Euclidean distance between the two average centers as shown in formula (1)

[0041]

[0042] where (x', i , y' i ) are the vertex coordinates of the n-sided polygon of the ideal formation shape, that is Figure 2 each vertex of the dotted pentagon.

[0043] Step 2: Calculate the error of the sum of the distances from the formation robots to the average center. According to the average center obtained in Step 1, calculate the sum of the distances L from each robot to the average center, as shown in formula (2).

[0044]

[0045] The error of the sum of the distances from the formation robots to the average center is E L , as shown in formula (3), where L and L' are the sums of the distances from each robot to the average centers C and C' in the actual formation and the ideal formation, respectively.

[0046] E L = |L - L'| (3)

[0047] Step 3: Calculate the angular error. During the arbitrary movement of the robot in the two-dimensional map, the yaw angle is used to describe the heading direction of the robot. The difference e between the actual yaw angle of the robot and the ideal yaw angle is as shown in formula (4). i That is

[0048]

[0049] For the formation integrity evaluation index, more emphasis is placed on the analysis of the actual deviation of the robot. The direction of the robot's angular error has no influence on the evaluation index. Therefore, the angular error is as shown in formula (5).

[0050]

[0051] Step 4: Calculate the formation integrity evaluation index according to the average center error, distance sum error, angular error, and their corresponding weights.

[0052] During the formation movement of the robot, the actual influence weights of the position deviation and the angular deviation on the formation integrity are different. For different application scenarios, there are different sensitivities to the position deviation and the angular deviation. Introduce the weight Q = [q1, q2, q3] T , where q1, q2, and q3 are all non-negative numbers, corresponding to the average center error E C , the error of the sum of the average center distances E L , and the angular error respectively.

[0053] The formation integrity evaluation index P is as shown in formula (6), where P is a non-negative number. The smaller P is, the closer the actual formation is to the ideal formation.

[0054]

[0055] The proportional relationship between q1, q2, and q3 affects the importance of the three parameters of the formation integrity index. Therefore, the specific values of q1, q2, and q3 can be scaled proportionally. The proportional relationship of q1, q2, and q3 is determined by the operating environment and the task requirements of the formation execution, and their specific values can be set within a corresponding range for convenient later data processing.

[0056] For the task environment, the maximum values of E C , E L , are determined, then q1 ≤ P / E C_max, q2 ≤ P / E L_max , q3 ≤ P / Then the proportional relationship can be deduced as

[0057] Step 5: According to the formation integrity evaluation index calculated in real time and a preset threshold, judge whether the machine uses position constraint or speed constraint for robot formation control, and implement the formation control method based on the formation integrity evaluation index.

[0058] During the formation operation using speed constraint, the movement speeds of the robots within the formation are kept consistent. Without considering errors, the relative speed between the robots within the formation is 0. Therefore, it can be considered that the robots maintain a stable formation.

[0059] During the formation movement, one of the robots acts as the leader for path planning to implement the complete target point navigation algorithm for the robots. The other robots in the formation use speed constraint to maintain the movement of the robot formation. The actual position of the robot subject to speed constraint is only related to the initial pose of the robot and subsequent speed control instructions, and has nothing to do with the positions of other robots within the formation. Without global map feedback, the formation maintenance at this time is an open-loop control. Therefore, in the ideal case without considering the robot movement error, the robot formation can still ensure the stability of the robot formation only through speed constraint after the formation is formed.

[0060] However, each robot in the actual robot formation will have different actual movement situations from the ideal state under the same movement instruction control due to individual differences, different movement routes, and the influence of the actual movement environment, resulting in cumulative errors. Eventually, under the influence of the cumulative errors, only the robot that fully operates the target point navigation algorithm in the formation can accurately reach the ideal target point, while the other robots within the formation subject to speed constraint will have position deviations during the movement, resulting in the loosening of the robot formation and being unable to reach the ideal target point, ultimately breaking the formation structure of the formation and not being able to maintain the formation stability solely by speed constraint.

[0061] With position constraints, each robot in the formation maintains the stability of the formation by keeping the relative position relationship within the formation stable. The robots in the formation can be regarded as followers in the leader-follower method, and each robot has a corresponding virtual leader. The movement of the virtual leader can be free from the influence of factors such as the robot's own hardware and actual friction. All virtual leaders can achieve ideal movement. Therefore, the formation formed by virtual leaders is absolutely stable. As followers, the actual robots only need to set their own target poses as the real-time poses of the virtual leaders, perform path planning, and have the global map as the feedback of pose information. Under the position constraint conditions, the formation maintenance is a closed-loop control. Through the continuous path planning of each robot, it can ensure that the formation of the robots is always complete. In this way, the robot formation is a rigid whole for the outside world, with a stable internal structure, realizing the robot formation.

[0062] The formation control method based on the integrity evaluation index preferentially uses speed constraints as the constraint conditions for the formation during the movement process after the formation is formed. In this open-loop control system, due to the existence of the cumulative error of the robots, the formation cannot maintain a stable structure for a long time. The integrity evaluation index of the formation is introduced to quantify the looseness of the robot formation. According to the running environment of the robots and the requirements of the formation task, a suitable threshold is set, and the position constraint conditions are switched in a timely manner. Each robot moves to the corresponding position in the ideal formation through path planning, and the cumulative error can be eliminated, so as to switch back to the speed constraint conditions again.

[0063] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope defined by the claims of the present invention.

Claims

1. A robot formation control method based on a formation integrity evaluation index, characterized in that: The method includes the following steps: Step 1: Calculate the actual average center coordinates based on the position coordinates of the robots within the actual formation, and simultaneously calculate the average center coordinates under the ideal formation; calculate the Euclidean distance between the two average center coordinates to obtain the average center error; Step 2: Calculate the Euclidean distances from the actual coordinates of each robot within the formation to the actual average center coordinates respectively and sum them up; simultaneously calculate the Euclidean distances from the ideal coordinates of each robot in the ideal formation to the ideal average center coordinates and sum them up; find the difference between the two sums of distances to obtain the sum-of-distances error; Step 3: Calculate the differences between the actual yaw-axis angles and the ideal yaw-axis angles of each robot respectively, and finally sum up each difference to obtain the angle error; Step 4: Sum up the average center error, the sum-of-distances error, and the angle error according to different weights to obtain the formation integrity evaluation index; Step 5: Determine whether to use position constraints or speed constraints for robot formation control based on the formation integrity evaluation index calculated in real time and a preset threshold, so as to implement the formation control method based on the formation integrity evaluation index.

2. The robot formation control method based on a formation integrity evaluation index according to claim 1, characterized in that: The average center error E in the above-mentioned step 1 c is defined as the Euclidean distance between the two average centers, that is, the average center error; In the map coordinate system, only considering the position coordinates of the robots, each robot forms an n-sided polygon with vertex coordinates (x i , y i ), where i = 1, 2... n. The coordinates of the average center C of this n-sided polygon are (x c , y c ), where The actual positions and ideal positions of the robots within the formation respectively form two polygons, and the average centers of the two polygons are C and C'; the average center error E c As shown in formula (1): Among them, (x′ i , y′ i ) are the vertex coordinates of the ideal formation shape of an n-sided polygon.

3. The robot formation control method based on a formation integrity evaluation index according to claim 2, characterized in that: The sum of the Euclidean distances L from the coordinates of each robot in Step 2 to the average center coordinates is shown in Formula (2); The distance from the formation robots to the average center and the error is E L As shown in Equation (3), where L and L’ are the sums of the distances from the coordinates of each robot in the actual formation and the ideal formation to the average center coordinates C and C’, respectively; E L = |L - L'| (3).

4. The robot formation control method based on a formation integrity evaluation index according to claim 3, characterized in that: In step 3, the yaw angle is used to describe the heading of the robot, and the actual yaw angle of the robot and the ideal yaw angle The difference e i is as shown in formula (4); Angle error As shown in formula (5); 5. The robot formation control method based on a formation integrity evaluation index according to claim 4, characterized in that: The formation integrity evaluation index P in Step 4 is shown in Formula (6), where P is a non-negative number, and the smaller P is, the closer the actual formation is to the ideal formation; Among them, the weight Q = [q1, q2, q3] T ; q1, q2, and q3 are all non-negative numbers, corresponding to the average center error E C , the error E of the sum of the average center distances L , and the angular error sensitivity.

6. The robot formation control method based on a formation integrity evaluation index according to claim 5, characterized in that: The specific values of q1, q2, and q3 are scaled proportionally; the proportional relationship of q1, q2, and q3 is determined by the operating environment and the task requirements of the formation execution, and their specific values are set within a corresponding value range for convenient later data processing.

7. The robot formation control method based on a formation integrity evaluation index according to claim 6, characterized in that: E C and E L and If the maximum value of is determined, then q1 ≤ P / E C_max and q2 ≤ P / E L_max and then the proportional relationship of q1, q2, q3 is 8. The robot formation control method based on a formation integrity evaluation index according to claim 1, characterized in that: In Step 5, during the movement process after the formation of the formation, speed constraints are preferentially used as the constraints for the formation. In this open-loop control system, due to the existence of cumulative errors of the robots, the formation cannot maintain a stable structure for a long time; the formation integrity evaluation index is introduced to quantify the looseness of the robot formation, and an appropriate threshold is set according to the robot operating environment and formation task requirements, and the position constraints are switched to timely. Each robot moves to the corresponding position in the ideal formation through path planning, which can eliminate the cumulative errors, and then switch back to the speed constraints.

9. The robot formation control method based on the formation integrity evaluation index according to claim 8, wherein: For the speed constraint condition, during the formation's progress, one of the robots acts as the leader for path planning to implement the complete target point navigation algorithm for the robots, while the other robots in the formation use speed constraints to maintain the progress of the robot formation. The actual position of the robot subject to the speed constraint is only related to the initial pose of the robot and subsequent speed control instructions, and has nothing to do with the positions of other robots within the formation. Without global map feedback, the formation maintenance at this time is open-loop control; therefore, in the ideal case without considering the robot motion errors, the robot formation can still ensure the stability of the robot formation only through speed constraints after the formation of the formation.

10. The robot formation control method based on the formation integrity evaluation index according to claim 8, wherein: Regarding the above-mentioned position constraint conditions, the robots in the formation are regarded as followers in the leader-follower method, and each robot has a corresponding virtual leader; the movement of the virtual leader can be independent of the influence of the robot's own hardware and actual friction factors, and all virtual leaders can achieve ideal movement states. Therefore, the formation shape composed of virtual leaders is absolutely stable; as followers, the actual robots only need to set their own motion target poses as the real-time poses of the virtual leaders, perform path planning, and have a global map as the pose information feedback. Under the position constraint conditions, the formation maintenance is a closed-loop control. Through continuous path planning of each robot, it can be ensured that the formation shape of the robots is always complete. In this way, the robot formation is a rigid whole for the outside world, with a stable internal structure, realizing the robot formation.

Images