A rudder failure detection method, device, equipment and medium

Abstract

The application relates to the technical field of underwater vehicles and discloses a rudder machine fault detection method, device, equipment and medium, which are applied to an autonomous underwater vehicle, wherein the method comprises the following steps: acquiring current state data and expected state data of the autonomous underwater vehicle, and determining virtual instruction rudder angles in different directions based on the current state data and the expected state data; determining actual instruction rudder angles corresponding to the virtual instruction rudder angles; determining target residual values between the virtual instruction rudder angles and actual driving rudder angles in different directions, and determining fault conditions of a rudder machine based on the target residual values and a pre-set residual threshold value; and in the case that the rudder machine has a fault, determining a jamming angle corresponding to a fault rudder machine based on the actual instruction rudder angles and the target residual values. The technical scheme provided by the application can detect rudder machine faults in real time and accurately, and can improve detection precision and reliability.

Description

Technical Field

[0001] This application relates to the field of underwater vehicle technology, and in particular to a method, device, equipment and medium for detecting servo motor faults. Background Technology

[0002] The servo system of an autonomous underwater vehicle (AUV) is crucial for attitude and heading control, especially the X-rudder system, which enhances stability but also complicates fault detection. AUV servo systems are prone to failure in complex environments. Traditional hardware redundancy or sensor solutions are costly and have poor adaptability, while data-driven intelligent detection methods are unsuitable for real-time monitoring of AUVs due to computational complexity and latency issues.

[0003] Therefore, how to detect servo motor faults in real time and accurately, and improve the accuracy and reliability of detection, is a technical problem that urgently needs to be solved. Summary of the Invention

[0004] This application provides a servo motor fault detection method, device, equipment, and medium, which achieves the technical effect of real-time and accurate detection of servo motor faults and improves detection accuracy and reliability.

[0005] To achieve the above objectives, the main technical solutions adopted in this application include: In a first aspect, embodiments of this application provide a servo motor fault detection method, applied to an autonomous underwater vehicle, the method comprising: The current state data and expected state data of the autonomous underwater vehicle are acquired, and virtual command rudder angles in different directions are determined based on the current state data and the expected state data. Determine the actual command rudder angle corresponding to the virtual command rudder angle; Determine the target residual value between the virtual command rudder angle and the actual drive rudder angle in different directions, and determine the servo motor malfunction based on the target residual value and a preset residual threshold. In the event of a servo malfunction, the jamming angle corresponding to the malfunctioning servo is determined based on the actual commanded servo angle and the target residual value.

[0006] This embodiment provides a servo malfunction detection method. It acquires the current and desired state data of the aircraft and calculates virtual command rudder angles in different directions based on this data, providing initial commands for servo control. The virtual command rudder angles are converted into actual command rudder angles for more accurate servo adjustments. Next, a target residual value between the virtual command rudder angle and the actual rudder angle is calculated and compared with a preset residual threshold to determine if a servo malfunction exists. If a servo malfunction is detected, the jamming angle of the faulty servo is calculated based on the actual command rudder angle and the target residual value. This embodiment not only effectively detects servo malfunctions but also identifies and locates the specific servo experiencing a jamming fault and quantifies the real-time jamming angle of the faulty servo.

[0007] In one implementation, when the direction is vertical, the current state data includes the current vertical velocity, current pitch rate, current pitch angle, and current depth, and the desired state data includes the desired depth; the method for determining the virtual command rudder angle includes: Determine the depth difference between the current depth and the desired depth, and integrate the depth difference to obtain a depth integral term; determine a vertical control gain that matches the current vertical velocity, the current pitch angular velocity, the current pitch angle, the depth difference, and the depth integral term; Based on the vertical direction control gain, determine vertical target terms that match the current vertical velocity, the current pitch angular velocity, the current pitch angle, the depth difference, and the depth integral term, respectively. The virtual command rudder angle in the vertical direction is obtained by summing all the vertical target items.

[0008] This embodiment determines the depth difference between the current depth and the desired depth, and integrates this difference to obtain a depth integral term. This helps eliminate steady-state errors, ensuring that the AUV can stably maintain a position near the desired depth over a long period. Next, based on the current vertical velocity, pitch rate, pitch angle, depth difference, and depth integral term, corresponding control gains are determined. These gains can be adjusted according to different state variables to optimize the control effect. Subsequently, based on the obtained control gains, corresponding vertical target terms are calculated for each state variable. These vertical target terms are used to precisely adjust the vertical motion. Finally, all vertical target terms are weighted and summed to obtain a virtual command rudder angle in the vertical direction. This rudder angle serves as the control input to adjust the AUV's control surfaces, thereby achieving precise control of the AUV's depth.

[0009] In one implementation, when the direction is horizontal, the current state data includes the current lateral velocity, the current yaw rate, and the current yaw angle, and the desired state data includes the desired yaw angle; the method for determining the virtual command rudder angle includes: Determine the yaw angle difference between the current yaw angle and the desired yaw angle, and integrate the yaw angle difference to obtain the yaw angle integral term; Determine a horizontal control gain that matches the current lateral velocity, the current yaw rate, the yaw angle difference, and the yaw angle integral term; Based on the horizontal control gain, determine the horizontal target terms that match the current lateral velocity, the current yaw rate, the yaw angle difference, and the yaw angle integral term, respectively. Adding all the horizontal target items together yields the virtual command rudder angle in the horizontal direction.

[0010] This embodiment calculates and integrates the yaw angle difference between the current and desired yaw angles to obtain the yaw angle integral term, which helps eliminate long-term steady-state errors. Then, based on the current lateral speed, yaw rate, yaw angle difference, and yaw angle integral term, corresponding horizontal control gains are determined. These gain values ​​optimize the control input to improve the accuracy of AUV heading control. Next, based on these gain values, horizontal target terms matching the current lateral speed, yaw rate, yaw angle difference, and yaw angle integral term are determined. These target terms play an adjusting and correcting role in horizontal control. Finally, by summing all horizontal target terms, a virtual command rudder angle in the horizontal direction is obtained, which serves as the control input to guide the AUV in adjusting the control surfaces, thereby achieving precise heading control.

[0011] In one implementation, when the direction is a roll direction, the current state data includes the current roll rate and the current roll angle, and the desired state data includes the desired roll angle; the method for determining the virtual command rudder angle includes: Determine the roll angle difference between the current roll angle and the desired roll angle, and integrate the roll angle difference to obtain the roll angle integral term; Determine a roll direction control gain that matches the current roll rate, the roll angle difference, and the roll angle integral term; Based on the roll direction control gain, roll target terms that match the current roll angular velocity, the roll angle difference, and the roll angle integral term are determined respectively. Add all the roll target items together to obtain the virtual command rudder angle in the roll direction.

[0012] This embodiment determines the roll angle difference between the current roll angle and the desired roll angle, integrates this difference to obtain the roll angle integral term, thereby accurately evaluating and correcting the roll error of the AUV. Next, it uses the roll angular velocity, the roll angle difference, and the roll angle integral term to calculate the roll direction control gain to meet system requirements, ensuring the system can quickly and effectively adjust the roll angle at any time. Then, based on this control gain, it calculates roll target terms that match each control variable, further refining the control strategy. Finally, it adds these target terms to obtain the virtual command rudder angle, which serves as the control input for the AUV, enabling precise control of the roll angle by adjusting the rudder surfaces.

[0013] In one implementation, determining the actual command rudder angle corresponding to the virtual command rudder angle includes: The virtual command rudder angles in different directions are combined into a virtual command rudder angle vector; The virtual command rudder angle vector is converted into an actual command rudder angle vector using a pre-set transformation matrix; wherein the actual command rudder angle vector includes the actual command rudder angle corresponding to each rudder angle.

[0014] This embodiment combines virtual command rudder angles from different directions into a single virtual command rudder angle vector. This vector effectively integrates control requirements from various directions, providing a unified control basis for subsequent rudder angle adjustments. Next, a preset transformation matrix converts the virtual command rudder angle vector into an actual command rudder angle vector. This transformation process ensures that each rudder angle corresponds to an actual control command, achieving precise matching between virtual commands and actual rudder angles. This optimizes the motion control of the AUV, making it more accurate and efficient in multi-dimensional attitude control.

[0015] In one implementation, determining the target residual value between the virtual command rudder angle and the actual drive rudder angle in different directions includes: For any direction, determine the error difference between the virtual command rudder angle and the actual drive rudder angle; Subtract the disturbance term corresponding to any direction from the error difference to obtain the initial residual value; The initial residual value is filtered to obtain the filtered target residual value.

[0016] This embodiment accurately identifies the deviation of the autonomous underwater vehicle in each direction by determining the error difference between the virtual command rudder angle and the actual driven rudder angle. Next, by subtracting the disturbance terms related to each direction, the influence of external disturbances is eliminated from the error difference, yielding the initial residual value. This process helps to remove noise interference. Finally, by filtering the initial residual value, the filtered target residual value is obtained, further eliminating high-frequency noise and ensuring stability and accuracy.

[0017] In one implementation, determining the jamming angle corresponding to the faulty servo based on the actual commanded rudder angle and the target residual value includes: Identify the faulty servo motors and determine the actual fault command rudder angle and the target fault residual values ​​in different directions corresponding to the faulty servo motors. Determine the target weighted residual that matches the target fault residual value; Determine the average weighted residual corresponding to the target weighted residual; The angle difference between the actual fault command rudder angle and the average weighted residual is determined as the jamming angle corresponding to the faulty servo.

[0018] This embodiment identifies the faulty servo and obtains its corresponding actual fault command rudder angle and target fault residual values ​​in different directions, providing fundamental information for subsequent fault compensation. Next, by weighting the target fault residual values ​​with the corresponding elements in the transformation matrix, the target weighted residual is obtained, thus quantifying the impact of the servo fault on each direction. Furthermore, by calculating the average value of the target weighted residual, a stable residual index is obtained, reducing the impact of errors in a single direction and ensuring balance. Finally, by comparing the angle difference between the actual fault command rudder angle and the average weighted residual, the jamming angle of the faulty servo is accurately determined.

[0019] Secondly, embodiments of this application provide a servo motor fault detection device, applied to an autonomous underwater vehicle, the device comprising: The virtual command determination unit is used to acquire the current state data and expected state data of the autonomous underwater vehicle, and determine the virtual command rudder angle in different directions based on the current state data and the expected state data; The actual command determination unit is used to determine the actual command rudder angle corresponding to the virtual command rudder angle; The fault condition determination unit is used to determine the target residual value between the virtual command rudder angle and the actual drive rudder angle in different directions, and to determine the fault condition of the servo motor based on the target residual value and a preset residual threshold. The jamming angle determination unit is used to determine the jamming angle corresponding to the faulty servo based on the actual commanded rudder angle and the target residual value when the servo is faulty.

[0020] Thirdly, embodiments of this application provide a computer device, including: The system includes a memory and a processor, which are interconnected. The memory stores computer instructions, and the processor executes these computer instructions to perform the aforementioned servo motor fault detection method.

[0021] Fourthly, embodiments of this application provide a computer-readable storage medium storing computer instructions, which are used to cause a computer to execute the aforementioned servo motor fault detection method. Attached Figure Description

[0022] To more clearly illustrate the technical solutions in the specific embodiments of this application or the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this application. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0023] Figure 1 A flowchart illustrating a servo motor fault detection method provided in this application embodiment; Figure 2 A flowchart illustrating the method for determining the virtual command rudder angle corresponding to the vertical direction, as provided in the embodiments of this application; Figure 3 A layout diagram of the servo motor provided in an embodiment of this application; Figure 4 A flowchart illustrating the method for determining the virtual command rudder angle corresponding to the horizontal direction, as provided in the embodiments of this application; Figure 5 A flowchart illustrating the method for determining the virtual command rudder angle corresponding to the roll direction in this embodiment of the application; Figure 6 A flowchart of step S3 provided in the embodiments of this application; Figure 7 A frame diagram of the actual command rudder angle provided in the embodiments of this application; Figure 8 A flowchart of step S5 provided in an embodiment of this application; Figure 9 A flowchart of step S7 provided in an embodiment of this application; Figure 10 The diagram showing the operating depth, yaw angle, and roll angle data provided in the embodiments of this application; Figure 11 A diagram showing rudder angle data provided in an embodiment of this application; Figure 12 This is a diagram showing the target residual value information provided in the embodiments of this application; Figure 13 A block diagram of a servo motor fault detection device provided in an embodiment of this application; Figure 14 This is a schematic diagram of the structure of a computer device provided in an embodiment of this application. Detailed Implementation

[0024] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0025] Autonomous Underwater Vehicles (AUVs) are increasingly demonstrating their value as important vehicles for marine resource exploration, underwater emergency rescue, and strategic reconnaissance missions. The servo system of an AUV is its core actuator, responsible for controlling the vehicle's attitude and heading. In particular, the X-rudder system, with its four control surfaces arranged in an X-shaped spatial symmetry, enhances the operational stability of the servo system. However, this also makes the detection of servo malfunctions a critical factor affecting the AUV's tracking accuracy and mission stability.

[0026] In complex surface and underwater environments, AUV servo systems are prone to malfunctions such as jamming, efficiency loss, and drift. These malfunctions not only affect the accuracy of the trajectory but can also lead to loss of control and even damage to the vehicle in severe cases. Therefore, real-time and accurate servo fault detection technology is crucial for ensuring the safe operation of AUVs. However, traditional hardware redundancy techniques or additional sensor solutions, while capable of fault detection, suffer from significant drawbacks, including high cost, poor adaptability to external environments, and complex installation and maintenance.

[0027] Compared to traditional methods, data-driven intelligent detection technologies, such as deep learning, while providing relatively accurate fault diagnosis, are not suitable for real-time monitoring of highly dynamic systems like AUVs due to their computational complexity, high latency, and requirement for large amounts of prior fault data. Therefore, observer-based fault detection methods have emerged as a more promising solution. This method combines the AUV's dynamic model, command rudder angle information, and real-time status information to perform timely and effective fault detection without increasing additional hardware costs.

[0028] Disturbance observers, as a typical disturbance estimation tool, have advantages such as direct disturbance estimation, independence from accurate models, low-pass noise suppression, and strong robustness, making them very suitable for detecting AUV servo motor malfunctions. Existing methods typically only determine whether a servo motor malfunction has occurred, lacking specific identification of the malfunction type, which limits the accuracy and reliability of malfunction detection.

[0029] Therefore, how to detect AUV servo motor faults in real time and accurately, so as to improve the accuracy and reliability of fault detection, is a technical problem that urgently needs to be solved.

[0030] To address the aforementioned technical problems, an embodiment of the servo motor fault detection method is provided according to the present application. It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. Furthermore, although a logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order than that shown here.

[0031] This embodiment provides a servo motor fault detection method. Figure 1 A flowchart of a servo motor fault detection method provided in this application embodiment is shown below. Figure 1 As shown, this process, applied to autonomous underwater vehicles, includes the following steps: Step S1: Obtain the current state data and expected state data of the autonomous underwater vehicle, and determine the virtual command rudder angle in different directions based on the current state data and expected state data.

[0032] Specifically, in the control system of an autonomous underwater vehicle (AUV), current state data and desired state data are acquired. Current state data typically includes the AUV's actual position, velocity, attitude angles (such as pitch, yaw, and roll angles), and other relevant motion data. This data is collected in real time by sensors (such as inertial measurement units, depth sensors, and speedometers). Desired state data is pre-set according to the AUV's mission objectives, such as desired depth, desired yaw angle, and desired lateral velocity. This data reflects the ideal state that the AUV should achieve when performing its mission.

[0033] Based on current and desired state data, the control system determines virtual command rudder angles in different directions through a series of calculations and algorithms. These virtual command rudder angles are key input signals for controlling the AUV's movement, guiding the adjustment of the AUV's various servos to make the AUV's actual motion state as close as possible to the desired state. Specifically, the control system calculates the required rudder angle adjustment in each direction, i.e., the virtual command rudder angle, based on the difference between the current and desired states, combined with a dynamic model and control algorithms (such as PID control and LQR control). These virtual command rudder angles are then converted into actual command rudder angles and sent to the AUV's actuators (such as servos), thereby achieving precise control of the AUV's movement.

[0034] Step S3: Determine the actual commanded rudder angle corresponding to the virtual commanded rudder angle.

[0035] Specifically, in the control system of an autonomous underwater vehicle (AUV), determining the actual commanded rudder angle corresponding to the virtual commanded rudder angle is a crucial step in achieving precise control. The virtual commanded rudder angle is an ideal rudder angle calculated by the controller based on current state data and desired state parameters, used to guide the AUV's movement. However, since the actual movement of the AUV is affected by various factors, such as hydrodynamics, mechanical hysteresis, and sensor noise, directly using the virtual commanded rudder angle may not fully achieve the desired movement effect. Therefore, it is necessary to convert the virtual commanded rudder angle into the actual commanded rudder angle, which can be done using a preset conversion matrix.

[0036] Step S5: Determine the target residual value between the virtual command rudder angle and the actual drive rudder angle in different directions, and determine the servo motor malfunction based on the target residual value and the preset residual threshold.

[0037] Specifically, the target residual value is obtained by calculating the difference between the virtual command rudder angle and the actual drive rudder angle, reflecting the deviation between the AUV's actual and desired motion. If the target residual value exceeds a preset residual threshold, it may indicate a servo malfunction. The residual threshold is preset based on the system's normal operating range and fault tolerance, used to distinguish between normal deviations and fault deviations. By comparing the target residual value with the residual threshold, it can be determined whether the servo is functioning normally. If the residual value continues to exceed the threshold, the system can further diagnose the fault type, such as jamming or malfunction, and take corresponding measures, such as switching to a backup system or adjusting the control strategy, to ensure the safe operation of the AUV.

[0038] Specifically, to accurately detect servo motor malfunctions, different residual thresholds need to be defined based on the different residual values ​​of the AUV in the vertical, horizontal, and roll directions: ±ε1 for the vertical direction, ±ε2 for the horizontal direction, and ±ε3 for the roll direction. During the fault-free phase, when the AUV is running in steady state, the residual thresholds are obtained using the following method: Based on the residual values ​​estimated by the interference observer and the set threshold, corresponding AUV servo motor fault judgment rules are formulated: when When the target residual value exceeds the positive residual threshold, it is indicated by the '+' symbol.

[0039] when When the target residual value exceeds the negative residual threshold, it is indicated by the '-' symbol.

[0040] when When the target residual value is within the residual threshold range, it is represented by the symbol '0'.

[0041] The specific rules for servo motor fault diagnosis are shown in Table 1.

[0042] Table 1 Rules for Servo Fault Diagnosis If the sign combination of the target residual value is not within the range defined in Table 1 above, it indicates that the state combination is invalid, which may be a false alarm caused by system anomalies or noise interference. By setting a reasonable residual threshold and comparing the target residual value estimated by the interference observer with the residual threshold, the fault condition of the AUV servo can be effectively detected. This fault judgment method based on sign combination can not only quickly identify the fault type, but also avoid false alarms and improve the reliability and safety of the system.

[0043] Step S7: In the event of a servo malfunction, determine the jamming angle corresponding to the malfunctioning servo based on the actual commanded servo angle and the target residual value.

[0044] Specifically, when a servo malfunction is confirmed, the jamming angle of the malfunctioning servo needs to be determined based on the actual commanded servo angle and the target residual value. The actual commanded servo angle is the ideal servo angle calculated by the controller based on the current and desired states, while the target residual value is estimated by the disturbance observer and reflects the deviation between the actual and desired servo angles. By weighted summing the target residual values ​​and subtracting them from the actual commanded servo angle, an estimated jamming angle can be obtained. This angle represents the actual jamming position of the servo and its deviation from the ideal position. By accurately estimating the jamming angle, the severity of the malfunction can be better understood, and corresponding measures can be taken to restore or compensate for the servo's function, thereby ensuring the stable operation of the AUV.

[0045] This embodiment provides a servo malfunction detection method. It acquires the current and desired state data of the aircraft and calculates virtual command rudder angles in different directions based on this data, providing initial commands for servo control. The virtual command rudder angles are converted into actual command rudder angles for more accurate servo adjustments. Next, a target residual value between the virtual command rudder angle and the actual rudder angle is calculated and compared with a preset residual threshold to determine if a servo malfunction exists. If a servo malfunction is detected, the jamming angle of the faulty servo is calculated based on the actual command rudder angle and the target residual value. This embodiment not only effectively detects servo malfunctions but also identifies and locates the specific servo experiencing a jamming fault and quantifies the real-time jamming angle of the faulty servo.

[0046] Figure 2 The flowchart illustrating the method for determining the virtual command rudder angle corresponding to the vertical direction provided in this application embodiment shows that, when the direction is vertical, the current state data includes the current vertical velocity, current pitch rate, current pitch angle, and current depth, and the desired state data includes the desired depth. This process may include the following steps: Step S111: Determine the depth difference between the current depth and the desired depth, and integrate the depth difference to obtain the depth integral term.

[0047] Specifically, the depth difference Δζ = the desired depth ζ d - Current depth ζ, this depth difference reflects the deviation between the AUV's current depth and the desired depth. To eliminate steady-state static error, i.e., to ensure that the AUV can accurately maintain the desired depth after long-term operation, this depth difference is integrated. The role of the integral term is to help the AUV stay near the desired depth, especially when there are external disturbances (such as changes in water flow) or changes in internal parameters (such as buoyancy, rudder control, etc.). The introduction of this integral term can effectively reduce static error, and the final depth integral term is ∫(ζ) d -ζ)dt.

[0048] Step S113: Determine the vertical control gain that matches the current vertical velocity, current pitch angular velocity, current pitch angle, depth difference, and depth integral term.

[0049] Specifically, to facilitate the study and analysis of the dynamic characteristics of autonomous underwater vehicles (AUVs), the dynamic characteristics of AUVs are simplified into a six-degree-of-freedom rigid body model, and its kinematic and dynamic equations are established based on Newton's laws. Please refer to [link / reference]. Figure 3 The model has three translational degrees of freedom: translational motion along the x, y, and z axes. It also has three rotational degrees of freedom: rotational motion about the x, y, and z axes (pitch, yaw, and roll). To further simplify the model, the original nonlinear and coupled dynamic model is decoupled into a linearized dynamic model with three dimensions: vertical, horizontal, and roll. The vertical direction mainly considers vertical motion, including vertical velocity, pitch rate, pitch angle, and depth. The horizontal direction mainly considers motion within the horizontal plane, including lateral velocity, yaw rate, and yaw angle. The roll direction mainly considers rotational motion about the body axis, including roll rate and roll angle. Different linearized state-space equations are obtained for each direction.

[0050] Vertical direction: in, x is the derivative of the state vector in the vertical direction (i.e., the rate of change of the state variables); v x is the state variable in the vertical direction. v = [w,q,θ,ζ]; w is the vertical velocity; q is the pitch angular velocity; θ is the pitch angle; ζ is the depth; A v and B v It is the linearized system matrix; δ vd This refers to the virtual command rudder angle in the vertical direction, i.e., the control input in the vertical direction; σ v =[σ v1 ,σ v2 ,0,0] T , σ v1 The disturbance caused by the vertical force affects the vertical velocity and depth, σ v2 The disturbance of pitch moment affects the pitch angular velocity and pitch angle; a v11 a v12 a v13 a v21 a v22 a v23 Here, is the dynamic coefficient, describing the interaction between system states; u is the speed in the X direction; b v1 and b v2 These are dynamic coefficients that describe the effect of control inputs on the system state.

[0051] The vertical control gain k1 = [k] is calculated using the LQR method. w ,k q ,k θ ,k ζ ], k1=lqr(A v B v Q v ,R v ); where Q v R is the state weight matrix in the vertical direction; v The vertical control input weight matrix is ​​used. The LQR method can find the optimal control gain k1, which minimizes control energy consumption while meeting dynamic performance requirements.

[0052] Step S115: Based on the vertical direction control gain, determine the vertical target terms that match the current vertical velocity, current pitch angular velocity, current pitch angle, depth difference, and depth integral term.

[0053] Specifically, -k w w represents the vertical target term that matches the current vertical velocity; -k q q represents the vertical target term that matches the current pitch angular velocity; -k θ θ represents the vertical target term that matches the current pitch angle; k ζ (ζ d -ζ) represents the vertical target item that matches the depth difference; k i ∫(ζ d -ζ)dt is the vertical objective term that matches the depth integral term.

[0054] Step S117: Add all vertical target items together to obtain the virtual command rudder angle in the vertical direction.

[0055] δ vd =-k w wk q qk θ θ+kζ(ζ d -ζ)+k i ∫(ζ d -ζ)dt Where, δ vd This is a virtual command rudder angle in the vertical direction, used to guide the vertical movement of the AUV to reach the desired depth; k w ,k q ,k θ ,k ζ To control the gain in the vertical direction, k i This is the integral gain, used to eliminate the steady-state error of the system, and is generally taken as a small value (e.g., between 0.01 and 0.1).

[0056] This embodiment determines the depth difference between the current depth and the desired depth, and integrates this difference to obtain a depth integral term. This helps eliminate steady-state errors, ensuring that the AUV can stably maintain a position near the desired depth over a long period. Next, based on the current vertical velocity, pitch rate, pitch angle, depth difference, and depth integral term, corresponding control gains are determined. These gains can be adjusted according to different state variables to optimize the control effect. Subsequently, based on the obtained control gains, corresponding vertical target terms are calculated for each state variable. These vertical target terms are used to precisely adjust the vertical motion. Finally, all vertical target terms are weighted and summed to obtain a virtual command rudder angle in the vertical direction. This rudder angle serves as the control input to adjust the AUV's control surfaces, thereby achieving precise control of the AUV's depth.

[0057] Figure 4 The flowchart for determining the virtual command rudder angle corresponding to the horizontal direction provided in this application embodiment shows that, when the direction is horizontal, the current state data includes the current lateral velocity, the current yaw rate, and the current yaw angle, and the desired state data includes the desired yaw angle; the process may include the following steps: Step S131: Determine the yaw angle difference between the current yaw angle and the desired yaw angle, and integrate the yaw angle difference to obtain the yaw angle integral term.

[0058] Specifically, the yaw angle difference ΔΨ = the desired yaw angle Ψ d - The current yaw angle Ψ reflects the deviation between the AUV's current yaw angle and the desired yaw angle. To eliminate steady-state error, i.e., to ensure that the AUV can accurately maintain the desired yaw angle after long-term operation, this yaw angle difference is integrated to avoid the accumulation of yaw error during long-term operation, thereby improving heading stability. The final yaw angle integral term is ∫(Ψ) d -Ψ)dt.

[0059] Step S133: Determine the horizontal control gain that matches the current lateral velocity, current yaw rate, yaw angle difference, and yaw angle integral term.

[0060] Specifically, in the horizontal direction: in, x is the derivative of the horizontal state vector (i.e., the rate of change of the state variables); h x is the horizontal state variable. h = [v, r, Ψ]; v is the lateral velocity; r is the yaw rate; Ψ is the yaw angle; A h and B h It is the linearized system matrix; δhd This refers to the virtual command rudder angle in the horizontal direction, i.e., the control input in the horizontal direction; σ h =[σ h1 ,σ h2 ,0] T , σ h1 The disturbance caused by the horizontal force affects the lateral velocity, σ h2 The disturbance of yaw moment affects the yaw rate and yaw angle; a h11 a h12 a h21 a h22 b are dynamic coefficients that describe the interactions between system states; h1 and b h2 These are dynamic coefficients that describe the effect of control inputs on the system state.

[0061] The horizontal control gain k2 = [k] is calculated using the LQR method. v ,k r ,k Ψ ],k2=lqr(A h B h Q h ,R h ); where Q h R is the horizontal state weight matrix; h The horizontal control input weight matrix is ​​used. The LQR method can find the optimal control gain k2, which minimizes control energy consumption while meeting dynamic performance requirements.

[0062] Step S135: Based on the horizontal control gain, determine the horizontal target terms that match the current lateral velocity, current yaw rate, yaw angle difference, and yaw angle integral term.

[0063] Specifically, -k v v represents the horizontal target term that matches the current lateral velocity; -k r r represents the horizontal target term matching the current yaw rate; k Ψ (Ψ d -Ψ) represents the horizontal target item that matches the yaw angle difference; k ih ∫(Ψ d -Ψ)dt is the horizontal objective term related to the yaw angle integral term.

[0064] Step S137: Add up all horizontal target items to obtain the virtual command rudder angle in the horizontal direction.

[0065] δ hd =-k v vk r r+k Ψ (Ψd -Ψ)+k ih ∫(Ψ d -Ψ)dt Where, δ hd This is a virtual command rudder angle in the horizontal direction, making its horizontal movement more stable and ensuring that the AUV can quickly and accurately approach and maintain the desired yaw angle; k v ,k r ,k Ψ For horizontal control gain; k ih This is the integral gain, used to eliminate the steady-state error of the system, and is generally taken as a small value (e.g., between 0.01 and 0.1).

[0066] This embodiment calculates and integrates the yaw angle difference between the current and desired yaw angles to obtain the yaw angle integral term, which helps eliminate long-term steady-state errors. Then, based on the current lateral speed, yaw rate, yaw angle difference, and yaw angle integral term, corresponding horizontal control gains are determined. These gain values ​​optimize the control input to improve the accuracy of AUV heading control. Next, based on these gain values, horizontal target terms matching the current lateral speed, yaw rate, yaw angle difference, and yaw angle integral term are determined. These target terms play an adjusting and correcting role in horizontal control. Finally, by summing all horizontal target terms, a virtual command rudder angle in the horizontal direction is obtained, which serves as the control input to guide the AUV in adjusting the control surfaces, thereby achieving precise heading control.

[0067] Figure 5 The flowchart for determining the virtual command rudder angle corresponding to the roll direction provided in this application embodiment shows that when the direction is the roll direction, the current state data includes the current roll angular velocity and the current roll angle, and the desired state data includes the desired roll angle; the process may include the following steps: Step S151: Determine the difference between the current roll angle and the desired roll angle, and integrate the roll angle difference to obtain the roll angle integral term.

[0068] Specifically, This roll angle difference reflects the deviation between the current roll angle and the desired roll angle of the AUV. To eliminate steady-state error, i.e., to ensure that the AUV can accurately maintain the desired roll angle after long-term operation, this roll angle difference is integrated, and the final integrated roll angle term is:

[0069] Step S153: Determine the roll direction control gain that matches the current roll angular velocity, roll angle difference, and roll angle integral term.

[0070] Specifically, the roll direction: in, x is the derivative of the roll-direction state vector (i.e., the rate of change of the state variable); r For the roll direction state variable, p is the roll velocity; For roll angle; A r and B r It is the linearized system matrix; δ rd This is the virtual command rudder angle in the roll direction, i.e., the control input in the roll direction; σ r =[σ r1 ,0] T , σ r1 The disturbance of the rolling moment affects the roll angular velocity and roll angle; a r11 a r12 b are dynamic coefficients that describe the interactions between system states; r1 These are dynamic coefficients that describe the effect of control inputs on the system state.

[0071] Calculate the roll direction control gain using the LQR method. k2=lqr(A r B r Q r ,R r ); where Q r R is the roll direction state weight matrix; r The input weight matrix is ​​used for roll direction control. The LQR method can find the optimal control gain k3, which minimizes control energy consumption while meeting dynamic performance requirements.

[0072] Step S155: Based on the roll direction control gain, determine the roll target terms that match the current roll angular velocity, roll angle difference, and roll angle integral term.

[0073] Specifically, -k p p is the roll target term that matches the current roll angular velocity; A roll target item that matches the roll angle difference; For the roll target term that matches the roll angle integral term: Step S157: Add up all roll target items to obtain the virtual command rudder angle in the roll direction.

[0074] Where, δ rdThis is a virtual command rudder angle in the roll direction, making its movement in the roll direction more stable and ensuring that the AUV can quickly and accurately approach and maintain the desired roll angle; k p , To control the gain in the roll direction; This is the integral gain, used to eliminate the steady-state error of the system, and is generally taken as a small value (e.g., between 0.01 and 0.1).

[0075] It should be noted that the kinetic coefficients are shown in Table 2.

[0076] Table 2 Kinetic coefficients <![CDATA[a v11 ]]> -0.4726 <![CDATA[a h22 ]]> -0.4818 <![CDATA[a v12 ]]> 0.3474 <![CDATA[a r11 ]]> -0.6023 <![CDATA[a v13 ]]> 0.0482 <![CDATA[a r12 ]]> -0.8236 <![CDATA[a v21 ]]> 0.5906 <![CDATA[b v1 ]]> -0.2032 <![CDATA[a v22 ]]> -0.4762 <![CDATA[b v2 ]]> 0.1143 <![CDATA[a v23 ]]> -1.7362 <![CDATA[b h1 ]]> 0.3374 <![CDATA[a h11 ]]> -0.4726 <![CDATA[b h2 ]]> 0.4818 <![CDATA[a h12 ]]> -0.3642 <![CDATA[b r1 ]]> -3.1623 <![CDATA[a h21 ]]> 0.5906 This embodiment determines the roll angle difference between the current roll angle and the desired roll angle, integrates this difference to obtain the roll angle integral term, thereby accurately evaluating and correcting the roll error of the AUV. Next, it uses the roll angular velocity, the roll angle difference, and the roll angle integral term to calculate the roll direction control gain to meet system requirements, ensuring the system can quickly and effectively adjust the roll angle at any time. Then, based on this control gain, it calculates roll target terms that match each control variable, further refining the control strategy. Finally, it adds these target terms to obtain the virtual command rudder angle, which serves as the control input for the AUV, enabling precise control of the roll angle by adjusting the rudder surfaces.

[0077] Figure 6 The flowchart for step S3 provided in the embodiments of this application may include the following steps: Step S31: Combine the virtual command rudder angles in different directions into a virtual command rudder angle vector.

[0078] Specifically, virtual command rudder angles in different directions are combined into a virtual command rudder angle vector Δ. vird : Where, δ vd The virtual command rudder angle in the vertical direction; δ hd The virtual command rudder angle in the horizontal direction; δ rd This is the virtual command rudder angle in the roll direction.

[0079] Step S33: Convert the virtual command rudder angle vector into the actual command rudder angle vector using a pre-set transformation matrix; wherein, the actual command rudder angle vector includes the actual command rudder angle corresponding to each rudder angle.

[0080] Specifically, the conversion relationship is as follows: Where, δ 1d ,δ2d ,δ 3d ,δ 4d This is the actual commanded rudder angle for the X rudder. Please refer to [link / reference]. Figure 7 As shown.

[0081] In order to make the virtual command rudder angle Convert to actual commanded rudder angle for X-rudder Design an allocation algorithm using the pseudo-inverse method: Δx d =H T (HH T ) -1 Δ vird Where H is the transformation matrix.

[0082] The final obtained X-rudder actual command angle Δ X d is used to control the movement of the AUV, enabling precise multi-dimensional control.

[0083] This embodiment combines virtual command rudder angles from different directions into a single virtual command rudder angle vector. This vector effectively integrates control requirements from various directions, providing a unified control basis for subsequent rudder angle adjustments. Next, a preset transformation matrix converts the virtual command rudder angle vector into an actual command rudder angle vector. This transformation process ensures that each rudder angle corresponds to an actual control command, achieving precise matching between virtual commands and actual rudder angles. This optimizes the motion control of the AUV, making it more accurate and efficient in multi-dimensional attitude control.

[0084] Figure 8 The flowchart for step S5 provided in the embodiments of this application may include the following steps: Step S51: For any direction, determine the error difference between the virtual command rudder angle and the actual drive rudder angle.

[0085] Specifically, residual estimates These represent the residual estimates in the vertical, horizontal, and roll directions, respectively.

[0086] For the vertical direction, the error difference = virtual command rudder angle δ vd -Actual drive rudder angle δ v For the horizontal direction, the error difference = virtual command rudder angle δ hd -Actual drive rudder angle δ h The error difference in the roll direction = virtual command rudder angle δ rd -Actual drive rudder angle δ r .

[0087] Step S53: Subtract the disturbance term corresponding to any direction from the error difference to obtain the initial residual value.

[0088] Specifically, for the initial residual value in the vertical direction: For the initial residual value in the horizontal direction: For the initial residual value in the roll direction: Step S55: Filter the initial residual value to obtain the filtered target residual value.

[0089] Specifically, for the target residual value in the vertical direction: For the target residual value in the horizontal direction: For the target residual value in the roll direction: Among them, g ob s is the bandwidth of the interference observer; s is the frequency of the interference observer.

[0090] It's important to note that in actual operation, AUV acceleration information suffers from significant noise interference, making it unusable for direct use of accelerometer data. Furthermore, the real-time drive rudder angle of the AUV cannot be directly acquired via sensors. To address these issues, a Laplace transform-based disturbance observer was designed. This observer performs a frequency domain transformation on the decoupled dynamic equations of the AUV in the vertical, horizontal, and roll directions. Using the input virtual command rudder angle information and real-time state information obtained from sensors (current state data of the autonomous underwater vehicle), the residual value between the virtual command rudder angle and the real-time drive rudder angle is estimated. The core performance of the disturbance observer depends on the frequency domain shaping design of the low-pass filter Q(s). Its functional architecture comprises three layers: High-frequency noise filtering: High-frequency noise is filtered out in the frequency domain to suppress sensor noise and unmodeled high-frequency dynamics, ensuring the robustness of the observer's disturbance estimation. Dynamic model regularization: The inverse model's ill-conditioned gain characteristics in the high-frequency band are smoothed to avoid observation distortion caused by numerical differential explosion. Balancing dynamic response performance and robustness: Higher bandwidth allows for faster disturbance estimation, but also increases sensitivity to high-frequency errors.

[0091] To ensure the accuracy of the estimation and eliminate high-frequency noise, the interference observer Q(s) adopts a first-order low-pass filter structure. Its core design principle is to maintain a gain coefficient of 1 within the effective operating frequency band, thereby preserving the useful signal components completely. In the high-frequency noise band, it achieves rapid gain attenuation, with the gain approaching 0. This frequency domain shaping mechanism effectively filters out various noise interferences mixed into the measurement signal.

[0092] Therefore, the above formula can be transformed as follows: For the target residual value in the vertical direction: For the target residual value in the horizontal direction: For the target residual value in the roll direction: With this design, the interference observer can effectively suppress noise interference in actual operation, thereby improving the control accuracy and robustness of the AUV.

[0093] This embodiment accurately identifies the deviation of the autonomous underwater vehicle in each direction by determining the error difference between the virtual command rudder angle and the actual driven rudder angle. Next, by subtracting the disturbance terms related to each direction, the influence of external disturbances is eliminated from the error difference, yielding the initial residual value. This process helps to remove noise interference. Finally, by filtering the initial residual value, the filtered target residual value is obtained, further eliminating high-frequency noise and ensuring stability and accuracy.

[0094] Figure 9 The flowchart for step S7 provided in the embodiments of this application may include the following steps: Step S71: Obtain the faulty servo motor, and determine the actual fault command rudder angle and the target fault residual value in different directions corresponding to the faulty servo motor.

[0095] Specifically, the faulty servos are identified and determined. For each faulty servo, its corresponding actual fault command rudder angle δ is obtained. id δ id The actual fault command rudder angle of the i-th servo motor in rudder X and the target fault residual values ​​in different directions.

[0096] Step S73: Determine the target weighted residual that matches the target fault residual value.

[0097] Specifically, the target weighted residual is calculated based on the target fault residual value and the corresponding elements of the transformation matrix H. The specific formula is as follows: Among them, H 1i H represents the value in the 1st row and i-th column of the transformation matrix H; 2i H represents the value in the 2nd row and i-th column of the transformation matrix H; 3i This is the value in the 3rd row and i-th column of the transformation matrix H.

[0098] Step S75: Determine the average weighted residual corresponding to the target weighted residual.

[0099] Specifically, the target weighted residual is divided by 3 to obtain the average weighted residual: Step S77: Determine the angle difference between the actual fault command rudder angle and the average weighted residual as the jamming angle corresponding to the faulty servo.

[0100] Specifically, the jerk angle refers to the deviation of the servo angle caused by the failure or malfunction of the servo. By calculating the actual fault command servo angle and the average weighted residual, a more accurate servo angle adjustment value, i.e., the jerk angle, can be obtained. Subtracting the average weighted residual when calculating the jerk angle helps to eliminate the influence caused by disturbances or errors, thus obtaining a more accurate servo correction value.

[0101] Locking angle: This embodiment identifies the faulty servo and obtains its corresponding actual fault command rudder angle and target fault residual values ​​in different directions, providing fundamental information for subsequent fault compensation. Next, by weighting the target fault residual values ​​with the corresponding elements in the transformation matrix, the target weighted residual is obtained, thus quantifying the impact of the servo fault on each direction. Furthermore, by calculating the average value of the target weighted residual, a stable residual index is obtained, reducing the impact of errors in a single direction and ensuring balance. Finally, by comparing the angle difference between the actual fault command rudder angle and the average weighted residual, the jamming angle of the faulty servo is accurately determined.

[0102] The specific implementation of this invention will be described below with reference to a specific application scenario. In the simulation model, the desired depth of the AUV is set to 1m, the desired yaw angle to 57.3°, and the desired roll angle to 0°. Uncertainty perturbations (σ) are also included in the model. v1 σ v2 σ h1 σ h2 σ r1 σ r2The values ​​are all 0.01sin(t). Using the LQR control algorithm, the AUV completes dynamic adjustment within 0–20 seconds and remains stable within 20–50 seconds. At 50 seconds, servo motor 1 experiences a mechanical jamming fault, with the jamming angle fixed at 20°. The total simulation time is 100 seconds, and the sampling time is 0.1 seconds.

[0103] Simulation results show that: from Figure 10 As can be seen, when servo motor 1 malfunctioned, the AUV deviated from its normal operating trajectory, with significant changes in depth, yaw angle, and roll angle. The light gray area in the figure represents the AUV's steady-state operation phase, while the dark area represents the AUV's servo motor malfunction phase. From... Figure 11 As can be seen, due to the malfunction of servo motor 1, the actual commanded rudder angle of the AUV's tail X rudder changed under the LQR control algorithm, no longer maintaining the original steady-state rudder angle. The light gray area in the figure represents the AUV's steady-state operation phase, while the dark area represents the AUV's servo motor malfunction operation phase. From... Figure 12 As can be seen, the target residual value estimated by the interference observer remains within the residual threshold range when the servo is operating normally, and the system judges the AUV to be in a normal state. However, when the servo malfunctions, the fault detection system detects that the target residual value exceeds the residual threshold within 1 second, and accurately identifies the specific location of the malfunctioning servo using the defined fault judgment rule table. Simultaneously, the system estimates the approximate jamming angle of the servo by calculating the jamming angle formula. The light gray area in the figure represents the steady-state operation phase of the AUV, and the dark area represents the servo malfunction operation phase.

[0104] This simulation verification result shows that the embodiments of this application can quickly and accurately detect servo motor failures and effectively estimate the jamming angle, thereby ensuring the control and stable operation of the AUV under failure conditions.

[0105] Accordingly, please refer to Figure 13 A block diagram of a servo motor fault detection device provided in this application embodiment, applied to an autonomous underwater vehicle, the device comprising: The virtual command determination unit 101 is used to acquire the current state data and expected state data of the autonomous underwater vehicle, and determine the virtual command rudder angle in different directions based on the current state data and expected state data. The actual command determination unit 103 is used to determine the actual command rudder angle corresponding to the virtual command rudder angle. The fault condition determination unit 105 is used to determine the target residual value between the virtual command rudder angle and the actual drive rudder angle in different directions, and to determine the fault condition of the servo motor based on the target residual value and a preset residual threshold. The jamming angle determination unit 107 is used to determine the jamming angle corresponding to the faulty servo based on the actual command rudder angle and the target residual value when the servo is faulty.

[0106] In some optional implementations, when the direction is vertical, the current state data includes the current vertical velocity, current pitch angular velocity, current pitch angle, and current depth, and the desired state data includes the desired depth; the virtual command determination unit 101 includes: Determine the depth difference between the current depth and the desired depth, and integrate the depth difference to obtain the depth integral term; Determine the vertical control gain that matches the current vertical velocity, current pitch rate, current pitch angle, depth difference, and depth integral term; Based on the vertical control gain, determine the vertical target terms that match the current vertical velocity, current pitch rate, current pitch angle, depth difference, and depth integral term respectively; Adding all the vertical target items together yields the virtual command rudder angle in the vertical direction.

[0107] In some optional implementations, when the direction is horizontal, the current state data includes the current lateral velocity, the current yaw rate, and the current yaw angle, and the desired state data includes the desired yaw angle; the virtual command determination unit 101 includes: Determine the difference between the current yaw angle and the desired yaw angle, and integrate the yaw angle difference to obtain the yaw angle integral term; Determine the horizontal control gain that matches the current lateral velocity, current yaw rate, yaw angle difference, and yaw angle integral term; Based on the horizontal control gain, determine the horizontal target terms that match the current lateral velocity, current yaw rate, yaw angle difference, and yaw angle integral term, respectively. Adding all the horizontal target items together yields the virtual command rudder angle in the horizontal direction.

[0108] In some optional implementations, when the direction is the roll direction, the current state data includes the current roll angular velocity and the current roll angle, and the desired state data includes the desired roll angle; the virtual command determination unit 101 includes: Determine the difference between the current roll angle and the desired roll angle, and integrate the roll angle difference to obtain the roll angle integral term; Determine the roll direction control gain that matches the current roll rate, roll angle difference, and roll angle integral term; based on the roll direction control gain, determine the roll target terms that match the current roll rate, roll angle difference, and roll angle integral term respectively. Add up all the roll target items to obtain the virtual command rudder angle in the roll direction.

[0109] In some optional implementations, the actual instruction determination unit 103 includes: The virtual command rudder angles in different directions are combined into a virtual command rudder angle vector; The virtual command rudder angle vector is converted into the actual command rudder angle vector using a pre-defined transformation matrix; wherein the actual command rudder angle vector includes the actual command rudder angle corresponding to each rudder angle.

[0110] In some optional implementations, the fault condition determination unit 105 includes: For any given direction, determine the error difference between the virtual command rudder angle and the actual driven rudder angle; Subtract the disturbance term corresponding to any direction from the error difference to obtain the initial residual value; The initial residual value is filtered to obtain the filtered target residual value.

[0111] In some alternative implementations, the jamming angle determination unit 107 includes: Identify the faulty servo motors and determine the actual fault command rudder angle and the target fault residual value in different directions corresponding to the faulty servo motors. Determine the target weighted residual that matches the target fault residual value; Determine the average weighted residual corresponding to the target weighted residual; The angle difference between the actual fault command rudder angle and the average weighted residual is determined as the jamming angle corresponding to the faulty servo.

[0112] Further functional descriptions of the above modules and units are the same as those in the corresponding embodiments described above, and will not be repeated here.

[0113] In this embodiment, a servo fault detection device is presented in the form of a functional unit. Here, a unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that execute one or more software or fixed programs, and / or other devices that can provide the above-mentioned functions.

[0114] Please see Figure 14 , Figure 14 This application provides a schematic diagram of the structure of a computer device, as shown in the embodiment of the present application. Figure 14As shown, the computer device includes one or more processors 10, memory 20, and interfaces for connecting the components, including high-speed interfaces and low-speed interfaces. The components communicate with each other via different buses and can be mounted on a common motherboard or otherwise installed as needed. The processors can process instructions executed within the computer device, including instructions stored in or on memory to display graphical information of a GUI on external input / output devices (such as display devices coupled to the interfaces). In some alternative implementations, multiple processors and / or multiple buses can be used with multiple memories and multiple memory modules, if desired. Similarly, multiple computer devices can be connected, each providing some of the necessary operations (e.g., as a server array, a group of blade servers, or a multiprocessor system). Figure 14 Take a processor 10 as an example.

[0115] Processor 10 may be a central processing unit, a network processor, or a combination thereof. Processor 10 may further include a hardware chip. The hardware chip may be an application-specific integrated circuit (ASIC), a programmable logic device (PLD), or a combination thereof. The programmable logic device may be a complex programmable logic device (CAMP), a field-programmable gate array (FPGA), a general-purpose array logic (GDA), or any combination thereof.

[0116] The memory 20 stores instructions executable by at least one processor 10 to cause the at least one processor 10 to perform the method shown in the above embodiments.

[0117] The memory 20 may include a program storage area and a data storage area. The program storage area may store the operating system and applications required for at least one function; the data storage area may store data created based on the use of the computer device. Furthermore, the memory 20 may include high-speed random access memory and may also include non-transitory memory, such as at least one disk storage device, flash memory device, or other non-transitory solid-state storage device. In some alternative embodiments, the memory 20 may optionally include memory remotely located relative to the processor 10, and these remote memories may be connected to the computer device via a network. Examples of such networks include, but are not limited to, the Internet, intranets, local area networks, mobile communication networks, and combinations thereof.

[0118] The memory 20 may include volatile memory, such as random access memory; the memory may also include non-volatile memory, such as flash memory, hard disk or solid-state drive; the memory 20 may also include a combination of the above types of memory.

[0119] The computer device also includes a communication interface 30 for communicating with other devices or communication networks.

[0120] This application also provides a computer-readable storage medium. The methods described in this application can be implemented in hardware or firmware, or implemented as recordable on a storage medium, or implemented as computer code downloaded over a network and originally stored on a remote storage medium or a non-transitory machine-readable storage medium and subsequently stored on a local storage medium. Thus, the methods described herein can be processed by software stored on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. The storage medium can be a magnetic disk, optical disk, read-only memory, random access memory, flash memory, hard disk, or solid-state drive, etc.; further, the storage medium can also include combinations of the above types of memory. It is understood that computers, processors, microprocessor controllers, or programmable hardware include storage components capable of storing or receiving software or computer code. When the software or computer code is accessed and executed by the computer, processor, or hardware, the methods shown in the above embodiments are implemented.

[0121] The apparatus and units described in the above embodiments can be implemented by a computer chip or physical entity, or by a product with a certain function. A typical implementation device is a computer. Specifically, a computer can be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smartphone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or any combination of these devices.

[0122] For ease of description, the above devices are described separately by function as various units. Of course, in implementing this application, the functions of each unit can be implemented in one or more software and / or hardware.

[0123] Those skilled in the art will understand that the embodiments of this application can be provided as methods or apparatus. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0124] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatuses, and devices according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0125] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0126] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0127] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0128] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, the apparatus embodiments are basically similar to the method embodiments, so the description is relatively simple; relevant parts can be referred to the descriptions of the method embodiments.

[0129] The above description is merely an embodiment of this application and is not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principle of this application should be included within the scope of the claims of this application.

[0130] Although embodiments of this application have been described in conjunction with the accompanying drawings, those skilled in the art can make various modifications and variations without departing from the spirit and scope of this application, and such modifications and variations all fall within the scope defined by the appended claims.

Claims

1. A method for detecting servo motor faults, characterized in that, Applied to autonomous underwater vehicles, the method includes: The system acquires the current state data and desired state data of the autonomous underwater vehicle (AUV), and determines virtual command rudder angles in different directions based on the current state data and the desired state data. When the direction is vertical, the current state data includes the current vertical speed, current pitch rate, current pitch angle, and current depth, and the desired state data includes the desired depth. The determination of the virtual command rudder angle includes: determining the depth difference between the current depth and the desired depth, and integrating the depth difference to obtain a depth integral term; determining a vertical direction control gain that matches the current vertical speed, current pitch rate, current pitch angle, depth difference, and depth integral term; determining vertical target terms that match the current vertical speed, current pitch rate, current pitch angle, depth difference, and depth integral term based on the vertical direction control gain; and summing all the vertical target terms to obtain the virtual command rudder angle in the vertical direction. The virtual command rudder angles in different directions are combined into a virtual command rudder angle vector; The virtual command rudder angle vector is converted into an actual command rudder angle vector using a pre-set transformation matrix; wherein, the actual command rudder angle vector includes the actual command rudder angle corresponding to each rudder angle; Determine the target residual value between the virtual command rudder angle and the actual drive rudder angle in different directions, and determine the servo motor malfunction based on the target residual value and a preset residual threshold. In the event of a malfunction in the servo motor Identify the faulty servo motors and determine the actual fault command rudder angle and the target fault residual values ​​in different directions corresponding to the faulty servo motors. Determine the target weighted residual that matches the target fault residual value; Determine the average weighted residual corresponding to the target weighted residual; The angle difference between the actual fault command rudder angle and the average weighted residual is determined as the jamming angle corresponding to the faulty servo.

2. The method according to claim 1, characterized in that, When the direction is horizontal, the current state data includes the current lateral velocity, current yaw rate, and current yaw angle, and the desired state data includes the desired yaw angle; the method for determining the virtual command rudder angle includes: Determine the yaw angle difference between the current yaw angle and the desired yaw angle, and integrate the yaw angle difference to obtain the yaw angle integral term; Determine a horizontal control gain that matches the current lateral velocity, the current yaw rate, the yaw angle difference, and the yaw angle integral term; Based on the horizontal control gain, determine the horizontal target terms that match the current lateral velocity, the current yaw rate, the yaw angle difference, and the yaw angle integral term, respectively. Adding all the horizontal target items together yields the virtual command rudder angle in the horizontal direction.

3. The method according to claim 1, characterized in that, When the direction is the roll direction, the current state data includes the current roll angular velocity and the current roll angle, and the desired state data includes the desired roll angle; the method for determining the virtual command rudder angle includes: Determine the roll angle difference between the current roll angle and the desired roll angle, and integrate the roll angle difference to obtain the roll angle integral term; Determine a roll direction control gain that matches the current roll rate, the roll angle difference, and the roll angle integral term; Based on the roll direction control gain, roll target terms that match the current roll angular velocity, the roll angle difference, and the roll angle integral term are determined respectively. Add all the roll target items together to obtain the virtual command rudder angle in the roll direction.

4. The method according to claim 1, characterized in that, Determining the target residual value between the virtual command rudder angle and the actual drive rudder angle in different directions includes: For any direction, determine the error difference between the virtual command rudder angle and the actual drive rudder angle; Subtract the disturbance term corresponding to any direction from the error difference to obtain the initial residual value; The initial residual value is filtered to obtain the filtered target residual value.

5. An apparatus for implementing the servo motor fault detection method according to any one of claims 1-4, characterized in that, The device, applied to autonomous underwater vehicles, includes: The virtual command determination unit is used to acquire the current state data and expected state data of the autonomous underwater vehicle, and determine the virtual command rudder angle in different directions based on the current state data and the expected state data; The actual command determination unit is used to determine the actual command rudder angle corresponding to the virtual command rudder angle; The fault condition determination unit is used to determine the target residual value between the virtual command rudder angle and the actual drive rudder angle in different directions, and to determine the fault condition of the servo motor based on the target residual value and a preset residual threshold. The jamming angle determination unit is used to determine the jamming angle corresponding to the faulty servo based on the actual commanded rudder angle and the target residual value when the servo is faulty.

6. A computer device, characterized in that, include: A memory and a processor are interconnected, the memory storing computer instructions, and the processor executing the computer instructions to perform the servo fault detection method according to any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions for causing the computer to execute the servo fault detection method according to any one of claims 1 to 4.

Images