Disclosure of Invention
In order to solve the problems, the invention provides a multi-robot elastic formation control method and system based on a state machine, which can dynamically switch system states according to environmental changes to realize cooperative control of formation maintenance and barrier crossing so as to solve the problem that the consistency of global formations is difficult to ensure due to local optimal solution of a distributed framework and overcome the defects of high computational complexity, weak real-time performance, poor expansibility and insufficient robustness of a centralized framework in a complex environment.
According to some embodiments, the first scheme of the present invention provides a multi-robot elastic formation control method based on a state machine, which adopts the following technical scheme:
A multi-robot elastic formation control method based on a state machine comprises the following steps:
initializing and updating an occupied grid map and generating an Euclidean symbol distance field;
Based on the updated occupied grid map as a search space, searching local path points of each robot by using a heuristic search algorithm, periodically judging the conflict degree of the robots on Euclidean coincidence distance fields, and when the conflict degree exceeds a set threshold, adjusting the local path points of the robots by using a cooperative mechanism of leader path offset mapping and local autonomous adjustment to realize local path planning;
Constructing an optimization objective function by taking dynamic feasibility, obstacle avoidance and formation maintenance among robots as constraint cost, and optimizing local path points by using the minimum optimization objective function to obtain an optimized path;
And periodically detecting potential collision of the robot in the process of walking on the optimized path, and carrying out local path planning again according to the detection result.
Further, the updated occupied grid map is used as a search space, and a heuristic search algorithm is utilized to search local path points of each robot, specifically:
based on the updated occupied grid map as a search space, selecting a current node from an open list which is not searched by taking the minimum cost function as a target;
After the current node is moved from the open list to the searched closed list, traversing the neighbor node of the current node, and skipping if the neighbor node is an obstacle or in the closed list;
If the neighbor node is not in the open list, adding the neighbor node into the open list, setting the current node as a father node of the neighbor node, and calculating the actual cost and cost function from the starting point to the current node;
if the neighbor node is already in the open list, comparing the cost function of the neighbor node with the cost function of the current node, and determining whether to update the current node, the actual cost from the starting point to the current node and the cost function of the current node according to the comparison result;
The path searching is successful and the algorithm is terminated until the current node is the target node, or the path searching fails when the open list is empty;
And obtaining the local path point of each robot.
Further, an optimization objective function is constructed by using dynamic feasibility, obstacle avoidance and formation of avoidance and formation between robots as constraint cost, and local path points are optimized by minimum optimization objective function to obtain an optimized path, specifically:
Based on a quintic spline curve obtained by the local path point balancing treatment, using MINCO tracks to represent the quintic spline curve, and obtaining M sections of local tracks in MINCO form;
constructing an optimization objective function by taking dynamic feasibility, obstacle avoidance, avoidance among robots and formation maintenance as constraint cost;
converting the constrained optimization problem into an unconstrained optimization problem to obtain a final optimization objective function, and solving the final optimization objective function by using a quasi-Newton method to obtain an optimization path.
Further, the final optimization objective function comprises energy cost, total time cost function, obstacle avoidance penalty term, formation cost function, penalty term for avoidance between groups and motion feasibility.
Further, when the collision degree exceeds the collision threshold, the local path point of the robot is adjusted by using a cooperative mechanism of the leader path bias mapping and local autonomous adjustment, specifically:
Obtaining the distance and the gradient of the distance from the nearest barrier to the position of the robot on the Euclidean coincidence distance field, and calculating the collision degree of each robot by combining the formation cost and the gradient thereof;
When the conflict degree of any robot exceeds a conflict threshold value, selecting the robot with the highest conflict degree from the formation as a leader of the formation through Raft dynamic election algorithm;
And adding bias to the leader path based on the heartbeat package of the formation leader according to the preset formation, so as to adjust the local path point of the follower and generate an adjusted local path of the follower.
Further, when the collision degree of any robot is detected to exceed the collision threshold, a robot with the highest collision degree is selected from the formation as a leader of the formation through Raft dynamic election algorithm, specifically:
When the conflict degree of the robot exceeds a conflict threshold, the robot is switched to a leading candidate, and an election request is initiated to all nodes in the formation;
the receiving node will decide whether to vote or not based on the conflict score priority, the tenure number validity and the log consistency:
if the candidate gets all votes or the election times out, the promotion is conducted to be the Leader, and LeaderConfirm information is broadcast, and all robots enter a Leader-Follower mode;
Periodically sending a heartbeat packet containing path points of the front end A of the leader, target positions and conflict information by the leader, and maintaining the leader right and synchronizing the leader right;
When the conflict degree of the leader is smaller than a conflict threshold value and the period of tenure often exceeds a basic period of tenure, the leader sends LeaderStepDown a message to all nodes and stops sending heartbeat packets, but continues to execute the current formation instruction until the period of tenure is completed;
After all Follower nodes receive LeaderStepDown message, they exit the Leader-Follower mode.
Further, the receiving node decides whether to vote according to the conflict scoring priority, the validity of the tenure number and the consistency of the log, specifically:
conflict scoring priority when the degree of conflict of the lead candidate is higher than the conflict scoring of the receiving node;
The validity of the period number is that the period number of the candidate must be greater than or equal to the current period number recorded by the receiving node;
log consistency-the log of the candidate must be consistent with the local log of the receiving node on the latest entry;
all three conditions are met, and the receiving node grants the ticket.
According to some embodiments, a second aspect of the present invention provides a state machine-based multi-robot elastic formation control system, which adopts the following technical scheme:
a state machine based multi-robot elastic formation control system comprising:
an initialization module configured to initialize updating the occupancy grid map and generating a Euclidean symbol distance field;
The local path searching module is configured to search local path points of each robot by using a heuristic search algorithm based on the updated occupied grid map as a search space, periodically judge the conflict degree of the robots on the Euclidean coincidence distance field, and adjust the local path points of the robots by using a cooperative mechanism of leader path bias mapping and local autonomous adjustment when the conflict degree exceeds a set threshold value so as to realize local path planning;
The local path optimization template is configured to construct an optimization objective function with dynamic feasibility, obstacle avoidance, avoidance among robots and formation maintenance as constraint cost, so that the optimization objective function is optimized to local path points at minimum, and an optimized path is obtained;
And the hybrid path planning module is configured to periodically detect potential collision of the robot in the process of optimizing path walking and re-perform local path planning according to the detection result.
According to some embodiments, a third aspect of the present invention provides a computer-readable storage medium.
A computer readable storage medium having stored thereon a computer program which when executed by a processor performs the steps of the state machine based multi-robot resilient queuing control method of the first aspect described above.
According to some embodiments, a fourth aspect of the invention provides a computer device.
A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the steps in the state machine based multi-robot resilient queuing control method of the first aspect described above when the program is executed.
Compared with the prior art, the invention has the beneficial effects that:
The invention designs a hierarchical control architecture driven by a finite state machine, combines front end A sampling and a rear end L-BFGS optimizer, constructs a space-time decoupling track representation method based on MINCO, reduces the path smooth calculation complexity to a linear magnitude while guaranteeing the kinematic feasibility, introduces a Raft consensus algorithm to construct a Leader dynamic election mechanism driven by conflict, quantifies the conflict degree of formation maintenance and obstacle avoidance based on Euclidean Symbol Distance Field (ESDF), triggers high conflict nodes to obtain coordination weight preferentially, realizes intelligent flow transfer of control weight through a time-out period and an active dumping mechanism, proposes a Leader-Follower hybrid path planning strategy, and effectively solves the core problems that the local optimal solution under the traditional distributed framework is difficult to guarantee global formation consistency, and the centralized framework has high calculation complexity and poor expansibility. By integrating multi-mode environment sensing, layered track optimization and dynamic role allocation mechanisms, the formation control high-efficiency and robustness collaborative optimization under a complex dynamic scene is realized.
Detailed Description
The invention will be further described with reference to the drawings and examples.
It should be noted that the following detailed description is illustrative and is intended to provide further explanation of the invention. 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 invention belongs.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of exemplary embodiments according to the present invention. As used herein, the singular is also intended to include the plural unless the context clearly indicates otherwise, and furthermore, it is to be understood that the terms "comprises" and/or "comprising" when used in this specification are taken to specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof.
Embodiments of the invention and features of the embodiments may be combined with each other without conflict.
Example 1
As shown in fig. 1, the present embodiment provides a multi-robot elastic formation control method based on a state machine, and the present embodiment is illustrated by applying the method to a server, and it can be understood that the method may also be applied to a terminal, and may also be applied to a system and a terminal, and implemented through interaction between the terminal and the server. The server can be an independent physical server, a server cluster or a distributed system formed by a plurality of physical servers, and can also be a cloud server for providing cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network servers, cloud communication, middleware services, domain name services, security services CDNs, basic cloud computing services such as big data and artificial intelligent platforms and the like. The terminal may be, but is not limited to, a smart phone, a tablet computer, a notebook computer, a desktop computer, a smart speaker, a smart watch, etc. The terminal and the server may be directly or indirectly connected through wired or wireless communication, and the present application is not limited herein. In this embodiment, the method includes the steps of:
A multi-robot elastic formation control method based on a state machine comprises the following steps:
initializing and updating an occupied grid map and generating an Euclidean symbol distance field;
Based on the updated occupied grid map as a search space, searching local path points of each robot by using a heuristic search algorithm, periodically judging the conflict degree of the robots on Euclidean coincidence distance fields, and when the conflict degree exceeds a set threshold, adjusting the local path points of the robots by using a cooperative mechanism of leader path offset mapping and local autonomous adjustment to realize local path planning;
Constructing an optimization objective function by taking dynamic feasibility, obstacle avoidance and formation maintenance among robots as constraint cost, and optimizing local path points by using the minimum optimization objective function to obtain an optimized path;
And periodically detecting potential collision of the robot in the process of walking on the optimized path, and carrying out local path planning again according to the detection result.
As shown in fig. 2, the present embodiment provides a distributed system architecture, which mainly comprises a sensing module, a planner and a controller, and the robots interact with each other through a WiFi wireless local area network. The sensing module is composed of a visual odometer composed of an RGBD camera and an IMU module and is used as a main data source of SLAM, so that the estimation of the pose of the robot and the sensing of environmental information are realized. The planner adopts a finite state machine mechanism to realize flexible switching of the system state. The free conversion of the distributed and Leader-Follower modes is supported by Raft dynamic election algorithm. Based on the front-end A route searching, the constrained optimization problem is converted into the unconstrained optimization problem by utilizing a MINCO curve and soft constraint method, and then the L-BFGS optimizer is used for carrying out back-end track optimization. The controller is responsible for receiving the optimized track information and executing corresponding control instructions to realize cooperative control of formation maintenance and obstacle crossing. The state transition of the architecture is shown in fig. 3, and the detailed steps are as follows:
Initializing a map, loading an expected formation, initializing the pose of a robot, setting a formation TARGET position, and initializing a finite state machine, wherein the system state is changed from INIT to WAIT TARGET;
(1) The initialization map is that the map information of the unknown environment only contains boundary information, the robot acquires the surrounding environment information and the pose information of the robot in real time through a sensor module (such as a visual odometer, a laser radar, a depth camera and the like), fuses the sensor data with the loaded map, updates the local point cloud map, updates the occupied grid map based on the local point cloud map, and generates the Euclidean Symbol Distance Field (ESDF) map.
(2) Loading a predefined formation, the formation being represented in a relative position matrix. Setting a desired formation layout for formation according to task requirementsWhereinIs the number of robots required to compose a formation,Is a robotIs used to determine the desired position coordinates of the object.
(3) For each robot according to the new occupancy grid mapAssigning an initial poseAnd a target positionThe setting of the target position should conform to the formation.
For the later planning requirement, euclidean Symbol Distance Field (ESDF) is generated according to the three-dimensional grid map calculation constructed by SLAM. Marking obstacle object elements by traversing all voxels, wherein the occupied voxel coordinate set is as follows, wherein,Is the firstEach obstacle voxel, for each free voxelObtaining the minimum Euclidean distance of all obstacle voxels。
TABLE 1 finite State machine State and function
Step 2, robotUpon receiving the target locationThen, the system state is changed from WAIT_TARGET to SEQUENTIAL_START, a small section of collision-free track is generated, and finally, the system state is switched to EXEC_TRAJ execution track, and the step is executed only after the global TARGET point is received;
step 3, in the motion process based on the small section of collision-free track in the step 2, changing the system state from EXEC_TRAJ to REPLAN _TRAJ, and sampling the path points by each robot by adopting A to generate local path points;
The specific steps for a sample are as follows:
(1) The core optimization state is entered, and the system state is changed from 'exec_traj' to 'REPALN _traj'.
(2) And loading the updated occupied grid map as a search space of the A-algorithm. Euclidean distance is chosen as a heuristic function:
(1);
Wherein,Is the location of the current node.
In the execution of the a-algorithm, the open list is first initializedClosing listThe open list stores the nodes to be explored and initializes to contain only the starting points, and the closed list stores the explored nodes and initializes to null.
Next, a cost function is selected from the open listThe smallest node serves as the current node, wherein,To be the actual cost from the origin to the current node,For heuristic estimation of the cost of the current node to the endpoint,Is the current node.
After the current node is moved from the open list to the closed list, traversing the neighbor nodes, if the neighbor nodes are barriers or are skipped in the closed list, if the neighbor nodes are not in the open list, adding the neighbor nodes into the open list, setting the current node as a father node, and calculatingAndIf the neighbor node is already in the open list, comparing the cost function of the neighbor node with the cost function of the current node, and determining whether to update the current node, the actual cost from the starting point to the current node and the cost function of the current node according to the comparison result, namely whenUpdating the current node to be a neighbor nodeAnd updating the actual cost from the starting point to the current nodeAnd the cost function of the current node isOtherwise, the traversal is continued.
And repeating the above process until the path searching is successful and the algorithm is terminated when the current node is the target node, or the path searching fails when the open list is empty.
Meanwhile, for better maintenance formation, the system periodically triggers a 'checkConflict' callback function to determine whether to use the local path points generated by the front end of the system, which comprises the following specific steps,
(1) According to ESDF map constructed by robot, the distance from the nearest obstacle to the position of robot can be obtainedGradient of distance. ESDF the map provides distance information from each position to the nearest obstacle and by calculating the gradient of the distance field, the gradient direction of the current robot position can be derived, which direction points to the position from the nearest obstacle. At the same time, formation costAnd gradients thereofIt can also be obtained by calculating the deviation between the robot and the formation target position. Formation cost is typically based on the difference between the position of the robot in the formation and a predetermined target position, and the gradient reflects the sensitivity of this difference as a function of position. Then the cosine value of the included angle between the two gradient directions can be obtained,
(2);
Then, the cosine value of the included angle is mapped into a conflict coefficient by using a sigmoid function,
(3);
Wherein,AndIs the adjustment parameter of the device, which is used for adjusting the parameters,Adjusting conflict coefficientsAlong with cosine valuesThe speed of the rise is increased and the speed of the rise is increased,The active area and dead area of the map are adjusted. The formation constraint perception should also consider the influence of formation cost and current nearest obstacle distance, and each robot calculates the collision degree of each robotThe degree of conflict mainly reflects the conflict of the robot in maintaining two tasks of obstacle avoidance and formation, and the calculation mode is as follows, whereinIs a proportionality coefficient of the material,
(4);
(2) When a triggering event occurs, i.e., the degree of conflict of any robot exceeds a predetermined conflict threshold, the Finite State Machine (FSM) will switch to the "global_remap" state. In this state, the system selects the robot with the highest conflict degree from the formation as the leader of the formation through Raft dynamic election algorithm. The specific implementation process is as shown in fig. 4, firstly, the robot with the most serious collision is identified by evaluating the collision degree of each robot, then, the Raft algorithm is started to select the leader so as to ensure that the leader with the most adaptability is selected under the current environment, and the specific implementation method is as follows,
1) When the collision score of the robot exceeds a preset threshold, the robot switches to the leader Candidate (CANDIDATE) role and initiates an election (RequestVote) request to all nodes in the team. The request comprises a current random number (Term), namely a globally increasing election round identifier, which is used for distinguishing different election periods, a real-time conflict score of a candidate and reflects the task conflict degree of the robot at present, and a log consistency identifier, namely a historical record abstract of formation state change, which ensures the consistency of state machines of all robots in a system and ensures that all nodes can effectively synchronize the state change.
Election (RequestVote) request contains the current period number (Term) globally incremented election round identification, candidate conflict score: sender's real-timeAnd log consistency identification, forming a history summary of the state change (used for guaranteeing the consistency of a state machine).
2) The receiving node (whether follower Follower or candidate CANDIDATE) will decide whether to vote based on the first, conflict level priority, the receiving node will only vote if the conflict level of the candidate is higher than the conflict score of the receiving node, second, the validity of the tenninal number must also be met, the tenninal number (Term) of the candidate must be greater than or equal to the current tenninal number recorded by the receiving node, and finally, the log consistency requires that the log of the candidate must be consistent with the local log of the receiving node on the latest entry to ensure state synchronization of each node in the system.
3) The decision on the election results depends on whether the candidate gets enough votes or whether it times out. If the candidate gets all votes or the election times out, the candidate will promote to the Leader and broadcast LeaderConfirm a message, then all robots will enter Leader-Follower mode, which is responsible for coordinating the formation tasks. If the candidate does not obtain a majority vote, it will return to the follower Follower role and go to the next round of election.
4) The leader tenn maintenance is achieved by periodically sending a heartbeat packet containing the current path information (LEADERPATH) of the leader, the target location, and collision information. By sending this information, the leader can maintain its leader rights and ensure that all robots in the team update their status synchronously. The regular sending of the heartbeat packet not only prevents frequent occurrence of elections, but also ensures continuous acceptance of the current leader by each node, thereby maintaining the stability and consistency of the system.
5) Leader shedding occurs when the degree of conflict of the leader is significantly less than the set conflict threshold and its tennination has exceeded the base tennination. In this case, the leader sends LeaderStepDown a message to all nodes, declaring an outage. After the dewing, the leader stops sending heartbeat packets, but continues to execute the current formation instruction until the dewing process is completed. All follower (Follower) nodes will exit the Leader-Follower mode after receiving the LeaderStepDown message.
(3) Detailed description of the entering Leader-Follower mode. Robot with identity FollowerRobot with identity Leader is receivedAfter the heartbeat package, the bias is increased by LEADERPATH according to the preset formationAnd then, the front-end paths of Follower are used as local path points obtained by the front end A in the self-track planning process, the formation is maintained macroscopically, and the back-end optimization pressure is reduced. The defect that the front end route finding result of robots is too large in gap and the formation is difficult to maintain only by the rear end optimization under the original distributed system is overcome. The process not only ensures consistency of formation morphology, but also maintains the local autonomous decision-making capability of Follower.
The procedure for entering the Leader-Follower mode is as follows. When the identity of the robot is Follower, after the robot receives the heartbeat packet from the Leader, the robot adjusts LEADERPATH according to a preset formation, and generates a path of the robot by adding a bias. This modified path will be the result of its front end a routing. Therefore, the front-end paths of all Follower nodes are kept consistent macroscopically, the stability of formation is ensured, and the pressure of the back-end optimization process is reduced. The method overcomes the problem of formation instability caused by overlarge front-end path finding difference among robots in the traditional distributed system, and avoids the defect that formation forms cannot be effectively maintained only by back-end optimization. By the method, consistency of formation forms is guaranteed, and local autonomous decision-making capability of Follower robots is reserved, so that necessary personalized path planning can be performed while formation forms are maintained.
In this step, there are two sources of local path points, one is a path point generated by the front end of the local path point through an a-x algorithm, and the other is a local path point provided by a Leader after entering a Leader-Follower mode. After determining the source of the local path points, the path points are smoothed by using a quintic spline curve so as to reduce curvature change of the path and improve smoothness and feasibility of the path. And then, collision detection is carried out on the smoothed path, so that collision with obstacles is avoided, and the safety requirement in practical application is met.
Step 4, optimizing the path obtained by front end sampling by using an L-BFGS optimizer, and switching the system state into EXEC_TRAJ after successful optimization;
(1) The quintic spline obtained by smoothing in the step 3 is represented by a MINCO trace. MINCO is by mapping with linear complexityDecoupling spatial and temporal parameters to achieve flat outputSegment trajectory. In MINCO, the trace is divided intoSegments, each segment being represented by a polynomial of order 2s-1, whereinIs the order of the track. The duration of each segment is defined by a time vectorRepresented by a vector of states of intermediate pointsAnd (3) representing. By linear mappingMINCO are able to decouple these parameters, enabling a spatiotemporal deformation of the trajectory. The method comprises the following steps:
(5);
Wherein,Representing the intermediate point between each pair of adjacent track segments,It is the time that is required for the device to be in contact with the substrate,The start time and the end time of the whole track, respectively, wherein,Namely the firstThe duration of the segment trajectory is chosen to be the same,Is the firstSegment and the firstThe connection point of the segment track,Representing the duration of each segment of track.Dimension(s)Segment trajectoryFrom segmentsThe order polynomial representation. First, theThe segment trajectory is defined as:
(6);
Wherein,Is a matrix of coefficients that are selected from the group consisting of,Is the basis function of the polynomial trajectory.
(2) By combining the adjustment of the motion stability and the system state, the following optimization objective function and constraint conditions thereof are constructed, and the optimization objective function is specifically as follows:
(7);
Wherein,Is a parameter of the time regularization,Is the total time of the M segments of the track,Is the firstSegment track, trackFrom variablesThe optimization is carried out, the process is carried out,Representing the chained higher derivative of the powertrain,AndRespectively representing an initial point and an end point,Is a trackA kind of electronic deviceAnd (5) an order derivative. Continuous time constraintThe feasibility constraint of the system is met, and the feasibility constraint comprises dynamic feasibility, obstacle avoidance, avoidance among robots and formation maintenance, and the inequality constraint is processed by using a penalty function. Finally, converting the constrained optimization problem into an unconstrained optimization problem to obtain a final optimization objective function:
(8);
Wherein,Weight vectors representing various cost functions or penalty terms.The cost of the energy is represented by the energy,Representing the function of the total time cost,Represents the obstacle avoidance penalty term,The formation is represented to form a cost function,A penalty term representing avoidance between the populations,Indicating the feasibility of the movement. And invoking quasi-Newton method to solve the unconstrained optimization problem.
Representing energy costs, the firstThe third order control input of the segment track is written as,Is the third derivative of the trace.
Representing a total time cost function, which can be written as。
Representing formation cost functions. The positions of the robots are used as nodes of the undirected graph, and the robots are connected one by one to form the edges of the undirected graph, so that the symmetrical Laplacian matrix of the undirected graph can be usedTo quantify the retention of formation formations as follows:
(9);
Wherein,Is a symmetric laplace matrix that is desirably formed,Is the symmetric laplace matrix currently being formed.
As this relates to trajectories of other robotsTime alignment is required, and the relative time of the self-track,Is the total length of the segment of the track,It is the sampling time that is to be taken,Global time stamping of other robot trajectoriesThe method comprises the following steps:
(10);
Then the first time period of the first time period,
(11)。
Representing the obstacle avoidance penalty term, calculating the obstacle avoidance penalty using the Euclidean Signed Distance Field (ESDF). Selecting a constraint point close to an obstacleThe following are provided:
(12);
Wherein,The safety threshold value is made such that,Representing the closest distance of the current location to the obstacle,Representing the current location. Then, it is possible to obtain:
(13);
Wherein,Is an orthogonal coefficient following the trapezoidal rule.Penalty terms representing avoidance between groups in a similar mannerThe following are provided:
(14);
Wherein,Representing a collection of other robots,Global time stamp,Indicating the position of the other robot at this time,,A threshold value representing a safe distance between robots.
Representing the feasibility of the movement, specifically:
(15)。
Wherein,AndWeight coefficients representing the velocity term and the acceleration term,AndRepresenting maximum speed and maximum acceleration.
The objective function will be optimized by the L-BFGS optimization algorithm. After the optimization process is successfully completed, the robot enters an EXEC TRAJ state, and the optimized track is sent to the controller for execution.
Step 5A "execFSM" callback function is used to periodically trigger a state update of the Finite State Machine (FSM). And when each callback is triggered, the system evaluates whether state transition is needed according to the state of the current robot. By periodically triggering the 'execFSM' callback, the system can monitor and update the state of the robot in real time, and ensure that the behavior of the robot always accords with a preset control target.
Step 6A "checkCollision" callback function is used to periodically detect potential collisions between the future trajectory of the robot and an obstacle or other robot. Through predicting future paths, the system can identify possible collision risks in advance according to the local map of the robot, and adjust the behavior or state of the robot according to detection results.
For example, when the system detects an impending collision, the system may trigger the state to transition to the "REPLAN _TRAJ" state, jump to step3, at which time the system recalculates the path, finding a new travel route to avoid the collision, or transition to the "EMERGENCY_STOP" state, in which case the robot immediately STOPs moving to prevent the accident from occurring.
Two environments were designed based on ROS platforms, respectively, as shown in fig. 5 and 6. The grey part is an obstacle, and the line segment crossing the obstacle in fig. 5 represents the shortest path for the robot to reach the target point when the environment constraint is not considered, and the path is used for heuristic calculation of the track planning front end so as to guide the path searching process. The other curves in fig. 5 and 6 then represent the trajectories of the robots. Simulation results show that the method can effectively coordinate path optimization, obstacle avoidance and formation maintenance in a complex environment, and stability and coordination capacity of group motions of the elevator robots provide powerful support for autonomous planning and control of the multi-robot system in the complex environment.
Example two
The embodiment provides a multi-robot elastic formation control system based on a state machine, which comprises:
an initialization module configured to initialize updating the occupancy grid map and generating a Euclidean symbol distance field;
The local path searching module is configured to search local path points of each robot by using a heuristic search algorithm based on the updated occupied grid map as a search space, periodically judge the conflict degree of the robots on the Euclidean coincidence distance field, and adjust the local path points of the robots by using a cooperative mechanism of leader path bias mapping and local autonomous adjustment when the conflict degree exceeds a set threshold value so as to realize local path planning;
The local path optimization template is configured to construct an optimization objective function with dynamic feasibility, obstacle avoidance, avoidance among robots and formation maintenance as constraint cost, so that the optimization objective function is optimized to local path points at minimum, and an optimized path is obtained;
And the hybrid path planning module is configured to periodically detect potential collision of the robot in the process of optimizing path walking and re-perform local path planning according to the detection result.
The above modules are the same as examples and application scenarios implemented by the corresponding steps, but are not limited to what is disclosed in the first embodiment. It should be noted that the modules described above may be implemented as part of a system in a computer system, such as a set of computer-executable instructions.
The foregoing embodiments are directed to various embodiments, and details of one embodiment may be found in the related description of another embodiment.
The proposed system may be implemented in other ways. For example, the system embodiments described above are merely illustrative, such as the division of the modules described above, are merely a logical function division, and may be implemented in other manners, such as multiple modules may be combined or integrated into another system, or some features may be omitted, or not performed.
Example III
The present embodiment provides a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps in the state machine based multi-robot elastic formation control method as described in the above embodiment.
Example IV
The present embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor implements the steps in the state machine based multi-robot elastic formation control method according to the above embodiment when executing the program.
It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as a method, system, or computer program product. Accordingly, the present invention may take the form of a hardware embodiment, a software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, magnetic disk storage, optical storage, and the like) having computer-usable program code embodied therein.
The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
Those skilled in the art will appreciate that implementing all or part of the above-described methods in accordance with the embodiments may be accomplished by way of a computer program stored on a computer readable storage medium, which when executed may comprise the steps of the embodiments of the methods described above. The storage medium may be a magnetic disk, an optical disk, a Read-Only Memory (ROM), a random-access Memory (Random Access Memory, RAM), or the like.
While the foregoing description of the embodiments of the present invention has been presented in conjunction with the drawings, it should be understood that it is not intended to limit the scope of the invention, but rather, it is intended to cover all modifications or variations within the scope of the invention as defined by the claims of the present invention.