A Coordinated Motion Planning Method for Rotorcraft Flight Manipulator Based on the Utilization of Redundant Degrees of Freedom

Abstract

The present invention proposes a coordinated motion planning method for a rotor flying robotic arm based on the utilization of redundant degrees of freedom, which generates a coordinated motion trajectory that satisfies the motion constraints and minimizes the centroid offset of the system. First, the kinematic model of the robotic arm and the integrated kinematic model of the flying robotic arm are established, and the corresponding Jacobian matrix is calculated. According to the underactuated nature of the quadrotor UAV, the Jacobian matrix is decomposed into a controllable part and an uncontrollable part. Subsequently, the centroid offset model of the system is established, and the corresponding Jacobian matrix is calculated. Using a hierarchical quadratic programming framework, with the tracking accuracy of the end of the flying robotic arm as the optimization objective, a quadratic programming problem is established and solved; on this basis, the centroid offset is incorporated into the optimization objective function, a new quadratic programming problem is established and solved, and a reference motion trajectory that satisfies the end target tracking performance and minimizes the centroid offset is obtained. The present invention is applicable to aerial operation tasks such as target capture and equipment operation.

Description

Technical Field

[0001] The present invention belongs to the field of motion planning of flying robots, and particularly relates to a coordinated motion planning method for a rotary-wing flying manipulator based on the utilization of redundant degrees of freedom, which is applicable to a flying robot system equipped with a manipulator for performing active operation tasks. Background Art

[0002] In recent years, a new type of flying manipulator system composed of a rotary-wing unmanned aerial vehicle and a multi-degree-of-freedom manipulator has received extensive attention from the academic and industrial communities. Compared with the rotary-wing unmanned aerial vehicles widely used in traditional industries such as aerial photography, inspection, mapping, and plant protection, the rotary-wing flying manipulator is usually oriented towards aerial operation tasks, aiming to perform autonomous operations using the payload, achieve interaction with the external environment, and thus realize a performance leap from "observing" to "observing - operating" integration. The rotary-wing flying manipulator system usually has the characteristic of redundant degrees of freedom. The rotary-wing unmanned aerial vehicle platform is a rigid body with six degrees of freedom. When an n-degree-of-freedom manipulator is added, the degrees of freedom of the entire system become 6 + n. When the system performs a grasping task, only 3 degrees of freedom are required at the end of the manipulator. At this time, there will be countless grasping postures to choose from. At the same time, the movement of the manipulator will cause the centroid of the entire system to shift during operation, thereby affecting the control accuracy and flight performance of the system. In addition, the flying manipulator is often subject to motion constraints when performing operation tasks, such as: UAV position / velocity constraints, manipulator joint angle / angular velocity constraints, end position / velocity constraints, etc. Therefore, in order to improve the operation performance of the system and the success rate of operation tasks, it is necessary to solve the coordinated motion planning problem of the UAV platform and the manipulator joints, so as to achieve minimizing the centroid shift as much as possible while ensuring the end position and satisfying various motion constraint conditions.

[0003] Chinese invention patent CN201810477920.0 proposes a design method for a flying robot system carrying a redundant-degree-of-freedom manipulator. For the motion planning of the redundant manipulator, this invention uses quadratic programming to design the planning scheme, but there are two drawbacks: (1) The integrated motion planning of the UAV platform and the manipulator is not considered. (2) Only the end pose is selected as the optimization objective of the quadratic programming, and other optimization objectives such as adjusting the centroid shift are not considered. Chinese invention patent CN201810094313.6 proposes a rotary-wing flying manipulator system and algorithm based on dynamic centroid compensation. By establishing a centroid shift calculation model, the centroid shift amount is compensated in advance in the control system. However, this invention does not consider minimizing the centroid shift as much as possible at the motion planning layer, and has strong conservatism. Summary of the Invention

[0004] In view of the problems existing in the prior art inventions, the present invention proposes a coordinated motion planning method for a rotary-wing flying robotic arm based on the utilization of redundant degrees of freedom, which takes into account the trajectory coordination between the UAV platform and the robotic arm, and also meets the constraint conditions of various tasks at the planning level and minimizes the centroid offset of the system to the greatest extent.

[0005] To achieve the above object, the present invention adopts the following technical solutions:

[0006] A coordinated motion planning method for a rotary-wing flying robotic arm based on the utilization of redundant degrees of freedom, comprising the following steps:

[0007] First step, establish the kinematic model of the robotic arm according to the screw theory and the product of exponentials formula;

[0008] Second step, establish the integrated kinematic model of the flying robotic arm, calculate the corresponding Jacobian matrix, and decompose the kinematic model into a controllable part and an uncontrollable part;

[0009] Third step, establish the centroid offset model of the flying robotic arm in the horizontal direction, and derive the Jacobian matrix corresponding to the centroid offset change rate;

[0010] Fourth step, take the trajectory tracking accuracy of the robotic arm end and the centroid offset as optimization objectives at the same time, establish and solve the hierarchical quadratic programming problem model, and obtain the motion trajectory of the flying robotic arm that meets the end target tracking performance and minimizes the centroid offset.

[0011] Further, the first step includes:

[0012] Define that when the robotic arm is in the initial pose, the motion screw ξ of the i-th joint of the robotic arm i :

[0013]

[0014] where ω i represents the unit angular velocity vector of joint i in the base coordinate system of the robotic arm, v i represents the linear velocity vector of joint i in the base coordinate system of the robotic arm, r i represents the coordinate of any point on the axis of joint i in the base coordinate system of the robotic arm, and the initial pose is the pose with each joint angle being 0;

[0015] Convert the motion screw ξ i into matrix form

[0016]

[0017] where S(·) is the skew-symmetric matrix operator;

[0018] According to the exponential product formula, the forward kinematics homogeneous transformation matrix of the robotic arm is calculated.

[0019]

[0020] Among them, represents the rotation matrix of the end of the robotic arm relative to the base coordinate system of the robotic arm, represents the position of the end of the robotic arm in the base coordinate system of the robotic arm, n is the number of joints of the robotic arm, q i is the joint angle of joint i, is the exponential map of joint i, T0 is the homogeneous transformation matrix in the initial pose, and:

[0021]

[0022] Calculate the Jacobian matrix of the robotic arm:

[0023]

[0024] Among them, represents the motion screw of joint i in any pose. can be obtained from ξ i after the adjoint transformation operation:

[0025]

[0026] Furthermore, the second step includes:

[0027] Establish an integrated forward kinematics model of the flying robotic arm based on the homogeneous transformation principle:

[0028]

[0029] Among them, P e represents the position of the robotic arm actuator in the inertial coordinate system, R e is the rotation matrix of the end of the robotic arm in the inertial coordinate system, P b represents the position of the UAV body coordinate system in the inertial coordinate system, R b is the rotation matrix of the UAV body coordinate system relative to the inertial coordinate system.

[0030] Differentiate the above formula to obtain the differential kinematics equation of the flying robotic arm:

[0031]

[0032] Among them, Ω b and Ω e respectively represent the angular velocities of the UAV centroid and the end of the flying robotic arm in the inertial coordinate system; is the angular velocity of the end of the flying robotic arm in the body coordinate system; is the joint angular velocity vector of the robotic arm; are respectively the derivatives of P e ,P b ,R b , with respect to time.

[0033] The representation of the end of the flying robotic arm in the inertial coordinate system is obtained as:

[0034]

[0035]

[0036] where, represents the linear velocity and angular velocity of the end coordinate system of the flying robotic arm, represents the linear velocity and angular velocity of the UAV body coordinate system.

[0037] The conversion matrices between the angular velocities of the end of the robotic arm and the UAV and the Euler angular velocities are defined as T e and T b respectively. The expression of the pose of the end of the flying robotic arm in the inertial coordinate system is:

[0038]

[0039] where, x e = [P e Θ e T ,x b = [P b Θ b T ; Θ e = [φ e θ e ψ e T ,Θ b = [φ b θ b ψ b T are respectively the Euler angles of the end of the robotic arm and the UAV; φ e ,φ b are respectively the roll angles of the end of the flying robotic arm and the UAV; θ e ,θ e are respectively the pitch angles of the end of the flying robotic arm and the UAV; ψ e ,ψ b are respectively the yaw angles of the end of the flying robotic arm and the UAV.

[0040] ​​​​Since the quadrotor UAV is an underactuated system, its three-dimensional position and yaw angle channels are controllable, while the pitch angle and roll angle channels are uncontrollable. Therefore, the differential kinematic equations are rearranged to obtain:

[0041]

[0042] where ζ c =[P b ψ b q] T , ζ uc =[φ b θ b T . J c and J uc are the controllable part Jacobian matrix and the uncontrollable part Jacobian matrix obtained after rearrangement.

[0043] Furthermore, the third step includes:

[0044] Calculating the coordinates of the manipulator's center of mass vector in the body coordinate system:

[0045]

[0046] where m i , respectively represent the mass of the manipulator link i and the coordinates of its center of mass position in the body system;

[0047] Calculating the coordinates of the manipulator's center of mass in the inertial coordinate system and projecting it onto the horizontal direction:

[0048]

[0049] Differentiating the above equation with respect to the manipulator joint angle vector q to obtain

[0050] Calculating the differential mapping relationship between the 2-norm of and ζ c and the corresponding Jacobian matrix:

[0051]

[0052] where is the derivative of the 2-norm of with respect to time t, which is defined as the center of mass offset change rate.

[0053] Furthermore, the fourth step includes:

[0054] Discretizing the kinematic equations. The kinematic equations at time step k are:

[0055] ​

[0056] Obtain the desired velocity of the end of the flying robotic arm:

[0057]

[0058] where x t，k and represent the position and velocity of the target at discrete time k, represents the desired velocity of the end at discrete time k, and K p is the gain coefficient, and represent the derivatives of ζ c and ζ uc with respect to time, respectively.

[0059] To make the end of the flying robotic arm track the target position, design the optimization objective function ①:

[0060]

[0061] After expansion and simplification, we get:

[0062]

[0063] The last term of the above formula is independent of the controllable input at each discrete time k. Omitting this term, we obtain the time-varying convex quadratic programming problem model ①:

[0064]

[0065] where

[0066] Constraints are imposed on the positions and velocities of the controllable degrees of freedom of the system and the velocity of the end of the flying robotic arm as the constraint conditions of the problem model ①:

[0067]

[0068] where and are the upper and lower bounds of the velocity of the controllable input, ζ c，max and ζ c，min are the upper and lower bounds of the position of the controllable input, △T is the discrete sampling time interval, and are the upper and lower bounds of the velocity of the end of the flying robotic arm.

[0069] According to the time-varying convex quadratic programming problem model ① and its corresponding constraint conditions, find the optimal solution α 0，k for each moment;

[0070] On the basis of the end of the robotic arm tracking the desired trajectory, further minimize the centroid offset of the robotic arm. To this end, design the optimization objective function ②:

[0071]

[0072] Among them, is the desired centroid offset change rate at discrete time k;

[0073] It is calculated by the following formula:

[0074]

[0075] K G is the gain coefficient, is the projection of the centroid coordinate of the robotic arm in the inertial coordinate system in the horizontal direction.

[0076] Substituting and expanding and simplifying, we can get:

[0077]

[0078] The last term of the above formula is independent of the controllable input at each discrete time k. Omitting this term, we get the time-varying convex quadratic programming problem model ② again:

[0079]

[0080] Among them,

[0081] The quadratic programming problem model ② adopts the same constraint conditions as the quadratic programming problem ①;

[0082] On this basis, add the solution of the quadratic programming problem ① to the quadratic programming problem model ② as an equality constraint:

[0083] The constraints of the quadratic programming problem model ② are:

[0084]

[0085] According to the time-varying convex quadratic programming problem model ② and its corresponding constraint conditions, find the corresponding optimal solution α 1，k at each moment; α 1，k is the final solution of the hierarchical quadratic programming problem model, that is, the desired speed of each degree of freedom of the rotorcraft robotic arm system.

[0086] The advantages of the present invention compared with the prior art are:

[0087] The present invention involves a method for coordinated motion planning of a rotorcraft flying manipulator based on the use of redundant degrees of freedom, which is mainly aimed at a flying manipulator system composed of a rotorcraft UAV and a multi-degree-of-freedom manipulator, and can ensure the coordinated motion and mutual cooperation between the UAV platform and the manipulator, and complete various aerial operation tasks. In view of the coordinated motion planning problem under various task constraints, this method first uses the screw method and the exponential product formula to establish a kinematic model of the manipulator. Compared with the traditional DH parameter method, it requires fewer coordinate systems and parameters, reducing the amount of calculation. Then, the integrated forward kinematic equation and differential kinematic equation of the flying manipulator are constructed to obtain the corresponding Jacobian matrix, and according to the under-actuated characteristics of the quadcopter UAV, the Jacobian matrix is ​​decomposed into a controllable part and an uncontrollable part. Then, a model of the center of mass offset of the flying manipulator in the horizontal direction is established, and the corresponding Jacobian matrix is ​​obtained. In order to solve the expected motion trajectory of each degree of freedom of the flying robot arm system, this method applies the framework of hierarchical quadratic programming for the first time. First, a time-varying convex quadratic programming problem model is constructed to solve the motion trajectory that enables the end of the flying robot arm to track the target. On this basis, another time-varying convex quadratic programming problem model is constructed, and finally the motion trajectory that minimizes the center of mass deviation while satisfying the tracking target of the end of the flying robot arm is solved. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] Figure 1 A design flow chart of a method for coordinated motion planning of a rotor flight manipulator based on the use of redundant degrees of freedom proposed by the present invention;

[0089] Figure 2 This is a structural diagram of the flight robotic arm system of the present invention. DETAILED DESCRIPTION

[0090] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention is described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in each embodiment of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0091] like Figure 1 As shown, the present invention proposes a coordinated motion planning method for a rotor flight manipulator based on the use of redundant degrees of freedom, and the specific implementation steps are as follows:

[0092] The first step is to establish the kinematic model of the robotic arm based on the screw theory and the exponential product formula.

[0093] Define the motion rotation ξ of joint i when the robot is in the initial position (each joint angle is 0) i :

[0094]

[0095] where ω i represents the unit angular velocity vector of joint i in the base coordinate system of the robotic arm, and v i represents the linear velocity vector of joint i in the base coordinate system of the robotic arm, and r i represents the coordinates of an arbitrary point on the axis of joint i in the base coordinate system of the robotic arm.

[0096] Convert the motion screw ξ i into matrix form:

[0097]

[0098] where S(·) is the skew-symmetric matrix operator. According to the product of exponentials formula, the forward kinematics homogeneous transformation matrix of the robotic arm can be calculated:

[0099]

[0100] where represents the rotation matrix of the end effector of the robotic arm relative to the base coordinate system of the robotic arm, represents the position of the end effector of the robotic arm in the base coordinate system of the robotic arm, n is the number of joints of the robotic arm, and q i is the joint angle of joint i, is the exponential map of joint i, T0 is the homogeneous transformation matrix in the initial pose, and:

[0101]

[0102] The Jacobian matrix of the robotic arm can be expressed as:

[0103]

[0104] where represents the motion screw of joint i in an arbitrary pose. can be obtained from ξ i after the adjoint transformation operation:

[0105]

[0106] Second, for Figure 2 the designed structure of the flying robotic arm, assuming that the base coordinate system of the robotic arm coincides with the body coordinate system of the unmanned aerial vehicle, establish an integrated forward kinematics model of the flying robotic arm based on the homogeneous transformation principle:

[0107]

[0108] In the formula, P e represents the position of the robotic arm actuator in the inertial coordinate system, and Re is the rotation matrix of the end of the robotic arm in the inertial coordinate system; P b represents the position of the UAV body coordinate system in the inertial coordinate system, R b is the rotation matrix of the UAV body coordinate system relative to the inertial coordinate system;

[0109] Differentiating the above equation, the differential kinematic equation of the flying robotic arm can be obtained:

[0110]

[0111] where, Ω b and Ω e respectively represent the angular velocities of the center of mass of the UAV and the end of the flying robotic arm in the inertial coordinate system; is the angular velocity of the end of the flying robotic arm in the body coordinate system; is the joint angular velocity vector of the robotic arm; are respectively the derivatives of P e , P b , R b , with respect to time.

[0112] Furthermore, the representation of the end of the flying robotic arm in the inertial coordinate system can be obtained:

[0113]

[0114]

[0115] where, represents the linear velocity and angular velocity of the end coordinate system of the flying robotic arm, represents the linear velocity and angular velocity of the UAV body coordinate system.

[0116] Define the conversion matrices between the angular velocities of the end of the robotic arm and the UAV and the Euler angular velocities as T e and T b , and the expression form of the pose of the end of the flying robotic arm in the inertial coordinate system can be obtained:

[0117]

[0118] where, x e = [P e Θ e T , x b = [P b Θ b T ; Θ e = [φ e θ e ​​ψ e T , Θ b = [φ b θ b ψ b T are the Euler angles of the end of the robotic arm and the UAV respectively; φ e , φ b are the roll angles of the end of the flying robotic arm and the UAV respectively; θ e , θ e are the pitch angles of the end of the flying robotic arm and the UAV respectively; ψ e , ψ b are the yaw angles of the end of the flying robotic arm and the UAV respectively.

[0119] Rearranging the differential kinematic equation, we can get:

[0120]

[0121] where, ζ c = [P b ψ b q] T , ζ uc = [φ b θ b T . J c and J uc are the controllable part Jacobian matrix and the uncontrollable part Jacobian matrix obtained after rearrangement.

[0122] Step 3: Establish the centroid offset model of the flying robotic arm in the horizontal direction.

[0123] The centroid vector of the robotic arm can be expressed in the body coordinate system as:

[0124]

[0125] where, m i , represent the mass of the i-th link of the robotic arm and the coordinates of its centroid position in the body coordinate system respectively. Calculate the coordinates of the centroid of the robotic arm in the inertial coordinate system and project it onto the horizontal direction:

[0126]

[0127] Differentiating the above equation with respect to the robotic arm joint angle vector q, we can get Further calculate the 2-norm of and the differential mapping relationship between ζ c and the corresponding Jacobian matrix:

[0128] ​​​

[0129] Among them, is the derivative of the 2-norm with respect to time t, which can be defined as the centroid offset change rate.

[0130] In the fourth step, construct a hierarchical quadratic programming problem model to solve the desired motion trajectories of each degree of freedom of the system.

[0131] First, discretize the discrete kinematic equation:

[0132]

[0133] Solve for the desired velocity at the end of the flying manipulator:

[0134]

[0135] where x t，k and represent the position and velocity of the target at discrete time k, represents the desired velocity at the end at discrete time k, K p is the gain coefficient, and respectively represent the derivatives of ζ c and ζ uc with respect to time.

[0136] In order to make the end of the flying manipulator track the target position, design the optimization objective function ①:

[0137]

[0138] After expanding and simplifying, we can get:

[0139]

[0140] The last term of the above formula is independent of the controllable input at each discrete time k. After omitting this term, we can obtain the time-varying convex quadratic programming problem model ①:

[0141]

[0142] where The constraint conditions of the problem model ① are set as:

[0143]

[0144] where and are the upper and lower bounds of the velocity of the controllable input, ζ c，max and ζ c，minare the upper and lower bounds of the position of the controllable input, and △T is the discrete sampling time interval. and are the upper and lower bounds of the end velocity of the flying manipulator.

[0145] According to the time-varying convex quadratic programming problem model ① and its corresponding constraint conditions, the corresponding optimal solution α can be obtained at each moment. 0，k .

[0146] On the basis of the end of the manipulator tracking the desired trajectory, the centroid offset of the manipulator is further minimized. For this purpose, the optimization objective function ② is designed:

[0147]

[0148] where is the desired centroid offset change rate at discrete time k. During the execution of the task by the rotary-wing flying manipulator, it is always desired that the centroid offset be as small as possible. Therefore can be calculated by the following formula:

[0149]

[0150] K G is the gain coefficient, is the projection of the centroid coordinate of the manipulator in the horizontal direction in the inertial coordinate system.

[0151] Substituting and expanding and simplifying, we can get:

[0152]

[0153] and the optimization function are similar. The last term at each discrete time k is independent of the controllable input After omitting this term, the time-varying convex quadratic programming problem model ② can be obtained again:

[0154]

[0155] where

[0156] The quadratic programming problem model ② adopts the same constraint conditions as the quadratic programming problem ①, and on this basis, the solution of the quadratic programming problem ① is added to the quadratic programming problem model ② as an equality constraint: Therefore, the constraints of the quadratic programming problem model ② can be expressed as:

[0157]

[0158] According to the time-varying convex quadratic programming problem model ② and its corresponding constraints, the corresponding optimal solution α can be obtained at each moment. 1，k α 1，k is the final solution of the hierarchical quadratic programming problem model, that is, the expected velocities of each degree of freedom of the rotor flying manipulator system.

[0159] The content not described in detail in the specification of the present invention belongs to the prior art well-known to those skilled in the art.

[0160] It is easy for those skilled in the art to understand that the above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A coordinated motion planning method for a rotor flight manipulator based on the utilization of redundant degrees of freedom, characterized in that, It includes the following steps: First step: According to the screw theory and the exponential product formula, establish the kinematic model of the robotic arm, including: Define the kinematic screw of the joint of the robotic arm when the robotic arm is in the initial pose : ； Among them, represents the unit angular velocity vector of the joint in the base coordinate system of the robotic arm, represents the linear velocity vector of the joint in the base coordinate system of the robotic arm, represents the coordinates of any point on the axis of rotation of the joint in the base coordinate system of the robotic arm. The initial pose is the pose when the joint angles of the robotic arm are ; Convert the motion screw into matrix form : ； Among them, , is an anti-symmetric matrix operator; According to the exponential product formula, the forward kinematics homogeneous transformation matrix of the robotic arm is calculated : ； Among them, represents the rotation matrix of the end - effector of the robotic arm with respect to the base coordinate system of the robotic arm, represents the position of the end - effector of the robotic arm in the base coordinate system of the robotic arm, is the number of joints of the robotic arm, is the joint angle, is the joint exponential map, is the homogeneous transformation matrix of the robotic arm in the initial pose, and: ； Calculate the Jacobian matrix of the robotic arm : ； Among them, represents the kinematic screw of the joint at any pose, which is obtained after the adjoint transformation operation: ； Second step: Establish the integrated kinematic model of the flying robotic arm, calculate the corresponding Jacobian matrix, and decompose the kinematic model into a controllable part and an uncontrollable part; Third step: Establish the horizontal center-of-mass offset model of the flying robotic arm, and derive the Jacobian matrix corresponding to the change rate of the center-of-mass offset; Fourth step: Take the trajectory tracking accuracy of the robotic arm end and the center-of-mass offset as optimization objectives at the same time, establish and solve the hierarchical quadratic programming problem model, and obtain the motion trajectory of the flying robotic arm that meets the end target tracking performance and minimizes the center-of-mass offset.

2. The coordinated motion planning method for a rotor flight manipulator based on the utilization of redundant degrees of freedom according to claim 1, characterized in that, The second step includes: Establish the integrated forward kinematic model of the flying robotic arm based on the homogeneous transformation principle: ； Among them, represents the position of the robotic arm actuator in the inertial coordinate system, is the rotation matrix of the end of the robotic arm in the inertial coordinates, represents the position of the UAV body coordinate system in the inertial coordinate system, is the rotation matrix of the UAV body coordinate system relative to the inertial coordinate system; Take the derivative of the above formula with respect to time to obtain the differential kinematic equation of the flying robotic arm: ； wherein, and respectively represent the angular velocities of the centroid of the drone and the end of the flying robotic arm in the inertial coordinate system; is the angular velocity of the end of the flying robotic arm in the body coordinate system; is the joint angular velocity vector of the robotic arm; , , , are respectively , , , the derivatives with respect to time; Further obtain the representation of the end of the flying robotic arm in the inertial coordinate system: ； ； Among them, represents the linear velocity and angular velocity of the end coordinate system of the flying robotic arm, represents the linear velocity and angular velocity of the UAV body coordinate system; Define the conversion matrices between the angular velocity of the end of the robotic arm and the Euler angular velocity of the drone as and , and the expression of the pose of the end of the flying robotic arm in the inertial coordinate system is obtained as: ； Among them, , ; , are respectively the Euler angles of the end of the robotic arm and the drone; , are respectively the roll angles of the end of the flying robotic arm and the drone; , are respectively the pitch angles of the end of the flying robotic arm and the drone; , are respectively the yaw angles of the end of the flying robotic arm and the drone; Since the quadrotor UAV is an underactuated system, its three-dimensional position and yaw angle channels are controllable, while the pitch angle and roll angle channels are uncontrollable. Therefore, rearrange the differential kinematic equation to obtain: ； Among them, , , and are the controllable part Jacobian matrix and the uncontrollable part Jacobian matrix obtained after re - arrangement.

3. The coordinated motion planning method of a rotor flying robotic arm based on the utilization of redundant degrees of freedom according to claim 2, characterized in that The third step includes: Calculate the coordinates of the centroid vector of the robotic arm in the body coordinate system : ； Among them, , respectively represent the mass of the robotic arm link and the coordinates of its center of mass position in the body coordinate system; Calculate the coordinates of the center of mass of the robotic arm in the inertial coordinate system and project it onto the horizontal direction: ； Differentiate the above equation with respect to the robotic arm joint angle vector to obtain ; Calculate the differential mapping relationship between the 2-norm of and and the corresponding Jacobian matrix: ； Among them, is the derivative of the 2-norm with respect to time , which is defined as the rate of change of the centroid offset.

4. The coordinated motion planning method of a rotor flying robotic arm based on the utilization of redundant degrees of freedom according to claim 3, characterized in that The fourth step includes: Discretize the kinematic equation, The kinematic equation at the moment is: ； Obtain the desired velocity of the end of the flying robotic arm: ； Among them, and represent the position and velocity of the target at discrete time . represents the desired end velocity at discrete time . is the gain coefficient, and respectively represent and at discrete time the derivatives with respect to time; In order to make the end of the flying robotic arm track the target position, design the optimization objective function ①: ； After expansion and simplification, it is obtained: ； The last term of the above formula At each discrete time Is independent of the controllable input All irrelevant, omit this term, and obtain the time-varying convex quadratic programming problem model ①: ； Among them, , , ; Constrain the positions and velocities of the controllable degrees of freedom of the system and the velocity of the end of the flying robotic arm as the constraint conditions of the problem model ①: ； wherein, and are the upper and lower bounds of the speed of the controllable input, and are the upper and lower bounds of the position of the controllable input, is the discrete sampling time interval, and are the upper and lower bounds of the speed of the end of the flying robotic arm; According to the time-varying convex quadratic programming problem model ① and its corresponding constraint conditions, find the optimal solution corresponding to each moment ; On the basis of the robotic arm end tracking the desired trajectory, further minimize the center-of-mass offset of the robotic arm. For this purpose, design the optimization objective function ②: ； wherein, is the expected centroid offset change rate at discrete moments ; Calculated by the following formula: ； Among them, is the gain coefficient, is the projection of the centroid coordinate of the robotic arm in the inertial coordinate system in the horizontal direction; After expanding and simplifying we get: ； The last term of the above formula At each discrete time is independent of the controllable input and this term is omitted. The time-varying convex quadratic programming problem model ② is obtained again as follows: ； Among them, , , ; The quadratic programming problem model ② adopts the same constraint conditions as the quadratic programming problem ①; On this basis, the solution of the quadratic programming problem ① is added to the quadratic programming problem model ② as an equality constraint: ; The constraints of the quadratic programming problem model ② are: ； According to the time-varying convex quadratic programming problem model ② and its corresponding constraint conditions, the corresponding optimal solution is obtained at each moment. is the final solution of the hierarchical quadratic programming problem model, that is, the expected velocities of each degree of freedom of the rotorcraft robotic arm system.

Images