CN107168312A - A kind of space tracking tracking and controlling method of compensation UUV kinematics and dynamic disturbance - Google Patents

Abstract

The invention discloses a space trajectory tracking control method for compensating for UUV kinematics and dynamics interference, comprising the following steps, step 1, given a smooth and bounded expected trajectory y _d ; step 2, using the inertial navigation carried by the UUV instrument, depth gauge, attitude sensor and Doppler to collect the current pose information and velocity information of the UUV; Step 3: Select the position of the virtual control point at the front end of the UUV; Step 4, establish the trajectory tracking error and perform filtering processing; Step 5, Using the neural network, the estimated UUV kinematics and dynamics interference items are obtained Obtain the adaptive control law u _l capable of compensating for disturbance items; Step 6, obtain the actuator control signal τ _a =[τ _u ,τ _q ,τ _r ] ^T ; Step 7, judge whether the position of the virtual control point at the front end of the UUV reaches the given Determine the end point of the desired trajectory, if yes, end the operation; otherwise, return to step 2. The invention can effectively compensate for UUV operational and dynamic interference, and improve control effect and control precision.

Description

A Spatial Trajectory Tracking Control Method Compensating for UUV Kinematics and Dynamics Disturbances

technical field

The invention belongs to the field of autonomous control of unmanned underwater vehicles, in particular to a space trajectory tracking control method for compensating UUV kinematics and dynamics interference.

Background technique

The emergence of Unmanned Underwater Vehicle (UUV) provides a very important means for ocean exploration and development, and has become the most effective ocean development tool currently recognized. UUV is an underwater unmanned vehicle with its own energy, autonomous navigation and control, and the ability to autonomously perform numerous marine missions without monitoring.

Feedback control of underactuated autonomous underwater vehicles has attracted the attention of a large number of researchers in the fields of control and ocean engineering in recent years. Compared with full-actuated UUV motion control, the main consideration in the design process of underactuated UUV controllers is that the number of UUV independent actuators is less than the number of degrees of freedom. This structure increases the difficulty of nonlinear controller design. The present invention performs trajectory tracking control for underdriven UUVs.

In the process of trajectory tracking control for UUV, we generally plan the trajectory first. When the UUV sails along the desired trajectory, due to the influence of the outside world and the UUV's own conditions, there is a gap between the actual trajectory of the UUV and the desired trajectory. Deviation, so we need to carry out reasonable control so that the UUV can better navigate along the desired trajectory and complete the recovery and docking. Cao Yonghui and Shi Xiuhua's "Underwater Vehicle Trajectory Tracking Control and Simulation" in the prior art proposes a trajectory tracking control method based on sliding mode control that combines the lateral trajectory error method and the line of sight method for the horizontal plane motion of UUV. Firstly, the sliding mode controller of the lateral trajectory error method and the sliding mode controller of the line-of-sight method are respectively established. When the heading angle deviation is large, the line-of-sight method is used, and when the heading deviation is less than a certain value, the lateral trajectory error method is used. Gao Jian, Xu Demin, Yan Weiwei et al. "Autonomous Underwater Vehicle Docking Path Planning and Tracking Control" also proposed a trajectory tracking control method for a cascade system including position tracking and heading angle tracking for the horizontal plane movement of UUV. The position tracking controller is designed according to the backstepping method, and the globally consistent asymptotic stability of the trajectory tracking error control is guaranteed. However, most of the prior art is to study the UUV's water surface trajectory tracking control problem, and the trajectory tracking problem in three-dimensional space is generally designed based on the backstepping method, and the mathematical complexity is high.

Contents of the invention

The purpose of the present invention is to provide a space trajectory tracking control method with high control precision and capable of compensating UUV kinematics and dynamics disturbance.

The present invention is achieved through the following schemes:

A space trajectory tracking control method for compensating UUV kinematics and dynamics interference, comprising the following steps,

Step 1: Given a smooth and bounded desired trajectory y _d ;

Step 2: Collect the UUV's current position and speed information through the UUV's inertial navigator, depth gauge, attitude sensor and Doppler log;

Among them, pose information η=[x,y,z,θ,ψ] ^T , including longitudinal displacement x, lateral displacement y, vertical displacement z, pitch angle θ and yaw angle ψ; velocity information includes direct drive speed Vector υ=[u,q,r] ^T and indirect driving velocity vector w=[v,w] ^T , including longitudinal velocity u, lateral velocity v, vertical velocity w, pitch angular velocity q and yaw angular velocity r;

Step 3: Select the position of the virtual control point at the front end of the UUV;

Step 4: Establish the trajectory tracking error e, filter the trajectory tracking error e, and obtain the filtered trajectory tracking error e _f ;

Step 5: Use the two-layer RBF neural network with l nodes to estimate the UUV kinematics and dynamics interference item F(α), and obtain the estimated value of the UUV kinematics and dynamics interference item Using the filtered trajectory tracking error e _f to get the neural network adaptive control law

Step 6: According to the neural network adaptive control law Obtain the trajectory tracking control signal τ _an , and further obtain the actuator control signal τ _a =[τ _u ,τ _q ,τ _r ] ^T , where τ _u is the longitudinal thrust generated by the UUV main propulsion, τ _q is the pitch control moment, τ _r is the bow control moment;

Step 7: Determine whether the position of the virtual control point at the front end of the UUV reaches the end point of the given desired trajectory, if yes, end the operation; otherwise, return to step 2.

A space trajectory tracking control method for compensating UUV kinematics and dynamics interference of the present invention may also include:

1. The position of the virtual control point of the UUV front end is,

in, is a constant positive constant, representing the distance between the virtual control point _PL and the UUV centroid COM.

2. The track tracking error e is:

e=yy _d ,

Filter the trajectory tracking error e to obtain the filtered trajectory tracking error e _f :

Among them, Q ₁ is the gain matrix, and k ₁ and k ₂ are adjustable coefficients.

3. The neural network adaptive control law described The process of obtaining is,

(1) Utilize a two-layer RBF neural network with l nodes to obtain the estimated kinematics and dynamics interference items of UUV

Among them, α=[η,υ,w,τ _an ] ^T , W is the adjustable parameter matrix of the neural network, ξ(α)=[ξ ₁ (α),,..,ξ _l (α) ^T ] is the neural network basis function vector, ξ _i (α) is the Gaussian function:

Among them, μ _i =[μ _i1 ,μ _i2 ,...μ _in ] ^T and β _i are the center and width of the Gaussian function respectively, and the vectors α and W belong to the compact sets U and Ω respectively, Wherein, M ₁ and M ₂ are parameters;

ρ ^* ＝ε(α)+ρ, ε(α) is the error of the neural network, the error ||ε(α)||≤B _ε , B _ε is a given threshold; the interference matrix ρ is bounded ||ρ|| ≤B _ρ , B _ρ is a given threshold; τ _a ＝[τ _u ,τ _q ,τ _r ] ^T is the actuator control signal, the inertia matrix is the estimated value of the inertia matrix M ₁ (η), m ₁₁ , m ₅₅ , m ₆₆ are the mass and inertia parameters of UUV;

(2) Using the filtered trajectory tracking error e _f to obtain the neural network adaptive control law

The update rules for W and ρ _M are:

Wherein, the threshold ρ _M =B _ε +B _ρ , γ _W and γ _ρ are adaptive gains, σ _W and σ _ρ are positive constants, and K _p is the gain.

4. The track tracking control signal τ _an is

in,

Among them, the Jacobian matrix

5. The UUV kinematics and dynamics interference include: measurement instrument uncertainty interference, model parameter uncertainty interference, ocean current and wave interference, and load dynamic interference.

6. The relationship between the position of the virtual control point at the front end of the UUV and the actuator control signal τ _a =[τ _u ,τ _q ,τ _r ] ^T is,

In the formula, are estimates of kinematic and dynamic interference terms.

7. The optimal matrix of the adjustable parameter matrix W of the neural network is:

The present invention has following beneficial effect:

The present invention is a space trajectory tracking control method that compensates UUV kinematics and dynamics interference, can successfully control the desired trajectory on UUV tracking, the tracking deviation converges to a neighborhood near the zero point, and all closed-loop signals are valid boundary. The outstanding advantage of neural network is to exhibit smooth response. The present invention considers the influence of measurement instrument uncertainty interference, model parameter uncertainty interference, ocean current and wave interference, and load dynamics interference on UUV control accuracy during the UUV trajectory tracking control process, and uses neural networks to approximate UUV kinematics and dynamics Disturbance can effectively compensate the kinematics and dynamics disturbance of UUV, and improve the trajectory tracking control accuracy. The invention effectively filters the trajectory tracking error and adopts a weighted dynamic filtering method to effectively reduce the risk of saturation of the actuator.

Instructions attached

Fig. 1 is a flow chart of the UUV three-dimensional space adaptive trajectory tracking control method of the present invention;

Fig. 2 is a schematic diagram of the position of the virtual control point of the UUV front end;

Figure 3 shows the UUV space trajectory tracking results: Figure 3(a) XYZ tracking results; Figure 3(b) XY tracking results; Figure 3(c) YZ tracking results.

Fig. 4 is the trajectory tracking error e _f (t) filtering effect figure after filtering of the present invention, and Fig. 4 (a) is the comparison figure of the signal after filtering of the present invention and original signal, and Fig. 4 (b) is the trajectory tracking after filtering of the present invention Effect diagram of error e _f (t).

detailed description

The present invention will be described in further detail below in conjunction with the accompanying drawings.

A UUV three-dimensional space adaptive trajectory tracking control method of the present invention, as shown in Figure 1, includes the following steps:

Step 1: Given a smooth and bounded expected trajectory y _d (t);

The control objective of the present invention is to design a tracking control law for the underactuated UUV with kinematics and dynamics interference, and make the tracking error is uniformly ultimately bounded in three dimensions.

Expected trajectory y _d (t) and its are bounded, sup _t≥0 ||y _d (t)||<B _dp , where B _dp , B _dv and B _da are boundary constants.

Step 2: Collect the current pose information and velocity information through the sensors carried by the UUV. The pose information η=[x,y,z,θ,ψ,] ^T includes surge x, sway y, Heave z, pitch angle θ and yaw angle ψ, velocity information includes surge u, sway v, heave w, pitch q and yaw velocity r in the hull coordinate system, which are recorded as direct drive velocity vector ν=[u,q,r] ^T and the indirect drive velocity vector w=[v,w] ^T .

The 5-DOF mathematical model of the underactuated UUV is as follows:

Among them, τ _u , τ _q , τ _r are the signals generated by the actuator, τ _wu (t), τ _wv (t), τ _ww (t), τ _wq (t), is a bounded time-varying disturbance. m _ii is the mass and inertia parameters of the UUV, d _ii is the damping coefficient, i=1, 2, 3, 5, 6. ρ is the density of water, g is the acceleration of gravity, ▽ is the volume of water, and _GML is the longitudinal metacentric height.

The kinematic model (1) can be expressed as follows:

Among them, υ=[u, q, r] ^T and w=[v, w] ^T are redefined velocity vectors, the former is directly driven, and the latter cannot be directly driven. S(η) and They are kinematic matrix and kinematic interference vector array, as follows:

Dynamic model of the direct drive part of the vehicle:

Where τ _a =[τ _u ,τ _q ,τ _r ] ^T is the control input vector. is the inertia matrix, is the Coriolis centripetal force, is the hydrodynamic damping matrix, is the gravity vector, Disturbances caused by waves and currents.

The dynamic model of the part that the aircraft cannot directly drive:

in,

in, is the Coriolis centripetal force, is the hydrodynamic damping matrix, Disturbances caused by waves and currents.

Explanation: 1) The sway and heave velocities of the aircraft are passively bounded sup _t≥0 ||w(t)||≤B _w , where B _w is a boundary constant.

2) Disturbance vector with The bound is: ||τ _w1 (t)|| _≤λ w11 , where λ _w11 and is a normal number.

3) In order to avoid singularity in stability analysis, the limit of pitch angle is defined as: |θ(t)|≤θ _max <π/2.

Step 3: Select the position of the virtual control point at the front end of the UUV;

Because the present invention mainly studies UUV three-dimensional point tracking control, the coordinates in the x, y, and z directions should be selected under the earth coordinate system. A simplified choice is the location of the centroid, denoted COM, as shown in Figure 2. However, the advantages of this choice are: (1) Based on the previously proposed UUV model, the controller does not exhibit perturbations in the pitch and yaw directions. (2) The centroid position is not affected by the pitch and yaw control inputs. Therefore, the following variable transformation is introduced, including all degrees of freedom and all control inputs incorporating UUV dynamics in all directions.

Select the position of the virtual control point at the front end of the UUV:

in, is a constant positive constant, representing the distance between the virtual control point _PL and the UUV centroid COM, as shown in Figure 2.

According to the current pose information and velocity information of the UUV, the relationship between the UUV front-end virtual control point and the actuator control signal τ _a = [τ _u ,τ _q ,τ _r ] ^T is constructed, that is, the input-output model of the UUV. The specific process is as follows:

(1) UUV model state space representation

Combining the UUV kinematics model (3) and the dynamics model (5) to obtain the state space representation:

in:

State variables The simplified state-space model (9), controller design, and stability analysis will be carried out below. State feedback control is:

where τ _a is the control input of the UUV, τ _an is a new control input, is an approximate value of M ₁ (η). Substituting formula (11) into formula (9), the UUV state-space model is rewritten as:

where, for simplicity, f(x) and g(x) represent the smooth vector field of the system, and q(x) represents the kinematic and dynamic perturbations.

(2) Input and output model of UUV

Through the UUV kinematics model and the UUV output equation, it can be concluded that:

Among them, L _f h(x)=▽hf, L _g h(x)=▽hg, L _q h(x)=▽hq, which represent the derivatives of h along the directions of vectors f, g, and q respectively. ▽h is the gradient (derivative) of h, and J _δ (η, w) is the part of the input-output model corresponding to the disturbance of the kinematic model.

Among them, the Jacobian matrix Contrary to the kinematic matrix S(η) in formula (4), J(η) for all θ, There are no singularities. Since equation (13) is not all related to the actuator control input, it is transformed again to:

in, It can be obtained that ρ≤B _ρ .

Step 4: Establish a trajectory tracking error e=yy _{d according to a given desired trajectory y d .}

Establish trajectory tracking error e:

e=yy _d ,

The trajectory tracking error e is filtered to obtain the filtered trajectory tracking error:

in,

tanh( ) is the hyperbolic tangent function, (x _d , y _d , z _d ) is the coordinate of the desired trajectory y _d , Q ₁ is the gain matrix, k ₁ and k ₂ are adjustable coefficients,

gives a smooth bounded desired trajectory The desired trajectory is given by an open-loop motion planner.

Among them, the desired trajectory state vector state variable η _d is the desired trajectory pose information, υ _d is the desired trajectory velocity information, τ _and is the control input of the desired trajectory.

Step 5: Use a two-layer RBF neural network with l nodes to estimate the UUV kinematics and dynamics interference item F(α), and obtain the estimated UUV kinematics and dynamics interference item Using the filtered trajectory tracking error e _f to get the neural network adaptive control law Ability to compensate for estimated UUV kinematic and dynamic disturbances

(1) Utilize a two-layer RBF neural network with l nodes to obtain the estimated kinematics and dynamics interference items of UUV

Among them, α=[η,υ,w,τ _an ] ^T , W is the adjustable parameter matrix of the neural network, ξ(α)=[ξ ₁ (α),...,ξ _l (α)] ^T is the neural network basis function vector, ξ _i (α) is the Gaussian function:

Among them, μ _i =[μ _i1 ,μ _i2 ,...μ _in ] ^T and β _i are the center and width of the Gaussian function respectively, and the vectors α and W belong to the compact sets U and Ω respectively, Wherein, M ₁ and M ₂ are parameters;

The optimal matrix of the adjustable parameter matrix W of the neural network is:

ρ ^* ＝ε(α)+ρ, ε(α) is the error of the neural network, the error ||ε(α)||≤B _ε , B _ε is a given threshold; the interference matrix ρ is bounded ||ρ|| ≤B _ρ , B _ρ is a given threshold; τ _a ＝[τ _u ,τ _q ,τ _r ] ^T is the actuator control signal, the inertia matrix is the estimated value of the inertia matrix M ₁ (η), m ₁₁ , m ₅₅ , m ₆₆ are the mass and inertia parameters of UUV;

Considering the actual system of UUV, including data acquisition of various sensors, physical properties of the medium in which the space maneuver is located, and combined with the five-degree-of-freedom mathematical model of UUV, the kinematic and dynamic disturbances encountered during UUV movement include: measurement instrument uncertainty interference, model parameter uncertainty interference, current and wave interference, and load dynamic interference.

Uncertainty interference of measuring instruments mainly refers to the noise interference in the measurement of the instruments. However, the actual marine environment is complex and changeable, coupled with the limitation of the technical level of its own components, the measurement system will inevitably be polluted by various noises. For example, the Doppler log uses the principle of the Doppler effect to measure the bottom tracking velocity or convective velocity. If the measurement process is affected by water scatterers, random errors will be introduced into the measured velocity. In this case, the velocity signal is a non-stationary signal whose frequency varies with time.

Uncertainty interference of model parameters mainly means that when establishing the UUV dynamics model, it is considered that the hydrodynamic coefficient is constant and is a fixed value. In practice, the hydrodynamic coefficient will produce a slight perturbation with the change of the motion state. At this time, the relevant hydrodynamic coefficient An offset should be added to the calculated value of the item, and the same scaled-scale model test research shows that the offset does not dominate in the range of knot speed and can be regarded as a disturbance.

UUV is greatly affected by ocean currents and waves when navigating near the sea surface with low speed. The fluid velocity is a complex function of space and time, which changes with the change of water area, depth and time. The anti-flow capability of the controller is used as the motion control An indicator of design.

During the navigation of UUV, when the attached load structure or shape changes, it will have an impact on the mass distribution of UUV.

(2) Use the filtered trajectory tracking error e _f to design the controller

Neural Network Adaptive Control Law

The update rules for W and ρ _M are:

Wherein, the threshold ρ _M = B _ε + B _ρ , γ _W and γ _ρ are adaptive gains, σ _W and σ _ρ are positive constants, and K _p is the gain;

Step 6: According to the neural network adaptive control law Get the trajectory tracking control signal τ _an ,

Further obtain the actuator control signal τ _a =[τ _u ,τ _q ,τ _r ] ^T , where τ _u is the longitudinal thrust generated by the UUV main propulsion, τ _q is the trim control moment, and τ _r is the bow control moment;

The following closed-loop dynamic error equation is obtained:

in, is the width estimation error.

Step 7: Determine whether the position of the virtual control point at the front end of the UUV reaches the end point of the given desired trajectory, if yes, end the operation; otherwise, return to step 2.

In the present invention, λ _max (·) (λ _min (·)) is defined as the largest (smallest) eigenvalue of the matrix. defined as a vector The Euclidean norm of . The induced norm of matrix A is The Frobenius norm of matrix A is: Where tr{·} represents the trace operation. The matrix I _n represents an n-dimensional unit matrix. The following notation Tanh(x)=[tanh(x ₁ ),...,tanh(x _n )] ^T , Sech ² (x)=diag[sech ² (x ₁ ),...,sech ² is also defined (x _n )] ^T . Where diag[·] represents a diagonal matrix, tanh(·) is a hyperbolic tangent function, sech(·)=1/cosh(·) is a hyperbolic secant function, and cosh(·) is a hyperbolic cosine function.

In the present invention, the UUV kinematics and dynamics interference term F(α)=d(α)+ρ can approximate the unknown function d(α) by using the approximation properties of the neural network:

The error of the neural network:

Here, W ^* is the estimated value of W, defining the estimation error The nonlinear uncertainty d is rewritten as: d(x)=W ^* ξ(x)+ε such that ||ε||≤B _ε . Therefore, formula (16) can be rewritten as:

Among them, the limit of ρ ^* (t) = ε(t) + ρ(t) in

An experiment of the present invention is provided below to verify the validity of the inventive method:

Using the randn(.) function, Gaussian white noise is added to the measurement of the UUV output to model the position measurement system. All simulations are performed using the Euler solution algorithm with a time step of 20 ms. UUVs are configured with propellers to provide longitudinal forces, pitch and yaw moments. For the actual UUV model used, the model parameters used are:

m ₁₁ ＝25kg, m ₂₂ ＝17.5kg, m ₃₃ ＝30kg, m ₅₅ ＝22.5kgm ² , m ₆₆ ＝15kgm ² , d ₁₁ ＝30kgs ^-1 , d ₂₂ ＝30kgs ^-1 , d ₃₃ ＝30kgs ^-1 , d ₅₅ =20kgm ² s ^-1 , d ₆₆ =20kgm ² s ^-1 , ρg▽GM _L =5. However, it is very difficult to determine the actual values of these parameters in practice, so UUV has parameter uncertainty. Also, add ambient noise by:

τ _w1 (t)＝0.5sgn(υ)+2[sin(0.1t),sin(0.1t),sin(0.1t)] ^T

The selection of control parameters is as follows: K _p =10I ₃ , Q=10I ₃ , γ _p =1, σ _p =0.005, ε _t =1. Set the limit value of the control signal to be |τ _ai |≤100Nm, i=1,2,,3 to model the saturation characteristics of the actuator. The initial pose of UUV in the experiment is x(0)=5m, y(0)=5m, z(0)=0m, θ(0)=0rad, ψ(0)=0rad. The reference trajectory y _d (t) of the UUV is generated by an open-loop motion planner. The initial pose and control signals of the reference trajectory are set to

x(0)=5m, y(0)=5m, z(0)=0m, θd(0)=0rad, _ψd (0)= _0rad , _τad =[7.5,1.5,3] ^T Nm.

In addition, a RBF neural network with 6 hidden nodes (l=6) and three output nodes is used to model and approximate the kinematics and dynamics disturbance of UUV. The parameters of the RBF neural network are γ _w =10, σ _w =0.04, μ _i =[-3,-2,-1,1,2,3] ^T , β _i =10. W is a 3×6 matrix with an initial value of 0. Figure 3 shows the tracking results, including three trajectory diagrams of UUV and reference trajectory XYZ, XY and YZ. It can be seen from the figure that the UUV successfully tracked the desired trajectory, and the tracking deviation converged to a neighborhood near the zero point. And all closed-loop signals are bounded. The outstanding advantage of neural network is to exhibit smooth response. The control signals generated by the present invention are all within acceptable saturation limits. Fig. 4(a) is a comparison diagram of the filtered signal and the original signal according to the present invention. It can be seen that the filtering effect of the present invention is very good, and the trajectory tracking error e _f signal is filtered by the present invention, where k ₁ =0.1, k ₂ =1. The comparison filter signal function in Fig. 4(b) is When the filtering effect is very poor, it is not as good as the filtering effect of the present invention.

Claims (8)

1. A space trajectory tracking control method for compensating UUV kinematics and dynamics disturbance, is characterized in that: comprise the following steps, Step 1: Given a smooth and bounded desired trajectory y _d ; Step 2: Collect the UUV's current position and speed information through the UUV's inertial navigator, depth gauge, attitude sensor and Doppler log; Among them, pose information η=[x,y,z,θ,ψ] ^T , including longitudinal displacement x, lateral displacement y, vertical displacement z, pitch angle θ and yaw angle ψ; velocity information includes direct drive speed Vector υ=[u,q,r] ^T and indirect driving velocity vector w=[v,w] ^T , including longitudinal velocity u, lateral velocity v, vertical velocity w, pitch angular velocity q and yaw angular velocity r; Step 3: Select the position of the virtual control point at the front end of the UUV; Step 4: Establish the trajectory tracking error e, filter the trajectory tracking error e, and obtain the filtered trajectory tracking error e _f ; Step 5: Use the two-layer RBF neural network with l nodes to estimate the UUV kinematics and dynamics interference item F(α), and obtain the estimated value of the UUV kinematics and dynamics interference item Using the filtered trajectory tracking error e _f to get the neural network adaptive control law Step 6: According to the neural network adaptive control law Obtain the trajectory tracking control signal τ _an , and further obtain the actuator control signal τ _a =[τ _u ,τ _q ,τ _r ] ^T , where τ _u is the longitudinal thrust generated by the UUV main propulsion, τ _q is the pitch control moment, τ _r is the bow control torque; Step 7: Determine whether the position of the virtual control point at the front end of the UUV reaches the end point of the given desired trajectory, and if so, end the operation; otherwise, return to step 2. 2. A kind of space trajectory tracking control method for compensating UUV kinematics and dynamics interference according to claim 1, characterized in that: the position of the virtual control point of the UUV front end is, in, is a constant positive constant, representing the distance between the virtual control point _PL and the UUV centroid. 3. A kind of spatial trajectory tracking control method for compensating UUV kinematics and dynamics interference according to claim 2, characterized in that: the trajectory tracking error e is: e=yy _d , Filter the trajectory tracking error e to obtain the filtered trajectory tracking error e _f : <mrow> <msub> <mi>e</mi> <mi>f</mi> </msub> <mo>=</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mover> <mi>e</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>+</mo> <msub> <mi>k</mi> <mn>1</mn> </msub> <msub> <mi>Q</mi> <mn>1</mn> </msub> <mi>T</mi> <mi>a</mi> <mi>n</mi> <mi>h</mi> <mrow> <mo>(</mo> <mi>e</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>k</mi> <mn>2</mn> </msub> <mi>e</mi> </mrow> Among them, Q ₁ is the gain matrix, and k ₁ and k ₂ are adjustable coefficients. 4. A kind of spatial trajectory tracking control method for compensating UUV kinematics and dynamics disturbance according to claim 3, characterized in that: the neural network adaptive control law The process of obtaining is, (1) Utilize a two-layer RBF neural network with l nodes to obtain the estimated kinematics and dynamics interference items of UUV Among them, α=[η,υ,w,τ _an ] ^T , W is the adjustable parameter matrix of the neural network, ξ(α)=[ξ ₁ (α),...,ξ _l (α) ^T ] is the basis function vector of the neural network, and ξ _i (α) is a Gaussian function: <mrow> <msub> <mi>&amp;xi;</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>&amp;alpha;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>exp</mi> <mrow> <mo>(</mo> <mo>-</mo> <mfrac> <mrow> <mo>|</mo> <mo>|</mo> <mi>&amp;alpha;</mi> <mo>-</mo> <msub> <mi>&amp;mu;</mi> <mi>i</mi> </msub> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> </mrow> <msubsup> <mi>&amp;beta;</mi> <mi>i</mi> <mn>2</mn> </msubsup> </mfrac> <mo>)</mo> </mrow> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mi>l</mi> </mrow> Among them, μ _i =[μ _i1 ,μ _i2 ,...μ _in ] ^T and β _i are the center and width of the Gaussian function respectively, and the vectors α and W belong to the compact sets U and Ω respectively, Wherein, M ₁ and M ₂ are parameters; ρ ^* ＝ε(α)+ρ, ε(α) is the error of the neural network, error ||ε(α)||≤B _ε , B _ε is a given threshold; the interference matrix ρ is bounded ||ρ|| ≤B _ρ , B _ρ is a given threshold; τ _a ＝[τ _u ,τ _q ,τ _r ] ^T is the actuator control signal, the inertia matrix is the estimated value of the inertia matrix M ₁ (η), m ₁₁ , m ₅₅ , m ₆₆ are the mass and inertia parameters of UUV; (2) Using the filtered trajectory tracking error e _f to obtain the neural network adaptive control law The update rules for W and ρ _M are: <mrow> <mover> <mi>W</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <msub> <mi>&amp;gamma;</mi> <mi>W</mi> </msub> <msub> <mi>e</mi> <mi>f</mi> </msub> <msup> <mi>&amp;xi;</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>&amp;alpha;</mi> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>&amp;sigma;</mi> <mi>W</mi> </msub> <msub> <mi>&amp;gamma;</mi> <mi>W</mi> </msub> <mi>W</mi> </mrow> <mrow> <msub> <mover> <mi>&amp;rho;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>M</mi> </msub> <mo>=</mo> <msub> <mi>&amp;gamma;</mi> <mi>&amp;rho;</mi> </msub> <mo>|</mo> <mo>|</mo> <msub> <mi>e</mi> <mi>f</mi> </msub> <mo>|</mo> <mo>|</mo> <mo>-</mo> <msub> <mi>&amp;gamma;</mi> <mi>&amp;rho;</mi> </msub> <msub> <mi>&amp;sigma;</mi> <mi>&amp;rho;</mi> </msub> <msub> <mi>&amp;rho;</mi> <mi>M</mi> </msub> </mrow> Wherein, the threshold ρ _M =B _ε +B _ρ , γ _W and γ _ρ are adaptive gains, σ _W and σ _ρ are positive constants, and K _p is the gain. 5. A kind of space trajectory tracking control method that compensates UUV kinematics and dynamics interference according to claim 4, is characterized in that: the optimal matrix of the adjustable parameter matrix W of described neural network is: <mrow> <msup> <mi>W</mi> <mo>*</mo> </msup> <mo>=</mo> <mi>arg</mi> <munder> <mrow> <mi>m</mi> <mi>i</mi> <mi>n</mi> </mrow> <mrow> <mi>W</mi> <mo>&amp;Element;</mo> <mi>&amp;Omega;</mi> </mrow> </munder> <mo>{</mo> <munder> <mrow> <mi>s</mi> <mi>u</mi> <mi>p</mi> </mrow> <mrow> <mi>&amp;alpha;</mi> <mo>&amp;Element;</mo> <mi>U</mi> </mrow> </munder> <mo>|</mo> <mi>&amp;epsiv;</mi> <mrow> <mo>(</mo> <mi>&amp;alpha;</mi> <mo>)</mo> </mrow> <mo>|</mo> <mo>}</mo> <mo>.</mo> </mrow> 6. A kind of space trajectory tracking control method that compensates UUV kinematics and dynamics disturbance according to claim 5, is characterized in that: described trajectory tracking control signal τ _an is in, <mrow> <mi>S</mi> <mrow> <mo>(</mo> <mi>&amp;eta;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mrow> <mo>(</mo> <mi>&amp;psi;</mi> <mo>)</mo> </mrow> <mi>cos</mi> <mrow> <mo>(</mo> <mi>&amp;theta;</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>sin</mi> <mrow> <mo>(</mo> <mi>&amp;psi;</mi> <mo>)</mo> </mrow> <mi>cos</mi> <mrow> <mo>(</mo> <mi>&amp;theta;</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mrow> <mo>(</mo> <mi>&amp;theta;</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mfrac> <mn>1</mn> <mrow> <mi>cos</mi> <mrow> <mo>(</mo> <mi>&amp;theta;</mi> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow> Among them, the Jacobian matrix 7. A space trajectory tracking control method for compensating UUV kinematics and dynamics interference according to claim 7, characterized in that: the position of the virtual control point at the front end of the UUV and the actuator control signal τ _a =[τ _u , The relationship between τ _q , τ _r ] ^T is, <mrow> <mover> <mi>y</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mo>=</mo> <mo>&amp;part;</mo> <mrow> <mo>(</mo> <mi>J</mi> <mo>(</mo> <mi>&amp;eta;</mi> <mo>)</mo> <mi>&amp;upsi;</mi> <mo>)</mo> </mrow> <mo>/</mo> <mo>&amp;part;</mo> <mi>&amp;eta;</mi> <mi>S</mi> <mrow> <mo>(</mo> <mi>&amp;eta;</mi> <mo>)</mo> </mrow> <mi>&amp;upsi;</mi> <mo>+</mo> <mi>J</mi> <mrow> <mo>(</mo> <mi>&amp;eta;</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;tau;</mi> <mrow> <mi>a</mi> <mi>n</mi> </mrow> </msub> <mo>+</mo> <mover> <mi>F</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>&amp;alpha;</mi> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>&amp;tau;</mi> <mi>a</mi> </msub> <mo>=</mo> <msub> <mover> <mi>M</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>&amp;eta;</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;tau;</mi> <mrow> <mi>a</mi> <mi>n</mi> </mrow> </msub> </mrow> In the formula, are estimates of kinematic and dynamic interference terms. 8. The space trajectory tracking control method for compensating UUV kinematics and dynamics disturbance according to claim 6, characterized in that: said UUV kinematics and dynamics disturbance include: measuring instrument uncertainty disturbance, model Parameter uncertainty interference, current and wave interference, load dynamics interference.