A collaborative control method for multi-robot satellites

Abstract

The embodiment of the present invention discloses a collaborative control method for a multi-robot satellite, and the control method comprises: designing a multi-robot satellite collaborative control system configuration; establishing a forward kinematics model of a robotic arm on a single robotic satellite; establishing a dynamics model of the robotic arm on a single robotic satellite; establishing a grasping matrix of an end effector of the robotic arm; establishing a dynamics model of a controlled object; establishing a kinematic relationship between the first-order derivative of a robotic arm joint variable and the linear velocity and angular velocity vector of the controlled object; establishing a correlation between the internal force and external force exerted on the controlled object and the control force applied by the end effector of the robotic arm; establishing a mapping relationship matrix between the angular velocity of the controlled object and the first-order derivative of the Euler angle; bringing the mapping relationship matrix into a coupled dynamics model to obtain a mechanical model of the collaborative control system; designing a PD controller based on the mechanical model of the collaborative control system, and using the PD controller to control the robot satellite to collaboratively control the object.

Description

Technical Field

[0001] The invention patent discloses a mechanical modeling and control method for multiple robot satellites to collaboratively manipulate the same object, which belongs to the field of aerospace technology. Background Art

[0002] In recent years, with the rapid development of science and technology, human exploration of outer space has continued to deepen, and space missions have become more and more complex. As a new type of high-tech product for space missions, robotic satellites are particularly favored by European and American countries. Practice has shown that robotic satellites have obvious advantages in the process of grasping and manipulating objects due to their good flexibility and high reliability, and are more suitable for high-precision space missions. With the increase in the complexity of the mission, a single robotic satellite often seems to be unable to cope with it, while the coordinated operation of multiple robotic satellites can better deal with such problems. Therefore, it is particularly important to develop collaborative control technology for multiple robotic satellites. However, the existing multi-robot collaborative control technology is mostly developed based on fixed-base robotic arms, and the degree of freedom of each robotic arm is relatively low, which makes it difficult to apply to robotic satellites with high degrees of freedom. Summary of the invention

[0003] To solve the above problems, this patent discloses a mechanical modeling and control method for multiple robotic satellites to collaboratively manipulate the same object, thereby realizing the collaborative operation of multiple robotic satellites to cope with more complex space missions.

[0004] To achieve the above object, an embodiment of the present invention adopts the following technical solution: a collaborative control method of a multi-robot satellite, the control method comprising:

[0005] S1, design the configuration of multi-robot satellite collaborative control system;

[0006] S2, establish the forward kinematic model of the manipulator on a single robotic satellite;

[0007] S3, establish the Jacobian matrix of the end effector of the robot arm;

[0008] S4, establish the dynamic model of the robotic arm on a single robotic satellite;

[0009] S5, establishing the grasping matrix of the end effector of the robot arm;

[0010] S6, establishing a dynamic model of the manipulated object;

[0011] S7, establish the dynamic model of the robotic arms on n robotic satellites;

[0012] S8, establishing a kinematic relationship between the first-order derivative of the joint variable of the robot arm and the linear velocity and angular velocity vector of the manipulated object, and a kinematic relationship between the second-order derivative of the joint variable of the robot arm and the linear acceleration and angular acceleration vector of the manipulated object;

[0013] S9, establishing a correlation between the internal force and external force on the manipulated object and the manipulation force applied by the end effector of the robot arm;

[0014] S10, establish a coupled dynamics model;

[0015] S11, establishing a mapping relationship matrix between the angular velocity of the controlled object and the first-order derivative of the Euler angle;

[0016] S12, extending the mapping relationship to all generalized coordinates;

[0017] S13, bringing the mapping relationship matrix into the coupling dynamics model to obtain the mechanical model of the cooperative control system;

[0018] S14, designing a PD controller based on the mechanical model of the collaborative manipulation system, and using the PD controller to control the robot satellite to collaboratively manipulate the object.

[0019] Further, the PD controller is:

[0020]

[0021] in,

[0022] A=D e J A -1 ,

[0023] B=-D e J A -1 J A ′J A -1 +C e J A -1 ,

[0024] ζ d is the desired position and posture trajectory of the controlled object, ζ is the actual position and posture trajectory of the controlled object, are the first and second order derivatives of ζ respectively, e = ζ d -ζ is the position error, K P is the position error feedback gain matrix of the controlled object, K D is the velocity error feedback gain matrix of the controlled object;

[0025] J Ais the deformation of the transformation matrix in the angular velocity vector of the object, C e , D e are respectively parameters in the mechanical model of the cooperative control system;

[0026] E + is the generalized inverse matrix of E, and J is the Jacobian matrix of the end effector of the robot arm.

[0027] Furthermore, the mechanical model of the collaborative control system is:

[0028]

[0029] Among them, C e , D e are the parameter matrices in the mechanical model of the collaborative manipulation system; E is the grasping matrix of the end effector of the robotic arm, and τ is the joint torque vector of the n robotic arms as a whole.

[0030] The solution provided by the embodiment of the present invention obtains a PD controller for multi-arm collaborative control. The z coordinate of the center of mass of the controlled object basically shows a linear change trend, and the other variables remain unchanged, which tracks the expected trajectory well. There is good symmetry between the joint variables of the robot arm and the control torque of the robot arm, which is in line with the actual situation. In summary, it can be seen that the collaborative control effect is good. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0032] Figure 1 It is a schematic diagram of the configuration of the multi-robot satellite collaborative control system of the present invention;

[0033] Figure 2 Schematic diagram of the position and posture of a controlled object in an embodiment of the present invention;

[0034] Figure 3 Schematic diagram of the first robot arm joint variables in an embodiment of the present invention;

[0035] Figure 4 Schematic diagram of the joint variables of the second robot arm in an embodiment of the present invention;

[0036] Figure 5 Schematic diagram of the control torque of the No. 1 mechanical arm in an embodiment of the present invention;

[0037] Figure 6 Schematic diagram of the control torque of the second robotic arm in an embodiment of the present invention. DETAILED DESCRIPTION

[0038] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. The embodiments of the present invention will be described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements with the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be interpreted as limiting the present invention. It can be understood by those skilled in the art that, unless specifically stated, the singular forms "one", "one", "said" and "the" used herein may also include plural forms. It should be further understood that the term "including" used in the specification of the present invention refers to the presence of the features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or their groups. It should be understood that when we say that an element is "connected" or "coupled" to another element, it can be directly connected or coupled to other elements, or there may also be intermediate elements. In addition, the "connection" or "coupling" used here may include wireless connection or coupling. The term "and / or" used herein includes any unit and all combinations of one or more associated listed items. It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical terms and scientific terms) used herein have the same meaning as generally understood by those of ordinary skill in the art to which the present invention belongs. It should also be understood that terms such as those defined in general dictionaries should be understood to have meanings consistent with the meanings in the context of the prior art, and will not be interpreted with idealized or overly formal meanings unless defined as herein.

[0039] Aiming at the problem of coordinated control of multiple robot satellites, the present invention discloses a method for coordinated control of multiple robot satellites. The implementation steps are described as follows:

[0040] The first step is to design Figure 1 The multi-robot satellite cooperative control system configuration shown in the figure, in which n robot satellites jointly grasp an object, n can be any positive integer greater than or equal to 2, and each robot satellite carries a robot arm with p degrees of freedom, p can be any positive integer. Take a fixed point O in space as the origin and establish the following Figure 1 The inertial coordinate system shown; the center of mass O of the controlled object o As the origin, establish Figure 1 The object coordinate system shown; the base mass center O of the lth robot satellite l As the origin, establish Figure 1The satellite base coordinate system shown in FIG. 1 is the center of mass O of the i-th link of the robotic arm on the l-th robotic satellite. i l As the origin, establish Figure 1 The link coordinate system shown in the figure is used as the origin of the joint point of the i-th link of the robotic arm on the l-th robotic satellite to establish the joint coordinate system. After the object is transported to the target location, the satellite base remains stationary, and the n robotic arms on the satellite coordinately manipulate the object to achieve a specific space mission.

[0041] The second step is to establish the forward kinematic model of the robotic arm on a single robotic satellite:

[0042]

[0043] in,

[0044]

[0045] Where i represents the i-th link of the robot arm, α i represents the angle between joint axis i-1 and joint axis i; a i represents the length of the common perpendicular line between joint axis i and joint axis i+1; d i represents the offset of the adjacent link on the i-th joint axis; θ i represents the angle between the i-1th link and the i-th link rotating around the joint axis i, represents the transformation matrix of link i relative to link i-1, cθ i and sθ i They are cosθ i and sinθ i Abbreviation, cα i and sα i They are cosα i and sinα i Abbreviation of .

[0046] The third step is to establish the Jacobian matrix of the end effector of the robotic arm:

[0047]

[0048] in,

[0049]

[0050] J ω =[z0 z1...z i ...z p ],

[0051] Among them, x e ,y e 、z eThey are the three components of the position vector of the origin of the end effector coordinate system relative to the base coordinates, q i is the joint variable of the i-th joint of the robot, z i is the unit vector of the i-th joint axis, referred to as J in this patent. V is the linear velocity Jacobian matrix, called J ω is the angular velocity Jacobian matrix.

[0052] The fourth step is to establish the dynamic model of the robotic arm on a single robotic satellite based on the Euler-Lagrange equation:

[0053]

[0054] Among them, the superscript l represents the lth robotic satellite, is a vector consisting of p joint variables, τ l is the joint torque vector of the robot arm, J l is the Jacobian matrix of the end effector of the robot arm, the superscript T is the transpose symbol of the matrix, F l D is the force and torque vector applied by the end effector of the robot arm. l is the mass matrix, expressed as follows:

[0055]

[0056] in, is the mass of the ith link of the robotic arm on the lth robotic satellite, is the linear velocity Jacobian matrix of the link coordinate system of the i-th link of the robot arm relative to the base coordinate system, is the angular velocity Jacobian matrix of the link coordinate system of the i-th link of the robot arm relative to the base coordinate system, is the rotation matrix of the link coordinate system of the i-th link relative to the base coordinate system, that is, The matrix constructed by the first three rows and first three columns of is the inertia tensor of the i-th rod relative to the connecting rod coordinate system.

[0057] C l is the centrifugal force and Coriolis force matrix, let C l The element in the i-th row and j-th column of the matrix is ​​c ij , and let D l The element in the i-th row and j-th column of the matrix is ​​d ij , then the matrix C l It is expressed as:

[0058]

[0059] in,

[0060]

[0061] c ijk is a first-type Christopher symbol, and c ijk =c ikj .

[0062] Step 5: Establish the grasping matrix of the end effector of the robot arm:

[0063] E=[E 1 E 2 ...E l ...E n ],

[0064] in,

[0065]

[0066]

[0067] Where l = 1, 2, ... n, r l is the vector from the object's center of mass to the grasping position of the lth robotic arm actuator.

[0068] The sixth step is to establish the dynamic model of the object based on the Newton-Euler equation:

[0069]

[0070] in, are the object velocity and angular velocity vectors, is the object acceleration and angular acceleration vector, F O is the vector of the resultant force and torque exerted by the end effector of the robot arm on the object, D O is the mass matrix, C O is the centrifugal force matrix, and its expression is as follows:

[0071]

[0072]

[0073] in, C I is the inertia tensor of the manipulated object relative to the center of mass coordinate system, ω is the object angular velocity vector, and

[0074]

[0075] Step 7: Integrate the dynamic models of n robotic arms to establish a new dynamic model:

[0076]

[0077] Where q = [q1T q2 T ...q n T ]∈R pn , q1, q2…q n ∈R p×1 ,

[0078] D=diag(D1 D2...D n )∈R pn×pn , D1, D2…D n ∈R p×p ,

[0079] C=diag(C1 C2...C n )∈R pn×pn , C1, C2…C n ∈R p×p ,

[0080] J=diag(J1 J2...J n )∈R pn×pn , J1, J2…J n ∈R p×p ,

[0081] F=[F1 T F2 T ...F n T ] T ∈R pn , F1, F2…F n ∈R p×1 ,

[0082] τ=[τ1 T τ2 T ...τ n T ] T ∈R pn , τ1, τ2…τ n ∈R p×1 .

[0083] Step 8. Based on the principle of virtual work and the properties of the Jacobian matrix and the grasping matrix, the following differential kinematic relationship can be obtained:

[0084]

[0085] Taking the first-order derivative, we get the following expression:

[0086]

[0087] The superscript -1 is the symbol for the matrix inversion.

[0088] Step 9: Establish the control force F applied by the end effector of the robot and the resultant force F exerted on the object O The relationship between them is expressed as follows:

[0089] F=E + F O +F I

[0090] Among them, F I It is called internal force and has no actual effect on the movement of the object. O The force that actually affects the movement of an object is called external force. The superscript + is the generalized inverse symbol of the matrix.

[0091] In the tenth step, based on the differential kinematic relationship in step (8) and the relationship between the internal force, external force, and control force in step (9), the dynamic models in step (6) and step (7) are combined to obtain the coupled dynamic model:

[0092]

[0093] in,

[0094] D e =EJ -T DJ -1 E T +D O ,

[0095]

[0096] The eleventh step is to establish the relationship between the angular velocity vector of the controlled object and the first-order derivative of its Euler angle vector with respect to time, which can be expressed as follows:

[0097]

[0098] Where ω is the angular velocity vector of the object, η=[ψ θ φ] T is the Euler angle vector, ψ, θ, φ represent the yaw, pitch, and roll angles respectively, J a is the transformation matrix, expressed as follows:

[0099]

[0100] Finding the second-order derivative gives:

[0101]

[0102] The twelfth step is to establish the generalized coordinate vector ζ of the manipulated object, expressed as follows:

[0103] ζ=[xyz ψ θ φ] T ,

[0104] Based on the transformation matrix J in step (11) a , establish a new transformation matrix J A , which is expressed as follows:

[0105]

[0106] We can get:

[0107]

[0108]

[0109] In the thirteenth step, the differential kinematic relationship in (12) is introduced into the coupled dynamics model in (10), and the mechanical model of the cooperative control system can be obtained:

[0110]

[0111] Step 14: Design a PD controller based on the mechanical model of the cooperative control system in step 13:

[0112]

[0113] in,

[0114]

[0115]

[0116] ζ d is the desired position and posture trajectory of the controlled object, ζ is the actual position and posture trajectory of the controlled object, e = ζ d -ζ is the position error, K P is the position error feedback gain matrix of the controlled object, K D It is the velocity error feedback gain matrix of the controlled object.

[0117] Simulation verification:

[0118] This patent verifies the effectiveness of the proposed mechanical modeling and control method through the following examples. Taking two robotic satellites as an example, the initial conditions are defined as follows:

[0119] m1=3.7Kg, m2=8.393Kg, m3=2.275Kg, m4=1.219Kg, m5=1.219Kg, m6=0.1879Kg, a1=0, a2=-0.425, a3=-0.39225, a4=0, a5=0, a6=0, α2=0,α3=0, α6=0, d1=0.089159, d2=0, d3=0, d4=0.10915, d5=0.09465, d6=0.0823.

[0120]

[0121]

[0122]

[0123]

[0124] ζ=[-0.1091 0 0.3953 0 0 0] T ,

[0125]

[0126] ζ d =[-0.1091 0 0.3593+0.01t 0 0 0] T .

[0127] According to the above design parameters, the PD controller for multi-arm collaborative control can be obtained by the design method proposed in this patent. The simulation results are shown in the figure below. Figure 2 is the position and posture curve of the object. It can be seen from the figure that the expected motion trajectory of the controlled object requires the center of mass of the object to move in a uniform straight line in the Z direction. Therefore, the linear change of the Z coordinate in the simulation curve indicates that the tracking effect is good. Figure 3 and Figure 4 are the joint variables of the robot arm, Figure 5 and Figure 6 is the control torque of the robot arm. The two figures have good symmetry, which is consistent with the actual situation. In summary, it can be seen that the collaborative control effect is good.

[0128] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment. The above is only a specific implementation method of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or replacements that can be easily thought of by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A collaborative control method for multiple robot satellites, characterized in that: The control method comprises: S1, design the configuration of multi-robot satellite collaborative control system; S2, establish the forward kinematic model of the manipulator on a single robotic satellite; S3, establish the Jacobian matrix of the end effector of the robot arm; S4, establish the dynamic model of the robotic arm on a single robotic satellite; S5, establishing the grasping matrix of the end effector of the robot arm; S6, establishing a dynamic model of the manipulated object; S7, establish the dynamic model of the robotic arms on n robotic satellites; S8, establishing a kinematic relationship between the first-order derivative of the joint variable of the robot arm and the linear velocity and angular velocity vector of the manipulated object, and a kinematic relationship between the second-order derivative of the joint variable of the robot arm and the linear acceleration and angular acceleration vector of the manipulated object; S9, establishing a correlation between the internal force and external force on the manipulated object and the manipulation force applied by the end effector of the robot arm; S10, establish a coupled dynamics model; S11, establishing a mapping relationship matrix between the angular velocity of the controlled object and the first-order derivative of the Euler angle; S12, extending the mapping relationship to all generalized coordinates; S13, bringing the mapping relationship matrix into the coupling dynamics model to obtain the mechanical model of the cooperative control system; S14, designing a PD controller based on the mechanical model of the collaborative manipulation system, and using the PD controller to control the robot satellite to collaboratively manipulate the object.

2. The method for cooperative control of a multi-robot satellite according to claim 1, characterized in that: The PD controller is: in, A=D e J A -1 , B=-D e J A -1 J A ′J A -1 +C e J A -1 , ζ d is the desired position and posture trajectory of the controlled object, ζ is the actual position and posture trajectory of the controlled object, are the first and second order derivatives of ζ respectively, e = ζ d -ζ is the position error, K P is the position error feedback gain matrix of the controlled object, K D is the velocity error feedback gain matrix of the controlled object; J A is the deformation of the transformation matrix in the angular velocity vector of the object, C e , D e are respectively the parameter matrices in the mechanical model of the cooperative control system; E + is the generalized inverse matrix of E, and J is the Jacobian matrix of the end effector of the robot arm.

3. The method for cooperative control of a multi-robot satellite according to claim 2, characterized in that: The mechanical model of the collaborative control system is: Among them, C e , D e are the parameter matrices in the mechanical model of the collaborative manipulation system; E is the grasping matrix of the end effector of the robotic arm, and τ is the joint torque vector of the n robotic arms as a whole.

Images