Distributed cooperative control algorithm for multiple manipulators based on fixed-time dual-loop sliding mode

Abstract

The present invention discloses a multi-manipulator distributed cooperative control algorithm based on fixed-time double-loop sliding mode, which belongs to the field of robot control. The kinematic model and dynamic model of the multi-manipulator cooperative operation system are established; the expected trajectory of the load is estimated; the position controller is designed based on the fixed-time convergent double-loop sliding mode control algorithm to obtain the position control torque of each joint of each manipulator; the internal force of the end clamping of each manipulator is obtained through the dynamic model and load force analysis; the internal force error is converted into the internal force control torque in each manipulator force controller, and the output force is converted into the force control torque of each joint through dynamics; the force-position hybrid control is adopted, and the force and position control torque are superimposed and input into the dynamic model of the multi-manipulator system, and the load and the angle and angular velocity information of each joint of the manipulator are calculated by numerical methods to realize the synchronous control of the multi-manipulator posture and clamping force. The steps are simple and easy to use.

Description

Technical Field

[0001] The invention relates to a multi-mechanical arm distributed cooperative control algorithm based on a fixed-time double-loop sliding mode, and belongs to the field of robot control. Background Art

[0002] The collaborative operation of multiple robotic arms can withstand loads with large mass or complex shapes, and can realize the generalization of robot autonomous operation. Therefore, the collaborative control strategy of multiple robotic arms and the torque control of multiple robotic arms have received widespread attention at home and abroad.

[0003] Compared with the single-arm operation system, the control of the multi-arm operation system is more complicated: on the one hand, there are many parameters in the multi-arm distributed collaborative control system, and some parameters are uncertain, which will make its dynamic model present nonlinearity, strong coupling and disturbance uncertainty, and it is very difficult to achieve high-precision control; on the other hand, it is necessary to consider the dynamic coordination between each manipulator and between it and the target. Dynamic coordination means that the end of the manipulator must maintain appropriate contact force to meet the handling requirements during the movement of the desired trajectory; this requires the end of the manipulator to not only run synchronously but also meet the posture constraint relationship to complete the task of handling objects. In terms of traditional control algorithms, PID, robust control and other methods are often used to meet control requirements. Although this type of control algorithm is simple and convenient to use, its error convergence speed and convergence accuracy are difficult to guarantee, especially when facing complex models.

[0004] Therefore, in the control process, on the one hand, how to improve the convergence speed of the tracking error so that the robot arm can quickly track the desired posture; on the other hand, how to optimize the synchronous control of the robot arm's clamping force to improve its stability and accuracy are both urgent problems that need to be solved in the current multi-robot algorithm. Summary of the invention

[0005] Purpose of the invention: In view of the problems and shortcomings of the above-mentioned prior art, the present invention aims to propose a multi-manipulator distributed collaborative control algorithm based on a fixed-time double-loop sliding mode. Through the force-position hybrid control strategy, it can not only ensure the rapid tracking of multiple manipulators to the desired posture, but also realize the synchronous control of the clamping force of multiple manipulators, and finally complete the multi-manipulator collaborative control task.

[0006] In order to achieve the above-mentioned invention object, the present invention discloses a multi-manipulator distributed cooperative control algorithm based on a fixed-time double-loop sliding mode, which is used in a multi-manipulator cooperative system. The multi-manipulator cooperative system includes multiple manipulators that cooperatively clamp and carry loads, and a communication network that can communicate with each other is provided between each manipulator and the load being carried. The specific steps are as follows:

[0007] Step 1: Assume that the multi-manipulator collaborative operation system is four six-degree-of-freedom manipulators arranged arbitrarily, use visual sensors to measure the posture information of the four manipulators holding the load in real time, obtain the joint angles of the four manipulators during the load handling process, and use the joint angles of the four manipulators to establish the kinematic model and dynamic model of the multi-manipulator collaborative system;

[0008] Step 2: Connect the four manipulators and the load to the communication network. Using a fixed time estimation algorithm, each manipulator estimates the expected trajectory of its load according to the network topology.

[0009] Step 3: According to the dynamic model of the multi-manipulator collaborative system, a dual-loop sliding mode control algorithm with fixed time convergence is proposed, and a position controller is designed based on the dual-loop sliding mode control algorithm. The position control torque of each joint of each manipulator is obtained through the position controller;

[0010] Step 4, obtaining the internal force of each robotic arm end clamping through the multi-robotic arm collaborative operation system dynamics model and the force relationship between each robotic arm and the load, including the conversion of load force to each robotic arm end force and the conversion of robotic arm end force to internal force;

[0011] Step 5, design the internal force controller of each robot end clamping, subtract the expected internal force in the clamping direction from the actual clamping internal force of each robot end to obtain the robot arm clamping internal force error, and convert the clamping internal force error into the internal force control torque in each robot arm force controller, and convert the internal force control torque into the joint force control torque of each robot arm through dynamics;

[0012] Step 6, using force-position hybrid control, superimposes the force control torque of each joint of the robot arm and the position control torque of each joint and inputs them into the dynamic model, so that the multi-robot arm can achieve synchronous control of posture and clamping force.

[0013] Beneficial effects:

[0014] (1) In terms of trajectory tracking, the present invention adopts a dual-loop sliding mode control algorithm with fixed-time convergence to set the position controller. On the one hand, with the help of the fixed-time convergence characteristics of the dual-loop fast sliding mode, the error convergence speed is accelerated, thereby improving the position trajectory tracking accuracy of the robot arm when carrying objects. On the other hand, the method adds a disturbance observer to make the observed signal more accurate, reduce hysteresis, and improve its anti-interference ability. While ensuring its error convergence, it also improves the stability of the sliding mode control.

[0015] (2) The present invention adopts a force-position hybrid control strategy for multi-arm collaborative object handling. Compared with the current master-slave method of object handling, the present invention controls the clamping force while the multi-arms are moving, which significantly improves the force control accuracy and handling efficiency.

[0016] (3) The present invention has made innovations in ensuring the fixed-time convergence algorithm. When designing the position controller, a new dual-loop sliding surface based on fixed-time convergence is proposed; by designing robust control items, its fixed-time convergence characteristics are guaranteed and its convergence speed and accuracy are improved; a new type of fixed-time disturbance observer is designed to accurately estimate the disturbance caused by external uncertainties in the system and compensate for the input. When processing the virtual control law differential, the instruction filtering method is used to realize the fixed-time approximation of the virtual control law. The above method significantly improves the convergence speed of the system error while ensuring the stability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is a flow chart of the multi-manipulator distributed cooperative control algorithm based on fixed-time dual-loop sliding mode of the present invention;

[0018] Figure 2 Schematic diagram of a multi-robot collaborative system model in an embodiment of the present invention;

[0019] Figure 3 A communication topology diagram of each robotic arm in an embodiment of the present invention;

[0020] Figure 4 is a block diagram of an estimation algorithm in an embodiment of the present invention;

[0021] Figure 5 It is a principle diagram of a force-position hybrid controller in an embodiment of the present invention;

[0022] Figure 6 It is a block diagram of the force-position hybrid control algorithm in an embodiment of the present invention;

[0023] Figure 7 A schematic diagram of load tracking effect in an embodiment of the present invention;

[0024] Figure 8 Schematic diagram of the internal force effect of each robotic arm in an embodiment of the present invention. DETAILED DESCRIPTION

[0025] Combine the following Figure 1 The embodiments of the present invention are explained in more detail.

[0026] As attached Figure 1 and Figure 2As shown, the present invention discloses a multi-manipulator distributed cooperative control algorithm based on a fixed-time double-loop sliding mode, which is applicable to a multi-manipulator cooperative system. The multi-manipulator cooperative system includes multiple manipulators cooperatively clamping and carrying loads, and each manipulator and the carried load are provided with a communication network capable of communicating with each other; the steps are as follows:

[0027] Step 1, taking four 6-DOF robotic arms as the research object, establish the kinematic model and dynamic model of the multi-robotic arm collaborative system;

[0028] Step 2: Design an estimation algorithm with fixed time convergence to estimate the expected trajectory of the load;

[0029] Step 3, according to the dynamic model, a position controller is designed based on a fixed-time convergence dual-loop sliding mode control algorithm to obtain the position control torque of each joint of each robotic arm;

[0030] Step 4, obtaining the internal force of each robot end clamp through dynamic model and load force analysis;

[0031] Step 5: Design an internal force controller to convert the internal force error into the internal force control torque in each robot arm force controller. And convert the output force into the joint force control torque through dynamics;

[0032] Step 6, using force-position hybrid control, superimposes the force and position control torque and inputs them into the dynamic model, so that the multi-manipulator can achieve synchronous control of posture and clamping force.

[0033] Through the posture information of the clamped object, the joint angles of the four robotic arms can be obtained, and the kinematic model of the multi-robotic arm collaborative system can be constructed: In the multi-robotic arm collaborative system, based on the working space range of each robotic arm, by setting the joint angles of each robotic arm, the homogeneous transformation matrix of the posture of each robotic arm end relative to the base coordinate system can be obtained as follows:

[0034]

[0035] In the formula, R oi ∈R 3×3 Represents the rotation matrix of each robot end coordinate system relative to the base coordinate system, Xe i ∈R 3×1 Represents the position coordinates of each robot end in the base coordinate system.

[0036] As in Figure 2 As shown, the object center point O B Establish the object coordinate system {b}, select the object reference point O, the object coordinate system {b} origin O B To the reference point O, the vector r is available O Indicates that in the base coordinate system {a}, the coordinate of the object reference point O is X o∈R 3×1 ;Origin O B The vector to each robot end point is r Oj ∈R 3×1 ,j=1,...,4; the object coordinate system {b} rotates around the X, Y and Z axes by angles α, β and γ respectively.

[0037] Since the ends of each robot arm and the object reference point O are in the same plane, the rotation matrix of the coordinate system of each robot arm end relative to the base coordinate system is the same as the rotation matrix of the object coordinate system relative to the base coordinate system.

[0038] According to the rotation angle of the object coordinate system, the rotation matrix of each robot end coordinate system relative to the base coordinate system can be obtained as shown in formula R oj As shown:

[0039]

[0040]

[0041] R oj =R ojx ·R ojy ·R ojz ,j=1,...,4

[0042] Where α, β and γ represent the rotation angles around the X, Y and Z axes respectively.

[0043] In summary, in the base coordinate system, the position of each robot end is derived from the position information of the clamped object as shown in the following formula:

[0044] XE j =X o +R oj ·(r Oj -r O )-Xa j

[0045] =X o +R oj ·r j -Xa j

[0046] In the formula, Xa j ∈R 3×1 Represents the coordinates of each robot base in the base coordinate system.

[0047] According to the position information of the clamped object, the end position of each robot arm and the rotation matrix relative to the base coordinate system are obtained. Substituting the above formula into In the equation, the new homogeneous transformation matrix of each robot end relative to the base coordinate system can be obtained as:

[0048]

[0049] Combine the above formula with the inverse kinematics model of a single robot arm to solve the joint angles of each robot arm in the multi-robot collaborative system. So far, the kinematic model of the multi-robot collaborative system has been established. That is, based on the position information of the clamped object, the joint angles of each robot arm are successfully solved.

[0050] In a multi-manipulator collaborative system, there is an interaction force between the end of each manipulator and the load object. For the i-th manipulator in the system; q i ∈R 6×1 represents the joint angle of the i-th robot arm; x ei =h(q i )∈R 6×1 represents the end effector state vector of the i-th robot arm; the Jacobian matrix J of the i-th robot arm ei It can be further expressed as:

[0051]

[0052] In the formula, represents the velocity vector of the end effector of the i-th robot arm; Represents the angular velocity of the i-th robot joint. Combining the single robot dynamics equation, the i-th robot load dynamics equation in the multi-robot collaborative system under the clamping load condition is derived as follows:

[0053]

[0054] In the formula, τ i ∈R 6×1 represents the control torque of the i-th robot arm; M i (q)∈R 6×1 represents the inertia matrix of the i-th robotic arm; represents the angular acceleration of the i-th robot arm; represents the centrifugal force and Coriolis force matrix of the i-th robot arm; G i (q)∈R 6×1 represents the gravity term of the i-th robotic arm; d ai ∈R 6×1 represents the uncertainty and external disturbance term of the i-th robot system; F ei ∈R 6×1 Represents the force vector of the i-th robot end effector acting on the object.

[0055] like Figure 4 As shown, the estimation algorithm that converges according to the fixed time is used to estimate the expected trajectory of the load.

[0056] Assumption: Communication diagram A communication graph consisting of a directed spanning tree with the leader as the root node and undirected connections

[0057] Based on the above assumptions, the communication diagram The relevant Laplace matrix design is shown below:

[0058]

[0059] In the formula, if the information of the leader robot arm can be obtained by operator i, then H = L + diag {a 10 ,...,a N0}, a i0 =1, i=1,...,N.

[0060] like Figure 3 As shown, the communication topology diagram can be obtained as a 12 =a 21 =1,a 23 =a 32 =1, a 34 =a 43 =1, a 41 =a 14 = 1. The desired trajectory information is only passed to robot arms 1 and 4, so a 10 =a 01 =1, a 40 =a 04 =1.

[0061] The fixed-time based estimator design is shown below:

[0062]

[0063] In the formula, sig β (·) represents the fractional level of each element in the vector ·|· i | β sign(· i ) are diagonal matrices of diagonal elements; kp1, kv1 and All are normal numbers; in and are the upper bounds of position, velocity and acceleration respectively; 0<ω1<1, a ij with a i0 All are normal numbers.

[0064] According to the designed trajectory estimation method based on fixed time convergence, the expected trajectory of each manipulator load can be obtained as The expected load speed is The expected acceleration of the load is Where i=1,...,4.

[0065] like Figure 5 , Figure 6 and Figure 7 As shown in the figure, based on the dynamic equation of multi-manipulator collaborative control, a double-loop sliding mode control is designed to obtain the position controller of multi-manipulator collaborative handling.

[0066] A rigid body is clamped by multiple robotic arms, acting on a mass m o The resultant force on a rigid body can be expressed as:

[0067]

[0068] Where M o (X o )∈R 6×6 Represents the inertia matrix of the object; represents the centrifugal force and Coriolis force matrix; G o (X o )∈R 6×1 Represents the gravity term of the object; X o ∈R 6×1 Represents the position and posture of the object reference point O in the base coordinate system; Indicates the velocity of the object reference point O in the base coordinate system; F o ∈R 6×1 It represents the sum of the generalized forces applied by each robot arm to the reference point O of the object. The generalized force F o It is expressed as:

[0069]

[0070] In the formula, F oi ∈R 6×1 represents the force of the i-th robot acting indirectly on the object reference point O; J oi ∈R 6×6 The grasping matrix representing the position of the end of the i-th robot arm to the object reference point O is shown in the following formula:

[0071]

[0072] Where, the generalized force F acting indirectly on the object reference point O by the i-th robot is oi , which can be further decomposed into:

[0073] F oi =F Mi +F Ii

[0074] In the formula, F Mi ∈R 6×1represents the motion-induced force of the i-th robot arm on the object, F Ii ∈R 6×1 represents the internal force between the i-th robot and the object and satisfies:

[0075]

[0076] When the multi-robot collaborative system performs the load handling task, the load distribution for each robot arm is as follows:

[0077]

[0078] Where, d i (t) represents the time-varying load distribution coefficient of the object on the i-th robot arm.

[0079] Combining the above formula, we can get:

[0080]

[0081] According to the formula and The angular velocity of the i-th robot arm can be expressed as:

[0082]

[0083] Differentiating the above formula, we can get the angular acceleration of the i-th robot arm:

[0084]

[0085] General Japanese style Substitute into the following formula The complete dynamic model of the multi-manipulator collaborative system can be obtained as shown in the following formula:

[0086]

[0087] In the formula, Represents the inertia matrix of the multi-manipulator collaborative system;

[0088] Indicates the centrifugal force and Coriolis force of the control system of the multi-manipulator system;

[0089] They represent the disturbance term and gravity term of the multi-manipulator system control system respectively, is the driving torque of the multi-robot collaborative system. Recorded as

[0090] Design a dual-loop sliding mode control and define the position loop tracking error as:

[0091] e1=qq d

[0092] The position loop sliding mode function is designed as follows:

[0093]

[0094] In the formula, K1, K2, K3 are all positive diagonal matrices, α 11 is a constant greater than 0 and less than 1, α 12 is a constant greater than 1, all of which are positive gains. The precision drive function is defined as:

[0095] ρ1(e1)=1+α 13 tanh(α 14 |e1|), where α 13 , α 14 A custom positive constant.

[0096] The time derivative of the sliding surface S1 of the position loop can be expressed as:

[0097]

[0098] Design speed loop The virtual control rate is denoted as α1 and is designed as:

[0099]

[0100] In the formula, k1, k2 are constants greater than 0, α 15 is a constant greater than 0 and less than 1, α 16 is a constant greater than 1, and is a positive gain.

[0101] In the speed loop, α1 is input into the inner loop and subtracted from the actual speed, which is defined as the inner loop error, as shown below:

[0102]

[0103] The inner ring sliding surface is defined as:

[0104]

[0105] In the formula, K4, K5, K6 are all positive diagonal matrices, α 21 is a constant greater than 0 and less than 1, α 22 is a constant greater than 1, all of which are positive gains. The precision drive function is defined as:

[0106] ρ2(e2)=1+α 23 tanh(α 24 |e2|), where α 23 , α24 is a defined positive constant.

[0107] According to the kinetic equation of formula (7), formula (17) can be derived and rewritten as:

[0108]

[0109] In the formula, Direct derivation will introduce singularity, so the filtering algorithm is used to estimate the derivative of α1 to achieve fixed-time approximation of the virtual control law, which ensures both convergence speed and system stability.

[0110]

[0111] In the formula, l 1,i , l 2,i and m i is a custom positive constant, α i Represents the input of the command filter. represents the output of the filter. And It can be rewritten as follows:

[0112]

[0113] According to the above formula, the control input τ can be designed w =τ eq +τ di +τ es , where τ eq , τ di , τ es They are the ideal control law, robust control term and external disturbance estimation term respectively. The ideal control law is:

[0114]

[0115] The robust control term is:

[0116]

[0117] Where, K7 and K8 are both positive diagonal matrices, α1 is a constant greater than 0 and less than 1, and α2 is a constant greater than 1, both of which are positive gains; It is the α1 power of the sign function sign(S2), and η is a positive constant.

[0118] The disturbance estimate is:

[0119]

[0120] For external disturbances and other factors aA fixed-time convergence disturbance observer is used for observation and compensation input into the controller to better improve control accuracy and robustness.

[0121] Define the auxiliary variable V as:

[0122]

[0123] In the formula, λ1>0, λ2>0, α0>1. For external disturbance d a The designed disturbance observer is as follows:

[0124]

[0125] In the formula, λ3>0, λ'>0, 0<ω<1; is an estimate of the unknown constant β, where d a The upper bound of ρ0 is a positive constant.

[0126] In step 4, according to the kinematic and load models, the rigid body is clamped by multiple robotic arms, and the resultant force acting on the rigid body with mass m0 is expressed as:

[0127]

[0128] Where M o (X o )∈R 6×6 Represents the inertia matrix of the object; represents the centrifugal force and Coriolis force matrix; g o (X o )∈R 6×1 Represents the gravity term of the object; X o ∈R 6×1 Represents the position and posture of the object reference point O in the base coordinate system; Indicates the velocity of the object at reference point O; F o ∈R 6×1 represents the sum of the generalized forces exerted by each robot on the object. o It is expressed as:

[0129]

[0130] In the formula, J oi ∈R 6×1 represents the grasping matrix between each robot and the object, F ei ∈R 6×1 Represents the gripping force between each robot arm and the object.

[0131] According to the force F between each robot arm and the object ei, decompose the internal force F required to clamp the object Ii The internal force expression is:

[0132]

[0133] In the formula, E n ∈R 6×6 represents the identity matrix,

[0134] In step 5, the clamping internal force F obtained in step 4 is Ii ; Expected internal force F in the clamping direction Id The force error is defined as:

[0135] ΔF Ii =F Ii -F Idi

[0136] In the formula, F Idi ∈R 6×1 is the expected internal clamping force at the end of each robot arm.

[0137] like Figure 8 As shown, the internal force error is converted into the internal force control torque in each robot force controller, as shown in the following formula:

[0138]

[0139] In the formula, k p ∈R 6×6 With k I ∈R 6×6 Represents the positive gain parameter matrix.

[0140] In step 6, the position control torque and force control torque required for the multi-manipulator collaborative handling obtained in steps 3 and 5 are superimposed to obtain τ=τ li +τ w By inputting it into the dynamic model, each robot can track the desired posture of the load and synchronously control the clamping force of the load when performing multi-robot collaborative handling tasks.

[0141] The expected load trajectory is set as follows:

[0142] Load expected trajectory X o d =[Xo d Yo d Zo d RX d RY d R d ] T , where Xod =0.05sin(2πt)-0.1;Yo d =0.3+0.05cos(2πt);Zo d =0.1;RX d =π / 3;RY d =π / 3;RZ d =π / 3.

Claims

1. A multi-manipulator distributed cooperative control algorithm based on fixed-time double-loop sliding mode, characterized in that: Applicable to a multi-robot collaborative system, which includes multiple robotic arms and loads that they collaboratively clamp and carry, and a communication network that enables mutual communication between each robotic arm and the load it carries; The specific steps are as follows: Step 1, assuming that the multi-manipulator collaborative system is four six-degree-of-freedom manipulators arranged arbitrarily, using visual sensors to measure the posture information of the four manipulators clamping the load in real time, obtaining the joint angles of the four manipulators in the process of carrying the load, and using the joint angles of the four manipulators to establish the kinematic model and dynamic model of the multi-manipulator collaborative system; Step 2: Connect the four manipulators and the load to the communication network. Using a fixed time estimation algorithm, each manipulator estimates the expected trajectory of its load according to the network topology. Step 3: According to the complete dynamics model of the multi-manipulator collaborative system, a dual-loop sliding mode control algorithm with fixed time convergence is proposed, and a position controller is designed based on the dual-loop sliding mode control algorithm. The position control torque of each joint of each manipulator is obtained through the position controller; Step 4, through the multi-manipulator collaborative system dynamics model and the force relationship between each manipulator and the load, including the conversion of load force to the end force of each manipulator, and the conversion of the end force of the manipulator to the internal force, the internal force of the end clamping of each manipulator is obtained; Step 5, design the internal force controller of each robot end clamping, subtract the expected internal force in the clamping direction from the actual clamping internal force of each robot end to obtain the robot arm clamping internal force error, and convert the clamping internal force error into the internal force control torque in each robot arm force controller, and convert the internal force control torque into the joint force control torque of each robot arm through dynamics; Step 6, using force-position hybrid control, superimposes the force control torque of each joint of the robot arm and the position control torque of each joint and inputs them into the dynamic model, so that the multi-robot arm can achieve synchronous control of posture and clamping force.

2. According to claim 1, a multi-manipulator distributed collaborative control algorithm based on fixed-time dual-loop sliding mode is characterized in that In step 1, in the kinematic model of the multi-manipulator collaborative system; In the multi-manipulator collaborative system, the base coordinate system {a} is constructed according to the working space range of each manipulator. By setting the joint angles of each manipulator, the homogeneous transformation matrix of the end position of each manipulator relative to the base coordinate system {a} is obtained as follows: In the formula, R oi ∈R 3×3 Represents the rotation matrix of each robot end coordinate system relative to the base coordinate system, Xe i ∈R 3×1 Represents the position coordinates of each robot end in the base coordinate system, R oi With Xe i are all functions of the i-th robot arm joint angle; the center point O of the load B Establish the object space coordinate system {b}, the center point of the load O B The coordinates of the contact points between the robot and the load are averaged and the load handling reference point O is selected. The load handling reference point O can be defined according to the use requirements. The origin of the load coordinate system {b} O B To the reference point O through the vector r O Indicates that in the base coordinate system {a}, the coordinate of the load reference point O is X o ∈R 3×1 ;Origin O B The vector to each end point of the robot arm is r Oj ∈R 3×1 , j=1,...,4; the load coordinate system {b} rotates around the X, Y and Z axes by angles α, β and γ respectively; Assume that the end of each manipulator and the load reference point O are in the same plane, so the rotation matrix of the coordinate system of each manipulator end relative to the base coordinate system is the same as the rotation matrix of the load coordinate system relative to the base coordinate system; According to the rotation angle of the load coordinate system, the rotation matrix of each robot end coordinate system relative to the base coordinate system is obtained as shown in formula R oj As shown: R oj =R ojx ·R ojy ·R ojz ,j=1,...,4 Where α, β and γ represent the rotation angles around the X, Y and Z axes respectively; In the base coordinate system {a}, the position Xe of each robot end is derived from the load posture information during the handling process. j , as shown below: Xe j =X o +R oj ·(r Oj -r O )-Xa j =X o +R oj r j -Xa j In the formula, Xa j ∈R 3×1 Represents the coordinates of each robot base in the base coordinate system.

3. According to claim 2, a multi-manipulator distributed collaborative control algorithm based on fixed-time dual-loop sliding mode is characterized in that: According to the position information of the clamped load during the handling process, the end position of each robot arm and the rotation matrix relative to the base coordinate system are obtained; the end position equation of each robot arm is substituted into the homogeneous transformation matrix In the equation, the new homogeneous transformation matrix of each robot end relative to the base coordinate system is obtained as follows: Combining the above formula with the inverse kinematics model of a single robot arm, the joint angles of each robot arm in the multi-robot collaborative system can be calculated, thereby completing the kinematic model of the multi-robot collaborative system. The kinematic model of the multi-robot collaborative system can be used to inversely solve the joint angle information of the four robots based on the load posture information. In a multi-manipulator collaborative system, there is an interaction force between the end of each manipulator and the load object; for the i-th manipulator in the system; q i ∈R 6×1 represents the joint angle of the i-th robot arm; x ei =h(q i )∈R 6×1 represents the state vector of the end effector of the i-th robot; the Jacobian matrix J of the i-th robot ei It can be further expressed as: In the formula, represents the velocity vector of the end effector of the i-th robot arm; represents the angular velocity of the i-th robot joint; combined with the single robot dynamics equation, the dynamics equation of the i-th robot in the multi-robot collaborative system under the clamping load is derived as follows: In the formula, τ i ∈R 6×1 represents the control torque of the i-th robot arm; M i (q)∈R 6×1 represents the inertia matrix of the i-th robotic arm; represents the angular acceleration of the i-th robotic arm; represents the centrifugal force and Coriolis force matrix of the i-th robot arm; G i (q)∈R 6×1 represents the gravity term of the i-th robotic arm; d ai ∈R 6×1 represents the model uncertainty and external disturbance term of the i-th robot; F ei ∈R 6×1 Represents the force vector of the i-th robot end effector acting on the object.

4. According to claim 1, a multi-manipulator distributed collaborative control algorithm based on fixed-time dual-loop sliding mode is characterized in that: According to the estimation algorithm with fixed time convergence, the specific process of estimating the expected trajectory of the load is: Construct a communication topology diagram between the payload and the four robotic arms. It is a directed spanning tree and an undirected connection structure, where the load is the root node and the four robotic arms are slave nodes. The load is numbered 0 and the four robotic arms are numbered 1, 2, 3 and 4; According to the communication topology diagram Construct the following Laplacian matrix: In the formula, 04 represents a 4×4 zero matrix, a i0 (i=1,...,4) represents the communication status between robot i and the load. If the load, i.e. the root node, can be obtained by robot i, a i0 =1, otherwise a i0 =0, according to the communication topology diagram 3, we can get a 12 =a 21 =1, a 23 =a 32 =1, a 34 =a 43 =1, a 41 =a 14 = 1, the load's expected trajectory information is only transmitted to robot arm 1 and robot arm 4, so a 10 =a 01 =1, a 40 =a 04 =1, a ij Defined as the information transfer constant between robot i and robot or load j, H = L + diag {a 10 ,...,a 40 }, Laplace matrix L = [l ij ]∈R 4×4 , for i not equal to j, there is l ij =-a ij , for i equals j, we have For robot arm i, design a fixed-time distributed trajectory estimator: In the formula, and is the position, velocity and acceleration estimation of robot i for the load, kp1, kp2, kv1, kv2, and are all positive numbers, representing the gains of position error, velocity error, and acceleration error, and satisfy are all normal numbers, among which and They are the upper bounds of position, velocity and acceleration respectively; 0<ω1<1, ω2>1, ω3=1 / 2; represents the pose estimation error of the end of the i-th robotic arm, represents the velocity estimation error of the end of the i-th robot arm, represents the acceleration estimation error of the end of the i-th robot arm, a ij with a i0 All are normal numbers; According to the actual expected position vector of the load Velocity Vector With the acceleration vector The trajectory estimator based on fixed time convergence calculates the expected trajectory of each manipulator load as follows: The expected speed of the load is The expected acceleration of the load is Where i=1,...,4.

5. According to claim 4, a multi-manipulator distributed collaborative control algorithm based on fixed-time dual-loop sliding mode is characterized in that: Based on the complete dynamic model of the multi-manipulator collaborative system, a dual-loop sliding mode control is designed to obtain the position controller for multi-manipulator collaborative handling; the load clamped by multiple manipulators is a rigid load, and the dynamic model of the rigid load is expressed as: Where M o (X o )∈R 6×6 The inertia matrix representing the rigid body load; represents the centrifugal force and Coriolis force matrix; G o (X o )∈R 6×1 represents the gravity term of the rigid body load; X o ∈R 6×1 Represents the position and posture of the rigid load reference point O in the base coordinate system; Indicates the velocity of the rigid body load reference point O in the base coordinate system; F o ∈R 6×1 represents the sum of the generalized forces applied by each robot arm to the rigid load reference point O. o It is expressed as: In the formula, is the total grasping matrix from each robot arm to the load reference point; is the matrix composed of the external force at the end of the robot arm; F oi ∈R 6×1 represents the force of the i-th robot arm acting indirectly on the rigid load reference point O; J oi ∈R 6×6 Represents the grasping matrix from the end position of the i-th robot arm to the rigid load reference point O: Where, the generalized force F acting indirectly on the rigid load reference point O by the i-th manipulator is oi , further decomposed into: F oi =F Mi +F Ii In the formula, F Mi ∈R 6×1 represents the motion-induced force of the i-th robot arm on the object, F Ii ∈R 6×1 represents the internal force between the i-th robot and the object and satisfies: When the multi-robot collaborative system performs the load handling task, the load distribution for each robot arm is as follows: Where, d i (t) represents the time-varying load distribution coefficient of the object on the i-th robot arm, and the variable load distribution coefficient is set according to the carrying capacity of the robot arm; Multiply both ends of the dynamic model of the single robot by J oi T J ei -T , and adding them together we get: According to the formula and The angular velocity of each joint of the i-th robotic arm is expressed as: Differentiate the above formula to obtain the angular acceleration of the i-th robot arm: General Japanese style Substitute into the following formula The complete dynamic model of the multi-manipulator collaborative system is obtained as follows: In the formula, Represents the inertia matrix of the multi-manipulator collaborative system; compared with the dynamics model of a single manipulator, the complete dynamics model of the multi-manipulator collaborative system describes the motion of all manipulators and loads based on the force and motion relationship between each manipulator and the load; Indicates the centrifugal force and Coriolis force of the control system of the multi-manipulator system; and They represent the disturbance term and gravity term of the multi-manipulator system control system respectively, is the driving torque of the multi-manipulator collaborative system; Recorded as Design a dual-loop sliding mode controller, where the outer loop and inner loop are position loop and speed loop respectively, and define the position loop tracking error as: e1=qq d The position loop sliding mode function is designed as follows: In the formula, sig β (·) represents the fractional level of each element in the vector ·|· i | β sign(· i ) are diagonal matrices of diagonal elements, K1, K2, K3 are all positive diagonal matrices, α 11 is a constant greater than 0 and less than 1, α 12 is a constant greater than 1, all of which are positive gains; the precision drive function is defined as: ρ1(e1)=1+α 13 tanh(a 14 |e1|) In the formula, α 13 With α 14 is a custom positive constant; the time derivative of the sliding surface S1 of the position loop can be expressed as: Design speed loop The virtual control rate is denoted as α1 and is designed as: In the formula, k1 and k2 are constants greater than 0, α 15 is a constant greater than 0 and less than 1, α 16 is a constant greater than 1, which is a positive gain. In the speed loop, α1 is input into the inner loop and subtracted from the actual speed, which is defined as the speed loop error, as shown below: The velocity loop sliding surface is defined as: In the formula, K4, K5, K6 are all positive diagonal matrices, α 21 is a constant greater than 0 and less than 1, α 22 is a constant greater than 1, all of which are positive gains; the precision drive function is defined as: p2(e2)=1+α 23 tanh(a 24 |e2|) In the formula, α 23 With α 24 is a positive constant defined; according to the kinetic equation in step 1, take the derivative of S2 and rewrite it as: In the formula, Direct derivation will introduce singularity, so the filtering algorithm is used to estimate the derivative of α1 to achieve fixed-time approximation of the virtual control law, which ensures both convergence speed and system stability. The fixed-time filter is designed as follows: In the formula, l 1,i , l 2,i and m i is a custom positive constant, α i Represents the input of the instruction filter, represents the output of the filter, It can be rewritten as follows: According to the above formula, the control input τ can be designed w =τ eq +τ di +τ es , where τ eq , τ di , τ es are the ideal control law, robust control term and external disturbance estimation term respectively, The ideal control law is: The robust control term is: Where, K7 and K8 are both positive diagonal matrices, α1 is a constant greater than 0 and less than 1, and α2 is a constant greater than 1, both of which are positive gains; is the α1 power of the sign function sign(S2), It is the sign function sign(S2) raised to the power of α2, and η is a positive constant; The disturbance estimate is: For external disturbance factors d a A fixed-time convergence disturbance observer is used for observation and compensation input into the controller to better improve control accuracy and robustness; Define the auxiliary variable V as: In the formula, λ1>0, λ2>0, α0>1, for external disturbance d a The designed disturbance observer is as follows: In the formula, λ3>0, λ'>0, is an estimate of the unknown constant β, where d a The upper bound of ρ0 is a positive constant.

6. According to claim 5, a multi-manipulator distributed collaborative control algorithm based on fixed-time dual-loop sliding mode is characterized in that: According to the rigid body load dynamics model and the force F between each robot arm and the rigid body load ei , decompose the internal force F required to clamp the rigid body load Ii : In the formula, E n ∈R 6×6 represents the 6×6 identity matrix, 7. According to claim 6, a multi-manipulator distributed collaborative control algorithm based on fixed-time dual-loop sliding mode is characterized in that: Through the clamping internal force F Ii , the desired internal force F in the clamping direction Id , define the internal force error of the robot arm clamping as: △F Ii =F Ii -F Idi In the formula, F Idi ∈R 6×1 is the expected clamping internal force at the end of the i-th robotic arm; the clamping internal force error of the robotic arm is converted into the internal force control torque in each robotic arm force controller, as shown in the following formula: In the formula, k p ∈R 6×6 With k I ∈R 6×6 Represents the positive gain parameter matrix.

8. According to claim 1, a multi-manipulator distributed collaborative control algorithm based on fixed-time dual-loop sliding mode is characterized in that: The position control torque and force control torque required for the multi-manipulator collaborative load handling obtained in step 3 and step 5 are superimposed to obtain τ=τ Ii +τ w , τ is input into the dynamic model, so that each robot arm can not only track the expected posture of the load, but also realize the synchronous control of the load clamping force when performing multi-robot collaborative handling tasks, thereby completing the force-position hybrid control.

Images