Multi-robot Elastic Formation Control Method and System Based on State Machine

Abstract

The present invention belongs to the fields of distributed systems and multi-robot formation control, and provides a multi-robot elastic formation control method and system based on a state machine. The occupied grid map is initialized and updated to generate an Euclidean signed distance field; based on the updated occupied grid map as the search space, a heuristic search algorithm is used to search for local path points of each robot to generate a local path; an optimization objective function is constructed with dynamic feasibility, obstacle avoidance, avoidance between robots, and formation shape maintenance as constraint costs, and the local path is optimized to obtain an optimized path; during the walking process based on the optimized path, the potential collisions of the robots are periodically detected, and on the Euclidean compliance distance field, according to the detection results, a cooperative mechanism of leader path offset mapping and local autonomous adjustment is used to perform hybrid path planning for the multi-robots; the present invention can dynamically switch the system state according to environmental changes and realize the cooperative control of formation maintenance and obstacle crossing.

Description

Technical Field

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

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

[0003] With the continuous improvement of social informatization and intelligence levels, formation control methods for multi-robot systems have developed rapidly. The robots in the formation can efficiently and flexibly execute various tasks. Especially in complex outdoor environments and urban areas, formation control methods show broad application potential in fields such as search and rescue, collaborative mapping, and package delivery.

[0004] In the formation control of multi-robot systems, there are mainly two system frameworks: distributed and centralized. Under the centralized architecture, all robots are uniformly planned and assigned formation information by a center. The formation is mostly regarded as a whole in the form of an envelope for path planning, which will discard a large number of feasible solutions, or sample the map according to the formation configuration to generate a path, and the caging process is extremely time-consuming. Even if some scholars have carried out adaptive granularity discretization, 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 among the 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 bottlenecks, the robots make autonomous decisions and negotiate task allocation, and it has strong adaptability. However, for this reason, the local optimal solutions of individuals in the distributed framework are difficult to match the global optimal solutions of the former in terms of formation maintenance. Summary of the Invention

[0005] To solve the above problems, the present invention proposes a multi-robot elastic formation control method and system based on a state machine. The present invention can dynamically switch the system state according to environmental changes, realize the coordinated control of formation maintenance and obstacle crossing, so as to solve the problem that the local optimal solutions in the distributed framework are 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, the first solution of the present invention provides a multi-robot elastic formation control method based on a state machine, and adopts the following technical solutions:

[0007] The multi-robot elastic formation control method based on a state machine includes:

[0008] Initialize and update the occupancy grid map and generate the Euclidean signed distance field;

[0009] Based on 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 compliance distance field. When the conflict degree exceeds the set threshold, use the collaborative mechanism of the leader path offset mapping and local autonomous adjustment to adjust the local path points of the robots to achieve local path planning;

[0010] Construct an optimization objective function with dynamic feasibility, obstacle avoidance, avoidance between robots, and formation shape maintenance as constraint costs, and optimize the local path points with the minimum of the optimization objective function to obtain the optimized path;

[0011] Periodically detect potential collisions of the robots during the walking based on the optimized path, and re - conduct local path planning according to the detection results.

[0012] Further, the specific method for using the heuristic search algorithm to search for the local path points of each robot based on the updated occupancy grid map as the search space is as follows:

[0013] Based on the updated occupancy grid map as the search space, with the goal of minimizing the cost function, select the current node from the un - searched open list;

[0014] After moving the current node from the open list to the searched closed list, traverse the neighbor nodes of the current node. If the neighbor node is an obstacle or in the closed list, skip it;

[0015] If the neighbor node is not in the open list, add it to the open list, set the current node as its parent node, and calculate the actual cost from the starting point to the current node and the cost function;

[0016] If the neighbor node is already in the open list, compare the cost function of the neighbor node with that 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 according to the comparison result;

[0017] Until the current node is the target node and the path search is successful and the algorithm terminates, or the open list is empty and the path search fails;

[0018] Obtain the local path points of each robot.

[0019] Further, the specific method for constructing an optimization objective function with dynamic feasibility, obstacle avoidance, avoidance between robots, and formation shape maintenance as constraint costs, and optimizing the local path points with the minimum of the optimization objective function to obtain the optimized path is as follows:

[0020] Based on the quintic spline curve obtained by local path point balancing processing, the MINCO trajectory is used to represent the quintic spline curve, and M segments of local trajectories in MINCO form are obtained.

[0021] Construct an optimization objective function with dynamic feasibility, obstacle avoidance, avoidance between robots, and formation shape maintenance as constraint costs.

[0022] Convert the constrained optimization problem into an unconstrained optimization problem to obtain the final optimization objective function, and use the quasi-Newton method to solve the final optimization objective function to obtain the optimized path.

[0023] 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 motion feasibility.

[0024] Furthermore, when the conflict degree exceeds the conflict threshold, the local path points of the robot are adjusted by the collaborative mechanism of leader path offset mapping and local autonomous adjustment. Specifically:

[0025] Obtain the distance to the nearest obstacle from its own position and the gradient of the distance on the Euclidean compliance distance field, and calculate the conflict degree of each robot itself by combining the formation cost and its gradient.

[0026] When it is detected that the conflict degree of any robot exceeds the conflict threshold, the robot with the highest conflict degree is selected as the leader of the formation through the Raft dynamic election algorithm.

[0027] According to the preset formation shape, the leader path is increased by a bias based on the heartbeat packet of the formation leader, so as to adjust the local path points of the followers and generate the adjusted local path of the followers.

[0028] Furthermore, when it is detected that the conflict degree of any robot exceeds the conflict threshold, the robot with the highest conflict degree is selected as the leader of the formation through the Raft dynamic election algorithm. Specifically:

[0029] When the conflict degree of the robot exceeds the conflict threshold, the robot will switch to a leadership candidate and send an election request to all nodes in the formation.

[0030] The receiving node will decide whether to vote according to the conflict scoring priority, term number validity, and log consistency:

[0031] If the candidate obtains all votes or the election times out, it will be promoted to the leader and broadcast the LeaderConfirm message, and all robots will enter the Leader-Follower mode; if it does not obtain a majority of votes, it will return to a follower and start a new round of elections.

[0032] 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;

[0033] 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;

[0034] After all Follower nodes receive the LeaderStepDown message, they exit the Leader-Follower mode.

[0035] Furthermore, the receiving node will decide whether to vote based on the conflict scoring priority, term number validity, and log consistency, specifically:

[0036] Conflict score priority: when the conflict level of the leader candidate is higher than the conflict score of the receiving node;

[0037] Term number validity: The candidate's term number must be greater than or equal to the current term number recorded by the receiving node;

[0038] Log consistency: The candidate’s log must be consistent with the receiving node’s local log on the latest entries;

[0039] If all three conditions above are met, the receiving node votes in favor.

[0040] 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:

[0041] The multi-robot flexible formation control system based on state machine includes:

[0042] An initialization module, configured to initialize and update the occupancy grid map and generate a Euclidean signed distance field;

[0043] 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;

[0044] 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;

[0045] The hybrid path planning module is configured to periodically detect potential collisions of the robot during the process of walking along the optimized path, and re - perform local path planning according to the detection results.

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

[0047] A computer - readable storage medium, on which a computer program is stored, and when the program is executed by a processor, it implements the steps in the state - machine - based multi - robot elastic formation control method described in the first aspect above.

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

[0049] A computer device includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the steps in the state - machine - based multi - robot elastic formation control method described in the first aspect above.

[0050] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0051] The present invention designs a hierarchical control architecture driven by a finite - state machine, combines front - end A* sampling and a back - end L - BFGS optimizer, constructs a spatio - temporal decoupled trajectory representation method based on MINCO, reduces the path smoothing computational complexity to a linear order while ensuring kinematic feasibility; introduces the Raft consensus algorithm to construct a conflict - driven leader dynamic election mechanism, quantifies the conflict degree of formation maintenance and obstacle avoidance based on the Euclidean signed distance field (ESDF), triggers high - conflict nodes to obtain coordination rights preferentially, and realizes intelligent transfer of control rights through tenure timeout and active abdication mechanisms; proposes a Leader - Follower hybrid path planning strategy through a collaborative mechanism of leader path offset mapping and local autonomous adjustment; effectively solves the core problems that it is difficult to guarantee global formation consistency with local optimal solutions in traditional distributed frameworks, and the high computational complexity and poor scalability in centralized frameworks. Through the integration of multi - modal environment perception, hierarchical trajectory optimization, and dynamic role assignment mechanisms, the present invention realizes the collaborative optimization of the efficiency and robustness of formation control in complex dynamic scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] The schematic diagram of the specification attached to the present invention is used to provide a further understanding of the present invention. The illustrative embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention.

[0053] Figure 1 It is a flowchart of a state - machine - based multi - robot elastic formation control method in an embodiment of the present invention;

[0054] Figure 2 is the overall block diagram of the distributed system architecture in the embodiment of the present invention;

[0055] Figure 3 is the schematic diagram of the state transition of the finite state machine in the embodiment of the present invention;

[0056] Figure 4 is the flowchart of the raft dynamic election algorithm in the embodiment of the present invention;

[0057] Figure 5 is the formation maintenance effect diagram in the first complex environment in the embodiment of the present invention;

[0058] Figure 6 is the formation maintenance effect diagram in the second complex environment in the embodiment of the present invention. Detailed implementation manners

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

[0060] It should be noted that the following detailed description is exemplary and is intended to provide further description of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs.

[0061] It should be noted that the terms used herein are only for describing specific implementation manners and are not intended to limit the 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, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0062] In the case of no conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0063] Embodiment 1

[0064] Such as Figure 1As shown, this embodiment provides a multi-robot elastic formation control method based on a state machine. This embodiment takes the application of this method to a server as an example for illustration. It can be understood that this method can also be applied to a terminal, or 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, web 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 thereto. The terminal and the server can be directly or indirectly connected through wired or wireless communication methods, and this application does not make any restrictions here. In this embodiment, the method includes the following steps:

[0065] The multi-robot elastic formation control method based on a state machine includes:

[0066] Initialize and update the occupancy grid map and generate an Euclidean signed distance field;

[0067] Based on the updated occupancy grid map as the search space, use a heuristic search algorithm to search for local path points of each robot, and periodically judge the conflict degree of the robots on the Euclidean signed distance field. When the conflict degree exceeds the set threshold, use the collaborative mechanism of leader path offset mapping and local autonomous adjustment to adjust the local path points of the robots to achieve local path planning;

[0068] Construct an optimization objective function with dynamic feasibility, obstacle avoidance, avoidance between robots, and formation shape maintenance as constraint costs, and optimize the local path points with the minimum of the optimization objective function to obtain an optimized path;

[0069] Periodically detect potential collisions of the robots during the walking process based on the optimized path, and re-perform local path planning according to the detection results.

[0070] As Figure 2As shown in the figure, this embodiment proposes a distributed system architecture, which mainly consists of three parts: a perception module, a planner, and a controller. Information is exchanged between robots through a WiFi wireless local area network. The perception module consists of a visual odometer composed of an RGBD camera and an IMU module, which serves as the main data source for SLAM to realize the estimation of the robot's own pose and the perception of 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 the free conversion between the distributed and Leader-Follower modes. Based on the A* pathfinding in the front end, using the MINCO curve and soft constraint methods, the constrained optimization problem is converted into an unconstrained optimization problem, and then the L-BFGS optimizer is used for the back-end trajectory optimization. The controller is responsible for receiving the optimized trajectory information and executing the corresponding control instructions to achieve the coordinated control of formation maintenance and obstacle crossing. The state transition of this architecture is as Figure 3 shown, and the detailed steps are as follows:

[0071] Step 1: Initialize the map, load the expected formation, initialize the robot pose, set the formation target position, and perform the initialization of the finite state machine. The system state changes from "INIT" to "WAIT_TARGET";

[0072] (1) Initialize the map. Since it is for an unknown environment, the map information only contains boundary information. The robot obtains the surrounding environment information and its own pose information in real time through the sensor module (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 an Euclidean signed distance field (ESDF) map.

[0073] (2) Load the predefined formation. The formation is represented by a relative position matrix. Set the expected formation layout according to the task requirements , where is the number of robots required to form the formation, and is the expected position coordinate of robot .

[0074] (3) According to the new occupancy grid map, assign the initial pose and the target position to each robot , and the setting of the target position should conform to the formation.

[0075] Among them, for the later planning requirements, it is necessary to calculate and generate an Euclidean signed distance field (ESDF) based on the three-dimensional grid map constructed by SLAM. Traverse all voxels to mark the obstacle voxels, and the set of occupied voxel coordinates is , where is the th obstacle voxel. For each free voxel , obtain the minimum Euclidean distance to all obstacle voxels .

[0076] Table 1 Finite State Machine States and Functions

[0077]

[0078] Step 2: After the robot receives the target position , the system state changes from "WAIT_TARGET" to "SEQUENTIAL_START". After generating a short collision-free trajectory, the system state finally switches to "EXEC_TRAJ" to execute the trajectory. This step is only executed after receiving the global target point;

[0079] Step 3: During the movement based on the short collision-free trajectory in Step 2, the system state changes from "EXEC_TRAJ" to "REPLAN_TRAJ". Each robot samples the path points using A* to generate local path points;

[0080] The specific steps for A* sampling are as follows:

[0081] (1) Enter the core optimization state, and the system state changes from "EXEC_TRAJ" to "REPALN_TRAJ".

[0082] (2) Load the updated occupancy grid map as the search space for the A* algorithm. Select the Euclidean distance as the heuristic function:

[0083] (1);

[0084] Among them, is the position of the current node.

[0085] During the execution of the A* algorithm, first initialize the open list and the closed list . The open list stores the nodes to be explored and is initialized to only contain the starting point. The closed list stores the explored nodes and is initialized to be empty.

[0086] Next, select the node with the minimum cost function from the open list as the current node. Among them, 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.

[0087] After moving the current node from the open list to the closed list, traverse its neighbor nodes. If a neighbor node is an obstacle or in the closed list, skip it; if a 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 and ; if the neighbor node is already in the open list, compare the cost function of the neighbor node with that of the current node, and determine whether to update the current node, the actual cost from the start point to the current node, and the cost function of the current node according to the comparison result; that is, when then update the current node to the neighbor node , and update the actual cost from the start point to the current node to and the cost function of the current node to , otherwise continue traversing.

[0088] Repeat the above process until the path search is successful and the algorithm terminates when the current node is the target node, or the path search fails when the open list is empty.

[0089] Meanwhile, 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.

[0090] (1) According to the ESDF map constructed by the robot, the distance to the nearest obstacle from its own position can be obtained and the gradient of the distance . The ESDF map provides the distance information from each position to the nearest obstacle. By calculating the gradient of this distance field, the gradient direction of the current robot position can be obtained, and this direction points to the position of the nearest obstacle. At the same time, the formation cost and its gradient 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 the position change. Then the cosine value of the included angle between the two gradient directions can be obtained.

[0091] (2);

[0092] Then, use the sigmoid function to map the cosine value of the included angle to the conflict coefficient .

[0093] (3);

[0094] Among them, and are adjustment parameters. Adjust the conflict coefficient With the 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 formation cost size and the distance to the current nearest obstacle. Each robot calculates its own conflict degree This conflict degree mainly reflects the conflict between the robot's obstacle avoidance and formation maintenance tasks. The calculation method is as follows, where Is the proportionality coefficient,

[0095] (4);

[0096] (2)When a trigger event occurs, that is, when the conflict degree 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 selects the robot with the highest conflict degree in the formation as the leader of the formation through the Raft dynamic election algorithm. The specific implementation process is as Figure 4 Shown. First, by evaluating the conflict degree of each robot, identify the robot with the most serious conflict; then, start the Raft algorithm for leader election to ensure the selection of the most adaptable leader in the current environment. The specific implementation method is as follows,

[0097] 1) When the conflict score of a robot exceeds the preset threshold, the robot will switch to the role of leader candidate (Candidate) and send a RequestVote request to all nodes in the formation. This request includes the current term number (Term), that is, a globally increasing election round identifier used to distinguish different election cycles; the real-time conflict score of the candidate, reflecting the task conflict degree faced by the robot; and the log consistency identifier, that is, a summary of the historical records of formation state changes, ensuring the consistency of the state machines of all robots in the system and guaranteeing that each node can effectively synchronize state changes.

[0098] The RequestVote request includes the current term number (Term): a globally increasing election round identifier, the candidate conflict score: the real-time And the log consistency identifier: a summary of the historical records of formation state changes (used to ensure state machine consistency).

[0099] 2) The receiving node (whether it is a Follower or a Candidate) will decide whether to vote based on the following conditions: First, the conflict level priority is the primary consideration. Only when the conflict level of the candidate is higher than the conflict score of the receiving node will the receiving node vote in favor. Second, the validity of the term number must also be satisfied. The term number of the candidate must be greater than or equal to the current term number recorded by the receiving node. Finally, the log consistency requires that the candidate's log must be consistent with the local log of the receiving node on the latest entry to ensure the state synchronization of all nodes in the system.

[0100] 3) The determination of the election result depends on whether the candidate obtains enough votes or times out. If the candidate obtains all the votes or the election times out, the candidate will be promoted to the Leader and broadcast the LeaderConfirm message. Subsequently, 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.

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

[0102] 5) The leader's 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 will continue 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.

[0103] (3) Detailed description of entering the Leader-Follower mode. The robot with the identity of Follower After receiving the heartbeat packet from the robot with the identity of Leader will add an offset to the LeaderPath according to the preset formation Subsequently, as the local path points obtained by the front-end A* in its own trajectory planning process, the front-end paths of each Follower maintain a formation macroscopically, reducing the pressure on the back-end optimization. It makes up for the deficiency that in the original distributed system, the gap between the front-end path-finding results of robots is too large, and it is difficult to maintain the formation only by back-end optimization. This process not only ensures the consistency of the formation shape but also retains the local autonomous decision-making ability of the Follower.

[0104] The process of entering the Leader-Follower mode is described as follows. When the identity of the robot is 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 an offset. This modified path will be used as the result of its front-end A* path-finding. In this way, the front-end paths of all Follower nodes are consistent macroscopically, ensuring the stability of the formation and reducing the pressure in the back-end optimization process. This method makes up for the problem of formation instability caused by excessive differences in front-end path-finding among robots in the traditional distributed system and avoids the defect that it is impossible to effectively maintain the formation shape only relying on back-end optimization. In this way, not only the consistency of the formation shape is ensured, 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.

[0105] In this step, there are two sources of local path points: one is the path points generated by its own 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 a quintic spline curve to reduce the curvature change of the path and improve the smoothness and feasibility of the path. Subsequently, collision detection is performed on the smoothed path to ensure that it has no conflict with obstacles and meets the safety requirements in practical applications.

[0106] Step 4: Optimize the path obtained by front-end sampling using the L-BFGS optimizer. After successful optimization, the system state switches to "EXEC_TRAJ";

[0107] (1) Represent the quintic spline curve obtained by smoothing in Step 3 using the MINCO trajectory. MINCO decouples the spatial and time parameters through a mapping with linear complexity to achieve flat output , and realizes flat output segment trajectory . In MINCO, the trajectory is divided into segments, each segment is represented by a polynomial of order 2s - 1, where is the order of the trajectory. The duration of each segment is determined by the time vector is represented, and the state of the intermediate points is represented by the vector . Through the linear mapping , MINCO can decouple these parameters to achieve spatio-temporal deformation of the trajectory. Specifically:

[0108] (5);

[0109] Among them, represents the intermediate points between each pair of adjacent trajectory segments, is the time, are the starting time and ending time of the entire trajectory respectively. Among them, , that is, the duration of the th segment of the trajectory, is the connection point between the th and the th segments of the trajectory, represents the duration of each segment of the trajectory. The -dimensional segment trajectory is represented by a piecewise -order polynomial. The

[0110] (6);

[0111] Among them, is the coefficient matrix, is the basis function of the polynomial trajectory.

[0112] (2) Combining motion stability and adjustment of system states, the following optimization objective function and its constraints are constructed as follows:

[0113] (7);

[0114] Among them, is the time regularization parameter, is the total time of the M segment trajectories, is the th segment of the trajectory, and the trajectory is optimized by the variable represents the chain higher-order derivative of the dynamic system, and represent the initial point and the end point respectively, is the th derivative of the trajectory . The continuous time constraint Indicates that the feasibility constraints of the system are satisfied, including dynamic feasibility, obstacle avoidance, avoidance between robots, and formation shape maintenance. This inequality constraint is handled using a penalty function. Finally, the constrained optimization problem is transformed into an unconstrained optimization problem to obtain the final optimization objective function:

[0115] (8);

[0116] where, 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, represents motion feasibility. The quasi-Newton method is called to solve this unconstrained optimization problem.

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

[0118] represents the total time cost function and can be written as .

[0119] represents the formation cost function. By taking the positions of each robot as the nodes of an undirected graph and connecting the robots pairwise to form the edges of the undirected graph, the symmetric Laplacian matrix of this undirected graph can be used to quantify the maintenance of the formation shape as follows:

[0120] (9);

[0121] where, is the symmetric Laplacian matrix of the desired formation, is the symmetric Laplacian matrix of the current formation.

[0122] Since this term involves the trajectories of other robots , time alignment is required. The relative time of its own trajectory, is the total duration of this segment of the trajectory, is the sampling time, , and the global timestamp of the trajectories of other robots is:

[0123] (10);

[0124] Then,

[0125] (11).

[0126] Denote the obstacle avoidance penalty term, and calculate the obstacle avoidance penalty using the Euclidean signed distance field (ESDF). Select the constraint points close to the obstacle , as follows:

[0127] (12);

[0128] Among them, is the safety threshold, denotes the closest distance from the current position to the obstacle, denotes the current position. Then, we can obtain:

[0129] (13);

[0130] Among them, is the orthogonal coefficient following the trapezoidal rule. Denote the penalty term for avoidance between groups, and the calculation method is similar to , as follows:

[0131] (14);

[0132] Among them, denotes the set of other robots, , the global timestamp , denotes the position of other robots at this time, , denotes the threshold of the safety distance between robots.

[0133] Denote the motion feasibility, specifically:

[0134] (15).

[0135] Among them, and denote the weight coefficients of the velocity term and the acceleration term, and denote the maximum velocity and the maximum acceleration.

[0136] This objective function will be optimized by 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.

[0137] Step 5: The "execFSM" callback function is used to periodically trigger the state update of the finite state machine (FSM). At each callback trigger, the system evaluates whether a state transition is required based on the current state of the robot. 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 conforms to the predefined control objectives.

[0138] Step 6: The "checkCollision" callback function is used to periodically detect potential collisions between the future trajectory of the robot 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 according to the detection results.

[0139] For example, when the system detects an impending collision, it may trigger a state transition to the "REPLAN_TRAJ" state and jump to Step 3, where the system recalculates the path to find a new route to avoid the collision; or it may transition to the "EMERGENCY_STOP" state, in which case the robot immediately stops moving to prevent an accident when the collision cannot be avoided.

[0140] Two environments are designed respectively based on the ROS platform, as Figure 5 and Figure 6 shown. The gray parts are obstacles, Figure 5 and the line segment passing through the obstacle in Figure 5 and Figure 6 represents the shortest path for the robot to reach the target point when ignoring environmental constraints. This path is used for heuristic calculations at the front end of trajectory planning to guide the path search process. The other curves in

[0141] represent the trajectories of the robots. The simulation results show that this method can effectively coordinate path optimization, obstacle avoidance, and formation maintenance in complex environments, improving the stability and cooperation ability of the multi-robot group movement, and providing strong support for the autonomous planning and control of multi-robot systems in complex environments.

[0141] Example 2

[0142] This embodiment provides a multi-robot elastic formation control system based on a state machine, including:

[0143] An initialization module configured to initialize and update the occupancy grid map and generate an Euclidean signed distance field;

[0144] The local path search module is configured to use the updated occupancy grid map as the search space, and utilize a heuristic search algorithm to search for the local path points of each robot. It periodically judges the conflict degree of the robot on the Euclidean compliance distance field. When the conflict degree exceeds the set threshold, it uses the collaborative mechanism of leader path offset mapping and local autonomous adjustment to adjust the local path points of the robot, so as to achieve local path planning;

[0145] The local path optimization template is configured to construct an optimization objective function with dynamic feasibility, obstacle avoidance, avoidance between robots, and formation shape maintenance as constraint costs, and optimize the local path points with the minimum of the optimization objective function to obtain an optimized path;

[0146] The hybrid path planning module is configured to periodically detect potential collisions of the robot during walking based on the optimized path, and re-perform local path planning according to the detection results.

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

[0148] In the above embodiments, the descriptions of each embodiment have their own emphases. For the parts not detailed in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0149] The proposed system can be implemented in other ways. For example, the system embodiments described above are merely illustrative. For example, the above module division is only a logical function division. In actual implementation, there can be other division methods. For example, multiple modules can be combined or integrated into another system, or some features can be ignored or not executed.

[0150] Embodiment Three

[0151] This embodiment provides a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, it implements the steps in the state machine-based multi-robot elastic formation control method described in the first embodiment above.

[0152] Embodiment Four

[0153] This embodiment provides a computer device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the steps in the state machine-based multi-robot elastic formation control method described in the first embodiment above.

[0154] Those skilled in the art should understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a hardware embodiment, a software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memories and optical memories, etc.) that contain computer-usable program code.

[0155] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present invention. It should be understood that each flow and / or block in the flowchart and / or block diagram, as well as the combination of flows 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 the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0156] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing devices to work in a specific manner, such that the instructions stored in the computer-readable memory generate a manufactured article including instruction means that implement the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0157] These computer program instructions can also be loaded onto a computer or other programmable data processing devices, such that a series of operation steps are executed on the computer or other programmable devices to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable devices provide steps for implementing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0158] Those of ordinary skill in the art can understand that all or part of the processes of implementing the above-described embodiment methods can be completed by instructing relevant hardware through a computer program. The program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the above-described method embodiments. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM), etc.

[0159] Although the specific implementation manners of the present invention have been described above in conjunction with the accompanying drawings, they are not limitations on the protection scope of the present invention. Those skilled in the art should understand that, based on the technical solutions of the present invention, various modifications or deformations that can be made by those skilled in the art without creative efforts are still within the protection scope 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, a heuristic search algorithm is used to search for 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 is terminated 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; The conflict degree of the robot is periodically judged on the Euclidean distance field. When the conflict degree exceeds the set threshold, the local path point of the robot is adjusted by using the collaborative mechanism of the leader's path bias mapping and local autonomous adjustment 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. 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; Convert the constrained optimization problem into an unconstrained optimization problem to obtain the final optimization objective function, use the quasi-Newton method to solve the final optimization objective function, and obtain the optimization path; 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 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.

3. 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.

4. The multi-robot flexible formation control method based on a state machine as claimed in claim 3, 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.

5. The multi-robot flexible formation control method based on a state machine as claimed in claim 4, 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.

6. 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 search for local path points of each robot using a heuristic search algorithm based on the updated occupancy grid map as the search space, 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 is terminated 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; The conflict degree of the robot is periodically judged on the Euclidean distance field. When the conflict degree exceeds the set threshold, the local path point of the robot is adjusted by using the collaborative mechanism of the leader's path bias mapping and 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. 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; Convert the constrained optimization problem into an unconstrained optimization problem to obtain the final optimization objective function, use the quasi-Newton method to solve the final optimization objective function, and obtain the optimization 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.

7. 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 5 are implemented.

8. 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-5 are implemented.

Images