CN114019805B - Model prediction butt joint control method of under-actuated auv - Google Patents

Model prediction butt joint control method of under-actuated auv Download PDF

Info

Publication number
CN114019805B
CN114019805B CN202111361802.1A CN202111361802A CN114019805B CN 114019805 B CN114019805 B CN 114019805B CN 202111361802 A CN202111361802 A CN 202111361802A CN 114019805 B CN114019805 B CN 114019805B
Authority
CN
China
Prior art keywords
auv
docking
angle
approach angle
design
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
Application number
CN202111361802.1A
Other languages
Chinese (zh)
Other versions
CN114019805A (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.)
Jiujiang Vocational and Technical College
Original Assignee
Jiujiang Vocational and Technical College
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 Jiujiang Vocational and Technical College filed Critical Jiujiang Vocational and Technical College
Priority to CN202111361802.1A priority Critical patent/CN114019805B/en
Publication of CN114019805A publication Critical patent/CN114019805A/en
Application granted granted Critical
Publication of CN114019805B publication Critical patent/CN114019805B/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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

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)

Abstract

The invention discloses a model predictive butt joint control method of under-actuated auv, which comprises the following steps: designing a butt joint error model; the design of the docking controller includes the design of the desired approach angle and the design of the dynamics controller. Aiming at a common underactuated AUV lacking transverse and vertical driving forces, the invention adopts the USBL to position, acquires coordinates of four transponders on the horn mouth guiding type docking device in a carrier coordinate system, obtains the position and the posture of the AUV in a fixed coordinate system through coordinate transformation, considers the constraint of the USBL visual angle, optimizes the expected approach angle in the docking process by applying the MPC, realizes the docking control in a three-dimensional space, and effectively shortens the docking distance.

Description

Model prediction butt joint control method of under-actuated auv
Technical Field
The invention relates to the technology of autonomous underwater vehicles, in particular to a model predictive butt joint control method of under-actuated auv.
Background
Currently, autonomous Underwater Vehicles (AUVs) are widely used in marine development. When the AUV is to execute a given task under water, the AUV is firstly laid out by a mother ship, then the AUV carries out tracking control on a pre-planned path to finish investigation or detection of a target area, and after the task is finished, the AUV is recovered, namely, the AUV is returned to the mother ship to carry out the work of energy supply, data exchange (downloading detection data or receiving new tasks), overhaul and the like. The traditional recovery method is to hoist the AUV to recover when the AUV returns to and approaches the mother ship by adopting equipment such as a crane, but the method not only can increase the workload of a shipman, but also has lower automation degree, so how to realize autonomous recovery of the AUV is a current research hot spot. Typical recovery modes at present mainly comprise: platform type, direction formula, catching formula, torpedo transmitting tube type etc. wherein horn mouth direction formula recovery unit is simpler, and reliability and practicality are better, can realize the autonomous recovery of AUV, and the application is comparatively extensive.
The autonomous recovery of the AUV mainly has two difficulties, namely the problem of navigation and positioning of the AUV, because the AUV needs to acquire the position and the posture of the AUV in real time in the recovery process. The guiding recovery mode generally divides recovery into two steps of dock returning and docking, the dock returning aims to enable the AUV to return to a docking area through a path planned by tracking, the control accuracy requirement is low in this step, and the AUV adopts autonomous navigation. The AUV starts the docking program after entering the docking area, and the control precision requirement is higher. The underwater acoustic navigation positioning technology is widely applied due to higher positioning precision, wherein an ultra-short baseline positioning system (USBL, ultroshort Base Line) is simple in structure and small in size and is favored. Another difficulty with autonomous recovery of an AUV is its control problem. In order to reduce the cost and improve the reliability, many AUVs are designed in underactuated mode, i.e. lack of driving force in certain degrees of freedom, and in addition, the motion model is difficult to accurately acquire, and various interferences exist in the working environment, which all make the design of the controller difficult. Aiming at the problem of AUV butt joint, USBL is applied to position, an improved Kalman filtering algorithm is designed, and the problems of signal lag and interference are improved. Aiming at the problem of docking of a full-drive AUV, a controller is designed by applying a neural network and a sliding mode technology, so that the gesture control in the docking process is realized. Model Predictive Control (MPC) is convenient to handle control problems with constraints and is also widely used in autonomous recovery control of AUVs. A docking guidance algorithm based on MPC and fuzzy control is designed in literature, and autonomous recovery of AUV is achieved. The USBL is used for positioning, and a controller is designed based on the MPC, so that the docking control of the full-drive AUV is realized. The problem of constraint of butt joint is solved by applying MPC in literature, and autonomous recovery of UUV with a movable base is realized.
The above document mainly solves the problem of recovery of the AUV in the horizontal plane, and basically does not consider the influence of depth errors in the docking process. Aiming at a common underactuated AUV (lack of transverse and vertical driving forces), the invention adopts the USBL for positioning, and adopts the MPC to optimize the expected approach angle in the butt joint process, thereby realizing the butt joint control in the three-dimensional space.
Disclosure of Invention
The invention mainly aims to provide a model predictive butt joint control method of under-actuated auv.
The technical scheme adopted by the invention is as follows: a model predictive dock control method of under-actuated auv, comprising: designing a butt joint error model; the design of the docking controller includes the design of the desired approach angle and the design of the dynamics controller.
Further, the docking error model design includes: two coordinate systems are arranged, one is a carrier coordinate system
Figure 790117DEST_PATH_IMAGE001
The origin of which is defined at the floating center of the AUVOThe other is a fixed coordinate system +.>
Figure 219962DEST_PATH_IMAGE002
The origin of which is defined in the center of the horn mouth of the guiding type docking deviceEA place; of the carrier coordinate systemxThe axis is directed forward,yThe axis points to the right,zThe axis pointing downwards, the fixed coordinate systemξThe axis points forward and coincides with the butt joint path, η A transponder 2 with its axis pointing to the right, ζ A transponder 4 with its axis directed downwards; the docking error model can be simplified as
Figure DEST_PATH_IMAGE003
(1)
In the formula (1), the components are as follows,
Figure 133691DEST_PATH_IMAGE004
for AUV->
Figure 939973DEST_PATH_IMAGE005
The position and attitude of the lens, i.e. the docking error,
Figure 583444DEST_PATH_IMAGE006
representing AUV at->
Figure 590714DEST_PATH_IMAGE005
The coordinates of (a),θIs the pitch angle of AUV,ψIs the bow rocking angle of the AUV;
Figure 319636DEST_PATH_IMAGE007
representing AUV at->
Figure 929609DEST_PATH_IMAGE008
Is provided, the speed of the roller,uis a longitudinal speed,vIs a transverse velocity,wIs vertical velocity,qIs the pitch angle speed,rIs the yaw rate; />
Figure 693165DEST_PATH_IMAGE009
Is->
Figure 340178DEST_PATH_IMAGE008
To->
Figure 821975DEST_PATH_IMAGE005
Is used for the rotation transformation matrix of the (c),
Figure 970060DEST_PATH_IMAGE010
first, 4 transponders are acquired by USBL receiver
Figure DEST_PATH_IMAGE011
The coordinates of (2) and the butt joint error satisfy the relation (2),
Figure 994648DEST_PATH_IMAGE012
(2)
in the formula (2), the amino acid sequence of the compound,
Figure 937196DEST_PATH_IMAGE013
the coordinates of the 4 transponders acquired for the USBL receiver,lfor USBL receiver and floating coreODistance of->
Figure 640710DEST_PATH_IMAGE014
For 4 transponders +.>
Figure 467851DEST_PATH_IMAGE015
Coordinates of->
Figure 940421DEST_PATH_IMAGE016
For 4 transponders +.>
Figure 319450DEST_PATH_IMAGE017
Coordinates of (a) and (b);
the radius of the bell mouth of the butt joint device is 1 meter, so
Figure 510260DEST_PATH_IMAGE018
The method comprises the steps of carrying out a first treatment on the surface of the Through the relation
Figure 875513DEST_PATH_IMAGE019
Is available in the form of
Figure 468168DEST_PATH_IMAGE020
(3)
Through the relation
Figure 18098DEST_PATH_IMAGE021
Is available in the form of
Figure 568641DEST_PATH_IMAGE022
(4)
Through the relation
Figure 862219DEST_PATH_IMAGE023
Is available in the form of
Figure 309381DEST_PATH_IMAGE024
(5)。
Still further, the design of the desired approach angle includes: designing a guidance law to generate expected values of a yaw angle and a pitch angle, namely a desired approach angle; the LOS guidance law is adopted as follows
Figure DEST_PATH_IMAGE025
(6)
In (6)
Figure 171158DEST_PATH_IMAGE026
For the approach angle of the vertical plane->
Figure 336560DEST_PATH_IMAGE027
Is the approach angle of the horizontal plane, is->
Figure 168250DEST_PATH_IMAGE028
And->
Figure 610863DEST_PATH_IMAGE029
Is the forward looking distance; the MPC design guidance law is adopted to optimize the expected approach angle, and the error equation of the pitch angle and the yaw angle on the expected approach angle can be approximated as the following differential equation
Figure 502596DEST_PATH_IMAGE030
(7)
In (7)
Figure 889715DEST_PATH_IMAGE031
Is an adjustable time constant, wherein->
Figure 525096DEST_PATH_IMAGE032
；
According to the butt-joint error model (1), the butt-joint error equation in the transverse direction and the vertical direction can be simplified into
Figure 822216DEST_PATH_IMAGE033
(8)
In (8)
Figure 884850DEST_PATH_IMAGE034
When->
Figure 759265DEST_PATH_IMAGE035
Can be simplified into
Figure 932757DEST_PATH_IMAGE036
When->
Figure 349963DEST_PATH_IMAGE037
Can be simplified to +.>
Figure 583499DEST_PATH_IMAGE038
The method comprises the steps of carrying out a first treatment on the surface of the Combining the formula (7) and the formula (8),
the docking control can be equivalently a calm problem of the following errors
Figure 945210DEST_PATH_IMAGE039
(9)
Discretizing the equation (9) to obtain a prediction model of the docking error as
Figure 797759DEST_PATH_IMAGE040
(10)
Subscript in formula (10)kThe representative is the use of a time series,
Figure 194106DEST_PATH_IMAGE041
Figure 598542DEST_PATH_IMAGE042
and->
Figure 181970DEST_PATH_IMAGE043
Respectively a state vector and an output vector, which are butt joint errors +.>
Figure 838211DEST_PATH_IMAGE044
For the input vector, i.e. the desired approach angle,Tis the sampling period; at the position ofkTime of day, available future according to the predictive model (10)k+1 to->
Figure DEST_PATH_IMAGE045
The predicted value of the time butt-joint error is
Figure 620222DEST_PATH_IMAGE046
Of the formula (I)
Figure DEST_PATH_IMAGE047
Representing the control step size and the prediction step size, respectively, wherein +.>
Figure 336505DEST_PATH_IMAGE048
The method comprises the steps of carrying out a first treatment on the surface of the The predicted output value is further available as +.>
Figure DEST_PATH_IMAGE049
The predicted output values may be organized into the following matrix form
Figure 672808DEST_PATH_IMAGE050
(11)
Figure DEST_PATH_IMAGE051
Figure 135669DEST_PATH_IMAGE052
Because the viewing angle of the USBL receiver is limited, the constraint of considering the desired approach angle is
Figure DEST_PATH_IMAGE053
(12)
Figure 647553DEST_PATH_IMAGE054
Sorting (12) into the following linear matrix inequality
Figure 393793DEST_PATH_IMAGE055
(13)
Figure 217392DEST_PATH_IMAGE056
/>
(symbol)
Figure DEST_PATH_IMAGE057
Is Cronecker product; constructing Lyapunov functions
Figure 215435DEST_PATH_IMAGE058
(14)
Deriving (14) to obtain
Figure 440880DEST_PATH_IMAGE059
In the method, in the process of the invention,
Figure 358020DEST_PATH_IMAGE060
so long as it satisfies->
Figure DEST_PATH_IMAGE061
Then
Figure 544282DEST_PATH_IMAGE062
；
The stability constraints are organized into the following linear matrix inequalities
Figure DEST_PATH_IMAGE063
(15)
Figure 736229DEST_PATH_IMAGE064
Solving the optimal problem, defining the following cost function
Figure DEST_PATH_IMAGE065
(16)
Weighting matrix in cost function (16)
Figure 957126DEST_PATH_IMAGE066
,/>
Figure DEST_PATH_IMAGE067
,
Figure 310747DEST_PATH_IMAGE068
， />
Figure 718726DEST_PATH_IMAGE070
Are positive definite matrixes, wherein
Figure DEST_PATH_IMAGE071
，/>
Figure 714364DEST_PATH_IMAGE072
Bringing (11) into (16) to obtain
Figure DEST_PATH_IMAGE073
(17)
In the formula (17)
Figure 789767DEST_PATH_IMAGE074
Because of->
Figure DEST_PATH_IMAGE075
The MPC optimization problem taking into account constraints can be equivalently solved for the following quadratic forms, with constant values at each moment of adoption>
Figure 314289DEST_PATH_IMAGE076
(18)
The future can be obtained by solving (18) at each application cycle
Figure DEST_PATH_IMAGE077
Sequence value of step optimal expected approach angle
Figure 475143DEST_PATH_IMAGE078
Of course, as long as it will->
Figure 743314DEST_PATH_IMAGE078
First set of values ∈>
Figure 532278DEST_PATH_IMAGE079
The current expected approach angle is used, and the calculation is repeated when the next adoption period is entered.
Still further, the design of the dynamics controller includes: achieving a desired longitudinal velocity
Figure 100138DEST_PATH_IMAGE080
And approach angle
Figure 607343DEST_PATH_IMAGE081
Wherein the desired value of the longitudinal speed +.>
Figure 413625DEST_PATH_IMAGE082
m.s -1 Is to control the rotation speed of the propeller>
Figure 932462DEST_PATH_IMAGE083
Generating longitudinal forcesXRealize, approach angle->
Figure 798787DEST_PATH_IMAGE084
By controlling the horizontal rudder angle->
Figure 527708DEST_PATH_IMAGE085
And vertical rudder angle->
Figure 278627DEST_PATH_IMAGE086
Generating pitching momentMAnd a yaw momentNTo realize the method; the dynamics controller adopts the following PID controller
Figure 776604DEST_PATH_IMAGE087
(19)
The parameters of the PID controller are set askX p=20，kX i=5，kX d=5，kM p=2，kM i=3，kM d=0.1，kN p=2，kN i=3，kN d=0.1。
The invention has the advantages that: aiming at a common underactuated AUV lacking transverse and vertical driving forces, the invention adopts the USBL to position, acquires coordinates of four transponders on the horn mouth guiding type docking device in a carrier coordinate system, obtains the position and the posture of the AUV in a fixed coordinate system through coordinate transformation, considers the constraint of the USBL visual angle, optimizes the expected approach angle in the docking process by applying the MPC, realizes the docking control in a three-dimensional space, and effectively shortens the docking distance.
In addition to the objects, features and advantages described above, the present invention has other objects, features and advantages. The present invention will be described in further detail with reference to the drawings.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the invention.
FIG. 1 is a schematic illustration of an AUV three-dimensional docking;
FIG. 2 is a schematic block diagram of AUV three-dimensional docking control in accordance with the present invention;
FIG. 3 is a three-dimensional schematic view of an AUV three-dimensional docking control simulation docking of the present invention;
FIG. 4 is a schematic view of an AUV three-dimensional docking control simulated docking level of the present invention;
FIG. 5 is a schematic view of an AUV three-dimensional docking control simulated docking vertical plane of the present invention;
FIG. 6 is a graph of AUV three-dimensional docking control simulated propeller rotational speed in accordance with the present invention;
FIG. 7 is a graph of simulated horizontal rudder angle for AUV three-dimensional docking control of the present invention;
FIG. 8 is a graph of simulated vertical rudder angle for AUV three-dimensional docking control of the present invention;
FIG. 9 is a simulated longitudinal velocity profile of an AUV three-dimensional docking control of the present invention;
FIG. 10 is a graph of simulated docking longitudinal error for AUV three-dimensional docking control of the present invention;
FIG. 11 is a graph of simulated docking lateral error for AUV three-dimensional docking control of the present invention;
FIG. 12 is a graph of simulated docking vertical error for AUV three-dimensional docking control in accordance with the present invention;
FIG. 13 is a graph of simulated vertical plane approach angle and pitch angle (MPC) for AUV three-dimensional docking control in accordance with the present invention;
FIG. 14 is a graph of simulated vertical surface approach angle and pitch angle (LOS forward looking distance 3 m) for AUV three-dimensional docking control of the present invention;
FIG. 15 is a graph of simulated vertical surface approach angle and pitch angle (LOS forward looking distance 8 m) for AUV three-dimensional docking control of the present invention;
FIG. 16 is a graph of simulated horizontal plane approach angle and yaw angle (MPC) for AUV three-dimensional docking control in accordance with the present invention;
FIG. 17 is a graph of simulated horizontal plane approach angle and yaw angle (LOS forward looking distance 3 m) for AUV three-dimensional docking control of the present invention;
fig. 18 is a graph of the horizontal plane approach angle and yaw angle (LOS forward looking distance 8 m) for an AUV three-dimensional docking control simulation of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
Referring to fig. 1-2, a model predictive butt control method of under-actuated auv includes: designing a butt joint error model; the design of the docking controller includes the design of the desired approach angle and the design of the dynamics controller.
The navigational speed of the underactuated AUV is realized by controlling the longitudinal speed through a propeller at the tail, the latency is indirectly realized by controlling the pitch angle through a horizontal rudder, the heading is realized by controlling the bow cranking angle through a vertical rudder, the AUV has no driving force in the roll, the transverse direction and the vertical direction, and the three-dimensional butting principle is shown in figure 1. To facilitate modeling of the docking errors.
The docking error model design includes: two coordinate systems are arranged, one is a carrier coordinate system
Figure 813830DEST_PATH_IMAGE088
The origin of which is defined at the floating center of the AUVOThe other is a fixed coordinate system +.>
Figure 170993DEST_PATH_IMAGE089
The origin of which is defined in the center of the horn mouth of the guiding type docking deviceEA place; of the carrier coordinate systemxThe axis is directed forward,yThe axis points to the right,zThe axis pointing downwards, the fixed coordinate systemξThe axis points forward and coincides with the butt joint path, η A transponder 2 with its axis pointing to the right, ζ A transponder 4 with its axis directed downwards; the docking error model can be simplified to +.>
Figure 319078DEST_PATH_IMAGE090
(1)
In the formula (1), the components are as follows,
Figure 937141DEST_PATH_IMAGE091
for AUV->
Figure 145269DEST_PATH_IMAGE092
The position and attitude of the lens, i.e. the docking error,
Figure 724149DEST_PATH_IMAGE093
representing AUV at->
Figure 675924DEST_PATH_IMAGE092
The coordinates of (a),θIs the pitch angle of AUV,ψIs the bow rocking angle of the AUV;
Figure 414073DEST_PATH_IMAGE094
representing AUV at->
Figure 668468DEST_PATH_IMAGE095
Is provided, the speed of the roller,uis a longitudinal speed,vIs a transverse velocity,wIs vertical velocity,qIs the pitch angle speed,rIs the yaw rate; />
Figure 859278DEST_PATH_IMAGE096
Is->
Figure 349165DEST_PATH_IMAGE097
To->
Figure 941820DEST_PATH_IMAGE098
Is used for the rotation transformation matrix of the (c),
Figure 101537DEST_PATH_IMAGE099
first obtain by USBL receiver4 transponders on
Figure 45223DEST_PATH_IMAGE100
The coordinates of (2) and the butt joint error satisfy the relation (2),
Figure 338801DEST_PATH_IMAGE101
(2)
in the formula (2), the amino acid sequence of the compound,
Figure 661329DEST_PATH_IMAGE102
the coordinates of the 4 transponders acquired for the USBL receiver,lfor USBL receiver and floating coreODistance of->
Figure 382160DEST_PATH_IMAGE103
For 4 transponders +.>
Figure 281983DEST_PATH_IMAGE104
Coordinates of->
Figure DEST_PATH_IMAGE105
For 4 transponders +.>
Figure 81767DEST_PATH_IMAGE106
Coordinates of (a) and (b);
the radius of the bell mouth of the butt joint device is 1 meter, so
Figure 914593DEST_PATH_IMAGE107
The method comprises the steps of carrying out a first treatment on the surface of the By the relation +.>
Figure 806326DEST_PATH_IMAGE108
Is available in the form of
Figure 68811DEST_PATH_IMAGE109
(3)
Through the relation
Figure 704192DEST_PATH_IMAGE110
Is available in the form of
Figure 125946DEST_PATH_IMAGE111
(4)
Through the relation
Figure 188580DEST_PATH_IMAGE112
Is available in the form of
Figure 203940DEST_PATH_IMAGE113
(5)。
The design of the desired approach angle includes: designing a guidance law to generate expected values of a yaw angle and a pitch angle, namely a desired approach angle; the LOS guidance law is adopted as follows
Figure 377433DEST_PATH_IMAGE114
(6)
In (6)
Figure 653693DEST_PATH_IMAGE115
For the approach angle of the vertical plane->
Figure 762595DEST_PATH_IMAGE116
Is the approach angle of the horizontal plane, is->
Figure DEST_PATH_IMAGE117
And->
Figure 655464DEST_PATH_IMAGE118
Is the forward looking distance; the MPC design guidance law is adopted to optimize the expected approach angle, and the error equation of the pitch angle and the yaw angle on the expected approach angle can be approximated as the following differential equation->
Figure 508014DEST_PATH_IMAGE119
(7)
In (7)
Figure 638781DEST_PATH_IMAGE120
Is an adjustable time constant, wherein->
Figure DEST_PATH_IMAGE121
；
According to the butt-joint error model (1), the butt-joint error equation in the transverse direction and the vertical direction can be simplified into
Figure 308797DEST_PATH_IMAGE122
(8)
In (8)
Figure 33170DEST_PATH_IMAGE123
When->
Figure 814044DEST_PATH_IMAGE124
Can be simplified to +.>
Figure 64897DEST_PATH_IMAGE125
When (when)
Figure 515601DEST_PATH_IMAGE126
Can be simplified to +.>
Figure 851905DEST_PATH_IMAGE127
The method comprises the steps of carrying out a first treatment on the surface of the Combining the formula (7) and the formula (8),
the docking control can be equivalently a calm problem of the following errors
Figure 170891DEST_PATH_IMAGE128
(9)
Discretizing the equation (9) to obtain a prediction model of the docking error as
Figure 276250DEST_PATH_IMAGE129
(10)
Subscript in formula (10)kThe representative is the use of a time series,
Figure 897855DEST_PATH_IMAGE130
Figure DEST_PATH_IMAGE131
and->
Figure 987034DEST_PATH_IMAGE132
Respectively a state vector and an output vector, which are butt joint errors +.>
Figure 844131DEST_PATH_IMAGE133
For the input vector, i.e. the desired approach angle,Tis the sampling period; at the position ofkTime of day, available future according to the predictive model (10)k+1 to->
Figure 942013DEST_PATH_IMAGE134
The predicted value of the time butt-joint error is
Figure 124733DEST_PATH_IMAGE135
Of the formula (I)
Figure 170049DEST_PATH_IMAGE136
Representing the control step size and the prediction step size, respectively, wherein +.>
Figure 706204DEST_PATH_IMAGE137
The method comprises the steps of carrying out a first treatment on the surface of the Then the predicted output value is further available as
Figure 786155DEST_PATH_IMAGE138
The predicted output values may be organized into the following matrix form
Figure 874197DEST_PATH_IMAGE139
(11)
Figure 406809DEST_PATH_IMAGE140
/>
Figure DEST_PATH_IMAGE141
Because the viewing angle of the USBL receiver is limited, the constraint of considering the desired approach angle is
Figure 277813DEST_PATH_IMAGE142
(12)
Figure 212271DEST_PATH_IMAGE143
Sorting (12) into the following linear matrix inequality
Figure 346581DEST_PATH_IMAGE144
(13)
Figure 632068DEST_PATH_IMAGE145
(symbol)
Figure 634660DEST_PATH_IMAGE057
Is Cronecker product; constructing Lyapunov functions
Figure 298990DEST_PATH_IMAGE146
(14)
Deriving (14) to obtain
Figure 994414DEST_PATH_IMAGE147
In the method, in the process of the invention,
Figure 501618DEST_PATH_IMAGE148
so long as it meets
Figure DEST_PATH_IMAGE149
Then->
Figure 183267DEST_PATH_IMAGE150
；
The stability constraints are organized into the following linear matrix inequalities
Figure 357896DEST_PATH_IMAGE151
(15)
Figure 958642DEST_PATH_IMAGE152
Solving the optimal problem, defining the following cost function
Figure 828509DEST_PATH_IMAGE153
(16)
Weighting matrix in cost function (16)
Figure 172902DEST_PATH_IMAGE154
,/>
Figure 202038DEST_PATH_IMAGE155
,
Figure 114631DEST_PATH_IMAGE156
， />
Figure 330848DEST_PATH_IMAGE157
Are positive definite matrixes, wherein
Figure 744512DEST_PATH_IMAGE158
，/>
Figure 240871DEST_PATH_IMAGE159
Bringing (11) into (16) to obtain
Figure 448999DEST_PATH_IMAGE160
(17)
In the formula (17)
Figure 418092DEST_PATH_IMAGE161
Because of->
Figure 979654DEST_PATH_IMAGE162
The MPC optimization problem taking into account constraints can be equivalently solved for the following quadratic forms, with constant values at each moment of adoption
Figure 717803DEST_PATH_IMAGE163
(18)
The future can be obtained by solving (18) at each application cycle
Figure 96832DEST_PATH_IMAGE164
Sequence value of step optimal expected approach angle
Figure 287642DEST_PATH_IMAGE165
Of course, as long as it will->
Figure 918474DEST_PATH_IMAGE166
First set of values ∈>
Figure 245550DEST_PATH_IMAGE167
As the current expected approach angle, when entering the nextThe above calculation is repeated again with each cycle of adoption.
The design of the dynamics controller comprises: achieving a desired longitudinal velocity
Figure 795480DEST_PATH_IMAGE168
And approach angle->
Figure 348953DEST_PATH_IMAGE169
Wherein the desired value of the longitudinal speed +.>
Figure 642531DEST_PATH_IMAGE170
m.s -1 Is to control the rotation speed of the propeller>
Figure 355272DEST_PATH_IMAGE171
Generating longitudinal forcesXRealize, approach angle->
Figure 685890DEST_PATH_IMAGE172
By controlling the horizontal rudder angle->
Figure 851292DEST_PATH_IMAGE173
And vertical rudder angle->
Figure 682982DEST_PATH_IMAGE174
Generating pitching momentMAnd a yaw momentNTo realize the method; the dynamics controller adopts the following PID controller>
Figure 250230DEST_PATH_IMAGE175
(19)
The parameters of the PID controller are set askX p=20，kX i=5，kX d=5，kM p=2，kM i=3，kM d=0.1，kN p=2，kN i=3，kN d=0.1。
The schematic block diagram of the three-dimensional docking control of the underactuated AUV is shown in FIG. 2.
Simulation experiment and analysis
To verify the performance of the controller, three dimensions were then performedThe butt joint control simulation experiment, and fig. 3 shows simulation results. The dynamics simulation model of the underactuated AUV REMUS-100 is adopted in the simulation. Initial pose of AUV docking is
Figure 17328DEST_PATH_IMAGE176
. In the horizontal direction and the vertical direction, the allowable error of the butt joint is +/-0.25 m, and the expected approach angle of the butt joint is generated by adopting two guidance rules of LOS and MPC respectively, wherein two different forward viewing distances of 3m and 8m are adopted in the LOS guidance rules respectively.
Fig. 3 is a schematic view of the three-dimensional butt joint, fig. 4 is a schematic view of the horizontal butt joint, and fig. 5 is a schematic view of the vertical butt joint. It can be seen that the initial error converges most rapidly when the forward looking distance of the LOS guidance law is 3m, but the vertical error is overshot, approximately at 40 m in front of the docking device, and the docking error in both the horizontal and vertical directions is within the allowable range. When the forward viewing distance of the LOS guidance law increases to 8m, the initial error converges most slowly, and the horizontal and vertical butt errors converge to within the allowable range at about 30 m in front of the butt joint device. When the MPC guidance law is adopted, the docking error in the horizontal and vertical directions is within the allowable range at about 50 a m a in front of the docking device, so that the docking distance required when the MPC guidance law is adopted is the shortest.
Fig. 6 to 8 are graphs of the rotation speed, the horizontal rudder angle and the vertical rudder angle of the propeller, respectively, and it is clear that the control signals are relatively stable.
It can be seen from fig. 9 that the longitudinal speed can be well stabilized at the desired value.
Fig. 10-12 are graphs of butted position errors, and it can be seen that all position errors are within the allowable range, but the convergence time is the shortest when MPC guidance is used.
Fig. 13-18 are graphs of expected approach angles and attitude errors for interfacing, where the expected approach angles are maximum and out of constraint when the forward looking distance of the LOS guidance law is 3m, but are within constraint when MPC guidance laws are employed.
Conclusion(s)
Aiming at a common underactuated AUV lacking transverse and vertical driving forces, the invention adopts the USBL to position, acquires coordinates of four transponders on the horn mouth guiding type docking device in a carrier coordinate system, obtains the position and the posture of the AUV in a fixed coordinate system through coordinate transformation, considers the constraint of the USBL visual angle, optimizes the expected approach angle in the docking process by applying the MPC, realizes the docking control in a three-dimensional space, and effectively shortens the docking distance.
The foregoing description of the preferred embodiments of the invention is not intended to limit the invention to the precise form disclosed, and any such modifications, equivalents, and alternatives falling within the spirit and scope of the invention are intended to be included within the scope of the invention.

Claims (1)

1. A model predictive butt-joint control method of under-actuated auv, comprising:
designing a butt joint error model;
the design of the docking controller, including the design of the desired approach angle and the design of the dynamics controller;
the docking error model design includes:
two coordinate systems are arranged, one is a carrier coordinate system { B }: O-xyz, the origin of which is defined at the floating center O of the AUV, and the other is a fixed coordinate system { I }: E- ζηζ, the origin of which is defined at the center E of the horn mouth of the guiding type docking device; the x axis of the carrier coordinate system points to the front, the y axis points to the right, the z axis points to the lower, the zeta axis of the fixed coordinate system points to the front and coincides with the butt joint path, the eta axis points to the transponder 2 on the right side, and the zeta axis points to the transponder 4 on the lower side;
the docking error model can be simplified as
Figure FDA0004184008040000011
In the formula (1), eta= [ zeta eta zeta theta phi ]] T The position and the posture of the AUV in { I } are indicated by docking errors, (ζ, eta, ζ) representing coordinates of the AUV in { I }, a pitch angle of the AUV being theta, and a yaw angle of the AUV being phi;
v＝[u v w q r] T representing the speed of the AUV in { B }, u being the longitudinal speed, v being the transverse speed, w being the vertical speed, q being the pitch angle speed, r being the yaw angle speed; j (eta) is a rotation transformation matrix from { B } to { I },
Figure FDA0004184008040000021
Figure FDA0004184008040000022
first, the coordinates of the 4 transponders in { B } are obtained by the USBL receiver, they satisfy relation (2) with the docking error,
Figure FDA0004184008040000023
/>
in the formula (2), the amino acid sequence of the compound,
Figure FDA0004184008040000024
the coordinates of the 4 transponders obtained for the USBL receiver, l is the distance of the USBL receiver from the centroid O, [ x ] i y i z i ] T (i=1, 2,3, 4) is the coordinates of 4 transponders in { B }, ζ i η i ζ i ] T (i=1, 2,3, 4) is the coordinates of 4 transponders in { I };
the radius of the bell mouth of the butt joint device is 1 meter, so eta 1 ＝-1,η 2 ＝1,ζ 3 ＝-1,ζ 4 =1; through the relation
Figure FDA0004184008040000031
Is available in the form of
Figure FDA0004184008040000032
Through the relation
Figure FDA0004184008040000033
Is available in the form of
Figure FDA0004184008040000034
Through the relation
Figure FDA0004184008040000035
Is available in the form of
Figure FDA0004184008040000036
The design of the desired approach angle includes:
designing a guidance law to generate expected values of a yaw angle and a pitch angle, namely a desired approach angle; the LOS guidance law is adopted as follows
Figure FDA0004184008040000041
θ in (6) d Is a vertical approach angle, ψ d For the approach angle of the horizontal plane, delta θ > 0 and delta ψ > 0 is forward looking distance;
the MPC design guidance law is adopted to optimize the expected approach angle, and the error equation of the pitch angle and the yaw angle on the expected approach angle can be approximated as the following differential equation
Figure FDA0004184008040000042
T in (7) 1 ,T 2 Is an adjustable time constant, wherein T 1 ＝T 2 ＝0.2；
According to the butt-joint error model (1), the butt-joint error equation in the transverse direction and the vertical direction can be simplified into
Figure FDA0004184008040000043
In (8)
Figure FDA0004184008040000044
When->
Figure FDA0004184008040000045
Can be simplified to k η When =cos θ
Figure FDA0004184008040000046
Can be simplified to k ζ =1; combining the formula (7) and the formula (8),
the docking control can be equivalently a calm problem of the following errors
Figure FDA0004184008040000051
Discretizing the equation (9) to obtain a prediction model of the docking error as
Figure FDA0004184008040000052
The subscript k in formula (10) represents the time series employed,
Figure FDA0004184008040000053
x＝[η ζ θ ψ] T and y= [ eta ζ theta ψ ]] T Respectively a state vector and an output vector, which are all butt joint errors, and u= [ theta ] d ψ d ] T For the input vector, i.e. the desired approach angle, T is the sampling period; at time k, future k+1 to k+N are available according to the predictive model (10) p The predicted value of the time butt-joint error is
Figure FDA0004184008040000061
N in the formula c ,N p Respectively represent a control step size and a prediction step size, wherein N is c ＝3,N p =6; then the predicted output value is further available as
Figure FDA0004184008040000062
The predicted output values may be organized into the following matrix form
Y k+1,k ＝Ψx k,k +ΘU k,k (11)
Figure FDA0004184008040000071
Figure FDA0004184008040000072
Because the viewing angle of the USBL receiver is limited, the constraint of considering the desired approach angle is
u min ≤u k+t,k ≤u max ,t＝0,1,…N c -1 (12)
Figure FDA0004184008040000073
Sorting (12) into the following linear matrix inequality
Figure FDA0004184008040000074
Figure FDA0004184008040000075
/>
Figure FDA0004184008040000076
(symbol)
Figure FDA0004184008040000077
Is Cronecker product; constructing Lyapunov functions
Figure FDA0004184008040000078
Deriving (14) to obtain
Figure FDA0004184008040000081
Wherein k is ψ ＝sgn(η)ψ d ,k θ ＝-sgn(ζ)θ d So long as k is satisfied ψ ≤0,k θ Less than or equal to 0, then
Figure FDA0004184008040000082
The stability constraints are organized into the following linear matrix inequalities
Figure FDA0004184008040000083
M 2 ＝-sgn(ζ)C θ ,
Figure FDA0004184008040000084
M 3 ＝sgn(η)C ψ ,
Figure FDA0004184008040000085
Solving the optimal problem, defining the following cost function
Figure FDA0004184008040000086
Weighting matrix in cost function (16)
Figure FDA0004184008040000087
Q＝diag(Q 11 ,Q 22 ,Q 33 ,Q 44 )，R＝diag(R 11 ,R 22 ) Are positive definite matrixes, wherein Q 11 ＝Q 22 ＝Q 33 ＝Q 44 ＝1，R 11 ＝R 22 ＝1
Bringing (11) into (16) to obtain
Figure FDA0004184008040000088
In the formula (17)
Figure FDA0004184008040000089
Because of->
Figure FDA0004184008040000091
The MPC optimization problem taking into account constraints can be equivalently solved for the following quadratic forms, with constant values at each moment of adoption
Figure FDA0004184008040000092
/>
Figure FDA0004184008040000093
The future N can be obtained by solving the equation (18) at each cycle of use c Sequence value U of optimal expected approach angle k,k Of course, only U k,k First group of values u k,k As the current expected approach angle, repeating the calculation when entering the next adoption period;
the design of the dynamics controller comprises:
achieving the desired longitudinal velocity u d And approach angle theta d ,ψ d Wherein the desired value u of the longitudinal speed d ＝1m.s -1 Is to control the rotation speed n of the propeller p To generate a longitudinal force X, approaching an angle theta d ,ψ d By controlling the horizontal rudder angle delta s And vertical rudder angle delta r Generating a pitching moment M and a yawing moment N; the dynamics controller adopts the following PID controller
Figure FDA0004184008040000094
The parameters of the PID controller are set as
Figure FDA0004184008040000101
Figure FDA0004184008040000102
/>
CN202111361802.1A 2021-11-17 2021-11-17 Model prediction butt joint control method of under-actuated auv Active CN114019805B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111361802.1A CN114019805B (en) 2021-11-17 2021-11-17 Model prediction butt joint control method of under-actuated auv

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111361802.1A CN114019805B (en) 2021-11-17 2021-11-17 Model prediction butt joint control method of under-actuated auv

Publications (2)

Publication Number Publication Date
CN114019805A CN114019805A (en) 2022-02-08
CN114019805B true CN114019805B (en) 2023-05-23

Family

ID=80064968

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111361802.1A Active CN114019805B (en) 2021-11-17 2021-11-17 Model prediction butt joint control method of under-actuated auv

Country Status (1)

Country Link
CN (1) CN114019805B (en)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
CN106444794A (en) * 2016-09-20 2017-02-22 哈尔滨工程大学 Sliding-mode control method for parameter-free driving-insufficient UUV (Unmanned Underwater Vehicle) vertical plane route tracking
CN108303079A (en) * 2017-12-21 2018-07-20 中国船舶重工集团公司第七0七研究所 A kind of data smoothing method of underwater USBL applied in reverse
FR3080194A1 (en) * 2018-04-12 2019-10-18 Cgg Services Sas METHOD FOR GUIDING AN AUTONOMOUS SUBMARINE VEHICLE AND ASSOCIATED SYSTEM FOR ACQUIRING UNDERWATER ANALYSIS DATA

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105527967A (en) * 2015-12-16 2016-04-27 西北工业大学 AUV inversion docking control method
CN110333739B (en) * 2019-08-21 2020-07-31 哈尔滨工程大学 AUV (autonomous Underwater vehicle) behavior planning and action control method based on reinforcement learning
CN112527018B (en) * 2020-12-26 2023-02-07 九江职业技术学院 Three-dimensional stabilization control method for under-actuated autonomous underwater vehicle

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106444794A (en) * 2016-09-20 2017-02-22 哈尔滨工程大学 Sliding-mode control method for parameter-free driving-insufficient UUV (Unmanned Underwater Vehicle) vertical plane route tracking
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
CN108303079A (en) * 2017-12-21 2018-07-20 中国船舶重工集团公司第七0七研究所 A kind of data smoothing method of underwater USBL applied in reverse
FR3080194A1 (en) * 2018-04-12 2019-10-18 Cgg Services Sas METHOD FOR GUIDING AN AUTONOMOUS SUBMARINE VEHICLE AND ASSOCIATED SYSTEM FOR ACQUIRING UNDERWATER ANALYSIS DATA

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于模型预测控制的自主水下航行器模糊回坞导引算法;齐贝贝;齐仲里;王林林;;国外电子测量技术(第04期);全文 *

Also Published As

Publication number Publication date
CN114019805A (en) 2022-02-08

Similar Documents

Publication Publication Date Title
CN106444806B (en) The drive lacking AUV three-dimensional track tracking and controlling method adjusted based on biological speed
CN109032178B (en) Full-drive AUV recovery control system and autonomous recovery method
CN107168312B (en) Space trajectory tracking control method for compensating UUV kinematic and dynamic interference
Xiang et al. Synchronized path following control of multiple homogenous underactuated AUVs
Li et al. Robust adaptive motion control for underwater remotely operated vehicles with velocity constraints
Amundsen et al. Autonomous ROV inspections of aquaculture net pens using DVL
CN111208845B (en) Path planning method and device for movable butt joint recovery of underwater robot
CN112650231B (en) Under-actuated ship formation control system for realizing collision and obstacle avoidance
Wang et al. A simultaneous planning and control method integrating APF and MPC to solve autonomous navigation for USVs in unknown environments
CN115903800A (en) Strict safety control method for multi-unmanned ship formation based on leader cooperation
He et al. Robust orientation-sensitive trajectory tracking of underactuated autonomous underwater vehicles
CN114967714A (en) Anti-interference motion control method and system for autonomous underwater robot
Du et al. Research on UUV recovery active disturbance rejection control based on LMNN compensation
Rober et al. Three-dimensional path-following control for an underwater vehicle
Silpa-Anan et al. Preliminary experiments in visual servo control for autonomous underwater vehicle
CN114019805B (en) Model prediction butt joint control method of under-actuated auv
David et al. Unmanned underwater vehicle navigation and collision avoidance using fuzzy logic
Sgorbissa et al. 3D path following with no bounds on the path curvature through surface intersection
Chavez‐Galaviz et al. ASV station keeping under wind disturbances using neural network simulation error minimization model predictive control
Hong et al. Vision-Based Underwater Inspection with Portable Autonomous Underwater Vehicle: Development, Control, and Evaluation
CN115657683A (en) Unmanned and cableless submersible real-time obstacle avoidance method capable of being used for inspection task
Bian et al. Visual docking control of an AUV with a moving station using a cross-tracking approach with unscented Kalman filtering
Rout Control of autonomous underwater vehicles
CN113110458A (en) Unmanned ship virtual target tracking control system
Heshmati-Alamdari Cooperative and Interaction Control for Underwater Robotic 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