Disclosure of Invention
Aiming at the defects in the prior art, the invention provides a group robot formation control method based on mixed measurement, which aims to solve the technical problems of difficult acquisition of absolute positions and high cost in the prior art.
The invention provides a group robot formation control method based on mixed measurement, which comprises the following steps:
Step 1, three robots provided with laser radars in a group of robots are respectively used as a master leader and two slave leaders, the other robots are used as followers, and a communication topological graph taking the master leader as a root node is constructed;
Step 2, a global coordinate system is established by taking the coordinates of a main leader as an origin, and an environment map is obtained through a laser radar of the main leader;
Step 3, acquiring a point cloud from a laser radar of a leader, and acquiring coordinates of the leader in a global coordinate system based on an environment map;
Step 4, acquiring respective expected positions of a master leader and a slave leader according to the environment map, the slave leader coordinates, the communication topological graph, the formed nominal formation and the affine formation stress coefficient;
Step 5, the master leader and the slave leader track the respective expected positions in real time based on the optimized control law of the leader;
Step 6, obtaining an affine transformation matrix of the adjacent robot of the current follower, and estimating the affine transformation matrix of the current follower; calculating 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;
and 7, acquiring the relative position of the adjacent robot under the own coordinate system of the current follower, and adjusting the real-time distance between the current follower and the adjacent robot according to the ideal distance based on the formation control rate of the follower to realize formation control.
Further, the desired positions in the steps 4 and 5 are:
wherein A (t) is an affine transformation matrix, B (t) is a translation matrix,R i is the nominal position of the leader robot node.
Further, in the step 5, the control law of the leader is:
Where a, b, k 1、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, and omega ij is the stress coefficient.
Further, in the step 6, an affine transformation matrix of the current follower is estimated based on a consistency algorithm converged in a preset time.
Further, the specific formula of the estimation is:
In the formula, Is a matrixIs selected from the group consisting of the elements of the ith column,Is thatIs the set of adjacent robots of the current follower, a ij is the weight of the connection between the current follower and the adjacent robot, α, β is the control gain, μ 1 is the time-varying function.
Further, in the step 7, the follower formation control rate is as follows:
Wherein a is positive control gain, a >0; e ij is the distance error between follower i and the neighboring robot j, Sgn (·) is a sign function.
The invention has the beneficial effects that:
according to the invention, all robots do not need to rely on GPS/GNSS to acquire global coordinates, and a leader and a follower can adopt different measurement modes, so that the flexibility of system construction is improved, and the cost is reduced. The algorithm provided by the invention only needs to calculate the stress coefficients among three leaders in the implementation process, so that the implementation difficulty is reduced, the leaders can converge to the designated positions at the designated time, and the stress matrix is not required to be recalculated when the topological structure of the formation changes.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to fall within the scope of the invention.
The application will be further elucidated with reference to specific examples. It will be appreciated by those skilled in the art that these examples are intended to illustrate the application and not to limit the scope of the application, and that various equivalent modifications to the application fall within the scope of the application as defined by the appended claims.
As shown in fig. 1, the invention provides a group robot formation control method based on hybrid measurement, which comprises the following steps:
Step S1, three robots provided with laser radars in a group of robots are respectively used as a master leader and two slave leaders, the other robots are used as followers, and a communication topological graph with the master leader as a root node is constructed, wherein the specific steps are as follows:
Step S11, defining topological relation between robots as a graph Nominal formation is thenWherein, Represents a nominal configuration, andWherein, Representing a collection of nodes in the graph,Representing the collection of edges in the graph, when the graphIn which n l nodes are taken as leaders and marked asThe remaining nodes are followers, denoted asThe robot adjacent to the robot node i is denoted asIn formation of teamIs divided into leadersAnd followerThe state vector of robot i is denoted by p i.
Step S12, the dynamics model of N robot nodes can be represented by a first-order integrator model:
In the formula, U i is the control input to the robot.
Step S2, a global coordinate system is established by taking the coordinates of a main leader as an origin, and an environment map is acquired through a laser radar of the main leader, wherein the method comprises the following specific steps of:
Step S21, a main leader robot scans the surrounding environment by using a laser radar to acquire environment data of objects such as obstacles, walls and the like, the main leader converts the scanning data of the laser radar into a global map by using a SLAM algorithm, and the main leader acquires the position of the main leader by using a matching algorithm;
and S22, the master leader transmits the constructed map to the slave leader through the wireless communication module.
Step S3, acquiring a point cloud from a laser radar of a leader, and acquiring coordinates of the leader in a global coordinate system based on an environment map, wherein the coordinates are specifically as follows:
the method comprises the steps that a leader scans the surrounding environment of the leader through a laser radar to obtain the relative distance and the direction between the leader and a reference point in a global map, the leader matches the received map information with local environment information acquired by the laser radar of the leader, and the absolute position of the leader in the global map is calculated.
S4, acquiring respective expected positions of a master leader and a slave leader according to an environment map, slave leader coordinates, a communication topological graph, a nominal formation and stress coefficients of an affine formation;
Nominal formation of settings The following must be satisfied: At the position of Can be affine stretched to makeWith general rigidity and affine positioning,
Queuing the nominal valuesEach edge (i, j) in the nominal formation corresponds to a stress weightThis stress may be positive or negative, the stress matrix being:
Affine mapping of nominal formations is defined as:
the affine transformation matrix a (t) and the translation matrix b (t) of the leader can be obtained by affine mapping of the nominal formation.
Affine transformation is carried out on the nominal formation by the leader robot according to the latest topological information, the target position of the leader robot is adjusted in real time to control the position of the follower, the formation of the whole formation is further controlled, and the expected position of the leader i is as follows:
In the formula, For the affine transformation matrix, r i is the nominal position of the leader robot node,For the translation matrix, a (t) and b (t) are both continuous for time t. The master leader and the two slave leaders can acquire the respective expected positions through the expected position formulas.
The master leader calculates affine matrix parameters to be adjusted according to the environment and task requirements based on the position information of each robot in the formation, sends out instructions to control the formation to carry out affine transformation, and the leader coordinates the relative movement of the followers and helps to adjust the local structure of the formation.
Step 5, the master leader and the slave leader track the expected position of the target formation in real time based on the optimized control law of the leader;
when the leader robot tracks its own desired position, it is necessary to ensure that the system reaches the desired position of the formation within a specific time in consideration of the response speed and performance of the actual system. To ensure that the leader robot can reach the desired location within a specified time, a time-varying function is introduced, and the time T at which the control system converges to the desired location can be within a range of preset times [ T 0, T ], the time-varying function μ (T) being expressed as:
Where ρ >0 is the control gain.
After adding the time-varying function to the control law of the leader, the optimized control law of the leader is as follows:
Where a, b, k 1、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, and omega ij is the stress coefficient.
Step 6, obtaining an affine transformation matrix of the adjacent robot of the current follower, and estimating the affine transformation matrix of the current follower based on a consistency algorithm converged in preset timeFrom estimated affine transformation matrixCalculating an ideal distance between the current follower and the adjacent robot according to the position difference between the current follower and the adjacent robot in the nominal configuration;
affine transformation matrix Dimension is m×n, affine transformation matrix is obtained by vectorization operationCan be expanded into column vectorsAt the same timeCan also be converted intoThe relationship is as follows
In the formula,Representation matrixIs the i-th column element of (c).
The follower robot adjusts the estimated value of the state variable through a consistency algorithm converged in preset timeAnd an estimate of the translation variableMaking it gradually trend to coincide with affine transformation and translation of the leader, the specific formula is as follows:
Wherein is of the formula Is the set of adjacent robots of the current follower, a ij is the weight of the connection between the current follower and the adjacent robot, α, β is the control gain, μ 1 is the time-varying function.
By combining the control law sum of the use-optimized leaderThe estimation method realizes the group robot system represented by the dynamics model and the affine transformation control motion of A (t), and can simultaneously enable the estimation value to be converged to the true value, namely, when t is in the range of [ t ]. Fwdarw ]
Will obtain convergence to a true valueConversion intoThe ideal distance between the follower and the adjacent robot p j can be obtained from the follower self position p i:
and 7, acquiring the relative position of the adjacent robot under the own coordinate system of the current follower, and adjusting the real-time distance between the current follower and the adjacent robot according to the ideal distance based on the formation control rate of the follower to realize formation control.
The follower i uses the sensor to measure the coordinates of the adjacent robot under the own coordinate system of the follower i, and the coordinates of the follower i are defined asThe coordinates of adjacent robots areThe distance error between follower i and the adjacent robot j is:
In the formula, Is the actual distance between two adjacent robots; the distance error between the robots is up and down limited as [ e min,emax ].
The square difference of the distance error between robots is expressed as:
In order to enable the formation to reach the target formation, the follower adopts the optimized follower formation control rate to control the relative distance between the follower formation control rate and the adjacent robots in real time, so that the relative distance between the robots is kept in a preset range [ e min,emax ],
The follower formation control rate is:
Wherein a is positive control gain, a >0; sgn (·) is a sign function.
Although embodiments of the present invention have been described in connection with the accompanying drawings, various modifications and variations may be made by those skilled in the art without departing from the spirit and scope of the invention, and such modifications and variations are within the scope of the invention as defined by the appended claims.