Underwater unmanned vehicle navigation control method based on course deviation segmented self-adaptive PID (Proportion Integration Differentiation) parameter adjustment
By employing a segmented adaptive PID parameter adjustment method for heading deviation in an underwater unmanned vehicle, the problems of slow response speed and rudder angle oscillation in traditional PID control under multi-disturbance environments are solved, achieving a heading control effect with fast response, high steady-state accuracy, and low energy consumption.
Patent Information
- Application Number
- CN202511621159.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-11-07
- Publication Date
- 2026-01-16
AI Technical Summary
Traditional PID control in underwater unmanned vehicles suffers from slow response, rudder angle oscillation, high energy consumption, and insufficient control sensitivity. It is particularly difficult to achieve fast response and high-precision steady-state control in environments with multiple disturbances.
A method based on segmented adaptive PID parameter adjustment for heading deviation is adopted. By acquiring heading angle, angular velocity and depth information in real time, the control section is divided, and the proportional, integral and derivative coefficients and rudder angle limit values are adaptively selected in different sections. Combined with low-pass filtering and anti-saturation integral mechanism, the heading control achieves speed, stability and robustness.
It achieves rapid response of heading control, high steady-state accuracy, strong rudder angle oscillation suppression capability, and low energy consumption. It also maintains stability and robustness in multi-disturbance environments, reduces response time by half, achieves steady-state navigation deviation of less than ±0.5°, and reduces energy consumption by about 30%.
Smart Images

Figure CN121349128A_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The present application relates to the technical field of intelligent control and autonomous navigation of underwater unmanned submersible, more particularly to an underwater unmanned submersible navigation control method based on segmented adaptive PID parameter adjustment of heading deviation. BACKGROUND
[0002] At present, the underwater unmanned submersible (Autonomous Underwater Vehicle, AUV) is an important operation equipment in the fields of ocean exploration, surveying and mapping, communication relay, environmental monitoring and military reconnaissance. The AUV usually realizes accurate tracking of the specified route through autonomous path planning and closed-loop control, and the control accuracy and system robustness directly affect the quality of task execution. The traditional AUV control system generally adopts proportional-integral-derivative (PID) controller or control strategy based on line of sight (LOS) algorithm. However, under the condition of multiple disturbances of marine environment (ocean current, vortex, attitude coupling, etc.), the traditional PID control has the following significant deficiencies: 1. Fixed parameters are not suitable for multiple working conditions: under large deviation condition, the response speed is slow; under small deviation condition, the rudder angle is easy to oscillate or overshoot.
[0003] 2. The differential term amplifies noise: the measurement noise of the heading angle speed sensor is amplified after differentiation, resulting in control oscillation; 3. The integral term is easy to "wind-up": under the condition of continuous deviation, the integral term rapidly accumulates, causing rudder angle saturation, resulting in system oscillation and response delay.
[0004] 4. Lack of intelligent adjustment mechanism: single PID parameter is difficult to balance the rapidity at the initial stage of turning and the smoothness at the steady state stage.
[0005] For example, when the submersible enters a small radius turning or depth changing section, the traditional PID needs to be manually re-tuned, otherwise it will cause heading drift, attitude oscillation and energy consumption increase.
[0006] In view of the above problems, domestic and foreign researchers have proposed improved methods such as fuzzy PID, adaptive PID, neural network PID, but these methods are generally complex in calculation, rely on a large number of parameter training, and it is difficult to balance real-time performance and stability. Especially in the scene of AUV heading control, the control period is usually less than 0.05 seconds, and complex algorithms are difficult to complete parameter update and instruction output in a short period.
[0007] Therefore, it is urgent to provide a segmented PID control method with simple structure, strong real-time performance and adaptive adjustment based on heading deviation, which realizes rapid response, smooth transition and high-precision steady-state control in different deviation sections, so as to improve the heading control accuracy and navigation stability of the submersible. SUMMARY
[0008] Therefore, in view of the problems of response delay, obvious overshoot and large steady-state deviation of the traditional PID control under different heading deviation sections, the application provides an underwater unmanned submarine navigation control method based on heading deviation segmented adaptive PID parameter adjustment, which realizes the rapidity, stability and robustness of heading control through segmented scheduling and interpolation smooth switching of parameter groups. The method does not rely on complex fuzzy reasoning or neural network model, does not need external training data, and only updates the parameters adaptively according to the heading deviation and its change rate, which can maintain the heading stability in complex sea conditions and multi-disturbance environment, and significantly reduce the rudder angle oscillation and energy consumption. Thus, the comprehensive optimization of response speed, control accuracy and system energy consumption is realized.
[0009] In order to achieve the above purpose, the application adopts the following technical solutions: The underwater unmanned submarine navigation control method based on heading deviation segmented adaptive PID parameter adjustment comprises the following steps: S1, real-time acquisition of the heading angle, angular velocity, depth and speed information of the submarine, and calculation of the heading deviation; S2, division of the control section according to the absolute value of the heading deviation, and establishment of three control modes of large deviation section, medium deviation section and small deviation section; S3, adaptive selection of the proportional coefficient , the integral coefficient , the differential coefficient , the low-pass filter coefficient and the rudder angle limiting value for different control sections; S4, first-order low-pass filtering of the angular velocity signal through a low-pass filtering formula, and updating of the PID control amount based on the anti-saturation integral mechanism; S5, PID calculation of the control output to obtain the rudder angle control amount, and limiting to obtain the rudder angle control command; S6, sending the rudder angle control command to the actuator, and automatically adjusting the thrust level according to the size of the heading deviation to complete the adjustment and thrust execution of the submarine heading.
[0010] Optionally, the formula for calculating the heading deviation is:
[0011] wherein represents the heading deviation (the angle difference between the desired heading angle and the current heading angle), and the unit is (degree °); represents the desired heading angle, and the unit is (degree °); represents the current heading angle, and the unit is (degree °); the ( ) function is used to constrain the heading angle deviation in the range of [-180°, 180°].
[0012] Optionally, the step based on heading deviation The absolute value of the control zone is divided as follows: when At that time, it enters a large deviation phase; when At that time, it enters the middle deviation section; when At that time, it enters a small deviation range.
[0013] Optionally, the low-pass filter formula is:
[0014] in This represents the filtered differential feedback signal, in units of degrees per second (° / s). These are the filter coefficients (ranging from 0 to 1, with larger values indicating stronger filtering). This indicates the angular velocity of heading, expressed in degrees per second (° / s).
[0015] Optionally, the low-pass filtering formula performs first-order low-pass filtering on the angular velocity signal and updates the PID control quantity based on the anti-saturation integral mechanism:
[0016] in, This is the filtered angular velocity feedback value (unit: ° / s). The raw angular velocity (unit: ° / s) is measured in real time by the sensor. The filter coefficients (ranging from 0 to 1; values closer to 1 indicate stronger filtering and smoother response) are used for differential term calculations, replacing the direct use of the original angular velocity signal. This effectively suppresses high-frequency noise amplification and improves the stability and vibration resistance of rudder angle control.
[0017] Meanwhile, an anti-saturation mechanism is introduced in the integral stage. When the control output reaches the rudder angle limit value of the current section, a saturation prevention mechanism is implemented. Furthermore, if the sign of the heading deviation has not reversed, the accumulation of the integral is paused; when the deviation direction reverses or the output returns to the non-saturation range, the integral update is restarted. Its discrete form can be expressed as:
[0018] in, This is the cumulative amount of the integral term. The current heading deviation (°) To control the cycle (s). The current control output (°) is output. Through the low-pass filter and the anti-saturation integral cooperative mechanism, the oscillation of the rudder angle and the excessive integral can be effectively suppressed while the response speed is maintained, and the robustness and stability of the system in a multi-disturbance environment are improved.
[0019] Optionally, the PID calculates the control output to obtain the rudder angle control quantity, and specifically:
[0020] PID control output (rudder angle control quantity), unit (degree °); , , , PID proportional, integral, and differential coefficients; heading deviation, unit (degree °); filtered differential feedback signal, unit (degree per second ° / s); filtering coefficient (value range 0 to 1, the larger the value, the stronger the filtering); heading angle speed, unit (degree per second ° / s).
[0021] Optionally, the limiting amplitude is performed to obtain the rudder angle control instruction, and specifically:
[0022] wherein actual rudder angle command issued, unit (degree °); rudder angle limiting value corresponding to the section, unit (degree °).
[0023] Optionally, a linear interpolation smoothing transition mechanism is introduced to avoid switching mutation:
[0024] wherein actual parameter value at the current time; is the parameter value corresponding to the previous section; is the parameter value of the target section; is a transition factor, the value range is between 0 and 1. Among them It can be adaptively adjusted according to the deviation change rate, and is typically taken as 0.2 to 0.4.
[0025] According to the above technical solution, compared with the prior art, the present application provides an underwater unmanned submarine navigation control method based on heading deviation segmented adaptive PID parameter adjustment, which has the following beneficial effects: 1. Fast response speed: When the initial heading deviation is 30°, the controller can converge the deviation to within ±1° within 1 second, which is more than half the response time of traditional PID.
[0026] 2. High steady-state accuracy: During steady-state navigation, the heading deviation is kept within ±0.5° and the track deviation is less than 2 meters.
[0027] 3. Strong oscillation suppression capability: After adopting low-pass filtering and integral anti-saturation mechanism, the rudder angle oscillation amplitude is reduced by about 80% compared with fixed PID.
[0028] 4. Low energy consumption: After the deviation enters the small segment, the thrust is automatically reduced, and the average energy consumption is reduced by about 30%.
[0029] 5. Strong robustness and adaptability: It can still maintain rapid convergence and stable control under external disturbances (such as 0.3 m / s side flow).
[0030] 6. Highly scalable: Suitable for multi-submarine collaborative systems and unmanned surface vessel systems.
[0031] In summary, this invention realizes a novel closed-loop heading control method that combines heading deviation drive, segmented adaptive PID parameter adjustment, and multi-mode control output, exhibiting comprehensive performance of high response, high precision, and strong robustness. Attached Figure Description
[0032] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.
[0033] Figure 1 This is a flowchart illustrating the overall process of the method of the present invention. Figure 2 This diagram illustrates the segmentation for calculating heading deviation, with the horizontal axis representing heading deviation. The vertical axis represents the control section number, showing the range of the large deviation section, medium deviation section, and small deviation section; Figure 3 The diagram shows the adaptive PID parameter adjustment structure, illustrating the heading deviation input, section determination, parameter scheduling table, and signal processing of the P, I, and D channels. Figure 4 This diagram illustrates low-pass filtering and differential feedback processing, comparing the phase delay and noise reduction effects of the original angular velocity signal and the filtered signal. Figure 5 This is a schematic diagram of the integration anti-saturation logic, showing the conditions for pausing integration accumulation and resuming integration accumulation. Figure 6 PID parameter for dynamic switching of segments 、 、 interpolation transition curve of Figure 7 Simulation comparison curve of traditional PID and segmented adaptive PID method of the application under the same initial deviation condition. DETAILED DESCRIPTION
[0034] The technical solutions in the embodiments of the application will be apparently and completely described below with reference to the drawings in the embodiments of the application. Obviously, the described embodiments are only part of the embodiments of the application, rather than all the embodiments. Based on the embodiments in the application, all other embodiments obtained by a person of ordinary skill in the art without creative work belong to the protection scope of the application.
[0035] The embodiments of the application disclose an underwater unmanned submarine navigation control method based on segmented adaptive PID parameter adjustment of heading deviation, as shown in Figure 1 The heading control flow can generally include a route planning module, a segmented calculation module based on heading deviation, an adaptive PID parameter adjustment module, and a control module. The four modules are sequentially executed through information transmission, forming a complete closed-loop control method. Specifically, the method includes the following steps: Route planning module: The route planning module is used to generate the desired track direction of the submarine according to the task target. A straight line, a curve, or an arbitrary path planning algorithm (such as track point interpolation or spline path) can be used. The current heading angle, the heading angle speed, the speed, and other auxiliary state information of the submarine are collected every 0.05 s, and the desired heading angle is output in each control cycle. The output of this module is used as the input variable of the heading deviation calculation, ensuring that the control target of the submarine is clear.
[0036] Segmented calculation module based on heading deviation: The system measures the current heading angle of the submarine in real time and calculates the heading deviation as follows:
[0037] wherein represents the heading deviation (the angle difference between the desired heading angle and the current heading angle), and the unit is (degree °); represents the desired heading angle, and the unit is (degree °); represents the current heading angle, and the unit is (degree °); The ( ) function is used to constrain the heading angle deviation in the range of [-180°, 180°]; according to the absolute value of the heading deviation, such as Figure 2 the heading deviation is divided into three segments, and the PID parameters of the three segments are different.As shown, the control state is divided into three segments: large deviation segment, medium deviation segment, and small deviation segment.
[0038] When , enter the large deviation segment, fast correction zone; When , enter the medium deviation segment, transition adjustment zone; When , enter the small deviation segment, fine steady state zone.
[0039] In each cycle, the system outputs the current segment label zone ∈ {FAR, MID, NEAR} as an adaptive PID parameter adjustment module input.
[0040] Each segment corresponds to different control strategies and PID parameter weights. This segmentation mechanism simulates the steering habits of human drivers - when the deviation is large, it mainly corrects quickly, and when the deviation is small, it mainly stabilizes smoothly, thereby providing a segmented basis for adaptive PID adjustment.
[0041] Adaptive PID parameter adjustment module: after the heading deviation is segmented, the controller reads the corresponding parameter group (proportional coefficient , integral coefficient , differential coefficient , low-pass filter coefficient and output limit ) from the parameter scheduling table, and uses interpolation transition when switching between segments to avoid sudden changes in rudder angle caused by parameter mutations.
[0042] According to the segment label output by the segmentation calculation module, the corresponding parameter group is automatically selected from the parameter scheduling table. To avoid sudden changes, a linear interpolation smoothing transition mechanism is introduced:
[0043] Where represents the actual parameter value at the current time; is the parameter value corresponding to the previous segment; is the parameter value of the target segment; is the transition factor, with a value range of 0 to 1. Among them can be adaptively adjusted according to the deviation rate, typically 0.2 to 0.4.
[0044] The differential element uses a low-pass filter:
[0045] Where represents the filtered differential feedback signal, with a unit of (degree per second ° / s); Filtering coefficient (value range 0 to 1, the greater the filter is stronger) ; Indicates the heading angular velocity, unit (degree per second ° / s).
[0046] The low-pass filter and the differential feedback processing schematic diagram is shown in Figure 4 The phase delay and noise suppression effect of the original angular velocity signal and the filtered signal are compared; Prevent angular velocity noise amplification. Filter coefficient Depends on the section, near deviation section Greater to suppress oscillation.
[0047] Its control law is as follows:
[0048] Where Indicates the PID control output (rudder angle control quantity), unit (degree °) ; , , Indicates the PID proportion, integral, and differential coefficient; Indicates the heading deviation, unit (degree °) ; The angular velocity feedback is calculated by using a first-order low-pass filter to filter out high-frequency noise, and the formula is:
[0049] Where Indicates the filtered differential feedback signal, unit (degree per second ° / s) ; Filtering coefficient (value range 0 to 1, the greater the filter is stronger) ; Indicates the heading angular velocity, unit (degree per second ° / s).
[0050] In the large deviation section: the controller mainly relies on the proportional term To quickly correct, the integral term And the differential term Is closed to avoid overshoot; In the medium deviation section: enable proportional and differential coordination adjustment to enhance response speed; In the small deviation section: reduce the proportional coefficient, start the integral term correction, and realize high-precision steady-state maintenance.
[0051] In addition, as shown in Figure 5 The integral element adopts an anti-saturation mechanism: when the control output reaches the amplitude limit And the heading deviation has not reversed, the integral accumulation is suspended; When the deviation direction is reversed or the output returns to normal, the integral accumulation is restarted. This mechanism effectively suppresses the rudder angle saturation phenomenon caused by excessive integral, and prevents the wind-up effect.
[0052] Control module: the control module receives the adaptive PID output signalu t ), and perform amplitude constraint:
[0053] where is the actual issued rudder angle command, unit (degree °); is the amplitude value of the corresponding section of the rudder angle, unit (degree °). The final rudder angle command is generated, which is used to drive the direction adjustment of the submarine.
[0054] When the heading deviation is less than 2°, the system automatically reduces the thrust gear to achieve energy-saving steady-state maintenance. This logic ensures that unnecessary energy consumption is reduced in high-precision state, improving the overall efficiency of the system.
[0055] In a specific embodiment, the core task of the segmented calculation module based on the heading deviation is to convert the heading control problem into a segmented scheduling problem.
[0056] The implementation process is as follows: 1. Heading deviation measurement The current heading angle is measured by a gyrocompass or attitude sensor , and the desired heading angle is compared to obtain the heading deviation .
[0057] 2. Deviation section division logic
[0058] Adopt double threshold decision method: When , it is considered that the heading deviation is large, and the fast correction mode is entered; When , enter the medium response zone; When , enter the fine control zone.
[0059] 3. Dynamic switching and hysteresis design
[0060] To prevent the deviation from shaking at the boundary, a hysteresis zone is introduced. For example At the boundary of 8° and 20°, a hysteresis range of ±0.5° is added to make the section switching smoother.
[0061] 4. Section output
[0062] Output the current section label and the corresponding weight factor w_zone, which provides the basis for the subsequent adaptive PID parameter adjustment module.
[0063] In a specific embodiment, as Figure 3 As shown, after determining the section, the adaptive PID parameter adjustment module dynamically selects the parameter group according to the section information and performs real-time interpolation update on the parameters.
[0064] 1. Parameter scheduling table design
[0065] As shown in Table 1, three groups of core parameters are defined, corresponding to different sections: Table 1 PID parameter scheduling table for different sections
[0066] Note: , , , is a dimensionless coefficient, is a filter coefficient, unit: °.
[0067] 2. Interpolation smooth transition
[0068] When the section is switched from the FAR section to the MID section or the MID section to the NEAR section, the controller uses linear interpolation:
[0069] wherein represents the actual parameter value at the current time; is the parameter value corresponding to the previous section; is the parameter value of the target section; is a transition factor, with a value range of 0 to 1. Among them can be adaptively adjusted according to the deviation change rate, and the typical value is 0.2-0.4, which can ensure the response sensitivity and avoid the discontinuity of the control output. When the section is dynamically switched, the interpolation transition curve of the PID parameters , , is shown in Figure 6 .
[0070] 3. Differential filtering and vibration suppression mechanism
[0071] The angular velocity signal is filtered by a first-order low-pass filter to eliminate sensor noise interference; the filter coefficient takes a small value (0.15) in the large deviation area to enhance the sensitivity, and a large value (0.6) in the small deviation area to improve the stability.
[0072] 4. Integral anti-windup mechanism
[0073] To prevent integral wind-up, when the output reaches the amplitude limit and the deviation direction does not change, the controller stops the integral term accumulation until the deviation reverses or the output returns to the linear area and restarts the integral. This design greatly improves the stability of the rudder angle control.
[0074] 5 Output limiting and thrust adjustment
[0075] The final rudder angle output is output to the control module after being limited, and the thrust level is automatically adjusted according to the size of the heading deviation, so that energy-saving control is realized.
[0076] In one specific embodiment, simulation verification and performance comparison are also included as follows: The simulation model is realized based on a joint simulation platform of the vehicle dynamics equation and the control model. To verify the effectiveness of the method, the following simulation scenarios are designed: Path type: straight line segment + radius 100 m circular arc turning segment + stable holding segment Initial heading deviation: 30° Cross-flow disturbance: 0.3 m / s Control period: 0.05 s The comparison object is the traditional fixed parameter PID and the segmented adaptive PID method of the present application. The simulation results are shown in Table 2: Table 2 Simulation results
[0077] From Figure 7 and the results in Table 2, it can be seen that the method of the present application is significantly better than the traditional PID in response speed, stability accuracy, disturbance rejection performance and energy consumption.
[0078] In one specific embodiment, safety and robustness mechanism
[0079] To ensure control safety and long-term stable operation, the present application sets the following protection logic in the algorithm: 1. Rudder angle output protection: When ≥ , it is immediately limited to prevent the control command from exceeding the mechanical constraints.
[0080] 2. Hysteresis switching protection: A threshold hysteresis is maintained at the segment boundary to prevent frequent segment dithering switching.
[0081] 3. Dynamic integral suppression: When the heading deviation remains in the same direction for a long time, the integral accumulation term is suspended to avoid accumulated deviation.
[0082] 4. Abnormal detection mechanism: When the heading measurement value is lost or drifts beyond the set range, it immediately enters the safe holding mode (zero rudder angle output).
[0083] 5. Energy adaptive mechanism: When the heading deviation is less than the threshold, the thrust level is automatically reduced to realize energy-saving cruising.
[0084] These protective measures make the control method stable and reliable in the case of sea state disturbance, sensor noise or communication delay, with excellent robustness.
[0085] In a specific embodiment, it also includes expanding application scenarios
[0086] The segmented adaptive PID parameter adjustment method of the application has good portability and universality, and is not only suitable for underwater unmanned submarine heading control, but also can be popularized to the following scenarios: 1. heading keeping and turning control of surface unmanned vehicle (USV); 2. attitude stabilization and lateral orientation control of remote control vehicle (ROV); 3. path alignment control of seabed pipeline inspection robot; 4. track angle control and wind resistance stabilization adjustment of unmanned aerial vehicle (UAV).
[0087] Therefore, the method has wide application prospects in the fields of marine intelligent equipment, aerospace control, robot navigation, etc.
[0088] Comprehensive performance conclusion: Through simulation and experimental verification under multiple working conditions, the application realizes the following performance indicators: heading steady-state deviation: ≤ ± 0.5° response time: shortened by about 60% compared with traditional PID rudder angle oscillation amplitude: reduced by about 80% average energy consumption: reduced by about 30-35% anti-interference recovery time: shortened to 1 / 3 of the original The application takes heading deviation as a driving signal, combines segmented parameter scheduling, interpolation transition, filter vibration suppression and integral anti-saturation mechanism, and constructs a heading control method with simple structure, rapid response and high stability. The application can be used as a general heading control core algorithm, which is suitable for different control platforms and task scenarios, and has significant engineering application value and popularization prospect.
[0089] The embodiments in the specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple, and the related parts can be referred to the method part.
[0090] The foregoing description of the disclosed embodiments enables a person skilled in the art to make or use the application. Modifications of these embodiments will occur to persons of skill in the art, and that the appended claims are intended to cover all such modifications that do not depart from the true spirit and scope of the application. Therefore, the application is not limited to the embodiments shown but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. An underwater unmanned vehicle navigation control method based on course deviation segmented adaptive PID parameter adjustment, characterized in that, The application relates to a control method for a remotely operated vehicle (ROV) and a control system thereof. S1, acquiring the heading angle, angular velocity, depth and speed information of the ROV in real time, and calculating the heading deviation; S2, dividing the control section according to the absolute value of the heading deviation, and establishing three control modes of a large deviation section, a medium deviation section and a small deviation section; S3, adaptive selection of proportional coefficient for different control sections , integral coefficient , derivative coefficient , low-pass filter coefficient , and rudder angle limiting value ; S4, performing first-order low-pass filtering on the angular velocity signal through a low-pass filtering formula, and updating the PID control amount based on a saturation prevention integral mechanism; S5, calculating the control output of the PID, obtaining the rudder angle control amount, and performing amplitude limiting to obtain the rudder angle control instruction; S6, sending the rudder angle control instruction to an executing mechanism, automatically adjusting the thrust level according to the size of the heading deviation, and completing the adjustment of the heading of the ROV and the execution of the thrust.
2. The underwater unmanned vehicle navigation control method based on the heading deviation segmented adaptive PID parameter adjustment according to claim 1, characterized in that, The formula for calculating the heading deviation is: wherein represents a heading deviation; represents a desired heading angle; represents a current heading angle; The ( ) function is used to constrain the heading angle deviation in the range [-180°, 180°].
3. The underwater unmanned vehicle navigation control method based on the heading deviation segmented adaptive PID parameter adjustment according to claim 1, characterized in that, The absolute value of the heading deviation The control section is divided according to the absolute value of the heading deviation When a large deviation section is entered; When the middle deviation section is entered; When a small deviation section is entered.
4. The underwater unmanned vehicle navigation control method based on the heading deviation segmented adaptive PID parameter adjustment according to claim 1, characterized in that, The low-pass filtering formula is: wherein denotes the filtered differential feedback signal; is a filter coefficient; denotes the yaw rate.
5. The underwater unmanned vehicle navigation control method based on the heading deviation segmented adaptive PID parameter adjustment according to claim 1, characterized in that, The low-pass filtering formula performs first-order low-pass filtering on the angular velocity signal, and updates the PID control amount based on a saturation prevention integral mechanism: wherein, is the filtered angular velocity feedback value, is the raw angular velocity measured by the sensor in real time, is the filter coefficient; the filter result is used for the calculation of the differential term to replace the direct use of the raw angular velocity signal; at the same time, the anti-saturation mechanism is introduced into the integral link, and when the control output reaches the rudder angle limiting value of the current section and the sign of the heading deviation does not reverse, the accumulation of the integral quantity is suspended; when the deviation direction reverses or the output returns to the non-saturated interval, the integral update is restarted; the discrete form can be expressed as: wherein, is the accumulated amount of the integral term, is the current course deviation, is the control period, is the current control output.
6. The underwater unmanned vehicle navigation control method based on heading deviation piecewise adaptive PID parameter adjustment according to claim 1, characterized in that, The PID calculates the control output to obtain the rudder angle control amount, and the specific process is: wherein represents a PID control output, i.e. a rudder angle control variable; , , represents a PID proportionality, integration, differentiation coefficient; represents a course deviation; represents a filtered differential feedback signal; is a filter coefficient; represents a course angular velocity.
7. The underwater unmanned vehicle navigation control method based on the heading deviation segmented adaptive PID parameter adjustment according to claim 1, characterized in that, The amplitude limiting is performed to obtain the rudder angle control instruction, and the specific process is: wherein is the actual issued rudder angle control command; is the rudder angle limiting value for the corresponding section.
8. The underwater unmanned vehicle navigation control method based on the heading deviation segmented adaptive PID parameter adjustment according to claim 1, characterized in that, In addition, a linear interpolation smooth transition mechanism is introduced to avoid switching mutation. wherein represents the actual parameter value at the current time instant; is the parameter value of the previous section; is the parameter value of the target section; is a transition factor, which takes a value in the range between 0 and 1.
Citation Information
Cited By
Motion optimization method, system and device for amphibious robot
CN122239726A
