CN116714780A - A planning and control method for a rotor flying mechanical arm and rapid grabbing in the air - Google Patents

Abstract

The invention discloses a rotor flying mechanical arm and a planning and control method for rapid grabbing in the air, wherein the rotor flying mechanical arm comprises an unmanned aerial vehicle frame, a controller, an onboard computer and a rotor flying mechanical arm; the on-board computer comprises a control system for controlling the rotor flying mechanical arm; the control system comprises a motion capture system, a tracking calculation system, a PD controller and a PID controller of the rotor flying mechanical arm; the controller comprises a centroid controller and a mechanical arm joint controller; the rotor flying mechanical arm comprises a plurality of joints, and each joint is provided with a joint motor and a disturbance observer; the control method is used for planning the movement of the rotor flying mechanical arm and tracking and controlling the rotor flying mechanical arm to execute the grabbing task; the control system can improve the control precision of the rotor flying mechanical arm, has lower deployment difficulty, and can enable the rotor flying mechanical arm to have the capability of executing high-precision quick grabbing tasks in an obstacle environment.

Description

A planning and control method for a rotor flying mechanical arm and rapid grabbing in the air

Technical field

The invention belongs to the field of UAV control, and specifically relates to a rotor flying mechanical arm and a planning and control method for rapid grabbing in the air.

Background technique

In recent years, as the technologies related to rotary-wing UAV systems continue to mature, more and more rotary-wing UAVs have been put into production and life, playing a significant role. Their applications are mainly limited to aerial photography, monitoring, remote sensing mapping, etc. field. For scenarios such as high-voltage power grids and bridge building maintenance, contact inspection of factory equipment, and high-altitude sample collection, which require aerial operations, some scholars and researchers install actuators such as clamps or vacuum suction cups directly or indirectly through robotic arms, and design Different forms of rotor flying robotic arm systems have been developed.

The patent with the publication number "CN110641738A" discloses a trajectory tracking control method for a five-degree-of-freedom free-flying manipulator in space, which can solve the problem of the accuracy of capturing and manipulating space objects by a five-degree-of-freedom space free-flying manipulator installed on a spacecraft. Question: This patent uses kinematics and dynamics equations and applies the Jacobian transpose matrix combined with the PD controller to control the trajectory of the robotic arm. However, the method proposed in this patent has high requirements for modeling accuracy and is very difficult to deploy for complex flying manipulator systems, and the PD controller cannot achieve high-precision tracking in practical applications.

Contents of the invention

The purpose of the present invention is to provide a rotary-wing robotic arm and a planning and control method for rapid grabbing in the air, so that the rotary-wing robotic arm has the ability to perform rapid grabbing tasks in an obstacle environment.

In order to achieve the above-mentioned object of the invention, the technical solutions adopted by the present invention are as follows:

A rotor flying mechanical arm, including a drone frame, a controller, an airborne computer, and a rotor flying mechanical arm; the airborne computer includes a control system for controlling the rotor flying mechanical arm; the control system includes motion capture system, a tracking solution system, a PD controller and a PID controller of a rotary-wing robotic arm; the controller includes a center of mass controller and a robotic arm joint controller; the rotary-wing robotic arm also includes a plurality of joints, and each Joints are equipped with joint motors and disturbance observers;

The control system is used to generate control instructions and send them to the center of mass controller and the manipulator joint controller respectively;

The tracking and solving system is used to track the center of mass controller and the robotic arm joint controller to obtain trajectory information of the rotor flight robotic arm;

The center of mass controller is used to receive control instructions sent by the control system and control the center of mass movement of the rotor flight mechanical arm according to the control instructions; the center of mass controller includes a center of mass position loop and a center of mass speed loop;

The robotic arm joint controller is used to receive control instructions sent by the control system, and control the joint angle motion of the rotor flight robotic arm according to the control instructions; the robotic arm joint controller includes a joint angle acceleration controller.

A rotor flying mechanical arm of the present invention controls the mass center movement and joint angle movement of the rotor flying mechanical arm through a center of mass controller and a mechanical arm joint controller respectively, and at the same time controls the movement of the rotor flying mechanical arm in real time through a tracking and solving system. Track the solution, and then the control system further controls the center of mass controller and the robotic arm joint controller by tracking the solution results to improve the control accuracy and grabbing speed of the rotor flight robotic arm. This control system is less difficult to deploy, and It enables the rotor flight manipulator to have high-precision and fast grabbing capabilities.

The present invention also provides a planning and control method for rapid mid-air grabbing of a rotor flying mechanical arm, which is applied to the above-mentioned rotary wing flying mechanical arm; the control method includes the following steps:

Step S1. Establish the global inverse kinematics algorithm of the rotor flight manipulator and calculate the system state of the control system;

Step S2. Based on the system status of the control system, establish a motion planning algorithm for the rotor flight robotic arm, and plan to obtain the center of mass trajectory and joint angle trajectory of the rotor flight robotic arm;

Step S3. The control system sends control instructions to the center of mass controller and the joint controller of the rotor flying manipulator to control the rotor flying manipulator to perform the grabbing task according to the planned mass center trajectory and joint angle trajectory. At the same time, the tracking and solving system is established. Rotary-wing manipulator trajectory tracking algorithm, and tracking and controlling the rotary-wing manipulator to continue to perform the grabbing task.

Preferably, the step S1 includes the following steps:

Step S1.1. Obtain the position of the rotation coordinate system of each joint of the rotor flight manipulator in the world coordinate system and the corresponding rotation matrix, and constrain it to multiple optimization variables;

Step S1.2. Perform a dot product of multiple optimization variables to obtain a constraint equation containing quadratic terms of the optimization variables; the constraint equation includes a non-convex constraint equation;

Step S1.3. Perform a linear approximation to the non-convex constraint equation to obtain the mixed integer quadratic constraint quadratic optimization equation and solve it to obtain the system state of the control system of the rotor flight manipulator.

Preferably, the control instructions include multiple center of mass adjustment instructions and multiple joint angle adjustment instructions; the multiple center of mass adjustment instructions include a first center of mass adjustment instruction and a second center of mass adjustment instruction; the multiple joint angle adjustment instructions include The first joint angle adjustment instruction and the second joint angle adjustment instruction; the system state includes the system state at the initial time, the system state at the grabbing time, the system state at the end time, and the center of mass state of the rotor flight manipulator; the system state at the grabbing time It includes the joint angle of the rotary-wing robotic arm at the grabbing moment; a set of initial values that satisfies the grasping posture of the end-effector of the rotary-wing robotic arm is obtained through the system state at the initial time.

Preferably, step S2 includes the following steps of planning the center of mass trajectory of the rotary-wing robotic arm:

Step S2.1a. Based on the system state at the initial time and the system state at the grabbing time, perform path sampling on the rotor flight manipulator to obtain multiple sampling points;

Step S2.2a. Construct a flight corridor and preprocess the map where the rotor flight robotic arm is located to obtain a safe area near each sampling point;

Step S2.3a. Obtain the intersection areas of each two adjacent safety areas to obtain multiple intersections, and summarize them to obtain the spatial envelope;

Step S2.4a. Establish an optimized trajectory that minimizes trajectory energy in the space envelope, obtain a polynomial trajectory segmented and continuously formed by multiple sampling points, and plan the centroid trajectory of each joint of the rotor flight manipulator arm;

The step S2 also includes the following steps of planning the joint angle trajectory of the rotor flying mechanical arm:

Step S2.1b. Based on the joint angles of the rotor flight manipulator at the grabbing moment, calculate multiple sets of joint angles that satisfy the grabbing posture of the end effector;

Step S2.2b. Use the artificial potential field method to apply virtual repulsion to each joint of the rotor flying mechanical arm;

Step S2.3b. Based on the calculated multiple sets of joint angles and the applied virtual repulsive forces, the joints of the rotor flight robotic arm are planned to obtain the joint angle trajectories of each joint of the rotor flight robotic arm.

Preferably, the step S3 includes the following steps:

Step S3.1. The control system sends the first center of mass adjustment instruction to the center of mass controller and the first joint angle adjustment instruction to the manipulator joint controller respectively, and controls the rotor flight manipulator to perform grasping according to the planned mass center trajectory and joint angle trajectory. Get tasks;

Step S3.2. The tracking solution system establishes a trajectory tracking algorithm for the rotor flight manipulator, and tracks the center of mass controller and the manipulator joint controller when performing the grabbing task to obtain the trajectory information of the rotor flight manipulator;

Step S3.3. The control system generates a second center of mass adjustment instruction based on the trajectory information and sends it to the center of mass controller, and generates a second joint angle adjustment instruction and sends it to the manipulator joint controller;

Step S3.4. The center of mass controller controls the center of mass movement of the rotor flight mechanical arm according to the second center of mass adjustment instruction, and at the same time, the robot arm joint controller controls the joint angle movement of the rotor flight mechanical arm according to the second joint angle adjustment instruction;

Step S3.5. The rotary-wing manipulator continues to perform the grabbing task, and the trajectory tracking algorithm continues to track the center of mass controller and the manipulator joint controller to obtain the trajectory information of the rotor-flying manipulator, and then returns to step S3.3.

Preferably, the step S3.3 also includes the following steps:

Step S3.3.1: Obtain the expected trajectory of the UAV platform by analyzing the trajectory information of the rotor flight manipulator; the motion capture system collects the position information of the UAV platform;

Step S3.3.2: Real-time analysis of the expected trajectory of the UAV platform and the position information of the UAV platform to obtain error information, and generate a second center of mass adjustment instruction based on the error information and send it to the center of mass controller, and generate a second joint angle adjustment instruction Sent to the robot arm joint controller.

Preferably, in step S1.1, the position of the rotational coordinate system of each joint of the rotor flight manipulator arm in the world coordinate system and the corresponding rotation matrix are obtained, and constrained to multiple optimization variables, where the constraints on the rotation matrix include Orthogonal constraints;

Assume that the rotor flight manipulator includes n joints, and the rotation matrix C of the i-th joint of the rotor flight manipulator is orthogonally constrained as an optimization variable to obtain the rotation matrix C;

In step S1.2, a dot product of multiple optimization variables is performed to obtain a constraint equation containing the quadratic term of the optimization variable. The dot product of the rotation matrix C of the i-th joint is performed to obtain the following non-convex constraints of the i-th joint. equation:

c ^T _j c _k = 1, j = k

c ^T _j c _k = 0, j≠k;

Where C is the rotation matrix of the i-th joint, c _k is the k-th column vector of the rotation matrix of the i-th joint, c ^T _j is the j-th column vector of the transpose matrix of the rotation matrix of the i-th joint, j= When k, the multiplication of the same column vectors of the rotation matrix C is equal to 1; when j≠k, the multiplication of different column vectors of the rotation matrix C is equal to 0;

In step S1.3, the linear approximation of the non-convex constraint equation of the i-th joint using the 2-norm is as follows:

|c _j +c _k | ² ≤2

|c _j -c _k | ² ≤ 2;

Where |c _j +c _k | ² represents the square of the 2 norm after adding the j-th column vector and the k-th column vector of matrix C, |c _j -c _k | ² represents the j-th column vector of matrix C and the k-th column vector After subtracting k column vectors, take the square of 2 norm.

Preferably, the rotor flight manipulator trajectory tracking algorithm in step S3.2 includes a center of mass trajectory tracking algorithm and a joint angle trajectory tracking algorithm; the second center of mass adjustment instruction in step S3.3 includes a center of mass speed control instruction and a center of mass Acceleration control instruction; the error information in step S3.3.2 includes the center of mass velocity error;

The centroid trajectory tracking algorithm obtains the centroid trajectory information of the centroid controller, and the joint angle trajectory tracking algorithm obtains the joint angle trajectory information of the i-th joint of the robotic arm joint controller;

Assume that steps S3.3 to S3.5 of each cycle are for the center of mass controller and the manipulator joint controller to execute a control period Δt, and assuming the last control time is t ₀ , then in this control time t ₁ :

The center of mass trajectory information includes the center of mass position P ₁ and the center of mass position feedback value P ₂ , then the center of mass position error

e ₁ =P ₁ -P ₂ ;

The center of mass trajectory information also includes the proportional coefficient K ₁ of the center of mass position loop and the center of mass velocity v ₁ , then the center of mass velocity control instruction

v ₂ =K ₁ e ₁ + v ₁ ;

The center of mass trajectory information also includes the center of mass velocity feedback value v ₃ , then the center of mass velocity error

e ₂ = v ₂ - v ₃ ;

The center of mass trajectory information also includes the proportional coefficient K ₂ of the center of mass velocity loop, the differential coefficient K ₃ of the center of mass velocity loop, the center of mass acceleration a ₁ , and the center of mass velocity error e ₀ of the previous control time t ₀ , then the PD controller calculates the center of mass acceleration Control instruction

a ₂ =K ₂ e ₂ +K ₃ (e ₂ -e ₀ )/Δt+a ₁ .

Preferably,

The joint angle trajectory information of the i-th joint also includes the mass m _i of the i-th joint, the rotation matrix R _i , the moment of inertia I _i , the center of mass linear velocity Jacobian matrix J _i , and the angular velocity Jacobian matrix Z _i ; then the center of mass The transpose matrix of the linear velocity Jacobian matrix J _i is J _i ^T , the transpose matrix of the angular acceleration Jacobian matrix Z _i is Z _i ^T , and the transpose matrix of the rotation matrix R _i is R _i ^T ;

The joint angle trajectory information of the i-th joint also includes the joint position B ₁ of the i-th joint, the joint angular velocity B ₂ , the joint angular acceleration B ₃ , and the expected joint position D ₁ and the expected joint angular velocity of the i-th joint. D ₂ , expected joint angular acceleration D ₃ ;

Then the gravity vector of the rotor flight manipulator arm

Where g is the acceleration of gravity, and the gravity vector G of the rotor flight manipulator arm is the product of the following parameters accumulated from the 1st joint to the nth joint of the rotor flight manipulator: mass, transpose matrix of the Jacobian matrix of angular acceleration, gravity acceleration;

Among them, the inertia matrix of the rotor flight manipulator is R ₂ , then

Assuming that the operation period of the disturbance observer of the i-th joint is equal to the control period Δt, then the current control time is t ₁ and the next control time predicted by the disturbance observer is t ₂ ;

The disturbance observer of the i-th joint includes the state vector x ₀ ;

The disturbance observer of the i-th joint also includes the first prediction vector x ₁ , the second prediction vector x ₂ , and the first deviation vector x ₃ when the control time is t ₀ ;

The disturbance observer of the i-th joint also includes the third prediction vector x ₁₁ , the fourth prediction vector x ₂₂ , and the second deviation vector x ₃₃ when the control time is t ₁ ;

The disturbance observer of the i-th joint also includes a first error term coefficient λ ₁ , a second error term coefficient λ ₂ , and a third error term coefficient λ ₃ ;

Then the error of the disturbance observer of the i-th joint when the control time is t ₁

e ₃ =x ₀ -B ₂ ;

Then the disturbance observer of the i-th joint also includes a first error amplification function f ₁ (e ₃ ) and a second error amplification function f ₂ (e ₃ );

Then the control time is the angular acceleration calculated by the disturbance observer of the i-th joint at t ₁

θ=-x ₃ ;

The joint angle trajectory information of the i-th joint also includes the moment coefficient diagonal matrix _R3 and current A of the joint motor of the i-th joint;

Then the dynamic model equation when the robot arm joint controller outputs is

R ₂ *(B ₃ +θ)+G=R ₃ *A;

Then the PID controller calculates the joint position error of the rotor flight manipulator.

e ₄ =(D ₁ -B ₁ );

Then the PID controller calculates the joint angular velocity error of the rotor flight manipulator.

e ₅ =(D ₂ -B ₂ );

The joint angular acceleration of the i-th joint

B ₃ =D ₃ +K _p e ₃ +K _d e ₄ +K _i ∑e ₀ ;

where ∑e ₀ is the sum of all joint position errors calculated by the PID controller calculated by the control system until the last control time t ₀ , K _p is the proportional coefficient of the joint angular acceleration controller, and K _d is the joint angular acceleration controller. Differential coefficient, K _i is the integral coefficient of the joint angular acceleration controller;

Then the acceleration of the rotor flight manipulator arm at control time t ₂

O=B ₃ +θ;

Then the third prediction vector x ₁₁ , the fourth prediction vector x ₂₂ , and the second deviation vector x ₃₃ when the control time predicted by the disturbance observer of the i-th joint is t ₂ satisfy the following equation:

Beneficial effects:

The present invention provides a planning and control method for a rotary wing flying mechanical arm and rapid grabbing in the air. First, the center of mass movement and joint angle movement of the rotary wing flying mechanical arm are planned to solve the obstacle avoidance problem of the rotary wing flying mechanical arm, and the rotary wing flying machinery The center of mass motion and joint angle motion of the arm are controlled respectively by the center of mass controller and the robotic arm joint controller; during the process of the rotor flying robotic arm performing the grabbing task, the tracking solution system is used to track and solve the motion of the rotary wing flying robotic arm in real time. Calculation, and then the control system further controls the center of mass controller and the manipulator joint controller by tracking the calculation results to improve the control accuracy of the rotor flying manipulator. This control system is less difficult to deploy and can make the rotor flying manipulator Ability to perform high-precision and fast grasping tasks in obstacle environments.

Description of the drawings

Figure 1 shows a structural diagram of a rotor flight robotic arm according to Embodiment 1;

Figure 2 shows the overall flow chart of the planning and control method for rapid mid-air grabbing of the rotary-wing manipulator in Embodiment 2;

Figure 3 shows the first sub-flow chart of Figure 2;

Figure 4 shows the second sub-flow chart of Figure 2;

Figure 5 shows the third sub-flow chart of Figure 2;

Figure 6 shows the fourth sub-flow chart of Figure 2;

Figure 7 shows the fifth sub-flow chart of Figure 2;

Figure 8 shows a schematic diagram of the constraint algorithm in step S1.1 of Embodiment 2;

Figure 9 shows a specific flow chart of the motion planning algorithm in step S2 of Embodiment 2;

Figure 10 shows a schematic diagram of the grasping space division in Embodiment 2.

Detailed ways

In order to explain the embodiments of the present invention or technical solutions in the prior art more clearly, the specific implementation modes of the present invention will be described below with reference to the accompanying drawings. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without exerting creative efforts, other drawings can also be obtained based on these drawings, and obtain Other embodiments.

The technical solution of the present invention is introduced in detail below with specific embodiments.

Embodiment 1

As shown in Figure 1, a rotor flying mechanical arm in this embodiment includes a drone frame, a controller, an airborne computer, and a rotor flying mechanical arm; the airborne computer includes a control system for controlling the rotor flying mechanical arm. ; The control system includes a motion capture system, a tracking solution system, a PD controller and a PID controller for the rotary-wing robotic arm; the controller includes a center of mass controller and a robotic arm joint controller; the rotary-wing robotic arm also includes multiple joints, and Each joint is equipped with a joint motor and a disturbance observer;

The control system is used to generate control instructions and send them to the center of mass controller and the manipulator joint controller respectively;

The tracking solution system is used to track the center of mass controller and the robotic arm joint controller to obtain the trajectory information of the rotor flight robotic arm;

The center of mass controller is used to receive control instructions sent by the control system and control the center of mass movement of the rotor flight manipulator according to the control instructions; the center of mass controller includes a center of mass position loop and a center of mass speed loop;

The robot arm joint controller is used to receive control instructions sent by the control system and control the joint angle motion of the rotor flight robot arm according to the control instructions; the robot arm joint controller includes a joint angle acceleration controller.

The rotor flight manipulator also includes a UAV power group; the UAV power group includes ESCs, motors, and propellers.

The rotor flight manipulator also includes a flight controller driven by the UAV power group for flight operations.

Specifically, the rotor flying robot arm of this embodiment is a coaxial eight-blade model with a wheelbase of 550 mm, which can provide sufficient thrust and the torque required for attitude adjustment. The CUAV X7+Pro flight controller and Intel NUC11 onboard computer are installed on the rack. Each rotating joint of the rotor flight manipulator is equipped with a Dynamixel servo. The model of the servo of one of the rotating joints is XM540-W270, and the model of the servos of the remaining rotating joints and the end effector driving gripper is XM430-W350. The components of each joint of the rotor flight manipulator are mainly obtained through carbon fiber plate processing and stereolithography printing. The total weight of the system is about 4.62kg (excluding battery), of which the weight of the robotic arm is about 1kg. The CUAV X7+Pro flight controller uses the STM32H7 series processor, with an operating frequency of up to 480Mhz, which can well meet the computing needs of the flight controller. The flight controller is also equipped with an aerospace-grade accelerometer and gyroscope, providing stable and accurate feedback. It can greatly improve the control effect of posture. The processor version of the onboard computer NUC11 is Intel Core i7-1165G7, with 4 cores and 8 threads, and the maximum core frequency can reach 4.70Ghz. The onboard computer is installed with Ubuntu 18.04 operating system and ROS robot operating system.

Specifically, the rotor flight manipulator of this embodiment also includes an external computing unit. The external computing unit establishes a global inverse kinematics algorithm and a motion planning algorithm. When the external computing unit completes the calculation, the calculation results are copied to the control unit in the form of a file. System execution.

Specifically, the global pose of the UAV platform is obtained by the OptiTrack motion capture system and Motive software by tracking the preset reflective ball position on the solving system. The calculated results are sent to the onboard computer via the wireless router through the LAN, and the position is The frequency of attitude feedback can reach up to 120Hz.

The rotary-wing mechanical arm of this embodiment controls the mass center movement and joint angle movement of the rotary-wing mechanical arm respectively through the center of mass controller and the mechanical arm joint controller, and simultaneously controls the movement of the rotary-wing mechanical arm in real time through the tracking and solving system. Do tracking calculations, and then the control system further controls the center of mass controller and the robotic arm joint controller through the tracking calculation results to improve the control accuracy and grabbing speed of the rotor flight robotic arm. This control system is less difficult to deploy. And it can enable the rotor flying robot arm to have high-precision and fast grabbing capabilities.

Embodiment 2

As shown in Figures 2 to 6, a planning and control method for rapid mid-air grabbing of a rotary-wing robotic arm in this embodiment is applied to the above-mentioned rotary-wing robotic arm; the control method includes the following steps:

Step S1. Establish the global inverse kinematics algorithm of the rotor flight manipulator and calculate the system state of the control system;

Step S2. Based on the system status of the control system, establish a motion planning algorithm for the rotor flight robotic arm, and plan to obtain the center of mass trajectory and joint angle trajectory of the rotor flight robotic arm;

Step S3. The control system sends control instructions to the center of mass controller and the joint controller of the rotor flying manipulator to control the rotor flying manipulator to perform the grabbing task according to the planned mass center trajectory and joint angle trajectory. At the same time, the tracking and solving system is established. Rotary-wing manipulator trajectory tracking algorithm, and tracking and controlling the rotary-wing manipulator to continue to perform the grabbing task.

Preferably, step S1 includes the following steps:

Step S1.1. Obtain the position of the rotation coordinate system of each joint of the rotor flight manipulator in the world coordinate system and the corresponding rotation matrix, and constrain it to multiple optimization variables;

Step S1.2. Perform dot product of multiple optimization variables to obtain a constraint equation containing quadratic terms of the optimization variables; the constraint equation includes non-convex constraint equation;

Step S1.3. Perform a linear approximation to the non-convex constraint equation to obtain the mixed integer quadratic constraint quadratic optimization equation and solve it to obtain the system state of the control system of the rotor flight manipulator.

Preferably, the control instructions include multiple center of mass adjustment instructions and multiple joint angle adjustment instructions; the multiple center of mass adjustment instructions include a first center of mass adjustment instruction and a second center of mass adjustment instruction; the multiple joint angle adjustment instructions include a first joint angle adjustment instruction. and the second joint angle adjustment command; the system state includes the system state at the initial time, the system state at the grasping time, the system state at the end time, and the center of mass state of the rotor flight manipulator; the system state at the capture time includes the grab time joint of the rotor flight manipulator Angle; a set of initial values that satisfies the grasping posture of the end effector of the rotor flight manipulator is obtained through the initial moment system state.

Preferably, step S2 includes the following steps of planning the center of mass trajectory of the rotor flight manipulator:

Step S2.1a. Based on the system state at the initial time and the system state at the grabbing time, perform path sampling on the rotor flight manipulator to obtain multiple sampling points;

Step S2.2a. Construct a flight corridor and preprocess the map where the rotor flight robotic arm is located to obtain a safe area near each sampling point;

Step S2.3a. Obtain the intersection areas of each two adjacent safety areas to obtain multiple intersections, and summarize them to obtain the spatial envelope;

Step S2.4a. Establish an optimized trajectory that minimizes trajectory energy in the space envelope, obtain a polynomial trajectory segmented and continuously formed by multiple sampling points, and plan the centroid trajectory of each joint of the rotor flight manipulator arm;

Step S2 also includes the following steps of joint angle trajectory planning for the rotor flight robotic arm:

Step S2.1b. Based on the joint angles of the rotor flight manipulator at the grabbing moment, calculate multiple sets of joint angles that satisfy the grabbing posture of the end effector;

Step S2.2b. Use the artificial potential field method to apply virtual repulsion to each joint of the rotor flying mechanical arm;

Step S2.3b. Based on the calculated multiple sets of joint angles and the applied virtual repulsive forces, the joints of the rotor flight robotic arm are planned to obtain the joint angle trajectories of each joint of the rotor flight robotic arm.

Preferably, step S3 includes the following steps:

Step S3.1. The control system sends the first center of mass adjustment instruction to the center of mass controller and the first joint angle adjustment instruction to the manipulator joint controller respectively, and controls the rotor flight manipulator to perform grasping according to the planned mass center trajectory and joint angle trajectory. Get tasks;

Step S3.2. The tracking solution system establishes a trajectory tracking algorithm for the rotor flight manipulator, and tracks the center of mass controller and the manipulator joint controller when performing the grabbing task to obtain the trajectory information of the rotor flight manipulator;

Step S3.3. The control system generates a second center of mass adjustment instruction based on the trajectory information and sends it to the center of mass controller, and generates a second joint angle adjustment instruction and sends it to the manipulator joint controller;

Step S3.4. The center of mass controller controls the center of mass movement of the rotor flight mechanical arm according to the second center of mass adjustment instruction, and at the same time, the robot arm joint controller controls the joint angle movement of the rotor flight mechanical arm according to the second joint angle adjustment instruction;

Step S3.5. The rotary-wing manipulator continues to perform the grabbing task, and the trajectory tracking algorithm continues to track the center of mass controller and the manipulator joint controller to obtain the trajectory information of the rotor-flying manipulator, and then returns to step S3.3.

Preferably, step S3.3 also includes the following steps:

Step S3.3.1: Obtain the expected trajectory of the UAV platform by analyzing the trajectory information of the rotor flight manipulator; the motion capture system collects the position information of the UAV platform;

Step S3.3.2: Real-time analysis of the expected trajectory of the UAV platform and the position information of the UAV platform to obtain error information, and generate a second center of mass adjustment instruction based on the error information and send it to the center of mass controller, and generate a second joint angle adjustment instruction Sent to the robot arm joint controller.

Preferably, in step S1.1, the position of the rotational coordinate system of each joint of the rotor flight manipulator arm in the world coordinate system and the corresponding rotation matrix are obtained, and constrained to multiple optimization variables, where the constraints on the rotation matrix include orthogonality constraint;

Assume that the rotor flight manipulator includes n joints, and the rotation matrix C of the i-th joint of the rotor flight manipulator is orthogonally constrained as an optimization variable to obtain the rotation matrix C;

In step S1.2, dot product multiple optimization variables to obtain a constraint equation containing the quadratic term of the optimization variable. Dot product is performed on the rotation matrix C of the i-th joint to obtain the following non-convex constraint equation of the i-th joint:

c ^T _j c _k = 1, j = k

c ^T _j c _k = 0, j≠k;

Where C is the rotation matrix of the i-th joint, c _k is the k-th column vector of the rotation matrix of the i-th joint, c ^T _j is the j-th column vector of the transpose matrix of the rotation matrix of the i-th joint, j= When k, the multiplication of the same column vectors of the rotation matrix C is equal to 1; when j≠k, the multiplication of different column vectors of the rotation matrix C is equal to 0;

In step S1.3, the non-convex constraint equation of the i-th joint is linearly approximated by taking the 2 norm as follows:

|c _j +c _k | ² ≤2

|c _j -c _k | ² ≤ 2;

Where |c _j +c _k | ² represents the square of the 2 norm after adding the j-th column vector and the k-th column vector of matrix C, |c _j -c _k | ² represents the j-th column vector of matrix C and the k-th column vector After subtracting k column vectors, take the square of 2 norm.

Preferably, the rotor flight manipulator trajectory tracking algorithm in step S3.2 includes a center of mass trajectory tracking algorithm and a joint angle trajectory tracking algorithm; the second center of mass adjustment instruction in step S3.3 includes a center of mass speed control instruction and a center of mass acceleration control instruction; The error information in step S3.3.2 includes center of mass velocity error;

The centroid trajectory tracking algorithm obtains the centroid trajectory information of the centroid controller, and the joint angle trajectory tracking algorithm obtains the joint angle trajectory information of the i-th joint of the manipulator joint controller;

Assume that steps S3.3 to S3.5 of each cycle are for the center of mass controller and the manipulator joint controller to execute a control period Δt, and assuming the last control time is t ₀ , then in this control time t ₁ :

The center of mass trajectory information includes the center of mass position P ₁ and the center of mass position feedback value P ₂ , then the center of mass position error

e ₁ =P ₁ -P ₂ ;

The center of mass trajectory information also includes the proportional coefficient K ₁ of the center of mass position loop and the center of mass velocity v ₁ , then the center of mass velocity control command

v ₂ =K ₁ e ₁ + v ₁ ;

The center of mass trajectory information also includes the center of mass velocity feedback value v ₃ , then the center of mass velocity error

e ₂ = v ₂ - v ₃ ;

The center of mass trajectory information also includes the proportional coefficient K ₂ of the center of mass velocity loop, the differential coefficient K ₃ of the center of mass velocity loop, the center of mass acceleration a ₁ , and the center of mass velocity error e ₀ of the previous control time t ₀ , then the PD controller calculates the center of mass acceleration control command

a ₂ =K ₂ e ₂ +K ₃ (e ₂ -e ₀ )/Δt+a ₁ .

Preferably,

The joint angle trajectory information of the i-th joint also includes the mass m _i of the i-th joint, the rotation matrix R _i , the moment of inertia I _i , the center of mass linear velocity Jacobian matrix J _i , and the angular velocity Jacobian matrix Z _i ; then the center of mass The transpose matrix of the linear velocity Jacobian matrix J _i is J _i ^T , the transpose matrix of the angular acceleration Jacobian matrix Z _i is Z _i ^T , and the transpose matrix of the rotation matrix R _i is R _i ^T ;

The joint angle trajectory information of the i-th joint also includes the joint position B ₁ of the i-th joint, the joint angular velocity B ₂ , the joint angular acceleration B ₃ , and the expected joint position D ₁ and expected joint angular velocity D ₂ of the i-th joint. , expected joint angular acceleration D ₃ ;

Then the gravity vector of the rotor flight manipulator arm

Where g is the acceleration of gravity, and the gravity vector G of the rotor flight manipulator arm is the product of the following parameters accumulated from the 1st joint to the nth joint of the rotor flight manipulator: mass, transpose matrix of the Jacobian matrix of angular acceleration, gravity acceleration;

Among them, the inertia matrix of the rotor flight manipulator is R ₂ , then

Assuming that the operation period of the disturbance observer of the i-th joint is equal to the control period Δt, then the current control time is t ₁ and the next control time predicted by the disturbance observer is t ₂ ;

The perturbation observer of the i-th joint includes the state vector x ₀ ;

The disturbance observer of the i-th joint also includes the first prediction vector x ₁ , the second prediction vector x ₂ , and the first deviation vector x ₃ when the control time is t ₀ ;

The disturbance observer of the i-th joint also includes the third prediction vector x ₁₁ , the fourth prediction vector x ₂₂ , and the second deviation vector x ₃₃ when the control time is t ₁ ;

The disturbance observer of the i-th joint also includes the first error term coefficient λ ₁ , the second error term coefficient λ ₂ , and the third error term coefficient λ ₃ ;

Then the error of the disturbance observer of the i-th joint when the control time is t ₁

e ₃ =x ₀ -B ₂ ;

Then the disturbance observer of the i-th joint also includes the first error amplification function f ₁ (e ₃ ) and the second error amplification function f ₂ (e ₃ ); then the disturbance observer of the i-th joint when the control time is t ₁ Calculated angular acceleration

θ=-x ₃ ;

The joint angle trajectory information of the i-th joint also includes the moment coefficient diagonal matrix _R3 and current A of the joint motor of the i-th joint;

Then the dynamic model equation when the robot arm joint controller outputs is

R ₂ *(B ₃ +θ)+G=R ₃ *A;

Then the PID controller calculates the joint position error of the rotor flight manipulator.

e ₄ =(D ₁ -B ₁ );

Then the PID controller calculates the joint angular velocity error of the rotor flight manipulator.

e ₅ =(D ₂ -B ₂ );

The joint angular acceleration of the i-th joint

B ₃ =D ₃ +K _p e ₃ +K _d e ₄ +K _i ∑e ₀ ;

where ∑e ₀ is the sum of all joint position errors calculated by the PID controller calculated by the control system until the last control time t ₀ , K _p is the proportional coefficient of the joint angular acceleration controller, and K _d is the joint angular acceleration controller. Differential coefficient, K _i is the integral coefficient of the joint angular acceleration controller;

Then the acceleration of the rotor flight manipulator arm at control time t ₂

O=B ₃ +θ;

Then the third prediction vector x ₁₁ , the fourth prediction vector x ₂₂ , and the second deviation vector x ₃₃ when the control time predicted by the disturbance observer of the i-th joint is t ₂ satisfy the following equation:

Specifically, as shown in Figure 8, the constraints on the rotation matrix in step S1.1 of this embodiment also include kinematic constraints between joints, approximate constraints on the vector unit module length, approximate constraints on vector orthogonality, and cross products. Approximation constraints on relationships, joint rotation angle constraints, center of mass position constraints, and target constraints.

Specifically, the specific flow chart of the motion planning algorithm in step S2 of this embodiment is shown in Figure 9, in which the collision detection bounding box switching is specifically: according to the distance from the grabbing target, the space is divided as shown in Figure 10 There are two different areas: the left and right areas far away from the target and the middle area close to the target; and the collision object is switched according to the different areas. For example, in the area close to the target, the lower half of the collision object cylinder is removed. At this time, the collision of the robotic arm is not considered, and only the rotary-wing robotic arm is ensured not to collide with obstacles in the environment; in areas far away from the target, a cylindrical collision object that can completely surround the system is used to ensure that the system is safe at any joint angle. Will not collide with obstacles in the environment.

Then the RRT-connect algorithm is used to sample the environment map where the rotor flight manipulator is located. The RRT-connect algorithm determines the feasibility of the sampling points and the connections between the sampling points based on the switching strategy of the collision object. Two random trees are generated simultaneously from the initial state and the system state at the time of grabbing, and a feasible path can be found with high search efficiency. In this embodiment, the sampling results are recorded in an octree map. Compared with the voxel map, the octree map can save more memory resources.

In steps S2.1a to S2.4a, after completing the path point sampling, by constructing an optimization problem that minimizes the trajectory energy, a piecewise continuous polynomial trajectory passing through these path points is obtained. Since the smoothed trajectory is often quite different from the original segmented polyline path, it is likely to collide with obstacles in the environment. In order to solve this problem, this embodiment uses the method of constructing a flight corridor to preprocess the environment map, that is, using the expansion method based on geometric configuration to find a closed collision-free safe area near each sampling point, and then calculates these adjacent The safe area is intersected to obtain the spatial envelope. Simply ensuring that the trajectory is contained within the union of these flight corridors ensures that the system does not collide with the environment. This embodiment uses an octree map to record environmental information and generate a cuboid-shaped flight corridor aligned with the coordinate axes.

This embodiment provides a planning and control method for rapid mid-air grabbing of a rotary-wing robotic arm. First, the center-of-mass motion and joint angle motion of the rotary-wing robotic arm are planned to solve the obstacle avoidance problem of the rotary-wing robotic arm. The center-of-mass motion and joint angle motion of the robotic arm are controlled respectively by the center-of-mass controller and the robotic arm joint controller; during the process of the rotary-wing robotic arm performing the grabbing task, the movement of the rotary-wing robotic arm is tracked in real time through the tracking solution system Solve, and then the control system further controls the center of mass controller and the robot arm joint controller by tracking the solution results to improve the control accuracy of the rotor flight robot arm. This control system is less difficult to deploy and can make the rotor flight machinery The arm has the ability to perform high-precision and fast grasping tasks in obstacle environments.

The embodiments of a rotary-wing flying mechanical arm and a planning and control method for rapid mid-air grabbing provided by the present invention have been described in detail above. This article uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the core idea of the present invention. It should be pointed out that for those of ordinary skill in the art, several improvements and modifications can be made to the present invention without departing from the principles of the present invention, and these improvements and modifications also fall within the protection scope of the claims of the present invention. .

Claims (10)

1. A rotor flying mechanical arm, characterized in that it includes a UAV frame, a controller, and an airborne computer; the airborne computer includes a control system for controlling the rotor flying mechanical arm; the control system includes actions Capture system, tracking solution system, PD controller and PID controller of the rotary-wing robotic arm; the controller includes a center of mass controller and a robotic arm joint controller; the rotary-wing robotic arm also includes multiple joints, and each Each joint is equipped with a joint motor and a disturbance observer; The control system is used to generate control instructions and send them to the center of mass controller and the manipulator joint controller respectively; The tracking and solving system is used to track the center of mass controller and the robotic arm joint controller to obtain trajectory information of the rotor flight robotic arm; The center of mass controller is used to receive control instructions sent by the control system and control the center of mass movement of the rotor flight mechanical arm according to the control instructions; the center of mass controller includes a center of mass position loop and a center of mass speed loop; The robotic arm joint controller is used to receive control instructions sent by the control system, and control the joint angle motion of the rotor flight robotic arm according to the control instructions; the robotic arm joint controller includes a joint angle acceleration controller. 2. A planning and control method for rapid mid-air grabbing of a rotor flying mechanical arm, characterized in that it is applied to the rotary wing flying mechanical arm according to claim 1; the control method includes the following steps: Step S1. Establish the global inverse kinematics algorithm of the rotor flight manipulator and calculate the system state of the control system; Step S2. Based on the system status of the control system, establish a motion planning algorithm for the rotor flight robotic arm, and plan to obtain the center of mass trajectory and joint angle trajectory of the rotor flight robotic arm; Step S3. The control system sends control instructions to the center of mass controller and the joint controller of the rotor flying manipulator to control the rotor flying manipulator to perform the grabbing task according to the planned mass center trajectory and joint angle trajectory. At the same time, the tracking and solving system is established. Rotary-wing manipulator trajectory tracking algorithm, and tracking and controlling the rotary-wing manipulator to continue to perform the grabbing task. 3. The control method according to claim 2, characterized in that step S1 includes the following steps: Step S1.1. Obtain the position of the rotation coordinate system of each joint of the rotor flight manipulator in the world coordinate system and the corresponding rotation matrix, and constrain it to multiple optimization variables; Step S1.2. Perform a dot product of multiple optimization variables to obtain a constraint equation containing quadratic terms of the optimization variables; the constraint equation includes a non-convex constraint equation; Step S1.3. Perform a linear approximation to the non-convex constraint equation to obtain the mixed integer quadratic constraint quadratic optimization equation and solve it to obtain the system state of the control system of the rotor flight manipulator. 4. The control method according to claim 3, characterized in that the control instructions include a plurality of center of mass adjustment instructions and a plurality of joint angle adjustment instructions; the plurality of center of mass adjustment instructions include a first center of mass adjustment instruction and a second Center of mass adjustment instructions; the plurality of joint angle adjustment instructions include a first joint angle adjustment instruction and a second joint angle adjustment instruction; the system state includes the initial time system state, the grabbing time system state, the termination time system state, and the rotor flight The state of the center of mass of the robotic arm; the system state at the grabbing moment includes the joint angle at the grasping moment of the rotary-wing robotic arm; a set of grasping poses that satisfy the end-effector of the rotary-wing robotic arm are obtained through the initial system state initial value. 5. The control method according to claim 4, characterized in that the step S2 includes the following steps of planning the center of mass trajectory of the rotor flying mechanical arm: Step S2.1a. Based on the system state at the initial time and the system state at the grabbing time, perform path sampling on the rotor flight manipulator to obtain multiple sampling points; Step S2.2a. Construct a flight corridor and preprocess the map where the rotor flight robotic arm is located to obtain a safe area near each sampling point; Step S2.3a. Obtain the intersection areas of each two adjacent safety areas to obtain multiple intersections, and summarize them to obtain the spatial envelope; Step S2.4a. Establish an optimized trajectory that minimizes trajectory energy in the space envelope, obtain a polynomial trajectory segmented and continuously formed by multiple sampling points, and plan the centroid trajectory of each joint of the rotor flight manipulator arm; The step S2 also includes the following steps of planning the joint angle trajectory of the rotor flying mechanical arm: Step S2.1b. Based on the joint angles of the rotor flight manipulator at the grabbing moment, calculate multiple sets of joint angles that satisfy the grabbing posture of the end effector; Step S2.2b. Use the artificial potential field method to apply virtual repulsion to each joint of the rotor flying mechanical arm; Step S2.3b. Based on the calculated multiple sets of joint angles and the applied virtual repulsive forces, the joints of the rotor flight robotic arm are planned to obtain the joint angle trajectories of each joint of the rotor flight robotic arm. 6. The control method according to claim 5, characterized in that step S3 includes the following steps: Step S3.1. The control system sends the first center of mass adjustment instruction to the center of mass controller and the first joint angle adjustment instruction to the manipulator joint controller respectively, and controls the rotor flight manipulator to perform grasping according to the planned mass center trajectory and joint angle trajectory. Get tasks; Step S3.2. The tracking solution system establishes a trajectory tracking algorithm for the rotor flight manipulator, and tracks the center of mass controller and the manipulator joint controller when performing the grabbing task to obtain the trajectory information of the rotor flight manipulator; Step S3.3. The control system generates a second center of mass adjustment instruction based on the trajectory information and sends it to the center of mass controller, and generates a second joint angle adjustment instruction and sends it to the manipulator joint controller; Step S3.4. The center of mass controller controls the center of mass movement of the rotor flight mechanical arm according to the second center of mass adjustment instruction, and at the same time, the robot arm joint controller controls the joint angle movement of the rotor flight mechanical arm according to the second joint angle adjustment instruction; Step S3.5. The rotary-wing manipulator continues to perform the grabbing task, and the trajectory tracking algorithm continues to track the center of mass controller and the manipulator joint controller to obtain the trajectory information of the rotor-flying manipulator, and then returns to step S3.3. 7. The control method according to claim 6, characterized in that step S3.3 further includes the following steps: Step S3.3.1: Obtain the expected trajectory of the UAV platform by analyzing the trajectory information of the rotor flight manipulator; the motion capture system collects the position information of the UAV platform; Step S3.3.2: Real-time analysis of the expected trajectory of the UAV platform and the position information of the UAV platform to obtain error information, and generate a second center of mass adjustment instruction based on the error information and send it to the center of mass controller, and generate a second joint angle adjustment instruction Sent to the robot arm joint controller. 8. The control method according to claim 7, characterized in that in step S1.1, the position of the rotational coordinate system of each joint of the rotor flight mechanical arm in the world coordinate system and the corresponding rotation matrix are obtained, and constrained For multiple optimization variables, the constraints on the rotation matrix include orthogonal constraints; Assume that the rotor flight manipulator includes n joints, and the rotation matrix C of the i-th joint of the rotor flight manipulator is orthogonally constrained as an optimization variable to obtain the rotation matrix C; In step S1.2, a dot product of multiple optimization variables is performed to obtain a constraint equation containing the quadratic term of the optimization variable. The dot product of the rotation matrix C of the i-th joint is performed to obtain the following non-convex constraints of the i-th joint. equation: c ^T _j c _k = 1, j = k c ^T _j c _k = 0, j≠k Where C is the rotation matrix of the i-th joint, c _k is the k-th column vector of the rotation matrix of the i-th joint, c ^T _j is the j-th column vector of the transpose matrix of the rotation matrix of the i-th joint, j= When k, the multiplication of the same column vectors of the rotation matrix C is equal to 1; when j≠k, the multiplication of different column vectors of the rotation matrix C is equal to 0; In step S1.3, the linear approximation of the non-convex constraint equation of the i-th joint using the 2-norm is as follows: |c _j +c _k | ² ≤2 |c _j -c _k | ² ≤ 2; Where |c _j +c _k | ² represents the square of the 2 norm after adding the i-th column vector and k-th column vector of matrix C, |c _j -c _k | ² represents the j-th column vector and k-th column vector of matrix C After subtracting k column vectors, take the square of 2 norm. 9. The control method according to claim 8, characterized in that the rotor flight manipulator trajectory tracking algorithm in step S3.2 includes a center of mass trajectory tracking algorithm and a joint angle trajectory tracking algorithm; in step S3.3 The second center of mass adjustment instruction includes a center of mass velocity control instruction and a center of mass acceleration control instruction; the error information in step S3.3.2 includes the center of mass velocity error; The centroid trajectory tracking algorithm obtains the centroid trajectory information of the centroid controller, and the joint angle trajectory tracking algorithm obtains the joint angle trajectory information of the i-th joint of the robotic arm joint controller; Assume that steps S3.3 to S3.5 of each cycle are for the center of mass controller and the manipulator joint controller to execute a control period Δt, and assuming the last control time is t ₀ , then in this control time t ₁ : The center of mass trajectory information includes the center of mass position P ₁ and the center of mass position feedback value P ₂ , then the center of mass position error e ₁ =P ₁ -P ₂ ; The center of mass trajectory information also includes the proportional coefficient K ₁ of the center of mass position loop and the center of mass velocity v ₁ , then the center of mass velocity control instruction v ₂ =K ₁ e ₁ + v ₁ ; The center of mass trajectory information also includes the center of mass velocity feedback value v ₃ , then the center of mass velocity error e ₂ = v ₂ - v ₃ ; The center of mass trajectory information also includes the proportional coefficient K ₂ of the center of mass velocity loop, the differential coefficient K ₃ of the center of mass velocity loop, the center of mass acceleration a ₁ , and the center of mass velocity error e ₀ of the previous control time t ₀ , then the PD controller calculates the center of mass acceleration Control instruction a ₂ =K ₂ e ₂ +K ₃ (e ₂ -e ₀ )/Δt+a ₁ . 10. The control method according to claim 9, characterized in that, The joint angle trajectory information of the i-th joint also includes the mass m _i of the i-th joint, the rotation matrix R _i , the moment of inertia I _i , the center of mass linear velocity Jacobian matrix J _i , and the angular velocity Jacobian matrix Z _i ; then the center of mass The transpose matrix of the linear velocity Jacobian matrix J _i is J _i ^T , the transpose matrix of the angular acceleration Jacobian matrix Z _i is Z _i ^T , and the transpose matrix of the rotation matrix R _i is R _i ^T ; The joint angle trajectory information of the i-th joint also includes the joint position B ₁ of the i-th joint, the joint angular velocity B ₂ , the joint angular acceleration B ₃ , and the expected joint position D ₁ and the expected joint angular velocity of the i-th joint. D ₂ , expected joint angular acceleration D ₃ ; Then the gravity vector of the rotor flight manipulator arm Where g is the acceleration of gravity, and the gravity vector G of the rotor flight manipulator arm is the product of the following parameters accumulated from the 1st joint to the nth joint of the rotor flight manipulator: mass, transpose matrix of the Jacobian matrix of angular acceleration, gravity acceleration; Among them, the inertia matrix of the rotor flight manipulator is R ₂ , then Assuming that the operation period of the disturbance observer of the i-th joint is equal to the control period Δt, then the current control time is t ₁ and the next control time predicted by the disturbance observer is t ₂ ; The disturbance observer of the i-th joint includes the state vector x ₀ ; The disturbance observer of the i-th joint also includes the first prediction vector x ₁ , the second prediction vector x ₂ , and the first deviation vector x ₃ when the control time is t ₀ ; The disturbance observer of the i-th joint also includes the third prediction vector x ₁₁ , the fourth prediction vector x ₂₂ , and the second deviation vector x ₃₃ when the control time is t ₁ ; The disturbance observer of the i-th joint also includes a first error term coefficient λ ₁ , a second error term coefficient λ ₂ , and a third error term coefficient λ ₃ ; Then the error of the disturbance observer of the i-th joint when the control time is t ₁ e ₃ =x ₀ -B ₂ ; Then the disturbance observer of the i-th joint also includes a first error amplification function f ₁ (e ₃ ) and a second error amplification function f ₂ (e ₃ ); Then the control time is the angular acceleration calculated by the disturbance observer of the i-th joint at t ₁ θ=-x ₃ ; The joint angle trajectory information of the i-th joint also includes the moment coefficient diagonal matrix _R3 and current A of the joint motor of the i-th joint; Then the dynamic model equation when the robot arm joint controller outputs is R ₂ *(B ₃ +θ)+G=R ₃ *A; Then the PID controller calculates the joint position error of the rotor flight manipulator. e ₄ =(D ₁ -B ₁ ); Then the PID controller calculates the joint angular velocity error of the rotor flight manipulator. e ₅ =(D ₂ -B ₂ ); The joint angular acceleration of the i-th joint B ₃ =D ₃ +K _p e ₃ +K _d e ₄ +K _i ∑e ₀ ; where ∑e ₀ is the sum of all joint position errors calculated by the PID controller of the control system until the last control time t ₀ , K _p is the proportional coefficient of the joint angular acceleration controller, and K _d is the differential of the joint angular acceleration controller Coefficient, K _i is the integral coefficient of the joint angular acceleration controller; Then the acceleration of the rotor flight manipulator arm at control time t ₂ O=B ₃ +θ; Then the third prediction vector x ₁₁ , the fourth prediction vector x ₂₂ , and the second deviation vector x ₃₃ when the control time predicted by the disturbance observer of the i- _{th joint is t 2} satisfy the following equation: