CN112148022B - System and method for controlling recovery three-dimensional path tracking of full-drive autonomous underwater robot - Google Patents

System and method for controlling recovery three-dimensional path tracking of full-drive autonomous underwater robot Download PDF

Info

Publication number
CN112148022B
CN112148022B CN202010988593.2A CN202010988593A CN112148022B CN 112148022 B CN112148022 B CN 112148022B CN 202010988593 A CN202010988593 A CN 202010988593A CN 112148022 B CN112148022 B CN 112148022B
Authority
CN
China
Prior art keywords
auv
control
coordinate system
plane
motion
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
CN202010988593.2A
Other languages
Chinese (zh)
Other versions
CN112148022A (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.)
Jiangsu University of Science and Technology
Original Assignee
Jiangsu University of Science and Technology
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 Jiangsu University of Science and Technology filed Critical Jiangsu University of Science and Technology
Priority to CN202010988593.2A priority Critical patent/CN112148022B/en
Publication of CN112148022A publication Critical patent/CN112148022A/en
Application granted granted Critical
Publication of CN112148022B publication Critical patent/CN112148022B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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

Abstract

The invention discloses a full-drive autonomous underwater robot recovery three-dimensional path tracking control system and method, and provides a sectional recovery three-dimensional path tracking method, and a model prediction integral s-plane control algorithm for a robot recovery straight line homing stage and a straight line tracking stage is provided. The adopted model predictive integral S-plane control algorithm provides advanced pose information through prediction of a nonlinear dynamic system. And detecting errors of the actual output of the carrier and the output of the prediction model through feedback correction time, correcting the prediction output in real time, and adjusting the parameters of the integral S-plane controller in a rolling way to form a parameter adjusting loop. The adopted control method combines model predictive control and S-plane control, so that the control method has a mechanism capable of processing multiple inputs and multiple outputs and is suitable for a nonlinear model, the Deltau term of the S-plane 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

System and method for controlling recovery three-dimensional path tracking of full-drive autonomous underwater robot
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 cable-free autonomous underwater robot) is widely applied to the field of military and civil use, is one of important tools in ocean development, and plays an important role in underwater observation, positioning, exploration and other tasks 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 tasks such as mission, smooth recovery and arrangement of the AUV. Three-dimensional path tracking control is a key technology for autonomous operation of an AUV as a main index for measuring the control performance of the AUV.
The three-dimensional path tracking control is mainly to decouple it into horizontal (lateral) and vertical (vertical) control. Due to uncertainty and nonlinear characteristics of an AUV model, and interference of ocean currents and the like, great difficulty is brought to the design of a controller. Currently, researchers are conducting research on three-dimensional path tracking of AUV around the above problems. 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 anti-interference performance, but AUV is not considered to be a discrete system; a horizontal plane tracking controller is designed by adopting a back-stepping method, and an unknown state and an uncertainty item are estimated by utilizing a differentiator, but the calculation amount of the method is large, and the real AUV system cannot be processed in real time; the constraint path tracking control law of the AUV is designed by adopting a nonlinear model predictive control method so as to solve the problem of constraint path tracking control, but still has the problem of large calculation amount. The patent document with the application number of 201010559361.1 discloses a small underwater robot combined navigation positioning method, and the adopted model prediction control algorithm is poor in consistency, so that the AUV is easy to diverge in autonomous navigation; patent document with the application number of 201210490435.X discloses a track tracking sliding mode control system and a control method for a medicine spraying mobile robot, wherein 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, so that the controller has better robustness and anti-interference performance, and the method is a target of the current AUV three-dimensional path tracking controller design.
Disclosure of Invention
The invention aims to provide a full-drive autonomous underwater robot recovery three-dimensional path tracking control system and a full-drive autonomous underwater robot recovery three-dimensional path tracking control method. The model prediction S-plane control method is adopted to fit the nonlinear characteristic of the AUV, and an integral term is introduced to enhance the anti-interference performance of the controller, so that the problem of simplifying the design of the controller is well solved, and the recovery precision and stability of the AUV are effectively improved.
The aim of the invention is realized by the following technical scheme:
the 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 at the middle section of the whole AUV, and comprises a main control board 3, a visual processing unit 4, a battery pack 5 and an ultra-short baseline receiver 6, a Doppler log 7 and a propulsion module 8, wherein the visual processing unit 4 is arranged at the bow 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 board 3 through a baffle plate, the ultra-short baseline receiver 6 and the Doppler log 7 are arranged at the bottom of the AUV and are exposed outside a cabin, the propulsion module 8 is positioned at the stern section of the AUV, the visual processing unit 4, the ultra-short baseline receiver 6, the Doppler log 7 and the propulsion module 8 are connected with the main control board 3 through a power line and a signal line by adopting a propeller rudder design, the battery pack 5 supplies power to the main control board 3 through a power line, the AUV body control system 1 and the monitoring upper computer 9 are communicated through wireless signals, during docking, the AUV body control system 1 and the docking station control system 2 are in wireless communication through the ultra-short base line 10 and the ultra-short base line receiver 6, the docking station control system 2 is arranged on the base 11, the bell mouth 12 is fixed on the base 11 through a support, the docking station comprises the ultra-short base line 10, the bell mouth 12 and the marker lamp 13, the ultra-short base line 10 is vertically arranged and is arranged right above the bell mouth 12, the marker lamp 13 is arranged below the bell mouth 12, the ultra-short base line 10 and the marker lamp 13 are connected with the docking station control system 2 through wires, and the docking station control system 2 and the monitoring station 14 are connected through the photoelectric composite cable 15.
A control method of a full-drive autonomous underwater robot recovery three-dimensional path tracking control system comprises the following two processes:
a straight line homing process 16, which means that the recovery device 17 starts to locate the AUV by using an ultrashort baseline and enters a tracking process of the butt joint central axis 18; the relative position and the gesture information provided by the ultra-short base line are utilized to adjust the positions of the AUV and the docking device, so that the AUV is navigated to the vicinity of the linear tracking point 19 under the condition of small consumption, and an improved S-plane controller, namely an improved Sigmoid plane controller, is started to ensure that the gesture of the AUV is consistent with the central axis 18, and further real-time docking is facilitated.
The straight line tracking process 20 is a stage in which the center of gravity of the AUV enters the central axis 18 and starts to be 3-5 meters away from the alignment interface, and in this stage, the AUV is guaranteed to navigate along the central axis 18, and after the heading angle points to the final alignment point 21, 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 recovery three-dimensional path tracking control system of the fully-driven autonomous underwater robot, wherein the three-dimensional path tracking control method of the linear homing process 16 and the linear tracking process 20 comprises the following steps:
step 1: establishing a coordinate system and constructing 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 neglecting the coupling between them.
Step 3: discretizing data of the vertical plane control model and the horizontal plane control model, carrying out real-time correction on the system according to the deviation of the AUV path tracking target quantity and the predicted value by using a model prediction control algorithm, and outputting the optimal parameters of S-plane control.
Step 4: and outputting the thrust of the propeller by adopting an improved S-plane control algorithm, and performing motion control on the AUV to complete recovery three-dimensional path tracking of the AUV.
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: first two coordinate systems are established: the geodetic coordinate system and the motion coordinate system, the kinematic equation of the AUV takes the centroid, namely the centroid of the AUV in the underwater partial volume, as the origin of the motion coordinate system to obtain the following equation:
wherein: m is the mass of the AUV body; x is x G ，y G ，z G The motion coordinate system coordinates of the gravity center of the AUV are respectively; i x ，I y ，I z The rotational inertia of the AUV in 3 coordinate axes of the motion coordinate system is respectively shown; u, v, w are components of AUV speed in 3 coordinate axes of a motion coordinate system; p, q, r are components of the angular velocity of the AUV in 3 coordinate axes of the motion coordinate system.
Step 2: based on the above equation, a simplified AUV vertical plane motion equation can be obtained, and the z-axis floating equation is:
the y-axis trim equation is:
in the above formula: f (F) i Is Gaussian white noise; z is Z w|w| ，Z q|q| The vertical dynamic coefficients of the speed and the angular speed on a motion coordinate system are respectively Z uq ，Z uw The vertical dynamic coefficient components of the speed and the angular speed on a motion coordinate system are respectively M w|w| ，M q|q| Pitch power coefficients of speed and angular speed on a motion coordinate system respectively,respectively a plurality of matrix functionsFirst-order partial derivative of w and q components in pitch plane, M uq ，M uw The pitch dynamics coefficient components of the speed and the angular speed in the motion coordinate system, respectively, +.>Respectively a plurality of matrix functionsFirst partial derivatives of w and q components in a vertical plane; z is Z g Is stress in the z-axis direction; m is M g Is gravity; z is Z prop ，M prop Thrust force in the z-axis direction and thrust moment in the y-axis direction respectively; x is x g ，z g Is the barycentric coordinates.
Then, substituting AUV gravity and rotational inertia of the AUV in a y axis into a vertical plane control model to obtain a z-axis floating equation:
the y-axis pitch motion equation is:
if the depth of the AUV navigation 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 relation of the AUV in the inertial coordinate system in the horizontal plane can be expressed as follows:
the geodetic coordinate system takes a point E of a horizontal plane as an origin, a zeta axis points to the north direction of the geography, a eta axis points to the east direction of the geography, and zeta axes respectively point to the earth center;
firstly, the kinematic equation of the AUV of the horizontal plane is obtained, namely
The x-axis forward and backward kinematic equation is:
the translational kinematic equation in the y-axis direction is:
the kinematic equation of the z-axis turning bow is that
Wherein X is u|u| For the axial power coefficient of the velocity in the motion coordinate system,as a multiple matrix functionFirst-order partial derivative of middle component u in axial direction, X vr ，X rr The axial dynamic coefficient components of the speed and the angular speed on a motion coordinate system are respectively, Y v|v| For the lateral dynamic coefficient of speed on the motion coordinate system, +.>For a multiple matrix function->First-order partial derivative of the mid-component v in lateral direction, Y vr ，Y uv The lateral dynamic coefficient components of the velocity and the angular velocity in the motion coordinate system respectively, N v|v| ，N r|r| The yaw dynamic coefficients of speed and angular speed on the motion coordinate system are respectively +.>For a multiple matrix function->First order partial derivative of middle component r in heading, N ur ，N uv The power coefficient components of the bow turning of the speed and the angular speed on a motion coordinate system are respectively, X prop ，Y prop ，N prop Respectively, axially, laterally andvertical pushing moment.
Then, substituting AUV gravity and rotational inertia of the AUV in a z axis into a horizontal plane control model to obtain an x-axis forward and backward motion equation as follows:
∑X＝6sinθ+X prop +Fi+-10.050u|u|-146.848wq+-12.816q 2 +146.848vr-12.816r 2 (10)
the translational motion equation in the y-axis direction is:
the z-axis bow-turning motion equation is:
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 model predictive control algorithm is a performance index which comprehensively reflects the rolling time domain optimization.
Wherein: t is the prediction period, mu 1 、μ 3 Respectively represent the weight value occupied by the constraint and control input of the middle end in the output in the performance index J, mu 2 The weight of the tracking error in the performance index is indicated.For the system in a certain time domain tau epsilon [0, T]Prediction output of intra->For T in a certain time domain tau epsilon [0, T]The desired output within is the reference output.
Step 2: and outputting control quantity by adopting an improved S-plane control algorithm, and performing motion control on the AUV to complete recovery three-dimensional path tracking of the AUV.
The control model of the S-plane controller is as follows:
wherein k is 1 And k 2 For control coefficients, equivalent to PD coefficients in a PID controller, deltau is the adjustment term, e andfor controlling the input information e is depth and heading angle error information +.>The change rate of the depth and the heading angle errors is represented by u, which is a control output, and the thrust and the torque of the corresponding propeller are represented in the AUV.
Combining the characteristics of PID control, designing an integral term for the Deltau term of the S-plane controller, wherein a control model is as follows:
i.e. whenOr->At the time of S-plane control, when +.>Or e (t) =0, the S-plane is not integrated.
Compared with the prior art, the invention has the beneficial effects that:
1. the full-drive autonomous underwater robot control system for recycling adopts modularized, distributed and star-shaped topological structure design. The system can provide a unified, 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 straight line homing stage and a straight line tracking stage in the robot recovery process, thereby improving the success rate of 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 errors of the actual output of the carrier and the output of the predictive model are detected at any time, the predictive output is corrected in real time, and the parameters of the S-plane controller are regulated to form a parameter regulation loop. The method can process the AUV which is a complex nonlinear system and can also perform multiple input and multiple output of parameters.
4. The improved model prediction S-plane control algorithm is adopted, an integral term is designed for the delta u term of the S-plane controller, steady-state errors during tracking are reduced by introducing the integral term, and the anti-interference capability of the AUV on ocean currents is enhanced.
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 a three-dimensional path tracking model predictive S-plane control algorithm in an AUV recovery process;
FIG. 5 is a flowchart of a three-dimensional path tracking model predictive S-plane control algorithm in an AUV recovery process.
Specific implementation measures
The invention will be further described with reference to the drawings and the specific examples.
As shown in fig. 1, the full-drive autonomous underwater robot recovery three-dimensional path tracking control system block diagram comprises an AUV body control system 1 and a docking station control system 2, wherein the AUV body control system 1 is positioned at the middle section of the whole AUV and comprises a main control board 3, a visual processing unit 4, a battery pack 5 and an ultra-short baseline receiver 6, a doppler log 7 and a propulsion module 8, the visual processing unit 4 is arranged at the bow 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 board 3 through a baffle plate, the ultra-short baseline receiver 6 and the doppler log 7 are arranged at the bottom of the AUV and are exposed outside a cabin, the propulsion module 8 is positioned at the stern section of the AUV and adopts a propeller rear rudder design, the visual processing unit 4, the ultra-short baseline receiver 6, the doppler log 7 and the propulsion module 8 are connected with the main control board 3 through a power line and a signal line, the battery pack 5 supplies power to the main control board 3 through a power line, the AUV body control system 1 and the monitoring upper computer 9 are communicated through wireless signals, during docking, the AUV body control system 1 and the docking station control system 2 are in wireless communication through the ultra-short base line 10 and the ultra-short base line receiver 6, the docking station control system 2 is arranged on the base 11, the bell mouth 12 is fixed on the base 11 through a support, the docking station comprises the ultra-short base line 10, the bell mouth 12 and the marker lamp 13, the ultra-short base line 10 is vertically arranged and is arranged right above the bell mouth 12, the marker lamp 13 is arranged below the bell mouth 12, the ultra-short base line 10 and the marker lamp 13 are connected with the docking station control system 2 through wires, and the docking station control system 2 and the monitoring station 14 are connected through the photoelectric composite cable 15.
As shown in fig. 2, the recovery 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 means that the recovery device 17 starts to locate the AUV by using an ultrashort baseline and enters a tracking process of the butt joint central axis 18; the relative position and the gesture information provided by the ultra-short base line are utilized to adjust the positions of the AUV and the docking device, so that under the condition of small consumption, the AUV is navigated to the vicinity of the linear tracking point 19, and an improved S-plane controller, namely an improved Sigmoid-plane controller, is started to enable the gesture of the AUV to be consistent with the central axis 18, and further real-time docking is facilitated;
the straight line tracking process 20 is a stage in which the center of gravity of the AUV enters the central axis 18 and starts to be 3-5 meters away from the alignment interface, and in this stage, the AUV is guaranteed to navigate along the central axis 18, and after the heading angle points to the final alignment point 21, 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- ζ ηζ static coordinate system) and a motion coordinate system (O-xyz dynamic coordinate system).
FIG. 4 is a block diagram showing the model predictive integral S-plane control algorithm of the recovery three-dimensional path tracking control system of the fully-driven autonomous underwater robot; FIG. 5 is a flowchart of a model predictive integral S-plane control algorithm of a recovery three-dimensional path tracking control system of a fully-driven autonomous underwater robot, wherein the algorithm comprises the following steps.
Step 1: the kinematic equation of the AUV takes a floating center as an origin of a motion coordinate system, and the following equation is obtained through long-term theoretical analysis and engineering practice:
wherein: m is the mass of the AUV body; x is x G ，y G ，z G Coordinates of the gravity center of the AUV; i x ，I y ，I z Moment of inertia of the AUV in 3 coordinate axes respectively; u, v, w are components of AUV speed in 3 coordinate axes of the carrier coordinate system; p, q, r are components of the angular velocity of the AUV in 3 coordinate axes of the carrier coordinate system.
Based on the AUV 6-degree-of-freedom model, an AUV vertical plane control model can be obtained. Firstly, a simplified motion equation of AUV vertical plane motion is obtained, wherein a z-axis floating equation is as follows:
the y-axis trim equation is:
in the above formula: f (F) i Is Gaussian white noise; z is Z w|w| ，Z q|q| The vertical dynamic coefficients of the speed and the angular speed on a motion coordinate system are respectively Z uq ，Z uw The vertical dynamic coefficient components of the speed and the angular speed on a motion coordinate system are respectively M w|w| ，M q|q| Pitch power coefficients of speed and angular speed on a motion coordinate system respectively,respectively a plurality of matrix functionsFirst-order partial derivative of w and q components in pitch plane, M uq ，M uw The pitch dynamics coefficient components of the speed and the angular speed in the motion coordinate system, respectively, +.>Respectively a plurality of matrix functionsFirst partial derivatives of w and q components in a vertical plane; z is Z g Is stress in the z-axis direction; m is M g Is gravity; z is Z prop ，M prop Thrust force in the z-axis direction and thrust moment in the y-axis direction respectively; x is x g ，z g Is the barycentric coordinates.
Then substituting the gravity of the AUV in the sea-exploring I type and the rotational inertia of the AUV in the y axis into a vertical plane control model to obtain a z-axis floating equation:
the y-axis pitch motion equation is:
if the depth of the AUV navigation 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 relation of the AUV in the inertial coordinate system in the horizontal plane can be expressed as follows:
the geodetic coordinate system takes a point E of a horizontal plane as an origin, a zeta axis points to the north direction of the geography, a eta axis points to the east direction of the geography, and zeta axes respectively point to the earth center;
firstly, the kinematic equation of the AUV of the horizontal plane is obtained, namely
The x-axis forward and backward kinematic equation is:
the translational kinematic equation in the y-axis direction is:
the z-axis bow-turning kinematic equation is:
wherein X is uu For the axial power coefficient of the velocity in the motion coordinate system,as a multiple matrix functionFirst-order partial derivative of middle component u in axial direction, X vr ，X rr The axial dynamic coefficient components of the speed and the angular speed on a motion coordinate system are respectively, Y v|v| For the lateral dynamic coefficient of speed on the motion coordinate system, +.>For a multiple matrix function->First-order partial derivative of the mid-component v in lateral direction, Y vr ，Y uv The lateral dynamic coefficient components of the velocity and the angular velocity in the motion coordinate system respectively, N v|v| ，N r|r| The yaw dynamic coefficients of speed and angular speed on the motion coordinate system are respectively +.>For a multiple matrix function->First order partial derivative of middle component r in heading, N ur ，N uv The power coefficient components of the bow turning of the speed and the angular speed on a motion coordinate system are respectively, X prop ，Y prop ，N prop The thrust moment is respectively axial, lateral and vertical.
Then, substituting AUV gravity and rotational inertia of the AUV in a z axis into a horizontal plane control model to obtain an x-axis forward and backward motion equation as follows:
∑X＝6sinθ+X prop +Fi+-10.050u|u|-146.848wq+-12.816q 2 +146.848vr-12.816r 2 (10)
the translational motion equation in the y-axis direction is:
the z-axis bow-turning motion equation is:
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.
Wherein: t is the prediction period, mu 1 、μ 3 Respectively represent the weight value occupied by the constraint and control input of the middle end in the output in the performance index J, mu 2 The weight of the tracking error in the performance index is indicated.For the system in a certain time domain tau epsilon [0, T]Prediction output of intra->For T in a certain time domain tau epsilon [0, T]The desired output within is the reference output.
Step 3: and outputting control quantity by adopting an improved S-plane control algorithm, and performing motion control on the AUV to complete recovery three-dimensional path tracking of the AUV.
The control model of the S-plane controller is as follows:
wherein k is 1 And k 2 For control coefficients, equivalent to PD coefficients in a PID controller, deltau is the adjustment term, e andfor controlling the input information e is depth and heading angle error information +.>The change rate of the depth and the heading angle errors is represented by u, which is a control output, and the thrust and the torque of the corresponding propeller are represented in the AUV.
Combining the characteristics of PID control, designing an integral term for the Deltau term of the S-plane controller, wherein a control model is as follows:
i.e. whenOr->At the time of S-plane control, when +.>Or e (t) =0, the S-plane is not integrated.
In addition to the above embodiments, other embodiments of the present invention are possible, and all technical solutions formed by equivalent substitution or equivalent transformation are within the scope of the present invention.

Claims (2)

1. The control method of the 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 board (3), a vision processing unit (4), a battery pack (5) and an ultra-short baseline receiver (6), a Doppler log (7) and a propulsion module (8), the vision processing unit (4) is arranged on the AUV bow section, the battery pack (5) is positioned at the bottom of the AUV body control system (1), the battery pack (5) is separated from the main control board (3) through a baffle plate, the ultra-short baseline receiver (6) and the Doppler log (7) are arranged at the bottom of the AUV, the propulsion module (8) is positioned outside a cabin shell, a propeller rudder after-blade design is adopted, the vision processing unit (4), the ultra-short baseline receiver (6) and the ultra-short baseline receiver (7) are connected with the main control board (3) through a power line and a signal line, the battery pack (5) is arranged on the AUV body (3) through a signal line, when the power supply line is in communication with the power supply system (9) through the power supply line, the wireless communication system is carried out between the AUV body (3), an ultra-short baseline (10) and an ultra-short baseline receiver (6) are used for wireless communication between an AUV body control system (1) and a docking station control system (2), the docking station control system (2) is arranged on a base (11), a bell mouth (12) is fixed on the base (11) through a bracket, the docking station comprises the ultra-short baseline (10), the bell mouth (12) and a marker lamp (13), the ultra-short baseline (10) is vertically arranged and is arranged right above the bell mouth (12), the marker lamp (13) is arranged below the bell mouth (12), the ultra-short baseline (10) and the marker lamp (13) are connected with the docking station control system (2) through wires, and the docking station control system (2) is connected with a monitoring station (14) through a photoelectric composite cable (15);
the control method of the full-drive autonomous underwater robot recovery three-dimensional path tracking control system is characterized in that the recovery control process comprises the following two processes:
a straight line homing process (16), wherein the recovery device (17) starts from the position of the ultra-short base line to the AUV and enters a tracking process of the butt joint central axis (18); the relative position and the gesture information provided by the ultra-short base line are utilized to adjust the positions of the AUV and the docking device, so that under the condition of low consumption, the AUV is navigated to the vicinity of a linear tracking point (19), an improved S-plane controller, namely an improved Sigmoid-plane controller, is started to enable the gesture of the AUV to be consistent with a central axis (18), and further real-time docking is facilitated;
a straight line tracking process (20), which refers to a stage that the gravity center of the AUV enters the central axis (18) until the distance from the AUV to the interface is 3-5 m, wherein the AUV is guaranteed to navigate along the central axis (18) at the stage, and the heading is maintained after the heading angle points to a final calibration point (21) and the straight line tracking is completed;
the three-dimensional path tracking control method of the straight line homing process (16) and the straight line tracking process (20) comprises the following steps:
step 1: establishing a coordinate system and constructing an AUV mathematical model;
step 2: decomposing the AUV6 degree-of-freedom model into a vertical plane control model and a horizontal plane control model while neglecting coupling therebetween;
step 3: discretizing data of a vertical plane control model and a horizontal plane control model, carrying out real-time correction on the system according to the deviation of an AUV path tracking target quantity and a predicted value by using a model prediction control algorithm, and outputting an optimal parameter of S-plane control;
step 4: outputting the thrust of the propeller by adopting an improved S-plane control algorithm, and performing motion control on the AUV to complete recovery three-dimensional path tracking of the AUV;
the improved S-plane control algorithm comprises the following steps:
step 1): the performance index of the applied model predictive control algorithm is a performance index which comprehensively reflects the rolling time domain optimization;
wherein: t is the prediction period, mu 1 、μ 3 Respectively represent the weight value occupied by the constraint and control input of the middle end in the output in the performance index J, mu 2 Represents the weight of the tracking error in the performance index,for the system in a certain time domain tau epsilon [0, T]Prediction output of intra->For T in a certain time domain tau epsilon [0, T]The desired output within, i.e., the reference output;
step 2): outputting control quantity by adopting an improved S-plane control algorithm, and performing motion control on the AUV to complete recovery three-dimensional path tracking of the AUV;
the control model of the S-plane controller is as follows:
wherein k is 1 And k 2 To control the coefficients, the analogy can be to PD coefficients in a PID controller, deltau is an adjustment term, which can be considered as a fixed disturbance force over a period of time, e andfor controlling the input information e is depth and heading angle error information +.>U is the control output, which is the depth and heading angle error rate, and is considered in the AUV to be the thrust and torque of the corresponding propeller;
combining the characteristics of PID control, designing an integral term for the Deltau term of the S-plane controller, wherein a control model is as follows:
i.e. whenOr->At the time of S-plane control, when +.>Or e (t) =0, the S-plane is not integrated, and k3 is an adjustable control parameter of the integral term.
2. The control method of a full-drive autonomous underwater robot recovery three-dimensional path tracking control system according to claim 1, wherein constructing an AUV mathematical model of step 1 includes the steps of:
step 1: first two coordinate systems are established: the geodetic coordinate system and the motion coordinate system, the kinematic equation of the AUV takes the floating center as the origin of the motion coordinate system, and the following equation is obtained:
wherein: x, Y and Z are AUV in motion coordinatesTying the forces in three axial directions; k, M and N are moments of the AUV in three axial directions on a motion coordinate system; m is the mass of the AUV body; x is x G ，y G ，z G Coordinates of the gravity center of the AUV; i x ，I y ，I z Moment of inertia of the AUV in 3 coordinate axes respectively; u, v, w are components of AUV speed in 3 coordinate axes of the carrier coordinate system; p, q, 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, and first, a simplified AUV vertical plane motion equation is obtained, wherein y g For 0, the z-axis float equation is:
Z uq uq+Z uw uw+Z prop +Fi (2)
constant speedFor 0, the y-axis trim equation is:
M uq uq+M uw uw+M prop +Fi (3)
in the above formula: f (F) i Is Gaussian white noise; z is Z w|w| ，Z q|q| The vertical dynamic coefficients of the speed and the angular speed on a motion coordinate system are respectively Z uq ，Z uw The vertical dynamic coefficient components of the speed and the angular speed on a motion coordinate system are respectively M w|w| ，M q|q| Pitch power coefficients of speed and angular speed on a motion coordinate system respectively,respectively a plurality of matrix functionsFirst-order partial derivative of w and q components in pitch plane, M uq ，M uw The pitch dynamics coefficient components of the speed and the angular speed in the motion coordinate system, respectively, +.>Respectively a plurality of matrix functionsFirst partial derivatives of w and q components in a vertical plane; z is Z g Is stress in the z-axis direction; m is M g Is gravity; z is Z prop ，M prop Thrust force in the z-axis direction and thrust moment in the y-axis direction respectively; x is x g ，z g A barycentric coordinate;
then substituting the model parameters into a vertical plane control model to obtain a z-axis floating equation:
wherein: the theta is the longitudinal inclination angle, the vertical inclination angle is the same as the vertical inclination angle,is a roll angle;
the y-axis pitch motion equation is:
if the depth of the AUV navigation 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 relation of the AUV in the inertial coordinate system in the horizontal plane can be expressed as follows:
wherein the geodetic coordinate system takes a point E of a horizontal plane as an origin, a zeta axis points to the north direction of the geography, a eta axis points to the east direction of the geography, zeta axes point to the earth center respectively, and psi is a course angle,is the integral of displacement of the xi axis, +.>Is the integral of the displacement of the zeta axis, +.>Integrating the heading angle rotation quantity;
where w=0, p=0, q=0, y g Under the condition of=0, the kinematic equation of the horizontal plane AUV is first simplified, namely
The x-axis forward and backward kinematic equation is:
the translational kinematic equation in the y-axis direction is:
the z-axis bow-turning kinematic equation is:
wherein X is u|u| For the axial power coefficient of the velocity in the motion coordinate system,as a multiple matrix functionFirst-order partial derivative of middle component u in axial direction, X vr ，X rr The axial dynamic coefficient components of the speed and the angular speed on a motion coordinate system are respectively, Y v|v| For the lateral dynamic coefficient of speed on the motion coordinate system, +.>For a multiple matrix function->First-order partial derivative of the mid-component v in lateral direction, Y vr ，Y uv The lateral dynamic coefficient components of the velocity and the angular velocity in the motion coordinate system respectively, N v|v| ，N r|r| The yaw dynamic coefficients of speed and angular speed on the motion coordinate system are respectively +.>For a multiple matrix function->First order partial derivative of middle component r in heading, N ur ，N uv The power coefficient components of the bow turning of the speed and the angular speed on a motion coordinate system are respectively, X prop ，Y prop ，N prop Respectively axial, lateral and vertical pushing moments;
then, AUV model parameters are substituted into a horizontal plane control model, and an x-axis forward and backward motion equation is obtained as follows:
∑X＝6sinθ+X prop +Fi+-10.050u|u|-146.848wq+-12.816q 2 +146.848vr-12.816r 2 (10)
the translational motion equation in the y-axis direction is:
the z-axis bow-turning motion equation is:
CN202010988593.2A 2020-09-18 2020-09-18 System and method for controlling recovery three-dimensional path tracking of full-drive autonomous underwater robot Active CN112148022B (en)

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 CN112148022A (en) 2020-12-29
CN112148022B true 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)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
CN113050666B (en) * 2021-03-26 2022-05-03 湖南大学 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
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
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

Citations (3)

* Cited by examiner, † Cited by third party
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

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10737781B2 (en) * 2017-09-14 2020-08-11 Drone Racing League, Inc. Three-dimensional pathway tracking system

Patent Citations (3)

* Cited by examiner, † Cited by third party
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

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
全驱动型AUV三维路径跟踪控制系统设计及分析;姚金艺等;《中国舰船研究》;20191231(第06期);全文 *
小型多用途自主式无人水下航行器MT-2010;任奕;《水雷战与舰船防护》;20150215(第01期);全文 *
自主水下机器人回收路径跟踪控制研究;姚金艺;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》;20200315;第I140-326页 *

Also Published As

Publication number Publication date
CN112148022A (en) 2020-12-29

Similar Documents

Publication Publication Date Title
CN112148022B (en) System and method for controlling recovery three-dimensional path tracking of full-drive autonomous underwater robot
CN107957727B (en) Underwater robot control system and dynamic positioning method
Fossen et al. Line-of-sight path following of underactuated marine craft
Abreu et al. The MEDUSA class of autonomous marine vehicles and their role in EU projects
CN108267953B (en) Underwater robot position tracking method based on navigator-tracker
CN113110495B (en) Formation control method of mobile robots under consideration of external interference
CN109131801A (en) The voluntary recall device and control method of the complete deep unmanned submersible in sea
CN105425812B (en) Unmanned aerial vehicle automatic landing trajectory control method based on dual models
CN113419428B (en) Machine/ship cooperative path tracking controller design method based on 3D mapping guidance
CN111208845A (en) Underwater robot mobile docking recovery path planning method and device
CN108519736A (en) A kind of USV track followings contragradience sliding-mode control
CN101419464A (en) Unmanned submersible depth-keeping navigation control method by employing vector thruster
CN103955224B (en) Attitude control method for relative motion visual line tracking
CN111055282A (en) Self-stabilization auxiliary structure for repairing and manufacturing parts in marine bumpy environment
Bibuli et al. Line following guidance control: Application to the Charlie unmanned surface vehicle
CN110244754B (en) Control system and method for fixed-point air parking of stratosphere aerostat
CN114089634B (en) Underwater robot control method based on multi-motor main shaft and slave shaft cooperative algorithm
CN114967714A (en) Anti-interference motion control method and system for autonomous underwater robot
CN112578814B (en) Linear track tracking control method for formation of multiple autonomous underwater vehicles
CN113296505A (en) Unmanned ship multi-mode path tracking control method based on speed change LOS
CN117389312A (en) Model-based three-dimensional tracking control method for counter roll of underwater vehicle
CN109752957B (en) Guidance instruction regulator structure of unmanned ship and design method
CN114706413B (en) Near-earth orbit micro-nano satellite centroid-changing gesture control method and system
CN114527772A (en) Design method and system of AUV trajectory tracking controller
CN113269100A (en) Vision-based aircraft offshore platform landing flight visual simulation system and method

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