An impedance control method for cooperative formation of UAVs with robotic arms

Abstract

The present invention belongs to the technical field of unmanned aerial vehicle flight control, and specifically relates to an impedance control method for cooperative formation of unmanned aerial vehicles with mechanical arms. By constructing a kinematic model and a dynamic model of unmanned aerial vehicles with mechanical arms, a cooperative formation impedance controller is constructed; according to the task requirements, a reference object trajectory is designed, and a synchronous error compensation controller is constructed in combination with the positive motion control of the kinematic model and the error relationship of the cooperative formation impedance controller; the kinematic model of the unmanned aerial vehicle with a mechanical arm is corrected and compensated by the synchronous error compensation controller. The present invention adopts impedance control to solve the compliant grasping of the mechanical arm, reduce damage to objects, and reduce disturbances to the unmanned aerial vehicle. When a single unmanned aerial vehicle with a mechanical arm has insufficient lift, the present invention can be used to realize the flight of multiple unmanned aerial vehicles with mechanical arms to collaboratively grasp objects. When unmanned aerial vehicles with mechanical arms collaboratively grasp objects, the present invention can be used to ensure interference and accuracy during flight.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicle flight control, and in particular relates to an impedance control method for a coordinated formation of unmanned aerial vehicles with mechanical arms. Background Art

[0002] In recent years, with the development of science and technology, the continuous maturity of navigation and control technology, the application of aircraft has been continuously expanded to various occasions, such as: disaster site investigation, environmental modeling and monitoring criminals. Therefore, a UAV with a robotic arm with operational capabilities has become a hot topic of research. The UAV with a robotic arm consists of a UAV and a robotic arm, and has the ability to interact with the environment. It has a wide range of application prospects, such as: dangerous capture and some fire scene operations and grabbing objects to a designated location. However, since the operational capability of a single UAV with a robotic arm cannot perform a single task, the cooperative control technology of UAVs with robotic arms has been developed. The development of cooperative control technology enables UAVs with robotic arms to complete more complex tasks and has great application prospects. The use of impedance control at the end of the robotic arm can more accurately control the robotic arm to grab objects and reduce damage to the objects.

[0003] When a single UAV with a robotic arm performs a task alone, the lift of the UAV with a robotic arm cannot support objects heavier than itself and cannot perform grasping. At the same time, without impedance control, the robotic arm will damage the object when grasping it and the operation will not be smooth. This will cause great disturbance to the UAV with a robotic arm.

[0004] When multiple drones with robotic arms work together to grab objects, formation flying will occur. Ensuring the stable flight of multiple drones with robotic arms is a problem that needs to be solved. Summary of the invention

[0005] In order to solve the above problems, the present invention provides an impedance control method for cooperative formation of UAVs with a mechanical arm. The specific technical solution is as follows:

[0006] A method for controlling impedance of a coordinated formation of unmanned aerial vehicles with a mechanical arm comprises the following steps:

[0007] Step S1, constructing a kinematic model of a UAV with a robotic arm according to homogeneous equations and Jacobian matrix;

[0008] Step S2, establishing a dynamic model of the UAV with a robotic arm according to the kinematic model established in step S1 and based on the Lagrange-Euler equation;

[0009] Step S3, referring to the grasping motion, force and torque of multiple UAVs with robotic arms, constructing a collaborative formation impedance controller;

[0010] Step S4, designing a reference object trajectory according to the task requirements, and constructing a synchronization error compensation controller by combining the forward motion control of the kinematic model and the error relationship of the cooperative formation impedance controller;

[0011] Step S5: Using a synchronous error compensation controller to correct and compensate the kinematic model of the UAV with a robotic arm.

[0012] Preferably, the kinematic model of the UAV with a robotic arm in step S1 is as follows:

[0013]

[0014]

[0015]

[0016]

[0017] Among them, P EK is the position matrix of the end of the robotic arm of the Kth UAV with robotic arm; P BK is the position matrix of the Kth UAV with robotic arm; R BK is the rotation matrix of the Kth UAV with a robotic arm, and T is the transpose; is the position matrix of the end of the robotic arm of the Kth UAV with a robotic arm relative to the UAV; ω EK is the angular velocity matrix of the end of the robotic arm of the Kth UAV with a robotic arm; ω BK is the angular velocity matrix of the Kth UAV with a robotic arm; is the angular velocity matrix of the end of the robotic arm of the Kth UAV with a robotic arm relative to the UAV;

[0018] V EK is the velocity matrix of the end of the robotic arm of the Kth UAV with robotic arm; J BK is the Jacobian matrix of the Kth UAV with a robotic arm; J EKBK is the Jacobian matrix of the robotic arm of the Kth UAV with robotic arm; is the position and velocity matrix of the Kth UAV with a robotic arm; is the robotic arm angle matrix of the Kth UAV with a robotic arm.

[0019] Preferably, wherein:

[0020]

[0021]

[0022] V EK =[ν EX ν EZν Eθ ] T ; (7)

[0023] in is the expected speed in the x direction of the Kth UAV with a robotic arm, is the expected speed in the z direction of the Kth UAV with a robotic arm, is the expected pitch angular velocity of the Kth UAV with a robotic arm, is the expected angular velocity of the first joint of the robotic arm of the Kth UAV with a robotic arm, is the expected angular velocity of the second joint of the robotic arm of the Kth UAV with robotic arm, ν EX is the speed of the end of the robotic arm of the Kth UAV in the x direction, ν EZ is the speed of the end of the robotic arm of the Kth UAV in the z direction, ν Eθ is the pitch angular velocity of the robotic arm end of the Kth UAV with a robotic arm.

[0024] Preferably, the method further comprises the following steps:

[0025] set up

[0026]

[0027] From the above formula, we can get:

[0028]

[0029]

[0030] In the formula, J ζK is the Jacobian matrix J of the Kth UAV with a robotic arm BK The values ​​of the 1st to 3rd rows and the 1st and 2nd columns, J σK is the Jacobian matrix J of the Kth UAV with a robotic arm BK The values ​​of the 1st to 3rd rows and 3rd columns;

[0031] set up J ηK =[J ζK J EKBK ], and the above formula is transformed into:

[0032]

[0033] In the formula, χ K is the symmetric gain matrix of the Kth UAV with a robotic arm; e K is the position error of the end of the robotic arm of the Kth UAV with a robotic arm; and the position y of the end of the robotic arm of the Kth UAV with a robotic armEK and the yaw angle ψ EK is the position y of the drone BK and the yaw angle ψ BK .

[0034] Preferably, the dynamic model of the UAV with a mechanical arm in step S2 is as follows:

[0035]

[0036]

[0037] X B1 =[xz θ] T ; (15)

[0038] Q1=[q1 q2] T ; (16)

[0039] X B2 =[xz θ] T ; (17)

[0040] Q2=[q1 q2] T ; (18)

[0041] Where x is the position of the UAV with a robotic arm in the x direction, z is the position of the UAV with a robotic arm in the z direction, θ is the pitch angle of the UAV with a robotic arm, q1 and q2 are the angles of the first and second joints of the robotic arm of the Kth UAV with a robotic arm respectively; is the inertia matrix of the Kth UAV with a robotic arm; is the matrix composed of the centrifugal force and Coriolis force of the Kth UAV with a robotic arm; is the matrix of gravity terms of the Kth UAV with a robotic arm; u=[u1u2] T ,u1=[f x f z τ θ τ q1 τ q2 ] T ,u2=[f x f z τ θ τ q1 τ q2 ] T , f x 、f z are the forces of the UAV with robotic arms in the x and z directions, τ θ is the pitch angle torque of the UAV with robotic arm, τ q1 , τ q2 The angular torque of the first and second robotic arms of the UAV with robotic arms respectively;

[0042]

[0043] Where J BK is the Jacobian matrix of the Kth UAV with a robotic arm, J EKBK is the Jacobian matrix of the robotic arm of the Kth UAV with robotic arm, F EK is the contact force matrix when the end of the robotic arm of the Kth UAV with a robotic arm contacts the environment.

[0044] Preferably, the control function of the cooperative formation impedance controller in step S3 is:

[0045]

[0046]

[0047] Among them, M SK is the expected inertia matrix of the Kth UAV with a robotic arm, B SK is the expected damping matrix of the Kth UAV with a robotic arm, G SK is the expected stiffness matrix of the Kth UAV with a robotic arm, F PK is the contact force matrix between the end of the robotic arm of the Kth UAV and the environment; x PK is the expected position of the end of the robotic arm of the Kth UAV with a robotic arm; x SK is the actual position of the end of the robotic arm of the Kth UAV with a robotic arm, is the expected position velocity of the end of the robotic arm of the Kth UAV with a robotic arm, is the actual position velocity of the end of the robotic arm of the Kth UAV with a robotic arm, is the expected position acceleration of the end of the robotic arm of the Kth UAV with a robotic arm, is the actual position acceleration of the end of the robotic arm of the Kth UAV with a robotic arm;

[0048] According to the mapping relationship, the end position and posture of the robotic arm of the multi-drone with a robotic arm are mapped to each other and to the object, and the equation is as follows:

[0049]

[0050] In the formula, x P1-2 The end position of the robotic arm of the first UAV with a robotic arm is mapped to the end position of the robotic arm of the second UAV with a robotic arm, x P2-1 The end position of the robotic arm of the second UAV with a robotic arm is mapped to the end position of the robotic arm of the first UAV with a robotic arm, x 12O is the position of the first and second UAVs with robotic arms mapped to the object O, T12 is the mapping matrix from the end of the first UAV with a robotic arm to the end of the second UAV with a robotic arm, T 21 is the mapping matrix from the end of the second UAV with a robotic arm to the end of the first UAV with a robotic arm, T 1O is the mapping matrix from the end of the first robotic arm drone to the object O, T 2O is the mapping matrix from the end of the robotic arm of the second UAV with a robotic arm to the object O;

[0051] The expected input pose for the robot arm and the grasped object can be obtained as follows:

[0052]

[0053] In the formula, x PO is the desired pose of the object, The output value of the impedance control mapping relationship of the first UAV with a robotic arm, The output value of the impedance control mapping relationship of the second UAV with a robotic arm, Output value for object mapping.

[0054] Preferably, the actual posture output of the synchronous impedance control of the UAV with a robotic arm in step S4 is as follows:

[0055]

[0056]

[0057]

[0058]

[0059] In the formula, k E1 k is the error compensation parameter of the actual posture of the robotic arm of the first UAV with a robotic arm and the synchronous input posture of the impedance control; A1 k is the synchronous posture error compensation parameter of the first and second UAVs with robotic arms; Q1 The error compensation parameters of the robot ends of the first and second robot arms are mapped to each other; k E2 The error compensation parameters of the actual posture of the second UAV with a robotic arm and the impedance control synchronous input posture; k A2 k is the synchronous posture error compensation parameter of the first and second UAVs with robotic arms; Q2 is the error compensation parameter of the end of the robotic arm of the second UAV with robotic arm; is the object pose at the end of the robotic arm of the UAV, X C1is the actual position output of the synchronous impedance control of the first UAV with a robotic arm; X C2 is the actual position output of the synchronous impedance control of the second UAV with a robotic arm; X1 is the actual output position of the end of the robotic arm of the first UAV with a robotic arm; X2 is the actual output position of the end of the robotic arm of the second UAV with a robotic arm.

[0060] Preferably, the motion trajectory of the object and the ends of the robotic arms of the first and second robotic arm drones are connected to each other through mapping, and the expression is:

[0061]

[0062] In the formula, is the object pose, is the expected position of the end of the robotic arm of the first UAV with a robotic arm, is the desired position of the end of the robotic arm of the second UAV with a robotic arm, T O1 and T O2 is the mapping matrix between the object and the end of the robotic arm of the first and second UAVs with robotic arms; according to the above formula, the motion trajectory of the object can be mapped to the robotic arms of the first and second UAVs with robotic arms, thereby realizing the motion trajectory planning of the robotic arms of the first and second UAVs with robotic arms.

[0063] The beneficial effects of the present invention are as follows: the present invention provides an impedance control method for cooperative formation of unmanned aerial vehicles with mechanical arms, by constructing a kinematic model and a dynamic model of unmanned aerial vehicles with mechanical arms, and constructing an impedance controller for cooperative formation; according to the task requirements, the reference object trajectory is designed, and a synchronous error compensation controller is constructed in combination with the forward motion control of the kinematic model and the error relationship of the impedance controller for cooperative formation; the kinematic model of the unmanned aerial vehicle with mechanical arms is corrected and compensated by the synchronous error compensation controller. The present invention adopts impedance control, which can solve the compliant grasping of the mechanical arm, reduce damage to objects, and reduce disturbances to the unmanned aerial vehicle. When a single unmanned aerial vehicle with a mechanical arm has insufficient lift, the present invention can be used to realize the cooperative grasping flight of multiple unmanned aerial vehicles with mechanical arms. When unmanned aerial vehicles with mechanical arms cooperatively grasp objects, they can fly stably under the synchronous impedance controller of the present invention, ensuring interference and accuracy during flight, and providing a new method for the compliant motion control of the mechanical arm of cooperative formation flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following is a brief introduction to the drawings required for the specific embodiments or the description of the prior art. In all the drawings, similar elements or parts are generally identified by similar reference numerals. In the drawings, the elements or parts are not necessarily drawn according to the actual scale.

[0065] Figure 1 It is a structural diagram of the UAV system with a mechanical arm in the present invention;

[0066] Figure 2 It is a block diagram of the synchronous impedance control system of the cooperative formation of unmanned aerial vehicles with mechanical arms in the present invention;

[0067] Figure 3 It is a flight simulation block diagram of a UAV with a mechanical arm cooperatively grasping an object in the present invention. DETAILED DESCRIPTION

[0068] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0069] It should be understood that when used in this specification and the appended claims, the terms "include" and "comprises" indicate the presence of described features, integers, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or combinations thereof.

[0070] It should also be understood that the terms used in the present specification are only for the purpose of describing specific embodiments and are not intended to limit the present invention. As used in the present specification and the appended claims, unless the context clearly indicates otherwise, the singular forms "a", "an" and "the" are intended to include plural forms.

[0071] It should be further understood that the term "and / or" used in the present description and the appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes these combinations.

[0072] like Figure 1 , Figure 2 The figure shows the block diagram of the synchronous impedance control system for cooperative formation of multiple UAVs with robotic arms, which mainly includes two parts: synchronous impedance control and error compensation. The contact force and torque are calculated online through impedance control, and the desired damping parameters are dynamically adjusted to achieve compliance when in contact with the environment and stable flight when collaboratively grasping objects. A method for impedance control of cooperative formation of UAVs with robotic arms provided in a specific embodiment of the present invention includes the following steps:

[0073] Step S1, constructing a kinematic model of a UAV with a robotic arm according to the homogeneous equation and the Jacobian matrix; the kinematic model of a UAV with a robotic arm is as follows:

[0074]

[0075]

[0076]

[0077]

[0078] Among them, P EK is the position matrix of the end of the robotic arm of the Kth UAV with robotic arm; P BK is the position matrix of the Kth UAV with robotic arm; R BK is the rotation matrix of the Kth UAV with a robotic arm, and T is the transpose; is the position matrix of the end of the robotic arm of the Kth UAV with a robotic arm relative to the UAV; ω EK is the angular velocity matrix of the end of the robotic arm of the Kth UAV with a robotic arm; ω BK is the angular velocity matrix of the Kth UAV with a robotic arm; V is the angular velocity matrix of the end of the robotic arm of the Kth UAV with a robotic arm relative to the UAV; EK is the speed of the end of the robotic arm of the Kth UAV with a robotic arm; J BK is the Jacobian matrix of the Kth UAV with a robotic arm; J EKBK is the Jacobian matrix of the robotic arm of the Kth UAV with robotic arm; is the position and velocity of the Kth UAV with a robotic arm; is the robotic arm angle of the Kth UAV with robotic arm, where K = 1,2.

[0079] Said

[0080]

[0081] V EK =[ν EX ν EZ ν Eθ ] T ; (7)

[0082] in is the expected speed in the x direction of the Kth UAV with a robotic arm, is the expected speed of the Kth UAV with a robotic arm in the z direction, is the expected pitch angular velocity of the Kth UAV with a robotic arm, is the expected angular velocity of the first joint of the robotic arm of the Kth UAV with a robotic arm, is the expected angular velocity of the second joint of the robotic arm of the Kth UAV with robotic arm, J BK is the Jacobian matrix of the Kth UAV with a robotic arm; J EKBK is the Jacobian matrix of the robotic arm of the Kth UAV with robotic arm, ν EX is the speed of the end of the robotic arm of the Kth UAV in the x direction, ν EZ is the speed of the end of the robotic arm of the Kth UAV in the z direction, ν Eθ is the pitch angular velocity of the robotic arm end of the Kth UAV with a robotic arm.

[0083] The following steps are also included:

[0084] set up

[0085]

[0086] From the above formula, we can get:

[0087]

[0088]

[0089] In the formula, J ζK is the Jacobian matrix J of the Kth UAV with a robotic arm BK The values ​​of the 1st to 3rd rows and the 1st and 2nd columns, J σK is the Jacobian matrix J of the Kth UAV with a robotic arm BK The values ​​of the 1st to 3rd rows and 3rd columns;

[0090] set up J ηK =[J ζK J EKBK ], and the above formula is transformed into:

[0091]

[0092] In the formula, χ K is the symmetric gain matrix of the Kth UAV with a robotic arm; e K is the position error of the end of the robotic arm of the Kth UAV with a robotic arm; and for the position y of the end of the robotic arm of the Kth UAV with a robotic arm EK and the yaw angle ψ EK is the position y of the drone BK and the yaw angle ψ BK .

[0093] Step S2, a dynamic model of the UAV with a robotic arm is established based on the kinematic model established in step S1 and the Lagrange-Euler equation. The dynamic model of the UAV with a robotic arm in step S2 is as follows:

[0094]

[0095]

[0096] X B1 =[xz θ] T ; (15)

[0097] Q1=[q1 q2] T ; (16)

[0098] X B2 =[xz θ] T ; (17)

[0099] Q2=[q1 q2] T ; (18)

[0100] Where x is the position of the UAV with a robotic arm in the x direction, z is the position of the UAV with a robotic arm in the z direction, θ is the pitch angle of the UAV with a robotic arm, q1 and q2 are the angles of the first and second joints of the robotic arm of the Kth UAV with a robotic arm respectively; is the inertia matrix of the Kth UAV with a robotic arm; is the matrix composed of the centrifugal force and Coriolis force of the Kth UAV with a robotic arm; is the matrix of gravity terms of the Kth UAV with a robotic arm; u=[u1u2] T ,u1=[f x f z τ θ τ q1 τ q2 ] T ,u2=[f x f z τ θ τ q1 τ q2 ] T , f x 、f z are the forces of the UAV with robotic arms in the x and z directions, τ θ is the pitch angle torque of the UAV with robotic arm, τ q1 , τ q2 The angular torque of the first and second robotic arms of the UAV with robotic arms respectively;

[0101]

[0102] Where J BK is the Jacobian matrix of the Kth UAV with a robotic arm, J EKBK is the Jacobian matrix of the robotic arm of the Kth UAV with robotic arm, F EK is the contact force matrix when the end of the robotic arm of the Kth UAV with a robotic arm contacts the environment.

[0103] For the Kth UAV with a robotic arm, the force f in the y direction is yK and roll angle φ K and the yaw angle ψ K The moment τ in the x and z directions xK and τ zK Its expression is:

[0104]

[0105] In the formula, is the partial derivative of the sum of the kinetic energy and potential energy of the Kth UAV with a robotic arm, is the partial derivative of the velocity of the Kth UAV with a robotic arm in the y direction, is the partial derivative of the roll angle of the Kth UAV with a robotic arm in the x direction, is the partial derivative of the yaw angle of the Kth UAV with a robotic arm in the z direction, is the partial derivative of the position of the Kth UAV with a robotic arm in the y direction, is the partial derivative of the rolling angular velocity of the Kth UAV with a robotic arm in the x direction, is the partial derivative of the yaw angular velocity of the Kth UAV with a robotic arm in the z direction.

[0106] In order to achieve closed-loop control of the UAV with a robotic arm, it is necessary to perform forward and inverse operations on the dynamics of the UAV with a robotic arm. The forward dynamics is mainly to calculate the position and posture of the Kth UAV with a robotic arm [x K y K z K φ K θ K ψ K ] T and the two rotation angles q of the two-degree-of-freedom robot i (i=1,2). Its equation is:

[0107]

[0108] in Through integration, the actual positions of x and z, the pitch angle of θ, and the angle of q1q2 are output. Similarly, y K Position and roll angle φ K and yaw angle ψ KPerform positive operation to output the actual position and actual attitude angle, then the overall output is

[0109] In step S3, referring to the grasping motion, force and torque of multiple UAVs with robotic arms, a collaborative formation impedance controller is constructed; the control function of the collaborative formation impedance controller in step S3 is:

[0110]

[0111]

[0112] Among them, M SK is the expected inertia matrix of the Kth UAV with a robotic arm, B SK is the expected damping matrix of the Kth UAV with a robotic arm, G SK is the expected stiffness matrix of the Kth UAV with a robotic arm, F PK is the contact force matrix between the end of the robotic arm of the Kth UAV and the environment; x PK is the expected position of the end of the robotic arm of the Kth UAV with a robotic arm; x SK is the actual position of the end of the robotic arm of the Kth UAV with a robotic arm, is the expected position velocity of the end of the robotic arm of the Kth UAV with a robotic arm, is the actual position velocity of the end of the robotic arm of the Kth UAV with a robotic arm, is the expected position acceleration of the end of the robotic arm of the Kth UAV with a robotic arm, is the actual position acceleration of the end of the robotic arm of the Kth UAV with a robotic arm;

[0113] According to the mapping relationship, the end position and posture of the robotic arm of the multi-drone with a robotic arm are mapped to each other and to the object, and the equation is as follows:

[0114]

[0115] In the formula, x P1-2 The end position of the robotic arm of the first UAV with a robotic arm is mapped to the end position of the robotic arm of the second UAV with a robotic arm, x P2-1 The end position of the robotic arm of the second UAV with a robotic arm is mapped to the end position of the robotic arm of the first UAV with a robotic arm, x 12O is the position of the first and second UAVs with robotic arms mapped to the object O, T 12 is the mapping matrix from the end of the first UAV with a robotic arm to the end of the second UAV with a robotic arm, T 21 is the mapping matrix from the end of the second UAV with a robotic arm to the end of the first UAV with a robotic arm, T1O is the mapping matrix from the end of the first robotic arm drone to the object O, T 2O is the mapping matrix from the end of the robotic arm of the second UAV with a robotic arm to the object O;

[0116] The expected input pose for the robot arm and the grasped object can be obtained as follows:

[0117]

[0118] In the formula, x PO is the desired pose of the object, The output value of the impedance control mapping relationship of the first UAV with a robotic arm, The output value of the impedance control mapping relationship of the second UAV with a robotic arm, Output value for object mapping.

[0119] Step S4, design the reference object trajectory according to the task requirements, and construct a synchronous error compensation controller by combining the forward motion control of the kinematic model and the error relationship of the cooperative formation impedance controller; the actual posture output of the synchronous impedance control of the UAV with a robotic arm in step S4 is as follows:

[0120]

[0121]

[0122]

[0123]

[0124] In the formula, k E1 k is the error compensation parameter of the actual posture of the robotic arm of the first UAV with a robotic arm and the synchronous input posture of the impedance control; A1 k is the synchronous posture error compensation parameter of the first and second UAVs with robotic arms; Q1 The error compensation parameters of the robot ends of the first and second robot arms are mapped to each other; k E2 The error compensation parameters of the actual posture of the second UAV with a robotic arm and the impedance control synchronous input posture; k A2 k is the synchronous posture error compensation parameter of the first and second UAVs with robotic arms; Q2 is the error compensation parameter of the end of the robotic arm of the second UAV with robotic arm; is the object pose at the end of the robotic arm of the UAV, X C1 is the actual position output of the synchronous impedance control of the first UAV with a robotic arm; X C2is the actual position output of the synchronous impedance control of the second UAV with a robotic arm; X1 is the actual output position of the end of the robotic arm of the first UAV with a robotic arm; X2 is the actual output position of the end of the robotic arm of the second UAV with a robotic arm.

[0125] The error is mainly composed of the error of the robot position control and the position error of the impedance control. In order to stably grasp the object, the motion trajectory of the object and the robot arms of the first and second robot drones are connected through mapping, and the expression is:

[0126]

[0127] In the formula, is the object pose, is the expected position of the end of the robotic arm of the first UAV with a robotic arm, is the desired position of the end of the robotic arm of the second UAV with a robotic arm, T O1 and T O2 is the mapping matrix between the object and the end of the manipulator of the first and second UAVs with manipulators; according to the above formula, the motion trajectory of the object can be mapped to the manipulator of the first and second UAVs with manipulators, thereby realizing the motion trajectory planning of the manipulator of the first and second UAVs with manipulators. When disturbed by the external force F, the position and posture of the manipulator of the first and second UAVs with manipulators are obtained by impedance control, and the position and posture are mapped to each other, thereby realizing the coordinated motion of the UAVs with manipulators when grasping objects.

[0128] Step S5: Use the synchronous error compensation controller to correct and compensate the kinematic model of the UAV with a robotic arm. Set the expected inertia matrix M of the synchronous impedance controller S1 =diag(0.1 0.1 0.1 0.1 0.1 0.1),M S2 =diag(0.1 0.1 0.1 0.1 0.1 0.1), initial damping matrix B S1 =diag(100 100 100 100 100 100), B S2 =diag(100 100 100 100 100 100), stiffness matrix G S1 =diag(1200 1200 1200 1200 12001200), G S2 =diag(1200 1200 1200 1200 1200 1200). Through MATLAB simulation, Figure 3As shown, when the robotic arm interacts with the environment, the forces and torques it receives are compliantly controlled through a synchronous impedance controller to achieve stable flight of multiple UAVs with robotic arms to collaboratively grasp objects.

[0129] Those of ordinary skill in the art will appreciate that the units of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition of each example has been generally described in terms of function in the above description. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0130] In the embodiments provided in the present application, it should be understood that the division of units is merely a logical function division, and there may be other division methods in actual implementation, for example, multiple units may be combined into one unit, one unit may be split into multiple units, or some features may be ignored.

[0131] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein by equivalents. These modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention, and they should all be included in the scope of the claims and specification of the present invention.

Claims

1. A method for controlling impedance of a coordinated formation of unmanned aerial vehicles with a mechanical arm, characterized in that: The following steps are involved: Step S1, constructing a kinematic model of a UAV with a robotic arm according to homogeneous equations and Jacobian matrix; Step S2, establishing a dynamic model of the UAV with a robotic arm according to the kinematic model established in step S1 and based on the Lagrange-Euler equation; Step S3, referring to the grasping motion, force and torque of multiple UAVs with robotic arms, constructing a collaborative formation impedance controller; Step S4, designing a reference object trajectory according to the task requirements, and constructing a synchronization error compensation controller by combining the forward motion control of the kinematic model and the error relationship of the cooperative formation impedance controller; Step S5: using a synchronous error compensation controller to correct and compensate the kinematic model of the UAV with a robotic arm; The control function of the cooperative formation impedance controller in step S3 is: ；（20） ； （21） in, For the The expected inertia matrix of a UAV with a robotic arm, For the The expected damping matrix of a UAV with a robotic arm, For the The expected stiffness matrix of a UAV with a robotic arm, For the The contact force matrix between the end of the robotic arm of a UAV and the environment; For the The desired position of the end of the robotic arm of the UAV with a robotic arm; For the The actual position of the end of the robotic arm of the UAV with a robotic arm. For the The expected position and velocity of the end of the robotic arm of a UAV with a robotic arm, For the The actual position and velocity of the end of the robotic arm of the UAV with a robotic arm, For the The expected position acceleration of the end of the robotic arm of a UAV with a robotic arm, For the The actual position acceleration of the end of the robotic arm of the UAV with a robotic arm; According to the mapping relationship, the end position and posture of the robotic arm of the multi-drone with a robotic arm are mapped to each other and to the object, and the equation is as follows: ；（22） In the formula, For the The end position of the robotic arm of the UAV with a robotic arm is mapped to the The end position of the robotic arm of the UAV with a robotic arm, For the The end position of the robotic arm of the UAV with a robotic arm is mapped to the The end position of the robotic arm of the UAV with a robotic arm, For the Frame and The end of the robotic arm of a drone with a robotic arm is mapped to the object The posture, For the The end of the robotic arm of the drone with a robotic arm is The end-arm mapping matrix of a UAV with a robotic arm. For the The end of the robotic arm of the drone with a robotic arm is The end-arm mapping matrix of a UAV with a robotic arm. For the The end of the robotic arm of a drone with a robotic arm reaches the object The mapping matrix, For the The end of the robotic arm of a drone with a robotic arm reaches the object The mapping matrix of The expected input pose for the robot arm and the grasped object can be obtained as follows: ；（23） In the formula, is the desired pose of the object, For the The impedance control mapping output value of the UAV with a robotic arm, For the The impedance control mapping output value of the UAV with a robotic arm, Output value for object mapping.

2. The impedance control method for cooperative formation of UAVs with mechanical arms according to claim 1, characterized in that: The kinematic model of the UAV with a robotic arm in step S1 is as follows: ；（1） ；（2） ；（3） ；（4） in, For the The end position matrix of the robotic arm of a UAV with a robotic arm; For the Position matrix of drones with robotic arms; For the The rotation matrix of a drone with a robotic arm, is transposed; For the The position matrix of the end of the robotic arm of the UAV with a robotic arm relative to the UAV; For the The angular velocity matrix of the end of the robotic arm of a UAV with a robotic arm; For the Angular velocity matrix of a UAV with a robotic arm; For the The angular velocity matrix of the end of the robotic arm of a UAV with a robotic arm relative to the UAV; For the The velocity matrix of the end of the robotic arm of a UAV with a robotic arm; For the Jacobian matrix of a drone with a robotic arm; For the Jacobian matrix of a robotic arm with a robotic drone; For the The pose and velocity matrix of a UAV with a robotic arm; For the The robot arm angle matrix of a drone with a robot arm.

3. The impedance control method for cooperative formation of UAVs with mechanical arms according to claim 2, characterized in that: in: ；（5） ；（6） ；（7） in For the A drone with a robotic arm Direction Expected Speed, For the Drone with arms Direction Expected Speed, For the The expected pitch angular velocity of a UAV with a robotic arm, For the The expected angular velocity of the first joint of the robotic arm of a drone with a robotic arm, For the The expected angular velocity of the second joint of the robotic arm of a drone with a robotic arm, For the The end of the robotic arm of a drone with a robotic arm The speed of the direction, For the The end of the robotic arm of a drone with a robotic arm The speed of the direction, For the The pitch angular velocity of the robotic arm end of a UAV with a robotic arm.

4. The impedance control method for cooperative formation of UAVs with mechanical arms according to claim 3 is characterized in that: The following steps are also included: set up ; (8) ；（9） From the above formula, we can get: ；（10） ；（11） In the formula, For the Jacobian matrix of a drone with a robotic arm The values ​​of the 1st to 3rd rows and the 1st and 2nd columns, For the Jacobian matrix of a drone with a robotic arm The values ​​of the 1st to 3rd rows and 3rd columns; set up , , transformed from the above formula: ； （12） In the formula, , For the A symmetric gain matrix for a UAV with a robotic arm; For the The end position error of the robotic arm of the UAV with a robotic arm; The position of the end of the robotic arm of a UAV with a robotic arm and yaw angle The position of the drone and yaw angle .

5. The impedance control method for cooperative formation of UAVs with mechanical arms according to claim 1, characterized in that: The dynamic model of the UAV with a robotic arm in step S2 is specifically as follows: ；（13） ；（14） ；（15） ；（16） ；（17） ；（18） in For drones with robotic arms Position in direction, For drones with robotic arms Position in direction, is the pitch angle of the UAV with robotic arm, , Respectively The angles of the first and second joints of the robotic arm of the UAV with robotic arm; For the The inertial matrix of a UAV with a robotic arm; For the The matrix of centrifugal and Coriolis forces of a drone with a robotic arm; For the The matrix of gravity terms of a drone with a robotic arm; , , , , They are respectively drones with robotic arms , Directional force, is the pitch angle torque of the UAV with robotic arm, , The angular torque of the first and second robotic arms of the UAV with robotic arms respectively; ； （19） In the formula For the The Jacobian matrix of a drone with a robotic arm, For the The Jacobian matrix of a robotic arm with a robotic drone, For the The contact force matrix of the end of the robotic arm of a UAV with a robotic arm when it contacts the environment.

6. The impedance control method for cooperative formation of UAVs with mechanical arms according to claim 1, characterized in that: The actual position output of the synchronous impedance control of the UAV with a robotic arm in step S4 is as follows: ；（24） ；（25） ；（26） ； （27） In the formula, Input the error compensation parameters of the actual posture of the robotic arm of the first UAV with a robotic arm and the impedance control synchronous posture; The compensation parameters for the synchronous posture errors of the first and second UAVs with robotic arms; For the The error compensation parameters of the robot arm end of the first and second UAVs with robot arms are mapped to each other; Input the error compensation parameters of the actual posture of the robotic arm of the second UAV with robotic arm and the impedance control synchronous posture; The compensation parameters for the synchronous posture errors of the first and second UAVs with robotic arms; For the Error compensation parameters of the end of the robotic arm of a UAV with a robotic arm; is the object pose at the end of the robotic arm of the UAV with robotic arm, For the Actual pose output of synchronous impedance control of a UAV with a robotic arm; For the Actual pose output of synchronous impedance control of a UAV with a robotic arm; For the The actual output position of the end of the robotic arm of the UAV with a robotic arm; For the The actual output pose of the end of the robotic arm of a UAV with a robotic arm.

7. The impedance control method for cooperative formation of UAVs with mechanical arms according to claim 1, characterized in that: The trajectory of the object and the Frame and The robotic arms of the UAV with robotic arms are connected to each other through mapping, and the expression is: ；（28） In the formula, is the object pose, For the The desired position of the end of the robotic arm of the UAV with a robotic arm, For the The desired position of the end of the robotic arm of the UAV with a robotic arm, and For the object and Frame and The mapping matrix of the end of the robotic arm of a UAV with a robotic arm; According to the above formula, the motion trajectory of the object can be mapped to the first Frame and Mount the robot with a robotic arm on the robot arm, and then realize the first Frame and Motion trajectory planning of the robotic arm of a UAV with a robotic arm.

Images