CN103576693B - Underwater robot three-dimensional path tracking and controlling method based on second order filter - Google Patents

Abstract

The present invention is to provide a kind of underwater robot three-dimensional path tracking and controlling method based on second order filter, filtering Backstepping is utilized to carry out underwater robot three-dimensional path tracing control, by introducing two second order filters set up based on underwater robot three-dimensional path pursuit movement error model, obtain attitude, speed, the virtual controlling amount of angular velocity and derivative thereof, control in conjunction with underwater human occupant dynamic model acquisition approach tracking control unit inputs, act on robot propeller and steering wheel, and then realize the tracking to three-dimensional path；And according to lyapunov energy function to position, gesture stability loop design filtering feedback compensation term, speed control loop is introduced integral element, constitute systematic error compensation loop, promote the precision of tracking system.

Description

Underwater robot three-dimensional path tracking and controlling method based on second order filter

Technical field

The present invention relates to the motion control method of a kind of underwater robot, particularly relate to a kind of underwater robot three-dimensional path tracking and controlling method based on second order filter.

Background technology

The tracking control problem of underwater robot is always up study hotspot both domestic and external.Path trace problem requires nothing more than the movement locus of underwater robot and converges to expected path, and is not required when arriving where.Path tracking technique is by introducing the concept of virtual guide point on expected path, it is to avoid owing to the kinetic model of underwater robot is subject to environmental perturbation effect during track following, it is impossible to accurately obtain the expectation state, the problem causing tracking control system degradation.Path trace is compared to track following problem, owing to desired locations is not retrained by time conditions, thus not being easily caused controller output saturation signal, meeting engineering reality, having more actual application value.

Based on the Backstepping of system layer Recursive Design thought, the design for underwater robot three-dimensional tracking control system provides effective means.But, in the process of Backstepping Recursive Design controller, need the derivative of step by step calculation intermediate virtual controlled quentity controlled variable, it is then based on the thought input of equal value by design subsequent subsystem of feedback linearization, realize calming to prime subsystem, by continuous iteration until obtaining and finally truly controlling input.When the exponent number increase of system or the form of virtual controlling are complex, derivation process will become very loaded down with trivial details.Feedback oscillator Backstepping design three-dimensional curve path following control device is adopted for this present invention, part nonlinear terms are eliminated by the parameter of reasonable selection control, simplify the form of virtual controlling amount compared to tradition Backstepping design process, but remain a need for the analytical form of step by step calculation virtual controlling derivative.For overcoming in conventional Backstepping recursive process the deficiency to dummy pilot signal progressively derivation, document " TheUseofSlidingModestoSimplifytheBacksteppingControl " and " Theuseoflinearfilteringofsimplifiedintegratorbacksteppin gcontrolofnonlinearsystems " are respectively adopted sliding formwork wave filter and linear filter approaches the derivative of virtual controlling；Document " CommandFilteredBackstepping " proposes the Backstepping Trajectory Tracking Control (IEEETransactionsOnAutomaticControl.2009 based on design of filter, 54th volume the 6th phase), second order filter is adopted to approach dummy pilot signal, simplifying controller design process, based on singular perturbation theoretical proof, the error controlled between output of filtering track and conventional Backstepping can converge to zero point relatively small neighbourhood.Filtering Backstepping is applied in the Trajectory Tracking Control of land vehicle by document " LandVehicleControlUsingaCommandFilteredBacksteppingAppro ach "；Document " the depopulated helicopter Trajectory Tracking Control based on filtering Backstepping " (controls and decision-making .2012,27th volume the 4th phase) and document " the small-sized depopulated helicopter Trajectory Tracking Control of state constraint " (control theory and application .2012,29th volume the 6th phase) disclosed in helicopter Trajectory Tracking Control in, enormously simplify controller design process.There is presently no pertinent literature discussion to design based on the underwater robot three-dimensional path tracing control of wave filter.

Summary of the invention

The underwater robot three-dimensional path tracking and controlling method based on second order filter that the present invention proposes a kind of process simplification, tracking accuracy is high.

The detailed process based on the underwater robot three-dimensional path tracking and controlling method of second order filter of the present invention is:

Step 1. sets up fixed coordinate system, robot carrier coordinate system and Serret-Frenet (path reference) coordinate system, obtains expected path, and underwater robot starts path trace, completes the initialization of two second order filters；

Fixed sonar sensor that step 2. is carried by underwater robot, attitude transducer, gather underwater robot current location, attitude angle, angular velocity and speed data information, and in conjunction with the direction of expected path and speed, guide thought according to the angle of sight and calculate and obtain the desirable attitude control quantity ψ of underwater robot_co、θ_co, and ideal velocity controlled quentity controlled variable u_co；

The involved desirable attitude control quantity ψ of underwater robot_co、θ_co, and ideal velocity controlled quentity controlled variable u_coCalculation expression be:

${\mathrm{\ψ}}_{co}=-\arcsin (k_{2}e/\sqrt{1+{\left(k_{2}e\right)}^2})---\left(1\right)$

${\mathrm{\θ}}_{co}=\arcsin (k_{3}h/\sqrt{1+{\left(k_{3}h\right)}^2})---\left(2\right)$

u_co=-k₁s+u_rcosψ_cocosθ_co(3)

Wherein gain factor k₁> 0, k₂> 0, k₃> 0 is angle of sight guidance law normalized parameter, and variable s, e and h represent robot and the forward direction of expected path reference point, transverse direction and vertical tracking error under robot carrier coordinate system respectively.

The desirable controlled quentity controlled variable ψ that step 3. will obtain in step 2_co、θ_co、u_coInput, to the second order filter set up based on underwater robot three-dimensional path pursuit movement error model, obtains attitude and the rate controlling amount ψ of underwater robot_c、θ_c、u_c, and derivativeIn conjunction with robot motion variable ψ, θ, u, obtain filtering attitude and the speed Tracking margin of error With desirable angle rate controlling amount r_co、q_co；

Involved underwater robot three-dimensional path pursuit movement error model is:

$\left\{\begin{array}{c}\stackrel{\·}{s}=re-qh+u-u_r{\mathrm{cos\ψ}}_e{\mathrm{cos\θ}}_e\\ \stackrel{\·}{e}=-rs+u_r{\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_e+v\\ \stackrel{\·}{h}=qs-u_r{\mathrm{sin\θ}}_e+w\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_e=\frac{r}{cos\mathrm{\θ}}-r_F\\ {\stackrel{\·}{\mathrm{\θ}}}_e=q-q_F\end{array}\right.---\left(5\right)$

Wherein ψ_e=ψ-ψ_F, θ_e=θ-θ_F,Robot longitudinal velocity u, lateral velocity v and vertical velocity w, yaw angle speed r and pitch velocity q, u_rFor the desired speed of virtual guide point on expected path to be designed, its direction is along the tangential direction of curved path；ψ_FFor u_rThe angle of velocity attitude and fixed coordinate system trunnion axis, θ_FFor u_rThe angle of velocity attitude and fixed coordinate system vertical axis；ψ be robot bow to angle, θ is robot Angle of Trim.

The underwater robot desirable angle rate controlling amount r that step 4. will obtain in step 3_co、q_coInput, to another second order filter set up based on underwater robot three-dimensional path pursuit movement error model, obtains the angular velocity controlled quentity controlled variable r of underwater robot_c、q_c, and derivativeIn conjunction with robot angular movement variable r, q, obtain filtering angular velocity tracking error amount

Step 5. utilizes the filtering attitude and the speed Tracking margin of error that obtain in step 3And the filtering angular velocity tracking error amount obtained in step 4Resolving obtains underwater robot propeller thrust F_u, with diving-plane angle δ_s, vertical rudder angle δ_r, it is respectively acting on robot propeller and steering wheel, it is achieved three-dimensional path tracing control；

Involved underwater robot propeller thrust F_u, with diving-plane angle δ_s, vertical rudder angle δ_rExpression formula be:

$\left\{\begin{array}{c}{F}_u=m_1(-k_u\tilde{u}-k_{iu}{\mathrm{\ϵ}}_1+{\stackrel{\·}{u}}_c-u_{bs})-f_u\\ {\mathrm{\δ}}_s=b_1^{-1}\[m_4(-k_q\tilde{q}-k_{iq}{\mathrm{\ϵ}}_2+{\stackrel{\·}{q}}_c-q_{bs})-f_q\]\\ {\mathrm{\δ}}_r=b_2^{-1}\[m_5(-k_r\tilde{r}-k_{ir}{\mathrm{\ϵ}}_3+{\stackrel{\·}{r}}_c-r_{bs})-f_r\]\end{array}\right.---\left(6\right)$

Whereinf_u、f_qAnd f_rFor model nonlinear hydrodynamic force item；U_bs、q_bsAnd r_bsFor feedback compensation robust item；M₁、m₄、m₅The additional mass respectively produced by fluid；K_u、k_q、k_r、k_iu、k_iqAnd k_irIt is controller parameter；

Involved underwater human occupant dynamic model is:

$\begin{array}{c}\stackrel{\·}{u}=\frac{m_2}{m_1}vr-\frac{m_3}{m_1}wq-\frac{d_1}{m_1}u+\frac{1}{m_1}{F}_u+{\mathrm{\ω}}_1\\ \stackrel{\·}{v}=-\frac{m_1}{m_2}vr-\frac{d_2}{m_2}v+{\mathrm{\ω}}_2\\ \stackrel{\·}{w}=\frac{m_1}{m_3}uq-\frac{d_3}{m_3}w+g_1+{\mathrm{\ω}}_3\\ \stackrel{\·}{q}=\frac{m_3-m_1}{m_5}uw-\frac{d_4}{m_5}q-g_2+\frac{1}{m_5}{b}_1{\mathrm{\δ}}_s+{\mathrm{\ω}}_4\\ \stackrel{\·}{r}=\frac{m_1-m_2}{m_6}uv-\frac{d_5}{m_6}r+\frac{1}{m_6}{b}_2{\mathrm{\δ}}_r+{\mathrm{\ω}}_5\end{array}---\left(7\right)$

Wherein

$m_1=m-X_{\stackrel{\·}{u}},m_2=m-Y_{\stackrel{\·}{v}},m_3=m-Z_{\stackrel{\·}{w}}$

$m_5=I_y-M_{\stackrel{\·}{q}},m_6=I_z-N_{\stackrel{\·}{r}}$

g₁=(W-B) cos θ, g₂=(z_gW-z_bB)sinθ

d₁=X_u+X_u|u_||u|,d₂=Y_v+Y_v|v||v|

d₃=Z_w+Z_w|w||w|,d₄=M_q+M_q|q||q|

d₅=N_r+N_r|r||r|

$b_1=u^2{M}_{{\mathrm{\δ}}_s},b_2=u^2{N}_{{\mathrm{\δ}}_r}$

Wherein, m and m_(·)Represent robot quality and the additional mass produced by fluid matasomatism, I respectively_yFor the robot rotary inertia around y-axis, I_zFor the robot rotary inertia around z-axis, X_(·)、Y_(·)、Z_(·)、M_(·)And N_(·)For viscous fluid hydrodynamic force coefficient；Z_gAnd z_bThe respectively coordinate position of center of gravity and centre of buoyancy on vertical axis under carrier coordinate, W and B represents the gravity and buoyancy, d that robot is subject to respectively_(·)For nonlinear dampling hydrodynamic force item,WithFor hydroplane and vertical rudder steerage coefficient, ω_(·)It is expressed as interference effect item.

Step 6. utilizes the underwater robot attitude and rate controlling amount ψ that obtain in step 3_c、θ_c、u_c, filtering attitude and the speed Tracking margin of errorWith desirable angle rate controlling amount r_co、q_co, the angular velocity controlled quentity controlled variable r of the underwater robot obtained in integrating step 4_c、q_c, and filtering angular velocity tracking error amountStructure filtering error compensates loop；

Involved filtering error compensates the expression formula of error compensation robust item in loop:

${\mathrm{\ψ}}_{bs}=g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(8\right)$

${\mathrm{\θ}}_{bs}=g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(9\right)$

$u_{bs}=\frac{1}{p_{21}}{A}^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(10\right)$

$r_{bs}=\frac{1}{p_{23}}\frac{{\mathrm{\υ}}_{\mathrm{\ψ}}}{cos\mathrm{\θ}}---\left(11\right)$

$q_{bs}=\frac{1}{p_{22}}\mathrm{\υ}\mathrm{\θ}---\left(12\right)$

Wherein, p₂₁、p₂₂、p₂₃For the element in the dematrix of Lyapunov Equation；

$A=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],B=\left[\begin{array}{cc}{\mathrm{cos\θ}}_e{\mathrm{cos\ψ}}_c& -{\mathrm{cos\θ}}_e{\mathrm{sin\ψ}}_c\\ {\mathrm{cos\θ}}_c{\mathrm{sin\ψ}}_c& {\mathrm{cos\θ}}_c{\mathrm{cos\ψ}}_c\\ 0& 0\end{array}\right],C=\left[\begin{array}{cc}{\mathrm{cos\ψ}}_e{\mathrm{cos\θ}}_c& -{\mathrm{cos\ψ}}_e{\mathrm{sin\θ}}_c\\ {\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_c& -{\mathrm{sin\ψ}}_e{\mathrm{sin\θ}}_c\\ -{\mathrm{sin\θ}}_c& -{\mathrm{cos\θ}}_c\end{array}\right],$

$g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\ψ}}-1}{\widetilde{\mathrm{\ψ}}}\\ \frac{sin\widetilde{\mathrm{\ψ}}}{\widetilde{\mathrm{\ψ}}}\end{array}\right],g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\θ}}-1}{\widetilde{\mathrm{\θ}}}\\ \frac{sin\widetilde{\mathrm{\θ}}}{\widetilde{\mathrm{\θ}}}\end{array}\right],$ And meet $\underset{\widetilde{\mathrm{\ψ}}\→0}{\lim }g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right],\underset{\widetilde{\mathrm{\θ}}\→0}{\lim }g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right];$

Position filtering signal compensation error is

$\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]=\left[\begin{array}{c}\tilde{s}-{\mathrm{\ζ}}_x\\ \tilde{s}-{\mathrm{\ζ}}_y\\ \tilde{h}-{\mathrm{\ζ}}_z\end{array}\right]---\left(13\right)$

Wherein position filtering compensates and dynamically measures ζ_x、ζ_yAnd ζ_zExpression formula is

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ζ}}}_x\\ {\stackrel{\·}{\mathrm{\ζ}}}_y\\ {\stackrel{\·}{\mathrm{\ζ}}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{r\ζ}}_y-{\mathrm{q\ζ}}_z\\ -{\mathrm{r\ζ}}_x\\ {\mathrm{q\ζ}}_x\end{array}\right]\left[\begin{array}{c}-k_x{\mathrm{\ζ}}_x\\ -k_y{\mathrm{\ζ}}_y\\ -k_z{\mathrm{\ζ}}_z\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{s}}_c-{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_c-{\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_c-{\stackrel{\·}{h}}_{co}\end{array}\right]+u_r\[\begin{array}{ccc}A& Bg\left(\widetilde{\mathrm{\ψ}}\right)& Cg\left(\widetilde{\mathrm{\θ}}\right)\end{array}\]\left[\begin{array}{c}{\mathrm{\ζ}}_u\\ {\mathrm{\ζ}}_{\mathrm{\ψ}}\\ {\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(14\right)$

And have ζ_x(0)=0, ζ_y(0)=0, ζ_z(0)=0；S_co、e_co、h_coFor expectation position signalling, s_c、e_c、h_cFor position control signal, ζ_u、ζ_ψAnd ζ_θThe filtering compensation variable respectively constructed；

The attitude signal compensation dosage of wave filter output is:

$\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\ψ}}\\ {\mathrm{\υ}}_{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}\widetilde{\mathrm{\ψ}}-{\mathrm{\ζ}}_{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}-{\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(15\right)$

Filtering attitude compensates Expression formula:

${\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\ψ}}=-k_{\mathrm{\ψ}}{\mathrm{\ζ}}_{\mathrm{\ψ}}+\frac{r_c-r_{co}}{cos\mathrm{\θ}}+\frac{{\mathrm{\ζ}}_r}{\cos \mathrm{\θ}}---\left(16\right)$

${\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\θ}}=-k_{\mathrm{\θ}}{\mathrm{\ζ}}_{\mathrm{\θ}}+(q_c-q_{co})+{\mathrm{\ζ}}_q---\left(17\right)$

And have ζ_ψ(0)=0, ζ_θ(0)=0, ζ_r=0, ζ_q=0.

Step 7. calculates current underwater robot position ηⁿ=(x, y, z) with the turning point WP demarcated_k=(x_k,y_k,z_k) between distanceIf less than the switching radius R set, then it represents that complete the tracing task in currently assigned path, otherwise continue step 2.

The present invention adopts filtering Backstepping to carry out underwater robot three-dimensional path tracing control, by designing second order filter, it is capable of the estimation to virtual controlling and its derivative signal, avoid the parsing derivation to virtual signal, introduce filtering compensation system and ensure the tracking accuracy of filtering signal, ensure that system tracking error converges on zero point based on Lyapunov stability theory, control performance is better than dynamic surface control.

The invention have the advantages that and effect:

1., by three-dimensional path tracking control problem, it is decomposed into the problem that position, attitude and speed loop are controlled respectively；

2. adopt the second order filter to obtain the filtering signal of virtual controlling and derivative form thereof, it is to avoid owing to needing the derivative form that step by step calculation intermediate virtual controls and the problem causing " item number expansion " in Backstepping design, to simplify controller design process；

3. by constructing filtering compensation system, it is ensured that the wave filter tracking accuracy to reference-input signal, it is achieved that system tracking error asymptotic convergence is to zero point；

4. the integral element introducing speed eliminates tracking signal steady-state error.

Accompanying drawing explanation

Fig. 1 provides the AUV three-dimensional path tracking control unit block diagram based on filtering Backstepping；

Fig. 2 robot three-dimensional path trace schematic diagram；

Fig. 3 second order filter structure chart；

Fig. 4 robot three-dimensional path trace track；

Fig. 5 robot three-dimensional path trace X/Y plane projects；

Fig. 6 robot three-dimensional path trace XZ plane projection；

Fig. 7 robot three-dimensional path trace curve of error；

Fig. 8 robot three-dimensional path trace speed responsive；

Fig. 9 robot three-dimensional path trace attitude angle responds；

Figure 10 robot three-dimensional path trace longitudinal velocity and compensation term；

Figure 11 robot three-dimensional path trace yaw angle speed and compensation term；

Figure 12 robot three-dimensional path trace pitch velocity and compensation term；

Figure 13 robot three-dimensional path trace bow is to angle and compensation term；

Figure 14 robot three-dimensional path trace Angle of Trim and compensation term；

Figure 15 robot three-dimensional path following control inputs.

Detailed description of the invention

Present invention is as follows based on the detailed description of the invention of the underwater robot three-dimensional path tracking and controlling method of second order filter:

1. Fig. 1 has marked the mutual relation of the status signal of AUV system, virtual controlling amount and filtering signal and derivative value thereof, forward loop obtains filtering signal and the derivative signal of virtual controlling mainly through second order filter, compensates the loop guarantee wave filter tracking accuracy to input signal by designing filtering error.

2. Fig. 3 is robot three-dimensional path trace schematic diagram, l_kFor expected path, { I}, { B} and { F} represents fixed coordinate system, robot carrier coordinate system and Serret-Frenet coordinate system respectively；P point is expected path l_kOn virtual guide, Q point represents robot centroid position, for given expected path l_kFor For the path parameter determined, with l_k{ F} is defined as coordinate system that { I} rotates ψ rotating around ζ axle and η for the moving coordinate system for initial point of upper virtual guide P_FAnd θ_FAngle, then translation makes fixed coordinate system initial point O overlap with P point on path to obtain, and the anglec of rotation is defined as here

WhereinFirst underwater robot three-dimensional path pursuit movement error model is provided

$\left\{\begin{array}{c}\stackrel{\·}{s}=re-qh+u-u_{r}cos{\mathrm{\ψ}}_{e}cos{\mathrm{\θ}}_e\\ \stackrel{\·}{e}=-rs+u_r{\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_e+v\\ \stackrel{\·}{h}=qs-u_{r}sin{\mathrm{\θ}}_e+w\end{array}\right.$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_e=\frac{r}{cos\mathrm{\θ}}-r_F\\ {\stackrel{\·}{\mathrm{\θ}}}_e=q-q_F\end{array}\right.---\left(4\right)$

Wherein variable s, e and h represent { forward direction of reference point, transverse direction and vertical tracking error on robot and expected path under B} coordinate system respectively, ψ and θ is the current bow of robot to angle and Angle of Trim, state variable u, v, w, q and r represent the longitudinal velocity of robot under carrier coordinate system, lateral velocity, vertical velocity, pitch velocity and yaw angle speed respectively；U_rFor virtual target translational speed, wherein

Ignore the impact that robot three-dimensional is moved by rolling, set up following robot with five degrees of freedom kinetic model.

$\begin{array}{c}\stackrel{\·}{u}=\frac{m_2}{m_1}vr-\frac{m_3}{m_1}wq-\frac{d_1}{m_1}u+\frac{1}{m_1}{F}_u+{\mathrm{\ω}}_1\\ \stackrel{\·}{v}=-\frac{m_1}{m_2}\mathrm{ur}-\frac{d_2}{m_2}v+{\mathrm{\ω}}_2\\ \stackrel{\·}{w}=\frac{m_1}{m_3}uq-\frac{d_3}{m_3}w+g_1+{\mathrm{\ω}}_3\\ \stackrel{\·}{q}=\frac{m_3-m_1}{m_5}uw-\frac{d_4}{m_5}q-g_2+\frac{1}{m_5}{b}_1{\mathrm{\δ}}_s+{\mathrm{\ω}}_4\\ \stackrel{\·}{r}=\frac{m_1-m_2}{m_6}uv-\frac{d_5}{m_6}r+\frac{1}{m_6}{b}_2{\mathrm{\δ}}_r+{\mathrm{\ω}}_5\end{array}---\left(5\right)$

Wherein

$m_1=m-X_{\stackrel{\·}{u}},m_2=m-Y_{\stackrel{\·}{v}},m_3=m-Z_{\stackrel{\·}{w}}$

$m_5=I_y-M_{\stackrel{\·}{q}},m_6=I_z-N_{\stackrel{\·}{r}}$

g₁=(W-B) cos θ, g₂=(z_gW-z_bB)sinθ

d₁=X_u+X_u|u||u|,d₂=Y_v+Y_v|v||v|

d₃=Z_w+Z_w|w||w|,d₄=M_q+M_q|q||q|

d₅=N_r+N_r|r||r|

$b_1=u^2{M}_{{\mathrm{\δ}}_s},b_2=u^2{N}_{{\mathrm{\δ}}_r}$

Wherein, state variable u, v, w, q and r represents the longitudinal velocity of robot under carrier coordinate system, lateral velocity, vertical velocity, pitch velocity and yaw angle speed respectively；M and m_(·)Represent robot quality and the additional mass produced by fluid matasomatism, I respectively_yFor the robot rotary inertia around y-axis, I_zFor the robot rotary inertia around z-axis, X_(·)、Y_(·)、Z_(·)、M_(·)And N_(·)For viscous fluid hydrodynamic force coefficient；Z_gAnd z_bThe respectively coordinate position of center of gravity and centre of buoyancy on vertical axis under carrier coordinate, W and B represents the gravity and buoyancy, d that robot is subject to respectively_(·)For nonlinear dampling hydrodynamic force item,WithFor hydroplane and vertical rudder steerage coefficient, control input F_u、δ_sAnd δ_rRepresent AUV propeller thrust, diving-plane angle and vertical rudder angle, ω respectively_(·)It is expressed as interference effect item.

3. the design process of step 3 median filter is

The calculating process of complexity is introduced in order to avoid virtual controlling amount is directly resolved derivation, utilize the characteristic of second order filter, using the virtual controlling amount reference input as wave filter, by integration but not the process of differential obtains its filtering signal and derivative value, filter construction definition is as follows:

$\left[\begin{array}{c}{x}_1\\ {\stackrel{\·}{x}}_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -{\mathrm{\ω}}_n^2& -2{\mathrm{\ζ\ω}}_n\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ {\mathrm{\ω}}_n^2\end{array}\right]\stackrel{\‾}{x}---\left(6\right)$

Wherein ${\left[\begin{array}{cc}{x}_1& x_2\end{array}\right]}^T={\left[\begin{array}{cc}{x}_c& {\stackrel{\·}{x}}_c\end{array}\right]}^T,$ For the reference-input signal of wave filter, above formula is linear stationary system, it is seen that whenFor there being dividing value, then x_cWithIt is continuous bounded signal, from input signalTo output signal x_cTransmission function be

$H\left(s\right)=\frac{X_c\left(s\right)}{\stackrel{\‾}{X}\left(s\right)}=\frac{{\mathrm{\ω}}_n^2}{s^2+2{\mathrm{\ζ\ω}}_{n}s+{\mathrm{\ω}}_n^2}---\left(7\right)$

Wherein ζ and ω_nRepresent damping ratio and natural angular frequency respectively；If signalBandwidth lower than H (s), then error signalWill be only small, it is assumed that knownWhen bandwidth, by selecting sufficiently large natural angular frequency ω_nJust it is obtained in that x_cAnd x_cAnd guarantee approximate errorOnly small.As can be seen from the above equation, signalBeing obtained by integral process but not additive process, this can greatly reduce based on measuring effect of noise in the control system of State Feedback Design, selects excessive ω simultaneously_nIncreasing again system high-frequency effect of noise, this is accomplished by considering, and selects rational ω_nMeet control performance.

4., for position tracking loop in step 6, guide thought according to the angle of sight and provide the expectation angle of sight guidance law of path trace

And longitudinal velocity, and design attitude tracking filter compensate system process be

For the kinematic controller u of position tracking error system (3) design robot, attitude angle ψ and θ virtual controlling amount respectively

u_co=-k₁s+u_rcosψ_cocosθ_co(8)

${\mathrm{\ψ}}_{co}=-\arcsin (k_{2}e/\sqrt{1+{\left(k_{2}e\right)}^2})---\left(9\right)$

${\mathrm{\θ}}_{co}=\arcsin (k_{3}h/\sqrt{1+{\left(k_{3}h\right)}^2})---\left(10\right)$

Wherein gain factor k₁> 0, k₂> 0, k₃> 0 is angle of sight guidance law normalized parameter, then system (3) becomes

$\left\{\begin{array}{c}\stackrel{\·}{s}=re-qh-k_{1}s\\ \stackrel{\·}{e}=-rs-u_r\frac{k_{2}e}{\sqrt{1+{\left(k_{2}e\right)}^2}}\frac{1}{\sqrt{1+{\left(k_{3}h\right)}^2}}+v\\ \stackrel{\·}{h}=qs-u_r\frac{k_{3}h}{\sqrt{1+{\left(k_{3}h\right)}^2}}+w\end{array}\right.---\left(11\right)$

For position tracking error systematic (3), construct lyapunov energy function

$V_1=\frac{1}{2}{l}^2---\left(12\right)$

WhereinTo equation (12) derivation, formula (11) is substituted into

$\begin{array}{c}{\stackrel{\·}{V}}_1=-k_1{s}^2-k_2{u}_r\frac{1}{\sqrt{1+{\left(k_{2}e\right)}^2}}\frac{1}{\sqrt{1+{\left(k_{3}h\right)}^2}}{e}^2\\ -k_3{u}_r\frac{1}{\sqrt{1+{\left(k_{3}e\right)}^2}}{h}^2+ev+hw\end{array}---\left(13\right)$

For avoiding Backstepping subsequent design needs θ_coAnd ψ_coEnter derivation, cause the deficiency of calculating " item number expansion ", define θ_c,And ψ_c,For ideal signal θ_coAnd ψ_coBy the filtering signal of second order filter and its derivative value, wave filter definition is as follows

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_c\\ {\stackrel{\·\·}{\mathrm{\θ}}}_c\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -{\mathrm{\ω}}_n^2& -2{\mathrm{\ζ\ω}}_n\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_c\\ {\stackrel{\·}{\mathrm{\θ}}}_c\end{array}\right]+\left[\begin{array}{c}0\\ {\mathrm{\ω}}_n^2\end{array}\right]{\mathrm{\θ}}_{co}---\left(14\right)$

The initial value θ of wave filter_c(0)=θ_co(0)；

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_c\\ {\stackrel{\·\·}{\mathrm{\ψ}}}_c\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -{\mathrm{\ω}}_n^2& -2{\mathrm{\ζ\ω}}_n\end{array}\right]\left[\begin{array}{c}{\mathrm{\ψ}}_c\\ {\stackrel{\·}{\mathrm{\ψ}}}_c\end{array}\right]+\left[\begin{array}{c}0\\ {\mathrm{\ω}}_n^2\end{array}\right]{\mathrm{\ψ}}_{co}---\left(15\right)$

The initial value ψ of wave filter_c(0)=ψ_co(0)；

Here definition position filtering tracking error signal

$\left[\begin{array}{c}\tilde{s}\\ \tilde{e}\\ \tilde{h}\end{array}\right]=\left[\begin{array}{c}s-s_c\\ e-e_c\\ h-h_c\end{array}\right]---\left(16\right)$

To formula (16) both sides derivation, formula (3) is substituted into

$\left[\begin{array}{c}\stackrel{\·}{\tilde{s}}\\ \stackrel{\·}{\tilde{e}}\\ \stackrel{\·}{\tilde{h}}\end{array}\right]=\left[\begin{array}{c}r\tilde{e}-q\tilde{h}\\ -r\tilde{s}\\ q\tilde{s}\end{array}\right]\[\begin{array}{ccc}A& ABg\left(\widetilde{\mathrm{\ψ}}\right)u_r& Cg\left(\widetilde{\mathrm{\θ}}\right)u_r\end{array}\]\left[\begin{array}{c}\tilde{u}\\ \widetilde{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}\end{array}\right]---\left(17\right)$

Wherein, definition filter tracking error is $\tilde{u}=u-u_c,\widetilde{\mathrm{\ψ}}={\mathrm{\ψ}}_e-{\mathrm{\ψ}}_c,\widetilde{\mathrm{\θ}}={\mathrm{\θ}}_e-{\mathrm{\θ}}_c,$

$A=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],B=\left[\begin{array}{cc}{\mathrm{cos\θ}}_e{\mathrm{cos\ψ}}_c& -{\mathrm{cos\θ}}_e{\mathrm{sin\ψ}}_c\\ {\mathrm{cos\θ}}_c{\mathrm{sin\ψ}}_c& {\mathrm{cos\θ}}_c{\mathrm{cos\ψ}}_c\\ 0& 0\end{array}\right]$

$C=\left[\begin{array}{cc}{\mathrm{cos\ψ}}_c{\mathrm{cos\θ}}_c& -{\mathrm{cos\ψ}}_c{\mathrm{sin\θ}}_c\\ {\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_c& -{\mathrm{sin\ψ}}_e{\mathrm{sin\θ}}_c\\ -{\mathrm{sin\θ}}_c& -{\mathrm{cos\θ}}_c\end{array}\right]$

$g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\ψ}}-1}{\widetilde{\mathrm{\ψ}}}\\ \frac{\sin \widetilde{\mathrm{\ψ}}}{\widetilde{\mathrm{\ψ}}}\end{array}\right],g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\θ}}-1}{\widetilde{\mathrm{\θ}}}\\ \frac{sin\widetilde{\mathrm{\θ}}}{\widetilde{\mathrm{\θ}}}\end{array}\right],$ And meet $\underset{\widetilde{\mathrm{\ψ}}\→0}{\lim }g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right],\underset{\widetilde{\mathrm{\θ}}\→0}{\lim }g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right]$ To the transposition of formula (16) left and right, increase and subtract one item missing ${\left[\begin{array}{ccc}{\stackrel{\·}{s}}_{co}& {\stackrel{\·}{e}}_{co}& {\stackrel{\·}{h}}_{co}\end{array}\right]}^T$ Obtain

$\left[\begin{array}{c}\stackrel{\·}{s}\\ \stackrel{\·}{e}\\ \stackrel{\·}{h}\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_{co}\end{array}\right]+\left[\begin{array}{c}\stackrel{\·}{\tilde{s}}\\ \stackrel{\·}{\tilde{e}}\\ \stackrel{\·}{\tilde{h}}\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{s}}_c-{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_c-{\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_c-{\stackrel{\·}{h}}_{co}\end{array}\right]---\left(18\right)$

According to Tracking Control Design target, select ideal expectation position signalling ${\left[\begin{array}{ccc}{\stackrel{\·}{s}}_{co}& {\stackrel{\·}{e}}_{co}& {\stackrel{\·}{h}}_{co}\end{array}\right]}^T$ For

$\left[\begin{array}{c}{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_{co}\end{array}\right]=\left[\begin{array}{c}-k_x\tilde{s}+{\stackrel{\·}{s}}_c\\ -k_y\tilde{e}+{\stackrel{\·}{e}}_c\\ -k_z\tilde{h}+{\tilde{h}}_c\end{array}\right]---\left(19\right)$

Definition angle filter tracking error signal is

$\left[\begin{array}{c}\tilde{u}\\ \widetilde{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}u-u_c\\ {\mathrm{\ψ}}_e-{\mathrm{\ψ}}_c\\ {\mathrm{\θ}}_e-{\mathrm{\θ}}_c\end{array}\right]---\left(20\right)$

Formula (19) and (17) are substituted into (18) arrange

$\left[\begin{array}{c}\stackrel{\·}{\tilde{s}}\\ \stackrel{\·}{\tilde{e}}\\ \stackrel{\·}{\tilde{h}}\end{array}\right]=\left[\begin{array}{c}r\tilde{e}-q\tilde{h}\\ -r\tilde{s}\\ q\tilde{s}\end{array}\right]+\left[\begin{array}{c}-k_x\tilde{s}\\ -k_y\tilde{e}\\ -k_z\tilde{h}\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{s}}_c-{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_c-{\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_c-{\stackrel{\·}{h}}_{co}\end{array}\right]+\[\begin{array}{ccc}A& Bg\left(\widetilde{\mathrm{\ψ}}\right)u_r& Cg\left(\widetilde{\mathrm{\θ}}\right)\end{array}{u}_r\]\left[\begin{array}{c}\tilde{u}\\ \widetilde{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}\end{array}\right]---\left(21\right)$

Definition filtering signal compensates error

$\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]=\left[\begin{array}{c}\tilde{s}-{\mathrm{\ζ}}_x\\ \tilde{e}-{\mathrm{\ζ}}_y\\ \tilde{h}-{\mathrm{\ζ}}_z\end{array}\right]---\left(22\right)$

The wherein dynamic ζ of construction location filtering compensation_x、ζ_yAnd ζ_zAs follows

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ζ}}}_x\\ {\stackrel{\·}{\mathrm{\ζ}}}_y\\ {\stackrel{\·}{\mathrm{\ζ}}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{r\ζ}}_y-{\mathrm{q\ζ}}_z\\ -{\mathrm{r\ζ}}_x\\ {\mathrm{q\ζ}}_x\end{array}\right]\left[\begin{array}{c}-k_x{\mathrm{\ζ}}_x\\ -k_y{\mathrm{\ζ}}_y\\ -k_z{\mathrm{\ζ}}_z\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{s}}_c-{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_c-{\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_c-{\stackrel{\·}{h}}_{co}\end{array}\right]+u_r\[\begin{array}{ccc}A& Bg\left(\widetilde{\mathrm{\ψ}}\right)& Cg\left(\widetilde{\mathrm{\θ}}\right)\end{array}\]\left[\begin{array}{c}{\mathrm{\ζ}}_u\\ {\mathrm{\ζ}}_{\mathrm{\ψ}}\\ {\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(23\right)$

Here ζ_x(0)=0, ζ_y(0)=0, ζ_z(0)=0.

5., for attitude angle tracking loop in step 6, the process of design angular velocity control law and attitude angle tracking filter compensation system is:

To filter tracking error type (20) derivation, attitude angle tracking error model formula (4) is substituted into

$\begin{array}{c}\stackrel{\·}{\widetilde{\mathrm{\ψ}}}=\frac{r}{cos\mathrm{\θ}}-r_F-{\stackrel{\·}{\mathrm{\ψ}}}_c\\ =\frac{r_{co}+(r_c-r_{co})+\tilde{r}}{\cos \mathrm{\θ}}-r_F-{\stackrel{\·}{\mathrm{\ψ}}}_c\end{array}---\left(24\right)$

$\begin{array}{c}\stackrel{\·}{\widetilde{\mathrm{\θ}}}=q-q_F-{\stackrel{\·}{\mathrm{\θ}}}_c\\ =q_{co}+(q_c-q_{co})+\tilde{q}-q_F-{\stackrel{\·}{\mathrm{\θ}}}_c\end{array}---\left(25\right)$

Wherein angular velocity tracking error is defined asHere ideal virtual control signal r is separately designed_coAnd q_coFor

$r_{co}=cos\mathrm{\θ}(r_F+{\stackrel{\·}{\mathrm{\ψ}}}_c-k_{\mathrm{\ψ}}\widetilde{\mathrm{\ψ}}-{\mathrm{\ψ}}_{bs})---\left(26\right)$

$q_{co}=q_F+{\stackrel{\·}{\mathrm{\θ}}}_c-k_{\mathrm{\θ}}\widetilde{\mathrm{\θ}}-{\mathrm{\θ}}_{bs}---\left(27\right)$

Here q_cWithr_cWithRespectively ideal signal q_coAnd r_coThe filtering signal obtained by second order filter and its derivative value, definition filter form is

$\left[\begin{array}{c}{\stackrel{\·}{q}}_c\\ {\stackrel{\·\·}{q}}_c\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -{\mathrm{\ω}}_n^2& -2{\mathrm{\ζ\ω}}_n\end{array}\right]\left[\begin{array}{c}{q}_c\\ {\stackrel{\·}{q}}_c\end{array}\right]+\left[\begin{array}{c}0\\ {\mathrm{\ω}}_n^2\end{array}\right]q_{co}---\left(28\right)$

Wave filter initial condition q_c(0)=q_co(0)；

$\left[\begin{array}{c}{\stackrel{\·}{r}}_c\\ {\stackrel{\·\·}{r}}_c\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -{\mathrm{\ω}}_n^2& -2{\mathrm{\ζ\ω}}_n\end{array}\right]\left[\begin{array}{c}{r}_c\\ {\stackrel{\·}{r}}_c\end{array}\right]+\left[\begin{array}{c}0\\ {\mathrm{\ω}}_n^2\end{array}\right]r_{co}---\left(29\right)$

Wave filter initial condition r_c(0)=r_co(0)；

Formula (26) and formula (27) are substituted into formula (24) and (25) obtain

$\stackrel{\·}{\widetilde{\mathrm{\ψ}}}=-k_{\mathrm{\ψ}}\widetilde{\mathrm{\ψ}}+\frac{(r_c-r_{co})+\tilde{r}}{\cos \mathrm{\θ}}-{\mathrm{\ψ}}_{bs}---\left(30\right)$

$\stackrel{\·}{\widetilde{\mathrm{\θ}}}=-k_{\mathrm{\θ}}\widetilde{\mathrm{\θ}}+(q_c-q_{co})+\tilde{q}-{\mathrm{\θ}}_{bs}---\left(31\right)$

Wherein k_ψ> 0 and k_θ> 0 is controller parameter, ψ_bsAnd θ_bsFor calm item to be designed, here by definition compensation tracking error, the signal that wave filter is exported carries out feedback compensation

$\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\ψ}}\\ {\mathrm{\υ}}_{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}\widetilde{\mathrm{\ψ}}-{\mathrm{\ζ}}_{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}-{\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(32\right)$

Here structure filtering compensation is dynamic

${\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\ψ}}=-k_{\mathrm{\ψ}}{\mathrm{\ζ}}_{\mathrm{\ψ}}+\frac{r_c-r_{co}}{cos\mathrm{\θ}}+\frac{{\mathrm{\ζ}}_r}{\cos \mathrm{\θ}}---\left(33\right)$

${\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\θ}}=-k_{\mathrm{\θ}}{\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\θ}}+(q_c-q_{co})+{\mathrm{\ζ}}_q---\left(34\right)$

Here initial condition ζ_ψ(0)=0, ζ_θ(0)=0, ζ_r=0, ζ_q=0.

6. for speed control loop in step 6, introducing integral element and reduce steady-state error, the detailed process of design true control input is as follows:

For ensureing that tracking system exists the robustness under outer interference, introducing integral term increases the robustness of system, definition Here design robot three-dimensional path tracking control unit is

$\left\{\begin{array}{c}{F}_u=m_1(-k_u\tilde{u}-k_{iu}{\mathrm{\ϵ}}_1+{\stackrel{\·}{u}}_c-u_{bs})-f_u\\ {\mathrm{\δ}}_s=b_1^{-1}\[m_4(-k_q\tilde{q}-k_{iq}{\mathrm{\ϵ}}_2+{\stackrel{\·}{q}}_c-q_{bs})-f_q\]\\ {\mathrm{\δ}}_r=b_2^{-1}\[m_5(-k_r\tilde{r}-k_{ir}{\mathrm{\ϵ}}_3+{\stackrel{\·}{r}}_c-r_{bs})-f_r\]\end{array}\right.---\left(35\right)$

Wherein f_u=m₂vr-m₃wq+d₁u、f_q=(m₁-m₃)uw+d₄q-g₂And f_r=(m₁-m₂)uv+d₅R is model nonlinear hydrodynamic force item, u_bs、q_bsAnd r_bsIt is designed in step 6 for feedback compensation robust item to be designed.

7. in step 6, the detailed process of feedback control item design is as follows:

Firstly for positioning control system convolution (22) structure lyapunov energy function in step 2

$V_1=\frac{1}{2}({\mathrm{\υ}}_x^2+{\mathrm{\υ}}_y^2+{\mathrm{\υ}}_z^2)---\left(36\right)$

To above formula derivation, formula (21) and (23) are substituted into formula (36) and obtains

$\begin{array}{c}{V}_1={\stackrel{\·}{\mathrm{\υ}}}_x{\mathrm{\υ}}_x+{\stackrel{\·}{\mathrm{\υ}}}_y{\mathrm{\υ}}_y+{\stackrel{\·}{\mathrm{\υ}}}_z{\mathrm{\υ}}_z\\ =-k_x{\mathrm{\υ}}_x^2-k_y{\mathrm{\υ}}_y^2-k_z{\mathrm{\υ}}_z^2+\\ \[\begin{array}{ccc}{\mathrm{\υ}}_x& {\mathrm{\υ}}_y& {\mathrm{\υ}}_z\end{array}\]\[\begin{array}{ccc}A& Bg\left(\widetilde{\mathrm{\ψ}}\right)u_r& Cg\left(\widetilde{\mathrm{\θ}}\right)u_r\end{array}\]\left[\begin{array}{c}{\mathrm{\υ}}_u\\ {\mathrm{\υ}}_{\mathrm{\ψ}}\\ {\mathrm{\υ}}_{\mathrm{\θ}}\end{array}\right]\\ =-k_x{\mathrm{\υ}}_x^2-k_y{\mathrm{\υ}}_y^2-k_z{\mathrm{\υ}}_z^2+A^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_u+\\ g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\ψ}}+g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\θ}}\end{array}---\left(37\right)$

Wherein υ_u、υ_ψ、υ_θDefinition is such as formula (32).

Then lyapunov energy function is constructed for posture tracing system convolution (32)

$V_2=\frac{1}{2}({\mathrm{\υ}}_{\mathrm{\ψ}}^2+{\mathrm{\υ}}_{\mathrm{\θ}}^2)---\left(38\right)$

To above formula derivation, formula (30)～(34) are substituted into:

$\begin{array}{c}{\stackrel{\·}{V}}_2={\stackrel{\·}{\mathrm{\υ}}}_{\mathrm{\ψ}}{\mathrm{\υ}}_{\mathrm{\ψ}}+{\stackrel{\·}{\mathrm{\υ}}}_0{\mathrm{\υ}}_0\\ =(\stackrel{\·}{\widetilde{\mathrm{\ψ}}}-{\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\ψ}}){\mathrm{\υ}}_{\mathrm{\ψ}}+(\stackrel{\·}{\widetilde{\mathrm{\θ}}}-{\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\θ}}){\mathrm{\υ}}_{\mathrm{\θ}}\\ =(-k_{\mathrm{\ψ}}\widetilde{\mathrm{\ψ}}+\frac{\tilde{r}}{\cos \mathrm{\θ}}-{\mathrm{\ψ}}_{bs}+k_{\mathrm{\ψ}}{\mathrm{\ζ}}_{\mathrm{\ψ}}-\frac{{\mathrm{\ζ}}_r}{\cos \mathrm{\θ}}){\mathrm{\υ}}_{\mathrm{\ψ}}\\ +(-k_{\mathrm{\θ}}\widetilde{\mathrm{\θ}}+\tilde{q}-{\mathrm{\θ}}_{bs}+k_{\mathrm{\θ}}{\mathrm{\ζ}}_{\mathrm{\θ}}-{\mathrm{\ζ}}_q){\mathrm{\υ}}_0\\ =-k_{\mathrm{\ψ}}{\mathrm{\υ}}_{\mathrm{\ψ}}^2-k_{\mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\θ}}^2+{\mathrm{\υ}}_q{\mathrm{\υ}}_{\mathrm{\θ}}+\frac{{\mathrm{\υ}}_r}{\cos \mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\ψ}}\\ -{\mathrm{\θ}}_{bs}{\mathrm{\υ}}_{\mathrm{\θ}}-{\mathrm{\ψ}}_{bs}{\mathrm{\υ}}_{\mathrm{\ψ}}\end{array}---\left(39\right)$

Wherein ${\mathrm{\υ}}_u=\tilde{u}-{\mathrm{\ζ}}_u,{\mathrm{\υ}}_r=\tilde{r}-{\mathrm{\ζ}}_r,{\mathrm{\υ}}_q=\tilde{q}-{\mathrm{\ζ}}_q.$

Again for speed control loop, formula (35) substitution formula (5) is obtained u, the error system of q and r is

$\left\{\begin{array}{c}\stackrel{\·}{\tilde{u}}=-k_u\tilde{u}-k_{iu}{\mathrm{\ϵ}}_1-u_{bs}\\ \stackrel{\·}{\tilde{q}}=-k_q\tilde{q}-k_{iq}{\mathrm{\ϵ}}_2-q_{bs}\\ \stackrel{\·}{\tilde{r}}=-k_r\tilde{r}-k_{ir}{\mathrm{\ϵ}}_3-r_{bs}\end{array}\right.---\left(40\right)$

Due to ζ_u=ζ_r=ζ_q=0, what obtain filtering compensation error system here is dynamically

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\υ}}}_u=-k_u{\mathrm{\υ}}_u-k_{iu}{\mathrm{\ϵ}}_1-u_{bs}\\ {\stackrel{\·}{\mathrm{\υ}}}_q=-k_q{\mathrm{\υ}}_q-k_{iq}{\mathrm{\ϵ}}_2-q_{bs}\\ {\stackrel{\·}{\mathrm{\υ}}}_r=-k_r{\mathrm{\υ}}_r-k_{ir}{\mathrm{\ϵ}}_3-r_{bs}\end{array}\right.---\left(41\right)$

Due toSo system (41) can be rewritten as

$\left\{\begin{array}{c}{\stackrel{\·\·}{\mathrm{\ϵ}}}_1=-k_u{\stackrel{\·}{\mathrm{\ϵ}}}_1-k_{iu}{\mathrm{\ϵ}}_1-u_{bs}\\ {\stackrel{\·\·}{\mathrm{\ϵ}}}_2=-k_q{\stackrel{\·}{\mathrm{\ϵ}}}_2-k_{iq}{\mathrm{\ϵ}}_2-q_{bs}\\ {\stackrel{\·\·}{\mathrm{\ϵ}}}_3=-k_r{\stackrel{\·}{\mathrm{\ϵ}}}_3-k_{ir}{\mathrm{\ϵ}}_3-r_{bs}\end{array}\right.---\left(42\right)$

Definition error vector ε=[ε₁,ε₂,ε₃]^T,Then system (42) can be expressed as

$\stackrel{\·}{E}=AE+BU---\left(43\right)$

WhereinK_I=diag{-k_iu,-k_iq,-k_ir, K_P=diag{-k_u,-k_q,-k_r}

Last convolution (36), (43) and formula (38) structure lyapunov energy function

$V_3=V_1+V_2+\frac{1}{2}{E}^{T}PE---\left(44\right)$

Wherein positive definite symmetric matrices P is the solution of linear Lyapunov Equation

A^TP+PA=-Q (45)

Wherein $P=\left[\begin{array}{cc}{P}_1& 0_{3\×3}\\ 0_{3\×3}& P_2\end{array}\right],$ P_i=diag{p_i1,p_i2,p_i3, i=1,2 is positive definite symmetric matrices, if selecting P₁=K_IP₂, then $Q=\left[\begin{array}{cc}{0}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 2K_I{P}_2\end{array}\right];$ Formula (44) is carried out derivation, formula (37), (39) and (43) is substituted into and arranges

$\begin{array}{c}{\stackrel{\·}{V}}_3={\stackrel{\·}{V}}_1+{\stackrel{\·}{V}}_2+E^{T}P\stackrel{\·}{E}\\ =-k_x{\mathrm{\υ}}_x^2-k_y{\mathrm{\υ}}_y^2-k_z{\mathrm{\υ}}_z^2-k_u{\mathrm{\υ}}_u^2-k_{\mathrm{\ψ}}{\mathrm{\υ}}_{\mathrm{\ψ}}^2-k_{\mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\θ}}^2\\ -\frac{1}{2}{E}^{T}QE+A^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_u+g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\ψ}}\\ -{\mathrm{\ψ}}_{bs}{\mathrm{\υ}}_{\mathrm{\ψ}}+\frac{{\mathrm{\υ}}_r}{\cos \mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\ψ}}+g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\θ}}\\ +{\mathrm{\υ}}_q{\mathrm{\υ}}_{\mathrm{\θ}}-{\mathrm{\θ}}_{bs}{\mathrm{\υ}}_{\mathrm{\θ}}+E^{T}PBU\end{array}---\left(46\right)$

Formula (46) is further changed to

$\begin{array}{c}{\stackrel{\·}{V}}_3=-k_x{\mathrm{\υ}}_x^2-k_y{\mathrm{\υ}}_y^2-k_z{\mathrm{\υ}}_z^2-k_u{\mathrm{\υ}}_u^2-k_{\mathrm{\ψ}}{\mathrm{\υ}}_{\mathrm{\ψ}}^2-k_{\mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\θ}}^2\\ -\frac{1}{2}{E}^{T}QE+A^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_u+g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\ψ}}\\ +{\mathrm{\υ}}_q{\mathrm{\υ}}_{\mathrm{\θ}}+\frac{{\mathrm{\υ}}_r}{\cos \mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\ψ}}+g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\θ}}-{\mathrm{\ψ}}_{bs}{\mathrm{\υ}}_{\mathrm{\ψ}}\\ -{\mathrm{\θ}}_{bs}{\mathrm{\υ}}_{\mathrm{\θ}}-p_{21}{\mathrm{\υ}}_u{u}_{bs}-p_{22}{\mathrm{\υ}}_q{q}_{bs}-p_{23}{\mathrm{\υ}}_r{r}_{bs}\end{array}---\left(47\right)$

If design of feedback compensation term is

${\mathrm{\ψ}}_{bs}=g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(48\right)$

${\mathrm{\θ}}_{bs}=g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(49\right)$

$u_{bs}=\frac{1}{p_{21}}{A}^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(50\right)$

$r_{bs}=\frac{1}{p_{23}}\frac{{\mathrm{\υ}}_{\mathrm{\ψ}}}{cos\mathrm{\θ}}---\left(51\right)$

$q_{bs}=\frac{1}{p_{22}}{\mathrm{\υ}}_{\mathrm{\θ}}---\left(52\right)$

Formula (48) (48)～(52) (52) is substituted into formula (47) (47) arrange

$\begin{array}{c}{\stackrel{\·}{V}}_3=-k_x{\mathrm{\υ}}_x^2-k_y{\mathrm{\υ}}_y^2-k_z{\mathrm{\υ}}_z^2-k_{\mathrm{\ψ}}{\mathrm{\υ}}_{\mathrm{\ψ}}^2-k_{\mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\θ}}^2\\ -\frac{1}{2}{E}^{T}QE\≤0\end{array}---\left(53\right)$

Above-mentioned theorem proving compensation tracking error system υ_iExponential Convergence, by the design process of second order filter it can be seen that when selecting suitable natural frequency ω_n, x_coFor the reference-input signal of wave filter, wave filter is linear stationary system, it is seen that work as x_coFor there being dividing value, then x_cWithIt is continuous bounded signal, if signal x_coBandwidth lower than design of filter bandwidth, then error signal | x_co(t)-x_c(t) | will be only small, due to ζ_iIt is single order stable linear system, so ζ_iNull value will be leveled off to, thus system tracking error exponential approach is in null value.Owing to introducing wave filter in controlling loop, the tracking accuracy of wave filter directly affects the control performance of system, and dynamic surface control, owing to not considering the tracking accuracy of filtering signal, is merely able to ensure that system tracking error converges to the neighborhood that initial point is less.Here by structure filtering compensation system, improve filtering signal tracking accuracy, ensure that closed loop system tracking error converges to zero point by stability analysis.

Simulating, verifying

It is exemplified below, the effectiveness of checking the inventive method:

Set up robot six degree of freedom simulation model according to hydrodynamic force coefficient, adopt Matlab environmental structure robot three-dimensional path following control analogue system.

For robot spiral dive operation, planning expectation three-dimensional curve path is (unit: m)

The initial position choosing robot is [x, y, z]^T=[10 ,-5,1]^TM (), initial bow is to for ψ=π/4 (rad), Angle of Trim θ=0 (rad), and robot initial speed is [u, v, w]^T=0 (m/s), initial angular velocity q=0 (rad/s), r=0 (rad/s), controller parameter k_x=k_y=k_z2=, k_ψ=k_θ=5, k_u=k_q=k_r=20, k_iu=k_iq=k_ir=10, p₂₁=p₂₂=p₂₃=5.

In order to avoid virtual controlling amount directly being resolved derivation, introducing complicated calculating process, utilizing the characteristic of second order filter herein, it would be desirable to virtual controlling amount a_coAs the reference input of wave filter, signalBeing obtained by integration but not the process of differential, this can greatly reduce based on measuring effect of noise in the control system of State Feedback Design, it is assumed that at known a_coWhen bandwidth, by selecting sufficiently large natural angular frequency ω_nJust it is obtained in that a_cWithAnd guarantee approximate error | α_co(t)-α_c(t) | only small；Select excessive ω simultaneously_nIncreasing again system high-frequency effect of noise, this is accomplished by considering, and selects rational ω_nMeet control performance, choose ζ=0.9, ω here_n=20；

Fig. 2～Figure 13 provides robot three-dimensional curved path tracing control simulation comparison result.

Fig. 2 is robot three-dimensional spiral dive path trace track, and Fig. 3 and Fig. 4 respectively robot three-dimensional path trace track is in the drop shadow curve of X/Y plane and XZ plane.It can be seen that due to conventional Backstepping be applied to true model based on the controller that mathematical models designs time, directly virtual controlling derivation is obtained its derivative obtains analytical form, effect is controlled poor when there is model uncertainty and measuring noise, and herein based on the gamma controller of design of filter, obtain the filter value of virtual controlling and derivative value by integral process but not additive process thus to measuring noise, there is certain filter action, by filtering compensation system, ensure that filter value the approaching time of day of ideal virtual controlled quentity controlled variable, and then compensate the condition responsive of nominal model and the deviation of true model condition responsive, model uncertainty is had good robustness, can be good at realizing tracing control, improve tracking accuracy.

Fig. 5 is tracking error curve in robot three-dimensional path following control, compared with conventional Backstepping controller, can be seen that the three-dimensional path controller designed herein improves the precision of path trace, shorten the redundancy voyage of robot, there is more stable control ability and ensure that robot follows the tracks of faster and converges to expected path, the tracking error is made finally to converge to zero, it was shown that controller has good tracking accuracy and response speed.

In Fig. 6 and Fig. 7 respectively robot three-dimensional, in path following control process, each state variable includes the change curve of linear velocity and attitude angle, can be seen that robot is less compared to longitudinal velocity at lateral velocity and vertical velocity along helix dive process, and for there being dividing value, conventional Backstepping design process cannot the measurement noise for the treatment of system state, and based on the controller of design of filter, the noise of measuring of system is had certain filter effect herein.

Fig. 8 is robot longitudinal velocity u, ideal virtual controlled quentity controlled variable u_coWith its filtering signal u_cResponse curve, from partial enlarged drawing, can be seen that filtering signal u_cFollow the tracks of ideal virtual signal u preferably_co, wave filter has certain filter action, u for the state u noise of measuring comprised_bsFor filtering compensation item, the u when following the tracks of system stability_bsFinally stablize and converge on zero.Fig. 9 is robot yaw angle speed r, ideal virtual controlled quentity controlled variable r_coWith its filtering signal r_cResponse curve, from partial enlarged drawing, can be seen that filtering signal r_cFollow the tracks of ideal virtual signal r preferably_co, wave filter has certain filter action, r for the yaw angle speed r noise of measuring comprised_bsFor filtering compensation item, the r when following the tracks of system stability_bsFinally converge to zero.

Figure 10 is robot pitch velocity q, ideal virtual controlled quentity controlled variable q_coWith its filtering signal q_cResponse curve, from partial enlarged drawing, can be seen that filtering signal q_cFollow the tracks of ideal virtual signal q preferably_co, wave filter has certain filter action, q for the yaw angle speed q noise of measuring comprised_bsFor filtering compensation item, the q when following the tracks of system stability_bsFinal stable convergence is in zero.

Figure 11 and 12 respectively filter the robot yaw angle in Backstepping design and Angle of Trim, desirable control signal and filtering signal change curve, can be seen that filtering signal ψ from partial enlarged drawing_cAnd θ_cFollow the tracks of ideal virtual signal ψ preferably_coAnd θ_co, wave filter has certain filter action for the noise of measuring comprised in yaw angle ψ and Angle of Trim θ, from filtering compensation item ψ_bsAnd θ_bsVariation tendency it can be seen that when follow the tracks of system stability time ψ_bsAnd θ_bsTo finally converge to zero point.

Figure 13 is the input response of robot three-dimensional path following control, and the method response curve of the present invention is more smooth as seen from the figure.

Claims (5)

1. the underwater robot three-dimensional path tracking and controlling method based on second order filter, it is characterised in that comprise the steps of

Step 1. sets up fixed coordinate system, robot carrier coordinate system and Serret-Frenet coordinate system, obtains expected path, and underwater robot starts path trace, completes the initialization of two second order filters；

Fixed sonar sensor that step 2. is carried by underwater robot, attitude transducer, gather underwater robot current location, attitude angle, angular velocity and speed data information, and in conjunction with the direction of expected path and speed, guide thought according to the angle of sight and calculate and obtain the desirable attitude control quantity ψ of underwater robot_co、θ_co, and ideal velocity controlled quentity controlled variable u_co；

The desirable controlled quentity controlled variable ψ that step 3. will obtain in step 2_co、θ_co、u_coInput, to the second order filter set up based on underwater robot three-dimensional path pursuit movement error model, obtains attitude and the rate controlling amount ψ of underwater robot_c、θ_c、u_c, and derivativeIn conjunction with robot motion variable ψ, θ, u, obtain filtering attitude and the speed Tracking margin of error With desirable angle rate controlling amount r_co、q_co；

The underwater robot desirable angle rate controlling amount r that step 4. will obtain in step 3_co、q_coInput, to another second order filter set up based on underwater robot three-dimensional path pursuit movement error model, obtains the angular velocity controlled quentity controlled variable r of underwater robot_c、q_c, and derivativeIn conjunction with robot angular movement variable r, q, obtain filtering angular velocity tracking error amount

Step 5. utilizes the filtering attitude and the speed Tracking margin of error that obtain in step 3And the filtering angular velocity tracking error amount obtained in step 4Resolving obtains underwater robot propeller thrust F_u, with diving-plane angleVertical rudder angle δ_r, it is respectively acting on robot propeller and steering wheel, it is achieved three-dimensional path tracing control；

Step 6. utilizes the underwater robot attitude and rate controlling amount ψ that obtain in step 3_c、θ_c、u_c, filtering attitude and the speed Tracking margin of errorWith desirable angle rate controlling amount r_co、q_co, the angular velocity controlled quentity controlled variable r of the underwater robot obtained in integrating step 4_c、q_c, and filtering angular velocity tracking error amountStructure filtering error compensates loop；

Step 7. calculates current underwater robot position ηⁿ=(x, y, z) with the turning point WP demarcated_k=(x_k,y_k,z_k) between distanceIf less than the switching radius R set, then it represents that complete the tracing task in currently assigned path, otherwise continue step 2.

2. the underwater robot three-dimensional path tracking and controlling method based on second order filter according to claim 1, it is characterised in that the desirable attitude control quantity ψ of underwater robot involved in step 2_co、θ_co, and ideal velocity controlled quentity controlled variable u_coCalculation expression be:

${\mathrm{\ψ}}_{co}=-arcsin(k_{2}e/\sqrt{1+{\left(k_{2}e\right)}^2})---\left(1\right)$

${\mathrm{\θ}}_{co}=arcsin\left(k_{3}h/\sqrt{1+{\left(k_{3}h\right)}^2}\right)---\left(2\right)$

u_co=-k₁s+u_rcosψ_cocosθ_co(3)

Wherein gain factor k₁> 0, k₂> 0, k₃> 0 is angle of sight guidance law normalized parameter, and variable s, e and h represent robot and the forward direction of expected path reference point, transverse direction and vertical tracking error under robot carrier coordinate system respectively.

3. the underwater robot three-dimensional path tracking and controlling method based on second order filter according to claim 1, it is characterised in that step 3, underwater robot three-dimensional path pursuit movement error model involved in 4 be:

$\left\{\begin{array}{c}\stackrel{\·}{s}=re-qh+u-u_r{\mathrm{cos\ψ}}_e{\mathrm{cos\θ}}_e\\ \stackrel{\·}{e}=-rs+u_r{\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_e+v\\ \stackrel{\·}{h}=qs-u_r{\mathrm{sin\θ}}_e+w\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_e=\frac{r}{cos\mathrm{\θ}}-r_F\\ {\stackrel{\·}{\mathrm{\θ}}}_e=q-q_F\end{array}\right.---\left(5\right)$

Wherein ψ_e=ψ-ψ_F, θ_e=θ-θ_F,Robot longitudinal velocity u, lateral velocity v and vertical velocity w, yaw angle speed r and pitch velocity q, u_rFor the desired speed of virtual guide point on expected path to be designed, its direction is along the tangential direction of curved path；ψ_FFor u_rThe angle of velocity attitude and fixed coordinate system trunnion axis, θ_FFor u_rThe angle of velocity attitude and fixed coordinate system vertical axis；ψ be robot bow to angle, θ is robot Angle of Trim.

4. the underwater robot three-dimensional path tracking and controlling method based on second order filter according to claim 1, it is characterised in that the underwater robot propeller thrust F related in step 5_u, with diving-plane angle δ_s, vertical rudder angle δ_rExpression formula be:

$\left\{\begin{array}{c}{F}_u=m_1\left(-k_u\tilde{u}-k_{iu}{\mathrm{\ϵ}}_1+{\stackrel{\·}{u}}_c-u_{bs}\right)-f_u\\ {\mathrm{\δ}}_s=b_1^{-1}\[m_4\left(-k_q\stackrel{\‾}{q}-k_{iq}{\mathrm{\ϵ}}_2+{\stackrel{\·}{q}}_c-q_{bs}\right)-f_q\]\\ {\mathrm{\δ}}_r=b_2^{-1}\[m_5\left(-k_r\tilde{r}-k_{ir}{\mathrm{\ϵ}}_3+{\stackrel{\·}{r}}_c-r_{bs}\right)-f_r\]\end{array}\right.---\left(6\right)$

Whereinf_u、f_qAnd f_rFor model nonlinear hydrodynamic force item；U_bs、q_bsAnd r_bsFor feedback compensation robust item；M₁、m₄、m₅The additional mass respectively produced by fluid；K_u、k_q、k_r、k_iu、k_iqAnd k_irIt is controller parameter；

Involved underwater human occupant dynamic model is:

$\stackrel{\·}{u}=\frac{m_2}{m_1}vr-\frac{m_3}{m_1}wq-\frac{d_1}{m_1}u+\frac{1}{m_1}{F}_u+{\mathrm{\ω}}_1$

$\stackrel{\·}{v}=-\frac{m_1}{m_2}ur-\frac{d_2}{m_2}v+{\mathrm{\ω}}_2$

$\stackrel{\·}{w}=\frac{m_1}{m_3}uq-\frac{d_3}{m_3}w+g_1+{\mathrm{\ω}}_3---\left(7\right)$

$\stackrel{\·}{q}=\frac{m_3-m_1}{m_5}uw-\frac{d_4}{m_5}q-g_2+\frac{1}{m_5}{b}_1{\mathrm{\δ}}_s+{\mathrm{\ω}}_4$

$\stackrel{\·}{r}=\frac{m_1-m_2}{m_6}uv-\frac{d_5}{m_6}r+\frac{1}{m_6}{b}_2{\mathrm{\δ}}_r+{\mathrm{\ω}}_5$

Wherein

$m_1=m-X_{\stackrel{\·}{u}},m_2=m-Y_{\stackrel{\·}{v}},m_3=m-Z_{\stackrel{\·}{w}}$

$m_5=I_y-M_{\stackrel{\·}{q}},m_6=I_z-N_{\stackrel{\·}{r}}$

g₁=(W-B) cos θ, g₂=(z_gW-z_bB)sinθ

d₁=X_u+X_u|u||u|,d₂=Y_v+Y_v|v||v|

d₃=Z_w+Z_w|w||w|,d₄=M_q+M_q|q||q|

d₅=N_r+N_r|r||r|

$b_1=u^2{M}_{{\mathrm{\δ}}_s},b_2=u^2{N}_{{\mathrm{\δ}}_r}$

Wherein, m and m_(·)Represent robot quality and the additional mass produced by fluid matasomatism, I respectively_yFor the robot rotary inertia around y-axis, I_zFor the robot rotary inertia around z-axis, X_(·)、Y_(·)、Z_(·)、M_(·)And N_(·)For viscous fluid hydrodynamic force coefficient；Z_gAnd z_bThe respectively coordinate position of center of gravity and centre of buoyancy on vertical axis under carrier coordinate, W and B represents the gravity and buoyancy, d that robot is subject to respectively_(·)For nonlinear dampling hydrodynamic force item,WithFor hydroplane and vertical rudder steerage coefficient, ω_(·)It is expressed as interference effect item.

5. the underwater robot three-dimensional path tracking and controlling method based on second order filter according to claim 1, it is characterised in that the filtering error related in step 6 compensates the expression formula of error compensation robust item in loop and is:

${\mathrm{\ψ}}_{bs}=g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(8\right)$

${\mathrm{\θ}}_{bs}=g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(9\right)$

$u_{bs}=\frac{1}{p_{21}}{A}^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(10\right)$

$r_{bs}=\frac{1}{p_{23}}\frac{{\mathrm{\υ}}_{\mathrm{\ψ}}}{cos\mathrm{\θ}}---\left(11\right)$

$q_{bs}=\frac{1}{p_{22}}{\mathrm{\υ}}_{\mathrm{\θ}}---\left(12\right)$

Wherein, p₂₁、p₂₂、p₂₃For the element in the dematrix of Lyapunov Equation；

$A=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],B=\left[\begin{array}{cc}{\mathrm{cos\θ}}_e{\mathrm{cos\ψ}}_c& -{\mathrm{cos\θ}}_e{\mathrm{sin\ψ}}_c\\ {\mathrm{cos\θ}}_c{\mathrm{sin\ψ}}_c& {\mathrm{cos\θ}}_c{\mathrm{cos\ψ}}_c\\ 0& 0\end{array}\right],C=\left[\begin{array}{cc}{\mathrm{cos\ψ}}_c{\mathrm{cos\θ}}_c& -{\mathrm{cos\ψ}}_c{\mathrm{sin\θ}}_c\\ {\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_c& -{\mathrm{sin\ψ}}_e{\mathrm{sin\θ}}_c\\ -{\mathrm{sin\θ}}_c& -{\mathrm{cos\θ}}_c\end{array}\right],$

$g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\ψ}}-1}{\widetilde{\mathrm{\ψ}}}\\ \frac{sin\widetilde{\mathrm{\ψ}}}{\widetilde{\mathrm{\ψ}}}\end{array}\right],g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\θ}}-1}{\widetilde{\mathrm{\θ}}}\\ \frac{sin\widetilde{\mathrm{\θ}}}{\widetilde{\mathrm{\θ}}}\end{array}\right],$ And meet $\underset{\widetilde{\mathrm{\ψ}}\→0}{\lim }g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right],\underset{\widetilde{\mathrm{\θ}}\→0}{\lim }g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right];$

Involved position filtering signal compensation error is

$\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]=\left[\begin{array}{c}\tilde{s}-{\mathrm{\ζ}}_x\\ \tilde{e}-{\mathrm{\ζ}}_y\\ \tilde{h}-{\mathrm{\ζ}}_z\end{array}\right]---\left(13\right)$

In formula, position filtering compensates and dynamically measures ζ_x、ζ_yAnd ζ_zExpression formula is

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ζ}}}_x\\ {\stackrel{\·}{\mathrm{\ζ}}}_y\\ {\stackrel{\·}{\mathrm{\ζ}}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{r\ζ}}_y-{\mathrm{q\ζ}}_z\\ -{\mathrm{r\ζ}}_x\\ {\mathrm{q\ζ}}_x\end{array}\right]\left[\begin{array}{c}-k_x{\mathrm{\ζ}}_x\\ -k_y{\mathrm{\ζ}}_y\\ -k_z{\mathrm{\ζ}}_z\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{s}}_c-{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_c-{\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_c-{\stackrel{\·}{h}}_{co}\end{array}\right]+u_r\left[\begin{array}{ccc}A& Bg\left(\widetilde{\mathrm{\ψ}}\right)& Cg\left(\widetilde{\mathrm{\θ}}\right)\end{array}\right]\left[\begin{array}{c}{\mathrm{\ζ}}_u\\ {\mathrm{\ζ}}_{\mathrm{\ψ}}\\ {\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(14\right)$

And have ζ_x(0)=0, ζ_y(0)=0, ζ_z(0)=0；S_co、e_co、h_coFor expectation position signalling, s_c、e_c、h_cFor position control signal；K_x=k_y=k_z=2；

The attitude signal compensation dosage of involved wave filter output is:

$\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\ψ}}\\ {\mathrm{\υ}}_{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}\widetilde{\mathrm{\ψ}}-{\mathrm{\ζ}}_{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}-{\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(15\right)$

In formula, filtering attitude compensates Expression formula and is:

${\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\ψ}}=-k_{\mathrm{\ψ}}{\mathrm{\ζ}}_{\mathrm{\ψ}}+\frac{r_c-r_{co}}{cos\mathrm{\θ}}+\frac{{\mathrm{\ζ}}_r}{\cos \mathrm{\θ}}---\left(16\right)$

And have ζ_ψ(0)=0, ζ_θ(0)=0, ζ_r=0, ζ_q=0；K_ψ=k_θ=5.