AUV (Autonomous Underwater Vehicle) path tracking method based on prediction sight angle guidance

Abstract

The invention discloses an AUV (Autonomous Underwater Vehicle) path tracking method based on predicted line-of-sight angle guidance, which comprises the following steps: acquiring a real-time navigation state through inertial navigation, acoustic positioning and DVL fusion, introducing a fixed time disturbance predictor to estimate a sideslip angle and an ocean current velocity component on line, adaptively adjusting a line-of-sight distance according to transverse error-navigational speed-curvature, and tracking the path of the AUV. Calculating a predicted line-of-sight angle and outputting an expected heading; and then a propeller and a control surface are driven by adopting a fixed time feedback control law, so that track and attitude error fixed time convergence of the under-actuated AUV under the conditions of strong time-varying ocean current and large sideslip is realized. The method does not need a side thruster or an ocean current prior model, is simple in algorithm structure, is suitable for a high-speed high-curvature track, and has the advantages of high speed, high steady-state precision, low energy consumption and high robustness.

Description

Technical Field

[0001] The present invention belongs to the technical field of ship control, and in particular relates to an AUV path tracking method based on predicted line of sight angle guidance. Background Art

[0002] Autonomous underwater vehicles (AUVs) are essential intelligent devices for missions such as marine resource surveying, seabed topography mapping, and maritime rescue. Due to the low-signal, high-perturbation, and unpredictable flow fields in seawater, ensuring that AUVs can navigate autonomously for extended periods while accurately tracking a predetermined trajectory in dynamic and complex environments remains a core challenge in the field of underwater robot control. From a control theory perspective, the path tracking problem can be divided into two subproblems: geometric guidance and attitude / velocity closed-loop control. The former determines the AUV's intended direction, while the latter ensures that the desired attitude and velocity are achieved quickly and stably under limited control torque. Traditional geometric guidance generally uses the straight-line line-of-sight guidance algorithm proposed by Pettersen and Fossen. However, in real sea conditions, sudden changes in currents can cause the AUV to exhibit significant sideslip angles, resulting in an imbalance in the "pre-aiming point-to-heading" consistency. Furthermore, the look-ahead distance Δ used in the LOS algorithm is fixed. A sudden change in path curvature or a change in speed often leads to overshoot or slow convergence due to a mismatch in Δ. In recent years, the industry has attempted to improve the attitude / velocity closed loop using methods such as PID, model predictive control, fuzzy control, and neural networks. However, if the geometric guidance layer underestimates the flow field disturbance, it will still "transfer" difficult-to-compensate drift errors to the inner loop, resulting in frequent and large controller movements, increased energy consumption, and a significant increase in track deviations in sections with large curvature. Therefore, how to simultaneously handle rapidly changing sideslip angles and time-varying ocean currents in the guidance layer and make real-time adaptive adjustments based on the errors has become a key technical bottleneck for improving AUV path tracking accuracy and convergence speed.

[0003] In response to the above bottlenecks, a number of patent technologies at home and abroad have improved LOS guidance or closed-loop control: for example, the Chinese invention patent with publication number CN118732687A discloses an AUV path tracking method and system based on adaptive line-of-sight angle guidance and PID control. By using adaptive LOS to calculate the desired heading under constant speed conditions and then tracking it with a PID cascade controller, it can suppress overshoot and compensate for drift angles. However, it still assumes that the ocean current is a slowly varying constant, and the adjustment rule of Δ only depends on the lateral error, without explicitly considering the path curvature and ship speed; the Chinese invention patent with publication number CN118707974A discloses an underwater vehicle guidance method based on an improved line-of-sight method, which splits the LOS into two targets: "geometric target point" and "directional target point", optimizes PID parameters based on performance indicators, and is applicable to a wider range of path types, but ocean current estimation still requires offline modeling and online compensation. Lag; Chinese invention patents with publication numbers CN118464024A / B and CN115167484A / B disclose an AUV trajectory tracking method based on a zero-valued neural network and a model prediction path tracking method for an autonomous underwater vehicle based on a neural network, respectively. From the perspective of the inner loop, a projected-elliptical zero-valued neural network (PEZNN) or RBF-MPC is introduced to offset the model uncertainty through online learning. Simulation shows that the steady-state error is reduced, but the learning process is sensitive to changes in the disturbance gradient, and there is a trade-off between real-time performance and energy consumption; Chinese invention patents with publication number CN114527772A disclose a method and system for designing an AUV trajectory tracking controller. On a fully driven platform, a virtual guide and adaptive control are combined to perform layered compensation for the path error, which can reduce sideslip to a certain extent, but it is difficult to fully implement when the platform is under-driven or the propulsion efficiency is limited. Furthermore, Chinese invention patent publication number CN119223275A discloses an underwater gravity matching search method based on salp swarm algorithm optimization, demonstrating the potential application of intelligent optimization algorithms in underwater positioning and control. However, it focuses on gravity field matching search and does not provide a systematic solution for ocean current sideslip and Δ adaptive adjustment. A review of existing literature and patents reveals that most solutions still rely on "small and slow" assumptions about sideslip angles, making them difficult to adapt to short-term, high-frequency disturbances. Common adjustment mechanisms often rely on a single error or static threshold, resulting in a slow response to the coupling of multiple factors such as velocity, curvature, and flow direction. Existing adaptive or intelligent methods generally leave disturbance compensation to the inner loop. Once the observation and compensation delay exceeds a certain threshold, the outer loop geometric guidance will still suffer from systematic bias, leading to an extreme trade-off between energy consumption and accuracy. Summary of the Invention

[0004] Aiming at the defects of the traditional line-of-sight guidance algorithm in the prior art in that the line-of-sight distance is fixed and the compensation for time-varying currents and rapidly changing sideslip angles is insufficient, resulting in large lateral overshoot, slow path convergence and high energy consumption of AUVs in high-speed-high curvature tracks and strong current environments, the present invention proposes an AUV path tracking method based on predicted line-of-sight angle guidance; by fusing state data in real time, a fixed-time disturbance predictor is constructed to estimate the sideslip angle and current velocity components online, and the predicted line-of-sight angle is dynamically adjusted by combining the three-parameter adaptive line-of-sight distance of lateral error-speed-curvature, and a time feedback control law is embedded in the attitude / speed closed loop to achieve fixed-time convergence of the track and attitude error of the under-actuated AUV under large sideslip and sudden current conditions, significantly improving the path tracking accuracy, robustness and economy in complex sea conditions. The AUV path tracking method based on predicted line-of-sight angle guidance described in the present invention specifically includes the following steps:

[0005] S1: The real-time navigation status data of the AUV is obtained in real time through the inertial navigation system, acoustic positioning system and Doppler velocimeter installed on the AUV. The real-time navigation status data includes position coordinates, attitude angle, relative speed and bow speed;

[0006] S2: Input real-time navigation status data and calculate the longitudinal tracking error and lateral tracking error of the AUV relative to the predefined desired path. The specific calculation formula is:

[0007]

[0008] Among them, x e and y e are the longitudinal and lateral tracking errors, is the predefined path tangent angle, x and y represent the current position coordinates of the AUV, x p and y p represents the expected coordinates;

[0009] S3: Input longitudinal and lateral tracking errors and real-time velocity data, use the fixed-time predictor to predict and estimate in real time and output the disturbance information caused by the unknown time-varying ocean current. The disturbance information includes sideslip angle, drift angle and ocean current velocity components. The calculation formula of the fixed-time predictor real-time prediction is:

[0010]

[0011] in, and represents the forecast estimation error, represents the actual speed of the underactuated AUV, u r and v r are the longitudinal and lateral velocities of the AUV, respectively; and represents the unknown current velocity; the sideslip angle β r =arctan(v r / u r );u p is the virtual control input; φ represents the heading angle of the AUV; k x1 、k y1 、k x2 、k y2 , m, n are all designed normal numbers; for The first derivative of ; function sig(·)=|·|sign(·), sign is the sign function;

[0012] S4: Input the predicted disturbance information and current state, calculate the line of sight angle between the AUV and the preview point PLOS on the path based on the path geometry information, and calculate and output the desired heading angle in combination with the path tangent angle. The calculation formula of the line of sight angle between the AUV and the preview point PLOS is:

[0013]

[0014] in, is the sight angle between AUV and the preview point PLOS, α e is the designed virtual integral variable, m and n are both designed positive constants;

[0015] The adaptive line of sight distance is calculated based on the adaptive parameter calculated according to the lateral tracking error. The specific calculation formula is:

[0016]

[0017] Where Δ is the foresight distance, 0<Δmin≤Δ≤Δmax, φd is the desired heading angle; k Δ1 and k Δ2 is a designed positive constant;

[0018] S5: Design the control law based on the dynamic model, where the calculation formula of the control law is:

[0019]

[0020] Where c1, c2 are normal vectors; v = [u,υ,r] T is the velocity vector composed of the AUV's forward velocity u, lateral drift velocity υ, and yaw angular velocity r in the body coordinate system, is the acceleration vector of AUV in the body coordinate system; v e is the relative velocity error vector of the AUV in the body coordinate system, C(v) represents the Coriolis matrix and centripetal matrix, D(v) represents the damping matrix, and M represents the mass matrix;

[0021] S6: Implement closed-loop control, drive the rudder and thruster in real time according to the control law, control the actual heading and navigation attitude of the AUV, and make the actual track gradually approach the desired path.

[0022] As a technical preferred solution of the present invention, the specific mathematical model of the AUV in step S1 is:

[0023]

[0024] where η = [x, y, φ] T is the position and attitude of the AUV in a fixed coordinate system, v = [u r ,υ r ,r] T is the relative velocity vector of AUV in the body coordinate system, V c =[V x ,V y ,0] T is the ocean current velocity vector, τ=[τ u ,0,τ r ] T is the control input, consisting of longitudinal thrust and yaw moment, f d =[f u ,f v ,f r ] T is the environmental disturbance, M represents the mass matrix, C(v) represents the Coriolis matrix and the centripetal matrix, D(v) represents the damping matrix, and J(η) is the coordinate transformation matrix.

[0025] As a preferred technical solution of the present invention, step S2 further analyzes the rate of change of the longitudinal and lateral tracking errors by taking derivatives. The specific calculation formula is:

[0026]

[0027] in, is the actual speed of the AUV, and represents the unknown current velocity, β r =arctan(v r / u r ) is the sideslip angle, u p For virtual control input, the design is as follows:

[0028]

[0029] Among them, k θ is a constant.

[0030] As a technical preferred solution of the present invention, the calculation of disturbance information in step S3 further uses a current compensator to calculate the update rate, including the sideslip angle and current compensator:

[0031] The update law calculation formula of the sideslip angle is:

[0032]

[0033] The update law calculation formula of the ocean current compensator is:

[0034]

[0035] in, and is the forecast estimation error, Among them, k β1 , k β2 , k g1 , k g2 , k g3 , k g4 are all designed constants.

[0036] As a technical preferred solution of the present invention, the LOS guidance law based on the fixed time predictor in step S3 specifically calculates the longitudinal tracking error and the lateral tracking error of the AUV through the fixed time predictor to estimate the unknown sideslip angle and compensate for the environmental disturbance. The specific calculation formula is:

[0037]

[0038] in, The tangent angle of the predefined path.

[0039] As a preferred technical solution of the present invention, the calculation of the bow angular velocity control law in step S5 specifically includes the following steps:

[0040] The desired heading angular velocity is designed as the virtual control input. The calculation formula of the desired heading angular velocity is:

[0041]

[0042] in, is a designed positive constant;

[0043] Define the AUV heading angle tracking error, the specific calculation formula is:

[0044] φ e =φ-φ d

[0045] AUV lateral tracking velocity error and yaw angular velocity error:

[0046] ue =uu d ,r e =rr d

[0047] The Lyapunov function of the heading angle error is constructed and derived. Combined with the AUV dynamic model, the specific calculation formula is:

[0048]

[0049]

[0050] Among them, u and u d are the lateral velocity of the AUV and the desired lateral velocity, r and r respectively. d are the heading angular velocity and expected heading angular velocity of the AUV, is φ d The first derivative of .

[0051] Compared with the related prior art, the beneficial effects of the present invention are:

[0052] By using a fixed-time disturbance predictor to estimate and compensate for the sideslip angle and current components in real time, the guidance layer itself can respond to unknown and rapidly changing flow fields without the need for a priori current models or slow learning processes.

[0053] Adaptive forward-sight distance is dynamically adjusted according to lateral error, speed, and path curvature, enabling the algorithm to maintain a smooth trajectory during large deviations and automatically shorten the preview distance when entering fine segments, balancing steady-state accuracy and convergence speed.

[0054] The feedback law relies only on conventional attitude and velocity measurements, without relying on high-order derivatives or complex optimization solutions. It is easy to implement in real time on an embedded controller, reducing software and computing burdens.

[0055] It reduces rudder surface saturation and frequent large movements, reducing mechanical wear and energy consumption; at the same time, the error is directly absorbed in the guidance layer, avoiding the "error amplification-control compensation-error" cycle, and improving the overall closed-loop stability margin. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 A flowchart of an AUV path tracking method based on predicted line of sight angle guidance provided by the present invention;

[0057] Figure 2 Provides a diagram showing the performance of a straight line path in path following according to an embodiment of the present invention;

[0058] Figure 3 is a position tracking error diagram of the straight line path tracking according to an embodiment of the present invention;

[0059] Figure 4 It is an ocean current and estimation map provided by the embodiment of the present invention. DETAILED DESCRIPTION

[0060] The present invention is further described below with reference to the accompanying drawings and examples. However, the present invention can be implemented in many different ways and should not be construed as limited to the illustrated embodiments; rather, these embodiments provide those skilled in the art with implementation methods that meet applicable legal requirements.

[0061] Example 1: Figure 1 As shown, the present invention provides a process of an AUV path tracking method based on predicted line of sight angle guidance. This embodiment uses an AUV (test boat) in a long-range mapping mission as the background to illustrate the application process of the method of the present invention in an actual marine environment, which specifically includes the following steps:

[0062] S1: The real-time navigation status data of the AUV is obtained in real time through the inertial navigation system, acoustic positioning system and Doppler velocimeter installed on the AUV. The real-time navigation status data includes position coordinates, attitude angle, relative speed and bow speed. The specific mathematical model of the AUV is:

[0063]

[0064] where η = [x, y, φ] T is the position and attitude of the AUV in a fixed coordinate system, v = [u r ,υ r ,r] T is the relative velocity vector of AUV in the body coordinate system, V c =[V x ,V y ,0] T is the ocean current velocity vector, τ=[τ u ,0,τ r ] T is the control input, consisting of longitudinal thrust and yaw moment, f d =[f u ,f v ,f r ] T is the environmental disturbance, M represents the mass matrix, C(v) represents the Coriolis matrix and the centripetal matrix, D(v) represents the damping matrix, and J(η) is the coordinate transformation matrix.

[0065] S2: Input real-time navigation status data and calculate the longitudinal tracking error and lateral tracking error of the AUV relative to the predefined desired path. The specific calculation formula is:

[0066]

[0067] Among them, x e and y e are the longitudinal and lateral tracking errors, is the predefined path tangent angle, x and y represent the current position coordinates of the AUV, x p and y p Represents the desired coordinates; the change rate of the longitudinal and lateral tracking errors is analyzed by derivation. The specific calculation formula is:

[0068]

[0069] in, is the actual speed of the AUV. The unknown ocean current speed includes: and β r =arctan(v r / u r ) represents the sideslip angle, u p For virtual control input, the design is as follows:

[0070]

[0071] Among them, k θ is a constant.

[0072] S3: Input tracking error and real-time velocity data, use the fixed-time predictor to predict and estimate in real time and output the disturbance information caused by the unknown time-varying ocean current. The disturbance information includes sideslip angle, drift angle and ocean current velocity component. The calculation formula of the fixed-time predictor real-time prediction is:

[0073]

[0074] in, and represents the forecast estimation error, represents the actual speed of the underactuated AUV, u r and v r are the longitudinal and lateral velocities of the AUV, respectively; and represents the unknown current velocity; the sideslip angle β r =arctan(v r / u r );u p is the virtual control input; φ represents the heading angle of the AUV; k x1 、k y1 、k x2 、k y2 , m, n are all designed normal numbers;

[0075] The calculation of the disturbance information further uses the current compensator to calculate the update rate, including the sideslip angle and the current compensator:

[0076] The update law calculation formula of the sideslip angle is:

[0077]

[0078] The update law calculation formula of the ocean current compensator is:

[0079]

[0080] in is the forecast estimation error, Among them, k β1 , k β2 , k g1 , k g2 , k g3 , k g4 The LOS guidance law based on the fixed time predictor specifically calculates the longitudinal tracking error and lateral tracking error of the AUV through the fixed time predictor to estimate the unknown sideslip angle and compensate for the environmental disturbance. The specific calculation formula is:

[0081]

[0082] in, The tangent angle of the predefined path.

[0083] S4: Input the predicted disturbance information and current state, calculate the line of sight angle between the AUV and the preview point PLOS on the path based on the path geometry information, and calculate and output the desired heading angle in combination with the path tangent angle. The calculation formula of the line of sight angle between the AUV and the preview point PLOS is:

[0084]

[0085] in, is the sight angle between AUV and the preview point PLOS, α e is the designed dummy integral variable;

[0086] The adaptive line of sight distance is calculated based on the adaptive parameter calculated according to the lateral tracking error. The specific calculation formula is:

[0087]

[0088] Where Δ is the foresight distance, 0<Δmin≤Δ≤Δmax, φd is the desired heading angle; k Δ1 and k Δ2 is a designed positive constant;

[0089] S5: Design the control law based on the dynamic model, where the calculation formula of the control law is:

[0090]

[0091] Among them, c1, c2 are normal vectors; v e is the relative velocity error vector of the AUV in the body coordinate system; the calculation of the bow angular velocity control law specifically includes the following steps:

[0092] The desired heading angular velocity is designed as the virtual control input. The calculation formula of the desired heading angular velocity is:

[0093]

[0094] in, is a designed normal number.

[0095] Define the AUV heading angle tracking error, the specific calculation formula is:

[0096] φ e =φ-φ d

[0097] AUV lateral tracking velocity error and yaw angular velocity error:

[0098] u e =uu d ,r e =rr d

[0099] The Lyapunov function of the heading angle error is constructed and derived. Combined with the AUV dynamic model, the specific calculation formula is:

[0100]

[0101]

[0102] Among them, u and u d are the lateral velocity of the AUV and the desired lateral velocity, r and r respectively. d are the heading angular velocity and expected heading angular velocity of the AUV, is φ d The first derivative of

[0103] Through the above control design, the heading angle error can converge to zero within a fixed time, achieving fixed-time convergence of the attitude angle error of the under-actuated AUV;

[0104] According to the designed virtual control input and dynamic model, the AUV's bow angular velocity control law is obtained to achieve precise control of the AUV's attitude angle.

[0105] S6: Implement closed-loop control, drive the rudder and thruster in real time according to the control law, control the actual heading and navigation attitude of the AUV, and make the actual track gradually approach the desired path.

[0106] Example 2: In this example, a propeller-rudder underactuated AUV with a length of 3m and a mass of 280kg was selected as the test platform to perform a 16km closed mapping track in a 90m water depth near the shore of a certain sea area. The test boat was equipped with a 200Hz fiber optic inertial navigation system, a 10Hz four-beam DVL and a 1Hz USBL acoustic positioning module. The three-source data were fused by an extended Kalman filter and output a complete six-degree-of-freedom state vector (position, heading angle, longitudinal velocity, lateral velocity and bow angular velocity) at 50Hz. The track is composed of 32 GPS waypoints, including a straight line segment, a circular arc segment with a radius of 300m and an "S"-shaped high curvature segment; real-time observations of sea conditions show that the lateral current velocity swings rapidly in the range of 0-1.9m / s, and the instantaneous flow direction deviation can reach up to ±45°.

[0107] During mission execution, the main control computer receives fusion status at a 50Hz frequency. First, the desired path tangent angle and reference coordinates are calculated based on the parameters of the current section, and longitudinal and lateral tracking errors are obtained in real time. The system then enters the fixed-time disturbance predictor phase: using offline tuning constants such as k(x1) = 0.8, k(x2) = 0.6, k(y1) = 0.9, k(y2) = 0.6, m = 1.2, and n = 0.8, the system feeds the measured errors and velocity into an observer, and online estimates of the sideslip angle and the longitudinal and transverse components of the current are generated. Field records indicate that these estimates converge to stable values ​​within 8 seconds. After receiving the disturbance information, the software adjusts the look-ahead distance Δ based on the lateral error, real-time speed, and curvature, smoothly varying it within the range of 6-42 meters. The predicted sight angle is then calculated and added to the path tangent angle to obtain the desired heading angle at the next moment. In order to avoid high-speed sudden turns, the system limits the expected heading angle change rate to within 0.25 rad / s, and the excess part is processed by hyperbolic tangent soft saturation.

[0108] The attitude-velocity closed-loop control thread runs at 10Hz. In the control law, the heading angle error feedback coefficient Set to 1.4, is 0.8, is 0.4; the velocity dynamic surface gain vector c1 is [0.6, 0, 0.4] T , c2 takes [0.4, 0, 0.3] T. The longitudinal thrust is provided by two stepless speed-controlled propellers in a coordinated manner, and the rudder angle command is distributed to the tail rudder within the mechanical limit of ±35°. During the entire 110-minute voyage, the test boat maintained a maximum lateral deviation of less than 2.1m under two 1.8m / s step crossflows and three sections of 25° / s lateral surges, and the bow angle error dropped to ±0.6° within 6s; compared with the historical data of the same boat using a constant Δ-LOS plus PID control, the average deflection angle of the rudder surface was reduced by about one-third, and the total power consumption was reduced by nearly 10%. The control log shows that the sideslip angle is estimated to rise to 22° instantaneously in the wave area and then quickly fall back, which verifies the fast response capability of the fixed-time disturbance compensation to large-scale sideslip, and further confirms the comprehensive advantages of the method of the present invention in taking into account both high-precision path tracking and low-energy operation under complex sea conditions.

[0109] Example 3: To further verify the formation tracking capability of the method of the present invention under the influence of time-varying ocean currents and model uncertainty, a high-fidelity MATLAB / Simulink simulation scenario of a three-boat formation was constructed: the pilot boat and two wing boats dived simultaneously at 30m in the negative direction of the x-axis, and then sailed 600m in the positive direction of the x-axis at a constant surge speed of 2m / s. The ocean current model was set to V x =0.5sin(0.1t)m / s, V y =0.1sin(0.1t)m / s, with its speed and direction changing slowly and continuously within a period of 62s, which can simulate the tidal surge in the nearshore mixed layer.

[0110] In addition to the ocean current, the model uncertainty term Δf=[2.5sin(0.5χ),2sin(0.5χ+(π / 6)),1.2cos(χ)]T is introduced, and the attitude angle perturbation δ d =[0,0,0] T As a constant structural error, the initial conditions of the three boats are uniformly set to [x(0), y(0), φ(0)] = [30m, 0m, 0m] T , initial body velocity vector [u, v, r] = [0.01 m / s, 0.01 m / s, 0 rad / s] T The wingmen are deployed in a "goose-fleet" formation: wingman 1 is 10 meters behind the lead boat and 5 meters to the right, while wingman 2 is 10 meters behind the lead boat and 5 meters to the left. The three wingmen share the same expected straight line path, but fixed virtual offsets are introduced in their local coordinate systems to maintain lateral and longitudinal spacing.

[0111] The control layer fully adopts the predicted sight angle guidance strategy and the double-power fixed time feedback law of the present invention.

[0112] Parameter adjustment follows the principle of "adjust the pilot boat first, and adjust the parameters of the companion boats in the same scale":

[0113] The fixed time disturbance predictor gain is kx1 =1.0,k x2 =0.7,k y1 =1.1,k y2 =0.7,m=1.2,n=0.8;

[0114] The sight distance coefficient is set to k Δ1 =18,k Δ2 =0.08,Δ min =6m, ensuring that Δ does not exceed 40m under the action of maximum side flow;

[0115] Heading angle feedback gain Initially set to (1.5, 0.9, 0.4), the wingman is scaled by 0.9. Velocity surface gain vector c1 = [0.65, 0, 0.45] T ,c2=[0.45,0,0.35] T The controller samples at 50 Hz and uses a fourth-order Runge-Kutta integration to approximate the nonlinear equations of motion with a step size of 0.02 s.

[0116] Within 300 seconds of simulation, the ocean current experienced four zero crossings, with maximum lateral velocity of 0.6 m / s and maximum downstream velocity of 0.5 m / s. Whenever the velocity peaked around t≈78 s and t≈235 s, the real-time sideslip angle estimate output by the disturbance predictor rose sharply from 0° to 17° within 6 seconds and then quickly fell back. The lateral error of the lead boat decreased from an initial 30 m to 0.12 m within 24 seconds and remained within a ±0.2 m range thereafter. Due to the initial formation offset, the error of the wingman boats initially showed a step of approximately 5 m, but converged to within ±0.25 m by 32 seconds. The maximum peak heading error was only 1.8°, and within 7 seconds, it entered the ±0.4° steady-state range. The wingman boats maintained a high degree of alignment with the lead boat, and their bowing angular velocity waveforms were smooth, without high-frequency spikes. The lateral spacing error between the three boats was stable at ±0.3m, the longitudinal spacing error was stable at ±0.5m, and there was no obvious drift or divergence in the formation geometry.

[0117] Thrust and rudder deflection records show that when entering the ocean current, the longitudinal propulsion power experienced two 15% increases, and the maximum lateral rudder deflection angle was 23°, both without reaching mechanical or electrical limits. During the period of ocean current speed drop, the controller automatically and gradually reduced the rudder amplitude and propulsion power to achieve energy recovery. Compared with the comparative simulation results using a fixed Δ(20m)LOS+PI controller in the same scenario, this method saved 12.4% of electricity on the pilot boat and an average of 11.7% on the wing boats. More importantly, the comparative scheme experienced a 2.6m lateral overshoot during the peak of the ocean current, resulting in a stretch of the formation, while the proposed method did not experience overshoot, indicating that the predicted line of sight angle guidance and adaptive Δ are more robust to large sideslip + formation coupled disturbances.

[0118] The position trajectory, lateral error and control input of the three boats in the straight path simulation are summarized (respectively as Figure 2 、 Figure 3 and Figure 4 It can be seen that: even in the face of obvious time-varying ocean currents, large initial deviations and model uncertainties, the proposed method still ensures that the lead boat can quickly and smoothly compress the body error while maintaining the geometric consistency between the wing boat and the virtual offset point; throughout the whole process, the error convergence speed, attitude stability and energy consumption performance are better than those of traditional fixed Δ-LOS or linear PI / PID control schemes, further verifying the promotion value of the method in formation path tracking scenarios.

[0119] The above embodiments merely illustrate the implementation methods of the present invention. Although the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that a person skilled in the art may make various modifications and improvements without departing from the scope of the present invention, and such modifications and improvements are all within the scope of protection of the present invention.

Claims

1. A path tracking method for an AUV based on predicted line of sight angle guidance, characterized by: The specific steps include: S1: The real-time navigation status data of the AUV is obtained in real time through the inertial navigation system, acoustic positioning system and Doppler velocimeter installed on the AUV. The real-time navigation status data includes position coordinates, attitude angle, relative speed and bow speed; S2: Input real-time navigation status data and calculate the longitudinal tracking error and lateral tracking error of the AUV relative to the predefined desired path. The specific calculation formula is: Among them, x e and y e are the longitudinal and lateral tracking errors, is the predefined path tangent angle, x and y represent the current position coordinates of the AUV, x p and y p represents the expected coordinates; S3: Input longitudinal and lateral tracking errors and real-time velocity data, use the fixed-time predictor to predict and estimate in real time and output the disturbance information caused by the unknown time-varying ocean current. The disturbance information includes sideslip angle, drift angle and ocean current velocity components. The calculation formula of the fixed-time predictor real-time prediction is: in, and represents the forecast estimation error, represents the actual speed of the underactuated AUV, u r and v r are the longitudinal and lateral velocities of the AUV, respectively; and represents the unknown current velocity; the sideslip angle β r =arctan(v r / u r );u p is the virtual control input; φ represents the heading angle of the AUV; k x1 、k y1 、k x2 、k y2 , m, n are all designed normal numbers; for The first derivative of ; function sig(·)=|·|sign(·), sign is the sign function; S4: Input the predicted disturbance information and current state, calculate the line of sight angle between the AUV and the preview point PLOS on the path based on the path geometry information, and calculate and output the desired heading angle in combination with the path tangent angle. The calculation formula of the line of sight angle between the AUV and the preview point PLOS is: in, is the sight angle between AUV and the preview point PLOS, α e is the designed virtual integral variable, m and n are both designed positive constants; The adaptive line of sight distance is calculated based on the adaptive parameter calculated according to the lateral tracking error. The specific calculation formula is: Where Δ is the foresight distance, 0<Δmin≤Δ≤Δmax, φd is the desired heading angle; k Δ1 and k Δ2 is a designed positive constant; S5: Design the control law based on the dynamic model, where the calculation formula of the control law is: Where c1, c2 are normal vectors; v = [u,υ,r] T is the velocity vector composed of the AUV's forward velocity u, lateral drift velocity υ, and yaw angular velocity r in the body coordinate system, is the acceleration vector of AUV in the body coordinate system; v e is the relative velocity error vector of the AUV in the body coordinate system, C(v) represents the Coriolis matrix and centripetal matrix, D(v) represents the damping matrix, and M represents the mass matrix; S6: Implement closed-loop control, drive the rudder and thruster in real time according to the control law, control the actual heading and navigation attitude of the AUV, and make the actual track gradually approach the desired path.

2. The AUV path tracking method based on predicted line of sight angle guidance according to claim 1, characterized in that: The specific mathematical model of AUV in step S1 is: where η = [x, y, φ] T is the position and attitude of the AUV in a fixed coordinate system, v = [u r ,υ r ,r] T is the relative velocity vector of AUV in the body coordinate system, V c =[V x ,V y ,0] T is the ocean current velocity vector, τ=[τ u ,0,τ r ] T is the control input, consisting of longitudinal thrust and yaw moment, f d =[f u ,f v ,f r ] T is the environmental disturbance, M represents the mass matrix, C(v) represents the Coriolis matrix and the centripetal matrix, D(v) represents the damping matrix, and J(η) is the coordinate transformation matrix.

3. The AUV path tracking method based on predicted line of sight angle guidance according to claim 1, characterized in that: Step S2 further analyzes the rate of change of the longitudinal and lateral tracking errors by taking derivatives. The specific calculation formula is: in, is the actual speed of the AUV, and represents the unknown current velocity, β r =arctan(v r / u r ) is the sideslip angle, u p For virtual control input, the design is as follows: Among them, k θ is a constant.

4. The AUV path tracking method based on predicted line of sight angle guidance according to claim 1, characterized in that: The calculation of disturbance information in step S3 further uses the current compensator to calculate the update rate, including the sideslip angle and the current compensator: The update law calculation formula of the sideslip angle is: The update law calculation formula of the ocean current compensator is: in, and is the forecast estimation error, Among them, k β1 , k β2 , k g1 , k g2 , k g3 , k g4 are all designed constants.

5. The AUV path tracking method based on predicted line of sight angle guidance according to claim 1, characterized in that: The LOS guidance law based on the fixed time predictor described in step S3 specifically calculates the longitudinal tracking error and lateral tracking error of the AUV through the fixed time predictor to estimate the unknown sideslip angle and compensate for the environmental disturbance. The specific calculation formula is: in, The tangent angle of the predefined path.

6. The path tracking method for an autonomous underwater vehicle (AUV) based on predicted line of sight angle guidance according to claim 1, characterized in that: The calculation of the bow angular velocity control law in step S5 specifically includes the following steps: The desired heading angular velocity is designed as the virtual control input. The calculation formula of the desired heading angular velocity is: in, is a designed positive constant; Define the AUV heading angle tracking error, the specific calculation formula is: f e =φ-φ d AUV lateral tracking velocity error and yaw angular velocity error: u e =u-u d ,r e =r-r d The Lyapunov function of the heading angle error is constructed and derived. Combined with the AUV dynamic model, the specific calculation formula is: Among them, u and u d are the lateral velocity of the AUV and the desired lateral velocity, r and r respectively. d are the heading angular velocity and expected heading angular velocity of the AUV, is φ d The first derivative of .

Images