CN103616821A - Design method of robust controller for vehicle with six degrees of freedom - Google Patents

Abstract

The invention designs a robust tracking controller for an under-actuated underwater vehicle with six degrees of freedom. Considering that under-actuation can not realize the property of asymptotic stability, a method of enabling errors to tend to be minimized is adopted to solve the selection of a Lyapunov function. A backstepping method and a function mapping technology are adopted for designing the robust controller, and the controller can start in any initial state (including the state with the initial speed of zero of course changing), automatically trace any smooth track and realize a relatively good self-adaptive tracking effect under the situation that the vehicle is subject to environmental disturbances.

Description

A kind of method for designing of robust controller of six degree of freedom aircraft

Technical field

The invention belongs to robot and control control technology field, relate to the method for designing that a kind of six degree of freedom owes to drive the robust contrail tracker of submarine navigation device.

Background technology

Owe drive system and refer to that independent variable number is less than the nonlinear system of degree of freedom in system number, it need to complete by less driver complicated motion control task; In most cases Underactuated Mechanical Systems is Nonholonomic Constraints Systems, and showing again general smooth state feedback control simultaneously cannot quelling character.Therefore, owe the control of drive system and the theory of tracking has caused increasing non-linear researcher's interest.

Aspect practical application, owe the minimizing of drive system due to driver, have lightweight, cost is low, less energy consumption, the many merits such as easy to maintenance; On the other hand, in occasions such as controller failures, full drive system can become owes drive system, owes drive system fault-tolerant ability and has also caused numerous concerns.In recent years, owe the new focus that drive system becomes a lot of research fields (as Aero-Space, boats and ships, vehicle, robot etc.).

Yet, in actual applications, for robot system, because its working environment is ever-changing, in working control, will face more uncertain factor, thereby its motion control requires to have stronger Disturbance Rejection ability; The control method with height robustness is one of effective way solving such problem.As the aircraft in space, its degree of freedom will expand to six by three of plane, and system is more complicated.

Therefore, design a kind ofly reasonably for owing the robust controller of drive system, especially for the controller of the six degree of freedom in space, there is important using value.

Summary of the invention

Technical matters to be solved by this invention be design a kind of six degree of freedom for owing to drive the robust controller of submarine navigation device track following, can make aircraft in any original state, exist in the situation of environmental perturbation, realize good tracking effect.

The present invention mainly comprises following content:

(1) set up the dynamic model that six degree of freedom owes to drive submarine navigation device;

(2) system of selection of Lyapunov;

(3) design of robust controller;

(4) selection of controller parameter.

Embodiment

The present invention be take six degree of freedom and is owed to drive submarine navigation device as research object.

1. the foundation of system model

Six degree of freedom owes to drive the mathematical model of submarine navigation device to be:

$\begin{array}{c}{\stackrel{\·}{\mathrm{\η}}}_1=J_1\left({\mathrm{\η}}_2\right)V_1\\ {\stackrel{\·}{\mathrm{\η}}}_2=J_2\left({\mathrm{\η}}_2\right)V_2\\ M_1{\stackrel{\·}{V}}_1=-M_{1}C\left(V_2\right)V_1-\stackrel{\‾}{V_1}{D}_1\left(V_1\right)+{\mathrm{\τ}}_1+{\mathrm{\τ}}_{\mathrm{\ω}1}\left(t\right)\\ M_2{\stackrel{\·}{V}}_2=-M_{2}C\left(V_1\right)V_1-C\left(V_2\right)V_2-D_2\left(V_2\right)V_2+{\mathrm{\τ}}_2+{\mathrm{\τ}}_{\mathrm{\ω}2}\left(t\right)\end{array}.---\left(1\right)$

η wherein ₁=(x y z) ^t, η ₂=(φ θ ψ) ^t, the pose of representative system in rectangular coordinate system; V ₁=(u v w) ^t, V ₂=(p q r) ^t, represent the speed of each pose, ${\stackrel{\‾}{V}}_1=\mathrm{diag}\left(\begin{array}{ccc}u& v& w\end{array}\right);$ J ₁(η ₂), J ₂(η ₂), M ₁, M ₂, C (V ₁), C (V ₂), D ₁(V ₁), D ₂(V ₂) represented respectively Jacobi's transformation matrix, inertial matrix, Coriolis matrix and damping matrix, be expressed as:

$J_1\left({\mathrm{\η}}_2\right)=\left(\begin{array}{ccc}\mathrm{c\ψc\θ}& -\mathrm{s\ψc\φ}+\mathrm{s\φs\θc\ψ}& \mathrm{s\ψs\φ}+\mathrm{s\θc\ψc\φ}\\ \mathrm{s\ψc\θ}& \mathrm{c\ψc\φ}+\mathrm{s\φs\θs\ψ}& -\mathrm{c\ψs\φ}+\mathrm{s\θs\ψc\φ}\\ -\mathrm{s\θ}& \mathrm{s\φc\θ}& \mathrm{c\φc\θ}\end{array}\right),J_2\left({\mathrm{\η}}_2\right)=\left(\begin{array}{ccc}1& \mathrm{s\φt\θ}& \mathrm{c\φt\θ}\\ 0& \mathrm{c\φ}& -\mathrm{s\φ}\\ 0& \mathrm{s\φ}/\mathrm{c\θ}& \mathrm{c\φ}/\mathrm{c\θ}\end{array}\right),C\left(V_1\right)=\left(\begin{array}{ccc}0& w& -v\\ -w& 0& u\\ v& -u& 0\end{array}\right),$

$C\left(V_2\right)=\left(\begin{array}{ccc}0& r& -q\\ -r& 0& p\\ q& -p& 0\end{array}\right),M_1=\mathrm{diag}\left(\begin{array}{ccc}{m}_{11}& m_{22}& m_{33}\end{array}\right),M_2=\mathrm{diag}\left(\begin{array}{ccc}{m}_{44}& m_{55}& m_{66}\end{array}\right),$

$D_1\left(v_1\right)=\mathrm{diag}(d_{11}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{u1}{\left|u\right|}^{i-1},d_{22}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{v1}{\left|v\right|}^{i-1},d_{33}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{w1}{\left|w\right|}^{i-1}),$

$D\left(v_2\right)=\mathrm{diag}(d_{44}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{p1}{\left|p\right|}^{i-1},d_{55}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{q1}{\left|q\right|}^{i-1},d_{66}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{r1}{\left|r\right|}^{i-1}),$

Control inputs is

τ ₁=(τ _u，0，0) ^T，τ ₂=(0，τ _q，τ _r) ^T，

τ wherein _u, τ _q, τ _rbe respectively boost torque, face upward the moment of bowing, turn bow moment.Finally, the disturbance quantity of environment is following vector

τ _w1(t)=(τ _wu(t)，τ _wv(t)，τ _ww(t)) ^T，τ _w2=(τ _wp(t)，τ _wq(t)，τ _wr(t)) ^T，

These disturbance quantities are unknown, only know that its upper bound is as follows:

$\left|{\mathrm{\&tau;}}_{\mathrm{wu}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{wu}}^{\max }<\&infin;,\left|{\mathrm{\&tau;}}_{\mathrm{wv}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{wv}}^{\max }<\&infin;,\left|{\mathrm{\&tau;}}_{\mathrm{ww}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{ww}}^{\max }<\&infin;,\left|{\mathrm{\&tau;}}_{\mathrm{wp}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{wp}}^{\max }<\&infin;,\left|{\mathrm{\&tau;}}_{\mathrm{wq}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{wq}}^{\max }<\&infin;,\left|{\mathrm{\&tau;}}_{\mathrm{wr}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{wr}}^{\max }<\&infin;$

|R _v(t)|≤R _vmax<∞，|R _r(t)|≤R _rmax<∞。

Controller design object is design τ _u, τ _q, τ _r, make the system can automatically track target track η _d=(x _d, y _d, z _d) ^teven, tracking error (x-x _dy-y _dz-z _d) ^texistence of Global Stable.Note e=(e ₁, e ₂, e ₃) ^t=η ₁-η _dfor tracking error.

2. the selection of system Lyapunov function

First be defined as follows:

wherein, β ₁>0, z ₁=(z ₁₁, z ₁₂, z ₁₃) ^t. $z_2=\left(\begin{array}{c}q\\ r\end{array}\right)-\left(\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\end{array}\right)\mathrm{\&alpha;},$ α=-B wherein ^-1(A+ β ₂(z ₁-δ)), $A=-M^{-1}{\stackrel{\&OverBar;}{V}}_{1}D\left(V_1\right)+M^{-1}{\widehat{\mathrm{\&tau;}}}_{w1}-J_1{\left({\mathrm{\&eta;}}_2\right)}^T{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&eta;}}}_d-{\left(\begin{array}{ccc}0& {\mathrm{\&delta;}}_3& {\mathrm{\&delta;}}_2\end{array}\right)}^{T}p+{\mathrm{\&beta;}}_1{z}_1,$ $B=\left(\begin{array}{ccc}1/m_{11}& {\mathrm{\&delta;}}_3& -{\mathrm{\&delta;}}_2\\ 0& 0& {\mathrm{\&delta;}}_1\\ 0& -{\mathrm{\&delta;}}_1& 0\end{array}\right),$ δ=(δ ₁, δ ₂, δ ₃) ^t, δ ₁≠ 0 is arbitrarily small constant, ${\widetilde{\mathrm{\&tau;}}}_{w1}={\widehat{\mathrm{\&tau;}}}_{w1}-{\mathrm{\&tau;}}_{w1},{\widetilde{\mathrm{\&tau;}}}_{w2}={\widehat{\mathrm{\&tau;}}}_{w2}-{\mathrm{\&tau;}}_{w2}.$

Select following Lyapunov function:

$W=\frac{1}{2}{e}^{T}e+\frac{1}{2{\mathrm{\&beta;}}_1^2}{(z_1-\mathrm{\&delta;})}^T(z_1-\mathrm{\&delta;})+\frac{{\mathrm{\&Gamma;}}_1^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w1}^T{\widetilde{\mathrm{\&tau;}}}_{w1}+\frac{1}{2}{z}_2^T{z}_2+\frac{{\mathrm{\&Gamma;}}_2^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w2}^T{\widetilde{\mathrm{\&tau;}}}_{w2},---\left(3\right)$

Its derivative along system is:

$\begin{array}{c}\stackrel{\&CenterDot;}{W}=-{\mathrm{\&beta;}}_1{e}^{T}e+{\mathrm{\&delta;}}^T{J}_1{\left({\mathrm{\&eta;}}_2\right)}^{T}e+\frac{1}{{\mathrm{\&beta;}}_1^2}{(z_1-\mathrm{\&delta;})}^T(A+\mathrm{Bw})+{\widetilde{\mathrm{\&tau;}}}_{w1}^T{\mathrm{\&Gamma;}}_1^{-1}({\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{w1}-\frac{1}{{\mathrm{\&beta;}}_1^2}{\mathrm{\&Gamma;}}_1{M}^{-1}(z_1-\mathrm{\&delta;}))\\ +z_2^T\left({\left(M_2^{\&prime;}\right)}^{-1}\mathrm{\&Lambda;}(-M_{2}C\left(V_1\right)V_1-C\left(V_2\right)V_2-D_2\left(V_2\right)V_2+{\mathrm{\&tau;}}_2+{\widehat{\mathrm{\&tau;}}}_{\mathrm{\&omega;}2}\left(t\right)-M_2\stackrel{\&CenterDot;}{\mathrm{\&alpha;}})\right)+{\mathrm{\&Gamma;}}_2^{-1}{\widetilde{\mathrm{\&tau;}}}_{w2}^T({\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{w2}-{\mathrm{\&Lambda;}}^T{\mathrm{\&Gamma;}}_2{z}_2)\end{array},---\left(4\right)$

Wherein, w=(τ _uq r) ^t.

3. the design of robust controller

Design following Robust Control Law:

${\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{\mathrm{wu}}=\frac{{\mathrm{\&Gamma;}}_1}{m_{11}}\frac{1}{{\mathrm{\&beta;}}_1^2}\mathrm{proj}({\widehat{\mathrm{\&tau;}}}_{\mathrm{wu}},-(z_{11}-{\mathrm{\&delta;}}_1)),{\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{\mathrm{wv}}=\frac{{\mathrm{\&Gamma;}}_1}{m_{22}}\frac{1}{{\mathrm{\&beta;}}_1^2}\mathrm{proj}({\widehat{\mathrm{\&tau;}}}_{\mathrm{wv}},-(z_{22}-{\mathrm{\&delta;}}_2)),{\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{\mathrm{ww}}=\frac{{\mathrm{\&Gamma;}}_1}{m_{33}}\frac{1}{{\mathrm{\&beta;}}_1^2}\mathrm{proj}({\widehat{\mathrm{\&tau;}}}_{\mathrm{ww}},-(z_{33}-{\mathrm{\&delta;}}_3)),---\left(5\right)$

${\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{\mathrm{\&omega;}2}=\mathrm{proj}({\widehat{\mathrm{\&tau;}}}_{\mathrm{\&omega;}2},{\mathrm{\&Lambda;}}^T{\mathrm{\&Gamma;}}_2{z}_2),---\left(6\right)$

By in above-mentioned value substitution (4), obtain:

$\stackrel{\&CenterDot;}{W}\&le;-{\mathrm{\&beta;}}_1{e}^{T}e+{\mathrm{\&delta;}}^T{J}_1{\left({\mathrm{\&eta;}}_2\right)}^{T}e-(1/{\mathrm{\&beta;}}_1^2){(z_1-\mathrm{\&delta;})}^T{\mathrm{\&beta;}}_2(z_1-\mathrm{\&delta;})-{\mathrm{\&beta;}}_3{z}_2^2.---\left(8\right)$

4. the selection of parameter

To appointing to γ >0, have:

${\mathrm{\&delta;}}^T{J}^T\left({\mathrm{\&eta;}}_2\right)e\&le;\frac{1}{2\mathrm{\&gamma;}}{\mathrm{\&delta;}}^T\mathrm{\&delta;}+\frac{\mathrm{\&gamma;}}{2}{e}^{T}J\left({\mathrm{\&eta;}}_2\right)J^T\left({\mathrm{\&eta;}}_2\right)e=\frac{\mathrm{\&gamma;}}{2}{e}^{T}e+\frac{1}{2\mathrm{\&gamma;}}{\mathrm{\&delta;}}^T\mathrm{\&delta;},---\left(9\right)$

(8) are changed to:

$\stackrel{\&CenterDot;}{W}\&le;-e^T({\mathrm{\&beta;}}_1-\frac{\mathrm{\&gamma;}}{2})e-{(z_1-\mathrm{\&delta;})}^T\frac{{\mathrm{\&beta;}}_2}{{\mathrm{\&beta;}}_1^2}(z_1-\mathrm{\&delta;})-{\mathrm{\&beta;}}_3{z}_2^2+\frac{1}{2\mathrm{\&gamma;}}{\left|\right|\mathrm{\&delta;}\left|\right|}^2,---\left(10\right)$

Appoint γ >0, order to λ >0 ${\mathrm{\&beta;}}_1-\frac{\mathrm{\&gamma;}}{2}\&GreaterEqual;\frac{\mathrm{\&lambda;}}{2},{\mathrm{\&beta;}}_2\&GreaterEqual;\frac{\mathrm{\&lambda;}}{2},{\mathrm{\&beta;}}_3\&GreaterEqual;\frac{\mathrm{\&lambda;}}{2}{m}_{\mathrm{\&psi;}},$ ?

$\stackrel{\&CenterDot;}{W}\&le;-\mathrm{\&lambda;W}+\frac{{\mathrm{\&Gamma;}}_1^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w1}^T{\widetilde{\mathrm{\&tau;}}}_{w1}+\frac{{\mathrm{\&Gamma;}}_2^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w2}^T{\widetilde{\mathrm{\&tau;}}}_{w2}+\frac{1}{2\mathrm{\&gamma;}}{\left|\right|\mathrm{\&delta;}\left|\right|}^2.---\left(11\right)$

According to the character of proj, can obtain

for bounded, therefore,

$\stackrel{\&CenterDot;}{W}\&le;-\mathrm{\&lambda;W}+l.---\left(12\right)$

Wherein, $l=\frac{{\mathrm{\&Gamma;}}_1^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w1}^T{\widetilde{\mathrm{\&tau;}}}_{w1}+\frac{{\mathrm{\&Gamma;}}_2^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w2}^T{\widetilde{\mathrm{\&tau;}}}_{w2}+\frac{1}{2\mathrm{\&gamma;}}{\left|\right|\mathrm{\&delta;}\left|\right|}^2,$ By comparing lemma, obtain

$W\left(t\right)\&le;e^{-\mathrm{\&lambda;t}}W\left(0\right)+\frac{1}{\mathrm{\&lambda;}}.---\left(13\right)$

I is error || e|| converges on radius and is

ball territory, system stability.

This is owed to drive submarine navigation device for six degree of freedom and has designed a kind of robust tracking controller.Consider to owe to drive and cannot realize asymptotically stable characteristic, taked to make error to be tending towards minimizing method, solved the selection of Lyapunov function.For the external disturbance of bounded, the controller that adopted Function Mapping technical design, has realized good tracking effect and robustness.

Claims (6)

1. for six degree of freedom, owe to drive a robust tracking controller for submarine navigation device, it is characterized in that, in the uncertain situation of controller parameter, can setting out by arbitrary initial state, automatically follow the tracks of the track of arbitrary smooth, and obtained good tracking effect.

2. according to the six degree of freedom described in right 1, owe to drive the robust tracking controller of submarine navigation device, it is characterized in that aircraft has following dynamic model:

$\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_1=J_1\left({\mathrm{\&eta;}}_2\right)V_1\\ {\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_2=J_2\left({\mathrm{\&eta;}}_2\right)V_2\\ M_1{\stackrel{\&CenterDot;}{V}}_1=-M_{1}C\left(V_2\right)V_1-\stackrel{\&OverBar;}{V_1}{D}_1\left(V_1\right)+{\mathrm{\&tau;}}_1+{\mathrm{\&tau;}}_{\mathrm{\&omega;}1}\left(t\right)\\ M_2{\stackrel{\&CenterDot;}{V}}_2=-M_{2}C\left(V_1\right)V_1-C\left(V_2\right)V_2-D_2\left(V_2\right)V_2+{\mathrm{\&tau;}}_2+{\mathrm{\&tau;}}_{\mathrm{\&omega;}2}\left(t\right)\end{array}.$

3. according to the six degree of freedom described in right 1, owe to drive the robust tracking controller of submarine navigation device, it is characterized in that the following form of being chosen as of Lyapunov function:

$W=\frac{1}{2}{e}^{T}e+\frac{1}{2{\mathrm{\&beta;}}_1^2}{(z_1-\mathrm{\&delta;})}^T(z_1-\mathrm{\&delta;})+\frac{{\mathrm{\&Gamma;}}_1^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w1}^T{\widetilde{\mathrm{\&tau;}}}_{w1}+\frac{1}{2}{z}_2^T{z}_2+\frac{{\mathrm{\&Gamma;}}_2^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w2}^T{\widetilde{\mathrm{\&tau;}}}_{w2}.$

4. according to the six degree of freedom described in right 1, owe to drive the robust tracking controller of submarine navigation device, it is characterized in that having adopted Function Mapping technology in the design of robust controller.

5. according to the six degree of freedom described in right 1, owe to drive the robust tracking controller of submarine navigation device, it is characterized in that original state is unfettered.

6. according to the six degree of freedom described in right 1, owe to drive the robust tracking controller of submarine navigation device, it is characterized in that the shape of track is unfettered, only require it is smooth, even and surging and the speed that turns bow be all zero, still have good tracking effect.