Multi-robot elastic formation control method and system based on state machine

Abstract

The invention belongs to the field of distributed system and multi-robot formation control, and provides a multi-robot elastic formation control method and system based on a state machine, and the method comprises the steps: initializing and updating an occupation grid map, and generating a Euclidean symbol distance field; on the basis of taking the updated occupied grid map as a search space, searching local path points of each robot by utilizing a heuristic search algorithm, and generating a local path; constructing an optimized objective function by taking dynamics feasibility, obstacle avoidance, avoidance between robots and formation keeping as constraint cost, and optimizing a local path to obtain an optimized path; the method comprises the following steps: periodically detecting potential collision of robots in an optimized path walking process, and carrying out mixed path planning on multiple robots on a Euclidean coincidence distance field by utilizing a cooperative mechanism of leader path offset mapping and local autonomous adjustment according to a detection result; according to the invention, system states can be dynamically switched according to environment changes, and cooperative control of formation maintenance and obstacle crossing is realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of distributed system and multi-robot formation control, and in particular relates to a multi-robot flexible formation control method and system based on a state machine. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] With the continuous improvement of social informatization and intelligence, the formation control method for multi-robot systems has made rapid progress. Robots in the formation can perform various tasks efficiently and flexibly, especially in complex field environments and urban areas. The formation control method has shown wide application potential in search and rescue, collaborative mapping, package delivery and other fields.

[0004] In the formation control of multi-robot systems, there are mainly two system frameworks: distributed and centralized. In the centralized architecture, all robots are planned and assigned formation information by the center. The formation is often considered as a whole for path planning in the form of envelopes, which will discard a large number of feasible solutions, or generate paths by sampling the map according to the formation configuration. The cage process is extremely time-consuming. Even if some scholars have discretized the adaptive granularity, it still needs to be done offline. In the distributed architecture, each robot plans its own trajectory based on the local information it obtains, and constraints are imposed between robots during the planning process to maintain the formation. Compared with the former, the distributed framework system has stronger robustness and scalability, almost no performance bottleneck, and the robots make autonomous decisions, negotiate task allocation, and have strong adaptability. However, because of this, the local optimal solution of individuals in the distributed framework is difficult to match the global optimal solution of the former in maintaining the formation. Summary of the invention

[0005] In order to solve the above problems, the present invention proposes a multi-robot flexible formation control method and system based on a state machine. The present invention can dynamically switch the system state according to environmental changes, and realize the coordinated control of formation maintenance and obstacle crossing, so as to solve the problem that the local optimal solution of the distributed framework is difficult to ensure the global formation consistency, and at the same time overcome the defects of the centralized framework in complex environments, such as high computational complexity, weak real-time performance, poor scalability and insufficient robustness.

[0006] According to some embodiments, a first solution of the present invention provides a multi-robot flexible formation control method based on a state machine, which adopts the following technical solution: The multi-robot flexible formation control method based on state machine includes: Initialize and update the occupancy grid map and generate the Euclidean signed distance field; Based on the updated occupancy grid map as the search space, the heuristic search algorithm is used to search for the local path points of each robot, and the conflict degree of the robot is periodically judged on the Euclidean distance field. When the conflict degree exceeds the set threshold, the leader path bias mapping and the local autonomous adjustment collaborative mechanism are used to adjust the local path points of the robot to achieve local path planning. The optimization objective function is constructed with dynamic feasibility, obstacle avoidance, avoidance between robots and formation maintenance as constraint costs. The local path points are optimized to minimize the objective function and the optimized path is obtained. Based on the periodic detection of potential collisions of the robot during walking on the optimized path, the local path planning is re-performed according to the detection results.

[0007] Furthermore, the updated occupancy grid map is used as the search space, and a heuristic search algorithm is used to search for the local path points of each robot, specifically: Based on the updated occupancy grid map as the search space, the current node is selected from the open list that has not been searched with the goal of minimizing the cost function; After moving the current node from the open list to the searched closed list, traverse the neighbor nodes of the current node and skip them if they are obstacles or in the closed list; If the neighbor node is not in the open list, add it to the open list and set the current node as its parent node, and calculate the actual cost and cost function from the starting point to the current node; If the neighbor node is already in the open list, compare the cost function of the neighbor node with the cost function of the current node, and determine 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 based on the comparison result; Until the path search succeeds and the algorithm terminates when the current node is the target node, or the path search fails when the open list is empty; Get the local path points of each robot.

[0008] Furthermore, the optimization objective function is constructed with dynamic feasibility, obstacle avoidance, avoidance between robots and formation maintenance as constraint costs. The local path points are optimized to minimize the objective function and the optimized path is obtained, which is specifically: Based on the quintic spline curve obtained by balancing the local path points, the MINCO trajectory is used to represent the quintic spline curve, and the M-segment local trajectory in the MINCO form is obtained; The optimization objective function is constructed with dynamic feasibility, obstacle avoidance, robot-to-robot avoidance, and formation maintenance as constraint costs; The constrained optimization problem is converted into an unconstrained optimization problem to obtain the final optimization objective function, and the quasi-Newton method is used to solve the final optimization objective function to obtain the optimization path.

[0009] Furthermore, the final optimization objective function includes energy cost, total time cost function, obstacle avoidance penalty term, formation cost function, penalty term for avoidance between groups and movement feasibility.

[0010] Furthermore, when the conflict degree exceeds the conflict threshold, the local path point of the robot is adjusted by using the collaborative mechanism of the leader path bias mapping and the local autonomous adjustment, specifically: Obtain the distance to the nearest obstacle from the robot's own position and the gradient of the distance in the Euclidean distance field, and calculate the conflict degree of each robot in combination with the formation cost and its gradient; When it is detected that the conflict level of any robot exceeds the conflict threshold, the Raft dynamic election algorithm is used to select the robot with the highest conflict level from the formation as the leader of the formation; According to the preset formation, the leader's path is offset based on the heartbeat packet of the formation leader to adjust the follower's local path points and generate the follower's adjusted local path.

[0011] Furthermore, when it is detected that the conflict level of any robot exceeds the conflict threshold, the robot with the highest conflict level is selected from the formation as the leader of the formation through the Raft dynamic election algorithm, specifically: When the robot's conflict level exceeds the conflict threshold, the robot will switch to a leader candidate and initiate an election request to all nodes in the formation; The receiving node will decide whether to vote based on the conflict score priority, term number validity, and log consistency: If a candidate obtains all votes or the election times out, it is promoted to the leader and broadcasts a LeaderConfirm message, and all robots enter the Leader-Follower mode. If it does not obtain a majority of votes, it returns to being a follower and starts the next round of elections. The leader periodically sends heartbeat packets containing the path points, target locations, and conflict information of its own front-end A* to maintain leadership and synchronize status; When the leader conflict level is less than the conflict threshold and the term often exceeds the basic term, the leader sends a LeaderStepDown message to all nodes and stops sending heartbeat packets, but continues to execute the current formation command until the resignation is completed; After all Follower nodes receive the LeaderStepDown message, they exit the Leader-Follower mode.

[0012] Furthermore, the receiving node will decide whether to vote based on the conflict scoring priority, term number validity, and log consistency, specifically: Conflict score priority: when the conflict level of the leader candidate is higher than the conflict score of the receiving node; Term number validity: The candidate's term number must be greater than or equal to the current term number recorded by the receiving node; Log consistency: The candidate’s log must be consistent with the receiving node’s local log on the latest entries; If all three conditions above are met, the receiving node votes in favor.

[0013] According to some embodiments, a second solution of the present invention provides a multi-robot flexible formation control system based on a state machine, which adopts the following technical solution: The multi-robot flexible formation control system based on state machine includes: An initialization module, configured to initialize and update the occupancy grid map and generate a Euclidean signed distance field; The local path search module is configured to use the updated occupancy grid map as the search space, use the heuristic search algorithm to search for the local path points of each robot, and periodically judge the conflict degree of the robots on the Euclidean distance field. When the conflict degree exceeds the set threshold, the local path points of the robots are adjusted by using the collaborative mechanism of the leader path bias mapping and the local autonomous adjustment to achieve local path planning; The local path optimization template is configured to construct an optimization objective function with dynamic feasibility, obstacle avoidance, avoidance between robots and formation maintenance as constraint costs, and optimize the local path points to minimize the objective function to obtain the optimized path; The hybrid path planning module is configured to periodically detect potential collisions of the robot based on the optimized path walking process and re-plan the local path according to the detection results.

[0014] According to some embodiments, a third aspect of the present invention provides a computer-readable storage medium.

[0015] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps in the state machine-based multi-robot flexible formation control method as described in the first aspect above.

[0016] According to some embodiments, a fourth aspect of the present invention provides a computer device.

[0017] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps in the state machine-based multi-robot flexible formation control method as described in the first aspect above are implemented.

[0018] Compared with the prior art, the present invention has the following beneficial effects: The present invention designs a hierarchical control architecture driven by a finite state machine, combines front-end A* sampling with back-end L-BFGS optimizer, and constructs a MINCO-based time-space decoupled trajectory representation method, which reduces the path smoothing calculation complexity to a linear level while ensuring kinematic feasibility; introduces the Raft consensus algorithm to construct a conflict-driven leader dynamic election mechanism, quantifies the conflict degree between formation maintenance and obstacle avoidance based on the Euclidean signed distance field (ESDF), triggers high-conflict nodes to obtain coordination rights first, and realizes intelligent transfer of control rights through term timeout and active resignation mechanism; proposes a Leader-Follower hybrid path planning strategy, through the collaborative mechanism of leader path bias mapping and local autonomous adjustment; effectively solves the core problems that the local optimal solution under the traditional distributed framework is difficult to ensure global formation consistency, and the centralized framework has high computational complexity and poor scalability. By integrating multimodal environmental perception, hierarchical trajectory optimization and dynamic role allocation mechanism, the efficient and robust collaborative optimization of formation control in complex dynamic scenes is achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0020] Figure 1 is a flow chart of a multi-robot flexible formation control method based on a state machine in an embodiment of the present invention; Figure 2 is an overall block diagram of the distributed system architecture in an embodiment of the present invention; Figure 3 is a schematic diagram of the state switching of a finite state machine in an embodiment of the present invention; Figure 4 This is a flow chart of the raft dynamic election algorithm in an embodiment of the present invention; Figure 5 This is a diagram showing the effect of maintaining a formation in a first complex environment in an embodiment of the present invention; Figure 6 This is a diagram showing the formation maintenance effect in the second complex environment in an embodiment of the present invention. DETAILED DESCRIPTION

[0021] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0022] It should be noted that the following detailed descriptions are all illustrative and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0023] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0024] In the absence of conflict, the embodiments of the present invention and the features of the embodiments may be combined with each other.

[0025] Embodiment 1 like Figure 1 As shown, this embodiment provides a multi-robot flexible formation control method based on a state machine. This embodiment uses the method applied to a server as an example for illustration. It can be understood that the method can also be applied to a terminal, and can also be applied to a system including a terminal and a server, and is implemented through the interaction between the terminal and the server. The server can be an independent physical server, or a server cluster or distributed system composed of multiple physical servers, or a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network servers, cloud communications, middleware services, domain name services, security services CDN, and big data and artificial intelligence platforms. The terminal can be a smart phone, a tablet computer, a laptop computer, a desktop computer, a smart speaker, a smart watch, etc., but is not limited to this. The terminal and the server can be directly or indirectly connected via wired or wireless communication, which is not limited in this application. In this embodiment, the method includes the following steps: The multi-robot flexible formation control method based on state machine includes: Initialize and update the occupancy grid map and generate the Euclidean signed distance field; Based on the updated occupancy grid map as the search space, the heuristic search algorithm is used to search for the local path points of each robot, and the conflict degree of the robot is periodically judged on the Euclidean distance field. When the conflict degree exceeds the set threshold, the leader path bias mapping and the local autonomous adjustment collaborative mechanism are used to adjust the local path points of the robot to achieve local path planning. The optimization objective function is constructed with dynamic feasibility, obstacle avoidance, avoidance between robots and formation maintenance as constraint costs. The local path points are optimized to minimize the objective function and the optimized path is obtained. Based on the periodic detection of potential collisions of the robot during walking on the optimized path, the local path planning is re-performed according to the detection results.

[0026] like Figure 2As shown, this embodiment proposes a distributed system architecture, which is mainly composed of three parts: a perception module, a planner and a controller. The robots exchange information through a WiFi wireless LAN. The perception module is composed of a visual odometer composed of an RGBD camera and an IMU module. As the main data source for SLAM, it enables the robot to estimate its own posture and perceive environmental information. The planner adopts a finite state machine mechanism to achieve flexible switching of system states. Through the Raft dynamic election algorithm, it supports free conversion between distributed and Leader-Follower modes. On the basis of the front-end A* pathfinding, the MINCO curve and soft constraint method are used to convert the constrained optimization problem into an unconstrained optimization problem, and then the L-BFGS optimizer is used for back-end trajectory optimization. The controller is responsible for receiving the optimized trajectory information and executing corresponding control instructions to achieve coordinated control of formation maintenance and obstacle crossing. The state transition of this architecture is shown in the figure. Figure 3 The detailed steps are as follows: Step 1: Initialize the map, load the desired formation, initialize the robot posture, set the formation target position, initialize the finite state machine, and the system state changes from "INIT" to "WAIT_TARGET"; (1) Initialize the map. Since the map information for an unknown environment only contains boundary information, the robot obtains the surrounding environment information and its own posture information in real time through sensor modules (such as visual odometer, lidar, depth camera, etc.), fuses the sensor data with the loaded map, updates the local point cloud map, and based on this updates the occupancy grid map to generate the Euclidean signed distance field (ESDF) map.

[0027] (2) Load the predefined formation, which is represented by a relative position matrix. Set the expected formation layout according to the mission requirements. ,in is the number of robots required to form a formation, It's a robot The expected position coordinates of .

[0028] (3) Based on the new occupancy grid map, Assigning initial pose and target location , the target position should be set in accordance with the formation.

[0029] In order to meet the needs of later planning, it is necessary to calculate and generate the Euclidean Signed Distance Field (ESDF) based on the three-dimensional grid map constructed by SLAM. Traverse all voxels to mark the obstacle pixels, and the occupied voxel coordinate set is ,in, It is obstacle voxels, for each free voxel , get the minimum Euclidean distance to all obstacle pixels .

[0030] Table 1 Finite state machine states and functions

[0031] Step 2: Robot Upon receiving the target location After that, the system state changes from "WAIT_TARGET" to "SEQUENTIAL_START". After a short collision-free trajectory is generated, the system state finally switches to "EXEC_TRAJ" execution trajectory. This step is only executed after receiving the global target point; Step 3: During the movement of a short collision-free trajectory based on step 2, the system state changes from "EXEC_TRAJ" to "REPLAN_TRAJ", and each robot uses A* to sample the waypoints and generate local waypoints; The specific steps for A* sampling are as follows: (1) Entering the core optimization state, the system state changes from "EXEC_TRAJ" to "REPALN_TRAJ".

[0032] (2) Load the updated occupancy grid map as the search space for the A* algorithm. Select Euclidean distance as the heuristic function: (1); in, The position of the current node.

[0033] During the execution of the A* algorithm, the open list is first initialized and close list , the open list stores the nodes to be explored and is initialized to contain only the starting point, and the closed list stores the nodes that have been explored and is initialized to be empty.

[0034] Next, select the cost function from the open list The smallest node is taken as the current node, where is the actual cost from the starting point to the current node, is the heuristic estimated cost from the current node to the end point, is the current node.

[0035] After moving the current node from the open list to the closed list, traverse its neighbor nodes. If the neighbor node is an obstacle or in the closed list, skip it; if the neighbor node is not in the open list, add it to the open list and set the current node as its parent node. Calculate and ; If the neighbor node is already in the open list, compare the cost function of the neighbor node with the cost function of the current node, and determine 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 based on the comparison result; that is, when Then update the current node to the neighbor node , and update the actual cost from the starting point to the current node to And the cost function of the current node is Otherwise, continue traversing.

[0036] The above process is repeated until the path search succeeds and the algorithm is terminated when the current node is the target node, or the path search fails when the open list is empty.

[0037] At the same time, in order to better maintain the formation, the system periodically triggers the "checkConflict" callback function to decide whether to use the local path points generated by its own front end. The specific steps are as follows: (1) Based on the ESDF map constructed by the robot, the distance to the nearest obstacle can be obtained. and the gradient of distance The ESDF map provides the 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 obtained, which points to the position of the nearest obstacle. and its gradient It can also be obtained by calculating the deviation between the robot and the formation target position. The formation cost is usually based on the difference between the position of the robot in the formation and the predetermined target position, and the gradient reflects the sensitivity of this difference to position changes. Then the cosine value of the angle between the two gradient directions can be obtained , (2); Then, the sigmoid function is used to map the angle cosine value to the conflict coefficient , (3); in, and is the adjustment parameter, Adjust the conflict coefficient With cosine value The speed of increase, Adjust the activation area and dead zone of the mapping. Formation constraint perception should also consider the impact of the formation cost size and the current distance to the nearest obstacle. Each robot calculates its own conflict degree. , the conflict degree mainly reflects the conflict between the robot's obstacle avoidance and formation maintenance tasks, and its calculation method is as follows, where is the proportionality coefficient, (4); (2) When a trigger event occurs, that is, when the conflict level of any robot exceeds the predetermined conflict threshold, the finite state machine (FSM) will switch to the "global_remap" state. In this state, the system uses the Raft dynamic election algorithm to select the robot with the highest conflict level from the formation as the leader of the formation. The specific implementation process is as follows Figure 4 As shown in the figure, first, by evaluating the conflict level of each robot, the robot with the most serious conflict is identified; then, the Raft algorithm is started to elect a leader to ensure that the most adaptable leader is selected in the current environment. The specific implementation method is as follows: 1) When the robot's conflict score exceeds the preset threshold, the robot will switch to the role of leader candidate (Candidate) and initiate an election (RequestVote) request to all nodes in the formation. The request contains the current term number (Term), which is a globally increasing election round identifier to distinguish different election cycles; the candidate's real-time conflict score, which reflects the degree of task conflict currently faced by the robot; and the log consistency identifier, which is a historical record summary of the formation's state changes, to ensure the consistency of the state machines of all robots in the system and to ensure that each node can effectively synchronize state changes.

[0038] The RequestVote request contains the current term number (Term): the globally increasing election round identifier, the candidate conflict score: the sender's real-time and log consistency flag: a historical summary of fleet state changes (used to ensure state machine consistency).

[0039] 2) The receiving node (whether it is a follower or a candidate) will decide whether to vote based on the following conditions: First, the priority of the degree of conflict is the primary consideration. The receiving node will only vote in favor when the candidate's conflict degree is higher than the conflict score of the receiving node; second, the validity of the term number must also be met. The candidate's term number (Term) must be greater than or equal to the current term number recorded by the receiving node; finally, log consistency requires that the candidate's log must be consistent with the local log of the receiving node in the latest entry to ensure that the status of each node in the system is synchronized.

[0040] 3) The election result is determined by whether the candidate obtains enough votes or the election times out. If the candidate obtains all votes or the election times out, the candidate will be promoted to the leader and broadcast the LeaderConfirm message. Then all robots will enter the Leader-Follower mode, and the leader will be responsible for coordinating the formation task. If the candidate does not obtain a majority of votes, it will return to the follower role and enter the next round of elections.

[0041] 4) Leader term maintenance is achieved by periodically sending heartbeat packets, which contain the leader's current path information (LeaderPath), target location, and conflict information. By sending this information, the leader can maintain its leadership and ensure that all robots in the formation update their status synchronously. The regular sending of heartbeat packets not only prevents frequent elections, but also ensures that each node continues to recognize the current leader, thereby maintaining the stability and consistency of the system.

[0042] 5) Leader resignation occurs when the leader's conflict level is significantly less than the set conflict threshold and its term has exceeded the basic term. In this case, the leader sends a LeaderStepDown message to all nodes to announce its resignation. After resignation, the leader stops sending heartbeat packets, but continues to execute the current formation instructions until the resignation process is completed. All follower nodes will exit the Leader-Follower mode after receiving the LeaderStepDown message.

[0043] (3) Detailed instructions for entering Leader-Follower mode. Robot with Follower status After receiving the robot with the identity of Leader After the heartbeat packet, the LeaderPath will be offset according to the preset formation. The latter is used as the local path point obtained by the front-end A* in the process of its own trajectory planning. The front-end path of each Follower maintains the formation in a macro sense, reducing the optimization pressure of the back-end. This makes up for the fact that the front-end path finding results of the robots in the original distributed system are too different, and it is difficult to maintain the formation by back-end optimization alone. This process ensures the consistency of the formation morphology and retains the local autonomous decision-making ability of the Follower.

[0044] The process of entering the Leader-Follower mode is described as follows. When the robot is a Follower, after receiving the heartbeat packet from the Leader, it adjusts the LeaderPath according to the preset formation and generates its own path by adding a bias. This modified path will be used as the result of its front-end A* pathfinding. In this way, the front-end paths of all Follower nodes remain consistent on a macro level, ensuring the stability of the formation and reducing the pressure of the back-end optimization process. This method makes up for the formation instability problem caused by the large differences in front-end pathfinding between robots in traditional distributed systems, and avoids the defect that the formation cannot be effectively maintained by relying solely on back-end optimization. In this way, not only the consistency of the formation is guaranteed, but also the local autonomous decision-making ability of the Follower robot is retained, enabling it to perform necessary personalized path planning while maintaining the formation.

[0045] In this step, there are two sources of local path points: one is the path points generated by the front end through the A* algorithm, and the other is the local path points provided by the leader after entering the leader-follower mode. After determining the source of the local path points, the path points are smoothed using quintic spline curves to reduce the curvature change of the path and improve the smoothness and feasibility of the path. Subsequently, the smoothed path is subjected to collision detection to ensure that it does not conflict with obstacles and meets the safety requirements in practical applications.

[0046] Step 4: Use the L-BFGS optimizer to optimize the path obtained by front-end sampling. After successful optimization, the system state switches to "EXEC_TRAJ"; (1) The quintic spline curve obtained by smoothing step 3 is represented by the MINCO trajectory. MINCO is a linearly complex mapping , decoupling spatial and temporal parameters to achieve flat output Segment Track In MINCO, the trajectory is divided into segments, each segment is represented by a 2s-1 order polynomial, where is the order of the trajectory. The duration of each segment is represented by the time vector , and the state of the intermediate point is represented by the vector Represented by linear mapping MINCO can decouple these parameters and achieve the spatiotemporal deformation of the trajectory. Specifically: (5); in, represents the midpoint between each pair of adjacent trajectory segments, It's time. are the starting time and ending time of the entire trajectory, respectively, where , that is, The duration of the segment trajectory, It is Section The connection points of the segment tracks, Indicates the duration of each trajectory. Victoria Segment Track By segment The polynomial representation of the order. The segment trajectory is defined as: (6); in, is the coefficient matrix, are the basis functions of the polynomial trajectory.

[0047] (2) Combining the motion stability and the adjustment of the system state, the following optimization objective function and its constraints are constructed, as follows: (7); in, is the temporal regularization parameter, is the total time of the M-segment trajectory, It is Segment track, track By variable optimization, represents the chained high-order derivatives of the dynamical system, and represent the initial point and the end point respectively. It's a track of Order derivatives. Continuous time constraints Indicates that the feasibility constraints of the system are met, including dynamic feasibility, obstacle avoidance, avoidance between robots, and formation maintenance. The inequality constraint is handled using a penalty function. Finally, the constrained optimization problem is converted into an unconstrained optimization problem, and the final optimization objective function is obtained: (8); in, Represents the weight vector of various cost functions or penalty terms. represents the energy cost, represents the total time cost function, represents the obstacle avoidance penalty term, represents the formation cost function, represents the penalty term for avoidance between groups, Denotes motion feasibility. The quasi-Newton method is called to solve the unconstrained optimization problem.

[0048] represents the energy cost, The third-order control input for the segment trajectory is written as , is the third-order derivative of the trajectory.

[0049] represents the total time cost function, which can be written as .

[0050] The position of each robot is regarded as the node of an undirected graph, and the robots are connected one by one to form the edge of the undirected graph. Then, the symmetric Laplace matrix of the undirected graph can be used. To quantify the maintenance of the formation, as follows: (9); in, is the symmetric Laplacian matrix of the desired formation, is the symmetric Laplacian matrix of the current formation.

[0051] Since this involves the trajectory of other robots , time alignment is required, the relative time of the trajectory , is the total duration of the trajectory, is the sampling time, , the global timestamps of other robot trajectories for: (10); So, (11).

[0052] represents the obstacle avoidance penalty term, and the obstacle avoidance penalty is calculated using the Euclidean Signed Distance Field (ESDF). Select the constraint point close to the obstacle ,as follows: (12); in, Make the safety threshold Indicates the shortest distance from the current position to the obstacle. represents the current position. Then, we can get: (13); in, are the orthogonal coefficients that follow the trapezoidal rule. represents the penalty term for avoidance between groups, which is calculated similarly to ,as follows: (14); in, represents the set of other robots, , global timestamp , Indicates the current position of other robots. , Indicates the threshold of safe distance between robots.

[0053] Indicates the feasibility of movement, specifically: (15).

[0054] in, and represents the weight coefficient of velocity term and acceleration term, and Indicates the maximum speed and acceleration.

[0055] This objective function will be optimized using the L-BFGS optimization algorithm. After the optimization process is successfully completed, the robot will enter the "EXEC_TRAJ" state and send the optimized trajectory to the controller for execution.

[0056] Step 5: The "execFSM" callback function is used to periodically trigger the state update of the finite state machine (FSM). Each time the callback is triggered, the system evaluates whether a state transition is required based on the current robot state. By periodically triggering the "execFSM" callback, the system can monitor and update the robot's state in real time to ensure that its behavior always meets the predetermined control objectives.

[0057] Step 6: The "checkCollision" callback function is used to periodically detect potential collisions between the robot's future trajectory and obstacles or other robots. By predicting the future path, the system can identify possible collision risks in advance based on the robot's local map and adjust the robot's behavior or state based on the detection results.

[0058] For example, when the system detects that a collision is about to occur, it may trigger a state transition to the "REPLAN_TRAJ" state and jump to step 3. At this time, the system recalculates the path and finds a new route to avoid the collision; or it transitions to the "EMERGENCY_STOP" state. If the collision cannot be avoided, the robot immediately stops moving to prevent an accident.

[0059] Two environments are designed based on the ROS platform, such as Figure 5 and Figure 6 The gray part is an obstacle. Figure 5 The line segment crossing the obstacle in represents the shortest path for the robot to reach the target point regardless of the environmental constraints. This path is used for heuristic calculation in the trajectory planning front end to guide the path search process. Figure 5 and Figure 6 The other curves in represent the robot's trajectory. The simulation results show that this method can effectively coordinate path optimization, obstacle avoidance and formation maintenance in complex environments, improve the stability and coordination ability of robot group motion, and provide strong support for the autonomous planning and control of multi-robot systems in complex environments.

[0060] Embodiment 2 This embodiment provides a multi-robot flexible formation control system based on a state machine, including: An initialization module, configured to initialize and update the occupancy grid map and generate a Euclidean signed distance field; The local path search module is configured to use the updated occupancy grid map as the search space, use the heuristic search algorithm to search for the local path points of each robot, and periodically judge the conflict degree of the robots on the Euclidean distance field. When the conflict degree exceeds the set threshold, the local path points of the robots are adjusted by using the collaborative mechanism of the leader path bias mapping and the local autonomous adjustment to achieve local path planning; The local path optimization template is configured to construct an optimization objective function with dynamic feasibility, obstacle avoidance, avoidance between robots and formation maintenance as constraint costs, and optimize the local path points to minimize the objective function to obtain the optimized path; The hybrid path planning module is configured to periodically detect potential collisions of the robot based on the optimized path walking process and re-plan the local path according to the detection results.

[0061] The examples and application scenarios implemented by the above modules and corresponding steps are the same, but are not limited to the contents disclosed in the above embodiment 1. It should be noted that the above modules as part of the system can be executed in a computer system such as a set of computer executable instructions.

[0062] The description of each embodiment in the above embodiments has different emphases. For parts not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0063] The proposed system can be implemented in other ways. For example, the system embodiment described above is only illustrative, and the division of the modules is only a logical function division. In actual implementation, there may be other division methods, such as multiple modules can be combined or integrated into another system, or some features can be ignored or not executed.

[0064] Embodiment 3 This embodiment provides a computer-readable storage medium having a computer program stored thereon. When the program is executed by a processor, the steps in the state machine-based multi-robot flexible formation control method as described in the first embodiment above are implemented.

[0065] Embodiment 4 This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps in the state machine-based multi-robot flexible formation control method as described in the first embodiment are implemented.

[0066] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of hardware embodiments, software embodiments, or embodiments combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage and optical storage, etc.) containing computer-usable program code.

[0067] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0068] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0069] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0070] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium, and when the program is executed, it can include the processes of the embodiments of the above-mentioned methods. The storage medium can be a disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM), etc.

[0071] Although the above describes the specific implementation mode of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without creative work are still within the scope of protection of the present invention.

Claims

1. A multi-robot flexible formation control method based on a state machine, characterized in that: include: Initialize and update the occupancy grid map and generate the Euclidean signed distance field; Based on the updated occupancy grid map as the search space, the heuristic search algorithm is used to search for the local path points of each robot, and the conflict degree of the robot is periodically judged on the Euclidean distance field. When the conflict degree exceeds the set threshold, the leader path bias mapping and the local autonomous adjustment collaborative mechanism are used to adjust the local path points of the robot to achieve local path planning. The optimization objective function is constructed with dynamic feasibility, obstacle avoidance, avoidance between robots and formation maintenance as constraint costs. The local path points are optimized to minimize the objective function and the optimized path is obtained. Based on the periodic detection of potential collisions of the robot during walking on the optimized path, the local path planning is re-performed according to the detection results.

2. The multi-robot flexible formation control method based on a state machine as claimed in claim 1, characterized in that: The updated occupancy grid map is used as the search space, and a heuristic search algorithm is used to search for the local path points of each robot, specifically: Based on the updated occupancy grid map as the search space, the current node is selected from the open list that has not been searched with the goal of minimizing the cost function; After moving the current node from the open list to the searched closed list, traverse the neighbor nodes of the current node and skip them if they are obstacles or in the closed list; If the neighbor node is not in the open list, add it to the open list and set the current node as its parent node, and calculate the actual cost and cost function from the starting point to the current node; If the neighbor node is already in the open list, compare the cost function of the neighbor node with the cost function of the current node, and determine 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 based on the comparison result; Until the path search succeeds and the algorithm terminates when the current node is the target node, or the path search fails when the open list is empty; Get the local path points of each robot.

3. The multi-robot flexible formation control method based on a state machine as claimed in claim 1, characterized in that: The optimization objective function is constructed with dynamic feasibility, obstacle avoidance, avoidance between robots and formation maintenance as constraint costs. The local path points are optimized to minimize the objective function and the optimized path is obtained. Specifically: Based on the quintic spline curve obtained by balancing the local path points, the MINCO trajectory is used to represent the quintic spline curve, and the M-segment local trajectory in the MINCO form is obtained; The optimization objective function is constructed with dynamic feasibility, obstacle avoidance, robot-to-robot avoidance, and formation maintenance as constraint costs; The constrained optimization problem is converted into an unconstrained optimization problem to obtain the final optimization objective function, and the quasi-Newton method is used to solve the final optimization objective function to obtain the optimization path.

4. The multi-robot flexible formation control method based on a state machine as claimed in claim 3, characterized in that: The final optimization objective function includes energy cost, total time cost function, obstacle avoidance penalty term, formation cost function, penalty term for avoidance between groups and motion feasibility.

5. The multi-robot flexible formation control method based on a state machine as claimed in claim 1, characterized in that: When the conflict level exceeds the conflict threshold, the local path point of the robot is adjusted by using the collaborative mechanism of the leader path bias mapping and local autonomous adjustment, specifically: Obtain the distance to the nearest obstacle from the robot's own position and the gradient of the distance in the Euclidean distance field, and calculate the conflict degree of each robot in combination with the formation cost and its gradient; When it is detected that the conflict level of any robot exceeds the conflict threshold, the Raft dynamic election algorithm is used to select the robot with the highest conflict level from the formation as the leader of the formation; According to the preset formation, the leader's path is offset based on the heartbeat packet of the formation leader to adjust the follower's local path points and generate the follower's adjusted path.

6. The multi-robot flexible formation control method based on a state machine as claimed in claim 5, characterized in that: When it is detected that the conflict level of any robot exceeds the conflict threshold, the robot with the highest conflict level is selected from the formation as the leader of the formation through the Raft dynamic election algorithm, specifically: When the robot's conflict level exceeds the conflict threshold, the robot will switch to a leader candidate and initiate an election request to all nodes in the formation; The receiving node will decide whether to vote based on the conflict score priority, term number validity, and log consistency: If a candidate obtains all votes or the election times out, it is promoted to the leader and broadcasts a LeaderConfirm message, and all robots enter the Leader-Follower mode. If it does not obtain a majority of votes, it returns to being a follower and starts the next round of elections. The leader periodically sends heartbeat packets containing the path points, target locations, and conflict information of its own front-end A* to maintain leadership and synchronize status; When the leader conflict level is less than the conflict threshold and the term often exceeds the basic term, the leader sends a LeaderStepDown message to all nodes and stops sending heartbeat packets, but continues to execute the current formation command until the resignation is completed; After all Follower nodes receive the LeaderStepDown message, they exit the Leader-Follower mode.

7. The multi-robot flexible formation control method based on a state machine as claimed in claim 6, characterized in that: The receiving node will decide whether to vote based on the conflict scoring priority, term number validity, and log consistency, specifically: Conflict score priority: when the conflict level of the leader candidate is higher than the conflict score of the receiving node; Term number validity: The candidate's term number must be greater than or equal to the current term number recorded by the receiving node; Log consistency: The candidate’s log must be consistent with the receiving node’s local log on the latest entries; If all three conditions above are met, the receiving node votes in favor.

8. A multi-robot flexible formation control system based on a state machine, characterized in that: include: An initialization module, configured to initialize and update the occupancy grid map and generate a Euclidean signed distance field; The local path planning module is configured to use the updated occupancy grid map as the search space, use the heuristic search algorithm to search for the local path points of each robot, and periodically judge the conflict degree of the robots on the Euclidean distance field. When the conflict degree exceeds the set threshold, the local path points of the robots are adjusted by using the collaborative mechanism of the leader path bias mapping and the local autonomous adjustment to achieve local path planning; The local path optimization template is configured to construct an optimization objective function with dynamic feasibility, obstacle avoidance, avoidance between robots and formation maintenance as constraint costs, and optimize the local path points to minimize the objective function to obtain the optimized path; The hybrid path planning module is configured to periodically detect potential collisions of the robot based on the optimized path during walking, and re-plan the local path based on the detection results on the Euclidean distance field.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps in the state machine-based multi-robot flexible formation control method as described in any one of claims 1 to 7 are implemented.

10. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the steps in the state machine-based multi-robot flexible formation control method as described in any one of claims 1-7 are implemented.

Images