Pigeon swarm hierarchy optimization-based parameter tuning method for active disturbance rejection control of unmanned aerial vehicle with arms

Abstract

This invention discloses a method for tuning active disturbance rejection control (ADRC) parameters for a quadcopter with an arm based on pigeon flock hierarchical optimization. The implementation steps are as follows: Step 1, establishing a model of the quadcopter with an arm; Step 2, designing the quadcopter with an arm and selecting its fitness function for ADRC; Step 3, using a pre-search-based pigeon flock hierarchical optimization method; Step 4, optimizing the ADRC parameters at the pigeon flock hierarchical level; Step 5, verifying the optimized ADRC parameters through simulation; Step 6, outputting the optimal ADRC parameters. This method aims to solve the difficulties in controller design and parameter tuning for quadcopter with an arm, obtaining the optimal ADRC controller parameters through parameter optimization, thus laying the foundation for the application of quadcopter with an arm.

Description

Technical Field

[0001] This invention is a method for tuning the active disturbance rejection control parameters of an arm-mounted unmanned aerial vehicle (UAV) based on pigeon flock hierarchy optimization, belonging to the field of UAV autonomous control technology. Background Technology

[0002] Quadrotor drones, due to their vertical takeoff and landing, hovering capability, and ease of operation, are widely used in aerial missions such as 3D mapping, power line inspection, and environmental reconnaissance. However, due to the inherent structural limitations of quadrotor drones, their current applications are mostly limited to scenarios with little or no interaction with the environment. Robotic arms, on the other hand, offer precise control and strong environmental interaction capabilities. Therefore, integrating robotic arms with drones can enable quadrotor drones to move from simply "seeing" to "doing," allowing for wider applications such as aerial logistics handling and cluster construction. Firstly, modeling a quadrotor with an arm is a complex problem. Currently, there are two main modeling methods: holistic modeling and independent modeling. Holistic modeling treats the robotic arm and quadrotor drone as a single unit, analyzing the impact of the external environment on the entire system. This method yields relatively accurate models, but the strong coupling between the robotic arm and the quadrotor drone makes controller design difficult. The independent modeling approach treats the robotic arm and the quadcopter drone as two separate systems. By analyzing the forces acting on each separately and considering the forces and torques exerted by the robotic arm on the drone as external disturbances to the quadcopter system, this modeling method is relatively simple, and the entire quadcopter system can be controlled by controlling the drone and the robotic arm separately. Therefore, the accuracy of the model and the convenience of controller design need to be considered comprehensively when modeling quadcopters with robotic arms. Secondly, the strong coupling between the robotic arm and the quadcopter drone increases the difficulty of quadcopter controller design. Currently, the mainstream control methods include traditional proportional-integral-differential (PID) control, backstepping (BS) control, and active disturbance rejection control (ADRC). While traditional PID control is relatively simple in principle, its large initial error makes it prone to overshoot, and it often fails to perform well in systems with strong disturbances and strong coupling. Backstepping control is accurate for some simpler systems, but the design process becomes more cumbersome as the model order increases. Active disturbance rejection control (ADRC) uses an extended state observer to estimate and compensate for the total disturbance by arranging a reasonable "transition process" for the input signal, thus giving the system strong anti-interference ability. However, ADRC has many parameters, and the manual tuning process is cumbersome and difficult. Therefore, optimization methods can obtain the optimal control parameters.Currently, the main methods used for optimizing the parameters of active disturbance rejection controllers include Particle Swarm Optimization (PSO) and Pigeon-inspired Optimization (PIO). PSO is inspired by the foraging behavior of birds, where particles randomly search for the optimal solution in the direction of individual optima and global optima, but its convergence process is relatively slow. Pigeon-inspired Optimization is based on a centralized approach, simulating the navigation mechanism of a pigeon flock. All pigeons move towards the globally optimal individual, lacking individual search processes and no hierarchy within the flock, resulting in fast convergence. However, due to the short search process, the search is prone to getting trapped in local optima. Summary of the Invention

[0003] The purpose of this invention is to address the shortcomings of existing technologies by providing a method for tuning active disturbance rejection control parameters of a drone with an arm based on pigeon flock hierarchical optimization. This method aims to solve the difficulties in controller design and parameter tuning for drones with arms, and to obtain optimal active disturbance rejection controller parameters through parameter optimization, thus laying the foundation for the application of drones with arms.

[0004] Technical solution of the present invention

[0005] A method for tuning active disturbance rejection control parameters of a quadrotor UAV based on pigeon flock hierarchical optimization is proposed. First, the quadrotor UAV and the robotic arm are modeled separately, and the torque interference of the robotic arm on the quadrotor UAV is analyzed to establish a model of the quadrotor UAV. Then, the attitude angle controller of the quadrotor UAV is designed using an active disturbance rejection controller. Finally, the parameters of the active disturbance rejection controller are optimized using the pigeon flock hierarchical optimization method to obtain the controller parameters with optimal performance.

[0006] The pigeon flock hierarchy optimization method includes: first, based on the pigeon flock hierarchy mechanism, individuals in the flock are divided into four levels: globally optimal individuals, locally optimal individuals, locally suboptimal individuals, and locally worst individuals. The local hierarchy is determined by comparing each individual with a fixed number of its neighbors. Each individual in the flock updates its optimal position record based on its historical position. Then, a pre-search phase is added to the geomagnetic navigation stage. In the pre-search phase, locally optimal individuals search towards the position of the globally optimal individual and the direction of their own optimal position; locally suboptimal individuals search in the opposite direction of the positions of locally optimal and locally worst individuals; and locally worst individuals search towards the direction of the position of the locally optimal individual. After the pre-search phase, globally optimal individuals only search towards their own optimal position, while individuals of other levels search towards the position of the globally optimal individual and the direction of their own optimal position.

[0007] The present invention provides a method for tuning the active disturbance rejection control parameters of an unmanned aerial vehicle (UAV) with an arm based on pigeon flock hierarchy optimization, comprising:

[0008] Step 1: Build a model of the drone with an arm.

[0009] S11. UAV Modeling: Establish geographic coordinate system and body coordinate system, define the position and attitude angles of the quadrotor UAV, obtain the rotation matrix from the body coordinate system to the geographic coordinate system, and obtain the dynamic model of the quadrotor UAV based on the Newton-Euler equations without considering air resistance, internal friction of the UAV and fuselage stiffness.

[0010] S12. Robotic Arm Interference Modeling: The robotic arm is a three-bar planar arm, mainly composed of three servo rotary joints, joint connectors, and an end effector. The axis distances of adjacent joints are parallel. A coordinate system is established at each joint of the robotic arm to obtain the parameter matrix of the three-bar planar arm. An interference model of the robotic arm is established based on the external interference torque generated by the robotic arm on the UAV.

[0011] S13. Modeling of the UAV with Arm: Substitute the disturbance torque output by the mechanical arm disturbance model into the dynamic model of the quadcopter UAV in S11 to obtain the dynamic model of the UAV with arm.

[0012] Step 2: Design an active disturbance rejection controller for the UAV with an arm and select a fitness function.

[0013] The overall framework of the active disturbance rejection controller for the UAV with arm adopts a cascade control structure. The outer loop is the position loop, which uses a PID controller, and the inner loop is the attitude loop, which uses an ADRC controller. The fitness function is selected to have a small integral value of the time absolute deviation.

[0014] Step 3: Pigeon flock hierarchy optimization method based on pre-search

[0015] S31. Initializing the pigeon flock includes: 1) initializing the pigeon flock position and speed, 2) updating the global optimal fitness and position, 3) updating the pigeon's neighbors and level, and 4) initializing the individual pigeon's optimal position and individual optimal fitness.

[0016] S32, the geomagnetic navigation stage includes: 1) performing a pre-search, 2) performing a global search, 3) updating the fitness of each pigeon, as well as the individual optimal fitness and individual optimal position, 4) after each pigeon has completed steps 1), 2), and 3) in S32, using steps 2) and 3) in S31 to update the global optimal fitness, global optimal position, each pigeon's neighbor set, and hierarchy.

[0017] S33, the landmark navigation stage includes: selecting pigeons with better fitness to identify landmarks, calculating the center position of the remaining pigeons, and during position updates, the globally optimal pigeon does not move towards the center of the flock, while other pigeons move towards the center of the flock. After each pigeon has completed speed and position updates, an ADRC attitude loop control simulation is performed to obtain the fitness of each pigeon, and then the globally optimal fitness and globally optimal position are updated.

[0018] Step 4: Optimize the self-disturbance rejection control parameters at the flock level.

[0019] S41. Initialize the drone and robotic arm interference model using the parameters instantiated in S11 and S12 in step one, and obtain the dynamic model of the drone with arm according to S13. Initialize the pigeon flock using the instantiated parameters in S31 in step three.

[0020] S42. Use S32 from step 3 for geomagnetic navigation. First, run a pre-search, then perform a global search, and record the global optimal fitness during the simulation until the geomagnetic navigation phase is completed.

[0021] S43. Use S33 from step three to perform landmark navigation, run and record the global optimal fitness during the simulation process until the landmark navigation phase is completed.

[0022] S44. Once the landmark navigation phase is completed, the simulation ends, and the final global optimal fitness and global optimal position are recorded.

[0023] Step 5: Simulation verification of the optimized active disturbance rejection control parameters.

[0024] The globally optimal position obtained from the pigeon flock hierarchy optimization in step four is passed to the optimal value in the angle loop active disturbance rejection control parameters of the boom-armed UAV. The desired pitch angle and roll angle of the three axes are input to perform three angle loop test simulations to verify the actual control effect of the optimized parameters.

[0025] Step six: Output the optimal active disturbance rejection control parameters.

[0026] The above scheme further

[0027] Step one, establishing the model of the drone with arm, includes:

[0028] Step 1: Build a model of the drone with an arm

[0029] S11, Drone Modeling

[0030] Establish a geographic coordinate system G and a body coordinate system B, and define the position of the quadcopter UAV as (x, y, z). T The attitude angles are (φ,θ,ψ). TT is the matrix transpose operator, which yields the rotation matrix from the body coordinate system to the geographic coordinate system. for:

[0031]

[0032] In the formula, pitch angle θ, roll angle φ, and yaw angle ψ.

[0033] Neglecting air resistance, internal friction of the UAV, and fuselage stiffness, the dynamic model of the quadcopter UAV, based on the Newton-Euler equations, is as follows:

[0034]

[0035] In the formula, Let be the second derivative of the drone's position. Let τ be the second derivative of the drone's attitude angle, m be the total mass of the quadcopter drone, g be the acceleration due to gravity, f be the total thrust generated by the propellers, and τ be the second derivative of the drone's attitude angle. x ,τ y ,τ z For the triaxial torque generated by the propeller, I x ,I y ,I z For the rotational inertia of the three axes, dis x ,dis y ,dis z External interference for the three axes.

[0036] For a quadcopter with an X-shaped configuration, the total thrust f and triaxial torque τ generated by its propellers x ,τ y ,τ z Represented as:

[0037]

[0038] In the formula, d represents the distance between the center of the machine body and any motor, and c T c is the tensile coefficient. M This is the torque coefficient. These represent the motor speeds.

[0039] S12, Robotic Arm Interference Modeling

[0040] A robotic arm is composed of links connected by joints. A coordinate system is established at each joint, and the transformation between adjacent joint coordinate systems is represented by a homogeneous transformation matrix T. Let the coordinate axes of coordinate system i be... Let a i ,α i ,d i , They represent along Shaft from Move to Distance, around Shaft from Rotate to Angle, along Shaft from Move to Distance, around Shaft from Rotate to If the angle is such that the transformation matrix from coordinate system i-1 to coordinate system i is written as:

[0041]

[0042] According to S12a i ,α i ,d i , By defining the parameters, the parameter matrix of the three-bar planar arm is obtained as follows:

[0043]

[0044] In the formula, L i (i = 1, 2, 3) is the length of the rod, θ i (i = 1, 2, 3) represents the joint angles of the robotic arm.

[0045] Based on the chain rule of coordinate transformation, using the transformation matrix... Obtain the position and attitude transformation matrix of the j-th joint relative to the origin of the coordinate system F between the robotic arm and the UAV fixed point. This yields the position of each joint relative to the origin of the fixed-point coordinate system F. Assuming the origin of the fixed-point coordinate system F coincides with the origin of the body coordinate system B, the position of joint j relative to the origin of the UAV body coordinate system B is obtained. B P j :

[0046]

[0047] In the formula, P0 = [0 0 0 1] T The four-dimensional representation of the origin of the fixed-point coordinate system F for the UAV. Let be the rotation matrix from the fixed point coordinate system F to the geographic coordinate system G.

[0048] Assuming the lever of the robotic arm is a uniform lever, then the center of mass r of the robotic arm system under the mechanical system is... B Represented as

[0049]

[0050] In the formula, m j(j=1,2,3) represents the mass of the j-th rod. When m4 is the mass of the block, M=∑m j The total weight of the entire robotic arm system. B P cj Let be the coordinates of the center of mass of the j-th rod or block in system B, and let the coordinates of the center of mass of the block be consistent with the coordinates of the 3rd joint.

[0051] When the robotic arm is in the gripping state, and the drone is moving, assuming the robotic arm is stationary relative to the drone, the external disturbance torque generated by the robotic arm on the drone is calculated as follows:

[0052]

[0053] In the formula, This refers to the three-axis external disturbance torque generated by the robotic arm on the drone.

[0054] S13, Modeling of a drone with an arm

[0055] The established robotic arm interference model outputs interference torque. Substituting the quadcopter drone's dynamic model into S11, and then replacing the mass m in the model with the sum of the quadcopter drone's mass m and the total mass M of the robotic arm system, we obtain the dynamic model of the drone with the robotic arm.

[0056]

[0057] In the formula, m total The total mass of the quadcopter drone with arms is m total =m+M.

[0058] The robotic arm is a three-bar planar arm, mainly composed of three servo rotary joints, joint connectors and end effector claws, with the axis of adjacent joints being parallel to each other.

[0059] Step two, the design of the active disturbance rejection controller for the UAV with arm and the selection of the fitness function, includes:

[0060] By arranging the transition process through the input signal of the tracking differentiator, a smoother input signal and its derivative signal are obtained, achieving tracking of the input signal with virtually no overshoot. The second-order discrete tracking differentiator is as follows:

[0061]

[0062] In the formula, v(k), v1(k), and v2(k) are the input signal, the tracking signal of the input signal, and the differential signal of the input signal, respectively. T is the sampling period, r is the velocity factor, h is the filter factor, and fhan(·) is the optimal control function, as follows:

[0063]

[0064] The total disturbance of the system is observed by extending a new observation variable through an extended state observer. The total disturbance includes internal and external disturbances, and its third-order discrete form is as follows:

[0065]

[0066] In the formula, z1(k), z2(k), and z3(k) are the observations of the input signal, the derivative of the input signal, and the total system disturbance at time k, respectively; z1(k+1), z2(k+1), and z3(k+1) are the observations of the input signal, the derivative of the input signal, and the total system disturbance at time k+1, respectively; T is the sampling period; β1, β2, and β3 are the feedback gains of the state error feedback; b is the system compensation factor; and u is the system control quantity. The fal(·) function is a nonlinear function, where a1, a2 ∈ a are the nonlinearity coefficients, δ is the width of the linear interval, and e is the error, expressed as follows:

[0067]

[0068] In the formula, |·| represents the absolute value operation.

[0069] The nonlinear error feedback control law obtains the pre-compensation control quantity of the system by nonlinearly combining the state feedback error, and then compensates for the total system disturbance obtained by the extended state observer to obtain the final compensated control quantity, as shown in the following form:

[0070]

[0071] In the formula, e1 is the original signal error, e2 is the original signal differential error, k1 and k2 are both control coefficients. The larger k1 is, the faster the system response. k2 increases the system damping. fal(·) function is a nonlinear function. δ0 is the width of the linear interval of the nonlinear error feedback control law. u0 is the control output before compensation, u is the control output after compensation, and b is the compensation coefficient.

[0072] The overall controller framework adopts a cascade control structure, with the outer loop being the position loop using a PID controller and the inner loop being the attitude loop using an ADRC controller. The control system first inputs the desired position p. d The PID position subsystem outputs the desired roll angle φ. d Desired pitch angle θ d and expected tension f d The ADRC attitude subsystem is based on the desired heading angle ψ d Expected roll angle φ d and desired pitch angle θ d Calculate the desired torque τd The control distributor is based on the desired tension f d and desired torque τ d Calculate the desired motor speed The motor model obtains the actual motor speed based on the desired speed. In addition to the total thrust f and total torque τ that the propeller can actually provide, the robotic arm calculates the torque disturbance M based on the states of the robotic arm and the drone. dis Finally, the quadrotor model with arms is used to update the quadrotor's position p, velocity v, attitude angle Θ, and attitude angular velocity ω. The key parameters that the active disturbance rejection controller needs to optimize are ω0, k1, and k2.

[0073] The fitness function of the optimization method should be chosen such that a smaller integral value of the absolute time deviation indicates better control performance. Let the desired roll angle be φ. d The actual roll angle φ and the simulation time T s Then the fitness of each set of ADRC control parameters is:

[0074]

[0075] In step two, the design of the active disturbance rejection controller (ADRC) for the quadcopter and the selection of its fitness function, considering the structural symmetry of the quadcopter, the ADRC has a total of 14 parameters, including r, h, a1, a2, δ, α1, α2, δ0, b, β1, β2, β3, k1, k2. The values ​​of r, h, a1, a2, δ, α1, α2, and δ0 are determined based on engineering experience. The value of b is related to the model of the quadcopter. β1, β2, and β3 are also determined by the observer's bandwidth ω0, i.e., β1 = 3ω0, β2 = 3ω0. 2 β3=ω0 3 Therefore, the key parameters that the active disturbance rejection controller needs to optimize are ω0, k1, and k2.

[0076] The pigeon flock hierarchy optimization method based on pre-search in step three includes:

[0077] S31. Initialize the pigeon flock

[0078] 1) Initialize the pigeon flock's position and speed

[0079] Let the total number of pigeons be N. p Then the initial position and speed of the flock of pigeons are:

[0080]

[0081] In the formula, X i,d and V i,d Let Δbound be the position and velocity of pigeon i in the d-th dimension. d and V represents the width and center coordinates of the d-th dimension, respectively. max The maximum speed of the pigeon (V) max >0), rands is a random number in the interval [-1,1].

[0082] 2) Global optimal fitness and position update

[0083]

[0084] In the formula, {f gbest ,X gbest} represents the fitness and position of the globally optimal individual.

[0085] 3) Update pigeon i's neighbors and level.

[0086] Let the fitness of pigeon i be denoted by fitness. i dist i,j Let dist be the distance between pigeons i and j, and let dist be the distance between them. i,j =0 (i=j), expected number of fixed neighbors N num Then the neighbor set N of pigeon i i for

[0087] N i =Index min k(dist,k),k=N num (18)

[0088] In the formula, the function Index min k(A,k) is used to obtain the indices of the k smallest values ​​of A.

[0089] Let the pigeon level set be {0,1,2,3}, representing the globally optimal individual, locally optimal individual, locally suboptimal individual, and locally worst individual, respectively. Then the level of pigeon i is Level. i for

[0090]

[0091] In the formula, fitness N,i Let f be the set of fitness values ​​of all neighbors of pigeon i. max ,X lworst,i} and {f min ,X lbest,i} represents the fitness and position of the worst and best local individuals among their neighbors, respectively.

[0092] 4) Initialize the optimal position X of pigeon i. pbest,i and individual optimal fitness f pbest,i

[0093] Let the initial position and initial fitness be the optimal position X for pigeon i. pbest,i and individual optimal fitness f pbest,i .

[0094]

[0095] S32, Geomagnetic Navigation Phase

[0096] During the geomagnetic navigation phase, let the total number of geomagnetic navigation attempts be T1, and the number of pre-search attempts be T. pre (T pre <T1).

[0097] 1) Perform a pre-search first:

[0098]

[0099] In the formula, K p K is an empirical factor. a As the attraction factor, K r It is an exclusion factor.

[0100] 2) Perform a global search again.

[0101]

[0102] After the speed update, V needs to be used. max Limit the speed, that is

[0103]

[0104] After the position is updated, if any pigeon's position goes out of bounds, the position needs to be randomly initialized again using formula (18).

[0105] 3) Update the fitness of each pigeon. i (t+1), and the individual's optimal fitness f pbest,i And the individual's optimal position X pbest,i :

[0106]

[0107] 4) After each pigeon has completed steps 1), 2), and 3) in S32, use steps 2) and 3) in S31 to calculate the global optimal fitness f. gbest and the global optimal position X gbest The set of neighbors of each pigeon N i and Level i Update.

[0108] S33, Landmark Navigation Phase

[0109] Let T2 be the number of landmark navigation attempts. During the landmark navigation phase, since pigeons that are farther away do not have the ability to recognize landmarks, the number of pigeons in the flock is halved first, and half of the individuals with better adaptability are selected to recognize landmarks.

[0110]

[0111] Then calculate the center position of the remaining pigeons.

[0112]

[0113] During position updates, the globally optimal pigeon does not move towards the center of the flock.

[0114]

[0115] Other pigeons moved towards the center of the flock.

[0116] X i (t)=X i (t-1)+rand·(X c (t)-X i (t-1)),Level i ≠0 (28)

[0117] After updating the speed and position of each pigeon, an ADRC attitude loop control simulation is performed using formulas (10)-(15) to obtain the fitness of each pigeon. i (t+1), then update the global optimal fitness f gbest and the global optimal position X gbest :

[0118]

[0119] The self-disturbance rejection control parameters for pigeon flock hierarchy optimization in step four include:

[0120] S41. Initialize the drone and robotic arm interference model using the parameters instantiated in S11 and S12 in step one, and obtain the dynamic model of the drone with arm according to S13. Initialize the pigeon flock using the instantiated parameters in S31 in step three.

[0121] S42. Use S32 from step three for geomagnetic navigation. First, run T... pre A preliminary search is performed, followed by a global search, and the global optimal fitness f is recorded during the simulation. gbest This continues until the geomagnetic navigation phase is completed.

[0122] S43. Use S33 from step 3 for landmark navigation, run it T2 times, and record the global optimal fitness f during the simulation. gbestThis continues until the landmark navigation phase is completed.

[0123] S44. When the landmark navigation phase is completed, the simulation ends, and the final global optimal fitness f is recorded. gbest and the global optimal position X gbest .

[0124] The active disturbance rejection control parameters obtained from the simulation verification and optimization in step five include: X obtained from the pigeon flock hierarchy optimization in step four. gbest The optimal values ​​of ω0, k1, k1 in the angle loop active disturbance rejection control parameters are passed to the UAV with arm, and the desired pitch angles θ of the three x and y axes are input. d Roll angle φ d Three angle loop tests and simulations were conducted to verify the actual control effect of the optimized parameters.

[0125] The optimal active disturbance rejection control parameters for output performance in step six include: the globally optimal position X obtained from the simulation in step four. gbest The optimal values ​​of the observer bandwidth and control coefficients ω0,k1,k1 in the angle loop active disturbance rejection control parameters of the UAV with arm are given.

[0126] The advantages of this invention are mainly reflected in two aspects: First, by modeling the quadcopter UAV and the robotic arm separately and analyzing the torque interference of the robotic arm on the quadcopter UAV, a model of the UAV with the robotic arm is established. Second, a pigeon-level optimization method is designed based on the pigeon-level hierarchy. By adding a pre-search stage in the geomagnetic operator stage, the optimal value search capability of the pigeon-level optimization method is improved, and the tuning of the angle loop active disturbance rejection controller parameters of the UAV with the robotic arm is realized. Attached Figure Description

[0127] Figure 1 This is a flowchart of an embodiment of the present invention: a method for tuning the active disturbance rejection control parameters of an arm-mounted UAV based on pigeon flock hierarchy optimization.

[0128] Figure 2 This is a schematic diagram of an embodiment of a planar three-joint robotic arm according to the present invention;

[0129] Figure 3 This is an overall block diagram of an embodiment of a quadcopter controller with arms in this invention;

[0130] Figure 4 This is a fitness diagram of a pigeon flock hierarchical optimization process in this invention;

[0131] Figure 5 This is a simulation result diagram of the optimization parameter verification in this invention;

[0132] Figure 6 This is a simulation result diagram of the optimization parameter verification in this invention;

[0133] Figure 7 This is a simulation result diagram of the optimization parameter verification in this invention.

[0134] The labels and symbols in the diagram are explained as follows:

[0135] PID Proportional-Integral-Derivative Control

[0136] ADRC Active Disturbance Rejection Control

[0137] p d Desired location

[0138] φ d ,θ d ,ψ d These are the desired roll angle, pitch angle, and yaw angle.

[0139] f d ,τ d These are the desired tensile force and the desired torque, respectively.

[0140] Desired motor speed

[0141] These are the actual motor speed, actual pulling force, and actual torque, respectively.

[0142] p and v are the actual position and velocity, respectively.

[0143] M dis External interference

[0144] Θ and ω are the actual angle and angular velocity, respectively.

[0145] X0,X1,X2,X3,X T These are the robotic arm base, joint 1, joint 2, joint 3, and the X-axis of the end effector coordinate system.

[0146] Y0,Y1,Y2,Y3,Y T These are the robotic arm base, joint 1, joint 2, joint 3, and the Y-axis of the end effector coordinate system.

[0147] L1, L2, and L3 are the lengths of levers 1, 2, and 3 of the robotic arm, respectively.

[0148] θ1, θ2, and θ3 are the joint angles of joint 1, joint 2, and joint 3 of the robotic arm, respectively. Detailed Implementation

[0149] The effectiveness of the proposed method for tuning the active disturbance rejection control parameters of an arm-mounted UAV based on pigeon flock hierarchy optimization will be verified through specific examples below.

[0150] Example 1

[0151] A method for tuning active disturbance rejection control parameters of a quadrotor UAV based on pigeon flock hierarchical optimization is proposed. First, the quadrotor UAV and the robotic arm are modeled separately. The torque interference of the robotic arm on the quadrotor UAV is analyzed to construct the model of the quadrotor UAV. Then, the attitude angle controller of the quadrotor UAV is designed using an active disturbance rejection controller. Finally, the parameters of the active disturbance rejection controller are optimized using the pigeon flock hierarchical optimization method to obtain the controller parameters with optimal performance.

[0152] Secondly, to improve the search capability of the pigeon flock optimization method, the proposed pigeon flock hierarchical optimization method first divides individuals in the flock into four levels based on the hierarchical mechanism of the flock: globally optimal individuals, locally optimal individuals, locally suboptimal individuals, and locally worst individuals. The local level is determined by comparing each individual with a fixed number of its neighbors. Then, a pre-search phase is added to the geomagnetic navigation stage. In the pre-search phase, locally optimal individuals search towards the globally optimal and locally optimal directions; locally suboptimal individuals search in the opposite directions to locally optimal and locally worst individuals; and locally worst individuals search towards locally optimal individuals. After the pre-search phase, globally optimal individuals search only towards their locally optimal direction, while other individuals search towards both the globally optimal and locally optimal directions.

[0153] Example 2

[0154] Based on the technical architecture of Embodiment 1, the technical solution of this embodiment includes:

[0155] Step 1: Build a model of the drone with an arm

[0156] S11, Drone Modeling

[0157] By establishing a geographic coordinate system G and a body coordinate system B, the position of the quadcopter UAV can be defined as (x, y, z). T The attitude angles are (φ,θ,ψ). T T is the matrix transpose operator, which yields the rotation matrix from the body coordinate system to the geographic coordinate system. for:

[0158]

[0159] In the formula, pitch angle θ, roll angle φ, and yaw angle ψ.

[0160] Neglecting air resistance, internal friction of the UAV, and fuselage stiffness, the dynamic model of the quadcopter UAV, based on the Newton-Euler equations, is as follows:

[0161]

[0162] In the formula, Let be the second derivative of the drone's position. Let τ be the second derivative of the drone's attitude angle, m be the total mass of the quadcopter drone, g be the acceleration due to gravity, f be the total thrust generated by the propellers, and τ be the second derivative of the drone's attitude angle. x ,τ y ,τ z The three-axis torque generated by the propeller. x ,I y ,I z For the rotational inertia of the three axes, dis x ,dis y ,dis z External interference for the three axes.

[0163] For a quadcopter with an X-shaped configuration, the total thrust f and triaxial torque τ generated by its propellers x ,τ y ,τ z It can be represented as:

[0164]

[0165] In the formula, d represents the distance between the center of the machine body and any motor, and c T c is the tensile coefficient. M This is the torque coefficient. These represent the motor speeds.

[0166] S12, Robotic Arm Interference Modeling

[0167] The installation of a robotic arm enables quadcopter drones to perform aerial operations. However, the presence of the robotic arm and the weight it grips affects the overall center of gravity and aerodynamic layout of the drone. This impact is significant for an underactuated system, making research into the role of robotic arms in quadcopter drones essential. The robotic arm employs methods such as... Figure 2 The three-bar planar arm shown mainly consists of three servo rotary joints, joint connectors, and an end effector. The wheelbases of adjacent joints are parallel to each other.

[0168] A robotic arm can be viewed as a series of links connected by joints. A coordinate system is established at each joint, and the transformation between adjacent joint coordinate systems can be represented by a homogeneous transformation matrix T. Let the coordinate axes of coordinate system i be... Let a i ,α i ,d i , They represent along Shaft from Move to Distance, around Shaft from Rotate to Angle, along Shaft from Move to Distance, around Shaft from Rotate to The angle. Then the transformation matrix from coordinate system i-1 to coordinate system i can be written as:

[0169]

[0170] According to S12a i ,α i ,d i , From the definition, the parameter matrix of the three-bar planar arm can be obtained as follows:

[0171]

[0172] In the formula, L i (i = 1, 2, 3) is the length of the rod, θ i (i = 1, 2, 3) represents the joint angles of the robotic arm.

[0173] Based on the chain rule of coordinate transformation, using the transformation matrix... This allows us to obtain the position and attitude transformation matrix of the j-th joint relative to the origin of the coordinate system F between the robotic arm and the drone's fixed connection point. This allows us to obtain the position of each joint relative to the origin of the fixed-point coordinate system F. Assuming that the origin of the fixed-point coordinate system F coincides with the origin of the body coordinate system B, we can then obtain the position of joint j relative to the origin of the UAV body coordinate system B. B P j :

[0174]

[0175] In the formula, P0 = [0 0 0 1] T The four-dimensional representation of the origin of the fixed-point coordinate system F for the UAV. Let be the rotation matrix from the fixed point coordinate system F to the geographic coordinate system G.

[0176] Assuming the lever of the robotic arm is a uniform lever, then the center of mass r of the robotic arm system under the mechanical system is... B It can be represented as

[0177]

[0178] In the formula, m j (j=1,2,3) represents the mass of the j-th rod. When m4 is the mass of the block, M=∑m j The total weight of the entire robotic arm system. B P cjLet be the coordinates of the center of mass of the j-th rod or block in system B, and let the coordinates of the center of mass of the block be consistent with the coordinates of the 3rd joint.

[0179] When the robotic arm is in the gripping state, and the drone is moving, assuming the robotic arm is stationary relative to the drone, the external disturbance torque generated by the robotic arm on the drone is calculated as follows:

[0180]

[0181] In the formula, This refers to the three-axis external disturbance torque generated by the robotic arm on the drone.

[0182] S13, Modeling of a drone with an arm

[0183] The established robotic arm interference model outputs interference torque. Substituting the dynamic model of the quadcopter drone in S11, and then replacing the mass m in the model with the sum of the mass m of the quadcopter drone and the total mass M of the robotic arm system, we can obtain the dynamic model of the drone with the arm.

[0184]

[0185] In the formula, m total The total mass of the quadcopter drone with arms is m total =m+M.

[0186] Step 2: Design of Active Disturbance Rejection Controller and Selection of Fitness Function for Arm-Mounted UAVs

[0187] A tracking differentiator, by arranging the transition process through the input signal, can obtain a relatively smooth input signal and its derivative signal, achieving tracking of the input signal with virtually no overshoot. A second-order discrete tracking differentiator is as follows:

[0188]

[0189] In the formula, v(k), v1(k), and v2(k) are the input signal at time k, the tracking signal of the input signal, and the differential signal of the input signal, respectively. T is the sampling period, r is the speed factor; the larger r is, the faster the tracking speed of the input signal, but the differential signal will increase high-frequency noise. h is the filtering factor, which can filter noise in the tracking signal; the larger its value, the better the filtering effect, but too large a value will cause phase loss of the input signal. fhan(·) is the optimal control function, as follows:

[0190]

[0191] By expanding the observation variable to observe the total disturbance of the system (including internal and external disturbances), its third-order discrete form is:

[0192]

[0193] In the formula, z1(k), z2(k), and z3(k) are the observation values ​​of the input signal, the derivative of the input signal, and the total system disturbance at time k, respectively; z1(k+1), z2(k+10, z3(k+1) are the observation values ​​of the input signal, the derivative of the input signal, and the total system disturbance at time k+1, respectively; T is the sampling period; β1, β2, and β3 are the feedback gains of the state error feedback. The larger the value, the faster the observation speed of the corresponding state quantity, affecting the speed of state observation; b is the system compensation factor; and u is the system control quantity. The fal(·) function is a nonlinear function, the purpose of which is to establish a nonlinear smooth observer; a1, a2∈a are the nonlinearity coefficients; δ is the width of the linear interval of the observer; and e is the observation error, expressed as follows:

[0194]

[0195] In the formula, |·| represents the absolute value operation.

[0196] The nonlinear error feedback control law obtains the pre-compensation control quantity of the system by nonlinearly combining the state feedback error, and then compensates for the total system disturbance obtained by the extended state observer to obtain the final compensated control quantity, as shown in the following form:

[0197]

[0198] In the formula, e1 is the original signal error, e2 is the original signal differential error, k1 and k2 are both control coefficients. The larger k1 is, the faster the system response. k2 can increase the system damping. fal(·) function is a nonlinear function. δ0 is the width of the linear interval of the nonlinear error feedback control law. u0 is the control output before compensation, u is the control output after compensation, and b is the compensation coefficient.

[0199] Considering that the torque interference from the robotic arm on the quadcopter mainly affects the x and y axes of the ADRC angle loop, the ADRC controller is only used in the quadcopter's angle loop. The overall controller framework adopts a cascade control structure, with the outer loop being the position loop, using a PID controller, and the inner loop being the attitude loop, using an ADRC controller. The control system block diagram is as follows: Figure 3 As shown, first input the desired position p. d The PID position subsystem outputs the desired roll angle φ. d Desired pitch angle θ d and expected tension f d The ADRC attitude subsystem is based on the desired heading angle ψ d Expected roll angle φ d and desired pitch angle θd Calculate the desired torque τ d The control distributor is based on the desired tension f d and desired torque τ d Calculate the desired motor speed The motor model obtains the actual motor speed based on the desired speed. In addition to the total thrust f and total torque τ that the propeller can actually provide, the robotic arm calculates the torque disturbance M based on the states of the robotic arm and the drone. dis Finally, the quadrotor model with arms is used to update the quadrotor's position p, velocity v, attitude angle Θ, and attitude angular velocity ω.

[0200] Due to the structural symmetry of the quadcopter, the following optimization of the active disturbance rejection controller (ADRC) parameters for the arm-mounted UAV will only be based on the x-axis. The ADRC has a total of 14 parameters: r, h, a1, a2, δ, α1, α2, δ0, b, β1, β2, β3, k1, and k2. The values ​​of r, h, a1, a2, δ, α1, α2, and δ0 can be determined based on engineering experience. The value of b is related to the model of the quadcopter UAV. β1, β2, and β3 can also be determined by the observer's bandwidth ω0, i.e., β1 = 3ω0, β2 = 3ω0. 2 β3=ω0 3 Therefore, the key parameters that the active disturbance rejection controller needs to optimize are ω0, k1, and k2.

[0201] The fitness function for the optimization method is the Integral Time Absolute Error (ITAE); the smaller the value, the better the control effect. Let the desired roll angle of the angle loop input be φ. d The actual roll angle φ, and the total simulation time T. s Then the fitness of each set of ADRC control parameters is:

[0202]

[0203] In the formula, t1 is the simulation time, and |·| is the absolute value operation.

[0204] Step 3: Pigeon Flock Hierarchy Optimization Method Based on Pre-Search

[0205] Traditional pigeon flock optimization methods consist of two stages. The first stage involves geomagnetic navigation, where the position and speed of pigeon i are set to X and X, respectively. i and V i The update formulas for map and compass operator position velocity are:

[0206]

[0207] In the formula: e (·)R is an exponential function; R is the geomagnetic operator; rand is a random number; X g It represents the current global best position, which can be obtained by comparing all positions among all pigeons; t represents the current iteration number.

[0208] After the geomagnetic navigation is completed, the landmark navigation is run. Pigeons far from their destination are unfamiliar with the landmarks and no longer have the ability to distinguish paths, so they are discarded. Then, the center position of the remaining pigeons is X... c As a landmark, all pigeons that see the landmark fly directly to it, eventually arriving at the optimal solution. The location update formula for landmark navigation is:

[0209]

[0210] In the formula, N p X represents the number of pigeons in the flock. c (t) represents the pigeon's center position after t iterations, and fitness(·) is the fitness function.

[0211] As can be seen from the geomagnetic navigation stage of traditional pigeon flock optimization methods, all pigeons tend to move towards the global optimum in the initial stage, ignoring the ability to explore other optimal solutions to some extent, and are also prone to getting trapped in local optima. In addition, in the landmark navigation stage, all pigeons move towards the center of the flock, and the pigeons that are globally optimal may reduce their fitness by moving their own positions. Therefore, to improve the optimal solution search performance of the pigeon flocking method, three improvements are made to the traditional pigeon flocking optimization method. First, based on the hierarchical mechanism of the pigeon flock, individuals in the flock are divided into four levels: globally optimal, locally optimal, locally suboptimal, and locally worst. The local level is determined by comparing each individual with a fixed number of its neighbors. Each individual in the flock updates its optimal position record based on its historical position. Then, a pre-search phase is added to the geomagnetic navigation stage. In the pre-search phase, locally optimal individuals search towards the position of the globally optimal individual and the direction of their own optimal position; locally suboptimal individuals search in the opposite direction of the positions of locally optimal and locally worst individuals; and locally worst individuals search towards the position of the locally optimal individual. After the pre-search phase, globally optimal individuals only search towards their own optimal position, while individuals of other levels search towards the position of the globally optimal individual and the direction of their own optimal position. Finally, in the landmark operator stage, globally optimal individuals remain stationary, while only individuals that are not globally optimal move towards the center of the flock. The specific steps of the pigeon flocking hierarchical optimization method are as follows:

[0212] S31. Initialize the pigeon flock

[0213] 1) Initialize the pigeon flock's position and speed

[0214] Let the total number of pigeons be N. p Then the initial position and speed of the flock of pigeons are:

[0215]

[0216] In the formula, X i,d and V i,d Let Δbound be the position and velocity of pigeon i in the d-th dimension. d and V represents the width and center coordinates of the d-th dimension, respectively. max The maximum speed of the pigeon (V) max >0), rands is a random number in the interval [-1,1].

[0217] 2) Global optimal fitness and position update

[0218]

[0219] In the formula, {f gbest ,X gbest} represents the fitness and position of the globally optimal individual.

[0220] 3) Update pigeon i's neighbors and level.

[0221] Let the fitness of pigeon i be denoted by fitness. i dist i,j Let dist be the distance between pigeons i and j, and let dist be the distance between them. i,j =0 (i=j), expected number of fixed neighbors N num Then the neighbor set N of pigeon i i for

[0222] N i =Index min k(dist,k),k=N num (49)

[0223] In the formula, the function Index min k(A,k) is used to obtain the indices of the k smallest values ​​of A.

[0224] Let the pigeon level set be {0,1,2,3}, representing the globally optimal individual, locally optimal individual, locally suboptimal individual, and locally worst individual, respectively. Then the level of pigeon i is Level. i for

[0225]

[0226] In the formula, fitness N,i Let f be the set of fitness values ​​of all neighbors of pigeon i. max ,X lworst,i} and {fmin ,X lbest,i} represents the fitness and position of the worst and best local individuals among their neighbors, respectively.

[0227] 4) Initialize the optimal position X of pigeon i. pbest,i and individual optimal fitness f pbest,i

[0228] Let the initial position and initial fitness be the optimal position X for pigeon i. pbest,i and individual optimal fitness f pbest,i .

[0229]

[0230] S32, Geomagnetic Navigation Phase

[0231] During the geomagnetic navigation phase, let the total number of geomagnetic navigation attempts be T1, and the number of pre-search attempts be T. pre (T pre <T1).

[0232] 1) Perform a pre-search first:

[0233]

[0234] In the formula, K p K is an empirical factor. a As the attraction factor, K r It is an exclusion factor.

[0235] 2) Perform a global search again.

[0236]

[0237] After the speed update, V needs to be used. max Limit the speed, that is

[0238]

[0239] After the position is updated, if any pigeon's position goes out of bounds, the position needs to be randomly initialized again using formula (18).

[0240] 3) Update the fitness of each pigeon. i (t+1), and the individual's optimal fitness f pbest,i And the individual's optimal position X pbest,i :

[0241]

[0242] 4) After each pigeon has completed steps 1), 2), and 3) in S32, use steps 2) and 3) in S31 to calculate the global optimal fitness f. gbest and the global optimal position X gbest The set of neighbors of each pigeon N i and Level i Update.

[0243] S33, Landmark Navigation Phase

[0244] Let T2 be the number of landmark navigation attempts. During the landmark navigation phase, since pigeons that are farther away do not have the ability to recognize landmarks, the number of pigeons in the flock is halved first, and half of the individuals with better adaptability are selected to recognize landmarks.

[0245]

[0246] Then calculate the center position X of the remaining pigeons. c (t)

[0247]

[0248] During position updates, the globally optimal pigeon does not move towards the center of the flock.

[0249] X i (t)=X i (t-1),Level i =0 (58)

[0250] Other pigeons moved toward the center of the flock.

[0251] X i (t)=X i (t-1)+rand·(X c (t)-X i (t-1)),Level i ≠0 (59)

[0252] After updating the speed and position of each pigeon, an ADRC attitude loop control simulation is performed using formulas (10)-(15) to obtain the fitness of each pigeon. i (t+1), then update the global optimal fitness f gbest and the global optimal position X gbest :

[0253]

[0254] Step 4: Optimize the self-disturbance rejection control parameters at the pigeon flock level

[0255] S41. Initialize the drone and robotic arm interference model using the parameters instantiated in S11 and S12 in step one, and obtain the dynamic model of the drone with arm according to S13. Initialize the pigeon flock using the instantiated parameters in S31 in step three.

[0256] S42. Use S32 from step three for geomagnetic navigation. First, run T... pre A preliminary search is performed, followed by a global search, and the global optimal fitness f is recorded during the simulation. gbest This continues until the geomagnetic navigation phase is completed.

[0257] S43. Use S33 from step 3 for landmark navigation, run it T2 times, and record the global optimal fitness f during the simulation. gbest This continues until the landmark navigation phase is completed.

[0258] S44. When the landmark navigation phase is completed, the simulation ends, and the final global optimal fitness f is recorded. gbest and the global optimal position X gbest .

[0259] Step 5: Simulation verification of the optimized active disturbance rejection control parameters

[0260] X obtained from the pigeon flock hierarchy optimization in step four gbest The optimal values ​​of [ω0,k1,k1] in the angle loop active disturbance rejection control parameters are passed to the UAV with arm, and the desired pitch angles θ of the three x and y axes are input. d Roll angle φ d Three angle loop tests and simulations were conducted to verify the actual control effect of the optimized parameters.

[0261] Step 6: Output the optimal active disturbance rejection control parameters.

[0262] X obtained from the simulation in step four gbest The optimal values ​​of ω0,k1,k1 are given in the angle loop active disturbance rejection control parameters of the UAV with arm.

[0263] Example 3

[0264] according to Figure 1 The flowchart is used for example simulation to verify the rationality and effectiveness of the proposed method for tuning the active disturbance rejection control parameters of an arm-mounted UAV based on pigeon flock hierarchy optimization.

[0265] The simulation environment for this example is configured with an Intel i7-10700 processor, 32.90GHz clock speed, 8GB RAM, and Matlab R2020b software. The specific steps of this method are as follows:

[0266] Step 1: Build a model of the drone with an arm

[0267] S11, Drone Modeling

[0268] Let the initial position of the quadcopter drone be (x, y, z). T =(0,0,0) T m, initial attitude angle (φ,θ,ψ) T =(0,0,0) T The total mass m is 13.5 kg, and the gravitational acceleration g is 9.8 m / s². 2 The rotational inertia of the three axes (I) x ,I y ,I z = (0.8244, 1.135, 1.759) kg.m 2 The distance d between the center of the machine body and any motor is 0.2m, and the tension coefficient is c. T =1.105e-5, torque coefficient c M =1.779e-7, the external disturbance of the three axes is (dis x ,dis y ,dis z () = (0,0,0). The motor speed. Substituting the above parameters into formulas (2) and (3) yields the dynamic model of the quadcopter UAV.

[0269] S12, Robotic Arm Interference Modeling

[0270] Reference Figure 2 Initialize the length L of the three-bar planar arm. i (i = 1, 2, 3) = [0.2, 0.3, 0.1]m, initial joint angle θ i (i=1,2,3)=[-90,0,0]·π / 180rad, the transformation matrix between adjacent joints can be determined using formulas (4) and (5), and the mass of the rod is m. j (j=1,2,3)=[0.02,0.03,0.01]kg, the mass of the block m4=0.5kg, M=∑m j The total mass of the entire robotic arm system, and the rotation matrix from the fixed point coordinate system F to the geographic coordinate system G. Based on the attitude angle of the quadcopter, the disturbance torque M of the robotic arm system on the quadcopter UAV at the current moment can be obtained using formulas (1), (6)-(8). dis .

[0271] S13, Modeling of a drone with an arm

[0272] The established robotic arm interference model outputs the interference torque M. disSubstitute the dynamic model of the quadcopter drone obtained in S11 (Formula (2)) and then replace the mass m in the model with the sum of the mass m of the quadcopter drone and the total mass M of the robotic arm system to obtain the dynamic model of the drone with arm (Formula (9)).

[0273] Step 2: Design of Active Disturbance Rejection Controller and Selection of Fitness Function for Quadcopter with Arm

[0274] Considering that the torque interference from the robotic arm on the quadcopter mainly affects the x and y axes of the ADRC angle loop, the ADRC controller is only used in the quadcopter's angle loop. The overall controller framework adopts a cascade control structure, with the outer loop being the position loop, using a PID controller, and the inner loop being the attitude loop, using an ADRC controller. The control system block diagram is as follows: Figure 3 As shown, first input the desired position p. d The PID position subsystem outputs the desired roll angle φ. d Desired pitch angle θ d and expected tension f d The ADRC attitude subsystem is based on the desired heading angle ψ d Expected roll angle φ d and desired pitch angle θ d Calculate the desired torque τ d The control distributor is based on the desired tension f d and desired torque τ d Calculate the desired motor speed The motor model obtains the actual motor speed based on the desired speed. In addition to the total thrust f and total torque τ that the propeller can actually provide, the robotic arm calculates the torque disturbance M based on the states of the robotic arm and the drone. dis Finally, the quadrotor model with arms is used to update the quadrotor's position p, velocity v, attitude angle Θ, and attitude angular velocity ω.

[0275] Due to the structural symmetry of the quadcopter, the following optimization of the active disturbance rejection controller (ADRC) parameters for the arm-mounted UAV will only be based on the x-axis. The ADRC has a total of 14 parameters: r, h, a1, a2, δ, α1, α2, δ0, b, β1, β2, β3, k1, and k2. The values ​​of r, h, a1, a2, δ, α1, α2, and δ0 are determined based on engineering experience: r = 0.5, h = 0.03, a1 = 0.5, a2 = 0.25, δ = 0.01, α1 = 0.75, α2 = 1.25, and δ0 = 0.1. The value of b depends on the model of the quadcopter UAV. x =1 / I x b y =1 / I y b z=1 / I z β1, β2, and β3 can also be determined by the observer's bandwidth ω0, i.e., β1 = 3ω0, β2 = 3ω0 2 β3=ω0 3 Therefore, the key parameters that the active disturbance rejection controller needs to optimize are ω0, k1, and k2, whose values ​​will be given in step four. Let the desired roll angle of the angle loop input be φ. d =10 ° The simulation time is T. s =10s.

[0276] Step 3: Pigeon Flock Hierarchy Optimization Method Based on Pre-Search

[0277] S31. Initialize the pigeon flock

[0278] 1) Initialize the pigeon flock's position and speed

[0279] Let N be the total number of pigeons. p =40. Since there are three parameters to be optimized, the space of the optimal solution is three-dimensional, with a width Δbound = [5, 10, 10] and a center coordinate bound. Center = [5, 10, 20], the pigeon's maximum speed V max =1.0m / s, the location X of the pigeon flock can be randomly generated using formula (18). i,d (0) and velocity V i,d (0).

[0280] 2) Global optimal fitness and position update

[0281] Based on the initial position of the flock, that is, the position X of each pigeon i (0) represents the parameters ω0, k1, k2 to be optimized in a set of ADRC controllers. By performing an ADRC control simulation using formulas (10)-(15), the fitness of pigeon i can be obtained. i Then, the global optimal fitness f can be obtained using formula (19). gbest and the global optimal position X gbest .

[0282] 3) Update pigeon i's neighbors and level.

[0283] Let N be the desired fixed number of neighbors. num =5. Based on the position of each pigeon in the flock, the neighbor set N of pigeon i can be calculated using formula (20). i Pigeon i can determine the local best individual X among the pigeons that can be interacted with by comparing its fitness with that of its neighbors using formula (21). lbest,i And the worst local individual X lworst,iAt the same time, determine your own level. i .

[0284] 4) Initialize the optimal position X of pigeon i. pbest,i and individual optimal fitness f pbest,i

[0285] Let the initial position and initial fitness be the optimal position X for pigeon i. pbest,i and individual optimal fitness f pbest,i That is, formula (22).

[0286] S32, Geomagnetic Navigation Phase

[0287] During the geomagnetic navigation phase, let the total number of geomagnetic navigation attempts T1 = 30, and the number of pre-search attempts T pre =5, initialize the empirical factor K p =0.5, attraction factor K a =2.0, Exclusion factor K r =0.2, geomagnetic operator R=0.15.

[0288] 1) Perform a pre-search first

[0289] The front T in geomagnetic navigation pre =5 times using pre-search (i.e., formula (23)) to update velocity V i and position X i .

[0290] 2) Perform a global search again.

[0291] The remaining pre-search iterations are updated using the global search (i.e., formula (24)) to update the velocity V. i and position X i .

[0292] After the velocity update, the velocity V needs to be adjusted using formula (25). i To limit the range, if a pigeon's position goes out of bounds after the position is updated, the position needs to be randomly initialized using the reuse formula (18).

[0293] 3) Perform an ADRC attitude loop control simulation using formulas (10)-(15) to update the fitness of each pigeon. i (t+1), and update the individual's optimal fitness f using formula (26). pbest,i And the individual's optimal position X pbest,i .

[0294] 4) After each pigeon has completed steps 1), 2), and 3) in S32, use steps 2) and 3) in S31 to calculate the global optimal fitness f. gbest and the global optimal position Xgbest The set of neighbors of each pigeon N i and Level i Update.

[0295] S33, Landmark Navigation Phase

[0296] Let the number of landmark navigation attempts T2 = 10. During the landmark navigation phase, the globally optimal pigeon does not move towards the center of the flock, while other pigeons move towards the center. After each pigeon has completed its position update using formulas (27) to (30), an ADRC attitude loop control simulation is performed using formulas (10) to (15) to obtain the fitness of each pigeon. i (t+1), the global optimal fitness f can be obtained using formula (19). gbest and the global optimal position X gbest .

[0297] Step 4: Optimize the self-disturbance rejection control parameters at the pigeon flock level

[0298] S41. Initialize the drone and robotic arm interference model using the parameters instantiated in S11 and S12 in step one, and obtain the dynamic model of the drone with arm according to S13. Initialize the pigeon flock using the instantiated parameters in S31 in step three.

[0299] S42. Use S32 from step three for geomagnetic navigation. First, run T... pre =5 pre-searches, then 25 global searches, and record the global optimal fitness f during the simulation. gbest This continues until the geomagnetic navigation phase is completed.

[0300] S43. Use S33 from step 3 for landmark navigation, run T2 = 10 times, and record the global optimal fitness f during the simulation. gbest This continues until the landmark navigation phase is completed.

[0301] S44. When the landmark navigation phase is completed, the simulation ends, and the final global optimal fitness f is recorded. gbest =4.1052 and the global optimal position X gbest =[7.481490,19.999891,19.340197].

[0302] Step 5: Simulation verification of the optimized active disturbance rejection control parameters

[0303] X obtained from the pigeon flock hierarchy optimization in step four gbestThe parameters [ω0,k1,k1] = [7.481490,19.999891,19.340197] are passed to the angle loop active disturbance rejection control parameters of the boom-mounted UAV. The desired pitch angle θ of the x and y axes is input. d and expected roll angle φ d Three angle loop tests were performed simultaneously at 10°, 15°, and 20°, with a simulation time T. s =10s, verify the actual control effect of the optimized parameters, and output the pitch and roll angle curves.

[0304] Step 6: Output the optimal active disturbance rejection control parameters.

[0305] X obtained from the simulation in step four gbest =[7.481490,19.999891,19.340197] is the optimal value of ω0,k1,k1 in the angle loop active disturbance rejection control parameters of the UAV with arm, that is, [ω0,k1,k1]=[7.481490,19.999891,19.340197].

[0306] To verify the effectiveness of the method proposed in this invention, this patent first analyzes the global optimal fitness curve during the parameter optimization process, such as... Figure 4 As shown, the global optimal fitness approaches and converges after 5 iterations, and a better solution can still be found during the landmark navigation stage. Simultaneously, the optimized parameters were verified through simulation of angle loop active disturbance rejection control for a drone with an arm. Figures 5-7 Three sets of simulation results for angle loop active disturbance rejection control of the UAV with arm were conducted for the optimized parameters. The simulation curves of angle loop tracking show that the roll angle and pitch angle of the UAV with arm can enter steady state within 1.5 seconds with almost no overshoot and a steady state error of 0. This indicates that the parameters have good control performance and verifies the feasibility of the proposed method.

Claims

1. A pigeon group hierarchy optimization-based parameter tuning method for an unmanned aerial vehicle with arms, characterized by: Firstly, the quadrotor unmanned aerial vehicle and the mechanical arm are modeled separately, the torque interference of the mechanical arm to the quadrotor unmanned aerial vehicle is analyzed, the unmanned aerial vehicle with arm model is established, then the attitude angle controller of the quadrotor unmanned aerial vehicle is designed by using the active disturbance rejection controller, finally the pigeon group hierarchical optimization method is used to optimize the parameters of the active disturbance rejection controller, and the optimal controller parameters are obtained; The pigeon group hierarchical optimization method comprises the following steps: firstly, according to the hierarchical mechanism of the pigeon group, individuals in the pigeon group are divided into four levels of global optimal individual, local optimal individual, local suboptimal individual and local worst individual, wherein the local level is determined by comparing each individual with a fixed number of neighbors; then a pre-search stage is added in the geomagnetic navigation stage, in the pre-search stage, the local optimal individual searches in the direction of the global optimal and individual optimal, the local suboptimal individual searches in the opposite direction of the local optimal individual and the local worst individual, and the local worst individual searches in the direction of the local optimal individual; after the pre-search stage ends, the global optimal individual only searches in the direction of the individual optimal, and the other individuals search in the direction of the global optimal and the individual optimal; specifically: Step one, establishing the unmanned aerial vehicle with arm model S11, unmanned aerial vehicle modeling: establishing a geographic coordinate system and a body coordinate system, defining the position and attitude angle of the quadrotor unmanned aerial vehicle, obtaining the rotation matrix from the body coordinate system to the geographic coordinate system, and obtaining the dynamic model of the quadrotor unmanned aerial vehicle according to the Newton-Euler equation without considering air resistance, unmanned aerial vehicle internal friction and body stiffness; S12, mechanical arm interference modeling: the mechanical arm adopts a three-link planar arm, which mainly comprises three servo rotating joints, joint connecting pieces and a terminal mechanical claw, and the adjacent joint shafts are parallel to each other; a coordinate system is established at each joint of the mechanical arm to obtain a parameter matrix of the three-link planar arm, and a mechanical arm interference model is established through the external interference torque of the mechanical arm to the unmanned aerial vehicle; S13, modeling of the unmanned aerial vehicle with arm: the interference torque output by the mechanical arm interference model is substituted into the dynamic model of the quadrotor unmanned aerial vehicle in S11 to obtain the dynamic model of the unmanned aerial vehicle with arm; Step two, designing the active disturbance rejection controller of the unmanned aerial vehicle with arm and selecting the fitness function The total framework of the active disturbance rejection controller of the unmanned aerial vehicle with arm adopts a cascade control structure, the outer loop is a position loop, a PID controller is used, and the inner loop is an attitude loop, an ADRC controller is used; the fitness function selects an integral value of time absolute deviation to be small; Step three, pigeon group hierarchical optimization method based on pre-search S31, initializing the pigeon group comprises: 1) initializing the position and speed of the pigeon group, 2) updating the global optimal fitness and position, 3) updating the neighbors and levels of the pigeons, and 4) initializing the individual optimal position and individual optimal fitness of the pigeons; S32, geomagnetic navigation stage comprises: 1) pre-searching first, 2) then globally searching, 3) updating the fitness of each pigeon, the individual best fitness and the individual best position, and 4) after each pigeon has completed steps 1), 2) and 3) in S32, the global optimal fitness, the global optimal position, the neighbor set and the level of each pigeon are updated by using steps 2) and 3) in S31; S33, the landmark navigation stage comprises: selecting a pigeon individual with better fitness to identify a landmark, calculating a remaining pigeon center position, when the position is updated, the globally optimal pigeon does not move to the pigeon group center, and other pigeons move to the pigeon group center, after each pigeon is updated in speed and position, ADRC attitude loop control simulation is performed once and the fitness of each pigeon is obtained, and then the globally optimal fitness and globally optimal position are updated; Step four, pigeon hierarchy optimization active disturbance rejection control parameter S41, the parameters instantiated in S11 and S12 in step one are used to initialize the unmanned aerial vehicle and mechanical arm disturbance model, and the dynamic model of the unmanned aerial vehicle with arm is obtained according to S13, and the pigeon is initialized by using the instantiated parameters in S31 in step three; S42, geomagnetic navigation is performed by using S32 in step three, pre-searching is first performed, then global searching is performed, and the globally optimal fitness in the simulation process is recorded until the geomagnetic navigation stage is completed; S43, landmark navigation is performed by using S33 in step three, and the globally optimal fitness in the simulation process is recorded until the landmark navigation stage is completed; S44, when the landmark navigation stage is completed, the simulation is ended, and the final globally optimal fitness and globally optimal position are recorded; Step five, simulation verification of the optimized active disturbance rejection control parameter The globally optimal position obtained by the pigeon hierarchy optimization in step four is transmitted to the optimal value in the active disturbance rejection control parameter of the angle loop of the unmanned aerial vehicle with arm, and three angle loop test simulations are performed by inputting the expected pitch angle and roll angle of three groups of shafts, so as to verify the actual control effect of the optimized parameter; Step six, output the performance optimal active disturbance rejection control parameter.

2. The pigeon hierarchy-based optimization of the strapdown UAV active disturbance rejection control parameter tuning method according to claim 1, characterized in that: The step one of establishing the unmanned aerial vehicle with arm model comprises: Step one: establishing the unmanned aerial vehicle with arm model S11, unmanned aerial vehicle modeling A geographic coordinate system G and a body coordinate system B are established, and the position of the quadrotor is defined as (x, y, z) T , and the attitude angle is (φ, θ, ψ) T , T is a matrix transposition operator, and a rotation matrix of the body coordinate system to the geographic coordinate system is obtained is: In the formula, the pitch angle θ, the roll angle φ, and the yaw angle ψ; Without considering air resistance, unmanned aerial vehicle internal friction and body stiffness, according to Newton-Euler equation, the dynamic model of the quadrotor unmanned aerial vehicle is obtained as follows: wherein, is the second derivative of the position of the UAV, is the second derivative of the attitude angle of the UAV, m is the total mass of the quadrotor UAV, g is the gravity acceleration, f is the total thrust generated by the propellers, τ x ,τ y ,τ z is the three-axis moment generated by the propellers, I x ,I y ,I z is the three-axis moment of inertia, dis x ,dis y ,dis z is the three-axis external disturbance; For a quadcopter with X-shaped layout, the total drag force f and three-axis torques τ generated by the propellers are expressed as: x y z ​​​ where d represents the distance between the center of the body and any motor, c T is the drag coefficient, c M is the moment coefficient, are the rotational speeds of the motors, respectively; where i = 1, 2, 3, 4. S12, mechanical arm disturbance modeling The mechanical arm is a set of links connected by joints. A coordinate system is established at each joint. The transformation between the coordinate systems of adjacent joints is represented by a homogeneous transformation matrix T. Let the coordinate axes of coordinate system i be Let a i , α i , represent the distance from to along the axis, the angle from to rotated about the axis, the distance from to along the axis, and the angle from to rotated about the axis, respectively. The transformation matrix from coordinate system i-1 to coordinate system i is written as: According to the definition of a i , α i ,d i , , the parameter matrix of the three-link planar arm is obtained as follows: wherein L i is the length of the rod, θ i is the joint angle of the robot arm, where i = 1, 2, 3; According to the chain rule of coordinate transformation, the conversion matrix is obtained by using the conversion matrix The position and attitude relationship conversion matrix of the jth joint node relative to the origin of the fixed point coordinate system F of the mechanical arm and the UAV is obtained Thus, the position of each joint relative to the origin of the fixed point coordinate system F is obtained, and the position of the joint j relative to the origin of the UAV body coordinate system B is obtained by assuming that the fixed point coordinate system F coincides with the origin of the body coordinate system B B P j : where P0= [0 0 0 1] T is a four-dimensional representation of the origin of the UAV fixed point coordinate system F, is a rotation matrix of the fixed point coordinate system F to the geographic coordinate system G, Assuming that the link of the robot arm is a homogeneous link, the center of mass r B of the robot arm system under the machine body is represented as wherein m j m4is the mass of the object, M =∑m j is the total weight of the whole robotic arm system, B P cj is the center of mass coordinate of the jth link or object under the base frame B, and let the center of mass coordinate of the object be identical to the coordinate of the 3rd joint. When the mechanical arm is in the state of clamping an object, it is assumed that the mechanical arm is stationary relative to the unmanned aerial vehicle during the movement of the unmanned aerial vehicle, and the external disturbance torque of the mechanical arm to the unmanned aerial vehicle is calculated as follows: In the formula, is the three-axis external disturbance torque generated by the manipulator on the UAV, S13, modeling of the unmanned aerial vehicle with arm Output the disturbance torque from the established robot arm disturbance model Substitute the dynamic model of the quadrotor UAV in S11, and replace the mass m in the model with the sum of the mass m of the quadrotor UAV and the total mass M of the robot arm system to obtain the dynamic model of the UAV with arm, where m total is the total mass of the quadcopter, i.e. m total = m + M.

3. The pigeon hierarchy-based optimization of the strapdown UAV active disturbance rejection control parameter tuning method according to claim 2, characterized in that: The mechanical arm adopts a three-link plane arm, which mainly comprises three servo rotating joints, joint connecting pieces and a terminal mechanical claw, and adjacent joint shafts are parallel to each other.

4. The pigeon hierarchy optimization based parameter tuning method for the bar-armed UAV active disturbance rejection control according to claim 2 or 3, characterized in that: The step two of the active disturbance rejection controller design and fitness function selection of the unmanned aerial vehicle with arm comprises: The transition process is arranged by tracking the differential input signal to obtain a relatively smooth input signal and its differential signal, and the tracking of the input signal is realized without overshoot, and the second-order discrete tracking differentiator is as follows: In the formula, v(k), v1(k) and v2(k) are respectively the input signal, the tracking signal of the input signal and the differential signal of the input signal; T is the sampling period, r is the speed factor, h is the filter factor, fhan(·) is the optimal control function, and the specific form is as follows: A new observation variable is expanded by an extended state observer to observe the total disturbance of the system, which includes internal disturbance and external disturbance, and the third-order discrete form is as follows: In the formula, z1(k), z2(k), z3(k) are respectively the observation values of the observer at k time on the input signal, the input signal differential, and the total disturbance of the system, z1(k+1), z2(k+1), z3(k+1) are respectively the observation values of the observer at k+1 time on the input signal, the input signal differential, and the total disturbance of the system, T is a sampling period, β1, β2, β3 are feedback gains of state error feedback, b is a system compensation factor, u is a system control quantity; the fal(·) function is a nonlinear function, a1, a2∈a are nonlinear degree coefficients, δ is the width of the linear interval, e is an error, and is as follows: In the formula, |·| is an absolute value operation; The nonlinear error feedback control rate is obtained by combining the state feedback error in a nonlinear manner to obtain the pre-compensation control quantity of the system, and then the total disturbance of the system obtained by the extended state observer is compensated to obtain the final compensated control quantity, and the specific form is as follows: In the formula, e1 is the original signal error, e2 is the original signal differential error, k1 and k2 are control coefficients, the larger k1 is, the faster the system responds, and the increase of k2 increases the damping of the system, the fal(·) function is a nonlinear function, δ0 is the width of the linear interval of the nonlinear error feedback control rate, u0 is the pre-compensation control output, u is the post-compensation control output, and b is the compensation coefficient; The overall controller framework adopts a cascade control structure, with the outer loop being the position loop using a PID controller and the inner loop being the attitude loop using an ADRC controller. The control system first inputs the desired position p. d The PID position subsystem outputs the desired roll angle φ. d Desired pitch angle θ d and expected tension f d The ADRC attitude subsystem is based on the desired heading angle ψ d Expected roll angle φ d and desired pitch angle θ d Calculate the desired torque τ d The control distributor is based on the desired tension f d and desired torque τ d Calculate the desired motor speed The motor model obtains the actual motor speed based on the desired speed. In addition to the total thrust f and total torque τ that the propeller can actually provide, the robotic arm calculates the torque disturbance M based on the states of the robotic arm and the drone. dis Finally, the quadrotor model with arms is used to update the quadrotor's position p, velocity v, attitude angle Θ, and attitude angular velocity ω; the key parameters that the active disturbance rejection controller needs to optimize are ω0, k1, and k2; The fitness function of the optimization method selects the smaller integral value of absolute deviation of time, the better the control effect is; the expected roll angle of the angle ring input is φ d , the actual roll angle is φ, and the simulation time is T s , and the fitness of each group of ADRC control parameters is 5. The pigeon hierarchy-based optimization of the strapdown UAV active disturbance rejection control parameter tuning method according to claim 4, characterized in that: The step two armless unmanned aerial vehicle active disturbance rejection controller design and fitness function selection includes: for the structural symmetry of the quadcopter, the active disturbance rejection controller has a total of 14 parameters, including r, h, a1, a2, δ, α1, α2, δ0, b, β1, β2, β3, k1, k2; wherein the parameter values of r, h, a1, a2, δ, α1, α2, δ0 are determined according to engineering experience, the size of b is related to the model of the quadcopter unmanned aerial vehicle, and β1, β2, β3 are determined by the bandwidth ω0 of the observer, that is, β1=3ω0, β2=3ω0 2 , β3=ω0 3 Therefore, the key parameters of the active disturbance rejection controller that need to be optimized are ω0, k1, k2.

6. The pigeon hierarchy-based optimization of the strapdown UAV active disturbance rejection control parameter tuning method according to claim 5, characterized in that: The pigeon group hierarchical optimization method based on pre-searching of the step three comprises: S31, initializing the pigeon group 1) initializing the pigeon group position and speed Let the total number of pigeons be N p The initial position and velocity of the pigeon group are where X i,d and V i,d are the pigeon i's d-dimensional spatial position and velocity, respectively, Δbound d and are the width and center coordinate of the d-dimensional space, respectively, V max is the pigeon's maximum speed, V max > 0, and rands are random numbers in the interval [-1, 1]. 2) updating the global optimal fitness and position In the formula, {f gbest ,X gbest} is the fitness and position of the globally optimal individual. 3) updating the neighbors and hierarchy of the pigeon i Let the fitness of pigeon i be fitness i , dist i,j is the distance between pigeon i and pigeon j, and let dist i,j = 0, i = j, and let the desired number of neighbors N num be fixed, then the set of neighbors N i for pigeon i is N i = Index min k(dist, k), k = N num (18) In the formula, the function of Index min k(A, k) is to obtain the number of the minimum k values of A; Let the pigeon level set be {0, 1, 2, 3} to represent the global optimal individual, local optimal individual, local suboptimal individual and local worst individual respectively, then the level Level of pigeon i is i for where fitness N,i is the set of fitness of all neighbors of pigeon i, {f max ,X lworst,i} and {f min ,X lbest,i} are the fitness and position of the local worst individual and the local best individual in the neighborhood, respectively. 4) initialize pigeon i individual best position X pbest,i and individual best fitness f pbest,i Let initial position and initial fitness be the pigeon i individual optimal position X pbest,i and individual optimal fitness f pbest,i ; S32, the geomagnetic navigation stage In the geomagnetic navigation stage, let the total number of geomagnetic navigation T1, the number of pre-search T pre , T pre <T1; 1) pre-searching is performed first wherein K p is an empirical factor, K a is an attraction factor, K r is a repulsion factor; 2) global searching is then performed After the speed update, it is necessary to utilize V max The speed is clipped, i.e. After the position is updated, if the position of a pigeon is out of the boundary, the position is randomly initialized again by using formula (18); 3) Update the fitness of each pigeon i (t+1), and the best fitness f pbest,i and the best position X pbest,i : 4) After each pigeon has performed steps 1), 2), 3) in S32, the global optimal fitness f gbest and the global optimal position X gbest , the neighbor set N i and the level Level i of each pigeon are updated; S33, the landmark navigation stage Supposing that the landmark navigation times are T2, in the landmark navigation stage, the number of the pigeon groups is halved first, and the half of the groups with better fitness are selected to identify the landmarks The center positions of the remaining pigeons are then calculated When the position is updated, the global optimal pigeon does not move to the center of the pigeon group X i (t) = X i (t-1), Level i = 0 (27) The other pigeons move to the center of the pigeon group; X i (t) = X i (t - 1) + rand - (X c (t) - X i (t - 1)), Level i ≠ 0 (28) After each pigeon has finished the speed and position update, the ADRC attitude loop control simulation is performed using equations (10) - (15) and the fitness of each pigeon is obtained i (t+1), and then the global optimal fitness f gbest and the global optimal position X gbest are updated 7. The pigeon hierarchy-based optimization of the strapdown UAV active disturbance rejection control parameter tuning method according to claim 6, characterized in that: The pigeon group hierarchical optimization active disturbance rejection control parameter of the step four comprises: S41, the parameters in the S11 and S12 in the step one are used to initialize the unmanned aerial vehicle and the mechanical arm disturbance model, the dynamic model of the unmanned aerial vehicle with the arm is obtained according to S13, and the pigeon group is initialized by using the parameters in S31 in the step three; S42, using S32 in step three for geomagnetic navigation, first running T pre second pre-search, then global search, and recording the global optimal fitness f gbest during simulation until the geomagnetic navigation phase is completed; S43, landmark navigation with S33 in step three, run T2 times, and record the global optimal fitness f in the simulation process gbest , until the landmark navigation phase is completed; S44, when the landmark navigation phase is completed, the simulation ends, and the final global optimal fitness f is recorded gbest and the global optimal position X gbest .

8. The pigeon hierarchy-based optimization method for tuning the parameters of the bar-armed UAV active disturbance rejection control according to claim 7, characterized in that: The self-disturbance control parameters optimized by the simulation verification of step five include: X gbest The optimal values of ω0, k1, k1 in the angle loop self-disturbance control parameters of the arm unmanned aerial vehicle are transmitted, and the expected pitch angle θ d , roll angle φ d Three angle loop test simulations are performed to verify the actual control effect of the optimized parameters. The output performance optimal active disturbance rejection control parameter of step six includes: the global optimal position X obtained by simulation in step four gbest The optimal values of the bandwidth and control coefficients ω0, k1, k1 of the observer in the angle loop active disturbance rejection control parameter of the winged unmanned aerial vehicle.

Images