CN116466746B

CN116466746B - Planning control method and device for four-rotor cluster to pass through dynamic waypoints at high speed

Info

Publication number: CN116466746B
Application number: CN202310448139.1A
Authority: CN
Inventors: 沈雨阳; 周靳; 赵方国; 徐金明; 李硕
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2023-04-24
Filing date: 2023-04-24
Publication date: 2023-09-29
Anticipated expiration: 2043-04-24
Also published as: CN116466746A

Abstract

The invention discloses a planning control method and a device for a four-rotor cluster to pass through dynamic waypoints at high speed, which comprises the steps of firstly establishing a dynamic model of the four-rotor cluster, acquiring a motion equation of the four-rotor cluster to be passed through the waypoints, introducing two progress variables to ensure that the four-rotor cluster can sequentially pass through each static or moving waypoint, constructing inter-aircraft obstacle avoidance constraint of the four-rotor cluster considering sinking loss, modeling the process of the whole cluster passing through the waypoints at high speed into a nonlinear optimization model, designing a corresponding optimization objective function through a progress variable matrix containing time information, and fixing discrete time intervals to ensure that the time stamps of the four-rotor cluster flying can be kept consistent. The invention can plan the track of the four-rotor cluster passing through the stationary or moving route points at high speed, designs a differential flat-based position speed feedback controller, and realizes the tracking control of the four-rotor cluster limit high-speed flight track, thereby realizing the purpose of passing through each stationary or moving target route point one by one.

Description

Planning control method and device for four-rotor cluster to pass through dynamic waypoints at high speed

Technical Field

The invention belongs to the field of four-rotor planning control, and particularly relates to a method and a device for enabling a four-rotor cluster to pass through stationary or moving waypoints at high speed.

Background

At present, the four-rotor cluster receives more and more attention in the field of robots, and becomes a current hot research topic. The four-rotor wing cluster is a research hotspot in unmanned plane technology, and has wide application in earthquake relief, search and rescue and material transportation due to the characteristics of high flexibility, strong adaptability and the like. The four-rotor-wing cluster high-speed passing route point technology is one of the research directions of current comparison attention because rescue time is very urgent when natural disasters occur, and the technology carries out planning control on the four-rotor-wing cluster through a dynamic model so as to realize high-speed and accurate passing route point. High speed refers to the limit control output of the four-rotor cluster motor which can fully exert the efficacy of the four-rotor motor and is always maintained for a period of time. At present, a genetic algorithm-based method, a deep reinforcement learning-based method and an optimization-based method are mainly adopted in the disclosed technical scheme to realize planning and control of the four-rotor cluster.

The conventional method needs to plan a smooth passing track of the four-rotor cluster, so that the effect of controlling the motor is correspondingly smooth, and therefore, the limit control output of the four-rotor cluster motor cannot be maintained for a period of time all the time, and the real limit high-speed passing cannot be achieved.

Disclosure of Invention

Aiming at the defects of the existing four-rotor-wing cluster high-speed passing waypoint method, the invention provides a method and a device for enabling the four-rotor-wing cluster to pass through static or moving waypoints at high speed, the optimal control problem is converted into a nonlinear optimization problem, an optimization objective function is skillfully designed, and the time stamp of the four-rotor-wing cluster flying is kept consistent by fixing discrete time intervals. Meanwhile, sensors are carried to sense the motion information of surrounding waypoints to be passed through, motion equations of the waypoints are obtained through analysis of Kalman filtering algorithm, the waypoints which are static or moving are passed through at high speed under the condition of taking a dynamic model into consideration, a feedforward-feedback controller based on differential flatness is designed to track, and inter-aircraft obstacle avoidance under the air flow interference among clusters is realized.

The invention aims at realizing the following technical scheme:

according to a first aspect of the present specification, a method for planning and controlling a four-rotor cluster to pass through a dynamic waypoint at a high speed is provided, including the following steps:

S1, modeling is carried out based on a dynamic model of four rotors in practical application, and a calculation formula of the dynamic model of the four rotors is as follows:

wherein ,f_dyn ( _i x, _i u) is the kinetic equation for the ith quad-rotor in the quad-rotor cluster,is that _i p derivative of _i p is the position of the ith quad-rotor, < >>Is that _i The derivative of v, _i v is the speed of the ith quadrotor, g is the gravitational acceleration at the current position, _i m is the mass of the ith frame of four rotors, R # _i q) _i Is a rotation matrix of the ith four-rotor wing converted from a world coordinate system to a machine body coordinate system, _i t is the thrust generated by the ith four-rotor motor, < >>Is that _i The derivative of q, _i q is the quaternion of the spatial rotation of the ith quad-rotor,>is the sign of the quaternion multiplication, +.>Is that _i The derivative of omega is used to determine, _i ω is the angular velocity of the ith quadrotor, _i J ^-1 is that _i The inversion of the J matrix is performed, _i j is the moment of inertia of the ith quad-rotor, _i τ is the moment generated by the ith four rotors;

0≤ _i T _min ≤ _i T _s ≤ _i T _max

wherein ,_i T _s the four thrust forces generated by the four blades of the ith four rotor wings are respectively, _i T _min is the minimum value of thrust generated by the single blade of the ith four rotors, _i T _max is the maximum value of thrust generated by the single blade of the ith four rotors, _i l is the ith frame of four rotationsThe wheelbase length of the wing, _i c _τ is the torque constant of the rotor of the ith four-rotor motor;

S2, performing fixed time interval dispersion on each item of equation constraint of the ith frame of four-rotor dynamics model in the step S1, performing numerical integration through a Dragon lattice tower algorithm with higher precision, and converting the value integration into discrete dynamic equation constraint of the ith frame of four-rotor dynamics model;

and S3, setting a plurality of waypoints which need to pass through for the four-rotor wing cluster, wherein the waypoints comprise static waypoints and waypoints which move along with time. The four-rotor-wing cluster is provided with a sensor to sense the motion information of surrounding route points to be passed through, the motion law and the motion equation of each route point are obtained through analysis, the coordinates of the route points to be passed through are expressed by quadratic distance cost, the expression form of the quadratic distance cost is converted into a constraint form, and the constraint of the dynamic equation of the ith four-rotor-wing dynamic model in the step S2 is added to construct the constraint of the four-rotor-wing cluster to pass through the route points;

s4, defining the sequence of passing through the waypoints, establishing progress variable constraint passing through each static or moving waypoint in sequence, and establishing inter-aircraft obstacle avoidance constraint of mutual obstacle avoidance among four rotor clusters;

s5, dispersing continuous control variables in a fixed time interval by adopting a multiple targeting method, simultaneously taking state variables of each four rotor wing on each discrete time interval as parameters to be optimized, introducing dynamic equality constraint of a four rotor wing cluster dynamics model, passing way point constraint, passing progress variable constraint and inter-plane obstacle avoidance constraint, and constructing a nonlinear optimization model of four rotor wing cluster high-speed passing way point track planning;

And S6, solving the state of each four rotor wings at each moment by adopting a large-scale nonlinear solver Casadi based on the nonlinear optimization problem model in the step S5 to obtain the track of the four rotor wing clusters passing through the waypoints at high speed, and designing a feedforward feedback controller to track.

According to a second aspect of the present specification, there is provided a four-rotor cluster high speed through dynamic waypoints planning control device, comprising a memory and one or more processors, the memory having executable code stored therein, the processors when executing the executable code being configured to implement the four-rotor cluster high speed through dynamic waypoints planning control method according to the first aspect.

The beneficial effects of the invention are as follows:

1. two progress variables are introduced for measuring the flight track process of the four-rotor cluster, and the progress variables and the approach degree of the passed waypoints are subjected to complementary relaxation constraint, so that the planned cluster track can sequentially pass through each static or moving waypoint;

2. the sensor is built in the four-rotor-wing cluster, surrounding environment and route point motion information are perceived, a Kalman filtering algorithm is adopted to calculate and know a motion equation which needs to be smoother and more accurate through the route points, and the motion equation comprises static and moving route points, so that the application scene and range are wider, and the method is suitable for other more complex scenes;

3. Converting the optimal control problem of the passing track into a nonlinear optimization problem by adopting a multiple targeting method, constructing an optimization objective function of a progress variable matrix containing time information, fixing discrete time intervals to ensure that the time stamps of the four-rotor clusters in flight are consistent, and simultaneously realizing inter-aircraft obstacle avoidance among the four-rotor clusters, thereby ensuring the safety of the self and being better used for passing the waypoints at high speed;

4. aiming at the high-speed passing route point task of the four-rotor cluster, the dynamic model of the four-rotor cluster is fully considered, and the non-linear optimization method is adopted for solving, so that the real limit high-speed passing can be realized, and the efficacy of the four-rotor cluster motor is fully exerted.

Drawings

FIG. 1 is a flow chart of a method for planning and controlling a four-rotor cluster to pass through a dynamic waypoint at a high speed according to an embodiment of the present invention;

FIG. 2 is a graph of a track of a racing quad-rotor cluster passing sequentially through waypoints at high speed in accordance with one embodiment of the present invention;

FIG. 3 is a graph of tracking position time on the X-axis of a quad-rotor cluster (exemplified by first frame uav 1) in Gazebo simulation with a real physical engine according to one embodiment of the present invention;

FIG. 4 is a graph of tracking position time on the Y-axis of a quad-rotor cluster (exemplified by first frame uav 1) in Gazebo simulation with a real physical engine according to one embodiment of the present invention;

FIG. 5 is a graph of tracking the time of the position of a quad-rotor cluster (exemplified by first frame uav 1) on the Z-axis in Gazebo simulation with a real physical engine according to one embodiment of the present invention;

FIG. 6 is a graph of the trace of speed time on the X axis for a quad rotor cluster (first frame uav1, for example) in Gazebo simulation with a real physical engine according to one embodiment of the present invention;

FIG. 7 is a graph of the trace of speed time on the Y-axis for a quad-rotor cluster (first frame uav1, for example) in Gazebo simulation with real physical engine according to one embodiment of the present invention;

FIG. 8 is a graph of tracking speed time on the Z axis of a quad rotor cluster (first frame uav1, for example) in Gazebo simulation with a real physical engine according to one embodiment of the present invention;

FIG. 9 illustrates the position and passed waypoints (the moving passed waypoints are represented by moving boxes) of a quad rotor cluster at different times in Gazebo simulation with a real physics engine according to one embodiment of the invention;

FIG. 10 is a first perspective view of a five-frame quad-rotor cluster passing through a moving waypoint (represented by a moving box) at a point in time in Gazebo simulation with a real physical engine according to one embodiment of the present invention;

figure 11 is a block diagram of a control system for each quad-rotor in a quad-rotor cluster in accordance with one embodiment of the present invention;

Fig. 12 is a block diagram of a four-rotor cluster high-speed through-dynamic waypoint planning control device according to an embodiment of the present invention.

Detailed Description

In the current society, the four-rotor cluster has been widely applied to the fields of earthquake relief, search and rescue, express delivery and the like. Among them, high speed passing waypoints are an important task of quad-rotor clusters in practical applications. The four-rotor cluster can greatly improve the efficiency of completing tasks through the waypoints at high speed, and has great significance in the occurrence of special emergency disasters. Meanwhile, the device has a certain effect on article conveying. The four-rotor cluster is generally smooth due to the smoothness brought by polynomial tracks, so that the motor thrust is also smooth, namely, bang-bang control of rapid acceleration and rapid deceleration cannot be realized, and limited high-speed flight cannot be realized. After the dynamic constraint of the four-rotor cluster in real application is constructed, a progress variable is introduced to measure the process of passing through the waypoints, and nonlinear optimization is adopted to solve the high-speed passing track. Therefore, the method and the device for enabling the four-rotor clusters to pass through static or moving waypoints at high speed are introduced, the optimal control problem is converted into the nonlinear optimization problem, the corresponding optimization objective function is designed through a progress variable matrix containing time information, and the time stamps of the four-rotor clusters are kept consistent by fixing discrete time intervals. Meanwhile, sensors are carried to sense the motion information of surrounding waypoints to be passed through, motion equations of the waypoints are obtained through analysis of Kalman filtering algorithm, the waypoints which are static or moving are passed through at high speed under the condition of taking a dynamic model into consideration, a feedforward-feedback controller based on differential flatness is designed to track, and inter-aircraft obstacle avoidance under the air flow interference among clusters is realized.

The four-rotor motor has the advantages that the effect of the four-rotor motor is fully exerted, and the high-speed passing route point of the four-rotor cluster is realized through the limit high-speed flight of the four-rotor cluster.

As shown in fig. 1, the method for planning and controlling the four-rotor cluster to pass through the dynamic waypoints at high speed provided in the embodiment includes the following steps:

s1, firstly, modeling is carried out based on a dynamic model of a four-rotor cluster in practical application according to parameter configuration of the four-rotor cluster required by experiments and considering motor limitation and the like in practical application scenes, and a calculation formula of the dynamic model of the four-rotor cluster is as follows:

wherein ,f_dyn ( _i x, _i u) is the kinetic equation for the ith quad-rotor in the quad-rotor cluster, _i v is the speed of the ith quadrotor, g is the gravitational acceleration at the current position, _i m is the mass of the ith frame of four rotors, R # _i q) _i Is a rotation matrix of the ith four-rotor wing converted from a world coordinate system to a machine body coordinate system, _i t is the thrust generated by the ith quad-rotor motor, _i q is the quaternion of the spatial rotation of the ith quad-rotor,is the sign of the quaternion multiplication, _i ω is the angular velocity of the ith quadrotor, _i J ^-1 is that _i The inversion of the J matrix is performed, _i j is the moment of inertia of the ith quad-rotor, _i τ is the moment generated by the ith four rotors;

0≤ _i T _min ≤ _i T _s ≤ _i T _max

wherein ,_i T _s the four thrust forces generated by the four blades of the ith four rotor wings are respectively, _i T _min is the minimum value of thrust generated by the single blade of the ith four rotors, _i T _max is the maximum value of thrust generated by the single blade of the ith four rotors, _i l is the wheelbase length of the ith quad rotor, _i c _τ is the torque constant of the rotor of the ith four-rotor motor;

in an embodiment, in step S1, a quaternion is used to represent the attitude rotation of the four rotors, and a specific calculation formula is as follows:

wherein ,q_a Is one ofQuaternion of each gesture, q _b Is a quaternion representing another gesture,is an operator representation of quaternion multiplication, ω _a 、ω _b The first parameter, x, of two quaternions respectively _a 、x _b The second of the two quaternions, y _a 、y _b The third parameter, z, is the two quaternions respectively _a 、z _b The fourth of the two quaternions, respectively.

S2, considering a computer simulation and real machine experiment, ensuring the high efficiency and feasibility of calculation, and simultaneously considering the influence of control precision, dispersing each item of equation constraint of the ith frame of four-rotor dynamics model in the step S1 for 0.03S at fixed time intervals, and then converting the discrete dynamic equation constraint of the ith frame of four-rotor dynamics model into a discrete dynamic equation constraint of the ith frame of four-rotor dynamics model through a Dragon lattice tower numerical integration algorithm;

In one embodiment, the step S2 specifically includes the following steps:

each dynamic constraint of the dynamic model of the four-rotor cluster in the step S1 is firstly discretized at a fixed time interval, then numerical integration is carried out by adopting a fourth-order Dragon-Grating algorithm, and the discrete dynamic constraint of the dynamic model of the four-rotor cluster is obtained by solving; the calculation formula of the fourth-order Dragon lattice tower algorithm is as follows:

k ₄ ＝h·f( _i x _n +h, _i u _n +k ₃ )

wherein ,_i x _n+1 is the state of the ith four rotors at the time of n+1, x _n Is the state of the ith four rotors at time n, h is a discrete time stepLength, k ₁ and h·f(_i x _n,i u _n ) All are function values of a fourth-order Dragon lattice tower algorithm under the moment n, _i u _n is the system control quantity k of the ith four-rotor dynamics model at the n moment ₂ Andare all atFunction value, k of fourth-order Dragon lattice tower algorithm at moment ₃ and />Are also all->Function value, k of fourth-order Dragon lattice tower algorithm at moment ₄ and h·f(_i x _n +h, _i u _n +k ₃ ) All are function values of a fourth-order Dragon lattice tower algorithm at the time of n+h;

and the discrete dynamic equation constraint calculation formula of the ith four-rotor dynamics model in the four-rotor cluster is as follows:

_i x _k+1 - _i x _k -f _Num ( _i x _k,i u _k )·dt＝0

wherein ,_i x _k+1 is the state of the ith four rotor wing at the time of k+1, _i x _k and _i x _dyn,k is the state of the ith four rotor wings at the moment k, f _Num ( _i x _k,i u _k ) Is a function of the fourth-order longgnus algorithm, _i u _k is the system control quantity of the dynamic model of the ith four rotor wing at the moment k, dt is a fixed discrete time interval, _i p _k is the ith four-rotor wing at the moment kIs provided in the position of (a), _i v _k is the speed of the ith quadrotor at time k, _i q _k is the quaternion of the spatial rotation of the ith quadrotor at the moment k, _i ω _k is the angular velocity of the ith quadrotor at time k.

S3, firstly setting a plurality of waypoints which need to pass through for the four-rotor wing cluster, wherein the waypoints comprise static waypoints and waypoints which move along with time variation. The four-rotor-wing cluster is provided with a sensor to sense the motion information of surrounding route points to be passed through, the motion law and the motion equation of each route point are obtained through analysis, the coordinates of the route points to be passed through are expressed by quadratic distance cost, the expression form of the quadratic distance cost is converted into a constraint form, and the constraint of the dynamic equation of the ith four-rotor-wing dynamic model in the step S2 is added to construct the constraint of the four-rotor-wing cluster to pass through the route points;

in one embodiment, the step S3 specifically includes the following steps:

s31, sensing surrounding environment and waypoints by the four-rotor clusters through the sensors, collecting motion information of the waypoints to be passed, preprocessing and filtering raw data collected by the sensors, and obtaining a motion equation of the waypoints by adopting a Kalman filtering algorithm, wherein the motion equation has a specific calculation formula as follows:

wherein ,is the prior state estimation value of the passing waypoint at the moment k, A is the state transition matrix of the passing waypoint, and x _o,k-1 Is the posterior state estimation value of the passing waypoint at time k-1, B is the matrix which is converted into the state by the input of the passing waypoint, u _o,k-1 Is the control input passed through the waypoint at time k-1,/>Is the prior estimated covariance of the passing waypoint at time k, P _o,k-1 Is the posterior estimated covariance of the passed waypoint at time k-1, A ^T Is the transpose of the A matrix, Q is the process excitation noise covariance, often used to represent the error between the state transition matrix and the actual process, K _k Is a filter gain matrix, H ^T Is the transpose of the H matrix, H is the transition matrix from the state variable passed through the waypoint to the observation, R is the measurement noise covariance passed through the waypoint, x _o,k Is the posterior state estimation value of the passing route point at the moment k, z _k Is the observed value input by the navigation point at the moment k, I is the identity matrix and P _o,k Is the posterior estimated covariance of the passing waypoint at time k;

the waypoint motion equation obtained by the Kalman filtering algorithm is as follows:

p _wj ＝f _j (t)

wherein ,p_wj Is the three-dimensional coordinates, i.e. the position, f, of the jth passed waypoint _j (t) is the equation of motion for the j-th passed waypoint if the j-th passed waypoint is stationary, f _j (t) is a constant three-dimensional coordinate of irrelevant time, f if the jth is moved through the waypoint _j (t) is an equation of motion whose three-dimensional coordinates change over time;

assuming that given M waypoints needing to be passed through, including stationary and moving waypoints, constructing a passing waypoint constraint, and representing three-dimensional coordinates of all the waypoints needing to be passed through and the positions of the four-rotor clusters with quadratic distance cost, the formula is as follows:

wherein ,_i L _d,j is the distance between the current position of the ith four rotors and the jth passed waypoint, _i p _k is the position of the ith four rotor wing at the moment k, p _wj,k Is the position of the j-th passed waypoint at the time of k, f _j (k) Is the j-th position of the passed waypoint at the k time;

s32, converting the expression form of the quadratic distance cost into a constraint form, wherein the expression form is specifically as follows:

( _i p _k -p _wj,k ) ^T ( _i p _k -p _wj,k )≤τ _j ²

wherein ,τ_j Is the j-th tolerance interval passed through the waypoint;

thus, a four-rotor cluster pass-through waypoint constraint is constructed.

in one embodiment, the step S4 specifically includes the following steps:

S41, introducing a progress variable mu and a progress variable lambda to measure the flight process that each quadrotor in the quadrotor cluster can sequentially pass through each static or moving route point;

s42, first aiming at the ith frame in the four-rotor clusterFour rotors, all of which are introduced _i Mu measures whether the ith quad-rotor track can pass through waypoints, each of which has a vector _i μ _k Is used for describing the process change of the ith four rotor wing passing through the waypoint at the time k, and all the progress variables are not less than zero, thus, the following is caused _i μ _k ^j ≥0， _i μ _k For use as a sign, thus _i μ _k ^j Only when the distance between the position of the ith four rotor wings at the kth moment and the passed waypoint is smaller than the tolerance interval, the method is formulated _i μ _k The rules of the change can be expressed as:

wherein ,_i μ _k ^j is the progress variable mu of the jth passing route point of the ith four-rotor wing at the k moment, mu is the progress variable for measuring whether the track of the ith four-rotor wing can pass the route point, _i p _k is the position of the ith four rotor wing at the moment k, p _wj,k Is the position of the j-th passed waypoint at time k, || _i p _k -p _wj,k || ₂ ² Is the distance between the position of the ith four rotor wing and the jth passed route point at the moment k, tau _j Is the tolerance interval of the jth passed route point, k is a discrete time node, N is a discrete last time node, M is the total number of the set passed route points, and Q is the number of frames of the four-rotor wing cluster;

Since the trajectory that defines the quadrotor cluster must satisfy the discrete dynamic equation constraints of the quadrotor dynamics model, it is not possible to pass exactly through the waypoints, so to make the trajectory as smooth and safe as possible, a relaxation variable δ is introduced to improve feasibility, giving a moderate tolerance distance δ for each distance between the ith quadrotor position and the waypoint being passed _k ^j To constrain, the tolerance can be expressed as:

wherein ,_i p _k is the position of the ith four rotor wings at the moment k, p _wj,k Is the position of the j-th passed waypoint at time k, || _i p _k -p _wj,k || ₂ ² Is the distance between the position of the ith quad-rotor and the jth passed waypoint at time k,is the tolerance distance, delta, of the jth passed waypoint at the k moment _tol The maximum tolerance distance of the passing waypoints is M, the total number of the set passing waypoints is M, and k is a discrete time node;

in trajectory planning of quad-rotor clusters, it is necessary to ensure that each quad-rotor can pass through stationary or moving waypoints within a tolerance interval, and _i μ _k as a marked progress variable, the complementary relaxation constraint of designing the ith quad-rotor through the waypoint can be expressed as:

wherein ,_i μ _k ^j Is the progress variable mu of the jth waypoint of the ith four-rotor wing at the moment k, mu is the progress variable for measuring whether the track of the ith four-rotor wing can pass through the set waypoint, _i p _k is the position of the ith four rotor wings at the moment k, p _wj,k Is the position of the j-th passed waypoint at time k, || _i p _k -p _wj,k || ₂ ² Is the distance between the position of the ith quad-rotor and the jth passed waypoint at time k,the tolerance distance of the jth waypoint at the moment k, wherein k is a discrete time node;

because the track not only needs to pass through each waypoint, but also needs to pass through each waypoint in sequence according to a certain order, another progress variable lambda is introduced for measuring whether the four-rotor track in the cluster can pass through each waypoint in sequence, wherein the vector is used for measuring the speed of the four-rotor track in the clusterDescribing whether the ith four rotor wing sequentially passes through each route point according to a certain sequence under the k moment, due to the fact that _i λ _k Also used as a sign only when _i λ _k ^j When passing through a certain waypoint according to a certain sequence, the navigation points are changed, _i λ _k the rules of the change can be expressed as:

_i λ _k+1 ＝ _i λ _k-i μ _k

wherein ,_i λ _k+1 is the progress variable lambda of the ith quadrotor at the time of k+1, _i λ _k is a progress variable for measuring whether the ith four-rotor track can sequentially pass through each route point according to a certain sequence at the moment k, _i μ _k The method is used for measuring the progress variable of whether the ith four-rotor wing track can pass through the waypoints at the moment k, so that the progress variable constraint of sequentially passing through each static or moving waypoint is established;

s43, in the process of passing through the route points at high speed of the four-rotor clusters, the most important is to ensure the safety of the self-flight of the clusters, so that the inter-aircraft obstacle avoidance of the four-rotor clusters is needed to be considered, and meanwhile, the influence of air flow in the four-rotor flight process is considered, and a corresponding inter-aircraft obstacle avoidance constraint equation is designed as follows:

wherein E is a diagonal matrix for reducing the risk of wash down affected by four rotor airflows, _i p _k is the position of the ith four rotors at the moment k, _r p _k is the position of the r-th four-rotor wing at the moment k, Q is the number of four-rotor wing clusters, delta _col Is the inter-machine safety distance of the four-rotor clusters.

In one embodiment, the step S5 specifically includes the following steps:

solving an optimal control problem by using a nonlinear optimization method, and constructing an optimization objective function in a nonlinear optimization model of the whole four-rotor cluster high-speed passing waypoint problem, wherein the optimization objective function is specifically expressed as follows:

wherein X is an optimization variable in a nonlinear optimization model, J is an optimization index function, i is the ith frame of four rotors, Q is the number of frames of a four-rotor cluster, J is the jth waypoint, M is the number of the passed waypoints, k is the kth moment, N is the last moment point after time dispersion, _i lambda is the progress variable matrix lambda of the ith four rotors;

the optimization variable X is used for optimizing the whole nonlinear model, so that the four-rotor cluster passes through the waypoint at high speed, and the four-rotor cluster consists of states of each four-rotor at each moment, two progress variable constraints and a tolerance interval passing through the waypoint constraint, and is specifically expressed as follows:

X＝[ ₀ X,…, _i X,, _Q-1 X]

_i X＝[ _i X _0,i X ₁ ,…,iX _N-1 ]

_i X _k ＝[ _i p _k _i v _k _i q _k _i ω _k _i u _k _i λ _k _i μ _k _i δ _k ]

wherein ,_i x is the optimization variable of the ith four rotors in the nonlinear optimization model, _i X _k is an optimization variable of a nonlinear optimization model of the ith four rotor wing at the moment k, _i p _k is the position of the ith four rotor wing at the moment k, _i v _k is the speed of the ith quadrotor at time k, _i q _k Is the quaternion of the spatial rotation of the ith quadrotor at the moment k, _i ω _k is the angular velocity of the ith quadrotor at time k, _i u _k is the system control quantity of the four-rotor dynamics model of the ith four-rotor at the k moment, _i λ _k is a progress variable for measuring whether the track can sequentially pass through each route point according to a certain sequence under the moment k of the ith four rotor wing, _i μ _k is a progress variable of the ith four-rotor wing for measuring whether the track can pass through the waypoint at the moment k, _i δ _k the tolerance distance of each passed route point of the ith four rotor wing at the moment k;

the nonlinear optimization model needs to meet dynamics constraint and upper and lower input and output boundary constraint, and is specifically expressed as follows:

_i x _k+1 - _i x _k -f _Num ( _i x _k,i u _k )·dt＝0

_i u _min - _i u _k ≤0， _i u _k - _i u _max ≤0

wherein ,_i x _k+1 is the state of the ith four rotor wing at the time of k+1, f _Num ( _i x _k,i u _k ) Is a fourth-order Dragon lattice libraryA function of the tower algorithm, _i u _k is the system control quantity of the dynamic model of the ith four rotor wing at the moment k, _i u _min is the minimum value of the system control quantity of the ith four-rotor dynamics model, _i u _max is the maximum value of the system control quantity of the ith four-rotor dynamics model;

meanwhile, the constraint of passing progress variables and obstacle avoidance among four-rotor clustered machines needs to be met, and the method is specifically expressed as follows:

wherein ,is the progress variable lambda and the +.f of the jth waypoint of the ith four rotor wing at the k moment >Is the progress variable lambda of the j+1th passing waypoint of the ith quadrotor at the k moment, lambda is the progress variable used for measuring whether the quadrotor track can pass through each waypoint in turn, _i λ _k is a progress variable of the ith four rotor wing for measuring whether the track can sequentially pass through each route point at the moment k, _i λ _k+1 is the progress variable lambda of the ith quadrotor at the time of k+1, _i μ _k is the progress variable mu of the ith four-rotor wing for measuring whether the track can pass through the waypoint at the moment k, mu is the progress variable for measuring whether the four-rotor wing track can pass through the waypoint, _i μ _k ^j is the progress variable mu of the jth passing route point of the ith four rotor wing at the k moment, I _i p _k -p _wj || ₂ ² Is the distance between the position of the ith four rotor wing at the moment k and the jth passing route point,/for the moment k>Is the tolerance distance delta of the jth passing route point of the ith four rotor wings at the k moment _col Is the minimum safety distance between four-rotor clusters to avoid obstacles, N is the last discrete time node, M is the total number of set passing waypoints, and Q is the number of frames of the four-rotor clusters.

In an embodiment, an open source software tool CasADi is used for numerical optimization, in particular optimization control (i.e. optimization involving differential equations). Therefore, the established nonlinear optimization model of the time optimal trajectory planning is solved by the CasADI solver, and the Python application programming interface is adopted for programming the optimization program.

The four-rotor state variable of the optimal track obtained through planning is used as a feedforward control reference quantity, and then the position and speed feedback control of the differential flat property in the four rotors is added, so that the design of the control rate is realized, and the method is specifically as follows:

a _fb ＝k _p (p-p _ref )+k _v (v-v _ref )

a _des ＝a _fb +a _ff -a _rd -g

wherein ,a_fb Is the acceleration obtained by feedback control, a _ff Is the feedforward acceleration obtained by the optimal track planning, a _rd The resistance acceleration caused by pneumatic resistance, g is the gravity acceleration, a _des Is the desired acceleration, k _p Is the p-parameter, k of the designed controller _v Is the v parameter of the designed controller, p is the position of the quadrotor, p _ref Is the feedforward position reference obtained by the optimal track planning, v is the speed of four rotors, v _ref Is a feed-forward speed reference obtained by optimal track planning.

A method and a device for enabling a four-rotor-wing cluster to pass through a static or moving route point at high speed convert an optimal control problem into a nonlinear optimization problem, and a corresponding optimization objective function is designed through a progress variable matrix containing time information, so that the efficacy of a four-rotor-wing motor is fully exerted, and the ultimate high-speed flight of rapid acceleration and rapid deceleration is realized. Thereby realizing the time optimization of the four-rotor cluster track. The key idea of the invention is that the progress variable matrix containing time information is skillfully converted, a corresponding optimized objective function is obtained through calculation, the time stamp of each frame of the four-rotor cluster can be kept consistent by fixing discrete time intervals, the safety of the cluster flight is ensured, the inter-aircraft obstacle avoidance at the same moment is realized, and a Kalman filtering algorithm is added to obtain a motion equation of a waypoint, so that the high-speed passing is realized. According to the invention, the optimal control problem through planning is converted into a nonlinear optimization problem, each dynamic constraint and progress variable constraint of the four rotors are used as constraint conditions of the whole nonlinear optimization model, and meanwhile, sensors are carried to sense the motion information of surrounding waypoints to be passed through, and the motion equation of each waypoint is obtained through analysis. The improvement fully realizes the inter-aircraft obstacle avoidance among the four-rotor clusters under the condition of ensuring the extreme high-speed flight of the four-rotor clusters, and obtains the motion equation of the passed waypoints through a Kalman filtering algorithm, thereby realizing the high-speed passing waypoints of the four-rotor clusters.

In this example, a self-made racing quadrotor model in the laboratory was used, and the specific basic parameter configuration of the dynamics model is shown in table 1. The table lists the parameter configurations of mass, wheelbase, thrust range, maximum rotation angle, moment constant and moment of inertia, respectively.

Table 1 four rotor parameter configuration

The specific implementation details are as follows:

in order to better perform the real-time test, the dynamic model of the four-rotor cluster is fully considered, and whether the four-rotor cluster can fly safely at a high speed is tested, so that a simulation experiment of five four rotors is performed in a Gazebo platform. Firstly, a series of six three-dimensional coordinate points which need to be passed through the waypoints are set, five static points which need to be passed through the waypoints and one movement need to be setThe motion information of the motion waypoints is observed through the waypoints through a Kalman filtering algorithm, and the motion equation is obtained according to the state value: wherein p_w2,k Is the second position at time k that needs to be passed through the waypoint. The stationary demand is passed through the waypoint motion equation as a constant three-dimensional coordinate, so all demands are passed through the waypoint motion equation as follows:

setting a tolerance interval d passing through the waypoint _tol =0.3m, solving a nonlinear optimization model of the four-rotor clusters passing through the waypoints at high speed by adopting a CasADi solver, solving the state information of each four-rotor at each moment, gradually smoothing the state information of the five four-rotors, meeting dynamics constraint, and finally achieving optimal convergence.

The high-speed passing waypoint track planned by the five four rotors is shown in fig. 2, the crosses represent passing waypoints, the five track curves represent the passing tracks of different four rotors planned, and as can be seen from fig. 2, the five tracks planned by the invention can sequentially pass through the waypoints in a tolerance interval, comprise a second motion which changes along with the time interval k and passes through the waypoints, and can realize inter-aircraft obstacle avoidance among the four rotor clusters, and the tracks are smooth and feasible, so that the dynamic constraint of the four rotor models is satisfied.

Based on a feedforward planning controller, a differential flatness property is utilized, and after a position and speed feedback controller is added, a tracking curve of position time and speed time of a four-rotor cluster in Gazebo simulation with a real physical engine is drawn. Taking the first four-rotor in the four-rotor cluster as an example, the position state information (positions on x, y and z axes) of the planned time optimal track is shown in fig. 3, 4 and 5 respectively, the abscissa in fig. 3 is the flight time, the ordinate is the position on the x axis, the solid line type is the x-axis position planning value of the first four-rotor obtained by feedforward, and the dotted line type is the actual tracking value of the x-axis position after feedback is added. The abscissa in fig. 4 is the time of flight, the ordinate is the position on the y-axis, the solid line type is the y-axis position planning value of the first four rotors obtained by feedforward, and the dotted line type is the actual tracking value of the y-axis position after feedback is added. The abscissa in fig. 5 is the time of flight, the ordinate is the position on the z-axis, the solid line type is the z-axis position planning value of the first four rotors obtained by feedforward, and the broken line type is the actual tracking value of the z-axis position after feedback is added. Taking the first four-rotor in the four-rotor cluster as an example, the speed state information (speeds on x, y and z axes) of the planned time optimal track is shown in fig. 6, 7 and 8 respectively, the abscissa in fig. 6 is the flight time, the ordinate is the speed on the x axis, the solid line type is the x axis speed planning value of the first four-rotor obtained by feedforward, and the dotted line type is the x axis speed actual tracking value after feedback is added. The abscissa in fig. 7 is the time of flight, the ordinate is the speed on the y-axis, the solid line type is the y-axis speed plan value of the first four rotors obtained by feedforward, and the broken line type is the y-axis speed actual tracking value after feedback is added. The abscissa in fig. 8 is the time of flight, the ordinate is the speed on the z-axis, the solid line type is the z-axis speed plan value of the first four rotors obtained by feedforward, and the broken line type is the z-axis speed actual tracking value after feedback is added.

The positions and passed waypoints (the moving passing waypoints are represented by moving boxes) of the quad-rotor clusters at different times in the Gazebo simulation are shown in fig. 9, and it can be seen that the quad-rotor clusters can pass through each waypoint in turn and realize inter-aircraft obstacle avoidance. A first perspective view of a five-frame quad-rotor cluster through a moving waypoint (represented by a moving box) in Gazebo simulation is shown in fig. 10. The control system block diagram of each quadrotor in the quadrotor cluster is shown in fig. 11, and the final system control quantity is obtained by combining a feedforward control item and a position speed feedback control item.

The position and speed tracking control diagram and Gazebo simulation diagram can show that the invention fully plays the role of the four-rotor motor, realizes the ultimate high-speed flight, ensures the safety of the cluster flight by reaching 15m/s at maximum speed, realizes the inter-aircraft obstacle avoidance of the four-rotor cluster, obtains the motion equation of the passed waypoint by adopting a Kalman filtering algorithm, and finally realizes the high-speed passing waypoint of the four-rotor cluster. Meanwhile, the controller has good tracking control precision, can track a high-speed aggressive track more accurately, and is verified on a Gazebo simulation platform, so that the method can be applied to a real object four-rotor cluster.

From the above experimental results, it can be found that:

1. according to the invention, the sensor senses the motion information of the environment and the waypoints, a Kalman filtering algorithm is adopted to obtain a motion equation of the waypoints to be passed, the problem that the whole four-rotor cluster passes through the waypoints at high speed is modeled into a nonlinear optimization model, and quadratic constraint of the passing waypoints is established, so that the process of passing the waypoints at high speed in turn of the four-rotor cluster is realized.

2. According to the method, a new objective function is designed by utilizing time information in the progress variable matrix, so that the solving of the nonlinear optimization model is more efficient, and the time stamp of the four-rotor cluster flight can be kept consistent, thereby adding inter-machine obstacle avoidance constraint of the cluster, and ensuring the flight safety of the four-rotor cluster.

3. According to the invention, by solving the nonlinear optimization model and adopting the solver Casida to solve, the method can be well expanded to a larger-scale four-rotor cluster, and meanwhile, discrete dynamics constraint can be conveniently modified, so that the method can be applied to four rotors and other dynamic models of vehicles and the like, and has strong expansibility and applicability. Meanwhile, the invention supports other constraint conditions added into the nonlinear optimization model, and different constraint conditions can be formulated according to application scenes and problem descriptions.

4. The four-rotor cluster dynamic constraint of real flight can be fully considered, the efficacy of the four-rotor motor can be fully exerted, and the control effect of rapid acceleration and rapid deceleration is realized, so that the limit high-speed flight of the four-rotor cluster is realized, and a foundation is laid for high-speed passing through a waypoint.

5. The feedforward control item is obtained through reference track calculation, the position and speed error is used as the feedback control item, and the feedforward-feedback controller with high-precision track tracking is designed by combining the feedforward control item and the feedback control item, so that the upper excitation track can be tracked well.

Corresponding to the embodiment of the method for planning and controlling the four-rotor-wing cluster to pass through the dynamic waypoint at high speed, the invention also provides an embodiment of a planning and controlling device for the four-rotor-wing cluster to pass through the dynamic waypoint at high speed.

Referring to fig. 12, the device for planning and controlling the high-speed passing of the four-rotor cluster through the dynamic waypoint provided by the embodiment of the invention comprises a memory and one or more processors, wherein executable codes are stored in the memory, and the processor is used for realizing the method for planning and controlling the high-speed passing of the four-rotor cluster through the dynamic waypoint in the embodiment when executing the executable codes.

The embodiment of the four-rotor cluster high-speed planning control device passing through the dynamic waypoints can be applied to any device with data processing capability, and the device with the data processing capability can be a device or a device such as a computer. The apparatus embodiments may be implemented by software, or may be implemented by hardware or a combination of hardware and software. Taking software implementation as an example, the device in a logic sense is formed by reading corresponding computer program instructions in a nonvolatile memory into a memory by a processor of any device with data processing capability. In terms of hardware, as shown in fig. 12, a hardware structure diagram of an apparatus with data processing capability where the four-rotor cluster high-speed dynamic waypoint planning control device of the present invention is located is shown, and in addition to the processor, the memory, the network interface, and the nonvolatile memory shown in fig. 12, any apparatus with data processing capability in the embodiment is generally according to the actual function of the apparatus with data processing capability, and may further include other hardware, which is not described herein.

The implementation process of the functions and roles of each unit in the above device is specifically shown in the implementation process of the corresponding steps in the above method, and will not be described herein again.

For the device embodiments, reference is made to the description of the method embodiments for the relevant points, since they essentially correspond to the method embodiments. The apparatus embodiments described above are merely illustrative, wherein elements illustrated as separate elements may or may not be physically separate, and elements shown as elements may or may not be physical elements, may be located in one place, or may be distributed over a plurality of network elements. Some or all of the modules may be selected according to actual needs to achieve the purposes of the present invention. Those of ordinary skill in the art will understand and implement the present invention without undue burden.

The embodiment of the invention also provides a computer readable storage medium, and a program is stored on the computer readable storage medium, and when the program is executed by a processor, the method for planning and controlling the four-rotor clusters to pass through dynamic waypoints at high speed in the embodiment is realized.

The computer readable storage medium may be an internal storage unit, such as a hard disk or memory, of any of the data processing enabled devices of any of the previous embodiments. The computer readable storage medium may be any external storage device of a device having data processing capability, such as a plug-in hard disk, smart Media Card (SMC), SD Card, flash memory Card (Flash Card), or the like, which are provided on the device. Further, the computer readable storage medium may include both internal storage units and external storage devices of any data processing device. The computer readable storage medium is used for storing a computer program and other programs and data required by any device having data processing capabilities, and can also be used for temporarily storing data that has been output or is to be output.

It should also be noted that the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article or apparatus that comprises an element.

The foregoing describes specific embodiments of the present disclosure. Other embodiments are within the scope of the following claims. In some cases, the actions or steps recited in the claims can be performed in a different order than in the embodiments and still achieve desirable results. In addition, the processes depicted in the accompanying figures do not necessarily require the particular order shown, or sequential order, to achieve desirable results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

The foregoing description is of preferred embodiments of the one or more embodiments of the present invention and is not intended to limit the one or more embodiments of the present invention to any modification which may be made within the spirit and principles of the one or more embodiments of the present invention.

Claims

1. The planning control method for the four-rotor clusters to pass through the dynamic waypoints at high speed is characterized by comprising the following steps:

s1, modeling is carried out based on a dynamic model of a four-rotor cluster in practical application, and the dynamic model of the four-rotor cluster has the following calculation formula:

wherein ,f_dyn ( _i x, _i u) is the kinetic equation for the ith quad-rotor in the quad-rotor cluster,is that _i p derivative of _i p is the position of the ith quad-rotor, < >>Is that _i The derivative of v, _i v is the ith frame of four rotationsThe speed of the wing, g is the gravitational acceleration at the current position, _i m is the mass of the ith frame of four rotors, R # _i q) is the rotation matrix of the ith quadrotor converted from the world coordinate system to the body coordinate system, _i t is the thrust generated by the ith four-rotor motor, < >>Is that _i The derivative of q, _i q is the quaternion of the spatial rotation of the ith quad-rotor,is the sign of the quaternion multiplication, +.>Is that _i The derivative of omega is used to determine, _i ω is the angular velocity of the ith quadrotor, _i J ^-1 is that _i The inversion of the J matrix is performed, _i J is the moment of inertia of the ith quad-rotor, _i τ is the moment generated by the ith four rotors;

0≤ _i T _min ≤ _i T _s ≤ _i T _max

wherein ,_i T _s the four thrust forces generated by the four blades of the ith four rotor wings are respectively, _i T ₁ is the thrust generated by the first blade of the ith four-rotor wing, _i T ₂ is the thrust generated by the second blade of the ith four-rotor wing, _i T ₃ is the thrust generated by the third blade of the ith four-rotor wing, _i T ₄ is the thrust generated by the fourth blade of the ith four rotor wings, _i T _min is the minimum value of thrust generated by the single blade of the ith four rotors, _i T _max is the maximum value of thrust generated by the single blade of the ith four rotors, _i l is the wheelbase length of the ith quad rotor, _i c _τ is the torque constant of the rotor of the ith four-rotor motor;

s2, dispersing each dynamic constraint of the dynamic model of the ith four rotors in the step S1 at fixed time intervals, and then carrying out numerical integration processing through a higher-precision Dragon-Greek tower algorithm to obtain the discrete dynamic constraint of the dynamic model of the ith four rotors; the method specifically comprises the following steps:

k ₄ ＝h·f( _i x _n +h, _i u _n +k ₃ )

wherein ,_i x _n+1 is the state of the ith four rotors at the time of n+1, _i x _n is the state of the ith four rotors at time n, h is a discrete time step, k ₁ and h·f(_i x _n,i u _n ) All are function values of a fourth-order Dragon lattice tower algorithm under the moment n, _i u _n is the system control quantity k of the ith four-rotor dynamics model at the n moment ₂ Andare all at->Function of fourth-order Dragon lattice tower algorithm under timeNumerical value k ₃ and />Are also all->Function value, k of fourth-order Dragon lattice tower algorithm at moment ₄ and h·f(_i x _n +h, _i u _n +k ₃ ) All are function values of a fourth-order Dragon lattice tower algorithm at the time of n+h;

_i x _k+1 - _i x _k -f _Num ( _i x _k,i u _k )·dt＝0

wherein ,_i x _k+1 is the state of the ith four rotor wing at the time of k+1, _i x _k and _i x _dyn,k is the state of the ith four rotor wings at the moment k, f _Num ( _i x _k,i u _k ) Is a function of the fourth-order longgnus algorithm, _i u _k is the system control quantity of the dynamic model of the ith four rotor wing at the moment k, dt is a fixed discrete time interval, _i p _k is the position of the ith four rotor wing at the moment k, _i v _k is the speed of the ith quadrotor at time k, _i q _k is the quaternion of the spatial rotation of the ith quadrotor at the moment k, _i ω _k is the angular velocity of the ith four rotor wing at the moment k;

S3, firstly setting a plurality of waypoints which need to pass through for the four-rotor clusters, wherein the plurality of waypoints comprise static waypoints and waypoints which move along with time variation, the four-rotor clusters are provided with sensors to sense the surrounding movement information which needs to pass through the waypoints, the movement rules and the movement equations of the waypoints are obtained through analysis, the coordinates of the waypoints which need to pass through are represented by quadratic distance cost, the representation form of the quadratic distance cost is converted into constraint form, and the dynamic constraint of the dynamic model of the ith four-rotor in the step S2 is added to construct the constraint of the passing waypoints of the four-rotor clusters; the method specifically comprises the following steps:

wherein ,is the prior state estimation value of the passing waypoint at the moment k, A is the state transition matrix of the passing waypoint, and x _o,k-1 Is turned on at time k-1 A posterior state estimate for the waypoint, B is a matrix that will be converted to state by the waypoint input, u _o,k-1 Is the control input passed through the waypoint at time k-1,/>Is the prior estimated covariance of the passing waypoint at time k, P _o,k-1 Is the posterior estimated covariance of the passed waypoint at time k-1, A ^T Is the transpose of the A matrix, Q is the process excitation noise covariance, often used to represent the error between the state transition matrix and the actual process, K _k Is a filter gain matrix, H ^T Is the transpose of the H matrix, H is the transition matrix from the state variable passed through the waypoint to the observation, R is the measurement noise covariance passed through the waypoint, x _o,k Is the posterior state estimation value of the passing route point at the moment k, z _k Is the observed value input by the navigation point at the moment k, I is the identity matrix and P _o,k Is the posterior estimated covariance of the passing waypoint at time k;

p _wj ＝f _j (t)

_i L _d,j ＝( _i p _k -p _wj,k ) ^T ( _i p _k -p _wj,k )，j∈[0,M)

p _wj,k ＝f _j (k)

( _i p _k -p _wj,k ) ^T ( _i p _k -p _wj,k )≤τ _j ²

wherein ,τ_j Is the j-th tolerance interval passed through the waypoint;

thus, a four-rotor cluster passing waypoint constraint is constructed;

s4, defining the sequence of passing through the waypoints, establishing progress variable constraint passing through each static or moving waypoint in sequence, and establishing inter-aircraft obstacle avoidance constraint for mutual obstacle avoidance among four rotor clusters; the method specifically comprises the following steps:

S42, firstly, aiming at the ith four rotors in the four-rotor cluster, all the rotors are introduced _i Mu measures whether the ith quad-rotor track can pass through waypoints, each of which has a vector _i μ _k Is used for describing the process change of the ith four rotor wing passing through the waypoint at the time k, and all the progress variables are not less than zero, thus, the following is caused _i μ _k ^j ≥0， _i μ _k For use as a sign, thus _i μ _k ^j Only when the distance between the position of the ith four rotor wing at the kth moment and the passed route point is smaller than the tolerance intervalWill change and make _i μ _k The rules of the change can be expressed as:

Since the trajectory of the four-rotor cluster must satisfy the discrete dynamic equation constraints of the four-rotor dynamics model, it is not possible to pass completely precisely through the waypoints, so to make the trajectory as smooth and safe as possible, a relaxation variable delta is introduced to improve feasibility, giving a moderate tolerance distance for each distance between the ith four-rotor position and the waypoint being passedTo constrain, the tolerance can be expressed as:

since the track needs to pass not only through each waypoint, but also through each waypoint in a certain order, another progress variable lambda is introduced,for measuring whether four rotor trajectories in a cluster can pass through each waypoint in turn, wherein the vectorDescribing whether the ith four rotor wing sequentially passes through each route point according to a certain sequence under the k moment, due to the fact that _i λ _k Also used as a sign only when _i λ _k ^j When passing through a certain waypoint according to a certain sequence, the navigation points are changed, _i λ _k the rules of the change can be expressed as:

_i λ _k+1 ＝ _i λ _k - _i μ _k

wherein E is a diagonal matrix for reducing the risk of wash down affected by four rotor airflows, _i p _k is the position of the ith four rotors at the moment k, _r p _k is the position of the r-th four-rotor wing at the moment k, Q is the number of four-rotor wing clusters, delta _col Is the inter-machine safety distance of the four-rotor clusters;

s5, dispersing continuous control variables in a fixed time interval by adopting a multiple targeting method, simultaneously taking state variables of each four rotor wing on each discrete time interval as parameters to be optimized, introducing dynamic constraint, passing waypoint constraint, passing progress variable constraint and inter-machine obstacle avoidance constraint of a dynamic model of the four rotor wing cluster, and constructing a nonlinear optimization model of the four rotor wing cluster for planning the high-speed passing waypoint track;

2. The method for planning and controlling the high-speed passing of the four-rotor clusters through the dynamic waypoints according to claim 1, wherein in the step S1, quaternion is adopted to represent the attitude rotation of the four-rotor, and a specific calculation formula is as follows:

wherein ,q_a Is a quaternion representing a gesture, q _b Is a quaternion representing another gesture,is an operator representation of quaternion multiplication, ω _a 、ω _b The first parameter, x, of two quaternions respectively _a 、x _b The second of the two quaternions, y _a 、y _b The third parameter, z, is the two quaternions respectively _a 、z _b The fourth of the two quaternions, respectively.

3. The method for planning and controlling the high-speed passing of the four-rotor cluster through the dynamic waypoint according to claim 1, wherein the step S5 specifically comprises the following steps:

X＝[ ₀ X,…, _i X,…, _Q-1 X]

_i X＝[ _i X _0,i X ₁ ,…, _i X _N-1 ]

_i X _k ＝[ _i p _ki v _ki q _ki ω _ki u _ki λ _ki μ _ki δ _k ]

wherein ,_i x is the optimization variable of the ith four rotors in the nonlinear optimization model, _i X _k is an optimization variable of a nonlinear optimization model of the ith four rotor wing at the moment k, _i p _k is the position of the ith four rotor wing at the moment k, _i v _k is the speed of the ith quadrotor at time k, _i q _k is the quaternion of the spatial rotation of the ith quadrotor at the moment k, _i ω _k is the angular velocity of the ith quadrotor at time k, _i u _k is the ith frame of four-screwThe system control quantity of the four-rotor dynamics model of the wing at the moment k, _i λ _k is a progress variable for measuring whether the track can sequentially pass through each route point according to a certain sequence under the moment k of the ith four rotor wing, _i μ _k is a progress variable of the ith four-rotor wing for measuring whether the track can pass through the waypoint at the moment k, _i δ _k the tolerance distance of each passed route point of the ith four rotor wing at the moment k;

_i x _k+1 - _i x _k -f _Num ( _i x _k,i u _k )·dt＝0

_i u _min - _i u _k ≤0， _i u _k - _i u _max ≤0

wherein ,_i x _k+1 is the state of the ith four rotor wing at the time of k+1, f _Num ( _i x _k,i u _k ) Is a function of the fourth-order longgnus algorithm, _i u _k is the system control quantity of the dynamic model of the ith four rotor wing at the moment k, _i u _min is the minimum value of the system control quantity of the ith four-rotor dynamics model, _i u _max is the maximum value of the system control quantity of the ith four-rotor dynamics model;

wherein ,is the progress variable lambda and the +.f of the jth waypoint of the ith four rotor wing at the k moment>Is the progress variable lambda of the j+1th passing waypoint of the ith quadrotor at the k moment, lambda is the progress variable used for measuring whether the quadrotor track can pass through each waypoint in turn, _i λ _k is a progress variable of the ith four rotor wing for measuring whether the track can sequentially pass through each route point at the moment k, _i λ _k+1 is the progress variable lambda of the ith quadrotor at the time of k+1, _i μ _k is the progress variable mu of the ith four-rotor wing for measuring whether the track can pass through the waypoint at the moment k, mu is the progress variable for measuring whether the four-rotor wing track can pass through the waypoint, _i μ _k ^j is the progress variable mu of the jth passing route point of the ith four rotor wing at the k moment, I _i p _k -p _wj || ₂ ² Is the distance between the position of the ith four rotor wing at the moment k and the jth passing route point,/for the moment k>Is the tolerance distance delta of the jth passing route point of the ith four rotor wings at the k moment _col Is the minimum safety distance between four-rotor clusters to avoid obstacles, N is the last discrete time node, M is the total number of set passing waypoints, and Q is the number of frames of the four-rotor clusters.

4. A four-rotor cluster high-speed dynamic waypoint planning control device, comprising a memory and one or more processors, wherein executable codes are stored in the memory, and the processor is used for realizing the four-rotor cluster high-speed dynamic waypoint planning control method according to any one of claims 1-3 when executing the executable codes.