Aerial manipulator system dynamics feasible trajectory implementation system and method

Abstract

The invention relates to a system and a method for realizing a dynamics feasible track of an air manipulator system, the system comprises a system state track inversion module and a dynamics feasible track optimization solving module, the system state track inversion module obtains a state track of the air manipulator system through system kinematics inverse solution according to a specified actuator task track; and the dynamics feasible trajectory optimization solving module obtains a dynamics feasible trajectory through numerical optimization solving according to the system differential algebraic equation constraint. According to the method, one-to-one mapping from the flat output of the system to the full state of the actuator is determined, so that the smoothness requirement of converting the dynamics feasibility into the flat output is realized, and the feasible state track of the system dynamics is determined by solving the optimization problem.

Description

Technical Field

[0001] The present invention relates to a technology in the field of robot control, specifically a system and method for realizing a dynamically feasible trajectory of an aerial manipulator system based on the inverse mapping of the actuator pose. Background Art

[0002] Existing aerial manipulators are mostly combinations of rotary-wing unmanned aerial vehicles (UAVs) and manipulators. However, quadrotor UAVs have underactuated characteristics, and there are complex dynamic coupling effects between the manipulator and the UAV, resulting in the fact that any arbitrarily specified actuator trajectory may not be dynamically feasible for the entire system, thereby affecting the operation and control accuracy of the actuator. Summary of the Invention

[0003] In view of the limitation that the prior art can only achieve kinematic pose inverse solution at the position and attitude levels or achieve dynamic trajectory inverse solution at the cost of sacrificing the pose freedom of the actuator when inverse-solving the state trajectory of the aerial manipulator system according to the actuator task trajectory, the present invention proposes a system and method for realizing a dynamically feasible trajectory of an aerial manipulator system. By determining a one-to-one mapping from the system flat output to the full state of the actuator, the dynamic feasibility is transformed into the smoothness requirement of the flat output, and the dynamically feasible state trajectory of the system is determined by solving an optimization problem.

[0004] The present invention is realized through the following technical solutions:

[0005] The present invention relates to a system for generating a dynamically feasible trajectory of an aerial manipulator system, including: a system state trajectory inversion module and a dynamically feasible trajectory optimization and solution module, wherein: the system state trajectory inversion module obtains the state trajectory of the aerial manipulator system through kinematic inverse solution according to the specified actuator task trajectory; the dynamically feasible trajectory optimization and solution module obtains the dynamically feasible trajectory through numerical optimization and solution according to the constraints of the system differential-algebraic equations.

[0006] The system state trajectory inversion module includes: a rotation matrix reparameterization unit, a manipulator joint angle inversion unit, a UAV yaw angle inversion unit, and a system centroid position inversion unit, wherein: the rotation matrix reparameterization unit describes the body rotation matrix in the form of angle-axis description, through the lift direction vector and the rotation angle around this direction; the manipulator joint angle inversion unit and the UAV yaw angle inversion unit respectively calculate the expected angles of the manipulator joints and the expected yaw angle of the UAV by analogy with the process of solving Euler angles from the rotation matrix equation formed by the expected attitude of the actuator and the body attitude; the system centroid position inversion unit obtains the expected position of the centroid of the aerial manipulator by solving the kinematic equation formed by the pose of the actuator and the system centroid according to the obtained expected angles of the manipulator joints.

[0007] The described dynamic feasible trajectory optimization and solution module includes: a system differential-algebraic equation constraint unit, a polar coordinate parameterization unit for the lift vector, a parameterization unit for the polar coordinate description angle, and a dynamic feasible trajectory solution unit based on numerical optimization. Among them: The system differential-algebraic equation constraint unit derives the differential-algebraic equations that the aerial manipulator needs to satisfy through dynamic modeling and analysis according to the underactuated characteristics of the quadrotor UAV and the relative motion relationship between the manipulator and the system; The polar coordinate parameterization unit for the lift vector realizes the parameterization of the motion trajectory of the lift direction through the direction angle and polar angle in the three-dimensional spherical coordinates according to the motion characteristics of the lift vector restricted to move on the three-dimensional unit sphere; The parameterization unit for the polar coordinate description angle realizes the parameterization of the azimuth angle and polar angle trajectories through the amplitude and angular frequency of the finite-order trigonometric functions according to the general representation ability of the trigonometric series for general functions; The dynamic feasible trajectory solution unit based on numerical optimization generates a system dynamic feasible trajectory that satisfies the differential-algebraic equation constraints by first searching in the low-dimensional parameter space composed of the trigonometric series and then fine-tuning in the high-dimensional parameter space composed of the entire lift vector trajectory. Technical effects

[0008] The present invention establishes a theoretical mapping relationship from the six-degree-of-freedom pose of the actuator to the full state of the aerial manipulator system, and proposes a method of parameterizing the lift direction trajectory represented in polar coordinates through trigonometric series and solving the differential-algebraic equations through numerical optimization, and then generating the full state trajectory of the system. Compared with the prior art, the present invention can achieve high-precision numerical solution of the full state trajectory of the aerial manipulator system when a six-degree-of-freedom pose trajectory of the actuator is given, and ensures that the obtained trajectory is dynamically feasible for the entire aerial manipulator system, thus providing a reliable reference trajectory for the precise control of the actuator. Brief description of the drawings

[0009] Figure 1 It is a schematic diagram of the system of the present invention;

[0010] Figure 2 It is a coordinate system description diagram of the aerial manipulator;

[0011] Figure 3 It is a schematic diagram of inverting the full state trajectory of the aerial manipulator according to the actuator task trajectory;

[0012] In the figure: The blue dashed line is the full state trajectory of the system including the UAV pose and the manipulator joint angles, the blue curved surface is the full state space of the aerial manipulator system, the orange dashed line is the task trajectory including the actuator pose, the orange plane is the actuator pose space, and the red arrow indicates the inversion process from the actuator pose task trajectory to the system full state trajectory;

[0013] Figure 4Schematic diagram of the system state trajectory obtained under the pose trajectory of the figure-eight position and attitude rotation actuator;

[0014] In the figure: (a) is the trajectory of the actuator position and the system centroid position, and (b) is the trajectory of the robotic arm joint and the body yaw;

[0015] Figure 5 Schematic diagram of the trajectory of the body lift direction under the figure-eight actuator task trajectory;

[0016] Figure 6 Schematic diagram of the complete trajectory of the aerial robotic arm in three-dimensional space under the figure-eight actuator task trajectory;

[0017] Figure 7 Schematic diagram of the system state trajectory obtained under the pose trajectory of the circular position and attitude rotation actuator;

[0018] In the figure: (a) is the trajectory of the actuator position and the system centroid position, and (b) is the schematic diagram of the trajectory of the robotic arm joint and the body yaw;

[0019] Figure 8 Schematic diagram of the trajectory of the body lift direction under the circular actuator task trajectory;

[0020] Figure 9 Schematic diagram of the complete trajectory of the aerial robotic arm in three-dimensional space under the circular actuator task trajectory. Detailed implementation manner

[0021] As Figure 1 shown, this embodiment relates to a method for realizing the dynamic feasible trajectory of an aerial robotic arm system based on a system. By determining the one-to-one mapping from the system flat output to the full state of the actuator, the dynamic feasibility of the actuator trajectory is transformed into the flat output smoothness and the system differential-algebraic equation constraints, and then the optimization problem is solved to determine the system dynamic feasible state trajectory, which specifically includes:

[0022] Step 1) Establish the coordinate system of the aerial robotic arm: To accurately describe the motion and state of the aerial robotic arm, as Figure 2 shown, establish a suitable space coordinate system to describe the motion states of the quadrotor UAV, the robotic arm, and the aerial robotic arm system in the state space, and describe the motion states of the robotic arm actuators in the task space.

[0023] The described space coordinate system includes: the inertial coordinate system {O I}, the body coordinate system {O B}, and the i-th link coordinate system of the robotic arm {O i}, and all coordinate systems follow the right-hand rule.

[0024] The motion state of the quadrotor UAV described in the state space includes: the position of the UAV's center of mass The rotation matrix R I from the inertial frame {O B} to the body frame {O B} ∈ SO(3) and the body yaw angle ψ; the motion state of the robotic arm includes: the angle of each joint which is restricted to move on the unit circle and the combined vector representation formed by all joint angles where: is the n-fold Cartesian product of; the motion state of the aerial robotic arm system includes: the position of the system's center of mass

[0025] In this embodiment, the number of robotic arm joints n = 2.

[0026] The motion state of the robotic arm actuator described in the task space includes: the actuator position and the attitude rotation matrix R e ∈ SO(3).

[0027] Step 2) As shown in Figure 3 , the process of inversely deriving the full state trajectory of the aerial robotic arm system from the actuator task trajectory aims to inversely derive the UAV pose and robotic arm joint angles that meet the dynamic feasibility of the aerial robotic arm based on the given actuator pose. Specifically: After reparameterizing the body rotation matrix using the body direction vector and the rotation angle around this axis, given the actuator pose and the lift direction vector, inverse mapping expressions for the robotic arm joint angles, body yaw angle, and system center of mass position are constructed in sequence, specifically including:

[0028] 2.1 Reparameterization of the body rotation matrix: Using the body lift direction vector t = [t1 t2 t3] T and the rotation angle ψ z around this axis as independent variables, the body rotation matrix R B is described in the form of an axis-angle. The rotation process from the body frame to the inertial frame is decomposed into two steps, specifically: R B (t, ψ z ) = R2(t)R1(ψ z ), where: represents the rotation matrix for the first step of rotating ψ z angles around the z-axis of the body frame; represents the rotation matrix for rotating to the inertial frame on the basis of the first step of rotation.

[0029] 2.2 Reconstruct the joint angles of the robotic arm given the pose of the actuator and the lift direction vector: Briefly denote the product of the two matrix attitude matrices as The rotation angle ψ about the lift direction t can be determined according to the steps of solving Euler angles from the known rotation matrix z = arctan(a 21 / a 11 ), and the two robotic arm joint angles are θ1 = arcsin(-a 31 ) and θ2 = arctan(a 32 / a 33 ), where: is the transpose of the R2(t) matrix; R e is the actuator attitude rotation matrix; a ij is the element corresponding to the i-th row and j-th column in matrix A.

[0030] 2.3 Given the pose of the actuator and the lift direction vector, solve the yaw angle of the airframe: Briefly denote the product of the two rotation matrices as R2(t)R1(ψ z ) = B, and then by analogy with the process of solving Euler angles from the known rotation matrix, the yaw angle of the airframe ψ = arctan(b 21 / b 11 ) can be determined, where: b ij is the element corresponding to the i-th row and j-th column in matrix B.

[0031] 2.4 Given the pose of the actuator and the lift direction vector, solve the position of the system centroid: Since the joint angles of the robotic arm have been obtained, the rotation matrix B from {O1} to {O } and the rotation matrix R 1 2 from {O2} to {O1} are both known. At the same time, the expression of the offset vector from the centroid of the first link of the robotic arm to O1 in the {O1} coordinate system The expression of the offset vector from O1 to O B in the {O1} coordinate system The expression of the offset vector from the centroid of the second link to O2 in the {O2} coordinate system The expression of the offset vector from O2 to O1 in the {O2} coordinate system These quantities are only related to the lengths of the robotic arm links and are all known quantities. Therefore, the position of the system centroid can be solved according to the kinematic relationship of the robotic arm: where: p c is the position of the system centroid; p e is the position of the actuator; m is the total mass of the system; m b is the mass of the UAV; m1, m2 are the masses of the two links.

[0032] In this embodiment, the mass of the quadrotor UAV is m b = 0.5 kg, the masses of the two robotic arm linkages are m1 = m2 = 0.25 kg, and the lengths of the two linkages are 0.25 m.

[0033] Step 3) The pose trajectories of the actuators with six degrees of freedom in total, including: parameterization of the actuator position trajectory and parameterization of the actuator attitude Euler angle trajectory, specifically including:

[0034] 3.1 Construct the actuator attitude rotation matrix according to the Z - Y - X Euler angle sequence Where: R z (ψ e ) represents the rotation matrix for rotating by an angle ψ around the z - direction; e The rotation matrix; R y (θ e ) represents the rotation matrix for rotating by an angle θ around the y - direction; R e The rotation matrix; R x (φ e ) represents the rotation matrix for rotating by an angle φ around the x - direction. e The rotation matrix.

[0035] 3.2 Parameterization of the actuator position trajectory, including: the figure - eight trajectory p e (t) = [r sin(ωt) r cos(2ωt) 0] T and the circular trajectory p e (t) = [r sin(ωt) r cos(2ωt) 0] T .

[0036] In this embodiment, the trajectory radius is r = 8 m, and the angular velocity is ω = π / 5 rad / s.

[0037] 3.3 Parameterization of the actuator attitude trajectory, that is, specifying the three Euler angle trajectories as uniform rotations: taking the yaw direction as an example, specifying Where: ω ψe is the rotational angular velocity, ψ e0 is the initial angular position, and the Euler angles in the other two directions are parameterized in a similar manner The corresponding rotational angular velocity is The initial angular position is

[0038] In this embodiment, the angular velocities corresponding to the three - direction Euler angles of the actuator are The initial value angular position is

[0039] Step 4) Add system differential - algebraic equation constraints, specifically including:

[0040] 4.1 The lift direction should always be perpendicular to the plane where the body propeller is located. Therefore, it satisfies the following relationship with the centroid acceleration of the system: Where: refers to a unit sphere in three-dimensional space, g is the acceleration due to gravity, and z W is the coordinate basis in the z direction of the inertial system, is the centroid acceleration of the system.

[0041] In this embodiment, the acceleration due to gravity g = 9.81 m / s 2 .

[0042] 4.2 Caused by the interaction between the movement of the robotic arm and the movement of the quadrotor UAV, that is, the centroid position p of the system c and the actuator position p e satisfy the following relationship: It is consistent with the inverse solution process of the centroid position of the system in step 2).

[0043] Step 5) Parameterization of the lift direction trajectory, specifically: using the azimuth angle and the polar angle θ t to parameterize the lift direction vector Using trigonometric series to parameterize the azimuth angle and the polar angle, and parameterizing the trajectory of the angle through the sine and cosine bases: Where: is the fundamental frequency of the trigonometric series corresponding to the polar angle and the azimuth angle; are the cosine and sine amplitudes of the trigonometric series corresponding to the polar angle and the azimuth angle respectively; the parameter set is the parameter to be optimized in the angle trajectory.

[0044] In this embodiment, the order of the trigonometric series is determined to be N t = 4.

[0045] Step 6) Construct a numerical optimization problem and solve the system dynamics feasible trajectory, specifically including:

[0046] 6.1 The constructed numerical optimization problem is specifically: Where: N T is the number of sampling points of the system trajectory within the entire time period T; t i is the sampling time; is the acceleration of the system centroid, approximately obtained by performing second-order numerical differentiation on the system centroid position p c ; the system centroid position p c is obtained by inverse solution in step 2).

[0047] 6.2 After numerically optimizing and solving the lift direction trajectory that satisfies the system differential-algebraic constraints in step 4), according to the inverse solution method from the actuator pose to the system state in step 2), the complete state trajectory of the entire aerial manipulator system is determined.

[0048] In this embodiment, on the trajectory with a total duration of T = 10 s, a total of N T = 100 discrete points are sampled.

[0049] To evaluate the dynamic feasibility of the system's complete trajectory, evaluate the cost value of the objective function of the optimization problem established in step 6), and at the same time, compare the error between the actual actuator pose trajectory calculated forward from the system's complete trajectory obtained by optimization and the specified actuator pose trajectory.

[0050] The evaluation of the cost value of the objective function of the optimization problem is to calculate the dynamic error e t (t) between the actually solved lift direction and the lift direction that theoretically conforms to dynamic feasibility according to the following formula: where: t ＊ (t) is the lift direction trajectory obtained in step 6); is the acceleration of the system's center of mass obtained.

[0051] The error between the actual actuator pose trajectory calculated forward from the system's complete trajectory obtained by optimization and the previously specified actuator pose trajectory can directly calculate the actuator position error as The error of the actuator attitude rotation matrix is where: is the specified actuator position and attitude trajectory; p e (t), R e (t) is the actual actuator position and attitude trajectory calculated according to the solved lift direction; tr() is the trace of the matrix.

[0052] Through specific simulation experiments, in the MATLAB numerical simulation environment, for the two scenarios specified in step 3), establish the numerical optimization problem constructed in step 6), and use the parametric technique of trigonometric series for the lift direction vector in step 5) to solve the lift direction trajectory and the system center-of-mass motion trajectory that satisfy the system differential-algebraic equation constraints in step 4). Finally, according to step 2), given the lift direction vector and the actuator full pose task trajectory, obtain the motion trajectory of the entire aerial manipulator system in the state space. Scenario 1: The actuator position trajectory is an eight-shaped trajectory, and the actuator attitude trajectory is the rotation matrix corresponding to three uniformly rotating Euler angles. Scenario 2: The actuator position trajectory is a circular-shaped trajectory, and the actuator attitude trajectory is the rotation matrix corresponding to three uniformly rotating Euler angles.

[0053] In the embodiment of Scenario 1: Based on the present invention, the full-state motion trajectory of the aerial manipulator under the figure-eight task trajectory is obtained, where: As Figure 4 (a) shows, the blue solid line is the motion trajectory of the centroid position of the quadrotor UAV, and the black dashed line is the motion trajectory of the centroid position of the aerial manipulator system. The three figures from left to right are the position trajectories corresponding to the x, y, and z channels respectively. As Figure 4 (b) shows, from left to right are the yaw angle of the quadrotor UAV and the angle trajectories corresponding to the rotation angles of the two manipulator joints respectively. As Figure 5 shown, the green dot is the initial direction of the lift force of the quadrotor UAV, the red dot is the termination direction of the trajectory, and the blue solid line on the unit sphere is the evolution process of the corresponding lift force direction during the entire motion of the aerial manipulator. Its motion process is consistent with the motion pattern of the figure-eight trajectory. As Figure 6 shown, the blue curve is the motion trajectory of the centroid position of the quadrotor UAV, the black dashed line is the motion trajectory of the centroid position of the aerial manipulator system, and the red solid line is the motion trajectory of the actuator position.

[0054] In the embodiment of Scenario 2: Based on the present invention, the full-state motion trajectory of the aerial manipulator under the circular task trajectory is obtained. The trajectory meanings are the same as those in Scenario 1. As Figure 7 (a) shows, the blue solid line and the black dashed line are respectively the motion trajectories of the centroid positions of the quadrotor UAV and the aerial manipulator system. The three figures from left to right are the position trajectories corresponding to the x, y, and z channels respectively. As Figure 7 (b) shows, from left to right are the yaw angle of the UAV and the angle trajectories corresponding to the rotation angles of the manipulator joints respectively. As Figure 8 shown, the green and red dots are the start and termination directions of the trajectory, and the blue solid line on the sphere is the evolution process of the lift force direction. Its motion process is consistent with the motion pattern of the circular trajectory. As Figure 9 shown, this figure shows the complete motion state of the aerial manipulator in three-dimensional space under the circular task trajectory.

[0055] To further evaluate the dynamic feasibility of the method of the present invention for obtaining the system state trajectory according to the actuator task trajectory, and to test the position error and attitude error between the specified pose trajectory of the actuator and the actual actuator pose trajectory calculated based on the solved lift force direction vector. The experimental results are shown in Tables 1 and 2.

[0056] Table 1: Numerical solution accuracy of the differential algebraic equation for the lift force direction

[0057] Table 2: Pose error between the actual actuator trajectory and the specified trajectory

[0058] In summary, when all six - degree - of - freedom position and attitude trajectories of the given actuator are provided, the present invention can obtain a system - state trajectory that meets the dynamic feasibility of the under - actuated aerial manipulator composed of a quad - rotor UAV and a multi - degree - of - freedom robotic arm. Among them: the mean - square error of the lift - direction vector trajectory is at the order of magnitude of 10 -15 , and at the same time, the mean - square error of the position trajectory between the actual actuator pose trajectory calculated forward according to the inversion state and the specified trajectory is at the order of magnitude of 10 -6 ~10 -4 m, and the error of the actuator attitude rotation matrix is at the order of magnitude of 10 -9 ~10 -6 . This indicates that the high - precision inversion state trajectory has good dynamic feasibility.

[0059] Specific implementations can be locally adjusted in different ways by those skilled in the art without departing from the principles and purposes of the present invention. The protection scope of the present invention is defined by the claims and is not limited by specific implementations. All implementation solutions within its scope are subject to the present invention.

Claims

1. A dynamic feasible trajectory generation system for an aerial robotic arm system, characterized in that, Comprising: A system state trajectory inversion module and a dynamic feasible trajectory optimization and solution module, where: the system state trajectory inversion module obtains the system state trajectory of the aerial manipulator system through inverse kinematic solution of the system according to the specified actuator task trajectory; the dynamic feasible trajectory optimization and solution module obtains the dynamic feasible trajectory through numerical optimization and solution according to the system differential-algebraic equation constraints.

2. The dynamic feasible trajectory generation system of the aerial manipulator system according to claim 1, characterized in that, The system state trajectory inversion module described above includes: a rotation matrix reparameterization unit, a manipulator joint angle inversion unit, a UAV yaw angle inversion unit, and a system centroid position inversion unit, where: the rotation matrix reparameterization unit describes the body rotation matrix in terms of the angle-axis description method through the lift direction vector and the rotation angle about this direction; the manipulator joint angle inversion unit and the UAV yaw angle inversion unit respectively calculate the desired joint angles of the manipulator and the desired yaw angle of the UAV according to the rotation matrix equation formed by the desired actuator attitude and the body attitude by analogy with the process of solving Euler angles from a known rotation matrix; the system centroid position inversion unit obtains the desired position of the centroid of the aerial manipulator by solving the kinematic equation formed by the actuator pose and the system centroid according to the desired joint angles of the manipulator obtained.

3. The dynamic feasible trajectory generation system of the aerial manipulator system according to claim 1, characterized in that The dynamic feasible trajectory optimization and solution module described above includes: a system differential-algebraic equation constraint unit, a polar coordinate parameterization unit of the lift vector, a polar coordinate description angle parameterization unit, and a dynamic feasible trajectory solution unit based on numerical optimization, where: the system differential-algebraic equation constraint unit derives the differential-algebraic equations that the aerial manipulator needs to satisfy through dynamic modeling and analysis according to the underactuated characteristics of the quadrotor UAV and the relative motion relationship between the manipulator and the system; the polar coordinate parameterization unit of the lift vector realizes the parameterization of the motion trajectory of the lift direction through the direction angle and polar angle in the three-dimensional spherical coordinates according to the motion characteristics of the lift vector restricted to move on the three-dimensional unit sphere; the polar coordinate description angle parameterization unit realizes the parameterization of the azimuth angle and polar angle trajectories through the amplitude and angular frequency of finite-order trigonometric functions according to the general representation ability of trigonometric series for general functions; the dynamic feasible trajectory solution unit based on numerical optimization generates the system dynamic feasible trajectory that satisfies the differential-algebraic equation constraints by first searching in the low-dimensional parameter space formed by trigonometric series and then fine-tuning in the high-dimensional parameter space formed by the entire lift vector trajectory.

4. A method for generating a dynamically feasible trajectory of an aerial robotic arm system according to any one of the systems described in claims 1-3, characterized in that, By determining the one-to-one mapping from the system flat output to the full state of the actuator, the dynamic feasibility of the actuator trajectory is transformed into the smoothness of the flat output and the system differential-algebraic equation constraints, and then the optimization problem is solved to determine the system dynamic feasible state trajectory.

5. The method for generating a dynamically feasible trajectory of the aerial manipulator system according to claim 4, characterized in that specifically Comprising: Step 1) Establish the coordinate system of the aerial manipulator: To accurately describe the motion and state of the aerial manipulator, establish a suitable spatial coordinate system, describe the motion states of the quadrotor UAV, the manipulator, and the aerial manipulator system in the state space, and describe the motion state of the manipulator actuator in the task space. Step 2) The process of inverting the full-state trajectory of the aerial manipulator system from the actuator task trajectory aims to invert the UAV pose and manipulator joint angles that are dynamically feasible for the aerial manipulator based on the given actuator pose. Specifically: After re-parameterizing the body rotation matrix using the body direction vector and the rotation angle about this axis, given the actuator pose and the lift direction vector, inverse mapping expressions for the manipulator joint angles, body yaw angle, and system centroid position are constructed successively. Step 3) The pose trajectory of the actuator with six degrees of freedom in total, including: parameterization of the actuator position trajectory and parameterization of the actuator attitude Euler angle trajectory. Step 4) Add system differential-algebraic equation constraints. Step 5) Parameterization of the lift direction trajectory. Step 6) Construct a numerical optimization problem and solve for the dynamically feasible trajectory of the system. The described space coordinate system includes: an inertial coordinate system {O I}, a body coordinate system {O B}, and the i-th link coordinate system of the robotic arm {O i}, and all coordinate systems follow the right-hand rule; The motion states of the quadrotor UAV described in the state space include: the position of the UAV's center of mass The rotation matrix R I from the inertial frame {O B} to the body frame {O B} ∈ SO(3) and the body yaw angle ψ; the motion states of the robotic arm include: the angles of each joint which are restricted to move on the unit circle and the combined vector representation formed by all joint angles where: is the n-fold Cartesian product of The motion state of the robotic arm actuator described in the task space includes: the actuator position and the attitude rotation matrix R e ∈ SO(3).

6. The method for generating a dynamically feasible trajectory of the aerial manipulator system according to claim 5, characterized in that, The specific content of the said Step 2 includes: 2.1 Reparameterization of the body rotation matrix: Using the body lift direction vector t = [t1 t2 t3] T and the rotation angle ψ about this axis z as independent variables, the body rotation matrix R is described in terms of the axis-angle method B , and the rotation process from the body coordinate system to the inertial coordinate system is decomposed into two steps, specifically: R B (t, ψ z ) = R2(t)R1(ψ z ), where: represents the rotation matrix for the first step of rotating by ψ z degrees about the z-axis of the body coordinate system; represents the rotation matrix for rotating to the inertial coordinate system on the basis of the first-step rotation; 2.2 Reconstruct the joint angles of the robotic arm given the pose of the actuator and the lift direction vector: Briefly denote the product of the two matrix attitude matrices as Following the steps to solve the Euler angles from the known rotation matrix, the rotation angle ψ about the lift direction t can be determined z = arctan(a 21 / a 11 ), and the two robotic arm joint angles are θ1 = arcsin(-a 31 ) and θ2 = arctan(a 32 / a 33 ), where: is the transpose of the R2(t) matrix; R e is the actuator attitude rotation matrix; a ij is the element corresponding to the i-th row and j-th column in matrix A; 2.3 Given the pose of the actuator and the lift direction vector, the inverse solution of the body yaw angle: Briefly denote the product of the two rotation matrices as R2(t)R1(ψ z ) = B. Then, by analogy with the process of solving Euler angles from a known rotation matrix, the body yaw angle ψ = arctan(b 21 / b 11 ), where: b ij is the element corresponding to the i-th row and j-th column in matrix B; 2.4 Given the actuator posture and lift direction vector, inversely solve the system center of mass position: Since the robot arm joint angle has been calculated, the rotation angle from {O1} to {O B The rotation matrix of and the rotation matrix from {O2} to {O1} All are known, and the offset vector from the center of mass of the first link of the manipulator to O1 is expressed in the {O1} system From O1 to O B The expression of the offset vector in the {O1} system The expression of the offset vector from the center of mass of the second connecting rod to O2 in the {O2} system The expression of the offset vector from O2 to O1 in the {O2} system These quantities are only related to the length of the robot arm connecting rod and are all known quantities. Therefore, the position of the center of mass of the system can be inversely solved based on the kinematic relationship of the robot arm: Where: p c is the position of the center of mass of the system; p e is the actuator position; m is the total mass of the system; m b is the mass of the UAV; m1 and m2 are the masses of the two connecting rods.

7. The method for generating a dynamically feasible trajectory of the aerial manipulator system according to claim 5, characterized in that, The specific content of the said Step 3 includes: 3.1 Construct the actuator attitude rotation matrix in the order of Z-Y-X Euler angles Where: R z (ψ e ) represents the rotation matrix for rotating by an angle of ψ around the z-axis; R e (θ y ) represents the rotation matrix for rotating by an angle of θ around the y-axis; R e (φ e ) represents the rotation matrix for rotating by an angle of φ around the x-axis; x (φ e ) represents the rotation matrix for rotating by an angle of φ around the x-axis; e angle; 3.2 Actuator position trajectory parameterization, including: figure-eight trajectory p e (t) = [r sin(ωt) r cos(2ωt) 0] T and circular trajectory p e (t) = [r sin(ωt) r cos(2ωt) 0] T ; 3.3 Actuator attitude trajectory parameterization, that is, specifying the three Euler angle trajectories as uniform rotations: taking the yaw direction as an example, specifying where: is the angular velocity of rotation, is the initial angular position, and the Euler angles in the other two directions are parameterized in a similar manner The corresponding angular velocity of rotation is The initial angular position is 8. The method for generating a dynamically feasible trajectory of the aerial manipulator system according to claim 5, wherein The specific content of the said Step 4) includes: 4.1 The lift direction should always be perpendicular to the plane where the body propeller is located, so it satisfies the following relationship with the centroid acceleration of the system: Where: refers to a unit sphere in three-dimensional space, g is the acceleration due to gravity, and z W is the coordinate basis in the z direction of the inertial system, is the centroid acceleration of the system; 4.2 Caused by the interaction between the manipulator movement and the quadrotor UAV movement, that is, the position p of the system centroid c and the position p of the actuator e satisfy the following relationship: It is consistent with the inverse solution process of the system centroid position in step 2).

9. The method for generating a dynamically feasible trajectory of the aerial manipulator system according to claim 5, characterized in that The specific step 5) is as follows: Using the azimuth angle and the polar angle θ t to parameterize the lift direction vector Using trigonometric series to parameterize the azimuth angle and the polar angle, and parameterizing the trajectory of the angle through the sine and cosine bases: Where: is the fundamental frequency of the trigonometric series corresponding to the polar angle and the azimuth angle; and are the cosine and sine amplitudes of the trigonometric series corresponding to the polar angle and the azimuth angle respectively; the parameter set is the parameter to be optimized in the angle trajectory.

10. The method for generating a dynamically feasible trajectory of the aerial manipulator system according to claim 5, characterized in that, The specific content of the said Step 6) includes: The specific numerical optimization problem of the 6.1 structure is as follows: Where: N T is the number of sampling points of the system trajectory within the entire time period T; t i is the sampling time; is the acceleration of the system centroid, approximately obtained by performing a second-order numerical difference on the system centroid position p c ; the system centroid position p c is obtained by inverse solution in step 2); 6.2 After numerically optimizing to solve for the lift direction trajectory that conforms to the system differential-algebraic constraints in Step 4), according to the inverse solution method from the actuator pose to the system state in Step 2), the complete state trajectory of the entire aerial manipulator system is determined.

Images