A trajectory planning method based on the dynamic model of a deformable quadrotor aircraft

Abstract

The present invention provides a trajectory planning method based on the dynamic model of a deformable quadrotor aircraft, comprising the following steps: establishing a dynamic model of the deformable quadrotor aircraft based on the Newton-Euler equations; constructing an objective function; setting constraints; the constraints including waypoint constraints, continuity constraints, differential constraints, and corridor constraints; time allocation; using uniform time allocation, that is, the time used for each segmented trajectory is the same; solving a quadratic programming problem; setting the coefficients of the piecewise polynomial as parameters to be optimized, converting the objective function into a quadratic programming problem, adding waypoint constraints, continuity constraints, and differential constraints as equality constraints, adding corridor constraints and related extreme value constraints as inequality constraints, and obtaining the optimal trajectory using MATLAB. Simulation results of the present invention show that the present invention can achieve the optimal trajectory path of the deformable quadrotor.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory planning methods, and in particular to a trajectory planning method based on a deformable quadrotor aircraft dynamics model. Background Art

[0002] Quadcopter unmanned aerial vehicles (UAVs) boast simple structure, flexible operation, and strong adaptability, making them widely used in aerial photography, reconnaissance, logistics, and other fields. Traditional quadcopters have motors and propellers mounted on a fixed frame. This underactuated structure limits their ability to maneuver quickly. Inspired by the flight behavior of animals such as birds and hawks, many believe that shapeshifting could enhance the performance of quadcopters, allowing them to change shape during flight to adapt to different environments.

[0003] Current research on deformable drones focuses primarily on structural design and feasibility verification. Trajectory planning is essential for achieving autonomous flight. Three main approaches are currently used for trajectory planning in quadrotor drones: search-based, sampling-based, and numerical optimization-based. Numerical optimization-based trajectory planning takes into account constraints such as the drone's dynamics. By converting these constraints into optimization constraints, a smooth trajectory that satisfies the drone's physical characteristics and environmental constraints is obtained. However, previous work has mostly focused on studying structural feasibility and improving controllers, with limited consideration of deformation factors in trajectory planning. Summary of the Invention

[0004] Based on the above-mentioned shortcomings of existing quadrotor trajectory planning and the technical problem of considering the impact of deformation in trajectory planning, a trajectory planning method based on the dynamic model of a deformable quadrotor is provided. The present invention first expands on the traditional quadrotor dynamic model, incorporating the characteristic that the moment of inertia of the deformable quadrotor changes with the arm length, and dynamically models the deformable quadrotor. Then, combined with the trajectory planning method of the deformable UAV, the energy consumption caused by the deformation of the deformable quadrotor is considered, and a trajectory planning method based on numerical optimization is used to generate the optimal trajectory of the deformable quadrotor that meets the constraints.

[0005] The technical means adopted in the present invention are as follows:

[0006] A trajectory planning method based on a deformable quadrotor dynamics model comprises the following steps:

[0007] S1: Establish a dynamic model of a deformable quadrotor based on the Newton-Euler equations;

[0008] S2: construct the objective function;

[0009] S3: Setting constraints; the constraints include waypoint constraints, continuity constraints, differential constraints, and corridor constraints;

[0010] S4: Time allocation; uniform time allocation is adopted, that is, the time used for each segment trajectory is the same;

[0011] S5: Solve the quadratic programming problem; set the coefficients of the piecewise polynomial as the parameters to be optimized, transform the objective function into a quadratic programming problem, add waypoint constraints, continuity constraints, and differential constraints as equality constraints, add corridor constraints and related maximum constraints as inequality constraints, and obtain the optimal trajectory using MATLAB.

[0012] Furthermore, the dynamic model of the deformable quadrotor aircraft is:

[0013]

[0014] Where m represents the mass of the quadrotor, g represents the acceleration due to gravity, and p = [x, y, z] T Indicates the position of the quadrotor, e3 = [0,0,1] T , R represents the rotation matrix from the body coordinate system to the world coordinate system, u1 represents the total thrust generated by the motor, ω represents the angular velocity in the body coordinate system, J = diag(J xx ,J yy ,J zz ) represents the moment of inertia of the quadrotor, M=[u2,u3,u4] T It represents the torque generated by the propeller on the fuselage shaft;

[0015]

[0016] Among them, ψ, Φ, θ represent the attitude angles of the quadrotor, which are yaw angle, pitch angle and roll angle respectively;

[0017]

[0018]

[0019] Among them, k F represents the rotor thrust constant, k M represents the rotor torque constant, ω i Indicates the speed of the corresponding four propellers; i=1,2,3,4;

[0020] The moment of inertia of the deformable quadrotor changes with the arm length. The relationship between the moment of inertia and arm length is:

[0021] J xx =J yy=A1l 2 +B1l+C1;

[0022] J zz =A2l 2 +B2l+C2;

[0023] Among them, A1, B1, C1 and A2, B2, C2 are the coefficients of the relationship; l represents the arm length of the deformable quadrotor;

[0024] The arm length of the deformable quadrotor is controlled by the motor. The motor angle and arm length correspond to the input and output of the model respectively. In order to calculate the corresponding motor angle from the desired arm length, the relationship between arm length and motor angle is given as:

[0025]

[0026] Among them, u5 represents the motor angle, and a, b, and c represent dimensionless constants.

[0027] Furthermore, in S2, the differential flat characteristics of the quadrotor, the state and input are written as flat output and its derivatives; u5 represents the input of the motor that controls the arm length; since the arm length l is one of the state vectors of the system, it is also one of the flat outputs of the system; let The trajectory in the flat output space is expressed as a piecewise polynomial function:

[0028]

[0029] Among them, n represents the order of the polynomial, m represents the number of time intervals, and the start and end time of each segment can be freely set. Represents the parameters to be optimized, i=1,2,…,n; j=1,2,…,m;

[0030] The purpose of trajectory planning is to minimize the energy during flight, so the objective function is:

[0031]

[0032] Among them, r T =[x T y T z T ] T , μ r , μ l ,k r , k l All represent dimensionless constants;

[0033] The coefficient to be determined Rewritten as a 5nm×1 vector c, the objective function is transformed into a quadratic programming problem:

[0034] min c T Hc+f T c;

[0035] sA ieq c≤b ineq ;

[0036] A eq C=b eq .

[0037] Furthermore, the waypoint constraint is: the points that the deformable quadrotor must pass through are selected and set at the endpoints of each segment, that is:

[0038] σ T (t i )=σ i ,i=0,1,…,m。

[0039] Furthermore, the continuity constraint is: the waypoint is at the connection point of the two end trajectories, ensuring that the states on the left and right sides are continuous:

[0040]

[0041] Among them, for The position part in, k=0,1,…,k r , for the yaw angle part, For the arm length part, k=0,1,…,k l .

[0042] Furthermore, the differential constraint is: setting differential constraints on waypoints located at the starting point and the end point.

[0043] Furthermore, the corridor constraint is:

[0044] Corridor restrictions are added to the inequality constraints of quadratic programming based on the consideration of arm length changes to ensure that the deformable quadrotor can safely pass through the corridor.

[0045] First, define the i to r i+1 The unit vector e=(r i+1 -r i ) / ||r i+1 -r i ||, vector r i+1 -r i Subtracting the projection component of this vector in the direction of the unit vector, the vertical distance vector of the i-th (i=1,…,m) segment of the trajectory can be obtained as:

[0046] di (t)=(r T (t)-r i )-((r T (t)-r i )·e)e;

[0047] Define the width of the i-th corridor as δ i , in order to make the deformable quadrotor aircraft pass through the corridor safely while taking into account the change of its arm length, the following conditions should be met:

[0048] ||d i (t)|| ∞ +l i ≤δ i ;

[0049] By introducing h intermediate points and integrating them into the quadratic programming problem, the expression is as follows:

[0050]

[0051] Compared with the prior art, the present invention has the following advantages:

[0052] This paper proposes a trajectory planning method for a morphing quadrotor aircraft. First, a quadrotor dynamics model is established based on the Newton-Euler equations. Deformation factors are then incorporated into this model to establish the dynamics of the morphing quadrotor. During trajectory planning, polynomials are used as the trajectory expression. Waypoints and corridor constraints are set based on dynamic constraints, while also considering the energy consumption caused by deformation. The trajectory planning problem is constructed based on this and converted into a quadratic problem for solution. Simulation results demonstrate that the present invention can achieve the optimal trajectory path for the morphing quadrotor. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0054] Figure 1 Schematic diagram of establishing the coordinate system of the deformable quadrotor aircraft of the present invention;

[0055] Figure 2 Schematic diagram of the relationship between arm length and motor rotation angle of the present invention;

[0056] Figure 3 It is a schematic diagram of the trajectory plane with and without corridor restriction of the present invention;

[0057] Figure 4 3D trajectory generation diagram with and without corridor restrictions according to the present invention;

[0058] Figure 5 It is a detailed schematic diagram of the corridor portion of the present invention;

[0059] Figure 6 The present invention is x, y, z, Schematic diagram of the changes of l;

[0060] Figure 7 It is a schematic diagram of the speed change in the x, y, and z directions of the present invention;

[0061] Figure 8 It is a schematic diagram of the acceleration changes in the x, y, and z directions of the present invention. DETAILED DESCRIPTION

[0062] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments of the present invention can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0063] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is in no way intended to limit the present invention and its application or use. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0064] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or combinations thereof.

[0065] Unless otherwise specifically stated, the relative arrangement of the parts and steps, numerical expressions and numerical values ​​set forth in these embodiments do not limit the scope of the present invention. At the same time, it should be clear that, for ease of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship. The technology, methods and equipment known to those of ordinary skill in the relevant art may not be discussed in detail, but in appropriate cases, the technology, methods and equipment should be considered as part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as being merely exemplary and not as a limitation. Therefore, other examples of the exemplary embodiments may have different values. It should be noted that similar numbers and letters represent similar items in the following figures, and therefore, once an item is defined in one figure, it does not need to be further discussed in subsequent figures.

[0066] In the description of the present invention, it should be understood that the directions or positional relationships indicated by directional words such as "front, back, up, down, left, right", "horizontal, vertical, vertical, horizontal" and "top, bottom" are usually based on the directions or positional relationships shown in the accompanying drawings. They are only for the convenience of describing the present invention and simplifying the description. Unless otherwise specified, these directional words do not indicate or imply that the device or element referred to must have a specific direction or be constructed and operated in a specific direction. Therefore, they cannot be understood as limiting the scope of protection of the present invention: the directional words "inside and outside" refer to the inside and outside relative to the outline of each component itself.

[0067] For ease of description, spatially relative terms such as "above", "above", "on the upper surface of", "above", etc. may be used herein to describe the spatial positional relationship of a device or feature to other devices or features as shown in the figures. It should be understood that spatially relative terms are intended to include different orientations of the device in use or operation in addition to the orientation described in the figures. For example, if the device in the drawings is inverted, the device described as "above other devices or structures" or "above other devices or structures" will be positioned as "below other devices or structures" or "below their position devices or structures". Thus, the exemplary term "above" can include both "above" and "below". The device can also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatially relative descriptions used here are interpreted accordingly.

[0068] In addition, it should be noted that the use of terms such as "first" and "second" to limit components is only for the convenience of distinguishing the corresponding components. Unless otherwise stated, the above terms have no special meaning and therefore cannot be understood as limiting the scope of protection of the present invention.

[0069] A trajectory planning method based on a deformable quadrotor dynamics model comprises the following steps:

[0070] S1: The dynamic model of the deformable quadrotor is established based on the Newton-Euler equation. The dynamic model of the deformable quadrotor is:

[0071]

[0072] Where m represents the mass of the quadrotor, g represents the acceleration due to gravity, and p = [x, y, z] T Indicates the position of the quadrotor, e3 = [0,0,1] T , R represents the rotation matrix from the body coordinate system to the world coordinate system, u1 represents the total thrust generated by the motor, ω represents the angular velocity in the body coordinate system, J = diag(J xx ,J yy ,J zz ) represents the moment of inertia of the quadrotor, M=[u2,u3,u4] T It represents the torque generated by the propeller on the fuselage shaft;

[0073]

[0074] Among them, ψ, Φ, θ represent the attitude angles of the quadrotor, which are yaw angle, pitch angle and roll angle respectively;

[0075]

[0076] Among them, k F represents the rotor thrust constant, k M represents the rotor torque constant, ω i Indicates the speed of the corresponding four propellers; i=1,2,3,4;

[0077] The moment of inertia of the deformable quadrotor changes with the arm length. The relationship between the moment of inertia and arm length is:

[0078] J xx =J yy =A1l 2 +B1l+C1;

[0079] J zz =A2l 2 +B2l+C2;

[0080] Among them, A1, B1, C1 and A2, B2, C2 are the coefficients of the relationship; l represents the arm length of the deformable quadrotor;

[0081] The arm length of the deformable quadrotor is controlled by the motor. The motor angle and arm length correspond to the input and output of the model respectively. In order to calculate the corresponding motor angle from the desired arm length, the relationship between arm length and motor angle is given as:

[0082]

[0083] Among them, u5 represents the motor angle, and a, b, and c represent dimensionless constants.

[0084] S2: Construct the objective function; in S2, the differential flatness characteristics of the quadrotor, the state and the input are written as flat output and its derivatives; u5 represents the input of the motor that controls the arm length; since the arm length l is also one of the state vectors of the system, it is also one of the flat outputs of the system. Let The trajectory in the flat output space is expressed as a piecewise polynomial function:

[0085]

[0086] Among them, n represents the order of the polynomial, m represents the number of time intervals, and the start and end time of each segment can be freely set. Represents the parameters to be optimized, i=1,2,…,n; j=1,2,…,m;

[0087] The purpose of trajectory planning is to minimize the energy during flight, so the objective function is:

[0088]

[0089] Among them, r T =[x T y T z T ] T , μ r , μ l ,k r , k l All represent dimensionless constants;

[0090] The coefficient to be determined Rewritten as a 5nm×1 vector c, the objective function is transformed into a quadratic programming problem:

[0091] min c T Hc+f T c;

[0092] stA ieq c≤b ineq ;

[0093] A eq c=b eq .

[0094] S3: Set constraints; the constraints include waypoint constraints, continuity constraints, differential constraints, and corridor constraints; waypoint constraints are: select points that the deformable quadrotor must pass through, and set them at the endpoints of each segment, that is:

[0095] σ T (t i )=σ i ,i=0,1,…,m。

[0096] The continuity constraint is: the waypoint is at the connection point of the two trajectories, ensuring that the states on the left and right sides are continuous:

[0097]

[0098] Among them, for The position part in, k=0,1,…,k r , for the yaw angle part, For the arm length part, k=0,1,…,k l .

[0099] The differential constraints are: setting differential constraints on the waypoints at the starting point and the end point, i.e. speed, acceleration, etc.

[0100] The corridor constraints are:

[0101] Corridor restrictions are added to the inequality constraints of quadratic programming based on the consideration of arm length changes to ensure that the deformable quadrotor can safely pass through the corridor.

[0102] First, define the i to r i+1 The unit vector e=(r i+1 -r i ) / ||r i+1 -r i ||, vector r i+1 -r i Subtracting the projection component of this vector in the direction of the unit vector, the vertical distance vector of the i-th (i=1,…,m) segment of the trajectory can be obtained as:

[0103] d i (t)=(r T (t)-r i )-((r T (t)-r i )·e)e;

[0104] Define the width of the i-th corridor as δ iIn order for the deformable quadrotor to pass through the corridor safely while taking into account the change in its arm length, the following conditions should be met:

[0105] ||d i (t)|| ∞ +l i ≤δ i

[0106] By introducing h intermediate points and integrating them into the quadratic programming problem, the expression is as follows:

[0107]

[0108] S4: Time allocation; uniform time allocation is adopted, that is, the time used for each segment trajectory is the same;

[0109] S5: Solve the quadratic programming problem; set the coefficients of the piecewise polynomial as the parameters to be optimized, transform the objective function into a quadratic programming problem, add waypoint constraints, continuity constraints, and differential constraints as equality constraints, add corridor constraints and related maximum constraints as inequality constraints, and obtain the optimal trajectory using MATLAB.

[0110] Example 1

[0111] The present invention provides a trajectory planning method based on a deformable quadrotor aircraft dynamics model, comprising the following steps:

[0112] S1, based on the Newton-Euler equation, establish the dynamic model of the deformable quadrotor aircraft:

[0113]

[0114] Where m = 1.587 kg is the mass of the quadrotor, g = 9.81 m / s 2 is the acceleration due to gravity, p=[x,y,z] T is the position of the quadrotor, e3=[0,0,1] T , R represents the rotation matrix from the body coordinate system to the world coordinate system, u1 is the total thrust generated by the motor, ω is the angular velocity in the body coordinate system, J = diag (J xx ,J yy ,J zz ) is the moment of inertia of the quadrotor, M=[u2,u3,u4] T It represents the torque generated by the propeller on the fuselage shaft;

[0115]

[0116] Where ψ, φ, θ are the attitude angles of the quadrotor, namely yaw angle, pitch angle and roll angle respectively;

[0117]

[0118] Among them, k F =4.0687×10 -7 is the rotor thrust constant, k M =8.4367×10 -9 is the rotor torque constant, ω i (i=1,2,3,4) corresponds to the rotation speed of the four propellers;

[0119] The moment of inertia of a deformable quadrotor changes with the arm length. The general relationship between the moment of inertia and arm length is as follows:

[0120] J xx =J yy =A1l 2 +B1l+C1;

[0121] J zz =A2l 2 +B2l+C2;

[0122] Among them, A1 = 0.205, B1 = -0.0095, C1 = 0.00088, A2 = 0.41, B2 = -0.02, C2 = 0.0011, l represents the arm length of the deformable quadrotor, which is achieved by telescoping the arm. Unlike traditional quadrotors, the arm length value changes with the change of the motor angle, so the arm length value will also be one of the state vectors;

[0123] Regarding the relationship between the moment of inertia and arm length, the SolidWorks Mass-Property toolbox was used to obtain the values ​​of the moment of inertia of the robotic arm when it was extended and retracted at different ratios. Based on the experimental data, the relationship between the moment of inertia and arm length was obtained by fitting.

[0124] The arm length of the deformable quadrotor is controlled by the motor. The motor angle and arm length correspond to the input and output of the model respectively. In order to calculate the corresponding motor angle from the desired arm length, the relationship between arm length and motor angle is given as follows:

[0125]

[0126] Where u5 represents the motor angle, a = -13.88, b = -77.40, c = 73.84, and the coefficients are obtained from the system identification toolbox in MATLAB;

[0127] The derivation process of the relationship between arm length and motor angle is as follows:

[0128] according to Figure 2 The geometric relationship can be obtained:

[0129]

[0130] l m The value of can be measured, so there are:

[0131]

[0132] Among them, l1 is the chord length from the servo angle to the center wall, l2 is the chord length from the center wall to the motor block, l h is the radius of the servo motor rocker arm, l m is the extended arm length, α is the servo motor angle, and r is the center body radius;

[0133] From the above formula, the motor angle command for a given desired arm length can be calculated, and finally the general expression of arm length and motor angle can be obtained;

[0134] S2, construct the objective function. According to the differential flatness characteristic of the quadrotor, the state and input can be written as a flat output and its derivatives. At the same time, since the arm length l is also one of the state vectors of the system and u5 is the input of the motor that controls the arm length, l is also one of the flat outputs of the system. Let Represent the trajectory in the flat output space as a piecewise polynomial function:

[0135]

[0136] Where n=6 is the order of the polynomial, m=5 is the number of time intervals, and the start and end time of each segment can be freely set. is the parameter to be optimized;

[0137] The purpose of trajectory planning is to consume as little energy as possible during flight, so the objective function is:

[0138]

[0139] Among them, r T =[x T y T z T ] T , μ r =1, μ l =1,k r , k l are dimensionless constants;

[0140] According to the differential flatness characteristic, the inputs u2 and u3 are functions of the fourth-order derivative of position, so the generated trajectory minimizes the integral of the square of the norm of the second-order derivative of acceleration, so k r =4, the input u4 appears in the second derivative of the yaw angle, so Similarly, input u5 appears in the second derivative of the arm length, so k l =2;

[0141] The coefficient to be determined Written as a 5nm×1 vector c, the objective function is transformed into a quadratic programming problem:

[0142] min c T Hc+f T c;

[0143] stA ieq c≤b ineq ;

[0144] A eq c=b eq ;

[0145] S3, set constraints:

[0146] Waypoint constraints. First, select the points that the deformable quadrotor must pass through and set them at the endpoints of each segment, namely:

[0147] σ T (t i )=σ i ,i=0,1,…,m;

[0148] In this example, the coordinates of the five waypoints are (0,0,0), (5,-7.5,0.8), (14,-4.5,2.3), (10,5,4), and (4.5,6,4.5).

[0149] Continuity constraint. The waypoint is at the junction of the two trajectories, and the states on both sides must be continuous:

[0150]

[0151] Among them, for The position part in, k=0,1,…,k r , for the yaw angle part, For the arm length part, k=0,1,…,k l ;

[0152] Differential constraints. Continuity constraints can ensure the smoothness of the trajectory. In addition, it is necessary to restrict the speed and acceleration at the starting and ending points of the trajectory. It is difficult to make the aircraft reach a set value for the speed and acceleration at the intermediate points, so only continuity constraints are set for the intermediate points.

[0153] In the differential constraint, set the velocity and acceleration values ​​at the beginning and end to 0;

[0154] Corridor constraint. Based on considering the change in arm length, the present invention adds corridor constraints to the inequality constraints of quadratic programming to ensure that the deformable quadrotor can safely pass through the corridor.

[0155] In this example, there is a corridor with a width of D = 0.20 m between the third and fourth waypoints. To ensure that the deformable quadrotor can pass through the corridor safely and intact, we set a corridor with a width of δ = 0.15 m (δ < D) in the simulation.

[0156] First, define the unit vector e from r i to r i+1 as e = (r i+1 - r i ) / ||r i+1 - r i ||. Subtracting the projection component of the vector r i+1 - r i in the direction of the unit vector, the perpendicular distance vector for the i-th (i = 1,..., m) segment of the trajectory can be obtained as

[0157] d i (t) = (r T (t) - r i ) - ((r T (t) - r i ) · e)e;

[0158] Define the width of the i-th corridor as δ i . To enable the deformable quadrotor aircraft to safely pass through the corridor and considering the change in its arm length, the following condition should be satisfied

[0159] ||d i (t)|| [[ID=​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​S4. Time allocation. The present invention adopts uniform time allocation, and each segmented trajectory is allocated 2 seconds, that is, the total time is 10 seconds;

[0166] S5. Solve the quadratic programming problem. Set the coefficients of the piecewise polynomial as the parameters to be optimized, transform the objective function into a quadratic programming problem, add waypoint constraints, continuity constraints, and differential constraints as equality constraints, add corridor constraints and related extreme value constraints as inequality constraints, and obtain the optimal trajectory using MATLAB.

[0167] Example 2:

[0168] For the sake of comparison, the simulation gives the trajectory generation results without and with corridor constraints, to illustrate that the deformable quadrotor can pass through narrow passages using its telescopic arms while meeting the shortest path requirement.

[0169] Figure 3 and Figure 4 is the trajectory planning result, where Figure 3 For the plan view, Figure 4 3D trajectory diagram, the solid line represents the trajectory generated with corridor constraints, the dashed line represents the trajectory generated without corridor constraints, and the corridor is set between the 3rd and 4th waypoints;

[0170] The simulation results show that both trajectories start from the first waypoint and return to the origin after passing through five waypoints. In addition, the deformable quadrotor can safely pass through the corridor;

[0171] Figure 5 A detailed diagram of the corridor is given. The wider one is the actual channel with a width of D = 0.2m, and the narrower one is the corridor with a width of δ = 0.15m set to ensure the safe passage of the quadcopter. In addition, the "cross" on the trajectory is used to represent the change in arm length of the deformable quadcopter during flight. It can be seen that the arm length of the deformable quadcopter shrinks significantly when passing through the corridor. The specific data can be viewed Figure 6 ;

[0172] Figure 6 The changing trends of position, yaw angle, and arm length are given. The smoothness of the image avoids sudden changes in the state of the deformable quadrotor during flight. In this example, the yaw angle is always set to 0. For the trajectory generated without corridor constraints, the arm length of the deformable quadrotor is always around 0.18m, with almost no deformation, which also reduces energy consumption. For the trajectory generated with corridor constraints, the arm length of the deformable quadrotor gradually decreases after the start, then maintains at 0.14m for 1 second, and then begins to shrink further before reaching the corridor, and then safely passes through the corridor.

[0173] Figure 7 and Figure 8The changing trends of the velocity and acceleration of the deformable quadrotor are given;

[0174] The simulation shows that the present invention can enable the deformable quadrotor to pass through a narrow space while achieving the shortest path.

[0175] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A trajectory planning method based on a deformable quadrotor dynamics model, characterized in that: The following steps are involved: S1: Establish a dynamic model of a deformable quadrotor based on the Newton-Euler equations; S2: construct the objective function; S3: Set constraints; the constraints include: waypoint constraints, continuity constraints, differential constraints and corridor constraints; the corridor constraints are: Corridor restrictions are added to the inequality constraints of the quadratic programming based on the change of arm length to ensure that the deformable quadrotor can safely pass through the corridor; First define arrive The unit vector ,vector Subtract the projection component of the vector in the direction of the unit vector to get the first The vertical distance vector of the segment trajectory is: ； Definition The width of the corridor is In order for the deformable quadrotor to pass through the corridor safely while taking into account the change in its arm length, the following conditions should be met: ； By introducing An intermediate point is integrated into the quadratic programming problem, and the expression is as follows: ； ； ； S4: Time allocation; uniform time allocation is adopted, that is, the time used for each segment trajectory is the same; S5: Solve the quadratic programming problem; set the coefficients of the piecewise polynomial as the parameters to be optimized, transform the objective function into a quadratic programming problem, add waypoint constraints, continuity constraints, and differential constraints as equality constraints, add corridor constraints and related maximum constraints as inequality constraints, and obtain the optimal trajectory using MATLAB.

2. A trajectory planning method based on a deformable quadrotor aircraft dynamic model according to claim 1, characterized in that: The dynamic model of the deformable quadrotor aircraft is: ； ； in, represents the mass of the quadrotor, represents the acceleration due to gravity, Indicates the position of the quadrotor, , Represents the rotation matrix from the body coordinate system to the world coordinate system, represents the total thrust generated by the motor, represents the angular velocity in the body coordinate system, represents the moment of inertia of the quadrotor, It represents the torque generated by the propeller on the fuselage shaft; ； in, Represents the attitude angles of the quadrotor, which are yaw angle, pitch angle and roll angle; ； ； in, represents the rotor thrust constant, represents the rotor torque constant, Indicates the rotation speed of the corresponding four propellers; ; The moment of inertia of the deformable quadrotor changes with the arm length. The relationship between the moment of inertia and arm length is: ； ； in, , , as well as , , is the coefficient of the relationship; Indicates the arm length of the deformable quadrotor; The arm length of the deformable quadrotor is controlled by the motor. The motor angle and arm length correspond to the input and output of the model respectively. In order to calculate the corresponding motor angle from the desired arm length, the relationship between arm length and motor angle is given as: ； in, Indicates the motor angle, Represents a dimensionless constant.

3. The trajectory planning method based on the dynamic model of a deformable quadrotor aircraft according to claim 1, characterized in that: In S2, the differential flat characteristic of the quadrotor, the state and the input are written as a flat output algebraic functions of and their derivatives; Represents the input of the motor that controls the arm length; since the arm length is one of the state vectors of the system, and therefore one of the flat outputs of the system; let , the trajectory in the flat output space is expressed as a piecewise polynomial function: ； in, represents the order of the polynomial, Indicates the number of time intervals. The start and end time of each segment can be set freely. is the parameter to be optimized, ; The purpose of trajectory planning is to minimize the energy during flight, so the objective function is: ； in, , All represent dimensionless constants; The coefficient to be determined Rewrite it as a Vector , so that the objective function is transformed into a quadratic programming problem: ； ； 。 4. The trajectory planning method based on the dynamic model of a deformable quadrotor aircraft according to claim 1, characterized in that: The waypoint constraint is to select the points that the deformable quadrotor must pass through and set them at the endpoints of each segment, that is: 。 5. The trajectory planning method based on the dynamic model of a deformable quadrotor aircraft according to claim 1, characterized in that: The continuity constraint is: the waypoint is at the connection point of the two end trajectories, ensuring that the states on the left and right sides are continuous: ； Among them, for The location part of , for the yaw angle part, , for the arm length part, .

6. The trajectory planning method based on the dynamic model of a deformable quadrotor aircraft according to claim 1, characterized in that: The differential constraint is: setting differential constraints on the waypoints located at the starting point and the end point.

Images