CN106950974B - Three-dimensional path understanding and tracking control method for under-actuated autonomous underwater vehicle - Google Patents
Three-dimensional path understanding and tracking control method for under-actuated autonomous underwater vehicle Download PDFInfo
- Publication number
- CN106950974B CN106950974B CN201710258682.XA CN201710258682A CN106950974B CN 106950974 B CN106950974 B CN 106950974B CN 201710258682 A CN201710258682 A CN 201710258682A CN 106950974 B CN106950974 B CN 106950974B
- Authority
- CN
- China
- Prior art keywords
- line segment
- tracking
- straight
- deviation
- underwater vehicle
- 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 50
- 238000004364 calculation method Methods 0.000 claims description 13
- 238000004422 calculation algorithm Methods 0.000 claims description 12
- 230000008859 change Effects 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 2
- 238000011835 investigation Methods 0.000 abstract description 4
- 238000012360 testing method Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000003032 molecular docking Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000036962 time dependent Effects 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
- 238000002054 transplantation Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Images
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/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a method for understanding and tracking and controlling a three-dimensional path of an under-actuated autonomous underwater vehicle. Firstly, the method comprises the following steps: the three-dimensional path obtained by the global path planning is understood as a space straight-line segment sequence; II, secondly: describing the space straight line segment as a target straight line segment of the autonomous underwater vehicle, projecting under an inertial coordinate system, forming a two-dimensional straight line on a horizontal plane, and forming a depth and height coordinate sequence in a vertical direction; thirdly, the method comprises the following steps: tracking and controlling a single horizontal plane target straight-line segment, wherein a controller adopts a layered structure, a guidance controller positioned on the upper layer converts the position deviation into a reference heading angle, and a state controller positioned on the lower layer converts the heading angle deviation into a rotation execution angle of a control surface; fourthly, the method comprises the following steps: and (4) replacing the target straight-line segments, completing the tracking of each target straight-line segment, and finally realizing the three-dimensional path tracking. The invention is suitable for the under-actuated autonomous underwater vehicle to carry out remote navigation and investigation operation, and has certain ocean current interference resistance.
Description
Technical Field
The invention relates to a control method of an autonomous underwater vehicle, in particular to a three-dimensional path understanding and tracking control method of an under-actuated autonomous underwater vehicle.
Background
The tasks performed by the AUV are mostly of an investigative nature. In fact, the form of tracking control of the target in space by the autonomous underwater vehicle is discrete and discontinuous. Even in the case of complex curves in analytical form, they are represented in the form of discrete points for the control system. In some applications, such as "an under-actuated AUV area search method based on electronic chart" (robot, 2014, vol 36, No. 5), "Docking control system for a 54-cm-diameter (21-in) AUV" (IEEE journal of Oceanic Engineering,2008, vol 33, No. 4), etc., an underwater vehicle is required to have good planar and linear course tracking capability, such as coverage underwater target search, submarine topography survey, autonomous underwater Docking, etc. According to submarine maneuverability knowledge, the underwater vehicle with the submarine body symmetrical about the xoz plane has linear stability, non-directional stability and course stability when moving under the condition of no maneuvering force. Therefore, a control system comprising actuators such as a propeller and a control surface is required to intervene to ensure the straight line course tracking of the underwater vehicle so as to perform reliable underwater navigation operation.
On the one hand, the dynamics model based controller has difficulty in application: although the backstepping design method, the local feedback linearization idea, the neural network control and the like are greatly developed in theory, the method is mostly in the stage of numerical simulation research (an under-actuated UUV space target tracking nonlinear control method based on the backstepping method, the doctrine of doctors at harbin university of engineering, 2012) and cannot be effectively applied to a carrier. On the one hand, most of the control methods applied to the tracking of the waypoints and the virtual points cannot effectively resist ocean current interference (regional search of underwater robots based on fuzzy theory in ocean current environment, university of Harbin engineering Master academic thesis, 2005). The control method adopted by the invention has the advantages of sufficient theoretical basis, simplicity, easy shape and good environment adaptability: the intelligent PID method does not depend on a mathematical model, has the advantages of mature theory, simple principle, good applicability, convenient use and the like, and is used as a planning layer of an intelligent system rational behavior model to form a path tracking guidance controller; on the other hand, according to the research on motion control of multifunctional autonomous underwater robots (doctrine of doctors, 2012), the S-plane control method is successfully applied to AUV motion control, so that the control of motion states such as speed, heading, trim and the like under interference is realized, the control is stable in ten-year marine exploration tests and tasks, and the control is used as an execution layer of an intelligent system rational behavior model to form a state controller for path tracking.
Disclosure of Invention
The invention aims to provide a method for understanding and tracking and controlling a three-dimensional path of an under-actuated autonomous underwater vehicle, which can enable the autonomous underwater vehicle to effectively resist ocean current interference and stably and accurately track the three-dimensional path.
The purpose of the invention is realized as follows:
the method comprises the following steps: the three-dimensional path obtained by the global path planning is understood as a space straight-line segment sequence;
step two: describing the space straight line segment as a target straight line segment of the autonomous underwater vehicle, projecting under an inertial coordinate system, forming a two-dimensional straight line on a horizontal plane, and forming a depth and height coordinate sequence in a vertical direction;
step three: the autonomous underwater vehicle in an under-actuated control mode performs tracking control on a single horizontal plane target straight-line segment, a controller adopts a layered structure, a guidance controller positioned on the upper layer converts position deviation into a reference heading angle, and a state controller positioned on the lower layer converts heading angle deviation into a rotation execution angle of a control surface;
step four: and D, replacing the target straight-line segments in the step two, sequentially operating the step three to complete the tracking of each target straight-line segment, and finally completing the tracking of the space straight-line segment sequence to realize the three-dimensional path tracking.
The invention may also include the following features:
1. the understanding method of the global path specifically includes:
globally transforming a given three-dimensional path into a sequence of spatial straight-line segments, using different understanding methods depending on the representation of the global path, said different understanding methods comprising:
(1) if the global path is in a sequence of discrete pointsp＝{p0,p1,p2,…,pMAnd expressing, performing interpolation calculation according to a cubic B-spline theory, firstly solving an analytical expression of a spline curve defined by discrete points, and according to a formula:
secondly, the value of the variable t is selected according to certain fineness requirements, and a uniform point sequence p '═ { p'0,p′1,p′2,…,p′QQ-M · s, s is the variable related to t representing the resolution,
with a uniform point sequence, the expression for the straight-line segment sequence is:
l＝{l0-1,l1-2,l2-3,…,li-i+1,…,lQ-1-Q},i＝0,1,2,…,Q-1，
expression for a single target straight-line segment:
(2) if the global path has an analytic expression, e.g., l-L (t), t ∈ (a, b), then the value of t is uniformly taken to obtain a point sequence, and the subsequent steps are the same as (1).
2. The tracking controller for tracking and controlling the single horizontal plane target straight-line segment specifically comprises:
the tracking controller has a hierarchical structure comprising: a guidance controller and a state controller,
the guidance controller takes the position deviation as control input and takes an intelligent PID algorithm as a control rate, and outputs a reference heading angle; the state controller takes the reference heading angle deviation as control input and takes the self-adaptive S-plane algorithm as a control rate to output the rotation execution angle of the control surface.
3. And the guidance controller calculates the distance between the carrier and the target straight-line segment, and calculates and outputs a reference heading angle by adopting an intelligent PID algorithm as a control rate.
4. Guidance controller passes through the geometry meterCalculating the transverse deviation P between the carrier and the target straight line segmenteThe three-dimensional space projection of a straight line route S in the three-dimensional space on a horizontal plane E- ξη is ST0-T1Zeta 0, defined by point T on the one-dimensional plane0(ξ0,η0) And T1(ξ1,η1) The determined direction line segment (T)0Not equal to T1), position P of AUVs(ξs,ηs) Heading psisThe method comprises the following steps:
(1): calculating the transverse distance, namely the absolute value of the deviation, and the formula is as follows:
(2): calculating positive and negative, and specifying that the autonomous underwater vehicle is located at a direction line segment T0T1Left side of (A) Pe＞0：
sgn(Pe)＝sgn[(ξ1-ξ0)·(η1-ηs)-(η1-η0)·(η1-ηs)]，
(3): calculating a transverse deviation;
Pe＝sgn(Pe)·|Pe|。
5. the guidance law adopts an intelligent PID algorithm, and the PID algorithm adopts a differential form:
n is the clock beat, MIClock beats for the start of integration, MDA starting beat calculated for the deviation average rate of change for the last few calculation cycles; k is a radical ofp，ki，kdRespectively, proportional-integral-derivative coefficient, according to PeAnd
the change of (c) is adjusted in a fuzzy adaptive manner:
6. the state controller adopts an S-surface control method of self-adaptive parameter adjustment, and the expression is as follows:
e、
respectively representing the deviation of the heading angle and the deviation change rate for control input, f is the heading turning moment with control output and normalized physical meaning, ke、kvFor controlling the parameters, the adjustment is made in a fuzzy self-adaptive manner
7. The target replacement is carried out according to the relative position relation between the autonomous underwater vehicle and the target straight-line segment, and the method specifically comprises the following steps:
computing vectors
And vector
If the mode exceeds, the target replacement is considered to be performed.
The invention provides a three-dimensional global path discretization understanding and layered self-adaptive tracking control method which can enable an autonomous underwater vehicle to effectively resist ocean current interference, stably and accurately track a three-dimensional path and further realize remote navigation and investigation tasks.
The present invention discretizes the curve trace into a straight line segment sequence of traces. The processing mode is simple, easy, safe and reliable, and is convenient to deal with the most common interference form of underwater navigation, namely ocean current. In addition, the simplification of the path and the precision loss caused by the discretization process are controllable.
The invention has the following advantages and beneficial effects:
1. the method is a complete control system solution and has the characteristic of modularization. Independent of mathematical models of carriers and other structural factors, from path understanding to path tracking control, a global three-dimensional path expressed in an analytic form or a discrete point form is input, and control surface rotation amplitude measured by an angle is output. Convenient for transplantation and engineering application.
2. The robustness of the control system is good. The control algorithm does not depend on a dynamic model of the underwater vehicle, has the self-adaptive adjustment characteristic, is integrated on a plurality of heterogeneous underwater vehicle platforms, and achieves excellent control effect when navigation operation tests are carried out in different environments such as a pool test, a lake test, a marine test and the like.
3. The stability of path tracking control is good, the precision is high, and the anti-ocean current interference ability is strong. Under the action of the designed linear segment tracking controller, the underwater vehicle can sense the influence of ocean current on the transverse deviation, the reference heading angle can be adjusted to output under the action of a control algorithm, and the AUV is turned through adjusting the angle of the control surface by the state controller, so that the transverse deviation is eliminated. Taking an 80 kg-class underwater vehicle as an example, in a remote navigation test, whether still water (lake) or ocean with unsteady ocean current interference exists, the tracking deviation can be ensured to be less than or equal to 2.0m (85%), and no steady-state deviation exists.
Drawings
FIG. 1 is a profile view of an under-actuated underwater vehicle;
FIG. 2 is a block diagram of a layered planar linear segment tracking controller;
FIG. 3 is a block diagram of the intelligent PID guidance controller;
FIG. 4 is a feasibility analysis diagram of the under-actuated underwater vehicle for straight course tracking;
FIG. 5 is a schematic diagram of an underwater vehicle understanding and tracking spatial paths in an analytic form;
FIG. 6 is a schematic view of an underwater vehicle understanding the spatial path in the form of discrete control points;
FIG. 7 is a horizontal plane path tracking deviation of sea trial data;
FIG. 8 is a vertical coordinate tracking of the sea trial experimental data;
FIG. 9 is a diagram showing the variation of the reference heading angle and the state heading angle of the sea test data;
FIG. 10 is a table 1 illustrating adjustment of proportional parameters for fuzzy adjustment of PID control parameters;
FIG. 11 is a table 2 of adjustment of integral term parameters for fuzzy adjustment of PID control parameters;
FIG. 12 is a table 3 of adjustment of derivative term parameters for fuzzy adjustment of PID control parameters;
FIG. 13 is a flow chart of the present invention.
Detailed Description
The method mainly comprises the following steps:
the method comprises the following steps: the three-dimensional path in the analytic form or the three-dimensional path represented by the point sequence is subjected to cubic B-spline interpolation and is discretized into a uniform point sequence p '═ { p'0,p′1,p′2,…,p′QAnd considering that the original three-dimensional path can be formed by connecting straight-line segments formed by two adjacent points of the point sequence end to form a straight-line segment sequence l ═ l0-1,l1-2,l2-3,…,li-i+1,…,lQ-1-QRepresents;
step two: coordinate projection of a target straight-line segment, namely describing a spatial straight-line segment as a target straight-line segment, namely a current only target route of the AUV, performing coordinate projection on the target straight-line segment according to a decoupling control theory of the weak mobile body, forming a depth/height coordinate sequence in the vertical direction, and forming a two-dimensional straight-line segment in the horizontal plane;
step three: tracking control of two-dimensional straight line segments, under-actuated AUV controls bow turning and pitching movement through forward speed and control surface rotation, and changes position by forming attack angle with incoming flowThe tracking controller of the horizontal plane projection straight line is designed into a layered structure and a guidance controller
Deviation of position PeConversion to angular deviation of heading that can be understood for under-actuated AUV
State controller
And converting the deviation of the heading angle into a heading turning moment, and finally outputting the rotation angle of the control surface through thrust distribution calculation.
Step four: realizing the replacement of the target straight line segment in the step two according to the position P of the AUVs(ξs,ηs) And T0(ξ0,η0)、T1(ξ1,η1) The position relation of the three-dimensional path tracking method realizes the updating of the plane target straight-line segments, sequentially runs the step three, completes the tracking of each target straight-line segment, traverses the space straight-line segment sequence and realizes the three-dimensional path tracking.
The invention will be further described, by way of example, with reference to the accompanying drawings, in which:
the implementation platform of the invention is a microminiature autonomous underwater vehicle as shown in figure 1. an execution mechanism of a control system adopts an underactuated arrangement scheme, a set of propeller 1 is arranged at the stern part of a central line and consists of a direct current brushless motor and propellers, a cross-shaped control surface is arranged at the stern part and is powered by a stepping motor, elevators 3 are symmetrically arranged on the port and the starboard, and steering rudders 2 are symmetrically arranged on the upper and the lower sides.
The understanding of the three-dimensional path in the invention refers to a process of obtaining a uniformly distributed waypoint sequence by interpolation calculation from a three-dimensional path in an analytic form given by global path planning or a three-dimensional path in a discrete form represented by a waypoint sequence received through medium communication, and finally obtaining a waypoint sequence which can enable a control system to directly understand the calculated straight-line segment sequence of the horizontal plane and the key point sequence in the vertical direction. The method comprises the following specific steps:
if the global path is in a sequence of discrete points p ═ p { (p)0,p1,p2,…,pMThen, performing interpolation calculation according to a cubic B-spline theory, firstly solving an analytical expression of a spline curve defined by discrete points, according to a formula:
secondly, the value of the variable t is selected according to certain fineness requirements, and a uniform point sequence p '═ { p'0,p′1,p′2,…,p′Q-where Q is M · s, s is a variable related to t, representing resolution, and saved in mypathpt.dat file in the following format:
“index(LONG)latitude(double)longitude(double)depth(double)height(double)isPassed(BOOL)”。
with a uniform sequence of points, the expression for the sequence of straight line segments can be given:
l＝{l0-1,l1-2,l2-3,…,li-i+1,…,lQ-1-Q},i＝0,1,2,…,Q-1
at the same time, | l is satisfied0-1|＝|l1-2|＝…＝|lQ-1-Q|。
Expression for a single target straight-line segment:
if the global path has an analytic expression, e.g., l-L (t), t ∈ (a, b), then the value of t is taken uniformly, resulting in a point sequence, and the subsequent steps are as above.
The tracking control method for the three-dimensional path of the invention refers to tracking the obtained target straight-line segment l on a horizontal planei-i+1Tracking height value height in vertical directioni-i+1Or depth value depthi-i+1Here, a tracking control method of a horizontal plane target straight line segment is detailed:
the tracking controller has a layered structure, as in fig. 2, comprising: a guidance controller and a state controller.
The guidance controller has the structure shown in fig. 3: and outputting a reference heading angle by taking the position deviation as control input and an intelligent PID algorithm as a control rate. The specific calculation process and basis are as follows:
calculating the lateral position deviation Pe。
Coordinate conversion, unifying units, converting variables of the marked position information expressed by all the longitudes and latitudes into variables expressed by a metric unit'm' under a relative coordinate system, and according to a formula:
wherein, lat0 and lon0 represent the longitude and latitude relative to the origin of the coordinate system, and are generally the positions of the aircraft during GPS calibration after the initial power-on;
the tracking of the target line by the aircraft is schematically illustrated in fig. 4. Setting the straight line segment of the current target as a starting point T0(ξ0,η0) And T1(ξ1,η1) The determined direction line segment (T)0≠T1) Position P of the aircrafts(ξs,ηs) Heading psisCalculating the distance of the vehicle from the straight line segment of the target according to the following formula:
its positive and negative utilization vector
And
symbolic judgment of cross product, specifying that the vehicle is located on the direction line segment T0T1Left side of (A) Pe＞0：
sgn(Pe)＝sgn[(ξ1-ξ0)·(η1-ηs)-(η1-η0)·(η1-ηs)]
Transverse deviation i.e. Pe＝sgn(Pe)·|Pe|；
And designing a guidance law.
The output of the guidance controller is the reference heading angle psi of the aircraftrefIt is composed of two parts as a whole:
ψref＝ψo+ψc(Pe)
wherein psioThe direction of the target straight line segment itself is calculated according to the following formula:
ψcthe relative heading angle determined by the relative position relationship of the aircraft and the target straight-line segment is the core content of the navigator, and the calculation process is described as follows:
Pefor the state variable of the rider, the control rate is designed according to L yapunov stability theorem, and a L yapunov function is taken
The only equilibrium state of the system is the origin P e0. Scalar function V > 0, is positive definite, and PeV → ∞ and → ∞; due to PeIs a true measure of spatial relative position, whose value varies continuously and whose derivative with time is the velocity of the straight-line segment near the target, so that the function V has a continuous derivative, expressed as:
the explanation is as follows: when deviation PeWhen the speed is more than 0, the aircraft is controlled to generate
I.e. PeA decreasing state (approaching the target course from the positive direction); when deviation Pe< 0 hours, control the aircraft to make it
I.e. PeIncreased state (negative direction approaching target course). According to the illustration in fig. 4, the under-actuated vehicle adjusts the distance between itself and the target route by adjusting the heading, so the control rate is as follows:
n is the clock beat, MIThe clock beat for the start of integration takes 5-10 s, MDCalculating the initial beat value of the deviation average change rate of nearly several calculation periods to be 3-6 s; phi' normalized angle coefficient, which can be obtained by nonlinear Sigmoid function
Mapping to angular space { ψ | - π/2 ≦ ψ ≦ π/2}
kp，ki，kdRespectively, proportional, integral and differential term coefficients. According to the deviation P by adopting a fuzzy control mode in applicationeAnd rate of change of deviation
Three control parameters were adjusted:
the fuzzy rules for the three parameter adjustments are shown in tables 1-3 of fig. 10-12.
The state controller outputs the reference heading angle psi by the guidance controllerrefAnd a state heading angle psi given by a magnetic compass of an aircraft navigation sensorAUVThe difference delta psi is used as control input, and the adaptive S-surface algorithm is used as a control rate to calculate and output the heading turning moment.
The control rate is as follows:
wherein k ise、kvFor controlling parameters, self-adaptive adjustment is carried out in a fuzzy control mode:
fuzzy control rule and guidance control kp，ki，kdThe adjustment is similar and will not be described herein.
f0For the normalization value of the bow turning moment, according to the formula Mz＝f0·M-zCalculating the moment of bow-zThe steering moment generated by the vertical rudder under the maximum navigational speed and the maximum rudder angle.
The lift force of the vertical rudder of the rudder is calculated, and two stern vertical rudders which generate the fore turning moment are arranged symmetrically up and down, so that:
l-zrepresenting the moment arm caused by the geometric position of the rudder;
according to the formula
Calculating rudder angle of vertical rudder, F-zIndicating lift of rudder βEAn effective angle of attack for the rudder wing; vEThe effective advancing speed of the rudder wing is obtained; a. thefinThe lateral projection area of the rudder wing is shown; cL(E,z) The lift coefficient of the rudder wing is a function of the effective attack angle and can be obtained through empirical formula estimation, CFD numerical calculation and experiments.
The method comprises the following steps of carrying out target straight-line segment replacement according to the relative position relation between an aircraft and the target straight-line segment, wherein the specific criterion is as follows:
computingVector quantity
And vector
If the mode exceeds, the target replacement is considered to be performed.
Marine test verification and analysis:
as illustrated below, to verify the three-dimensional path understanding and tracking control method effectiveness of the autonomous underwater vehicle, a path understanding and tracking test under a given three-dimensional path was conducted, wherein two different forms of three-dimensional paths were understood as described in step 1 of the summary of the invention, first, given initial points (L at.135296.3, L on.440174.2, Ang.90) and end points (L at.135263.9, L on.440174.2, Ang.180), a global path planner gave a resolved form of the path, and the path understanding method described in this patent comprehended a resolved form of the path representation as a sequence of target straight-line segments as shown in FIG. 5, and second, given key path points p, and shown in FIG. 50(308，416，40)、p1(297.4，156.6，40)、p2(275.3，189.9，40)、p3(220.8，207.8，36)、p4(182.8，200.7，32)、p5(93.5，141.7，20)、p6(114.6，-84.1，8)、p7(84.9，-103.7，4)、p8(38.6，-98.4，40)、p9(-2.5，-40，0)、p10(-9.2，-15，40)、p11(0, 0, 0), the path understanding method described in this patent understands a path in the form of discrete points as a sequence of target straight-line segments, represented as fig. 6;
taking the path of fig. 5 as an example, the path tracking control is performed at sea, the cruising speed of the aircraft is set to be 1.2m/s, and the vertical coordinate of the path is set to be the depth of 2.0 m. The blue solid line in fig. 5 is a projection of a flight path plane of the aircraft, and it can be seen that the aircraft can track a target straight-line segment well, the target straight-line segment can be replaced according to the advance of the aircraft, and finally the aircraft can traverse a straight-line segment sequence forming a path;
FIG. 7 is a time-dependent variation of the lateral deviation, and it can be seen that after each change of the straight line segment of the target, particularly at a location where the path curvature is large, the lateral deviation has a step, and then the final convergence at zero point is reduced;
FIG. 8 is a plot of the depth variation of the aircraft, and it can be seen that the vertical coordinate controller is able to track the vertical coordinates stably and unbiased;
FIG. 9 is a comparison of a reference heading angle calculated by the guidance controller during path tracking with a state heading angle of the aircraft, which shows that the reference heading angle is constantly changing as the output of the guidance controller, and the state heading angle can be better consistent with the reference heading angle under the action of the state controller.
The sea test result shows that the method for understanding and controlling tracking of the three-dimensional path has the global gradual stability characteristic, can ensure the tracking without deviation, and is suitable for the under-actuated autonomous underwater vehicle to execute remote navigation and investigation type tasks under the complex sea condition.
The three-dimensional path is discretized into a straight line segment sequence according to a certain resolution ratio according to the requirement, and the path tracking is finished by tracking the straight line segment sequence. Tracking each target straight-line segment and achieving target replacement enables the tracking of a sequence of straight-line segments. And tracking the projection straight line segment of the target straight line on the horizontal plane, and simultaneously tracking and controlling the depth or the height on the vertical plane. The horizontal plane control adopts a layered control system structure: the planning layer calculates and outputs a reference heading angle through an intelligent PID control rate according to the transverse position deviation to form a guidance controller; and the execution layer calculates the output heading moment through a self-adaptive S control law according to the heading angle deviation and provides a control surface rotation angle through thrust distribution calculation to form a state controller. The method is suitable for the under-actuated autonomous underwater vehicle to carry out remote navigation and investigation operation, has certain ocean current interference resistance, does not have model dependence, has small quantity of control parameters, is easy to adjust, and has the accuracy and reliability which are verified by a plurality of ocean trials of a plurality of heterogeneous vehicles, thereby being widely applied.
Claims (1)
1. A three-dimensional path understanding and tracking control method of an under-actuated autonomous underwater vehicle is characterized by comprising the following steps:
the method comprises the following steps: the three-dimensional path obtained by the global path planning is understood as a space straight-line segment sequence;
step two: describing the space straight line segment as a target straight line segment of the autonomous underwater vehicle, projecting under an inertial coordinate system, forming a two-dimensional straight line on a horizontal plane, and forming a depth and height coordinate sequence in a vertical direction;
step three: the autonomous underwater vehicle in an under-actuated control mode performs tracking control on a single horizontal plane target straight-line segment, a controller adopts a layered structure, a guidance controller positioned on the upper layer converts position deviation into a reference heading angle, and a state controller positioned on the lower layer converts heading angle deviation into a rotation execution angle of a control surface;
step four: replacing the target straight-line segments in the second step, sequentially operating the third step to complete the tracking of each target straight-line segment, and finally completing the tracking of a space straight-line segment sequence to realize three-dimensional path tracking;
the understanding manner of the global path is specifically as follows:
globally transforming a given three-dimensional path into a sequence of spatial straight-line segments, using different understanding methods depending on the representation of the global path, said different understanding methods comprising:
(1) if the global path is in a sequence of discrete points p ═ p { (p)0,p1,p2,…,pMAnd expressing, performing interpolation calculation according to a cubic B-spline theory, firstly solving an analytical expression of a spline curve defined by discrete points, and according to a formula:
secondly, the value of the variable t is selected according to certain fineness requirements, and a uniform point sequence p '═ { p'0,p′1,p′2,…,p′QQ-M · s, s is the variable related to t representing the resolution,
with a uniform point sequence, the expression for the straight-line segment sequence is:
l＝{l0-1,l1-2,l2-3,…,li-i+1,…,lQ-1-Q},i＝0,1,2,…,Q-1，
expression for a single target straight-line segment:
(2) if the global path has an analytic expression that l is L (t) and t ∈ (a, b), uniformly taking the value of t to obtain a point sequence, and the subsequent steps are the same as (1);
the tracking controller for tracking and controlling the single horizontal plane target straight-line segment specifically comprises:
the guidance controller takes the position deviation as control input and takes an intelligent PID algorithm as a control rate, and outputs a reference heading angle; the state controller takes the reference heading angle deviation as control input and takes the self-adaptive S-plane algorithm as a control rate to output a rotation execution angle of the control surface;
the guidance controller calculates the transverse deviation P between the carrier and the target straight line segment in a geometric calculation modeeThe three-dimensional space projection of a straight line route S in the three-dimensional space on a horizontal plane E- ξη is ST0-T1Zeta 0, defined by point T on the one-dimensional plane0(ξ0,η0) And T1(ξ1,η1) Determined direction line segment, T0≠T1Position P of autonomous underwater vehicles(ξs,ηs) Heading psisThe method comprises the following steps:
(1) calculating the transverse distance, namely the absolute value of the deviation, and the formula is as follows:
(2) calculating positive and negative, and specifying that the autonomous underwater vehicle is located at a direction line segment T0T1Left side of (A) Pe＞0：
sgn(Pe)＝sgn[(ξ1-ξ0)·(η1-ηs)-(η1-η0)·(η1-ηs)]，
(3) Calculating a transverse deviation;
Pe＝sgn(Pe)·|Pe|；
the PID algorithm takes a differential form:
n is the clock beat, MIClock beats for the start of integration, MDA starting beat calculated for the deviation average rate of change for the last few calculation cycles; k is a radical ofp，ki，kdRespectively, proportional-integral-derivative coefficient, according to PeAnd
the change of (c) is adjusted in a fuzzy adaptive manner:
the state controller adopts an S-surface control method of self-adaptive parameter adjustment, and the expression is as follows:
e、
respectively representing the deviation of the heading angle and the deviation change rate for control input, f is the heading turning moment with control output and normalized physical meaning, ke、kvFor controlling the parameters, the adjustment is made in a fuzzy self-adaptive manner
The target replacement is carried out according to the relative position relation between the autonomous underwater vehicle and the target straight-line segment, and the method specifically comprises the following steps:
computing vectors
And vector
If the mode exceeds, the target replacement is considered to be performed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710258682.XA CN106950974B (en) | 2017-04-19 | 2017-04-19 | Three-dimensional path understanding and tracking control method for under-actuated autonomous underwater vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710258682.XA CN106950974B (en) | 2017-04-19 | 2017-04-19 | Three-dimensional path understanding and tracking control method for under-actuated autonomous underwater vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106950974A CN106950974A (en) | 2017-07-14 |
| CN106950974B true CN106950974B (en) | 2020-07-28 |
Family
ID=59477384
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710258682.XA Active CN106950974B (en) | 2017-04-19 | 2017-04-19 | Three-dimensional path understanding and tracking control method for under-actuated autonomous underwater vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106950974B (en) |
Families Citing this family (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107831781B (en) * | 2017-10-26 | 2020-10-09 | 成都信息工程大学 | Method and system for controlling movement of robotic fish |
| CN108008631B (en) * | 2017-11-29 | 2020-01-17 | 中国地质大学（武汉） | Underwater robot path tracking method, device and storage device |
| CN108415423B (en) * | 2018-02-02 | 2020-12-04 | 哈尔滨工程大学 | High-interference-rejection self-adaptive path following method and system |
| CN108444478B (en) * | 2018-03-13 | 2021-08-10 | 西北工业大学 | Moving target visual pose estimation method for underwater vehicle |
| CN110362098B (en) * | 2018-03-26 | 2022-07-05 | 北京京东尚科信息技术有限公司 | Unmanned aerial vehicle visual servo control method and device and unmanned aerial vehicle |
| CN109141421A (en) * | 2018-07-13 | 2019-01-04 | 哈尔滨工程大学 | Expected path building method in the underwater dynamic target tracking of drive lacking AUV |
| CN109814392B (en) * | 2019-02-21 | 2021-10-26 | 大连海事大学 | Fault-tolerant control method for fault robustness of under-actuated underwater robot actuator |
| CN110045751B (en) * | 2019-05-14 | 2022-10-04 | 哈尔滨工程大学 | Submarine fixed-point landing motion control method for cabled underwater robot based on SI control |
| CN113805598B (en) * | 2020-06-11 | 2023-03-28 | 中国科学院沈阳自动化研究所 | Navigation control method for under-actuated AUV (autonomous underwater vehicle) |
| CN112000116A (en) * | 2020-07-24 | 2020-11-27 | 西北工业大学 | Heading angle control method of autonomous underwater vehicle based on improved firefly PID method |
| CN112698273B (en) * | 2020-12-15 | 2022-08-02 | 哈尔滨工程大学 | Multi-AUV single-standard distance measurement cooperative operation method |
| CN113805486B (en) * | 2021-10-13 | 2024-06-28 | 哈尔滨工程大学 | USV path tracking control method |
| CN114489091B (en) * | 2022-01-21 | 2023-01-13 | 湖南大学无锡智能控制研究院 | Guidance law control method, equipment and system for autonomous underwater robot path tracking |
| CN115562313B (en) * | 2022-10-17 | 2024-09-17 | 武汉理工大学 | Autonomous underwater vehicle motion control method for bridge pier flaw detection |
| CN118331285B (en) * | 2024-06-13 | 2024-08-16 | 中国船舶集团有限公司第七〇七研究所 | Channel type underwater unmanned aircraft position control method based on control lever displacement signals |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102541062A (en) * | 2012-02-24 | 2012-07-04 | 天津大学 | Local path planning method for underwater autonomous aircraft |
| CN102722177A (en) * | 2012-06-27 | 2012-10-10 | 哈尔滨工程大学 | Autonomous underwater vehicle (AUV) three-dimensional straight path tracking control method with PID (Piping and Instruments Diagram) feedback gain |
| CN102768539A (en) * | 2012-06-26 | 2012-11-07 | 哈尔滨工程大学 | AUV (autonomous underwater vehicle) three-dimension curve path tracking control method based on iteration |
| US8600592B2 (en) * | 2012-03-02 | 2013-12-03 | Her Majesty The Queen In Right Of Canada As Represented By The Minister Of National Defence | Homing system and method for an autonomous underwater vehicle |
| CN103576693A (en) * | 2013-11-11 | 2014-02-12 | 哈尔滨工程大学 | Underwater robot three-dimensional path tracking control method based on second-order filter |
| CN103760907A (en) * | 2013-12-30 | 2014-04-30 | 哈尔滨工程大学 | Underwater vehicle three-dimensional route planning method based on cuckoo search algorithm |
| CN104020771A (en) * | 2014-06-13 | 2014-09-03 | 大连海事大学 | Under-actuated ship path tracking planning method based on dynamic virtual ship guidance algorithm |
| CN106444806A (en) * | 2016-09-27 | 2017-02-22 | 哈尔滨工程大学 | Under-actuated AUV (autonomous underwater vehicle) three-dimensional trajectory tracking control method based on biological speed regulation |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8830793B2 (en) * | 2012-06-28 | 2014-09-09 | Massachusetts Institute Of Technology | System and method for collision avoidance in underwater vehicles |
| US20160274259A1 (en) * | 2015-03-16 | 2016-09-22 | Cgg Services Sa | Burial depth detection apparatus and method for marine seismic surveys |
-
2017
- 2017-04-19 CN CN201710258682.XA patent/CN106950974B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102541062A (en) * | 2012-02-24 | 2012-07-04 | 天津大学 | Local path planning method for underwater autonomous aircraft |
| US8600592B2 (en) * | 2012-03-02 | 2013-12-03 | Her Majesty The Queen In Right Of Canada As Represented By The Minister Of National Defence | Homing system and method for an autonomous underwater vehicle |
| CN102768539A (en) * | 2012-06-26 | 2012-11-07 | 哈尔滨工程大学 | AUV (autonomous underwater vehicle) three-dimension curve path tracking control method based on iteration |
| CN102722177A (en) * | 2012-06-27 | 2012-10-10 | 哈尔滨工程大学 | Autonomous underwater vehicle (AUV) three-dimensional straight path tracking control method with PID (Piping and Instruments Diagram) feedback gain |
| CN103576693A (en) * | 2013-11-11 | 2014-02-12 | 哈尔滨工程大学 | Underwater robot three-dimensional path tracking control method based on second-order filter |
| CN103760907A (en) * | 2013-12-30 | 2014-04-30 | 哈尔滨工程大学 | Underwater vehicle three-dimensional route planning method based on cuckoo search algorithm |
| CN104020771A (en) * | 2014-06-13 | 2014-09-03 | 大连海事大学 | Under-actuated ship path tracking planning method based on dynamic virtual ship guidance algorithm |
| CN106444806A (en) * | 2016-09-27 | 2017-02-22 | 哈尔滨工程大学 | Under-actuated AUV (autonomous underwater vehicle) three-dimensional trajectory tracking control method based on biological speed regulation |
Non-Patent Citations (3)
| Title |
|---|
| 一种基于电子海图的欠驱动AUV区域搜索方案;李晔等;《机器人》;20140930;第609-618页 * |
| 具有学习能力的智能机器人体系结构研究;张汝波等;《华中科技大学学报（自然科学版）》;20041031;第58-60页 * |
| 考虑几何约束的AUV回收路径规划;李晔等;《机器人》;20150731;第478-485页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106950974A (en) | 2017-07-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106950974B (en) | Three-dimensional path understanding and tracking control method for under-actuated autonomous underwater vehicle | |
| CN107957727B (en) | Underwater robot control system and dynamic positioning method | |
| CN107168312B (en) | Space trajectory tracking control method for compensating UUV kinematic and dynamic interference | |
| Wang et al. | Surge-heading guidance-based finite-time path following of underactuated marine vehicles | |
| Xiang et al. | Smooth transition of AUV motion control: From fully-actuated to under-actuated configuration | |
| CN109828570B (en) | Control and guide method for self-adaptive boundary layer water surface unmanned ship | |
| CN106444806B (en) | The drive lacking AUV three-dimensional track tracking and controlling method adjusted based on biological speed | |
| Cardenas et al. | Estimation of AUV hydrodynamic coefficients using analytical and system identification approaches | |
| CN107953350A (en) | It is a kind of to be used to detect the underwater robot control system with operation | |
| CN108829099A (en) | A kind of drive lacking unmanned boat trace tracking method based on the control of limited Backstepping | |
| CN108319140A (en) | A kind of output redefinition formula model-free adaption course heading control method and system | |
| CN113467231B (en) | Unmanned ship path tracking method based on sideslip compensation ILOS guidance law | |
| CN112068440B (en) | AUV recovery butt joint power positioning control method based on model prediction control | |
| CN109946976B (en) | Wide-navigational-speed AUV motion control method | |
| CN105974930A (en) | Method for tracking movement mother ship by UUV (Unmanned Underwater Vehicle) based on nonlinear model predictive control | |
| CN117389312B (en) | Model-based anti-roll three-dimensional tracking control method for underwater vehicle | |
| Dai et al. | Dual closed loop AUV trajectory tracking control based on finite time and state observer | |
| CN114609905A (en) | Ship formation event trigger control method | |
| Hu et al. | Disturbance Observer-Based Model Predictive Control for an Unmanned Underwater Vehicle | |
| Wang et al. | Adaptive nonlinear model predictive control for autonomous surface vessels with largely varying payload | |
| Nguyen et al. | Robust Adaptive Depth Control of hybrid underwater glider in vertical plane | |
| Wang et al. | Dynamic model-aided localization of underwater autonomous gliders | |
| CN106094842B (en) | A kind of UUV diving plane H based on T-S model and PDC∞Control method | |
| Harris et al. | Preliminary evaluation of null-space dynamic process model identification with application to cooperative navigation of underwater vehicles | |
| Zheng et al. | Survey of approaches for improving the intelligence of marine surface vehicles |
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 |