Multi-robot cluster cooperative formation cooperative control method and control system based on graph optimization

Abstract

The invention discloses a multi-robot cluster cooperative formation cooperative control method and system based on graph optimization, and the method comprises the steps: firstly storing the poses of robots under a predetermined formation structure, pre-calculating the control information of all robots according to the poses of the robots under the current formation structure in each motion control period, and carrying out the pre-calculation of the control information of all robots; taking the data as nodes of the graph optimization model; and then calculating updated poses according to the state transition equation, and constructing the updated poses of all the robots and the poses of the robots under the predetermined formation structure into constraints of a graph optimization model to obtain optimal control information of all the robots in the cluster. Through the method, the stable and accurate formation shape can be kept, the relative positions of the robots in the cluster are kept constant in a conventional motion mode of the robot cluster, and when the whole formation structure of the cluster is interfered, the formation shape can be quickly recovered to the preset formation shape, so that the formation quality of the robot cluster is improved. And the stability and anti-interference performance of formation control are ensured.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-robot formation cooperative control, and particularly relates to a multi-robot formation cooperative control method based on graph optimization. Background Art

[0002] Mobile robots play a very important role in current logistics, warehousing, and industrial production. A robot cluster composed of multiple mobile robots can cooperate to complete tasks that a single robot cannot complete, especially in scenarios such as transporting and handling large parts, such as train car bodies, aircraft wings, and large wind turbine blades. During the process of multi-robot cluster cooperating to handle large parts, maintaining the relative poses between robots (i.e., formation control and formation maintenance) is crucial. Otherwise, due to the pose deviation between multi-robots, rigid stress may be generated during movement, which may damage the transported workpiece or cause a failure of the robot body, resulting in unforeseen consequences and economic losses.

[0003] In the existing technical solutions for multi-robot formation control and formation maintenance, refer to the following two patents:

[0004] For example, a multi-robot formation control method based on double closed-loop adaptive PID with the authorization announcement number CN115236972B. This patent adopts a leader + follower multi-robot formation control method. According to the movement of the leader robot, virtual movement trajectories of multiple follower robots are generated, and each actual follower robot real-time tracks the virtual follower robot, thereby realizing formation control during the movement of multi-robots. This patent adopts a PID formation control method for all multi-robots. However, the parameter adjustment of its model is complex, and it is difficult to achieve a perfect balance between dynamic performance and steady-state performance. And in this patent, the formation maintenance between the follower robot and the leader robot is ensured, but the relative poses between follower robots cannot be guaranteed.

[0005] A multi-robot formation tracking sampling control method and system based on lidar with the authorization announcement number CN114326731B. This patent adopts a formation tracking control method based on distance. The specific method is to sense the relative distance from the lidar on each robot to the surrounding robots to determine the relative position relationship, and then use the PI control method to maintain the formation shape of multi-robots approaching the expected formation shape as the goal, thereby realizing the formation tracking control of multi-robots. The disadvantages of this method are as follows: 1. The relative positions are sensed between robots through lidar, and the accuracy of this measured distance cannot be guaranteed, thus affecting subsequent formation control; 2. The patent adopts the PI control method to calculate the movement speed of each robot, similar to the problem of the PID control method, with a complex model, complex parameter adjustment, and inability to achieve equilibrium in terms of dynamic performance and steady-state performance. Summary of the Invention

[0006] In view of the above background, as well as the defects and improvement requirements of the prior art, the present invention provides a stable and reliable multi-robot cluster cooperative formation control method based on graph optimization, aiming to control a mobile robot cluster to maintain a stable and precise formation shape, so that the relative positions between the robots within the cluster remain constant during conventional motion modes (such as translational motion and rotational motion), and when the overall formation structure of the cluster is disturbed, it can quickly return to the predetermined formation shape, thereby ensuring the stability and anti-interference ability of the formation control.

[0007] The present invention provides a multi-robot cluster cooperative formation control method based on graph optimization. The control method models the motion control problem of the multi-robot cluster as a graph optimization problem, and realizes the stable cooperative formation control of the cluster by solving the graph optimization problem. The control method includes the following steps:

[0008] S1: Arrange all the robots in the cluster at predetermined positions in advance to form a predetermined formation structure of the cluster;

[0009] In each control cycle, the following steps are executed:

[0010] S2: Obtain the current pose information of all the robots in the system to form the current formation structure, and calculate the pose deviation between the current formation structure and the predetermined formation structure;

[0011] S3: Construct a graph optimization model for multi-robot cluster formation control: Model the robot control speed information as nodes in graph optimization, and model the pose deviation in S2 as constraints in graph optimization;

[0012] S4: Iteratively solve the graph optimization problem constructed in S3. By gradually optimizing the nodes in the graph optimization model, that is, the control speed information of each robot, the error between the current formation structure and the predetermined formation structure is continuously reduced, and finally the optimal control speed information is obtained;

[0013] S5: Distribute the optimal control speed information obtained in S4 to each robot in the cluster to complete the robot motion drive in the current control cycle.

[0014] Furthermore, both the predetermined formation structure and the current formation structure include a virtual robot node, and its functions are as follows:

[0015] During translational motion, the orientation of the virtual robot node remains unchanged, and its position translates synchronously with the centroid of the cluster;

[0016] During rotational motion, the position of the virtual robot node is fixed at the center of rotation of the cluster, and its orientation changes with the rotation of the cluster;

[0017] When constructing the graph optimization model, the virtual robot node is set as the anchor node. During the iterative optimization process, the other robot nodes in the cluster will use this anchor node as a reference.

[0018] Furthermore, within each control cycle, first obtain the current poses of all robots and virtual robot nodes, and calculate the global motion speed of each robot.

[0019] Calculate the body speed of each robot based on the following formula:

[0020] For translational motion:

[0021] v x = v gx cos θ i + v gy sin θ i

[0022] v y = -vg x sin θ i + v gy cos θ i

[0023] ω = 0

[0024] For rotational motion:

[0025] v x = v gx cos θ i + v gy sin θ i

[0026] v y = -v gx sin θ i + v gy cos θ i

[0027] ω = ω g

[0028] Where, v gx , v gy , ω g are the global speeds of the robot; θ i is the attitude angle of the robot; v x , v y , ω are the body speeds of the robot.

[0029] Furthermore, the graph optimization solving module realizes optimization solving through the following steps:

[0030] Initialize the graph optimization model.

[0031] Take the body velocity (v x , v y , ω) of the robot as the nodes of graph optimization;

[0032] Take the predetermined formation structure and the current formation structure {p0, p1,..., p n} as constraints;

[0033] Calculate the relative pose error function: where is the relative pose between node i and node j in the predetermined formation; is the transformation matrix corresponding to in the predetermined formation structure; T′ i , T′ j is the transformation matrix corresponding to p i , p j , the pose p′ updated according to the state transition equation in the current formation structure i , p′ j ;

[0034] Construct the error function and solve the optimization problem:

[0035]

[0036] where Ω ij is the information matrix corresponding to the error function between node i and node j; E is the error function constructed by all nodes; v * is the optimal control velocity information of all robots obtained by solving.

[0037] Furthermore, the control velocities obtained by solving the optimization problem are distributed to each robot, so that after each control cycle ends, the pose error between the updated formation structure of the robot and the predetermined formation structure is minimized, realizing cooperative formation control.

[0038] Furthermore, the control method controls the robot to switch between the following stages:

[0039] Standby stage, in which all robots start and initialize, obtain global pose information, and communicate with the master computer in real time to receive control instructions;

[0040] Formation stage, in which the robot pose is adjusted in the formation stage to make the cluster form a specific formation structure, and the current formation structure is saved as the predetermined formation structure;

[0041] During the formation maintenance phase, according to the motion instructions input by the system in the formation maintenance phase, the cluster is controlled to perform translational or rotational motion. According to steps S2 - S5, in each control cycle, graph optimization modeling and solution are carried out to keep the current formation structure consistent with the predetermined formation structure.

[0042] This application also provides a multi - robot cluster collaborative formation collaborative control system based on graph optimization. The control system includes the following five modules:

[0043] Robot cluster module: It contains all the robots in the system. Each robot can obtain its current real - time pose and can perform speed drive according to the control information.

[0044] Global positioning module: It provides the robot pose information in a unified coordinate system, using Simultaneous Localization and Mapping (SLAM) or Real - Time Kinematic (RTK) technology.

[0045] Motion control module: It calculates the predicted poses of all robots in the cluster and distributes control information to achieve cluster motion control.

[0046] Graph optimization solution module: It models the robot poses and control information as nodes of a graph optimization problem, models the relative poses between robots as constraints, and solves for the optimal control information.

[0047] Communication module: It is responsible for the communication between robots and with the main control computer to ensure the stability and real - time performance of data transmission.

[0048] Beneficial effects:

[0049] The present invention adopts a graph - optimization - based method to realize the motion control of all robots in a multi - robot cluster. The present invention constructs the motion control information (motion speed) of all robots in the cluster as nodes in graph optimization, and constructs the difference between the relative positions of robots in each control cycle and the relative positions of robots under the predetermined formation as constraints in graph optimization. During the process of graph optimization solution, by optimizing the speeds of all nodes, the overall constraint is minimized, so that the robot cluster always maintains the predetermined formation shape during the motion process and ensures the stability of the motion process. The method based on the present invention has simple modeling and does not require complex parameter - tuning steps. And during the motion process, the optimal control speed of the cluster can always be obtained in each control cycle. Compared with the PID control method, it realizes the balance of dynamic performance and steady - state performance.

[0050] All the robots in the present invention have equal roles, thus avoiding the situation where the failure of the leading robot leads to the failure of the cluster system. The method adopted in the present invention can flexibly increase and decrease the number of robots. When the system starts, by setting the numbers of the starting robots, these numbered robots can be uniformly incorporated into the cluster formation, thus realizing the flexibility of the system.

[0051] All the robots in the present invention have relatively accurate global positioning. The global positioning depends on the accurate positioning of SLAM. All the robots are in the unified global coordinate system and can obtain the current global position in real time. Based on the accurate global positioning, by modeling the cluster system as a graph optimization system, the accuracy of formation control and the stability of motion are guaranteed. Compared with the above invention, it is more competitive in formation control. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 It is the architecture diagram of the multi-robot cooperative formation control system;

[0053] Figure 2 It is the flowchart of the multi-robot cooperative formation control;

[0054] Figure 3 It is the schematic diagram of the cluster standby stage;

[0055] Figure 4 It is the schematic diagram of the cluster formation stage;

[0056] Figure 5 It is the schematic diagram of the cluster formation maintenance stage;

[0057] Figure 6 It is the control flowchart of the cluster formation maintenance stage. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0058] The technical solutions of the present invention will be described in detail below with reference to the accompanying drawings in the embodiments of the present invention.

[0059] Embodiment 1:

[0060] Figure 1 It is the architecture diagram of the multi-robot cooperative formation control system. As Figure 1 shown, the present application provides a multi-robot cluster cooperative formation control system based on graph optimization. The system architecture mainly includes: a robot cluster, a global positioning module, a motion control module, a graph optimization solution module, and a communication module;

[0061] The robot cluster includes all the robots in the system. All the robots have global poses, and the pose sequence is: {p0, p1,..., p n}, particularly, this system can theoretically be applicable to a robot cluster system with any number of robots;

[0062] The global positioning module is used to provide pose information of each robot in a unified coordinate system. This module can adopt methods such as a simultaneous localization and mapping (SLAM) system based on lidar and vision sensors, or a real-time kinematic (RTK) system, etc., to provide real-time pose information of the robot cluster. The entire cluster relies on this positioning module to obtain the global poses of all robots and uses them as the basis for motion control.

[0063] The motion control module is used to update the states of the robots in the cluster system. According to the motion mode of the cluster system, the poses of the robots, and the pre-set speed information of the cluster, it calculates the predicted poses of all robots in the cluster system. This module is also responsible for distributing the control information of the robots in the cluster to achieve the purpose of driving and controlling the cluster. First, the cluster determines the motion centroid according to the instruction, and this centroid serves as the reference point for the robot cluster to perform translational and rotational motions. At the same time, the motion control module determines the pre-set formation structure of the cluster, that is, the relative position relationship between the robots in the cluster. During the motion process, the pre-set formation structure serves as the motion constraint standard. Based on the global pose information of the robots and the pre-set formation structure, the global motion control information of each robot is calculated. Since the control information of each robot is relative to its own body coordinate system, the motion control module converts the global motion control information into motion control information in the body coordinate system according to the orientation information of each robot, and sends the calculated control information to each robot in the cluster to ensure that the cluster maintains the pre-set formation structure during the motion process.

[0064] The graph optimization solution module is responsible for modeling the multi-robot cluster system as a graph optimization model, and modeling the motion control information of each robot in the cluster as a node in the graph optimization model. Specifically, for each mobile robot's motion speed (v x , v y , ω), including the translational speed v x , v yand the angular velocity ω of rotation around itself; the relative pose relationship between the robots in the "current formation structure" of the cluster and the relative pose relationship between the robots in the "predetermined formation structure" is modeled as an edge (i.e., a constraint) in the graph optimization model. In each motion control cycle, according to the real-time poses of all robots at the current moment, the control speed relative to the motion centroid is calculated, which is the "pre-computed speed", and this control speed information is added to the graph optimization model as a node to be optimized; then the deviation between the relative poses of all current robots and the relative positions of all robots in the predetermined formation is added to the graph optimization model as a constraint; in the calculation of the constraint error, according to the pose of the robot at the current moment and the pre-computed speed of the robot, the predicted pose after one motion cycle is calculated according to the state transition equation, and the error function between the "current formation structure" and the "predetermined formation structure" is constructed. Through the step-by-step iteration solution of graph optimization, the cluster control information that minimizes the error function can be obtained, so as to achieve the consistency between the current cluster structure and the predetermined formation structure of the cluster.

[0065] The communication module is mainly responsible for information interaction with all machines in the cluster. The specific interaction information includes: (1) The global positioning module performs real-time positioning with the robots in the cluster to provide global pose information for each robot; (2) Each robot reports its current real-time pose to the motion control module; (3) The motion control module sends the calculated real-time control information to all robots in the cluster; (4) The data interaction between the motion control module and the graph optimization solution module mainly includes pose information and control speed information.

[0066] The global positioning module can be deployed in various ways: (1) It can be deployed on an independent server. In this mode, the global positioning service is deployed on the server, and each robot sends the perception information of the current sensor to the global positioning server to obtain the global pose of the current robot; (2) The global positioning module can be deployed on each robot, and each robot outputs the global positioning pose of the robot according to the sensor perception information. The motion control module and the graph optimization solution module can be deployed on an independent main control computer or on the control computer of a certain robot in the cluster. Regardless of the deployment method, it is necessary to ensure that this computer maintains stable network data communication with all robots in the cluster. The communication module needs to be deployed above all the above modules and on each robot controller in the cluster to ensure effective information interaction of data such as pose information and control information between the modules. In the robot cluster, an independent control system and sensors need to be configured on each robot device. The control system is responsible for processing the logical processing of the control process, the drive control of each motor, and the processing and fusion of all sensor data. The sensors can be configured as lidar, vision cameras, etc. In this embodiment, a two-dimensional lidar is used as the perception sensor, and the perception data can be matched with the global map data to obtain the real-time positioning information of each robot.

[0067] Embodiment 2:

[0068] Based on the control system provided in Embodiment 1, the present application also provides a multi-robot cluster collaborative formation and collaborative control method based on graph optimization, as Figure 2 shown, Figure 2 which is the control flow chart for multi-robot collaborative formation control. To achieve the collaborative formation control of the multi-robot cluster, three steps need to be experienced. According to different tasks, the system can issue different instructions to realize the state transition between different steps.

[0069] Step S1: Standby stage: All robots in the cluster start and are initialized, and can output the global pose in real time, accept control information and move. When the "formation instruction" is triggered, the system switches from step S1 to step S2;

[0070] Step S2: Formation stage: Adjust the poses of the robots in the cluster to form a specific cluster formation structure, that is, the "predetermined formation structure". This step can be that all robots are controlled by a human through a remote control to form the "predetermined formation structure", or can be automatically formed into the "predetermined formation structure" through the formation algorithm and task requirements. Finally, the formation list is: When the "formation maintenance instruction" is triggered, the system switches from step S2 to step S3.

[0071] Step S3: Formation maintenance phase: According to the cluster speed command set by the system, start the cluster motion control. During the control process, according to the "current formation structure": poses_curr = {p0, p1,..., p n}, and the control information of all robots in the cluster, solve the graph optimization model to obtain the optimized optimal control information, so as to achieve consistency with the "predetermined formation structure"; when the "exit formation maintenance instruction" is triggered, the system switches from step S3 to step S2; when the "exit formation instruction" is triggered, the system switches from step S3 to step S1.

[0072] Figure 3 It is a schematic diagram of the cluster standby phase; when the system is in the standby phase, all robots in the cluster have been started and initialized, and their respective real-time poses can be obtained from the global positioning module, or each robot can obtain the current real-time pose from its own positioning system; and all robots communicate with the main control computer in real time and can accept the control commands of the main control computer to perform motion driving.

[0073] Figure 4 It is a schematic diagram of the cluster formation phase; the standby cluster drives all robots to specific global positions according to the task requirements, and triggers the "formation instruction" according to the system instruction. In this implementation, the system sets the trigger variable "MODE = 1", indicating that the system is to switch to the formation phase. At this time, the system will perform the following two operations:

[0074] (1) According to the system instruction, set the centroid of the cluster. The centroid can be the current centroid of a certain robot or any position in the global position. The cluster will perform translational and rotational motions of the cluster with this centroid as the reference point. In this embodiment, the centroid is Figure 4 Robot-0 in, and this centroid is an arbitrary position in the cluster. The system can perform two-dimensional translational motion or rotational motion around this centroid with this centroid as the reference.

[0075] (2) The system saves the current poses of all robots in the cluster. To ensure the stability of the system, the centroid of the cluster is specially set as a virtual robot node here, that is, Robot-0 in the figure, and it is saved into the predetermined formation structure together. The pose (x, y, θ) of this virtual robot node will fix a certain part respectively according to different motion forms.

[0076] (2.1) When the cluster performs a translational motion, the azimuth angle θ of the virtual robot node is fixed, that is, during the translational motion, the azimuth orientation remains unchanged all the time. The position (x0, y0) of the virtual robot node is relatively stationary with respect to the pose of a certain robot in the cluster. In the figure, it remains fixed relative to the position (x1, y1) of Robot-1. That is, during the translational motion, the position of the virtual robot node moves translationally synchronously with Robot-1, but its azimuth orientation is always the initial pose: θ0 = θ; while the azimuth orientation of Robot-1 during the motion is its actual pose angle: θ1.

[0077] (2.2) When the cluster performs a rotational motion, the position (x0, y0) of the virtual robot node always remains fixed, that is, during the rotational motion, the global coordinates remain constant, that is, the coordinates of the rotation center of the cluster are constant; the azimuth angle of the virtual robot node is always consistent with the azimuth angle of a certain robot in the cluster. In the figure, it is consistent with the pose of Robot-1, that is: θ0 = θ1, so as to ensure that during the rotational motion of the entire cluster, the centroid position of the cluster remains fixed, and the orientation of the cluster changes at a fixed rate consistent with the orientation of a certain robot in the cluster.

[0078] The poses of all robots in the cluster and the pose of the virtual robot together constitute a formation structure, called the "predetermined formation structure": Among them is the pose of the i-th robot in the cluster, is the pose of the virtual robot node in the cluster.

[0079] Figure 5 is a schematic diagram of the cluster formation maintenance stage; after the system forms the "predetermined formation structure", according to the system input instructions, such as Figure 5 in, set the trigger variable "MODE = 2" to trigger the "formation maintenance instruction", and at the same time according to the input speed configured by the main control system: v g =(v gx , v gy , ω g ), the translational motion or rotational motion of the cluster can be executed, and at the same time, the predetermined formation structure is maintained during the motion; when set to translational motion: v g =(v gx , v gy , 0), the cluster system will move according to the given two-dimensional speed. During the motion, the azimuth orientations of all robots in the cluster remain the same as the initial orientation; when set to rotational motion: v g =(0, 0, ω g) In the two-dimensional plane, the cluster system will rotate around the centroid of Robot-0. During the rotation, the centroid of the cluster remains fixed at the position of Robot-0.

[0080] Figure 6 : Flowchart for controlling the cluster formation maintenance stage. Further, within each control cycle, the system will perform process control according to Figure 6 ;

[0081] First, obtain the current poses of all robots in the cluster, and at the same time, the current pose of the virtual robot node needs to be saved. Specifically, during translational motion, the position of the virtual robot changes with the motion of the cluster centroid and can actually be set as the relative offset from the actual position of a certain real robot. The attitude of the virtual robot remains the same as the initial attitude of the virtual robot in the "predetermined formation structure". During rotational motion, the position of the virtual robot is fixed, i.e., the position of the cluster centroid, which is also the initial position of the virtual robot in the "predetermined formation structure", and the attitude of the virtual robot changes with the rotation of the cluster and can actually be set as the current attitude of a certain real robot. At this time, the poses of all robots including the virtual robot node are obtained, which is called the "current formation structure": poses_curr = {p0, p1,..., p n}, where p i is the current real-time pose of the i-th robot, and p0 is the current pose of the virtual robot node;

[0082] Further, calculate the global motion speed of each robot based on the cluster centroid and the current poses of each robot;

[0083] For the translational motion of the cluster, the control system gives the translational speed of the cluster as: (v gx , v gy ), which is the speed relative to the global system. The body speed (v x , v y ) of each robot in the cluster is calculated as follows, where the current orientation of each robot is θ i ;

[0084] v x = v gx cos θ i + v gy sin θ i

[0085] v y = -v gx sin θ i + v gy cos θ i

[0086] ω = 0

[0087] For the rotational motion of the cluster, the control system sets the rotational angular velocity of the cluster as: g_ω. The calculation method of the body velocity of each robot in the cluster is as follows. First, calculate the global velocity of each robot, where Δx i , Δy i is the relative offset of each robot relative to the virtual robot node;

[0088] v gx = -ω g ·Δy i

[0089] v gy = ω g ·Δx i

[0090] ω g = ω g

[0091] Then, according to the global velocity (v gx , v gy ), calculate the body velocity (v x , v y ) of each robot, where the current orientation of each robot is θ i ;

[0092] v x = v gx cos θ i + v gy sin θ i

[0093] v y = -v gx sin θ i + v gy cos θ i

[0094] ω = ω g

[0095] Furthermore, it is necessary to model the graph optimization model, add nodes and constraints, and then solve it;

[0096] First, perform the initialization configuration of the graph optimization model;

[0097] Then, add the body velocity (v x , v y , ω) of each robot in the cluster calculated above as nodes in the graph optimization;

[0098] Then, the "predetermined formation structure": "Current formation structure": poses_curr = {p0, p1,..., p n} is added as a constraint in graph optimization. The specific operation steps are as follows.

[0099] First, based on the current pose p i and the current speed v i information of each robot, the pose is updated, and the updated pose is p' i . The calculation formula is as follows:

[0100]

[0101] For any two robots in the cluster, according to the poses in the "predetermined formation structure", their relative poses can be calculated, such as Then, their predetermined relative offsets can be calculated therefrom. Assuming the coordinate transformation corresponding to the two coordinates is: Therefore, the relative offset from robot i to robot j is: According to the poses in the "current formation structure", the poses of robot i and robot j are updated according to the state transition formula to obtain p' i , p' j , and then the coordinate transformation corresponding to the updated pose is obtained as: T' i , T' j Thus, the relative error function between robot node i and robot node j is constructed as:

[0102]

[0103] Then, the relative error functions between all robot nodes in the cluster are constructed into a unified error function:

[0104]

[0105] By solving the above graph optimization problem:

[0106]

[0107] After the control speeds of each robot node obtained by solving are distributed to each robot, after one control cycle, the pose between the updated formation structure and the "predetermined formation structure" can be made as small as possible. Thus, the purpose of cooperative formation control is achieved.

[0108] Finally, the optimized control speeds are distributed to each robot in the cluster for motion control to complete the motion control of the entire control cycle.

Claims

1. A multi-robot cluster collaborative formation and cooperative control method based on graph optimization, characterized in that The control method models the motion control problem of a multi-robot cluster as a graph optimization problem, and realizes the stable cooperative formation control of the cluster by solving the graph optimization problem. The control method includes the following steps: S1 Pre-arrange all robots in the cluster at predetermined positions to form a predetermined formation structure of the cluster; In each control cycle, perform the following steps: S2 Obtain the current pose information of all robots in the system to form the current formation structure, and calculate the pose deviation between the current formation structure and the predetermined formation structure; S3 Construct a graph optimization model for multi-robot cluster formation control: model the robot control speed information as nodes in graph optimization, and model the pose deviation in S2 as constraints in graph optimization; S4 Iteratively solve the graph optimization problem constructed in S3. By gradually optimizing the nodes in the graph optimization model, that is, the control speed information of each robot, the error between the current formation structure and the predetermined formation structure is continuously reduced, and finally the optimal control speed information is obtained; S5 Distribute the optimal control speed information obtained in S4 to each robot in the cluster to complete the robot motion drive in this control cycle.

2. The method according to claim 1, wherein Both the predetermined formation structure and the current formation structure include a virtual robot node, and its functions are: During translational motion, the orientation of the virtual robot node remains unchanged, and its position moves synchronously with the centroid of the cluster; During rotational motion, the position of the virtual robot node is fixed at the center of rotation of the cluster, and its orientation changes with the rotation of the cluster; When constructing the graph optimization model, set the virtual robot node as an anchor node. During the iterative optimization process, other robot nodes in the cluster will use this anchor node as a reference.

3. The method according to claim 2, wherein In each control cycle, first obtain the current poses of all robots and the virtual robot node, and calculate the global motion speed of each robot; Calculate the body speed of each robot based on the following formula: For translational motion: v x = v gx cosθ i + v gy sinθ i v y = -v gx sinθ i + v gy cosθ i ω = 0 For rotational motion: v x = v gx cosθ i + v gy sinθ i v y = -v gx sinθ i + v gy cosθ i ω = ω g Among them, v gx , v gy , ω g is the global speed of the robot; θ i is the attitude angle of the robot; v x , v y , ω is the body speed of the robot.

4. The method according to claim 2, characterized in that The graph optimization solving module realizes the optimization solution through the following steps: Initialize the graph optimization model; Take the body speed (v x , v y , ω) of the robot as the nodes for graph optimization; Take a predetermined formation structure and the current formation structure {p0, p1,..., p n} as constraints; Calculate the relative pose error function: where is the relative pose between node i and node j in the predetermined formation; in the predetermined formation structure corresponding transformation matrix; T′ i , T′ j is p in the current formation structure i , p j the pose p′ updated according to the state transition equation i , p′ j corresponding transformation matrix; Construct an error function and solve the optimization problem: Among them, Ω ij is the information matrix corresponding to the error function between node i and node j; E is the error function constructed by all nodes; v * is the optimal control speed information of all robots obtained by solving.

5. The method according to claim 4, wherein Distribute the control speed obtained by solving the optimization problem to each robot, so that after each control cycle ends, the pose error between the updated formation structure of the robot and the predetermined formation structure is minimized, realizing cooperative formation control.

6. The method according to any one of claims 1 to 5, characterized in that The control method controls the robot to switch between the following several stages: Standby stage. In the standby stage, all robots start and initialize, obtain global pose information, and communicate with the main control computer in real time to receive control instructions; Formation stage. In the formation stage, adjust the poses of the robots so that the cluster forms a specific formation structure, and save the current formation structure as the predetermined formation structure; Formation maintenance stage. In the formation maintenance stage, according to the motion instructions input by the system, control the cluster to perform translational or rotational motion. According to steps S2 - S5, in each control cycle, perform graph optimization modeling and solution to keep the current formation structure consistent with the predetermined formation structure.

7. The multi-robot cluster collaborative formation and coordination control system based on graph optimization is characterized in that The control system includes the following five modules: Robot cluster module: It includes all the robots in the system. Each robot can obtain its current real-time pose and can be speed-driven according to the control information. Global positioning module: It provides the pose information of the robots in a unified coordinate system, and adopts technologies such as Simultaneous Localization and Mapping (SLAM) or Real-Time Kinematic (RTK) differential positioning. Motion control module: It calculates the predicted poses of all the robots in the cluster and distributes the control information to achieve cluster motion control. Graph optimization solution module: It models the robot poses and control information as nodes of a graph optimization problem, models the relative poses between robots as constraints, and solves the optimal control information. Communication module: It is responsible for the communication between robots and with the main control computer to ensure the stability and real-time nature of data transmission.

Images