CN114721412A - Unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control - Google Patents

Unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control Download PDF

Info

Publication number
CN114721412A
CN114721412A CN202210263895.2A CN202210263895A CN114721412A CN 114721412 A CN114721412 A CN 114721412A CN 202210263895 A CN202210263895 A CN 202210263895A CN 114721412 A CN114721412 A CN 114721412A
Authority
CN
China
Prior art keywords
aerial vehicle
unmanned aerial
obstacle
obstacle avoidance
constraint
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.)
Granted
Application number
CN202210263895.2A
Other languages
Chinese (zh)
Other versions
CN114721412B (en
Inventor
戴荔
霍达
薛若宸
王沛展
周小婷
夏元清
孙中奇
张金会
翟弟华
崔冰
高寒
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202210263895.2A priority Critical patent/CN114721412B/en
Publication of CN114721412A publication Critical patent/CN114721412A/en
Application granted granted Critical
Publication of CN114721412B publication Critical patent/CN114721412B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The invention discloses an unmanned aerial vehicle track tracking obstacle avoidance method based on model predictive control. Through the transformation solution of the obstacle avoidance constraint, the effect of the obstacle avoidance constraint can be visually embodied, and the obstacle avoidance success rate of the unmanned aerial vehicle in the track tracking process is improved. The inner ring, namely the attitude control, adopts a first-order controller, and the integrity of the unmanned aerial vehicle obstacle avoidance track tracking control is ensured. And meanwhile, considering the state constraint, the control constraint and the reference track of the system, controlling the outer ring through model prediction control, designing reasonable terminal cost, terminal controller and terminal constraint conditions, constructing an optimization model, and proving the feasibility of the algorithm.

Description

Unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control
Technical Field
The invention relates to the technical field of unmanned aerial vehicle control, in particular to an unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control.
Background
Because the unmanned aerial vehicle can complete some highly autonomous tasks by applying a control theory and a control method under the condition of no manual participation, particularly under the constraint of challenging environment and complex structure, the unmanned aerial vehicle can be widely applied to the fields of military affairs, spaceflight, industry, entertainment and the like. The trajectory tracking is the basis of the control of the unmanned aerial vehicle in the task execution, and efficient trajectory tracking control methods are various, such as PID control, sliding mode control, self-adaptive control and the like. However, in the trajectory tracking control process of the unmanned aerial vehicle, besides the tracking performance, constraint conditions that the unmanned aerial vehicle needs to meet, such as constraints in terms of torque and speed, need to be considered, and the control method cannot directly handle the constraints, and can only meet the constraint conditions by adjusting parameters.
In addition, unmanned aerial vehicle need guarantee the security of self when carrying out the task, in the coexistent environment of static and dynamic barrier, avoids colliding with the barrier. There are many existing track planning and obstacle avoidance algorithms, such as discrete point method, a-star algorithm, RRT algorithm, etc., but these methods are large in calculation amount or cannot obtain the optimal track.
Disclosure of Invention
In view of the above, the invention provides an unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control, which can solve the problem of trajectory tracking control of an unmanned aerial vehicle in an environment with static and dynamic obstacles.
The invention adopts the following specific technical scheme:
an unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control is characterized in that an MINGO-based calculation is adopted to obtain a minimum external polyhedron surrounding a dynamic obstacle predictive motion range, and an obstacle set is constructed according to the external polyhedron; constructing obstacle avoidance constraints according to the obstacle set, constructing an optimization model of unmanned aerial vehicle trajectory tracking obstacle avoidance based on the obstacle avoidance constraints, and solving according to the optimization model to obtain an optimal control sequence;
obtaining a separation plane separating the predicted track of the unmanned aerial vehicle from the obstacle set according to the optimal control sequence, and finishing the position control of the unmanned aerial vehicle;
performing decoupling operation on the optimal control sequence, and performing attitude control through a first-order controller; therefore, the unmanned aerial vehicle motion control in the unmanned aerial vehicle trajectory tracking process is completed.
Further, the outer polyhedron is:
Vj(τi|t)＝Qj(τi|t)A-1(τi|t)
wherein ,Vj(τiT) is the set of vertices of the outer polyhedron, Qj(τi| t) is a polynomial curve coefficient matrix of the predicted trajectory of the obstacle, A-1(τiT) is a time correlation matrix; i represents the time interval number of the unmanned aerial vehicle system, j represents the number of obstacles, and i and j are positive integers; (τ)i| t) represents the forward prediction τ at time tiAnd (5) carrying out the steps.
Further, the building of the obstacle set according to the outer polyhedron is as follows: calculating a barrier set according to the barrier expansion shell of the barrier and the vertex set of the outer polyhedron;
the set of obstacles is formulated as:
Figure BDA0003548809990000021
wherein ,Oj(τiT) represents a set of obstacles, conv {. represents a convex hull,
Figure BDA0003548809990000022
represents Minkowski and, BjObstacle-expandable casing, V, representing an obstaclej(τi| t) is the set of vertices of the outer polyhedron; i represents the time interval number of the unmanned aerial vehicle system, j represents the number of obstacles, and i and j are positive integers; (τ)i| t) denotes predicting τ forward at time tiAnd (5) carrying out the steps.
Further, the constructing of the obstacle avoidance constraint according to the obstacle set is as follows: separating the barrier from the unmanned aerial vehicle by adopting a separation plane to realize obstacle avoidance constraint; the obstacle avoidance constraint is expressed by a formula as follows:
Figure BDA0003548809990000023
Figure BDA0003548809990000024
wherein ,
Figure BDA0003548809990000025
set of representatives of the obstacle Oj(τiThe coordinates of the vertices of | t),
Figure BDA0003548809990000026
is a normal vector separating planes, dj(τiI t) is a constant separating the planes, p (τ)iI t) is the position of the unmanned aerial vehicle, i represents the time interval number of the unmanned aerial vehicle system, j represents the number of obstacles, and i and j are positive integers; (τ)i| t) denotes predicting τ forward at time tiAnd (5) carrying out the steps.
Further, the optimization model for unmanned aerial vehicle trajectory tracking obstacle avoidance comprises: cost function, position system constraint, error system constraint, obstacle avoidance constraint, state constraint, control constraint and terminal constraint;
the optimization model is formulated as:
Figure BDA0003548809990000031
s.t.ξ(t|t)＝ξ(t)
Figure BDA0003548809990000032
Figure BDA0003548809990000033
Figure BDA0003548809990000034
Figure BDA0003548809990000035
Figure BDA0003548809990000036
Figure BDA0003548809990000037
ξe(T|t)∈Ω
wherein ,J(ξe(t),ue(t)) represents a cost function of the drone at time t; min represents the minimum value of the obtained data,
Figure BDA0003548809990000038
indicating a position system constraint, ξ (τ | t) indicating a state quantity of the position system, u (τ | t) indicating a control quantity of the position system,
Figure BDA0003548809990000039
representing error system constraints ξe(τ | t) represents the state quantity of the error system, r (τ | t) represents the reference trajectory, u represents the error systemr(τ | t) denotes control of the reference systemThe amount of the (B) component (A),
Figure BDA00035488099900000310
set of representatives of the obstacle Oj(τiThe coordinates of the vertices of | t),
Figure BDA00035488099900000311
is a normal vector separating planes, dj(τiI t) is a constant separating the planes, p (τ)iI t) is the drone position,
Figure BDA00035488099900000312
a control input is represented that is a control input,
Figure BDA00035488099900000313
a set of control constraints is represented that are,
Figure BDA00035488099900000314
the state constraint is represented by a number of state constraints,
Figure BDA00035488099900000315
representing a set of speed constraints, ξe(T | T) ∈ Ω denotes the termination constraint, and Ω denotes the set of terminations.
Further, the first-order controller is:
Figure BDA00035488099900000316
Figure BDA00035488099900000317
wherein phi represents a roll angle, theta represents a pitch angle,
Figure BDA00035488099900000318
the acceleration representing the roll angle is shown,
Figure BDA00035488099900000319
acceleration, τ, representing pitch angleφAnd τθTime constants, k, of the roll and pitch angles, respectivelyθAnd k isφGain constants, phi, representing respectively the roll angle and the pitch angleref and θrefAre reference angles for roll and pitch angles.
Has the advantages that:
(1) an unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control adopts an MINGO base to obtain an outer polyhedron of a predicted trajectory of an obstacle, adopts a separation plane as an online optimization variable, separates the predicted trajectory of the unmanned aerial vehicle from an obstacle set, and completes position control of the unmanned aerial vehicle. Through the transformation solution of the obstacle avoidance constraint, the effect of the obstacle avoidance constraint can be visually embodied, and the obstacle avoidance success rate of the unmanned aerial vehicle in the track tracking process is improved. The inner ring, namely the attitude control, adopts a first-order controller, and the integrity of the unmanned aerial vehicle obstacle avoidance track tracking control is ensured.
(2) And constructing an optimization model of the unmanned aerial vehicle for tracking and avoiding the obstacle by considering a cost function, position system constraint, error system constraint, obstacle avoidance constraint, state constraint, control constraint and terminal constraint, and ensuring that the final solution of the algorithm is feasible and stable.
Drawings
Fig. 1 is a flow chart of generation of an unmanned aerial vehicle obstacle avoidance trajectory in a two-dimensional plane by the unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control.
Fig. 2 is a schematic diagram of unmanned aerial vehicle track obstacle avoidance.
Fig. 3 is a predicted trajectory of the drone at an initial time.
Fig. 4 shows the predicted trajectory of the drone at t-5.5 s.
Fig. 5 shows the predicted trajectory of the drone at t ═ 7.5 s.
Fig. 6 shows the predicted trajectory of the drone at t-10 s.
Fig. 7 shows the actual motion trajectory and the reference trajectory of the drone.
Fig. 8 shows the movement locus of the drone in the x, y and z axes.
Fig. 9 illustrates the constraints of the drone on the amount of control and the amount of speed.
Fig. 10 is a distance between the drone and the dynamic obstacle.
Detailed Description
The invention provides a trajectory tracking obstacle avoidance control method of an unmanned aerial vehicle, which comprises the steps of dividing an unmanned aerial vehicle model into a position subsystem and an attitude subsystem, obtaining an obstacle set in a MINVO (mixed input video) based mode, separating a predicted trajectory of the unmanned aerial vehicle from the obstacle set by adopting a separation plane, simultaneously considering state constraint, control constraint and reference trajectory of the system, controlling an outer ring through model prediction control, designing reasonable terminal cost, terminal controller and terminal constraint conditions, constructing an optimization model, proving feasibility of an algorithm, decoupling a first control quantity in an optimal control sequence obtained for an optimization problem to obtain a reference attitude angle of an attitude ring, controlling the attitude ring of the unmanned aerial vehicle through a first-stage controller, and repeating the steps to carry out rolling solution.
The invention is described in detail below by way of example with reference to the accompanying drawings.
The invention provides an unmanned aerial vehicle track tracking obstacle avoidance method based on model predictive control, as shown in figure 1, for the generation of the unmanned aerial vehicle obstacle avoidance track, an MINGO basis is adopted to calculate and obtain an outer polyhedron of the minimum volume surrounding a dynamic obstacle prediction motion range, and an obstacle set is constructed according to the outer polyhedron; constructing obstacle avoidance constraints according to the obstacle set, constructing an optimization model of unmanned aerial vehicle track tracking obstacle avoidance based on the obstacle avoidance constraints, and solving according to the optimization model to obtain an optimal control sequence; obtaining a separation plane separating the predicted track of the unmanned aerial vehicle from the obstacle set according to the optimal control sequence, and finishing the position control of the unmanned aerial vehicle; decoupling operation is carried out on the optimal control sequence, and attitude control is carried out through a first-order controller; therefore, the unmanned aerial vehicle motion control in the unmanned aerial vehicle trajectory tracking process is completed.
The unmanned aerial vehicle track tracking obstacle avoidance method specifically comprises the following steps, and it is noted that for clarity of expression, the method is described in a step description form, but the labels of the specific steps are not used for limiting the sequence, such as derivation operation of various constraints, and the sequence is not included, but the construction of an optimization model needs to be considered at the same time.
Step 1: the method comprises the steps of establishing a system model of the unmanned aerial vehicle, dividing the unmanned aerial vehicle system into an inner ring and an outer ring, wherein the inner ring is an attitude control ring of the unmanned aerial vehicle, the outer ring is a position control ring of the unmanned aerial vehicle, meanwhile, the control constraint and the obstacle avoidance constraint are considered, and an error system is established by taking a given reference track as a control target. Wherein the reference track is a track that the unmanned aerial vehicle needs to track. The method specifically comprises the following steps:
step 1.1: the system model of the unmanned aerial vehicle is
Figure BDA0003548809990000061
Figure BDA0003548809990000062
Wherein m represents the mass of the unmanned aerial vehicle, g is the acceleration of gravity,
Figure BDA0003548809990000063
is the state quantity of the unmanned plane position system, and x, y and z respectively represent the position coordinates of an x axis, a y axis and a z axis, vx,vy,vzRepresenting the x, y and z-axis velocities, respectively, F is the actual input torque,
Figure BDA0003548809990000064
the state quantities of the attitude system, and phi, theta, psi represent the roll angle, pitch angle and yaw angle, respectively,
Figure BDA0003548809990000065
angular acceleration representing its attitude angle, Jx,Jy,JzMoment of inertia about the x, y and z axes, Mφ,Mθ,MψFor its directional torque, u ═ ux,uy,uz]TRepresenting the amount of control of the position system.
Step 1.2: the unmanned aerial vehicle system model is divided into an inner ring attitude subsystem and an outer ring position subsystem, and the subsystem models are as follows:
Figure BDA0003548809990000071
Figure BDA0003548809990000072
and satisfy
Figure BDA0003548809990000073
And the actual control moment F, and the generated reference attitude angle phiref and θrefCan be expressed as
Figure BDA0003548809990000074
Figure BDA0003548809990000075
Figure BDA0003548809990000076
Step 1.3: the state constraints, control constraints and obstacle avoidance constraints of the drone may be expressed as:
and (3) state constraint:
Figure BDA0003548809990000077
and (3) controlling and constraining:
Figure BDA0003548809990000079
obstacle avoidance and restraint:
Figure BDA0003548809990000081
wherein ,
Figure BDA0003548809990000082
and
Figure BDA0003548809990000083
is a known normal number, and
Figure BDA0003548809990000084
j represents the serial number of the obstacle, and
Figure BDA0003548809990000085
Figure BDA0003548809990000086
representing the actual position of the obstacle at time t, and D representing the minimum safe distance between the drone and the obstacle.
Step 1.4: taking r as a virtual state quantity, generating a reference track through a second-order integrator, wherein the expression form of the second-order integrator is as follows:
Figure BDA0003548809990000087
wherein ,r1The epsilon R is a position parameter,
Figure BDA0003548809990000088
is a speed parameter and has
Figure BDA0003548809990000089
Figure BDA00035488099900000810
Is a reference control quantity and satisfies
Figure BDA00035488099900000811
Here,
Figure BDA00035488099900000812
and
Figure BDA00035488099900000813
is a known normal constant, reference trajectory pr(t) can be represented as
Figure BDA00035488099900000814
Step 1.5: taking peIs the position error between the actual state and the reference state, veIs the speed error between the actual state and the reference state, and
pe＝p-S(r1) (18a)
Figure BDA00035488099900000815
defining the state quantity of the error system according to the position error and the speed error as
Figure BDA00035488099900000816
The error system can be expressed as
Figure BDA00035488099900000817
wherein ,h(ξe(τ|t),r(τ|t),u(τ|t),ur(τ | t)) is abbreviated h (ξ)e,r,u,ur) Same principle ue(ξe,r,u,ur) Xi of middle schoole,r,u,urAlso in short. u. ofe(ξe,r,u,ur) Can be expressed as
Figure BDA0003548809990000091
Step 2: and (3) processing obstacle avoidance constraints, constructing an outer polyhedron with the minimum volume which can surround the motion range, namely the predicted track, of the dynamic obstacle within a period of time in the future by adopting an MINVO base, taking the outer polyhedron as an obstacle set, and separating the predicted track of the unmanned aerial vehicle from the obstacle set by adopting a separation plane as a decision variable. The method specifically comprises the following steps:
step 2.1: taking the sampling time as delta, equally dividing the future prediction time domain T into N segments, and taking T as N delta. Subscripts on the time interval
Figure BDA0003548809990000092
Subscript of barrier
Figure BDA0003548809990000093
Definition (τ)i| t) denotes τ at time tiThe step is predicted and the future predicted trajectory of obstacle j is represented as:
Figure BDA0003548809990000094
wherein ,qj(τi| t) denotes τ at time tiStep predict position, for
Figure BDA0003548809990000095
Is provided with
Figure BDA0003548809990000096
wherein ,Qj(τiI t) is with respect to qj(τiI t), s is the order of the prediction polynomial curve. If the obstacle is a static obstacle, qj(τi|t)＝qj＝[xj,yj,zj]TIs a constant and has Qj(τi|t)＝[03×s,qj]。
Step 2.2: in order to obtain the outer polyhedron with the smallest volume, the MINVO-based approach is adopted for solving. The relationship of the set of vertices of the external polyhedron to the polynomial curve coefficient matrix can be described as
Vj(τi|t)＝Qj(τi|t)A-1(τi|t) (23)
wherein ,A-1(τiI t) is a time correlation matrix, which can be obtained by solving an optimization problem, and varies with time interval. In particular, the time correlation matrix can be calculated by the MINVO theory of Jesus Tordesillas, and will not be described herein.
Step 2.3: assuming that the actual position of the obstacle is satisfied
Figure BDA0003548809990000101
Wherein conv {. represents a convex hull,
Figure BDA0003548809990000102
represents Minkowski and, BjObstacle-inflated envelope representing obstacle subject to predicted trajectory error
Figure BDA0003548809990000103
Size of obstacle itself
Figure BDA0003548809990000104
And a safe distance
Figure BDA0003548809990000105
Can be influenced by 2 (. alpha.),j+βj+ D). The expansion shell of the obstacle means that the actual size of the obstacle is expanded to a certain range by considering the actual shape of the obstacle and the deviation existing in the prediction process, and the expansion shell is influenced by the deviation between the size and the shape of the obstacle and the future predicted track.
Step 2.4: defining a set of obstacles
Figure BDA0003548809990000106
According to step 2.2 and step 2.3, one can obtain
Figure BDA0003548809990000107
According to (26), if the position of the unmanned plane
Figure BDA0003548809990000108
The drone will not collide with the obstacle j and meet the requirements of (15).
Step 2.5: for all
Figure BDA0003548809990000109
And
Figure BDA00035488099900001010
set of obstacles satisfies
Figure BDA00035488099900001011
Step 2.6: selecting a plane pij(τi| t) (by normal vector
Figure BDA00035488099900001012
And constant dj(τiT) as a decision variable, set the obstacles Oj(τi| t) and drone position p (τ)iT) separation, and obstacle avoidance constraints can be described as
Figure BDA00035488099900001013
wherein ,
Figure BDA00035488099900001014
set of representatives of obstacle Oj(τi| t) vertex coordinates. Plane pij(τiI t), drone position p (τ)iT) and set of obstacles Oj(τiI t) is shown in fig. 2.
And step 3: and designing a corresponding cost function according to the position subsystem and the error system by considering state constraint, control constraint and obstacle avoidance constraint, and constructing an optimization problem. The method specifically comprises the following steps:
step 3.1: defining a cost function at time t as
Figure BDA00035488099900001015
wherein ,ue(t)＝{ue(τ|t),τ∈[t,t+T]}，
Figure BDA0003548809990000111
Represents a stage cost, and
Figure BDA0003548809990000112
and
Figure BDA0003548809990000113
is a semi-positive definite matrix and is provided with a positive definite matrix,
Figure BDA0003548809990000114
represents the terminal cost, and
Figure BDA0003548809990000115
is a positive definite matrix.
Step 3.2: the optimization problem of unmanned aerial vehicle trajectory tracking obstacle avoidance is described as
Figure BDA0003548809990000116
s.t.ξ(t|t)＝ξ(t) (29b)
Figure BDA0003548809990000117
Figure BDA0003548809990000118
Figure BDA0003548809990000119
Figure BDA00035488099900001110
Figure BDA00035488099900001111
Figure BDA00035488099900001112
ξe(T|t)∈Ω (29i)
wherein ,J(ξe(t),ue(t)) represents a cost function of the drone at time t; min represents the minimum value to be calculated,
Figure BDA00035488099900001113
indicating a position system constraint, ξ (τ | t) indicating a state quantity of the position system, u (τ | t) indicating a control quantity of the position system,
Figure BDA00035488099900001114
representing error system constraints ξe(τ | t) represents the state quantity of the error system, r (τ | t) represents the reference trajectory, u represents the error systemr(τ | t) represents a control amount of the reference system,
Figure BDA00035488099900001115
set of representatives of the obstacle Oj(τiThe coordinates of the vertices of | t),
Figure BDA00035488099900001116
is a normal vector separating planes, dj(τiL t) is a constant separating the planes, p (τ)iI t) is the drone position,
Figure BDA00035488099900001117
a control input is represented that is a control input,
Figure BDA00035488099900001118
a set of control constraints is represented that are,
Figure BDA00035488099900001119
the state constraint is represented by a number of state constraints,
Figure BDA00035488099900001120
representing a set of speed constraints, ξe(T | T) ∈ Ω denotes the termination constraint, and Ω denotes the set of terminations.
In the terminal domain omega and the corresponding terminal controller k (-) in the constraint (29i), the optimal control sequence u is obtained by solving the optimization problem, i.e. the optimization model*(t) separating one planar sequence from each other
Figure BDA00035488099900001121
And
Figure BDA00035488099900001122
composition of
Figure BDA00035488099900001123
And the corresponding optimal state sequence is
ξ*(t)＝{ξ*(τ|t),τ∈[t,t+T]} (30)
And 4, step 4: and designing a terminal controller and a terminal cost function to prove the feasibility and stability of the algorithm. The method specifically comprises the following steps:
step 4.1: for the error system in (19), for arbitrary ξe(T + T) belongs to omega, and the designed terminal domain omega and the corresponding terminal controller kappa (-) belong to tau belongs to (T + T, T + delta + T)]Need to satisfy
Figure BDA0003548809990000121
Figure BDA0003548809990000122
Figure BDA0003548809990000123
ξe(τ|t)∈Ω (31d)
Step 4.2: to ensure that all constraints of step 4.1 are met, the terminal controller is designed to
Figure BDA0003548809990000124
Wherein K ═ diag { K ═ K11,k12,k13},diag{k21,k22,k23}]，kab< 0 and for a ═ 1,2, b ═ 1,2,3 there are
Figure BDA0003548809990000125
The terminal domain is represented as
Ω＝{ξe:‖ξe‖P≤ε} (33)
wherein
Figure BDA0003548809990000126
and
Figure BDA0003548809990000127
The selection of the positive definite matrix P needs to satisfy
ζTP+Pζ+Q+KTRK≤0 (35)
and
Figure BDA0003548809990000128
Step 4.3: according to the constraint in equation (34), there are
Figure BDA0003548809990000131
Thus, (31a) was confirmed.
According to (34), the boundary of the terminal controller is represented as
Figure BDA0003548809990000132
Thus, (31b) was confirmed.
According to (16) and (32), there is ue＝KξeAnd
Figure BDA0003548809990000133
to obtain
Figure BDA0003548809990000134
Thus, (31c) was confirmed.
According to (38) and having L (ξ)e(τ|t),ue(tau | t)) > 0 or more and is easy to obtain
Figure BDA0003548809990000135
Satisfies (31 d).
And 5: obtaining the optimal control sequence u by the solution*In the step (t), decoupling operation is carried out on the first control quantity u, attitude loop control is carried out through a first-order controller, and the adopted controller is as follows:
Figure BDA0003548809990000136
wherein phi represents a roll angle, theta represents a pitch angle,
Figure BDA0003548809990000137
the acceleration representing the roll angle is shown,
Figure BDA0003548809990000138
acceleration, τ, representing pitch angleφAnd τθAre respectively tumblingTime constants of angle and pitch angle, kθAnd k isφGain constants, phi, representing respectively the roll angle and the pitch angleref and θrefAre reference angles for roll and pitch angles.
Step 6: and (3) carrying out a simulation experiment on MATLAB by using a YALMIP tool box and an IPOPT solver, and verifying the effectiveness of the algorithm.
The parameters selected by simulation are as follows: m is 1kg, g is 9.81m/s2，
Figure BDA0003548809990000141
And having a reference track pr(t)＝[3 cos(r1(t)),3 sin(r1(t)),3]T，
Figure BDA0003548809990000142
The safety distance D is 0.2m, the sampling time δ is 0.1s and T is 20 δ, the cost function matrix is Q diag {100,10, 10,10}, R diag {0.1,0.1,0.1}, K [ diag { -5, -5, -5}, diag { -4, -4, -4}]Selecting P matrix as
Figure BDA0003548809990000143
At the same time, in the terminal domain, epsilon is 0.7124, and the inner loop parameter is kφ＝kθ＝1，τφ＝τθ＝0.1。
As shown in fig. 3 to 6, the obstacle can avoid the static obstacle and the dynamic obstacle and track the reference trajectory with minimum cost. As shown in fig. 7, the drone may track in an environment of 16 static obstacles and 1 dynamic obstacle, where the red line represents the actual trajectory of the drone and the blue line represents the reference trajectory. As shown in fig. 8, the drone follows the reference trajectory in each coordinate axis, where the red line represents the reference trajectory and the blue line represents the actual trajectory. As shown in fig. 9, the drone satisfies control constraints and speed constraints. As shown in fig. 10, the drone and the obstacle are always kept at a distance of at least 0.2 m.
The above embodiments only describe the design principle of the present invention, and the shapes and names of the components in the description may be different without limitation. Therefore, a person skilled in the art of the present invention can modify or substitute the technical solutions described in the foregoing embodiments; such modifications and substitutions do not depart from the spirit and scope of the present invention.

Claims (6)

1. An unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control is characterized in that an external polyhedron surrounding the minimum dynamic obstacle predictive motion range is obtained by adopting MINVO base calculation, and an obstacle set is constructed according to the external polyhedron; constructing obstacle avoidance constraints according to the obstacle set, constructing an optimization model of unmanned aerial vehicle trajectory tracking obstacle avoidance based on the obstacle avoidance constraints, and solving according to the optimization model to obtain an optimal control sequence;
obtaining a separation plane separating the predicted track of the unmanned aerial vehicle from the obstacle set according to the optimal control sequence, and completing the position control of the unmanned aerial vehicle;
performing decoupling operation on the optimal control sequence, and performing attitude control through a first-order controller; therefore, the unmanned aerial vehicle motion control in the unmanned aerial vehicle trajectory tracking process is completed.
2. The unmanned aerial vehicle trajectory tracking obstacle avoidance method of claim 1, wherein the outer polyhedron is:
Vj(τi|t)＝Qj(τi|t)A-1(τi|t)
wherein ,Vj(τiI t) is the set of vertices of the outer polyhedron, Qj(τi| t) is a polynomial curve coefficient matrix of the predicted trajectory of the obstacle, A-1(τi| t) is a time correlation matrix; i represents the time interval number of the unmanned aerial vehicle system, j represents the number of obstacles, and i and j are positive integers; (tau. is)i| t) represents the forward prediction τ at time tiAnd (5) carrying out the steps.
3. The unmanned aerial vehicle trajectory tracking obstacle avoidance method of claim 1, wherein the building of the obstacle set according to the outer polyhedron is: calculating a barrier set according to the barrier expansion shell of the barrier and the vertex set of the outer polyhedron;
the set of obstacles is formulated as:
Figure FDA0003548809980000011
wherein ,Oj(τiT) represents a set of obstacles, conv {. represents a convex hull,
Figure FDA0003548809980000012
representing Minkowski and, BjObstacle-expandable casing, V, representing an obstaclej(τi| t) is the set of vertices of the outer polyhedron; i represents the time interval number of the unmanned aerial vehicle system, j represents the number of obstacles, and i and j are positive integers; (τ)i| t) denotes predicting τ forward at time tiAnd (5) carrying out the following steps.
4. The unmanned aerial vehicle trajectory tracking obstacle avoidance method of claim 1, wherein the constructing obstacle avoidance constraints according to the set of obstacles is: separating the barrier from the unmanned aerial vehicle by adopting a separation plane to realize obstacle avoidance constraint; the obstacle avoidance constraint is formulated as:
Figure FDA0003548809980000021
Figure FDA0003548809980000022
wherein ,
Figure FDA0003548809980000023
set of representatives of the obstacle Oj(τiThe coordinates of the vertices of | t),
Figure FDA0003548809980000024
is a normal vector separating planes, dj(τiI t) is a constant separating the planes, p (τ)iI t) is the position of the unmanned aerial vehicle, i represents the time interval number of the unmanned aerial vehicle system, j represents the number of obstacles, and i and j are positive integers; (τ)i| t) denotes predicting τ forward at time tiAnd (5) carrying out the steps.
5. The unmanned aerial vehicle trajectory tracking obstacle avoidance method of claim 1, wherein the optimization model for unmanned aerial vehicle trajectory tracking obstacle avoidance comprises: cost function, position system constraint, error system constraint, obstacle avoidance constraint, state constraint, control constraint and terminal constraint;
the optimization model is formulated as:
Figure FDA0003548809980000025
s.t.ξ(t|t)＝ξ(t)
Figure FDA0003548809980000026
Figure FDA0003548809980000027
Figure FDA0003548809980000028
Figure FDA0003548809980000029
Figure FDA00035488099800000210
Figure FDA00035488099800000211
ξe(T|t)∈Ω
wherein ,J(ξe(t),ue(t)) represents a cost function of the drone at time t; min represents the minimum value of the obtained data,
Figure FDA00035488099800000212
indicating a position system constraint, ξ (τ | t) indicating a state quantity of the position system, u (τ | t) indicating a control quantity of the position system,
Figure FDA00035488099800000213
representing error system constraints, ξe(τ | t) represents the state quantity of the error system, r (τ | t) represents the reference trajectory, u represents the error systemr(τ | t) represents a control amount of the reference system,
Figure FDA00035488099800000214
set of representatives of obstacle Oj(τiThe coordinates of the vertices of | t),
Figure FDA0003548809980000031
is a normal vector separating planes, dj(τiI t) is a constant separating the planes, p (τ)iI t) is the drone position,
Figure FDA0003548809980000032
a control input is represented that is a control input,
Figure FDA0003548809980000033
a set of control constraints is represented that are,
Figure FDA0003548809980000034
the state constraint is represented by a number of state constraints,
Figure FDA0003548809980000035
representing a set of speed constraints, ξe(T | T) ∈ Ω represents a termination constraint, and Ω represents a set of terminations.
6. The unmanned aerial vehicle trajectory tracking obstacle avoidance method of claim 1, wherein the first-order controller is:
Figure FDA0003548809980000036
Figure FDA0003548809980000037
wherein phi represents a roll angle, theta represents a pitch angle,
Figure FDA0003548809980000038
the acceleration representing the roll angle is shown,
Figure FDA0003548809980000039
acceleration, τ, representing pitch angleφAnd τθTime constants, k, of the roll and pitch angles, respectivelyθAnd k isφGain constants, phi, representing respectively the roll angle and the pitch angleref and θrefAre reference angles for roll and pitch angles.
CN202210263895.2A 2022-03-16 2022-03-16 Unmanned aerial vehicle track tracking obstacle avoidance method based on model predictive control Active CN114721412B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210263895.2A CN114721412B (en) 2022-03-16 2022-03-16 Unmanned aerial vehicle track tracking obstacle avoidance method based on model predictive control

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210263895.2A CN114721412B (en) 2022-03-16 2022-03-16 Unmanned aerial vehicle track tracking obstacle avoidance method based on model predictive control

Publications (2)

Publication Number Publication Date
CN114721412A true CN114721412A (en) 2022-07-08
CN114721412B CN114721412B (en) 2023-06-16

Family

ID=82237786

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210263895.2A Active CN114721412B (en) 2022-03-16 2022-03-16 Unmanned aerial vehicle track tracking obstacle avoidance method based on model predictive control

Country Status (1)

Country Link
CN (1) CN114721412B (en)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107563044A (en) * 2017-08-29 2018-01-09 武汉科技大学 The four rotor wing unmanned aerial vehicle path tracking control methods based on safety on line study
CN108549407A (en) * 2018-05-23 2018-09-18 哈尔滨工业大学（威海） A kind of control algolithm of multiple no-manned plane collaboration formation avoidance
CN108614580A (en) * 2018-06-22 2018-10-02 中国人民解放军国防科技大学 Layered obstacle avoidance control method in target tracking of unmanned aerial vehicle
US20190019423A1 (en) * 2017-07-17 2019-01-17 Aurora Flight Sciences Corporation System and Method for Detecting Obstacles in Aerial Systems
CN111413966A (en) * 2020-03-12 2020-07-14 天津大学 Progressive model prediction unmanned planning tracking cooperative control method
CN111580548A (en) * 2020-04-17 2020-08-25 中山大学 Unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle
CN111633646A (en) * 2020-05-22 2020-09-08 北京理工大学 Robot motion planning method based on DMPs and modified obstacle avoidance algorithm
CN112506210A (en) * 2020-12-04 2021-03-16 东南大学 Unmanned aerial vehicle control method for autonomous target tracking
CN112666975A (en) * 2020-12-18 2021-04-16 中山大学 Unmanned aerial vehicle safety trajectory tracking method based on predictive control and barrier function

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190019423A1 (en) * 2017-07-17 2019-01-17 Aurora Flight Sciences Corporation System and Method for Detecting Obstacles in Aerial Systems
CN107563044A (en) * 2017-08-29 2018-01-09 武汉科技大学 The four rotor wing unmanned aerial vehicle path tracking control methods based on safety on line study
CN108549407A (en) * 2018-05-23 2018-09-18 哈尔滨工业大学（威海） A kind of control algolithm of multiple no-manned plane collaboration formation avoidance
CN108614580A (en) * 2018-06-22 2018-10-02 中国人民解放军国防科技大学 Layered obstacle avoidance control method in target tracking of unmanned aerial vehicle
CN111413966A (en) * 2020-03-12 2020-07-14 天津大学 Progressive model prediction unmanned planning tracking cooperative control method
CN111580548A (en) * 2020-04-17 2020-08-25 中山大学 Unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle
CN111633646A (en) * 2020-05-22 2020-09-08 北京理工大学 Robot motion planning method based on DMPs and modified obstacle avoidance algorithm
CN112506210A (en) * 2020-12-04 2021-03-16 东南大学 Unmanned aerial vehicle control method for autonomous target tracking
CN112666975A (en) * 2020-12-18 2021-04-16 中山大学 Unmanned aerial vehicle safety trajectory tracking method based on predictive control and barrier function

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
代进进: "基于模型预测控制的无人机避障路径规划方法", 《火力与指挥控制》 *

Also Published As

Publication number Publication date
CN114721412B (en) 2023-06-16

Similar Documents

Publication Publication Date Title
Jasim et al. Robust team formation control for quadrotors
Xiang et al. Robust fuzzy 3D path following for autonomous underwater vehicle subject to uncertainties
CN107490965B (en) Multi-constraint trajectory planning method for space free floating mechanical arm
Aguiar et al. Trajectory-tracking and path-following of underactuated autonomous vehicles with parametric modeling uncertainty
Molnar et al. Model-free safety-critical control for robotic systems
Kaminer et al. Path following for small unmanned aerial vehicles using L1 adaptive augmentation of commercial autopilots
Wang et al. The trajectory tracking problem of quadrotor UAV: Global stability analysis and control design based on the cascade theory
Seddaoui et al. Combined nonlinear H∞ controller for a controlled-floating space robot
Yan et al. Consensus formation tracking for multiple AUV systems using distributed bioinspired sliding mode control
Liu et al. Saturated coordinated control of multiple underactuated unmanned surface vehicles over a closed curve
Rajappa et al. Adaptive super twisting controller for a quadrotor uav
CN112241125A (en) Unmanned aerial vehicle trajectory tracking method based on differential flatness characteristic
Yang et al. Distributed optimal consensus with obstacle avoidance algorithm of mixed-order UAVs–USVs–UUVs systems
Omran et al. Optimal task space control design of a Stewart manipulator for aircraft stall recovery
Oliveira et al. Three dimensional moving path following for fixed-wing unmanned aerial vehicles
CN115639830A (en) Air-ground intelligent agent cooperative formation control system and formation control method thereof
Tayal et al. Control barrier functions in dynamic UAVs for kinematic obstacle avoidance: a collision cone approach
Huo et al. Collision-free model predictive trajectory tracking control for uavs in obstacle environment
Araar et al. Quadrotor control for trajectory tracking in presence of wind disturbances
Cheng et al. Disturbance observer-based nonsingular fixed-time sliding mode tracking control for a quadcopter
CN114721412A (en) Unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control
CN110109357B (en) Semi-global self-adaptive control method for non-standard type non-linear aircraft
Nguyen et al. Formation control scheme with reinforcement learning strategy for a group of multiple surface vehicles
Ali et al. Control system analysis and design of quadcopter in the presence of unmodelled dynamics and disturbances
Ghosh et al. Near-optimal feedback strategies for optimal control and pursuit-evasion games: a spatial statistical approach

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