Under-actuated AUV (Autonomous Underwater Vehicle) trajectory tracking control method suitable for adaptive sight guidance method with high navigational speed and large turning radius

Abstract

The invention relates to an under-actuated AUV (Autonomous Underwater Vehicle) trajectory tracking control method suitable for a high-speed and large-turning radius self-adaptive sight guidance method, and belongs to the field of AUV control. The method comprises the steps of establishing an under-actuated AUV horizontal plane kinematics and dynamics model, obtaining a speed and course angle expectation model, designing a step-by-step decreasing expectation speed correction function, designing fuzzy control rule adjustment parameter self-adaption, designing a weighting function, designing a first-order inertia low-pass filter and the like, and correcting the real-time change of the expectation speed along with distance deviation. In the navigation process of the AUV, the navigational speed and course controller achieves double-closed-loop control. According to the method, the adaptability to the set complex navigation trajectory is good, the convergence speed is high, the proposed navigation speed ring continuously adjusts the expected navigation speed, the maneuverability of the AUV is effectively improved, and meanwhile the energy consumption of the AUV is saved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous underwater vehicle trajectory tracking, and in particular to an underactuated AUV trajectory tracking control method using an adaptive line-of-sight guidance method suitable for high speeds and large turning radii. Background Art

[0002] The trajectory tracking ability of an autonomous underwater vehicle (AUV) is an important factor affecting its mission execution efficiency and accuracy. However, due to the complexity of the underwater environment, interference uncertainty, and the underactuated characteristics of the AUV itself, trajectory tracking control has become a highly challenging problem. By using an improved line-of-sight guidance method and a double closed-loop control of heading and speed, accurate trajectory tracking on the horizontal plane of an underactuated AUV can be achieved, effectively improving the maneuverability of the AUV.

[0003] Using traditional pure pursuit methods and feedback linearization control, there are obviously the following problems:

[0004] First, for sharp turns and complex curve trajectories, the AUV may have difficulty accurately tracking, resulting in a large lateral error. In addition, the underactuated AUV may face the problem of insufficient lateral control when implementing this method.

[0005] Second, in the existing control methods for micro AUVs, by performing nonlinear dynamic inversion linearization on the nonlinear dynamic model of the AUV, it requires accurate model parameters and has a high computational complexity, making it unsuitable for real-time control.

[0006] The underactuated characteristics of the AUV mean that the number of its actuators is less than the degrees of freedom of motion. Usually, the AUV can only control the longitudinal thrust and yaw moment, and cannot directly control the lateral motion. Coupled with the disturbances of the fluid in the underwater environment and the requirements of complex three-dimensional space trajectories, it is difficult for traditional trajectory tracking algorithms to achieve high-precision control in such a complex environment. In the traditional line-of-sight guidance method in a complex dynamic environment (such as ocean currents, waves, etc.), the heading adjustment of the AUV cannot immediately compensate for sudden trajectory deviations. In addition, there are also technical problems such as poor adaptability to curve trajectories, insufficient robustness to environmental disturbances, sensitivity to initial conditions, and the need for a fixed line-of-sight angle design. Summary of the Invention

[0007] The applicant provides an underactuated AUV trajectory tracking control method using an adaptive line-of-sight guidance method suitable for high speeds and large turning radii with a reasonable structure. This method has good adaptability to a set complex navigation trajectory and a fast convergence speed. The proposed speed loop continuously adjusts the desired speed, effectively improving the maneuverability of the AUV while saving the energy consumption of the AUV.

[0008] The technical solution adopted by the present invention is as follows:

[0009] An underactuated AUV trajectory tracking control method for an adaptive line-of-sight guidance method applicable to high sailing speeds and large turning radii, comprising the following steps:

[0010] Step 1: Establish the kinematic and dynamic models of the underactuated AUV in the horizontal plane;

[0011] Step 2: Through the acquisition of the real-time position and longitudinal speed information of the AUV itself, combined with the set desired trajectory, use the improved line-of-sight guidance method to derive the desired models of speed and heading angle;

[0012] Step 3: Design a desired speed correction function with a gradually decreasing value according to the distance between the AUV's real-time position and the desired track point.

[0013] Step 4: According to the speed and heading angle tracking errors, use a PID controller to construct a speed and heading double closed-loop controller, design fuzzy control rules to adjust parameter adaptively, and design a weight function;

[0014] Step 5: Design a first-order inertial low-pass filter, perform a limiting process on the heading angle control output, and suppress the step change of the desired heading angle caused by heading switching;

[0015] Further, in Step 1, the kinematic and dynamic models of the underactuated AUV in the horizontal plane are established as follows:

[0016] The kinematic model is:

[0017]

[0018] In the formula: x and y respectively represent the horizontal position coordinates of the AUV in the earth coordinate system ξEη, ψ is the yaw angle, are the first derivatives of x, y, and ψ respectively.

[0019] The dynamic model is:

[0020]

[0021] In the formula: u, v, and r are respectively the longitudinal, lateral linear velocities, and yaw angular velocity of the AUV in the body coordinate system XOY (the origin O is the buoyancy center of the AUV), are the accelerations corresponding to u, v, and r respectively, δ r is the vertical rudder angle, X prop is the thrust of the propeller, m is the mass of the AUV, I z is the moment of inertia of the AUV about the z-axis, is the added mass, X |u|u 、Y |v|v 、Y |r|r 、N|v|v , N |r|r is the damping force coefficient, X vr , X rr , Y ur , Y uv , N ur , N uv is the Coriolis force coefficient, is the vertical rudder effectiveness coefficient.

[0022] Furthermore, in step two, by collecting the real-time position and longitudinal speed information of the AUV itself, combined with the set desired trajectory, based on the speed and course angle desired model of the line-of-sight guidance method:

[0023] In the plane of the earth coordinate system ξEη,, from the target point P i (i = 0, 1, 2 …) form the desired straight-line trajectory. The real-time position of the AUV is P t =(x t , y t ), the coordinate points forming the desired trajectory segment are P k =(x k , y k ) and P k+1 =(x k+1 , y k+1 ), the heading azimuth angle formed by points P k , P k+1 is:

[0024] α k = a tan2(y k+1 - y k , x k+1 - x k )

[0025] The vertical deviation from the real-time position P t =(x t , y t ) of the AUV to the desired trajectory P k , P k+1 is y e (t)

[0026] y e (t) = (y t - y k ) cosα k -(x t - x k ) sinα k

[0027] Define the distance from the foot of the perpendicular of the real-time position of the AUV on the desired trajectory to the line-of-sight point P los as the forward-looking distance Δ, then the output desired course angle is:

[0028] ψ d = α k - a tan2(y e (t), Δ)

[0029] Introduce the vertical distance y e (t), and design a time-varying look-ahead distance function:

[0030] Δ = (Δ max - Δ min )exp(-λy e 2 (t)) + Δ min

[0031] Where Δmin and Δmax are the designed look-ahead distances respectively, generally selected as integer multiples of L; λ is the design coefficient, and the principle for the AUV to switch the desired trajectory:

[0032] (x t - x k+1 ) 2 + (y t - y k+1 ) 2 < R 2

[0033] It can make the AUV approach the desired route more smoothly, effectively reduce the overshoot of the position error, enable the AUV to track multiple target points, and complete all desired trajectory segments.

[0034] Furthermore, in step three, the proposed gradually decreasing desired speed correction function

[0035]

[0036] Among them, δ d is the distance from the current position of the AUV to the end point of the current trajectory segment, R, R', and R'' are the boundary distances of the three distance rings. Set three distance critical values and set the corresponding speed functions respectively to achieve the three-level distance speed adjustment control of the AUV.

[0037] Furthermore, in step four, the speed controller uses PID control, and the heading controller uses the combined control of PID and fuzzy PID. Design the fuzzy control law and design the control weight function:

[0038]

[0039] Among them, u c is the control output of the PID controller, u cf is the control output of the fuzzy PID, U(u c , u cf) is the final output of the controller after weight matching using the weight function.

[0040] Furthermore, in step five, a first-order inertial filter is designed for the output of the heading angle controller, and its mathematical model and parameters are as follows:

[0041] ψ d (t) = 0.8ψ d (t - 1) + 0.2ψ d (t - 1)

[0042] Perform a limiter processing on the output of the heading angle controller, and the saturation function is:

[0043]

[0044] When switching the desired trajectory, a step signal will appear, and the saturation function avoids an overly large control signal output by the heading angle controller.

[0045] The beneficial effects of the present invention are as follows:

[0046] The present invention provides an underactuated AUV trajectory tracking control method based on the line-of-sight guidance method to address the drawbacks such as complex navigation trajectory conditions and excessive overshoot of position errors. It has good adaptability and a fast convergence speed. The proposed speed loop continuously adjusts the desired speed, effectively improving the maneuverability of the AUV while saving the energy consumption of the AUV;

[0047] Compared with the traditional line-of-sight guidance method, the line-of-sight guidance method with a time-varying forward-looking distance is used to obtain the desired heading, which can make the AUV approach the desired route more smoothly and effectively reduce the overshoot of the position error.

[0048] At the same time, a parameter adaptive algorithm is introduced to adapt to various navigation conditions. The heading and speed are designed as a double closed-loop control. The desired speed of the speed loop is adjusted according to the distance deviation of the AUV to the end point of the desired trajectory. Decelerate to a low speed in advance for turning, reducing the trajectory overshoot during the turning process of the AUV, and effectively ensuring the maneuverability and energy-saving efficiency of the AUV. Description of the Drawings

[0049] Figure 1 is the schematic diagram of the horizontal plane motion of the underactuated AUV in the embodiment of the present invention.

[0050] Figure 2 is the flow chart of the plane straight-line trajectory tracking control algorithm provided by the embodiment of the present invention.

[0051] Figure 3 is the schematic diagram of the line-of-sight guidance method provided by the embodiment of the present invention.

[0052] Figure 4 is the structure diagram of the heading controller provided by the embodiment of the present invention.

[0053] Figure 5a It is the Surface View diagram of the fuzzy control law of kp provided by the embodiment of the present invention.

[0054] Figure 5b It is the Surface View diagram of the fuzzy control law of ki provided by the embodiment of the present invention.

[0055] Figure 5c It is the Surface View diagram of the fuzzy control law of kd provided by the embodiment of the present invention.

[0056] Figure 6 It is the double closed-loop control block diagram of the heading and speed provided by the embodiment of the present invention.

[0057] Figure 7a It is the comparison effect diagram of the horizontal plane path tracking and the desired trajectory provided by the embodiment of the present invention.

[0058] Figure 7b It is the detailed comparison effect diagram of the horizontal plane path tracking and the desired trajectory (300, 0) provided by the embodiment of the present invention.

[0059] Figure 7c It is the detailed comparison effect diagram of the horizontal plane path tracking and the desired trajectory (0, -300) provided by the embodiment of the present invention.

[0060] Figure 8 It is the actual heading angle change diagram of the distance double closed-loop algorithm provided by the embodiment of the present invention.

[0061] Figure 9 It is the actual speed change diagram of the distance double closed-loop algorithm provided by the embodiment of the present invention. Specific embodiments

[0062] The following combines the drawings to illustrate the specific embodiments of the present invention.

[0063] The object of the present invention is to solve the technical problems existing in the traditional line-of-sight guidance method during the trajectory tracking control of an underwater underactuated AUV, such as the step signal appearing during large-scale turning and the poor adaptability.

[0064] The underactuated AUV trajectory tracking control method provided by this application includes an improved line-of-sight guidance method based on the time-varying forward-looking distance, and a trajectory tracking control method that uses a fuzzy PID to construct a double closed-loop controller for the speed and heading to achieve smooth steering transition and improve the maneuverability of the AUV.

[0065] The specific steps are as follows:

[0066] As Figure 1 shown, a non-linear kinematic model and a dynamic model of the horizontal plane motion of an underactuated AUV are established.

[0067] The kinematic model is as follows:

[0068]

[0069] In the formula, x and y respectively represent the horizontal position coordinates of the AUV in the earth coordinate system ξEη, and ψ is the yaw angle. are the first derivatives of x, y, and ψ respectively.

[0070] The dynamic model is as follows:

[0071]

[0072] In the formula, u, v, and r are respectively the longitudinal, lateral linear velocities, and yaw angular velocity of the AUV in the body coordinate system XOY (the origin O is the buoyancy center of the AUV), are the accelerations corresponding to u, v, and r respectively, and δ r is the vertical rudder angle, X prop is the thrust of the propeller, m is the mass of the AUV, and I z is the moment of inertia of the AUV about the z-axis. is the added inertia mass, X |u|u , Y |v|v , Y |r|r , N |v|v , N |r|r are the hydrodynamic damping force coefficients, X vr , X rr , Y ur , Y uv , N ur , N uv are the Coriolis force coefficients. is the vertical rudder effectiveness coefficient.

[0073] In an embodiment of the present application, m is set to 100 kg, and the moment of inertia I z is assigned 10.0 kg·m 2 . are respectively set to -9.3 kg, -35.5 kg, 1.93 kg, 1.93 kg, -4.88 kg·m 2 / rad. X |u|u , Y |v|v , Y |r|r , N |v|v , N |r|r are respectively set to -1.62 kg / m, -131 kg / m, 0.632 kg·m 2 / rad, -3.18 kg, -9.4 kg·m 2 / rad. X vr , X rr , Y ur , Yuv , N ur , N uv are respectively set as 35.5 kg / rad, -1.93 kg·m / rad, 5.22 kg / rad, -28.6 kg / m, -2 kg·m / rad, -24 kg. is set as -6.15 kg / rad.

[0074] As Figure 3 shown, considering three straight-line paths in the water surface, the starting and midpoint positions of each desired track are P k =(x k , y k ) and P k+1 =(x k+1 , y k+1 ). In this embodiment, it is designed that the AUV travels along the following waypoints connected in sequence: A(0,0), B(300,0), C(0, -300), D(300, -300) (m). Points P k , P k+1 constitute the course azimuth angle of the desired track:

[0075] α k = a tan 2(y k+1 - y k , x k+1 - x k )

[0076] The vertical deviation of the real-time position P t =(x t , y t ) of the AUV to the desired track P k , P k+1 is y e (t).

[0077] y e (t) = (y t - y k ) cosα k - (x t - x k ) sinα k

[0078] Define the distance from the foot of the perpendicular of the real-time position of the AUV on the desired track to the sight point P los as the forward-looking distance Δ, then the output desired course angle is:

[0079] ψ d = α k - a tan 2(y e (t), Δ)

[0080] Ordinary line-of-sight guidance uses a fixed forward-looking distance without considering the relationship between the vertical distance and the forward distance. To enable the AUV to quickly approach the desired trajectory when it is far from the expected track, reduce the lateral error, and when the AUV is near the expected track, make the AUV slowly approach the expected track and minimize overshoot, the vertical distance y e (t) is introduced, and a time-varying forward-looking distance function is designed:

[0081] Δ=(Δ max -Δ min )exp(-λy e 2 (t))+Δ min

[0082] Principle for the AUV to switch the desired trajectory:

[0083] (x t -x k+1 ) 2 +(y t -y k+1 ) 2 <R 2

[0084] In this embodiment, the switching threshold R is set to 3,

[0085] To smoothly switch between two desired track segments, a first-order filter is designed for the desired heading angle:

[0086] ψ d (t)=0.8ψ d (t - 1)+0.2ψ d (t - 1)

[0087] To make the speed of the AUV gradually decrease when turning and not damage the overall mechanical performance of the AUV, it is designed that the corrected desired speed changes with the distance deviation. The specific correction function is as follows:

[0088]

[0089] R″, R′, and R are all constants. R is the same as R in the track update principle and is designed based on the turning radius; R′ and R″ are the preparatory deceleration distance deviations. In this embodiment, R″ and R′ are 30 and 10 respectively.

[0090] As Figure 4 shown, to improve the robustness of the heading control and adapt to the step-like desired signal caused by the track segment switching, the PID parameters are dynamically adjusted through fuzzy rules, enabling the control system to flexibly adjust the control strategy according to the real-time error and error change. The fuzzy control law is as Figure 5a 、 Figure 5b 、 Figure 5cAs shown, the specific fuzzy rules define the adjustment strategy of the PID gain according to the input variables error e(t) and error change rate Δe(t). The linguistic variables used in this embodiment are: Negative Big (NB), Negative Medium (NM), Negative Small (NS), Zero (ZE), Positive Small (PS), Positive Medium (PM), Positive Big (PB). When the error e(t) and error change rate Δe(t) are in different combinations, the fuzzy controller outputs the corresponding k p , k i , k d values. By adjusting these outputs, the controller can adaptively adjust the response and optimize the dynamic performance of the system. Combining traditional PID and fuzzy PID, the weight function is designed as:

[0091]

[0092] where, u c is the control output of the PID controller, u cf is the control output of the fuzzy PID, and U(u c , u cf ) is the final controller output after weight ratio matching using the weight function.

[0093] As Figure 6 shown, the speed PID controller is combined with the heading controller to construct a speed and heading double closed-loop controller. In practical applications, the speed loop continuously adjusts the desired speed to achieve the purpose of "sailing at low speed during turning and high speed during straight sailing".

[0094] Considering the actual servo situation and the physical model of the AUV, a limit treatment is performed on the servo output rudder angle, and the limit function is as follows:

[0095]

[0096] Figure 2 shows the flow chart of the trajectory tracking algorithm of the present invention, Figure 3 shows the line-of-sight guidance method used in the present invention, Figure 7a shows the horizontal plane path tracking effect diagram of the embodiment of the present invention, Figure 7b , Figure 7c respectively show the tracking effects of the AUV near two large turning points (300, 0) and (0, -300). It can be seen that the AUV can converge stably and travel along the desired path, Figure 8 and Figure 9 show the actual heading angle and actual speed change diagram of the AUV. It can be seen that the heading angle of the AUV changes smoothly, and the speed adjusts the desired speed of the speed loop according to the distance deviation of the AUV to the end point of the desired track section, decelerates to low speed in advance for turning, reduces the track overshoot during the turning of the AUV, and both the heading angle and the track are within a reasonable range, which conforms to the dynamic model of the AUV.

[0097] This application is an AUV trajectory tracking control method based on the line-of-sight guidance method. When designing, a time-varying forward-looking distance is designed to improve the maneuverability of the AUV. A speed function is proposed to correct the expected speed that changes in real time with the distance deviation. During the navigation of the AUV, the speed and heading controller realizes double-loop control. This method has good adaptability to complex set navigation trajectories and a fast convergence speed. The proposed speed loop continuously adjusts the expected speed, effectively improving the maneuverability of the AUV while saving the energy consumption of the AUV.

[0098] The above description is an explanation of the present invention, not a limitation of the invention. For the scope defined by the present invention, refer to the claims. Within the protection scope of the present invention, any form of modification can be made.

Claims

1. A trajectory tracking control method for an underactuated AUV using an adaptive line-of-sight guidance method suitable for high speed and large turning radius, characterized in that: The steps include: Step 1: Establish the kinematic and dynamic model of the underactuated AUV horizontal plane; Step 2: By collecting the real-time position and longitudinal velocity information of the AUV itself and combining it with the set expected trajectory, the expected model of velocity and heading angle is derived; Step 3: Design a step-by-step decreasing expected speed correction function based on the distance between the AUV real-time position and the expected track point; Step 4: Based on the speed and heading angle tracking errors, use the PID controller to build a speed and heading dual closed-loop controller, design fuzzy control rules to adjust the parameters adaptively, and design the weight function; Step 5: Design a first-order inertial low-pass filter to limit the heading angle control output to suppress the step change of the desired heading angle caused by heading switching.

2. The underactuated AUV trajectory tracking control method of the adaptive line of sight guidance method suitable for high speed and large turning radius as claimed in claim 1 is characterized in that: In step 1, the kinematic model of the underactuated AUV horizontal plane is established as: Where: x, y represent the horizontal position coordinates of the AUV in the geodetic coordinate system ξEη, ψ is the yaw angle, are the first-order derivatives of x, y, and ψ respectively; The kinetic model is: Where: u, v, and r are the longitudinal and transverse linear velocities and yaw angular velocity of the AUV in the body coordinate system XOY with the center of buoyancy as the origin, respectively, and δ is the acceleration corresponding to u, v, and r, respectively. r is the vertical rudder angle, X prop is the propeller thrust, m is the AUV mass, I z is the moment of inertia of the AUV around the z-axis, For the additional mass, Xuu , Yvv , Yrr , Nvv , Nrr is the damping force coefficient, Xvr , Xrr , Yur , Yuv , Nur , Nuv is the Coriolis force coefficient, is the vertical rudder efficiency coefficient.

3. The underactuated AUV trajectory tracking control method of the adaptive line of sight guidance method applicable to high speed and large turning radius as claimed in claim 1 is characterized in that: In step 2, the specific process of obtaining the expected model of speed and heading angle is: In the plane of the geodetic coordinate system ξEη, the target point P i (i=0,1,2…) form the expected straight line track; the real-time position of AUV is P t =(x t ,y t ), the coordinate points that make up the expected track segment are P k =(x k ,y k ) and P k+1 =(x k+1 ,y k+1 ), click P k , P k+1 The heading angles that make up the desired track: α k =atan2(y k+1 -y k ,x k+1 -x k ) AUV real-time position P t =(x t ,y t ) to the desired track P k , P k+1 The vertical deviation is y e (t) and e (t)=(y t -and k )things k -(x t -x k )sinα k Define the AUV real-time position on the desired track from the foot of the perpendicular to the line of sight point P los The distance is the foresight distance Δ, and the output expected heading angle is: ψ d =a k -atan2(y e (t),Δ) Introduce the vertical distance y e (t), design time-varying foresight distance function, Δ=(Δ max -D min )exp(-λy e 2 (t))+Δ min Principles for AUV to switch desired trajectory: (x t -x k+1 ) 2 +(and t -and k+1 ) 2 <R 2 That is, the AUV can be made close to the desired route, the overshoot of the position error can be reduced, and the AUV can track multiple target points and complete all the desired track segments.

4. The underactuated AUV trajectory tracking control method of the adaptive line of sight guidance method applicable to high speed and large turning radius as claimed in claim 1 is characterized in that: In step 3, the expected speed correction function that decreases step by step is: Among them, δ d is the distance from the current position of the AUV to the end point of the current track segment, and R, R′, and R″ are the limit distances of the three distance rings.

5. The underactuated AUV trajectory tracking control method of the adaptive line of sight guidance method applicable to high speed and large turning radius as claimed in claim 1, characterized in that: In step 4, the speed controller uses PID control, the heading controller uses PID and fuzzy PID to control together, the fuzzy control law is designed, and the control weight function is designed: Among them, u c is the control output of the PID controller, u cf is the control output of fuzzy PID, U(u c ,u cf ) is the final controller output after weight matching using the weight function.

6. The underactuated AUV trajectory tracking control method of the adaptive line of sight guidance method applicable to high speed and large turning radius as claimed in claim 1, characterized in that: In step 5, a first-order inertial filter is designed for the heading angle controller output, and its mathematical model and parameters are: ψ d (t)=0.8ψ d (t-1)+0.2ψ d (t-1) For the heading angle controller, limit the amplitude and saturation function: A step signal will appear when switching the desired track, and the saturation function is used to prevent the heading angle controller from outputting an excessively large control signal.

Images