A hybrid measurement based affine formation control method for swarm robots

Abstract

The application discloses a kind of group robot formation control method based on hybrid measurement, steps are as follows: step 1: select a main leader and two sub-leaders, construct the communication topology graph with main leader as root node;Step 2: with the coordinates of main leader as origin, establish global coordinate system, and obtain environment map;Step 3: obtain the coordinates of sub-leader in global coordinate system based on environment map;Step 4: obtain the desired position of main leader and sub-leader respectively;Step 5: main leader and sub-leader track the desired position of each based on the control law of optimized leader in real time;Step 6: calculate the ideal distance between current follower and adjacent robot;Step 7: based on follower formation control rate, adjust the real-time distance between current follower and adjacent robot, realize formation control.The application can obtain global coordinates without relying on GPS / GNSS, can use different measurement methods, improve the flexibility of system construction, and reduce cost.

Description

Technical Field

[0001] This invention relates to the field of swarm robot formation control technology, and in particular to a swarm robot formation control method based on hybrid measurement. Background Technology

[0002] With the development of technologies such as embedded systems, the Internet, and artificial intelligence, mobile robot control technology is becoming increasingly mature. Multi-robot collaboration is an effective method for accomplishing large-scale tasks in complex environments. By having multiple robots cooperate, work efficiency can be improved, and tasks that a single robot cannot complete can be accomplished. Formation control is one of the core technologies of swarm robot collaboration and is currently a hot topic.

[0003] When robots traverse areas with obstacles or narrow passages, they need to adjust their formation in real time according to the surrounding environment. Traditional formation control algorithms treat the entire formation as a rigid body, which cannot effectively realize formation transformations. In recent years, to solve the formation transformation problem, the academic community has proposed affine formation control algorithms. This algorithm calculates the target position of the robots based on a distributed stress matrix, enabling affine transformations of the formation, such as scaling, rotation, and translation. However, this algorithm requires at least some robots to obtain their absolute positions in the global coordinate system. In GPS / GNSS-denied environments such as tunnels and indoor spaces, it is difficult to obtain the absolute positions of the robots.

[0004] As the price of LiDAR drops rapidly, its applications are becoming increasingly widespread. For swarm robot systems, if all robots use LiDAR, the total cost will rise rapidly with the increase in the number of robots. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a swarm robot formation control method based on hybrid measurement to solve the technical problems of difficulty in obtaining absolute position and high cost in existing technologies.

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

[0007] Step 1: Construct a communication topology with the three robots equipped with LiDAR as the master leader and two as slave leaders, and the remaining robots as followers.

[0008] Step 2: Establish a global coordinate system with the coordinates of the main leader as the origin, and obtain an environmental map through the main leader's LiDAR;

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

[0010] Step 4: Based on the environment map, leader coordinates, communication topology, nominal formation, and stress coefficient of the affine formation, obtain the desired positions of the primary leader and secondary leaders.

[0011] Step 5: The primary leader and secondary leaders 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 neighboring robots, and estimate the affine transformation matrix of the current follower; calculate the ideal distance between the current follower and the neighboring robots based on the estimated affine transformation matrix and the position difference between the neighboring robots in the nominal configuration.

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

[0014] Furthermore, the desired positions in steps 4 and 5 are:

[0015]

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

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

[0018]

[0019] In the formula, a, b, k1, and k2 are control gains; p i The actual position of the leader; μ(t) is a time-varying function; Let ω be 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 consensus algorithm with preset time convergence.

[0021] Furthermore, the specific formula for the estimation is as follows:

[0022]

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

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

[0025]

[0026] In the formula, a is the positive control gain, and a>0; e ij Let i be the distance error between follower i and its neighboring robot j. sgn(·) is a function for positive and negative signs.

[0027] The beneficial effects of this invention are:

[0028] All robots in this invention do not rely on GPS / GNSS to obtain global coordinates, and the leaders and followers can use different measurement methods, improving the flexibility of system construction and helping to reduce costs. The algorithm proposed in this invention only requires calculating the stress coefficients between the three leaders during implementation, reducing the implementation difficulty; the leaders can converge to a specified position within a specified time; and when the formation topology changes, the stress matrix does not need to be recalculated. Attached Figure Description

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

[0030] Figure 1 This is a flowchart of a specific embodiment of the present invention. Detailed Implementation

[0031] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0032] The present invention will be further illustrated below with reference to specific embodiments. Those skilled in the art should understand that these embodiments are for illustrative purposes only and are not intended to limit the scope of the invention. Modifications to the present invention in various equivalent forms all fall within the scope defined by the appended claims.

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

[0034] Step S1: Assign three robots equipped with LiDAR to be a master leader and two to be slave leaders, with the remaining robots as followers. Construct a communication topology with the master 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, Indicates the nominal configuration, and in, This represents the set of nodes in the graph. Represent the set of edges in a graph; when the graph There is n l Each node acts as the leader, denoted as... The remaining nodes are followers, denoted as The robot adjacent to robot node i is 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 N robot nodes can be represented by a first-order integrator model:

[0037]

[0038] In the formula, u i This serves as 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 acquire the environment map using the main leader's LiDAR. The specific steps are as follows:

[0040] Step S21: The leader robot uses LiDAR to scan the surrounding environment and obtain environmental data of objects such as obstacles and walls. The leader robot converts the LiDAR scanning data into a global map through the SLAM algorithm and obtains its own position through the 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 point cloud data from the leader's LiDAR and acquire the leader's coordinates in the global coordinate system based on the environment map, specifically:

[0043] The leader scans its surroundings using LiDAR to obtain the relative distance and orientation between itself and reference points on the global map. The leader then matches the received map information with the local environmental information collected by its LiDAR to calculate its absolute position on the global map.

[0044] Step S4: Based on the environment map, leader coordinates, communication topology, nominal formation, and stress coefficients of affine formation, obtain the desired positions of the primary leader and secondary leaders.

[0045] Nominal formation set Must meet: exist It can be used to imitate Zhang Cheng, making It has universal rigidity and affine positioning.

[0046] 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 ​​as follows:

[0047]

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

[0049]

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

[0051] Based on the latest topology information, the leader robot performs an affine transformation on the nominal formation, adjusting its own target position in real time to control the positions of the followers, thereby controlling the formation of the entire group. The desired position of leader i is:

[0052]

[0053] In the formula, Let r be the affine transformation matrix. i The nominal location of the leader robot node. Let A(t) and b(t) be translation matrices, both of which are continuous with respect to time t. The primary leader and the two secondary leaders can obtain their respective desired positions using the aforementioned formula.

[0054] Based on the position information of each robot in the formation, the 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 transformations; the leader coordinates the relative motion of the followers and helps adjust the local structure of the formation.

[0055] Step 5: The primary leader and secondary leaders 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 actual system's response speed and performance, 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 for the control system to converge to the desired position is within the preset time range [t0, T]. The expression for the time-varying function μ(t) is:

[0057]

[0058] In the formula, ρ>0 represents the control gain.

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

[0060]

[0061] In the formula, a, b, k1, and k2 are control gains; p i The actual position of the leader; μ(t) is a time-varying function; Let ω be 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 pre-defined time-converged consensus algorithm. Based on the estimated affine transformation matrix Calculate the ideal distance between the current follower and its neighboring robots based on the positional difference between the current follower and the neighboring robots in the nominal configuration;

[0063] Affine transformation matrix For a dimension of m×n, the affine transformation matrix is ​​obtained through vectorization operations. It can be expanded into a column vector at the same time It can also be converted into Their relationship is as follows:

[0064]

[0065] In the formula, Representation matrix The element in the i-th column.

[0066] The follower robot adjusts the estimates of its state variables using a pre-defined time-converged consensus algorithm. Estimates of translation variables This gradually brings it closer to aligning with the affine transformation and translation of the leader, as shown in the following formula:

[0067]

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

[0069] By combining the optimized leader's control law and The estimation method realizes the swarm robot system represented by the dynamic model and the affine transformation of A(t) to control the motion, while also allowing the estimated value to converge to the true value, that is, as t→∞, we have

[0070] This will converge to the true value. Transform into With respect to the follower's own position p i It is possible to obtain the followers and neighboring robots p j The ideal distance between them is:

[0071]

[0072] Step 7: Obtain the relative position of adjacent robots in the current follower's own coordinate system, and adjust the real-time distance between the current follower and adjacent robots 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 neighboring robots in its own coordinate system; the coordinates of follower i are defined as follows: The coordinates of the adjacent robots are The distance error between follower i and its neighboring robot j is:

[0074]

[0075] In the formula, It is the actual distance between two adjacent robots; 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 expression for the squared difference of distance error between robots is:

[0077]

[0078] To achieve the target formation, the follower uses an optimized follower formation control rate to control its relative distance to adjacent robots in real time, ensuring that the relative distance between robots remains within a preset range. min ,e max ],

[0079] Follower group control rate:

[0080]

[0081] In the formula, a is the positive control gain, and a>0; sgn(·) is a function for positive and negative signs.

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

Claims

1. A swarm robot formation control method based on hybrid measurement, characterized in that, The method comprises the following steps: Step 1: three robots equipped with laser radars in the swarm robot are respectively taken as a master leader and two slave leaders, and the rest of the robots are taken as followers, and a communication topology graph with the master leader as a root node is constructed; Step 2: a global coordinate system is established with the coordinates of the master leader as an origin, and an environment map is obtained through the laser radar of the master leader; Step 3: point clouds are obtained through the laser radars of the slave leaders, and coordinates of the slave leaders in the global coordinate system are obtained based on the environment map; Step 4: expected positions of the master leader and the slave leaders are obtained according to the environment map, the coordinates of the slave leaders, the communication topology graph, a nominal formation of the formation and stress coefficients of the affine formation; Step 5: the master leader and the slave leaders track the expected positions of the leaders in real time based on optimized control laws of the leaders, wherein the optimized control laws of the leaders are: ； In the formula, , , , is a control gain; is the actual position of the leader; is a time-varying function; is the target formation at time t; is a stress coefficient; is the leader in the formation; Step 6: an affine transformation matrix of a neighboring robot of a current follower is obtained, and an affine transformation matrix of the current follower is estimated; an ideal distance between the current follower and the neighboring robot is calculated according to a position difference between the estimated affine transformation matrix and a position of the neighboring robot in a nominal configuration; Step 7: a relative position of the neighboring robot in a coordinate system of the current follower is obtained, and a real-time distance between the current follower and the neighboring robot is adjusted according to the ideal distance based on a formation control rate of the follower, so that formation control is realized. 2.The hybrid measurement based swarm robot formation control method of claim 1, wherein, The expected positions in steps 4 and 5 are: ； wherein is an affine transformation matrix, ; is a translation matrix, ; is the nominal position of the leader robot node. 3.The hybrid measurement based swarm robot formation control method of claim 1, wherein, In step 6, the affine transformation matrix of the current follower is estimated based on a preset time convergence consistency algorithm. 4.The hybrid measurement based swarm robot formation control method of claim 3, wherein, A specific formula of the estimation is: ； wherein is the matrix is the element of the column of the matrix ; is the set of neighboring robots of the current follower; is the weight of the connection between the current follower and a neighboring robot; , is the control gain; is the time-varying function; is the real value obtained by convergence. 5.The hybrid measurement based formation control method of swarming robots according to claim 1, wherein, The formation control rate of the follower in step 7 is: ； wherein is a positive control gain, ; ; ; is a follower and a neighboring robot distance error, ; is a sign function; is a square difference of distance error between robots; is a coordinate of a follower ; is a coordinate of a neighboring robot; is an actual distance between two neighboring robots; is an ideal distance between two neighboring robots; is an upper and lower bound of distance error between robots.

Images