Rotor wing flight mechanical arm grabbing trajectory tracking control method

Abstract

The invention discloses a rotor flight mechanical arm grabbing trajectory tracking control method. The method comprises the steps that a mathematical model of a four-rotor unmanned aerial vehicle with a mechanical arm is established; designing a position controller; designing an attitude controller; a mechanical arm controller is designed; and carrying out stability analysis. Motion disturbance of a mechanical arm is regarded as external disturbance, a decoupling control strategy is adopted, a predefined time terminal sliding mode controller is designed in a position system and a mechanical arm system, and an adaptive law is introduced to compensate system disturbance; pre-defined time nonlinear disturbance observation is designed in an attitude system, the influence of mechanical arm disturbance on the system is compensated, a pre-defined time terminal sliding mode controller is designed, it is guaranteed that system tracking errors converge to zero within pre-defined time, the effectiveness of an algorithm is verified through numerical simulation and actual flight, and the system tracking precision is improved. High tracking precision and rapid convergence of the grabbing track of the rotor wing flying mechanical arm are achieved, the state error is controlled to be zero within the predefined time, and the stability of the flying attitude is achieved.

Description

Technical Field

[0001] The invention relates to a method for tracking and controlling a grabbing trajectory of a rotor flight mechanical arm, and belongs to the technical field of unmanned aerial vehicle (UAV) flight control. Background Art

[0002] Quadcopter drones, with their low cost, flexible operation, free hovering, and vertical takeoff and landing, play a key role in disaster relief, environmental monitoring, power inspections, and agricultural and forestry plant protection. While quadcopter drones offer certain advantages in applications such as inspection, patrol, and remote sensing, they are significantly limited in tasks like contact, gripping, and transportation. Robotic arms, due to their high flexibility and ability to directly interact with their environment, are widely used in agricultural harvesting and industrial handling. To expand the application range of quadcopter drones and enable them to interact with their environment, quadcopter drones with robotic arms have emerged. These drones consist of a drone and a multi-degree-of-freedom robotic arm or gripper, enabling active interactive operations.

[0003] Quadrotors equipped with robotic arms can perform complex tasks such as grasping and releasing, and their end effectors can be designed in a variety of ways based on task requirements, enhancing the system's flexibility and adaptability. In industrial production, quadrotors equipped with robotic arms can perform inspections and maintenance in hazardous environments such as high altitude, high pressure, and high temperature. This not only improves work efficiency and quality, but also prevents workers from being exposed to dangerous environments and ensures their personal safety. In disaster relief, quadrotors equipped with robotic arms can quickly reach disaster sites to assist in searching and rescuing trapped people, clearing obstacles, and providing relief supplies, thereby improving rescue efficiency and reducing the risk of casualties. In agriculture, quadrotors equipped with robotic arms can perform operations such as tree pruning, fruit bagging, and harvesting, reducing both manpower and material costs while also increasing the yield and quality of agricultural products.

[0004] While the introduction of a robotic arm expands the application scope of UAV systems, it also increases the control complexity of these systems. This composite system exhibits the inherent nonlinearity and underactuation of UAVs. Furthermore, due to the coordinated motion of the robotic arm, the system dynamics exhibit time-varying inertia and strong dynamic coupling. Furthermore, during actual operation, the system must continuously withstand time-varying external disturbances such as wind field disturbances and sudden load changes. These factors contribute to uncertainty in the system dynamics model and increase the difficulty of controller design. For quadrotor UAVs with robotic arms, composite system control strategies based on a separate control architecture are often used. These strategies independently control the UAV and robotic arm, treating disturbances caused by the robotic arm's motion as external disturbances. This controller design overcomes the impact of the robotic arm's motion on the system and enhances system stability. With the rapid development of control and robotics technologies, higher requirements are being placed on the dynamic performance of UAV systems with robotic arms, particularly in terms of control accuracy and response speed. Currently, asymptotically convergent control methods are the predominant approach. However, these methods suffer from inherent drawbacks such as slow convergence, dependence on the system's initial state, and complex parameter tuning, making them difficult to meet the convergence time requirements of practical applications. Summary of the Invention

[0005] In response to the problems existing in the above-mentioned prior art, the present invention provides a rotor flight manipulator grasping trajectory tracking control method, which can achieve high tracking accuracy and rapid convergence of the rotor flight manipulator grasping trajectory, improve the anti-interference performance of the system, and control the state error to zero within a predefined time, thereby quickly achieving flight attitude stabilization.

[0006] In order to achieve the above object, the technical solution adopted by the present invention is: a method for tracking and controlling a grasping trajectory of a rotorcraft flight manipulator, comprising the following steps:

[0007] S1. Establish a mathematical model of a quadrotor drone with a robotic arm;

[0008] S2. Design position controller;

[0009] S3. Design attitude controller;

[0010] S4. Design the robot controller;

[0011] S5. Perform stability analysis.

[0012] Furthermore, the specific process of S1 is as follows:

[0013] S1.1. During motion, a quadrotor drone with a robotic arm is subject to the coupling influence of multiple complex factors, including the system's inherent dynamic coupling, external wind disturbances and aerodynamic effects, and changes in inertial parameters and torque caused by the robotic arm's motion. To accurately describe and analyze the kinematic characteristics of the quadrotor drone system, simplify the analysis, and ensure model accuracy, the coordinate systems of the drone body and the robotic arm are set at the locations of their corresponding rigid body centers of mass.

[0014] S1.2. The angles formed by the rotation around the X-axis, Y-axis, and Z-axis in the body coordinate system are the roll angle φ, pitch angle θ, and yaw angle ψ of the quadrotor drone, respectively. The ranges of the roll angle φ and pitch angle θ are (-0.5π, 0.5π), and the range of the yaw angle ψ is (-π, π). The inertial coordinate system is I(O I X I Y I Z I ), the body coordinate system is b(O b X b Y b Z b ), and the origin of the body coordinate system is at the center of mass of the drone; the coordinate system of the robot arm joint 1 is O1, and the coordinate system of the robot arm joint 2 is O2;

[0015] S1.3. Define the joints of the robot arm. Their positions in the geodetic coordinate system are:

[0016]

[0017] in is the center of mass position of the i-th robotic arm, is the position of the center of mass of the UAV, is the rotation transformation matrix between the drone coordinate system and the inertial system;

[0018] S1.4. The spatial position of the manipulator is uniquely determined by the lengths of its links and the rotation angles of its joints. Using homogeneous coordinate transformation and the DH parameter method, the linear velocity of any joint of the manipulator in the inertial coordinate system is:

[0019]

[0020] in is the position of joint i in the inertial coordinate system, J( t,i ) is the Jacobian matrix, q=[q1,q2] T is the rotation angle of the robot arm, ζ=[φ,θ,ψ] T is the posture of the quadrotor drone;

[0021] S1.5. The kinematic relationship of a quadrotor drone with a robotic arm is:

[0022]

[0023] in, is the angular velocity vector of the i-th joint in the body coordinate system;

[0024] S1.6. The external forces acting on the quadrotor system with arms in the non-contact state are:

[0025]

[0026] Among them F t is the lift of the system, m s =m b +m ra is the total mass of the system, where m b is the mass of the drone, m ra is the mass of the robotic arm, e z =[0,0,1] T ;

[0027] S1.7. External torque on a quadrotor drone in non-contact state for;

[0028]

[0029] where τ q is the torque of the robotic arm acting on the drone, B r oc The center of gravity of the system;

[0030] S1.8. The system dynamics equation of a quadrotor drone with a robotic arm is:

[0031]

[0032] Where U1 is the total thrust of the actuator in the body coordinate system; F dis and τ dis They are the interference caused by the movement of the robotic arm on the UAV, the external disturbance to the UAV, and the coupling interference between the systems; d p =[d x ,d y ,d z ] T with d a =[d φ ,d θ ,d ψ ] T They represent the external disturbances to the UAV position subsystem and attitude subsystem respectively; k p =diag(k x ,k y ,k z ) and ka =diag(k φ ,k θ ,k ψ ) is the resistance coefficient. b =diag(I x ,I y ,I z ) is the inertia matrix and I x ,I y ,I z Indicates the moment of inertia of the body around the x, y, and z axes, U ζ =[U2,U3,U4] T represents the input torque; For drones in O b Angular velocity of the coordinate system; B τ dis is the time-varying moment of inertia of the system; is the change in the moment of inertia of the system; r o is the origin position vector of the body coordinate system; r oc is the position of the center of gravity of the system in the inertial coordinate system.

[0033] S1.9. The dynamic model of the quadrotor drone system with a robotic arm is:

[0034]

[0035] Among them U ζ =[U2,U3,U4] T is the input torque; is the positive definite inertia matrix of the manipulator; is the Coriolis force and centrifugal force matrix; is the gravity vector; u q ∈R n is the control torque of the robot joint; d q =[d7,d8] T It represents the comprehensive interference caused by the motion of the quadrotor drone body and external environmental disturbance factors on the first and second links of the robotic arm.

[0036] S1.10. Arrange formula (7) to obtain:

[0037]

[0038] where τ ξ =[U1(cosφsinθcosψ+sinφsinψ) / m s ,U1(cosφsinθcosψ-sinφsinψ) / m s ,U1(cosφsinθ) / m s ]T , F d =F dis / m s , d p =-M -1 (q)d q .

[0039] Furthermore, the specific process of S2 is as follows:

[0040] S2.1. For the quadrotor UAV system with a robotic arm obtained in S1, assume that the disturbance F d is a bounded value, that is, there is a constant F m Satisfy ||F d ||<F m , then the dynamic model of the position system of the quadrotor drone with a robotic arm is:

[0041]

[0042] S2.2. Define the tracking error of the position system as E ξ :

[0043] E ξ =ξ-ξ d , (10)

[0044] where ξ d =[x d ,y d ,z d ] T is the desired position, and the derivative of formula (10) is:

[0045]

[0046] S2.3. The predefined time terminal sliding surface is:

[0047]

[0048] where γ ξ is a positive parameter and satisfies 1 / 2<γ ξ <1, I n is the n×n identity matrix, T sξ >0 is a predefined time parameter; taking its derivative, we get:

[0049]

[0050] S2.4. The predefined time control law of the position system is:

[0051] τ ξ =τ ξ1 +τ ξ2 +τξ3 , (14)

[0052] where τ ξ1 is the equivalent control term, τ ξ2 is the arrival control term, τ ξ3 is the adaptive disturbance compensation term;

[0053] According to formula (9), τ ξ1 Designed to:

[0054]

[0055] τ ξ2 Designed to:

[0056]

[0057] In order to improve the stability of the system, τ ξ3 Designed to:

[0058]

[0059] where σ ξ >0, is the interference estimate;

[0060] S2.5. Define the adaptive update law as:

[0061]

[0062] where β ξ >0,ω ξ >0.

[0063] Furthermore, the specific process of S3 is as follows:

[0064] S3.1. Assume that the disturbance τ to which the UAV is subjected is dis is a bounded value, that is, there is a constant d ζ Satisfy ||τ dis ||<d ζ , for the quadrotor UAV system with a robotic arm in formula (8), its dynamic model is:

[0065]

[0066] S3.2. Based on the attitude system model of formula (19), let The predefined time disturbance observer is designed as:

[0067]

[0068] in for The state estimation, is τdis Interference estimation, γ ζ1 is a positive parameter and satisfies 1 / 2<γ ξ <1, T ζ1 is a predefined time parameter,

[0069] S3.3. Tracking error E of attitude system ζ Defined as:

[0070] E ζ =ζ d -ζ, (21)

[0071] Among them d is the desired attitude angle, and taking its derivative, we get:

[0072]

[0073] S3.4. Predefined time terminal sliding surface s ζ for:

[0074]

[0075] where γ ζ is a positive constant, satisfying 1 / 2<γ ζ <1, T sζ is a predefined time parameter, and taking its derivative, we get:

[0076]

[0077] S3.5. The predefined time control law of the attitude system is designed as follows:

[0078]

[0079] in is an adaptive parameter, and its update law is designed as:

[0080]

[0081] Where σ1,τ2>0.

[0082] Furthermore, the specific process of S4 is as follows:

[0083] S4.1. Assume that the manipulator is subjected to a disturbance d q is a bounded value, that is, there is a constant D q Satisfy ||d q ||<D q , for the quadrotor UAV system with a robotic arm in formula (8), its dynamic model is:

[0084]

[0085] S4.2. Tracking error E of the robot arm q Defined as:

[0086] E q =qq d , (28)

[0087] where q d is the desired trajectory of the robot arm, and taking its derivative, we get:

[0088]

[0089] S4.3. Define the time terminal surface s q for:

[0090]

[0091] where γ q is a positive constant and satisfies 1 / 2<γ q <1, T sq is a predefined time parameter, and its derivative is:

[0092]

[0093] S4.4. The predefined time controller is designed as follows:

[0094] u q =u q1 +u q2 +u q3 , (32)

[0095] where u q1 is an equivalent control term, u q2 is the arrival control item, u q3 is the adaptive disturbance compensation term, and the control rate u q1 Designed to:

[0096]

[0097]

[0098] Arrival control quantity u q2 Designed to:

[0099]

[0100] where γ q >0;

[0101] To improve the stability of the system, u q3 Designed to:

[0102]

[0103] where σ q >0, It's D q The interference estimate of

[0104] S4.5. The adaptive update law is designed as:

[0105]

[0106] where β q >0,ω q >0.

[0107] Furthermore, the specific process of S5 is as follows:

[0108] S5.1. For the position system of the quadcopter with a robotic arm in formula (9), based on the predefined time sliding surface of formula (12), design the predefined time controller of formulas (14) to (17) to ensure that the position tracking error E ξ At a predefined time T sξ Inner convergence means that the position system converges within a predefined time T sξ Track the desired trajectory;

[0109] S5.2. Select the Lyapunov function V ξ1 for:

[0110]

[0111] in is the interference estimation error, The derivative is:

[0112]

[0113] Further:

[0114]

[0115] S5.3. Order Simplifying formula (39) we can get:

[0116]

[0117] When μ>1 / 2, Established, we get:

[0118] When ω≥1, we get:

[0119]

[0120] When ω<1, we get:

[0121]

[0122] Combining formula (42) and formula (43), Δ ξ1 for:

[0123]

[0124] have to

[0125]

[0126] S5.4. The system meets the conditions of convergence in a predefined time, that is, the system error E ξ At a predefined time T sξ Inner convergence, when the system reaches the sliding surface s ξ =0, that is:

[0127]

[0128] In summary, when s ξ =0,E ξ At the predefined time T sξ Converges to zero internally;

[0129] S5.5. According to the attitude model in formula (19), the disturbance observer in formula (20), the state estimation error At a predefined time T ζ1 Inner convergence, and the perturbation estimation error Also at the predefined time T ζ1 Converges to zero value;

[0130] S5.6. Select the Lyanov function V D :

[0131]

[0132] in Taking its derivative, we get:

[0133]

[0134] S5.7. Lyapunov function V D System Status At a predefined time T ζ1 Converges to zero, where the state There is an upper bound, namely the perturbation estimation error At a predefined time T ζ1 Converges, so for all t≥T ζ1 , there exists a constant d ζ Make

[0135] S5.8. For the attitude system model of formula (19), the predefined time terminal sliding surface in formula (23) and the predefined time controller in formula (25) ensure that the system attitude tracking error E ζ , at a predefined time T sζ Inner convergence;

[0136] S5.9. Lyapunov function V ζ Select as:

[0137]

[0138] in The derivative is:

[0139]

[0140] S5.10. Rearrange and simplify formula (50) to obtain:

[0141]

[0142] in ω ζ are the parameters to be designed;

[0143] S5.11. Lyapunov function V ζ Satisfying the property of predefined time convergence, the posture tracking error E ζ , at a predefined time T sζ Converges to zero; According to the definition of strong predefined time, the tracking error of the UAV system with a robotic arm is within the predefined time T s =max{T sζ ,T ζ1}converges to zero.

[0144] This invention treats the motion disturbance of the manipulator as external disturbance and adopts a decoupling control strategy. Predefined-time terminal sliding mode controllers are designed for the position system and the manipulator system, and an adaptive law is introduced to compensate for system disturbances. Predefined-time nonlinear disturbance observation is designed for the attitude system to compensate for the impact of the manipulator disturbance on the system. A predefined-time terminal sliding mode controller is designed to ensure that the system tracking error converges to zero within a predefined time. The effectiveness and feasibility of the algorithm are verified through numerical simulation and actual flight. This invention achieves high tracking accuracy and rapid convergence of the grasping trajectory of the rotorcraft manipulator, improves the system's anti-interference performance, and controls the state error to zero within a predefined time, quickly achieving flight attitude stabilization. BRIEF DESCRIPTION OF THE DRAWINGS

[0145] Figure 1 This is a coordinate system diagram of a quadrotor drone with a robotic arm according to the present invention;

[0146] Figure 2 is a tracking response curve diagram of a quadrotor UAV system with a robotic arm in an embodiment of the present invention;

[0147] Figure 3 This is a comparison chart of tracking response curves of a quadrotor UAV system with a robotic arm under the predefined time adaptive terminal sliding mode control method, the finite time control method, and the fixed time control method in an embodiment of the present invention;

[0148] Figure 4 1. This is a comparison chart of tracking error response curves of a quadrotor UAV system with a robotic arm under the predefined time adaptive terminal sliding mode control method, the finite time control method, and the fixed time control method in an embodiment of the present invention;

[0149] Figure 5 1 is a control quantity response comparison curve of a quadrotor UAV system with a robotic arm under the predefined time adaptive terminal sliding mode control method, the finite time control method, and the fixed time control method in an embodiment of the present invention;

[0150] Figure 6 3. This is a graph showing the experimental results of a hovering test using a predefined time adaptive terminal sliding mode control method for a quadrotor drone with a robotic arm according to an embodiment of the present invention;

[0151] Figure 7 3. This is a diagram showing the flight process test results of the predefined time adaptive terminal sliding mode control method for a quadrotor drone with a robotic arm in an embodiment of the present invention;

[0152] Figure 8 This is a graph showing the experimental results of a robotic arm extension and retraction test using a predefined time adaptive terminal sliding mode control method for a quadrotor drone with a robotic arm in an embodiment of the present invention. DETAILED DESCRIPTION

[0153] The present invention will be further described below with reference to the accompanying drawings.

[0154] Example: In order to verify the effectiveness of the control method of the present invention, Figure 1 In the model of the quadrotor UAV system with a robotic arm shown in the figure, the position controller adopts a predefined time adaptive terminal sliding mode controller, the attitude system adopts a predefined time disturbance observer and a composite predefined time terminal sliding mode controller, and the robotic arm system controller adopts a predefined time adaptive terminal sliding mode controller. Simulation verification and actual flight test verification are carried out.

[0155] The present invention builds a simulation model in the Matlab / Simulink environment to simulate the trajectory tracking control of each channel of a four-rotor drone system with a robotic arm, and conducts actual flight tests based on the four-rotor drone with a robotic arm of the present invention. The four-rotor drone system with a robotic arm mainly consists of two parts: a four-rotor drone with a carrying platform and a two-degree-of-freedom robotic arm for performing operational tasks. The four-rotor drone adopts an X-shaped frame with a wheelbase of 360mm, a Langyu 2212 direct brushless motor as the motor, universal propeller blades, a Lotte 60A electronic speed controller, and a 4600mAh lithium battery. The two-degree-of-freedom robotic arm consists of a motor and an acrylic plate.

[0156] The structural parameters of the quadrotor drone with a robotic arm are set to the following: x =I y =I z =1.25kg.m 2 , the mass of the quadcopter is m = 1.8 kg, the distance from the rotor center to the center of mass of the drone is l = 0.22 m, the acceleration of gravity is g = 9.8 N / kg, the mass of joint 1 of the robotic arm is m1 = 0.049 kg, and the arm length is l1 = 0.015 m, the mass of joint 2 is m2 = 0.05 kg, and the arm length is l2 = 0.015 m. The initial position of the drone is (1, 2, 3), and the initial attitude angle of the drone is (0, 0, 0). The desired position of the drone is x d =0.4t,y d =5,z d =10,φ d =π / 36sin(3t+π / 6),θ d =0.5sin(2t),ψ d =0.3cos(2.1t). The parameters of the predefined time sliding mode controller of the UAV position system with a robotic arm are set to γ x =γ y =γ z =0.55,ω x =ω y =ω z =0.7, μ x =μ y =μ z =2,k x =k y =k z =3,σ x =σ y =σ z = 0.07. The parameters of the predefined time sliding mode controller of the attitude system of the UAV with a robotic arm are set as γ φ =γ θ =γ ψ=0.7, σ1=1.7, σ2=3.4. The initial position of the manipulator is q1=1, q2=1.5, and the expected trajectory of the manipulator is q d1 =1.25-7 / 5e -t +7 / 20e -4t ,q d2 =1.25+e -t -1 / 16e -4t The parameters of the predefined time sliding mode controller of the manipulator are set to γ q1 =γ q2 =0.8,ω q1 =ω q2 =1.2, μ q1 =μ q2 =2,σ q1 =σ q2 =0.04, k φ =k θ =2.

[0157] like Figure 2 The figure shows a quadrotor drone system with a robotic arm at a predefined time T. s =Trajectory tracking response curve when =0.8s. Figure 2 (a) is the position tracking response curve, (b) is the attitude angle tracking response curve, and (c) is the manipulator tracking response curve; among them, the black solid line is the desired trajectory, and the red dotted line is the actual trajectory. It can be seen from the simulation results that the actual trajectory of the system of the present invention can track the desired trajectory within 0.6 seconds, and the trajectory tracking performance of the manipulator is significantly better than that of the position loop and attitude loop control. The manipulator can quickly track the desired trajectory within 0.1 seconds and maintain sub-millimeter tracking accuracy, which reflects the robustness and stability of the control algorithm of the present invention.

[0158] The present invention conducts comparative experiments on the designed controller with finite time control and fixed time control. Figure 3 (a), (b), and (c) are the position tracking response curves, attitude angle tracking response curves, and manipulator tracking response curves under the predefined time adaptive terminal sliding mode control method, finite time control method, and fixed time control method, respectively; Figure 4 (a), (b), and (c) are the response curves of trajectory tracking errors under predefined time adaptive terminal sliding mode control, finite time control, and fixed time control. From the experimental results, it can be seen that although these three controllers can all achieve finite time trajectory tracking control of a quadrotor UAV system with a robotic arm, the predefined time control method designed in this invention has faster tracking speed, higher tracking accuracy, and shorter error convergence time compared with fixed time and finite time control.

[0159] To further evaluate the tracking performance of the system under different controllers, the present invention uses mean absolute error (MAE) and root mean square error (RMSE) as system evaluation indicators. Table 1 shows the results of the tracking error performance indicators of each controller:

[0160] Table 1

[0161]

[0162] The experimental data above show that compared with the finite-time control method, the controller of the present invention reduces the tracking error (MAE) by 12.9% to 98.6% and the RMSE by 2.2% to 85.44%. Compared with the fixed-time control method, the MAE is reduced by 22.6% to 91.2% and the RMSE by 16.7% to 92.8%. The control method proposed in this invention improves the tracking response speed and tracking accuracy of the system.

[0163] Figure 5 (a), (b) and (c) are comparative experiments on the output torque response curves of predefined time control, finite time control and fixed time control. The experimental results show that there is an obvious jitter phenomenon in the output torque of fixed time control and finite time control. From the local magnification of 0 to 1 second, it can be seen that the output torque of the predefined time controller fluctuates at the initial moment of the system, but compared with the torque output structure of finite time and fixed time, the torque output of the predefined time is stable throughout the entire process of system operation, and can ensure the stable operation of the system. The experimental results show that the predefined time controller proposed in the present invention not only improves the anti-interference ability of the system, but also enhances the robustness of the system, ensuring that the system can achieve fast and accurate trajectory tracking control. This feature enables the four-rotor drone with a robotic arm in the present invention to perform tasks in complex environments, and provides theoretical support for the practical application of drone target capture.

[0164] Figure 6 These are the hovering stability test results for a quadrotor drone with a robotic arm. This test verified the control performance of the designed controller during takeoff, hovering, and landing. During the test, the drone completed a smooth takeoff and entered a hovering state. The host computer data acquisition system monitored the changes in the drone's attitude in real time. The experimental results showed that during the hovering phase, the drone was able to maintain a stable flight state, with roll angle fluctuations within ±0.5°, yaw angle fluctuations within ±0.3°, and pitch angle fluctuations within ±0.8°. The errors in each attitude angle were all controlled within 1°. This met the test requirements and verified the stability of the controller.

[0165] Figure 7This test involved a quadrotor drone with a robotic arm flying vertically to a height of 1 meter and then performing a steady flight at that target altitude. Simultaneously, real-time data collection and recording of the drone's attitude was used to assess system stability. Analysis of the experimental data showed that the quadrotor drone system with a robotic arm was able to reach the desired altitude within the specified timeframe and adjust its attitude angles to maintain the desired tracking angle. While the roll and pitch angles fluctuated slightly during the test, no significant buffeting was observed, meeting the mission requirements.

[0166] Figure 8 These are the results of a hovering robotic arm extension and retraction test. During the test, the drone first flew to a height of 1 meter and maintained this stable position. The robotic arm was then controlled to perform an extension and retraction motion while simultaneously collecting and recording real-time body posture data to assess system stability. Analysis of the experimental data shows that during the robotic arm movement, the drone's pitch angle fluctuated within a range of approximately ±0.8, which is normal and does not affect overall system stability. The roll and pitch angle channels exhibited slight fluctuations, but no noticeable jitter was observed. The test results confirm that the drone of the present invention maintains excellent hovering stability during robotic arm movement, fully meeting mission requirements.

Claims

1. A method for tracking and controlling a grasping trajectory of a rotorcraft manipulator, characterized in that: The steps include: S1. Establish a mathematical model of a quadrotor drone with a robotic arm; S2. Design position controller; S3. Design attitude controller; S4. Design the robotic arm controller; S5. Perform stability analysis.

2. The method for tracking and controlling the grasping trajectory of a rotorcraft flight manipulator according to claim 1, wherein: The specific process of S1 is as follows: S1.

1. To accurately describe and analyze the kinematic characteristics of the quadrotor UAV system, simplify the analysis, and ensure model accuracy, the coordinate systems of the UAV body and the robotic arm are set at the centers of mass of their respective rigid bodies. S1.

2. The angles formed by the rotation around the X-axis, Y-axis, and Z-axis in the body coordinate system are the roll angle φ, pitch angle θ, and yaw angle ψ of the quadrotor drone, respectively. The ranges of the roll angle φ and pitch angle θ are (-0.5π, 0.5π), and the range of the yaw angle ψ is (-π, π). The inertial coordinate system is I(O I X I Y I Z I ), the body coordinate system is b(O b X b Y b Z b ), and the origin of the body coordinate system is at the center of mass of the drone; the coordinate system of the robot arm joint 1 is O1, and the coordinate system of the robot arm joint 2 is O2; S1.

3. Define the joint i of the manipulator, i = 1, 2, and the position of the earth coordinate system is: in is the center of mass position of the i-th robotic arm, is the position of the center of mass of the UAV, is the rotation transformation matrix between the drone coordinate system and the inertial system; S1.

4. Using homogeneous coordinate transformation and the DH parameter method, the linear velocity of any joint of the manipulator in the inertial coordinate system is: in is the position of joint i in the inertial coordinate system, J (t,i) is the Jacobian matrix, q=[q1,q2] T is the rotation angle of the robot arm, ζ=[φ,θ,ψ] T is the posture of the quadrotor drone; S1.

5. The kinematic relationship of a quadrotor drone with a robotic arm is: in, is the angular velocity vector of the i-th joint in the body coordinate system; S1.

6. The external forces acting on the quadrotor system with arms in the non-contact state are: Among them F t is the lift of the system, m s =m b +m ra is the total mass of the system, where m b is the mass of the drone, m ra is the mass of the robotic arm, e z =[0,0,1] T ; S1.

7. External torque on a quadrotor drone in non-contact state for; where τ q is the torque of the robotic arm acting on the drone, B r oc The center of gravity of the system; S1.

8. The system dynamics equation of a quadrotor drone with a robotic arm is: Where U1 is the total thrust of the actuator in the body coordinate system; F dis and τ dis They are the interference caused by the movement of the robotic arm on the UAV, the external disturbance to the UAV, and the coupling interference between the systems; d p =[d x ,d y ,d z ] T with d a =[d φ ,d θ ,d ψ ] T They represent the external disturbances to the UAV position subsystem and attitude subsystem respectively; k p =diag(k x ,k y ,k z ) and k a =diag(k φ ,k θ ,k ψ ) is the resistance coefficient, I b =diag(I x ,I y ,I z ) is the inertia matrix and I x ,I y ,I z Indicates the moment of inertia of the body around the x, y, and z axes, U ζ =[U2,U3,U4] T represents the input torque; For drones in O b Angular velocity of the coordinate system; B τ dis is the time-varying moment of inertia of the system; is the change in the moment of inertia of the system; r o is the origin position vector of the body coordinate system; r oc is the position of the center of gravity of the system in the inertial coordinate system; S1.

9. The dynamic model of the quadrotor drone system with a robotic arm is: Among them U ζ =[U2,U3,U4] T is the input torque; is the positive definite inertia matrix of the manipulator; is the Coriolis force and centrifugal force matrix; is the gravity vector; u q ∈R n is the control torque of the robot arm joint; d q =[d7,d8] T It represents the comprehensive interference caused by the quadrotor UAV's main body motion and external environmental disturbance factors on the first and second links of the robotic arm; S1.

10. Arrange formula (7) to obtain: among them ξ =[U1(cosφsinθcosψ+sinφsinψ) / m s ,U1(cosφsinθcosψ-sinφsinψ) / m s ,U1(cosφsinθ) / m s ] T ,F d =F dis / m s , d p =-M -1 (q)d q 。 3. The method for tracking and controlling the grasping trajectory of a rotorcraft flight manipulator according to claim 2, wherein: The specific process of S2 is as follows: S2.

1. For the quadrotor UAV system with a robotic arm obtained in S1, assume that the disturbance F d is a bounded value, that is, there is a constant F m Satisfy ||F d ||<F m , then the dynamic model of the position system of the quadrotor drone with a robotic arm is: S2.

2. Define the tracking error of the position system as E ξ : E ξ =ξ-ξ d , (10) where ξ d =[x d ,y d ,z d ] T is the desired position, and the derivative of formula (10) is: S2.

3. The predefined time terminal sliding surface is: where γ ξ is a positive parameter and satisfies 1 / 2<γ ξ <1, I n is the n×n identity matrix, T sξ >0 is a predefined time parameter; Taking its derivative, we get: S2.

4. The predefined time control law of the position system is: t ξ =t ξ1 +t ξ2 +t ξ3 , (14) where τ ξ1 is the equivalent control term, τ ξ2 is the arrival control term, τ ξ3 is the adaptive disturbance compensation term; According to formula (9), τ ξ1 Designed to: τ ξ2 Designed to: In order to improve the stability of the system, τ ξ3 Designed to: where σ ξ >0, is the interference estimate; S2.

5. Define the adaptive update law as: among themb ξ >0,ω ξ >0.

4. The method for tracking and controlling the grasping trajectory of a rotorcraft flight manipulator according to claim 3, wherein: The specific process of S3 is as follows: S3.

1. Assume that the disturbance τ to which the UAV is subjected is dis is a bounded value, that is, there is a constant d ζ Satisfy ‖τ dis ||<d ζ , for the quadrotor UAV system with a robotic arm in formula (8), its dynamic model is: S3.

2. Based on the attitude system model of formula (19), let The predefined time disturbance observer is designed as: in for The state estimation, is τ dis Interference estimation, T ζ1 is the predefined time parameter, γ ζ1 is a positive parameter and satisfies 1 / 2<γ ξ <1, S3.

3. Tracking error E of attitude system ζ Defined as: E ζ =ζ d -ζ, (21) Among them d is the desired attitude angle, and taking its derivative, we get: S3.

4. Predefined time terminal sliding surface s ζ for: where γ ζ is a positive constant, satisfying 1 / 2<γ ζ <1, T sζ is a predefined time parameter, and taking its derivative, we get: S3.

5. The predefined time control law of the attitude system is designed as follows: in is an adaptive parameter, and its update law is designed as: Where σ1,σ2>0.

5. The method for tracking and controlling the grasping trajectory of a rotorcraft flight manipulator according to claim 4, characterized in that: The specific process of S4 is as follows: S4.

1. Assume that the manipulator is subjected to a disturbance d q is a bounded value, that is, there is a constant D q Satisfy ||d q ||<D q , for the quadrotor UAV system with a robotic arm in formula (8), its dynamic model is: S4.

2. Tracking error E of the robot arm q Defined as: E q =q-q d , (28) where q d is the desired trajectory of the robot arm, and taking its derivative, we get: S4.

3. Define the time terminal surface s q for: where γ q is a positive constant and satisfies 1 / 2<γ q <1, T sq is a predefined time parameter, and its derivative is: S4.

4. The predefined time controller is designed as follows: in q =in q1 +in q2 +in q3 , (32) where u q1 is an equivalent control term, u q2 is the arrival control item, u q3 is the adaptive disturbance compensation term, and the control rate u q1 Designed to: Arrival control quantity u q2 Designed to: where γ q >0; To improve the stability of the system, u q3 Designed to: where σ q >0, It's D q The interference estimate of S4.

5. The adaptive update law is designed as: among themb q >0,oh q >0.

6. The method for tracking and controlling the grasping trajectory of a rotorcraft manipulator according to claim 5, characterized in that: The specific process of S5 is as follows: S5.

1. For the position system of the quadcopter with a robotic arm in formula (9), based on the predefined time sliding surface of formula (12), design the predefined time controller of formulas (14) to (17) to ensure that the position tracking error E ξ At a predefined time T sξ Inner convergence means that the position system converges within a predefined time T sξ Track the desired trajectory; S5.

2. Select the Lyapunov function V ξ1 for: in is the interference estimation error, The derivative is: Further: S5.

3. Order and Simplifying formula (39) we can get: When μ>1 / 2, Established, we get: When ω≥1, we get: When ω<1, we get: Combining formula (42) and formula (43), Δ ξ1 for: have to S5.

4. The system meets the conditions of convergence in a predefined time, that is, the system error E ξ At a predefined time T sξ Inner convergence, when the system reaches the sliding surface s ξ =0, that is: In summary, when s ξ =0,E ξ At the predefined time T sξ Converges to zero internally; S5.

5. According to the attitude model in formula (19), the disturbance observer in formula (20), the state estimation error At a predefined time T ζ1 Inner convergence, and the perturbation estimation error Also at the predefined time T ζ1 Converges to zero value; S5.

6. Select the Lyanov function V D : in Taking its derivative, we get: S5.

7. Lyapunov function V D System Status At a predefined time T ζ1 Converges to zero, where the state There is an upper bound, namely the perturbation estimation error At a predefined time T ζ1 Converges, so for all t≥T ζ1 , there exists a constant d ζ Make S5.

8. For the attitude system model of formula (19), the predefined time terminal sliding surface in formula (23) and the predefined time controller in formula (25) ensure that the system attitude tracking error E ζ , at a predefined time T sζ Inner convergence; S5.

9. Lyapunov function V ζ Select as: in The derivative is: S5.

10. Rearrange and simplify formula (50) to obtain: in ω ζ are the parameters to be designed; S5.

11. Lyapunov function V ζ Satisfying the property of predefined time convergence, the posture tracking error E ζ , at a predefined time T sζ Converges to zero; According to the definition of strong predefined time, the tracking error of the UAV system with a robotic arm is within the predefined time T s =max{T sζ ,T ζ1 }converges to zero.

Images