Underwater vehicle attitude regulation method and system based on disturbance force and disturbance torque

Abstract

This invention discloses an attitude control method and system for underwater vehicles based on disturbance forces and moments, belonging to the field of ship motion control technology. The invention includes the following steps: establishing a kinematic model of the underwater vehicle; constructing a state observer to estimate unknown disturbance forces and moments; calculating a compensating pitch angle to generate compensating lift and a compensating rudder angle to generate compensating moment based on the estimated unknown disturbance forces; combining the attitude tracking control rudder angle with the compensating rudder angle to generate the final control rudder angle command, and sending it to the servo actuator. This invention counteracts the continuous disturbances generated by disturbance forces and moments through compensating pitch angle and compensating rudder angle, eliminating the need for complex "adversarial" operations by the operator. The input commands are directly used for the vehicle's maneuver control, making operation more intuitive and simpler, and greatly reducing the workload of personnel.

Description

Technical Field

[0001] This invention belongs to the field of ship motion control technology, specifically relating to an attitude control method and system for underwater vehicles based on disturbance forces and disturbance torques. Background Technology

[0002] Underwater vehicles, such as autonomous underwater vehicles (AUVs) and remotely operated underwater vehicles (ROVs), play a crucial role in marine resource exploration, underwater operations, and scientific research. The vehicle's attitude stability, including depth, navigation, and pitch, is a key factor in its successful mission execution. Traditional underwater vehicles primarily alter their attitude through two control methods: one uses hydrodynamic forces generated by rudders and stern rudders to adjust attitude, and the other uses buoyancy adjustment systems to change buoyancy and the position of the center of buoyancy, thereby generating corresponding forces and torques. However, existing technologies suffer from significant drawbacks, particularly poor adaptability to disturbances / torques. Traditional PID controllers struggle to effectively suppress these persistent disturbances of unknown magnitude, leading to unstable attitudes and requiring operators to constantly manually adjust rudder angles to counteract the disturbances, significantly increasing the operational burden. Furthermore, systems based on internal load adjustment typically have slow response times (on the order of minutes), making them unable to handle sudden changes in disturbances / torques, potentially causing the vehicle to lose attitude control within a short period, posing a serious safety hazard. When the disturbance force / moment exceeds the compensation capacity of the control surfaces or load adjustment system, the aircraft may experience dangerous situations such as "depth drop" or "loss of control upon ascent." Blindly operating the aircraft without knowing the specific magnitude of the disturbance force / moment can easily lead to misjudgment and operational errors. Therefore, there is an urgent need for a new attitude adjustment technology that can quickly and accurately estimate and actively compensate for disturbance forces / moments, thereby improving the robustness of aircraft attitude control, reducing the workload of operators, and enhancing safety. Summary of the Invention

[0003] This invention addresses the problems of poor adaptability to disturbance forces and moments, heavy maneuvering burden, slow response, and insufficient safety in existing underwater vehicle attitude control technologies. It proposes an underwater vehicle attitude control method based on disturbance forces and moments that enables rapid and stable control of the vehicle's attitude and depth. One of the above objectives of this invention is achieved through the following technical solution: An attitude control method for underwater vehicles based on disturbance forces and moments includes the following steps: Step 1: Establish a longitudinal motion dynamics model of the underwater vehicle and obtain the vehicle's real-time depth, attitude, speed information and control input information; Step 2: Construct a state observer and estimate in real time the unknown disturbance torque caused by mass imbalance and the unknown disturbance force caused by buoyancy imbalance based on the real-time information and control input information obtained in Step 1. Step 3: Based on the unknown disturbance force estimated in Step 2, calculate a compensating pitch angle to generate compensating lift and a compensating rudder angle to generate compensating torque. Step 4: Based on the compensated pitch angle calculated in Step 3, calculate the attitude tracking control rudder angle, and superimpose the attitude tracking control rudder angle with the compensated rudder angle calculated in Step 3 to form the final control rudder angle command, which is then sent to the servo actuator.

[0004] Furthermore, in step 3, the compensation pitch angle The calculation formula is: in, The lift coefficient, With speed It is proportional to the square of the value, obtained through fluid dynamics calculations or system identification; Uneven forces, This is a real-time estimate of the lumped disturbance caused by buoyancy imbalance.

[0005] Furthermore, in step 3, the rudder angle is... The calculation formula is: in, This is the hydrodynamic coefficient related to the elevator angle in the pitch direction; The unknown disturbance torque is caused by mass imbalance; This is a real-time estimate of the lumped disturbance caused by mass imbalance.

[0006] Furthermore, step 4 includes: Step 4.1: Calculate the attitude tracking control rudder angle: The operator's desired pitch angle With compensated pitch angle The summation forms the overall expected pitch angle. The attitude regulator adjusts the pitch angle based on the current attitude and the desired pitch angle. The error is used to generate the attitude tracking control rudder angle. Step 4.2: Summate the attitude tracking control rudder angle and the compensation rudder angle algebraically: This allows us to obtain the final control rudder angle.

[0007] One of the above-mentioned objectives of the present invention is achieved by the following technical solution: An underwater vehicle attitude control system based on disturbance forces and moments, used to implement the aforementioned underwater vehicle attitude control method based on disturbance forces and moments, includes a sensor module, a controller module, and an actuator module; the controller module embeds a state observer and a compensation command generation module; the state observer uses the depth z and pitch angle θ measured by the sensors as inputs to estimate the lumped disturbance in real time. and The estimated value and The controller module combines compensation commands with the operator's intention commands to synthesize the final control commands.

[0008] The advantages and positive effects of this invention are as follows: 1. The method of this invention automatically counteracts the continuous disturbances caused by the disturbance force / torque by compensating for the pitch angle and rudder angle, eliminating the need for operators to perform complex "counter-attack" operations. The input commands are directly used for the aircraft's maneuver control, making operation more intuitive and simpler, and greatly reducing the workload of personnel.

[0009] 2. This invention is based on an electronic observer and control law, with an extremely fast response speed (millisecond level). The system can also quickly follow and compensate for dynamically changing disturbances, ensuring the attitude and depth stability and dynamic performance of the vehicle under complex operating conditions. Attached Figure Description

[0010] Figure 1 This is a schematic diagram of the control flow of the underwater vehicle attitude control method of the present invention. Detailed Implementation

[0011] The structure of the present invention will be further described below with reference to the accompanying drawings and embodiments. It should be noted that these embodiments are descriptive and not limiting.

[0012] An attitude control method for underwater vehicles based on disturbance forces and disturbance torques includes the following steps: Step 1: Establish a longitudinal motion dynamics model for the underwater vehicle. The longitudinal motion dynamics model selects the vehicle's depth z, depth rate w, pitch angle θ, and pitch angular velocity q as core state variables, and considers the unknown disturbance torque generated by mass imbalance and the unknown disturbance force generated by buoyancy imbalance as the total disturbance of the underwater vehicle.

[0013] Step 2: Design an Extended State Observer (ESO). Based on the longitudinal motion dynamics model established in Step 1, an extended state observer (ESO) is designed. This ESO utilizes information such as depth, attitude angles, and angular velocities measured by the vehicle's sensors, as well as the current rudder angle control input, to estimate in real time the unknown disturbance torque caused by mass imbalance. and the unknown disturbance force caused by buoyancy imbalance .

[0014] Step 3: Generate the compensated pitch angle and generating compensation rudder angle : The unbalanced disturbance force estimated by the state observer A desired compensating pitch angle is calculated. This pitch angle aims to generate a hydrodynamic lift equal in magnitude and opposite in direction to the unbalanced force through the angle of attack effect of the vehicle in the water, thereby balancing the vertical forces on the vehicle and maintaining depth stability. This compensating pitch angle is inversely proportional to the square of the vehicle speed.

[0015] The unbalanced disturbance moment estimated by the state observer And combined with the means to compensate for the pitch angle The required attitude adjustment torque is used to calculate a "compensation rudder angle". The function of this compensation rudder angle is to generate a control torque to counteract the unbalanced torque and drive the aircraft to quickly reach and maintain the compensated pitch angle.

[0016] Step 4 The externally input maneuvering control angle is superimposed with the compensation control angle to form the final control angle command, which is then sent to the servo actuator.

[0017] Furthermore, the Extended State Observer (ESO) treats the unknown disturbances of the system (including unbalanced torques and unbalanced forces) as new state variables and estimates them in real time, which has the advantages of not relying on an accurate model and being robust.

[0018] The coupled dynamics model of the vertical plane motion of the aircraft can be simplified as follows: (1) in, , These represent the aircraft's mass and moment of inertia about the Y-axis, respectively; w, These are depth rate and pitch angular velocity, respectively. , These are vertical acceleration and tilt acceleration, respectively. The forward speed of the aircraft; , , , These are the hydrodynamic coefficients related to vertical velocity, pitch rate, pitch angle, and elevator angle in the depth direction, respectively. , , , These are the hydrodynamic coefficients related to vertical velocity, pitch angular velocity, pitch angle, and elevator angle in the pitch direction, respectively. To raise or lower the rudder angle; This refers to the unknown disturbance force generated by the imbalance of buoyancy; This refers to the unknown disturbance torque generated by mass imbalance.

[0019] The compensated pitch angle The calculation principle is as follows: the lift it generates needs to balance the unbalanced forces. : (2) Among them, the lift coefficient With speed The square of the angle is approximately proportional to the pitch angle.

[0020] The compensated rudder angle The calculation principle is as follows: the torque it generates needs to balance the unbalanced torques. : (3) This invention also discloses an attitude adjustment system for implementing the above method. The attitude adjustment system includes a sensor module, a controller module, and an actuator module. The controller module embeds a state observer and a compensation command generation module.

[0021] The control flow of this invention embodiment is as follows: S01: System initialization, the controller module obtains the initial state information of the vehicle from the sensor module.

[0022] S02: In each control cycle, the controller module acquires real-time sensor data and the control rudder angle δe(k-1) of the previous cycle. The sensor data includes depth z, pitch angle θ, speed u, etc.

[0023] S03: State Observer Operation. Taking vertical plane motion as an example, an observer model is established that incorporates unbalanced forces and moments as extended states. The disturbance forces generated by buoyancy imbalance are... and the disturbance torque caused by mass imbalance After appropriate transformation, it is regarded as a lumped disturbance of the system. and This is taken as the extended state. Based on the aircraft dynamics model, a high-order extended state observer (ESO) is constructed, using the depth z and pitch angle θ measured by sensors as inputs to estimate the lumped disturbance in real time. and The estimated value and .

[0024] S04: The compensation instruction generation module calculates the compensation pitch angle. and compensating rudder angle .

[0025] Calculate the compensated pitch angle This pitch angle is used to balance the vertical force generated by buoyancy imbalance (corresponding to disturbance estimate zf1). The resulting lift L must counteract this disturbance force. The lift is proportional to the square of the speed and the pitch angle. Therefore, the compensating pitch angle can be obtained: The lift coefficient With speed The square of the sum of the pitch angle and the inclination angle are approximately proportional and can be obtained through fluid dynamics calculations or system identification.

[0026] Calculate the compensation rudder angle This rudder angle is used to balance the imbalance caused by mass (corresponding to disturbance estimation). The resulting disturbance torque. The resulting control torque. The disturbance torque needs to be counteracted: in These are the hydrodynamic coefficients related to the elevator angle. From this, the compensated rudder angle can be obtained: .

[0027] S05: Synthesize final control command: The controller module combines the compensation command with the operator's intention command.

[0028] Attitude control loop: This loop controls the operator's desired pitch angle. (Or derived from depth commands) and compensating pitch angle The summation forms the overall expected pitch angle. A typical attitude controller (such as a PID controller) adjusts the current attitude relative to the given attitude. The error is used to generate a rudder angle for attitude tracking. .

[0029] Final rudder angle synthesis: The final rudder angle output to the actuator is the algebraic sum of the attitude tracking rudder angle and the compensation rudder angle. The physical meaning of this operation is: the system through It directly offset the unbalanced moment, while adjusting the desired pitch angle (by adding...) This indirectly balances the unbalanced forces, allowing the attitude controller to... It only needs to track the dynamic process of the desired attitude, which greatly improves control performance and robustness.

[0030] S06: The actuator module drives the elevator to deflect to the target angle. This completes one closed-loop control cycle. The system executes S02 to S06 repeatedly to achieve continuous and stable control of attitude and depth.

[0031] Example: Imagine an underwater vehicle navigating at a constant depth when its forward payload compartment suddenly floods. This not only causes the center of gravity to shift forward, generating a "nose-down" moment... Furthermore, the increased overall weight disrupts neutral buoyancy, generating a downward disturbance force. After applying this invention, the extended state observer immediately estimates the disturbance torque zf2 and disturbance force zf1. The compensation command generation module 22 then calculates: 1) a positive compensation rudder angle. 1) Used to directly generate a head-up moment to counteract the "head-down" tendency; 2) A compensating pitch angle This serves as a new attitude equilibrium point, allowing the vehicle to navigate with a slight pitch, thereby utilizing the continuous lift generated by the vehicle itself to counteract the increased weight. Ultimately, the system automatically stabilizes the vehicle at a new equilibrium state, resuming constant depth navigation. The operator experiences this process almost imperceptibly, fundamentally improving both the handling experience and safety.

[0032] Although embodiments and drawings of the present invention have been disclosed for illustrative purposes, those skilled in the art will understand that various substitutions, variations and modifications are possible without departing from the spirit and scope of the present invention and the appended claims. Therefore, the scope of the present invention is not limited to the contents disclosed in the embodiments and drawings.

Claims

1. A method for attitude control of an underwater vehicle based on disturbance force and torque, characterized in that: Includes the following steps: Step 1: Establish a longitudinal motion dynamics model of the underwater vehicle and obtain the vehicle's real-time depth, attitude, speed information and control input information; Step 2: Construct a state observer and estimate in real time the unknown disturbance torque caused by mass imbalance and the unknown disturbance force caused by buoyancy imbalance based on the real-time information and control input information obtained in Step 1. Step 3: Based on the unknown disturbance force estimated in Step 2, calculate a compensating pitch angle to generate compensating lift and a compensating rudder angle to generate compensating torque. Step 4: Based on the compensated pitch angle calculated in Step 3, calculate the attitude tracking control rudder angle, and superimpose the attitude tracking control rudder angle with the compensated rudder angle calculated in Step 3 to form the final control rudder angle command, which is then sent to the servo actuator.

2. The underwater vehicle attitude control method based on disturbance force and torque according to claim 1, characterized in that: In step 3, the compensated pitch angle The calculation formula is: in, The lift coefficient, With speed It is proportional to the square of the value, obtained through fluid dynamics calculations or system identification; Uneven forces, This is a real-time estimate of the lumped disturbance caused by buoyancy imbalance.

3. The underwater vehicle attitude control method based on disturbance force and torque according to claim 1, characterized in that, In step 3, the rudder angle is... The calculation formula is: in, This is the hydrodynamic coefficient related to the elevator angle in the pitch direction; The unknown disturbance torque is caused by mass imbalance; This is a real-time estimate of the lumped disturbance caused by mass imbalance.

4. The underwater vehicle attitude control method based on disturbance force and torque according to claim 1, characterized in that, Step 4 includes: Step 4.1: Calculate the attitude tracking control rudder angle: The operator's desired pitch angle With compensated pitch angle The summation forms the overall expected pitch angle. The attitude regulator adjusts the pitch angle based on the current attitude and the desired pitch angle. The error is used to generate the attitude tracking control rudder angle. Step 4.2: Summate the attitude tracking control rudder angle and the compensation rudder angle algebraically: This allows us to obtain the final control rudder angle.

5. An underwater vehicle attitude control system based on disturbance forces and torques, used to implement the underwater vehicle attitude control method based on disturbance forces and torques as described in any one of claims 1-4, characterized in that: It includes a sensor module, a controller module, and an actuator module; the controller module embeds a state observer and a compensation command generation module; the state observer uses the depth z and pitch angle θ measured by the sensor as input to estimate the lumped disturbance in real time. and The estimated value and The controller module combines compensation commands with the operator's intention commands to synthesize the final control commands.

Images