CN105786014A - Control method and device of unmanned device - Google Patents

Abstract

The invention discloses a control method and device of an unmanned device. The method comprises that external input reference which includes a preset planned path is obtained; the planned path is divided into at least one sub path according to inflection points in the planned path; an invariant set and a corresponding reachable set of each sub path are obtained according to an initial power system model of the unmanned device and a preset external reference input integration model; and a practical path of the unmanned device is determined according to the intersection points between the reachable set of each sub path and the sub path itself. Requirements of the external environment and dynamic requirements of the unmanned device are met at the same time, and the unmanned device is prevented from colliding with barriers.

Description

Control method and device of unmanned equipment

Technical Field

The present disclosure relates to the field of control of an unmanned aerial vehicle, and in particular, to a method and an apparatus for controlling an unmanned aerial vehicle.

Background

Unmanned equipment such as unmanned aerial vehicles, unmanned ships, unmanned submersibles and the like are increasingly widely applied to the military and civil fields. In the face of different working environments, unmanned equipment needs to have the capability of ensuring the safe operation of the unmanned equipment and completing given tasks. However, achieving this capability presents a number of challenges due to its own dynamic constraints and external environmental constraints.

In the current prior art, a series of path planning methods have been proposed, such as an artificial potential field-based method, a graph theory-based method, a random adoption method, and the like, which can obtain a series of path points or feasible paths that satisfy external environmental constraints, and ensure that the unmanned device satisfies the external environmental constraints, such as no collision with an obstacle, by enabling the unmanned device to track the path points or feasible paths. However, due to the dynamic constraint of the unmanned device, the unmanned device cannot track a feasible path without deviation in a real situation, so that the unmanned device may collide with an obstacle.

Disclosure of Invention

The disclosure provides a control method and a control device for an unmanned device, which are used for solving the problem that the unmanned device possibly collides with an obstacle in the prior art.

In order to achieve the above object, the present disclosure provides a control method of an unmanned aerial vehicle, the method including:

acquiring an external input reference, wherein the external input reference is a preset planning path;

dividing the planned path into at least one section of sub-path according to the inflection point in the planned path;

respectively acquiring an invariant set and a corresponding reachable set of each sub-path according to an initial power system model of the unmanned equipment and a preset external reference input integral model;

and determining the actual path of the unmanned equipment according to the intersection point of the reachable set of each section of sub-path and the section of sub-path.

Optionally, the obtaining the invariant set and the corresponding reachable set of each segment of sub-path according to the initial power system model of the unmanned aerial vehicle and a preset external reference input integral model respectively includes:

a. acquiring a target power system model of the unmanned equipment according to the initial power system model and a preset external reference input integral model;

b. generating a function model of a static state feedback controller according to the target power system model;

c. generating an invariant set S of the target power system model and an accessible set Sr corresponding to the invariant set S according to the target power system model and a function model of the static state feedback controller;

d. judging whether the end point of the current sub-path is contained in the reachable set Sr;

e. when the end point of the current sub-path is not included in the reachable set Sr, generating a new power system model having an intersection of the reachable set Sr and the current sub-path as a balance point, and performing steps a to e again as the initial power system model until the end point of the current sub-path is included in the currently generated reachable set Sr;

and c, executing the steps a to e to other sub paths in the at least one section of sub path to obtain an invariant set and a corresponding reachable set of each section of sub path.

Optionally, the generating an invariant set S of the target power system model and a reachable set Sr corresponding to the invariant set S according to the target power system model and the function model of the static state feedback controller includes:

taking a balance point of the initial power system model as a circle center, and acquiring the distance between the circle center and an obstacle closest to the circle center;

generating a circle tangent to the edge of the obstacle by taking the distance between the circle center and the obstacle closest to the circle center as a radius;

and generating an invariant set S and an reachable set Sr corresponding to the invariant set under the target power system model and the function model of the static state feedback controller in the circle by utilizing the preset constraint condition of the external reference input and the preset invariant set constraint condition.

Optionally, when the current initial power system model is not an initial power system model with the starting point of the current sub-path as the balance point, the generating an invariant set S of the target power system model and a reachable set Sr corresponding to the invariant set S according to the target power system model and the function model of the static state feedback controller further includes:

and converting the invariant set and the reachable set corresponding to the invariant set into an invariant set and a reachable set in a coordinate system of the initial power system model taking the starting point of the current sub-path as the balance point according to the coordinate relationship between the balance point and the starting point of the current initial power system model.

Optionally, when the end point of the current sub-path is not included in the reachable set Sr, generating a new power system model with an intersection point of the reachable set Sr and the current sub-path as a balance point, includes:

determining constraint conditions of system input of the new power system model according to the intersection point of the reachable set Sr and the current sub-path;

and determining the new power system model according to the constraint conditions of the system input of the new power system model.

Optionally, the initial power system model includes:

$\left\{\begin{array}{c}\stackrel{\·}{x}\left(t\right)=Ax\left(t\right)+Bu\left(t\right)+D\mathrm{\ω}\left(t\right)\\ y\left(t\right)=Cx\left(t\right)\end{array}\right.$

wherein x represents the system state, u represents the control input, ω represents the external disturbance, y represents the system output, and Ω_u＝{u(t)|-u _i≤u_i(t)≤u _i1,. m } and Ω_ω＝{ω(t)|-1≤ω(t)≤1}，h， uConstant vectors are adopted, and A, B, C and D are constant matrixes with preset dimensions;

the external reference input integral model comprises:

e(t)＝∫(ref-y(t))dt

where e (t) represents the error vector, ref represents the external reference input,

the target power system model includes:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_e\left(t\right)=A_e{x}_e\left(t\right)+B_{e}u\left(t\right)+D_e\mathrm{\ω}\left(t\right)+F_{e}ref\\ y\left(t\right)=C_e{x}_e\left(t\right)\end{array}\right.$

wherein A is_e，B_e，C_e，D_eRespectively, matrix after dimension expansion of A, B, C and D, F_eA constant matrix of a predetermined dimension, x_e(t) represents the system state after the dimension expansion, x_e(t)＝[x^T(t)e^T(t)]^T∈Rⁿ。

Optionally, the constraint condition of the external reference input includes:

the external input reference satisfies Ω_ref＝{ref|ref^TQ_rref is less than or equal to 1 }; where ref represents the external reference input,

the invariant constraint condition comprises:

when the parameter η exists₁＞0，η₂＞0，η₃> 0, diagonal matrixQ_uSymmetric positive definite matrix W_cR, parameter λ, positive definite symmetry matrix W ∈ R^n×n，W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen the preset condition is met, the invariant set of the target power system model isThe corresponding reachable set is S_r＝{ref|ref^TQ_rref is less than or equal to 1}, andu(t)∈Ω_uwherein P ═ W^-1,K_e＝YP,Q_r＝W_r ^-1The preset conditions include:

$\begin{array}{c}\underset{W,Y,W_r}{\min }\mathrm{\λ}\\ \left[\begin{array}{cc}{\underset{\‾}{u}}_i^2& Y_i\\ *& W\end{array}\right]\≤0,i=1,...,m\end{array}$

$\left[\begin{array}{ccccc}\begin{array}{c}{\mathrm{WA}}_e^T+A_{e}W+Y^T{B}_e^T\\ +B_{e}Y+({\mathrm{\&eta;}}_1+{\mathrm{\&eta;}}_2)W\end{array}& F_e{W}_r& D_e& Y^T& W\\ *& -{\mathrm{\&eta;}}_1{W}_r& 0& 0& 0\\ *& *& -{\mathrm{\&eta;}}_2& 0& 0\\ *& *& *& -Q_u^{-1}& 0\\ *& *& *& *& -Q_{x_e}^{-1}\end{array}\right]<0$

[I_p0_p×(n-p)]^TW_r[I_p0_p×(n-p)]≤W

$\left[\begin{array}{cc}\mathrm{\&lambda;}R& I\\ *& W_r\end{array}\right]\&le;0$

W≤W₀

$\left[\begin{array}{cc}-{\mathrm{\&eta;}}_{3}W& W{\left[\begin{array}{cc}{h}_i& 0_{1\&times;(n-n_p)}\end{array}\right]}^T\\ *& {\mathrm{\&eta;}}_3-2{\stackrel{\&OverBar;}{x}}_i\end{array}\right]\&le;0,i=1,...,n_p$

λ＞0

wherein, Y_iRepresenting the ith column of the matrix Y, diagonal matrixQ_uIs a linear quadratic regulator LQR controller parameter matrix,thenIs an invariant set of the system (2), S_r＝{ref|ref^TQ_rref ≦ 1 is the corresponding reachable set andand u (t) ∈ Ω_uWherein A is_e，B_e，D_eRespectively are matrix A, B, D and F after dimension expansion_eIs a constant matrix of a predetermined dimension, P ═ W^-1，K_e＝YP，Q_r＝W_r ^-1。

The present disclosure also provides a control apparatus of an unmanned aerial vehicle, the apparatus including:

the system comprises an acquisition module, a planning module and a processing module, wherein the acquisition module is used for acquiring an external input reference, and the external input reference is a preset planning path;

the path dividing module is used for dividing the planned path into at least one section of sub-path according to an inflection point in the planned path;

the calculation module is used for respectively acquiring an invariant set and a corresponding reachable set of each section of sub-path according to an initial power system model of the unmanned equipment and a preset external reference input integral model;

and the path determining module is used for determining the actual path of the unmanned equipment according to the intersection point of the reachable set of each section of sub-path and the section of sub-path.

Optionally, the calculation module includes:

the first modeling submodule is used for executing a, acquiring a target power system model of the unmanned equipment according to the initial power system model and a preset external reference input integral model;

a second modeling submodule for executing b, generating a function model of a static state feedback controller according to the target power system model;

c, generating an invariant set S of the target power system model and a reachable set Sr corresponding to the invariant set S according to the target power system model and a function model of the static state feedback controller;

a judgment sub-module, configured to perform d, judge whether the end point of the current sub-path is included in the reachable set Sr;

when the end point of the current sub-path is not contained in the reachable set Sr, generating a new power system model taking the intersection point of the reachable set Sr and the current sub-path as a balance point, and performing the steps a to e again as the initial power system model until the end point of the current sub-path is contained in the currently generated reachable set Sr;

the first modeling submodule, the second modeling submodule, the calculating submodule, the judging submodule and the third modeling submodule are further used for respectively executing the steps a to e to other sub paths in the at least one section of sub path to obtain an invariant set and a corresponding reachable set of each section of sub path.

Optionally, the calculation submodule is configured to:

taking a balance point of the initial power system model as a circle center, and acquiring the distance between the circle center and an obstacle closest to the circle center;

generating a circle tangent to the edge of the obstacle by taking the distance between the circle center and the obstacle closest to the circle center as a radius;

and generating an invariant set S and an reachable set Sr corresponding to the invariant set under the target power system model and the function model of the static state feedback controller in the circle by utilizing the preset constraint condition of the external reference input and the preset invariant set constraint condition.

Optionally, when the current initial power system model is not an initial power system model with the starting point of the current sub-path as the balance point, the calculation sub-module is further configured to:

and converting the invariant set and the reachable set corresponding to the invariant set into an invariant set and a reachable set in a coordinate system of the initial power system model taking the starting point of the current sub-path as the balance point according to the coordinate relationship between the balance point and the starting point of the current initial power system model.

Optionally, the third modeling submodule is configured to:

determining constraint conditions of system input of the new power system model according to the intersection point of the reachable set Sr and the current sub-path;

and determining the new power system model according to the constraint conditions of the system input of the new power system model.

Optionally, the initial power system model includes:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}\left(t\right)=Ax\left(t\right)+Bu\left(t\right)+D\mathrm{\&omega;}\left(t\right)\\ y\left(t\right)=Cx\left(t\right)\end{array}\right.$

wherein x represents the system state, u represents the control input, ω represents the external disturbance, y represents the system output, and Ω_u＝{u(t)|-u _i≤u_i(t)≤u _i1,. m } and Ω_ω＝{ω(t)|-1≤ω(t)≤1}，h， uConstant vectors are adopted, and A, B, C and D are constant matrixes with preset dimensions;

the external reference input integral model comprises:

e(t)＝∫(ref-y(t))dt

where e (t) represents the error vector, ref represents the external reference input,

the target power system model includes:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e\left(t\right)=A_e{x}_e\left(t\right)+B_{e}u\left(t\right)+D_e\mathrm{\&omega;}\left(t\right)+F_{e}ref\\ y\left(t\right)=C_e{x}_e\left(t\right)\end{array}\right.$

wherein A is_e，B_e，C_e，D_eRespectively, matrix after dimension expansion of A, B, C and D, F_eA constant matrix of a predetermined dimension, x_e(t) represents the system state after the dimension expansion, x_e(t)＝[x^T(t)e^T(t)]^T∈Rⁿ。

Optionally, the constraint condition of the external reference input includes:

the external input reference satisfies Ω_ref＝{ref|ref^TQ_rref is less than or equal to 1 }; where ref represents the external reference input,

the invariant constraint condition comprises:

when the parameter η exists₁＞0，η₂＞0，η₃> 0, diagonal matrixQ_uSymmetric positive definite matrix W_cR, parameter λ, positive definite symmetry matrix W ∈ R^n×n，W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen the preset condition is met, the invariant set of the target power system model isThe corresponding reachable set is S_r＝{ref|ref^TQ_rref is less than or equal to 1}, andu(t)∈Ω_uwherein P ═ W^-1,K_e＝YP,Q_r＝W_r ^-1The preset conditions include:

$\begin{array}{c}\underset{W,Y,W_r}{\min }\mathrm{\&lambda;}\\ \left[\begin{array}{cc}{\underset{\&OverBar;}{u}}_i^2& Y_i\\ *& W\end{array}\right]\&le;0,i=1,...,m\end{array}$

$\left[\begin{array}{ccccc}\begin{array}{c}{\mathrm{WA}}_e^T+A_{e}W+Y^T{B}_e^T\\ +B_{e}Y+({\mathrm{\&eta;}}_1+{\mathrm{\&eta;}}_2)W\end{array}& F_e{W}_r& D_e& Y^T& W\\ *& -{\mathrm{\&eta;}}_1{W}_r& 0& 0& 0\\ *& *& -{\mathrm{\&eta;}}_2& 0& 0\\ *& *& *& -Q_u^{-1}& 0\\ *& *& *& *& -Q_{x_e}^{-1}\end{array}\right]<0$

[I_p0_p×(n-p)]^TW_r[I_p0_p×(n-p)]≤W

$\left[\begin{array}{cc}\mathrm{\&lambda;}R& I\\ *& W_r\end{array}\right]\&le;0$

W≤W₀

$\left[\begin{array}{cc}-{\mathrm{\&eta;}}_{3}W& W{\left[\begin{array}{cc}{h}_i& 0_{1\&times;(n-n_p)}\end{array}\right]}^T\\ *& {\mathrm{\&eta;}}_3-2{\stackrel{\&OverBar;}{x}}_i\end{array}\right]\&le;0,i=1,...,n_p$

λ＞0

wherein, Y_iRepresenting the ith column of the matrix Y, diagonal matrixQ_uIs a linear quadratic regulator LQR controller parameter matrix,thenIs an invariant set of the system (2), S_r＝{ref|ref^TQ_rref ≦ 1 is the corresponding reachable set andand u (t) ∈ Ω_uWherein A is_e，B_e，D_eRespectively are matrix A, B, D and F after dimension expansion_eIs a constant matrix of a predetermined dimension, P ═ W^-1，K_e＝YP，Q_r＝W_r ^-1。

According to the control method and device of the unmanned equipment, the external input reference is obtained, the external input reference is a preset planned path, the planned path is divided into at least one section of sub-path according to an inflection point in the planned path, then the invariant set and the corresponding reachable set of each section of sub-path are respectively obtained according to an initial power system model of the unmanned equipment and a preset external reference input integral model, and finally the actual path of the unmanned equipment is determined according to the intersection point of the reachable set of each section of sub-path and the section of sub-path. Therefore, by combining the preset planned path and the power system model of the unmanned equipment, the external environment constraint and the dynamics constraint of the unmanned equipment can be simultaneously met, and the condition that the unmanned equipment collides with the barrier can be avoided.

Additional features and advantages of the disclosure will be set forth in the detailed description which follows.

Drawings

The accompanying drawings, which are included to provide a further understanding of the disclosure and are incorporated in and constitute a part of this specification, illustrate embodiments of the disclosure and together with the description serve to explain the disclosure without limiting the disclosure. In the drawings:

fig. 1 is a schematic flowchart of a control method of an unmanned aerial vehicle according to an embodiment of the present disclosure;

fig. 2 is a schematic flow chart of a control method of an unmanned aerial vehicle according to another embodiment of the present disclosure;

FIG. 3 is a diagram illustrating the relationship of invariant and reachable sets shown in the embodiment of FIG. 2;

FIG. 4a is a schematic diagram of a compute invariant and reachable set shown in the embodiment of FIG. 2;

FIG. 4b is a diagram illustrating one of the compute invariant and reachable sets shown in the embodiment of FIG. 2;

FIG. 4c is a schematic diagram of the actual path shown in the embodiment of FIG. 2;

FIG. 5a is a schematic diagram of a planned path shown in the embodiment of FIG. 2;

FIG. 5b is a schematic diagram of a reachable set in a planned path shown in the embodiment of FIG. 2;

FIG. 5c is a schematic diagram of an actual path based on a planned path in the embodiment shown in FIG. 2;

fig. 6 is a block diagram of a control apparatus of an unmanned aerial vehicle according to an embodiment of the present disclosure;

fig. 7 is a block diagram of a computing module provided in the embodiment shown in fig. 6.

Detailed Description

The following detailed description of specific embodiments of the present disclosure is provided in connection with the accompanying drawings. It should be understood that the detailed description and specific examples, while indicating the present disclosure, are given by way of illustration and explanation only, not limitation.

Fig. 1 is a schematic flowchart of a control method of an unmanned aerial vehicle according to an embodiment of the present disclosure, and referring to fig. 1, the control method of the unmanned aerial vehicle may include the following steps:

step 101, obtaining an external input reference, wherein the external input reference is a preset planning path.

And 102, dividing the planned path into at least one section of sub-path according to the inflection point in the planned path.

And 103, respectively acquiring an invariant set and a corresponding reachable set of each sub-path according to an initial power system model of the unmanned equipment and a preset external reference input integral model.

And 104, determining the actual path of the unmanned equipment according to the intersection point of the reachable set of each section of sub-path and the section of sub-path.

The external input is referred to as a planned path which is planned in advance, and the planned path may include a plurality of straight roads (or approximately straight roads), so that the planned path may be divided into at least one sub-path according to an inflection point (which may be understood as a connection point of two adjacent roads with different trends) in the planned path (where, if there is only one sub-path, the sub-path is the planned path itself, for example, there is no inflection point in the planned path).

The unmanned device involved in various embodiments of the present disclosure may be an unmanned device such as an unmanned aerial vehicle, an unmanned ship, an unmanned submersible vehicle, etc., and the initial power system model of the unmanned device is introduced based on consideration of the dynamics constraints of the unmanned device itself, and may include, for example:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}\left(t\right)=Ax\left(t\right)+Bu\left(t\right)+D\mathrm{\&omega;}\left(t\right)\\ y\left(t\right)=Cx\left(t\right)\end{array}\right.---\left(1\right)$

the above formula (1) is hereinafter referred to as a system (1), where x represents a system state, u represents a control input, ω represents an external disturbance, and y represents a system output, and satisfies Ω_u＝{u(t)|-u _i≤u_i(t)≤u _i1,. m } and Ω_ω＝{ω(t)|-1≤ω(t)≤1}，Denotes the first derivative of x (t), h, uis a constant vector, and A, B, C and D are constant matrixes with preset dimensions.

The preset external reference input integration model is introduced to achieve tracking of the external reference input, and may include, for example, the following integrators:

e(t)＝∫(ref-y(t))dt

where e (t) represents the error vector, ref represents the external reference input,

the system (1) is expanded in dimension according to an external reference input integral model and a new state vector x is defined_e(t)＝[x^T(t)e^T(t)]^T∈RⁿA new power system model may then be obtained, which we may make into the target power system model, which may include:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e\left(t\right)=A_e{x}_e\left(t\right)+B_{e}u\left(t\right)+D_e\mathrm{\&omega;}\left(t\right)+F_{e}ref\\ y\left(t\right)=C_e{x}_e\left(t\right)\end{array}\right.---\left(2\right)$

the above formula (2) is hereinafter referred to as system (2), wherein A_e，B_e，C_e，D_eRespectively, matrix after dimension expansion of A, B, C and D, F_eA constant matrix of a predetermined dimension, x_e(t) represents the system state after the dimension expansion.

Based on the system (2), the following functional model of the static state feedback controller can be determined:

u(t)＝K_ex_e(t)(3)

wherein, K_eIs the feedback matrix to be designed. Equation (3) will be referred to as static state feedback controller (3) hereinafter.

The system (2) and static state feedback controller (3) may be used to calculate the invariant and reachable sets.

The relevant definitions of invariant sets and reachable sets are given below:

invariant set if there is x (t) ∈ S for all x (0) ∈ S, setIs an invariant set of the system (1). In particular, when t > 0, S is positive and constant if the above condition is satisfied.

Robust controllable invariant set if x ∈ omega_x，u∈Ω_u，ω∈Ω_ωAnd there is a feedback controller u (t) kx (t) such that S is positive for this closed loop system, thenIs a robust controllable invariant set of the system (1), where K is the feedback matrix to be designed.

In addition, let V > 0 be a function of x (t), ifThen geometry S_ρWhere ρ is a positive real number, is positive invariant, { x (t) | V (x (t) ≦ ρ }.

The method comprises the following steps: reachable set S of system (2)_rIs defined as:

$S_r\stackrel{\mathrm{\&Delta;}}{=}\{y\left(t\right)=C_e{x}_e\left(t\right)|x_e\left(t\right)\&Element;S_{{\mathrm{rx}}_e}\};$

$S_{{\mathrm{rx}}_e}\stackrel{\mathrm{\&Delta;}}{=}\left\{x_e\left(t\right)|\begin{array}{c}{x}_e\left(t\right),\mathrm{\&omega;}\left(t\right),refsatisfy\left(2\right)\left(3\right)\\ x\&Element;{\mathrm{\&Omega;}}_x,u\&Element;{\mathrm{\&Omega;}}_u,\mathrm{\&omega;}\&Element;{\mathrm{\&Omega;}}_{\mathrm{\&omega;}}\\ ref\&Element;{\mathrm{\&Omega;}}_{ref},x_e\left(0\right)=0,t\&GreaterEqual;0\end{array}\right\}.$

fig. 2 is a schematic flowchart of a control method of an unmanned aerial vehicle according to another embodiment of the present disclosure, and referring to fig. 2, the step 103 of respectively obtaining an invariant set and a corresponding reachable set of each sub-path according to an initial power system model of the unmanned aerial vehicle and a preset external reference input integral model may include the following steps:

and step 1031, acquiring a target power system model of the unmanned equipment according to the initial power system model and a preset external reference input integral model.

Wherein the initial power system model takes the starting point of the current sub-path as a balance point.

Assume that the current sub-path is path L₀L₁Wherein, the path L₀L₁Starting point of (1) is L₀End point is L₁. The initial power system model starts from the current sub-path L₀Starting from a point L for a balance point, i.e. a coordinate system of the initial power system model₀Being the origin of the coordinate system. Since the method for obtaining the target power system model of the unmanned aerial vehicle according to the initial power system model and the preset external reference input integral model has been described above, the detailed description is omitted here.

And 1032, generating a function model of the static state feedback controller according to the target power system model.

Since the method for generating the function model of the static state feedback controller according to the target dynamic system model has been described above, it will not be described herein again.

And 1033, generating an invariant set S of the target power system model and an reachable set Sr corresponding to the invariant set S according to the target power system model and the function model of the static state feedback controller.

Firstly, taking a balance point of the initial power system model as a circle center, and acquiring the distance between the circle center and an obstacle closest to the circle center;

secondly, generating a circle tangent to the edge of the obstacle by taking the distance between the circle center and the obstacle closest to the circle center as a radius;

thirdly, generating an invariant set S and an reachable set Sr corresponding to the invariant set S under the function models of the target power system model and the static state feedback controller in the circle by utilizing the preset constraint condition of the external reference input and the preset invariant set constraint condition.

Illustratively, FIG. 3 is a diagram illustrating the relationship between invariant and reachable sets in the embodiment shown in FIG. 2, see FIG. 3, along a path L₀L₁For example, assume that the balance point of the current initial power system model is path L₀L₁Starting point L of₀Then, starting from the starting point L₀As the center of a circle, obtain the starting point L₀And a distance starting point L₀Distance d of the nearest obstacle; then, with a starting point L₀A circle S is constructed by taking the distance d as the radius and the center of the circle_cThen this circle S_cTangent to the edge of the nearest obstacle, so as to be on the circle S_cThe invariant set and the reachable set are generated internally, so that the unmanned equipment can be ensured not to be crossed with the obstacle.

The computation process of the invariant set and the reachable set is described as follows:

illustratively, in view of the system (2) and the static state feedback controller (3), the constraints of said external reference input as described above may include: the external input reference satisfies Ω_ref＝{ref|ref^TQ_rref is less than or equal to 1 }; where ref represents the external reference input,and can employ invariant sets of ellipses, i.e.The invariant set S and the corresponding reachable set Sr can be calculated by the invariant set constraints described below, which can include:

when the parameter η exists₁＞0，η₂＞0，η₃> 0, diagonal matrixQ_uSymmetric positive definite matrix W_cR, parameter λ, positive definite symmetry matrix W ∈ R^n×n，W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen the preset condition is met, the invariant set of the target power system model isThe corresponding reachable set is S_r＝{ref|ref^TQ_rref is less than or equal to 1}, andu(t)∈Ω_uwherein P ═ W^-1,K_e＝YP,Q_r＝W_r ^-1The preset conditions include:

$\begin{array}{c}\underset{W,Y,W_r}{\min }\mathrm{\&lambda;}\\ \left[\begin{array}{cc}{\underset{\&OverBar;}{u}}_i^2& Y_i\\ *& W\end{array}\right]\&le;0,i=1,...,m\end{array}$

$\left[\begin{array}{ccccc}\begin{array}{c}{\mathrm{WA}}_e^T+A_{e}W+Y^T{B}_e^T\\ +B_{e}Y+({\mathrm{\&eta;}}_1+{\mathrm{\&eta;}}_2)W\end{array}& F_e{W}_r& D_e& Y^T& W\\ *& -{\mathrm{\&eta;}}_1{W}_r& 0& 0& 0\\ *& *& -{\mathrm{\&eta;}}_2& 0& 0\\ *& *& *& -Q_u^{-1}& 0\\ *& *& *& *& -Q_{x_e}^{-1}\end{array}\right]<0$

[I_p0_p×(n-p)]^TW_r[I_p0_p×(n-p)]≤W

$\left[\begin{array}{cc}\mathrm{\&lambda;}R& I\\ *& W_r\end{array}\right]\&le;0$

W≤W₀

$\left[\begin{array}{cc}-{\mathrm{\&eta;}}_{3}W& W{\left[\begin{array}{cc}{h}_i& 0_{1\&times;(n-n_p)}\end{array}\right]}^T\\ *& {\mathrm{\&eta;}}_3-2{\stackrel{\&OverBar;}{x}}_i\end{array}\right]\&le;0,i=1,...,n_p$

λ＞0

wherein, Y_iRepresenting the ith column of the matrix Y, diagonal matrixQ_uIs a matrix of LQR (Linear quadratic regulator) controller parameters,. It can be ensuredIs an invariant set of the system (2), S_r＝{ref|ref^TQ_rref ≦ 1 is the corresponding reachable set andand u (t) ∈ Ω_uWherein A is_e，B_e，D_eRespectively are matrix A, B, D and F after dimension expansion_eIs a constant matrix of a predetermined dimension, P ═ W^-1,K_e＝YP,Q_r＝W_r ^-1。

Thus, the starting point L can be calculated by the above method₀The invariant set S and the reachable set Sr of the system (2) at the balance point, for example, the relationship between the reachable set Sr and the invariant set S can be as shown in fig. 3.

At step 1034, it is determined whether the end point of the current sub-path is included in the reachable set Sr.

And 1035, when the end point of the current sub-path is not included in the reachable set Sr, generating a new power system model taking the intersection point of the reachable set Sr and the current sub-path as a balance point, and executing steps 1031 to 1035 again as the initial power system model until the end point of the current sub-path is included in the currently generated reachable set Sr.

Since the invariant set S and the reachable set Sr calculated in step 1034 are starting points L₀The invariant set and the reachable set of the system (2) being balance points, and thus the invariant set S and the reachable set Sr may be denoted as invariant set S hereinafter₀And can reach set Sr₀。

Illustratively, FIG. 4a is a schematic diagram of a computation invariant set and a reachable set as shown in the embodiment shown in FIG. 2, see FIG. 4a, path L₀L₁End point L of₁Is not included in the reachable set Sr₀In (3), then the accessible set Sr can be added₀And path L₀L₁Point of intersection c₁As a new balance point, generate c₁A new power system model of the balance point.

First, Sr may be based on the reachable set Sr₀And the current sub-path L₀L₁Point of intersection c₁Determining constraints on system inputs of the new power system model, for example, taking into account that the new system differs from the original system only in the point of equilibrium (relative to the coordinate system of the original system), the system's point of equilibrium is moved, using a method of changing constraints on the control inputs of the system, this approximationThe bundle condition may include:

$\left\{\begin{array}{ccc}-{\underset{\&OverBar;}{u}}_i+2{\stackrel{\&OverBar;}{u}}_i\&le;u_i\left(t\right)\&le;{\underset{\&OverBar;}{u}}_i& if& {\stackrel{\&OverBar;}{u}}_i\&GreaterEqual;0\\ -{\underset{\&OverBar;}{u}}_i\&le;u_i\left(t\right)\&le;{\underset{\&OverBar;}{u}}_i+2{\stackrel{\&OverBar;}{u}}_i& if& {\stackrel{\&OverBar;}{u}}_i<0\end{array}\right.$

wherein,B⁺representing the pseudo-inverse of the matrix B, the constraint of the control input of the new power system model is rewritten to- β ≦ u (t) ≦ α, which is then converted to the following symmetric form:

$u\left(t\right)=\frac{\mathrm{\&alpha;}+\mathrm{\&beta;}}{2}z\left(t\right)+\frac{\mathrm{\&alpha;}-\mathrm{\&beta;}}{2}\mathrm{\&zeta;}=\frac{\mathrm{\&alpha;}+\mathrm{\&beta;}}{2}z\left(t\right)+\stackrel{\&OverBar;}{u},-1\&le;z\left(t\right)\&le;1$

wherein, ζ and z (t) respectively represent time-dependent constant terms and related terms in u (t), and new state variables are definedThe new powertrain model may be represented as:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\tilde{x}}\left(t\right)=A\tilde{x}\left(t\right)+B\frac{\mathrm{\&alpha;}+\mathrm{\&beta;}}{2}z\left(t\right)+D\mathrm{\&omega;}\left(t\right)\\ =A\tilde{x}\left(t\right)+\tilde{B}z\left(t\right)+D\mathrm{\&omega;}\left(t\right)\\ y\left(t\right)=Cx\left(t\right)\end{array}\right.---\left(4\right)$

the above formula (4) is hereinafter referred to as a system (4), wherein the equilibrium point of the system (4) is x_c1(which is relative to the system (1) coordinate system) and relative to its own coordinate system as the origin, so the invariant and reachable sets, respectively denoted as the invariant and reachable sets, can be calculated using the method described above under the system (4) own coordinate systemAndand will not change the setAnd reachable setTranslating to the coordinate system of the system (1), and respectively recording the invariant set and the reachable set after translating to the coordinate system of the system (1) as an invariant set S₁And can reach set Sr₁。

Then, continuously judging the current sub-path end point L₁Whether or not to be contained in the reachable set Sr₁If the end point of the current sub-path is contained in the reachable set Sr₁In, then the current sub-path L₀L₁The process of computing the invariant set and the reachable set is completed. If the end point of the current sub-path is not contained in the reachable set Sr₁In, then can reach set Sr₁And path L₀L₁Point of intersection c₂As a new balance point, generate c₂A new power system model of the balance point, and the method is repeatedly carried out to calculate the invariant set S₂And can reach set Sr₂And so on until the reachable set Sr is calculated_nSo that the reachable set Sr_nCan contain a path L₀L₁End point L of₁Until now.

The step of determining the actual path of the unmanned aerial vehicle according to the intersection point of the reachable set of each segment of sub-path and the segment of sub-path in step 104 includes:

and similarly executing steps 1031 to 1035 to other paths in the multi-planning path to obtain an invariant set and a corresponding reachable set of each section of sub-path, and then determining the actual path of the unmanned equipment according to the intersection point of the reachable set of each section of sub-path and the section of sub-path.

Illustratively, FIG. 4b is a schematic diagram of a method for computing invariant and reachable sets, as shown in the embodiment shown in FIG. 2, and referring to FIG. 4b, the diagram includes an invariant set S₁And can reach set Sr₁Path L₀L₁End point L of₁Is included in the reachable set Sr₁In, then according to the reachable set Sr₁And L₀L₁Cross point of (2) and path L₀L₁Starting point L of₀The final actual path is determined. Illustratively, the actual path may be as shown in FIG. 4 c.

Similarly, a final actual path of the entire planned path may be obtained, as shown in fig. 5a to 5c, where fig. 5a is a schematic diagram of the planned path shown in the embodiment shown in fig. 2, fig. 5b is a schematic diagram of a reachable set in the planned path shown in the embodiment shown in fig. 2, and fig. 5c is a schematic diagram of the actual path obtained based on the planned path shown in the embodiment shown in fig. 2.

According to the control method of the unmanned equipment, the external input reference is obtained, the external input reference is a preset planned path, the planned path is divided into at least one section of sub-path according to an inflection point in the planned path, then the invariant set and the corresponding reachable set of each section of sub-path are respectively obtained according to an initial power system model of the unmanned equipment and a preset external reference input integral model, and finally the actual path of the unmanned equipment is determined according to the intersection point of the reachable set of each section of sub-path and the section of sub-path. Therefore, by combining the preset planned path and the power system model of the unmanned equipment, the external environment constraint and the dynamics constraint of the unmanned equipment can be simultaneously met, and the condition that the unmanned equipment collides with the barrier can be avoided.

Fig. 6 is a block diagram of a control apparatus of an unmanned aerial vehicle according to an embodiment of the present disclosure, and referring to fig. 6, the control apparatus 600 of the unmanned aerial vehicle includes:

an obtaining module 610, configured to obtain an external input reference, where the external input reference is a preset planned path;

a path dividing module 620, configured to divide the planned path into at least one segment of sub-path according to an inflection point in the planned path;

the calculation module 630 is configured to obtain an invariant set and a corresponding reachable set of each segment of sub-path according to an initial power system model of the unmanned device and a preset external reference input integral model;

and the path determining module 640 is used for determining the actual path of the unmanned equipment according to the intersection point of the reachable set of each segment of the sub-path and the segment of the sub-path.

Optionally, fig. 7 is a block diagram of a computing module provided in the embodiment shown in fig. 6, and referring to fig. 7, the computing module 630 includes:

a first modeling sub-module 631, configured to perform a, obtaining a target power system model of the unmanned aerial vehicle according to the initial power system model and a preset external reference input integral model, where the initial power system model takes a starting point of a current sub-path as a balance point, and the initial power system model takes the starting point of the current sub-path as a balance point;

a second modeling submodule 632 for performing b, generating a function model of a static state feedback controller according to the target power system model;

a calculation submodule 633 for performing c, generating an invariant set S of the target power system model and a reachable set Sr corresponding to the invariant set S according to the target power system model and the function model of the static state feedback controller;

a determining submodule 634, configured to perform d-determining whether an end point of the current sub-path is included in the reachable set Sr;

a third modeling submodule 635, configured to execute e, when the end point of the current sub-path is not included in the reachable set Sr, generating a new power system model having an intersection of the reachable set Sr and the current sub-path as a balance point, and executing steps a to e again as the initial power system model until the end point of the current sub-path is included in the currently generated reachable set Sr;

the first modeling submodule 631, the second modeling submodule 632, the calculating submodule 633, the judging submodule 634 and the third modeling submodule 635 are further configured to perform steps a to e on other sub paths in the at least one segment of sub path respectively, so as to obtain an invariant set and a corresponding reachable set of each segment of sub path.

Optionally, the calculating sub-module 633 is configured to:

taking a balance point of the initial power system model as a circle center, and acquiring the distance between the circle center and an obstacle closest to the circle center;

generating a circle tangent to the edge of the obstacle by taking the distance between the circle center and the obstacle closest to the circle center as a radius;

and generating an invariant set S and an reachable set Sr corresponding to the invariant set under the target power system model and the function model of the static state feedback controller in the circle by utilizing the preset constraint condition of the external reference input and the preset invariant set constraint condition.

Optionally, when the current initial power system model is not an initial power system model with the starting point of the current sub-path as the balance point, the calculating sub-module 633 is further configured to:

and converting the invariant set and the reachable set corresponding to the invariant set into an invariant set and a reachable set in a coordinate system of the initial power system model taking the starting point of the current sub-path as the balance point according to the coordinate relationship between the balance point and the starting point of the current initial power system model.

Optionally, the third modeling submodule 635 is configured to:

determining constraint conditions of system input of the new power system model according to the intersection point of the reachable set Sr and the current sub-path;

and determining the new power system model according to the constraint conditions of the system input of the new power system model.

Optionally, the initial power system model includes:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}\left(t\right)=Ax\left(t\right)+Bu\left(t\right)+D\mathrm{\&omega;}\left(t\right)\\ y\left(t\right)=Cx\left(t\right)\end{array}\right.$

wherein x represents the system state, u represents the control input, ω represents the external disturbance, y represents the system output, and Ω_u＝{u(t)|-u _i≤u_i(t)≤u _i1,. m } and Ω_ω＝{ω(t)|-1≤ω(t)≤1}，h， uConstant vectors are adopted, and A, B, C and D are constant matrixes with preset dimensions;

the external reference input integral model comprises:

e(t)＝∫(ref-y(t))dt

where e (t) represents the error vector, ref represents the external reference input,

the target power system model includes:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e\left(t\right)=A_e{x}_e\left(t\right)+B_{e}u\left(t\right)+D_e\mathrm{\&omega;}\left(t\right)+F_{e}ref\\ y\left(t\right)=C_e{x}_e\left(t\right)\end{array}\right.$

wherein x is_e(t) represents the system state after the dimension expansion, x_e(t)＝[x^T(t)e^T(t)]^T∈Rⁿ。

Optionally, the constraint condition of the external reference input includes:

the external input reference satisfies Ω_ref＝{ref|ref^TQ_rref is less than or equal to 1 }; where ref represents the external reference input,

the invariant constraint condition comprises:

when the parameter η exists₁＞0，η₂＞0，η₃> 0, diagonal matrixQ_uSymmetric positive definite matrix W_cR, parameter λ, positive definite symmetry matrix W ∈ R^n×n，W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen the preset condition is met, the invariant set of the target power system model isThe corresponding reachable set is S_r＝{ref|ref^TQ_rref is less than or equal to 1}, andu(t)∈Ω_uwherein P ═ W^-1,K_e＝YP,Q_r＝W_r ^-1The preset conditions include:

$\begin{array}{c}\underset{W,Y,W_r}{\min }\mathrm{\&lambda;}\\ \left[\begin{array}{cc}{\underset{\&OverBar;}{u}}_i^2& Y_i\\ *& W\end{array}\right]\&le;0,i=1,...,m\end{array}$

$\left[\begin{array}{ccccc}\begin{array}{c}{\mathrm{WA}}_e^T+A_{e}W+Y^T{B}_e^T\\ +B_{e}Y+({\mathrm{\&eta;}}_1+{\mathrm{\&eta;}}_2)W\end{array}& F_e{W}_r& D_e& Y^T& W\\ *& -{\mathrm{\&eta;}}_1{W}_r& 0& 0& 0\\ *& *& -{\mathrm{\&eta;}}_2& 0& 0\\ *& *& *& -Q_u^{-1}& 0\\ *& *& *& *& -Q_{x_e}^{-1}\end{array}\right]<0$

[I_p0_p×(n-p)]^TW_r[I_p0_p×(n-p)]≤W

$\left[\begin{array}{cc}\mathrm{\&lambda;}R& I\\ *& W_r\end{array}\right]\&le;0$

W≤W₀

$\left[\begin{array}{cc}-{\mathrm{\&eta;}}_{3}W& W{\left[\begin{array}{cc}{h}_i& 0_{1\&times;(n-n_p)}\end{array}\right]}^T\\ *& {\mathrm{\&eta;}}_3-2{\stackrel{\&OverBar;}{x}}_i\end{array}\right]\&le;0,i=1,...,n_p$

λ＞0

wherein Y is_iRepresenting the ith column of the matrix Y, diagonal matrixQ_uIs a matrix of the parameters of the LQR controller,it can be ensuredIs an invariant set of the system (2), S_r＝{ref|ref^TQ_rref ≦ 1 is the corresponding reachable set andand u (t) ∈ Ω_uWherein P ═ W^-1,K_e＝YP,Q_r＝W_r ^-1。

According to the control device of the unmanned equipment, the external input reference is obtained, the external input reference is a preset planned path, the planned path is divided into at least one section of sub-path according to an inflection point in the planned path, then the invariant set and the corresponding reachable set of each section of sub-path are respectively obtained according to an initial power system model of the unmanned equipment and a preset external reference input integral model, and finally the actual path of the unmanned equipment is determined according to the intersection point of the reachable set of each section of sub-path and the section of sub-path. Therefore, by combining the preset planned path and the power system model of the unmanned equipment, the external environment constraint and the dynamics constraint of the unmanned equipment can be simultaneously met, and the condition that the unmanned equipment collides with the barrier can be avoided.

The preferred embodiments of the present disclosure are described in detail with reference to the accompanying drawings, however, the present disclosure is not limited to the specific details of the above embodiments, and various simple modifications may be made to the technical solution of the present disclosure within the technical idea of the present disclosure, and these simple modifications all belong to the protection scope of the present disclosure.

It should be noted that, in the foregoing embodiments, various features described in the above embodiments may be combined in any suitable manner, and in order to avoid unnecessary repetition, various combinations that are possible in the present disclosure are not described again.

In addition, any combination of various embodiments of the present disclosure may be made, and the same should be considered as the disclosure of the present disclosure, as long as it does not depart from the spirit of the present disclosure.

Claims (14)

1. A method of controlling an unmanned aerial device, the method comprising:

acquiring an external input reference, wherein the external input reference is a preset planning path;

dividing the planned path into at least one section of sub-path according to the inflection point in the planned path;

respectively acquiring an invariant set and a corresponding reachable set of each sub-path according to an initial power system model of the unmanned equipment and a preset external reference input integral model;

and determining the actual path of the unmanned equipment according to the intersection point of the reachable set of each section of sub-path and the section of sub-path.

2. The method of claim 1, wherein the obtaining the invariant set and the corresponding reachable set of each sub-path according to the initial power system model of the unmanned aerial vehicle and a preset external reference input integral model respectively comprises:

a. acquiring a target power system model of the unmanned equipment according to the initial power system model and a preset external reference input integral model;

b. generating a function model of a static state feedback controller according to the target power system model;

c. generating an invariant set S of the target power system model and an accessible set Sr corresponding to the invariant set S according to the target power system model and a function model of the static state feedback controller;

d. judging whether the end point of the current sub-path is contained in the reachable set Sr;

e. when the end point of the current sub-path is not included in the reachable set Sr, generating a new power system model having an intersection of the reachable set Sr and the current sub-path as a balance point, and performing steps a to e again as the initial power system model until the end point of the current sub-path is included in the currently generated reachable set Sr;

and c, executing the steps a to e to other sub paths in the at least one section of sub path to obtain an invariant set and a corresponding reachable set of each section of sub path.

3. The method of controlling an unmanned aerial device of claim 2, wherein the generating an invariant set S of the target power system model and a reachable set Sr corresponding to the invariant set S from the target power system model and the functional model of the static state feedback controller comprises:

taking a balance point of the initial power system model as a circle center, and acquiring the distance between the circle center and an obstacle closest to the circle center;

generating a circle tangent to the edge of the obstacle by taking the distance between the circle center and the obstacle closest to the circle center as a radius;

and generating an invariant set S and an reachable set Sr corresponding to the invariant set under the target power system model and the function model of the static state feedback controller in the circle by utilizing the preset constraint condition of the external reference input and the preset invariant set constraint condition.

4. The method of controlling an unmanned aerial vehicle of claim 3, wherein the generating an invariant set S of the target power system model and a reachable set Sr corresponding to the invariant set S according to the target power system model and the functional model of the static state feedback controller when a current initial power system model is not an initial power system model having a start point of the current sub-path as an equilibrium point further comprises:

and converting the invariant set and the reachable set corresponding to the invariant set into an invariant set and a reachable set in a coordinate system of the initial power system model taking the starting point of the current sub-path as the balance point according to the coordinate relationship between the balance point and the starting point of the current initial power system model.

5. The method according to claim 2, wherein the generating a new power system model having an intersection of the reachable set Sr with the current sub-path as a balance point when the end point of the current sub-path is not included in the reachable set Sr, comprises:

determining constraint conditions of system input of the new power system model according to the intersection point of the reachable set Sr and the current sub-path;

and determining the new power system model according to the constraint conditions of the system input of the new power system model.

6. The method of controlling an unmanned aerial device of claim 1, wherein the initial power system model comprises:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}\left(t\right)=Ax\left(t\right)+Bu\left(t\right)+D\mathrm{\&omega;}\left(t\right)\\ y\left(t\right)=Cx\left(t\right)\end{array}\right.$

wherein x represents the system state, u represents the control input, ω represents the external disturbance, y represents the system output, and Ω_u＝{u(t)|-u _i≤u_i(t)≤u _i1,. m } and Ω_ω＝{ω(t)|-1≤ω(t)≤1}，Denotes the first derivative of x (t), h, uconstant vectors are adopted, and A, B, C and D are constant matrixes with preset dimensions;

the external reference input integral model comprises:

e(t)＝∫(ref-y(t))dt

where e (t) represents the error vector, ref represents the external reference input,

the target power system model includes:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e\left(t\right)=A_e{x}_e\left(t\right)+B_{e}u\left(t\right)+D_e\mathrm{\&omega;}\left(t\right)+F_{e}ref\\ y\left(t\right)=C_e{x}_e\left(t\right)\end{array}\right.$

wherein A is_e，B_e，C_e，D_eRespectively, matrix after dimension expansion of A, B, C and D, F_eA constant matrix of a predetermined dimension, x_e(t) represents the system state after the dimension expansion, x_e(t)＝[x^T(t)e^T(t)]^T∈Rⁿ。

7. The control method of the unmanned aerial device of claim 3, wherein the constraint condition of the external reference input comprises:

the external input reference satisfies Ω_ref＝{ref|ref^TQ_rref is less than or equal to 1 }; where ref represents the external reference input,

the invariant constraint condition comprises:

when the parameter η exists₁＞0，η₂＞0，η₃> 0, diagonal matrixQ_uSymmetric positive definite matrix W_cR, parameter λ, positive definite symmetry matrix W ∈ R^n×n，W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen the preset condition is met, the invariant set of the target power system model isThe corresponding reachable set is S_r＝{ref|ref^TQ_rref is less than or equal to 1}, andu(t)∈Ω_uwherein P ═ W^-1，K_e＝YP，Q_r＝W_r ^-1The preset conditions include:

$\underset{W,Y,W_r}{min}\mathrm{\&lambda;}$

$\left[\begin{array}{cc}{\underset{\&OverBar;}{u}}_i^2& Y_i\\ *& W\end{array}\right]\&le;0,i=1,...,m$

$\left[\begin{array}{ccccc}\begin{array}{c}{\mathrm{WA}}_e^T+A_{e}W+Y^T{B}_e^T\\ +B_{e}Y+({\mathrm{\&eta;}}_1+{\mathrm{\&eta;}}_2)W\end{array}& F_e{W}_r& D_e& Y^T& W\\ *& -{\mathrm{\&eta;}}_1{W}_r& 0& 0& 0\\ *& *& -{\mathrm{\&eta;}}_2& 0& 0\\ *& *& *& -Q_u^{-1}& 0\\ *& *& *& *& -Q_{x_e}^{-1}\end{array}\right]<0$

[I_p0_p×(n-p)]^TW_r[I_p0_p×(n-p)]≤W

$\left[\begin{array}{cc}\mathrm{\&lambda;}R& I\\ *& W_r\end{array}\right]\&le;0$

W≤W₀

$\left[\begin{array}{cc}-{\mathrm{\&eta;}}_{3}W& W{\left[\begin{array}{cc}{h}_i& 0_{1\&times;(n-n_p)}\end{array}\right]}^T\\ *& {\mathrm{\&eta;}}_3-2\stackrel{\&OverBar;}{x_i}\end{array}\right]\&le;0,i=1,...,n_p$

λ＞0

wherein, Y_iRepresenting the ith column of the matrix Y, diagonal matrixQ_uIs a linear quadratic regulator LQR controller parameter matrix,thenIs an invariant set of the system (2), S_r＝{ref|ref^TQ_rref ≦ 1 is the corresponding reachable set andand u (t) ∈ Ω_uWherein A is_e，B_e，D_eRespectively are matrix A, B, D and F after dimension expansion_eIs a constant matrix of a predetermined dimension, P ═ W^-1，K_e＝YP，Q_r＝W_r ^-1。

8. An apparatus for controlling an unmanned aerial device, the apparatus comprising:

the system comprises an acquisition module, a planning module and a processing module, wherein the acquisition module is used for acquiring an external input reference, and the external input reference is a preset planning path;

the path dividing module is used for dividing the planned path into at least one section of sub-path according to an inflection point in the planned path;

the calculation module is used for respectively acquiring an invariant set and a corresponding reachable set of each section of sub-path according to an initial power system model of the unmanned equipment and a preset external reference input integral model;

and the path determining module is used for determining the actual path of the unmanned equipment according to the intersection point of the reachable set of each section of sub-path and the section of sub-path.

9. The control device of claim 8, wherein the calculation module comprises:

the first modeling submodule is used for executing a, acquiring a target power system model of the unmanned equipment according to the initial power system model and a preset external reference input integral model;

a second modeling submodule for executing b, generating a function model of a static state feedback controller according to the target power system model;

c, generating an invariant set S of the target power system model and a reachable set Sr corresponding to the invariant set S according to the target power system model and a function model of the static state feedback controller;

a judgment sub-module, configured to perform d, judge whether the end point of the current sub-path is included in the reachable set Sr;

when the end point of the current sub-path is not contained in the reachable set Sr, generating a new power system model taking the intersection point of the reachable set Sr and the current sub-path as a balance point, and performing the steps a to e again as the initial power system model until the end point of the current sub-path is contained in the currently generated reachable set Sr;

the first modeling submodule, the second modeling submodule, the calculating submodule, the judging submodule and the third modeling submodule are further used for respectively executing the steps a to e to other sub paths in the at least one section of sub path to obtain an invariant set and a corresponding reachable set of each section of sub path.

10. The control device of the unmanned aerial vehicle of claim 9, wherein the calculation submodule is configured to:

taking a balance point of the initial power system model as a circle center, and acquiring the distance between the circle center and an obstacle closest to the circle center;

generating a circle tangent to the edge of the obstacle by taking the distance between the circle center and the obstacle closest to the circle center as a radius;

and generating an invariant set S and an reachable set Sr corresponding to the invariant set under the target power system model and the function model of the static state feedback controller in the circle by utilizing the preset constraint condition of the external reference input and the preset invariant set constraint condition.

11. The control apparatus of claim 10, wherein the calculation sub-module is further configured to, when the current initial power system model is not an initial power system model having a start point of the current sub-path as a balance point:

and converting the invariant set and the reachable set corresponding to the invariant set into an invariant set and a reachable set in a coordinate system of the initial power system model taking the starting point of the current sub-path as the balance point according to the coordinate relationship between the balance point and the starting point of the current initial power system model.

12. The control device of the unmanned aerial vehicle of claim 9, wherein the third modeling submodule is configured to:

determining constraint conditions of system input of the new power system model according to the intersection point of the reachable set Sr and the current sub-path;

and determining the new power system model according to the constraint conditions of the system input of the new power system model.

13. The control apparatus of the unmanned aerial device of claim 8, wherein the initial power system model comprises:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}\left(t\right)=Ax\left(t\right)+Bu\left(t\right)+D\mathrm{\&omega;}\left(t\right)\\ y\left(t\right)=Cx\left(t\right)\end{array}\right.$

wherein x represents the system state, u represents the control input, ω represents the external disturbance, y represents the system output, and Ω_u＝{u(t)|-u _i≤u_i(t)≤u _i1,. m } and Ω_ω＝{ω(t)|-1≤ω(t)≤1}，h， uConstant vectors are adopted, and A, B, C and D are constant matrixes with preset dimensions;

the external reference input integral model comprises:

e(t)＝∫(ref-y(t))dt

where e (t) represents the error vector, ref represents the external reference input,

the target power system model includes:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e\left(t\right)=A_e{x}_e\left(t\right)+B_{e}u\left(t\right)+D_e\mathrm{\&omega;}\left(t\right)+F_{e}ref\\ y\left(t\right)=C_e{x}_e\left(t\right)\end{array}\right.$

wherein A is_e，B_e，C_e，D_eRespectively, matrix after dimension expansion of A, B, C and D, F_eA constant matrix of a predetermined dimension, x_e(t) represents the system state after the dimension expansion, x_e(t)＝[x^T(t)e^T(t)]^T∈Rⁿ。

14. The control apparatus of an unmanned aerial vehicle of claim 10, wherein the constraints of the external reference input comprise:

the external input reference satisfies Ω_ref＝{ref|ref^TQ_rref is less than or equal to 1 }; where ref represents the external reference input,

the invariant constraint condition comprises:

when the parameter η exists₁＞0，η₂＞0，η₃> 0, diagonal matrixQ_uSymmetric positive definite matrix W_cR, parameter λ, positive definite symmetry matrix W ∈ R^n×n，W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen the preset condition is met, the invariant set of the target power system model isThe corresponding reachable set is S_r＝{ref|ref^TQ_rref is less than or equal to 1}, andu(t)∈Ω_uwherein P ═ W^-1,K_e＝YP，Q_r＝W_r ^-1The preset conditions include:

$\underset{W,Y,W_r}{min}\mathrm{\&lambda;}$

$\left[\begin{array}{cc}{\underset{\&OverBar;}{u}}_i^2& Y_i\\ *& W\end{array}\right]\&le;0,i=1,...,m$

$\left[\begin{array}{ccccc}\begin{array}{c}{\mathrm{WA}}_e^T+A_{e}W+Y^T{B}_e^T\\ +B_{e}Y+({\mathrm{\&eta;}}_1+{\mathrm{\&eta;}}_2)W\end{array}& F_e{W}_r& D_e& Y^T& W\\ *& -{\mathrm{\&eta;}}_1{W}_r& 0& 0& 0\\ *& *& -{\mathrm{\&eta;}}_2& 0& 0\\ *& *& *& -Q_u^{-1}& 0\\ *& *& *& *& -Q_{x_e}^{-1}\end{array}\right]<0$

[I_p0_p×(n-p)]^TW_r[I_p0_p×(n-p)]≤W

$\left[\begin{array}{cc}\mathrm{\&lambda;}R& I\\ *& W_r\end{array}\right]\&le;0$

W≤W₀

$\left[\begin{array}{cc}-{\mathrm{\&eta;}}_{3}W& W{\left[\begin{array}{cc}{h}_i& 0_{1\&times;(n-n_p)}\end{array}\right]}^T\\ *& {\mathrm{\&eta;}}_3-2\stackrel{\&OverBar;}{x_i}\end{array}\right]\&le;0,i=1,...,n_p$

λ＞0

wherein, Y_iRepresenting the ith column of the matrix Y, diagonal matrixQ_uIs a linear quadratic regulator LQR controller parameter matrix,thenIs an invariant set of the system (2), S_r＝{ref|ref^TQ_rref ≦ 1 is the corresponding reachable set andand u (t) ∈ Ω_uWherein A is_e，B_e，D_eRespectively are matrix A, B, D and F after dimension expansion_eIs a constant matrix of a predetermined dimension, P ═ W^-1，K_e＝YP，Q_r＝W_r ^-1。