Rotor flight mechanical arm and aerial rapid grabbing planning and control method

Abstract

The application discloses a rotor flight mechanical arm and a planning and control method for aerial rapid grabbing, and relates to the technical field of unmanned aerial vehicles. The rotor flight mechanical arm comprises a UAV frame, a controller, an on-board computer and a rotor flight mechanical arm. The on-board computer comprises a control system for controlling the rotor flight mechanical arm. The control system comprises a motion capture system, a tracking calculation system, a PD controller and a PID controller of the rotor flight mechanical arm. The controller comprises a center of mass controller and a mechanical arm joint controller. The rotor flight 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 motion planning of the rotor flight mechanical arm and tracking control of the rotor flight mechanical arm for performing a grabbing task. The application can improve the control precision of the rotor flight mechanical arm, and the deployment difficulty of the control system is relatively low, so that the rotor flight mechanical arm can perform a high-precision rapid grabbing task in an obstacle environment.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) control, specifically relating to a planning and control method for a rotary-wing flying robotic arm and rapid aerial grasping. Background Technology

[0002] In recent years, with the continuous maturation of related technologies for rotary-wing unmanned aerial vehicle (UAV) systems, more and more rotary-wing UAVs have been put into production and daily life, playing a significant role. Their applications are mainly limited to aerial photography, monitoring, remote sensing and mapping, and other fields. Some scholars and researchers have designed rotary-wing flying robotic arm systems of different forms, targeting scenarios with aerial operation requirements such as high-voltage power grid and bridge and building maintenance, contact inspection of factory equipment, and high-altitude sample collection. These systems directly or indirectly mount actuators such as clamps or vacuum suction cups onto the UAVs.

[0003] Patent publication number "CN110641738A" discloses a trajectory tracking control method for a five-DOF (degree-of-freedom) free-flying manipulator, which can solve the accuracy problem of capturing and manipulating space objects by a five-DOF manipulator installed on a spacecraft. This patent controls the manipulator's trajectory by applying the Jacobian transpose matrix combined with a PD (Programmable Detector) controller using kinematic and dynamic equations. However, the method proposed in this patent requires very high modeling accuracy, making it difficult to deploy in complex flying manipulator systems, and the PD controller cannot achieve high-precision tracking in practical applications. Summary of the Invention

[0004] The purpose of this invention is to provide a planning and control method for a rotorcraft flying robotic arm and a rapid aerial grasping method, enabling the rotorcraft flying robotic arm to perform rapid grasping tasks in obstacle environments.

[0005] To achieve the above-mentioned objectives, the technical solution adopted by the present invention is as follows:

[0006] A rotary-wing flying robotic arm includes a drone frame, a controller, an onboard computer, and a rotary-wing flying robotic arm. The onboard computer includes a control system for controlling the rotary-wing flying robotic arm. The control system includes a motion capture system, a tracking and calculation system, a PD controller and a PID controller for the rotary-wing flying robotic arm. The controller includes a center of mass controller and a robotic arm joint controller. The rotary-wing flying robotic arm also includes multiple joints, and each joint is equipped with a joint motor and a disturbance observer.

[0007] The control system is used to generate control commands and send them to the center of mass controller and the robotic arm joint controller, respectively.

[0008] The tracking and calculation system is used to track the center of mass controller and the robotic arm joint controller to obtain the trajectory information of the rotor flying robotic arm;

[0009] The center of mass controller is used to receive control commands sent by the control system and control the center of mass movement of the rotorcraft arm according to the control commands; the center of mass controller includes a center of mass position loop and a center of mass velocity loop;

[0010] The robotic arm joint controller is used to receive control commands sent by the control system and control the joint angle movement of the rotorcraft robotic arm according to the control commands; the robotic arm joint controller includes a joint angle acceleration controller.

[0011] The present invention discloses a rotorcraft flying robot arm, which controls the center of mass movement and joint angle movement of the rotorcraft flying robot arm through a center of mass controller and a robot arm joint controller, respectively. At the same time, a tracking calculation system tracks and calculates the movement of the rotorcraft flying robot arm in real time. Then, the control system further controls the center of mass controller and the robot arm joint controller based on the tracking calculation results, thereby improving the control accuracy and grasping speed of the rotorcraft flying robot arm. This control system has low deployment difficulty and enables the rotorcraft flying robot arm to have high-precision and rapid grasping capabilities.

[0012] This invention also provides a planning and control method for rapid aerial grasping by a rotorcraft robotic arm, applicable to the aforementioned rotorcraft robotic arm; the control method includes the following steps:

[0013] Step S1. Establish a global inverse kinematics algorithm for the rotorcraft flying arm and calculate the system state of the control system;

[0014] Step S2. Based on the system state of the control system, establish a motion planning algorithm for the rotorcraft, and plan the center of mass trajectory and joint angle trajectory of the rotorcraft.

[0015] Step S3. The control system sends control commands to the center of mass controller and joint controller of the rotorcraft, controlling the rotorcraft to perform the grasping task according to the planned center of mass trajectory and joint angle trajectory. At the same time, the tracking and calculation system establishes the rotorcraft trajectory tracking algorithm and tracks and controls the rotorcraft to continue to perform the grasping task.

[0016] Preferably, step S1 includes the following steps:

[0017] Step S1.1. Obtain the position of the rotation coordinate system of each joint of the rotorcraft in the world coordinate system and the corresponding rotation matrix, and constrain it as multiple optimization variables;

[0018] 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.

[0019] Step S1.3. Linearize the non-convex constraint equation to obtain a mixed-integer quadratic constraint quadratic optimization equation and solve it to obtain the system state of the control system of the rotor flying robot arm.

[0020] Preferably, the control commands include multiple center of mass adjustment commands and multiple joint angle adjustment commands; the multiple center of mass adjustment commands include a first center of mass adjustment command and a second center of mass adjustment command; the multiple joint angle adjustment commands include a first joint angle adjustment command and a second joint angle adjustment command; the system state includes the initial system state, the grasping system state, the termination system state, and the center of mass state of the rotorcraft; the grasping system state includes the joint angles of the rotorcraft at the grasping moment; and a set of initial values ​​satisfying the grasping posture of the end effector of the rotorcraft is obtained through the initial system state.

[0021] Preferably, step S2 includes the following steps: planning the center-of-mass trajectory of the rotorcraft arm:

[0022] Step S2.1a. Based on the system state at the initial moment and the system state at the grasping moment, perform path sampling on the rotor flying robot arm to obtain multiple sampling points;

[0023] Step S2.2a. Constructing the flight corridor: Preprocess the map of the rotorcraft to obtain the safe zone near each sampling point;

[0024] Step S2.3a. Obtain the intersection of every two adjacent safe regions to get multiple intersections, and summarize them to obtain the spatial envelope;

[0025] Step S2.4a. Establish the optimal trajectory that minimizes trajectory energy in the spatial envelope, obtain the polynomial trajectory formed by multiple sampling points in segments, and plan the center-of-mass trajectory of each joint of the rotor flying robot arm;

[0026] Step S2 further includes the following step of planning the joint angle trajectory of the rotorcraft arm:

[0027] Step S2.1b. Based on the joint angles at the grasping moment of the rotorcraft arm, calculate multiple sets of joint angles that satisfy the grasping posture of the end effector;

[0028] Step S2.2b. Apply virtual repulsive forces to each joint of the rotorcraft arm using the artificial potential field method;

[0029] Step S2.3b. Based on the calculated joint angles and the applied virtual repulsive force, the joint angle trajectory of each joint of the rotorcraft is planned.

[0030] Preferably, step S3 includes the following steps:

[0031] Step S3.1. The control system sends the first center of mass adjustment command to the center of mass controller and the first joint angle adjustment command to the robotic arm joint controller, respectively, to control the rotor flying robotic arm to perform the grasping task according to the planned center of mass trajectory and joint angle trajectory.

[0032] Step S3.2. The tracking and calculation system establishes a trajectory tracking algorithm for the rotorcraft, and tracks the center of mass controller and the joint controller of the rotorcraft during the grasping task to obtain the trajectory information of the rotorcraft.

[0033] Step S3.3. The control system generates a second center of mass adjustment command based on the trajectory information and sends it to the center of mass controller, and generates a second joint angle adjustment command and sends it to the robotic arm joint controller;

[0034] Step S3.4. The center of mass controller controls the movement of the center of mass of the rotor flying robot arm according to the second center of mass adjustment command, and at the same time the robot arm joint controller controls the joint angle movement of the rotor flying robot arm according to the second joint angle adjustment command.

[0035] Step S3.5. The rotorcraft continues to perform the grasping task, while the trajectory tracking algorithm continues to track the center of mass controller and the joint controller of the rotorcraft to obtain the trajectory information of the rotorcraft, and then returns to step S3.3.

[0036] Preferably, step S3.3 further includes the following steps:

[0037] Step S3.3.1: By analyzing the trajectory information of the rotorcraft arm, the desired trajectory of the UAV platform is obtained; the motion capture system collects the position information of the UAV platform;

[0038] Step S3.3.2: Analyze the expected trajectory and position information of the UAV platform in real time to obtain error information, and generate a second center of mass adjustment command based on the error information and send it to the center of mass controller, and generate a second joint angle adjustment command and send it to the robotic arm joint controller.

[0039] Preferably, in step S1.1, the position of the rotation coordinate system of each joint of the rotorcraft arm in the world coordinate system and the corresponding rotation matrix are obtained and constrained into multiple optimization variables, wherein the constraint on the rotation matrix includes orthogonal constraint;

[0040] Suppose that the rotorcraft has n joints. After orthogonally constraining the rotation matrix of the i-th joint of the rotorcraft as the optimization variable, we obtain the rotation matrix C.

[0041] In step S1.2, the dot product of multiple optimization variables is performed to obtain the constraint equation containing quadratic terms of the optimization variables. The dot product of the rotation matrix C of the i-th joint yields the following non-convex constraint equation for the i-th joint:

[0042] c T j c k =1, j=k

[0043] c T j c k =0, j≠k;

[0044] Where C is the rotation matrix of the i-th joint, c k Let c be the k-th column vector of the rotation matrix of the i-th joint. T j Let be the j-th column vector of the transpose of the rotation matrix of the i-th joint. When j = k, the product of the same column vectors of the rotation matrix C equals 1. When j ≠ k, the product of different column vectors of the rotation matrix C equals 0.

[0045] In step S1.3, the non-convex constraint equation of the i-th joint is linearized using the 2-norm as follows:

[0046] |c j +c k | 2 ≤2

[0047] |c j -c k | 2 ≤2;

[0048] Where |c j +c k | 2 |c| represents the sum of the j-th and k-th column vectors of matrix C, taking the square of the 2-norm. j -c k | 2 It represents the square of the 2-norm after subtracting the j-th column vector and the k-th column vector of matrix C.

[0049] Preferably, the rotorcraft 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 command in step S3.3 includes a center of mass velocity control command and a center of mass acceleration control command; and the error information in step S3.3.2 includes a center of mass velocity error.

[0050] 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.

[0051] Let each cycle's steps S3.3 to S3.5 represent one control cycle Δt executed by the center of mass controller and the robotic arm joint controller, and let the previous control time be t0. Then, at the current control time t1:

[0052] The centroid trajectory information includes the centroid position P1 and the centroid position feedback value P2, then the centroid position error...

[0053] e1 = P1 - P2;

[0054] The centroid trajectory information also includes the proportional coefficient K1 of the centroid position loop and the centroid velocity v1, then the centroid velocity control command

[0055] v2 = K1e1 + v1;

[0056] The centroid trajectory information also includes the centroid velocity feedback value v3, then the centroid velocity error

[0057] e2 = v2 - v3;

[0058] The centroid trajectory information also includes the proportional coefficient K2 of the centroid velocity loop, the differential coefficient K3 of the centroid velocity loop, the centroid acceleration a1, and the centroid velocity error e0 at the previous control time t0. The PD controller then calculates the centroid acceleration control command.

[0059] a2=K2e2+K3(e2-e0) / Δt+a1.

[0060] Preferred,

[0061] The joint angle trajectory information of the i-th joint also includes the mass m of the i-th joint. i Rotation matrix R i Moment of inertia I i The Jacobian matrix of the center of mass linear velocity J i Angular velocity Jacobian matrix Z i The Jacobian matrix of the center-of-mass linear velocity J i The transpose matrix is ​​J i T Angular acceleration Jacobian matrix Z i The transpose matrix is ​​Z i T Rotation matrix R i The transpose matrix is ​​R i T ;

[0062] The joint angle trajectory information of the i-th joint also includes the joint position B1, joint angular velocity B2, joint angular acceleration B3 of the i-th joint, and the expected joint position D1, expected joint angular velocity D2, and expected joint angular acceleration D3 of the i-th joint.

[0063] Then the gravity vector of the rotorcraft arm

[0064]

[0065] Where g is the gravitational acceleration, and the gravity vector G of the rotorcraft is the product of the following parameters of the rotorcraft from the first joint to the nth joint: mass, transpose of the angular acceleration Jacobian matrix, and gravitational acceleration.

[0066] The inertia matrix of the rotorcraft arm is R2, then

[0067]

[0068] If 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 t1, and the next control time predicted by the disturbance observer is t2.

[0069] The perturbation observer of the i-th joint includes the state vector x0;

[0070] The disturbance observer of the i-th joint also includes a first prediction vector x1, a second prediction vector x2, and a first deviation vector x3 at control time t0;

[0071] The disturbance observer for the i-th joint also includes a third prediction vector x at control time t1. 11 Fourth prediction vector x 22 The second deviation vector x 33 ;

[0072] The disturbance observer of the i-th joint also includes a first error term coefficient λ1, a second error term coefficient λ2, and a third error term coefficient λ3;

[0073] Then the error of the disturbance observer for the i-th joint at control time t1.

[0074] e3 = x0 - B2;

[0075] The disturbance observer of the i-th joint further includes a first error amplification function f1(e3) and a second error amplification function f2(e3);

[0076] Then, the angular acceleration calculated by the disturbance observer of the i-th joint at control time t1.

[0077] θ = -x3;

[0078] The joint angle trajectory information of the i-th joint also includes the torque coefficient diagonal matrix R3 and current A of the joint motor of the i-th joint;

[0079] The dynamic model equation for the output of the robotic arm joint controller is as follows:

[0080] R2*(B3+θ)+G=R3*A;

[0081] The PID controller then calculates the joint position error of the rotorcraft arm.

[0082] e4 = (D1 - B1);

[0083] The PID controller then calculates the joint angular velocity error of the rotorcraft arm.

[0084] e5 = (D2 - B2);

[0085] The joint angular acceleration of the i-th joint

[0086] B3 = D3 + K p e3+K d e4+K i ∑e0;

[0087] Where ∑e0 is the sum of all joint position errors calculated by the PID controller and statistically analyzed by the control system up to the last control time t0, K p K is the proportional coefficient of the joint angle acceleration controller. d K represents the differential coefficient of the joint angle acceleration controller. i The integral coefficient of the joint angle acceleration controller;

[0088] Then, the acceleration of the rotorcraft arm at control time t2

[0089] O = B³ + θ;

[0090] Then the third prediction vector x predicted by the disturbance observer of the i-th joint at time t2 is... 11 Fourth prediction vector x 22 The second deviation vector x 33 The following equations must be satisfied:

[0091]

[0092] Beneficial effects:

[0093] This invention discloses a planning and control method for a rotorcraft flying robotic arm and a rapid aerial grasping mechanism. First, the motion of the rotorcraft's center of mass and the joint angle motion are planned to address obstacle avoidance. The motion of the rotorcraft's center of mass and the joint angle motion are controlled separately by a center of mass controller and a joint controller. During the grasping task, a tracking and calculation system tracks and calculates the rotorcraft's motion in real time. The control system then uses the tracking and calculation results to further control the center of mass controller and the joint controller, thereby improving the control accuracy of the rotorcraft. This control system is relatively easy to deploy and enables the rotorcraft to perform high-precision, rapid grasping tasks in obstacle-prone environments. Attached Figure Description

[0094] Figure 1 The diagram shown is a structural diagram of the rotorcraft flying robot of Embodiment 1;

[0095] Figure 2 The diagram shown is the overall flowchart of the planning and control method for rapid aerial grasping by the rotor-wing flying robotic arm in Embodiment 2.

[0096] Figure 3 As shown Figure 2 The first sub-flowchart;

[0097] Figure 4 As shown Figure 2 The second sub-flowchart;

[0098] Figure 5 As shown Figure 2 The third sub-flowchart;

[0099] Figure 6 As shown Figure 2 The fourth sub-flowchart;

[0100] Figure 7 As shown Figure 2 The fifth sub-flowchart;

[0101] Figure 8 The diagram shown is a schematic diagram of the constraint algorithm in step S1.1 of Embodiment 2;

[0102] Figure 9 The following is a flowchart of the motion planning algorithm in step S2 of Embodiment 2;

[0103] Figure 10 The diagram shown is a schematic diagram of the grabbing space division in Example 2. Detailed Implementation

[0104] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the specific implementation methods of the present invention will be described below with reference to the accompanying drawings. Obviously, the drawings described below are merely some embodiments of the present invention. For those skilled in the art, other drawings and other implementation methods can be obtained based on these drawings without any creative effort.

[0105] The technical solution of the present invention will be described in detail below with specific embodiments.

[0106] Example 1

[0107] like Figure 1 As shown, this embodiment of a rotorcraft includes a drone frame, a controller, an onboard computer, and a rotorcraft. The onboard computer includes a control system for controlling the rotorcraft. The control system includes a motion capture system, a tracking and calculation system, a PD controller and a PID controller for the rotorcraft. The controller includes a center of mass controller and a joint controller for the rotorcraft. The rotorcraft also includes multiple joints, and each joint is equipped with a joint motor and a disturbance observer.

[0108] The control system is used to generate control commands and send them to the center of mass controller and the robotic arm joint controller, respectively.

[0109] The tracking and calculation system is used to track the center of mass controller and the robotic arm joint controller to obtain the trajectory information of the rotor-flying robotic arm;

[0110] The center of mass controller is used to receive control commands sent by the control system and control the center of mass movement of the rotorcraft arm according to the control commands; the center of mass controller includes a center of mass position loop and a center of mass velocity loop;

[0111] The robotic arm joint controller is used to receive control commands sent by the control system and control the joint angle movement of the rotorcraft robotic arm according to the control commands; the robotic arm joint controller includes a joint angle acceleration controller.

[0112] The rotary-wing robotic arm also includes a drone power unit; the drone power unit includes an ESC, a motor, and propellers.

[0113] The rotorcraft also includes a flight controller for flight operations, powered by the drone's power unit.

[0114] Specifically, the rotorcraft arm in this embodiment is a coaxial eight-propeller model with a wheelbase of 550mm, providing sufficient thrust and torque for attitude adjustment. The frame houses the CUAV X7+Pro flight controller and an Intel NUC11 onboard computer. Each rotating joint of the rotorcraft arm is equipped with a Dynamixel servo; one joint uses an XM540-W270 servo, while the remaining joints and the end effector's gripper use XM430-W350 servos. The components of each joint are primarily fabricated from carbon fiber sheets using photopolymerization printing. The total system weight is approximately 4.62kg (excluding battery), with the robotic arm itself weighing approximately 1kg. The CUAV X7+Pro flight controller utilizes an STM32H7 series processor with an operating frequency of up to 480MHz, effectively handling the flight controller's computational demands. The controller also incorporates aerospace-grade accelerometers and gyroscopes, providing stable and accurate feedback that significantly improves attitude control. The onboard NUC11 is equipped with an Intel Core i7-1165G7 processor, featuring 4 cores and 8 threads, with a maximum turbo frequency of 4.70GHz. It runs Ubuntu 18.04 and ROS (Robotics Operating System).

[0115] Specifically, the rotorcraft flying arm in this embodiment also includes an external computing unit. The external computing unit establishes a global inverse kinematics algorithm and a motion planning algorithm. After the external computing unit completes the calculation, it copies the calculation results to the control system in the form of a file for execution.

[0116] Specifically, the global pose of the drone platform is obtained by the OptiTrack motion capture system and Motive software by tracking and calculating the position of a pre-set reflective ball on the system. The calculation results are sent to the onboard computer via a wireless router through the local area network, and the pose feedback frequency can reach up to 120Hz.

[0117] This embodiment of a rotorcraft robotic arm controls the center of mass movement and joint angle movement of the rotorcraft robotic arm through a center of mass controller and a robotic arm joint controller, respectively. At the same time, a tracking and calculation system tracks and calculates the movement of the rotorcraft robotic arm in real time. Then, the control system further controls the center of mass controller and the robotic arm joint controller based on the tracking and calculation results, thereby improving the control accuracy and grasping speed of the rotorcraft robotic arm. This control system is relatively easy to deploy and enables the rotorcraft robotic arm to have high-precision and rapid grasping capabilities.

[0118] Example 2

[0119] like Figures 2-6As shown, this embodiment presents a planning and control method for rapid aerial grasping by a rotorcraft robotic arm, applied to the aforementioned rotorcraft robotic arm; the control method includes the following steps:

[0120] Step S1. Establish a global inverse kinematics algorithm for the rotorcraft flying arm and calculate the system state of the control system;

[0121] Step S2. Based on the system state of the control system, establish a motion planning algorithm for the rotorcraft, and plan the center of mass trajectory and joint angle trajectory of the rotorcraft.

[0122] Step S3. The control system sends control commands to the center of mass controller and joint controller of the rotorcraft, controlling the rotorcraft to perform the grasping task according to the planned center of mass trajectory and joint angle trajectory. At the same time, the tracking and calculation system establishes the rotorcraft trajectory tracking algorithm and tracks and controls the rotorcraft to continue to perform the grasping task.

[0123] Preferably, step S1 includes the following steps:

[0124] Step S1.1. Obtain the position of the rotation coordinate system of each joint of the rotorcraft in the world coordinate system and the corresponding rotation matrix, and constrain it as multiple optimization variables;

[0125] 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 non-convex constraint equations.

[0126] Step S1.3. Linearize the non-convex constraint equation to obtain a mixed-integer quadratic constraint quadratic optimization equation and solve it to obtain the system state of the control system of the rotor flying robot arm.

[0127] Preferably, the control commands include multiple center of mass adjustment commands and multiple joint angle adjustment commands; the multiple center of mass adjustment commands include a first center of mass adjustment command and a second center of mass adjustment command; the multiple joint angle adjustment commands include a first joint angle adjustment command and a second joint angle adjustment command; the system state includes the initial system state, the grasping system state, the termination system state, and the center of mass state of the rotorcraft; the grasping system state includes the joint angles of the rotorcraft at the grasping moment; and a set of initial values ​​that satisfy the grasping posture of the end effector of the rotorcraft is obtained through the initial system state.

[0128] Preferably, step S2 includes the following steps for planning the center-of-mass trajectory of the rotorcraft:

[0129] Step S2.1a. Based on the system state at the initial moment and the system state at the grasping moment, perform path sampling on the rotor flying robot arm to obtain multiple sampling points;

[0130] Step S2.2a. Constructing the flight corridor: Preprocess the map of the rotorcraft to obtain the safe zone near each sampling point;

[0131] Step S2.3a. Obtain the intersection of every two adjacent safe regions to get multiple intersections, and summarize them to obtain the spatial envelope;

[0132] Step S2.4a. Establish the optimal trajectory that minimizes trajectory energy in the spatial envelope, obtain the polynomial trajectory formed by multiple sampling points in segments, and plan the center-of-mass trajectory of each joint of the rotor flying robot arm;

[0133] Step S2 also includes the following steps for planning the joint angle trajectory of the rotorcraft:

[0134] Step S2.1b. Based on the joint angles at the grasping moment of the rotorcraft arm, calculate multiple sets of joint angles that satisfy the grasping posture of the end effector;

[0135] Step S2.2b. Apply virtual repulsive forces to each joint of the rotorcraft arm using the artificial potential field method;

[0136] Step S2.3b. Based on the calculated joint angles and the applied virtual repulsive force, the joint angle trajectory of each joint of the rotorcraft is planned.

[0137] Preferably, step S3 includes the following steps:

[0138] Step S3.1. The control system sends the first center of mass adjustment command to the center of mass controller and the first joint angle adjustment command to the robotic arm joint controller, respectively, to control the rotor flying robotic arm to perform the grasping task according to the planned center of mass trajectory and joint angle trajectory.

[0139] Step S3.2. The tracking and calculation system establishes a trajectory tracking algorithm for the rotorcraft, and tracks the center of mass controller and the joint controller of the rotorcraft during the grasping task to obtain the trajectory information of the rotorcraft.

[0140] Step S3.3. The control system generates a second center of mass adjustment command based on the trajectory information and sends it to the center of mass controller, and generates a second joint angle adjustment command and sends it to the robotic arm joint controller;

[0141] Step S3.4. The center of mass controller controls the movement of the center of mass of the rotor flying robot arm according to the second center of mass adjustment command, and at the same time the robot arm joint controller controls the joint angle movement of the rotor flying robot arm according to the second joint angle adjustment command.

[0142] Step S3.5. The rotorcraft continues to perform the grasping task, while the trajectory tracking algorithm continues to track the center of mass controller and the joint controller of the rotorcraft to obtain the trajectory information of the rotorcraft, and then returns to step S3.3.

[0143] Preferably, step S3.3 further includes the following steps:

[0144] Step S3.3.1: By analyzing the trajectory information of the rotorcraft arm, the desired trajectory of the UAV platform is obtained; the motion capture system collects the position information of the UAV platform;

[0145] Step S3.3.2: Analyze the expected trajectory and position information of the UAV platform in real time to obtain error information, and generate a second center of mass adjustment command based on the error information and send it to the center of mass controller, and generate a second joint angle adjustment command and send it to the robotic arm joint controller.

[0146] Preferably, in step S1.1, the position of the rotation coordinate system of each joint of the rotor flying robot in the world coordinate system and the corresponding rotation matrix are obtained and constrained into multiple optimization variables, wherein the constraint on the rotation matrix includes orthogonal constraint;

[0147] Suppose that the rotorcraft has n joints. After orthogonally constraining the rotation matrix of the i-th joint of the rotorcraft as the optimization variable, we obtain the rotation matrix C.

[0148] In step S1.2, the dot product of multiple optimization variables is performed to obtain the constraint equation containing quadratic terms of the optimization variables. The dot product of the rotation matrix C of the i-th joint yields the following non-convex constraint equation for the i-th joint:

[0149] c T j c k =1, j=k

[0150] c T j c k =0, j≠k;

[0151] Where C is the rotation matrix of the i-th joint, c k Let c be the k-th column vector of the rotation matrix of the i-th joint. T jLet be the j-th column vector of the transpose of the rotation matrix of the i-th joint. When j = k, the product of the same column vectors of the rotation matrix C equals 1. When j ≠ k, the product of different column vectors of the rotation matrix C equals 0.

[0152] In step S1.3, the non-convex constraint equation of the i-th joint is linearized using the 2-norm as follows:

[0153] |c j +c k | 2 ≤2

[0154] |c j -c k | 2 ≤2;

[0155] Where |c j +c k | 2 |c| represents the sum of the j-th and k-th column vectors of matrix C, taking the square of the 2-norm. j -c k | 2 It represents the square of the 2-norm after subtracting the j-th column vector and the k-th column vector of matrix C.

[0156] Preferably, the rotorcraft 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 command in step S3.3 includes a center of mass velocity control command and a center of mass acceleration control command; the error information in step S3.3.2 includes a center of mass velocity error;

[0157] The center of mass trajectory tracking algorithm obtains the center of mass trajectory information of the center of mass 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.

[0158] Let each cycle's steps S3.3 to S3.5 represent one control cycle Δt executed by the center of mass controller and the robotic arm joint controller, and let the previous control time be t0. Then, at the current control time t1:

[0159] The centroid trajectory information includes the centroid position P1 and the centroid position feedback value P2, then the centroid position error...

[0160] e1 = P1 - P2;

[0161] The center of mass trajectory information also includes the proportional coefficient K1 of the center of mass position loop and the center of mass velocity v1, then the center of mass velocity control command

[0162] v2 = K1e1 + v1;

[0163] The center of mass trajectory information also includes the center of mass velocity feedback value v3, then the center of mass velocity error...

[0164] e2 = v2 - v3;

[0165] The center of mass trajectory information also includes the proportional coefficient K2 of the center of mass velocity loop, the differential coefficient K3 of the center of mass velocity loop, the center of mass acceleration a1, and the center of mass velocity error e0 at the previous control time t0. The PD controller then calculates the center of mass acceleration control command.

[0166] a2=K2e2+K3(e2-e0) / Δt+a1.

[0167] Preferred,

[0168] The joint angle trajectory information of the i-th joint also includes the mass m of the i-th joint. i Rotation matrix R i Moment of inertia I i The Jacobian matrix of the center of mass linear velocity J i Angular velocity Jacobian matrix Z i The Jacobian matrix of the center-of-mass linear velocity J i The transpose matrix is ​​J i T Angular acceleration Jacobian matrix Z i The transpose matrix is ​​Z i T Rotation matrix R i The transpose matrix is ​​R i T ;

[0169] The joint angle trajectory information of the i-th joint also includes the joint position B1, joint angular velocity B2, joint angular acceleration B3 of the i-th joint, as well as the expected joint position D1, expected joint angular velocity D2, and expected joint angular acceleration D3 of the i-th joint.

[0170] Then the gravity vector of the rotorcraft arm

[0171]

[0172] Where g is the gravitational acceleration, and the gravity vector G of the rotorcraft is the product of the following parameters of the rotorcraft from the first joint to the nth joint: mass, transpose of the angular acceleration Jacobian matrix, and gravitational acceleration.

[0173] The inertia matrix of the rotorcraft arm is R2, then

[0174]

[0175] If 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 t1, and the next control time predicted by the disturbance observer is t2.

[0176] The perturbation observer for the i-th joint includes the state vector x0;

[0177] The disturbance observer for the i-th joint also includes the first prediction vector x1, the second prediction vector x2, and the first deviation vector x3 at control time t0;

[0178] The disturbance observer for the i-th joint also includes a third prediction vector x at control time t1. 11 Fourth prediction vector x 22 The second deviation vector x 33 ;

[0179] The disturbance observer for the i-th joint also includes the first error term coefficient λ1, the second error term coefficient λ2, and the third error term coefficient λ3;

[0180] Then the error of the disturbance observer for the i-th joint at control time t1.

[0181] e3 = x0 - B2;

[0182] The disturbance observer for the i-th joint also includes a first error amplification function f1(e3) and a second error amplification function f2(e3); the angular acceleration calculated by the disturbance observer for the i-th joint at control time t1 is...

[0183] θ = -x3;

[0184] The joint angle trajectory information of the i-th joint also includes the torque coefficient diagonal matrix R3 and current A of the joint motor of the i-th joint;

[0185] The dynamic model equation for the output of the robotic arm joint controller is as follows:

[0186] R2*(B3+θ)+G=R3*A;

[0187] The PID controller then calculates the joint position error of the rotorcraft arm.

[0188] e4 = (D1 - B1);

[0189] The PID controller then calculates the joint angular velocity error of the rotorcraft arm.

[0190] e5 = (D2 - B2);

[0191] The joint angular acceleration of the i-th joint

[0192] B3 = D3 + K p e3+Kd e4+K i ∑e0;

[0193] Where ∑e0 is the sum of all joint position errors calculated by the PID controller and statistically analyzed by the control system up to the last control time t0, K p K is the proportional coefficient of the joint angle acceleration controller. d K represents the differential coefficient of the joint angle acceleration controller. i The integral coefficient of the joint angle acceleration controller;

[0194] Then, the acceleration of the rotorcraft arm at control time t2

[0195] O = B³ + θ;

[0196] Then the third prediction vector x predicted by the disturbance observer of the i-th joint at time t2 is... 11 Fourth prediction vector x 22 The second deviation vector x 33 The following equations must be satisfied:

[0197]

[0198] Specifically, such as Figure 8 As shown, the constraints on the rotation matrix in step S1.1 of this embodiment also include kinematic constraints between joints, approximate constraints on the unit magnitude of vectors, approximate constraints on vector orthogonality, approximate constraints on cross product relationships, joint rotation angle constraints, centroid position constraints, and target constraints.

[0199] Specifically, the flowchart of the motion planning algorithm in step S2 of this embodiment is as follows: Figure 9 As shown, the collision detection bounding box switching specifically involves dividing the space into sections based on the distance to the target being captured. Figure 10 The diagram shows two distinct regions: the left and right sides, which are far from the target, and the middle region, which is close to the target. The collision object is switched according to the region. For example, in the region close to the target, the lower half of the cylindrical collision object is removed. In this case, the collision of the robotic arm is not considered, and only the rotorcraft robotic arm is ensured not to collide with obstacles in the environment. In the region far from the target, a cylindrical collision object that can completely surround the system is used to ensure that the system will not collide with obstacles in the environment at any joint angle.

[0200] Then, the RRT-connect algorithm is used to sample the environment map of the rotorcraft. The RRT-connect algorithm judges the feasibility of sampling points and connections between sampling points based on the collision object switching strategy. Two random trees are generated simultaneously from the initial state and the system state at the grasping moment, which can find a feasible path with high search efficiency. In this embodiment, the sampling results are recorded in an octree map, which can save more memory resources than a voxel map.

[0201] In steps S2.1a to S2.4a, after sampling the path points, an optimization problem that minimizes the trajectory energy is constructed to obtain a piecewise continuous polynomial trajectory passing through these path points. Since the smoothed trajectory often differs significantly from the original piecewise polygonal path, collisions with obstacles in the environment are likely to occur. To address this issue, this embodiment employs a method of constructing flight corridors to preprocess the environmental map. Specifically, a geometrically based dilatation method is used to find a closed, collision-free safe region near each sampling point, and the intersection of these adjacent safe regions is taken to obtain the spatial envelope. As long as the trajectory is contained within the union of these flight corridors, the system will not collide with the environment. This embodiment uses an octree map to record environmental information and generates cuboid-shaped flight corridors aligned with the coordinate axes.

[0202] This embodiment presents a planning and control method for rapid aerial grasping by a rotorcraft. First, the motion of the rotorcraft's center of mass and the joint angle motion are planned to address obstacle avoidance. These motions are controlled separately by a center of mass controller and a joint controller. During the grasping task, a tracking and calculation system tracks and calculates the rotorcraft's motion in real time. The control system then uses the tracking and calculation results to further control the center of mass controller and the joint controller, thereby improving the control accuracy of the rotorcraft. This control system is relatively easy to deploy and enables the rotorcraft to perform high-precision, rapid grasping tasks in obstacle-prone environments.

[0203] The embodiments of the rotor-type flying robotic arm and the planning and control method for rapid aerial grasping provided by the present invention have been described in detail above. Specific examples have been used to illustrate the principles and implementation methods of the present invention, and the descriptions of the embodiments above are only for the purpose of helping to understand the core ideas of the present invention. It should be noted that those skilled in the art can make several improvements and modifications 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

1. A planning and control method for rapid aerial grasping by a rotorcraft robotic arm, characterized in that, The rotorcraft includes a drone frame, a controller, and an onboard computer; the onboard computer includes a control system for controlling the rotorcraft; the control system includes a motion capture system, a tracking and calculation system, a PD controller and a PID controller for the rotorcraft; the controller includes a center of mass controller and a joint controller for the rotorcraft; the rotorcraft 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 commands and send them to the center of mass controller and the robotic arm joint controller, respectively. The tracking and calculation system is used to track the center of mass controller and the robotic arm joint controller to obtain the trajectory information of the rotor flying robotic arm; The center of mass controller is used to receive control commands sent by the control system and control the center of mass movement of the rotorcraft arm according to the control commands; the center of mass controller includes a center of mass position loop and a center of mass velocity loop; The robotic arm joint controller is used to receive control commands sent by the control system and control the joint angle movement of the rotorcraft robotic arm according to the control commands; the robotic arm joint controller includes a joint angle acceleration controller. The control method includes the following steps: Step S1. Establish a global inverse kinematics algorithm for the rotorcraft flying arm and calculate the system state of the control system; Step S2. Based on the system state of the control system, establish a motion planning algorithm for the rotorcraft, and plan the center of mass trajectory and joint angle trajectory of the rotorcraft. Step S3. The control system sends control commands to the center of mass controller and joint controller of the rotorcraft, and controls the rotorcraft to perform the grasping task according to the planned center of mass trajectory and joint angle trajectory. At the same time, the tracking and calculation system establishes the rotorcraft trajectory tracking algorithm and tracks and controls the rotorcraft to continue to perform the grasping task. The control commands include multiple center of mass adjustment commands and multiple joint angle adjustment commands; the multiple center of mass adjustment commands include a first center of mass adjustment command and a second center of mass adjustment command; the multiple joint angle adjustment commands include a first joint angle adjustment command and a second joint angle adjustment command; the system state includes the initial system state, the grasping system state, the termination system state, and the center of mass state of the rotorcraft; the grasping system state includes the joint angles of the rotorcraft at the grasping moment; a set of initial values ​​that satisfy the grasping posture of the end effector of the rotorcraft is obtained through the initial system state.

2. The control method according to claim 1, 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 rotorcraft in the world coordinate system and the corresponding rotation matrix, and constrain it as 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. Linearize the non-convex constraint equation to obtain a mixed-integer quadratic constraint quadratic optimization equation and solve it to obtain the system state of the control system of the rotor flying robot arm.

3. The control method according to claim 2, characterized in that, Step S2 includes the following steps for planning the center-of-mass trajectory of the rotorcraft: Step S2.1a. Based on the system state at the initial moment and the system state at the grasping moment, perform path sampling on the rotor flying robot arm to obtain multiple sampling points; Step S2.2a. Constructing the flight corridor: Preprocess the map of the rotorcraft to obtain the safe zone near each sampling point; Step S2.3a. Obtain the intersection of every two adjacent safe regions to get multiple intersections, and summarize them to obtain the spatial envelope; Step S2.4a. Establish the optimal trajectory that minimizes trajectory energy in the spatial envelope, obtain the polynomial trajectory formed by multiple sampling points in segments, and plan the center-of-mass trajectory of each joint of the rotor flying robot arm; Step S2 further includes the following step of planning the joint angle trajectory of the rotorcraft arm: Step S2.1b. Based on the joint angles at the grasping moment of the rotorcraft arm, calculate multiple sets of joint angles that satisfy the grasping posture of the end effector; Step S2.2b. Apply virtual repulsive forces to each joint of the rotorcraft arm using the artificial potential field method; Step S2.3b. Based on the calculated joint angles and the applied virtual repulsive force, the joint angle trajectory of each joint of the rotorcraft is planned.

4. The control method according to claim 3, characterized in that, Step S3 includes the following steps: Step S3.

1. The control system sends the first center of mass adjustment command to the center of mass controller and the first joint angle adjustment command to the robotic arm joint controller, respectively, to control the rotor flying robotic arm to perform the grasping task according to the planned center of mass trajectory and joint angle trajectory. Step S3.

2. The tracking and calculation system establishes a trajectory tracking algorithm for the rotorcraft, and tracks the center of mass controller and the joint controller of the rotorcraft during the grasping task to obtain the trajectory information of the rotorcraft. Step S3.

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

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

5. The rotorcraft continues to perform the grasping task, while the trajectory tracking algorithm continues to track the center of mass controller and the joint controller of the rotorcraft to obtain the trajectory information of the rotorcraft, and then returns to step S3.

3.

5. The control method according to claim 4, characterized in that, Step S3.3 further includes the following steps: Step S3.3.1: By analyzing the trajectory information of the rotorcraft arm, the desired trajectory of the UAV platform is obtained; the motion capture system collects the position information of the UAV platform; Step S3.3.2: Analyze the expected trajectory and position information of the UAV platform in real time to obtain error information, and generate a second center of mass adjustment command based on the error information and send it to the center of mass controller, and generate a second joint angle adjustment command and send it to the robotic arm joint controller.

6. The control method according to claim 5, characterized in that, In step S1.1, the position of the rotation coordinate system of each joint of the rotor flying robot in the world coordinate system and the corresponding rotation matrix are obtained and constrained into multiple optimization variables, including orthogonal constraints on the rotation matrix. Suppose that the rotorcraft has n joints. After orthogonally constraining the rotation matrix of the i-th joint of the rotorcraft as the optimization variable, we obtain the rotation matrix C. In step S1.2, the dot product of multiple optimization variables is performed to obtain the constraint equation containing quadratic terms of the optimization variables. The dot product of the rotation matrix C of the i-th joint yields the following non-convex constraint equation for the i-th joint: Where C is the rotation matrix of the i-th joint, c k Let k be the column vector of the rotation matrix of the i-th joint. Let be the j-th column vector of the transpose of the rotation matrix of the i-th joint. When j = k, the product of the same column vectors of the rotation matrix C equals 1. When j ≠ k, the product of different column vectors of the rotation matrix C equals 0. In step S1.3, the non-convex constraint equation of the i-th joint is linearized using the 2-norm as follows: |c j +c k | 2 ≤2 |c j -c k | 2 ≤2; Where |c j +c k | 2 |c| represents the sum of the j-th and k-th column vectors of matrix C, taking the square of the 2-norm. j -c k | 2 It represents the square of the 2-norm after subtracting the j-th column vector and the k-th column vector of matrix C.

7. The control method according to claim 6, characterized in that, The rotorcraft 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 command in step S3.3 includes a center of mass velocity control command and a center of mass acceleration control command; the error information in step S3.3.2 includes a 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. Let each cycle's steps S3.3 to S3.5 represent one control cycle Δt executed by the center of mass controller and the robotic arm joint controller, and let the previous control time be t0. Then, at the current control time t1: The centroid trajectory information includes the centroid position P1 and the centroid position feedback value P2, then the centroid position error... e1 = P1 - P2; The centroid trajectory information also includes the proportional coefficient K1 of the centroid position loop and the centroid velocity v1, then the centroid velocity control command v2 = K1e1 + v1; The centroid trajectory information also includes the centroid velocity feedback value v3, then the centroid velocity error e2 = v2 - v3; The centroid trajectory information also includes the proportional coefficient K2 of the centroid velocity loop, the differential coefficient K3 of the centroid velocity loop, the centroid acceleration a1, and the centroid velocity error e0 at the previous control time t0. The PD controller then calculates the centroid acceleration control command. a2=K2e2+K3(e2-e0) / Δt+a1.

8. The control method according to claim 7, characterized in that, The joint angle trajectory information of the i-th joint also includes the mass m of the i-th joint. i Rotation matrix R i Moment of inertia I i The Jacobian matrix of the center of mass linear velocity J i Angular velocity Jacobian matrix Z i The Jacobian matrix of the center-of-mass linear velocity J i The transpose matrix is ​​J i T Angular acceleration Jacobian matrix Z i The transpose matrix is ​​Z i T Rotation matrix R i The transpose matrix is ​​R i T ; The joint angle trajectory information of the i-th joint also includes the joint position B1, joint angular velocity B2, joint angular acceleration B3 of the i-th joint, and the expected joint position D1, expected joint angular velocity D2, and expected joint angular acceleration D3 of the i-th joint. Then the gravity vector of the rotorcraft arm Where g is the gravitational acceleration, and the gravity vector G of the rotorcraft is the product of the following parameters of the rotorcraft from the first joint to the nth joint: mass, transpose of the angular acceleration Jacobian matrix, and gravitational acceleration. The inertia matrix of the rotorcraft arm is R2, then If 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 t1, and the next control time predicted by the disturbance observer is t2. The perturbation observer of the i-th joint includes the state vector x0; The disturbance observer of the i-th joint also includes a first prediction vector x1, a second prediction vector x2, and a first deviation vector x3 at control time t0; The disturbance observer for the i-th joint also includes a third prediction vector x at control time t1. 11 Fourth prediction vector x 22 The second deviation vector x 33 ; The disturbance observer of the i-th joint also includes a first error term coefficient λ1, a second error term coefficient λ2, and a third error term coefficient λ3; Then the error of the disturbance observer for the i-th joint at control time t1. e3 = x0 - B2; The disturbance observer of the i-th joint further includes a first error amplification function f1(e3) and a second error amplification function f2(e3); Then, the angular acceleration calculated by the disturbance observer of the i-th joint at control time t1. θ = -x3; The joint angle trajectory information of the i-th joint also includes the torque coefficient diagonal matrix R3 and current A of the joint motor of the i-th joint; The dynamic model equation for the output of the robotic arm joint controller is as follows: R2*(B3+θ)+G=R3*A; The PID controller then calculates the joint position error of the rotorcraft arm. e4 = (D1 - B1); The PID controller then calculates the joint angular velocity error of the rotorcraft arm. e5 = (D2 - B2); in Joint angular acceleration of the i-th joint B3=D3+K p e3+K d e4+K i ∑e0: Where ∑e0 is the sum of all joint position errors calculated by the PID controller of the control system up to the last control time t0, K p K is the proportional coefficient of the joint angle acceleration controller. d K represents the differential coefficient of the joint angle acceleration controller. i The integral coefficient of the joint angle acceleration controller; Then, the acceleration of the rotorcraft arm at control time t2 O = B³ + θ; Then the third prediction vector x predicted by the disturbance observer of the i-th joint at time t2 is... 11 Fourth prediction vector x 22 The second deviation vector x 33 The following equations must be satisfied:

Images