CN105974930A - Method for tracking movement mother ship by UUV (Unmanned Underwater Vehicle) based on nonlinear model predictive control - Google Patents

Abstract

The invention discloses a method for tracking a movement mother ship by a UUV (Unmanned Underwater Vehicle) based on nonlinear model predictive control, and relates to a tracking control method of an under-actuated UUV for a movement mother ship. In order to solve a problem of low accuracy when an existing processing method for linear models carries out control on the UUV, the method disclosed by the invention comprises the steps of firstly building a horizontal prediction model of the under-actuated UUV and corresponding constraint conditions; then selecting comprehensive performance indexes, and converting a tracking control problem of the under-actuated UUV for the movement mother ship into an optimization problem under the constraint conditions; processing the optimization problem under the constraint conditions in the step 3 by using Taylor series expansion and a Lie derivative, and using a solved analytic solution to act as control input of a UUV tracking system; and refreshing the analytic solution with a new state value in each prediction time domain, and running continuously until the UUV complete a tracking operation for the movement mother ship. The method disclosed by the invention is applicable to tracking control of the under-actuated UUV for the movement mother ship.

Description

A kind of UUV based on Nonlinear Model Predictive Control tracking to motion lash ship

Technical field

The present invention relates to the drive lacking UUV tracking and controlling method to motion lash ship.

Background technology

UAV navigation (Unmanned Underwater Vehicle, UUV) has that range of activity is big, diving is deep, The advantages such as mobility is good, intelligent, operation and maintenance cost is low, as the mankind in Activities of Ocean, particularly in the activity of deep-sea Important replacer and executor, be widely used in the fields such as scientific investigation, deep ocean work, when operation completes, need It is reclaimed, is especially taking motion lash ship back of the body camel to carry in removal process, need UUV motion lash ship is carried out accurately with Track.

At present, Chinese scholars has done substantial amounts of research for UUV tracking control problem, proposes a lot of controlling party accordingly Method, such as sliding formwork control, Reverse Step Control etc..Sliding mode controller not only have to system model Parameter uncertainties and external disturbance Robustness, and drastically increase system mode convergence rate near equilibrium point.Backstepping calculates simple, real-time Good, response is fast, low to sensor requirements, is widely used, but there is virtual controlling in the method in engineer applied The higher derivative problem of amount.Mostly all have not been able to the tracking control problem taking into full account UUV under constraints, and in rapidity On have certain limitation, and Model Predictive Control (Model predictive control, MPC) can solve this type of well Problem, it is therefore proposed that be applied to Nonlinear Model Predictive Control have great importance during motion lash ship is followed the tracks of by UUV.

In May, 2014 Proceedings of the Institution of Mechanical Engineers Part " Model in M:Journal of Engineering for the Maritime Environment volume 228 predictive control of a hybrid autonomous underwater vehicle with Experimental verification " propose to be used for Model Predictive Control severity control during AUV floating state, but the party Method processes just for linear model, if the method moving to UUV in motion lash ship tracing control, is entering model Accuracy impact controlled during line linearity, the method is in line computation simultaneously, rapidity impact controlled.

Summary of the invention

The present invention is that the method carrying out processing to solve existing linear model exists standard when being controlled UUV The problem that really property is the highest.

A kind of UUV based on Nonlinear Model Predictive Control tracking to motion lash ship, comprises the steps:

Step one: set up the forecast model of drive lacking UUV horizontal plane；

Step 2: according to reality, set up the corresponding constraints of forecast model of UUV horizontal plane；

Step 3: choose integrated performance index, is converted into constraint by drive lacking UUV to the tracking control problem of motion lash ship Under the conditions of optimization problem；

Step 4: utilize Taylor series expansion and Lie derivatives to the optimization problem under constraints in step 3 at Reason, and the analytic solutions tried to achieve are followed the tracks of as UUV the control input of system；

Step 5: in each prediction time domain, by the analytic solutions in new state value refresh step four, constantly run with this, Until UUV completes motion lash ship is followed the tracks of operation.

The present invention has the effect that

The invention provides a kind of UUV based on Nonlinear Model Predictive Control tracking to motion lash ship, the party UUV pursuit movement lash ship control problem is converted to the optimization problem under certain constraints by method, and utilizes Taylor series expansion With Lie derivatives, optimization problem is processed, finally try to achieve the analytic solutions controlling input in each prediction time domain, can well Solve for the UUV nonlinear system rapid track and control to motion lash ship.The analytic solutions that the method obtains possess offline optimization Feature, be possible not only to reduce the cumulative function of ocean current uncertain factor, simultaneously can strengthen the quick of tracking control system Property.

Accompanying drawing explanation

Fig. 1 is the horizontal plane modeling figure of UUV；

Fig. 2 is that UUV based on Nonlinear Model Predictive Control is to motion lash ship tracing control block diagram；

Fig. 3 is that UUV based on Nonlinear Model Predictive Control is to motion lash ship tracing control flow chart；

Track when Fig. 4 is UUV tracking sinusoidal motion lash ship；

Longitudinal velocity and lateral velocity when Fig. 5 is UUV tracking sinusoidal motion lash ship；Wherein, Fig. 5 a is that UUV follows the tracks of sine fortune Longitudinal velocity curve chart during dynamic lash ship, lateral velocity curve chart when Fig. 5 b is UUV tracking sinusoidal motion lash ship；

When Fig. 6 is UUV tracking sinusoidal motion lash ship, bow is to angle and acceleration thereof；Wherein, Fig. 6 a is the acceleration to angle of the UUV bow Writing music line chart, Fig. 6 b is that the bow of UUV is to curve chart；

Fig. 7 is UUV longitudinal thrust and turn bow moment when following the tracks of sinusoidal motion lash ship；Wherein, Fig. 7 a is UUV actuator Thrust curve figure, Fig. 7 b be UUV actuator turn bow M curve figure；

Trajectory error when Fig. 8 is UUV tracking sinusoidal motion lash ship；Wherein, Fig. 8 a is UUV seat of x-axis when following the tracks of lash ship Mark error curve diagram, Fig. 8 b is UUV y-axis error of coordinate curve chart when following the tracks of lash ship；

Track when Fig. 9 is UUV tracking circular motion lash ship；

Longitudinal velocity and lateral velocity when Figure 10 is UUV tracking circular motion lash ship；Wherein, Figure 10 a is that UUV follows the tracks of circular motion Longitudinal velocity curve chart during lash ship, lateral velocity curve chart when Figure 10 b is UUV tracking circular motion lash ship；

When Figure 11 is UUV tracking circular motion lash ship, bow is to angle and acceleration thereof；Wherein, Figure 11 a is that UUV follows the tracks of circular motion mother Ship time acceleration plots, when Figure 11 b is UUV tracking circular motion lash ship, bow is to angular curve figure；

Figure 12 is UUV longitudinal thrust and turn bow moment when following the tracks of circular motion lash ship；Wherein, Figure 12 a is that UUV follows the tracks of circular motion Longitudinal thrust curve chart during lash ship, Figure 12 b is to turn bow M curve figure during UUV tracking circular motion lash ship；

Figure 13 is the lash ship trajectory error that UUV follows the tracks of circular motion；Wherein, Figure 13 a is the lash ship x-axis that UUV follows the tracks of circular motion Error of coordinate curve chart, Figure 13 b be UUV follow the tracks of circular motion lash ship y-axis error of coordinate curve chart.

Detailed description of the invention

Detailed description of the invention one: combine Fig. 1 to Fig. 3 and present embodiment is described,

A kind of UUV based on Nonlinear Model Predictive Control tracking to motion lash ship, comprises the steps:

Step one: set up the forecast model of drive lacking UUV horizontal plane；

Step 2: according to reality, set up the corresponding constraints of forecast model of UUV horizontal plane；

Step 3: choose integrated performance index, is converted into constraint by drive lacking UUV to the tracking control problem of motion lash ship Under the conditions of optimization problem；

Step 4: utilize Taylor series expansion and Lie derivatives to the optimization problem under constraints in step 3 at Reason, and the analytic solutions tried to achieve are followed the tracks of as UUV the control input of system；

Step 5: in each prediction time domain, by the analytic solutions in new state value refresh step four, constantly run with this, Until UUV completes motion lash ship is followed the tracks of operation.

Detailed description of the invention two:

The detailed process of the forecast model setting up drive lacking UUV horizontal plane described in present embodiment step one is as follows:

In conjunction with Fig. 1, establishing the coordinate system of UUV, E-ξ η ζ is fixed coordinate system, and O-XYZ is kinetic coordinate system；Fixing The initial point of coordinate system E-ξ η ζ is appointed and is taken on the earth or ocean a bit, and E ξ holding level also points to geographical north orientation, is chosen as the master of UUV Course；E η points to geographical east orientation, represents the horizontal of UUV；E ζ points to the earth's core, represents the vertical of UUV；Kinetic coordinate system O-XYZ puts On UUV, and along with one plays motion, its initial point O is placed in the position of UUV center of gravity；The direction that Ox sensing UUV moves ahead just is, around Ox rotates and produces angular velocity in roll p, forms roll angle φ；It is just that Oy points to the starboard direction of UUV, rotates generation Angle of Trim around Oy Speed q, forms Angle of Trim θ；It just be that Oz points to bottom UUV, rotates around Oz and produces bow to angle angular velocity r, and formation bow is to angle ψ, root UUV horizontal plane system is set up according to this coordinate system；

Assume UUV track (x, y, ψ) and lash ship track (x_d,y_d,ψ_d) be all continuous print and meet can make many subdifferentials fortune Calculate；X, y in UUV track is UUV position in fixed coordinate system, and ψ is the bow of the UUV x to angle, in lash ship track_d、y_dFor Lash ship position in fixed coordinate system, ψ_dFor lash ship bow in fixed coordinate system to angle；

Definition x=[u v r x y ψ]^TFor state variable, definition y is UUV track；Wherein, the state of UUV includes: x, Y, ψ, longitudinal velocity u, lateral velocity v, bow are to angle angular velocity r；Runic x is state variable, and light face type x is that UUV is at fixed coordinate system In position；Runic y is UUV track, and light face type y is UUV position in fixed coordinate system；

Make x=[u v r x y ψ] further^T=[x₁ x₂ x₃ x₄ x₅ x₆]^T, obtain UUV horizontal plane forecast model:

Wherein, x ∈ R⁶,u∈R²,y∈R³, u is for controlling input, by u₁、u₂Collectively form, and u₁=X_prop, u₂=τ_r；Slightly Body u is for controlling input, and light face type u is UUV longitudinal velocity；Simultaneously in order to represent convenient, by [u v r x y ψ]^TIt is expressed as [x₁ x₂ x₃ x₄ x₅ x₆]^T；

Provide the concrete form of each in formula (6) simultaneously:

U=[u₁ u₂]^T=[X_prop τ_r]^T, y=[h₁ h₂ h₃]^T=[x₄ x₅ x₆]^T

Wherein, X_prop,τ_rIt is respectively main thruster thrust and turns bow moment；d₁For the hydrodynamic force viscosity term of direction of advance, d₂ The hydrodynamic force viscosity term in traversing direction, d₃Turn the hydrodynamic force viscosity in bow direction, m₁For direction of advance hydrodynamic force additional mass, m₂The hydrodynamic force additional mass in traversing direction, m₃For turning the hydrodynamic force additional mass in bow direction.

Other step and parameter are identical with detailed description of the invention one.

Detailed description of the invention three:

The corresponding constraints of forecast model of the UUV horizontal plane described in present embodiment step 2 is as follows:

For UUV horizontal plane forecast model, in certain prediction time domain [t, t+T], by satisfied constraint:

Wherein τ is time variable, and τ ∈ [0, T]；For UUV prediction of state variable in time domain [t, t+T] Value；For UUV predictive value of track in time domain [t, t+T]；Control defeated in time domain [t, t+T] for UUV Enter；

Meanwhile, in [t, t+T] prediction time domain, state variable predictive valueInitial value meet:

Wherein, t represents that moment, T represent the time of prediction time domain.

Other step and parameter are identical with detailed description of the invention two.

Detailed description of the invention four:

Present embodiment step 3 detailed process is as follows:

In each prediction time domain, choosing integrated performance index according to control requirement is:

Wherein,For UUV in terminal juncture t+T trajectory predictions value,For lash ship at terminal juncture t+T Track,For lash ship track in time domain [t, t+T]；

By minimizing performance indications J given by formula (9), it is thus achieved that the optimum control input of tracking control systemAnd need the constraint met according to UUV horizontal plane forecast model in step 2, by UUV to motion The tracking problem of lash ship is converted into optimization problem:

Other structure and parameter are identical with one of detailed description of the invention one to three.

Detailed description of the invention five:

Present embodiment step 4 detailed process is as follows:

According to Model Predictive Control ultimate principle, it is assumed that controlling input in prediction time domain [t, t+T] is a constant value:

Wherein, const represents constant；

With this understanding, obtain predicting that in time domain [t, t+T], tracking control system controls inputAll-order derivative be all Zero；

Utilize Taylor series expansion and Lie derivatives that optimization problem is processed, according to Taylor series theorem, UUV track Prediction outputAnd lash ship trackCan expand into:

Runic τ is the function with time variable τ as independent variable；

In formula, the parameter of all band footmark d all represents lash ship correspondence parameter；

Wherein,Represent UUV trajectory predictions component respectivelyWith lash ship trajectory componentsI rank Time-derivative；I=0 ..., N；J=1,2,3；N is the highest order by Taylor series expansion；

Formula (12) is brought in formula (9), by the arrangement of performance indications J is further:

Wherein,For intermediate object program；μ₁For outlet terminal constraint it is Number, μ₂For tracking error coefficient, μ₃For controlling input coefficient；

Then formula (10) be equivalent to formula (13) aboutTake minimum minimum point, can obtain:

Further willBy Taylor series expansion, j=1,2,3, according to hypothesisAll-order derivative is equal to zero, i.e.(include zero order derivative (being exactly self) above for the description of i, and herein in relation to the description of i be from First derivative starts, reconcilable)；Therefore can removeInAll-order derivative item, obtained by Lie derivativesAll-order derivative, j=1,2,3；Its solving result is as follows:

The most each coefficient is:

q1_0,0=x₄

q1_1,0=L_fh₁=x₁cosx₆-x₂sinx₆

In like manner can derive:

The most each coefficient is:

q2_0,0=h₂=x₅

q2_1,0=L_fh₂=x₁sinx₆+x₂cosx₆

The most each coefficient is:

q3_0,0=h₃=x₆

q3_1,0=L_fh₃=x₃

Choosing Taylor series expansion order is N=3, and takes L=2, thenAll-order derivative be expressed as about control defeated EnterMultinomial, and correspondingThe coefficient table of item is shown as:

The most each component can be expressed as:

Therefore output is predictedCan be further represented as:

Wherein_,Q (x)=[q_·,0(x)q·_·,1(x)…q·_·,L(x)] it is intermediate object program,For centre Result；

From the foregoing,Start coefficient from quadratic power and be zero, can ignoreQuadratic power and above item, 1≤l ≤ L, then have:

(20) and (21) are brought into (14) obtain:

Wherein

Then obtain Nonlinear Model Predictive Control device algorithm controls input analytic solutions:

WillControl as UUV tracking control system inputs.

Embodiment

Carrying out emulation experiment according to detailed description of the invention five, simulation result is as shown in Fig. 4～Figure 13；

Fig. 4～Fig. 8 sets forth UUV without ocean current and constant value ocean current u_c=0.5m/s, v_cSine is followed the tracks of during=0.1m/s The track situation of motion lash ship, Fig. 4 can be seen that UUV exists bigger error when initial position with lash ship sinusoidal motion track, Under Nonlinear Model Predictive Control device effect, UUV quickly moves to lash ship direction, and is gradually reduced error, the most accurately Sinusoidal motion lash ship in tracking；Fig. 5 a and Fig. 5 b gives UUV longitudinal velocity in both cases and lateral velocity, can see Going out at the initial time followed the tracks of, longitudinal velocity and lateral velocity all occur in that large change, are making motion lash ship in UUV tracking After track, tend to be steady, and with cyclically-varying；Fig. 6 a and Fig. 6 b sets forth UUV bow adding to angle in both cases Speed and bow are to change；Fig. 7 a and Fig. 7 b gives the control input of UUV actuator, i.e. longitudinal thrust and turns bow moment, can To find out that actuator all occurs in that change by a relatively large margin at the initial time followed the tracks of, on following the tracks of after UUV, change is gradually Tend to be steady；Fig. 8 a and Fig. 8 b gives UUV x-axis and error of coordinate of y-axis when following the tracks of lash ship.Fig. 9～Figure 13 is given respectively UUV is without ocean current and constant value ocean current u_c=0.5m/s, v_cThe track situation of circus movement lash ship is followed the tracks of, simultaneously during=0.1m/s There is the rule identical with following the tracks of sinusoidal motion lash ship.In actual applications, parameter can be controlled by regulation to meet further Control requirement.

Claims (5)

1. a UUV based on the Nonlinear Model Predictive Control tracking to motion lash ship, it is characterised in that include as follows Step:

Step one: set up the forecast model of drive lacking UUV horizontal plane；

Step 2: set up the corresponding constraints of forecast model of UUV horizontal plane；

Step 3: choose integrated performance index, is converted into constraints by drive lacking UUV to the tracking control problem of motion lash ship Under optimization problem；

Step 4: utilize Taylor series expansion and Lie derivatives that the optimization problem under constraints in step 3 is processed, and The analytic solutions tried to achieve are followed the tracks of as UUV the control input of system；

Step 5: in each prediction time domain, by the analytic solutions in new state value refresh step four, constantly run with this, until UUV completes motion lash ship is followed the tracks of operation.

A kind of UUV based on the Nonlinear Model Predictive Control the most according to claim 1 tracking to motion lash ship, It is characterized in that the detailed process of the forecast model setting up drive lacking UUV horizontal plane described in step one is as follows:

Assume UUV track (x, y, ψ) and lash ship track (x_d,y_d,ψ_d) be all continuous print and satisfied repeatedly differentiate；UUV rail X, y in mark is UUV position in fixed coordinate system, and ψ is the bow of the UUV x to angle, in lash ship track_d、y_dFor lash ship solid Position in position fixing system, ψ_dFor lash ship bow in fixed coordinate system to angle；

Definition x=[u v r x y ψ]^TFor state variable, definition y is UUV track；Wherein, the state of UUV includes: x, y, ψ, vertical To speed u, lateral velocity v, bow to angle angular velocity r；Runic x is state variable, and light face type x is UUV position in fixed coordinate system Put；Runic y is UUV track, and light face type y is UUV position in fixed coordinate system；

Make x=[u v r x y ψ] further^T=[x₁ x₂ x₃ x₄ x₅ x₆]^T, obtain UUV horizontal plane forecast model:

$\left\{\begin{array}{c}\stackrel{\·}{x}=f\left(x\right)+g\left(x\right)u\\ y=h\left(x\right)\end{array}\right.---\left(6\right)$

Wherein, x ∈ R⁶,u∈R²,y∈R³, u is for controlling input, by u₁、u₂Collectively form, and u₁=X_prop, u₂=τ_r；Runic u is Controlling input, light face type u is UUV longitudinal velocity；

Provide the concrete form of each in formula (6) simultaneously:

$f\left(x\right)=\left[\begin{array}{c}{a}_1{x}_2{x}_3+a_2{x}_1\\ b_1{x}_1{x}_3+b_2{x}_2\\ c_1{x}_1{x}_2+c_2{x}_3\\ x_1\cos x_6-x_2\sin x_6\\ x_1\sin x_6+x_2\cos x_6\\ x_3\end{array}\right],g\left(x\right)=\left[\begin{array}{cc}{g}_1\left(x\right)& g_2\left(x\right)\end{array}\right]={\left[\begin{array}{cccccc}{a}_3& 0& 0& 0& 0& 0\\ 0& 0& c_3& 0& 0& 0\end{array}\right]}^T$

U=[u₁ u₂]^T=[X_prop τ_r]^T, y=[h₁ h₂ h₃]^T=[x₄ x₅ x₆]^T

$a_1=\frac{m_2}{m_1},a_2=-\frac{d_1}{m_1},a_3=\frac{1}{m_1},b_1=-\frac{m_1}{m_2},b_2=-\frac{d_2}{m_2},c_1=\frac{m_1-m_2}{m_3},c_2=-\frac{d_3}{m_3},c_3=\frac{1}{m_3}$

Wherein, X_prop,τ_rIt is respectively main thruster thrust and turns bow moment；d₁For the hydrodynamic force viscosity term of direction of advance, d₂Traversing The hydrodynamic force viscosity term in direction, d₃Turn the hydrodynamic force viscosity in bow direction, m₁For direction of advance hydrodynamic force additional mass, m₂Horizontal Move the hydrodynamic force additional mass in direction, m₃For turning the hydrodynamic force additional mass in bow direction.

A kind of UUV based on the Nonlinear Model Predictive Control the most according to claim 2 tracking to motion lash ship, It is characterized in that the corresponding constraints of forecast model of UUV horizontal plane described in step 2 is as follows:

For UUV horizontal plane forecast model, in certain prediction time domain [t, t+T], by satisfied constraint:

$\left\{\begin{array}{c}\stackrel{\·}{\hat{x}}(t+\mathrm{\τ})=f\left(\hat{x}\left(t+\mathrm{\τ}\right)\right)+g\left(\hat{x}\right(t+\mathrm{\τ}\left)\right)\hat{u}(t+\mathrm{\τ})\\ \hat{y}(t+\mathrm{\τ})=h\left(\hat{x}\right(t+\mathrm{\τ}\left)\right)\end{array}\right.,\mathrm{\τ}\∈\[0,T\]---\left(7\right)$

Wherein τ is time variable, and τ ∈ [0, T]；For UUV predictive value of state variable in time domain [t, t+T]；For UUV predictive value of track in time domain [t, t+T]；In time domain [t, t+T], input is controlled for UUV；

Meanwhile, in [t, t+T] prediction time domain, state variable predictive valueInitial value meet:

$\hat{x}(t+\mathrm{\τ})=x\left(t\right),\mathrm{\τ}=0---\left(8\right)$

Wherein, t represents that moment, T represent the time of prediction time domain.

A kind of UUV based on the Nonlinear Model Predictive Control the most according to claim 3 tracking to motion lash ship, It is characterized in that step 3 detailed process is as follows:

In each prediction time domain, choosing integrated performance index is:

$\begin{array}{c}J=\frac{1}{2}{\mathrm{\μ}}_1{\[\hat{y}\left(t+T\right)-{\hat{y}}_d\left(t+T\right)\]}^T\[\hat{y}\left(t+T\right)-{\hat{y}}_d\left(t+T\right)\]+\\ \frac{1}{2}{\∫}_0^T\[{\mathrm{\μ}}_2{\left(\hat{y}\left(t+\mathrm{\τ}\right)-{\hat{y}}_d\left(t+\mathrm{\τ}\right)\right)}^T\left(\hat{y}\left(t+\mathrm{\τ}\right)-{\hat{y}}_d\left(t+\mathrm{\τ}\right)\right)+{\mathrm{\μ}}_3{\hat{u}}^T\left(t+\mathrm{\τ}\right)\hat{u}\left(t+\mathrm{\τ}\right)\]d\mathrm{\τ}\end{array}---\left(9\right)$

Wherein,For UUV in terminal juncture t+T trajectory predictions value,For lash ship at the rail of terminal juncture t+T Mark,For lash ship track in time domain [t, t+T]；

By minimizing performance indications J given by formula (9), it is thus achieved that the optimum control input of tracking control systemAnd need the constraint met according to UUV horizontal plane forecast model in step 2, by UUV to motion The tracking problem of lash ship is converted into optimization problem:

$\underset{\hat{u}\left(t\right)}{min}J---\left(10\right).$

S.t. formula (7) and (8) constraint

A kind of UUV based on the Nonlinear Model Predictive Control the most according to claim 4 tracking to motion lash ship, It is characterized in that step 4 detailed process is as follows:

Assume that controlling input in prediction time domain [t, t+T] is a constant value:

$\hat{u}(t+\mathrm{\τ})=u\left(t\right)=const,\mathrm{\τ}\∈\[0,T\]---\left(11\right)$

Wherein, const represents constant；

With this understanding, obtain predicting that in time domain [t, t+T], tracking control system controls inputAll-order derivative be all zero；

Utilize Taylor series expansion and Lie derivatives that optimization problem is processed, the prediction output of UUV trackAnd it is female Ship trackExpand into:

$\begin{array}{c}\hat{y}(t+\mathrm{\τ})={\mathrm{\τ}}^T\left(\mathrm{\τ}\right)\hat{y}\left(t\right)\\ {\hat{y}}_d(t+\mathrm{\τ})={\mathrm{\τ}}^T\left(\mathrm{\τ}\right){\hat{y}}_d\left(t\right)\end{array}---\left(12\right)$

Runic τ is the function with time variable τ as independent variable；

In formula, the parameter of all band footmark d all represents lash ship correspondence parameter；

$\hat{y}\left(t+\mathrm{\τ}\right)=\left[\begin{array}{c}{\hat{h}}_1\left(t+\mathrm{\τ}\right)\\ {\hat{h}}_2\left(t+\mathrm{\τ}\right)\\ {\hat{h}}_3\left(t+\mathrm{\τ}\right)\end{array}\right]=\left[\begin{array}{c}\hat{x}\left(t+\mathrm{\τ}\right)\\ \hat{y}\left(t+\mathrm{\τ}\right)\\ \widehat{\mathrm{\ψ}}\left(t+\mathrm{\τ}\right)\end{array}\right],{\hat{y}}_d\left(t+\mathrm{\τ}\right)=\left[\begin{array}{c}{\hat{h}}_{1d}\left(t+\mathrm{\τ}\right)\\ {\hat{h}}_{2d}\left(t+\mathrm{\τ}\right)\\ {\hat{h}}_{3d}\left(t+\mathrm{\τ}\right)\end{array}\right]=\left[\begin{array}{c}{\hat{x}}_d\left(t+\mathrm{\τ}\right)\\ {\hat{y}}_d\left(t+\mathrm{\τ}\right)\\ {\widehat{\mathrm{\ψ}}}_d\left(t+\mathrm{\τ}\right)\end{array}\right]$

$\mathrm{\τ}\left(\mathrm{\τ}\right)=\left[\begin{array}{ccc}{\mathrm{\τ}}_1& & \\ & {\mathrm{\τ}}_2& \\ & & {\mathrm{\τ}}_3\end{array}\right],{\mathrm{\τ}}_1={\mathrm{\τ}}_2={\mathrm{\τ}}_3={\left[\begin{array}{cc}1& \mathrm{\τ}...\frac{{\mathrm{\τ}}^N}{N!}\end{array}\right]}^T$

$\hat{y}\left(t\right)={\left[\begin{array}{cccccc}{\hat{h}}_1\left(t\right)& {\stackrel{\·}{\hat{h}}}_1\left(t\right)...{{\hat{h}}_1}^{\[N\]}\left(t\right)& {\hat{h}}_2\left(t\right)& {\stackrel{\·}{\hat{h}}}_2\left(t\right)...{{\hat{h}}_2}^{\[N\]}\left(t\right)& {\hat{h}}_3\left(t\right)& {\stackrel{\·}{\hat{h}}}_3\left(t\right)...{{\hat{h}}_3}^{\[N\]}\left(t\right)\end{array}\right]}^T$

${\hat{y}}_d\left(t\right)={\left[\begin{array}{cccccc}{\hat{h}}_{1d}\left(t\right)& {\stackrel{\·}{\hat{h}}}_{1d}\left(t\right)...{{\hat{h}}_{1d}}^{\[N\]}\left(t\right)& {\hat{h}}_{2d}\left(t\right)& {\stackrel{\·}{\hat{h}}}_{2d}\left(t\right)...{{\hat{h}}_{2d}}^{\[N\]}\left(t\right)& {\hat{h}}_{3d}\left(t\right)& {\stackrel{\·}{\hat{h}}}_{3d}\left(t\right)...{{\hat{h}}_{3d}}^{\[N\]}\left(t\right)\end{array}\right]}^T$

Wherein,Represent UUV trajectory predictions component respectivelyWith lash ship trajectory componentsThe i rank time Derivative；I=0 ..., N；J=1,2,3；N is the highest order by Taylor series expansion；

Formula (12) is brought in formula (9), by the arrangement of performance indications J is further:

$\begin{array}{c}J\≈\frac{1}{2}{\mathrm{\μ}}_1{\[{\mathrm{\τ}}^T\left(T\right)\hat{y}\left(t\right)-{\mathrm{\τ}}^T\left(T\right){\hat{y}}_d\left(t\right)\]}^T\[{\mathrm{\τ}}^T\left(T\right)\hat{y}\left(t\right)-{\mathrm{\τ}}^T\left(T\right){\hat{y}}_d\left(t\right)\]+\frac{1}{2}{\∫}_0^T\[{\mathrm{\μ}}_2({\mathrm{\τ}}^T\left(\mathrm{\τ}\right)\hat{y}\left(t\right)\\ -{\mathrm{\τ}}^T\left(\mathrm{\τ}\right){\hat{y}}_d\left(t\right){)}^T\left({\mathrm{\τ}}^T\left(\mathrm{\τ}\right)\hat{y}\left(t\right)-{\mathrm{\τ}}^T\left(\mathrm{\τ}\right){\hat{y}}_d\left(t\right)\right)+{\mathrm{\μ}}_3{\hat{u}}^T\left(t+\mathrm{\τ}\right)\hat{u}\left(t+\mathrm{\τ}\right)\]d\mathrm{\τ}\\ =\frac{1}{2}{\[\hat{y}\left(t\right)-{\hat{y}}_d\left(t\right)\]}^{T}M\[\hat{y}\left(t\right)-{\hat{y}}_d\left(t\right)\]+\frac{1}{2}{\mathrm{\μ}}_{3}T\hat{u}{\left(t\right)}^T\hat{u}\left(t\right)\end{array}---\left(13\right)$

Wherein,For intermediate object program；μ₁For outlet terminal constraint factor, μ₂ For tracking error coefficient, μ₃For controlling input coefficient；

Then formula (10) be equivalent to formula (13) aboutTake minimum minimum point:

${\left(\frac{\∂\hat{y}\left(t\right)}{\∂\hat{u}\left(t\right)}\right)}^{T}M\left(\hat{y}\right(t)-{\hat{y}}_d(t\left)\right)+{\mathrm{\μ}}_{3}T\hat{u}\left(t\right)=0---\left(14\right)$

Further willBy Taylor series expansion, j=1,2,3, according to hypothesisAll-order derivative is equal to zero, i.e. RemoveInAll-order derivative item, obtained by Lie derivativesAll-order derivative, j=1,2,3；

Choosing Taylor series expansion order is N=3, and takes L=2, thenAll-order derivative be expressed as about control inputMultinomial, and correspondingThe coefficient table of item is shown as:

$q{\·}_{\·,l}\left(x\right)=\left[\begin{array}{c}q{1}_{\·,l,}\left(x\right)\\ q{2}_{\·,l}\left(x\right)\\ q{3}_{\·,l}\left(x\right)\end{array}\right],l=0,...,L---\left(18\right)$

The most each representation in components is:

${\mathrm{qk}}_{\·,l}\left(x\right)=\left[\begin{array}{c}{\mathrm{qk}}_{0,l}\left(x\right)\\ {\mathrm{qk}}_{1,l}\left(x\right)\\ \begin{array}{c}.\\ .\\ .\end{array}\\ {\mathrm{qk}}_{N,l}\left(x\right)\end{array}\right],k=1,2,3;l=0,...,L---\left(19\right)$

$\begin{array}{ccc}\left\{\begin{array}{cc}{\mathrm{qk}}_{0,l}\left(x\right)=0& 0<l\&le;L\\ {\mathrm{qk}}_{j,l}\left(x\right)=0& 1\&le;j\&le;l\end{array}\right.& j=1,...,N& l=0,...,L\end{array}$

Therefore output is predictedIt is further represented as:

$\hat{y}\left(t\right)=\left[\begin{array}{cccc}q{\&CenterDot;}_{\&CenterDot;,0}\left(x\right)& q{\&CenterDot;}_{\&CenterDot;,1}\left(x\right)& ...& q{\&CenterDot;}_{\&CenterDot;,L}\left(x\right)\end{array}\right]\left[\begin{array}{c}1\\ \hat{u}\left(t\right)\\ \begin{array}{c}.\\ .\\ .\end{array}\\ {\hat{u}}^L\left(t\right)\end{array}\right]=Q\left(x\right)\hat{U}---\left(20\right)$

Wherein, Q (x)=[q_.,0(x)q·_.,1(x)…q·_.,L(x)] it is intermediate object program,For intermediate object program；

Start coefficient from quadratic power and be zero, ignoreQuadratic power and above item, 1≤l≤L, then have:

$\hat{U}=\left[\begin{array}{c}{I}_3\\ 0\end{array}\right]\left[\begin{array}{c}1\\ \hat{u}\end{array}\right]---\left(21\right)$

(20) and (21) are brought into (14) obtain:

$q{{\&CenterDot;}_{\&CenterDot;,1}}^T\left(x\right)M\left(q{\&CenterDot;}_{\&CenterDot;,0}\right(x)-{\hat{y}}_d(t\left)\right)+\left(q{{\&CenterDot;}_{\&CenterDot;,1}}^T\right(x\left)Mq{\&CenterDot;}_{\&CenterDot;,1}\right(x)+M_3)\hat{u}\left(t\right)=0---\left(22\right)$

Wherein

Then obtain Nonlinear Model Predictive Control device algorithm controls input analytic solutions:

$\hat{u}\left(t\right)=-{\left(q{{\&CenterDot;}_{\&CenterDot;,1}}^T\right(x\left)Mq{\&CenterDot;}_{\&CenterDot;,1}\right(x)+M_3)}^{-1}q{{\&CenterDot;}_{\&CenterDot;,1}}^T\left(x\right)M\left(q{\&CenterDot;}_{\&CenterDot;,0}\right(x)-{\hat{y}}_d(t\left)\right)---\left(23\right)$

WillControl as UUV tracking control system inputs.