CN113985927B - Method for optimizing motion trail of amphibious shut-down of four-rotor unmanned aerial vehicle - Google Patents
Method for optimizing motion trail of amphibious shut-down of four-rotor unmanned aerial vehicle Download PDFInfo
- Publication number
- CN113985927B CN113985927B CN202111291256.9A CN202111291256A CN113985927B CN 113985927 B CN113985927 B CN 113985927B CN 202111291256 A CN202111291256 A CN 202111291256A CN 113985927 B CN113985927 B CN 113985927B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- constraint
- track
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 77
- 238000005457 optimization Methods 0.000 claims abstract description 61
- 238000013461 design Methods 0.000 claims abstract description 21
- 230000008569 process Effects 0.000 claims description 18
- 208000028804 PERCHING syndrome Diseases 0.000 claims description 14
- 238000005070 sampling Methods 0.000 claims description 13
- 241000269799 Perca fluviatilis Species 0.000 claims description 7
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 230000002068 genetic effect Effects 0.000 claims description 4
- 230000010354 integration Effects 0.000 claims description 2
- 238000004364 calculation method Methods 0.000 abstract description 14
- 230000000694 effects Effects 0.000 abstract description 3
- 230000008859 change Effects 0.000 description 5
- 102100029469 WD repeat and HMG-box DNA-binding protein 1 Human genes 0.000 description 2
- 101710097421 WD repeat and HMG-box DNA-binding protein 1 Proteins 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 230000036962 time dependent Effects 0.000 description 2
- 241001465754 Metazoa Species 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000006866 deterioration Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000004146 energy storage Methods 0.000 description 1
- 230000002349 favourable effect Effects 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 238000004904 shortening Methods 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/12—Target-seeking control
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a method for optimizing a motion trail of a four-rotor unmanned aerial vehicle during a amphibious shutdown, belonging to the technical field of unmanned aerial vehicles; the method comprises the specific steps of modeling four-rotor unmanned aerial vehicle dynamics, generating an open-loop motion track and optimizing the track under a mixed constraint condition; firstly, the method is based on a dynamic model, gives consideration to the constraint of the unmanned aerial vehicle capability, and is a dynamically feasible flight track. Secondly, the method includes the steps that both equality constraint and inequality constraint are included in the penalty function, and constraint-containing optimization problems are converted into unconstrained minimum problems in a mode of minimum value of the penalty function. The track optimization method provided by the invention is used for optimizing on the basis of the open-loop motion track, so that the optimal flight track can be directly obtained, the calculation time and calculation resources required by the wide sequence of the rear-end track are avoided, and the algorithm is more direct and simple. The numerical calculation workload of the method is smaller, the calculation efficiency is higher, and a better optimal design effect can be achieved.
Description
Technical Field
The invention belongs to the technical field of unmanned aerial vehicles, and particularly relates to a method for optimizing a motion track of a four-rotor unmanned aerial vehicle during a amphibious stop.
Background
Microminiature quadrotor unmanned aerial vehicle (Micro Quadrotor Unmanned Aerial Vehicle, abbreviated as MQUAV) is a current research hot spot and is widely applied to the fields of military, civil use and the like. In the military field, MQUAVs are equipped for team or individual soldiers for battlefield investigation surveillance or battle. In the civil field, the MQUAV can bear the tasks of regional monitoring, data acquisition, aerial shooting and the like. The MQUAV obtains wide attention by the advantages of low cost, light portability, strong maneuverability and the like, but the reduction of the unmanned aerial vehicle size is accompanied by the rapid reduction of the energy storage space, so that the serious problems of shortening of the endurance time and deterioration of the use efficiency are caused. Therefore, increasing MQUAV duration is a hot spot problem that is currently in need.
The vertical plane perching stopping strategy is an effective means for solving the problem of cruising currently, and the vertical plane perching stopping process of the unmanned plane is shown in figure 1. The sense of the perching strategy is derived from bird perching behaviors, and the perching capability of the unmanned aerial vehicle on a cable or a building wall is given by additionally arranging a perching device simulating animal limbs on the unmanned aerial vehicle. When the unmanned aerial vehicle is perched, gravity is overcome only by means of external acting force, and a propeller is not required to be driven to generate thrust, so that the aims of reducing energy consumption and prolonging effective task time can be achieved.
With the increasing range of applications of the amphibious vehicle, the track planning problems associated therewith are also receiving increasing attention. Due to the constraint characteristics of the amphibious stopping moving process and the under-actuated and strong coupling characteristics of the four-rotor unmanned aerial vehicle, how to plan the perching moving track of the four-rotor unmanned aerial vehicle becomes an important technical problem to be solved by the person skilled in the art.
CN113342056a provides an unmanned plane track planning method based on an improved a star algorithm in the technical field of unmanned plane intelligent control, a starting point and a target point are set on a map, random sampling is performed based on the starting point and the target point to obtain a sampling point, and shortest path searching is performed by using the a star algorithm and the node distance matrix, so that unmanned plane track planning is completed. CN106774421B discloses a unmanned aerial vehicle trajectory planning system, comprising: the unmanned aerial vehicle comprises an unmanned aerial vehicle module and a cloud platform module, wherein the unmanned aerial vehicle module is used for collecting flight track information of a current unmanned aerial vehicle and uploading the flight track information to the cloud platform module; the cloud platform module is used for analyzing the received flight track information and feeding back the corresponding optimal track planning information to the unmanned aerial vehicle corresponding to the unmanned aerial vehicle module. CN110308699a discloses a trajectory planning method, which firstly uses a PH curve to smooth a right-angle transition portion between vertical motion and horizontal motion in cartesian space, and determines a motion trajectory; planning the displacement of the 1-dimensional curve of the track by utilizing a polynomial motion rule, and determining the position of an interpolation point of the motion track; finally, the PH curve parameters are optimized by taking the minimum pick-and-place operation period as a target, and the minimum curve difference under the PH curve can be obtained through deduction proving the optimizing method.
In the prior art, the generation of a front-end path (CN 113342056A) and the optimization of a rear-end path (CN 110308699A) can be carried out according to a motion path, and the technical core is that a motion path meeting the requirements of obstacle avoidance and smooth and continuous track is generated. However, for the problem of stopping, the generated trajectory needs to satisfy not only the dynamics constraint but also the process constraint and the constraints such as the speed, angle and the like of the tail end, and thus, the problem is not a simple path planning problem but a trajectory planning problem under the condition of mixed constraint. Therefore, the invention provides a trajectory planning method for constructing the vertical surface perching stop by combining a penalty function method with numerical optimization.
Disclosure of Invention
The technical problems to be solved are as follows:
in order to avoid the defects of the prior art, the invention provides a four-rotor unmanned aerial vehicle amphibious stop motion track optimization method, which is used for solving the problem of planning the four-rotor unmanned aerial vehicle vertical plane perch stop motion track.
The technical scheme of the invention is as follows: a method for optimizing the motion trail of a four-rotor unmanned aerial vehicle during the amphibious shutdown is characterized by comprising the following specific steps:
step one: modeling the dynamics of a four-rotor unmanned aerial vehicle;
and (3) coordinate system selection: the coordinate systems required by the modeling of the four-rotor unmanned aerial vehicle are respectively the ground coordinate system O g x g ygz g And body coordinate system O b x b y b z b The method comprises the steps of carrying out a first treatment on the surface of the The relative relationship of the two coordinate systems is described by three angles of a roll angle phi, a pitch angle theta and a yaw angle phi;
establishing a dynamics equation, and establishing a dynamics equation of longitudinal motion of the quadrotor unmanned aerial vehicle according to the rigid body kinematics equation and the dynamics equation:
wherein F is x In the ground coordinate system x for combining external force g Component of axis, F z In the ground coordinate system z for combining external force g The component of the axis, M is the sum moment vector of the unmanned plane in the ground coordinate system y g A component of the shaft; i is the propeller count number, i=1, 2,3,4; f (F) i For thrust of propeller i, M i Is the torque of the propeller i; u is the mass center speed vector of the unmanned aerial vehicle in a ground coordinate system x g The component of the axis, W is the mass center speed vector of the unmanned plane in the ground coordinate system z g The component of the axis, q is the angular velocity vector of the unmanned aerial vehicle in the ground coordinate system y g A component; i is four-rotor unmanned aerial vehicle about body coordinate system y b The rotational inertia of the shaft, m is the mass of the unmanned aerial vehicle;
step two: generating an open loop motion trail;
2.1. solving a motion trail based on time;
through integration along time, a time-varying curve of displacement and pitch angle of the unmanned aerial vehicle is obtained, and the specific expression is as follows:
in U 0 U value at time t=0, W 0 W is t=0, q 0 Is the q value at time t=0. X is x 0 、z 0 Coordinate values at time t=0, θ 0 Pitch angles at time t=0, respectively;
2.2. solving a motion trail based on the distance;
according to the coordinate position of the current moment, solving the thrust of the current moment by adopting an interpolation method, and specifically comprising the following steps:
2.2.1 problem settings: determining motion constraint by giving a starting position and a target position of track optimization;
2.2.2 thrust setting: dividing within a given flight distance, setting a sampling point number of N+1, uniformly dividing a track into N sections, and setting a thrust sequence { F ] by adopting a random sampling method 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) J is the sampling point sequence number and j=1 to n+1, and a spline method is adopted to generate a thrust curve relative to the distance coordinate;
2.2.3 trajectory generation: solving the flight track by adopting a time propulsion method, wherein the thrust at the current moment is in a thrust sequence { F ] according to the position coordinates corresponding to the current moment 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) Interpolation;
2.2.4 cut-off condition: when the horizontal coordinate of the solving track is consistent with the horizontal coordinate of the target point, the track solving is terminated;
step three: track optimization under a mixing constraint condition;
3.1. establishing a constraint;
the four-rotor unmanned aerial vehicle needs to meet that the maximum required thrust in the motion process is smaller than the maximum thrust of the motor in the vertical plane amphibious stop motion process, and meets the motion constraint determined in the second step during landing, and the four-rotor unmanned aerial vehicle is specifically expressed as follows:
3.1.1 dynamic constraints: in the track optimization process, the thrust output at any moment is smaller than the single-shaft maximum thrust F max ；
3.1.2 pitch angle constraint: the pitch angle of the four-rotor unmanned aerial vehicle is theta at the moment of contact of the unmanned aerial vehicle and the surface of a perch des Ensuring that the unmanned aerial vehicle directs the perching device to the perching surface;
3.1.3 speed constraints: the surface speed U of the four-rotor unmanned plane at the moment of contact between the unmanned plane and the surface of the amphibious stall des In the range of U min ≤U des ≤U max Normal speed range is W min ≤W des ≤W max ；
3.1.4 position constraints: the landing position z of the four-rotor unmanned aerial vehicle at the moment of contact of the unmanned aerial vehicle with the surface of the perch f With the target position z des Not exceeding a certain value, i.e., |z f -z des The I is less than or equal to epsilon, and epsilon is an allowable error;
3.2. and constructing a penalty function comprising a performance function, a target constraint function and a power constraint function, wherein the expression is as follows:
Cost＝J 1 +J 2 +J 3
wherein J is 1 As a function of performance, J 2 For target constraint, J 3 For dynamic constraints, each constraint expression is as follows:
J 1 ＝∑(T·v)·Δt
J 2 ＝(U f -U des ) 2 +(W f -W des ) 2 +(θ f -θ des ) 2
J 3 ＝∑max{F i (t)-F max ,0}
3.3. constructing an optimization solver;
distance-based thrust sequence { F established in step two 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) The j=1 to n+1, and a feasible track of the motion constraint and the dynamic constraint is obtained by combining a penalty function with a numerical optimization algorithm; the track optimization flow comprises the following steps:
3.3.1 initial setting: producing an initial sample of an initial thrust time sequence by adopting a random sampling method, wherein the thrust sequence takes a distance as a reference and is marked as { F } 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) J=1 to n+1, and the internal interpolation adopts a spline method;
3.3.2 calculating a motion trail based on the motion trail generation method in the second step, and calculating a penalty function;
3.3.3 applying numerical optimization algorithm to the design variable { F 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) J=1 to n+1, searching and updating the design variables;
3.3.4 when the penalty function value is less than the threshold, the optimization is ended, otherwise, steps 3.3.2 and 3.3.3 are repeated.
The invention further adopts the technical scheme that: in the first step, the ground coordinate system is an inertial coordinate system fixed on the earth, the body coordinate system is a coordinate system fixed on the unmanned aerial vehicle, and the center O of the coordinate system b Is positioned at the mass center x of the unmanned plane b Is positioned in the symmetry plane of the unmanned plane and is directed forwards, z b Is positioned in the symmetry plane of the unmanned plane and points upwards, y b Is satisfied.
The invention further adopts the technical scheme that: in the step two 2.2.1, the motion constraint includes equality constraint and inequality constraint.
The invention further adopts the technical scheme that: in the second step 2.2.1, the motion constraint includes a position, a speed, an attitude angle, and an angular speed.
The invention further adopts the technical scheme that: in the step III 3.1.3, U des And W is des Is a range of values of (a)Depending on the speed envelope of the perch in step 3.1.2.
The invention further adopts the technical scheme that: in the step three 3.3.3, a genetic optimization algorithm is adopted in the numerical optimization algorithm.
The invention further adopts the technical scheme that: in the third step 3.3.4, the principle of setting the threshold value is as follows:
designing the current optimization iteration for i times in total, and generating i optimization design results; reading the current optimization result opt i Opt from last optimization result i-1 If |opt i -opt i-1 |/|opt i-1 The optimization process is considered to be converged and ended if the I is less than delta; delta is an artificially set convergence error, typically a value on the order of 1% or 1%.
Advantageous effects
The invention has the beneficial effects that: the invention provides a method for optimizing a motion trail of a four-rotor unmanned aerial vehicle during amphibious shutdown. Firstly, the method is based on a dynamic model, gives consideration to the constraint of the unmanned aerial vehicle capability, and is a dynamically feasible flight track. Secondly, the method includes the steps that both equality constraint and inequality constraint are included in the penalty function, and constraint-containing optimization problems are converted into unconstrained minimum problems in a mode of minimum value of the penalty function. Compared with the method of front end path planning and back end track fairing proposed by the prior 'CN 113342056A' and other patents, the track optimizing method provided by the invention carries out optimizing on the basis of the open loop motion track, can directly obtain the optimal flight track, avoids the calculation time and calculation resources required by the wide and smooth back end track, and has more direct and simple algorithm. From the perspective of numerical calculation, the penalty function method simplifies the calculation amount required for constraint processing, and the penalty function combined with the digital optimization method is favorable for realizing global optimization of design variables, so that the numerical calculation workload of the method is smaller, the calculation efficiency is higher, and a better optimization design effect can be achieved.
The invention provides a method for optimizing the motion trail of a four-rotor unmanned aerial vehicle during the amphibious shutdown, which has the advantages of dynamic feasibility, reduced numerical calculation workload and high planning efficiency.
Drawings
FIG. 1 is a schematic illustration of a four-rotor unmanned aerial vehicle hanging surface perch;
FIG. 2 is a flow chart for solving open loop motion trajectories of a quad-rotor unmanned helicopter;
FIG. 3 is a flow chart of a four-rotor unmanned aerial vehicle trajectory optimization in an embodiment of the invention;
fig. 4 is a schematic diagram of a motion trail of a quadrotor unmanned aerial vehicle in an embodiment of the invention;
fig. 5 is a graph of a trajectory change of a quad-rotor unmanned helicopter in an embodiment of the invention; (a) The height-dependent position change curve of the quadrotor unmanned plane is adopted; (b) The pitch angle change curve of the four-rotor unmanned aerial vehicle along with the position is adopted; (c) The horizontal speed change curve of the four-rotor unmanned plane along with the position is adopted; (d) The vertical speed of the quadrotor unmanned plane is a position-dependent curve.
Detailed Description
The embodiments described below by referring to the drawings are illustrative and intended to explain the present invention and should not be construed as limiting the invention.
Referring to fig. 1-5, the method for optimizing the motion trail of the amphibious unmanned aerial vehicle comprises the following steps:
step one, four rotor unmanned aerial vehicle dynamics modeling;
in the embodiment, the mass m of the four-rotor unmanned aerial vehicle is 0.235kg, and the moment of inertia I is 0.002 kg.m 2 The distance L between the rotating shafts of two adjacent propellers is 0.15m, namely the propeller force arm is 0.075m.
And (3) coordinate system selection: the coordinate systems required by the modeling of the four-rotor unmanned aerial vehicle are respectively the ground coordinate system O g x g y g z g And body coordinate system O b x b y b z b The method comprises the steps of carrying out a first treatment on the surface of the The relative relationship of the two coordinate systems is described by three angles of a roll angle phi, a pitch angle theta and a yaw angle phi; the ground coordinate system is an inertial coordinate system fixed on the earth, the machine body coordinate system is a coordinate system fixed on the unmanned aerial vehicle, and the center of the coordinate system is O b Is positioned at the mass center x of the unmanned plane b Is positioned in the symmetry plane of the unmanned plane and is directed forwards, z b Is positioned in the symmetry plane of the unmanned plane and points upwards, y b Is determined to satisfyA right hand coordinate system.
The kinetic equation of the longitudinal motion of the quadrotor unmanned aerial vehicle is shown as follows
I is the relation y of the four-rotor unmanned aerial vehicle b The rotational inertia of the shaft, M is the sum moment vector of the unmanned plane in the ground coordinate system y g Component of the axis.
Step two, generating an open loop motion trail
1. Time-based motion trajectory solution
The problem of the four-rotor unmanned aerial vehicle stopping can be simplified to longitudinal movement in a two-dimensional plane, so that the unmanned aerial vehicle in y is ignored g Displacement of the shaft, consider only the unmanned aerial vehicle in x g Axis and z g The displacement movement and the rotation movement of the shaft. Thus, the unmanned aerial vehicle longitudinal dynamics equation can be expressed as follows:
wherein F is x In the ground coordinate system x for combining external force g Component of axis, F z In the ground coordinate system z for combining external force g The component of the axis, M is the sum moment vector of the unmanned plane in the ground coordinate system y g A component of the shaft; i is the propeller count number, i=1, 2,3,4; f (F) i For thrust of propeller i, M i Is the torque of the propeller i; u is the mass center speed vector of the unmanned aerial vehicle in a ground coordinate system x g The component of the axis, W is the mass center speed vector of the unmanned plane in the ground coordinate system z g The component of the axis, q is the angular velocity vector of the unmanned aerial vehicle in the ground coordinate system y g A component; i is four-rotor unmanned aerial vehicle about body coordinate system y b The rotational inertia of the shaft, m is the mass of the unmanned aerial vehicle;
the format described above is rewritten as:
the rewriting types show that the speed change rate of the quadrotor unmanned aerial vehicle at a certain momentIs determined by the thrust and pitch angle at the present moment, and pitch angle acceleration +>Still determined by the thrust. Therefore, given the time-dependent thrust curves of the respective axes of the unmanned aerial vehicle, the time-dependent speed and angular velocity curves of the unmanned aerial vehicle can be obtained. Further, by integrating over time, a displacement versus pitch angle time profile of the drone can be obtained. The specific expression is
In U 0 U value at time t=0, W 0 W is t=0, q 0 Is the q value at time t=0. X is x 0 、z 0 Coordinate values at time t=0, θ 0 Pitch angles at times t=0, respectively.
2. Distance-based motion trajectory solution
The track solving method can obtain the open-loop motion track on the premise of giving an initial value and a thrust time sequence. However, in actual flight, the flight time cannot be obtained, and the flight distance, that is, the starting position and the target position, is set, so that the method does not adopt a thrust sequence of relative time, but adopts a distance-based thrust sequence as a solving input. In the track solving process, according to the coordinate position of the current moment, adopting an interpolation method to solve the thrust of the current moment. The method comprises the following specific steps:
1) Problem setting: determining motion constraints (equality constraints and inequality constraints) by giving a starting position and a target position of track optimization; the motion constraints include position, speed, attitude angle, angular velocity;
2) Thrust setting: dividing within a given flight distance, setting a sampling point number of N+1, uniformly dividing a track into N sections, and setting a thrust sequence { F ] by adopting a random sampling method 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) J is the sampling point sequence number and j=1 to n+1, and a spline method is adopted to generate a thrust curve relative to the distance coordinate;
3) Track generation: solving the flight track by adopting a time propulsion method, wherein the thrust at the current moment is in a thrust sequence { F ] according to the position coordinates corresponding to the current moment 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) Interpolation;
4) Cut-off condition: when the horizontal coordinate of the solving track is consistent with the horizontal coordinate of the target point, the track solving is terminated;
step three, track optimization
1. Constraint setting
The four rotor unmanned aerial vehicle is for position, speed, attitude angle, angular velocity that the motion process contained in the vertical plane is perched, needs to satisfy the maximum required thrust in the motion process and be less than motor maximum thrust and satisfy constraint such as speed, pitch angle, position error when landing, specifically expressed as follows:
1) Power constraint: in the track optimization process, the thrust output at any moment must be smaller than the uniaxial maximum thrust F max ；
2) Pitch angle constraint: the pitch angle theta of the four-rotor unmanned aerial vehicle at the moment of contact of the unmanned aerial vehicle with the vertical plane des -pi/2, ensuring that the unmanned aerial vehicle directs its perching device to the vertical plane;
3) Speed constraint: the vertical wall speed U of the four-rotor unmanned aerial vehicle at the moment of contact of the unmanned aerial vehicle with the vertical surface des The range is 0.8 m/s-1.2 m/s, and the normal velocity W des The range is-0.2 m/s to 0.2m/s; u (U) des And W is des The value range of (2) depends on the speed envelope of the stopping device in step 2).
4) Position constraint: the unmanned aerial vehicle is in contact with the vertical plane at the moment of fourRotor unmanned aerial vehicle landing position z f With the target position z des The height error of (2) is not more than 0.05m.
2. Penalty function definition
The motion track of the amphibious machine comprises constraints such as speed, pitch angle, position error and the like, and the constraint-containing optimization problem can be converted into the unconstrained optimization problem by a penalty function method. The flight track meeting the motion constraint is obtained by constructing the penalty function, the principle of constructing the penalty function ensures that the motion track meets the target constraint when the penalty function approaches 0, and therefore, the penalty function is constructed by the following expression:
Cost＝J 1 +J 2 +J 3
wherein J is 1 As a function of performance, J 2 For target constraint, J 3 For dynamic constraints, each constraint expression is as follows:
J 1 ＝∑(T·v)·Δt
J 2 ＝(U f -1) 2 +(W f -0) 2 +(θ f -π/2) 2
J 3 ＝∑max{F i (t)-F max ,0}
3. and constructing a hybrid optimization solver.
Distance-based thrust sequence { F established in step two 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) The j=1 to n+1, and a feasible track of the motion constraint and the dynamic constraint is obtained by combining a penalty function with a numerical optimization algorithm; the track optimization flow comprises the following steps:
1. an initial value of the motion state is set. Setting U 0 ＝0，W 0 ＝0，q 0 ＝0，θ 0 ＝0。x 0 、z 0 The coordinate values at the time t=0 are set to-2 m and-1 m, respectively.
2. Setting initial values of design variables to optimize design samples. Will { F 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j )},j＝1～10, definition F 1 (x j ),F 2 (x j ) Is prop1_stress, and, likewise, F is defined 3 (x j ),F 4 (x j ) Is prop2_stress. The generation formula of prop_stress is as follows, in which the first parameter is 0.25 mg, and the parameter is set to ensure that the initial thrust is equal to gravity and meets the suspension start condition. Subsequently [1.0+0.2 x rand (1, 9)]In total, 9 variables, were added with a random variation of + -20% on a 0.25 mg basis to generate a random initial value. Prop_stress generates 10-dimensional vector in total
prop_guess＝0.25*m*g,0.25*m*g*[1.0+0.2*rand(1,9)]
In the embodiment of the invention, the generated sequence is shown as follows, F 1 (x j ),F 2 (x j ) Is prop1_stress, and likewise F 3 (x j ),F 4 (x j ) Is prop2_stress. This sequence will ultimately be the initial sample of the design variables of the optimal design.
prop1_guess＝[1.183 1.396 1.956 1.698 2.037 1.518 2.080 0.870 1.243 1.181]；
prop2_guess＝[0.861 2.000 1.784 1.520 1.262 1.777 1.104 1.573 1.214 1.341]；
3. And (5) constructing an optimal design framework. The embodiment of the invention adopts a genetic optimization algorithm to carry out optimization design work, wherein the optimization target is a penalty function constructed in the third step, and the initial value is set as x 0 、z 0 Respectively-2 m and-1 m, and U 0 ＝0，W 0 ＝0，q 0 ＝0，θ 0 =0. The design variables are prop1_guide and prop2_guide, so that optimization design iteration can be performed according to the flow chart.
4. And (3) ending the optimization when the penalty function value is smaller than the threshold value, otherwise, repeating the step (2) and the step (3). The principle of threshold setting is: designing the current optimization iteration for i times in total, and generating i optimization design results; reading the current optimization result opt i Opt from last optimization result i-1 If |opt i -opt i-1 |/|opt i-1 I < delta, thenThe optimization process is considered to be converged and ended; delta is an artificially set convergence error, typically a value on the order of 1% or 1%.
And iterating for 20 times, finishing the optimal design, and calculating the result as shown in fig. 4 and 5.
By combining the calculation process, the method of combining the penalty function with the numerical optimization adopted by the embodiment of the invention can better obtain the optimal design result, and the optimal result meets the dynamic constraint, the motion state constraint and the maximum thrust constraint. In addition, from the algorithm perspective, the genetic algorithm has better global convergence, reduces the dependence on design samples, and can perform global optimization. From the aspect of calculation efficiency, in the embodiment of the invention, the optimal result is obtained by iterating 20 times in the optimization process, and compared with the mountain brother search algorithm which is required in the CN113342056A and the CN110308699A patents, the calculation amount is smaller, and the optimizing efficiency is higher.
Although embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and not to be construed as limiting the invention, and that variations, modifications, alternatives, and variations may be made in the above embodiments by those skilled in the art without departing from the spirit and principles of the invention.
Claims (7)
1. A method for optimizing the motion trail of a four-rotor unmanned aerial vehicle during the amphibious shutdown is characterized by comprising the following specific steps:
step one: modeling the dynamics of a four-rotor unmanned aerial vehicle;
and (3) coordinate system selection: the coordinate systems required by the modeling of the four-rotor unmanned aerial vehicle are respectively the ground coordinate system O g x g y g z g And body coordinate system O b x b y b z b The method comprises the steps of carrying out a first treatment on the surface of the The relative relationship of the two coordinate systems is described by three angles of a roll angle phi, a pitch angle theta and a yaw angle phi;
establishing a dynamics equation, and establishing a dynamics equation of longitudinal motion of the quadrotor unmanned aerial vehicle according to the rigid body kinematics equation and the dynamics equation:
wherein F is x In the ground coordinate system x for combining external force g Component of axis, F z In the ground coordinate system z for combining external force g The component of the axis, M is the sum moment vector of the unmanned plane in the ground coordinate system y g A component of the shaft; i is the propeller count number, i=1, 2,3,4; f (F) i For thrust of propeller i, M i Is the torque of the propeller i; u is the mass center speed vector of the unmanned aerial vehicle in a ground coordinate system x g The component of the axis, W is the mass center speed vector of the unmanned plane in the ground coordinate system z g The component of the axis, q is the angular velocity vector of the unmanned aerial vehicle in the ground coordinate system y g A component; i is four-rotor unmanned aerial vehicle about body coordinate system y b The rotational inertia of the shaft, m is the mass of the unmanned aerial vehicle;
step two: generating an open loop motion trail;
2.1. solving a motion trail based on time;
through integration along time, a time-varying curve of displacement and pitch angle of the unmanned aerial vehicle is obtained, and the specific expression is as follows:
in U 0 U value at time t=0, W 0 W is t=0, q 0 Q value, x at time t=0 0 、z 0 Coordinate values at time t=0, θ 0 Pitch angles at time t=0, respectively;
2.2. solving a motion trail based on the distance;
according to the coordinate position of the current moment, solving the thrust of the current moment by adopting an interpolation method, and specifically comprising the following steps:
2.2.1 problem settings: determining motion constraint by giving a starting position and a target position of track optimization;
2.2.2 thrust setting: dividing within a given flight distance, setting a sampling point number of N+1, uniformly dividing a track into N sections, and setting a thrust sequence { F ] by adopting a random sampling method 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) J is the sampling point sequence number and j=1 to n+1, and a spline method is adopted to generate a thrust curve relative to the distance coordinate;
2.2.3 trajectory generation: solving the flight track by adopting a time propulsion method, wherein the thrust at the current moment is in a thrust sequence { F ] according to the position coordinates corresponding to the current moment 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) Interpolation;
2.2.4 cut-off condition: when the horizontal coordinate of the solving track is consistent with the horizontal coordinate of the target point, the track solving is terminated;
step three: track optimization under a mixing constraint condition;
3.1. establishing a constraint;
the four-rotor unmanned aerial vehicle needs to meet that the maximum required thrust in the motion process is smaller than the maximum thrust of the motor in the vertical plane amphibious stop motion process, and meets the motion constraint determined in the second step during landing, and the four-rotor unmanned aerial vehicle is specifically expressed as follows:
3.1.1 dynamic constraints: in the track optimization process, the thrust output at any moment is smaller than the single-shaft maximum thrust F max ；
3.1.2 pitch angle constraint: the pitch angle of the four-rotor unmanned aerial vehicle is theta at the moment of contact of the unmanned aerial vehicle and the surface of a perch des Ensuring that the unmanned aerial vehicle directs the perching device to the perching surface;
3.1.3 speed constraints: the vertical wall speed U of the four-rotor unmanned aerial vehicle at the moment of contact of the unmanned aerial vehicle with the vertical surface des In the range of U min ≤U des ≤U max Normal speed range is W min ≤W des ≤W max ；
3.1.4 position constraints: the landing position z of the four-rotor unmanned aerial vehicle at the moment of contact of the unmanned aerial vehicle with the surface of the perch f With the target position z des Not exceeding a certain value, i.e., |z f -z des The I is less than or equal to epsilon, and epsilon is an allowable error;
3.2. and constructing a penalty function comprising a performance function, a target constraint function and a power constraint function, wherein the expression is as follows:
Cost＝J 1 +J 2 +J 3
wherein J is 1 As a function of performance, J 2 For target constraint, J 3 For dynamic constraints, each constraint expression is as follows:
J 1 ＝∑(T·v)·Δt
J 2 ＝(U f -U des ) 2 +(W f -W des ) 2 +(θ f -θ des ) 2
J 3 ＝∑max{F i (t)-F max ,0}
3.3. constructing an optimization solver;
distance-based thrust sequence { F established in step two 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) The j=1 to n+1, and a feasible track of the motion constraint and the dynamic constraint is obtained by combining a penalty function with a numerical optimization algorithm; the track optimization flow comprises the following steps:
3.3.1 initial setting: producing an initial sample of an initial thrust time sequence by adopting a random sampling method, wherein the thrust sequence takes a distance as a reference and is marked as { F } 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) J=1 to n+1, and the internal interpolation adopts a spline method;
3.3.2 calculating a motion trail based on the motion trail generation method in the second step, and calculating a penalty function;
3.3.3 applying numerical optimization algorithm to the design variable { F 1 (x j ),F 2 (x j ),F 3 (x j ),F 4 (x j ) J=1 to n+1, searching and updating the design variables;
3.3.4 when the penalty function value is less than the threshold, the optimization is ended, otherwise, steps 3.3.2 and 3.3.3 are repeated.
2. The method for optimizing the motion trail of the amphibious unmanned aerial vehicle with four rotor wings according to claim 1 is characterized in that: in the first step, the ground coordinate system is an inertial coordinate system fixed on the earth, the body coordinate system is a coordinate system fixed on the unmanned aerial vehicle, and the center O of the coordinate system b Is positioned at the mass center x of the unmanned plane b Is positioned in the symmetry plane of the unmanned plane and is directed forwards, z b Is positioned in the symmetry plane of the unmanned plane and points upwards, y b Is satisfied.
3. The method for optimizing the motion trail of the amphibious unmanned aerial vehicle with four rotor wings according to claim 1 is characterized in that: in the step two 2.2.1, the motion constraint includes equality constraint and inequality constraint.
4. The method for optimizing the motion trail of the amphibious unmanned aerial vehicle with four rotor wings according to claim 1 is characterized in that: in the second step 2.2.1, the motion constraint includes a position, a speed, an attitude angle, and an angular speed.
5. The method for optimizing the motion trail of the amphibious unmanned aerial vehicle with four rotor wings according to claim 1 is characterized in that: in the step III 3.1.3, U des And W is des The value range of (2) depends on the speed envelope of the perching device in step 3.1.2.
6. The method for optimizing the motion trail of the amphibious unmanned aerial vehicle with four rotor wings according to claim 1 is characterized in that: in the step three 3.3.3, a genetic optimization algorithm is adopted in the numerical optimization algorithm.
7. The method for optimizing the motion trail of the amphibious unmanned aerial vehicle with four rotor wings according to claim 1 is characterized in that: in the third step 3.3.4, the principle of setting the threshold value is as follows:
designing the current optimization iteration for i times in total, and generating i optimization design results; reading the current optimization result opt i Opt from last optimization result i-1 If |opt i -opt i-1 |/|opt i-1 The optimization process is considered to be converged and ended if the I is less than delta; delta is an artificially set convergence error, typically a value on the order of 1% or 1%.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111291256.9A CN113985927B (en) | 2021-10-28 | 2021-10-28 | Method for optimizing motion trail of amphibious shut-down of four-rotor unmanned aerial vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111291256.9A CN113985927B (en) | 2021-10-28 | 2021-10-28 | Method for optimizing motion trail of amphibious shut-down of four-rotor unmanned aerial vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113985927A CN113985927A (en) | 2022-01-28 |
| CN113985927B true CN113985927B (en) | 2023-11-21 |
Family
ID=79745973
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111291256.9A Active CN113985927B (en) | 2021-10-28 | 2021-10-28 | Method for optimizing motion trail of amphibious shut-down of four-rotor unmanned aerial vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113985927B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116992575B (en) * | 2023-09-26 | 2024-01-09 | 国科大杭州高等研究院 | Space-time baseline-based air target single-star positioning method |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108445898A (en) * | 2018-05-14 | 2018-08-24 | 南开大学 | Quadrotor unmanned vehicle system motion planing method based on differential flat characteristic |
| WO2019025293A1 (en) * | 2017-08-01 | 2019-02-07 | Airbus Defence And Space Sas | Onboard control unit for a drone system, drone and drone system comprising the onboard control unit |
| WO2019122842A1 (en) * | 2017-12-21 | 2019-06-27 | AV8OR IP Limited | Autonomous unmanned aerial vehicle and method of control thereof |
| CN112241125A (en) * | 2020-10-29 | 2021-01-19 | 北京理工大学 | Unmanned aerial vehicle trajectory tracking method based on differential flatness characteristic |
| CN112947593A (en) * | 2020-08-25 | 2021-06-11 | 北京蓝警科技有限公司 | Method and system for intercepting target by using unmanned aerial vehicle |
| CN113342059A (en) * | 2021-05-31 | 2021-09-03 | 西安电子科技大学 | Multi-unmanned aerial vehicle tracking mobile radiation source method based on position and speed errors |
-
2021
- 2021-10-28 CN CN202111291256.9A patent/CN113985927B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019025293A1 (en) * | 2017-08-01 | 2019-02-07 | Airbus Defence And Space Sas | Onboard control unit for a drone system, drone and drone system comprising the onboard control unit |
| WO2019122842A1 (en) * | 2017-12-21 | 2019-06-27 | AV8OR IP Limited | Autonomous unmanned aerial vehicle and method of control thereof |
| CN108445898A (en) * | 2018-05-14 | 2018-08-24 | 南开大学 | Quadrotor unmanned vehicle system motion planing method based on differential flat characteristic |
| CN112947593A (en) * | 2020-08-25 | 2021-06-11 | 北京蓝警科技有限公司 | Method and system for intercepting target by using unmanned aerial vehicle |
| CN112241125A (en) * | 2020-10-29 | 2021-01-19 | 北京理工大学 | Unmanned aerial vehicle trajectory tracking method based on differential flatness characteristic |
| CN113342059A (en) * | 2021-05-31 | 2021-09-03 | 西安电子科技大学 | Multi-unmanned aerial vehicle tracking mobile radiation source method based on position and speed errors |
Non-Patent Citations (1)
| Title |
|---|
| 基于hp自适应伪谱法的四旋翼无人机编队轨迹优化;张博渊;宗群;鲁瀚辰;邵士凯;;中国科学:技术科学(第03期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113985927A (en) | 2022-01-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110673623B (en) | Quad-rotor unmanned aerial vehicle landing method based on dual-ring PD control algorithm control | |
| CN113985927B (en) | Method for optimizing motion trail of amphibious shut-down of four-rotor unmanned aerial vehicle | |
| CN112684781B (en) | Multi-agent distributed model prediction control method and system | |
| CN111948940B (en) | Trajectory optimization method of tilt rotor unmanned aerial vehicle based on dynamic optimal control | |
| CN115494866A (en) | Multi-unmanned aerial vehicle global and local path intelligent planning method and system | |
| CN110304244A (en) | Flight control method, device, vert rotor aircraft and medium | |
| CN113848982B (en) | Four-rotor unmanned aerial vehicle amphibious shut-down motion trail planning and tracking control method | |
| Zhang et al. | Research on obstacle avoidance algorithm of multi-uav consistent formation based on improved dynamic window approach | |
| CN113608543A (en) | Method, device, equipment and storage medium for self-adaptive planning of flight path of aircraft | |
| Gu et al. | Design and Verification of UAV Formation Controller based on Leader-Follower Method | |
| CN116540780A (en) | Unmanned aerial vehicle decision control method based on game guidance | |
| Yanhui et al. | Flight control system simulation platform for UAV based on integrating simulink with stateflow | |
| CN114089780B (en) | Urban space-oriented multi-rotor unmanned aerial vehicle path planning method | |
| CN113885549B (en) | Four-rotor gesture track control method based on dimension clipping PPO algorithm | |
| CN205652355U (en) | Aircraft | |
| CN111459179A (en) | Four-rotor active disturbance rejection controller based on particle swarm optimization algorithm | |
| Zu et al. | A trajectory design method for quadrotor based on DMOC method and Dubins path algorithm | |
| CN116774735B (en) | Unmanned aerial vehicle cluster track planning method and system based on edge calculation | |
| CN117590862B (en) | Distributed unmanned aerial vehicle preset time three-dimensional target surrounding control method and system | |
| Chen et al. | The design of the four rotor unmanned aircraft control algorithm | |
| Dong et al. | Design of heading control system for USV based on MOOS-IvP | |
| Li et al. | Multi-quadrotor UAVs formation obstacle avoidance control based on virtual leader | |
| CN116719347A (en) | Multi-rotor unmanned aerial vehicle online safety track generation method under multi-constraint | |
| Zhao et al. | Aerodynamic characteristics analysis and robustness analysis of ducted-fan UAV based on Magnus effect | |
| Orhan et al. | A Robust Controller for Transition between Hover and Forward Flight for Hybrid Fixed Wing-Multicopters |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |