A multi-robot affine formation control and dynamic obstacle avoidance method

Abstract

The present invention belongs to the technical field of multi-robot formation control, and specifically relates to a multi-robot affine formation control and dynamic obstacle avoidance method, including: S1, dividing the multi-robots into followers and navigators, and further dividing the navigators into first-level navigators and ordinary navigators; S2, determining the multi-robot affine formation structure; S3, calculating the relative speed between the affine formation and the obstacle, and judging whether the robot needs to avoid the obstacle by the relative speed; S4, determining the repulsive field function and the gravitational field function; S5, the affine formation moves to the target position according to the calculation results of steps S3 and S4. The present invention realizes the transformation of the nominal formation by changing the three variables of the control affine transformation structure through two-layer navigator control; based on the affine formation control method, an artificial potential field function is introduced to realize the transformation of the nominal formation of the multi-robot affine formation and the avoidance of dynamic obstacles.

Description

Technical Field

[0001] The invention belongs to the technical field of multi-robot formation control, and in particular relates to a multi-robot affine formation control and dynamic obstacle avoidance method. Background Art

[0002] In recent years, multi-robots have been widely used in disaster relief, cargo handling, patrolling, dynamic target search, etc. because they have greater flexibility, adaptability and robustness than single robots. Multi-robots perform various tasks through teamwork, and their overall work efficiency is greater than the sum of the work efficiencies of the same number of single robots. In order to better complete the collaboration between multi-robots and improve work efficiency, the research on multi-robot formations has become a research hotspot.

[0003] Multi-robot formation requires control methods to achieve. Since the successful application of self-consistency theory to formation control, people have conducted a lot of research on formation control methods that guarantee convergence, such as displacement-based methods, distance-based methods, and orientation-based methods. They are three traditional formation control methods that control robot formations by imposing constant constraints on the displacement, distance, and orientation between robots. The invariance of the constant constraints of robot formations has an important impact on the maneuverability of the formation. For example, the formation control method based on displacement constraints can track robot formations with time-varying translations, but it is difficult to control the scale or direction of the formation. This is because changing the scale or direction requires changing the displacement constraints, while the formation control method based on displacement constraints is invariant to the displacement of the formation. The formation control method based on distance constraints can track robot formations with time-varying translations and directions, but it is difficult to track formations with time-varying scales. The formation control method based on orientation constraints can track formations with time-varying translations and scales, but it is difficult to track formations with time-varying directions.

[0004] In order to make up for the above shortcomings, researchers have integrated special consistency theory into formation control. In his paper "Necessary and sufficient graphical conditions for affine formation control", Professor Lin Zhiyun realized the translation, rotation and scaling of robot formations based on the complex Laplace operator control method, but this formation method is only applicable to two-dimensional planes. In his paper "Affine formation maneuver control of multiagent systems", Dr. Zhao Shiyu combined affine formations and stress matrices to propose an affine formation control method, which can realize the translation, rotation and scaling of robot formations in three or higher dimensions, and consider the avoidance of static obstacles. It assumes that the field where the formation moves is completely stationary, plans the movement trajectory of the leader in advance, and avoids obstacles by controlling the formation changes of the entire formation. However, when there are dynamic obstacles in the environment or the environment cannot be perceived, the robots in the formation will be in danger of hitting obstacles and failing to work. In addition, the formation changes do not consider the transformation of the nominal formation, which limits the application scope of multi-robot formations.

[0005] In order to realize the transformation of the nominal formation of a multi-robot affine formation and the avoidance of dynamic obstacles, the present invention proposes a multi-robot affine formation control and dynamic obstacle avoidance method. Summary of the invention

[0006] In order to solve the above technical problems, the present invention provides a multi-robot affine formation control and dynamic obstacle avoidance method, which realizes the transformation of the nominal formation of the affine formation by changing the three variables of the control affine transformation structure through a two-layer leader control mode. The avoidance of dynamic obstacles is realized by introducing an artificial potential field function into the affine formation control method. The method enables the leader in the formation to have the ability to avoid dynamic obstacles, thereby enabling the entire formation to avoid dynamic obstacles.

[0007] In order to achieve the above object, the technical solution of the present invention is as follows:

[0008] A multi-robot affine formation control and dynamic obstacle avoidance method comprises the following steps:

[0009] S1. Divide multiple robots into followers and navigators, divide the navigators again, and select a first-level navigator and the rest of the ordinary navigators from the navigators;

[0010] S2, determine the multi-robot affine formation structure;

[0011] S3, calculating the relative speed between the affine formation and the obstacle, and judging whether the robot needs to avoid the obstacle based on the relative speed;

[0012] S4, determining the repulsive field function of the obstacle and the gravitational field function between the robot and the target position;

[0013] S5. The affine formation moves to the target position according to the calculation results of steps S3 and S4.

[0014] Furthermore, the specific method of dividing multiple robots into followers and leaders includes:

[0015] Assume that there are n robots in the plane, and the interactions between the robots are described by a fixed graph G∈(V,E), is an affine formation; the leader’s formation is The formation of the followers is

[0016] Furthermore, the navigators include a first-level navigator and several ordinary navigators.

[0017] Furthermore, the ordinary navigator determines its own position through the first-level navigator and the affine formation, and the follower determines its own position through the first-level navigator and the ordinary navigator; the expected position of each robot is The expected position of the pilot is The expected position of the follower is Set the first robot as the first-level navigator, and the expected position of the first-level navigator is equal to the actual position, that is, The relative displacement of the ordinary navigator and the follower to the first-level navigator is d = [d1, d2, ... d n ] T , then the expected positions of the normal leader and follower are:

[0018]

[0019] Where x1 represents the actual position of the first-level pilot; d i represents the relative displacement between the ith robot and the first-level navigator, d i =p1-p i , i=1,2,…n.

[0020] Furthermore, the specific method of step S3 includes:

[0021] Treat the obstacle as a circle, with the center of the obstacle as p o , with a radius of r o , with speed v o Movement; the transformation area of ​​the affine formation is also considered as a circle with the center as p lo , with a radius of r lo , the speed is vl ;v lo is the projection of the relative velocity between the affine formation area and the dynamic obstacle on the position vector between them, through v lo To determine whether the two will collide, it can be expressed as:

[0022] v lo =[v l -v o ] T n lo

[0023] Among them, n lo Represents the unit vector from the center of the affine formation to the center of the obstacle;

[0024] If v lo ≤0, indicating that the affine formation is far away from obstacles and does not need to avoid them;

[0025] If v lo >0 means that the affine formation is approaching an obstacle and may collide with it.

[0026] Furthermore,

[0027] Furthermore, suppose that after time Δt, the distance between the affine formation and the obstacle is the smallest.

[0028]

[0029] Where t represents time, therefore, the shortest distance between the affine formation and the dynamic obstacle can be calculated as

[0030] D min (t+Δt)=||x i (t+Δt)-p o (t+Δt)||

[0031] If D min (t+Δt)≤R o (R o =r o +r lo ), the affine formation and the obstacle will collide and need to be avoided, otherwise no obstacle avoidance is needed.

[0032] Furthermore, when it is determined that the robot will collide with an obstacle after a period of time Δt, the robot accelerates at an acceleration of -u omax Slow down the speed, so that the relative speed between the robot and the obstacle at time t is v lo (t) drops rapidly to zero.

[0033] Furthermore, during the robot's deceleration, the robot's movement distance dm (v lo (t)) is:

[0034]

[0035] Furthermore, the repulsive field function between the robot and the dynamic obstacle in step S4 is:

[0036]

[0037] Among them, φ β (s) = ρ h (s / d β )(σ1(sd β )-1), represents the action function between the robot and the obstacle, d β =||d o || σ , d o is the expected distance between the robot and the obstacle;

[0038] ρ h (·) is a continuous and smooth collision function from 0 to 1:

[0039]

[0040]

[0041] Then, the repulsive field function between the affine formation and the dynamic obstacle is:

[0042] V β =ψ β (||d s (p lo ,q o )-d m (v lo )|| σ )

[0043] Among them, d s (p lo ,q o ) represents the shortest distance between the robot and the obstacle.

[0044] Furthermore, the gravitational field function between the robot and the target position is:

[0045]

[0046] where k γ1 , k γ2 is a constant, q goal is the location of the target point,

[0047] Derivative the gravitational field function to obtain the gravitational force f at the target position att (p lo ,v l )for:

[0048]

[0049] Furthermore, the specific method of moving the robot in step S5 includes:

[0050] Suppose the total input of the ith robot is:

[0051] u i =(1-I l,f ) d +I l,f (u γ +u β +u α )

[0052] where u d is the control input of the basic affine formation, u γ is the gravitational control input of the gravitational field function, u β is the repulsive force control input of the repulsive field, u α It is the control input of the ordinary pilot No. i tracking the first-level pilot. l,f is a quantity related to i, when i∈V l When l,f is 1; when i∈V f When l,f is 0, where V l represents the set of pilot robots, V f represents the set of follower robots, i.e.

[0053]

[0054] Furthermore,

[0055]

[0056] u γ =d a (f att (p lo ,v l ))

[0057]

[0058]

[0059] Among them, f att (·) represents the calculated gravitational force, d aIn order to prevent the gravity from being too strong and affecting the stability of multi-robot operation, the gravity value is limited;

[0060] k α , k β is the gain coefficient of each item; k1, k2 are the gain coefficients of each item.

[0061] Furthermore,

[0062]

[0063] Where n att =f att / ||f att || is the unit vector of gravity, f max is the maximum value of gravity, which is a constant and f max =u max -u omax ,u omax is the maximum acceleration of the robot when avoiding obstacles, u max is the maximum acceleration that the robot can achieve.

[0064] Compared with the prior art, the present invention has the following beneficial effects:

[0065] (1) The present invention adopts a two-layer navigator control method, in which the navigators are divided into a first-level navigator and a common navigator, and the transformation of the nominal formation is realized by changing the three variables of the control affine transformation structure.

[0066] (2) The present invention determines whether it is necessary to avoid an obstacle by calculating the relative speed between the robot and the obstacle, and introduces a speed term into the gravitational field function to accurately control the robot's movement input, thereby achieving flexible avoidance of dynamic obstacles and accurately reaching the target position. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 Types of affine formations provided for embodiments of the present invention: (a) original formation, (b) rotated formation, (c) reduced formation, and (d) oblique formation.

[0068] Figure 2 This is a diagram illustrating the obstacle prediction mechanism provided in an embodiment of the present invention.

[0069] Figure 3 A schematic diagram of a nominal formation of an affine formation according to an embodiment of the present invention.

[0070] Figure 4 Schematic diagram of another nominal formation of an affine formation according to an embodiment of the present invention.

[0071] Figure 5 Schematic diagram of the process of affine formation avoiding obstacles according to an embodiment of the present invention.

[0072] Figure 6 This is a graph showing the average error tracking curve of an embodiment of the present invention.

[0073] Figure 7 4 is a distance curve diagram between the robot and obstacles according to an embodiment of the present invention.

[0074] Figure 8 4 is a motion speed curve diagram of each robot in an embodiment of the present invention.

[0075] Fig. 9 The figure is a flow chart of formation control and dynamic obstacle avoidance according to an embodiment of the present invention. DETAILED DESCRIPTION

[0076] The technical solution of the present invention will be clearly described below in conjunction with the accompanying drawings. Obviously, the described embodiments are not all embodiments of the present invention, and all other embodiments obtained by ordinary technicians in the field without making creative work are within the protection scope of the present invention.

[0077] like Fig. 9 As shown, the present invention provides a multi-robot affine formation control and dynamic obstacle avoidance method, comprising the following steps:

[0078] S1. Divide multiple robots into followers and navigators, divide the navigators again, and select a first-level navigator and the rest of the ordinary navigators from the navigators;

[0079] S2, determine the multi-robot affine formation structure;

[0080] S3, calculating the relative speed between the affine formation and the obstacle, and judging whether the robot needs to avoid the obstacle based on the relative speed;

[0081] S4, determining the repulsive field function of the obstacle and the gravitational field function between the robot and the target position;

[0082] S5. The affine formation moves to the target position according to the calculation results of steps S3 and S4.

[0083] The design idea of ​​dividing the formation robot into a leader and a follower includes: using a follower-leader structure to realize an affine formation. The leader determines the formation of the formation, and the follower determines its own desired position through the leader's position and according to the control method, and moves from the current position to the desired position, so that the entire formation forms a target formation. The leader can realize affine formation transformations such as beveling, scaling, and rotation by changing the relative position.

[0084] In this embodiment, it is assumed that there are n robots in the plane, and the interaction between the robots is described by a fixed graph G∈(V,E). is an affine formation. Among them, the leader’s formation is The formation of the followers is

[0085] The robots in the affine formation include two types: leaders and followers. The goal is to keep the followers and leaders at the same speed and in a stable formation. Assume that there are n robots in the plane, and the interaction between the robots is described by a fixed graph G∈(V,E). The graph consists of a vertex set and an edge set E∈V×V. Edge (i,j)∈E indicates that robot i can receive information from robot j, and robot j is i's neighbor. The neighbor set of vertex i is N i ={j∈V:(j,i)∈E}. This invention only considers undirected graphs, that is, Assume that the first n l The robot is the leader, and the remaining n f The robots are followers, where n f =nn l , then V l ={1,...,n l} and V f =V\V l are the sets of leaders and followers respectively. is an affine formation. Among them, the leader’s formation is The formation of the followers is A formation (G, p) represents a one-to-one mapping between the formation of the i-th robot and the i-th node in the graph G.

[0086] A nominal formation corresponding to a graph G can be defined as (G, r), where the nominal formation is a constant. Affine transformation is a conventional form of linear transformation, such as translation, rotation, reduction, skew (such as Figure 1 ), and combinations of these transformations. Therefore, the affine mapping of the nominal formation r can be defined as

[0087]

[0088] Among them I n represents the n×n identity matrix, represents the Kronecker product, 1 n is an n-dimensional column vector with values ​​of 1, b and Both represent 2D column vectors, represents a 2×2 dimensional matrix, and the affine transformation is denoted by (A,b).

[0089] The navigator includes a first-level navigator and several ordinary navigators. The ordinary navigator determines its own position through the first-level navigator and the affine formation, and the follower determines its own position through the first-level navigator and the ordinary navigator; the expected position of each robot is The expected position of the pilot is The expected position of the follower is Set the first robot as the first-level navigator, and the expected position of the first-level navigator is equal to the actual position, that is, The relative displacement of the ordinary navigator and the follower to the first-level navigator is d = [d1, d2, ... d n ] T , then the expected positions of the normal leader and follower are:

[0090]

[0091] Where x1 represents the actual position of the first-level pilot; d i represents the relative displacement between the ith robot and the first-level navigator, d i =p1-p i , i=1,2,…n.

[0092] From the calculation formula of the relative position between robots, we can know that the formation of the affine formation is related to the position of the robot, which is determined by the parameters A, r and b. A is a 2×2 matrix, b is a 2D column vector, and r is the nominal formation. Different nominal formations can be obtained by giving different r values, and then the transformation of the nominal formation can be realized; by modifying (A, b), affine transformations such as translation, rotation, scaling, and beveling can be realized.

[0093] Treat obstacles as circles, such as Figure 2 As shown, the center of the obstacle is p o , with a radius of r o , with speed v o Movement; the transformation area of ​​the affine formation is also considered as a circle with the center as p lo , with a radius of r lo , the speed is v l ;v lo is the projection of the relative velocity between the affine formation area and the dynamic obstacle on the position vector between them, through v lo To determine whether the two will collide, it can be expressed as:

[0094] v lo =[v l -v o ] T n lo

[0095] Among them, nlo Represents the unit vector from the center of the affine formation to the center of the obstacle;

[0096] If v lo ≤0, indicating that the affine formation is far away from obstacles and does not need to avoid them;

[0097] If v lo >0 means that the affine formation is approaching an obstacle and may collide with it.

[0098] in, Represents the unit vector from the center of the affine formation to the center of the obstacle.

[0099] Assume that after time Δt, the distance between the affine formation and the obstacle is the smallest.

[0100]

[0101] Where t represents time, p o (t), v o (t), x i (t), v i (t) respectively represent p o , v o , x i , v i The value at time t; p o (t+Δt), x i (t+Δt) represents the p after Δt time from time t. o , x i Therefore, the shortest distance between the affine formation and the dynamic obstacle can be calculated as

[0102] D min (t+Δt)=||x i (t+Δt)-p o (t+Δt)||

[0103] If D min (t+Δt)≤R o (R o =r o +r lo ), the affine formation and the obstacle will collide and need to be avoided, otherwise no obstacle avoidance is needed.

[0104] When it is determined that the robot will collide with an obstacle after a period of time Δt, the robot moves at an acceleration of -u omax Slow down the speed, so that the relative speed between the robot and the obstacle at time t is v lo (t) drops rapidly to zero.

[0105] During the robot's deceleration, the robot's movement distance d m (v lo (t)) is:

[0106]

[0107] Calculate the shortest distance d between the robot and the obstacle at time t s (p lo (t),p o (t)), i.e.

[0108] d s (p lo (t),p o (t))=||p lo (t)-p o (t)||-R o .

[0109] In this embodiment, the σ norm is used to calculate the distance between the affine formation and the obstacle, in order to construct a smooth repulsive field function. The repulsive field function between the robot and the dynamic obstacle is:

[0110]

[0111] Among them, φ β (s) = ρ h (s / d β )(σ1(sd β )-1), represents the action function between the robot and the obstacle,

[0112] d β =||d o || σ , d o is the expected distance between the robot and the obstacle;

[0113] ρ h (·) is a continuous and smooth collision function from 0 to 1:

[0114]

[0115]

[0116] Then, the repulsive field function between the affine formation and the dynamic obstacle is:

[0117] V β =ψ β (||d s (p lo ,p o )-d m (v lo )||σ )

[0118] Among them, d s (p lo ,p o ) represents the shortest distance between the robot and the obstacle. The required repulsive force is calculated by calculating the difference between the shortest distance between the robot and the obstacle and the distance required to slow down. The smaller the distance difference, the greater the required repulsive force.

[0119] The traditional gravitational field function between the robot and the target position is:

[0120]

[0121] In order to allow the affine formation to move smoothly to the target point, a velocity term is added to the traditional gravitational field function. The gravitational field function between the robot and the target position is:

[0122]

[0123] where k γ1 , k γ2 is a constant, q goal is the location of the target point,

[0124] Derivative the gravitational field function to obtain the gravitational force f at the target position att (p lo ,v l )for:

[0125]

[0126] Considering that the actual robot's movement speed is limited, in order to better avoid dynamic obstacles, the gravity is limited while ensuring that the affine formation reaches the destination under the traction of the gravitational field.

[0127]

[0128] Where n att =f att / ||f att || is the unit vector of gravity, f max is the maximum value of gravity, which is a constant and f max =u max -u omax ,u omax is the maximum acceleration of the robot when avoiding obstacles, u max is the maximum acceleration that the robot can achieve.

[0129] The specific method of moving the robot in step S5 includes:

[0130] Suppose the total input of the ith robot is:

[0131] u i =(1-I l,f ) d +I l,f (u γ +u β +u α )

[0132] where u d is the control input of the basic affine formation, u γ is the gravitational control input of the gravitational field function, u β is the repulsive force control input of the repulsive field, u α It is the control input of the ordinary pilot No. i tracking the first-level pilot. l,f is a quantity related to i, when i∈V l When l,f is 1; when i∈V f When l,f is 0, where V l represents the set of pilot robots, V f represents the set of follower robots, i.e.

[0133]

[0134] Furthermore,

[0135]

[0136] u γ =d a (f att (p lo ,v l ))

[0137]

[0138]

[0139] Among them, f att (·) represents the calculated gravitational force, d a In order to prevent the gravity from being too strong and affecting the stability of multi-robot operation, the gravity value is limited; k α , k β is the gain coefficient of each item; k1, k2 are the gain coefficients of each item. The control input of the robot consists of two parts, one is the input of the leader, which is used to lead the entire affine formation to move; the other is the input of the follower, which is determined according to the position and speed of the leader. The obstacle avoidance and driving to the target position of the entire affine formation are all affected by the leader.

[0140] In order to verify the dynamic obstacle avoidance function of the multi-robot affine formation, it is assumed that the affine formation first forms a triangle formation, such as Figure 4 As shown. Assume that there are 7 robots in a two-dimensional space moving in formation and there is a dynamic obstacle on the way. Robot 1 is the first-level navigator, robots 2 and 3 are ordinary navigators, and the remaining robots are followers. The initial center position of the obstacle is q0 = (15, -7) m, and the initial speed is v o =(-0.5,-0.5)m / s and maintain uniform motion, with a radius of r o = 2.5m, the expected safe distance d between the robot and the obstacle o =R o =2.5m. The maximum movement speed of a single robot is v max =10m / s, maximum acceleration is u max =10m / s 2 , maximum obstacle avoidance acceleration u omax =7m / s 2 , the initial speed is 0m / s, and the initial position is generated in [0, 4]m×[-4, 0]m. The target position q of the affine formation goal =[25, -20]m. Other parameters are set as follows: k r1 =1, k r2 =3,ε=0.1,h=0.8,k α =18, k β =-1, k1 =-1, k2 =-2, k p =4, k v =1, the simulation step is set to 0.005s.

[0141] Figure 5 The process diagram of multi-robot affine formation avoiding dynamic obstacles is given. Figure 5 As can be seen from (a)-5(d), the affine formation can form the desired triangular formation movement. When the navigator robots 1, 2, and 3 detect a dynamic obstacle and detect that they will collide with it, the entire affine formation begins to avoid the obstacle and the running trajectory changes significantly. When the affine formation successfully avoids the dynamic obstacle, the formation will be reconstructed and transformed into Figure 3 The nominal formation shown, such as Figure 5 (e)-5(f) The results show that this method can effectively avoid dynamic obstacles and transform multiple nominal formations.

[0142] In order to obtain the accuracy of the affine formation from a dispersed formation to a stable target formation, Figure 6 The average error tracking curve of the affine formation is plotted. The error formula used in this curve is: As can be seen from the figure, at the beginning (such as Figure 5 (a) shows that the robots are scattered on the two-dimensional plane, and the formation is far from the target formation. As time goes by, the error of the formation gradually approaches 0 (as shown in Figure 5 (d) shows), at this time the formation changes to Figure 4 The nominal formation is shown. Starting from 18s, the target formation of the affine formation is set to Figure 3 Because the triangle formation is far from the target formation, the average error increases sharply until it becomes Figure 3 The nominal formation shown (such as Figure 5 (f) shows that the average error approaches 0 again. The change of the average error tracking curve shows that this method can effectively achieve the transformation of the nominal formation.

[0143] Figure 7 The distances between the seven robots and the obstacles are described. As can be seen from the figure, at the beginning, the leader leads the entire affine formation to move. Although the obstacle is getting closer and closer to the affine formation, the obstacle avoidance condition has not been met and there is no need to avoid the obstacle. At about 2.5s, the leader detects that the affine formation may collide with the obstacle, and the affine formation begins to avoid the obstacle. In the 2.5-7.5s period, the affine formation avoids obstacles under the control of the control method. After 8s, the affine formation successfully avoids the obstacle and the distance from the obstacle is getting farther and farther. At 18s, due to the change in the formation of the affine formation, the distance between some robots and the obstacle changes, so the distance curve between the robot and the obstacle fluctuates. The simulation results show that the proposed control method for avoiding dynamic obstacles is effective and can realize flexible obstacle avoidance of multiple robots in complex dynamic environments.

[0144] Figure 8 is the motion speed curve of the 7 robots during the simulation, where Figure 8 (a) is the x-axis velocity of the robot, Figure 8(b) is the y-axis speed of the robot. As can be seen from the figure, the speeds of all robots are basically consistent, but there are inconsistencies in the three time periods of 1s-2s, 2.5s-7.5s and 18s-35s. The reason for the inconsistency in speed between 1s and 2s is that the robot affine formation appears in order to form a triangular formation from a scattered state. The reason for the inconsistency in speed between 2.5s and 7.5s is that this period is the obstacle avoidance time. The affine formation detects that the distance between each robot and the obstacle is different, and then adopts different speeds to effectively avoid the obstacle. The speeds of the robots are consistent until the obstacle avoidance ends after 7.5s. From 18s, due to the change of the affine formation formation, the distances to be moved by each robot are different, so the movement speeds of each robot are inconsistent again. It is not until 35s later that the affine formation formation is transformed into the target formation, and the speeds of each robot gradually become consistent. The simulation results show that the control method can realize the control of the speed of each robot, and then realize the avoidance of dynamic obstacles and the transformation of the nominal formation.

[0145] In order to realize the transformation of the nominal formation of a multi-robot affine formation and the avoidance of dynamic obstacles, the present invention proposes a multi-robot affine formation obstacle avoidance control method. The transformation of the nominal formation of a multi-robot affine formation is realized by two-layer leader control; the avoidance of dynamic obstacles is realized by combining the affine formation control method and the artificial potential field function. The simulation results show that this method can effectively realize the transformation of the nominal formation of a multi-robot affine formation and the avoidance of dynamic obstacles.

[0146] The above specific implementation methods are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to examples, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the scope of the technical solutions of the present invention, which should be included in the scope of the claims of the present invention.

Claims

1. A multi-robot affine formation control and dynamic obstacle avoidance method, characterized in that: The following steps are involved: S1. Divide multiple robots into followers and navigators, divide the navigators again, and select a first-level navigator and the rest of the ordinary navigators from the navigators; S2, determine the multi-robot affine formation structure; S3, calculating the relative speed between the affine formation and the obstacle, and judging whether the robot needs to avoid the obstacle based on the relative speed; Treat the obstacle as a circle, with the center of the obstacle being , the radius is , with speed Movement; the transformation area of ​​the affine formation is also considered as a circle with the center , the radius is , the speed is ; S4, determining the repulsive field function of the obstacle and the gravitational field function between the robot and the target position; The repulsive field function between the robot and the dynamic obstacle in step S4 is: in, , represents the action function between the robot and the obstacle, , is the expected distance between the robot and the obstacle; A continuous and smooth collision function from 0 to 1: Then, the repulsive field function between the affine formation and the dynamic obstacle is: in, Indicates the shortest distance between the robot and the obstacle; is the movement distance of the robot during the robot deceleration process; is the projection of the relative velocity between the affine formation area and the dynamic obstacle on the position vector between them; The gravitational field function between the robot and the target position is: in , is a constant, is the location of the target point, Derivative the gravitational field function to obtain the gravitational force at the target location for: ； S5. The affine formation moves to the target position according to the calculation results of steps S3 and S4.

2. The multi-robot affine formation control and dynamic obstacle avoidance method according to claim 1, characterized in that: The specific methods for dividing multiple robots into followers and leaders include: Assume that there is a n The interaction between robots is determined by a fixed graph. To describe, is an affine formation; the leader’s formation is , the formation of the followers is .

3. The multi-robot affine formation control and dynamic obstacle avoidance method according to claim 1, characterized in that: The pilots include a first-class pilot and several ordinary pilots; The ordinary navigator determines its position through the first-level navigator and the affine formation, and the follower determines its position through the first-level navigator and the ordinary navigator; the expected position of each robot is , where the expected position of the pilot is , the expected position of the follower is ; Set the first robot as the first-level navigator, and the expected position of the first-level navigator is equal to the actual position, that is, ; The relative displacement of the ordinary navigator and the follower to the first-level navigator is , then the expected positions of the normal leader and follower are: in, Indicates the actual position of the first-level pilot; Indicates i The relative displacement between the robot and the first-level navigator, , i =1, 2,…n.

4. The multi-robot affine formation control and dynamic obstacle avoidance method according to claim 3, characterized in that: The specific method of step S3 includes: is the projection of the relative velocity between the affine formation area and the dynamic obstacle on the position vector between them, through To determine whether the two will collide, it can be expressed as: in, Represents the unit vector from the center of the affine formation to the center of the obstacle; if , indicating that the affine formation is far away from obstacles and does not need to avoid them; if It means that the affine formation is approaching an obstacle and may collide with it; 。 5. The multi-robot affine formation control and dynamic obstacle avoidance method according to claim 4, characterized in that: Assuming the time , the distance between the affine formation and the obstacle is the smallest. In the formula represents time, therefore, the shortest distance between the affine formation and the dynamic obstacle can be calculated as if ( ), the affine formation and the obstacle will collide and need to be avoided, otherwise no obstacle avoidance is needed.

6. The multi-robot affine formation control and dynamic obstacle avoidance method according to claim 5, characterized in that: When the elapsed time is determined The robot will collide with the obstacle, and the robot will accelerate Slow down the speed, so that the robot and the obstacle are t Relative speed of time Quickly dropped to zero.

7. The multi-robot affine formation control and dynamic obstacle avoidance method according to claim 6, characterized in that: During the robot's deceleration, the robot's movement distance for: 。 8. The multi-robot affine formation control and dynamic obstacle avoidance method according to claim 7, characterized in that: The specific method of moving the robot in step S5 includes: Set up i The total input for each robot is: in is the control input of the basic affine formation, is the gravitational control input of the gravitational field function, is the repulsive force control input of the repulsive field, yes i The regular pilot tracked the control input of the first pilot, is a i The relevant amount, when hour, is 1; when hour, is 0, where represents the set of navigator robots, represents the set of follower robots, i.e. 。 9. The multi-robot affine formation control and dynamic obstacle avoidance method according to claim 8, characterized in that: in, represents the calculated gravitational force, In order to prevent the gravity from being too strong and affecting the stability of multi-robot operation, the gravity value is limited; , is the gain coefficient of each term; , is the gain coefficient of each term.

10. The multi-robot affine formation control and dynamic obstacle avoidance method according to claim 9, characterized in that: in is the unit vector of gravity, is the maximum value of gravity, which is a constant and , is the maximum acceleration of the robot when avoiding obstacles. is the maximum acceleration that the robot can achieve.

Images