Underwater unmanned vehicle anti-interference control method
By evaluating and correcting the measured depth values on the underwater unmanned aircraft, the problem of measurement errors in the disturbed environment is solved, and the control accuracy and stability of the underwater unmanned aircraft are improved.
Patent Information
- Application Number
- CN202510093736.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-01-21
- Publication Date
- 2025-05-02
AI Technical Summary
During underwater navigation, underwater unmanned vehicles are disturbed by factors such as the external environment and their own electromagnetic compatibility, resulting in sensor measurement errors and affecting the control performance of the vehicles.
By carrying sensors, the measured depth values and motion parameters of the underwater unmanned aircraft are obtained, the theoretical depth values are calculated based on the virtual model, and the correct probability of the measured depth values is evaluated, and the fusion correction of the measured depth values is carried out. Finally, the pitch rudder angle output value of the fused PID controller and the virtual controller is used to control the attitude of the aircraft.
It effectively reduces the measurement error of the sensor in an interfering environment, improves the control accuracy and stability of underwater unmanned vehicles, and ensures the safety of the vehicle in a deep-sea environment and fixed-depth navigation performance.
Smart Images
Figure CN119916832A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of underwater unmanned vehicle control, and in particular to an underwater unmanned vehicle anti-interference control method. Background Art
[0002] Underwater unmanned vehicles are an important type of unmanned equipment that can replace humans in marine environment exploration. Due to the complexity and variability of the underwater environment, underwater unmanned vehicles will be disturbed by various external environments and their own electromagnetic compatibility during underwater navigation, resulting in measurement errors in the sensors they carry, which in turn affects the working performance of the underwater unmanned vehicles.
[0003] The traditional method reduces the impact of measurement errors by filtering the sensor's measurement values. However, since the sensor's measurement values themselves have errors, the filtered measurement values cannot reflect the actual situation of the underwater unmanned vehicle, affecting the control performance of the underwater unmanned vehicle. Summary of the invention
[0004] In view of the above problems and technical requirements, this application proposes an anti-interference control method for underwater unmanned vehicles. The technical solution of this application is as follows:
[0005] An anti-interference control method for an underwater unmanned vehicle comprises the following steps:
[0006] The actual depth value of the underwater unmanned vehicle at any time t is obtained through the onboard sensor and measured values of motion parameters;
[0007] According to the measured value of the motion parameter at time t and the depth output value z at time t-1 t-1 Calculate the theoretical depth of the underwater unmanned vehicle at time t
[0008] According to the measured depth values at each moment and the theoretical depth value at moment t Evaluate the measured depth at time t The correct probability P t ;
[0009] According to the correct probability P t The theoretical value of depth at time t and the measured depth at time t Fusion is performed to obtain the depth output value z at time t t ; Among them, the measured depth value at time t The correct probability P t The larger the value, the greater the measured depth at time t. The more accurate, the deeper the theoretical value Output value z for depth tThe lower the contribution;
[0010] Output value z according to the depth at time t t Use PID controller to determine the pitch angle output value σ at time t t , and output the value σ according to the pitch rudder angle t Control the attitude of underwater unmanned vehicles.
[0011] A further technical solution is to evaluate the measured depth value at time t The correct probability P t include:
[0012] According to the measured depth at time t The measured depth value at time t is determined by comparing the changes in the measured depth values at historical moments The measured change probability P t 1 , measured depth value The greater the change in the measured depth value relative to the historical moment, the greater the measured change probability P t 1 The smaller;
[0013] According to the theoretical value of depth at time t The measured depth at time t The difference determines the theoretical depth value at time t Theoretical difference probability P t 2 , measured depth value and depth theoretical value The greater the difference between them, the greater the theoretical difference probability P t 2 The smaller;
[0014] Take the measured change probability P t 1 and the theoretical difference probability P t 2 The smaller value of is taken as the measured depth value at time t The correct probability P t .
[0015] Its further technical solution is to fuse the depth output value at time t
[0016] A further technical solution is to determine the pitch rudder angle output value σ at time t t Also includes:
[0017] Output value z according to the depth at time t t and target depth z d The difference between the two values is used to determine the actual control value of the pitch angle using the PID controller.
[0018] Output value z according to the depth at time t t and target depth z d The difference between the measured values of the motion parameters at time t is combined with the kinematic model of the underwater unmanned vehicle to determine the theoretical control value of the pitch rudder angle.
[0019] According to the correct probability P t Actual control value of pitch rudder angle and the theoretical control value of pitch rudder angle Fusion gets the pitch angle output value σ t .
[0020] A further technical solution is that the measured values of the motion parameters at time t include the measured value of the forward speed Lateral velocity measured value Measured vertical velocity Measured value of heel angular velocity Measured value of pitch angular velocity
[0021] Determine the theoretical control value of pitch and rudder angle based on the kinematic model of underwater unmanned vehicle for:
[0022]
[0023] Among them, Δt is the target control time, k1, k2, k3, k4, and k5 are constant parameters determined based on the kinematic model of the underwater unmanned vehicle.
[0024] A further technical solution is to fuse the pitch rudder angle output value σ t include:
[0025] According to the correct probability P t Actual control value of pitch rudder angle and the theoretical control value of pitch rudder angle The fusion rudder angle σ is obtained by fusion t ′;
[0026] For the fusion rudder angle σ t ′ is processed by limiting to obtain the pitch angle output value σ max is the maximum pitch angle of the underwater unmanned vehicle, σ max It is the minimum pitch and rudder angle of the underwater unmanned vehicle.
[0027] Its further technical solution is to use the correct probability P t The actual control value of the pitch rudder angle at time t and the theoretical control value of pitch rudder angle Fusion gets fusion rudder angle
[0028] A further technical solution is to determine the measured depth value at time t The measured change probability P t 1 include:
[0029] The measured depth at time t The measured depth at time t-1 Absolute value of difference Exceeds the predetermined threshold V k1 When the measured change probability at time t is determined Otherwise, determine the measured change probability P at time t t 1 =1.
[0030] Its further technical solution is to determine the theoretical depth value at time t Theoretical difference probability P t 2 include:
[0031] The measured depth at time t The theoretical depth value at time t Absolute value of difference Exceeds the predetermined threshold V k2 When the theoretical difference probability at time t is determined Otherwise, determine the theoretical difference probability P at time t t 2 =1.
[0032] A further technical solution is that the measured values of the motion parameters at time t include the measured values of the vertical velocity Calculate the theoretical depth value at time t
[0033] The beneficial technical effects of this application are:
[0034] The anti-interference control method for underwater unmanned vehicles proposed in this application can fundamentally solve the problem of inaccurate sensor measurements in interference environments compared to traditional filtering processing methods. A virtual model is used to calculate the theoretical depth value in real time and the correct probability of the actual depth value is evaluated in combination with the actual depth value at historical moments. The actual depth value is corrected according to the correct probability to ensure the accuracy of the depth output value input to the controller. This solves the problem of underwater unmanned vehicles losing control due to sensor measurement errors caused by external environment and electromagnetic interference of the sensor itself, and is the basis for ensuring the stability and safety of underwater unmanned vehicles.
[0035] The theoretical control value of the pitch rudder angle calculated by the virtual model controller is integrated with the actual control value of the pitch rudder angle calculated by the PID controller, which fully combines the advantages of the virtual controller being less affected by inaccurate measurements and the PID controller being less affected by inaccurate models, further improving the control accuracy of the underwater unmanned vehicle. In addition, by limiting the fused rudder angle, the problem of excessive output value of the pitch rudder angle caused by interference is further prevented. Through multiple means, layer-by-layer control ensures the safe and stable operation of underwater unmanned vehicles, which is of great significance for the depth-fixing navigation of underwater unmanned vehicles in deep-sea environments. BRIEF DESCRIPTION OF THE DRAWINGS
[0036] Figure 1 It is a structural diagram of the anti-interference control method of underwater unmanned vehicle.
[0037] Figure 2 It is a flow chart of the anti-interference control method of underwater unmanned vehicle.
[0038] Figure 3 It is a flow chart for evaluating the probability that the measured depth value at time t is correct. DETAILED DESCRIPTION
[0039] The specific implementation of the present application is further described below in conjunction with the accompanying drawings.
[0040] This application proposes an anti-interference control method for underwater unmanned vehicles, which improves the accuracy of the output value of the measurement system by fusing and correcting the measured values of the sensors in the measurement system under strong interference conditions. Figure 1 , mainly includes perception module, fusion module and control module.
[0041] The perception module is composed of a measurement system composed of actual sensors carried by the underwater unmanned vehicle and a virtual model built based on the kinematic model to measure the posture of the underwater unmanned vehicle itself. The actual measurement value is obtained by using the measurement system, and the theoretical value is calculated by using the virtual model. The operating status of the underwater unmanned vehicle is fully analyzed by combining the virtual and the real. The sensors of the measurement system include depth gauges, attitude sensors, speed sensors, etc. to measure the depth, attitude and speed of the underwater unmanned vehicle. The virtual model is a mathematical model that is always running and can reflect the real motion state of the underwater unmanned vehicle. The virtual model needs to receive part of the output value of the measurement system to calculate the theoretical value.
[0042] The expression of the virtual model based on the kinematic model of the underwater unmanned vehicle is:
[0043]
[0044] Wherein, z is the depth measured by the depth gauge, w is the vertical velocity, v is the lateral velocity, u is the forward velocity, and the vertical, lateral and forward velocities are all measured by the velocity sensor. k1, k2, k3, k4 and k5 are constant parameters determined based on the kinematic model of the underwater unmanned vehicle. p is the roll angular velocity, q is the pitch angular velocity, and the roll and pitch angular velocities are measured by the attitude sensor. τ w is the rudder control force generated by the pitch rudder angle σ.
[0045] The fusion module integrates the evaluation and data fusion functions. In the traditional method, the input of the controller is the measured value of the measurement system. However, when the measurement system is disturbed and the measured value is incorrect, the data input to the controller will cause the controller output value to be abnormal, thereby affecting the working performance of the underwater unmanned vehicle. Therefore, in order to solve this problem, the present application uses the fusion module to evaluate the accuracy of the measured value based on the measured value of the measurement system and the theoretical value calculated by the virtual model. When the measured value is correct, it is directly output to the controller for use to enhance real-time performance; when the measured value is wrong due to interference factors, the measured value is fused with the theoretical value to correct the erroneous data of the measured value and then output to the controller for use.
[0046] The control module uses the controller to calculate the corresponding control quantity and control the position and posture of the underwater unmanned vehicle. The control module includes a conventional PID controller and a virtual controller built based on a virtual model. The correction data of the error data is used as input by the fusion module and the two controllers are flexibly combined to achieve precise control output.
[0047] Based on the above structure, the depth control of the underwater unmanned vehicle is realized by an anti-interference control method of the underwater unmanned vehicle proposed in this application. Please refer to Figure 2 The flowchart shown in the figure has the following specific steps:
[0048] Step 1: Obtain the measured depth value z of the underwater unmanned vehicle at any time t through the onboard sensor t 0 and measured values of motion parameters.
[0049] Step 2: Based on the measured values of the motion parameters at time t and the depth output value z at time t-1 t-1 Calculate the theoretical depth of the underwater unmanned vehicle at time t
[0050] According to the expression of the virtual model, the theoretical depth value can be obtained by integrating the vertical velocity. In one embodiment, the measured value of the motion parameter at time t includes the measured value of the vertical velocity. Calculate the theoretical depth value at time t Among them, z t-1is the depth output value at time t-1, which is recursively calculated from the depth output value at time t-2 and the measured vertical velocity value at time t-1. The theoretical depth value at each moment is recursively calculated from the depth output value at the previous moment. For the initial moment t=0, the theoretical depth value is That is, the theoretical depth value at the initial moment is equal to the depth output value and equal to the measured depth value.
[0051] Step 3: Based on the measured depth values at each moment and the theoretical depth value at moment t Evaluate the measured depth at time t The correct probability P t .
[0052] The actual depth value can be determined by analyzing the fluctuation of the overall or local data of the actual depth value measured by the measurement system at each moment. The correct probability can also be obtained by observing the theoretical depth value at time t. Measured value of depth The difference in depth is used to evaluate the measured value In one embodiment, the depth measured value at time t is evaluated. The correct probability P t Methods such as Figure 3 As shown, the specific process is:
[0053] (1) According to the measured depth value at time t The measured depth value at time t is determined by comparing the changes in the measured depth values at historical moments The measured change probability P t 1 , measured depth value The greater the change in the measured depth value relative to the historical moment, the greater the measured change probability P t 1 The smaller.
[0054] Since the number of abnormal situations caused by interference is relatively small compared to normal situations during the navigation of underwater unmanned vehicles, the accuracy of the data can be evaluated by observing the changes in the data at each moment. The measured depth at time t-1 Absolute value of difference Exceeds the predetermined threshold V k1 When the measured change probability at time t is determined Otherwise, determine the measured change probability P at time t t 1 =1, indicating the actual depth value at this time is a normal value. Among them, the predetermined threshold V k1The settings can be customized according to the actual application. The measured change probability can characterize the correctness of the measured depth value. Exceeds the predetermined threshold V k1 When the depth is measured It is an outlier and the larger the difference, the smaller the probability of its correctness.
[0055] (2) According to the theoretical value of depth at time t The measured depth at time t The difference determines the theoretical depth value at time t Theoretical difference probability P t 2 , measured depth value and depth theoretical value The greater the difference between them, the greater the theoretical difference probability P t 2 The smaller.
[0056] On the other hand, the accuracy of the measured depth value can be evaluated by comparing the difference between the theoretical depth value calculated by the virtual model and the measured depth value. The theoretical depth value at time t Absolute value of difference Exceeds the predetermined threshold V k2 When the theoretical difference probability at time t is determined Otherwise, determine the theoretical difference probability P at time t t 2 =1, indicating the actual depth value z at this time t 0 is a normal value. The theoretical difference probability can also characterize the correctness of the measured depth value. Exceeds the predetermined threshold V k2 When the measured depth value z t 0 It is an outlier and the larger the difference, the smaller the probability of its correctness.
[0057] (3) Take the measured change probability P t 1 and the theoretical difference probability P t 2 The smaller value of is taken as the measured depth value at time t The correct probability P t .
[0058] In order to strictly control the accuracy of the depth output value and fully combine the output results of the measurement system and the virtual model, only when the measured change probability and the theoretical difference probability are both 1, the correct probability of the measured depth value is 1, and the measured depth value can be considered completely correct.
[0059] Step 4: According to the correct probability Pt The theoretical value of depth at time t and the measured depth z at time t t 0 Fusion is performed to obtain the depth output value z at time t t ; Among them, the measured depth value at time t The correct probability P t The larger the value, the greater the measured depth at time t. The more accurate, the deeper the theoretical value Output value z for depth t The lower the contribution.
[0060] In one embodiment, with a correct probability P t The theoretical depth value at time t is weighted and the measured depth at time t Fusion is performed to obtain the depth output value at time t When the correct probability P t =1, indicating the actual depth value at this time This is absolutely correct. The depth output value after data fusion will be output based on the actual depth measured by the measurement system. Otherwise, it will be output based on the correct probability fusion of a part of the virtual model's depth theoretical value.
[0061] Step 5: Output value z according to the depth at time t t Use PID controller to determine the pitch angle output value σ at time t t , and output the value σ according to the pitch rudder angle t Control the attitude of underwater unmanned vehicles.
[0062] In order to further improve the control performance and avoid the PID controller being easily affected by inaccurate measurements, which may cause the calculated control quantity to deviate and affect the control performance of the underwater unmanned vehicle, the present application improves the accuracy of the calculation results by fusing the calculation results of the virtual controller with the calculation results of the PID controller. In one embodiment, the pitch rudder angle output value σ at time t is determined. t Also includes:
[0063] (1) Output value z according to the depth at time t t and target depth z d The difference between the two values is used to determine the actual control value of the pitch angle using the PID controller. The specific expression is:
[0064]
[0065] Among them, k p is the proportional coefficient of the PID controller, k d is the differential coefficient of the PID controller, kI is the integral coefficient of the PID controller, is the differential of the depth output value at time t, is the differential of the target depth.
[0066] (2) Output value z according to the depth at time t t and target depth z d The difference between the measured values of the motion parameters at time t is combined with the kinematic model of the underwater unmanned vehicle to determine the theoretical control value of the pitch rudder angle.
[0067] The measured values of motion parameters at time t include the measured value of forward velocity Lateral velocity measured value Measured vertical velocity Measured value of heel angular velocity Measured value of pitch angular velocity Determine the theoretical control value of pitch and rudder angle based on the virtual model constructed based on the kinematic model of the underwater unmanned vehicle for:
[0068]
[0069] Among them, Δt is the target control time, k1, k2, k3, k4, and k5 are constant parameters determined based on the kinematic model of the underwater unmanned vehicle.
[0070] (3) According to the correct probability P t Actual control value of pitch rudder angle and the theoretical control value of pitch rudder angle Fusion gets the pitch angle output value σ t .
[0071] Since the PID controller is more affected by inaccurate measurement, while the model controller is less affected by inaccurate measurement but easily affected by inaccurate model, it is necessary to merge the actual control value of the pitch rudder angle calculated by the PID controller with the theoretical control value of the pitch rudder angle calculated by the model controller to fully combine the respective advantages of the two controllers.
[0072] In one embodiment, according to the correct probability P t Actual control value of pitch rudder angle and the theoretical control value of pitch rudder angle σ t 2 The fusion rudder angle σ is obtained by fusion t ′. In order to further prevent the problem of excessive control quantity output by the controller due to calculation errors caused by interference and calculation accuracy, the fusion rudder angle σ t ′ is processed by limiting to obtain the pitch angle output value σmax is the maximum pitch angle of the underwater unmanned vehicle, σ max It is the minimum pitch rudder angle of the underwater unmanned vehicle. When the calculated fusion rudder angle exceeds the set upper and lower limits of the pitch rudder angle, the maximum upper and lower limit pitch rudder angle is output, and the fusion rudder angle within the upper and lower limits of the pitch rudder angle is output at this fusion rudder angle to ensure that the output pitch rudder angle is within the operable range. The upper and lower limits of the pitch rudder angle are set according to the operable range of the actual pitch rudder angle.
[0073] Similarly, the fusion of pitch and rudder angles can also be calculated using the correct probability P t As weights, in one embodiment, according to the correct probability P t The actual control value of the pitch rudder angle at time t and the theoretical control value of pitch rudder angle Fusion gets fusion rudder angle
[0074] The above is only a preferred embodiment of the present application, and the present application is not limited to the above embodiments. It is understood that other improvements and changes directly derived or associated by those skilled in the art without departing from the spirit and concept of the present application should be considered to be included in the protection scope of the present application.
Claims
1. An anti-interference control method for an underwater unmanned vehicle, characterized in that: The anti-interference control method for underwater unmanned vehicle comprises: The depth of the underwater unmanned vehicle at any time t is obtained through the onboard sensor and measured values of motion parameters; According to the measured value of the motion parameter at time t and the depth output value z at time t-1 t-1 Calculate the theoretical depth of the underwater unmanned vehicle at time t According to the measured depth values at each moment and the theoretical depth value at moment t Evaluate the measured depth at time t The correct probability P t ; According to the correct probability P t The theoretical value of depth at time t and the measured depth at time t Fusion is performed to obtain the depth output value z at time t t ; Among them, the measured depth value at time t The correct probability P t The larger the value, the greater the measured depth at time t. The more accurate, the deeper the theoretical value Output value z for depth t The lower the contribution; Output value z according to the depth at time t t Use PID controller to determine the pitch angle output value σ at time t t , and output the value σ according to the pitch rudder angle t Control the attitude of underwater unmanned vehicles.
2. The anti-interference control method for underwater unmanned vehicles according to claim 1, characterized in that: Evaluate the measured depth at time t The correct probability P t include: According to the measured depth at time t The measured depth value at time t is determined by comparing the changes in the measured depth values at historical moments The measured change probability P t 1 , measured depth value The greater the change in the measured depth value relative to the historical moment, the greater the measured change probability P t 1 The smaller; According to the theoretical value of depth at time t The measured depth at time t The difference determines the theoretical depth value at time t Theoretical difference probability P t 2 , measured depth value and depth theoretical value The greater the difference between them, the greater the theoretical difference probability P t 2 The smaller; Take the measured change probability P t 1 and the theoretical difference probability P t 2 The smaller value of is taken as the measured depth value at time t The correct probability P t .
3. The anti-interference control method for underwater unmanned vehicles according to claim 1, characterized in that: Fusion obtains the depth output value at time t 4. The anti-interference control method for underwater unmanned vehicles according to claim 1, characterized in that: Determine the pitch rudder angle output value σ at time t t Also includes: Output value z according to the depth at time t t and target depth z d The difference between the two values is used to determine the actual control value of the pitch angle using the PID controller. Output value z according to the depth at time t t and target depth z d The difference between the measured values of the motion parameters at time t is combined with the kinematic model of the underwater unmanned vehicle to determine the theoretical control value of the pitch rudder angle. According to the correct probability P t Actual control value of pitch rudder angle and the theoretical control value of pitch rudder angle Fusion gets the pitch angle output value σ t .
5. The anti-interference control method for underwater unmanned vehicle according to claim 4, characterized in that: The measured values of motion parameters at time t include the measured value of forward velocity Lateral velocity measured value Measured vertical velocity Measured value of heel angular velocity Measured value of pitch angular velocity Determine the theoretical control value of pitch and rudder angle based on the kinematic model of underwater unmanned vehicle for: Among them, Δt is the target control time, k1, k2, k3, k4, and k5 are constant parameters determined based on the kinematic model of the underwater unmanned vehicle.
6. The anti-interference control method for underwater unmanned vehicle according to claim 4, characterized in that: Fusion gets the pitch angle output value σ t include: According to the correct probability P t Actual control value of pitch rudder angle and the theoretical control value of pitch rudder angle The fusion rudder angle σ is obtained by fusion t ′; The fusion rudder angle σ t ′ is processed by limiting to obtain the pitch angle output value σ max is the maximum pitch angle of the underwater unmanned vehicle, σ max It is the minimum pitch rudder angle of the underwater unmanned vehicle.
7. The anti-interference control method for underwater unmanned vehicle according to claim 6, characterized in that: According to the correct probability P t The actual control value of the pitch rudder angle at time t and the theoretical control value of pitch rudder angle Fusion gets fusion rudder angle 8. The anti-interference control method for underwater unmanned vehicles according to claim 2, characterized in that: Determine the measured depth at time t The measured change probability P t 1 include: The measured depth at time t The measured depth at time t-1 Absolute value of difference Exceeds the predetermined threshold V k1 When the measured change probability at time t is determined Otherwise, determine the measured change probability P at time t t 1 =1.
9. The anti-interference control method for underwater unmanned vehicles according to claim 2, characterized in that: Determine the theoretical depth value at time t Theoretical difference probability P t 2 include: The measured depth at time t The theoretical depth value at time t Absolute value of difference Exceeds the predetermined threshold V k2 When the theoretical difference probability at time t is determined Otherwise, determine the theoretical difference probability P at time t t 2 =1.
10. The anti-interference control method for underwater unmanned vehicle according to claim 1, characterized in that: The measured values of motion parameters at time t include the measured value of vertical velocity Calculate the theoretical depth value at time t