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

The control method of unmanned machine and device

Technical field

It relates to unmanned machine control field, particularly relate to control method and the device of a kind of unmanned machine.

Background technology

The unmanned machines such as unmanned plane, unmanned vehicle, unmanned boat, unmanned submersible have increasingly been widely used in military and civil field.In the face of different working environments, unmanned machine needs possess the ability guaranteeing that inherently safe runs and complete assigned tasks.But, due to the constraint of himself Dynamic Constraints and external environment condition, it is achieved this ability faces lot of challenges.

In current prior art, have been proposed for a series of paths planning method, such as based on the method for Artificial Potential Field, based on method theoretical for figure and random employing method etc., above-mentioned several based on paths planning method, a series of path point or the feasible path of external environment condition constraint can be met, by making unmanned machine follow the tracks of these path point or feasible path ensures that unmanned machine meets external environment condition constraint, for instance will not collide with barrier.But, due to the Dynamic Constraints of unmanned machine self, in reality unmanned machine cannot agonic tracking feasible path, thus there is the situation that unmanned machine and barrier collide.

Summary of the invention

The disclosure provides control method and the device of a kind of unmanned machine, for solving there is the problem that unmanned machine likely can collide with barrier in prior art.

To achieve these goals, the disclosure provides the control method of a kind of unmanned machine, and described method includes:

Obtaining externally input reference, described externally input is referenced as the path planning pre-set；

According to the flex point in described path planning, described path planning is divided at least one cross-talk path；

Initial driving force system model and default external reference input integral model according to unmanned machine obtain the invariant set in every cross-talk path and the reachable set of correspondence respectively；

The intersection point in the reachable set according to every cross-talk path and this cross-talk path determines the Actual path of unmanned machine.

Optionally, the described initial driving force system model according to unmanned machine and default external reference input integral model obtains every invariant set in cross-talk path respectively and the reachable set of correspondence includes:

A. the target power system model of described unmanned machine is obtained according to described initial driving force system model and default external reference input integral model；

B. the function model of static state-feedback controller is generated according to described target power system model；

C. according to described target power system model and the function model of described static state-feedback controller generate described target power system model invariant set S and with described reachable set Sr corresponding for invariant set S；

D. judge whether the terminal of described current subpath is comprised in described reachable set Sr；

E. when the terminal of described current subpath is not comprised in described reachable set Sr, generate the new dynamical system model being equilibrium point with the intersection point of described reachable set Sr Yu described current subpath, and again perform step a～e as described initial driving force system model, till the terminal of described current subpath is comprised in the reachable set Sr being currently generated；

Other subpaths in described at least one cross-talk path are performed step a～e, obtains the invariant set in every cross-talk path and the reachable set of correspondence.

Optionally, described generate the invariant set S of described target power system model according to described target power system model and the function model of described static state-feedback controller and include with described reachable set Sr corresponding for invariant set S:

With the equilibrium point of described initial driving force system model for the center of circle, obtain the distance in the described center of circle and the barrier nearest apart from the described center of circle；

The circle tangent with the edge of described barrier is generated with the distance of the barrier nearest apart from the described center of circle for radius with the described center of circle；

Utilize the constraints of the described external reference input preset, and the invariant set constraints preset generates described target power system model and invariant set S under the function model of described static state-feedback controller and the reachable set Sr corresponding with described invariant set in described circle.

Optionally, when current initial driving force system model is not with initial driving force system model that the starting point of described current subpath is equilibrium point, described generate the invariant set S of described target power system model according to described target power system model with the function model of described static state-feedback controller and also include with described reachable set Sr corresponding for invariant set S:

The coordinate relation of the equilibrium point according to described current initial driving force system model and described starting point, described invariant set and the reachable set corresponding with described invariant set are converted to the starting point of described current subpath be equilibrium point initial driving force system model coordinate system under invariant set and reachable set.

Optionally, described when the terminal of described current subpath is not comprised in described reachable set Sr, generate the new dynamical system model being equilibrium point with the intersection point of described reachable set Sr Yu described current subpath, including:

Intersection point according to described reachable set Sr Yu described current subpath determines the constraints of the system input of described new dynamical system model；

The constraints of the system input according to described new dynamical system model determines described new dynamical system model.

Optionally, described initial driving force 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 that system mode, u represent control input, and ω represents that external disturbance, y represent that system exports, and meets Ω_u=u (t) |-u _i≤u_i(t)≤u _i, i=1 ... m} and Ω_ω=ω (t) |-1≤ω (t)≤1}, h, uFor constant vector, A, B, C, D is the constant matrices presetting dimension；

Described external reference input integral model includes:

E (t)=∫ (ref-y (t)) dt

Wherein, e (t) represents that error vector, ref represent that external reference inputs,

Described 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_e, B_e, C_e, D_eRespectively A, B, C, D be augmented after matrix, F_eFor the constant matrices of default dimension, x_eT () represents the system mode after being augmented, x_e(t)=[x^T(t)e^T(t)]^T∈Rⁿ。

Optionally, the constraints of described external reference input includes:

Described externally input is with reference to meeting Ω_ref=ref | ref^TQ_rref≤1}；Wherein, ref represents that external reference inputs,

Described invariant set constraints includes:

When there is parameter η₁> 0, η₂> 0, η₃> 0, diagonal matrixQ_u, symmetric positive definite matrix W_c, R, parameter lambda, positive definite symmetric matrices W ∈ R^n×n, W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen meeting pre-conditioned, the invariant set of described target power system model isCorresponding reachable set is S_r=ref | ref^TQ_rRef≤1}, andu(t)∈Ω_u, wherein P=W^-1,K_e=YP, Q_r=W_r ^-1, described pre-conditioned 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_iI-th row of representing matrix Y, diagonal matrixQ_uFor linearquadratic regulator LQR controller parameter matrix,ThenIt is the invariant set of system (2), S_r=ref | ref^TQ_rRef≤1} be corresponding reachable set andAnd u (t) ∈ Ω_u, wherein, A_e, B_e, D_eRespectively A, B, D be augmented after matrix, A, B, D, F_eFor the constant matrices of default dimension, P=W^-1, K_e=YP, Q_r=W_r ^-1。

The disclosure also provides for the control device of a kind of unmanned machine, and described device includes:

Acquisition module, is used for obtaining externally input reference, and described externally input is referenced as the path planning pre-set；

Path divides module, for described path planning being divided at least one cross-talk path according to the flex point in described path planning；

Computing module, obtains the invariant set in every cross-talk path and the reachable set of correspondence respectively for initial driving force system model and the default external reference input integral model according to unmanned machine；

Path determination module, for determining the Actual path of unmanned machine according to the reachable set in every cross-talk path and the intersection point in this cross-talk path.

Optionally, described computing module includes:

First modeling submodule, obtains the target power system model of described unmanned machine for performing a. according to described initial driving force system model and default external reference input integral model；

Second modeling submodule, generates the function model of static state-feedback controller for performing b. according to described target power system model；

Calculating sub module, for perform c. according to described target power system model and the function model of described static state-feedback controller generate described target power system model invariant set S and with described reachable set Sr corresponding for invariant set S；

Judge submodule, judge whether the terminal of described current subpath is comprised in described reachable set Sr for performing d.；

3rd modeling submodule, for performing e. when the terminal of described current subpath is not comprised in described reachable set Sr, generate the new dynamical system model being equilibrium point with the intersection point of described reachable set Sr Yu described current subpath, and again perform step a～e as described initial driving force system model, till the terminal of described current subpath is comprised in the reachable set Sr being currently generated；

Described first modeling submodule, described second modeling submodule, described calculating sub module, described judgement submodule and described 3rd modeling submodule are additionally operable to other subpaths in described at least one cross-talk path are performed step a～e respectively, obtain the invariant set in every cross-talk path and the reachable set of correspondence.

Optionally, described calculating sub module is used for:

With the equilibrium point of described initial driving force system model for the center of circle, obtain the distance in the described center of circle and the barrier nearest apart from the described center of circle；

The circle tangent with the edge of described barrier is generated with the distance of the barrier nearest apart from the described center of circle for radius with the described center of circle；

Utilize the constraints of the described external reference input preset, and the invariant set constraints preset generates described target power system model and invariant set S under the function model of described static state-feedback controller and the reachable set Sr corresponding with described invariant set in described circle.

Optionally, when current initial driving force system model is not with initial driving force system model that the starting point of described current subpath is equilibrium point, described calculating sub module is additionally operable to:

The coordinate relation of the equilibrium point according to described current initial driving force system model and described starting point, described invariant set and the reachable set corresponding with described invariant set are converted to the starting point of described current subpath be equilibrium point initial driving force system model coordinate system under invariant set and reachable set.

Optionally, described 3rd modeling submodule is used for:

Intersection point according to described reachable set Sr Yu described current subpath determines the constraints of the system input of described new dynamical system model；

The constraints of the system input according to described new dynamical system model determines described new dynamical system model.

Optionally, described initial driving force 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 that system mode, u represent control input, and ω represents that external disturbance, y represent that system exports, and meets Ω_u=u (t) |-u _i≤u_i(t)≤u _i, i=1 ... m} and Ω_ω=ω (t) |-1≤ω (t)≤1}, h, uFor constant vector, A, B, C, D is the constant matrices presetting dimension；

Described external reference input integral model includes:

E (t)=∫ (ref-y (t)) dt

Wherein, e (t) represents that error vector, ref represent that external reference inputs,

Described 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_e, B_e, C_e, D_eRespectively A, B, C, D be augmented after matrix, F_eFor the constant matrices of default dimension, x_eT () represents the system mode after being augmented, x_e(t)=[x^T(t)e^T(t)]^T∈Rⁿ。

Optionally, the constraints of described external reference input includes:

Described externally input is with reference to meeting Ω_ref=ref | ref^TQ_rref≤1}；Wherein, ref represents that external reference inputs,

Described invariant set constraints includes:

When there is parameter η₁> 0, η₂> 0, η₃> 0, diagonal matrixQ_u, symmetric positive definite matrix W_c, R, parameter lambda, positive definite symmetric matrices W ∈ R^n×n, W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen meeting pre-conditioned, the invariant set of described target power system model isCorresponding reachable set is S_r=ref | ref^TQ_rRef≤1}, andu(t)∈Ω_u, wherein P=W^-1,K_e=YP, Q_r=W_r ^-1, described pre-conditioned 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_iI-th row of representing matrix Y, diagonal matrixQ_uFor linearquadratic regulator LQR controller parameter matrix,ThenIt is the invariant set of system (2), S_r=ref | ref^TQ_rRef≤1} be corresponding reachable set andAnd u (t) ∈ Ω_u, wherein, A_e, B_e, D_eRespectively A, B, D be augmented after matrix, A, B, D, F_eFor the constant matrices of default dimension, P=W^-1, K_e=YP, Q_r=W_r ^-1。

The control method of the unmanned machine that the disclosure provides and device, by obtaining externally input reference, described externally input is referenced as the path planning pre-set, and according to the flex point in described path planning, described path planning is divided at least one cross-talk path, initial driving force system model and default external reference input integral model further according to unmanned machine obtain the invariant set in every cross-talk path and the reachable set of correspondence respectively, determine the Actual path of unmanned machine finally according to the reachable set in every cross-talk path and the intersection point in this cross-talk path.The visible dynamical system model by combining path planning and the unmanned machine pre-set, it is possible to simultaneously meet the Dynamic Constraints of external environment condition constraint and unmanned machine such that it is able to avoid the situation that unmanned machine and barrier collide.

Other feature and advantage of the disclosure will be described in detail in detailed description of the invention part subsequently.

Accompanying drawing explanation

Accompanying drawing is used to provide further understanding of the disclosure, and constitutes the part of description, is used for explaining the disclosure, but is not intended that restriction of this disclosure together with detailed description below.In the accompanying drawings:

Fig. 1 is the schematic flow sheet of the control method of a kind of unmanned machine that the disclosure one embodiment provides；

Fig. 2 is the schematic flow sheet of the control method of a kind of unmanned machine that another embodiment of the disclosure provides；

Fig. 3 is the relation schematic diagram of the invariant set shown in embodiment illustrated in fig. 2 and reachable set；

Fig. 4 a is a kind of schematic diagram calculating invariant set and reachable set shown in embodiment illustrated in fig. 2；

Fig. 4 b is a kind of schematic diagram calculating invariant set and reachable set shown in embodiment illustrated in fig. 2；

Fig. 4 c is the schematic diagram of the Actual path shown in embodiment illustrated in fig. 2；

Fig. 5 a is the schematic diagram of the path planning shown in embodiment illustrated in fig. 2；

Fig. 5 b is the schematic diagram of the reachable set in the path planning shown in embodiment illustrated in fig. 2；

Fig. 5 c is the schematic diagram of the Actual path obtained based on path planning shown in embodiment illustrated in fig. 2；

Fig. 6 is the block diagram controlling device of a kind of unmanned machine that disclosure embodiment provides；

Fig. 7 is the block diagram of a kind of computing module that embodiment illustrated in fig. 6 provides.

Detailed description of the invention

It is described in detail below in conjunction with accompanying drawing detailed description of the invention of this disclosure.It should be appreciated that detailed description of the invention described herein is merely to illustrate and explains the disclosure, it is not limited to the disclosure.

Fig. 1 is the schematic flow sheet of the control method of a kind of unmanned machine that the disclosure one embodiment provides, and referring to Fig. 1, the control method of this unmanned machine may comprise steps of:

Step 101, obtains externally input reference, and described externally input is referenced as the path planning pre-set.

Step 102, is divided at least one cross-talk path according to the flex point in described path planning by described path planning.

Step 103, obtains the invariant set in every cross-talk path and the reachable set of correspondence respectively according to the initial driving force system model of unmanned machine and default external reference input integral model.

Step 104, determines the Actual path of unmanned machine according to the intersection point of the reachable set in every cross-talk path Yu this cross-talk path.

Wherein, externally input is referenced as the path planning planned in advance, this path planning can include some sections of forthrights (or approximate forthright), therefore at least one cross-talk path can be divided into (wherein according to flex point (can be understood as the adjacent junction point moving towards different Liang Duan road) this path planning in path planning, if an only cross-talk path, this subpath is exactly this path planning itself, such as, path planning is absent from the situation of flex point).

Unmanned machine involved in each embodiment of the disclosure can be the unmanned machines such as unmanned plane, unmanned vehicle, unmanned boat, unmanned submersible, the initial driving force system model introducing described unmanned machine is based on the consideration to the constraint of unmanned machine own dynamics, illustratively, the initial driving force system model of this unmanned machine may include that

$\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)$

Hereinafter above-mentioned formula (1) being called system (1), wherein, x represents that system mode, u represent control input, and ω represents that external disturbance, y represent that system exports, and meets Ω_u=u (t) |-u _i≤u_i(t)≤u _i, i=1 ... m} and Ω_ω=ω (t) |-1≤ω (t)≤1},The first derivative of expression x (t), h, uFor constant vector, A, B, C, D is the constant matrices presetting dimension.

Introduce described default external reference input integral model, be that illustratively, this external reference input integral model can include following integral device in order to realize the tracking to outside reference input:

E (t)=∫ (ref-y (t)) dt

Wherein, e (t) represents that error vector, ref represent that external reference inputs,

According to external reference input integral model, system (1) is augmented, and defines new state vector x_e(t)=[x^T(t)e^T(t)]^T∈Rⁿ, then can receive a fresh impetus system model, and we can be become target power system model, and this target power system model may include that

$\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)$

Hereinafter above-mentioned formula (2) is called system (2), wherein, A_e, B_e, C_e, D_eRespectively A, B, C, D be augmented after matrix, F_eFor the constant matrices of default dimension, x_eT () represents the system mode after being augmented.

The function model of following static state-feedback controller is may determine that based on system (2):

U (t)=K_ex_e(t)(3)

Wherein, K_eFor feedback matrix to be designed.Hereinafter formula (3) is called static state-feedback controller (3).

System (2) and static state-feedback controller (3) may be used for calculating invariant set and reachable set.

The related definition of invariant set and reachable set is given below:

Invariant set: if having x (t) ∈ S for all of x (0) ∈ S, then gatherIt it is the invariant set of system (1).Especially, as t > 0, if above-mentioned condition is set up, then S is just constant.

Robust controllable invariant set: if x is ∈ Ω_x, u ∈ Ω_u, ω ∈ Ω_ωAnd one feedback controller u (t)=Kx (t) of existence makes S be just constant for this closed loop system, thenBeing the robust controllable invariant set of system (1), wherein K is feedback matrix to be designed.

It addition, set the function that V > 0 is x (t), ifThen geometry S_ρ=x (t) | and V (x (t))≤ρ } it is just constant, wherein ρ is arithmetic number.

Reachable set: the reachable set S of system (2)_rIt is 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 the schematic flow sheet of the control method of a kind of unmanned machine that another embodiment of the disclosure provides, referring to Fig. 2, the initial driving force system model according to unmanned machine described in above-mentioned steps 103 and default external reference input integral model obtains every invariant set in cross-talk path respectively and the reachable set of correspondence may comprise steps of:

Step 1031, obtains the target power system model of described unmanned machine according to described initial driving force system model and default external reference input integral model.

Wherein, described initial driving force system model is with the starting point of current subpath for equilibrium point.

Assume that current subpath is path L₀L₁, wherein, path L₀L₁Starting point be L₀, terminal is L₁.Described initial driving force system model is with the starting point L of current subpath₀For the coordinate system at equilibrium point and described initial driving force system model place with starting point L₀Initial point for coordinate system.Due to the method that foregoing is directed the target power system model obtaining described unmanned machine according to described initial driving force system model and default external reference input integral model, therefore it is not discussed here.

Step 1032, generates the function model of static state-feedback controller according to described target power system model.

Due to the method that foregoing is directed the function model generating static state-feedback controller according to described target power system model, repeat no more herein.

Step 1033, according to described target power system model and the function model of described static state-feedback controller generate described target power system model invariant set S and with described reachable set Sr corresponding for invariant set S.

First, with the equilibrium point of described initial driving force system model for the center of circle, obtain the distance in the described center of circle and the barrier nearest apart from the described center of circle；

Secondly, with the edge of described barrier tangent circle is generated with the distance of the barrier nearest apart from the described center of circle for radius with the described center of circle；

Again, utilize the constraints of the described external reference input preset, and the invariant set constraints preset generate in described circle described target power system model and invariant set S under the function model of described static state-feedback controller and with described reachable set Sr corresponding for invariant set S.

Illustratively, Fig. 3 is the relation schematic diagram of the invariant set shown in embodiment illustrated in fig. 2 and reachable set, referring to Fig. 3, with path L₀L₁For example, it is assumed that the equilibrium point of current initial driving force system model is path L₀L₁Starting point L₀, then with starting point L₀For the center of circle, acquisition starting point L₀With distance starting point L₀The distance d of nearest barrier；Then, with starting point L₀For the center of circle, distance d is radius one round S of structure_c, then this circle S_cTangent with the edge of this closest barrier, thus at this circle S_cInterior generation invariant set and reachable set can ensure that unmanned machine and barrier are not desired to hand over.

The calculating process of invariant set and reachable set is described below:

Illustratively, it is contemplated that system (2) and static state-feedback controller (3), the constraints of above-mentioned described external reference input may include that described externally input is with reference to meeting Ω_ref=ref | ref^TQ_rref≤1}；Wherein, ref represents that external reference inputs,And Ellipsoid invariant set can be adopted, namelyThen can be calculated the reachable set Sr of invariant set S and correspondence by the invariant set constraints of the following stated, this invariant set constraints may include that

When there is parameter η₁> 0, η₂> 0, η₃> 0, diagonal matrixQ_u, symmetric positive definite matrix W_c, R, parameter lambda, positive definite symmetric matrices W ∈ R^n×n, W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen meeting pre-conditioned, the invariant set of described target power system model isCorresponding reachable set is S_r=ref | ref^TQ_rRef≤1}, andu(t)∈Ω_u, wherein P=W^-1,K_e=YP, Q_r=W_r ^-1, described pre-conditioned 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_iI-th row of representing matrix Y, diagonal matrixQ_uFor LQR (LinearQuadraticRegulator, linearquadratic regulator) controller parameter matrix,.Then can ensure thatIt is the invariant set of system (2), S_r=ref | ref^TQ_rRef≤1} be corresponding reachable set andAnd u (t) ∈ Ω_u, wherein, A_e, B_e, D_eRespectively A, B, D be augmented after matrix, A, B, D, F_eFor the constant matrices of default dimension, P=W^-1,K_e=YP, Q_r=W_r ^-1。

Thus, can be calculated by said method with starting point L₀For the invariant set S and reachable set Sr of the system (2) of equilibrium point, illustratively, the relation of reachable set Sr and invariant set S can be as shown in Figure 3.

Step 1034, it is judged that whether the terminal of described current subpath is comprised in described reachable set Sr.

Step 1035, when the terminal of described current subpath is not comprised in described reachable set Sr, generate the new dynamical system model being equilibrium point with the intersection point of described reachable set Sr Yu described current subpath, and again perform step 1031～1035 as described initial driving force system model, till the terminal of described current subpath is comprised in the reachable set Sr being currently generated.

Invariant set S and reachable set Sr owing to calculating in step 1034 are with starting point L₀For equilibrium point system (2) invariant set and and reachable set, therefore, this invariant set S and this reachable set Sr can be designated as invariant set S below₀With reachable set Sr₀。

Illustratively, Fig. 4 a is a kind of schematic diagram calculating invariant set and reachable set shown in embodiment illustrated in fig. 2, referring to Fig. 4 a, path L₀L₁Terminal L₁It is not included in reachable set Sr₀In, then can by reachable set Sr₀With path L₀L₁Intersection point c₁As new equilibrium point, generate with c₁The new dynamical system model of equilibrium point.

It is possible, firstly, to according to described reachable set Sr₀With described current subpath L₀L₁Intersection point c₁Determine the constraints of the system input of described new dynamical system model, illustratively, consider to differ only in equilibrium point different (coordinate system relative to original system) between new system and original system, therefore the method adopting the constraint controlling input of change system, the equilibrium point of mobile system, this constraints may include that

$\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⁺The pseudoinverse of representing matrix B, is rewritten as-β≤u (t)≤α by the constraints controlling input of this new dynamical system model, then is converted into 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) represents constant term relevant with the time in u (t) and continuous item respectively, meanwhile, defines new state variableThen this new dynamical system model can be expressed 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)$

Hereinafter above-mentioned formula (4) is called system (4), wherein, owing to the equilibrium point of system (4) is x_c1(it is relative to system (1) coordinate system), and be initial point relative to himself coordinate system, aforesaid method therefore can be utilized under system (4) local Coordinate System to calculate invariant set and reachable set, be designated as respectivelyWithAnd by invariant setAnd reachable setMove to system (1) coordinate system, it is possible to the invariant set after moving to system (1) coordinate system and reachable set are designated as invariant set S respectively₁With reachable set Sr₁。

Then, continue to judge the terminal L when described current subpath₁Whether it is comprised in described reachable set Sr₁In, if the terminal of current subpath is comprised in described reachable set Sr₁In, then current subpath L₀L₁The flow process calculating invariant set and reachable set just finish.If the terminal of current subpath is not comprised in described reachable set Sr₁In, then by reachable set Sr₁With path L₀L₁Intersection point c₂As new equilibrium point, generate with c₂The new dynamical system model of equilibrium point, and repeat said method and calculate invariant set S₂With reachable set Sr₂, and so on, until calculating reachable set Sr_nSo that this reachable set Sr_nPath L can be comprised₀L₁Terminal L₁Till.

The intersection point in the reachable set according to every cross-talk path described in above-mentioned steps 104 and this cross-talk path determines that the step of the Actual path of unmanned machine includes:

Other paths in multiple objective programming path are performed above-mentioned step 1031～1035 too, thus obtaining the invariant set in every cross-talk path and the reachable set of correspondence, then determine the Actual path of unmanned machine according to the intersection point of the reachable set according to every cross-talk path Yu this cross-talk path.

Illustratively, Fig. 4 b is a kind of schematic diagram calculating invariant set and reachable set shown in embodiment illustrated in fig. 2, includes invariant set S referring to Fig. 4 b, figure₁With reachable set Sr₁, path L₀L₁Terminal L₁It is included in reachable set Sr₁In, then according to reachable set Sr₁With L₀L₁Intersection point and path L₀L₁Starting point L₀Determine final Actual path.Illustratively, this Actual path can be as illustrated in fig. 4 c.

In like manner, the final actual path of whole path planning can be drawn, as illustrated in figs. 5 a-5 c, Fig. 5 a is the schematic diagram of the path planning shown in embodiment illustrated in fig. 2, Fig. 5 b is the schematic diagram of the reachable set in the path planning shown in embodiment illustrated in fig. 2, and Fig. 5 c is the schematic diagram of the Actual path obtained based on path planning shown in embodiment illustrated in fig. 2.

The control method of the unmanned machine that the disclosure provides, by obtaining externally input reference, described externally input is referenced as the path planning pre-set, and according to the flex point in described path planning, described path planning is divided at least one cross-talk path, initial driving force system model and default external reference input integral model further according to unmanned machine obtain the invariant set in every cross-talk path and the reachable set of correspondence respectively, determine the Actual path of unmanned machine finally according to the reachable set in every cross-talk path and the intersection point in this cross-talk path.The visible dynamical system model by combining path planning and the unmanned machine pre-set, it is possible to simultaneously meet the Dynamic Constraints of external environment condition constraint and unmanned machine such that it is able to avoid the situation that unmanned machine and barrier collide.

Fig. 6 is the block diagram controlling device of a kind of unmanned machine that disclosure embodiment provides, and referring to Fig. 6, the control device 600 of this unmanned machine includes:

Acquisition module 610, is used for obtaining externally input reference, and described externally input is referenced as the path planning pre-set；

Path divides module 620, for described path planning being divided at least one cross-talk path according to the flex point in described path planning；

Computing module 630, obtains the invariant set in every cross-talk path and the reachable set of correspondence respectively for initial driving force system model and the default external reference input integral model according to unmanned machine；

Path determination module 640, for determining the Actual path of unmanned machine according to the reachable set in every cross-talk path and the intersection point in this cross-talk path.

Optionally, Fig. 7 is the block diagram of a kind of computing module that embodiment illustrated in fig. 6 provides, and referring to Fig. 7, described computing module 630 includes:

First modeling submodule 631, obtain the target power system model of described unmanned machine according to described initial driving force system model and default external reference input integral model for performing a., described initial driving force system model is with the starting point of current subpath for equilibrium point, and described initial driving force system model is with the starting point of current subpath for equilibrium point；

Second modeling submodule 632, generates the function model of static state-feedback controller for performing b. according to described target power system model；

Calculating sub module 633, for perform c. according to described target power system model and the function model of described static state-feedback controller generate described target power system model invariant set S and with described reachable set Sr corresponding for invariant set S；

Judge submodule 634, judge whether the terminal of described current subpath is comprised in described reachable set Sr for performing d.；

3rd modeling submodule 635, for performing e. when the terminal of described current subpath is not comprised in described reachable set Sr, generate the new dynamical system model being equilibrium point with the intersection point of described reachable set Sr Yu described current subpath, and again perform step a～e as described initial driving force system model, till the terminal of described current subpath is comprised in the reachable set Sr being currently generated；

Described first modeling submodule 631, described second modeling submodule 632, described calculating sub module 633, described judgement submodule 634 and described 3rd modeling submodule 635 are additionally operable to other subpaths in described at least one cross-talk path are performed step a～e respectively, obtain the invariant set in every cross-talk path and the reachable set of correspondence.

Optionally, described calculating sub module 633 is used for:

With the equilibrium point of described initial driving force system model for the center of circle, obtain the distance in the described center of circle and the barrier nearest apart from the described center of circle；

The circle tangent with the edge of described barrier is generated with the distance of the barrier nearest apart from the described center of circle for radius with the described center of circle；

Utilize the constraints of the described external reference input preset, and the invariant set constraints preset generates described target power system model and invariant set S under the function model of described static state-feedback controller and the reachable set Sr corresponding with described invariant set in described circle.

Optionally, when current initial driving force system model is not with initial driving force system model that the starting point of described current subpath is equilibrium point, described calculating sub module 633 is additionally operable to:

The coordinate relation of the equilibrium point according to described current initial driving force system model and described starting point, described invariant set and the reachable set corresponding with described invariant set are converted to the starting point of described current subpath be equilibrium point initial driving force system model coordinate system under invariant set and reachable set.

Optionally, described 3rd modeling submodule 635 is used for:

Intersection point according to described reachable set Sr Yu described current subpath determines the constraints of the system input of described new dynamical system model；

The constraints of the system input according to described new dynamical system model determines described new dynamical system model.

Optionally, described initial driving force 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 that system mode, u represent control input, and ω represents that external disturbance, y represent that system exports, and meets Ω_u=u (t) |-u _i≤u_i(t)≤u _i, i=1 ... m} and Ω_ω=ω (t) |-1≤ω (t)≤1}, h, uFor constant vector, A, B, C, D is the constant matrices presetting dimension；

Described external reference input integral model includes:

E (t)=∫ (ref-y (t)) dt

Wherein, e (t) represents that error vector, ref represent that external reference inputs,

Described 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_eT () represents the system mode after being augmented, x_e(t)=[x^T(t)e^T(t)]^T∈Rⁿ。

Optionally, the constraints of described external reference input includes:

Described externally input is with reference to meeting Ω_ref=ref | ref^TQ_rref≤1}；Wherein, ref represents that external reference inputs,

Described invariant set constraints includes:

When there is parameter η₁> 0, η₂> 0, η₃> 0, diagonal matrixQ_u, symmetric positive definite matrix W_c, R, parameter lambda, positive definite symmetric matrices W ∈ R^n×n, W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen meeting pre-conditioned, the invariant set of described target power system model isCorresponding reachable set is S_r=ref | ref^TQ_rRef≤1}, andu(t)∈Ω_u, wherein P=W^-1,K_e=YP, Q_r=W_r ^-1, described pre-conditioned 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_iI-th row of representing matrix Y, diagonal matrixQ_uFor LQR controller parameter matrix,Then can ensure thatIt is the invariant set of system (2), S_r=ref | ref^TQ_rRef≤1} be corresponding reachable set andAnd u (t) ∈ Ω_u, wherein P=W^-1,K_e=YP, Q_r=W_r ^-1。

The control device of the unmanned machine that the disclosure provides, by obtaining externally input reference, described externally input is referenced as the path planning pre-set, and according to the flex point in described path planning, described path planning is divided at least one cross-talk path, initial driving force system model and default external reference input integral model further according to unmanned machine obtain the invariant set in every cross-talk path and the reachable set of correspondence respectively, determine the Actual path of unmanned machine finally according to the reachable set in every cross-talk path and the intersection point in this cross-talk path.The visible dynamical system model by combining path planning and the unmanned machine pre-set, it is possible to simultaneously meet the Dynamic Constraints of external environment condition constraint and unmanned machine such that it is able to avoid the situation that unmanned machine and barrier collide.

The preferred implementation of the disclosure is described in detail above in association with accompanying drawing; but; the disclosure is not limited to the detail in above-mentioned embodiment; in the technology concept of the disclosure; technical scheme of this disclosure can carrying out multiple simple variant, these simple variant belong to the protection domain of the disclosure.

It is further to note that, each concrete technical characteristic described in above-mentioned detailed description of the invention, in reconcilable situation, it is possible to be combined by any suitable mode, in order to avoid unnecessary repetition, various possible compound modes are no longer illustrated by the disclosure separately.

Additionally, can also carry out combination in any between the various different embodiment of the disclosure, as long as it is without prejudice to the thought of the disclosure, it should be considered as disclosure disclosure of that equally.

Claims (14)

1. the control method of a unmanned machine, it is characterised in that described method includes:

Obtaining externally input reference, described externally input is referenced as the path planning pre-set；

According to the flex point in described path planning, described path planning is divided at least one cross-talk path；

Initial driving force system model and default external reference input integral model according to unmanned machine obtain the invariant set in every cross-talk path and the reachable set of correspondence respectively；

The intersection point in the reachable set according to every cross-talk path and this cross-talk path determines the Actual path of unmanned machine.

2. the control method of unmanned machine according to claim 1, it is characterized in that, the described initial driving force system model according to unmanned machine and default external reference input integral model obtains every invariant set in cross-talk path respectively and the reachable set of correspondence includes:

A. the target power system model of described unmanned machine is obtained according to described initial driving force system model and default external reference input integral model；

B. the function model of static state-feedback controller is generated according to described target power system model；

C. according to described target power system model and the function model of described static state-feedback controller generate described target power system model invariant set S and with described reachable set Sr corresponding for invariant set S；

D. judge whether the terminal of described current subpath is comprised in described reachable set Sr；

E. when the terminal of described current subpath is not comprised in described reachable set Sr, generate the new dynamical system model being equilibrium point with the intersection point of described reachable set Sr Yu described current subpath, and again perform step a～e as described initial driving force system model, till the terminal of described current subpath is comprised in the reachable set Sr being currently generated；

Other subpaths in described at least one cross-talk path are performed step a～e, obtains the invariant set in every cross-talk path and the reachable set of correspondence.

3. the control method of unmanned machine according to claim 2, it is characterized in that, described generate the invariant set S of described target power system model according to described target power system model and the function model of described static state-feedback controller and include with described reachable set Sr corresponding for invariant set S:

With the equilibrium point of described initial driving force system model for the center of circle, obtain the distance in the described center of circle and the barrier nearest apart from the described center of circle；

The circle tangent with the edge of described barrier is generated with the distance of the barrier nearest apart from the described center of circle for radius with the described center of circle；

Utilize the constraints of the described external reference input preset, and the invariant set constraints preset generates described target power system model and invariant set S under the function model of described static state-feedback controller and the reachable set Sr corresponding with described invariant set in described circle.

4. the control method of unmanned machine according to claim 3, it is characterized in that, when current initial driving force system model is not with initial driving force system model that the starting point of described current subpath is equilibrium point, described generate the invariant set S of described target power system model according to described target power system model with the function model of described static state-feedback controller and also include with described reachable set Sr corresponding for invariant set S:

The coordinate relation of the equilibrium point according to described current initial driving force system model and described starting point, described invariant set and the reachable set corresponding with described invariant set are converted to the starting point of described current subpath be equilibrium point initial driving force system model coordinate system under invariant set and reachable set.

5. the control method of unmanned machine according to claim 2, it is characterized in that, described when the terminal of described current subpath is not comprised in described reachable set Sr, generate the new dynamical system model being equilibrium point with the intersection point of described reachable set Sr Yu described current subpath, including:

Intersection point according to described reachable set Sr Yu described current subpath determines the constraints of the system input of described new dynamical system model；

The constraints of the system input according to described new dynamical system model determines described new dynamical system model.

6. the control method of unmanned machine according to claim 1, it is characterised in that described initial driving force 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 that system mode, u represent control input, and ω represents that external disturbance, y represent that system exports, and meets Ω_u=u (t) |-u _i≤u_i(t)≤u _i, i=1 ... m} and Ω_ω=ω (t) |-1≤ω (t)≤1},The first derivative of expression x (t), h, uFor constant vector, A, B, C, D is the constant matrices presetting dimension；

Described external reference input integral model includes:

E (t)=∫ (ref-y (t)) dt

Wherein, e (t) represents that error vector, ref represent that external reference inputs,

Described 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_e, B_e, C_e, D_eRespectively A, B, C, D be augmented after matrix, F_eFor the constant matrices of default dimension, x_eT () represents the system mode after being augmented, x_e(t)=[x^T(t)e^T(t)]^T∈Rⁿ。

7. the control method of unmanned machine according to claim 3, it is characterised in that the constraints of described external reference input includes:

Described externally input is with reference to meeting Ω_ref=ref | ref^TQ_rref≤1}；Wherein, ref represents that external reference inputs,

Described invariant set constraints includes:

When there is parameter η₁> 0, η₂> 0, η₃> 0, diagonal matrixQ_u, symmetric positive definite matrix W_c, R, parameter lambda, positive definite symmetric matrices W ∈ R^n×n, W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen meeting pre-conditioned, the invariant set of described target power system model isCorresponding reachable set is S_r=ref | ref^TQ_rRef≤1}, andu(t)∈Ω_u, wherein P=W^-1, K_e=YP, Q_r=W_r ^-1, described pre-conditioned 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_iI-th row of representing matrix Y, diagonal matrixQ_uFor linearquadratic regulator LQR controller parameter matrix,ThenIt is the invariant set of system (2), S_r=ref | ref^TQ_rRef≤1} be corresponding reachable set andAnd u (t) ∈ Ω_u, wherein, A_e, B_e, D_eRespectively A, B, D be augmented after matrix, A, B, D, F_eFor the constant matrices of default dimension, P=W^-1, K_e=YP, Q_r=W_r ^-1。

8. the control device of a unmanned machine, it is characterised in that described device includes:

Acquisition module, is used for obtaining externally input reference, and described externally input is referenced as the path planning pre-set；

Path divides module, for described path planning being divided at least one cross-talk path according to the flex point in described path planning；

Computing module, obtains the invariant set in every cross-talk path and the reachable set of correspondence respectively for initial driving force system model and the default external reference input integral model according to unmanned machine；

Path determination module, for determining the Actual path of unmanned machine according to the reachable set in every cross-talk path and the intersection point in this cross-talk path.

9. control device according to claim 8, it is characterised in that described computing module includes:

First modeling submodule, obtains the target power system model of described unmanned machine for performing a. according to described initial driving force system model and default external reference input integral model；

Second modeling submodule, generates the function model of static state-feedback controller for performing b. according to described target power system model；

Calculating sub module, for perform c. according to described target power system model and the function model of described static state-feedback controller generate described target power system model invariant set S and with described reachable set Sr corresponding for invariant set S；

Judge submodule, judge whether the terminal of described current subpath is comprised in described reachable set Sr for performing d.；

3rd modeling submodule, for performing e. when the terminal of described current subpath is not comprised in described reachable set Sr, generate the new dynamical system model being equilibrium point with the intersection point of described reachable set Sr Yu described current subpath, and again perform step a～e as described initial driving force system model, till the terminal of described current subpath is comprised in the reachable set Sr being currently generated；

Described first modeling submodule, described second modeling submodule, described calculating sub module, described judgement submodule and described 3rd modeling submodule are additionally operable to other subpaths in described at least one cross-talk path are performed step a～e respectively, obtain the invariant set in every cross-talk path and the reachable set of correspondence.

10. the control device of unmanned machine according to claim 9, it is characterised in that described calculating sub module is used for:

With the equilibrium point of described initial driving force system model for the center of circle, obtain the distance in the described center of circle and the barrier nearest apart from the described center of circle；

The circle tangent with the edge of described barrier is generated with the distance of the barrier nearest apart from the described center of circle for radius with the described center of circle；

Utilize the constraints of the described external reference input preset, and the invariant set constraints preset generates described target power system model and invariant set S under the function model of described static state-feedback controller and the reachable set Sr corresponding with described invariant set in described circle.

11. the control device of unmanned machine according to claim 10, it is characterised in that when current initial driving force system model is not with initial driving force system model that the starting point of described current subpath is equilibrium point, described calculating sub module is additionally operable to:

The coordinate relation of the equilibrium point according to described current initial driving force system model and described starting point, described invariant set and the reachable set corresponding with described invariant set are converted to the starting point of described current subpath be equilibrium point initial driving force system model coordinate system under invariant set and reachable set.

12. the control device of unmanned machine according to claim 9, it is characterised in that described 3rd modeling submodule is used for:

Intersection point according to described reachable set Sr Yu described current subpath determines the constraints of the system input of described new dynamical system model；

The constraints of the system input according to described new dynamical system model determines described new dynamical system model.

13. the control device of unmanned machine according to claim 8, it is characterised in that described initial driving force 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 that system mode, u represent control input, and ω represents that external disturbance, y represent that system exports, and meets Ω_u=u (t) |-u _i≤u_i(t)≤u _i, i=1 ... m} and Ω_ω=ω (t) |-1≤ω (t)≤1}, h, uFor constant vector, A, B, C, D is the constant matrices presetting dimension；

Described external reference input integral model includes:

E (t)=∫ (ref-y (t)) dt

Wherein, e (t) represents that error vector, ref represent that external reference inputs,

Described 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_e, B_e, C_e, D_eRespectively A, B, C, D be augmented after matrix, F_eFor the constant matrices of default dimension, x_eT () represents the system mode after being augmented, x_e(t)=[x^T(t)e^T(t)]^T∈Rⁿ。

14. the control device of unmanned machine according to claim 10, it is characterised in that the constraints of described external reference input includes:

Described externally input is with reference to meeting Ω_ref=ref | ref^TQ_rref≤1}；Wherein, ref represents that external reference inputs,

Described invariant set constraints includes:

When there is parameter η₁> 0, η₂> 0, η₃> 0, diagonal matrixQ_u, symmetric positive definite matrix W_c, R, parameter lambda, positive definite symmetric matrices W ∈ R^n×n, W_r∈R^p×pAnd matrix Y ∈ R^m×nWhen meeting pre-conditioned, the invariant set of described target power system model isCorresponding reachable set is S_r=ref | ref^TQ_rRef≤1}, andu(t)∈Ω_u, wherein P=W^-1,K_e=YP, Q_r=W_r ^-1, described pre-conditioned 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_iI-th row of representing matrix Y, diagonal matrixQ_uFor linearquadratic regulator LQR controller parameter matrix,ThenIt is the invariant set of system (2), S_r=ref | ref^TQ_rRef≤1} be corresponding reachable set andAnd u (t) ∈ Ω_u, wherein, A_e, B_e, D_eRespectively A, B, D be augmented after matrix, A, B, D, F_eFor the constant matrices of default dimension, P=W^-1, K_e=YP, Q_r=W_r ^-1。