CN113805486A - USV path tracking control method - Google Patents

USV path tracking control method Download PDF

Info

Publication number
CN113805486A
CN113805486A CN202111194077.3A CN202111194077A CN113805486A CN 113805486 A CN113805486 A CN 113805486A CN 202111194077 A CN202111194077 A CN 202111194077A CN 113805486 A CN113805486 A CN 113805486A
Authority
CN
China
Prior art keywords
heading
time
discrete
error
tracking
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.)
Granted
Application number
CN202111194077.3A
Other languages
Chinese (zh)
Other versions
CN113805486B (en
Inventor
徐玉杰
高嵩
付明玉
杨乐
董李晶
张国荣
王俊一
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202111194077.3A priority Critical patent/CN113805486B/en
Publication of CN113805486A publication Critical patent/CN113805486A/en
Application granted granted Critical
Publication of CN113805486B publication Critical patent/CN113805486B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)

Abstract

The invention discloses a USV accurate path tracking control method, which comprises the following steps of 1: establishing an SF coordinate system position tracking error system, and designing a limited time sideslip angle observer to obtain a sideslip angle estimated value; step 2, an FLOS guidance algorithm is adopted, and the estimated value of the sideslip angle is utilized to give the updating rate of the expected heading, the expected speed and the path parameters; and step 3: establishing a heading dynamic error discrete control model and a longitudinal speed tracking error discrete control model; and 4, step 4: a heading discrete controller and a longitudinal speed discrete controller are designed by constructing a discrete sliding mode surface and a discrete adaptive fast power approach law, and a discrete interference estimator is adopted to compensate the external environment force, so that the tracking of the expected heading and the expected speed under disturbance is completed. The method has the advantages that the path tracking error gradually converges to zero, the global gradual stability is realized, the continuous fast approach law is discretized and improved, the stability is decoupled from the sampling period, and the stability is not limited by the sampling period.

Description

USV path tracking control method
Technical Field
The invention relates to the field of unmanned vehicle (USV) motion control, relates to a USV path tracking control method, and particularly relates to a USV path tracking control method based on a discrete sliding mode.
Background
Compared with a conventional ship, an Unmanned Surface Vehicle (USV) as a water surface high-speed ship has small hydrodynamic resistance and large influence caused by wind and wave interference, and meanwhile, due to transverse underactuation, the high-precision path tracking control difficulty is large. The existing method for improving the path tracking precision mainly starts from the aspects of improving a guidance algorithm, quickly estimating environmental disturbance, improving control precision and the like. However, most of the path tracking algorithms are designed based on a continuous system, and the control law of the unmanned ship in actual engineering is realized through a digital controller, and the control law designed based on the continuous system directly acts on the digital controller, so that unknown dynamics are introduced, and even the system is unstable.
Although a discrete sliding mode control algorithm designed for a discrete system can directly act on the USV digital controller, the sampling period of the system is required to be as short as possible to ensure the stability of the system. However, in practical engineering, the sampling period of the system is constrained by hardware and cost, and cannot be infinitely small, so that the method has limitation in use. Therefore, the research on the high-precision path tracking control algorithm which is not constrained by the sampling time has very important theoretical value and practical significance.
Disclosure of Invention
Aiming at the prior art, the technical problem to be solved by the invention is to provide a USV path tracking control method which is not constrained by sampling time, and the stability of the system and the sampling period are decoupled, so that the system can be ensured to stably and reliably run under the condition of any sampling period, and the path tracking control accuracy is improved.
In order to solve the technical problem, the USV path tracking control method of the present invention includes the following steps:
step 1: establishing an SF coordinate system position tracking error system, designing a limited time sideslip angle observer to obtain an estimated value of the sideslip angle
Figure BDA0003302355050000012
；
Step 2, adopting FLOS guiding algorithm, utilizing the estimated value of the sideslip angle
Figure BDA0003302355050000013
Giving an expected heading, an expected speed and an update rate of the path parameters;
and step 3: establishing an USV heading dynamic error discrete control model and a longitudinal speed tracking error discrete control model;
and 4, step 4: a heading discrete controller and a longitudinal speed discrete controller are designed by constructing a discrete sliding mode surface and a discrete adaptive fast power approach law, and a discrete interference estimator is adopted to compensate the external environment force, so that the tracking of the expected heading and the expected speed under disturbance is completed.
Further, in step 1, a finite time sideslip angle observer is designed to obtain an estimated value of sideslip angle
Figure BDA0003302355050000011
The method specifically comprises the following steps:
designing an unknown time-varying sideslip angle observer:
Figure BDA0003302355050000021
Figure BDA0003302355050000022
Figure BDA0003302355050000023
wherein λ isi＞0,i＝1,2,L＞0，λiL is a positive integer, theta is a path parameter, psi is a real-time heading angle of the unmanned ship, u is a transverse speed of the unmanned ship, and g (u, psi)θ,β)＝ucos(ψ-ψθ)tanβ，
Figure BDA0003302355050000024
Initial value of
Figure BDA0003302355050000025
ψθThe rotation angle from the northeast coordinate system to the SF coordinate system meets the following requirements: psiθ＝arctan 2(y′θ,x′θ)，pθ＝[xθ,yθ]TCoordinates of target tracking points for arbitrary paths on the desired path curve, pe＝[xe,ye]TFor a target error value, satisfy:
Figure BDA0003302355050000026
wherein p ═ x, y]TIs a target real-time location;
the sideslip angle estimate is then:
Figure BDA0003302355050000027
further, the sideslip angle estimation value is utilized in step 2
Figure BDA0003302355050000028
The update rate of the expected heading, the expected speed and the path parameters is given by the following formula:
Figure BDA0003302355050000029
Figure BDA00033023550500000210
Figure BDA00033023550500000211
wherein psidTo expect heading, udIn order to be able to take the desired speed,
Figure BDA00033023550500000212
the path parameter update rate; psiθIs the angle of rotation, psi, of the North east coordinate system to the SF coordinate systemθ＝arctan2(y′θ,x′θ)，kxIf the parameter is more than 0, the U is a set initial path tracking speed, and psi is a real-time heading angle of the unmanned ship; delta is a time-varying dynamic foresight distance parameter and satisfies the following conditions:
Figure BDA00033023550500000213
where k > 0 is the parameter to be designed, ΔmaxAnd deltaminFor maximum and minimum values, p, of a given parameter time-varying dynamic look-ahead distance parameterθ＝[xθ,yθ]TCoordinates of target tracking points for arbitrary paths on the desired path curve, pe＝[xe,ye]TFor a target error value, satisfy:
Figure BDA0003302355050000031
wherein p ═ x, y]TIs the target real-time location.
Further, the establishment of the USV heading dynamic error discrete control model and the longitudinal speed tracking error control model in step 3 specifically includes:
defining the heading angle error psie＝ψ-ψdThe slew rate tracking error is
Figure BDA0003302355050000032
r is the real-time rotation rate of the unmanned ship, and a Euler approximate discretization method is adopted to obtain a USV heading dynamic error discretization control model:
Figure BDA0003302355050000033
wherein m is11,m22,m33Inertia matrix for unmanned boat mathematical model
Figure BDA0003302355050000034
A middle corresponding position variable, wherein m11＝m22＝m，m33＝IzM is the unmanned surface vehicle mass, IzRotational inertia about the z-axis, d, for unmanned boats32，d33Damping matrix for unmanned ship mathematical model
Figure BDA0003302355050000035
A corresponding position variable of (A), T is the sampling period of the digital controller, k > 0 and is a positive integer, #e[k]Is the heading angle error at the moment k, re[k]For the slew rate tracking error at time k, psid[k]For the desired heading angle at time k, u k]Is the transverse velocity at time k, vk]Longitudinal velocity at time k, τr[k]Is a heading controller dr[k]Disturbance of k moment in the heading dynamic error model is obtained;
define the longitudinal velocity tracking error as: u. ofe＝u-udAnd obtaining a longitudinal speed tracking error discrete control model by adopting an Euler approximate discrete method:
Figure BDA0003302355050000036
wherein d is11，d12 d13Damping matrix for unmanned ship mathematical model
Figure BDA0003302355050000037
Middle corresponding position variable, du[k]For longitudinal environmental disturbances at time k, τu[k]Is a longitudinal speed discrete controller.
Further, designing a heading discrete controller by constructing a discrete sliding mode surface and a discrete adaptive fast power approach law in the step 4 specifically comprises:
selecting a discrete sliding mode surface of a heading dynamic error system:
Figure BDA0003302355050000041
wherein alpha is11、α10、β2Is selected as the parameter value and satisfies 0 < T alpha10＜1、α11＞0、0＜β2＜1，ψe[k]Is the heading angle error at the moment k, re[k]The rotation rate tracking error at the moment k is obtained;
designing a heading discrete adaptive fast power approach law:
Figure BDA0003302355050000042
wherein, c7，c8Is a selected parameter value and satisfies c7，c8＞0，c7T＜1；
Adaptive factor
Figure BDA0003302355050000043
The expression is as follows:
Figure BDA0003302355050000044
wherein,
Figure BDA0003302355050000045
estimating disturbance d of current moment acting on USV heading dynamic error model by adopting time delay estimation methodr[k]Item (1):
Figure BDA0003302355050000046
wherein m is11，m22，m33Inertia matrix for unmanned boat mathematical model
Figure BDA0003302355050000047
Middle corresponding position changeAmount wherein m11＝m22＝m，m33＝IzM is the unmanned surface vehicle mass, IzRotational inertia about the z-axis, d, for unmanned boats32,d33Damping matrix for unmanned ship mathematical model
Figure BDA0003302355050000051
A middle corresponding position variable, T is a sampling period of the digital controller, u [ k-1 ]]Transverse velocity at time k-1, v [ k-1 ]]Longitudinal velocity at time k-1, τr[k-1]Is a heading controller dr[k]For disturbances experienced at time k in the dynamic error model of heading, psid[k-1]The desired heading angle at time k-1;
designing a heading discrete controller taur[k]Comprises the following steps:
Figure BDA0003302355050000052
in the formula, x4Comprises the following steps:
Figure BDA0003302355050000053
wherein, taur[k]Is a heading controller, #d[k]For the desired heading angle at time k, u k]Is the transverse velocity at time k, vk]Longitudinal velocity at time k, r k]For the real-time slew rate at time k, dr[k]The disturbance of k time in the heading dynamic error model.
Further, in step 4, designing a longitudinal speed discrete controller by constructing a discrete sliding mode surface and a discrete adaptive fast power approach law specifically comprises:
selecting a discrete integral sliding mode surface of a longitudinal speed tracking error model as follows:
Figure BDA0003302355050000054
wherein alpha is12Is a selected parameter value and satisfies alpha12＞0，ue[k]Longitudinal velocity tracking error at time k;
designing a longitudinal speed discrete adaptive fast power approximation law:
Figure BDA0003302355050000055
wherein, c9，c10Is a selected parameter value and satisfies: c. C9，c10＞0，c9T＜1，
Figure BDA0003302355050000056
The expression is as follows:
Figure BDA0003302355050000057
method for estimating longitudinal environment disturbance d by adopting time delay estimation methodu[k]Comprises the following steps:
Figure BDA0003302355050000061
wherein u ise[k]For longitudinal velocity tracking error at time k, u [ k-1 ]]Transverse velocity at time k-1, v [ k-1 ]]Longitudinal velocity at time k-1, r k-1]Is the real-time slew rate at the time of k-1, wherein m11,m22Inertia matrix for unmanned boat mathematical model
Figure BDA0003302355050000062
A middle corresponding position variable, wherein m11＝m22M is the unmanned ship mass, d11，d12，d13Damping matrix for unmanned ship mathematical model
Figure BDA0003302355050000063
Middle corresponding position variable
Designing a longitudinal velocity discrete controlleru[k]Comprises the following steps:
Figure BDA0003302355050000064
compared with the prior art, the invention has the beneficial effects that:
1. the invention adopts FLOS guidance algorithm to replace ELOS guidance algorithm, so that the path tracking error of the control system can gradually converge to zero, and the overall gradual stability is realized.
2. The invention discretizes and improves the continuous fast approach law, decouples the stability of the control system from the sampling period, ensures that the stability of the system is not limited by the sampling period, and ensures the reliability and stability of the unmanned ship in the operation process.
Drawings
FIG. 1 is a schematic block diagram of a discrete sliding mode control for path tracking of a discrete sampling system according to the present invention;
FIG. 2 is a continuous system USV sinusoidal path tracking track curve of the present invention;
FIG. 3 is a longitudinal and transverse position error curve of the SF coordinate system of the present invention;
FIG. 4 is a plot of actual values of the sideslip angle and observed error of the sideslip angle observer of the present invention;
FIG. 5 is a USV heading angle and heading angle tracking error curve for a continuous system of the present invention;
FIG. 6 is a USV longitudinal velocity and longitudinal velocity tracking error curve for a continuous system of the present invention;
FIG. 7 is a graph of the actual interference of the continuous system and its interference estimation according to the present invention;
FIG. 8 is a continuous system USV control input curve of the present invention.
Detailed Description
The invention is further described with reference to the drawings and the detailed description.
With reference to fig. 1, the present invention comprises the following steps:
step 1, establishing an SF coordinate system position tracking error system, designing a limited time sideslip angle observer on the basis of the coordinate system, and constructing
Figure BDA0003302355050000077
The accurate estimation of the sideslip angle is realized, and the problem that the sideslip angle cannot be measured under complex sea conditions is solved;
step 1.1 assumes that the expected path takes theta as a path parameter on a horizontal plane, and a first-order derivative and a second-order derivative exist and are bounded, and in a northeast coordinate system, the coordinate of an arbitrary path target tracking point on an expected path curve is marked as p (theta) ═ x (theta), y (theta)]TThe target real-time position is p ═ x, y]TTarget error value is pe＝[xe，ye]TWhen ψ (θ) is defined as a rotation angle from the northeast coordinate system to the SF coordinate system, there are:
ψθ＝arctan2(y′θ，x′θ)
consider the position error in the northeast coordinate system as: eta ═ x-x (theta), y-y (theta), psi-psi (theta)]TLower attitude error of SF coordinate system
Figure BDA0003302355050000071
Constructing an error system of horizontal and vertical positions of the SF coordinate system as follows:
Figure BDA0003302355050000072
step 1.2, designing a finite time sideslip angle observer:
the derivation of the longitudinal position error in the above equation can be found:
Figure BDA0003302355050000073
wherein β ═ arctan 2(v, u) represents the sideslip angle, and g (u, ψ)θ,β)＝ucos(ψ-ψθ) tan beta, psi is the real-time heading angle of the unmanned ship, u is the transverse velocity of the unmanned ship, v is the longitudinal velocity of the unmanned ship, and obviously the nonlinear function g (.) is continuous and differentiable. An observer designed for an unknown time-varying sideslip angle is as follows:
Figure BDA0003302355050000074
Figure BDA0003302355050000075
Figure BDA0003302355050000076
wherein λ isi> 0, i ═ 1,2, L > 0 and λiAnd L are all positive integers,
Figure BDA0003302355050000081
initial value of
Figure BDA0003302355050000082
The sideslip angle estimate is then:
Figure BDA0003302355050000083
the observation error of the finite time sideslip angle observer formula is proved to be converged to 0 in a finite time as follows:
the observation error is defined as follows:
Figure BDA0003302355050000084
Figure BDA0003302355050000085
derivation of the observation error of the above equation can be obtained:
Figure BDA0003302355050000086
Figure BDA0003302355050000087
error of observation e1、e2Will converge to zero in a finite time, i.e. there is tob> 0, for arbitrary t > tobThe method comprises the following steps:
Figure BDA0003302355050000088
so when the estimated value of g
Figure BDA0003302355050000089
Estimate of time-varying unknown sideslip angle beta when known
Figure BDA00033023550500000810
Comprises the following steps:
Figure BDA00033023550500000811
and 2, designing a FLOS guidance algorithm based on the sideslip angle observer, and combining the sideslip angle estimated value to give an expected heading, an expected speed and an update rate of the path parameter. Designing a time-varying dynamic foresight distance parameter, reducing a transverse tracking position error, and realizing accurate path tracking under environmental interference by improving a guidance algorithm;
the desired heading is designed as follows:
Figure BDA00033023550500000812
to reduce lateral position tracking errors and improve path tracking accuracy, a time-varying dynamic foresight distance parameter is selected
Figure BDA00033023550500000813
Wherein k > 0 is a design parameter, ΔmaxAnd deltaminIs a self-selection parameter.
It can be seen that whenLateral position error yeWhen larger, Δ approaches ΔminThe shorter forward-looking distance parameter enables the unmanned ship to quickly approach the expected path; when the transverse position error yeWhen smaller, Δ approaches ΔmaxLonger forward-looking distance parameters help to avoid lateral position errors y when the unmanned vehicle is tasked with path trackingeAnd (4) overshooting.
The desired speed and path parameter update rate are designed as follows:
Figure BDA0003302355050000091
Figure BDA0003302355050000092
in the formula, kxAnd the & gt 0 is a set design parameter, and the U is a set initial path tracking speed.
It is proven that FLOS guidance system can make path tracking error (x)e,ye) The convergence to zero can be gradually carried out, and the process with the global gradual stability characteristic is as follows:
Figure BDA0003302355050000093
Figure BDA0003302355050000094
thus, it is possible to obtain:
Figure BDA0003302355050000095
Figure BDA0003302355050000096
when t is more than or equal to tobWhen there is
Figure BDA0003302355050000097
At this time, the above formula can be rewritten as:
Figure BDA0003302355050000098
Figure BDA0003302355050000099
definition of ε ═ xe,ye]TSelecting the following Lyapunov function V for the position error vector under the SF coordinate systemε：
Figure BDA00033023550500000910
Derivation of the above equation yields:
Figure BDA0003302355050000101
due to the presence of actual path tracking errors
Figure BDA0003302355050000102
The above formula can be arranged into the following form
Figure BDA0003302355050000103
Wherein
Figure BDA0003302355050000104
k＝2min(k1,k2)
The flo guidance system has a global asymptotic stability characteristic.
And 3, establishing a USV heading dynamic error discrete control model and a longitudinal speed tracking error control model. Based on a USV continuous motion mathematical model, a Euler approximate discretization method is adopted in consideration of the sampling frequency of the digital controller, and a longitudinal and rotary discretization error model is constructed, so that the method is suitable for the heading tracking control and longitudinal speed control design of the digital controller.
Step 3.1 define heading error psie＝ψ-ψdThe real-time rotation rate of the unmanned ship is r, and the tracking error of the rotation rate is r
Figure BDA0003302355050000105
Establishing a heading dynamic error continuous model based on the USV horizontal plane three-degree-of-freedom motion model as follows:
Figure BDA0003302355050000106
wherein m is11,m22,m33Inertia matrix for unmanned boat mathematical model
Figure BDA0003302355050000107
A middle corresponding position variable, wherein m11＝m22＝m，m33＝IzM is the unmanned surface vehicle mass, IzThe unmanned ship rotates around the z-axis. d32,d33Damping matrix for unmanned ship mathematical model
Figure BDA0003302355050000108
Corresponding to the position variable.
The Euler approximate discrete model heading dynamic error system is as follows:
Figure BDA0003302355050000109
wherein T is the sampling period of the digital controller, k is greater than 0 and is a positive integer.
Step 3.2, when defining a path tracking task, the tracking error of the longitudinal speed is as follows: u. ofe＝u-udThen the longitudinal velocity tracking based on the Euler approximate discrete model can be obtainedTracking error system:
Figure BDA0003302355050000111
and 4, constructing a discrete terminal sliding mode surface and a dynamic discrete adaptive fast power approach law, respectively designing a heading and speed discrete controller, compensating the external environment force by introducing a discrete interference estimator, realizing fast tracking of the expected heading and the expected speed under disturbance, and further realizing accurate path tracking of the USV. The improved discrete adaptive fast power approach law can meet the conditions of the existence and accessibility of a system sliding mode in any given sampling period, namely, the decoupling of the control stability of the system and the sampling period of a digital controller is realized:
step 4.1USV heading controller design:
selecting a discrete sliding mode surface of a heading dynamic error system as follows:
Figure BDA0003302355050000112
wherein, T alpha is more than 010＜1、α11＞0、0＜β2< 1 is the selected design parameter.
Designing a discrete adaptive fast power approach law of heading:
Figure BDA0003302355050000113
wherein, c7，c8＞0,c7T < 1 is a selected design parameter,
adaptive factor
Figure BDA0003302355050000114
The expression is as follows:
Figure BDA0003302355050000115
wherein,
Figure BDA0003302355050000116
estimating the disturbance d acting on the USV heading dynamic error model at the current moment by adopting a time delay estimation methodr[k]Item (1):
Figure BDA0003302355050000117
designing heading discrete tracking controller taur[k]The following were used:
Figure BDA0003302355050000121
in the formula,
Figure BDA0003302355050000122
step 4.2USV longitudinal speed controller design
Selecting a discrete integral sliding mode surface of a speed dynamic error model as follows:
Figure BDA0003302355050000123
wherein alpha is12> 0 are selected design parameters.
Designing a discrete adaptive fast power approach law of speed:
Figure BDA0003302355050000124
wherein, c9,c10＞0,c9T < 1 is a parameter to be designed,
Figure BDA0003302355050000125
the expression is as follows:
Figure BDA0003302355050000126
method for estimating longitudinal environment disturbance d by adopting time delay estimation methodu[k]The specific expression is as follows:
Figure BDA0003302355050000127
wherein d is11,d12,d13Damping matrix for unmanned ship mathematical model
Figure BDA0003302355050000128
Corresponding to the position variable.
Designing a speed discrete tracking controller tauu[k]The following were used:
Figure BDA0003302355050000131
FIG. 2 is a USV sinusoidal path tracking track curve, and it can be seen that when a controller designed based on the Euler approximate discrete model acts on the original continuous system, the USV path tracking task can be excellently completed. Fig. 3 shows the position error vector of the USV in the SF coordinate system, and it can be seen that the convergence accuracy of the longitudinal position error is high, and the convergence accuracy of the lateral position error is low. The convergence rate of the longitudinal position error is faster than that of the lateral position error. Fig. 4 shows the real value of the sideslip angle and the observer observed value of the USV in the path tracking process, and it can be seen that the sideslip angle always fluctuates in a small range, and the observation error is large in the initial stage of the observation error of the sideslip angle, but still rapidly converges to 0. FIG. 5 is a USV heading angle and heading angle error curve, and it can be seen from the graph that the USV heading angle is approximately in a sinusoidal variation trend after 60s, which meets the condition that the USV needs to continuously adjust the heading angle when tracking a sinusoidal path. FIG. 6 is a USV longitudinal velocity and longitudinal velocity error curve, and compared with the USV heading control system, the USV longitudinal velocity control system has a relatively slow convergence rate, but the convergence accuracy of the longitudinal velocity tracking error is higher than that of the heading angle tracking error. Fig. 7 shows the USV interference actual value and the interference value estimated by the delay estimation method, and it can be seen that the disturbance estimated by the delay estimation method has a certain lag compared with the true disturbance. FIG. 8 is a USV actual control input curve, and it can be seen that the control input is maintained in zero order in adjacent sampling periods, the USV longitudinal thrust completes convergence within 100s, and the heading turning moment completes convergence within 30 s. Due to the adoption of sliding mode control, the USV control input has certain buffeting.

Claims (6)

1. A USV path tracking control method is characterized by comprising the following steps:
step 1: establishing an SF coordinate system position tracking error system, designing a limited time sideslip angle observer to obtain an estimated value of the sideslip angle
Figure FDA0003302355040000011
Step 2, adopting FLOS guiding algorithm, utilizing the estimated value of the sideslip angle
Figure FDA0003302355040000012
Giving an expected heading, an expected speed and an update rate of the path parameters;
and step 3: establishing an USV heading dynamic error discrete control model and a longitudinal speed tracking error discrete control model;
and 4, step 4: a heading discrete controller and a longitudinal speed discrete controller are designed by constructing a discrete sliding mode surface and a discrete adaptive fast power approach law, and a discrete interference estimator is adopted to compensate the external environment force, so that the tracking of the expected heading and the expected speed under disturbance is completed.
2. The USV path tracking control method according to claim 1, wherein: step 1, designing a finite time sideslip angle observer to obtain an estimated value of the sideslip angle
Figure FDA0003302355040000013
The method specifically comprises the following steps:
designing an unknown time-varying sideslip angle observer:
Figure FDA0003302355040000014
Figure FDA0003302355040000015
Figure FDA0003302355040000016
wherein λ isi＞0,i＝1,2,L＞0，λiL is a positive integer, theta is a path parameter, psi is a real-time heading angle of the unmanned ship, u is a transverse speed of the unmanned ship, and g (u, psi)θ,β)＝ucos(ψ-ψθ)tanβ，
Figure FDA0003302355040000017
Initial value of
Figure FDA0003302355040000018
ψθThe rotation angle from the northeast coordinate system to the SF coordinate system meets the following requirements: psiθ＝arctan2(y′θ,x′θ)，pθ＝[xθ,yθ]TCoordinates of target tracking points for arbitrary paths on the desired path curve, pe＝[xe,ye]TFor a target error value, satisfy:
Figure FDA0003302355040000019
wherein p ═ x, y]TIs a target real-time location;
the sideslip angle estimate is then:
Figure FDA00033023550400000110
3. the USV path tracking control method according to claim 1, wherein: step 2 utilizing the slip angle estimate
Figure FDA0003302355040000021
The update rate of the expected heading, the expected speed and the path parameters is given by the following formula:
Figure FDA0003302355040000022
Figure FDA0003302355040000023
Figure FDA0003302355040000024
wherein psidTo expect heading, udIn order to be able to take the desired speed,
Figure FDA0003302355040000025
the path parameter update rate; psiθIs the angle of rotation, psi, of the North east coordinate system to the SF coordinate systemθ＝arctan2(y′θ,x′θ)，kxIf the parameter is more than 0, the U is a set initial path tracking speed, and psi is a real-time heading angle of the unmanned ship; delta is a time-varying dynamic foresight distance parameter and satisfies the following conditions:
Figure FDA0003302355040000026
where k > 0 is the parameter to be designed, ΔmaxAnd deltaminTime varying dynamic look-ahead distance parameter for a given parameterMaximum and minimum values of pθ＝[xθ,yθ]TCoordinates of target tracking points for arbitrary paths on the desired path curve, pe＝[xe,ye]TFor a target error value, satisfy:
Figure FDA0003302355040000027
wherein p ═ x, y]TIs the target real-time location.
4. The USV path tracking control method according to claim 1, wherein: step 3, establishing the USV heading dynamic error discrete control model and the longitudinal speed tracking error control model specifically comprises the following steps:
defining the heading angle error psie＝ψ-ψdThe slew rate tracking error is
Figure FDA0003302355040000028
r is the real-time rotation rate of the unmanned ship, and a Euler approximate discretization method is adopted to obtain a USV heading dynamic error discretization control model:
Figure FDA0003302355040000029
wherein m is11,m22,m33Inertia matrix for unmanned boat mathematical model
Figure FDA00033023550400000210
A middle corresponding position variable, wherein m11＝m22＝m，m33＝IzM is the unmanned surface vehicle mass, IzRotational inertia about the z-axis, d, for unmanned boats32,d33Damping matrix for unmanned ship mathematical model
Figure FDA0003302355040000031
Middle corresponding positionA set variable, T is the sampling period of the digital controller, k > 0 and is a positive integer, psie[k]Is the heading angle error at the moment k, re[k]For the slew rate tracking error at time k, psid[k]For the desired heading angle at time k, u k]Is the transverse velocity at time k, vk]Longitudinal velocity at time k, τr[k]Is a heading controller dr[k]Disturbance of k moment in the heading dynamic error model is obtained;
define the longitudinal velocity tracking error as: u. ofe＝u-udAnd obtaining a longitudinal speed tracking error discrete control model by adopting an Euler approximate discrete method:
Figure FDA0003302355040000032
wherein d is11，d12 d13Damping matrix for unmanned ship mathematical model
Figure FDA0003302355040000033
Middle corresponding position variable, du[k]For longitudinal environmental disturbances at time k, τu[k]Is a longitudinal speed discrete controller.
5. The USV path tracking control method according to claim 1, wherein: step 4, designing a heading discrete controller by constructing a discrete sliding mode surface and a discrete adaptive fast power approach law specifically comprises the following steps:
selecting a discrete sliding mode surface of a heading dynamic error system:
Figure FDA0003302355040000034
wherein alpha is11、α10、β2Is selected as the parameter value and satisfies 0 < T alpha10＜1、α11＞0、0＜β2＜1，ψe[k]Is the heading angle error at the moment k, re[k]The rotation rate tracking error at the moment k is obtained;
designing a heading discrete adaptive fast power approach law:
Figure FDA0003302355040000035
wherein, c7,c8Is a selected parameter value and satisfies c7,c8＞0,c7T＜1；
Adaptive factor
Figure FDA0003302355040000036
The expression is as follows:
Figure FDA0003302355040000037
wherein,
Figure FDA0003302355040000041
estimating disturbance d of current moment acting on USV heading dynamic error model by adopting time delay estimation methodr[k]Item (1):
Figure FDA0003302355040000042
wherein m is11,m22,m33Inertia matrix for unmanned boat mathematical model
Figure FDA0003302355040000043
A middle corresponding position variable, wherein m11＝m22＝m，m33＝IzM is the unmanned surface vehicle mass, IzRotational inertia about the z-axis, d, for unmanned boats32,d33Damping matrix for unmanned ship mathematical model
Figure FDA0003302355040000044
Middle correspondencePosition variable, T is sampling period of digital controller, u [ k-1 ]]Transverse velocity at time k-1, v [ k-1 ]]Longitudinal velocity at time k-1, τr[k-1]Is a heading controller dr[k]For disturbances experienced at time k in the dynamic error model of heading, psid[k-1]The desired heading angle at time k-1;
designing a heading discrete controller taur[k]Comprises the following steps:
Figure FDA0003302355040000045
in the formula, x4Comprises the following steps:
Figure FDA0003302355040000046
wherein, taur[k]Is a heading controller, #d[k]For the desired heading angle at time k, u k]Is the transverse velocity at time k, vk]Longitudinal velocity at time k, r k]For the real-time slew rate at time k, dr[k]The disturbance of k time in the heading dynamic error model.
6. The USV path tracking control method according to claim 1, wherein: step 4, designing a longitudinal speed discrete controller by constructing a discrete sliding mode surface and a discrete adaptive fast power approach law specifically comprises the following steps:
selecting a discrete integral sliding mode surface of a longitudinal speed tracking error model as follows:
Figure FDA0003302355040000051
wherein alpha is12Is a selected parameter value and satisfies alpha12＞0，ue[k]Longitudinal velocity tracking error at time k;
designing a longitudinal speed discrete adaptive fast power approximation law:
Figure FDA0003302355040000052
wherein, c9,c10Is a selected parameter value and satisfies: c. C9,c10＞0,c9T＜1，
Figure FDA0003302355040000053
The expression is as follows:
Figure FDA0003302355040000054
method for estimating longitudinal environment disturbance d by adopting time delay estimation methodu[k]Comprises the following steps:
Figure FDA0003302355040000055
wherein u ise[k]For longitudinal velocity tracking error at time k, u [ k-1 ]]Transverse velocity at time k-1, v [ k-1 ]]Longitudinal velocity at time k-1, r k-1]Is the real-time slew rate at the time of k-1, wherein m11,m22Inertia matrix for unmanned boat mathematical model
Figure FDA0003302355040000056
A middle corresponding position variable, wherein m11＝m22M is the unmanned ship mass, d11,d12,d13Damping matrix for unmanned ship mathematical model
Figure FDA0003302355040000057
Middle corresponding position variable
Designing a longitudinal velocity discrete controlleru[k]Comprises the following steps:
Figure FDA0003302355040000058
CN202111194077.3A 2021-10-13 2021-10-13 USV path tracking control method Active CN113805486B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111194077.3A CN113805486B (en) 2021-10-13 2021-10-13 USV path tracking control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111194077.3A CN113805486B (en) 2021-10-13 2021-10-13 USV path tracking control method

Publications (2)

Publication Number Publication Date
CN113805486A true CN113805486A (en) 2021-12-17
CN113805486B CN113805486B (en) 2024-06-28

Family

ID=78897700

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111194077.3A Active CN113805486B (en) 2021-10-13 2021-10-13 USV path tracking control method

Country Status (1)

Country Link
CN (1) CN113805486B (en)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20110114039A (en) * 2010-04-12 2011-10-19 전북대학교산학협력단 Dr/gps data fusion method
CN106950974A (en) * 2017-04-19 2017-07-14 哈尔滨工程大学 Drive lacking Autonomous Underwater Vehicle is understood three-dimensional path and tracking and controlling method
CN108563113A (en) * 2018-06-12 2018-09-21 哈尔滨工程大学 Naval vessel is with multiloop model-free adaption bow to control method
CN108829102A (en) * 2018-06-12 2018-11-16 哈尔滨工程大学 The wave glider course heading control method that adaptive bow is merged to information
CN111045332A (en) * 2019-12-27 2020-04-21 哈尔滨工程大学 Unmanned ship path tracking guidance strategy and disturbance compensation method
CN111580523A (en) * 2020-05-19 2020-08-25 哈尔滨工程大学 Unmanned ship path tracking active disturbance rejection control method based on sideslip angle compensation
CN113110551A (en) * 2021-04-23 2021-07-13 西安理工大学 Design method of attitude subsystem controller for controlling input saturation aiming at attitude angle

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20110114039A (en) * 2010-04-12 2011-10-19 전북대학교산학협력단 Dr/gps data fusion method
CN106950974A (en) * 2017-04-19 2017-07-14 哈尔滨工程大学 Drive lacking Autonomous Underwater Vehicle is understood three-dimensional path and tracking and controlling method
CN108563113A (en) * 2018-06-12 2018-09-21 哈尔滨工程大学 Naval vessel is with multiloop model-free adaption bow to control method
CN108829102A (en) * 2018-06-12 2018-11-16 哈尔滨工程大学 The wave glider course heading control method that adaptive bow is merged to information
CN111045332A (en) * 2019-12-27 2020-04-21 哈尔滨工程大学 Unmanned ship path tracking guidance strategy and disturbance compensation method
CN111580523A (en) * 2020-05-19 2020-08-25 哈尔滨工程大学 Unmanned ship path tracking active disturbance rejection control method based on sideslip angle compensation
CN113110551A (en) * 2021-04-23 2021-07-13 西安理工大学 Design method of attitude subsystem controller for controlling input saturation aiming at attitude angle

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
BINGBING QIU: "Adaptive Sliding Mode Trajectory Tracking Control for Unmanned Surface Vehicle with Modeling Uncertainties and Input Saturation", APPLIED SCIENCES, vol. 9, no. 6, pages 1 - 18 *
吴博: "无人艇艏向自适应离散滑模控制器设计", 中国航海, vol. 44, no. 3, pages 126 - 133 *

Also Published As

Publication number Publication date
CN113805486B (en) 2024-06-28

Similar Documents

Publication Publication Date Title
CN106292287B (en) A kind of UUV path following method based on adaptive sliding-mode observer
CN109460043B (en) Multimode nonsingular terminal sliding mode based ship track active disturbance rejection control method
CN109189071B (en) Robust self-adaptive unmanned ship path tracking control method based on fuzzy observer
CN110032197B (en) Unmanned ship path tracking control method based on limited time guidance and control
CN108614426B (en) Multi-mobile-robot formation robust control method based on disturbance observer
CN106950974B (en) Three-dimensional path understanding and tracking control method for under-actuated autonomous underwater vehicle
CN109857124B (en) Unmanned ship accurate path tracking control method based on speed and course dual guidance
Park Adaptive formation control of underactuated autonomous underwater vehicles
CN111580523B (en) Unmanned ship path tracking active disturbance rejection control method based on sideslip angle compensation
CN107479384B (en) The non-backstepping control method of hypersonic aircraft neural network Hybrid Learning
CN106444806B (en) The drive lacking AUV three-dimensional track tracking and controlling method adjusted based on biological speed
CN112346465B (en) IALOS guide law-based adaptive fuzzy control method for under-actuated unmanned ship
CN110618686B (en) Unmanned ship track control method based on explicit model predictive control
CN110687799B (en) Fuzzy self-adaptive output feedback control method and system for intelligent ship autopilot system
CN111897225B (en) Fuzzy self-adaptive output feedback control method and system for intelligent ship autopilot system
CN113467231B (en) Unmanned ship path tracking method based on sideslip compensation ILOS guidance law
CN105549395B (en) Ensure the mechanical arm servo-drive system dead time compensation control method of mapping
CN112462773B (en) Path tracking anti-saturation robust control method of under-actuated surface vessel
CN109656142B (en) Cascade structure model-free self-adaptive guidance method for unmanned ship
CN110007606A (en) A kind of unmanned surface vehicle error constraints control method considering input saturation
CN111221335A (en) Fuzzy self-adaptive output feedback finite time control method and system for intelligent ship autopilot system
CN111930124A (en) Fuzzy self-adaptive output feedback finite time control method and system for intelligent ship autopilot system
CN113110511B (en) Intelligent ship course control method based on generalized fuzzy hyperbolic model
CN113608534A (en) Unmanned ship tracking control method and system
CN107976296A (en) A kind of aerodynamic characteristics of vehicle on-line identification method based on backtracking adaptive algorithm

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