CN115145275B - Multi-robot formation obstacle avoidance control method based on improved artificial potential field method - Google Patents
Multi-robot formation obstacle avoidance control method based on improved artificial potential field method Download PDFInfo
- Publication number
- CN115145275B CN115145275B CN202210735335.2A CN202210735335A CN115145275B CN 115145275 B CN115145275 B CN 115145275B CN 202210735335 A CN202210735335 A CN 202210735335A CN 115145275 B CN115145275 B CN 115145275B
- Authority
- CN
- China
- Prior art keywords
- robot
- formation
- speed
- repulsive force
- obstacle avoidance
- 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 124
- 238000000034 method Methods 0.000 title claims abstract description 122
- 230000008569 process Effects 0.000 claims abstract description 22
- 230000009471 action Effects 0.000 claims description 17
- 230000004044 response Effects 0.000 claims description 8
- 230000004888 barrier function Effects 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 claims description 7
- 239000013598 vector Substances 0.000 claims description 5
- 238000005755 formation reaction Methods 0.000 description 94
- 238000010586 diagram Methods 0.000 description 11
- 230000000694 effects Effects 0.000 description 9
- 238000004422 calculation algorithm Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 8
- 238000006243 chemical reaction Methods 0.000 description 6
- 238000004088 simulation Methods 0.000 description 5
- 238000002474 experimental method Methods 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 3
- 238000013459 approach Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- 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/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles
-
- 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)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a multi-robot formation obstacle avoidance control method based on an improved artificial potential field method, which comprises the following steps: establishing a multi-robot formation geometry of a virtual pilot-follower mode; when the multi-robot formation advances in a desired queue configuration, the obstacle avoidance control of the multi-robot formation is performed through an improved artificial potential field method, and an obstacle avoidance control law is output; obtaining the output line speed and the angular speed of the formation robot according to the obstacle avoidance control law, and controlling the formation robot to avoid the obstacle in the travelling process; in the improved artificial potential field method, the formation robot is made to avoid obstacle in the safety area of road by adjusting the direction of the combined repulsive force combining speed of the repulsive force speed acted by obstacle and the repulsive force speed acted by road boundary. The invention effectively solves the problems of local optimum and unreachable targets existing in the traditional artificial potential field method.
Description
Technical Field
The invention belongs to the field of multi-robot formation obstacle avoidance control, and particularly relates to a multi-robot formation obstacle avoidance control method based on an improved artificial potential field method.
Background
In recent years, fire robots are also increasingly used in actual combat. However, from the application effect, most of the current fire robots are required to rely on remote operation of background firefighters, and technical challenges such as narrow reconnaissance field of view, poor man-machine interaction, limited fire source positioning and the like exist, so that the fire robots cannot adapt to more complex disaster conditions. Aiming at the fire-fighting demands under complex fire environments such as strong interference, high dynamic and the like, in order to realize the task capacity expansion and the overall fire-fighting efficiency improvement of a robot system, a mode of cooperative operation of a plurality of intelligent fire-fighting robots is adopted, and the realization of capacity complementation and action coordination become the main development direction of future fire-fighting application. When the intelligent group fire-fighting robot executes formation tracking tasks, the robot is ensured to travel in a safety area and quickly avoid the front obstacle in the traveling process because various unknown obstacles exist on the traveling road and the robot is constrained by the edges of two sides of the road. Therefore, an efficient, safe and reliable formation obstacle avoidance algorithm is needed to improve flexibility of formation obstacle avoidance and stability of formation structures.
Existing methods for solving the problem of local obstacle avoidance of robots include dynamic window method (Dynamic Window approach, DWA), artificial potential field method (ARTIFICIAL POTENTIAL FIELD, APF), ant colony algorithm (Ant Colony Optimization, ACO), particle swarm algorithm (PARTILCE SWARM Optimization, PSO), and the like. The method is widely applied to local obstacle avoidance or local path planning of the robot due to the advantages of simple manual potential field method, small calculated amount, strong real-time performance and the like.
However, the traditional artificial potential field method is easy to fall into the problems of local optimum and unreachable targets, and when aiming at the fire-fighting robot to execute formation following tasks under a complex road environment, the fire-fighting robot needs to consider not only unknown obstacles on the road, but also that the formation must travel in a road safety area and cannot cross the road boundary.
Disclosure of Invention
In view of the above analysis, the invention aims to disclose a multi-robot formation obstacle avoidance control method based on an improved artificial potential field method, so as to solve the problems of local minimum and unreachable target existing in the traditional artificial potential field method.
The invention discloses a multi-robot formation obstacle avoidance control method based on an improved artificial potential field method, which comprises the following steps:
establishing a multi-robot formation geometry of a virtual pilot-follower mode;
Designating one robot in formation as a pilot robot, the rest as a following robot, and setting a virtual pilot robot which moves along with the formation on a formation reference track point;
when the multi-robot formation advances in a desired queue configuration, the obstacle avoidance control of the multi-robot formation is performed through an improved artificial potential field method, and an obstacle avoidance control law is output;
Obtaining the output line speed and the angular speed of the formation robot according to the obstacle avoidance control law, and controlling the formation robot to avoid the obstacle in the travelling process;
in the improved artificial potential field method, the formation robot is enabled to avoid the obstacle in the safety area of the road by adjusting the direction of the combined repulsive force combining speed of the repulsive force speed acted by the obstacle and the repulsive force speed acted by the road boundary.
Further, the desired queue configuration of the formation is determined by setting the distance and azimuth angle between each robot and the virtual pilot robot.
Further, in the process of performing multi-robot formation obstacle avoidance control by improving the artificial potential field method, the method comprises the following steps:
Setting a 'gravitational speed' acted by a virtual pilot robot, a 'repulsive speed' acted by an obstacle and a 'repulsive speed' acted by a road boundary for the formation member robots;
Calculating the repulsive force closing speed of the formation member robot, and then adjusting the repulsive force closing speed direction to obtain a new repulsive force closing speed, so that the repulsive force closing speed is perpendicular to the attractive force speed direction acted by the target point and is far away from the obstacle;
and adjusting the ratio of the formation control input to the obstacle avoidance control input by using the dynamic weight factors to obtain the formation speed.
Further, the "repulsive force speed" of the obstacle is:
wherein t is time, and k o is a repulsive force gain coefficient; ρ 0 >0 is the obstacle avoidance response distance; The number of obstacles within the obstacle avoidance response distance ρ 0;
p i (t) is the position of the robot i under the navigation coordinate system; /(I) Is the position of obstacle k under the navigation coordinate system.
Further, "repulsive force speed" of road boundary action is:
Wherein k e >0 is the repulsive force gain coefficient of road boundary action;
Definition of the definition The position p i (t) of the robot i in the navigation coordinate system is the position of the nearest point on the road boundary; ζ i is a logarithmic barrier function.
Further, the logarithmic barrier function is that,
Wherein W is the width of the channel, and b is the width of the robot; y i is the y-axis coordinate of robot i in the navigational coordinate system.
Further, the new "repulsive force closing speed" is:
in the method, in the process of the invention, Expressed original "repulsive force closing velocity"/>And "attraction speed"/>Included angle between Representation/>
To adjust the "repulsive force closing speed" experienced by the robot i in front of the direction;
Rotation matrix Or/>
Further, the ratio of the formation control input to the obstacle avoidance control input is adjusted by using a dynamic weight factor to obtain the formation speed
In the method, in the process of the invention,Is a dynamic weight factor for adjusting the internal ratio of the robot control inputs.
Further, the robot adopts a crawler robot with incomplete constraint; selecting one offset point with the distance d not equal to 0 along the direction axis of the robot from the centroid to mount the positioning sensor; and taking the position of the offset point measured by the positioning sensor as the position of the robot.
Further, the relationship between the linear velocity and the angular velocity of the robot and the bias point obtained according to the kinematic equation of the robot with the bias point as the base point is:
wherein θ is a yaw angle θ ε [ -pi, pi) in the navigation coordinate system; v and ω are the speed and angular velocity at the robot centroid, respectively; Is a velocity vector of the robot bias point in the navigation coordinate system.
The invention can realize at least one of the following beneficial effects:
1. according to the multi-robot formation obstacle avoidance control method based on the improved artificial potential field method, the obstacle avoidance control method based on the improved artificial potential field method can be independent of an accurate mathematical model of a system, and in the process of executing formation following tasks, the obstacle avoidance control of a nonlinear coupling dynamic system with high uncertainty is realized by a very simple algorithm, and efficient, safe and reliable formation obstacle avoidance is realized.
2. The invention not only considers the action effect of the obstacle on the robot, but also considers the action effect of the road boundary, so that the robot can effectively avoid the obstacle in the safety area of the road. The invention can effectively solve the problems of local optimum and unreachable targets in the traditional artificial potential field method.
Drawings
The drawings are only for purposes of illustrating particular embodiments and are not to be construed as limiting the invention, like reference numerals being used to refer to like parts throughout the several views.
FIG. 1 is a flow chart of a multi-robot formation obstacle avoidance control method for improving an artificial potential field method in an embodiment of the invention;
FIG. 2 is a schematic diagram of a virtual pilot-follower queue structure in an embodiment of the invention;
FIG. 3 is a flowchart of a multi-fire-fighting robot formation obstacle avoidance control method based on an improved artificial potential field method in an embodiment of the invention;
FIG. 4 is a schematic diagram of the relationship between three coordinate systems in an embodiment of the present invention;
FIG. 5 is a schematic view of a kinematic model of a tracked fire robot in an embodiment of the invention;
FIG. 6 is a schematic diagram of a problem of local minima and unreachable targets in a conventional artificial potential field method in an embodiment of the invention;
FIG. 7 is a schematic diagram of an obstacle avoidance method of the improved artificial potential field method according to an embodiment of the present invention;
FIG. 8 is a graph of a ζ i function according to an embodiment of the present invention;
FIG. 9 is a simulation result of obstacle avoidance by multi-fire-fighting robot formation based on a traditional artificial potential field method in an embodiment of the invention;
Fig. 9 (a) is a movement track of a multi-fire-fighting robot formation under an obstacle avoidance control law by using a traditional artificial potential field method in the embodiment of the invention;
FIG. 9 (b) is a schematic diagram of the distance and azimuth tracking error under the obstacle avoidance control law of the multi-fire robot formation using the conventional artificial potential field method in the embodiment of the present invention;
FIG. 9 (c) is a graph showing the output line speed and angular speed under the obstacle avoidance control law of the multi-fire-fighting robot formation using the conventional artificial potential field method in the embodiment of the present invention;
FIG. 10 is a simulation result of obstacle avoidance by multi-fire-fighting robot formation based on an improved artificial potential field method in an embodiment of the invention;
FIG. 10 (a) is a motion trace of a multi-fire robot formation under the control law of obstacle avoidance using an improved artificial potential field method in an embodiment of the present invention;
FIG. 10 (b) is a diagram illustrating the range and azimuth tracking error under the obstacle avoidance control law of a multi-fire robot formation using an improved artificial potential field method in accordance with an embodiment of the present invention;
FIG. 10 (c) is a graph showing the output line speed and angular speed under the obstacle avoidance control law of the multi-fire robot formation using the improved artificial potential field method in accordance with the embodiment of the present invention;
FIG. 11 is a diagram of a multi-fire-fighting robot formation obstacle avoidance physical experiment result based on an improved artificial potential field method in an embodiment of the invention;
FIG. 11 (a) is a diagram showing obstacle avoidance motion using an improved artificial potential field method for multi-fire robot formation in an embodiment of the present invention;
FIG. 11 (b) is a diagram illustrating a motion trajectory of obstacle avoidance by a multi-fire robot formation using an improved artificial potential field method in an embodiment of the present invention;
FIG. 11 (c) is a schematic diagram of the distance and azimuth tracking error of a fire-fighting robot formation using an improved artificial potential field method in an embodiment of the present invention;
fig. 11 (d) shows the output line speed and the angular speed of the fire-fighting robot formation using the improved artificial potential field method in the embodiment of the present invention.
Detailed Description
Preferred embodiments of the present application are described in detail below with reference to the attached drawing figures, which form a part of the present application and are used in conjunction with embodiments of the present application to illustrate the principles of the present application.
One embodiment of the invention discloses a multi-robot formation obstacle avoidance control method based on an improved artificial potential field method, which comprises the following steps of:
step S1, establishing a multi-robot formation geometric structure of a virtual navigator-follower mode;
Designating one robot in formation as a pilot robot, the rest as a following robot, and setting a virtual pilot robot which moves along with the formation on a formation reference track point;
S2, when the multi-robot formation advances in a desired queue configuration, performing obstacle avoidance control of the multi-robot formation through an improved artificial potential field method, and outputting an obstacle avoidance control law;
in the improved artificial potential field method, the robot is enabled to avoid the obstacle in a safety area of the road by adjusting the direction of a combined repulsive force closing speed synthesized by a repulsive force speed acted by the obstacle and a repulsive force speed acted by the road boundary;
and step S3, obtaining the output line speed and the angular speed of the robot formation according to the obstacle avoidance control law, and controlling the robot to avoid the obstacle in the traveling process.
Specifically, in step S1, in a multi-robot formation geometry that establishes a virtual pilot-follower mode,
As shown in FIG. 2, one virtual pilot robot R 0 is set, the virtual pilot robot advances along with the formation on the formation reference track point, one of the n (n is more than or equal to 2) fire robots is designated as pilot robot R 1, and the other robots are designated as follower robots R 2,…,Rn.
Due to the fact that the follower is excessively dependent on the navigator in the traditional navigator-follower model, the chain structure is easy to cause tracking error accumulation, and the problem that the whole system is crashed due to the fact that the navigator breaks down is solved.
In the embodiment of the invention, a virtual navigator-follower formation structure is adopted, and the navigator robot R 1 tracks the position of the virtual navigator robot R 0 in real time, namely, the expected distance and azimuth angle between the navigator robot and the virtual navigator robot are converged to zero, namelyIn addition, the expected distance/>, between each following robot and the piloting robot in the traditional piloting-follower mode, is keptAnd azimuth/>The conversion is performed to a desired value corresponding to the virtual pilot robot R 0. The advantage of this formation design is that the pilot robot R 1 is responsible for overall system planning and coordination, but does not affect the motion state of other robots.
Each robot is modeled as a first order integrator model:
in the method, in the process of the invention, Is the position of robot i in the navigational coordinate system. Is the control input for robot i, where u ix and u iy are the control input components for its X n and Y n directions in the navigational coordinate system.
The virtual pilot robot is also modeled as a first order integrator model:
in the method, in the process of the invention, Is the position and velocity of the virtual pilot robot in the navigation coordinate system, wherein v 0x and v 0y are the velocity components of the virtual pilot robot in the X n direction and the Y n direction in the navigation coordinate system.
Defining the relative displacement between the robot i and the virtual pilot as:
the relative speed between robot i and the virtual pilot is defined as:
Defining the distance between the robot i and the virtual navigator as Azimuth angle is/>The calculation formula is as follows:
Wherein,
According to the relative position of the robot i and the virtual pilot robot, a unique value with the range of [ -pi, pi) is obtained by utilizing a smooth arctangent function atan2 (y, x), and then the unique value passes throughThe function converts the atan2 (y, x) range to [0,2 pi ].
Defining a desired distance between the robot i and the virtual pilot asDesired azimuth angle is/> The expected distance and direction angle between the robot i and the virtual pilot are written as vectors:
If it meets The formation structure designed is/>
That is, the desired queue configuration of the formation is achieved by setting the distance between each robot and the virtual pilot robotAnd azimuth/>To make the determination.
Specifically, in the process of performing multi-robot formation obstacle avoidance control by improving an artificial potential field method, the method comprises the following steps:
1) Setting a 'gravitational speed' acted by a virtual pilot robot, a 'repulsive speed' acted by an obstacle and a 'repulsive speed' acted by a road boundary for the formation member robots;
2) Calculating the repulsive force closing speed of the formation member robot, and then adjusting the repulsive force closing speed direction to obtain a new repulsive force closing speed, so that the repulsive force closing speed is perpendicular to the attractive force speed direction acted by the target point and is far away from the obstacle;
3) And adjusting the ratio of the formation control input to the obstacle avoidance control input by using the dynamic weight factors to obtain the formation speed.
Specifically, the "repulsive force speed" of the obstacle is:
wherein t is time, and k o is a repulsive force gain coefficient; ρ 0 >0 is the obstacle avoidance response distance; The number of obstacles within the obstacle avoidance response distance ρ 0;
p i (t) is the position of the robot i under the navigation coordinate system; /(I) Is the position of obstacle k under the navigation coordinate system.
Specifically, the "repulsive force speed" of the road boundary action is:
Wherein k e >0 is the repulsive force gain coefficient of road boundary action;
Definition of the definition The position p i (t) of the robot i in the navigation coordinate system is the position of the nearest point on the road boundary; ζ i is a logarithmic barrier function.
The logarithmic barrier function is that,
Wherein W is the width of the channel, and b is the width of the robot; y i is the y-axis coordinate of robot i in the navigational coordinate system.
The "repulsive force closing speed" applied to the robot i in front of the adjustment direction is
The new repulsive force closing speed obtained by adjusting the direction is as follows:
in the method, in the process of the invention, Expressed original "repulsive force closing velocity"/>And "attraction speed"/>Included angle between Representation/>
Rotation matrix Or/>
Specifically, the control law of the obstacle avoidance control input of the multi-fire-fighting robot formation based on the improved artificial potential field method is as follows:
in the method, in the process of the invention, Is a dynamic weight factor for adjusting the internal ratio of the robot control inputs.
The embodiment of the invention also discloses a multi-fire-fighting robot formation obstacle avoidance control method based on the improved artificial potential field method, wherein in the method, the application scene is an outdoor scene, the adopted fire-fighting robots are non-fully constrained crawler robots, and stable centimeter-level positioning precision is provided for the fire-fighting robots through an RTK positioning technology.
The implementation flow of which is shown in figure 3,
S1, establishing a conversion relation among a geocentric coordinate system, a navigation coordinate system and a robot body coordinate system, so that the position of the fire-fighting robot can be unified under the navigation coordinate system taking a datum point as an origin;
In order to facilitate the pose representation of the robot, the longitude, latitude and height data measured by the RTK are required to be converted to the position of the robot under the navigation coordinate system, and the course angle of north is required to be converted to the yaw angle of the robot under the navigation coordinate system. The coordinate system is defined as shown in FIG. 4, wherein Σ { O e,Xe,Ye,Ze } is the geocentric fixed coordinate system (ECEF system), the origin of coordinates O e coincides with the earth center, X e and Y e lie in the equatorial plane but point to the initial meridian and the east meridian 90 degrees meridian, respectively, Z e coincides with the earth's rotation axis; Σ { O n,Xn,Yn,Zn } is a navigation coordinate system fixedly connected with the ground and is established according to an northeast (ENU) coordinate system, namely, the three axes X n、Yn、Zn point to the right east, right north and right above the geography respectively. Sigma { O r,Xr,Yr,Zr } is the body coordinate system of the fire-fighting robot, wherein O r is fixedly connected with the geometric center of the robot, and the three axes X r,Yr,Zr of the fire-fighting robot are respectively selected according to the directions of the front, the right left and the right upper direction of the robot.
Firstly, longitude, latitude and height data are required to be converted into ECEF Cartesian positions through ellipsoidal parameter conversion, and a conversion formula is as follows:
Wherein B, L, H represents the latitude, longitude and altitude measured by the RTK, N is the radius of curvature of the circle of the mortise, and a and e represent the long half axis and the first eccentricity of the ellipse of the earth, respectively. Wherein a= 6378137 ±2m, e 2 = 0.0066943799013m.
Then, converting the ECEF coordinate system into a navigation coordinate system, wherein the conversion relation is as follows:
in the method, in the process of the invention, Is the position of the reference point (B 0,L0,H0) in ECEF system.
The position of the fire-fighting robot in the navigation coordinate system with the datum point as the origin can be obtained through the formula. Meanwhile, in order to obtain the yaw angle theta epsilon [ -pi, pi) of the robot in the navigation coordinate system, the north-pointing course angle phi epsilon [0,2 pi ] needs to be converted into the absolute yaw angle of the robot, and the conversion relation is as follows:
S2, establishing a kinematic model of the crawler-type fire-fighting robot;
a simplified model of the motion of a tracked fire robot in a two-dimensional plane is shown in fig. 5, where the speed of O r at the centroid is v and the angular velocity is ω.
From the practical engineering point of view, due to the limitation of the installation position, the positioning sensor is difficult to be completely installed at the position of the mass center of the robot, and in the embodiment of the invention, one offset point p with the distance d not equal to 0 from the mass center along the direction axis of the robot is selected to represent the position of the robot.
From fig. 5, the speed of the robot bias point p can be derivedThe relation with the forward direction speed v of the robot is:
since the fire-fighting robot belongs to an incomplete constraint system, namely the longitudinal speed component is zero, the constraint equation is as follows:
the constraint equation can be rewritten as:
Wherein A (q) = [ -sin theta, cos theta, d ] is a constraint matrix, and q= [ x, y, theta ] T is the pose of the robot.
It is further possible to obtain:
then, the kinematic equation of the robot with the bias point as the base point is:
In the formula, v is less than or equal to v max,|ω|≤ωmax, and the linear speed and the angular speed of the robot meet the maximum linear speed and the angular speed constraint.
The relation between the linear speed and the angular speed of the robot and the offset point can be obtained by a kinematic equation:
wherein θ is a yaw angle θ ε [ -pi, pi) in the navigation coordinate system; v and ω are the speed and angular velocity at the robot centroid, respectively; Is a velocity vector of the robot bias point in the navigation coordinate system.
S3, establishing a multi-fire-fighting robot formation structure as a virtual navigator-follower mode;
The virtual pilot robot R 0 consists of a series of continuously-guided reference track points, one of n (n is more than or equal to 2) fire robots is designated as pilot robot R 1, and the other robots are trailing robots R 2,…,Rn;
See the previous embodiment for more details.
S4, defining initial formation of formation, such as triangle, column, character formation and the like.
S5, designing a multi-fire-fighting robot formation obstacle avoidance control algorithm based on an improved artificial potential field method.
The method comprises the following steps:
s501, setting the 'attraction speed' acted by a virtual pilot, the 'repulsion speed' acted by an obstacle and the 'repulsion speed' acted by a road boundary.
The "gravitational velocity" imposed by the virtual pilot can be referred to our previous work "Cooperative Target Enclosing and Tracking Control with Obstacles Avoidance for Multiple Nonholonomic Mobile Robots", which included circular motion control and azimuth positioning control:
the designed circular motion control law is as follows:
Wherein k p >0 is a position adjustment parameter for adjusting the convergence speed of the circular track tracking of the robot. Is a rotation matrix with rotation angle phi i (t) epsilon [0,2 pi ]/>, the rotation matrix is a rotation matrix with rotation angle phi i (t) epsilon [0,2 pi ]For the initial time t 0, a unit vector pointing to the robot i from the target is defined
Due toThe circular motion control law may be rewritten as:
Wherein,
Introducing a nonlinear function into the circular motion controller can obtain an azimuth control law that:
in the method, in the process of the invention, Is a phase adjustment constant for adjusting the convergence speed of the robot phase angle and angular spacing.
Aiming at the 'repulsive force speed' of the obstacle action, the invention adopts the repulsive force action generated by the traditional repulsive force potential field to obtain the 'repulsive force speed' of the robot.
The conventional repulsive field function is defined as:
Where k o is a repulsive gain factor. ρ 0 >0 is the obstacle avoidance response distance. Is the number of obstacles within obstacle avoidance response distance ρ 0. Definition/>Wherein/> Is the position of obstacle k under the navigation coordinate system.
Then, since the robot receives the repulsive force of the obstacle along the negative gradient direction of the repulsive force field function, the obstacle avoidance control law is expressed as follows:
there are problems with local minima and unreachable targets due to the traditional artificial potential field. As shown in fig. 6.
According to the improved artificial potential field method, the road boundary repulsive force speed is established to limit the running area of the robot, and the repulsive force closing speed direction generated by the road boundary and the obstacle is changed to solve the problems of local minimum and unreachable target, so that the implementation effect is shown in fig. 7. It is assumed here that the navigation coordinate system is located in the middle of the road, i.e. the two ends of the road are symmetrical with respect to X n.
The repulsive force speed of road boundary effect is set as follows:
Wherein,
Where k e >0 is the repulsive gain factor of the road boundary effect. Definition of the definition Is the position of the offset point p i of robot i from the nearest point on the road boundary. ζ i is a selected logarithmic obstacle function, a schematic diagram of which is shown in fig. 8. It can be seen that when the robot approaches the road potential field region, the robot receives a "repulsive force speed" from the normal direction of the road boundary, and as the robot approaches the road boundary, the "repulsive force speed" thereof is also greater, and finally tends to infinity; when the robot is in the safe area, its "repulsive force speed" is 0.
Then, the "repulsive force closing speed" received by the robot i is:
s502, calculating the repulsive force closing speed of the formation member robot, and adjusting the direction of the repulsive force closing speed to be perpendicular to the direction of the attractive force speed acted by the target and far away from the obstacle.
As can be seen from fig. 7, the "repulsive force speed" has 2 functions, namely, acceleration and deceleration of the robot in the target direction and tangential force of the robot for turning around the obstacle, so that the combined speed of the robot in the target direction is 0, and the robot is easy to sink into the problems of local optimum and unreachable target. By adjusting the "repulsive force and closing speed" herein "To change it into a "gravitational velocity"/>, which acts with the targetThe direction is perpendicular and away from the obstacle so that the repulsive force only changes the direction of movement and does not change the speed to the target. Then, a new "repulsive force closing speed" is set as:
/>
in the method, in the process of the invention, Expressed original "repulsive force closing velocity"/>And "attraction speed"/>Included angle between Representation/>
S503, calculating the combination speed of formation members;
Finally, the multi-fire-fighting robot formation obstacle avoidance control input based on the improved artificial potential field method provided by the invention is as follows:
in the method, in the process of the invention, Is a dynamic weighting factor for adjusting the internal ratio of the robot control inputs.
S6, updating the pose and the speed of the formation member robot;
The output line speed and the angular speed of the robot formation are obtained according to the obstacle avoidance control law, the output line speed and the angular speed are issued to the robot chassis to control the robot to move, and the current positioning information of the robot is obtained in real time through the RTK;
S7, updating the pose of the virtual pilot robot (reference track point), and converting the pose into a pose expected by formation;
S8, judging whether formation following tasks are completed or not; if so, ending the formation task; if not, repeating steps S5, S6 and S7.
Simulation experiment and analysis
Referring to fig. 9 and 10, in order to verify that the improved artificial potential field method provided by the invention has advantages compared with the traditional artificial potential field method, 2 groups of comparison simulation experiments are performed by using MATLAB software. As can be seen from fig. 9 (a), the motion track of the 2 following robots oscillates in the initial time, because the road boundary repulsive potential field is set as well as the obstacle, if the robot is in the influence range of the repulsive force action of the road boundary at the initial time, the "repulsive force speed" generated by the boundary repulsive force action on the robot and the "resultant speed" obtained by combining the "attractive force speed" generated by the formation controller are excessively changed, and as can be seen from fig. 9 (c), the linear speed and the angular speed of the 2 following robots oscillate repeatedly in the initial time. In addition, it can also be seen from fig. 9 (a) that 2 following robots have a long time "zero closing speed", which will cause the robots to sink to a local optimum and collide with obstacles. It can also be seen from fig. 9 b) that the following robot 2 diverges in distance error, which presents a problem in that the target is not reachable. As can be seen from fig. 10 (a), the improved artificial potential field method provided by the invention realizes that 3 robots effectively avoid obstacles and do not collide with boundaries, and can quickly recover the original formation after the formation avoids the obstacles to continue to execute formation following tasks. It can also be seen from fig. 10 (b) that the distance error and azimuth error of the robot can quickly converge to zero after the robot has avoided the obstacle. In addition, as can be seen from fig. 10 (c), the linear speed and the angular speed output by the formation obstacle avoidance controller designed by the invention meet the speed constraint, and the change is smoother.
Physical experiment and analysis
3 Intelligent fire robots (1 reconnaissance robot and 2 fire robots) which are independently researched and developed are utilized for carrying out physical experiment verification, and the experimental result is shown in figure 11. It can be seen that the formation obstacle avoidance algorithm of the improved artificial potential field method provided by the invention can meet the requirement that the fire-fighting robot formations run in a safety area and can effectively avoid the front obstacle, and the formation originally set can be quickly restored after the obstacle avoidance is completed, so that the structural stability of the formation is ensured.
Simulation and physical experiment results show that the formation obstacle avoidance algorithm based on the improved artificial potential field method provided by the invention well solves the problems of local optimum and unreachable target existing in the traditional artificial potential field method. By setting the repulsive force action of the road boundary and the obstacle on the robot to be different, the action direction of the repulsive force closing speed is changed. The weight of the repulsive force closing speed and the attractive force speed is dynamically adjusted through the dynamic adjusting factors, so that the repulsive force closing speed and the attractive force closing speed are better obtained, and the problems of local optimum and unreachable targets of the traditional artificial potential field method are well solved.
The invention discloses the following technical effects:
1. According to the invention, the multi-fire-fighting robot formation obstacle avoidance control method based on the improved artificial potential field method is adopted, so that the method is independent of an accurate mathematical model of a system, and in the process of executing formation following tasks, the obstacle avoidance control of a nonlinear coupling dynamic system with high uncertainty is realized by a very simple algorithm, and the efficient, safe and reliable formation obstacle avoidance is realized.
2. The invention not only considers the action effect of the obstacle on the robot, but also considers the action effect of the road boundary, so that the robot can effectively avoid the obstacle in the safety area of the road. The invention can effectively solve the problems of local optimum and unreachable targets in the traditional artificial potential field method.
The present invention is not limited to the above-mentioned embodiments, and any changes or substitutions that can be easily understood by those skilled in the art within the technical scope of the present invention are intended to be included in the scope of the present invention.
Claims (7)
1. A multi-robot formation obstacle avoidance control method based on an improved artificial potential field method is characterized by comprising the following steps:
establishing a multi-robot formation geometry of a virtual pilot-follower mode;
Designating one robot in formation as a pilot robot, the rest as a following robot, and setting a virtual pilot robot which moves along with the formation on a formation reference track point;
when the multi-robot formation advances in a desired queue configuration, the obstacle avoidance control of the multi-robot formation is performed through an improved artificial potential field method, and an obstacle avoidance control law is output;
Obtaining the output line speed and the angular speed of the formation robot according to the obstacle avoidance control law, and controlling the formation robot to avoid the obstacle in the travelling process;
in the improved artificial potential field method, the formation robot is enabled to avoid barriers in a safety area of a road by adjusting the direction of a repulsive force combining speed synthesized by a repulsive force speed acted by an obstacle and a repulsive force speed acted by a road boundary;
the desired queue configuration of the formation is determined by setting the distance and azimuth angle between each robot and the virtual pilot robot;
In the process of carrying out multi-robot formation obstacle avoidance control by improving an artificial potential field method, the method comprises the following steps:
setting a 'gravitational speed' acted by a virtual pilot robot, a 'repulsive speed' acted by an obstacle and a 'repulsive speed' acted by a road boundary for the formation robot;
Calculating the repulsive force closing speed of the formation robot, and then adjusting the repulsive force closing speed direction to obtain a new repulsive force closing speed, so that the new repulsive force closing speed is perpendicular to the attractive force speed direction acted by the virtual pilot robot and is far away from the obstacle;
adjusting the ratio of the formation control input and the obstacle avoidance control input by using the dynamic weight factors to obtain the formation speed
In the method, in the process of the invention,The dynamic weight factor is used for adjusting the internal ratio of the robot control input; /(I)Is the gravitational velocity "/>Is a new "repulsive force closing speed".
2. The multi-robot formation obstacle avoidance control method of claim 1, wherein,
The repulsive force speed of the obstacle is:
wherein t is time, and k o is a repulsive force gain coefficient; ρ 0 >0 is the obstacle avoidance response distance; The number of obstacles within the obstacle avoidance response distance ρ 0;
p i (t) is the position of the robot i under the navigation coordinate system; /(I) Is the position of obstacle k under the navigation coordinate system.
3. The multi-robot formation obstacle avoidance control method of claim 1, wherein,
The "repulsive force speed" of the road boundary action is:
Wherein k e >0 is the repulsive force gain coefficient of road boundary action;
Definition of the definition The position p i (t) of the robot i in the navigation coordinate system is the position of the nearest point on the road boundary; ζ i is a logarithmic barrier function.
4. The multi-robot formation obstacle avoidance control method of claim 3, wherein,
The logarithmic barrier function is that,
Wherein W is the width of the channel, and b is the width of the robot; y i is the y-axis coordinate of robot i in the navigational coordinate system.
5. The multi-robot formation obstacle avoidance control method of claim 4, wherein,
The new repulsive force closing speed is as follows:
in the method, in the process of the invention, Expressed original "repulsive force closing velocity"/>And "attraction speed"/>Included angle between Representation/>
To adjust the "repulsive force closing speed" experienced by the robot i in front of the direction;
Rotation matrix Or/>
6. The multi-robot formation obstacle avoidance control method according to any one of claims 1 to 5, wherein,
The robot adopts a crawler robot with incomplete constraint; selecting one offset point with the distance d not equal to 0 along the direction axis of the robot from the centroid to mount the positioning sensor; and taking the position of the offset point measured by the positioning sensor as the position of the robot.
7. The multi-robot formation obstacle avoidance control method of claim 6, wherein,
The relationship between the linear speed and the angular speed of the robot and the offset point is obtained according to a kinematic equation of the robot taking the offset point as a base point:
wherein θ is a yaw angle θ ε [ -pi, pi) in the navigation coordinate system; b. omega is the speed and angular speed of the robot mass center respectively; Is a velocity vector of the robot bias point in the navigation coordinate system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210735335.2A CN115145275B (en) | 2022-06-24 | 2022-06-24 | Multi-robot formation obstacle avoidance control method based on improved artificial potential field method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210735335.2A CN115145275B (en) | 2022-06-24 | 2022-06-24 | Multi-robot formation obstacle avoidance control method based on improved artificial potential field method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115145275A CN115145275A (en) | 2022-10-04 |
| CN115145275B true CN115145275B (en) | 2024-04-30 |
Family
ID=83407867
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210735335.2A Active CN115145275B (en) | 2022-06-24 | 2022-06-24 | Multi-robot formation obstacle avoidance control method based on improved artificial potential field method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115145275B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105974917A (en) * | 2016-05-11 | 2016-09-28 | 江苏大学 | Vehicle obstacle-avoidance path planning research method based on novel manual potential field method |
| CN107608346A (en) * | 2017-08-30 | 2018-01-19 | 武汉理工大学 | Ship intelligent barrier avoiding method and system based on Artificial Potential Field |
| CN108459612A (en) * | 2017-02-21 | 2018-08-28 | 北京航空航天大学 | Unmanned plane formation control method based on Artificial Potential Field Method and device |
| CN111290429A (en) * | 2019-12-16 | 2020-06-16 | 南京信息工程大学 | Unmanned aerial vehicle formation and obstacle avoidance control method thereof based on consistency algorithm and artificial potential field method |
| CN111489023A (en) * | 2020-04-03 | 2020-08-04 | 武汉理工大学 | Multifunctional intelligent robot system for processing dynamic crew service demand information |
| CN113515125A (en) * | 2021-07-05 | 2021-10-19 | 中国石油大学(华东) | Unmanned vehicle full-working-condition obstacle avoidance control method and performance evaluation method |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9436187B2 (en) * | 2015-01-15 | 2016-09-06 | The United States Of America As Represented By The Secretary Of The Navy | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
| US11231715B2 (en) * | 2018-05-22 | 2022-01-25 | King Fahd University Of Petroleum And Minerals | Method and system for controlling a vehicle |
| US11801937B2 (en) * | 2018-07-26 | 2023-10-31 | California Institute Of Technology | Systems and methods for avian flock flight path modification using UAVs |
| CN110550029B (en) * | 2019-08-12 | 2021-02-09 | 华为技术有限公司 | Obstacle avoiding method and device |
-
2022
- 2022-06-24 CN CN202210735335.2A patent/CN115145275B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105974917A (en) * | 2016-05-11 | 2016-09-28 | 江苏大学 | Vehicle obstacle-avoidance path planning research method based on novel manual potential field method |
| CN108459612A (en) * | 2017-02-21 | 2018-08-28 | 北京航空航天大学 | Unmanned plane formation control method based on Artificial Potential Field Method and device |
| CN107608346A (en) * | 2017-08-30 | 2018-01-19 | 武汉理工大学 | Ship intelligent barrier avoiding method and system based on Artificial Potential Field |
| CN111290429A (en) * | 2019-12-16 | 2020-06-16 | 南京信息工程大学 | Unmanned aerial vehicle formation and obstacle avoidance control method thereof based on consistency algorithm and artificial potential field method |
| CN111489023A (en) * | 2020-04-03 | 2020-08-04 | 武汉理工大学 | Multifunctional intelligent robot system for processing dynamic crew service demand information |
| CN113515125A (en) * | 2021-07-05 | 2021-10-19 | 中国石油大学(华东) | Unmanned vehicle full-working-condition obstacle avoidance control method and performance evaluation method |
Non-Patent Citations (3)
| Title |
|---|
| 基于人工势场法的多机器人编队与避障;桑雷 等;信息系统工程;20200320(第3期);第139-145页 * |
| 基于改进人工势场法的无人车避障路径规划研究;韩宇洪;中国优秀硕士学位论文全文库，工程科技Ⅱ辑;20200815;第24-33页 * |
| 基于模型预测控制的汽车主动避障系统;杨丰萍 等;华东交通大学学报;20200215(第1期);第70-76页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115145275A (en) | 2022-10-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112378408B (en) | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot | |
| CN110398969A (en) | Automatic driving vehicle adaptive prediction time domain rotating direction control method and device | |
| Zheng | Moving path following control for a surface vessel with error constraint | |
| Frew et al. | Cooperative stand-off tracking of moving targets by a team of autonomous aircraft | |
| CN111338384B (en) | Self-adaptive path tracking method of snake-like robot | |
| CN104392047A (en) | Quick trajectory programming method based on smooth glide trajectory analytic solution | |
| CN109669481A (en) | Spacecraft Safe Approach Control Method Based on Equal Collision Probability Surface Method | |
| WO2021077769A1 (en) | Streaming computing-based map creation method and system therefor | |
| CN101807081A (en) | Autonomous navigation guidance method used for pilotless plane | |
| CN112947572B (en) | Terrain following-based four-rotor aircraft self-adaptive motion planning method | |
| CN105045284A (en) | Anti-interference drone path tracking control method | |
| Chen et al. | Path planning for autonomous vehicle based on a two-layered planning model in complex environment | |
| CN115167440A (en) | Virtual navigation-following-based multi-robot formation control method | |
| CN113885570B (en) | Satellite formation reconstruction control method based on rotation potential field | |
| Wang et al. | UAV standoff tracking for narrow-area target in complex environment | |
| Zhang et al. | Research on obstacle avoidance algorithm of multi-uav consistent formation based on improved dynamic window approach | |
| CN115145275B (en) | Multi-robot formation obstacle avoidance control method based on improved artificial potential field method | |
| CN113589809A (en) | Obstacle-avoidable excavator working device operation track planning method and device | |
| CN113176788A (en) | Aircraft path tracking method based on variable forward distance LOS guidance law | |
| Lu et al. | An information potential approach for tracking and surveilling multiple moving targets using mobile sensor agents | |
| Frew | Cooperative standoff tracking of uncertain moving targets using active robot networks | |
| CN114200926B (en) | Local path planning method and system for unmanned vehicle | |
| CN116009558A (en) | Mobile robot path planning method combined with kinematic constraint | |
| Wang et al. | Shortest path planning of UAV for target tracking and obstacle avoidance in 3D environment | |
| Li et al. | A novel hybrid path planning method for mobile robots |
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 |