CN111103897A - Multi-robot formation control method and system in obstacle environment - Google Patents
Multi-robot formation control method and system in obstacle environment Download PDFInfo
- Publication number
- CN111103897A CN111103897A CN201911359820.9A CN201911359820A CN111103897A CN 111103897 A CN111103897 A CN 111103897A CN 201911359820 A CN201911359820 A CN 201911359820A CN 111103897 A CN111103897 A CN 111103897A
- Authority
- CN
- China
- Prior art keywords
- robot
- piloting
- following
- formation
- obstacle
- 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.)
- Pending
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 95
- 238000000034 method Methods 0.000 title claims abstract description 41
- 238000012545 processing Methods 0.000 claims abstract description 11
- 238000004891 communication Methods 0.000 claims description 8
- 238000004590 computer program Methods 0.000 claims description 6
- 230000010339 dilation Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 230000004888 barrier function Effects 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000010485 coping Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 241000251468 Actinopterygii Species 0.000 description 1
- 241000272814 Anser sp. Species 0.000 description 1
- 241000282461 Canis lupus Species 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000003542 behavioural effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000012544 monitoring process 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/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a multi-robot formation control method and a system under an obstacle environment, which comprises the steps of regularly receiving state data packets of all robots, judging whether a pilot robot has a fault or not, determining the current target position of each following robot according to a preset formation instruction if the pilot robot has no fault, calculating linear velocity and angular velocity, and sending the linear velocity and the angular velocity to the corresponding following robot; otherwise, performing expansion processing according to the current position coordinate of the piloting robot and updating the known obstacle coordinate, calculating the maximum passable distance according to the updated known obstacle coordinate, and if the maximum passable distance cannot meet the requirement of passing the following robot, returning the following robot; otherwise, the following robots are re-formed by the root. The invention solves the problem that the navigation robot can not continuously operate when a plurality of robots are in failure in a complex terrain, greatly reduces the dependence of the system on the navigation robot, and obviously improves the robustness and reliability of the system in an obstacle environment.
Description
Technical Field
The application belongs to the field of robot control, and particularly relates to a multi-robot formation control method and system in an obstacle environment.
Background
With the technological innovation and the continuous expansion of the application field of robots, more and more people are added into the research of the multi-robot system, and the multi-robot system is also developed rapidly. A plurality of single robots with relatively simple structures and sensing capabilities are adopted to form the multi-robot cooperation system, and the reliability of the multi-robot cooperation system is improved while the design cost of the robot structure is saved. The multi-robot cooperation is one of the hot problems of multi-robot research, and the basic problems include formation control, map construction, unmanned disaster relief, cooperative transportation and the like, wherein the formation control is a prerequisite for researching other problems. The cooperation of multiple robots is currently applied to the fields of industry, military, national defense and life.
The multi-robot formation control means that a multi-robot system relies on a sensor to sense the surrounding environment and the self state, and mutually cooperates to complete formation, so that target-oriented autonomous motion is realized in the environment with obstacles. Formation behaviors are often seen in nature, such as fish shoal formation, wild goose formation flight, wolf formation predation, etc. With the advancement of technology, more and more human activities are using formation activities, such as aircraft carrier mixed formation and airplane fleet formation. At present, the main methods for controlling multi-robot formation comprise: based on a behavioral method, an artificial potential field method, a piloting following method, a virtual structure method and a distributed control algorithm.
The robot replaces human work and has important significance in the aspect of protecting human personal safety, and as the robot technology and the multi-robot formation control technology are mature day by day, the multi-robot system can be applied to complex environments, such as disaster relief sites, military front lines and other environments. The complex environment has the characteristics of irregular terrain, more obstacles, large signal interference and the like, and the performance of the robot is tested to determine whether the coping strategy is complete when an accident occurs. Therefore, a method for coping with a failure of a robot in a complex environment is required.
Disclosure of Invention
The application aims to provide a multi-robot formation control method and system in an obstacle environment, the problem that a navigation robot cannot continue to operate when a fault occurs in the navigation robot in a complex terrain is solved, dependence of the system on the navigation robot is greatly reduced, and robustness and reliability of the system in the obstacle environment are remarkably improved.
In order to achieve the purpose, the technical scheme adopted by the application is as follows:
a multi-robot formation control method in an obstacle environment comprises a pilot robot and a plurality of following robots, the multi-robot formation control method in the obstacle environment is used for controlling the multi-robots to operate according to preset formation instructions under the condition that obstacle coordinates in the obstacle environment are known, each robot has a unique ID number, and the multi-robot formation control method in the obstacle environment comprises the following steps:
step S1, receiving state data packets of each robot at regular time, wherein the state data packets comprise position coordinates and course angles of the current robot, judging whether the piloted robot has a fault or not according to the state data packets of the piloted robot, and executing step S2 if the piloted robot has no fault; otherwise, executing step S3;
step S2, determining the current target orientation of each following robot according to a preset formation instruction, calculating linear velocity and angular velocity by combining the current state data packet and the current target orientation of each following robot, generating an instruction data packet by the linear velocity and the angular velocity, sending the instruction data packet to the corresponding following robot, and executing step S1 again;
step S3, performing dilation processing according to the current position coordinate of the piloting robot, updating the known coordinates of the obstacle by using the coordinates obtained after dilation processing, and executing step S4;
step S4, calculating the maximum passable distance according to the known obstacle coordinates updated in the step S3, and controlling the following robot to return if the maximum passable distance cannot meet the requirement of the following robot to pass; otherwise re-formation for the following robots is performed according to the updated known obstacle coordinates, and step S1 is re-executed.
Preferably, the queuing instruction consists of a matrix FdExpressed, the expression is as follows:
Fd＝[Fs1Fs2Fs3…]4×n
Fsn＝[f1nf2nf3nf4n]T,n＝1,2,3,…
wherein, FdFormation instruction representing the entirety of all robots, FsnIndicating the status information of the nth robot, f1nID number, f, indicating the nth robot2nAn ID number indicating a piloting robot followed by the nth robot, f if the robot is a piloting robot2nIs set to 0, f3nRepresenting the desired distance between the nth robot and the piloting robot, f if the robot is a piloting robot3nIs set to 0, f4nRepresenting the desired heading angle between the nth robot and the piloting robot, if the robot is a piloting robot f4nIs set to 0.
Preferably, the determining whether the piloted robot has a fault includes:
if the state data packet of the piloting robot is not received regularly, judging that a communication module of the piloting robot breaks down, and stopping the operation of the piloting robot;
if the received state data packet of the piloting robot contains fault information, judging that the piloting robot has a fault, and stopping the operation of the piloting robot;
if the variation of the position coordinate of the piloting robot in the preset time is smaller than the threshold value, judging that the piloting robot has a fault, and stopping the operation of the piloting robot;
otherwise, the piloting robot has no fault.
Preferably, the determining the current target position of each following robot according to a preset formation instruction includes:
the target position comprises a desired position and a desired heading angle;
calculating a desired position (x) of the following robotv,yv) And a desired heading angle θvThe formula is as follows:
θv＝θl
wherein x islAnd ylAs position coordinates of the piloting robot, thetalIs the course angle of the piloting robot,
representing the desired distance between the lead robot and the following robot to be currently calculated,
representing the desired heading angle between the piloted robot and the following robot currently to be calculated.
Preferably, the expanding process is performed according to the current position coordinates of the pilot robot, and the known obstacle coordinates are updated using the coordinates obtained after the expanding process, and the method includes:
s3.1, acquiring a preset position expansion processing parameter R;
s3.2, determining position coordinates (x) of the piloting robot during faultl,yl)；
S3.3, using the position coordinate (x)l,yl) Is expanded by taking R as radius and the center of a circleAnd (3) converting to obtain a set Q, wherein the expression is as follows:
Q＝{x,y|(x-xl)2+(y-yl)2≤R2}
and S3.4, taking the set Q as an obstacle, adding the coordinates in the set Q into the known obstacle coordinates, and updating the known obstacle coordinates.
Preferably, the calculating the maximum passable distance according to the known obstacle coordinates updated in step S3 includes:
s4.1, reading the original known coordinates of the obstacle and recording the coordinates as [ (x)z1,yz1),(xz2,yz2),…,(xzn,yzn)]And a navigation robot inflated set Q;
s4.2, calculating the maximum passable distance Ds，DsThe expression of (a) is:
wherein x isl,ylAnd R is the expansion radius.
Preferably, if the maximum passable distance cannot meet the requirement that the following robot passes through, controlling the following robot to return; otherwise re-queuing for the following robots according to the updated known obstacle coordinates, including:
s4.3, setting the maximum passable distance DsThe width D of the current following robot queue and the minimum width D of the similar queue formsmAnd the passable width D of a single robotrComparing;
s4.4, if D is less than or equal to DsThe following robot passes through the current formation and modifies the formation instruction according to the ID number proximity principle;
s4.5, if Dm≤Ds<D, the following robot passes through the original formation, the width of the queue is reduced by reducing the expected distance or the expected course angle, and the formation instruction is modified according to the ID number proximity principle;
S4.6、if D isr≤Ds<DmThe following robot changes the formation into a cylindrical formation and modifies the formation instruction according to the ID number proximity principle;
s4.7, if Ds<DrAnd the following robot cannot pass through and returns.
The application also provides a multi-robot formation control system in the obstacle environment, the multi-robot formation control system in the obstacle environment comprises a server and a plurality of robots, the plurality of robots comprise a pilot robot and a plurality of following robots, each robot has a unique ID number, and the server is connected with a socket of each robot;
the server comprises a memory and a processor, wherein a computer program is stored in the memory, and the processor executes the computer program to realize the formation control method of the multiple robots in the obstacle environment according to any one technical scheme.
The formation control method and system for multiple robots in the obstacle environment regularly judge the fault information of the piloting robot, and two sets of operation schemes are set according to whether the piloting robot has a fault or not so as to deal with the fault condition of the piloting robot, and simultaneously complete the obstacle avoidance operation of each robot, so that the problem that the piloting robot cannot continue to operate when the piloting robot has a fault in a complex terrain is solved, dependence of the system on the piloting robot is greatly reduced, and robustness and reliability of the system in the obstacle environment are remarkably improved.
Drawings
FIG. 1 is a flow chart of a multi-robot formation control method in the obstacle environment of the present application;
FIG. 2 is a flowchart illustrating a method for determining whether a piloted robot has a fault according to the present disclosure;
FIG. 3 is a state diagram of the present application in determining the current target position of a following robot;
FIG. 4 is a schematic diagram illustrating the expansion of the location of a failed piloting robot according to the present application;
FIGS. 5 a-5 c are schematic diagrams of the present application illustrating the calculation of minimum passable distance and the modification of the formation commands after a fault;
fig. 6 is a schematic diagram of information interaction between a server and a robot according to the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used in the description of the present application herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the application.
As shown in fig. 1, in one embodiment, a multi-robot formation control method in an obstacle environment is provided, which is used for controlling the operation of multiple robots according to preset formation instructions under the condition that coordinates of obstacles in the obstacle environment are known.
The multiple robots in the embodiment comprise a pilot robot and a plurality of following robots, and each robot is provided with a unique ID number. Specifically, the formation control method of multiple robots in the obstacle environment comprises the following steps:
step S1, receiving state data packets of each robot at regular time, wherein the state data packets comprise position coordinates and course angles of the current robot, judging whether the piloted robot has a fault or not according to the state data packets of the piloted robot, and executing step S2 if the piloted robot has no fault; otherwise, step S3 is executed.
It should be noted that the status data packet is sent by the robot in the working status, and in an embodiment, in order to obtain whether the robot is online or in a specified area in real time, each robot further sends a heartbeat packet in real time (including the working status and the non-working status).
As shown in fig. 2, the determining whether the piloted robot has a fault includes:
1) if the state data packet of the piloting robot is not received regularly, judging that a communication module of the piloting robot breaks down, and stopping the operation of the piloting robot; when a communication module of the piloting robot breaks down, the communication module cannot receive information sent by the outside, namely after the piloting robot does not receive the information of the outside within the set time, the piloting robot judges that the communication module breaks down, and then the operation is stopped. Just as above, if the state data packet or heartbeat packet of the piloting robot is received before fixed time, it is also determined that the piloting robot has a fault.
2) If the received state data packet of the piloting robot contains fault information, judging that the piloting robot has a fault, and stopping the operation of the piloting robot; a self-checking program is preset in the piloting robot, and if the self-checking program detects a fault of a certain module of the piloting robot, fault information is sent and the operation of the self-checking program is stopped.
3) And if the variation of the position coordinate of the piloting robot in a certain time is smaller than the threshold value, judging that the piloting robot possibly fails due to the interference of the external environment, judging that the piloting robot fails, and commanding the piloting robot to stop running.
4) Otherwise, judging that the piloting robot normally operates if the piloting robot fails, namely the piloting robot does not fail.
And S2, determining the current target orientation of each following robot according to a preset formation instruction, calculating linear velocity and angular velocity by combining the current state data packet and the current target orientation of each following robot, generating an instruction data packet by the linear velocity and the angular velocity, sending the instruction data packet to the corresponding following robot, and re-executing the step S1.
Wherein the enqueue instruction consists of a matrix FdExpressed, the expression is as follows:
Fd＝[F21Fs2Fs3…]4×n
Fsn＝[f1nf2nf3nf4n]T,n＝1,2,3,…
wherein, FdFormation instruction representing the entirety of all robots, FsnIndicating the status information of the nth robot, f1nID number, f, indicating the nth robot2nAn ID number indicating a piloting robot followed by the nth robot, f if the robot is a piloting robot2nIs set to 0, f3nRepresenting the desired distance between the nth robot and the piloting robot, f if the robot is a piloting robot3nIs set to 0, f4nRepresenting the desired heading angle between the nth robot and the piloting robot, if the robot is a piloting robot f4nIs set to 0.
The formation instruction is preset according to current operation requirements, if the piloting robot has no fault, each robot is controlled to operate according to the preset formation instruction, and as shown in fig. 3, the following robots are controlled to operate according to the preset formation instruction by the following steps:
determining the current target position of each following robot according to a preset formation instruction, wherein the target position comprises an expected position and an expected course angle; taking the position relation shown in fig. 3 as an example, the target position of the following robot is calculated, the piloting robot is at the position L in fig. 3, and the position coordinate of the piloting robot is (x)l,yl) The course angle of the piloting robot is thetalThe following robot to be calculated is currently actually at position F, and the current position coordinate is (x)f,yf) And heading angle thetafThe expected position of the following robot to be calculated is V, and the expected position coordinate is (x)v,yv) And a desired heading angle θv。DlfRepresenting the actual distance of the piloting robot from the following robot,
and the actual course angle between the piloting robot and the following robot is represented, namely an included angle formed between the motion direction of the piloting robot and the direction of a connecting line between the piloting robot and the actual following robot.
The expected position (x) of the following robot is calculatedv,yv) And desire toCourse angle thetavThe formula is as follows:
θv＝θl
wherein
Representing the desired distance between the lead robot and the following robot to be currently calculated,
and the heading angle between the piloting robot and the current expected following robot to be calculated is represented, namely an included angle formed between the motion direction of the piloting robot and the direction of a connecting line between the piloting robot and the expected following robot.
After the current target position of the following robot is obtained through calculation, the linear velocity and the angular velocity can be calculated by combining the current state data packet of each following robot, and the linear velocity and the angular velocity are sent to the following robot, so that the operation control of the following robot is completed.
After the operation control is completed, step S1 is executed again, that is, the status data packets of the robots are continuously and periodically received, and whether the piloted robot has a fault is determined again. The method can detect the state of the piloting robot at regular time so as to deal with the sudden change situation in time and minimize the running loss of each robot.
It should be noted that, in the present embodiment, the PID control algorithm is used to calculate the linear velocity and the angular velocity that need to be executed along with the robot according to the current position coordinate, the current heading angle, and the target azimuth, and the used PID control algorithm is the existing calculation algorithm, that is, the middle calculation process does not belong to the improvement focus of the present application, and is not further detailed.
Step S3 is to perform dilation processing according to the current position coordinates of the pilot robot, update the coordinates of the known obstacle with the coordinates obtained after dilation processing, and execute step S4.
After the pilot robot fails, the pilot robot at this time is equal to the obstacle with respect to other robots, and the position coordinate is considered as a point, and the pilot robot has a certain volume, so that an appropriate range needs to be extended according to the current position coordinate of the pilot robot, so as to facilitate normal obstacle avoidance of other robots, in an embodiment, as shown in fig. 4, the expansion processing is specifically as follows:
and S3.1, acquiring a preset position expansion processing parameter R.
S3.2, determining position coordinates (x) of the piloting robot during faultl,yl)。
S3.3, using the position coordinate (x)l,yl) Expanding the R as a radius and taking the R as a circle center to obtain a set Q, wherein the expression is as follows:
Q＝{x,y|(x-xl)2+(y-yl)2≤R2}
and S3.4, taking the set Q as an obstacle, adding the coordinates in the set Q into the known obstacle coordinates, and updating the known obstacle coordinates.
Step S4, calculating the maximum passable distance according to the known obstacle coordinates updated in the step S3, and controlling the following robot to return if the maximum passable distance cannot meet the requirement of the following robot to pass; otherwise re-formation for the following robots is performed according to the updated known obstacle coordinates, and step S1 is re-executed.
The following robot operation process is controlled according to the maximum passable distance as follows:
s4.1, reading the original known coordinates of the obstacle and recording the coordinates as [ (x)z1,yz1),(xz2,yz2),…,(xzn,yzn)]And a navigation robot inflated set Q;
s4.2, calculating the maximum passable distance Ds，DsThe expression of (a) is:
wherein x isl,ylAnd R is the expansion radius.
S4.3, setting the maximum passable distance DsThe width D of the current following robot queue and the minimum width D of the similar queue formsmAnd the passable width D of a single robotrComparing;
s4.4, as shown in FIG. 5a, if D ≦ DsThe following robot passes through the current formation according to the current formation and modifies the formation instruction according to the ID number proximity principle, for example, in the original formation instruction, the 1 st robot is a pilot robot, when the robot has a fault, the 2 nd robot is designated as a new 1 st robot according to the proximity principle, namely the pilot robot, that is, the original formation instruction Fs1＝[R001 0 0 0]Modified to Fs1＝[R002 0 0 0]Meanwhile, the original 3 rd robot is used as a new 2 nd robot, namely, the original queuing command Fs2＝[R002 R001 Ld5π/4]Modified to Fs2＝[R003 R002 Ld5π/4]And by analogy, modifying the formation instruction according to the principle that the latter robot is used as a new former robot.
S4.5, as shown in FIG. 5b, if Dm≤Ds<D, the following robot passes through the original formation according to the original formation, but the width of the formation is reduced, the formation command is modified according to the ID number proximity principle, and the width of the formation command is reduced by reducing the expected distance or the expected heading angle, for example, the formation command is changed from Fs1＝[R001 0 0 0]Modified to Fs1＝[R002 0 0 0]，Fs2＝[R002 R001 Ld5π/4]Modified to Fs2＝[R003 R002 ld5π/4](ldThe maximum length that a team can pass), and so on, i.e., modifying the formation command on the basis of the latter robot as the new former robot while reducing the desired distance or desired heading angle.
S4.6, as shown in FIG. 5c, if Dr≤Ds<DmThe following robot changes the formation form into a cylindrical formation form and modifies the formation command according to the ID number proximity principle, for example, the formation command is changed from Fs1＝[R001 0 0 0]Modified to Fs1＝[R002 0 00]，Fs2＝[R002 R001 Ld5π/4]Modified to Fs2＝[R003 R002 Ldπ]And in the same way, modifying the formation command according to the principle that the next robot is used as a new previous robot and the expected course angle is set to be pi.
S4.7, if Ds<DrAnd the following robot cannot pass through and returns.
After the formation command is modified, the new pilot robot builds a map and navigates to a target point by means of a camera or a radar of the new pilot robot, and the new following robot continues to run after receiving corresponding linear velocity and angular velocity according to the new formation command.
According to the formation control method for multiple robots in the obstacle environment, the fault information of the piloting robot is judged at regular time, two sets of operation schemes are set according to whether the piloting robot has a fault or not, so that the fault condition of the piloting robot is met, the obstacle avoidance operation of each robot is completed simultaneously, the problem that the piloting robot cannot continue to operate when the piloting robot has a fault in a complex terrain is solved, the dependence of a system on the piloting robot is greatly reduced, and the robustness and the reliability of the system in the obstacle environment are remarkably improved.
In another embodiment, a formation control system of multiple robots in an obstacle environment is provided, the formation control system of multiple robots in an obstacle environment comprises a server and multiple robots, the multiple robots comprise a pilot robot and multiple following robots, each robot has a unique ID number, the server establishes socket connection with each robot, as shown in fig. 6, after the server establishes connection with each robot, the robots regularly send information such as position coordinates, heading angles, fault information and heartbeat packets to the server, the server regularly sends heartbeat packets to the pilot robot, and the server regularly sends information such as angular velocity, linear velocity and heartbeat packets to each following robot, so as to implement monitoring of robot operation.
The server comprises a memory and a processor, wherein a computer program is stored in the memory, and the processor executes the computer program to realize the multi-robot formation control method under the obstacle environment.
In order to facilitate the control operation of each robot, in another embodiment, a piloting robot is generally configured to include modules such as a camera module, an industrial personal computer module, a main control module, a power supply module, and a motor driving module. The piloting robot moves to a target point according to a map planning path of the current environment and sends own position coordinates and course angles to the server cloud platform in real time.
The following robot is composed of a camera module, an industrial personal computer module, a main control module, a power supply module, a motor driving module and the like. The following robot runs along with the piloting robot according to the linear speed and the angular speed sent by the server cloud platform, and sends the position coordinate and the course angle of the following robot to the server cloud platform in real time.
The server also comprises a communication module and a formation module, the communication module is connected with each robot to receive the state data packets of each robot, and the formation module calculates the target position of the following robot, resolves the target position into linear velocity and angular velocity and sends the linear velocity and the angular velocity to the following robot.
For specific limitations of the formation control system for multiple robots in a barrier environment, reference may be made to the above limitations on the formation control method for multiple robots in a barrier environment, and details thereof are not repeated here.
The technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present application, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the concept of the present application, which falls within the scope of protection of the present application. Therefore, the protection scope of the present patent shall be subject to the appended claims.
Claims (8)
1. A multi-robot formation control method in an obstacle environment is characterized in that the multi-robot formation control method in the obstacle environment is used for controlling the operation of multiple robots according to preset formation instructions under the condition that obstacle coordinates in the obstacle environment are known, each robot has a unique ID number, and the multi-robot formation control method in the obstacle environment comprises the following steps:
step S1, receiving state data packets of each robot at regular time, wherein the state data packets comprise position coordinates and course angles of the current robot, judging whether the piloted robot has a fault or not according to the state data packets of the piloted robot, and executing step S2 if the piloted robot has no fault; otherwise, executing step S3;
step S2, determining the current target orientation of each following robot according to a preset formation instruction, calculating linear velocity and angular velocity by combining the current state data packet and the current target orientation of each following robot, generating an instruction data packet by the linear velocity and the angular velocity, sending the instruction data packet to the corresponding following robot, and executing step S1 again;
step S3, performing dilation processing according to the current position coordinate of the piloting robot, updating the known coordinates of the obstacle by using the coordinates obtained after dilation processing, and executing step S4;
step S4, calculating the maximum passable distance according to the known obstacle coordinates updated in the step S3, and controlling the following robot to return if the maximum passable distance cannot meet the requirement of the following robot to pass; otherwise re-formation for the following robots is performed according to the updated known obstacle coordinates, and step S1 is re-executed.
2. The formation control of multiple robots in a barrier environment as claimed in claim 1Method, characterized in that said enqueuing instruction is formed by a matrix FdExpressed, the expression is as follows:
Fd＝[Fs1Fs2Fs3…]4×n
Fsn＝[f1nf2nf3nf4n]T，n＝1，2，3，…
wherein, FdFormation instruction representing the entirety of all robots, FsnIndicating the status information of the nth robot, f1nID number, f, indicating the nth robot2nAn ID number indicating a piloting robot followed by the nth robot, f if the robot is a piloting robot2nIs set to 0, f3nRepresenting the desired distance between the nth robot and the piloting robot, f if the robot is a piloting robot3nIs set to 0, f4nRepresenting the desired heading angle between the nth robot and the piloting robot, if the robot is a piloting robot f4nIs set to 0.
3. The method for controlling formation of multiple robots in an obstacle environment according to claim 1, wherein the determining whether the piloted robot has a fault comprises:
if the state data packet of the piloting robot is not received regularly, judging that a communication module of the piloting robot breaks down, and stopping the operation of the piloting robot;
if the received state data packet of the piloting robot contains fault information, judging that the piloting robot has a fault, and stopping the operation of the piloting robot;
if the variation of the position coordinate of the piloting robot in the preset time is smaller than the threshold value, judging that the piloting robot has a fault, and stopping the operation of the piloting robot;
otherwise, the piloting robot has no fault.
4. The method for controlling formation of multiple robots in an obstacle environment according to claim 2, wherein the determining the current target orientation of each following robot according to a preset formation instruction comprises:
the target position comprises a desired position and a desired heading angle;
calculating a desired position (x) of the following robotv，yv) And a desired heading angle θvThe formula is as follows:
θv＝θl
wherein x islAnd ylAs position coordinates of the piloting robot, thetalIs the course angle of the piloting robot,
representing the desired distance between the lead robot and the following robot to be currently calculated,
representing the desired heading angle between the piloted robot and the following robot currently to be calculated.
5. The method for controlling formation of multiple robots in an obstacle environment according to claim 1, wherein the expanding process is performed based on the current position coordinates of the pilot robot, and the coordinates obtained after the expanding process are used to update the coordinates of the known obstacle, and the method includes:
s3.1, acquiring a preset position expansion processing parameter R;
s3.2, determining position coordinates (x) of the piloting robot during faultl，yl)；
S3.3, using the position coordinate (x)l，yl) Expanding the R as a radius and taking the R as a circle center to obtain a set Q, wherein the expression is as follows:
Q＝{x，y|(x-x1)2+(y-yl)2≤R2}
and S3.4, taking the set Q as an obstacle, adding the coordinates in the set Q into the known obstacle coordinates, and updating the known obstacle coordinates.
6. The method for controlling formation of multiple robots under an obstacle environment according to claim 5, wherein the calculating of the maximum passable distance from the known obstacle coordinates updated in step S3 includes:
s4.1, reading the original known coordinates of the obstacle and recording the coordinates as [ (x)z1，yz1)，(xz2，yz2)，…，(xzn，yzn)]And a navigation robot inflated set Q;
s4.2, calculating the maximum passable distance Ds，DsThe expression of (a) is:
wherein x isl，ylAnd R is the expansion radius.
7. The formation control method of multiple robots in an obstacle environment according to claim 6, wherein if the maximum passable distance cannot satisfy the passing of the following robot, the following robot is controlled to return; otherwise re-queuing for the following robots according to the updated known obstacle coordinates, including:
s4.3, setting the maximum passable distance DsThe width D of the current following robot queue and the minimum width D of the similar queue formsmAnd the passable width D of a single robotrComparing;
s4.4, if D is less than or equal to DsThe following robot passes through the current formation and modifies the formation instruction according to the ID number proximity principle;
s4.5, if Dm≤Ds＜D，The following robot passes through the following robot according to the original formation, the width of the queue is reduced by reducing the expected distance or the expected course angle, and the formation instruction is modified according to the ID number proximity principle;
s4.6, if Dr≤Ds＜DmThe following robot changes the formation into a cylindrical formation and modifies the formation instruction according to the ID number proximity principle;
s4.7, if Ds＜DrAnd the following robot cannot pass through and returns.
8. A formation control system for multiple robots in an obstacle environment is characterized by comprising a server and multiple robots, wherein the multiple robots comprise a pilot robot and a plurality of following robots, each robot has a unique ID number, and the server is connected with a socket of each robot;
the server comprises a memory and a processor, wherein a computer program is stored in the memory, and the processor executes the computer program to realize the multi-robot formation control method in the obstacle environment according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911359820.9A CN111103897A (en) | 2019-12-25 | 2019-12-25 | Multi-robot formation control method and system in obstacle environment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911359820.9A CN111103897A (en) | 2019-12-25 | 2019-12-25 | Multi-robot formation control method and system in obstacle environment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111103897A true CN111103897A (en) | 2020-05-05 |
Family
ID=70425228
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911359820.9A Pending CN111103897A (en) | 2019-12-25 | 2019-12-25 | Multi-robot formation control method and system in obstacle environment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111103897A (en) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111766879A (en) * | 2020-06-24 | 2020-10-13 | 天津大学 | Intelligent vehicle formation system based on autonomous collaborative navigation |
| CN112068584A (en) * | 2020-06-16 | 2020-12-11 | 中国人民解放军军事科学院国防科技创新研究院 | Unmanned aerial vehicle area coverage control system and control method thereof |
| CN112486184A (en) * | 2020-12-10 | 2021-03-12 | 北京小狗吸尘器集团股份有限公司 | Floor sweeping robot and obstacle avoidance path determination method thereof |
| CN113687657A (en) * | 2021-08-26 | 2021-11-23 | 鲁东大学 | Method and storage medium for multi-agent formation dynamic path planning |
| WO2022032443A1 (en) * | 2020-08-10 | 2022-02-17 | 深圳技术大学 | Transport method for multi-intelligent agent formation, system, and computer-readable storage medium |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106325280A (en) * | 2016-10-20 | 2017-01-11 | 上海物景智能科技有限公司 | Multirobot collision preventing method and system |
| US20180143645A1 (en) * | 2016-11-18 | 2018-05-24 | Robert Bosch Start-Up Platform North America, LLC, Series 1 | Robotic creature and method of operation |
| CN110398975A (en) * | 2019-09-04 | 2019-11-01 | 西北工业大学 | A kind of navigator's follower type multiple aircraft formation fault tolerant control method based on broadcast operation framework |
-
2019
- 2019-12-25 CN CN201911359820.9A patent/CN111103897A/en active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106325280A (en) * | 2016-10-20 | 2017-01-11 | 上海物景智能科技有限公司 | Multirobot collision preventing method and system |
| US20180143645A1 (en) * | 2016-11-18 | 2018-05-24 | Robert Bosch Start-Up Platform North America, LLC, Series 1 | Robotic creature and method of operation |
| CN110398975A (en) * | 2019-09-04 | 2019-11-01 | 西北工业大学 | A kind of navigator's follower type multiple aircraft formation fault tolerant control method based on broadcast operation framework |
Non-Patent Citations (2)
| Title |
|---|
| 任立敏等: "障碍环境下多移动机器人动态优化队形变换", 《机器人》 * |
| 杜玉立等: "搬运机器人路径规划策略的研究", 《信息技术与信息化》 * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112068584A (en) * | 2020-06-16 | 2020-12-11 | 中国人民解放军军事科学院国防科技创新研究院 | Unmanned aerial vehicle area coverage control system and control method thereof |
| CN111766879A (en) * | 2020-06-24 | 2020-10-13 | 天津大学 | Intelligent vehicle formation system based on autonomous collaborative navigation |
| WO2022032443A1 (en) * | 2020-08-10 | 2022-02-17 | 深圳技术大学 | Transport method for multi-intelligent agent formation, system, and computer-readable storage medium |
| CN112486184A (en) * | 2020-12-10 | 2021-03-12 | 北京小狗吸尘器集团股份有限公司 | Floor sweeping robot and obstacle avoidance path determination method thereof |
| CN112486184B (en) * | 2020-12-10 | 2023-08-11 | 北京小狗吸尘器集团股份有限公司 | Sweeping robot and obstacle avoidance path determining method thereof |
| CN113687657A (en) * | 2021-08-26 | 2021-11-23 | 鲁东大学 | Method and storage medium for multi-agent formation dynamic path planning |
| CN113687657B (en) * | 2021-08-26 | 2023-07-14 | 鲁东大学 | Method and storage medium for multi-agent formation dynamic path planning |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111103897A (en) | Multi-robot formation control method and system in obstacle environment | |
| US9938008B2 (en) | Systems and methods for execution of recovery actions on an unmanned aerial vehicle | |
| US9286807B2 (en) | Collision avoidance system and a method for determining an escape manoeuvre trajectory for collision avoidance | |
| Spurny et al. | Complex manoeuvres of heterogeneous mav-ugv formations using a model predictive control | |
| Zhang et al. | Leader–follower system using two robot tractors to improve work efficiency | |
| US11014650B2 (en) | Moving body, moving body control system, moving body control method, interface device, and recording medium having program recorded thereon | |
| WO2016172251A1 (en) | Systems and methods for execution of recovery actions on an unmanned aerial vehicle | |
| US20200393853A1 (en) | Moving platform and control method therefor | |
| Sisto et al. | A fuzzy leader-follower approach to formation control of multiple mobile robots | |
| WO2012145608A1 (en) | Systems and methods for autonomously landing an aircraft | |
| CN114594788A (en) | Four-rotor unmanned aerial vehicle track planning method and system in unknown environment | |
| CN204527375U (en) | A kind of crawler type detection multi-robot system | |
| de Vargas et al. | Patrolling strategy for multiple UAVs with recharging stations in unknown environments | |
| CN109696908B (en) | Robot and flight path setting method and system thereof | |
| CN115639769A (en) | Safe intelligent control method and system | |
| Witus et al. | Methods for UGV teleoperation with high latency communications | |
| Simon | Autonomous navigation in rubber plantations | |
| CN114326816A (en) | Method and device for planning formation flight path of fixed-wing unmanned aerial vehicle based on artificial potential field | |
| CN116848485A (en) | Unmanned aerial vehicle control method, unmanned aerial vehicle and storage medium | |
| CN110998468B (en) | Split control system configuration for UAV autopilot architecture | |
| KR102070110B1 (en) | Intelligent agent structure for autonomous unmanned system | |
| CN113110547B (en) | Flight control method, device and equipment of miniature aviation aircraft | |
| TW201035524A (en) | Path planning method of adaptive obstacle avoidance for mobile robot | |
| Villa et al. | Outdoor navigation using two quadrotors and adaptive sliding mode control | |
| CN115248599B (en) | Priority-variable multi-robot zero-space behavior fusion formation 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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20200505 |
|
| RJ01 | Rejection of invention patent application after publication |