Track planning method based on kinetic model of deformation four-rotor aircraft

Abstract

The invention provides a trajectory planning method based on a dynamic model of a deformed four-rotor aircraft. The trajectory planning method comprises the following steps: establishing the dynamic model of the deformed four-rotor aircraft on the basis of a Newton-Euler equation; constructing a target function; setting constraint conditions; the constraint conditions comprise waypoint constraint, continuity constraint, differential constraint and corridor constraint; time distribution; uniform time distribution is adopted, that is, the time used by each segmented track is the same; solving a quadratic programming problem; and the coefficients of the piecewise polynomial are set as parameters to be optimized, the objective function is converted into a quadratic programming problem, waypoint constraints, continuity constraints and differential constraints are added as equality constraints, corridor constraints and related extreme and extreme constraints are added as inequality constraints, and the optimal trajectory is obtained through MATLAB. Simulation results show that the method can realize the optimal path of the trajectory of the deformed 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) have the characteristics of simple structure, flexible operation, and strong adaptability. They have been widely used in aerial photography, reconnaissance, logistics and transportation. The motors and propellers of traditional quadcopters are installed on a fixed frame. This under-driven structural feature limits its ability to maneuver quickly. Inspired by the flight behavior of some creatures such as birds and goshawks, many people believe that deformation can make quadcopters have better performance, allowing them to change shape during flight to adapt to different environments.

[0003] At present, the research on deformable UAVs is mainly focused on mechanism design and feasibility verification. As the basis for achieving autonomous flight, trajectory planning is essential. There are three main methods used for trajectory planning of quadrotor UAVs: search-based trajectory planning method, sampling-based trajectory planning method and numerical optimization-based trajectory planning method. The trajectory planning method based on numerical optimization solution can take into account the constraints such as the dynamics of the UAV, and obtain a smooth trajectory that satisfies the physical characteristics and environmental constraints of the UAV itself by converting these constraints into constraints of the optimization problem. However, most of the previous work has studied its structural feasibility and improved the controller, and few works have considered deformation factors in trajectory planning. Summary of the invention

[0004] Based on the above-mentioned deficiencies in the existing quadrotor trajectory planning, the technical problem of considering the influence of deformation in trajectory planning is proposed, and 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, integrates the characteristics that the moment of inertia of the deformable quadrotor changes with the arm length, and performs dynamic modeling on the deformable quadrotor. Then, combined with the trajectory planning method of the deformation of the UAV, the energy consumption of the deformable quadrotor caused by the deformation 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 by the present invention are as follows:

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

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

[0008] S2: construct 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 segmented 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 through 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 the 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 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 the arm length and the 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 characteristic of the quadrotor, the state and the input are written as a 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] Where 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 end points 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 tracks, 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 a differential constraint on the 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 change of arm length to ensure that the deformable quadrotor can safely pass through the corridor.

[0045] First, define r 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 considering the change of its arm length, it should meet the following conditions:

[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] The present invention proposes a trajectory planning method for a deformable quadrotor aircraft. First, a dynamic model of a quadrotor is established based on the Newton-Euler equations, and on this basis, a deformation factor is added to establish the dynamic model of the deformable quadrotor. In the trajectory planning process, a polynomial is used as the expression of the trajectory. Waypoints and corridor constraints are set on the basis of considering the dynamic constraints. At the same time, the energy consumption caused by the deformation is considered. Based on this, a trajectory planning problem is constructed and converted into a quadratic problem for solution. Simulation results show that the present invention can achieve the optimal trajectory path of the deformable 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 drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. 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 creative labor.

[0054] Figure 1 It is a schematic diagram of establishing a coordinate system of a deformable quadrotor aircraft of the present invention;

[0055] Figure 2 It is a schematic diagram of the relationship between the arm length and the 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 It is a schematic diagram of 3D trajectory generation with and without corridor restrictions of 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 change 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 of the present invention and the features in the embodiments 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 by no means 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 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 exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0065] Unless otherwise specifically stated, the relative arrangement, numerical expressions and numerical values ​​of the parts and steps described 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 ordinary technicians in the relevant field may not be discussed in detail, but in appropriate cases, the technology, methods and equipment should be regarded as a part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as being merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters represent similar items in the following drawings, so once a certain item is defined in an accompanying drawing, it does not need to be further discussed in subsequent drawings.

[0066] In the description of the present invention, it is necessary to understand that the directions or positional relationships indicated by directional words such as "front, back, up, down, left, right", "lateral, vertical, perpendicular, horizontal" and "top, bottom" are usually based on the directions or positional relationships shown in the 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 contours 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 here to describe the spatial positional relationship between a device or feature and other devices or features as shown in the figure. 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 figure. For example, if the device in the accompanying 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. If not 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 aircraft dynamics model comprises the following steps:

[0070] S1: The dynamic model of the deformable quadrotor aircraft is established based on the Newton-Euler equation; the dynamic model of the deformable quadrotor aircraft 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 the 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 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 the arm length and the 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 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] Where 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] sA 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, set at the end points 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 end tracks, 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 change of arm length 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 δ i, in order to make the deformable quadrotor aircraft pass through the corridor safely while considering the change of its arm length, it should meet the following conditions:

[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 segmented 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 through 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, the dynamic model of the deformable quadrotor aircraft is established based on the Newton-Euler equation:

[0113]

[0114] Among them, m = 1.587kg is the mass of the quadcopter, g = 9.81m / 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] Among them, ψ, φ, θ are the attitude angles of the quadrotor, which are 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 the deformable quadrotor changes with the arm length. The general relationship between the moment of inertia and the 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, and the change of the arm length is achieved by telescopic arms. Unlike traditional quadrotors, the arm length value will change 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 the 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 the 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 the arm length and the motor angle is given as follows:

[0125]

[0126] Among them, u5 represents the motor angle, a=-13.88, b=-77.40, c=73.84, and the coefficients are obtained by 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] Where 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, and 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 characteristics 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. 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] By 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 norm square 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] sA 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, that is:

[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 the left and right 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 of the trajectory at the starting point and the end point. It is difficult for the aircraft to reach a set value for the speed and acceleration of the intermediate point, so only continuity constraints are set for the intermediate point.

[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 of 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 safely pass through the corridor, 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 can obtain the perpendicular distance vector of the i-th (i = 1,..., m) segment of the trajectory 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 of its arm length, it should satisfy

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

[0160] By introducing h intermediate points and integrating them into the quadratic programming problem, the following expression is obtained:

[0161]

[0162] In this example, h = 5;

[0163] In addition, the maximum and minimum values of the arm length are set as follows:

[0164] In the non-corridor area, l min = 0.11 m, l max = 0.18 m; in the corridor area, l min = 0.11 m, l max = 0.15 m;

[0165] S4. Time allocation. The present invention adopts uniform time allocation, and each segmented trajectory is allocated 2s, that is, the total time is 10s;

[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 through MATLAB;

[0167] Embodiment 2:

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

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

[0170] From the simulation results, we can see 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 The detailed diagram of the corridor is given. The wider one is the actual 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" shape on the trajectory is used to indicate the change in arm length during the flight of the deformable quadcopter. 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 the sudden change of state during the flight of the deformable quadrotor. 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 beginning, and then maintains at 0.14m for 1s, 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 realize the deformation of the quadrotor 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 aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned 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 aircraft dynamics model, characterized in that: The following steps are involved: S1: Establish the dynamic model of the deformable quadrotor based on the Newton-Euler equation; S2: construct objective function; S3: setting constraints; the constraints include: waypoint constraints, continuity constraints, differential constraints and corridor constraints; S4: Time allocation: uniform time allocation is adopted, that is, the time used for each segmented 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 through MATLAB.

2. A trajectory planning method based on a deformable quadrotor aircraft dynamics model according to claim 1, characterized in that: The dynamic model of the deformable quadrotor aircraft is: 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; Among them, ψ, Φ, θ represent the attitude angles of the quadrotor, which are yaw angle, pitch angle and roll angle respectively; 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; The moment of inertia of the deformable quadrotor changes with the arm length. The relationship between the moment of inertia and the arm length is: J xx =J yy =A1l 2 +B1l+C1; <h2 style=";text-align:left;direction:ltr">J<h2 style=";text-align:left;direction:ltr"> zz <h2 style=";text-align:left;direction:ltr"> =A2l<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> +B2l+C2; Among them, A1, B1, C1 and A2, B2, C2 are coefficients of the relationship; l represents 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 the arm length and the motor angle is given as: Among them, u5 represents the motor angle, and a, b, and c represent dimensionless constants.

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 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: 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. Represented as the parameter to be optimized, i = 1, 2, ..., n; j = 1, 2, ..., m; The purpose of trajectory planning is to minimize the energy during flight, so the objective function is: Among them, r T =[x T y T z T ] T , All represent dimensionless constants; The coefficient to be determined Rewritten as a 5nm×1 vector c, the objective function is transformed into a quadratic programming problem: min c T Hc+f T c; s.t.A ieq c≤b ineq ; A eq c=b eq 。 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: select the points that the deformable quadrotor must pass through and set them at the endpoints of each segment, that is: s T (t i )=s i ,i=0,1,...,m.

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 tracks, ensuring that the states on the left and right sides are continuous: 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 .

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 a differential constraint on the waypoints located at the starting point and the end point.

7. The trajectory planning method based on the dynamic model of a deformable quadrotor aircraft according to claim 1, characterized in that: The corridor constraints are: Corridor restrictions are added to the inequality constraints of quadratic programming based on the change of arm length to ensure that the deformable quadrotor can safely pass through the corridor. 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: d i (t)=(r T (t)-r i )-((r T (t)-r i )·e)e; Define the width of the i-th corridor as δ i , in order to make the deformable quadrotor aircraft pass through the corridor safely while considering the change of its arm length, it should meet the following conditions: ||d i (t)|| ∞ +l i ≤δ i ; By introducing h intermediate points and integrating them into the quadratic programming problem, the expression is as follows:

Images