Unmanned ship guidance and tracking control method based on full-drive system method

Abstract

The embodiment of the invention provides an unmanned surface vessel (USV) guidance and path tracking control method based on a full-drive system method, and aims to simplify the design of a controller and improve the control precision and efficiency. The method comprises the steps that a three-degree-of-freedom kinematics and dynamics mathematical model of the unmanned ship is established, and an LOS guidance law based on a reduced-order observer and a course angle and forward speed controller based on a full-drive system are designed. A simulation test on an MATLAB / SIMULINK platform shows that compared with a traditional PID controller and a backstepping controller, the method shows higher tracking precision and smaller control efficiency in linear path and curve path tracking scenes, and has remarkable theoretical value and engineering application prospects.

Description

Technical Field

[0001] The embodiments of the present disclosure relate to control technology for unmanned surface vessels (USVs), and more particularly to a path tracking control device for unmanned surface vessels based on a fully actuated system (FAS) method. Background Art

[0002] Unmanned surface vessels (USVs) are unmanned surface systems that have been widely used in a variety of large-scale applications, from military to civilian, covering reconnaissance, patrol, surveillance, search and rescue, and many other fields. As a result, increasing attention is focused on the full autonomy of USVs to enable operations in diverse and complex environments.

[0003] Control systems are fundamental and crucial to the autonomy of unmanned underwater vehicles (USVs). However, most USVs are underactuated, and various small uncertainties, such as modeling inaccuracies, unmodeled dynamics, and modifications to physical parameters, can affect control system performance. Furthermore, external disturbances such as wind, waves, and currents must be considered. Therefore, designing control systems to ensure that USVs successfully complete various aquatic missions presents a significant challenge.

[0004] Path-following control is a typical application scenario for unmanned vessels (USVs). Its control objective is to follow a predefined desired path in a time-dependent manner. There are two main approaches to solving the path-following problem: using only a control law and combining guidance and control laws. The first approach, based on the Lyapunov direct method, employs methods such as differential homomorphism and subsystem partitioning, combined with algorithms such as backstepping, sliding mode, adaptive methods, and artificial neural networks to achieve controller design. However, these methods complicate the controller design process and are often subject to continuous excitation constraints, limiting the control system to tracking curved paths. The second approach, which is relatively more mainstream in the field of USVs, employs guidance laws, such as line-of-sight (LOS), to transform path tracking into heading angle tracking. A controller based on heading angle and speed is then designed to ultimately achieve the path-following objective. Currently, PID controllers, backstepping controllers, adaptive controllers, and dynamic surface controllers have been theoretically proven to be suitable for combining with navigation laws.

[0005] As a new control theory, the full drive system (FAS) theory has attracted widespread attention since it was proposed. It reveals that most physical systems can be described and studied using the FAS system model, and many physical under-actuated systems can also be converted into mathematically fully driven systems. The clearer physical meaning and controller-oriented characteristics of FAS greatly simplify the controller design process. As a complete control theory comparable to the state space theory, the FAS theoretical framework is solid and has a series of theoretical foundations such as controllability, observability, full drive conditions and sub-full drive conditions. With the enrichment of application scenarios, a series of results have been achieved in robust control, adaptive control, optimal control, predictive control, coordinated control and fault-tolerant control. Therefore, in recent years, the research on full drive system methods has gradually become a new hotspot. The proposal of the present invention is a new exploration of its application in the field of unmanned ships, which simplifies the controller design process and improves the control effect. Therefore, it has broader theoretical significance and application prospects. Summary of the Invention

[0006] In view of this, the present invention aims to propose an unmanned surface vessel (USV) guidance and tracking control method based on a full-drive system method, which overcomes the complexity of controller design while ensuring accurate tracking control and low control efficiency.

[0007] To achieve the above objectives, the technical route of the present invention is as follows:

[0008] Step 1: Establish a three-degree-of-freedom kinematic and dynamic mathematical model of the unmanned ship;

[0009] Step 2: Design a LOS guidance law based on a reduced-order observer with variable forward line-of-sight.

[0010] Step 3: Design the heading angle and forward velocity controller based on FAS;

[0011] Step 4: Combine the guidance law designed in Step 2 with the controller designed in Step 3 and apply them to the initial unmanned vessel system in Step 1 for testing.

[0012] In step 1, considering the need for tracking and controlling the target and the complexity of the unmanned vessel system, the initial USV model with three degrees of freedom is established as follows:

[0013]

[0014] where η = [x, y, ψ] T , x, y are position coordinates, ψ is the yaw angle, v = [u, v, r] T , u represents the forward velocity of the ship, v represents the lateral velocity, and r represents the yaw velocity. M is the inertia matrix, which is the sum of the rigid body mass matrix and the additional mass matrix, and is expressed as

[0015]

[0016] C(v) is the Coriolis matrix, which is the sum of the standard Coriolis rigid body matrix and the additional mass Coriolis matrix, denoted as

[0017]

[0018] D(v) is the resistance matrix composed of linear and nonlinear resistance matrices, and its expression is:

[0019]

[0020] J(η) is the transformation matrix, which is expressed as follows:

[0021]

[0022] τ represents the external force vector applied to the three degrees of freedom of the hull, which is expressed as:

[0023] τ=[τ x τ y τ z ] T =[F u 0 T r ] T

[0024] Among them, F u and T r are forward thrust and bow torque, respectively.

[0025] The off-diagonal terms of the inertia and drag matrices are small compared to the main diagonal terms and can be ignored for the purpose of designing the guidance law and the heading and forward velocity controllers. Therefore, the equations of motion and dynamics of the underactuated USV can be simplified to:

[0026]

[0027] In step 2, the following drift angle observer is designed to cope with the influence of lateral ocean currents:

[0028]

[0029] Among them, p is the auxiliary state, k g is the observer gain, and β is the drift angle.

[0030] The following variable look-ahead distance is then designed to reduce path tracking overshoot and improve convergence speed:

[0031]

[0032] Among them, k Δ1 , kΔ2 >0 is the parameter to be designed, Δ min is the minimum foresight distance, y e is the longitudinal tracking error.

[0033] Therefore, the guided subsystem that satisfies uniform ultimate boundedness is as follows:

[0034]

[0035] Among them, x e and y e They are the longitudinal position error and lateral position error in SF (Serret-Frenet) coordinates.

[0036] In step 3, the forward speed controller based on FAS is designed as follows:

[0037]

[0038] Among them, u e =uu d is the forward velocity error, k u It is a parameter that needs to be designed.

[0039] The FAS-based heading angle controller is redesigned as follows:

[0040]

[0041] Among them, ψ e =ψ-ψ d is the heading angle error, k r and k ψ It is a parameter that needs to be designed.

[0042] In step 4, a test is performed based on the MATLAB / SIMULINK simulation platform to obtain the time-varying curves of the position, heading angle, and forward speed of the original unmanned ship system during tracking, as well as the time-varying curves of the forward thrust and bow torque output by the controller. These curves are then compared with the most commonly used PID controller and a proven backstepping controller, thereby verifying the feasibility and superiority of the present invention.

[0043] The beneficial effects and improvements of the present invention compared to the prior art are mainly reflected in the following aspects:

[0044] Firstly, the present invention realizes the natural full drive of the forward speed and heading systems by combining the time-varying forward-looking range (ELOS) guidance law. Secondly, the forward speed controller and heading controller are designed according to the FAS method, so that the closed-loop characteristic structure can be arbitrarily distributed, which is convenient for analysis and design to ensure closed-loop stability. In addition, the proposed FAS controller cascade method with ELOS effectively avoids the hierarchical controller design and continuous excitation conditions, simplifying the design process of the unmanned ship path tracking controller. Finally, two sets of comparative simulations verify that the path tracking accuracy of the proposed method is high under the same navigation law, the required control efficiency is small, and it has certain advantages. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] In order to more clearly illustrate the technical solution of the invention, the following is a brief description of the accompanying drawings of the present invention:

[0046] Figure 1 is a flow chart of the present invention;

[0047] Figure 2 A framework for cascading guidance and control laws;

[0048] Figure 3 It is a schematic diagram of LOS in SF coordinate system;

[0049] Figure 4 Schematic diagram of the unmanned ship's motion path and the expected straight path;

[0050] Figure 5 Schematic diagram of the heading angle of the unmanned ship and the desired heading angle during the process of tracking a straight path;

[0051] Figure 6 Schematic diagram of the forward velocity and expected forward velocity of the unmanned vessel when tracking a straight path;

[0052] Figure 7 Schematic diagram of the propulsion force and yaw moment output by the controller during the process of tracking a straight path;

[0053] Figure 8 Schematic diagram of the unmanned ship's motion path and the desired curved path;

[0054] Figure 9 Schematic diagram of the heading angle and expected heading angle of the unmanned ship during the process of tracking the curved path;

[0055] Figure 10 Schematic diagram of the forward speed and expected forward speed of the unmanned vessel when tracking a curved path;

[0056] Figure 11 Schematic diagram of the propulsion force and yaw moment output by the controller during the process of tracking the curved path;

[0057] Note: Figures 2 to 11 Contains comparative experimental results of the present invention, PID and backstepping methods. DETAILED DESCRIPTION

[0058] To make the objectives, technical solutions, and advantages of the present invention more apparent, the following is a further explanation with reference to the accompanying drawings. It is apparent that the described embodiments are only a portion of the embodiments of the present invention, and are not exhaustive. Based on the described embodiments, all other embodiments derived by those skilled in the art without requiring creative effort are also within the scope of protection of the present invention.

[0059] Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs.

[0060] In order to reduce the complexity of the path tracking controller design while ensuring accurate tracking effect and low control efficiency, the present invention proposes an unmanned ship guidance and path tracking control method based on a full-drive system method. Figure 1 This is an exemplary flow chart of the unmanned vessel guidance and path tracking control design based on the all-drive system method according to an embodiment of the present invention.

[0061] Step 1: Establish the three-degree-of-freedom kinematic and dynamic mathematical model of the unmanned ship:

[0062] Considering the need for tracking control targets and the complexity of the unmanned vessel system, the initial USV model with three degrees of freedom is established as shown in formula (1.1):

[0063]

[0064] where η = [x, y, ψ] T , x, y are position coordinates, ψ is the yaw angle, v = [u, v, r] T , u represents the forward velocity of the ship, v represents the lateral velocity, and r represents the yaw velocity. M is the inertia matrix, which is the sum of the rigid body mass matrix and the additional mass matrix, and is expressed as:

[0065]

[0066] C(ν) is the Coriolis matrix, which is the sum of the standard Coriolis rigid body matrix and the added mass Coriolis matrix, denoted as:

[0067]

[0068] D(v) is the resistance matrix composed of linear and nonlinear resistance matrices, and its expression is:

[0069]

[0070] J(η) is the transformation matrix, which is expressed as follows:

[0071]

[0072] τ represents the external force vector applied to the three degrees of freedom of the hull, which is expressed as:

[0073] τ=[τ x τ y τ z ] T =[F u 0 T r ] T (1.6)

[0074] Among them, F u and T r are forward thrust and bow torque, respectively.

[0075] In order to facilitate the design of guidance laws and controllers, the smaller off-diagonal terms in the inertia and resistance matrices are appropriately ignored, and the simplified kinematic and dynamic models of the underactuated USV are obtained as shown in Equations (1.7) and (1.8):

[0076]

[0077] like Figure 2 As shown, the path-following problem of an underactuated unmanned vessel can be solved by cascading the guidance and control laws into two parts: the first part is to design a guidance law based on the kinematic model to converge the vessel's position to the desired path position and output the desired heading angle; the second part is to design two controllers based on the dynamic model to converge the vessel's heading to the desired heading generated by the guidance law and the forward velocity to the desired speed. Because the dynamic system models of the forward velocity and heading are naturally fully driven, controller design based on FAS theory is easy, which can effectively simplify the controller design.

[0078] Step 2: Design a LOS guidance law based on a reduced-order observer with variable forward line of sight:

[0079] First, the SF (Serret-Frenet) coordinate system is introduced as Figure 3 As shown, the parameterized target path P in the SF coordinate system is defined d (θ)=[x d (θ), y d (θ)] T , where θ is the path parameterization variable. For any position on the desired path, the path cut angle can be described as γ d =atan2(y′ d (θ), x′ d(θ)), where P(x,y)=[x,y] T represents the current position of the ship, then the position error in SF coordinates can be expressed as

[0080]

[0081] Considering that an under-actuated ship may generate a drift angle under the action of the ocean current during navigation, Figure 3 As shown, the desired heading angle should be set to:

[0082] ψ d =γ d +arctan(-y e / Δ)-β (1.10)

[0083] In order to increase the robustness of the guidance system, the following drift angle observer is designed to cope with the influence of time-varying lateral currents:

[0084]

[0085] Among them, p is the auxiliary state, k g is the observer gain, and β is the drift angle.

[0086] The following variable look-ahead distance is then designed to reduce path tracking overshoot and improve convergence speed:

[0087]

[0088] Among them, k Δ1 , k Δ2 >0 is the parameter to be designed, Δ min is the minimum foresight distance, y e is the longitudinal tracking error.

[0089] Therefore, the guidance subsystem that satisfies the uniform final boundedness is obtained as follows:

[0090]

[0091] Among them, x e and y e are the lateral tracking error and longitudinal tracking error in SF coordinates, respectively.

[0092] Step 3: Design the FAS-based heading angle and forward velocity controller

[0093] First, the forward speed controller based on FAS is designed as follows:

[0094]

[0095] Among them, ue =uu d is the forward velocity error, k u It is a parameter that needs to be designed.

[0096] The FAS-based heading angle controller is redesigned as follows:

[0097]

[0098] Among them, ψ e =ψ-ψ d is the heading angle error, k r and k ψ It is a parameter that needs to be designed.

[0099] Step 4: Simulation test based on MATLAB / SIMULINK

[0100] The guidance law designed in step 2 is combined with the controller designed in step 3 and applied to the initial unsimplified three-degree-of-freedom underactuated unmanned vessel system in step 1. The system is tested on the MATLAB / SIMULINK simulation platform.

[0101] The corresponding parameters of the initial unsimplified three-degree-of-freedom underactuated unmanned ship model are shown in Table 1. The initial state of the unmanned ship is set to x(0) = 15, y(0) = 0, ψ(0) = 45°, u(0) = 0.01, v(0) = 0, r(0) = 0, and the parameters of the guidance law and control law are designed to be k u =11,k r =5,k ψ =6.25, k g =2,k Δ1 =1,k Δ2 =0.1,Δ min =2.5.

[0102] Table 1 Unmanned ship model parameters

[0103]

[0104] The first set of simulations considers the scenario of linear path tracking, and the expected linear path is selected as x d =2.3θ;y d = 23θ. The actual tracking path of the ship and the expected straight path are as follows Figure 3 As shown, where N and E refer to the x and y position coordinates in the inertial coordinate system respectively. Figure 4 It can be seen that compared with the traditional PID controller and backstepping controller, under the same guidance law, the FAS controller proposed in the present invention has the smallest path tracking error and the best tracking effect. Figure 5The figure shows the changes in the heading angle of the unmanned vessel under the action of three different controllers during the process of tracking a straight path. All controllers can make the yaw angle of the vessel converge to the desired heading, but the steady-state error of the FAS controller proposed in this invention is the smallest. Figure 6 The figure shows the change in the forward velocity of the unmanned vessel under the action of three different controllers while tracking a straight path. All controllers can converge the forward velocity of the vessel, but the proposed FAS controller has a smaller steady-state error and no oscillation. Figure 7 The thrust and yaw forces output by different controllers during straight-line path tracking are shown. The FAS controller uses the least control efficiency. Therefore, in the straight-line path tracking scenario, the proposed method achieves accurate tracking with minimal control efficiency, offering significant advantages.

[0105] The second set of simulations considers the scenario of curved path tracking, and the desired curved path is selected as: d =20sin(0.1θ)+θ;y d =2θ. The actual tracking path of the ship and the expected curved path are as follows Figure 8 As shown. Figure 8 It can be seen that compared with the traditional PID controller and backstepping controller, under the same guidance law, the FAS controller proposed in the present invention has the smallest path tracking error and the best tracking effect. Figure 9 The figure shows the changes in the heading angle of the unmanned ship under the action of three different controllers during the process of tracking the curved path. All controllers can make the yaw angle of the ship converge to the desired heading, but the steady-state error of the FAS controller proposed in this invention is the smallest and there is no oscillation. Figure 10 The figure shows the change in the forward velocity of the unmanned vessel under the action of three different controllers while tracking a curved path. All controllers can converge the forward velocity of the vessel, but the proposed FAS controller has a smaller steady-state error and the smallest overshoot. Figure 11 The thrust and yaw forces output by different controllers during curved path tracking are shown. The FAS controller uses the least control efficiency. Therefore, in the curved path tracking scenario, the proposed method also achieves accurate tracking with minimal control efficiency.

[0106] Therefore, the method proposed in the present invention can effectively reduce the complexity of the path tracking controller design while ensuring good path tracking accuracy and smaller controller output. It can be applied to different path scenarios of unmanned ship path tracking and has certain theoretical value and good engineering application value.

Claims

1. An unmanned ship guidance and tracking control method based on a full drive system method, characterized in that: The following steps are involved: Step 1: Establish the three-degree-of-freedom kinematic and dynamic mathematical model of the unmanned ship; Step 2: Design a LOS guidance law based on a reduced-order observer with variable forward line-of-sight; Step 3: Design the heading angle and forward speed controller based on FAS; Step 4: Combine the guidance law designed in step 2 with the controller designed in step 3 and apply them to the initial unmanned ship system in step 1 for testing.

2. The unmanned ship guidance and tracking control method based on the full drive system method according to claim 1 is characterized in that: In step 1, considering the need for tracking and controlling the target and the complexity of the unmanned ship system, an initial USV model with three degrees of freedom is established as shown in formula (1.1): Where η = [x, y, ψ] T , x, y are the position coordinates, ψ is the yaw angle, ν = [u, v, r] T , u represents the forward speed of the ship, v represents the lateral speed, and r represents the yaw speed. M is the inertia matrix, which is the sum of the rigid body mass matrix and the additional mass matrix, and the expression is: C(v) is the Coriolis matrix, which is the sum of the standard Coriolis rigid body matrix and the additional mass Coriolis matrix, denoted as: D(v) is the resistance matrix composed of linear and nonlinear resistance matrices, and its expression is: J(η) is the transformation matrix, which is expressed as follows: τ represents the external force vector applied to the three degrees of freedom of the hull, expressed as: τ=[τ x t y t z ] T =[F u 0 T r ] T (1.6) Among them, F u and T r are forward thrust and bow turning moment respectively. In order to facilitate the design of guidance law and controller, the smaller non-diagonal terms in the inertia and resistance matrices are appropriately ignored, and the simplified kinematic and dynamic models of the underactuated USV are obtained as shown in equations (1.7) and (1.8):

3. The method according to claim 1, wherein: The variable forward line-of-sight guidance law described in step 2 includes the design of a drift angle observer to cope with the effects of lateral ocean currents. First, the SF (Serret-Frenet) coordinate system is introduced and the parameterized target path P in the SF coordinate system is defined. d (θ) = [x d (θ), y d (θ)] T , where θ is the path parameterization variable. For any position on the desired path, the path cut angle can be described as γ d = atan2(y′ d (θ),x′ d (θ)), where P(x,y)=[x,y] T represents the current position of the ship, then the position error in SF coordinates can be expressed as Considering that the under-actuated ship may generate a drift angle under the action of the ocean current during navigation, as shown in Figure 3, the desired heading angle should be set as: ψ d =c d +arctan(-y e / D)-b (1.10) In order to increase the robustness of the guidance system, the following drift angle observer is designed to cope with the influence of time-varying lateral currents: Among them, p is the auxiliary state, k g is the observer gain, and β is the drift angle. The following variable foresight distance is designed to reduce the overshoot of path tracking and improve the convergence speed: Among them, k Δ1 , k Δ2 >0 is the parameter to be designed, Δ min is the minimum foresight distance, y e is the longitudinal tracking error. Therefore, the guidance subsystem that satisfies the uniform final boundedness is obtained as follows: Among them, x e and e They are the lateral tracking error and the longitudinal tracking error in SF coordinates, respectively.

4. The method according to claim 1, wherein: The heading angle and forward speed controller based on FAS described in step 3 respectively controls the heading angle and forward speed of the unmanned ship, so that the closed-loop characteristic structure can be arbitrarily distributed to ensure the closed-loop stability. First, the FAS-based forward speed controller is designed as follows: Among them, u e =uu d is the forward velocity error, k u It is a parameter that needs to be designed. The FAS-based heading angle controller is redesigned as follows: Among them, ψ e =ψ-ψ d is the heading angle error, k r and k ψ It is a parameter that needs to be designed.

5. The method according to claim 1, wherein the path tracking control method includes testing on a MATLAB / SIMULINK simulation platform, and the feasibility and superiority of the method are verified by comparing it with a PID controller and a backstepping controller. The guidance law designed in step 2 of claim 1 is combined with the controller designed in step 3, applied to the initial unsimplified three-degree-of-freedom underactuated unmanned ship system in step 1, and tested on a MATLAB / SIMULINK simulation platform.

6. An unmanned ship guidance and path tracking control system, applying the method according to any one of claims 1 to 5, the system comprising: Modules for building three-degree-of-freedom kinematic and dynamic models; Module for designing variable forward line-of-sight guidance laws; Modules for designing heading angle and forward speed controllers for all-drive based systems; Modules for combining the guidance law with the controller and performing path-following control.

Images