UUV control method based on feedforward PID

Abstract

The invention relates to a UUV (Unmanned Underwater Vehicle) control method based on a feed-forward PID (Proportion Integration Differentiation). The objective of the invention is to solve the problems of poor precision and poor stability of a traditional PID control algorithm in UUV control. The method comprises the following steps: 1, setting an initial position and a target position of the UUV in an inertial coordinate system; 2, real-time sea wave interference information and the real state of the UUV are obtained; 3, outputting angular acceleration by the PID attitude controller; 4, outputting the optimized depth information by the filter; 5, outputting linear acceleration by the PID position controller; 6, the angular acceleration and the linear acceleration act on an executing mechanism, the thrust and the torque of a propeller are changed by controlling the rotating speed of the executing mechanism, the thrust and the torque act on the UUV, and the underwater movement track of the UUV is changed; 7, judging whether the UUV reaches the target position or not; if yes, the UUV stops sailing; and if not, repeatedly executing the steps 2-6 until the UUV reaches the target position. The method is applied to the field of UUV control.

Description

Technical Field

[0001] The invention relates to a UUV control method based on feedforward PID. Background Art

[0002] An underwater unmanned vehicle is an intelligent system that is unmanned and navigates underwater through autonomous control or remote control. Against the backdrop of the rapid development of emerging technologies, people are increasingly interested in the exploration and development of marine resources, and underwater unmanned vehicles play an important role in this. When UUVs operate in water, their stability and maneuverability are seriously threatened due to the huge uncertainty of ocean conditions such as waves and currents. This requires UUVs to have robust navigation and control capabilities to adapt to changes in dynamic environments. The key factors affecting the navigation stability of UUVs include attitude control, position control, and disturbance filtering. Attitude control is crucial to the stability of UUVs, including adjusting the pitch, roll, and yaw angles to maintain the desired orientation and heading. In addition, the position of the UUV in the water needs to be precisely controlled to ensure that it follows the predetermined trajectory. However, dynamic ocean environments such as waves can cause frequent attitude changes and position deviations, affecting navigation accuracy. Therefore, effectively filtering out disturbances caused by waves is crucial to improving navigation stability.

[0003] Establishing a reasonable mathematical model of space motion is the basis for studying UUV maneuverability and designing control systems. The document "Research on maneuverability and simulation of small autonomous underwater vehicle" (Journal of System Simulation, 2009, Vol. 21, No. 13), drawing on the research method of submarine maneuverability, constructed a nonlinear UUV space motion mathematical model and performed simulation prediction. The document "Six-degree-of-freedom underwater vehicle maneuverability simulation and performance evaluation" (Journal of Huazhong University of Science and Technology, 2015, Vol. 43, No. S1) used a six-degree-of-freedom model to complete the UUV maneuvering motion simulation and compared it with the KT model simulation results. However, the above models still need to be improved in terms of accuracy and applicability. In the present invention, the accuracy and adaptability of the model are further enhanced by introducing a fluid dynamics model and optimizing the dynamic parameters of the UUV.

[0004] Among the current UUV control methods, backstepping control (BC), model predictive control (MPC), neural network control, classical PID control, etc. are commonly used. Model predictive control originated from industrial process control and has inherent advantages in directly processing system internal constraints. It can effectively solve the thrust saturation problem caused by speed changes during tracking and has been widely used in trajectory tracking control. The document "Model Predictive Control for Autonomous Underwater Vehicle" (Indian Journal of Geo-marine Sciences, 2011, Vol. 40, No. 2) proves the feasibility and effectiveness of MPC in UUV trajectory tracking under the conditions of robustness and interference suppression. Furthermore, the document "QPSO-model Predictive Control-based Approach to Dynamic Trajectory Tracking Control for Unmanned Underwater Vehicles" (Ocean Engineering, 2018, Vol. 158, pp. 208-220) proposes an MPC method based on quantum particle swarm intelligent optimization to improve the accuracy of trajectory tracking. However, these methods do not consider the impact of actual tracking environment such as ocean currents.

[0005] Classical PID control has been widely used in the fields of motion and process control due to its simplicity, strong robustness, and high reliability. However, due to the drawbacks of the traditional PID tuning process, such as its complexity and inability to effectively handle nonlinear and uncertain systems, its control performance is suboptimal and its adaptability is poor. In response to these challenges, the present invention combines the structural analysis of UUV and proposes a dual closed-loop feedforward PID controller to solve the problems of slow response speed, poor control performance, and low adaptability of the traditional PID control algorithm in UUV control. In addition, the present invention proposes a feedforward strategy for adjusting the adaptive controller under nonlinear wave interference, which improves the control performance and adaptability of UUV in complex marine environments, and further improves the control accuracy and robustness of UUV.

[0006] UUVs operate in complex underwater environments and face many uncertainties and interferences. Precise control is the key to ensure that they can complete their tasks safely and efficiently. As an effective state estimation and prediction tool, the Kalman filter algorithm has been widely used in the field of UUV control. The paper "Consistent Extended Kalman Filter-Based Cooperative Localization of Multiple Autonomous Underwater Vehicles" (Sensors, 2022, Vol. 22, No. 12) considers the nonlinear characteristics of the UUV system and uses extended Kalman filtering for state estimation, solving the problem of inconsistent state estimation when multiple autonomous underwater vehicles are co-located. The paper "AFuzzy Kalman Filter Optimized Using a Genetic Algorithm for Accurate Navigation of an Autonomous Underwater Vehicle" (IFAC Proceedings Volumes, 2003, Vol. 36, No. 21) fuses data from various INS sensors through a linear Kalman filter (LKF), and then integrates it with GPS data through an extended Kalman filter (EKF), improving the overall accuracy of the UUV integrated navigation system. Summary of the invention

[0007] The purpose of the present invention is to solve the problems of poor accuracy and poor stability of traditional PID control algorithm in UUV control, and to propose a UUV control method based on feedforward PID.

[0008] A UUV control method based on feedforward PID has the following specific process:

[0009] Step 1: Set the starting position of the UUV and the target position of the UUV in the inertial coordinate system;

[0010] The target position of the UUV contains the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d ), and the expected attitude information of the UUV in the inertial coordinate system

[0011] Among them, x d is the x of UUV in the inertial coordinate system e Axis expected position information, y d is the y of UUV in the inertial coordinate system e Axis expected position information, z dis the z of UUV in the inertial coordinate system e Axis expected position information, is the desired roll angle of the UUV, θ d is the desired pitch angle of the UUV, ψ d is the desired yaw angle of the UUV;

[0012] Step 2: The sensors configured on the UUV obtain real-time sea wave interference information and the real status of the UUV.

[0013] Among them, (x s ,y s ,z s ) is the real position information of UUV in the inertial coordinate system; is the real attitude information of UUV in the inertial coordinate system;

[0014] x s is the x of UUV in the inertial coordinate system e Axis real position information, y s is the y of UUV in the inertial coordinate system e Axis real position information, z s is the z of UUV in the inertial coordinate system e Axis real position information, is the real roll angle of the UUV, θ s is the real pitch angle of UUV, ψ s is the real yaw angle of UUV;

[0015] The sensors configured in the UUV include a posture sensor, a position sensor and a filter;

[0016] The attitude sensor obtains the real attitude information of the UUV

[0017] The position sensor obtains the real position information of the UUV

[0018] The filter obtains real-time sea wave interference information d;

[0019] Step 3: The real attitude information of the UUV obtained by the attitude sensor and the desired attitude information of the UUV Input PID attitude controller, PID attitude controller outputs three-axis angular acceleration in inertial coordinate system

[0020] in, is the x of the UUV in the inertial coordinate system e Axis angular acceleration; is the y of the UUV in the inertial coordinate system e Axis angular acceleration; is the z of the UUV in the inertial coordinate system e Axis angular acceleration;

[0021] Step 4: The real-time wave interference information d obtained by the filter and the real-time UUV z in the inertial coordinate system are combined. e Axis real position information z s Input filter, filter output optimized depth information

[0022] Step 5: Use the real position information of the UUV obtained by the position sensor and filter and the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d ) Input PID position controller, PID position controller outputs three-axis acceleration in inertial coordinate system

[0023] in, is the x of the UUV in the inertial coordinate system e Axis acceleration; is the y of the UUV in the inertial coordinate system e Axis acceleration; is the z of the UUV in the inertial coordinate system e Axis acceleration;

[0024] Step 6: Three-axis angular acceleration in the inertial coordinate system output by the PID attitude controller And the three-axis acceleration in the inertial coordinate system output by the PID position controller Acting on the actuator, the thrust and torque τ of the propeller are changed by controlling the rotation speed of the actuator. The thrust and torque τ act on the UUV to change the motion trajectory of the UUV underwater.

[0025] The thrust and torque τ act on the UUV to change the motion trajectory of the UUV underwater. The specific process is:

[0026] Substitute the thrust and torque into the system dynamics model to calculate the linear velocity and angular velocity of the UUV in the carrier coordinate system; change the motion trajectory of the UUV underwater based on the linear velocity and angular velocity of the UUV;

[0027] Step 7: Determine whether the UUV has reached the target position of the UUV;

[0028] If so, the UUV ends its voyage;

[0029] If not, repeat steps 2 to 6 until the UUV reaches the target position of the UUV;

[0030] The target position of the UUV contains the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d ), and the expected attitude information of the UUV in the inertial coordinate system

[0031] The beneficial effects of the present invention are:

[0032] The purpose of the present invention is to improve the navigation stability of UUV in a dynamic ocean environment through a UUV control method based on feedforward PID.

[0033] In the present invention, an integral Kalman filter (IKF) is used to estimate the wave height, and the nonlinear characteristics of the system are processed by introducing an integral state variable. The IKF decomposes the system state into a linear part and a nonlinear part. It uses the Kalman filter to estimate the state and covariance of the linear part, and uses the integral state variable to estimate the influence of the nonlinear part. The present invention improves the stability and control accuracy of the UUV in a dynamic marine environment by introducing random changes in wave height and random disturbances in wave parameters.

[0034] The present invention discloses a feedforward PID control method for an unmanned underwater vehicle (UUV) based on Kalman filtering. The method first optimizes the depth information of the UUV under the interference of the dynamic ocean environment through Kalman filtering, effectively reducing the influence of measurement noise and interference on the system. Then, the wave interference feedforward control link is introduced, and combined with the traditional PID control, the response speed and control accuracy of the UUV control system are significantly improved. Considering the nonlinearity and strong coupling of the UUV system, the present invention adopts a double closed-loop control method to design the UUV attitude control inner loop and the position control outer loop. The feedforward-feedback composite control method is adopted, and the control parameters are monitored and adjusted in real time, so as to adapt to the needs of the UUV in complex and changeable underwater environments and diversified tasks. The present invention realizes the precise control of the UUV in a dynamic ocean environment, and enhances the robustness, stability and control accuracy of the UUV control system. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 This is the structural block diagram of the UUV feedforward PID control system;

[0036] Figure 2 This is the module diagram of the UUV control system;

[0037] Figure 3 This is the schematic diagram of the Kalman filter;

[0038] Figure 4This is the PID control principle diagram;

[0039] Figure 5 is the attitude response curve of the UUV system, Roll Step Response is the roll step response, Pitch Step Response is the pitch step response, Yaw Step Response is the bow step response, response curve is the response curve, and target curve is the target curve;

[0040] Figure 6 is the position response curve of the UUV system, x response curve is the x-axis coordinate response curve, y response curve is the y-axis coordinate response curve, and z response curve is the z-axis coordinate response curve;

[0041] Figure 7 It is the trajectory tracking curve diagram of the UUV system, xresponse curve is the x-axis coordinate response curve, yresponse curve is the y-axis coordinate response curve, and z response curve is the z-axis coordinate response curve. DETAILED DESCRIPTION

[0042] Specific implementation method 1: The specific process of a UUV control method based on feedforward PID in this implementation method is as follows:

[0043] Step 1: Set the starting position of the UUV and the target position of the UUV in the inertial coordinate system;

[0044] The target position of the UUV contains the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d ), and the expected attitude information of the UUV in the inertial coordinate system

[0045] Among them, x d is the x of UUV in the inertial coordinate system e Axis expected position information, y d is the y of UUV in the inertial coordinate system e Axis expected position information, z d is the z of UUV in the inertial coordinate system e Axis expected position information, is the desired roll angle of the UUV, θ d is the desired pitch angle of the UUV, ψ d is the desired yaw angle of the UUV;

[0046] Step 2: During navigation, the sensors configured on the UUV obtain real-time sea wave interference information and the real status of the UUV.

[0047] Among them, (x s ,y s ,z s ) is the real position information of UUV in the inertial coordinate system; is the real attitude information of UUV in the inertial coordinate system;

[0048] x s is the x of UUV in the inertial coordinate system e Axis real position information, y s is the y of UUV in the inertial coordinate system e Axis real position information, z s is the z of UUV in the inertial coordinate system e Axis real position information, is the real roll angle of the UUV, θ s is the actual pitch angle of the UUV, ψ s is the real yaw angle of the UUV;

[0049] The sensors configured in the UUV include a posture sensor, a position sensor and a filter;

[0050] The attitude sensor obtains the real attitude information of the UUV

[0051] The position sensor obtains the real position information of the UUV (x s ,y s ,z s );

[0052] The filter obtains real-time sea wave interference information d;

[0053] Step 3: The real attitude information of the UUV obtained by the attitude sensor and the desired attitude information of the UUV Input PID attitude controller, PID attitude controller outputs three-axis angular acceleration in inertial coordinate system

[0054] in, is the x of the UUV in the inertial coordinate system e Axis angular acceleration; is the y of the UUV in the inertial coordinate system e Axis angular acceleration; is the z of the UUV in the inertial coordinate system e Axis angular acceleration;

[0055] Step 4: The real-time wave interference information d obtained by the filter and the real-time UUV z in the inertial coordinate system are combined. e Axis real position information z s Input filter, filter output optimized depth information

[0056] Step 5: Use the real position information of the UUV obtained by the position sensor and filter and the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d ) Input PID position controller, PID position controller outputs three-axis acceleration in inertial coordinate system

[0057] in, is the x of the UUV in the inertial coordinate system e Axis acceleration; is the y of the UUV in the inertial coordinate system e Axis acceleration; is the z of the UUV in the inertial coordinate system e Axis acceleration;

[0058] Step 6: Three-axis angular acceleration in the inertial coordinate system output by the PID attitude controller And the three-axis acceleration in the inertial coordinate system output by the PID position controller Acting on the actuator, the thrust and torque τ of the propeller are changed by controlling the rotation speed of the actuator. The thrust and torque τ act on the UUV to change the motion trajectory of the UUV underwater.

[0059] The thrust and torque τ act on the UUV to change the motion trajectory of the UUV underwater. The specific process is:

[0060] Substitute the thrust and torque into the system dynamics model to calculate the linear velocity and angular velocity of the UUV in the carrier coordinate system; change the motion trajectory of the UUV underwater based on the linear velocity and angular velocity of the UUV;

[0061] Step 7: Determine whether the UUV has reached the target position of the UUV;

[0062] If so, the UUV ends its voyage;

[0063] If not, repeat steps 2 to 6 until the UUV reaches the target position of the UUV;

[0064] The target position of the UUV contains the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d), and the expected attitude information of the UUV in the inertial coordinate system

[0065] Specific implementation method 2: This implementation method is different from the specific implementation method 1 in that the inertial coordinate system in step 1 is O e -x e y e z e , the origin of the coordinate system O e is the deployment point of the UUV (fixed), x e The y axis points north. e The axis points east, z e The axis points toward the center of the Earth.

[0066] The other steps and parameters are the same as those in the first embodiment.

[0067] Specific implementation method three: This implementation method is different from specific implementation method one or two in that the carrier coordinate system in step six is ​​O b -x b y b z b , the origin of the coordinate system O b is the UUV center of mass, x b The y axis points to the UUV's forward direction. b The axis points to the right of the UUV's forward direction, z b The axis points directly below the UUV.

[0068] The other steps and parameters are the same as those in the first or second embodiment.

[0069] Specific implementation method 4: This implementation method is different from any one of the specific implementation methods 1 to 3 in that in step 3, the real posture information of the UUV obtained by the posture sensor is and the desired attitude information of the UUV Input PID attitude controller, PID attitude controller outputs three-axis angular acceleration in inertial coordinate system

[0070] in, is the x of the UUV in the inertial coordinate system e Axis angular acceleration; is the y of the UUV in the inertial coordinate system e Axis angular acceleration; is the z of the UUV in the inertial coordinate system e Axis angular acceleration;

[0071] The specific process is:

[0072]

[0073] Among them, Kp is the proportional controller coefficient of the PID controller, K i is the integral controller coefficient of the PID controller, K d is the coefficient of the differential controller of the PID controller, and t is the time.

[0074] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0075] Specific implementation method 5: This implementation method is different from any one of the specific implementation methods 1 to 4 in that in step 4, the real-time sea wave interference information d obtained by the filter and the real-time UUV z in the inertial coordinate system are combined. e Axis real position information z s Input filter, filter output optimized depth information

[0076] The specific process is:

[0077] Combined with Figure 3 It can be obtained that the specific steps of Kalman filtering are:

[0078] (1) State prediction:

[0079]

[0080]

[0081] in, is the predicted state at time k, is the posterior state covariance at time k-1;

[0082] F k is the state transfer matrix at time k, G k is the control input matrix at time k;

[0083] x k-1 is the posterior state at time k-1, u k-1 is the input state at time k-1;

[0084] Q k is the process noise covariance at time k, that is, the real-time wave interference information d obtained by the filter;

[0085] The superscript T means to find the transpose;

[0086] (2) Measurement update:

[0087]

[0088]

[0089]

[0090] Among them, K k is the Kalman gain at time k;

[0091] H k is the observation matrix at time k;

[0092] R k is the measurement noise covariance at time k;

[0093] is the estimated state at time k, that is, the depth information after the filter output is optimized

[0094] z k is the measurement state at time k, i.e., the real-time z of the UUV in the inertial coordinate system e Axis real position information z s ;

[0095] is the posterior state covariance at time k;

[0096] I is the identity matrix.

[0097] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 5.

[0098] Specific implementation method 6: This implementation method is different from any one of the specific implementation methods 1 to 5 in that in step 5, the real position information of the UUV obtained by the position sensor and the filter is and the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d ) Input PID position controller, PID position controller outputs three-axis acceleration in inertial coordinate system

[0099] in, is the x of the UUV in the inertial coordinate system e Axis acceleration; is the y of the UUV in the inertial coordinate system e Axis acceleration; is the z of the UUV in the inertial coordinate system e Axis acceleration;

[0100] The specific process is:

[0101]

[0102] Among them, K p is the proportional controller coefficient of the PID controller, K i is the integral controller coefficient of the PID controller, Kd is the coefficient of the differential controller of the PID controller, and t is the time.

[0103] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 5.

[0104] Specific implementation method 7: This implementation method is different from any one of specific implementation methods 1 to 6 in that the thrust and torque τ of the propeller in step 6 = (X, Y, Z, K, M, N);

[0105] Where τ is the thrust and torque, X is the carrier coordinate system x b Thrust on the axis, Y is the carrier coordinate system y b Thrust on the axis, Z is the carrier coordinate system z b The thrust on the axis, K is the carrier coordinate system x b The moment on the axis, M is the carrier coordinate system y b The moment on the axis, N is the carrier coordinate system z b Torque on the shaft.

[0106] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0107] Specific implementation eight: This implementation differs from any one of the specific implementations one to seven in that the system dynamics model in step six

[0108] Substitute thrust and torque τ=(X,Y,Z,K,M,N) into the system dynamics model Calculate the linear velocity and angular velocity V = (u, v, w, p, q, r) of the UUV in the carrier coordinate system;

[0109] in, is the inertia matrix, C (V) is the Coriolis matrix, D (V) is the damping matrix, g (η) for resilience;

[0110] V is the linear velocity and angular velocity in the carrier coordinate system;

[0111] &

[0112] V is the first derivative of V;

[0113] u is the carrier coordinate system x b Linear velocity on the axis, v is the carrier coordinate system y b Linear velocity on the axis, w is the carrier coordinate system z b Linear speed on the axis;

[0114] pCarrier coordinate system x b The angular velocity on the axis, q is the carrier coordinate system y bThe angular velocity on the axis, r is the carrier coordinate system z b Angular velocity about the axis;

[0115] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.

[0116] Specific implementation method 9: This implementation method is different from any one of specific implementation methods 1 to 8 in that the actuators in step 6 are four motors.

[0117] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0118] In a dynamic ocean environment, considering the influence of wave height on the depth position of UUV, the measurement information z is filtered by wave interference. s Filter to obtain optimized depth information Introducing a wave disturbance feedforward control loop to further improve the control accuracy of the UUV;

[0119] The UUV feedforward PID control method, whose control process is divided into three parts: wave interference feedforward control, PID attitude control and PID position control, is a composite control method of feedforward-feedback. Due to the strong coupling between the six degrees of freedom of the UUV, changes in the attitude of the UUV will cause changes in its position, so the control of the UUV is designed as a double closed loop, one is the inner loop of attitude control, and the other is the outer loop of position control. At the same time, considering the interference of waves on the depth information of the UUV, a feedforward control loop is introduced to further improve the control accuracy and stability of the UUV. The UUV feedforward PID control method designs a wave interference filter through the Kalman filter algorithm to estimate the influence of wave height on the depth meter measurement information, thereby optimizing the depth information of the UUV;

[0120] PID control module: The UUV controller designed by the present invention adopts a double closed-loop PID controller, with the attitude control loop as the inner loop and the position control loop as the outer loop. The position control loop is used to control the UUV to move along a specific trajectory. Based on the dynamic model of the UUV, the roll angle and pitch angle required for the UUV movement are calculated and used as the input of the attitude loop. The attitude loop can enable the UUV to quickly reach the desired state, thereby realizing precise control of the UUV. Figure 4 , further explain the PID control principle;

[0121] Execution module: The output of the PID controller is the three-axis acceleration and three-axis angular acceleration in the inertial coordinate system. By controlling the speed of the four motors, the thrust and torque generated by the four thrusters are changed to control the movement of the UUV.

[0122] The following examples are used to verify the beneficial effects of the present invention:

[0123] Embodiment 1:

[0124] Embodiments of the present invention are described below.

[0125] Example 1: Posture Response

[0126] In order to verify the effectiveness of the method proposed in this invention, Figure 1 The control system structure block diagram is given and verified by simulation experiments. In this example, the expected angles of the three Euler angles are set to 30°, and the attitude response curves of the three axes are obtained, as shown in the attached figure. Figure 5 shown.

[0127] Depend on Figure 5 It can be seen that the controller designed by the present invention can realize the posture control of UUV, the adjustment time is less than 5s, and the system steady-state error is very small, which can meet the actual needs. However, the system overshoot is large, which is also a problem to be solved in future research.

[0128] Example 2: Position Response

[0129] In this simulation experiment, the expected position of the UUV is set to (-1,-1,-1). The simulation results are shown in the attached figure. Figure 6 As shown. Figure 6 It can be seen that the UUV can reach the set value relatively stably. In particular, since the wave interference filter is set to optimize the depth information, the control accuracy of the system in the depth direction is improved, making the UUV control system have a certain robustness.

[0130] Example 3: Trajectory tracking

[0131] In this example, the three-axis desired trajectories in the inertial coordinate system are set as Attached Figure 7 The trajectory tracking curve of the UUV under the action of the controller designed by the present invention is shown. It can be seen that the UUV has good trajectory tracking ability in water, which further proves the effectiveness and feasibility of the algorithm of the present invention.

[0132] The present invention may also have many other embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art may make various corresponding changes and modifications based on the present invention, but these corresponding changes and modifications should all fall within the scope of protection of the claims attached to the present invention.

Claims

1. A UUV control method based on feedforward PID, characterized in that: The specific process of the method is: Step 1: Set the starting position of the UUV and the target position of the UUV in the inertial coordinate system; The target position of the UUV contains the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d ), and the expected attitude information of the UUV in the inertial coordinate system Among them, x d is the x of UUV in the inertial coordinate system e Axis expected position information, y d is the y of UUV in the inertial coordinate system e Axis expected position information, z d is the z of UUV in the inertial coordinate system e Axis expected position information, is the desired roll angle of the UUV, θ d is the desired pitch angle of the UUV, ψ d is the desired yaw angle of the UUV; Step 2: The sensors configured on the UUV obtain real-time sea wave interference information and the real status of the UUV. Among them, (x s ,y s ,z s ) is the real position information of UUV in the inertial coordinate system; is the real attitude information of UUV in the inertial coordinate system; x s is the x of UUV in the inertial coordinate system e Axis real position information, y s is the y of UUV in the inertial coordinate system e Axis real position information, z s is the z of UUV in the inertial coordinate system e Axis real position information, is the real roll angle of the UUV, θ s is the actual pitch angle of the UUV, ψ s is the real yaw angle of the UUV; The sensors configured in the UUV include a posture sensor, a position sensor and a filter; The attitude sensor obtains the real attitude information of the UUV The position sensor obtains the real position information of the UUV (x s ,y s ,z s ); The filter obtains real-time sea wave interference information d; Step 3: The real attitude information of the UUV obtained by the attitude sensor and the desired attitude information of the UUV Input PID attitude controller, PID attitude controller outputs three-axis angular acceleration in inertial coordinate system in, is the x of the UUV in the inertial coordinate system e Axis angular acceleration; is the y of the UUV in the inertial coordinate system e Axis angular acceleration; is the z of the UUV in the inertial coordinate system e Axis angular acceleration; Step 4: The real-time wave interference information d obtained by the filter and the real-time UUV z in the inertial coordinate system are combined. e Axis real position information z s Input filter, filter output optimized depth information Step 5: Use the real position information of the UUV obtained by the position sensor and filter and the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d ) Input PID position controller, PID position controller outputs three-axis acceleration in inertial coordinate system in, is the x of the UUV in the inertial coordinate system e Axis acceleration; is the y of the UUV in the inertial coordinate system e Axis acceleration; is the z of the UUV in the inertial coordinate system e Axis acceleration; Step 6: Three-axis angular acceleration in the inertial coordinate system output by the PID attitude controller And the three-axis acceleration in the inertial coordinate system output by the PID position controller Acting on the actuator, the thrust and torque τ of the propeller are changed by controlling the rotation speed of the actuator. The thrust and torque τ act on the UUV to change the motion trajectory of the UUV underwater. The thrust and torque τ act on the UUV to change the motion trajectory of the UUV underwater. The specific process is: Substitute the thrust and torque into the system dynamics model to calculate the linear velocity and angular velocity of the UUV in the carrier coordinate system; change the motion trajectory of the UUV underwater based on the linear velocity and angular velocity of the UUV; Step 7: Determine whether the UUV has reached the target position of the UUV; If so, the UUV ends its voyage; If not, repeat steps 2 to 6 until the UUV reaches the target position of the UUV; The target position of the UUV contains the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d ), and the expected attitude information of the UUV in the inertial coordinate system 2. A UUV control method based on feedforward PID according to claim 1, characterized in that: The inertial coordinate system in step 1 is O e -x e y e z e , the origin of the coordinate system O e is the deployment point of the UUV (fixed), x e The y axis points north. e The axis points east, z e The axis points toward the center of the Earth.

3. A UUV control method based on feedforward PID according to claim 2, characterized in that: The carrier coordinate system in step 6 is O b -x b y b z b , the origin of the coordinate system O b is the UUV center of mass, x b The y axis points to the UUV's forward direction. b The axis points to the right of the UUV's forward direction, z b The axis points directly below the UUV.

4. A UUV control method based on feedforward PID according to claim 3, characterized in that: In step 3, the real posture information of the UUV obtained by the posture sensor and the desired attitude information of the UUV Input PID attitude controller, PID attitude controller outputs three-axis angular acceleration in inertial coordinate system in, is the x of the UUV in the inertial coordinate system e Axis angular acceleration; is the y of the UUV in the inertial coordinate system e Axis angular acceleration; is the z of the UUV in the inertial coordinate system e Axis angular acceleration; The specific process is: Among them, K p is the proportional controller coefficient of the PID controller, K i is the integral controller coefficient of the PID controller, K d is the coefficient of the differential controller of the PID controller, and t is the time.

5. A UUV control method based on feedforward PID according to claim 4, characterized in that: In step 4, the real-time sea wave interference information d obtained by the filter and the real-time UUV z in the inertial coordinate system are combined. e Axis real position information z s Input filter, filter output optimized depth information The specific process is: (1) State prediction: in, is the predicted state at time k, is the posterior state covariance at time k-1; F k is the state transfer matrix at time k, G k is the control input matrix at time k; x k-1 is the posterior state at time k-1, u k-1 is the input state at time k-1; Q k is the process noise covariance at time k, that is, the real-time wave interference information d obtained by the filter; The superscript T means to find the transpose; (2) Measurement update: Among them, K k is the Kalman gain at time k; H k is the observation matrix at time k; R k is the measurement noise covariance at time k; is the estimated state at time k, that is, the depth information after the filter output is optimized z k is the measurement state at time k, i.e., the real-time z of the UUV in the inertial coordinate system e Axis real position information z s ; is the posterior state covariance at time k; I is the identity matrix.

6. A UUV control method based on feedforward PID according to claim 5, characterized in that: In step 5, the real position information of the UUV obtained by the position sensor and the filter and the expected position information of the UUV in the inertial coordinate system (x d ,y d ,z d ) Input PID position controller, PID position controller outputs three-axis acceleration in inertial coordinate system in, is the x of the UUV in the inertial coordinate system e Axis acceleration; is the y of the UUV in the inertial coordinate system e Axis acceleration; is the z of the UUV in the inertial coordinate system e Axis acceleration; The specific process is: Among them, K p is the proportional controller coefficient of the PID controller, K i is the integral controller coefficient of the PID controller, K d is the coefficient of the differential controller of the PID controller, and t is the time.

7. A UUV control method based on feedforward PID according to claim 6, characterized in that: The thrust and torque τ of the propeller in step 6 = (X, Y, Z, K, M, N); Where τ is the thrust and torque, X is the carrier coordinate system x b Thrust on the axis, Y is the carrier coordinate system y b Thrust on the axis, Z is the carrier coordinate system z b The thrust on the axis, K is the carrier coordinate system x b The moment on the axis, M is the carrier coordinate system y b The moment on the axis, N is the carrier coordinate system z b Torque on the shaft.

8. A UUV control method based on feedforward PID according to claim 7, characterized in that: The system dynamics model in step 6 Substitute thrust and torque τ=(X,Y,Z,K,M,N) into the system dynamics model Calculate the linear velocity and angular velocity V = (u, v, w, p, q, r) of the UUV in the carrier coordinate system; in, is the inertia matrix, C (V) is the Coriolis matrix, D (V) is the damping matrix, g (η) for resilience; V is the linear velocity and angular velocity in the carrier coordinate system; is the first-order derivative of V; u is the carrier coordinate system x b Linear velocity on the axis, v is the carrier coordinate system y b Linear velocity on the axis, w is the carrier coordinate system z b Linear speed on the axis; pCarrier coordinate system x b The angular velocity on the axis, q is the carrier coordinate system y b The angular velocity on the axis, r is the carrier coordinate system z b Angular velocity about the axis.

9. A UUV control method based on feedforward PID according to claim 8, characterized in that: The actuators in step six are four motors.

Images