CN112148022A - System and method for tracking and controlling recovery three-dimensional path of full-drive autonomous underwater robot - Google Patents
System and method for tracking and controlling recovery three-dimensional path of full-drive autonomous underwater robot Download PDFInfo
- Publication number
- CN112148022A CN112148022A CN202010988593.2A CN202010988593A CN112148022A CN 112148022 A CN112148022 A CN 112148022A CN 202010988593 A CN202010988593 A CN 202010988593A CN 112148022 A CN112148022 A CN 112148022A
- Authority
- CN
- China
- Prior art keywords
- auv
- control
- coordinate system
- axis
- model
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 238000011084 recovery Methods 0.000 title claims description 34
- 238000012545 processing Methods 0.000 claims abstract description 10
- 238000005096 rolling process Methods 0.000 claims abstract description 4
- 230000005484 gravity Effects 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 16
- 239000011159 matrix material Substances 0.000 claims description 15
- 238000003032 molecular docking Methods 0.000 claims description 15
- 238000013461 design Methods 0.000 claims description 9
- 239000003550 marker Substances 0.000 claims description 9
- 210000001503 joint Anatomy 0.000 claims description 8
- 230000000007 visual effect Effects 0.000 claims description 6
- 238000012544 monitoring process Methods 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 3
- 238000004891 communication Methods 0.000 claims description 3
- 239000002131 composite material Substances 0.000 claims description 3
- 238000007667 floating Methods 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 3
- 238000005192 partition Methods 0.000 claims description 3
- 230000008878 coupling Effects 0.000 claims description 2
- 238000010168 coupling process Methods 0.000 claims description 2
- 238000005859 coupling reaction Methods 0.000 claims description 2
- 238000004064 recycling Methods 0.000 abstract description 4
- 230000001276 controlling effect Effects 0.000 abstract 1
- 238000005312 nonlinear dynamic Methods 0.000 abstract 1
- 230000001105 regulatory effect Effects 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 239000000575 pesticide Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005507 spraying Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000000844 transformation Methods 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 discloses a system and a method for tracking and controlling a three-dimensional path for recycling of a fully-driven autonomous underwater robot, provides a sectional type three-dimensional path for recycling and provides a model prediction integral s-plane control algorithm for a linear homing stage and a linear tracking stage of the robot for recycling. The model prediction integral S-surface control algorithm provides advanced pose information through the prediction of the nonlinear dynamic system. And detecting the error between the actual output of the carrier and the output of the prediction model at the moment of feedback correction, correcting the prediction output in real time, and regulating the parameters of the integral S-plane controller in a rolling manner to form a parameter regulation loop. The adopted control method combines model prediction control and S-surface control, so that the method has a mechanism capable of processing multiple input and multiple output and is suitable for a nonlinear model, a delta u term of an S-surface controller is designed into an integral term, steady-state errors during tracking are reduced, and the anti-interference capability of the autonomous underwater robot on ocean currents is enhanced.
Description
Technical Field
The invention relates to an autonomous underwater robot, in particular to a full-drive autonomous underwater robot control system and a recovery three-dimensional path tracking control method, and belongs to the technical field of robot control.
Background
AUV (Autonomous Underwater Vehicle) is widely applied to military and civil fields, is one of important tools in ocean development, and plays an important role in tasks such as Underwater observation, positioning and exploration due to the advantages of good maneuverability, large cruising range and the like. The three-dimensional path tracking is an important function of the AUV, and has important significance for accurately completing specified mission, smoothly recovering and laying tasks and the like of the AUV. The three-dimensional path tracking control is a key technology for autonomous operation of the AUV as a main index for measuring the AUV control performance.
The three-dimensional path tracking control is mainly to decouple the three-dimensional path tracking control into horizontal plane (transverse) control and vertical plane (vertical) control. Due to the uncertainty and the non-linear characteristics of the AUV model, and the interference of ocean currents and the like, great difficulty is brought to the design of the controller. At present, researchers mostly focus on the above-mentioned problems and develop research aiming at three-dimensional path tracking of AUV. For example, a neural network is adopted for nonlinear reconstruction, and an adaptive method is introduced to enable the controller to have better robustness and interference resistance, but the AUV is not considered to be a discrete system; a horizontal plane tracking controller is designed by adopting a backstepping method, and an unknown state and an uncertain item are estimated by using a differentiator, but the method has large calculation amount and cannot process an actual AUV system in real time; a constrained path tracking control law of the AUV is designed by adopting a nonlinear model predictive control method, so that the problem of constrained path tracking control is solved, but the problem of large calculation amount still exists. The patent document with the application number of '201010559361.1' discloses a small underwater robot combined navigation positioning method, and the consistency of the adopted model predictive control algorithm is poor, so that AUV is easy to diverge during autonomous navigation; patent document with application number "201210490435. X" discloses a track tracking sliding mode control system and a control method for a pesticide spraying mobile robot, and the buffeting problem caused by sliding mode control is not solved, and the tracking precision is poor. Therefore, the design of the controller is simplified, the robustness and the interference resistance of the controller are better, and the controller is a target of the current AUV three-dimensional path tracking controller design.
Disclosure of Invention
The invention aims to provide a recovery three-dimensional path tracking control system and a recovery three-dimensional path tracking control method for a full-drive autonomous underwater robot. A model prediction S-surface control method is adopted, the self nonlinear characteristic of the AUV is matched, and an integral term is introduced, so that the anti-interference performance of the controller is enhanced, the design problem of simplifying the controller is solved well, and the accuracy and the stability of AUV recovery are effectively improved.
The purpose of the invention is realized by the following technical scheme:
a full-drive autonomous underwater robot recovery three-dimensional path tracking control system comprises an AUV body control system 1 and a docking station control system 2, wherein the AUV body control system 1 is positioned in the middle section of the whole AUV and comprises a main control panel 3, a visual processing unit 4, a battery pack 5, an ultra-short baseline receiver 6, a Doppler log 7 and a propulsion module 8, the visual processing unit 4 is installed at the head section of the AUV, the battery pack 5 is positioned at the bottom of the AUV body control system 1 and is separated from the main control panel 3 through a partition plate, the ultra-short baseline receiver 6 and the Doppler log 7 are installed at the bottom of the AUV and are exposed outside a cabin shell, the propulsion module 8 is positioned at the tail section of the AUV, the design of a propeller rear rudder is adopted, the visual processing unit 4 and the ultra-short baseline receiver 6 are adopted, the Doppler log 7 and the propulsion module 8 are connected with the main control panel 3 through a power line and a signal line, the battery, AUV body control system 1 communicates through radio signal between the host computer 9 with the control, during the butt joint, use ultrashort baseline 10 and ultrashort baseline receiver 6 to carry out radio communication between AUV body control system 1 and dock control system 2, dock control system 2 installs on base 11, horn mouth 12 passes through the support to be fixed on base 11, the dock includes ultrashort baseline 10, horn mouth 12 and marker light 13, ultrashort baseline 10 is placed perpendicularly, install directly over horn mouth 12, marker light 13 is installed in horn mouth 12 below, ultrashort baseline 10 and marker light 13 pass through the wire and link to each other with dock control system 2, connect through photoelectric composite cable 15 between dock control system 2 and the monitoring station 14.
A control method of a recovery three-dimensional path tracking control system of a full-drive autonomous underwater robot comprises the following two processes:
a straight line homing process 16, which is a tracking process that the recovery device 17 starts to locate the AUV by using an ultra-short baseline and enters the butt joint central axis 18; the position of the AUV and the position of the docking device are adjusted by utilizing the relative position and posture information provided by the ultra-short baseline, so that the AUV is navigated to the position near the linear tracking point 19 under the condition of low consumption, and an improved S-plane controller, namely an improved Sigmoid plane controller, is started to reach the position near the linear tracking point, so that the posture of the AUV is consistent with the central axis 18, and the real-time docking is further facilitated.
And a straight line tracking process 20, which is a stage that the gravity center of the AUV enters the central axis 18 until the distance from the interface is 3-5 meters, and in the stage, the AUV is ensured to navigate along the central axis 18, and the heading angle points to the final calibration point 21, and then the heading is maintained and the straight line tracking is completed.
The aim of the invention can be further realized by adopting the following technical measures:
the control method of the three-dimensional path tracking control system for recovering the fully-driven autonomous underwater robot comprises the following steps of:
step 1: establishing a coordinate system and establishing an AUV mathematical model;
step 2: the AUV6 degree of freedom model is decomposed into a vertical plane control model and a horizontal plane control model while ignoring the coupling between them.
And step 3: discretizing data of the vertical plane control model and the horizontal plane control model, applying a model prediction control algorithm, correcting the system in real time according to the deviation of the AUV path tracking target quantity and the predicted value, and outputting optimal parameters of S-plane control.
And 4, step 4: and outputting the thrust of the propeller by adopting an improved S-surface control algorithm, performing motion control on the AUV, and completing AUV recovery three-dimensional path tracking.
The control method of the three-dimensional path tracking control system for recovering the fully-driven autonomous underwater robot comprises the following steps of:
step 1: first, two coordinate systems are established: the motion system comprises a geodetic coordinate system and a motion coordinate system, and the following equation is obtained by taking a floating center, namely the centroid of the underwater partial volume of the AUV, as the origin of the motion coordinate system by the kinematic equation of the AUV:
in the formula: m is the mass of the AUV body; x is the number ofG，yG，zGRespectively are the coordinates of the motion coordinate system of the gravity center of the AUV; i isx，Iy，IzRespectively the rotational inertia of the AUV in the coordinate axes of the 3 motion coordinate systems; u, v and w are components of the AUV speed in 3 coordinate axes of the motion coordinate system; and p, q and r are components of the AUV angular velocity in 3 coordinate axes of the motion coordinate system.
Step 2: based on the above equation, a simplified motion equation of the AUV vertical plane can be obtained, and the z-axis float-submergence equation is as follows:
the y-axis pitch equation is:
in the above formula: fiIs white gaussian noise; zw|w|，Zq|q|Vertical power coefficients, Z, of velocity and angular velocity, respectively, in a motion coordinate systemuq，ZuwThe vertical dynamic coefficient components of the speed and the angular velocity on the motion coordinate system, Mw|w|，Mq|q|The pitch kinetic coefficients of the speed and the angular velocity on the motion coordinate system respectively,
are respectively a multivariate matrix function
First partial derivative of middle w and q components in the pitch plane, Muq，MuwThe pitch dynamic coefficient components of the velocity and the angular velocity on the motion coordinate system respectively,
are respectively a multivariate matrix function
The first-order partial derivatives of the middle w and q components on the vertical plane; zgStress in the z-axis direction; mgIs gravity; zprop，MpropThrust in the z-axis direction and thrust moment in the y-axis direction are respectively; x is the number ofg，zgAre barycentric coordinates.
Then, substituting the gravity of the AUV and the rotational inertia of the AUV in the y axis into the vertical plane control model to obtain a z-axis buoyancy equation:
the y-axis pitch motion equation is:
if it is assumed that the navigation depth of the AUV is not changed, and only the heading and the track are changed, the gravity center of the AUV is considered to be kept on the horizontal plane, and the coordinate transformation relationship of the AUV in the inertial coordinate system in the horizontal plane can be expressed as:
the earth coordinate system takes a horizontal plane point E as an original point, the zeta axis points to the geographical north direction, the eta axis points to the geographical east direction, and the zeta axis points to the earth center respectively;
first, the kinematic equation of the horizontal plane AUV is obtained, i.e.
The x-axis forward and backward kinematic equation is:
the y-axis translational kinematic equation is as follows:
the z-axis bow-turning kinematic equation is
In the formula, Xu|u|For the axial power coefficient of the speed on the moving coordinate system,
as a function of a multivariate matrix
First partial derivative, X, of the median component u in the axial directionvr，XrrAxial dynamic coefficient components, Y, of velocity and angular velocity, respectively, in a motion coordinate systemv|v|For the lateral power coefficient of the speed on the motion coordinate system,
as a function of a multivariate matrix
First partial derivative of the median component v in the lateral direction, Yvr，YuvThe lateral dynamic coefficient components of velocity and angular velocity, N, respectively, in a motion coordinate systemv|v|，Nr|r|Velocity and angular velocity, respectivelyThe dynamic coefficient of the rotating bow on the moving coordinate system,
as a function of a multivariate matrix
First partial derivative of the mid-component r in heading, Nur，NuvThe components of the dynamic coefficient of the rotating bow, X, of the speed and the angular velocity on the motion coordinate systemprop，Yprop，NpropAxial, lateral and vertical thrust moments, respectively.
Then, substituting the AUV gravity and the rotational inertia of the AUV in the z axis into the horizontal plane control model to obtain an x axial forward and backward motion equation:
∑X＝6sinθ+Xprop+Fi+-10.050u|u|-146.848wq+-12.816q2+146.848vr-12.816r2(10)
the y-axis translational motion equation is as follows:
the z-axis bow-turning motion equation is as follows:
the control method of the recovery three-dimensional path tracking control system of the fully-driven autonomous underwater robot comprises the following steps of:
step 1: the performance index of the applied model predictive control algorithm is a performance index which comprehensively reflects the rolling time domain optimization.
In the formula: t is the prediction period, mu1、μ3Respectively represent output middle end constraints andcontrolling the weight, mu, of the input in the performance index J2Representing the weight of the tracking error in the performance indicator.
For a system in a certain time domain tau epsilon [0, T]The output of the prediction within the block is,
to be within a certain time domain tau epsilon [0, T]The desired output of (a) is the reference output.
Step 2: and outputting the control quantity by adopting an improved S-surface control algorithm, performing motion control on the AUV, and completing AUV recovery three-dimensional path tracking.
The control model of the S-surface controller is as follows:
wherein k is1And k2For control coefficients, equivalent to the PD coefficient in a PID controller, Δ u is the adjustment term, e and
e is depth and heading angle error information for controlling the input information,
the error change rate of the depth and the heading angle, u is control output, and the thrust and the torque of the corresponding propeller are in AUV.
Combining the characteristics of PID control, designing the delta u term of the S-surface controller into an integral term, wherein the control model is as follows:
namely when
Or
When the S-plane control is integrated, when
Or e (t) is 0, the S-plane is not integrated.
Compared with the prior art, the invention has the beneficial effects that:
1. the control system of the full-drive autonomous underwater robot for recovery adopts a modular, distributed and star-shaped topological structure design. The system can provide a uniform, efficient and stable information interaction environment for each module running on the intelligent underwater robot.
2. The invention designs a control algorithm aiming at the requirements of a linear homing stage and a linear tracking stage in the robot recovery process, and improves the success rate of the integral recovery.
3. The model predictive control algorithm adopted by the invention is improved on the basis of the existing algorithm, an S-plane control algorithm is fused, the error between the actual output of the carrier and the output of the predictive model is detected at any time, the predicted output is corrected in real time, and the parameters of the S-plane controller are adjusted to form a parameter adjusting loop. The method can not only process a complex nonlinear system such as AUV, but also carry out multi-input and multi-output of parameters.
4. According to the improved model prediction S-surface control algorithm, a delta u term of an S-surface controller is designed into an integral term, and the integral term is introduced to reduce steady-state errors during tracking and enhance the anti-interference capability of the AUV on ocean currents.
Drawings
FIG. 1 is a block diagram of an AUV recovery docking control system;
FIG. 2 is an AUV recovery docking flow diagram;
FIG. 3 is a schematic diagram of an AUV motion coordinate system and a geodetic coordinate system;
FIG. 4 is a block diagram of the structure of the S-plane control algorithm for three-dimensional path tracking model prediction in the AUV recovery process;
FIG. 5 is a flowchart of a three-dimensional path tracking model prediction S-plane control algorithm in the AUV recovery process.
Detailed description of the preferred embodiments
The invention is further described with reference to the following figures and specific examples.
As shown in fig. 1, a block diagram of a three-dimensional path tracking control system for recycling of a fully-driven autonomous underwater robot comprises an AUV body control system 1 and a docking station control system 2, wherein the AUV body control system 1 is located in the middle section of the whole AUV and comprises a main control board 3, a vision processing unit 4, a battery pack 5, an ultra-short baseline receiver 6, a doppler log 7 and a propulsion module 8, the vision processing unit 4 is installed at the front section of the AUV, the battery pack 5 is located at the bottom of the AUV body control system 1 and is separated from the main control board 3 through a partition plate, the ultra-short baseline receiver 6 and the doppler log 7 are installed at the bottom of the AUV and are exposed outside a cabin shell, the propulsion module 8 is located at the front section of the AUV and adopts a paddle rear rudder design, the vision processing unit 4 and the ultra-short baseline receiver 6 are connected with the main control board 3 through a power line and a signal line, the battery pack 5 supplies power to, AUV body control system 1 communicates through radio signal between the host computer 9 with the control, during the butt joint, use ultrashort baseline 10 and ultrashort baseline receiver 6 to carry out radio communication between AUV body control system 1 and dock control system 2, dock control system 2 installs on base 11, horn mouth 12 passes through the support to be fixed on base 11, the dock includes ultrashort baseline 10, horn mouth 12 and marker light 13, ultrashort baseline 10 is placed perpendicularly, install directly over horn mouth 12, marker light 13 is installed in horn mouth 12 below, ultrashort baseline 10 and marker light 13 pass through the wire and link to each other with dock control system 2, connect through photoelectric composite cable 15 between dock control system 2 and the monitoring station 14.
As shown in fig. 2, the recovery and docking flow chart of the recovery three-dimensional path tracking control system of the fully-driven autonomous underwater robot includes:
a straight line homing process 16, which is a tracking process that the recovery device 17 starts to locate the AUV by using an ultra-short baseline and enters the butt joint central axis 18; the position of the AUV and the position of the butt joint device are adjusted by utilizing the relative position and posture information provided by the ultra-short baseline, so that the AUV is navigated to the position near the linear tracking point 19 under the condition of low consumption, and an improved S-plane controller, namely an improved Sigmoid plane controller, is started near the linear tracking point, so that the posture of the AUV is consistent with the central axis 18, and the further real-time butt joint is facilitated;
and a straight line tracking process 20, which is a stage that the gravity center of the AUV enters the central axis 18 until the distance from the interface is 3-5 meters, and in the stage, the AUV is ensured to navigate along the central axis 18, and the heading angle points to the final calibration point 21, and then the heading is maintained and the straight line tracking is completed.
As shown in fig. 3, the fully-driven autonomous underwater robot recovers two coordinate systems of the three-dimensional path tracking control system: a geodetic coordinate system (E-xi eta zeta static coordinate system) and a kinematic coordinate system (O-xyz kinematic coordinate system).
FIG. 4 is a structural block diagram of a model prediction integral S-plane control algorithm of a recovery three-dimensional path tracking control system of a fully-driven autonomous underwater robot; fig. 5 is a flowchart of a model prediction integral S-plane control algorithm of a recovery three-dimensional path tracking control system of a fully-driven autonomous underwater robot, and the algorithm comprises the following steps.
Step 1: the kinematic equation of the AUV takes the floating center as the origin of a moving coordinate system, and obtains the following equation through long-term theoretical analysis and engineering practice:
in the formula: m is the mass of the AUV body; x is the number ofG，yG，zGCoordinates of the center of gravity of the AUV respectively; i isx，Iy，IzThe rotational inertia of the AUV on 3 coordinate axes is respectively; u, v and w are components of AUV speed in 3 coordinate axes of a carrier coordinate system; and p, q and r are components of the AUV angular velocity in 3 coordinate axes of the carrier coordinate system.
Based on the 6-degree-of-freedom model of the AUV, the AUV vertical plane control model can be obtained. Firstly, obtaining a motion equation of the simplified AUV vertical plane motion, wherein a z-axis floating and submerging equation is as follows:
the y-axis pitch equation is:
in the above formula: fiIs white gaussian noise; zw|w|，Zq|q|Vertical power coefficients, Z, of velocity and angular velocity, respectively, in a motion coordinate systemuq，ZuwThe vertical dynamic coefficient components of the speed and the angular velocity on the motion coordinate system, Mw|w|，Mq|q|The pitch kinetic coefficients of the speed and the angular velocity on the motion coordinate system respectively,
are respectively a multivariate matrix function
First partial derivative of middle w and q components in the pitch plane, Muq，MuwThe pitch dynamic coefficient components of the velocity and the angular velocity on the motion coordinate system respectively,
are respectively a multivariate matrix function
The first-order partial derivatives of the middle w and q components on the vertical plane; zgStress in the z-axis direction; mgIs gravity; zprop，MpropThrust in the z-axis direction and thrust moment in the y-axis direction are respectively; x is the number ofg，zgAre barycentric coordinates.
Then, substituting 'sea exploration I type' AUV gravity and rotational inertia of the AUV on the y axis into the vertical plane control model to obtain a z-axis floating and submerging equation as follows:
the y-axis pitch motion equation is:
if it is assumed that the navigation depth of the AUV is not changed, and only the heading and the track are changed, the gravity center of the AUV is considered to be kept on the horizontal plane, and the coordinate transformation relationship of the AUV in the inertial coordinate system in the horizontal plane can be expressed as:
the earth coordinate system takes a horizontal plane point E as an original point, the zeta axis points to the geographical north direction, the eta axis points to the geographical east direction, and the zeta axis points to the earth center respectively;
first, the kinematic equation of the horizontal plane AUV is obtained, i.e.
The x-axis forward and backward kinematic equation is:
the y-axis translational kinematic equation is as follows:
the z-axis bow-turning kinematic equation is as follows:
in the formula, XuuFor the axial power coefficient of the speed on the moving coordinate system,
as a function of a multivariate matrix
First partial derivative, X, of the median component u in the axial directionvr，XrrAxial dynamic coefficient components, Y, of velocity and angular velocity, respectively, in a motion coordinate systemv|v|For the lateral power coefficient of the speed on the motion coordinate system,
as a function of a multivariate matrix
First partial derivative of the median component v in the lateral direction, Yvr，YuvThe lateral dynamic coefficient components of velocity and angular velocity, N, respectively, in a motion coordinate systemv|v|，Nr|r|Respectively are the turning dynamic coefficients of the speed and the angular speed on a moving coordinate system,
as a function of a multivariate matrix
First partial derivative of the mid-component r in heading, Nur，NuvThe components of the dynamic coefficient of the rotating bow, X, of the speed and the angular velocity on the motion coordinate systemprop，Yprop，NpropAxial, lateral and vertical thrust moments, respectively.
Then, substituting the AUV gravity and the rotational inertia of the AUV in the z axis into the horizontal plane control model to obtain an x axial forward and backward motion equation:
∑X＝6sinθ+Xprop+Fi+-10.050u|u|-146.848wq+-12.816q2+146.848vr-12.816r2(10)
the y-axis translational motion equation is as follows:
the z-axis bow-turning motion equation is as follows:
step 2: a performance index of a model predictive control algorithm is applied, and the index is a performance index which comprehensively reflects rolling time domain optimization.
In the formula: t is the prediction period, mu1、μ3Respectively representing the weight of the output middle end constraint and the control input in the performance index J, mu2Representing the weight of the tracking error in the performance indicator.
For a system in a certain time domain tau epsilon [0, T]The output of the prediction within the block is,
to be within a certain time domain tau epsilon [0, T]The desired output of (a) is the reference output.
And step 3: and outputting the control quantity by adopting an improved S-surface control algorithm, performing motion control on the AUV, and completing AUV recovery three-dimensional path tracking.
The control model of the S-surface controller is as follows:
wherein k is1And k2For control coefficients, equivalent to the PD coefficient in a PID controller, Δ u is the adjustment term, e and
e is depth and heading angle error information for controlling the input information,
for the rate of change of the error of depth and heading angle, u is controlThe output, in AUV, is the thrust and torque of the corresponding propeller.
Combining the characteristics of PID control, designing the delta u term of the S-surface controller into an integral term, wherein the control model is as follows:
namely when
Or
When the S-plane control is integrated, when
Or e (t) is 0, the S-plane is not integrated.
In addition to the above embodiments, the present invention may have other embodiments, and any technical solutions formed by equivalent substitutions or equivalent transformations fall within the scope of the claims of the present invention.
Claims (5)
1. A full-drive autonomous underwater robot recovery three-dimensional path tracking control system is characterized by comprising an AUV body control system (1) and a docking station control system (2), wherein the AUV body control system (1) is positioned in the middle section of the whole AUV and comprises a main control panel (3), a visual processing unit (4), a battery pack (5), an ultra-short baseline receiver (6), a Doppler log (7) and a propulsion module (8), the visual processing unit (4) is arranged at the fore section of the AUV, the battery pack (5) is positioned at the bottom of the AUV body control system (1) and is separated from the main control panel (3) through a partition plate, the ultra-short baseline receiver (6) and the Doppler log (7) are arranged at the bottom of the AUV and are exposed out of a cabin shell, the propulsion module (8) is positioned at the stern section of the AUV, the design behind a propeller is adopted, the visual processing unit (4) and the ultra-short baseline receiver (6), the Doppler log (7) and the propulsion module (8) are connected with the main control panel (3) through a power line and a signal line, the battery pack (5) supplies power to the main control panel (3) through the power line, the AUV body control system (1) and the monitoring upper computer (9) communicate through wireless signals, during docking, an ultra-short baseline (10) and an ultra-short baseline receiver (6) are used between the AUV body control system (1) and the docking station control system (2) for wireless communication, the docking station control system (2) is installed on a base (11), a horn mouth (12) is fixed on the base (11) through a support, the docking station comprises the ultra-short baseline (10), a horn mouth (12) and a marker lamp (13), the ultra-short baseline (10) is vertically arranged and is installed right above the horn mouth (12), the marker lamp (13) is installed below the horn mouth (12), and the ultra-short baseline (10) and the marker lamp (13) are connected with the docking control system (2) through wires, the docking station control system (2) is connected with the monitoring console (14) through a photoelectric composite cable (15).
2. The control method of the recovery three-dimensional path tracking control system of the fully driven autonomous underwater robot as claimed in claim 1, wherein the recovery control process includes the following two processes:
a straight line homing process (16), which is a tracking process that the recovery device (17) starts to locate the AUV by using an ultra-short baseline and enters a docking central axis (18); the position of the AUV and the position of the butt joint device are adjusted by utilizing the relative position and posture information provided by the ultra-short baseline, so that the AUV is navigated to the position near the linear tracking point (19) under the condition of low consumption, an improved S-plane controller, namely an improved Sigmoid plane controller, is started to reach the position near the linear tracking point (19), the posture of the AUV is consistent with the central axis (18), and the real-time butt joint is further facilitated;
and a straight line tracking process (20), wherein the stage refers to a stage that the gravity center of the AUV enters the central axis (18) until the distance from the interface is 3-5 meters, the AUV is ensured to sail along the central axis (18) at the stage, and the heading angle points to the final calibration point (21) and then keeps heading and completes the tracking of the straight line.
3. The control method of the all-drive autonomous underwater vehicle recovery three-dimensional path tracking control system according to claim 2, wherein the three-dimensional path tracking control method of the straight line homing process (16) and the straight line tracking process (20) comprises the steps of:
step 1: establishing a coordinate system and establishing an AUV mathematical model;
step 2: decomposing the AUV6 freedom degree model into a vertical plane control model and a horizontal plane control model, and neglecting the coupling between the two models;
and step 3: discretizing data of the vertical plane control model and the horizontal plane control model, applying a model prediction control algorithm, correcting the system in real time according to the deviation of the AUV path tracking target quantity and a predicted value, and outputting optimal parameters of S-plane control;
and 4, step 4: and outputting the thrust of the propeller by adopting an improved S-surface control algorithm, performing motion control on the AUV, and completing AUV recovery three-dimensional path tracking.
4. The control method of the recovery three-dimensional path tracking control system of the fully-driven autonomous underwater robot as claimed in claim 3, wherein the step 1 of constructing the AUV mathematical model comprises the steps of:
step 1: first, two coordinate systems are established: the method comprises the following steps that a geodetic coordinate system and a motion coordinate system are adopted, the floating center is used as the origin of the motion coordinate system by the kinematic equation of the AUV, and the following equation is obtained:
in the formula: m is the mass of the AUV body; x is the number ofG，yG，zGCoordinates of the center of gravity of the AUV respectively; i isx，Iy，IzThe rotational inertia of the AUV on 3 coordinate axes is respectively; u, v and w are components of AUV speed in 3 coordinate axes of a carrier coordinate system; p, q and r are components of the AUV angular velocity in 3 coordinate axes of the carrier coordinate system;
step 2: based on the above equation, an AUV vertical plane control model can be obtained, first, a simplified equation of motion of the AUV vertical plane is obtained, where ygTypically 0, the z-axis float equation is:
constant speed
To be 0, the y-axis pitch equation is:
in the above formula: fiIs white gaussian noise; zw|w|，Zq|q|Vertical power coefficients, Z, of velocity and angular velocity, respectively, in a motion coordinate systemuq，ZuwThe vertical dynamic coefficient components of the speed and the angular velocity on the motion coordinate system, Mw|w|，Mq|q|The pitch kinetic coefficients of the speed and the angular velocity on the motion coordinate system respectively,
are respectively a multivariate matrix function
First partial derivative of middle w and q components in the pitch plane, Muq，MuwThe pitch dynamic coefficient components of the velocity and the angular velocity on the motion coordinate system respectively,
are respectively a multivariate matrix function
The first-order partial derivatives of the middle w and q components on the vertical plane; zgStress in the z-axis direction; mgIs gravity; zprop，MpropThrust in the z-axis direction and y-axis direction respectivelyA thrust moment in the direction; x is the number ofg，zgIs a barycentric coordinate;
then, substituting the model parameters into the vertical plane control model to obtain a z-axis floating and submerging equation as follows:
the y-axis pitch motion equation is:
if it is assumed that the navigation depth of the AUV is not changed, and only the heading and the track are changed, the gravity center of the AUV is considered to be kept on the horizontal plane, and the coordinate transformation relationship of the AUV in the inertial coordinate system in the horizontal plane can be expressed as:
the earth coordinate system takes a horizontal plane point E as an original point, the zeta axis points to the geographical north direction, the eta axis points to the geographical east direction, and the zeta axis points to the earth center respectively;
when w is 0, p is 0, q is 0, ygWith 0, the kinematic equation for the horizontal plane AUV is first simplified, i.e.
The x-axis forward and backward kinematic equation is:
the y-axis translational kinematic equation is as follows:
the z-axis bow-turning kinematic equation is as follows:
in the formula, Xu|u|For the axial power coefficient of the speed on the moving coordinate system,
as a function of a multivariate matrix
First partial derivative, X, of the median component u in the axial directionvr，XrrAxial dynamic coefficient components, Y, of velocity and angular velocity, respectively, in a motion coordinate systemv|v|For the lateral power coefficient of the speed on the motion coordinate system,
as a function of a multivariate matrix
First partial derivative of the median component v in the lateral direction, Yvr，YuvThe lateral dynamic coefficient components of velocity and angular velocity, N, respectively, in a motion coordinate systemv|v|，Nr|r|Respectively are the turning dynamic coefficients of the speed and the angular speed on a moving coordinate system,
as a function of a multivariate matrix
First partial derivative of the mid-component r in heading, Nur，NuvThe components of the dynamic coefficient of the rotating bow, X, of the speed and the angular velocity on the motion coordinate systemprop，Yprop，NpropThrust moments in the axial direction, the lateral direction and the vertical direction respectively;
then, substituting the parameters of the AUV model into the horizontal plane control model to obtain an x axial forward and backward movement equation:
∑X＝6sinθ+Xprop+Fi+-10.050u|u|-146.848wq+-12.816q2+146.848vr-12.816r2(10)
the y-axis translational motion equation is as follows:
the z-axis bow-turning motion equation is as follows:
5. the control method of the all-drive autonomous underwater vehicle recovery three-dimensional path tracking control system according to claim 3, wherein the improved S-plane control algorithm of the step 4 comprises the steps of:
step 1: the performance index of the applied model predictive control algorithm is a performance index which comprehensively reflects the rolling time domain optimization;
in the formula: t is the prediction period, mu1、μ3Respectively representing the weight of the output middle end constraint and the control input in the performance index J, mu2Represents the weight of the tracking error in the performance indicator,
for a system in a certain time domain tau epsilon [0, T]The output of the prediction within the block is,
to be within a certain time domain tau epsilon [0, T]The desired output within, i.e., the reference output;
step 2: outputting a control quantity by adopting an improved S-surface control algorithm, performing motion control on the AUV, and completing AUV recovery three-dimensional path tracking;
the control model of the S-surface controller is as follows:
wherein k is1And k2To control the coefficients, one can analogize them to the PD coefficients in a PID controller, Δ u is an adjustment term, one can consider it as a fixed disturbance force over a period of time or as another adjustment factor, e and
e is depth and heading angle error information for controlling the input information,
the error change rate of the depth and the heading angle is U, the control output is U, and the control output is considered as the thrust and the torque of a corresponding propeller in AUV;
combining the characteristics of PID control, designing the delta u term of the S-surface controller into an integral term, wherein the control model is as follows:
namely when
Or
When the S-plane control is integrated, when
Or e (t) is 0, the S-plane is not integrated.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010988593.2A CN112148022B (en) | 2020-09-18 | 2020-09-18 | System and method for controlling recovery three-dimensional path tracking of full-drive autonomous underwater robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010988593.2A CN112148022B (en) | 2020-09-18 | 2020-09-18 | System and method for controlling recovery three-dimensional path tracking of full-drive autonomous underwater robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112148022A true CN112148022A (en) | 2020-12-29 |
| CN112148022B CN112148022B (en) | 2023-10-03 |
Family
ID=73893901
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010988593.2A Active CN112148022B (en) | 2020-09-18 | 2020-09-18 | System and method for controlling recovery three-dimensional path tracking of full-drive autonomous underwater robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112148022B (en) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112947423A (en) * | 2021-01-29 | 2021-06-11 | 深圳市银星智能科技股份有限公司 | Cleaning path planning method, mobile robot and storage medium |
| CN112947067A (en) * | 2021-01-26 | 2021-06-11 | 大连海事大学 | Three-dimensional track accurate tracking control method for underwater robot |
| CN113050420A (en) * | 2021-03-04 | 2021-06-29 | 山东大学 | AUV path tracking method and system based on S-plane control and TD3 |
| CN113050666A (en) * | 2021-03-26 | 2021-06-29 | 湖南大学 | Depth and longitudinal decoupling control method and system for underwater autonomous vehicle |
| CN113485389A (en) * | 2021-08-03 | 2021-10-08 | 艾乐尔机器人科技(南京)有限公司 | Underwater robot three-dimensional trajectory tracking control method based on self-adaptive prediction |
| CN113885534A (en) * | 2021-11-19 | 2022-01-04 | 江苏科技大学 | Intelligent prediction control-based water surface unmanned ship path tracking method |
| CN114771778A (en) * | 2022-04-12 | 2022-07-22 | 株洲中车时代电气股份有限公司 | Recovery method and recovery device for underwater robot and underwater robot |
| CN115102632A (en) * | 2022-05-19 | 2022-09-23 | 西北工业大学 | AUV underwater docking station non-contact communication device |
| CN114035550B (en) * | 2021-11-15 | 2023-11-21 | 江苏科技大学 | Autonomous underwater robot actuating mechanism fault diagnosis method based on ESO |
| CN117389312A (en) * | 2023-11-07 | 2024-01-12 | 中国人民解放军海军工程大学 | Model-based three-dimensional tracking control method for counter roll of underwater vehicle |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107953350A (en) * | 2016-10-17 | 2018-04-24 | 江苏舾普泰克自动化科技有限公司 | It is a kind of to be used to detect the underwater robot control system with operation |
| CN109032178A (en) * | 2018-08-06 | 2018-12-18 | 江苏科技大学 | Full driving AUV recovery control system and voluntary recall method |
| CN109050835A (en) * | 2018-08-06 | 2018-12-21 | 江苏科技大学 | Full driving autonomous underwater robot structure and recycling three-dimensional path tracking |
| US20190077507A1 (en) * | 2017-09-14 | 2019-03-14 | Drone Racing League, Inc. | Three-dimensional pathway tracking system |
-
2020
- 2020-09-18 CN CN202010988593.2A patent/CN112148022B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107953350A (en) * | 2016-10-17 | 2018-04-24 | 江苏舾普泰克自动化科技有限公司 | It is a kind of to be used to detect the underwater robot control system with operation |
| US20190077507A1 (en) * | 2017-09-14 | 2019-03-14 | Drone Racing League, Inc. | Three-dimensional pathway tracking system |
| CN109032178A (en) * | 2018-08-06 | 2018-12-18 | 江苏科技大学 | Full driving AUV recovery control system and voluntary recall method |
| CN109050835A (en) * | 2018-08-06 | 2018-12-21 | 江苏科技大学 | Full driving autonomous underwater robot structure and recycling three-dimensional path tracking |
Non-Patent Citations (3)
| Title |
|---|
| 任奕: "小型多用途自主式无人水下航行器MT-2010", 《水雷战与舰船防护》 * |
| 姚金艺: "自主水下机器人回收路径跟踪控制研究", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 * |
| 姚金艺等: "全驱动型AUV三维路径跟踪控制***设计及分析", 《中国舰船研究》 * |
Cited By (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112947067A (en) * | 2021-01-26 | 2021-06-11 | 大连海事大学 | Three-dimensional track accurate tracking control method for underwater robot |
| CN112947067B (en) * | 2021-01-26 | 2024-02-20 | 大连海事大学 | Precise tracking control method for three-dimensional track of underwater robot |
| CN112947423A (en) * | 2021-01-29 | 2021-06-11 | 深圳市银星智能科技股份有限公司 | Cleaning path planning method, mobile robot and storage medium |
| CN113050420B (en) * | 2021-03-04 | 2022-02-18 | 山东大学 | AUV path tracking method and system based on S-plane control and TD3 |
| CN113050420A (en) * | 2021-03-04 | 2021-06-29 | 山东大学 | AUV path tracking method and system based on S-plane control and TD3 |
| CN113050666A (en) * | 2021-03-26 | 2021-06-29 | 湖南大学 | Depth and longitudinal decoupling control method and system for underwater autonomous vehicle |
| CN113485389A (en) * | 2021-08-03 | 2021-10-08 | 艾乐尔机器人科技(南京)有限公司 | Underwater robot three-dimensional trajectory tracking control method based on self-adaptive prediction |
| CN114035550B (en) * | 2021-11-15 | 2023-11-21 | 江苏科技大学 | Autonomous underwater robot actuating mechanism fault diagnosis method based on ESO |
| CN113885534A (en) * | 2021-11-19 | 2022-01-04 | 江苏科技大学 | Intelligent prediction control-based water surface unmanned ship path tracking method |
| CN113885534B (en) * | 2021-11-19 | 2023-11-21 | 江苏科技大学 | Intelligent predictive control-based water surface unmanned ship path tracking method |
| CN114771778A (en) * | 2022-04-12 | 2022-07-22 | 株洲中车时代电气股份有限公司 | Recovery method and recovery device for underwater robot and underwater robot |
| CN115102632A (en) * | 2022-05-19 | 2022-09-23 | 西北工业大学 | AUV underwater docking station non-contact communication device |
| CN115102632B (en) * | 2022-05-19 | 2024-05-03 | 西北工业大学 | AUV underwater docking station non-contact communication device |
| CN117389312A (en) * | 2023-11-07 | 2024-01-12 | 中国人民解放军海军工程大学 | Model-based three-dimensional tracking control method for counter roll of underwater vehicle |
| CN117389312B (en) * | 2023-11-07 | 2024-05-28 | 中国人民解放军海军工程大学 | Model-based anti-roll three-dimensional tracking control method for underwater vehicle |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112148022B (en) | 2023-10-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112148022A (en) | System and method for tracking and controlling recovery three-dimensional path of full-drive autonomous underwater robot | |
| CN107168312B (en) | Space trajectory tracking control method for compensating UUV kinematic and dynamic interference | |
| CN109032178B (en) | Full-drive AUV recovery control system and autonomous recovery method | |
| Abreu et al. | The MEDUSA class of autonomous marine vehicles and their role in EU projects | |
| Ihle et al. | Formation control of marine surface craft: A Lagrangian approach | |
| CN111208845B (en) | Path planning method and device for movable butt joint recovery of underwater robot | |
| CN105807789B (en) | UUV control methods based on the compensation of T-S Fuzzy Observers | |
| CN109131801A (en) | The voluntary recall device and control method of the complete deep unmanned submersible in sea | |
| CN111857165B (en) | Trajectory tracking control method of underwater vehicle | |
| CN107991872B (en) | Virtual anchoring horizontal area stabilization control method of under-actuated AUV (autonomous underwater vehicle) and implementation method | |
| CN112947504B (en) | Autonomous recovery control system and control method for unmanned underwater vehicle | |
| CN113162698B (en) | Underwater unmanned vehicle isomer networking detection system and detection method thereof | |
| CN112068440A (en) | Model prediction control-based AUV (autonomous Underwater vehicle) recovery butt joint power positioning control method | |
| CN114967714A (en) | Anti-interference motion control method and system for autonomous underwater robot | |
| Wang et al. | Development of the control system for a wave driven glider | |
| CN112925332B (en) | Cooperative intersection butt joint control method for unmanned ship and underwater unmanned submersible vehicle combined system | |
| Silpa-Anan et al. | Preliminary experiments in visual servo control for autonomous underwater vehicle | |
| CN114815859A (en) | Portable autonomous underwater robot system and control system thereof | |
| CN114791735A (en) | Unmanned ship deviation rectifying method and system | |
| CN113110458A (en) | Unmanned ship virtual target tracking control system | |
| Feng et al. | H∞ autopilot design for autonomous underwater vehicles | |
| CN114019805B (en) | Model prediction butt joint control method of under-actuated auv | |
| ZHAO et al. | Fusion guiding technology solution and algorithm for underwater docking of autonomous underwater vehicles | |
| Koh et al. | Preliminary studies of the modelling and control of a twin-barrel underactuated underwater robotic vehicle | |
| Matos et al. | Coordinated operation of autonomous underwater and 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 |