A UUV Control Method Based on Feedforward PID

Abstract

A UUV control method based on feedforward PID, the present invention relates to a UUV control method based on feedforward PID. The purpose of the present invention is to solve the problems of poor accuracy and poor stability existing in the traditional PID control algorithm in UUV control. The process is as follows: First, set the starting position and target position of the UUV in the inertial coordinate system; Second, obtain the real-time wave interference information and the real state of the UUV; Third, the PID attitude controller outputs the angular acceleration; Fourth, the filter outputs the optimized depth information; Fifth, the PID position controller outputs the linear acceleration; Sixth, apply the angular acceleration and linear acceleration to the actuator, and change the thrust and torque of the thruster by controlling the rotation speed of the actuator. The thrust and torque act on the UUV to change the movement trajectory of the UUV underwater; Seventh, judge whether the UUV has reached the target position; if so, the UUV ends the navigation; if not, repeat steps two to six until the UUV reaches the target position. The present invention is used in the field of UUV control.

Description

Technical Field

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

[0002] An underwater unmanned vehicle (UUV) is an intelligent system that operates underwater without a crew and is controlled autonomously or remotely. In the context of the rapid development of emerging technologies, people's interest in the exploration and development of marine resources has been increasing, and UUVs play an important role in this process. When a UUV operates in water, its stability and maneuverability are severely threatened due to the great uncertainties of ocean conditions such as waves and ocean currents. This makes it necessary for UUVs to have the ability of robust navigation and control to adapt to the changes in the dynamic environment. The key factors affecting the navigation stability of UUVs include: attitude control, position control, and disturbance filtering. Attitude control is crucial for the stability of UUVs, including adjusting the pitch angle, roll angle, and yaw angle to maintain the required orientation and heading. In addition, precise control of the position of the UUV in water is also required to ensure that it follows a predetermined trajectory. However, dynamic ocean environments such as waves can cause frequent attitude changes and position deviations, affecting the navigation accuracy. Therefore, effectively filtering out the disturbances caused by waves is crucial for improving the navigation stability.

[0003] Establishing a reasonable mathematical model of spatial motion is the basis for studying the maneuverability of UUVs and designing control systems. The literature "Research on maneuverability and simulation of small autonomous underwater vehicle" (Journal of System Simulation, 2009, Vol. 21, No. 13) constructed a nonlinear mathematical model of UUV spatial motion and carried out simulation predictions by referring to the research methods of submarine maneuverability. The literature "Six-degree-of-freedom underwater vehicle maneuverability simulation and performance evaluation" (Journal of Huazhong University of Science and Technology, 2015, Vol. 43, No. S1) completed the maneuvering motion simulation of UUVs using a six-degree-of-freedom model and compared the simulation results with those of the K-T model. 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 hydrodynamic model and optimizing the dynamic parameters of the UUV.

[0004] In current UUV control methods, commonly used ones include Backstepping Control (BC), Model Predictive Control (MPC), neural network control, classical PID control, etc. Model Predictive Control originated from industrial process control and has innate advantages in directly dealing with internal system constraints. It can effectively solve the problem of thrust saturation caused by speed changes during the tracking process and has been widely applied in trajectory tracking control. The literature "Model Predictive Control for Autonomous Underwater Vehicle" (Indian Journal of Geo-marine Sciences, 2011, Vol. 40, No. 2) proved the feasibility and effectiveness of MPC in UUV trajectory tracking under the conditions of considering robustness and disturbance rejection. Further, the literature "QPSO-model Predictive Control-based Approach to Dynamic Trajectory Tracking Control for Unmanned Underwater Vehicles" (Ocean Engineering, 2018, Vol. 158, pp. 208-220) proposed an MPC method based on quantum particle swarm intelligent optimization, improving the accuracy of trajectory tracking. However, these methods do not consider the influence of actual tracking environments such as ocean currents.

[0005] Classical PID control has been widely applied in the fields of motion and process control due to its advantages such as simplicity, strong robustness, and high reliability. However, due to the complex tuning process of traditional PID and its inability to effectively handle nonlinear and uncertain systems, its control performance is suboptimal and its adaptability is poor. To address these challenges, the present invention combines the structural analysis of UUVs and proposes a double-closed-loop feedforward PID controller to solve the problems existing in the traditional PID control algorithm for UUV control, such as slow response speed, poor control performance, and low adaptability. In addition, the present invention proposes a feedforward strategy for adaptive controller adjustment under non-linear ocean wave disturbances, improving the control performance and adaptability of UUVs in complex ocean environments and further enhancing the control accuracy and robustness of UUVs.

[0006] Unmanned Underwater Vehicles (UUVs) operate in complex underwater environments, facing numerous uncertainties and interferences. Precise control is the key to ensuring their safe and efficient mission completion. As an effective state estimation and prediction tool, the Kalman filter algorithm has been widely applied in the field of UUV control. The literature "Consistent Extended Kalman Filter-Based Cooperative Localization of Multiple Autonomous Underwater Vehicles" (Sensors, 2022, Vol. 22, No. 12) considered the non-linear characteristics of the UUV system and used the extended Kalman filter for state estimation, solving the problem of inconsistent state estimation during the cooperative localization of multiple autonomous underwater vehicles. The literature "A Fuzzy Kalman Filter Optimized Using a Genetic Algorithm for Accurate Navigation of an Autonomous Underwater Vehicle" (IFAC Proceedings Volumes, 2003, Vol. 36, No. 21) fused data from various INS sensors through linear Kalman filter (LKF), and then integrated it with GPS data through extended Kalman filter (EKF), improving the overall accuracy of the UUV integrated navigation system. Summary of the Invention

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

[0008] The specific process of a UUV control method based on feed-forward PID is as follows:

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

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

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

[0012] Step 2: The sensors configured on the UUV obtain the real-time ocean wave interference information d and the real state of the UUV

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

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

[0015] The sensors configured on the UUV include an attitude sensor, a position sensor, and a filter;

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

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

[0018] where the filter obtains the real-time ocean wave interference information d;

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

[0020] where, is the angular acceleration of the UUV's x-axis in the inertial coordinate system e axis; is the angular acceleration of the y-axis of the UUV in the inertial coordinate system; e axis angular acceleration; is the angular acceleration of the z-axis of the UUV in the inertial coordinate system; e axis angular acceleration;

[0021] Step 4: Input the real-time wave interference information d obtained by the filter and the real-time z-axis true position information z of the UUV in the inertial coordinate system into the filter, and the filter outputs the optimized depth information e axis true position information z s into the filter, and the filter outputs the optimized depth information

[0022] Step 5: Input the true position information of the UUV obtained by the position sensor and the filter and the desired position information (x , y d , z d , z d ) of the UUV in the inertial coordinate system into the PID position controller, and the PID position controller outputs the three-axis linear accelerations in the inertial coordinate system

[0023] wherein, is the x-axis linear acceleration of the UUV in the inertial coordinate system; e axis linear acceleration; is the y-axis linear acceleration of the UUV in the inertial coordinate system; e axis linear acceleration; is the z-axis linear acceleration of the UUV in the inertial coordinate system; e axis linear acceleration;

[0024] Step 6: The three-axis angular accelerations in the inertial coordinate system output by the PID attitude controller and the three-axis linear accelerations in the inertial coordinate system output by the PID position controller act on the actuator. By controlling the rotational speed of the actuator, the thrust and torque τ of the thruster are changed. The thrust and torque τ act on the UUV to change the movement trajectory of the UUV underwater;

[0025] The thrust and torque τ act on the UUV to change the movement trajectory of the UUV underwater; the specific process is as follows:

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

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

[0028] If so, the UUV ends navigation;

[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 (x d , y d , z d ) of the UUV in the inertial coordinate system, as well as the expected attitude information of the UUV in the inertial coordinate system

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

[0032] The purpose of the present invention is to improve the navigation stability of the 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 integral state variables. 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 at the same time uses the integral state variables to estimate the influence of the nonlinear part. The present invention improves the stability and control accuracy of the UUV in a dynamic ocean environment by introducing random variations in wave height and random perturbations in wave parameters.

[0034] The present invention discloses a feedforward PID control method for an unmanned underwater vehicle (UUV) based on Kalman filtering. This method first optimizes the UUV depth information under the interference of a dynamic ocean environment through Kalman filtering, effectively reducing the influence of measurement noise and interference on the system. Then, a feedforward control link for wave interference is introduced and combined with traditional PID control, significantly improving the response speed and control accuracy of the UUV control system. Considering the nonlinearity and strong coupling of the UUV system, the present invention adopts a double-closed-loop control method, designing an inner loop for UUV attitude control and an outer loop for position control. Adopting a feedforward-feedback composite control method, by real-time monitoring and adjusting control parameters, it can meet the requirements of the UUV in complex and variable underwater environments and diverse tasks. The present invention realizes precise control of the UUV in a dynamic ocean environment, enhancing the robustness, stability, and control accuracy of the UUV control system. Description of the Drawings

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

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

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

[0038] Figure 4It is the schematic diagram of PID control;

[0039] Figure 5 It is the attitude response curve diagram 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 yaw step response, response curve is the response curve, and target curve is the target curve;

[0040] Figure 6 It is the position response curve diagram 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. 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. Specific implementation manners

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

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

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

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

[0046] Step 2: During navigation, the sensors equipped on the UUV obtain real-time ocean wave interference information d and the true state of the UUV

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

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

[0049] The sensors equipped on the UUV include an attitude sensor, a position sensor, and a filter;

[0050] where the attitude sensor obtains the true attitude information of the UUV

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

[0052] where the filter obtains the real-time ocean wave interference information d;

[0053] Step 3: Input the true attitude information of the UUV obtained by the attitude sensor and the desired attitude information of the UUV into the PID attitude controller, and the PID attitude controller outputs the three-axis angular accelerations

[0054] in the inertial coordinate system. Among them, is the angular acceleration of the UUV's x e axis in the inertial coordinate system; is the angular acceleration of the UUV's y e axis in the inertial coordinate system; is the angular acceleration of the UUV's z e axis in the inertial coordinate system;

[0055] Step 4: Input the real-time ocean wave interference information d obtained by the filter and the real z-axis position information z of the UUV in the inertial coordinate system into the filter, and the filter outputs the optimized depth information. e axis real position information z s into the filter, and the filter outputs the optimized depth information

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

[0057] Among them, is the x-axis linear acceleration of the UUV in the inertial coordinate system; e axis linear acceleration; is the y-axis linear acceleration of the UUV in the inertial coordinate system; e axis linear acceleration; is the z-axis linear acceleration of the UUV in the inertial coordinate system; e axis linear acceleration;

[0058] Step 6: The three-axis angular accelerations in the inertial coordinate system output by the PID attitude controller and the three-axis linear accelerations in the inertial coordinate system output by the PID position controller act on the actuator. By controlling the rotational speed of the actuator, the thrust and torque τ of the thruster are changed. The thrust and torque τ act on the UUV to change the underwater movement trajectory of the UUV. and the three-axis linear accelerations in the inertial coordinate system output by the PID position controller act on the actuator. By controlling the rotational speed of the actuator, the thrust and torque τ of the thruster are changed. The thrust and torque τ act on the UUV to change the underwater movement trajectory of the UUV. The specific process is as follows:

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

[0060] Substitute the thrust and torque into the system dynamics model to calculate the linear velocity and angular velocity of the UUV in the body coordinate system. Based on the linear velocity and angular velocity of the UUV, change the underwater movement trajectory of the UUV.

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

[0062] If so, the UUV ends navigation.

[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 includes the desired position information (x d , y d , z d), and the desired attitude information of the UUV in the inertial coordinate system

[0065] Specific Embodiment 2: The difference between this embodiment and Specific Embodiment 1 is that in step 1, the inertial coordinate system is O e -x e y e z e , and the coordinate origin O e is the deployment point of the UUV (fixed), the x e axis points north, the y e axis points east, and the z e axis points towards the center of the earth.

[0066] Other steps and parameters are the same as those in Specific Embodiment 1.

[0067] Specific Embodiment 3: The difference between this embodiment and Specific Embodiment 1 or 2 is that in step 6, the body coordinate system is O b -x b y b z b , and the coordinate origin O b is the centroid of the UUV, the x b axis points in the forward direction of the UUV, the y b axis points directly to the right of the forward direction of the UUV, and the z b axis points directly below the UUV.

[0068] Other steps and parameters are the same as those in Specific Embodiment 1 or 2.

[0069] Specific Embodiment 4: The difference between this embodiment and any one of Specific Embodiments 1 to 3 is that in step 3, the actual attitude information of the UUV obtained by the attitude sensor and the desired attitude information of the UUV are input into the PID attitude controller, and the PID attitude controller outputs the three-axis angular accelerations in the inertial coordinate system

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

[0071] The specific process is:

[0072]

[0073] where, 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 derivative controller of the PID controller, and t is the time.

[0074] Other steps and parameters are the same as those in any one of the specific embodiments 1 to 3.

[0075] Specific embodiment 5: The difference between this embodiment and any one of the specific embodiments 1 to 4 is that in step 4, the real-time ocean wave interference information d obtained by the filter and the real-time z-axis true position information z of the UUV in the inertial coordinate system e are input into the filter, and the filter outputs the optimized depth information s The specific process is as follows:

[0076] Specifically:

[0077] Combined with the attached Figure 3 It can be obtained that the specific steps of the Kalman filter are as follows:

[0078] (1) State prediction:

[0079]

[0080]

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

[0082] F k is the state transition 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 ocean wave interference information d obtained by the filter;

[0085] The superscript T represents taking 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 optimized depth information output by the filter

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

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

[0096] I is the identity matrix.

[0097] Other steps and parameters are the same as those in any one of the specific embodiments 1 to 5.

[0098] Specific embodiment 6: The difference between this embodiment and any one of the specific embodiments 1 to 5 is that in step 5, the real position information of the UUV obtained by the position sensor and the filter and the expected position information (x d , y d , z d ) of the UUV in the inertial coordinate system are input into the PID position controller, and the PID position controller outputs the three-axis accelerations in the inertial coordinate system

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

[0100] The specific process is as follows:

[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 derivative controller of the PID controller, and t is the time.

[0103] Other steps and parameters are the same as those in any one of the specific embodiments one to five.

[0104] Specific embodiment seven: The difference between this embodiment and any one of the specific embodiments one to six is that the thrust and moment of the thruster in step six are τ=(X, Y, Z, K, M, N);

[0105] where τ is the thrust and moment, X is the thrust on the x-axis of the body coordinate system b axis, Y is the thrust on the y-axis of the body coordinate system b axis, Z is the thrust on the z-axis of the body coordinate system b and K is the moment on the x-axis of the body coordinate system b axis, M is the moment on the y-axis of the body coordinate system b axis, and N is the moment on the z-axis of the body coordinate system b axis. b

[0106] Other steps and parameters are the same as those in any one of the specific embodiments one to six.

[0107] Specific embodiment eight: The difference between this embodiment and any one of the specific embodiments one to seven is that the system dynamics model in step six

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

[0109] where is the inertia matrix, C (V) is the Coriolis matrix, D (V) is the damping matrix, and g (η) is the restoring force;

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

[0111] &

[0112] V is the first derivative of V;

[0113] u is the linear velocity on the x-axis of the body coordinate system b axis, v is the linear velocity on the y-axis of the body coordinate system b axis, and w is the linear velocity on the z-axis of the body coordinate system b axis;

[0114] p is the angular velocity on the x-axis of the body coordinate system b axis, q is the angular velocity on the y-axis of the body coordinate system bThe angular velocity on the axis, and r is the z-axis of the carrier coordinate system b The angular velocity on the z-axis;

[0115] Other steps and parameters are the same as those in any one of the first to seventh specific embodiments.

[0116] Specific Embodiment Nine: The difference between this embodiment and any one of the first to eighth specific embodiments is that in step six, the actuator is four motors.

[0117] Other steps and parameters are the same as those in any one of the first to eighth specific embodiments.

[0118] In a dynamic ocean environment, considering the influence of the wave height of ocean waves on the depth position of the UUV, the measured information z s is filtered through a wave interference filter to obtain the optimized depth information A wave interference feedforward control loop is introduced 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, belongs to a feedforward-feedback composite control method. Due to the strong coupling between the six degrees of freedom of the UUV, a change in the UUV's attitude will cause a change in its position. Therefore, the control of the UUV is designed as a double closed-loop, one is the attitude control inner loop, and the other is the position control outer loop. At the same time, considering the interference of ocean 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 the wave height on the measurement information of the depth gauge, thereby optimizing the depth information of the UUV;

[0120] PID control module: The UUV controller designed in 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 function of the position control loop is 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's 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 the precise control of the UUV. Combining with the attached Figure 4 , the PID control principle is further explained;

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

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

[0123] Embodiment One:

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

[0125] Example 1: Attitude response

[0126] To verify the effectiveness of the method proposed by the present invention, combined with the Figure 1 control system block diagram given in the appendix, simulation experiments are used for verification. In this example, the desired 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 appendix Figure 5 shown.

[0127] As can be seen from Figure 5 this, the controller designed by the present invention can achieve the attitude control of the UUV, with the adjustment time less than 5 s, and the steady-state error of the system is very small, which can meet the actual requirements. However, the overshoot of the system is relatively large, which is also a problem to be solved in future research.

[0128] Example 2: Position response

[0129] In this example, the simulation experiment sets the desired position of the UUV to (-1, -1, -1), and the simulation results are as shown in the appendix Figure 6 shown. As can be seen from Figure 6 this, the UUV can reach the set value relatively stably. In particular, due to the setting of the wave interference filter 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 desired trajectories of the three axes in the inertial coordinate system are respectively set as The appendix Figure 7 shows the UUV trajectory tracking curve under the action of the controller designed by the present invention. It can be seen that the UUV has good trajectory tracking ability in water, further proving the effectiveness and feasibility of the algorithm of the present invention.

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

Claims

1. A UUV control method based on feedforward PID, characterized in that: The specific process of the method is as follows: Step 1: Set the starting position of the UUV in the inertial coordinate system and 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 ), as well as the expected attitude information of the UUV in the inertial coordinate system where x d is the desired position information of the x-axis of the UUV in the inertial coordinate system, y e is the desired position information of the y-axis of the UUV in the inertial coordinate system, z d is the desired position information of the z-axis of the UUV in the inertial coordinate system, e d e is the desired position information of the z-axis of the UUV in the inertial coordinate system, e 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; d Step 2: The sensors configured on the UUV obtain real-time wave interference information d and the true state of the UUV Among them, (x s , y s , z s ) is the true position information of the UUV in the inertial coordinate system; is the true attitude information of the UUV in the inertial coordinate system; x s The true position information of the x-axis of the UUV in the inertial coordinate system, e y s is the true position information of the y-axis of the UUV in the inertial coordinate system, e z s is the true position information of the z-axis of the UUV in the inertial coordinate system, e and is the true roll angle of the UUV, θ s is the true pitch angle of the UUV, ψ s is the true yaw angle of the UUV; The sensors configured on the UUV include an attitude sensor, a position sensor, and a filter; Among them, the attitude sensor obtains the true attitude information of the UUV where the position sensor obtains the true position information (x s , y s , z s ) of the UUV; Among them, the filter obtains real-time ocean wave interference information d; Step 3: Input the true attitude information of the UUV obtained by the attitude sensor and the desired attitude information of the UUV into the PID attitude controller, and the PID attitude controller outputs the three-axis angular acceleration in the inertial coordinate system Among them, is the angular acceleration of the UUV's x-axis in the inertial coordinate system; e axis angular acceleration; is the angular acceleration of the UUV's y-axis in the inertial coordinate system; e axis angular acceleration; is the angular acceleration of the UUV's z-axis in the inertial coordinate system; e axis angular acceleration; Step 4: Input the real-time ocean wave interference information d obtained by the filter and the real z-axis position information z of the UUV in the inertial coordinate system into the filter, and the filter outputs the optimized depth information e axis real position information z s into the filter, and the filter outputs the optimized depth information Step 5: Input the true position information of the UUV obtained by the position sensor and the filter and the desired position information (x d , y d , z d ) of the UUV in the inertial coordinate system into the PID position controller, and the PID position controller outputs the three-axis accelerations in the inertial coordinate system Among them, is the x e axis acceleration of the UUV in the inertial coordinate system; is the y e axis acceleration of the UUV in the inertial coordinate system; is the z e axis acceleration of the UUV in the inertial coordinate system; Step 6: The three-axis angular accelerations in the inertial coordinate system output by the PID attitude controller and the three-axis linear accelerations in the inertial coordinate system output by the PID position controller act on the actuator. By controlling the rotational speed of the actuator, the thrust and torque τ of the thruster are changed. The thrust and torque τ act on the UUV to change the movement trajectory of the UUV underwater; The thrust and moment τ act on the UUV to change the movement trajectory of the UUV underwater; the specific process is as follows: Substitute the thrust and moment into the system dynamics model to calculate the linear velocity and angular velocity of the UUV in the body coordinate system; change the movement 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 navigation; 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 desired position information of the UUV in the inertial coordinate system (x d , y d , z d ), and the desired attitude information of the UUV in the inertial coordinate system 2. The UUV control method based on feedforward PID according to claim 1, wherein: In the first step, the inertial coordinate system is O e -x e y e z e , and the origin O of the coordinate system e is the deployment point of the UUV. The x e axis points north, the y e axis points east, and the z e axis points towards the center of the earth.

3. A UUV control method based on feedforward PID according to claim 2, characterized in that: In step six, the body coordinate system is O b -x b y b z b , and the origin O of the coordinate system b is the centroid of the UUV. The x b axis points in the forward direction of the UUV, the y b axis points directly to the right of the forward direction of the UUV, and the z b 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 true attitude information of the UUV obtained by the attitude sensor and the desired attitude information of the UUV are input into the PID attitude controller, and the PID attitude controller outputs the three-axis angular acceleration in the inertial coordinate system wherein, is the x-axis angular acceleration of the UUV in the inertial coordinate system; e axis angular acceleration; is the y-axis angular acceleration of the UUV in the inertial coordinate system; e axis angular acceleration; is the z-axis angular acceleration of the UUV in the inertial coordinate system; e axis angular acceleration; The specific process is as follows: 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 derivative 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 the fourth step, the real-time ocean wave interference information d obtained by the filter and the real-time z-axis true position information z of the UUV in the inertial coordinate system e are input into the filter, and the filter outputs the optimized depth information s . The specific process is as follows: Specifically: (1) State prediction: Among them, is the predicted state at time k, is the posterior state covariance at time k-1; F k is the state transition matrix at time k, and 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 ocean wave interference information d obtained by the filter; The superscript T represents taking 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 instant k; is the estimated state at time k, i.e., the optimized depth information output by the filter z k is the measurement state at time k, i.e., the real position information z of the UUV in the inertial coordinate system along the z-axis e axis s ; is the posterior state covariance at time k; I is the identity matrix.

6. The UUV control method based on feedforward PID according to claim 5, characterized in that: In step five, the true position information of the UUV obtained by the position sensor and the filter and the desired position information (x d , y d , z d ) of the UUV in the inertial coordinate system are input into the PID position controller, and the PID position controller outputs the three-axis accelerations in the inertial coordinate system Wherein, is the x-axis acceleration of the UUV in the inertial coordinate system; e Axis acceleration; is the y-axis acceleration of the UUV in the inertial coordinate system; e Axis acceleration; is the z-axis acceleration of the UUV in the inertial coordinate system; e Axis acceleration; The specific process is as follows: 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 derivative controller of the PID controller, and t is time.

7. A UUV control method based on feedforward PID according to claim 6, characterized in that: In step 6, the thrust and moment τ of the thruster = (X, Y, Z, K, M, N); Among them, τ is the thrust and moment, X is the thrust on the x-axis of the vehicle coordinate system, Y is the thrust on the y-axis of the vehicle coordinate system, b Z is the thrust on the z-axis of the vehicle coordinate system, K is the moment on the x-axis of the vehicle coordinate system, b M is the moment on the y-axis of the vehicle coordinate system, N is the moment on the z-axis of the vehicle coordinate system. b Among them, τ is the thrust and moment, X is the thrust on the x-axis of the vehicle coordinate system, Y is the thrust on the y-axis of the vehicle coordinate system, b Z is the thrust on the z-axis of the vehicle coordinate system, K is the moment on the x-axis of the vehicle coordinate system, b M is the moment on the y-axis of the vehicle coordinate system, N is the moment on the z-axis of the vehicle coordinate system. b Axial moment.

8. A UUV control method based on feedforward PID according to claim 7, characterized in that: The system dynamics model in Step 6 Substitute the thrust and moment τ = (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 body coordinate system; wherein, is the inertia matrix, C (V) is the Coriolis matrix, D (V) is the damping matrix, g (η) is the restoring force; V is the linear velocity and angular velocity in the body coordinate system; The first derivative of V; u is the linear velocity on the x-axis of the carrier coordinate system, b v is the linear velocity on the y-axis of the carrier coordinate system, b w is the linear velocity on the z-axis of the carrier coordinate system; b ​ Angular velocity on the x-axis of the p body coordinate system, q is the angular velocity on the y-axis of the body coordinate system b , r is the angular velocity on the z-axis of the body coordinate system b . b ​ 9. A UUV control method based on feedforward PID according to claim 8, characterized in that: In step 6, the actuator is four motors.

Images