CN114637301B - Multi-robot dynamic obstacle avoidance device and method based on optimal affine formation transformation - Google Patents
Multi-robot dynamic obstacle avoidance device and method based on optimal affine formation transformation Download PDFInfo
- Publication number
- CN114637301B CN114637301B CN202210291561.6A CN202210291561A CN114637301B CN 114637301 B CN114637301 B CN 114637301B CN 202210291561 A CN202210291561 A CN 202210291561A CN 114637301 B CN114637301 B CN 114637301B
- Authority
- CN
- China
- Prior art keywords
- robot
- formation
- gradient
- time
- estimator
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 126
- PXFBZOLANLWPMH-UHFFFAOYSA-N 16-Epiaffinine Natural products C1C(C2=CC=CC=C2N2)=C2C(=O)CC2C(=CC)CN(C)C1C2CO PXFBZOLANLWPMH-UHFFFAOYSA-N 0.000 title claims abstract description 53
- 230000009466 transformation Effects 0.000 title claims abstract description 36
- 238000000034 method Methods 0.000 title claims description 27
- 239000011159 matrix material Substances 0.000 claims abstract description 43
- 238000013461 design Methods 0.000 claims abstract description 21
- 238000004422 calculation algorithm Methods 0.000 claims description 16
- 230000004888 barrier function Effects 0.000 claims description 13
- 230000033001 locomotion Effects 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 8
- 230000011218 segmentation Effects 0.000 claims description 3
- 238000005755 formation reaction Methods 0.000 description 108
- 238000005457 optimization Methods 0.000 description 32
- 238000010586 diagram Methods 0.000 description 12
- 238000004891 communication Methods 0.000 description 8
- 230000008859 change Effects 0.000 description 6
- 239000003795 chemical substances by application Substances 0.000 description 4
- 230000003993 interaction Effects 0.000 description 4
- 238000013519 translation Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000012887 quadratic function Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 241000251468 Actinopterygii Species 0.000 description 1
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The invention provides a multi-robot dynamic obstacle avoidance device based on optimal affine formation transformation, which comprises a gradient tracking module, wherein the gradient tracking module comprises a gradient tracking variable updating submodule and a feedforward feedback control module; the gradient tracking variable update subA module for calculating the gradient tracking variable y according to a preset update law i So that t ≧ t for all times 0 The weighted sum of the gradient tracking variables and the local cost function gradients with respect to the configuration matrix column vector is always equal, t 0 Represents a fixed time; and the feedforward feedback control module calculates the control input based on a control model which takes the output of the fixed time speed estimator as feedforward and takes affine formation constraint error and gradient tracking variable as feedback design, and controls the formation of the robot based on the control input. Based on the device, the robot formation can be ensured to be converged to the time-varying optimal solution track rapidly without error.
Description
Technical Field
The invention belongs to the technical field of multi-agent control, and particularly relates to a multi-robot dynamic obstacle avoidance device and method based on optimal affine formation transformation.
Background
With the rapid development of science and technology at home and abroad and the increasing demand of human beings, the traditional working field is gradually replaced by robots represented by unmanned vehicles, unmanned planes and the like. Because a single robot has great limitations in the aspects of information acquisition, processing, control and the like, people begin to widely research the distributed formation control problem of multiple robots under the inspiration of natural phenomena such as bird groups, fish groups and the like. Through the connection of a communication network and the structural decomposition of a controller, local information interaction and smaller operation burden can be utilized among all robots, and the cooperative control can be realized in a self-organizing way, so that the working efficiency and robustness are effectively improved, and the working range and flexibility are enlarged.
The traditional multi-robot distributed formation control technology is generally consistent formation control based on relative positions, only a few leader robots can obtain reference information of a desired formation, and other multi-robots need to measure relative positions with adjacent robots, finally and accurately reach the desired formation through communication and interaction, so that target formation is realized. However, this method requires setting desired formation information for each robot in advance, and therefore only a fixed target formation can be achieved, and it is not possible to avoid obstacles in the work environment by formation change.
In order to realize obstacle avoidance through formation transformation, the document (Mohammad Deghat, brian DO Anderson, zhiyun Lin. Combined filling and distance-based shape Control of multi-agent formation [ J ]. IEEE Transactions on Automatic Control,2016,61 (7): 1824-1837.) expresses a desired formation by using relative distance between robots, and formation is realized by introducing rigid graph theory to ensure uniqueness of the formation and further controlling the robots to reach the desired relative distance. The method can realize the translation and rotation transformation of the formation, but cannot realize the transformation such as scaling. The document (Shiyu ZHao, daniel Zelazo. Bearing rigidity and movement global bearing-only formation stabilization [ J ]. IEEE Transactions on Automatic Control,2016,61 (5): 1255-1268.) uses the relative angle between robots to represent the desired formation, and uses the angular rigidity condition and the distance change between two leader robots to implement a variety of different formations. The method can only realize the translation and the scaling of the formation, and can not realize the transformation such as rotation and the like. The document (Shiyu zhao. Affinition maneuver Control of multi-agent systems [ J ], IEEE Transactions on Automatic Control,2018,63 (12): 4140-4155.) proposes that a generalized Laplacian matrix with positive or negative weights realizes angle-based formation Control, and proves that the method can realize all affine formation transformations such as translation, scaling, rotation and the like, but more than 3 leader robots are required to obtain the desired formation positions to realize the unique formation transformation track. The formation transformation methods are all realized based on the relative position of the leader robot, and the leader robot is required to obtain a given expected formation transformation track in advance. For a dynamic and unknown obstacle environment, each robot can only carry out local observation on the obstacle, the global information of the obstacle cannot be obtained usually, and the expected formation transformation track cannot be given in advance.
In order to consider a dynamic and unknown obstacle environment, a document (Yi X, li X, xie L, et al, distributed online coordinated optimization with time-varying simultaneous optimization constraints [ J ]. IEEE Transactions on Signal Processing,2020, 68-746) establishes an optimization index as a time-varying cost function, and solves a time-varying optimization problem on line by using an original dual mirror descent algorithm to obtain a formation transformation trajectory, but a steady-state error exists between the obtained trajectory and a solution of the optimization problem. In order to eliminate steady-state errors, the literature (Sun C, ye M, hu G. Distributed time-varying quadratic optimization for multiple agents under direct graphs [ J ]. IEEE Transactions on Automatic Control,2017,62 (7): 3687-3694) proposes a robust gradient descent algorithm based on a smooth penalty function, but such an algorithm has certain requirements on the type of cost function, is only suitable for quadratic functions or strong convex functions with the same Hessian matrix, and cannot deal with time-varying safety constraints.
Therefore, how to utilize local observation information of each robot to the obstacle and affine formation transformation requirements, an optimization problem with time-varying cost function and time-varying safety constraint is reasonably designed, and a distributed time-varying optimization algorithm is provided to adjust on line and realize expected formation transformation to complete dynamic obstacle avoidance, which is a problem to be solved urgently.
Disclosure of Invention
In view of this, the invention provides a multi-robot dynamic obstacle avoidance device and method based on optimal affine formation transformation.
The technical scheme for realizing the invention is as follows:
on one hand, the multi-robot dynamic obstacle avoidance device based on the optimal affine formation transformation comprises a gradient tracking module, wherein the gradient tracking module comprises a gradient tracking variable updating sub-module and a feedforward feedback control module;
the gradient tracking variable updating submodule is used for calculating the gradient tracking variable y according to a preset updating law i So that t ≧ t for all times 0 The weighted sum of the gradient tracking variables and the local cost function gradients with respect to the configuration matrix column vector is always equal, t 0 Represents a fixed time;
and the feedforward feedback control module calculates the control input based on a control model which takes the output of the fixed time speed estimator as feedforward and takes affine formation constraint error and gradient tracking variable as feedback design, and controls the formation of the robot based on the control input.
Further, the control model of the present invention is:
wherein z is ii Representing the output of the estimator for the ith robot,
representing affine formation constraint error, ω ij Is the stress weight, x, between adjacent robots i, j i ,x j Is the position of the robot i, j->
Is the neighbor set of the ith robot and alpha represents the control gain.
Further, the update law of the present invention is:
wherein f is i (x i T) a local time-varying cost function for the ith robot,
denotes f i (x i T) for x i Is greater than or equal to>
The derivative of the position of the ith robot.
Further, the affine formation constraint of the present invention is:
wherein Ω is a stress matrix according toEach pair of neighboring robots (i, j) is assigned a scalar stress weight ω ij The determination is as follows: />
Wherein, I d Is a d-dimensional identity matrix, and x is the position coordinates of all robots.
Further, the local time-varying cost function f of the ith robot is provided by the invention i (x i And t) is: a weighted sum of the ith robot logarithmic barrier function and the trajectory error function.
Further, the process of establishing the trajectory error function of the present invention is: for the ith robot, the given expected motion track is
Where t is time based on the current position coordinates x i And constructing a track error function.
Further, the establishing process of the barrier function of the present invention is: based on the position of the obstacle measured by the sensor of the robot, calculating a segmentation hyperplane tangent to each obstacle and vertical to the connecting line of the robot and the obstacle, expressing the intersection of all hyperplanes in the area at one side of the robot as a group of time-varying linear inequality constraints, and establishing a logarithmic barrier function based on the constraints.
Further, the invention designs an estimator state H for the ith robot i And ζ i Respectively used to estimate the gradient of a global time-varying cost function
Derivatives on x and t, i.e. [ or ] H>
And &>
The design of the fixed time estimator is divided into two parts:
the first part is estimator state updating law, corresponding to local time-varying cost function available for each robot
And &>
As reference information, the estimator state of each robot with respect to all other robots is updated using a discontinuous consistency tracking algorithm such that
The second part is the estimator output, the design estimator output is z i ＝-P(P T H i P) -1 P T ζ i Wherein P is a configuration matrix of the robot.
Further, the configuration matrix P and the stress matrix Ω of the present invention satisfy the following conditions:
(a) The stress matrix is semi-positive and the rank is n-d-1.
(b)ΩP＝PΩ＝0。
(c) The null space of the matrix omega is equal to the column space of the configuration matrix P.
On the other hand, the invention discloses a multi-robot dynamic obstacle avoidance method based on optimal affine formation transformation, which comprises the following specific processes:
calculating the gradient tracking variable y according to a preset updating law i So that t ≧ t for all times 0 The weighted sum of the gradient tracking variables and the local cost function gradients with respect to the configuration matrix column vector is always equal, t 0 Represents a fixed time;
and calculating the control input based on a control model which takes the output of the fixed time speed estimator as feedforward, affine formation constraint error and gradient tracking variable as feedback design, and controlling the formation of the robot based on the control input.
Has the beneficial effects that:
the method is based on gradient tracking variable updating, a control model which is based on the design that the output of a fixed time speed estimator is used as feedforward, affine formation constraint error and gradient tracking variable are used as feedback is constructed, and the formation of the robot is controlled through the control model, so that the formation of the robot can be ensured to be converged to a time-varying optimal solution track quickly without error.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
FIG. 1 is a schematic diagram of a multi-robot formation system and a working scenario in an embodiment of the invention;
FIG. 2 is a schematic diagram of a multi-robot formation framework in accordance with an embodiment of the present invention;
fig. 3 (a) is a schematic diagram of multi-robot formation change obstacle avoidance at 20s according to the embodiment of the present invention;
fig. 3 (b) is a schematic diagram of multi-robot formation change obstacle avoidance at time 80s in the embodiment of the present invention;
fig. 3 (c) is a schematic diagram of multi-robot formation change obstacle avoidance at 120s according to the embodiment of the present invention;
fig. 3 (d) is a schematic diagram of multi-robot formation change obstacle avoidance at 140s according to the embodiment of the present invention;
FIG. 4 is a schematic diagram of the state error of the multi-robot stationary time estimator in accordance with the present invention;
FIG. 5 is a diagram illustrating an optimal formation condition and a cost function error of multiple robots according to an embodiment of the present invention;
FIG. 6 is a schematic diagram of a trajectory tracking error of an optimal solution of multiple robots in an embodiment of the present invention.
Detailed Description
The invention is further illustrated with reference to the following figures and examples:
it should be noted that, in the case of no conflict, the features in the following embodiments and examples may be combined with each other; moreover, all other embodiments that can be derived by one of ordinary skill in the art from the embodiments disclosed herein without making any creative effort fall within the scope of the present disclosure.
It is noted that various aspects of the embodiments are described below within the scope of the appended claims. It should be apparent that the aspects described herein may be embodied in a wide variety of forms and that any specific structure and/or function described herein is merely illustrative. Based on the disclosure, one skilled in the art should appreciate that one aspect described herein may be implemented independently of any other aspects and that two or more of these aspects may be combined in various ways. For example, an apparatus may be implemented and/or a method practiced using any number of the aspects set forth herein. Additionally, such an apparatus may be implemented and/or such a method may be practiced using other structure and/or functionality in addition to one or more of the aspects set forth herein.
The embodiment of the invention provides a multi-robot dynamic obstacle avoidance device based on optimal affine formation transformation, which comprises a gradient tracking module, wherein the gradient tracking module comprises a gradient tracking variable updating sub-module and a feedforward feedback control module;
a gradient tracking variable updating submodule for calculating the gradient tracking variable y according to a preset updating law i (ii) a So that t ≧ t for all times t 0 The weighted sum of the gradient tracking variable and the local cost function gradient with respect to the configuration matrix column vector is always equal;
and the feedforward feedback control module is used for calculating the control input according to the control model based on the control model which takes the output z of the fixed time speed estimator as feedforward and takes the affine formation constraint error and the gradient tracking variable y as feedback design, and controlling the formation of the robot based on the control input.
The multi-robot dynamic obstacle avoidance method based on the optimal affine formation transformation comprises the following steps:
step 1: the dynamics of the robot and the formation framework are modeled.
The robot dynamics adopts a single integrator model for modeling, and describes the motion state of the node through a first-order differential equation. Modeling formation frame of robot as a binary group
Wherein->
In a directionless communication topology, i.e. two-way communication between neighboring robots can be made, and &>
In formation, d i For the relative position of the ith robot with respect to the center of the formation, the corresponding configuration matrix +>
Step 2: and distributing stress weight, and designing a stress matrix and affine formation constraint.
Formation-based framework
Is undirected graph>
Assigning a scalar stress weight ω to each pair of neighboring robots (i, j) in (e) ij So that ω is ij ＝ω ji While ensuring that>
Wherein->
Is the neighbor set of the ith robot. The stress matrix is designed to
Wherein ω is ij And may be a positive number, a negative number, or 0. Affine formation constraint is designed as
Wherein
Position coordinates for all robots, d spatial dimension, I d Is a d-dimensional identity matrix.
And step 3: and designing a track error function and a logarithm barrier function, and constructing a time-varying optimization problem under the constraint of an affine formation.
For the ith robot, the given expected motion track is
Where t is time based on the current position coordinates x i And constructing a track error function. Based on the position of the obstacle measured by the sensor of the robot, calculating a segmentation hyperplane tangent to each obstacle and vertical to the connecting line of the robot and the obstacle, expressing the intersection of all hyperplanes in the area at one side of the robot as a group of time-varying linear inequality constraints, and establishing a logarithmic barrier function based on the constraints. In the finally constructed time-varying optimization problem, the local time-varying cost function f of the ith robot i (x i And t) is the weighted sum of the logarithmic barrier function and the trajectory error function of the robot, the global time-varying cost function f (x, t) is the sum of the local cost functions of all the robots, and the constraint condition is the affine formation constraint designed in the step 2.
Step four: and designing a fixed time speed estimator, and predicting the derivative of the solution track of the optimization problem to the time, namely estimating the derivative of the expected motion track.
For the ith robot, the estimator state H is designed i And ζ i Respectively used to estimate the gradient of a global time-varying cost function
Derivative of x and t, i.e. [ alpha ]>
And &>
The design of the fixed time estimator is divided into two parts. The first part is estimator state updating law, corresponding to local time-varying cost function available for each robot
And &>
As reference information, the estimator state of each robot with respect to all other robots is updated using a discontinuous consistency tracking algorithm such that
Wherein t is 0 Is a fixed time instant. The second part is the estimator output, the design estimator output is z i ＝-P(P T H i P) -1 P T ζ i Wherein P is the configuration matrix defined in step 1.
Step five: and designing a gradient tracking algorithm based on a speed estimator, solving a time-varying optimization problem in a distributed mode, and realizing the target formation transformation which minimizes a time-varying cost function and meets the constraint of a stress matrix.
Designing a gradient tracking variable y for the ith robot i Let us order
Wherein t is 0 Is the fixed time defined in step 4. The design of the gradient tracking algorithm based on a velocity estimator is divided into two parts. The first part is the gradient tracking variable update law, so that t ≧ t for all times 0 Addition of gradient tracking variables and local cost function gradients to the column vector of the configuration matrixThe sum of weights is always equal. The second part is a feedforward feedback control law, the output z of the estimator in the step 4 is used as feedforward and affine formation constraint error->
And the gradient tracking variable y is used as feedback, so that the affine formation constraint error and the gradient weighted sum of the local cost function are converged to 0 at the same time, and then the condition of being greater than or equal to 0 is ensured>
Wherein
Is the solution trajectory of the time-varying optimization problem in step 3, and t is time.
In the embodiment, for the time-varying optimal solution trajectory in the time-varying optimization problem, a fixed time estimator is adopted to predict the solution trajectory speed, the output of the estimator is introduced to serve as feedforward, a gradient tracking variable is designed to compensate errors caused by the gradient weighted sum of the local cost function, and the affine formation constraint error and the gradient tracking variable are used for feedback control, so that the robot formation can be rapidly converged on the time-varying optimal solution trajectory without errors.
By solving the distributed time-varying optimization problem, the expected formation can be adjusted on line by using local observation information of each robot aiming at dynamic and unknown obstacle constraints in the environment, obstacle avoidance can be realized through optimal affine formation transformation, and a leader robot and an expected formation transformation track do not need to be given in advance.
In the embodiment, each robot can realize real-time transformation of the whole formation only by utilizing self-position information and communication interaction with the adjacent robot, global information interaction is not needed, system communication and calculation burden are reduced, and stronger survivability and robustness are achieved compared with centralized control.
In this embodiment, the stress matrix in step two and the configuration matrix in step one satisfy the following conditions:
(d) The stress matrix is semi-positive and has a rank of n-d-1.
(e)ΩP＝PΩ＝0。
(f) The null space of the matrix omega is equal to the column space of the configuration matrix P.
In this embodiment, the time-varying optimization problem under the affine formation constraint described in step three satisfies the following conditions:
(a) The locally time-varying cost function of each robot is composed of a weighted sum of a logarithmic barrier function and a trajectory error function, wherein the trajectory error function is a strongly convex function but not necessarily a quadratic function. Furthermore, the Hessian matrices for both the logarithmic barrier function and the trajectory error function may be different from one robot to another.
(b) The expected motion trajectory given by each robot and the locally observed obstacles are dynamically time-varying, and the global cost function is the sum of all local cost functions, so that the global time-varying cost function is the time-varying function of all robot states, and the solution trajectory of the corresponding optimization problem is a coupling term about all robot position information, cannot be obtained by a single robot, and is difficult to solve in a distributed manner.
In yet another embodiment of the present application, a formation control system comprising 6 robots is constructed, and a working scenario comprising a plurality of static and dynamic obstacles is constructed, as shown in fig. 1, wherein a rectangle represents the formation of the robots, a circle represents the static obstacles and the dynamic obstacles, and a dotted line represents a time-varying safety zone calculated by the robots.
The embodiment establishes a derivation process of the control law of robot formation: by utilizing a stress matrix and a rigid formation theory, affine formation constraint is introduced into an optimization problem with a time-varying cost function and time-varying safety constraint, and a solution track of the optimization problem is constructed into a time-varying affine formation transformation track of a given formation configuration. In order to process the time-varying terms in the cost function and the safety constraint, the method designs a fixed time speed estimator to predict the optimal affine formation speed on line, thereby eliminating the steady-state tracking error caused by the time-varying terms. In order to solve the time-varying optimization problem in a distributed mode, the method designs a time-varying affine formation optimization algorithm based on weighted gradient tracking, and utilizes formation speed and weighted gradient estimation values as control feedforward and feedback respectively, so that balance points of system dynamics are guaranteed to meet optimality conditions of the time-varying optimization problem, and the multi-robot formation is converged on an optimal solution track of the time-varying optimization problem. The specific implementation process of the process is as follows:
the method comprises the following steps: modeling dynamics and formation framework of the robot;
if the number of the robots n =6, the single integrator dynamical model of the ith robot is
Wherein the content of the first and second substances,
is the position of the i-th robot>
Is the control input for the ith robot and d is the spatial dimension.
Modeling robot formation frame as a binary group
As shown in fig. 2, wherein +>
Is a directionless communication topology map, and>
for formation configuration, the corresponding configuration matrix->
In this case 6 robotsThe configuration matrix may specifically be:
the two-way communication relationship between the robots is shown by the connecting lines in fig. 2, and the positions of the robots relative to the centers of the formations are shown by the nodes in fig. 2.
Step two: and distributing stress weight, and designing a stress matrix and affine formation constraint.
Formation-based framework
Is undirected graph>
Each pair of neighboring robots (i, j) in (a) is assigned a scalar stress weight £ based>
As indicated by the values marked on the connecting lines in fig. 2, such that ω is ij ＝ω ji While ensuring
Wherein->
Is the neighbor set of the ith robot.
The stress matrix designed based on the stress weight is as follows:
in particular to
Affine formation constraint is designed as
Wherein->
For the position coordinates of all robots, d is the spatial dimension, I d For the d-dimensional unit matrix, based on the affine formation constraint design, the robot formation can be controlled in a required formation.
Step three: and designing a track error function and time-varying safety constraint, and constructing a time-varying optimization problem under the constraint of affine formation.
For the ith robot, the given expected motion track is
Where t is time and d is the spatial dimension. Based on the current position coordinate x of the ith robot i The trajectory error function is constructed as follows
Wherein, c i Is a constant.
Defining the position of the obstacle measured by the self-sensor of the ith robot
Wherein s is i Is the number of obstacles measured by the ith robot. Calculating a dividing hyperplane which is tangent to the obstacle and vertical to the connecting line of the robot and the obstacle, and expressing the intersection of all hyperplanes in the area on one side of the robot as a group of linear inequalities as follows
That is, the time-varying safety constraint based on which collision between the robot and the obstacle can be avoided.
Two-dimensional air spaceA between ik And b ik Is calculated by
Wherein x is *,1 And x *,2 Are respectively a position x * (including x) i And
) 1 and 2 components, i.e. position x * The abscissa and the ordinate.
Establishing a logarithmic barrier function based on time-varying security constraints
In the finally constructed time-varying optimization problem, the local time-varying cost function f of the ith robot i (x i T) is a weighted sum of the logarithmic barrier function and the trajectory error function of the robot, i.e.
f i (x i ,t)＝w i h i (x i ,t)+(1-w i )g i (x i ,t),
Wherein, 0<w i <1。
And (3) making a global time-varying cost function f (x, t) be the sum of local cost functions of all robots, wherein the constraint condition is an affine formation constraint designed in the step (2), and the obtained time-varying optimization problem under the affine formation constraint is as follows:
step four: and designing a fixed time speed estimator to predict the time derivative of the trajectory of the solution of the optimization problem.
Designing estimator states for the ith robot
And
are used to estimate the gradient of the global time-varying cost function, respectively>
Derivative of x and t, i.e. [ alpha ]>
And &>
The design of the fixed time estimator is divided into two parts. The first part is the estimator state update law, based on the local time-varying cost function available to each robot>
And &>
As reference information, a discontinuous consistency tracking algorithm is designed as follows
Wherein, beta, gamma>0，0<μ 1 <1<μ 2 For estimator parameter, sig (·) α ＝sgn(·)|·| α Is a continuous symbolic function. Updating the estimator state of each robot with respect to all other robots using the non-continuous consistency tracking algorithm described above such that
Wherein t is 0 Is a fixed time instant.
The second part is the estimator output
Design the estimator output as
z i ＝-P(P T H i P) -1 P T ζ i ,
Wherein P is the configuration matrix defined in step 1.
Step five: and designing a gradient tracking algorithm based on a speed estimator, solving a time-varying optimization problem in a distributed mode, and realizing the target formation transformation which minimizes a time-varying cost function and meets the constraint of a stress matrix.
Designing gradient tracking variables for the ith robot
Make/combine>
Wherein t is 0 Is the fixed time defined in step 4. The design of the gradient tracking algorithm based on a velocity estimator is divided into two parts.
The first part is a gradient tracking variable updating law which is designed as follows
The update law is such that t ≧ for all timest 0 The weighted sum of the gradient tracking variables and the local cost function gradients with respect to the configuration matrix column vector is always equal, i.e.
/>
The second part is a feedforward feedback control law, and the estimator output z in the step 4 is used as feedforward and affine formation constraint error
And the gradient tracking variable y as feedback, are designed as follows
Wherein z is ii Representing the estimator output for the ith robot (the solution trajectory of the ith robot in the time-varying optimization problem of the estimator output of the ith robot)
Derivative of).
The control law ensures that the gradient weighted sum of affine formation constraint error and local cost function is converged to 0 at the same time, thereby ensuring that
Wherein->
Is the solution trajectory of the time-varying optimization problem in step 3, and t is time.
The software simulation results of this example are given below to demonstrate the effectiveness of the invention.
As shown in fig. 3 (a) -3 (d), the four diagrams respectively show the results of 6 robots performing formation transformation obstacle avoidance at 20 th, 80 th, 120 th and 140 th based on the optimal affine formation transformation multi-robot dynamic obstacle avoidance method. From the figure, it can be found that, for the formation control system composed of the 6 robots, the desired formation shape can be adjusted and realized on line through a distributed time-varying optimization algorithm to complete dynamic obstacle avoidance.
Fig. 4 is a state error diagram of a fixed time estimator in the embodiment of the present invention, and fig. 5 and fig. 6 are a diagram of a formation transformation control error and an optimal solution trajectory tracking error of a gradient tracking algorithm based on a velocity estimator, respectively, it can be seen that the fixed time of the estimator state error in fig. 4 converges to 0, and the optimal formation condition and the cost function error in fig. 5 converge to 0, which ensures that the finally formed affine formation is a solution of the time-varying optimization problem. The optimal solution trajectory tracking error index in fig. 6 converges to 0, so that the multi-robot system can rapidly realize the optimal affine formation in real time, and the effectiveness of the method is proved.
The above description is only a preferred embodiment of the present invention, and is not intended to limit the scope of the present invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (7)
1. A multi-robot dynamic obstacle avoidance device based on optimal affine formation transformation is characterized by comprising a gradient tracking module, wherein the gradient tracking module comprises a gradient tracking variable updating sub-module and a feedforward feedback control module;
the gradient tracking variable updating submodule is used for calculating the gradient tracking variable y according to a preset updating law i So that t ≧ t for all times 0 The weighted sum of the gradient tracking variables and the local cost function gradients with respect to the configuration matrix column vector is always equal, t 0 Represents a fixed time;
the feedforward feedback control module calculates the control input based on a control model which takes the output of the fixed time speed estimator as feedforward and takes affine formation constraint error and gradient tracking variable as feedback design, and controls the formation of the robot based on the control input;
the control model is as follows:
wherein z is ii Representing the output of the estimator for the ith robot,
representing affine formation constraint error, ω ij Is the stress weight, x, between adjacent robots i, j i ,x j For the position of robot i, j>
Is the neighbor set of the ith robot, and alpha represents the control gain;
the update law is as follows:
wherein, f i (x i T) a local time-varying cost function for the ith robot,
denotes f i (x i T) for x i Is greater than or equal to>
A derivative of the position of the ith robot;
the affine formation constraint is as follows:
wherein omega is a stress matrix, and a scalar stress weight omega is distributed to each pair of neighboring robots (i, j) ij The determination is as follows:
wherein, I d Is a d-dimensional identity matrix, and x is the position coordinates of all robots.
2. The optimal affine formation transformation-based multi-robot dynamic obstacle avoidance device according to claim 1, wherein the local time-varying cost function f of the ith robot i (x i And t) is: a weighted sum of the ith robot logarithmic barrier function and the trajectory error function.
3. The optimal affine formation transformation-based multi-robot dynamic obstacle avoidance device according to claim 2, wherein the track error function is established by the following steps: for the ith robot, the given expected motion trajectory is
Where t is time based on the current position coordinate x i A trajectory error function is constructed.
4. The optimal affine formation transformation-based multi-robot dynamic obstacle avoidance device according to claim 2, wherein the barrier function is established by the following steps: based on the position of the obstacle measured by the sensor of the robot, calculating a segmentation hyperplane tangent to each obstacle and vertical to the connecting line of the robot and the obstacle, expressing the intersection of all hyperplanes in the area at one side of the robot as a group of time-varying linear inequality constraints, and establishing a logarithmic barrier function based on the constraints.
5. The optimal affine formation transformation-based multi-robot dynamic obstacle avoidance device according to claim 1, wherein an estimator state H is designed for the ith robot i And ζ i Respectively used to estimate the gradient of a global time-varying cost function
Derivative of x and t, i.e. [ alpha ]>
And &>
The design of the fixed time estimator is divided into two parts:
the first part is estimator state updating law, corresponding to local time-varying cost function available for each robot
And &>
As reference information, the estimator state of each robot with respect to all other robots is updated using a discontinuous consistency tracking algorithm such that
The second part is the estimator output, the design estimator output is z i ＝-P T (P T H i P) -1 P T ζ i Wherein P is a configuration matrix of the robot.
6. The multi-robot dynamic obstacle avoidance device based on the optimal affine formation transformation as claimed in claim 1, wherein the configuration matrix P and the stress matrix Ω satisfy the following conditions:
(a) The stress matrix is semi-positive and the rank is n-d-1; d is the spatial dimension;
(b)ΩP＝PΩ＝0；
(c) The null space of the matrix omega is equal to the column space of the configuration matrix P.
7. The multi-robot dynamic obstacle avoidance method based on optimal affine formation transformation, which is performed by the multi-robot dynamic obstacle avoidance device according to claim 1, is characterized by comprising the following specific processes:
calculating a gradient tracking variable y according to a preset updating law i So that t ≧ t for all times 0 The weighted sum of the gradient tracking variable and the local cost function gradient with respect to the configuration matrix column vector is always equal, t 0 Represents a fixed time;
and calculating the control input based on a control model which takes the output of the fixed time speed estimator as feedforward, affine formation constraint error and gradient tracking variable as feedback design, and controlling the robot formation based on the control input.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210291561.6A CN114637301B (en) | 2022-03-23 | 2022-03-23 | Multi-robot dynamic obstacle avoidance device and method based on optimal affine formation transformation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210291561.6A CN114637301B (en) | 2022-03-23 | 2022-03-23 | Multi-robot dynamic obstacle avoidance device and method based on optimal affine formation transformation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114637301A CN114637301A (en) | 2022-06-17 |
| CN114637301B true CN114637301B (en) | 2023-04-11 |
Family
ID=81950351
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210291561.6A Active CN114637301B (en) | 2022-03-23 | 2022-03-23 | Multi-robot dynamic obstacle avoidance device and method based on optimal affine formation transformation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114637301B (en) |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8731306B2 (en) * | 2008-07-29 | 2014-05-20 | Superfish Ltd. | Increasing interest point coverage in an image |
| CN102096415B (en) * | 2010-12-31 | 2012-09-26 | 重庆邮电大学 | Multi-robot formation method based on Ad-Hoc network and leader-follower algorithm |
| CN104076688A (en) * | 2014-07-17 | 2014-10-01 | 山东省科学院海洋仪器仪表研究所 | Master-slave type cooperative control method for autonomous underwater vehicles |
| CN105700527A (en) * | 2016-01-26 | 2016-06-22 | 哈尔滨工业大学 | A path programming method for a plane redundancy robot to avoid obstacles and avoid singularities |
| CN108469822B (en) * | 2018-04-04 | 2020-12-15 | 天津理工大学 | Path planning method of indoor blind guiding robot in dynamic environment |
| CN109254298B (en) * | 2018-09-30 | 2019-07-26 | 中国安全生产科学研究院 | The positioning system of safety patrol inspection robot in subway tunnel |
| CN110989639B (en) * | 2019-12-31 | 2023-08-11 | 中国电子科技集团公司信息科学研究院 | Underwater vehicle formation control method based on stress matrix |
| CN111522341A (en) * | 2020-04-23 | 2020-08-11 | 中国地质大学（武汉） | Multi-time-varying formation tracking control method and system for network heterogeneous robot system |
-
2022
- 2022-03-23 CN CN202210291561.6A patent/CN114637301B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN114637301A (en) | 2022-06-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Zhang et al. | Distributed control of coordinated path tracking for networked nonholonomic mobile vehicles | |
| Dutta et al. | A decentralized formation and network connectivity tracking controller for multiple unmanned systems | |
| Yan et al. | Consensus formation tracking for multiple AUV systems using distributed bioinspired sliding mode control | |
| CN110658821B (en) | Multi-robot anti-interference grouping time-varying formation control method and system | |
| Xiao et al. | Incremental updating multirobot formation using nonlinear model predictive control method with general projection neural network | |
| Vargas et al. | MPC-based distributed formation control of multiple quadcopters with obstacle avoidance and connectivity maintenance | |
| CN112631335B (en) | Event triggering-based multi-quad-rotor unmanned aerial vehicle fixed time formation method | |
| Xu et al. | Two-layer distributed hybrid affine formation control of networked Euler–Lagrange systems | |
| Ebel et al. | A comparative look at two formation control approaches based on optimization and algebraic graph theory | |
| CN112083727B (en) | Multi-autonomous system distributed collision avoidance formation control method based on speed obstacle | |
| CN114721275B (en) | Visual servo robot self-adaptive tracking control method based on preset performance | |
| CN114237041B (en) | Space-ground cooperative fixed time fault tolerance control method based on preset performance | |
| CN114779645B (en) | Pectoral fin flapping robot fish formation control method under directional fixed communication topology | |
| CN112947407A (en) | Multi-agent finite-time formation path tracking control method and system | |
| Lopez-Gonzalez et al. | Multi-robot formation control using distance and orientation | |
| Yan et al. | Decentralized formation trajectory tracking control of multi-AUV system with actuator saturation | |
| CN114237247A (en) | Variable formation incomplete mobile robot consistency control method based on prediction | |
| CN112947086A (en) | Self-adaptive compensation method for actuator faults in formation control of heterogeneous multi-agent system consisting of unmanned aerial vehicle and unmanned vehicle | |
| Barghi et al. | On the finite horizon Nash equilibrium solution in the differential game approach to formation control | |
| Hasan et al. | Optimized formation control of multi-agent system using PSO algorithm | |
| CN111176324B (en) | Method for avoiding dynamic obstacle by multi-unmanned aerial vehicle distributed collaborative formation | |
| CN114637301B (en) | Multi-robot dynamic obstacle avoidance device and method based on optimal affine formation transformation | |
| Park et al. | Formation reconfiguration control with collision avoidance of nonholonomic mobile robots | |
| CN114333429B (en) | Rule extraction method for multi-unmanned aerial vehicle target coverage task | |
| CN115857501A (en) | Networked multi-mobile-robot distributed performance-guaranteeing inclusion control method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |