A Robot Cooperative Control System Based on Rigid Formation

Abstract

A robot collaborative control system based on rigid formation includes: an image acquisition module for acquiring the geometry of the object to be transported; an undirected perception module for confirming the desired formation based on the geometry of the object and constructing an undirected perception topology graph; calculating the relative distance and relative angle between agents based on the global coordinates of each agent in the undirected perception topology graph, and confirming the adjacency relationship; a motion control module for receiving control commands and constructing a controller based on the relative distance error, relative angle error, and adjacency relationship between agents; and a drive module for generating torque according to the controller to drive the motor to perform the task. This invention uses a distributed rigid formation control method to form a directional rigid formation at the grasping end that matches the shape of the object being transported, thereby avoiding pulling damage to the object during transport and effectively controlling the posture of the object during transport.

Description

Technical Field

[0001] This invention relates to the field of multi-agent control technology, and more specifically to a robot cooperative control system based on rigid formation. Background Technology

[0002] The continuous advancement of intelligent industries has brought numerous conveniences to humanity, enabling people to use robots for a large amount of repetitive and simple labor. Among these, material handling is a crucial application scenario for robots. In industrial applications, it has become increasingly apparent that individual robots are less capable of handling some heavy tasks, such as moving large objects. Therefore, we need to explore how to enable robots to cooperate and coordinate to achieve desired goals. The scenario where multiple robots collaborate to achieve a material handling objective is called collaborative handling. Currently, common collaborative handling platforms use multiple movable robotic arms for material handling, employing force control methods during the process. This method requires force sensors on the robots, resulting in high costs. Existing position control methods, including compliant control, can easily generate internal stress within the object being handled during transport, potentially causing varying degrees of damage. Furthermore, existing collaborative handling methods only consider moving the object from one location to another, neglecting to control the object's posture during transport, thus limiting practical applications.

[0003] Therefore, how to avoid damage to the transported object and control its posture are problems that urgently need to be solved by those skilled in the art. Summary of the Invention

[0004] In view of this, the present invention provides a robot collaborative control system based on rigid formation, which avoids pulling damage to the transported object during the transport process and can effectively control the posture of the transported object during the transport process.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A robot cooperative control system based on rigid formation includes: an image acquisition module, a multi-agent perception module, a motion control module, and a drive module;

[0007] The image acquisition module is used to acquire the geometric shape of the object to be transported;

[0008] The undirected perception module is used to determine the desired formation based on the geometry of the object to be transported and to construct an undirected perception topology graph; it calculates the relative distance and relative angle between agents based on the global coordinates of each agent in the undirected perception topology graph and confirms the adjacency relationship.

[0009] The motion control module is used to receive control commands and construct a controller based on the relative distance error, relative angle error and adjacency relationship between intelligent agents;

[0010] The drive module is used to generate torque according to the controller to drive the motor to perform tasks.

[0011] Furthermore, the desired formation includes: confirming the spatial position and desired distance of each agent in the formation, so that the shape and desired distance of the desired formation are consistent with the shape characteristics of the object being transported.

[0012] Furthermore, the construction of the undirected perceptual graph includes:

[0013] Introduce virtual intelligent agents and assign them numbers;

[0014] An undirected perceptual topological graph is represented as: G = (N, E);

[0015] Where N = {1, 2, ..., n}, E = {(i, j) | (i, j) ∈ N × N, i, j ∈ N}, N and E represent the vertex set and edge set of the undirected graph consisting of n agents, respectively; the vertex set coordinates p = [p1, ... p n ]

[0016] Based on the vertex set coordinates p = [p1,...p...] n Calculate the length of each edge in an undirected topological graph. The boundary field function set f(p) = [d1,...,d2] is obtained. k [,...] and confirm that the rigid matrix is:

[0017] The undirected topological graph is constrained based on the rigid matrix, and the constraint conditions are as follows:

[0018]

[0019] Where m is the dimension of the space in which the agent resides; n is the number of agents.

[0020] Furthermore, the method for calculating the relative position and angle between adjacent intelligent agents is as follows:

[0021] The relative distance between agent i and agent j is r ij =||p i -p j ||;

[0022] The relative angle is vector p j -p i The angle θ between the x-axis and the positive direction of the global coordinate system ij Accordingly, the expected angle between agent i and agent j is denoted as...

[0023] Furthermore, the confirmation of adjacency relationships includes:

[0024] The calculation method is as follows: For two adjacent agents, the distance error between agent i and agent j is given by e. ij =r ij -d ij Calculation; the included angle error is calculated by Calculate the connection parameter a. ij The preset parameters are determined as follows: if agents i and j are adjacent, then a ij =1, otherwise a ij =0.

[0025] Furthermore, the build controller includes:

[0026] Construct an end effector for the robotic arm based on the distance and angle errors between intelligent agents;

[0027] A mobile base controller is constructed based on the preset expected trajectory and weight constant parameters of each intelligent agent.

[0028] Furthermore, the robotic arm end effector includes:

[0029]

[0030]

[0031]

[0032]

[0033]

[0034] in, For the controller applied to the virtual intelligent agent, For the controller imposed on the leader, For controllers applied to other intelligent agents, ρ1, ρ2, ρ m ,ρ d Ω is a preset weight parameter. i ,Τ i m is an intermediate variable. tra , These represent the desired trajectory, desired velocity, and desired acceleration of the robotic arm end effector corresponding to the leader agent.

[0035] Furthermore, the mobile base controller includes:

[0036]

[0037] in, These represent the desired trajectory, desired velocity, and desired acceleration of the leader agent's mobile base, respectively.

[0038] Furthermore, the generation of torque according to the controller includes:

[0039] The torque is obtained by substituting the robotic arm end effector and / or the mobile base controller into the constructed torque expression.

[0040] Furthermore, the torque expression is obtained by dynamically decoupling the end effector of the robotic arm and the moving base.

[0041] As can be seen from the above technical solution, compared with the prior art, the present invention discloses a robot collaborative control system based on rigid formation. By using a distributed rigid formation control method, the grasping end forms a directional rigid formation that matches the shape of the object being transported, thereby avoiding pulling damage to the object being transported during the transport process and effectively controlling the posture of the object being transported during the transport process. Attached Figure Description

[0042] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0043] Figure 1 A schematic diagram illustrating the implementation process of a mobile robot collaborative control system based on rigid formation provided by the present invention;

[0044] Figure 2 This is an overall framework diagram of a mobile robot collaborative control system based on rigid formation according to the present invention;

[0045] Figure 3 This is a movable robotic arm model in an embodiment of the present invention;

[0046] Figure 4 This is the communication topology network structure in the embodiments of the present invention;

[0047] Figure 5 This is a schematic diagram of the motion trajectory of the robot in collaborative handling in this invention;

[0048] Figure 6 This is a schematic diagram illustrating the distance error of the robotic arm end-effector formation in this invention;

[0049] Figure 7 This is a tracking error diagram of the leader tracking the desired transport trajectory in this invention. Detailed Implementation

[0050] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0051] like Figure 1 and Figure 2 This invention discloses a robot collaborative control system based on rigid formation, comprising: an image acquisition module, a multi-agent perception module, a motion control module, and a drive module.

[0052] The image acquisition module is used to acquire the geometric shape of the object to be transported; the undirected perception module is used to determine the desired formation based on the geometric shape of the object and construct an undirected perception topology graph; it also determines the spatial position and desired distance of each agent in the formation, ensuring that the shape and desired distance of the desired formation are consistent with the shape characteristics of the object being transported. The desired distance between agent i and agent j can be denoted as d. ij The relative distances and angles between agents are calculated based on the global coordinates of each agent in the undirected perceptual topology graph, and the adjacency relationships are confirmed.

[0053] 1. Constructing an undirected perceptual graph includes:

[0054] To construct the topology graph, all agents need to be numbered. Let the total number of agents be n-2. Select one agent as the leader, numbered n, and the other agents be numbered 3, 4, ..., n-1 respectively; introduce two first-order integrator models. The virtual intelligent agents are designated as virtual systems, numbered 1 and 2. These two virtual systems participate only in formation and controller calculations within the onboard computer, for the directional control of the formation.

[0055] The constructed undirected perceptual topology graph is represented by G = (N, E), where N = {1, 2, ..., n} and E = {(i, j) | (i, j) ∈ N × N, i, j ∈ N} represent the vertex set of n agents and the edge set of the undirected graph, respectively. According to the undirected perceptual topology graph, if there is an edge between two agents, the two agents are adjacent; define p = [p1, ... p...]. n Where p i Let represent the coordinates of the i-th agent in the global coordinate system of m-dimensional Euclidean space. The length of the l-th edge (i,j) in an undirected graph is denoted as . The lengths of all sides can be written as a function of p: f(p) = [d1,...,d...].k [,...], define the rigid matrix as:

[0056] To ensure the uniqueness of the generated formations, the construction method of the above-mentioned undirected perceptual topology graph must specifically satisfy the following conditions:

[0057]

[0058] 2. Confirm adjacent relationships, including:

[0059] The calculation method is as follows: For two adjacent agents, the distance error between agent i and agent j is given by e. ij =r ij -d ij Calculation; the included angle error is calculated by Calculate the connection parameter a. ij The determination method is as follows: if agents i and j are adjacent, then a ij =1, otherwise a ij =0.

[0060] The motion control module receives control commands and constructs a controller based on the relative distance error, relative angle error, and adjacency relationship between the intelligent agents. The controller includes an end effector for the robotic arm and a mobile base controller.

[0061] In this embodiment, the weight constant parameters ρ1, ρ2, ρ are set. m ,ρ d And the desired formation tracking trajectory m(t). All weight constant parameters ρ1, ρ2, ... m ,ρ d All are greater than zero, and the expected formation tracking trajectory m(t) is continuous and second-differentiable.

[0062] The controller at the end effector of the robotic arm includes:

[0063]

[0064]

[0065]

[0066]

[0067]

[0068] The controller for the mobile base includes;

[0069]

[0070] The expected trajectory calculation method for each mobile base is as follows: Based on the designed formation shape of the mobile robotic arm's end effector, the distance of each agent from the leader within the formation is obtained. The distance of the i-th agent from the leader is represented by... This means that, based on the designed formation tracking trajectory, the trajectory of the mobile base corresponding to the i-th agent is calculated. Weight constant parameter k bv ,k bp It must be greater than 0.

[0071] The drive module is used to generate torque based on the controller to drive the motor to perform tasks. The torque expression used by the controller to calculate the torque is obtained through dynamic decoupling.

[0072] For example Figure 3 A dynamic model of the vehicle-mounted boom system is established, and then the task space and null space are decoupled to obtain:

[0073] A second-order integrator model for the robotic arm's end effector (task space):

[0074] Second-order integrator model of the car (zero space):

[0075] Control input of vehicle-mounted boom system

[0076] The dynamic equations of a mobile robotic arm system can be written as:

[0077]

[0078] Where M is the inertia matrix, Let G be the Coriolis force, B be the transformation matrix, q be the generalized coordinates of the system, and τ be the input force applied to each operating joint. According to the system shown in the figure, X b =[x b ,y b ] T For the Cartesian coordinates of the movable base, Let be the orientation angle of the movable base, be b (half the width of the movable base), be r (radius of the drive wheel), be d (distance from the axis of the drive wheel to the center of the movable base), and be L (distance from the center of the movable base to the base of the robotic arm). a The lengths of the arms are L1 and L2, respectively. cm1 ,L cm2 These are the lengths from the center of mass of each arm to the joint, respectively. θ = [θ r ,θ l ,θ1,θ2] TLet represent the rotation angles of the right and left wheels of the base and the two joints of the robotic arm, respectively. This system is a dynamic system with nonholonomic constraints. By eliminating the nonholonomic constraints, the dynamic equations can be obtained:

[0079]

[0080] Among them, M θ =S T HS, G θ =S T V, τ θ To control the input, τ e This refers to the external force acting on the robotic arm.

[0081] The values ​​of each matrix are as follows:

[0082]

[0083]

[0084] p1 = m c +m1+m2

[0085] p2 = L a (m1+m2)

[0086] p3 = m1L cm1 +m2L1

[0087] p4 = m2L cm2

[0088]

[0089]

[0090] p7 = m w r 2 +I w

[0091]

[0092]

[0093]

[0094] p 11 =m2L1L cm2 cos(θ1-θ2)

[0095]

[0096]

[0097]

[0098]

[0099]

[0100]

[0101] Where, m w ,m c m1 and m2 represent the masses of the wheel, base, and two arms, respectively. w ,I c I1 and I2 represent the moments of inertia of the wheel, base, and two arms, respectively, and c1 and c2 represent the joint damping at the joints of the two arms. Let the position of the end effector of the robotic arm be X. m =[x m ,y m ] T Its speed can be written as in,

[0102] J m =[J B J M ]

[0103]

[0104]

[0105] Next, the independent generalized velocity will be decomposed in the task space:

[0106]

[0107] in, For J m The generalized inverse, For J m The null space. The dynamic equations of the task space are:

[0108]

[0109] in,

[0110] Design the following force rectangle:

[0111]

[0112] The dynamic model of the robotic arm's end effector, i.e., the task space, is as follows:

[0113]

[0114] The dynamic model of the zero space, i.e., the moving base, is as follows:

[0115]

[0116] Considering the dynamics of the moving base alone:

[0117]

[0118] make Control input is To obtain, that is:

[0119]

[0120] Next, the technical effects of the present invention will be illustrated through example simulations:

[0121] gazebo collaborative transport simulation

[0122] Consider a system in a 2D XY plane consisting of four real mobile robots P3, P4, P5, and P6, and two virtual mobile robots P1 and P2, where P6 is the leader. The four real mobile robots are assigned to form a square formation. The desired distance between each side of the formation is...

[0123] d 34 =d 45 =d 56 =d 36 =10.00m, The expected formation distance between the two virtual agents and the real agents is d 16 =15.00m, d 23 =5.00m, set the desired formation The desired angle with the positive X-axis direction in the XY plane is The desired trajectory of P6 is set to x. d (t)=[0.005t 2 +8.48, 10sin(0.157×0.005t) 2 )).

[0124] The motion process of the mobile robot's collaborative handling within 90 seconds was obtained through simulation. Figure 4 The communication topology of 4 real robots and 2 virtual mobile robots is shown, where 1 and 2 are virtual robots, 3-6 are real robots, and 6 is the leader. The robots communicate with each other in two directions. Figure 5 It shows the movement trajectory of four real robots coordinating to carry materials over 90 seconds. Figure 6 and Figure 7The simulation graphs show the distance error and tracking error of the end effector formation of four real robots within 90 seconds. It can be seen that the distance error and tracking error of the formation tend to zero within a finite time, indicating that the formation tracking control of the end effector is complete. The simulation results demonstrate that the distance and attitude control of the end effector formation were ultimately achieved, thus realizing cooperative transport by multiple mobile robots.

[0125] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0126] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A robot cooperative control system based on rigid formation, characterized in that, include: Image acquisition module, multi-agent perception module, motion control module, and drive module; The image acquisition module is used to acquire the geometric shape of the object to be transported; The undirected sensing module is used to determine the desired formation and construct an undirected sensing topology graph based on the geometry of the objects to be transported. The confirmation of the desired formation includes: confirming the spatial position and desired distance of each agent in the formation, so that the shape and desired distance of the desired formation are consistent with the shape characteristics of the object being transported; The construction of the undirected perceptual topology graph includes: Introduce virtual intelligent agents and assign them numbers; The undirected perceptual topological graph is represented as: ; in, , N and E represent the vertex set consisting of n agents and the edge set of the undirected graph, respectively; the vertex coordinates... ; Based on vertex set coordinates Calculate the length of each edge in an undirected topological graph. The set of boundary field functions is obtained. And the rigid matrix is ​​confirmed to be: ; The undirected topological graph is constrained based on the rigid matrix, and the constraint conditions are as follows: Where m is the dimension of the space where the agent resides; n is the number of agents; Calculate the relative distance and relative angle between agents based on the global coordinates of each agent in the undirected perceptual topology graph, and confirm the adjacency relationship; The motion control module is used to receive control commands and construct a controller based on the relative distance error, relative angle error and adjacency relationship between intelligent agents; The construction controller includes: Construct an end effector for the robotic arm based on the distance and angle errors between intelligent agents; A mobile base controller is constructed based on the preset expected trajectory and weight constant parameters of each intelligent agent; The robotic arm end effector includes: in, For the controller applied to the virtual intelligent agent, The controller is applied to the leader; when i = 3 to n-1, As a controller imposed on other intelligent agents, For the preset weight parameters, As an intermediate variable; These represent the desired trajectory, desired velocity, and desired acceleration of the robotic arm end effector corresponding to the leader agent; The mobile base controller includes: in, The preset weight parameters, The coordinates of the center of mass of the moving base are given. These represent the expected trajectory, expected velocity, and expected acceleration of the leader agent's moving base, respectively. The drive module is used to generate torque according to the controller to drive the motor to perform tasks.

2. The robot cooperative control system based on rigid formation according to claim 1, characterized in that, The method for calculating the relative position and angle between adjacent agents is as follows: The relative distance between agent i and agent j is ; The relative angle is a vector Angle with the positive direction of the global coordinate system's x-axis Correspondingly, the expected angle between agent i and agent j is denoted as... .

3. The robot cooperative control system based on rigid formation according to claim 2, characterized in that, The confirmation of adjacent relationships includes: The calculation method is as follows: The distance error between two adjacent agents, agent i and agent j, is calculated by... The calculation shows that the included angle error is caused by... calculate; The communication topology between multiple agents is determined by parameters. The representation is determined as follows: if agents i and j are adjacent, then... ,on the contrary .

4. The robot cooperative control system based on rigid formation according to claim 1, characterized in that, The torque is generated according to the controller, including: The torque is obtained by substituting the robotic arm end effector and / or the mobile base controller into the constructed torque expression.

5. A robot cooperative control system based on rigid formation according to claim 4, characterized in that, The torque expression is obtained by dynamically decoupling the end effector of the robotic arm and the moving base.

Images