Cooperative control method and system for low-altitude operation of multiple aerial mechanical arms

Abstract

The invention provides a cooperative control method and system for low-altitude operation of multiple air mechanical arms, and the method comprises the following steps: building dynamic models of the air mechanical arms in multiple free flight modes, and designing a sliding mode controller according to the dynamic models of the multiple air mechanical arms, so as to achieve the pose control of the multiple air mechanical arms; parameters of the sliding mode controllers of the multiple aerial mechanical arms are set through an improved crown porcupine algorithm; contact collision dynamic models of the multiple aerial mechanical arms are established, an operation space compliance controller of the aerial mechanical arms is designed according to the multiple contact collision dynamic models, and grabbing of the platform is achieved according to the operation space compliance controller and the set sliding mode controller; and a rigid-flexible coupling dynamic model is established, a fault-tolerant controller is designed according to the rigid-flexible coupling dynamic model, and pose control of the grabbing platform is achieved according to the fault-tolerant controller, the operation space compliance controller and the set sliding mode controller.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a collaborative control method for low-altitude operations of multiple aerial manipulators and a collaborative control system for low-altitude operations of multiple aerial manipulators. Background Art

[0002] In the fields of industrial automation, logistics, and automated assembly, fixed-base robotic arms are increasingly unable to meet the needs of a large workspace, especially when dealing with complex handling tasks, where a single fixed-base robotic arm has limitations in load capacity and flexibility. In contrast, mobile robotic arms, including ground mobile robotic arms and aerial robotic arms, can more effectively meet these challenges. Ground robotic arms can move flexibly over a large range to meet the operational needs of different locations, while aerial robotic arms can move freely in three-dimensional space to cover a wider work area. In the prior art, although multiple aerial robotic arms can improve the overall handling capacity when working in collaboration, the control of the accuracy and stability of multiple aerial robotic arms when working in collaboration still needs to be further improved. Summary of the invention

[0003] In order to solve the above technical problems, the present invention provides a collaborative control method and system for low-altitude operations of multiple aerial manipulators, which can significantly improve the stability and accuracy of collaborative control of multiple aerial manipulators during low-altitude operations.

[0004] The technical solution adopted by the present invention is as follows:

[0005] A collaborative control method for low-altitude operations of multiple aerial manipulators comprises the following steps: establishing dynamic models of the aerial manipulators in multiple free flight modes, and designing sliding mode controllers according to the dynamic models of the aerial manipulators to achieve posture control of the aerial manipulators; tuning the parameters of the sliding mode controllers of the aerial manipulators through an improved crown porcupine algorithm; establishing contact and collision dynamic models of the aerial manipulators, designing operation space compliance controllers of the aerial manipulators according to the contact and collision dynamic models, and achieving grasping of a platform according to the operation space compliance controller and the adjusted sliding mode controller; establishing a rigid-flexible coupling dynamic model, designing a fault-tolerant controller according to the rigid-flexible coupling dynamic model, and achieving posture control of a grasping platform according to the fault-tolerant controller, the operation space compliance controller and the adjusted sliding mode controller.

[0006] In one embodiment of the present invention, the aerial manipulator includes a flight platform unit and a manipulator unit, and establishing a dynamic model of the aerial manipulator includes establishing a flight platform dynamic model for the flight platform unit and establishing a manipulator dynamic model for the manipulator unit.

[0007] In one embodiment of the present invention, when the plurality of aerial manipulators are in a collaborative transport mode, the plurality of aerial manipulators and the target object together constitute an equivalent parallel mechanism, and the base of the equivalent parallel mechanism is fixed.

[0008] In one embodiment of the present invention, the expression of the contact collision dynamics model is:

[0009]

[0010] Among them, F t is the contact collision force, k is the Hertzian stiffness, D = λδ n , n is the collision coefficient, λ is the damping coefficient, δ is the relative compression of the two monomers along the normal direction of the contact surface, is the relative compression speed.

[0011] In one embodiment of the present invention, the expression of the rigid-flexible coupling dynamic model is:

[0012]

[0013] Where M is the mass matrix, C is the damping matrix generated by the actuator damping and stiffness, K is the stiffness matrix generated by the actuator damping and stiffness, and K e is the stiffness matrix caused by the flexure hinge stiffness, is the transposed matrix of the Jacobian matrix of the load platform, is the displacement acceleration and rotational angular acceleration matrix of the load platform, is the displacement velocity and rotation angular velocity matrix of the load platform, f a is a vector of scalars corresponding to each leg, x p is the displacement and rotation angle matrix of the load platform.

[0014] A collaborative control system for low-altitude operations of multiple aerial manipulators comprises: a position control module, the position control module is used to establish a dynamic model of the aerial manipulators in multiple free flight modes, and a sliding mode controller is designed according to the dynamic models of the multiple aerial manipulators to achieve posture control of the multiple aerial manipulators; a tuning module, the tuning module is used to tune the parameters of the sliding mode controllers of the multiple aerial manipulators through an improved crown porcupine algorithm; a platform grasping module, the platform grasping module is used to establish a contact and collision dynamic model of the multiple aerial manipulators, an operation space compliance controller of the aerial manipulators is designed according to the multiple contact and collision dynamic models, and the grasping of the platform is achieved according to the operation space compliance controller and the adjusted sliding mode controller; a posture control module, the posture control module is used to establish a rigid-flexible coupling dynamic model, a fault-tolerant controller is designed according to the rigid-flexible coupling dynamic model, and the posture control of the grasping platform is achieved according to the fault-tolerant controller, the operation space compliance controller and the adjusted sliding mode controller.

[0015] In one embodiment of the present invention, the aerial manipulator includes a flight platform unit and a manipulator unit, and establishing a dynamic model of the aerial manipulator includes establishing a flight platform dynamic model for the flight platform unit and establishing a manipulator dynamic model for the manipulator unit.

[0016] In one embodiment of the present invention, when the plurality of aerial manipulators are in a collaborative transport mode, the plurality of aerial manipulators and the target object together constitute an equivalent parallel mechanism, and the base of the equivalent parallel mechanism is fixed.

[0017] In one embodiment of the present invention, the expression of the contact collision dynamics model is:

[0018]

[0019] Among them, F t is the contact collision force, k is the Hertzian stiffness, D = λδ n , n is the collision coefficient, λ is the damping coefficient, δ is the relative compression of the two monomers along the normal direction of the contact surface, is the relative compression speed.

[0020] In one embodiment of the present invention, the expression of the rigid-flexible coupling dynamic model is:

[0021]

[0022] Where M is the mass matrix, C is the damping matrix generated by the actuator damping and stiffness, K is the stiffness matrix generated by the actuator damping and stiffness, and K e is the stiffness matrix caused by the flexure hinge stiffness, is the transposed matrix of the Jacobian matrix of the load platform, is the displacement acceleration and rotational angular acceleration matrix of the load platform, is the displacement velocity and rotation angular velocity matrix of the load platform, f a is a vector of scalars corresponding to each leg, x p is the displacement and rotation angle matrix of the load platform.

[0023] Beneficial effects of the present invention:

[0024] The present invention establishes dynamic models of multiple aerial manipulators in free flight modes, and designs sliding mode controllers according to the dynamic models of the multiple aerial manipulators to achieve posture control of the multiple aerial manipulators. The parameters of the sliding mode controllers of the multiple aerial manipulators are adjusted through an improved crown porcupine algorithm. A contact collision dynamic model and a rigid-flexible coupling dynamic model of the multiple aerial manipulators are established, and an operating space compliant controller and a fault-tolerant controller are designed according to the multiple contact collision dynamic models to achieve posture control of a grasping platform. Therefore, the stability and accuracy of the coordinated control of the multiple aerial manipulators during low-altitude operations can be significantly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 It is a flow chart of a collaborative control method for low-altitude operation of multiple aerial manipulators according to an embodiment of the present invention;

[0026] Figure 2 A schematic diagram of a sliding mode control block for the position and attitude of a flight platform according to an embodiment of the present invention;

[0027] Figure 3 A simulation diagram of the response of a sliding mode controller about the X-axis according to a specific embodiment of the present invention;

[0028] Figure 4 A simulation diagram of the response of a sliding mode controller about the Y axis according to a specific embodiment of the present invention;

[0029] Figure 5 A simulation diagram of the response of a sliding mode controller about the Z axis according to a specific embodiment of the present invention;

[0030] Figure 6 A simulation diagram of the response of a sliding mode controller about the φ axis according to a specific embodiment of the present invention;

[0031] Figure 7 A simulation diagram of the response of a sliding mode controller about the θ axis according to a specific embodiment of the present invention;

[0032] Figure 8 A simulation diagram of the response of a sliding mode controller about the ψ axis according to a specific embodiment of the present invention;

[0033] Fig. 9 It is a schematic diagram of the impedance control block under the contact collision mode according to an embodiment of the present invention;

[0034] Fig.10 A coordinate diagram of an equivalent parallel platform according to an embodiment of the present invention;

[0035] Fig.11 A schematic diagram of a backstepping fault-tolerant control block based on a fixed-time disturbance observer according to an embodiment of the present invention;

[0036] Fig.12 A position response simulation diagram of a loading platform according to a specific embodiment of the present invention;

[0037] Fig.13 A simulation diagram of the speed response of a loading platform according to a specific embodiment of the present invention;

[0038] Fig.14 A schematic diagram of a working scenario of multiple aerial manipulators according to a specific embodiment of the present invention;

[0039] Fig.15 It is a block diagram of a collaborative control system for low-altitude operations of multiple aerial manipulators according to an embodiment of the present invention. DETAILED DESCRIPTION

[0040] 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 only 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.

[0041] Figure 1 The present invention is a flowchart of a collaborative control method for low-altitude operations of multiple aerial manipulators according to an embodiment of the present invention.

[0042] like Figure 1 As shown, the collaborative control method for low-altitude operation of multiple aerial manipulators according to an embodiment of the present invention comprises the following steps:

[0043] S1, establish the dynamic models of the aerial manipulators under multiple free flight modes, and design the sliding mode controllers according to the dynamic models of the multiple aerial manipulators to realize the position and posture control of the multiple aerial manipulators.

[0044] In one embodiment of the present invention, a single aerial manipulator may include a flight platform, a two-degree-of-freedom rope-driven manipulator, a drive motor, a flight control system and other devices, thereby, the aerial manipulator can be divided into a flight platform unit and a manipulator unit. First, a dynamic model of the aerial manipulator in a free flight mode can be established, including establishing a flight platform dynamics model for the flight platform unit and a manipulator dynamics model for the manipulator unit, wherein the gravity and joint torque of the manipulator can be included in the flight platform dynamics model as interference force and interference torque.

[0045] Specifically, the Lagrangian function can be introduced to establish the dynamic model of the flight platform, which can be expressed as:

[0046]

[0047] in, is the Lagrangian function, χ=[x,y,z,φ,θ,ψ] T is the transpose of the generalized coordinate matrix of the flight platform, Transpose of the generalized coordinate velocity matrix of the flight platform, ε = [x, y, z] T is the transpose of the position matrix of the flight platform in the world coordinate system, The transpose of the position velocity matrix of the flight platform in the world coordinate system, φ, θ, ψ are the Euler angles of the flight platform in the world coordinate system, ν is the transpose of the Euler angular velocity matrix of the flight platform in the world coordinate system, I is the moment of inertia matrix of the flight platform, m is the mass of the flight platform, and g is the acceleration due to gravity.

[0048] According to the flight platform dynamics model, the rigid body dynamics model of the flight platform unit can be derived, which can be expressed as:

[0049]

[0050] Among them, F S =RU F , F S is the total lift U F The force components on the three axes of the world coordinate system, τ includes the pitch moment τ φ , rolling moment τ θ and yaw moment τ ψ The set of τ=[τ φ ,τ θ ,τ ψ ] T , R is the transformation matrix from the body coordinate system to the inertial coordinate system, which can be expressed as:

[0051]

[0052] Among them, F Si is the thrust generated by the i-th rotor, MSi is the torque generated by the i-th rotor, i=1,2,3,4, and h is the semi-wheelbase.

[0053] Furthermore, the relationship between the i-th rotor thrust, torque and rotor speed can be expressed as:

[0054]

[0055] Among them, P r is the blade radius, A is the blade area, ρ is the local air density, Ω is the rotor speed, C T is the lift coefficient, C Q is the counter torque coefficient.

[0056] In general, the flight platform is basically in a hovering state during operation, and the aircraft is basically motionless or moves slightly. At this time, the changes in the pitch angle and roll angle are very small, and it can be considered that φ=θ≈0, so sinφ≈φ, cosφ≈1, sinθ≈θ, cosθ≈1. At the same time, the yaw angle usually does not need to be frequently controlled and can be considered In addition, the mechanical structure of the flight platform can be considered to be symmetrical. Therefore, the rigid body dynamics model of the flight platform can be simplified to obtain the dynamics model of the flight platform under hovering conditions, which can be expressed as:

[0057]

[0058] in, and is the angular velocity, and is the acceleration of the aerial manipulator position.

[0059] Furthermore, the designed two-degree-of-freedom rope-driven manipulator installs two sets of DC reduction motors on the lower hanging plate of the flight platform pod, and realizes the remote transmission of torque through flexible rope traction, which reduces the joint inertia ratio. Therefore, the Newton-Euler method can be introduced to derive the dynamic model of the manipulator, which can be expressed as:

[0060]

[0061] Among them, M J (q) is the inertia matrix, is the Coriolis force and centrifugal force matrix, G J (q) is the gravity torque of the robot arm, q is the joint angle vector, is the joint angular velocity vector, is the joint angular acceleration vector, τ e is the external torque on the robot arm, τ=[τ1,τ2] T is the transpose of the two joint torque matrices.

[0062] It should be noted that the embodiment of the present invention does not take into account the mass of the connecting rod itself, and concentrates the mass of the robot arm at the joint, thereby meeting the design requirements of the robot arm being lightweight and slender.

[0063] When the aerial manipulator performs a grasping task, the external torque τ e It not only includes the contact torque component generated by the external force on the end of the aerial manipulator, but also includes the interference torque component generated by the environment on the connecting rod, which can be expressed as:

[0064] τ e =τ c +τ d

[0065] Among them, τ c is the contact torque, τ d is the interference torque.

[0066] Subsequently, the gravity and joint torque of the manipulator can be taken into account as interference force and interference torque in the dynamic model of the flight platform, and the expression of the dynamic model of the aerial manipulator can be obtained as follows:

[0067]

[0068] Among them, m1 is the total mass of the robot arm, τ1 and τ2 are the torques of the two joints, and the motion equations of the first three positions can be expressed as:

[0069]

[0070] Among them, u 1x 、u 1y and u 1z is a sliding mode controller.

[0071] In one embodiment of the present invention, Figure 2 As shown, the position of the aerial manipulator can be controlled by referencing the sliding mode function in the dynamic model. For the motion equations of the first three positions in the dynamic model of the aerial manipulator, the sliding mode function can be expressed as:

[0072]

[0073] Among them, s1, s2, s3 are sliding mode variables, c1, c2 and c3 are sliding surface coefficients, e x =xx d 、e y =yy d and e z =zz d is the tracking error of the three positions of the aerial manipulator, and is the tracking error derivative of the three positions of the aerial manipulator, x d ,y d and z d are the desired positions of the three positions of the aerial manipulator.

[0074] Among them, when s1, s2, and s3 are all equal to 0, it can be said that the error between the state of the aerial robot arm and the expected state and the rate of change thereof have been controlled within a preset range.

[0075] Deriving s1, s2, and s3 respectively, and substituting the motion equations of the first three positions in the aerial manipulator dynamics model into the sliding mode function, we can obtain:

[0076]

[0077] in, and is the acceleration of the three desired positions of the aerial manipulator.

[0078] Can be designed separately 1x 、u 1y and u 1z The sliding mode controller is:

[0079]

[0080] Among them, k1, k2 and k3 are the exponential approach rates of the three position sliding mode controllers, and k1, k2 and k3 are all greater than zero, η1, η2 and η3 are the constant speed approach rates of the three position sliding mode controllers, and η1, η2 and η3 are all greater than a user-defined constant, and sgn is a symbolic formula.

[0081] For the motion equations of the last three positions in the aerial manipulator dynamics model, the sliding mode function can be expressed as:

[0082]

[0083] Among them, c4, c5 and c6 are sliding surface coefficients, e φ =φ-φ d 、e θ =θ-θ d and e ψ =ψ-ψ d are the three angle tracking errors of the aerial manipulator, and is the tracking error derivative of the desired angle of the three axes of the aerial manipulator, φ d ,θ d and ψ d is the expected three-axis angle of the aerial manipulator in the world coordinate system.

[0084] The desired posture can be set manually and kept as a small value, and the actual posture is obtained through the sensors of the aerial manipulator itself.

[0085] Deriving s4, s5 and s6 respectively, and substituting the motion equations of the last three positions in the aerial manipulator dynamics model into the sliding mode function, we can obtain:

[0086]

[0087] in, and is the expected angular acceleration of the three axes of the aerial manipulator in the world coordinate system.

[0088] Can design τ separately φ , τ θ and τ ψ The sliding mode controller is:

[0089]

[0090] Among them, k4, k5 and k6 are the exponential approach rates of the three attitude sliding mode controllers, and k4, k5 and k6 are all greater than zero, η4, η5 and η6 are the constant speed approach rates of the three attitude sliding mode controllers, and η4, η5 and η6 are all greater than a custom constant.

[0091] In a specific embodiment of the present invention, Figure 3 , Figure 4 , Figure 5 , Figure 6 , Figure 7 and Figure 8 They are the corresponding simulation diagrams of the sliding mode controller about the X-axis, Y-axis, Z-axis, φ-axis, θ-axis and ψ-axis respectively.

[0092] S2, the parameters of the sliding mode controllers of multiple aerial manipulators are tuned using the improved crown porcupine algorithm.

[0093] In one embodiment of the present invention, the sliding mode controller of a single UAV has 9 parameters to be optimized, including 3 sliding surface coefficients, 3 exponential approach rates and 3 constant velocity approach rates. These parameters generally require manual adjustment or empirical selection, which is not only time-consuming but may also lead to suboptimal control effects. The introduction of the improved crested porcupine algorithm can not only automatically optimize this process, making the selection of UAV sliding mode controller parameters more systematic and scientific, but also reduce the workload of manual adjustment and improve efficiency, so that the optimal controller parameters can be obtained when multiple aerial manipulators operate at low altitudes.

[0094] In a specific embodiment of the present invention, the optimization problem of a single aerial manipulator can be expanded to six, that is, the same constraints are applied to each aerial manipulator, and the total cost of the six aerial manipulators is used as the objective function. The total cost is calculated by the tracking error cost E of each aerial manipulator. i , System control quantity cost U i The objective function of the optimal control can be expressed as:

[0095]

[0096] Among them, a and b are the weights of tracking error and control quantity, E i is the tracking error cost of the i-th aerial manipulator, U i is the control cost of the i-th aerial robot arm, time is the operation time of the aerial robot arm, and f obj is the adaptation value.

[0097] In one embodiment of the present invention, the improved crested porcupine algorithm may have four defense mechanisms: vision, hearing, smell, and physical attack. Among them, the vision and hearing mechanisms reflect the exploration strategy of the improved crested porcupine algorithm, providing more candidate solutions for the three parameters of the sliding mode controller; while the smell and physical attack mechanisms reflect the convergence strategy of the improved crested porcupine algorithm, thereby improving the optimization accuracy of the three parameters of the sliding mode controller.

[0098] Specifically, similar to other metaheuristic group-based algorithms, the improved crested porcupine algorithm starts searching from an initial set of individuals, and the search process can be expressed as:

[0099] X i =L+r×(UL)|i=1,2...,N

[0100] Where N is the population size, X i is the i-th solution in the search space, L is the lower limit of the search range, U is the upper limit of the search range, and r is a random number between 0 and 1.

[0101] In the improved crested porcupine algorithm, not all crested porcupine individuals will activate the defense mechanism, only those that are threatened will activate it. Therefore, a cyclic population reduction strategy can be proposed based on this phenomenon, which can not only speed up the convergence speed, but also retain the population diversity. Among them, the cyclic population reduction strategy selects some crested porcupine individuals from the population during the optimization process to speed up the convergence speed, and reintroduces them afterwards to enhance diversity and avoid falling into local minima. The cycle is based on a loop variable to determine the number of times the operation is performed during the optimization process. The mathematical model of the gradual reduction of the population size can be expressed as:

[0102]

[0103] Among them, N′ is the initial population size, T is the variable that determines the number of cycles, t is the current number of iterations, and T max is the maximum number of function evaluations, % is the remainder, N min is the minimum number of individuals in the newly generated population.

[0104] In the visual defense stage, when a crested porcupine individual senses a predator, it will raise and fan its quills to attract the predator's attention. At this point, the predator has two choices: approach or move away from the crested porcupine individual. In the case of approaching, the distance between the predator and the crested porcupine individual is shortened, which promotes the exploration of the area between the predator and the crested porcupine individual and accelerates the convergence rate; in the case of moving away, the distance increases, encouraging exploration of farther areas to discover solutions that may not have been visited. Among them, the normal distribution can be used to generate random values ​​to simulate the above two situations. When the random value is between -1 and 1, it is encouraged to approach the crested porcupine individual, otherwise the predator will move away from the crested porcupine individual. Therefore, a simulation formula can be mathematically proposed based on the behavior of the predator, which can be expressed as:

[0105]

[0106] in, is the best solution of the tth iteration, is the i-th solution of the t-th iteration, R a1 is a random number based on normal distribution, R a2 is a random value in the interval [0,1], is a vector generated between the current CP and a randomly selected CP from the population, used to represent the position of the predator at the tth iteration, which can be expressed as:

[0107]

[0108] Where r is a random number between [1, N].

[0109] During the sound defense stage, crested porcupine individuals use sound methods to make noise and threaten predators. When the predator approaches the porcupine, the porcupine's voice will become louder. Therefore, a simulation formula can be proposed based on this behavior, which can be expressed as:

[0110]

[0111] Among them, r1 and r2 are random integers between [1, N], R a3 is a random number generated between [0, 1], and U1 is a randomly generated binary vector containing 0 and 1.

[0112] Among them, there are three possible situations that may occur in U1's strategy during the simulated sound defense phase:

[0113] (1) When U1 is equal to 0, the crested porcupine individual will stop diffusing the scent because the predator will stop moving due to fear of the crested porcupine individual, so the distance between the predator and the crested porcupine individual remains unchanged;

[0114] (2) When U1 is equal to 1, the crested porcupine will emit a significant scent due to the presence of predators nearby;

[0115] (3) When U1 is a combination of 0 and 1, the predator keeps a safe distance from the crested porcupine individual, so there is no need to release its scent widely.

[0116] In the odor defense stage, CP will secrete a foul odor and spread it in the surrounding area to prevent predators from approaching it. Therefore, a simulation formula can be proposed based on this behavior, which can be expressed as:

[0117]

[0118] Among them, r3 is a random number between [1, N], δ is a parameter used to control the search direction, is the position of the i-th crested porcupine individual at the t-th iteration, γ t As a defense factor, is the odor diffusion factor, which can be expressed as:

[0119]

[0120] in, is the objective function value of the tth individual at the tth iteration, ε is the minimum value to avoid division by zero, rand is a vector of randomly generated values ​​between [0,1], t is the number of current iterations, t max is the maximum number of iterations.

[0121] In the physical attack defense phase, when the predator is very close to it and attacks it with short and thick feathers, the crested porcupine individual will take physical attack. During the physical attack, the two objects merge strongly, which can represent a one-dimensional inelastic collision. Therefore, a simulation formula can be proposed based on this behavior, which can be expressed as:

[0122]

[0123] in, To obtain the best solution, α CPO is the convergence speed factor, R a4 and R a5 is a random value between [0,1], is the average force of the crested porcupine individual that affects the i-th predator, which can be expressed as:

[0124]

[0125] Among them, m i is the mass of the i-th predator individual at iteration t, is the final velocity of the i-th predator individual at the next iteration t+1, is the initial velocity of the i-th predator at iteration t, R a6 is a vector of random values ​​between [0,1].

[0126] S3, establish contact and collision dynamic models of multiple aerial manipulators, design the operation space compliance controller of the aerial manipulators according to the multiple contact and collision dynamic models, and realize the grasping of the platform according to the operation space compliance controller and the adjusted sliding mode controller.

[0127] In one embodiment of the present invention, the aerial manipulator will inevitably come into contact and collide with the target object during the process of grasping the target object. The impact caused by the contact process will not only cause a certain degree of disturbance to the end and the target object, but also cause disturbance to the base due to the unique dynamic coupling characteristics of the aerial manipulator, which may affect the stability of the aerial manipulator. Therefore, a corresponding contact and collision dynamics model can be established during the process of contact and collision between the aerial manipulator and the target object to characterize the relationship between the contact and collision force and the movement of the aerial manipulator.

[0128] Specifically, if Fig. 9 As shown in the figure, the nonlinear damping model can be used to describe the process of contact and collision between the aerial manipulator and the target object, which can avoid the situation where the contact force at the initial position and the end position of the collision is not zero. In addition, since the damping term is considered in this model, the energy loss can be avoided from being ignored. Therefore, the contact collision dynamics model can be expressed as:

[0129]

[0130] Among them, F t is the contact collision force, k is the Hertzian stiffness, D = λδ n , n is the collision coefficient, λ is the damping coefficient, δ is the relative compression of the two monomers along the normal direction of the contact surface, is the relative compression speed.

[0131] The contact force collision force vector of the aerial robot arm is:

[0132] F e =NF t

[0133] Where N is the normal vector at the contact collision point.

[0134] According to the kinematic analysis of the aerial manipulator, the end motion and joint motion of the aerial manipulator can satisfy the following relationship:

[0135]

[0136] in, is the terminal posture velocity vector, J is the Jacobian matrix, is the joint velocity of the aerial manipulator.

[0137] When the redundancy of the aerial manipulator is not considered, the mapping relationship between the joint acceleration and the manipulator end acceleration can be obtained by taking the derivative of both sides of the relationship between the manipulator end motion and the joint motion:

[0138]

[0139] in, is the joint acceleration, is the acceleration of the end position of the robot arm, J -1 is the inverse of the Jacobian matrix, is the derivative of the Jacobian matrix.

[0140] The relationship between the joint torque τ and the terminal generalized driving force F is:

[0141] τ=J T F

[0142] Substitute the relationship between the end motion of the aerial manipulator and the joint motion, the relationship between the joint acceleration and the end acceleration of the manipulator, and the relationship between the joint torque and the end generalized driving force into the manipulator dynamics model, and multiply (J -1 ) T The dynamic equation of the aerial manipulator in the operating space can be obtained as follows:

[0143]

[0144] Among them, M o is the inertia matrix, C o is the coefficient matrix of Coriolis force and centrifugal force, G o is the gravity coefficient matrix, and M o =(J -1 ) T MJ -1 , G o =(J -1 ) T G. F c is the generalized contact force at the end of the robot arm, F d is the generalized disturbance force at the end of the robot arm, and F c =(J -1 )T τ c , F d =(J -1 ) T τ d , (J -1 ) T is the transpose of the generalized inverse Jacobian matrix.

[0145] According to the generalized contact force F at the end of the robot c The impedance equation of the aerial manipulator operation space can be constructed, and the impedance equation can be expressed as:

[0146]

[0147] Among them, M I is the positive definite matrix of inertia of the expected control of the equivalent model, B I is the positive definite matrix of the damping of the expected control of the equivalent model, K I are the positive definite matrices of the stiffness of the expected control of the equivalent model, x Jd is the desired displacement of the end of the robot arm, is the desired velocity of the end of the robot arm, is the desired acceleration at the end of the robot arm, x J is the displacement of the end of the robot arm, is the speed of the end of the robot arm, is the acceleration of the end of the robot arm.

[0148] The acceleration of the end of the robot arm The acceleration input at the end of the robot arm is recorded as It can be expressed as:

[0149]

[0150] In order to achieve tracking control of the contact force, the acceleration input can be Modification is made to introduce the difference F between the actual contact force and the expected contact force e -F d and the force control gain K f To replace F c Therefore, the input acceleration of the end of the robot arm is can be re-expressed as:

[0151]

[0152] Therefore, the joint control torque of the aerial manipulator can be obtained through the above formula:

[0153]

[0154] S4, establish a rigid-flexible coupling dynamic model, design a fault-tolerant controller based on the rigid-flexible coupling dynamic model, and realize the posture control of the grasping platform based on the fault-tolerant controller, the operating space compliant controller and the adjusted sliding mode controller.

[0155] In one embodiment of the present invention, when the multiple aerial manipulators are in the collaborative handling mode, the multiple aerial manipulators and the target object together constitute an equivalent parallel mechanism, and the base of the equivalent parallel mechanism is fixed, such as Fig.10 shown.

[0156] In one embodiment of the present invention, based on the analysis of the internal coupling characteristics of the system, the inertial force, Coriolis force, centripetal force and elastic force generated by each leg during the movement and the load platform can be described as a transmission relationship between force and movement.

[0157] Calculate the virtual power of the active force and inertia force acting on the entire equivalent parallel mechanism. According to the virtual power principle, we can get:

[0158]

[0159] Among them, δ is the change of imaginary quantity, p is the displacement of the load platform, and f a , l, θ u , u ,θ s , s and γ s is a vector of corresponding scalars in each leg, for example, f a =(f a1 … f a6 ) T , l=(l1…l6) T , k is the stiffness of each leg, Δl is the instantaneous slight change in the length of the six-leg ball joint and the universal joint connection direction, l i is the length of the line connecting the ball joint and the universal joint of the i-th leg, c is the damping value of each leg, k is the rate of change of the length of the six-leg ball joint and the universal joint in the direction of the connection line with time, θu The flexible universal joint is ui Shaft stiffness, k ψu For the flexible universal joint around x ui Shaft stiffness, k θs The flexible universal joint is si Shaft stiffness, k ψs For the flexible universal joint around x si Shaft stiffness, k γs The flexible universal joint is si Axis stiffness, m p is the mass of the load platform, I p is the inertia of the load platform, is the displacement velocity of the load platform, is the displacement acceleration of the load platform, ω p is the rotation speed of the center of mass of the load platform, is the rotational acceleration of the load platform center of mass, ω d is the angular velocity of the connecting rod in the leg, is the angular velocity of the connecting rod in the leg in the {Bi} coordinate system, m d is the mass of the connecting rod in the leg, is the velocity of the connecting rod center of mass in {Bi}, is the angular velocity of the connecting rod center of mass in {Bi}, is the moment of inertia of the connecting rod in {Bi}, and {Bi} is the coordinate system of the i-th flexible universal joint.

[0160] Since the working space of the equivalent parallel mechanism of the embodiment of the present invention is very small, the joint variables have sufficient accuracy when linearized at the equilibrium position. Therefore, the Coriolis force and the centripetal force can be ignored, so that the virtual power formula of the load platform displacement can be simplified to:

[0161]

[0162] in, equal is the transpose of the displacement velocity and rotation angular velocity matrix of the load platform, J lx is the Jacobian matrix of the load platform, J θux and J ψux is the Jacobian matrix of the universal joint, J θsx , J ψsx and J ψsx is the Jacobian matrix of the spherical joint, is the Jacobian matrix for each link.

[0163] because The components of are independent of each other, which means that all coefficients are zero, thus the rigid-flexible coupling dynamic model can be obtained, and its expression can be:

[0164]

[0165] Where M is the mass matrix, C is the damping matrix generated by the actuator damping and stiffness, K is the stiffness matrix generated by the actuator damping and stiffness, and K e is the stiffness matrix caused by the flexure hinge stiffness, is the transposed matrix of the Jacobian matrix of the load platform, is the displacement acceleration and rotational angular acceleration matrix of the load platform, is the displacement velocity and rotation angular velocity matrix of the load platform, fa is a vector of scalars corresponding to each leg, x p is the displacement and rotation angle matrix of the load platform.

[0166] In actual work, the aerial manipulator may have certain faults during operation, such as mechanical faults and manipulator arm faults. Considering that the manipulator may have gain faults and bias faults, the manipulator arm fault model can be expressed as:

[0167]

[0168] Among them, f a p is the actual output force, ρ is the force failure ratio matrix, and ρ i ∈(0,1), f is the bias force function, I is the identity matrix, and diag is the diagonal matrix.

[0169] In this regard, the embodiment of the present invention designs a fault-tolerant controller for the rigid-flexible coupling dynamics model, which is used to achieve stable control of the position when a flight platform fails. Fig.11 shown.

[0170] Specifically, the state variables x1 and x2 can be preset, and x1 = x P , From this we can get:

[0171]

[0172] Considering the uncertainty of the rigid-flexible coupling dynamic model, the nominal mass matrix M and the nominal damping matrix C can be expressed as follows using nominal values ​​and deviations: M = M n +ΔM,C=C n +ΔC, where M n is the nominal mass matrix of the model, C n is the nominal damping matrix of the model, thus, the rigid-flexible coupling dynamic model can be rewritten as:

[0173]

[0174] Furthermore, during the operation of the aerial manipulator, the acceleration of the carrier platform is unknown and its upper limit is difficult to determine. Therefore, a fixed-time disturbance observer can be introduced to estimate the uncertainty set of the state variables in real time and compensate for the uncertainty in the controller to ensure that the fault-tolerant controller has higher control accuracy and stronger anti-interference ability.

[0175] Among them, the fixed-time disturbance observer can be expressed as:

[0176]

[0177] in, is the estimated value of x2, is the estimated value of the uncertainty set D, K1 and K2 are gains, ζ∈(0,1) is the amplification factor, is the error between the estimated value of x2 and the true value of x2, v1 and v2 are power parameters, sig is the power sign function, is the error between the estimated value of D and the true value of D, where the power value satisfies v1∈(0.5,1.0), v2∈(1.0,1.5).

[0178] Therefore, the error can be defined based on the backstepping principle as:

[0179]

[0180] Among them, z1 and z2 are state variables under error constraints, x Pd is the expected position of the platform, and α is the virtual control variable.

[0181] Furthermore, the maximum pose tracking error value can be set to K a , and |z 1,i |≤K a (i=1,2,…,6), so that the tracking between the aerial manipulator and the desired posture is more accurate.

[0182] Specifically, the Lyapunov function can be designed in the form of V1, which can be expressed as:

[0183]

[0184] Then, the virtual control amount α is set, which can be expressed as:

[0185]

[0186] Among them, λ1 is the parameter to be designed.

[0187] Taking the derivative of z2, we can get:

[0188]

[0189] Taking the derivative of V1, we can get:

[0190]

[0191] Secondly, α and Substitute into In, we can get:

[0192]

[0193] Furthermore, in order to further improve the position tracking and speed control accuracy of the parallel platform, a maximum error limit K is introduced into the speed tracking error. b And|z 2,i |≤K b (i=1,2,…,6).

[0194] Specifically, first, the Lyapunov function can be designed in the form of V2:

[0195]

[0196] Then, taking the derivative of V2, we get:

[0197]

[0198] By taking the derivative of the virtual control amount α with respect to time, we can obtain:

[0199]

[0200] in, is the displacement acceleration and rotation angular acceleration matrix of the load platform.

[0201] Thus, the backstepping fault-tolerant controller can be obtained, which can be expressed as:

[0202]

[0203] Among them, Ξ and Γ are diagonal matrices, and λ1, λ2 and γ are parameters to be designed.

[0204] In a specific embodiment of the present invention, the response simulation diagram of the position and speed of the loading platform is as follows: Fig.12 and Fig.13 shown.

[0205] In a specific embodiment of the present invention, the equivalent parallel mechanism working scene composed of 6 aerial manipulators and the target object is as follows: Fig.14 shown.

[0206] According to the collaborative control method for low-altitude operations of multiple aerial manipulators in an embodiment of the present invention, dynamic models of the aerial manipulators in multiple free flight modes are established, and sliding mode controllers are designed based on the dynamic models of the multiple aerial manipulators to achieve posture control of the multiple aerial manipulators, and the parameters of the sliding mode controllers of the multiple aerial manipulators are adjusted through the improved crown porcupine algorithm, and contact and collision dynamic models and rigid-flexible coupling dynamic models of the multiple aerial manipulators are established, and an operating space compliant controller and a fault-tolerant controller are designed based on the multiple contact and collision dynamic models to achieve posture control of the grasping platform, thereby significantly improving the stability and accuracy of the collaborative control of multiple aerial manipulators during low-altitude operations.

[0207] In order to implement the collaborative control method of low-altitude operations of multiple aerial manipulators in the above-mentioned embodiment, the present invention also proposes a collaborative control system for low-altitude operations of multiple aerial manipulators.

[0208] like Fig.15 As shown, the collaborative control system for low-altitude operation of multiple aerial manipulators according to an embodiment of the present invention includes: a position control module 100, a tuning module 200, a platform grasping module 300 and a posture control module 400, wherein the position control module 100 is used to establish a dynamic model of the aerial manipulators under multiple free flight modes, and design a sliding mode controller according to the dynamic model of the multiple aerial manipulators to realize the posture control of the multiple aerial manipulators; the tuning module 200 is used to tune the parameters of the sliding mode controllers of the multiple aerial manipulators through the improved crown porcupine algorithm; the platform grasping module 300 is used to establish a contact and collision dynamic model of the multiple aerial manipulators, design an operating space compliance controller of the aerial manipulators according to the multiple contact and collision dynamic models, and realize the grasping of the platform according to the operating space compliance controller and the adjusted sliding mode controller; the posture control module 400 is used to establish a rigid-flexible coupling dynamic model, design a fault-tolerant controller according to the rigid-flexible coupling dynamic model, and realize the posture control of the grasping platform according to the fault-tolerant controller, the operating space compliance controller and the adjusted sliding mode controller.

[0209] In one embodiment of the present invention, a single aerial manipulator may include a flight platform, a two-degree-of-freedom rope-driven manipulator, a drive motor, a flight control system and other devices, thereby, the aerial manipulator can be divided into a flight platform unit and a manipulator unit, and the position control module 100 can be used to first establish a dynamic model of the aerial manipulator in a free flight mode, including establishing a flight platform dynamic model for the flight platform unit and a manipulator dynamic model for the manipulator unit, wherein the gravity and joint torque of the manipulator can be included in the flight platform dynamic model as interference force and interference torque.

[0210] Specifically, the Lagrangian function can be introduced to establish the dynamic model of the flight platform, which can be expressed as:

[0211]

[0212] in, is the Lagrangian function, χ=[x,y,z,φ,θ,ψ] T is the transpose of the generalized coordinate matrix of the flight platform, Transpose of the generalized coordinate velocity matrix of the flight platform, ε = [x, y, z] T is the transpose of the position matrix of the flight platform in the world coordinate system, The transpose of the position velocity matrix of the flight platform in the world coordinate system, φ, θ, ψ are the Euler angles of the flight platform in the world coordinate system, ν is the transpose of the Euler angular velocity matrix of the flight platform in the world coordinate system, I is the moment of inertia matrix of the flight platform, m is the mass of the flight platform, and g is the acceleration due to gravity.

[0213] According to the flight platform dynamics model, the rigid body dynamics model of the flight platform unit can be derived, which can be expressed as:

[0214]

[0215] Among them, F S =RU F , F S is the total lift U F The force components on the three axes of the world coordinate system, τ includes the pitch moment τ φ , rolling moment τ θ and yaw moment τ ψ The set of τ=[τ φ ,τ θ ,τ ψ ] T , R is the transformation matrix from the body coordinate system to the inertial coordinate system, which can be expressed as:

[0216]

[0217] Among them, F Si is the thrust generated by the i-th rotor, M Si is the torque generated by the i-th rotor, i=1,2,3,4, and h is the semi-wheelbase.

[0218] Furthermore, the relationship between the i-th rotor thrust, torque and rotor speed can be expressed as:

[0219]

[0220] Among them, P r is the blade radius, A is the blade area, ρ is the local air density, Ω is the rotor speed, C T is the lift coefficient, C Q is the counter torque coefficient.

[0221] In general, the flight platform is basically in a hovering state during operation, and the aircraft is basically motionless or moves slightly. At this time, the changes in the pitch angle and roll angle are very small, and it can be considered that φ=θ≈0, so sinφ≈φ, cosφ≈1, sinθ≈θ, cosθ≈1. At the same time, the yaw angle usually does not need to be frequently controlled and can be considered In addition, the mechanical structure of the flight platform can be considered to be symmetrical. Therefore, the rigid body dynamics model of the flight platform can be simplified to obtain the dynamics model of the flight platform under hovering conditions, which can be expressed as:

[0222]

[0223] in, and is the angular velocity, and is the acceleration of the aerial manipulator position.

[0224] Furthermore, the designed two-degree-of-freedom rope-driven manipulator installs two sets of DC reduction motors on the lower hanging plate of the flight platform pod, and realizes the remote transmission of torque through flexible rope traction, which reduces the joint inertia ratio. Therefore, the Newton-Euler method can be introduced to derive the dynamic model of the manipulator, which can be expressed as:

[0225]

[0226] Among them, M J (q) is the inertia matrix, is the Coriolis force and centrifugal force matrix, G J (q) is the gravity torque of the robot arm, q is the joint angle vector, is the joint angular velocity vector, is the joint angular acceleration vector, τ e is the external torque on the robot arm, τ=[τ1,τ2] T is the transpose of the two joint torque matrices.

[0227] It should be noted that the embodiment of the present invention does not take into account the mass of the connecting rod itself, and concentrates the mass of the robot arm at the joint, thereby meeting the design requirements of the robot arm being lightweight and slender.

[0228] When the aerial manipulator performs a grasping task, the external torque τ e It not only includes the contact torque component generated by the external force on the end of the aerial manipulator, but also includes the interference torque component generated by the environment on the connecting rod, which can be expressed as:

[0229] τ e =τ c +τ d

[0230] Among them, τ c is the contact torque, τ d is the interference torque.

[0231] Subsequently, the gravity and joint torque of the manipulator can be taken into account as interference force and interference torque in the dynamic model of the flight platform, and the expression of the dynamic model of the aerial manipulator can be obtained as follows:

[0232]

[0233] Among them, m1 is the total mass of the robot arm, τ1 and τ2 are the torques of the two joints, and the motion equations of the first three positions can be expressed as:

[0234]

[0235] Among them, u 1x 、u 1y and u 1z is a sliding mode controller.

[0236] In one embodiment of the present invention, Figure 2 As shown, the position of the aerial manipulator can be controlled by referencing the sliding mode function in the dynamic model. For the motion equations of the first three positions in the dynamic model of the aerial manipulator, the sliding mode function can be expressed as:

[0237]

[0238] Among them, s1, s2, s3 are sliding mode variables, c1, c2 and c3 are sliding surface coefficients, e x =xx d 、e y =yy d and e z =zz d is the tracking error of the three positions of the aerial manipulator, and is the tracking error derivative of the three positions of the aerial manipulator, x d ,y d and z d are the desired positions of the three positions of the aerial manipulator.

[0239] Among them, when s1, s2, and s3 are all equal to 0, it can be said that the error between the state of the aerial robot arm and the expected state and the rate of change thereof have been controlled within a preset range.

[0240] Deriving s1, s2, and s3 respectively, and substituting the motion equations of the first three positions in the aerial manipulator dynamics model into the sliding mode function, we can obtain:

[0241]

[0242] in, and is the acceleration of the three desired positions of the aerial manipulator.

[0243] Can be designed separately 1x 、u 1y and u 1z The sliding mode controller is:

[0244]

[0245] Among them, k1, k2 and k3 are the exponential approach rates of the three position sliding mode controllers, and k1, k2 and k3 are all greater than zero, η1, η2 and η3 are the constant speed approach rates of the three position sliding mode controllers, and η1, η2 and η3 are all greater than a user-defined constant, and sgn is a symbolic formula.

[0246] For the motion equations of the last three positions in the aerial manipulator dynamics model, the sliding mode function can be expressed as:

[0247]

[0248] Among them, c4, c5 and c6 are sliding surface coefficients, e φ =φ-φ d 、e θ =θ-θ d and e ψ =ψ-ψ d are the three angle tracking errors of the aerial manipulator, and is the tracking error derivative of the desired angle of the three axes of the aerial manipulator, φ d ,θ d and ψ d is the expected three-axis angle of the aerial manipulator in the world coordinate system.

[0249] The desired posture can be set manually and kept as a small value, and the actual posture is obtained through the sensors of the aerial manipulator itself.

[0250] Deriving s4, s5 and s6 respectively, and substituting the motion equations of the last three positions in the aerial manipulator dynamics model into the sliding mode function, we can obtain:

[0251]

[0252] in, and is the expected angular acceleration of the three axes of the aerial manipulator in the world coordinate system.

[0253] Can design τ separately φ , τ θ and τ ψ The sliding mode controller is:

[0254]

[0255] Among them, k4, k5 and k6 are the exponential approach rates of the three attitude sliding mode controllers, and k4, k5 and k6 are all greater than zero, η4, η5 and η6 are the constant speed approach rates of the three attitude sliding mode controllers, and η4, η5 and η6 are all greater than a custom constant.

[0256] In one embodiment of the present invention, the sliding mode controller of a single UAV has 9 parameters to be optimized, including 3 sliding surface coefficients, 3 exponential approach rates and 3 constant velocity approach rates. These parameters generally require manual adjustment or empirical selection, which is not only time-consuming but may also lead to suboptimal control effects. The introduction of the improved crested porcupine algorithm can not only automatically optimize this process, making the selection of UAV sliding mode controller parameters more systematic and scientific, but also reduce the workload of manual adjustment and improve efficiency, so that the optimal controller parameters can be obtained when multiple aerial manipulators operate at low altitudes.

[0257] In a specific embodiment of the present invention, the tuning module 200 can expand the optimization problem of a single aerial manipulator to six aerial manipulators, that is, apply the same constraints to each aerial manipulator, and use the total cost of the six aerial manipulators as the objective function. The total cost is determined by the tracking error cost E of each aerial manipulator. i , System control quantity cost U i The objective function of the optimal control can be expressed as:

[0258]

[0259] Among them, a and b are the weights of tracking error and control quantity, E i is the tracking error cost of the i-th aerial manipulator, U i is the control cost of the i-th aerial robot arm, time is the operation time of the aerial robot arm, and f obj is the adaptation value.

[0260] In one embodiment of the present invention, the improved crested porcupine algorithm may have four defense mechanisms: vision, hearing, smell, and physical attack. Among them, the vision and hearing mechanisms reflect the exploration strategy of the improved crested porcupine algorithm, providing more candidate solutions for the three parameters of the sliding mode controller; while the smell and physical attack mechanisms reflect the convergence strategy of the improved crested porcupine algorithm, thereby improving the optimization accuracy of the three parameters of the sliding mode controller.

[0261] Specifically, similar to other metaheuristic group-based algorithms, the improved crested porcupine algorithm starts searching from an initial set of individuals, and the search process can be expressed as:

[0262] X i =L+r×(UL)|i=1,2...,N

[0263] Where N is the population size, X i is the i-th solution in the search space, L is the lower limit of the search range, U is the upper limit of the search range, and r is a random number between 0 and 1.

[0264] In the improved crested porcupine algorithm, not all crested porcupine individuals will activate the defense mechanism, only those that are threatened will activate it. Therefore, a cyclic population reduction strategy can be proposed based on this phenomenon, which can not only speed up the convergence speed, but also retain the population diversity. Among them, the cyclic population reduction strategy selects some crested porcupine individuals from the population during the optimization process to speed up the convergence speed, and reintroduces them afterwards to enhance diversity and avoid falling into local minima. The cycle is based on a loop variable to determine the number of times the operation is performed during the optimization process. The mathematical model of the gradual reduction of the population size can be expressed as:

[0265]

[0266] Among them, N′ is the initial population size, T is the variable that determines the number of cycles, t is the current number of iterations, and T max is the maximum number of function evaluations, % is the remainder, N min is the minimum number of individuals in the newly generated population.

[0267] In the visual defense stage, when a crested porcupine individual senses a predator, it will raise and fan its quills to attract the predator's attention. At this point, the predator has two choices: approach or move away from the crested porcupine individual. In the case of approaching, the distance between the predator and the crested porcupine individual is shortened, which promotes the exploration of the area between the predator and the crested porcupine individual and accelerates the convergence rate; in the case of moving away, the distance increases, encouraging exploration of farther areas to discover solutions that may not have been visited. Among them, the normal distribution can be used to generate random values ​​to simulate the above two situations. When the random value is between -1 and 1, it is encouraged to approach the crested porcupine individual, otherwise the predator will move away from the crested porcupine individual. Therefore, a simulation formula can be mathematically proposed based on the behavior of the predator, which can be expressed as:

[0268]

[0269] in, is the best solution of the tth iteration, is the i-th solution of the t-th iteration, R a1 is a random number based on normal distribution, R a2 is a random value in the interval [0,1], is a vector generated between the current CP and a randomly selected CP from the population, used to represent the position of the predator at the tth iteration, which can be expressed as:

[0270]

[0271] Where r is a random number between [1, N].

[0272] During the sound defense stage, crested porcupine individuals use sound methods to make noise and threaten predators. When the predator approaches the porcupine, the porcupine's voice will become louder. Therefore, a simulation formula can be proposed based on this behavior, which can be expressed as:

[0273]

[0274] Among them, r1 and r2 are random integers between [1, N], R a3 is a random number generated between [0, 1], and U1 is a randomly generated binary vector containing 0 and 1.

[0275] Among them, there are three possible situations that may occur in U1's strategy during the simulated sound defense phase:

[0276] (1) When U1 is equal to 0, the crested porcupine individual will stop diffusing the scent because the predator will stop moving due to fear of the crested porcupine individual, so the distance between the predator and the crested porcupine individual remains unchanged;

[0277] (2) When U1 is equal to 1, the crested porcupine will emit a significant scent due to the presence of predators nearby;

[0278] (3) When U1 is a combination of 0 and 1, the predator keeps a safe distance from the crested porcupine individual, so there is no need to release its scent widely.

[0279] In the odor defense stage, CP will secrete a foul odor and spread it in the surrounding area to prevent predators from approaching it. Therefore, a simulation formula can be proposed based on this behavior, which can be expressed as:

[0280]

[0281] Among them, r3 is a random number between [1, N], δ is a parameter used to control the search direction, is the position of the i-th crested porcupine individual at the t-th iteration, γ t As a defense factor, is the odor diffusion factor, which can be expressed as:

[0282]

[0283] in, is the objective function value of the tth individual at the tth iteration, ε is the minimum value to avoid division by zero, rand is a vector of randomly generated values ​​between [0,1], t is the number of current iterations, t max is the maximum number of iterations.

[0284] In the physical attack defense phase, when the predator is very close to it and attacks it with short and thick feathers, the crested porcupine individual will take physical attack. During the physical attack, the two objects merge strongly, which can represent a one-dimensional inelastic collision. Therefore, a simulation formula can be proposed based on this behavior, which can be expressed as:

[0285]

[0286] in, To obtain the best solution, α CPO is the convergence speed factor, R a4 and R a5 is a random value between [0,1], is the average force of the crested porcupine individual that affects the i-th predator, which can be expressed as:

[0287]

[0288] Among them, m i is the mass of the i-th predator individual at iteration t, is the final velocity of the i-th predator individual at the next iteration t+1, is the initial velocity of the i-th predator at iteration t, R a6 is a vector of random values ​​between [0,1].

[0289] In one embodiment of the present invention, the aerial manipulator will inevitably come into contact and collide with the target object during the process of grasping the target object. The impact caused by the contact process will not only cause a certain degree of disturbance to the end and the target object, but also cause disturbance to the base due to the unique dynamic coupling characteristics of the aerial manipulator, which may affect the stability of the aerial manipulator. Therefore, a corresponding contact and collision dynamics model can be established during the process of contact and collision between the aerial manipulator and the target object to characterize the relationship between the contact and collision force and the movement of the aerial manipulator.

[0290] Specifically, if Fig. 9 As shown in the figure, the nonlinear damping model can be used to describe the process of contact and collision between the aerial manipulator and the target object, which can avoid the situation where the contact force at the initial position and the end position of the collision is not zero. In addition, since the damping term is considered in this model, the energy loss can be avoided from being ignored. Therefore, the contact collision dynamics model can be expressed as:

[0291]

[0292] Among them, F t is the contact collision force, k is the Hertzian stiffness, D = λδ n, n is the collision coefficient, λ is the damping coefficient, δ is the relative compression of the two monomers along the normal direction of the contact surface, is the relative compression speed.

[0293] The contact force collision force vector of the aerial robot arm is:

[0294] F e =NF t

[0295] Where N is the normal vector at the contact collision point.

[0296] According to the kinematic analysis of the aerial manipulator, the end motion and joint motion of the aerial manipulator can satisfy the following relationship:

[0297]

[0298] in, is the terminal posture velocity vector, J is the Jacobian matrix, is the joint velocity of the aerial manipulator.

[0299] When the redundancy of the aerial manipulator is not considered, the mapping relationship between the joint acceleration and the manipulator end acceleration can be obtained by taking the derivative of both sides of the relationship between the manipulator end motion and the joint motion:

[0300]

[0301] in, is the joint acceleration, is the acceleration of the end position of the robot arm, J -1 is the inverse of the Jacobian matrix, is the derivative of the Jacobian matrix.

[0302] The relationship between the joint torque τ and the terminal generalized driving force F is:

[0303] τ=J T F

[0304] Substitute the relationship between the end motion of the aerial manipulator and the joint motion, the relationship between the joint acceleration and the end acceleration of the manipulator, and the relationship between the joint torque and the end generalized driving force into the manipulator dynamics model, and multiply (J -1 ) T The dynamic equation of the aerial manipulator in the operating space can be obtained as follows:

[0305]

[0306] Among them, M o is the inertia matrix, C o is the coefficient matrix of Coriolis force and centrifugal force, Go is the gravity coefficient matrix, and M o =(J -1 ) T MJ -1 , G o =(J -1 ) T G. F c is the generalized contact force at the end of the robot arm, F d is the generalized disturbance force at the end of the robot arm, and F c =(J -1 ) T τ c , F d =(J -1 ) T τ d , (J -1 ) T is the transpose of the generalized inverse Jacobian matrix.

[0307] According to the generalized contact force F at the end of the robot c The impedance equation of the aerial manipulator operation space can be constructed, and the impedance equation can be expressed as:

[0308]

[0309] Among them, M I is the positive definite matrix of inertia of the expected control of the equivalent model, B I is the positive definite matrix of the damping of the expected control of the equivalent model, K I are the positive definite matrices of the stiffness of the expected control of the equivalent model, x Jd is the desired displacement of the end of the robot arm, is the desired velocity of the end of the robot arm, is the desired acceleration at the end of the robot arm, x J is the displacement of the end of the robot arm, is the speed of the end of the robot arm, is the acceleration of the end of the robot arm.

[0310] The acceleration of the end of the robot arm The acceleration input at the end of the robot arm is recorded as It can be expressed as:

[0311]

[0312] In order to achieve tracking control of the contact force, the acceleration input can be Modification is made to introduce the difference F between the actual contact force and the expected contact force e -F d and the force control gain K f To replace Fc Therefore, the input acceleration of the end of the robot arm is can be re-expressed as:

[0313]

[0314] Therefore, the joint control torque of the aerial manipulator can be obtained through the above formula:

[0315]

[0316] In one embodiment of the present invention, when the multiple aerial manipulators are in a collaborative transport mode, the multiple aerial manipulators and the target object together form an equivalent parallel mechanism, and the base of the equivalent parallel mechanism is fixed.

[0317] In one embodiment of the present invention, the posture control module 400 can analyze the internal coupling characteristics of the system and describe the inertial force, Coriolis force, centripetal force, elastic force and load platform generated by each leg during the movement as a transmission relationship between force and motion.

[0318] Calculate the virtual power of the active force and inertia force acting on the entire equivalent parallel mechanism. According to the virtual power principle, we can get:

[0319]

[0320] Among them, δ is the change of imaginary quantity, p is the displacement of the load platform, and f a , l, θ u , u ,θ s , s and γ s is a vector of corresponding scalars in each leg, for example, f a =(f a1 … f a6 ) T , l=(l1 … l6) T , k is the stiffness of each leg, Δl is the instantaneous slight change in the length of the six-leg ball joint and the universal joint connection direction, l i is the length of the line connecting the ball joint and the universal joint of the i-th leg, c is the damping value of each leg, k is the rate of change of the length of the six-leg ball joint and the universal joint in the direction of the connection line with time, θu The flexible universal joint is ui Shaft stiffness, k ψu For the flexible universal joint around x ui Shaft stiffness, k θs The flexible universal joint is si Shaft stiffness, k ψs For the flexible universal joint around x siShaft stiffness, k γs The flexible universal joint is si Axis stiffness, m p is the mass of the load platform, I p is the inertia of the load platform, is the displacement velocity of the load platform, is the displacement acceleration of the load platform, ω p is the rotation speed of the center of mass of the load platform, is the rotational acceleration of the load platform center of mass, ω d is the angular velocity of the connecting rod in the leg, is the angular velocity of the connecting rod in the leg in the {Bi} coordinate system, m d is the mass of the connecting rod in the leg, is the velocity of the connecting rod center of mass in {Bi}, is the angular velocity of the connecting rod center of mass in {Bi}, is the moment of inertia of the connecting rod in {Bi}, and {Bi} is the coordinate system of the i-th flexible universal joint.

[0321] Since the working space of the equivalent parallel mechanism of the embodiment of the present invention is very small, the joint variables have sufficient accuracy when linearized at the equilibrium position. Therefore, the Coriolis force and the centripetal force can be ignored, so that the virtual power formula of the load platform displacement can be simplified to:

[0322]

[0323] in, equal is the transpose of the displacement velocity and rotation angular velocity matrix of the load platform, J lx is the Jacobian matrix of the load platform, J θux and J ψux is the Jacobian matrix of the universal joint, J θsx , J ψsx and J ψsx is the Jacobian matrix of the spherical joint, is the Jacobian matrix for each link.

[0324] because The components of are independent of each other, which means that all coefficients are zero, thus the rigid-flexible coupling dynamic model can be obtained, and its expression can be:

[0325]

[0326] Where M is the mass matrix, C is the damping matrix generated by the actuator damping and stiffness, K is the stiffness matrix generated by the actuator damping and stiffness, and K e is the stiffness matrix caused by the flexure hinge stiffness, is the transposed matrix of the Jacobian matrix of the load platform, is the displacement acceleration and rotational angular acceleration matrix of the load platform, is the displacement velocity and rotation angular velocity matrix of the load platform, f a is a vector of scalars corresponding to each leg, x p is the displacement and rotation angle matrix of the load platform.

[0327] In actual work, the aerial manipulator may have certain faults during operation, such as mechanical faults and manipulator arm faults. Considering that the manipulator may have gain faults and bias faults, the manipulator arm fault model can be expressed as:

[0328]

[0329] Among them, f a p is the actual output force, ρ is the force failure ratio matrix, and ρ i ∈(0,1), f is the bias force function, I is the identity matrix, and diag is the diagonal matrix.

[0330] In this regard, the embodiment of the present invention designs a fault-tolerant controller for the rigid-flexible coupling dynamics model, which is used to achieve stable control of the position when a flight platform fails. Fig.11 shown.

[0331] Specifically, the state variables x1 and x2 can be preset, and x1 = x P , From this we can get:

[0332]

[0333] Considering the uncertainty of the rigid-flexible coupling dynamic model, the nominal mass matrix M and the nominal damping matrix C can be expressed as follows using nominal values ​​and deviations: M = M n +ΔM,C=C n +ΔC, where M n is the nominal mass matrix of the model, C n is the nominal damping matrix of the model, thus, the rigid-flexible coupling dynamic model can be rewritten as:

[0334]

[0335] Furthermore, during the operation of the aerial manipulator, the acceleration of the carrier platform is unknown and its upper limit is difficult to determine. Therefore, a fixed-time disturbance observer can be introduced to estimate the uncertainty set of the state variables in real time and compensate for the uncertainty in the controller to ensure that the fault-tolerant controller has higher control accuracy and stronger anti-interference ability.

[0336] Among them, the fixed-time disturbance observer can be expressed as:

[0337]

[0338] in, is the estimated value of x2, is the estimated value of the uncertainty set D, K1 and K2 are gains, ζ∈(0,1) is the amplification factor, is the error between the estimated value of x2 and the true value of x2, v1 and v2 are power parameters, sig is the power sign function, is the error between the estimated value of D and the true value of D, where the power value satisfies v1∈(0.5,1.0), v2∈(1.0,1.5).

[0339] Therefore, the error can be defined based on the backstepping principle as:

[0340]

[0341] Among them, z1 and z2 are state variables under error constraints, x Pd is the expected position of the platform, and α is the virtual control variable.

[0342] Furthermore, the maximum pose tracking error value can be set to K a , and |z 1,i |≤K a (i=1,2,…,6), so that the tracking between the aerial manipulator and the desired posture is more accurate.

[0343] Specifically, the Lyapunov function can be designed in the form of V1, which can be expressed as:

[0344]

[0345] Then, the virtual control amount α is set, which can be expressed as:

[0346]

[0347] Among them, λ1 is the parameter to be designed.

[0348] Taking the derivative of z2, we can get:

[0349]

[0350] Taking the derivative of V1, we can get:

[0351]

[0352] Secondly, α and Substitute into In, we can get:

[0353]

[0354] Furthermore, in order to further improve the position tracking and speed control accuracy of the parallel platform, a maximum error limit K is introduced into the speed tracking error. b And|z 2,i |≤K b (i=1,2,…,6).

[0355] Specifically, first, the Lyapunov function can be designed in the form of V2:

[0356]

[0357] Then, taking the derivative of V2, we get:

[0358]

[0359] By taking the derivative of the virtual control amount α with respect to time, we can obtain:

[0360]

[0361] in, is the displacement acceleration and rotation angular acceleration matrix of the load platform.

[0362] Thus, the backstepping fault-tolerant controller can be obtained, which can be expressed as:

[0363]

[0364] Among them, Ξ and Γ are diagonal matrices, and λ1, λ2 and γ are parameters to be designed.

[0365] In summary, the dynamic models of multiple aerial manipulators in free flight modes are established through the position control module, and the sliding mode controller is designed according to the dynamic model of the aerial manipulator to realize the posture control of multiple aerial manipulators. The parameters of the sliding mode controllers of multiple aerial manipulators are tuned by the improved crown porcupine algorithm of the tuning module. The contact collision dynamics model and rigid-flexible coupling dynamics model of multiple aerial manipulators are established through the platform grasping module and the posture control module, and the operating space compliant controller and fault-tolerant controller are designed according to the contact collision dynamics model to realize the posture control of the grasping platform. Therefore, the stability and accuracy of the collaborative control of multiple aerial manipulators during low-altitude operations can be significantly improved.

[0366] In the description of the present invention, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. "Multiple" means two or more, unless otherwise clearly and specifically defined.

[0367] In the present invention, unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", "fixed" and the like should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0368] In the present invention, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may mean that the first and second features are in direct contact, or the first and second features are in indirect contact through an intermediate medium. Moreover, a first feature being "above", "above" or "above" a second feature may mean that the first feature is directly above or obliquely above the second feature, or simply means that the first feature is higher in level than the second feature. A first feature being "below", "below" or "below" a second feature may mean that the first feature is directly below or obliquely below the second feature, or simply means that the first feature is lower in level than the second feature.

[0369] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, unless they are contradictory.

[0370] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code that includes one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present invention includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present invention belong.

[0371] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor, or other system that can fetch instructions from an instruction execution system, device or apparatus and execute instructions), or in combination with these instruction execution systems, devices or apparatuses. For the purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus, or in combination with these instruction execution systems, devices or apparatuses. More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection with one or more wires (electronic devices), a portable computer disk box (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), a fiber optic device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing in other suitable ways if necessary, and then stored in a computer memory.

[0372] It should be understood that the various parts of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, a plurality of steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0373] A person skilled in the art may understand that all or part of the steps in the method for implementing the above-mentioned embodiment may be completed by instructing related hardware through a program, and the program may be stored in a computer-readable storage medium, which, when executed, includes one or a combination of the steps of the method embodiment.

[0374] In addition, each functional unit in each embodiment of the present invention may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0375] Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may change, modify, replace and vary the above embodiments within the scope of the present invention.

Claims

1. A collaborative control method for low-altitude operations of multiple aerial manipulators, characterized in that: The following steps are involved: Establishing a plurality of dynamic models of the aerial manipulators in free flight modes, and designing a sliding mode controller according to the dynamic models of the plurality of aerial manipulators to achieve position and posture control of the plurality of aerial manipulators; The parameters of the sliding mode controllers of the plurality of aerial manipulators are adjusted by using an improved crested porcupine algorithm; Establishing multiple contact and collision dynamic models of the aerial manipulator, designing an operation space compliance controller of the aerial manipulator according to the multiple contact and collision dynamic models, and realizing the grasping of the platform according to the operation space compliance controller and the adjusted sliding mode controller; A rigid-flexible coupling dynamics model is established, a fault-tolerant controller is designed according to the rigid-flexible coupling dynamics model, and the position and posture control of the grasping platform is realized according to the fault-tolerant controller, the operating space compliant controller and the adjusted sliding mode controller.

2. The collaborative control method for low-altitude operations of multiple aerial manipulators according to claim 1 is characterized in that: The aerial manipulator comprises a flight platform unit and a manipulator unit, and establishing a dynamic model of the aerial manipulator comprises establishing a flight platform dynamic model for the flight platform unit and establishing a manipulator dynamic model for the manipulator unit.

3. The collaborative control method for low-altitude operations of multiple aerial manipulators according to claim 2 is characterized in that: When the plurality of aerial manipulators are in a collaborative transport mode, the plurality of aerial manipulators and the target object together form an equivalent parallel mechanism, and the base of the equivalent parallel mechanism is fixed.

4. The collaborative control method for low-altitude operations of multiple aerial manipulators according to claim 3 is characterized in that: The expression of the contact collision dynamics model is: Among them, F t is the contact collision force, k is the Hertzian stiffness, D = λδ n , n is the collision coefficient, λ is the damping coefficient, δ is the relative compression of the two monomers along the normal direction of the contact surface, is the relative compression speed.

5. The collaborative control method for low-altitude operation of multiple aerial manipulators according to claim 4 is characterized in that: The expression of the rigid-flexible coupling dynamic model is: Where M is the mass matrix, C is the damping matrix generated by the actuator damping and stiffness, K is the stiffness matrix generated by the actuator damping and stiffness, and K e is the stiffness matrix caused by the flexure hinge stiffness, is the transposed matrix of the Jacobian matrix of the load platform, is the displacement acceleration and rotational angular acceleration matrix of the load platform, is the displacement velocity and rotation angular velocity matrix of the load platform, f a is a vector of scalars corresponding to each leg, x p is the displacement and rotation angle matrix of the load platform.

6. A collaborative control system for low-altitude operations of multiple aerial manipulators, characterized in that: include: A position control module, the position control module is used to establish a dynamic model of the aerial manipulator under multiple free flight modes, and design a sliding mode controller according to the dynamic models of the multiple aerial manipulators to achieve position control of the multiple aerial manipulators; A tuning module, the tuning module is used to tune the parameters of the sliding mode controllers of the plurality of aerial manipulators by using an improved crown porcupine algorithm; A platform grasping module, the platform grasping module is used to establish a plurality of contact collision dynamic models of the aerial manipulators, design an operation space compliance controller of the aerial manipulators according to the plurality of contact collision dynamic models, and realize grasping of the platform according to the operation space compliance controller and the adjusted sliding mode controller; The posture control module is used to establish a rigid-flexible coupling dynamics model, design a fault-tolerant controller based on the rigid-flexible coupling dynamics model, and realize the posture control of the grasping platform based on the fault-tolerant controller, the operating space compliance controller and the adjusted sliding mode controller.

7. The collaborative control system for low-altitude operations of multiple aerial manipulators according to claim 6 is characterized in that: The aerial manipulator comprises a flight platform unit and a manipulator unit, and establishing a dynamic model of the aerial manipulator comprises establishing a flight platform dynamic model for the flight platform unit and establishing a manipulator dynamic model for the manipulator unit.

8. The collaborative control system for low-altitude operations of multiple aerial manipulators according to claim 7 is characterized in that: When the plurality of aerial manipulators are in a collaborative transport mode, the plurality of aerial manipulators and the target object together form an equivalent parallel mechanism, and the base of the equivalent parallel mechanism is fixed.

9. The collaborative control system for low-altitude operations of multiple aerial manipulators according to claim 8 is characterized in that: The expression of the contact collision dynamics model is: Among them, F t is the contact collision force, k is the Hertzian stiffness, D = λδ n , n is the collision coefficient, λ is the damping coefficient, δ is the relative compression of the two monomers along the normal direction of the contact surface, is the relative compression speed.

10. The collaborative control system for low-altitude operations of multiple aerial manipulators according to claim 9, characterized in that: The expression of the rigid-flexible coupling dynamic model is: Where M is the mass matrix, C is the damping matrix generated by the actuator damping and stiffness, K is the stiffness matrix generated by the actuator damping and stiffness, and K e is the stiffness matrix caused by the flexure hinge stiffness, is the transposed matrix of the Jacobian matrix of the load platform, is the displacement acceleration and rotational angular acceleration matrix of the load platform, is the displacement velocity and rotation angular velocity matrix of the load platform, f a is a vector of scalars corresponding to each leg, x p is the displacement and rotation angle matrix of the load platform.

Images