Swarm robot affine formation control method based on hybrid measurement

Abstract

The invention discloses a swarm robot formation control method based on hybrid measurement, and the method comprises the following steps: 1, selecting a master leader and two slave leader, and constructing a communication topological graph with the master leader as a root node; 2, establishing a global coordinate system by taking the coordinate of the main leader as an original point, and obtaining an environment map; step 3, acquiring coordinates of the slave leader in the global coordinate system based on the environment map; 4, acquiring respective expected positions of the master leader and the slave leader; 5, the master leader and the slave leader track respective expected positions in real time based on the optimized control law of the leader; 6, calculating an ideal distance between the current follower and the adjacent robot; and 7, adjusting the real-time distance between the current follower and the adjacent robot based on the follower formation control rate to realize formation control. The method does not need to depend on GPS / GNSS to obtain global coordinates, different measurement modes can be adopted, the flexibility of system construction is improved, and the cost is reduced.

Description

Technical Field

[0001] The invention relates to the technical field of group robot formation control, and in particular to a group robot formation control method based on hybrid measurement. Background Art

[0002] With the development of embedded, Internet and artificial intelligence technologies, mobile robot control technology is becoming more and more mature. In order to achieve some large-scale tasks in complex environments, multi-robot collaboration is an effective method. Through the cooperation of multiple robots, work efficiency can be improved and tasks that a single robot cannot complete can be completed. Formation control is one of the core technologies of group robot collaboration and is also a hot issue at present.

[0003] When a robot formation passes through an area with obstacles or a narrow passage, it needs to adjust the formation in real time according to the surrounding environment. The traditional formation control algorithm regards the entire formation as a rigid body and cannot effectively realize the transformation of the formation. In recent years, in order to solve the formation transformation problem, the academic community has proposed an affine formation control algorithm. This algorithm calculates the target position of the robot based on the stress matrix, and can realize the affine transformation of the formation, thereby realizing transformations such as formation expansion, rotation and translation. However, the algorithm requires at least some robots to obtain the absolute position in the global coordinate system. In GPS / GNSS-denied environments such as tunnels and indoors, it is difficult to obtain the absolute position of the robot.

[0004] As the price of LiDAR drops rapidly, its application range is getting wider and wider. For swarm robot systems, if all robots use LiDAR, the total cost will also rise rapidly as the number of robots increases. Summary of the invention

[0005] In view of the deficiencies in the prior art, the present invention provides a group robot formation control method based on hybrid measurement to solve the technical problems in the prior art that the absolute position is difficult to obtain and the cost is high.

[0006] The present invention provides a group robot formation control method based on hybrid measurement, comprising the following steps:

[0007] Step 1: The three robots equipped with laser radar in the group of robots are respectively used as a main leader and two slave leaders, and the remaining robots are used as followers, and a communication topology graph with the main leader as the root node is constructed;

[0008] Step 2: Establish a global coordinate system with the coordinates of the main leader as the origin, and obtain the environment map through the laser radar of the main leader;

[0009] Step 3: Obtain the point cloud from the leader's lidar and obtain the leader's coordinates in the global coordinate system based on the environment map;

[0010] Step 4: Obtain the expected positions of the master leader and the slave leader according to the environment map, the slave leader coordinates, the communication topology map, the nominal formation of the formation, and the stress coefficient of the affine formation;

[0011] Step 5: The master leader and the slave leader track their respective desired positions in real time based on the optimized leader's control law;

[0012] Step 6: Obtain the affine transformation matrix of the current follower's adjacent robot and estimate the affine transformation matrix of the current follower; calculate the ideal distance between the current follower and the adjacent robot according to the estimated affine transformation matrix and the position difference of the adjacent robot in the nominal configuration;

[0013] Step 7: Obtain the relative position of the adjacent robot in the current follower's own coordinate system, and adjust the real-time distance between the current follower and the adjacent robot according to the ideal distance based on the follower formation control rate to achieve formation control.

[0014] Furthermore, the desired position in steps 4 and 5 is:

[0015]

[0016] Where A(t) is the affine transformation matrix, b(t) is the translation matrix, r i is the nominal position of the leader robot node.

[0017] Furthermore, in step 5, the control law of the leader is:

[0018]

[0019] Where a, b, k1, k2 are control gains; p i is the actual position of the leader; μ(t) is a time-varying function; is the target formation at time t; ω ij is the stress coefficient.

[0020] Furthermore, in step 6, the affine transformation matrix of the current follower is estimated based on a consistency algorithm that converges within a preset time.

[0021] Furthermore, the specific formula of the estimation is:

[0022]

[0023] In the formula, For the matrix The i-th column element of yes is the set of neighboring robots of the current follower; a ij is the weight of the connection between the current follower and the adjacent robot; α and β are control gains; μ1 is a time-varying function.

[0024] Furthermore, the follower formation control rate in step 7 is:

[0025]

[0026] Where a is the positive control gain, a>0; e ij is the distance error between follower i and adjacent robot j, sgn(·) is a sign function.

[0027] Beneficial effects of the present invention:

[0028] All robots of the present invention do not need to rely on GPS / GNSS to obtain global coordinates, and leaders and followers can use different measurement methods, which improves the flexibility of system construction and helps reduce costs. The algorithm proposed by the present invention only needs to calculate the stress coefficients between three leaders during implementation, which reduces the difficulty of implementation; the leader can converge to a specified position at a specified time; when the topological structure of the formation changes, the stress matrix does not need to be recalculated. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The features and advantages of the present invention will be more clearly understood by referring to the accompanying drawings, which are schematic and should not be construed as limiting the present invention in any way. In the accompanying drawings:

[0030] Figure 1 It is a method flow chart of a specific embodiment of the present invention. DETAILED DESCRIPTION

[0031] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0032] The present invention is further illustrated below in conjunction with specific embodiments. Those skilled in the art should understand that these embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention, and modifications to various equivalent forms of the present invention fall within the scope defined by the appended claims of this application.

[0033] like Figure 1 As shown, the present invention provides a group robot formation control method based on hybrid measurement, comprising the following steps:

[0034] Step S1: The three robots equipped with laser radar in the group of robots are respectively used as a main leader and two slave leaders, and the remaining robots are used as followers to build a communication topology graph with the main leader as the root node. The specific steps are as follows:

[0035] Step S11: Define the topological relationship between robots as a graph The nominal formation is in, represents the nominal configuration, and in, represents the set of nodes in the graph, represents the set of edges in the graph; when the graph There are n l The node is the leader, denoted by The remaining nodes are followers, denoted by The robots adjacent to robot node i are denoted as In formation Divided into leaders and followers The state vector of robot i is represented by p i express.

[0036] Step S12: The dynamic model of the N robot nodes can be represented by a first-order integrator model:

[0037]

[0038] In the formula, u i The control input for the robot.

[0039] Step S2: Establish a global coordinate system with the coordinates of the main leader as the origin, and obtain the environment map through the laser radar of the main leader. The specific steps are as follows:

[0040] Step S21: The main leader robot uses a laser radar to scan the surrounding environment and obtain environmental data of obstacles, walls and other objects. The main leader converts the laser radar scanning data into a global map through a SLAM algorithm, and the main leader obtains its own position through a matching algorithm;

[0041] Step S22: The master leader sends the constructed map to the slave leader via the wireless communication module.

[0042] Step S3: Obtain the point cloud from the leader's laser radar and obtain the leader's coordinates in the global coordinate system based on the environment map, specifically:

[0043] The slave leader scans the environment around itself through lidar to obtain the relative distance and direction between itself and the reference point in the global map. The slave leader matches the received map information with the local environment information collected by its own lidar to calculate its absolute position in the global map.

[0044] Step S4: Obtain the expected positions of the master leader and the slave leader according to the environment map, the slave leader coordinates, the communication topology map, the nominal formation, and the stress coefficients of the affine formation;

[0045] Nominal formation set Must meet: exist can be affine spanned so that Universally rigid and affine positionable,

[0046] The nominal formation Each edge (i, j) in the nominal formation corresponds to a stress weight This stress can be positive or negative, and the stress matrix is:

[0047]

[0048] The affine mapping of the nominal formation is defined as:

[0049]

[0050] The leader’s affine transformation matrix A(t) and translation matrix b(t) can be obtained through affine mapping of the nominal formation.

[0051] The leader robot performs affine transformation on the nominal formation based on the latest topological information, and adjusts its own target position in real time to control the position of the followers, and thus controls the formation of the entire formation. The expected position of leader i is:

[0052]

[0053] In the formula, is the affine transformation matrix, r i is the nominal position of the leader robot node, is the translation matrix, and A(t) and b(t) are both continuous for time t. The master leader and the two slave leaders can obtain their respective expected positions through the above expected position formula.

[0054] Based on the position information of each robot in the formation, the main leader calculates the affine matrix parameters that need to be adjusted according to the environment and task requirements, issues instructions, and controls the formation to perform affine transformation; the leader coordinates the relative motion of the followers and helps adjust the local structure of the formation.

[0055] Step 5: The master leader and the slave leader track the desired position of the target formation in real time based on the optimized leader's control law;

[0056] When the leader robot tracks its desired position, considering the response speed and performance of the actual system, it is necessary to ensure that the system reaches the desired position of the formation within a specific time. To ensure that the leader robot can reach the desired position within the specified time, a time-varying function is introduced. The time t when the control system converges to the desired position can be within the preset time [t0, T]. The expression of the time-varying function μ(t) is:

[0057]

[0058] Where ρ>0 is the control gain.

[0059] After adding the time-varying function to the leader's control law, the optimized leader's control law is:

[0060]

[0061] Where a, b, k1, k2 are control gains; p i is the actual position of the leader; μ(t) is a time-varying function; is the target formation at time t; ω ij is the stress coefficient.

[0062] Step 6: Obtain the affine transformation matrix of the current follower's neighboring robots, and estimate the affine transformation matrix of the current follower based on a consistency algorithm that converges within a preset time. According to the estimated affine transformation matrix The ideal distance between the current follower and the neighboring robot is calculated based on the position difference with the neighboring robot in the nominal configuration;

[0063] Affine transformation matrix The dimension is m×n, through vectorized operation, affine transformation matrix can be expanded into a column vector at the same time Can also be converted to The relationship is as follows

[0064]

[0065] In the formula, Representation Matrix The i-th column element of .

[0066] The follower robot adjusts the estimated value of its state variable through a consensus algorithm that converges in a preset time. Estimates of the shift variables Make it gradually tend to be consistent with the leader's affine transformation and translation. The specific formula is as follows:

[0067]

[0068] In the formula, is the set of neighboring robots of the current follower; a ij is the weight of the connection between the current follower and the adjacent robot; α and β are control gains; μ1 is a time-varying function.

[0069] By combining the control law of the optimized leader and The estimation method realizes the affine transformation control motion of the group robot system represented by the dynamic model and A(t), and at the same time allows the estimated value to converge to the true value, that is, when t→∞, there is

[0070] will converge to the true value Convert to and the follower's own position p i The follower and the adjacent robot p can be obtained j The ideal distance between is:

[0071]

[0072] Step 7: Obtain the relative position of the adjacent robot in the current follower's own coordinate system, and adjust the real-time distance between the current follower and the adjacent robot according to the ideal distance based on the follower formation control rate to achieve formation control.

[0073] Follower i uses sensors to measure the coordinates of the adjacent robots in the follower i's own coordinate system; the coordinates of follower i are defined as The coordinates of the adjacent robots are Then the distance error between follower i and adjacent robot j is:

[0074]

[0075] In the formula, is the actual distance between two adjacent robots; is the ideal distance between two adjacent robots; the upper and lower limits of the distance error between robots are [e min ,e max ].

[0076] The squared difference of the distance error between robots is expressed as:

[0077]

[0078] In order to make the formation reach the target formation, the follower uses the optimized follower formation control rate to control the relative distance between itself and the adjacent robot in real time, so that the relative distance between the robots remains within the preset range [e min ,e max ],

[0079] The follower formation control rate is:

[0080]

[0081] Where a is the positive control gain, a>0; sgn(·) is a sign function.

[0082] Although the embodiments of the present invention have been described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present invention, and such modifications and variations are all within the scope defined by the appended claims.

Claims

1. A group robot formation control method based on hybrid measurement, characterized in that: The steps include: Step 1: The three robots equipped with laser radar in the group of robots are respectively used as a main leader and two slave leaders, and the remaining robots are used as followers, and a communication topology graph with the main leader as the root node is constructed; Step 2: Establish a global coordinate system with the coordinates of the main leader as the origin, and obtain the environment map through the laser radar of the main leader; Step 3: Obtain the point cloud from the leader's lidar and obtain the leader's coordinates in the global coordinate system based on the environment map; Step 4: Obtain the expected positions of the master leader and the slave leader according to the environment map, the slave leader coordinates, the communication topology map, the nominal formation of the formation, and the stress coefficient of the affine formation; Step 5: The master leader and the slave leader track their respective desired positions in real time based on the optimized leader's control law; Step 6: Obtain the affine transformation matrix of the current follower's adjacent robot and estimate the affine transformation matrix of the current follower; calculate the ideal distance between the current follower and the adjacent robot according to the estimated affine transformation matrix and the position difference of the adjacent robot in the nominal configuration; Step 7: Obtain the relative position of the adjacent robot in the current follower's own coordinate system, and adjust the real-time distance between the current follower and the adjacent robot according to the ideal distance based on the follower formation control rate to achieve formation control.

2. The group robot formation control method based on hybrid measurement according to claim 1, characterized in that: The desired positions in steps 4 and 5 are: Where A(t) is the affine transformation matrix, b(t) is the translation matrix, r i is the nominal position of the leader robot node.

3. The group robot formation control method based on hybrid measurement according to claim 1 or 2, characterized in that: In step 5, the control law of the leader is: Where a, b, k1, k2 are control gains; p i is the actual position of the leader; μ(t) is a time-varying function; is the target formation at time t; ω ij is the stress coefficient.

4. The group robot formation control method based on hybrid measurement according to claim 1, characterized in that: In step 6, the affine transformation matrix of the current follower is estimated based on a consistency algorithm that converges within a preset time.

5. The group robot formation control method based on hybrid measurement as claimed in claim 4, characterized in that: The specific formula for the estimation is: In the formula, For the matrix The i-th column element of yes is the set of neighboring robots of the current follower; a ij is the weight of the connection between the current follower and the adjacent robot; α and β are control gains; μ1 is a time-varying function.

6. The group robot formation control method based on hybrid measurement according to claim 1, characterized in that: The follower formation control rate in step 7 is: Where a is the positive control gain, a>0; e ij is the distance error between follower i and adjacent robot j, sgn(·) is a sign function.

Images