CN107168312B - Space trajectory tracking control method for compensating UUV kinematic and dynamic interference - Google Patents
Space trajectory tracking control method for compensating UUV kinematic and dynamic interference Download PDFInfo
- Publication number
- CN107168312B CN107168312B CN201710347913.4A CN201710347913A CN107168312B CN 107168312 B CN107168312 B CN 107168312B CN 201710347913 A CN201710347913 A CN 201710347913A CN 107168312 B CN107168312 B CN 107168312B
- Authority
- CN
- China
- Prior art keywords
- uuv
- kinematic
- tau
- interference
- neural network
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 44
- 238000013528 artificial neural network Methods 0.000 claims abstract description 37
- 238000001914 filtration Methods 0.000 claims abstract description 16
- 230000007246 mechanism Effects 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 38
- 239000013598 vector Substances 0.000 claims description 21
- 230000003044 adaptive effect Effects 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 7
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 230000000694 effects Effects 0.000 abstract description 7
- 230000006870 function Effects 0.000 description 13
- 238000005259 measurement Methods 0.000 description 5
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 5
- 230000008901 benefit Effects 0.000 description 3
- 238000013016 damping Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 230000009131 signaling function Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012938 design process Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000012530 fluid Substances 0.000 description 1
- 230000036541 health Effects 0.000 description 1
- 210000001503 joint Anatomy 0.000 description 1
- 238000003032 molecular docking Methods 0.000 description 1
- 230000000704 physical effect Effects 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/0088—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Business, Economics & Management (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Game Theory and Decision Science (AREA)
- Medical Informatics (AREA)
- Feedback Control In General (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a space trajectory tracking control method for compensating UUV kinematic and dynamic interference, which comprises the following steps of firstly, giving a smooth and bounded expected trajectory yd; secondly, acquiring current pose information and speed information of the UUV through an inertial navigator, a depth meter, an attitude sensor and Doppler carried by the UUV; step three: selecting the position of a virtual control point at the front end of the UUV; step four, establishing a track tracking error, and carrying out filtering processing; utilizing a neural network to obtain an estimated UUV kinematic and dynamic interference term to obtain a self-adaptive control law ul capable of compensating the interference term; step six, obtaining an actuating mechanism control signal tau a ═ tau u, tau q, tau r ] T; step seven, judging whether the position of a virtual control point at the front end of the UUV reaches the end point of the given expected track, and if so, ending the operation; otherwise, returning to the step two. The invention can effectively compensate the interference caused by UUV operation science and dynamics, and improve the control effect and the control precision.
Description
Technical Field
The invention belongs to the field of autonomous control of unmanned underwater vehicles, and particularly relates to a space trajectory tracking control method for compensating UUV kinematics and dynamics interference.
background
the emergence of Unmanned Underwater Vehicles (UUV) provides a very important means for ocean exploration and development, and has become the most effective ocean development tool recognized at present. A UUV is an autonomous underwater vehicle with energy, autonomous navigation and control, and the ability to perform numerous marine missions without supervision.
feedback control of under-actuated autonomous underwater vehicles has attracted a great deal of attention in recent years from the field of control and marine engineering. Compared with the motion control of a fully-driven UUV, the design process of the under-driven UUV controller mainly considers that the number of independent actuating mechanisms of the UUV is less than that of the degrees of freedom. This configuration increases the difficulty of designing a non-linear controller. The invention aims at the trajectory tracking control of the under-actuated UUV.
In the process of trajectory tracking control of the UUV, generally, a trajectory is planned first, when the UUV navigates along an expected trajectory, due to the influence of the outside world and the conditions of the UUV, the actual motion trajectory of the UUV deviates from the expected motion trajectory, and therefore reasonable control needs to be performed, the UUV can navigate along the expected trajectory better, and recovery butt joint is completed. In the prior art, Cao Yonghui and Shixihua 'underwater vehicle trajectory tracking control and simulation' propose a trajectory tracking control method combining a transverse trajectory error method and a line-of-sight method based on sliding mode control aiming at horizontal plane motion of a UUV. Firstly, a sliding mode controller of a transverse track error method and a sliding mode controller of a line of sight method are respectively established, the line of sight method is adopted when the deviation of a course angle is large, and the transverse track error method is adopted when the deviation of the course is smaller than a fixed value. The track tracking control method of the cascade system comprising position tracking and course angle tracking is provided for the horizontal plane motion of the UUV by Gao Sword, Xundemin, Severe health and other people 'autonomous underwater vehicle docking path planning and tracking control'. And the position tracking controller is designed according to a backstepping method, and the overall consistent asymptotic stability of the track tracking error control is ensured. However, in the prior art, the water surface trajectory tracking control problem of the UUV is mostly researched, the trajectory tracking problem of the three-dimensional space is generally designed based on a backstepping method, and the mathematical complexity is high.
Disclosure of Invention
The invention aims to provide a space trajectory tracking control method which is high in control precision and can compensate UUV kinematic and dynamic interference.
The invention is realized by the following scheme:
A space trajectory tracking control method for compensating UUV kinematic and dynamic interference comprises the following steps,
The method comprises the following steps: given a smoothly bounded expected trajectory yd;
Step two: acquiring pose information and speed information of the UUV at the current moment through an inertial navigator, a depth gauge, an attitude sensor and a Doppler log carried by the UUV;
the pose information eta is [ x, y, z, theta, psi ] T and comprises longitudinal displacement x, transverse displacement y, vertical displacement z, a pitching angle theta and a yawing angle psi; the speed information comprises a direct drive speed vector upsilon ═ u, q, r ] T and an indirect drive speed vector w ═ v, w ] T, and comprises a longitudinal speed u, a transverse speed v, a vertical speed w, a longitudinal angular speed q and a yawing angular speed r;
Step three: selecting the position of a virtual control point at the front end of the UUV;
Step four: establishing a track tracking error e, and filtering the track tracking error e to obtain a filtered track tracking error ef;
step five: estimating UUV kinematics and dynamics interference terms F (alpha) by using a two-layer RBF neural network with l nodes to obtain the kinematics and dynamics interference term estimation value of the UUV, and obtaining the neural network adaptive control law by using the filtered track tracking error ef
Step six: obtaining a track tracking control signal tau an according to a neural network self-adaptive control law, and further obtaining an actuating mechanism control signal tau a ═ tau u, tau q, tau r ] T, wherein tau u is longitudinal thrust generated by UUV main thrust, tau q is a pitching control moment, and tau r is a turning control moment;
step seven: judging whether the position of a virtual control point at the front end of the UUV reaches the end point of a given expected track, and if so, ending the operation; otherwise, returning to the step two.
The invention relates to a space trajectory tracking control method for compensating UUV kinematics and dynamics interference, which can also comprise the following steps:
1. the position of the virtual control point at the front end of the UUV is,
where is a constant positive constant representing the distance between virtual control point PL and UUV centroid COM.
2. the track tracking error e is as follows:
e＝y-y，
Filtering the track tracking error e to obtain a filtered track tracking error ef:
wherein Q1 is a gain matrix, and k1 and k2 are adjustable coefficients.
3. The process of obtaining the self-adaptive control law of the neural network comprises the following steps,
(1) Obtaining estimated kinematic and dynamic interference terms of UUV by utilizing two-layer RBF neural network with l nodes
Where α ═ η, ν, W, τ an ] T, W is the adjustable parameter matrix of the neural network, ξ (α) ═ ξ 1(α), ·, ξ l (α) T ] is the neural network basis function vector, ξ i (α) is a gaussian function:
where μ i ═ μ i1, μ i 2.. μ in ] T and β i are the center and width of the gaussian function, respectively, and vectors α and W belong to tight sets U and Ω, respectively, where M1 and M2 are parameters;
Rho is equal to epsilon (alpha) + rho, epsilon (alpha) is the error of the neural network, the error is less than or equal to B epsilon (alpha), and B epsilon is a given threshold value; the interference matrix rho has a boundary | rho | less than or equal to B rho, and the B rho is a given threshold value; τ a ═ τ u, τ q, τ r ] T for actuator control signals, the inertia matrix is an estimate of the inertia matrix M1(η), M11, M55, M66 are mass and inertia parameters of the UUV;
(2) Obtaining a neural network adaptive control law by using a filtered trajectory tracking error ef
The update rule of W and ρ M is:
Where, the threshold ρ M is B ∈ + B ρ, γ W and γ ρ are adaptive gains, σ W and σ ρ are normal numbers, and Kp is a gain.
4. the track tracking control signal tau an is
Wherein,
wherein, Jacobian matrix
5. the UUV kinematic and kinetic disturbances include: the method comprises the following steps of measuring instrument uncertainty interference, model parameter uncertainty interference, ocean current and wave interference and load dynamics 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 as follows,
in the formula, the estimated values of the kinematic and kinetic interference terms are shown.
7. The optimal matrix of the adjustable parameter matrix W of the neural network is as follows:
the invention has the following beneficial effects:
The space trajectory tracking control method for compensating UUV kinematic and dynamic interference can successfully control the expected trajectory on UUV tracking, the tracking deviation converges to a neighborhood near a zero point, and all closed-loop signals are bounded. A significant advantage of neural networks is that they exhibit a smooth response. The method considers the influence of uncertainty interference of a measuring instrument, uncertainty interference of model parameters, ocean current and sea wave interference and load dynamics interference on the UUV control precision in the UUV trajectory tracking control process, adopts the neural network to approach the UUV kinematics and dynamics interference, can effectively compensate the kinematics and dynamics interference of the UUV, and improves the trajectory tracking control precision. The invention effectively filters the track tracking error, and effectively reduces the risk of saturation of the actuating mechanism by adopting a weighted dynamic filtering mode.
drawings
FIG. 1 is a flow chart of a UUV three-dimensional space adaptive trajectory tracking control method of the present invention;
FIG. 2 is a schematic diagram of the positions of virtual control points at the front end of a UUV;
FIG. 3 shows UUV spatial trajectory tracking results: FIG. 3(a) XYZ tracking results; FIG. 3(b) XY tracking results; FIG. 3(c) YZ tracking results.
fig. 4 is a graph showing the filtering effect of the track tracking error ef (t) after filtering, fig. 4(a) is a graph showing the comparison between the signal after filtering and the original signal, and fig. 4(b) is a graph showing the effect of the track tracking error ef (t) after filtering.
Detailed Description
the present invention will be described in further detail below with reference to the accompanying drawings.
The invention discloses a UUV three-dimensional space self-adaptive trajectory tracking control method, which comprises the following steps as shown in figure 1:
The method comprises the following steps: given a smoothly bounded expected trajectory yd (t);
The control targets of the invention are: a tracking control law is designed for an underactuated UUV with kinematic and dynamic interference, and the tracking error is consistent and finally bounded in a three-dimensional space.
The desired trajectory yd (t) and all of them are bounded, Supt ≧ 0| | | yd (t) | < Bdp, where Bdp, Bdv, and Bda boundary constants.
Step two: the method comprises the steps that current pose information and speed information are collected through a sensor carried by a UUV, wherein the pose information eta is [ x, y, z, theta, psi ], ] T comprises a surge x, a sway y, a heave z, a pitch angle theta and a yaw angle psi under a ground coordinate system, and the speed information comprises a surge u, a sway v, a heave w, a pitch q and a yaw speed r under a ship body coordinate system, and is respectively marked as a direct driving speed vector upsilon is [ u, q, r ] T and an indirect driving speed vector w is [ v, w ] T.
the 5 degree of freedom mathematical model for an under-actuated UUV is as follows:
wherein τ u, τ q, τ r are signals generated by the actuator, τ wu (t), τ wv (t), τ ww (t), τ wq (t), are bounded time-varying disturbances. mii are mass and inertia parameters of the UUV, dii is the damping coefficient, i is 1,2,3, 5, 6. Rho is the water density, g is the acceleration of gravity, v is the volume of water, and GML is the longitudinal metacentric height.
the kinematic model (1) can be represented as follows:
Where υ ═ u, q, r ] T and w ═ v, w ] T are redefined velocity vectors, the former being directly driven and the latter not being directly drivable. The sum of S (η) is the kinematic matrix and kinematic interference vector matrix, respectively, as follows:
Dynamic model of the direct drive part of the aircraft:
Where τ a ═ τ u, τ q, τ r ] T is the control input vector. The matrix is an inertia matrix, is a Coriolis centripetal force, is a hydrodynamic damping matrix, is a gravity vector, and is disturbance caused by sea waves and ocean currents.
kinetic models of the portion that the aircraft cannot directly drive:
Wherein,
The Coriolis centripetal force is a hydrodynamic damping matrix and is disturbance caused by sea waves and ocean currents.
Description of the drawings: 1) the swaying and heaving speed of the aircraft is passively bounded, the Supt is more than or equal to 0| | w (t) | | is less than or equal to Bw, and the Bw is a boundary constant.
2) the bounds of the perturbation vector sum are: the | | tau w1(t) | | is less than or equal to lambda w11, wherein the sum of lambda w11 is a normal number.
3) to avoid singularities in the stability analysis, the limits for the pitch angle were defined as: and | theta (t) | is not less than theta max and less than pi/2.
Step three: selecting the position of a virtual control point at the front end of the UUV;
Because the invention mainly researches UUV three-dimensional point tracking control, the coordinates in the x, y and z directions should be selected under a geodetic coordinate system. One simplified choice is the location of the centroid, denoted COM, as shown in fig. 2. However, the advantages of this option are: (1) based on the UUV model presented above, the controller does not exhibit disturbances in pitch and yaw directions. (2) The centroid position is not affected by pitch and yaw control inputs. Therefore, the following variable transformations are introduced, including all degrees of freedom and all control inputs that combine UUV dynamics in each direction.
selecting the position of a virtual control point at the front end of the UUV:
Where it is a constant positive constant representing the distance between virtual control point PL and UUV centroid COM, as shown in fig. 2.
According to the current pose information and speed information of the UUV, a relation between a virtual control point at the front end of the UUV and an actuator control signal tau a ═ tau u, tau q and tau r ] T is established, namely an input and output model of the UUV is specifically formed by the following steps:
(1) UUV model state space representation
Combining the UUV kinematic model formula (3) and the kinetic model formula (5) to obtain a state space representation form:
Wherein:
the state variables are subjected to a simplified state space model (9) below, controller design and stability analysis. The state feedback control is:
where τ a is the control input to the UUV and τ an is a new control input, which is an approximation of M1(η). Substituting equation (11) into equation (9), the UUV state space model is rewritten as:
Wherein, for simplicity, f (x) and g (x) represent the smooth vector field of the system, and q (x) represents the kinematic and kinetic perturbations.
(2) input and output model of UUV
through the UUV kinematic model and the UUV output equation, it can be obtained that:
Where lfh (x) ═ hf, lgh (x) ═ hg, lqh (x) ═ hq, denotes the derivative of h along the direction of vectors f, g, q, respectively. H is the gradient (derivative) of h, and J δ (η, w) is the portion of the input-output model corresponding to the kinematics model perturbation.
wherein the Jacobian matrix is inverse to the kinematic matrix S (η) in equation (4), and J (η) has no singular points for all θ. Since equation (13) is not all related to actuator control inputs, another variation is:
wherein rho is less than or equal to Brho.
Step four: the trajectory tracking error e-y-yd is established based on the given desired trajectory yd.
establishing a track tracking error e:
e＝y-y，
filtering the track tracking error e to obtain a filtered track tracking error:
Wherein,
tanh (-) is a hyperbolic tangent function, (xd, yd, zd) are coordinates of the desired trajectory yd, Q1 is a gain matrix, k1 and k2 are adjustable coefficients,
the desired trajectory, which gives a smoothly bounded desired trajectory, is given by an open-loop motion planner.
and the state variable eta d of the expected track state vector is expected track pose information, the upsilond is expected track speed information, and the tau and is control input of the expected track.
Step five: estimating UUV kinematics and dynamics interference terms F (alpha) by using a two-layer RBF neural network with l nodes, obtaining the estimated kinematics and dynamics interference terms of the UUV, obtaining a neural network adaptive control law by using the filtered track tracking error ef, and compensating the estimated kinematics and dynamics interference terms of the UUV
(1) obtaining estimated kinematic and dynamic interference terms of UUV by utilizing two-layer RBF neural network with l nodes
where α ═ η, ν, W, τ an ] T, W is the adjustable parameter matrix of the neural network, ξ (α) ═ ξ 1(α),.., ξ l (α) ] T is the neural network basis function vector, ξ i (α) is a gaussian function:
where μ i ═ μ i1, μ i 2.. μ in ] T and β i are the center and width of the gaussian function, respectively, and vectors α and W belong to tight sets U and Ω, respectively, where M1 and M2 are parameters;
the optimal matrix of the adjustable parameter matrix W of the neural network is as follows:
Rho is equal to epsilon (alpha) + rho, epsilon (alpha) is the error of the neural network, the error is less than or equal to B epsilon (alpha), and B epsilon is a given threshold value; the interference matrix rho has a boundary | rho | less than or equal to B rho, and the B rho is a given threshold value; τ a ═ τ u, τ q, τ r ] T for actuator control signals, the inertia matrix is an estimate of the inertia matrix M1(η), M11, M55, M66 are mass and inertia parameters of the UUV;
considering the UUV actual system, including data acquisition of various sensors, physical properties of a medium where space maneuvering is located, and meanwhile, combining a five-degree-of-freedom mathematical model of the UUV, the kinematic and dynamic interference in the UUV motion process comprises the following steps: the method comprises the following steps of uncertainty interference of a measuring instrument, uncertainty interference of model parameters, interference of ocean currents and ocean waves and dynamic interference of loads.
The uncertainty interference of the measuring instrument mainly refers to the noise interference in the measurement of the instrument, the actual marine environment is complex and changeable, and the measurement system is inevitably polluted by various noises due to the process level limitation of the components of the instrument. If the Doppler log measures the bottom tracking velocity or the convection velocity by using the Doppler effect principle, random errors are introduced into the measured velocity quantity if the measurement process is influenced by the scatterer in water. In this case, the speed signal is a non-stationary signal whose frequency varies with time.
The model parameter uncertainty interference mainly refers to that when a UUV dynamic model is established, a hydrodynamic coefficient is considered to be constant and is a fixed value, the hydrodynamic coefficient can generate tiny perturbation along with the change of a motion state in practice, an offset is added to a related hydrodynamic item at the moment, and the same-scale model experiment research shows that the offset does not dominate in a navigational speed range and can be regarded as disturbance.
the UUV is greatly influenced by ocean current and sea waves when sailing on an offshore surface with low speed, the fluid flow speed is a complex function of space and time and changes along with the change of water area, depth and time, and the flow resistance of the controller is used as an index of motion control design.
the UUV can influence the mass distribution of the UUV when the load structure or the shape to which the UUV is attached changes during navigation.
(2) controller design using filtered trajectory tracking error ef
obtaining a neural network adaptive control law
The update rule of W and ρ M is:
wherein, the threshold rho M is B epsilon + B rho, gamma W and gamma rho are adaptive gains, sigma W and sigma rho are normal numbers, and Kp is a gain;
Step six: obtaining a track tracking control signal tau according to a self-adaptive control law of a neural network,
further obtaining an actuating mechanism control signal tau a ═ tau u, tau q, tau r ] T, wherein tau u is longitudinal thrust generated by UUV main thrust, tau q is pitching control moment, and tau r is heading control moment;
the following closed loop power error equation is obtained:
Among these are width estimation errors.
step seven: judging whether the position of a virtual control point at the front end of the UUV reaches the end point of a given expected track, and if so, ending the operation; otherwise, returning to the step two.
in the present invention, λ max (·) (λ min (·)) is defined as the largest (smallest) eigenvalue of the matrix. Defined as the euclidean norm of the vector. The induced norm of the matrix A is the Frobenius norm of the matrix A: wherein tr {. cndot } represents a trace-seeking operation. The matrix In represents an n-dimensional unit matrix. The following symbols tanh (x) ([ tanh (x1),. ], tanh (xn) ] T, Sech2(x) ═ diag [ Sech2(x1),. ], Sech2(xn) ] T are also defined. Wherein diag [. cndot. ] represents a diagonal matrix, tanh (.) is a hyperbolic tangent function, sech (. cndot.). cndot. 1/cosh (. cndot.) is a hyperbolic secant function, and cosh (. cndot.) is a hyperbolic cosine function.
in the invention, a UUV kinematic and dynamic interference term F (alpha) is d (alpha) + rho, and an unknown function d (alpha) can be approximated by utilizing the approximation property of a neural network:
error of neural network:
here, W is an estimated value of W, defining the estimation error nonlinear uncertainty d as: d (x) ═ W ξ (x) + ε such that | | | ε | ≦ B ε. Therefore, equation (16) can be rewritten as:
Wherein ρ × t is the boundary of ∈ (t) + ρ (t) where
An experiment of the present invention is given below to verify the effectiveness of the method of the present invention:
The position measurement system is modeled using a randn (.) function with white gaussian noise added to the measurement of the UUV output. All simulations were done with euler's solution algorithm with a time step of 20 ms. UUVs are equipped with propellers to provide longitudinal force, pitch and yaw moments. For the actual UUV model, the used model parameters are as follows:
m 11-25 kg, m 22-17.5 kg, m 33-30 kg, m 55-22.5 kgm2, m 66-15 kgm2, d 11-30 kgs-1, d 22-30 kgs-1, d 33-30 kgs-1, d 55-20 kgm2s-1, d 66-20 kgm2s-1, and ρ g-5. However, in practice it is very difficult to determine the actual values of these parameters, and thus UUVs are parameter uncertain. In addition, environmental interference is added by:
τ(t)＝0.5sgn(υ)+2[sin(0.1t),sin(0.1t),sin(0.1t)]
the control parameters were chosen as follows: kp is 10I3, Q is 10I3, γ p is 1, σ p is 0.005, and ∈ t is 1. The control signal is set to a limit of | τ ai | ≦ 100Nm, i ═ 1,2, and 3 to model the saturation characteristics of the actuator. In the experiment, the initial pose of the UUV is x (0) ═ 5m, y (0) ═ 5m, z (0) ═ 0m, θ (0) ═ 0rad, and ψ (0) ═ 0 rad. The reference trajectory yd (t) of the UUV is generated by an open-loop motion planner. Initial pose of reference trajectory and control signal set to
x(0)＝5m,y(0)＝5m,z(0)＝0m,θ(0)＝0rad,ψ(0)＝0rad,τ＝[7.5,1.5,3]Nm。
In addition, an RBF neural network with 6 hidden nodes (l ═ 6) and three output nodes is adopted to model and approximate the kinematic and dynamic interference of the UUV. The RBF neural network has parameters γ w ═ 10, σ w ═ 0.04, μ i [ -3, -2, -1,1,2,3] T, β i ═ 10. W is a 3 x 6 matrix with an initial value of 0. Fig. 3 shows the tracking result, including XYZ, XY, and YZ maps of the UUV and the reference trajectory. It can be seen from the figure that the UUV successfully traces the desired trajectory, and the tracking offset converges to a neighborhood around the zero point. And all closed loop signals are bounded. A significant advantage of neural networks is that they exhibit a smooth response. The control signals generated by the present invention are all within acceptable saturation limits. Fig. 4(a) is a comparison graph of the filtered signal and the original signal, and it can be seen that the filtering effect of the present invention is very good, and the track tracking error ef signal is filtered by the present invention, where k1 is 0.1 and k2 is 1. In fig. 4(b), the filtering effect is very poor when the comparison filtering signal function is taken as the comparison filtering signal function, and the filtering effect is good without the invention.
Claims (8)
1. a space trajectory tracking control method for compensating UUV kinematic and dynamic interference is characterized in that: comprises the following steps of (a) carrying out,
the method comprises the following steps: given a smoothly bounded expected trajectory yd;
step two: acquiring pose information and speed information of the UUV at the current moment through an inertial navigator, a depth gauge, an attitude sensor and a Doppler log carried by the UUV;
The pose information eta is [ x, y, z, theta, psi ] T and comprises longitudinal displacement x, transverse displacement y, vertical displacement z, a pitching angle theta and a yawing angle psi; the speed information comprises a direct drive speed vector upsilon ═ u, q, r ] T and an indirect drive speed vector w ═ v, w ] T, and comprises a longitudinal speed u, a transverse speed v, a vertical speed w, a longitudinal angular speed q and a yawing angular speed r;
step three: selecting the position of a virtual control point at the front end of the UUV;
step four: establishing a track tracking error e, and filtering the track tracking error e to obtain a filtered track tracking error ef;
step five: estimating UUV kinematics and dynamics interference item F (alpha) by using two-layer RBF neural network with l nodes to obtain the kinematics and dynamics interference item estimation value of UUV, and obtaining the adaptive control law of the neural network by using the filtered track tracking error ef
step six: obtaining a track tracking control signal tau an according to a neural network self-adaptive control law, and further obtaining an actuating mechanism control signal tau a ═ tau u, tau q, tau r ] T, wherein tau u is a longitudinal thrust generated by UUV main thrust, tau q is a pitching control moment, and tau r is a turning control moment;
Step seven: judging whether the position of a virtual control point at the front end of the UUV reaches the end point of a given expected track, and if so, ending the operation; otherwise, returning to the step two.
2. The spatial trajectory tracking control method for compensating for UUV kinematic and dynamic disturbances according to claim 1, wherein: the position of the virtual control point at the front end of the UUV is,
where is a constant positive constant representing the distance between virtual control point PL and the UUV centroid.
3. The spatial trajectory tracking control method for compensating for UUV kinematic and dynamic disturbances according to claim 2, wherein: the track tracking error e is as follows:
e＝y-y，
Filtering the track tracking error e to obtain a filtered track tracking error ef:
wherein Q1 is a gain matrix, and k1 and k2 are adjustable coefficients.
4. The spatial trajectory tracking control method for compensating for UUV kinematic and dynamic disturbances according to claim 3, wherein: the process of obtaining the self-adaptive control law of the neural network comprises the following steps,
(1) Obtaining estimated kinematic and dynamic interference terms of UUV by utilizing two-layer RBF neural network with l nodes
where α ═ η, ν, W, τ an ] T, W is an adjustable parameter matrix of the neural network, ξ (α) ═ ξ 1(α),.. till.,. ξ l (α) T ] is a neural network basis function vector, ξ i (α) is a gaussian function:
Where μ i ═ μ i1, μ i 2.. μ in ] T and β i are the center and width of the gaussian function, respectively, and vectors α and W belong to tight sets U and Ω, respectively, where M1 and M2 are parameters;
Rho is equal to epsilon (alpha) + rho, epsilon (alpha) is the error of the neural network, the error is less than or equal to B epsilon (alpha), and B epsilon is a given threshold value; the interference matrix rho is bounded, | | rho | | | is less than or equal to B rho, and the B rho is a given threshold value; τ a ═ τ u, τ q, τ r ] T for actuator control signals, the inertia matrix is an estimate of the inertia matrix M1(η), M11, M55, M66 are mass and inertia parameters of the UUV;
(2) obtaining a neural network adaptive control law by using a filtered trajectory tracking error ef
The update rule of W and ρ M is:
Where, the threshold ρ M is B ∈ + B ρ, γ W and γ ρ are adaptive gains, σ W and σ ρ are normal numbers, and Kp is a gain.
5. the spatial trajectory tracking control method for compensating for UUV kinematic and dynamic disturbances according to claim 4, wherein: the optimal matrix of the adjustable parameter matrix W of the neural network is as follows:
6. The spatial trajectory tracking control method for compensating for UUV kinematic and dynamic disturbances according to claim 5, wherein: the track tracking control signal tau an is
wherein,
wherein, Jacobian matrix
7. the spatial trajectory tracking control method for compensating for UUV kinematic and dynamic disturbances according to claim 6, wherein: 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, the estimated values of the kinematic and kinetic interference terms are shown.
8. The spatial trajectory tracking control method for compensating for UUV kinematic and dynamic disturbances according to claim 6, wherein: the UUV kinematic and kinetic disturbances include: the method comprises the following steps of measuring instrument uncertainty interference, model parameter uncertainty interference, ocean current and wave interference and load dynamics interference.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710347913.4A CN107168312B (en) | 2017-05-17 | 2017-05-17 | Space trajectory tracking control method for compensating UUV kinematic and dynamic interference |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710347913.4A CN107168312B (en) | 2017-05-17 | 2017-05-17 | Space trajectory tracking control method for compensating UUV kinematic and dynamic interference |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107168312A CN107168312A (en) | 2017-09-15 |
| CN107168312B true CN107168312B (en) | 2019-12-06 |
Family
ID=59816457
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710347913.4A Active CN107168312B (en) | 2017-05-17 | 2017-05-17 | Space trajectory tracking control method for compensating UUV kinematic and dynamic interference |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107168312B (en) |
Families Citing this family (22)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107894775B (en) * | 2017-10-16 | 2020-08-28 | 中国科学院声学研究所 | Under-actuated unmanned underwater vehicle track generation and control method |
| CN108008628B (en) * | 2017-11-17 | 2020-02-18 | 华南理工大学 | Method for controlling preset performance of uncertain underactuated unmanned ship system |
| CN108789426A (en) * | 2017-12-29 | 2018-11-13 | 金门工程建设有限公司 | The mechanical people of surface treatment |
| CN108334677B (en) * | 2018-01-17 | 2021-06-11 | 哈尔滨工程大学 | UUV real-time collision avoidance planning method based on GRU network |
| CN108073175B (en) * | 2018-01-23 | 2020-11-06 | 上海交通大学 | Under-actuated unmanned ship formation intelligent control method based on virtual ship self-adaptive planning |
| CN108427414B (en) * | 2018-03-31 | 2020-10-27 | 西北工业大学 | Horizontal plane self-adaptive trajectory tracking control method for autonomous underwater vehicle |
| CN108674616B (en) * | 2018-07-11 | 2020-04-17 | 哈尔滨工程大学 | Recovery method of autonomous underwater vehicle |
| CN109270837B (en) * | 2018-07-31 | 2021-12-24 | 哈尔滨工程大学 | Cascade control method for underwater ultrahigh-speed navigation body |
| CN109240091B (en) * | 2018-11-13 | 2020-08-11 | 燕山大学 | Underwater robot control method based on reinforcement learning and tracking control method thereof |
| DE102018222783A1 (en) * | 2018-12-21 | 2020-06-25 | Robert Bosch Gmbh | Method for controlling an automated or autonomous means of transportation and evaluation unit |
| CN109597418B (en) * | 2019-02-27 | 2021-03-02 | 福州大学 | Robot distributed cooperative obstacle avoidance method based on independent virtual center point |
| CN110083057B (en) * | 2019-06-12 | 2021-12-07 | 三亚哈尔滨工程大学南海创新发展基地 | PID control method based on hydrofoil motion attitude |
| CN110456658B (en) * | 2019-07-24 | 2022-04-05 | 哈尔滨工程大学 | Variable rotation center motion control simulation method for dynamic positioning ship |
| CN110598311B (en) * | 2019-09-06 | 2022-08-02 | 广东工业大学 | Automatic driving vehicle track tracking method |
| CN111624549B (en) * | 2020-05-14 | 2023-03-24 | 哈尔滨工程大学 | Passive filtering tracking method under non-common-view condition |
| CN111650832B (en) * | 2020-05-28 | 2022-04-15 | 哈尔滨工程大学 | Method for tracking and controlling mechanical foot posture of underwater multi-foot walking robot |
| CN111665722A (en) * | 2020-06-18 | 2020-09-15 | 哈尔滨工程大学 | Sea wave active compensation control method based on predictive control |
| CN111830832B (en) * | 2020-07-27 | 2021-08-31 | 中国科学院自动化研究所 | Bionic gliding machine dolphin plane path tracking method and system |
| CN112327892B (en) * | 2020-11-17 | 2023-03-24 | 哈尔滨工程大学 | Anti-interference control method with AUV (autonomous Underwater vehicle) error limited |
| CN113110537B (en) * | 2021-03-31 | 2023-05-12 | 北京控制工程研究所 | Cooperative control method for relative sight tracking of spacecraft |
| CN113589831B (en) * | 2021-08-11 | 2023-03-21 | 江南大学 | Submersible control method and system based on interference fine estimation and neural network |
| CN118331315B (en) * | 2024-04-12 | 2024-10-11 | 哈尔滨工业大学 | System and method for controlling fault-tolerant output constraint of submarine with preset tracking precision |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7048506B2 (en) * | 2003-11-18 | 2006-05-23 | The Boeing Company | Method and apparatus for magnetic actuation of variable pitch impeller blades |
| CN103968830A (en) * | 2014-05-14 | 2014-08-06 | 哈尔滨工程大学 | Multi-way guiding device and multi-way guiding method for near-surface tracking of mother ship in navigation by UUV (Unmanned Underwater Vehicle) |
| CN105929842A (en) * | 2016-04-20 | 2016-09-07 | 哈尔滨工程大学 | Underactuated UUV plane trajectory tracking control method based on dynamic speed adjustment |
| CN106227223A (en) * | 2016-09-27 | 2016-12-14 | 哈尔滨工程大学 | A kind of UUV trace tracking method based on dynamic sliding mode control |
-
2017
- 2017-05-17 CN CN201710347913.4A patent/CN107168312B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7048506B2 (en) * | 2003-11-18 | 2006-05-23 | The Boeing Company | Method and apparatus for magnetic actuation of variable pitch impeller blades |
| CN103968830A (en) * | 2014-05-14 | 2014-08-06 | 哈尔滨工程大学 | Multi-way guiding device and multi-way guiding method for near-surface tracking of mother ship in navigation by UUV (Unmanned Underwater Vehicle) |
| CN105929842A (en) * | 2016-04-20 | 2016-09-07 | 哈尔滨工程大学 | Underactuated UUV plane trajectory tracking control method based on dynamic speed adjustment |
| CN106227223A (en) * | 2016-09-27 | 2016-12-14 | 哈尔滨工程大学 | A kind of UUV trace tracking method based on dynamic sliding mode control |
Non-Patent Citations (3)
| Title |
|---|
| Globally finite-time stable tracking control of underactuated UUVs;Zheping Yan，等;《Ocean Engineering》;20151231;第132-146页 * |
| Trajectory Tracking Control of Underactuated UUV for Underwater Recovery;ZHANG Wei,等;《2012 Second International Conference on Instrumentation & Measurement, Computer, Communication and Control》;20121231;第386-391页 * |
| 改进规则下自适应神经网络的UUV 水平面路径跟随控制;周佳加，等;《中南大学学报(自然科学版)》;20140930;第45卷(第9期);第3021-3028页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107168312A (en) | 2017-09-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107168312B (en) | Space trajectory tracking control method for compensating UUV kinematic and dynamic interference | |
| CN108803321B (en) | Autonomous underwater vehicle track tracking control method based on deep reinforcement learning | |
| CN111650948B (en) | Quick tracking control method for horizontal plane track of benthonic AUV | |
| Moreira et al. | Path following control system for a tanker ship model | |
| Fossen et al. | Kalman filtering for positioning and heading control of ships and offshore rigs | |
| CN107024863B (en) | UUV trajectory tracking control method for avoiding differential explosion | |
| CN106444806B (en) | The drive lacking AUV three-dimensional track tracking and controlling method adjusted based on biological speed | |
| Zhang et al. | NNFFC-adaptive output feedback trajectory tracking control for a surface ship at high speed | |
| CN108829099A (en) | A kind of drive lacking unmanned boat trace tracking method based on the control of limited Backstepping | |
| CN105807789B (en) | UUV control methods based on the compensation of T-S Fuzzy Observers | |
| CN109583144B (en) | Dynamics optimization controller structure of unmanned marine vehicle and design method | |
| CN109189103B (en) | Under-actuated AUV trajectory tracking control method with transient performance constraint | |
| CN111857165B (en) | Trajectory tracking control method of underwater vehicle | |
| CN114115262B (en) | Multi-AUV actuator saturation cooperative formation control system and method based on azimuth information | |
| CN113467231A (en) | Unmanned ship path tracking method based on sideslip compensation ILOS guidance law | |
| Shojaei | Three-dimensional tracking control of autonomous underwater vehicles with limited torque and without velocity sensors | |
| CN114967714A (en) | Anti-interference motion control method and system for autonomous underwater robot | |
| CN113485389A (en) | Underwater robot three-dimensional trajectory tracking control method based on self-adaptive prediction | |
| CN113741433A (en) | Distributed formation method for unmanned surface ship | |
| CN115480580A (en) | NMPC-based underwater robot path tracking and obstacle avoidance control method | |
| CN117389312A (en) | Model-based three-dimensional tracking control method for counter roll of underwater vehicle | |
| CN113110512B (en) | Benthonic AUV self-adaptive trajectory tracking control method for weakening unknown interference and buffeting influence | |
| de Cerqueira Gava et al. | AUV cruising auto pilot for a long straight confined underwater tunnel | |
| CN115657713B (en) | Launching decision control method considering launching platform sinking and floating and shaking conditions | |
| CN112904719B (en) | Annular area tracking control method suitable for underwater robot position |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |