A method for coordinated control of three-dimensional trajectory of a towed buoy

Abstract

The application discloses a kind of three-dimensional trajectory coordinated control methods of towed buoy, first establish cable-float system dynamics model, six degrees of freedom dynamics model of towed buoy is converted into affine nonlinear form;Then the unmeasurable cable tension suffered by buoy is regarded as system disturbance, and an extended state observer is constructed to obtain disturbance estimate value. Then the obtained estimate value is used as feedforward compensation term, and a towed buoy trajectory controller is designed using backstepping method, the side vertical movement of buoy is actively controlled by directly adjusting rudder control quantity, and the passive control of indirectly controlling the forward movement of towed buoy using cable length is realized by adjusting the cable winding and unwinding speed using PID control algorithm. The application regards cable tension as system disturbance, proposes an active and passive composite control strategy, designs a kind of three-dimensional trajectory coordinated control method of towed buoy, realizes the double-end coordinated control of side vertical and forward of towed buoy, and improves the docking accuracy and flight safety of air-based recovery.

Description

Technical Field

[0001] This invention relates to a three-dimensional trajectory coordination control method for towed buoys, belonging to the field of unmanned aerial vehicle (UAV) recovery technology. Background Technology

[0002] Small military drones play a crucial role in enhancing UAV combat capabilities due to their absolute advantage of small size. However, their limited payload and range increase operational costs. Improving the reusability of small drones has become a research hotspot in future UAV warfare. Using large and medium-sized transport aircraft as recovery platforms and employing flexible cable-buoy systems for rapid deployment and recovery of small drones is an effective solution for recovering small drones in situations lacking reliable land-based or sea-based landing platforms. The docking accuracy between the buoy and the small drone directly determines the success or failure of aerial recovery. During aerial recovery, the buoy is susceptible to unpredictable tension from the flexible cable, affecting the stable control of the towed buoy. Therefore, researching a three-dimensional trajectory coordinated control method for towed buoys to achieve rapid and stable recovery control of the cable-buoy-UAV combination is of great significance for realizing the reuse of small drones and reducing UAV operational costs.

[0003] The trajectory control of a cable-and-buoy system mainly employs two strategies: active control and passive control. Active control directly stabilizes the buoy's attitude and trajectory using aerodynamic control surfaces on the towed buoy. While active control can quickly and accurately control the buoy's trajectory, the limited control capabilities of the towed buoy's aerodynamic control surfaces make it difficult to control the buoy's trajectory in the forward direction. Passive control indirectly controls the towed buoy's movement by controlling the length of the cable or the movement of the transport aircraft. Although passive control offers a more flexible maneuvering range, the complex and flexible cable makes it difficult to meet the precision requirements of towed buoy trajectory control. Therefore, to achieve stable trajectory control of the cable-and-buoy system, a combined "active + passive" control strategy is proposed. A three-dimensional trajectory coordination control method for the towed buoy is designed to achieve coordinated control of the towed buoy's lateral and forward directions, improving the docking accuracy of airborne recovery and reducing the docking risks during airborne recovery. Summary of the Invention

[0004] Objective of the invention: This invention addresses the aforementioned prior art by utilizing an extended state observer to analyze system disturbances F. i i = 2, 3, 4 and the buoy's flight status information X i Accurate estimation of i = 1, 2, 3, 4 is performed, and an "active + passive" composite control strategy is proposed. A three-dimensional trajectory coordination control method for towed buoys is designed to achieve dual-end coordinated control of the towed buoys in the lateral vertical and forward directions, thereby improving the docking accuracy and flight safety of airborne recovery.

[0005] To achieve the above objectives, the technical solution provided by this invention is as follows: a three-dimensional trajectory coordinated control method for a towed buoy. This method treats the unmeasurable tension of the flexible cable as a system disturbance, designs an extended state observer to accurately obtain the disturbance estimate, and proposes an "active + passive" composite control strategy, combining active and passive control of the towed buoy trajectory control to design a three-dimensional trajectory coordinated control method for a towed buoy. By adjusting the buoy rudder surface control quantity and the cable length, coordinated control of the towed buoy's lateral and forward directions is achieved, thereby realizing precise control of the towed buoy trajectory. Specifically, it includes the following steps:

[0006] Step 1: Based on the mechanical relationship between the cable and the towed buoy, establish a dynamic model of the cable-buoy system, and convert the six-degree-of-freedom dynamic model of the towed buoy into an affine nonlinear form;

[0007] Step 2: Treat the unmeasurable tension in the flexible cable as a system disturbance, and construct an extended state observer to assess the system disturbance F. i i = 2, 3, 4 and buoy flight status X i For i = 2, 3, 4, make accurate estimates;

[0008] Step 3: Design a lateral vertical controller for the towed buoy. Based on the dynamic model of the cable-buoy system obtained in Step 1, introduce the backstepping method to design a lateral vertical controller to realize the active control of the buoy's lateral vertical motion using the buoy rudder surface control quantity.

[0009] Step 4: Design a forward controller for the towed buoy, using the deviation between the actual cable length and the desired cable length as feedback input, and adjust the cable winding and unwinding speed using a PID control algorithm, thereby indirectly controlling the forward movement of the buoy by utilizing the cable winding and unwinding length.

[0010] Step 5: Combine the lateral vertical controller established in Step 3 with the forward controller established in Step 4, and design a dual-end coordinated controller for the three-dimensional trajectory of the towed buoy using a combination of active and passive control strategies; control the lateral vertical movement of the buoy by adjusting the buoy rudder surface control quantity, and control the forward movement of the buoy by using the cable extension and retraction length, and finally achieve dual-end coordinated control of the lateral vertical and forward movements of the towed buoy, thereby improving the docking accuracy during the airborne recovery process.

[0011] Furthermore, the specific steps of step 1 are as follows:

[0012] First, the cable is assumed to consist of N segments of equal length, mass, and uniform density. Adjacent segments are formed by frictionless nodes, and the mass of each segment is concentrated at the node. The buoy is connected to the last segment. The dynamic model of the cable-buoy system is described as follows:

[0013]

[0014] In the formula, r j P is the distance vector from node j-1 to node j; j m is the position vector of the j-th cable segment; j Let m be the mass of the j-th segment of the cable. d G is the mass of the towed buoy. j T j and Q j Let represent the gravity, tension, and external aerodynamic force on the j-th cable segment, respectively; E is the elastic Young's modulus of the cable; A = π(d / 2). 2 d is the cross-sectional area of ​​the cable; d is the diameter of the cable; l0 is the initial length of each cable segment; L j and D j Q represents the lift and resistance on the j-th cable segment; d It is the aerodynamics of the buoy;

[0015] Next, a six-DOF dynamic model of the towed buoy is constructed using Newton's second law and aerodynamic theory. The following state vector is defined, and the six-DOF dynamic model of the towed buoy is converted into an affine nonlinear form to facilitate the subsequent controller design:

[0016]

[0017]

[0018]

[0019] In the formula, X i i = 1, 2, 3, 4 represent the flight state information of the trajectory loop, flight path loop, attitude loop, and angular velocity loop, respectively; x, y, z are the positions of the towed buoy; V x V y V z V0 is the normalized velocity; α and β are the airflow angles; φ is the roll angle; p, q, and r are the angular velocities; δ a ,δ e, ,δ r F represents the deflection angle of the ailerons, elevator, and rudder. i i = 2, 3, 4 are used to describe unmeasurable cable tension; H i ,i=2,3,4 is the auxiliary function matrix; B i i = 2, 3, 4 is the system input matrix; U act For rudder surface control variables.

[0020] Furthermore, in step 2, the following extended state observer is designed for the affine nonlinear models of the track loop, attitude loop, and angular velocity loop which are affected by external disturbances:

[0021]

[0022]

[0023]

[0024] In the formula, X is determined by the extended state observer i Estimate the values ​​of i = 2, 3, 4; The disturbance term F is determined by the extended state observer. i Estimate the parameter L for i = 2, 3, 4. 21 =diag(2ω) 21 ,2ω 22 ), L 31 =diag(2ω) 31 ,2ω 32 ,2ω 33 ), L 41 =diag(2ω) 41 ,2ω 42 ,2ω 43 ) These are the bandwidths of the linearly extended state observers for the track loop, attitude loop, and angular velocity loop, respectively.

[0025] Furthermore, in step 3, a towed buoy side vertical controller is designed. The system disturbance estimate obtained in step 2 is used as a feedforward compensation term. The controllers for trajectory loop, track loop, attitude loop and angular velocity loop are designed in combination with the backstepping method. The precise control of the towed buoy side vertical trajectory is achieved by directly controlling the deflection of the towed buoy rudder surface.

[0026] Furthermore, the specific steps of step 3 are as follows:

[0027] Step 31: Define X1 as the actual trajectory of the towed buoy, X 1c For the desired trajectory, the trajectory tracking error is e1 = X1 - X 1c Differentiating both sides simultaneously, we get:

[0028]

[0029] Therefore, the virtual control law for the trajectory loop can be set as follows:

[0030]

[0031] In the formula, the positive definite matrix μ1=diag(μ 11 ,μ 12 ) represents the control parameters of the control law;

[0032] Step 32: Define the actual speed X2 and the desired speed X 2c The difference is the speed tracking error e2 = X2 - X 2c Differentiating it, we get:

[0033]

[0034] This allows us to set the virtual control law for the trajectory loop:

[0035]

[0036] In the formula, the positive definite matrix μ2=diag(μ 21 ,μ 22 ) represents the control parameters of the control law;

[0037] Step 33: Define the tracking error of the attitude loop as e3 = X3 - X 3c Taking its derivative, we get

[0038]

[0039] Based on the above equation, its virtual attitude control law can be designed.

[0040]

[0041] In the formula, the positive definite matrix μ3=diag(μ 31 ,μ 32 ,μ 33 ) represents the control parameters of the control law;

[0042] Step 34: Define the angular velocity tracking error as the difference between the actual value and the expected value of the angular velocity, i.e., e4 = X4 - X 4c Taking the derivative of both sides of the tracking error equation, we can obtain:

[0043]

[0044] Therefore, the control law required for the final control input can be obtained as follows:

[0045]

[0046] In the formula, the positive definite matrix μ4 = diag(μ 41 ,μ 42 ,μ 43 ) represents the control parameters of this control law.

[0047] Furthermore, in step 4, the deviation e between the actual cable length and the desired cable length is... l =L lc -L lAs error feedback, a corresponding PID cable length controller is designed. By adjusting the cable winding and unwinding speed, the cable length can be stably controlled, thereby indirectly controlling the movement of the buoy in the forward direction.

[0048] Furthermore, the selected PID control algorithm is described as follows:

[0049]

[0050] In the formula, L lc L represents the desired cable length. l K represents the actual cable length. p K i K d These are the proportional, integral, and derivative coefficients of the PID control algorithm.

[0051] Compared with the prior art, the present invention has the following obvious advantages:

[0052] The towed buoy three-dimensional trajectory coordination control method provided by this invention features simple docking operation and high safety, effectively improving the efficiency of airborne base recovery. By using a "active + passive" composite control strategy for the towed buoy, a three-dimensional trajectory coordination controller for the towed buoy is designed to achieve coordinated control of the towed buoy in both the lateral and forward directions. This effectively solves the problem that the buoy's aerodynamic control surfaces are unable to meet the trajectory control requirements in the forward direction due to limited control capabilities. At the same time, it suppresses the problem of low buoy trajectory control accuracy caused by the complex movement of the cable, ensuring docking accuracy during airborne base recovery and improving docking safety. Attached Figure Description

[0053] Figure 1 This is a schematic diagram of the aerial recovery of the cable-buoy-UAV combination of the present invention;

[0054] Figure 2 This is a flowchart illustrating a three-dimensional trajectory coordination control method for towed buoys according to the present invention.

[0055] Figure 3 This is a comparison chart of the actual trajectory and the expected trajectory tracking results of the towed buoy in this invention;

[0056] Figure 4 This is a simulation result diagram of the cable winding and unwinding length in this invention;

[0057] Figure 5 This is a simulation result diagram of the buoy rudder surface control quantity in this invention. Detailed Implementation

[0058] To facilitate understanding and implementation of the present invention by those skilled in the art, the present invention will be further described in detail below with reference to the accompanying drawings.

[0059] This invention discloses a three-dimensional trajectory coordinated control method for towed buoys. First, a dynamic model of the cable-buoy system is established, and the six-degree-of-freedom dynamics of the towed buoy are treated as affine nonlinearity. Based on this, an extended state observer is constructed to accurately reconstruct system disturbances. Then, an "active + passive" composite control strategy is proposed, using the obtained disturbance estimate as a feedforward compensation term. Feedback linearization control theory is introduced, and a towed buoy trajectory controller is designed. Active control of the towed buoy's lateral and vertical motion is achieved by directly adjusting the control surface deflection. Simultaneously, a PID control algorithm is used to adjust the cable deployment and retraction speed to achieve passive control of the towed buoy's forward motion indirectly using the cable length. This three-dimensional trajectory coordinated control method for towed buoys achieves coordinated control of both the lateral and vertical and forward directions, improving the docking accuracy and flight safety of airborne recovery.

[0060] In this example, during the in-flight recovery process, the mother aircraft is set to perform a constant level flight motion, such as... Figure 1 As shown, the selected transport aircraft has the following parameters: mass of 136,000 kg, wingspan of 39.88 m, and flight parameters: flight altitude H = 5,000 m and flight speed V = 100 m / s.

[0061] like Figure 2 The diagram shown is a control flowchart of a three-dimensional trajectory coordination control method for towed buoys according to the present invention, which specifically includes the following steps:

[0062] Step 1: Based on the mechanical relationship between the cable and the towed buoy, establish a dynamic model of the cable-buoy system, and convert the six-degree-of-freedom dynamic model of the towed buoy into an affine nonlinear form to facilitate the subsequent design of a three-dimensional trajectory dual-end coordinated controller. The specific steps are as follows:

[0063] First, to accurately establish the dynamic model of the cable-buoy system, this invention comprehensively considers the deformation caused by the tensile stress of the flexible cable during airborne recovery. It assumes the cable consists of N segments of equal length, mass, and uniform density distribution, with adjacent segments forming frictionless nodes. The mass of each segment is concentrated at the nodes, and the buoy is connected to the last cable segment. The dynamic model of the cable-buoy system can be described as follows:

[0064]

[0065] In the formula, r j P is the distance vector from node j-1 to node j; j m is the position vector of the j-th cable segment; j Let m be the mass of the j-th segment of the cable. d G is the mass of the towed buoy. j T j and Q jLet represent the gravity, tension, and external aerodynamic force on the j-th cable segment, respectively; E is the elastic Young's modulus of the cable; A = π(d / 2). 2 d is the cross-sectional area of ​​the cable; d is the diameter of the cable; l0 is the initial length of each cable segment; L j and D j Q represents the lift and resistance on the j-th cable segment; d It is the aerodynamics of the buoy.

[0066] Next, a six-DOF dynamic model of the towed buoy is constructed using Newton's second law and aerodynamic theory. For ease of calculation, the following state vector is defined, and the six-DOF dynamic model of the towed buoy is converted into an affine nonlinear form to facilitate the subsequent controller design:

[0067]

[0068]

[0069]

[0070] In the formula, X i i = 1, 2, 3, 4 represent the flight state information of the trajectory loop, flight path loop, attitude loop, and angular velocity loop, respectively; x, y, z are the positions of the towed buoy; V x V y V z V0 is the normalized velocity; α and β are the airflow angles; φ is the roll angle; p, q, and r are the angular velocities; δ a ,δ e, ,δ r F represents the deflection angle of the ailerons, elevator, and rudder. i i = 2, 3, 4 are used to describe unmeasurable cable tension; H i ,i=2,3,4 is the auxiliary function matrix; B i i = 2, 3, 4 is the system input matrix; U act For rudder surface control variables.

[0071] Step 2: Treat the unmeasurable tension in the flexible cable as a system disturbance, and construct an extended state observer to assess the system disturbance F. i i = 2, 3, 4 and buoy flight status X i Accurate estimations are made for i = 2, 3, 4, and compensation is incorporated in subsequent controller design. For the affine nonlinear models of the trajectory loop, attitude loop, and angular velocity loop affected by external disturbances, the following extended state observer is designed:

[0072]

[0073]

[0074]

[0075] In the formula, X is determined by the extended state observer i Estimate the values ​​of i = 2, 3, 4; The disturbance term F is determined by the extended state observer. i Estimate the parameter L for i = 2, 3, 4. 21 =diag(2ω) 21 ,2ω 22 ), L 31 =diag(2ω) 31 ,2ω 32 ,2ω 33 ),

[0076] L 41 =diag(2ω) 41 ,2ω 42 ,2ω 43 ) These are the bandwidths of the linearly extended state observers for the track loop, attitude loop, and angular velocity loop, respectively.

[0077] After repeated adjustments and selection of appropriate parameters, the designed extended state observer can accurately obtain the buoy's flight state X. i i = 2, 3, 4 and system interference F i , i = 2, 3, 4. The bandwidth of the final selected extended state observer is: ω 21 =ω 22 =100,ω 31 =ω 32 =ω 33 =20, ω 41 =ω 42 =ω 43 =30.

[0078] Step 3: Design a lateral vertical controller for the towed buoy. Using the system disturbance estimate obtained in Step 2 as a feedforward compensation term, and combining it with the backstepping method, design controllers for the trajectory loop, flight path loop, attitude loop, and angular velocity loop. This allows for precise control of the towed buoy's lateral vertical trajectory by directly controlling the deflection of the towed buoy's control surfaces. The specific design steps are as follows:

[0079] Step 31: Define X1 as the actual trajectory of the towed buoy, X 1c For the desired trajectory, the trajectory tracking error is e1 = X1 - X 1c Differentiating both sides simultaneously, we get:

[0080]

[0081] Therefore, the virtual control law for the trajectory loop can be set as follows:

[0082]

[0083] In the formula, the positive definite matrix μ1=diag(μ 11 ,μ 12 ) represents the control parameters of this control law.

[0084] Step 32: Define the actual speed X2 and the desired speed X 2c The difference is the speed tracking error e2 = X2 - X 2c Differentiating it, we get:

[0085]

[0086] This allows us to set the virtual control law for the trajectory loop:

[0087]

[0088] In the formula, the positive definite matrix μ2=diag(μ 21 ,μ 22 ) represents the control parameters of this control law.

[0089] Step 33: Define the tracking error of the attitude loop as e3 = X3 - X 3c Taking its derivative, we get

[0090]

[0091] Based on the above equation, its virtual attitude control law can be designed.

[0092]

[0093] In the formula, the positive definite matrix μ3=diag(μ 31 ,μ 32 ,μ 33 ) represents the control parameters of this control law.

[0094] Step 34: Define the angular velocity tracking error as the difference between the actual value and the expected value of the angular velocity, i.e., e4 = X4 - X 4c Taking the derivative of both sides of the tracking error equation, we can obtain:

[0095]

[0096] Therefore, the control law required for the final control input can be obtained as follows:

[0097]

[0098] In the formula, the positive definite matrix μ4 = diag(μ 41 ,μ 42 ,μ 43 ) represents the control parameters of this control law.

[0099] Following the order from the inner loop to the outer loop, i.e., the angular velocity loop, attitude loop, trajectory loop, and path loop, the controller parameters μ of each loop are adjusted sequentially. i Let i = 1, 2, 3, 4. Select appropriate parameters so that the towed buoy can accurately track the given position command. After repeated debugging, in this invention, μ1 = diag(1,1), μ2 = diag(15,15), μ3 = diag(5,5,5), and μ4 = diag(15,15,15).

[0100] Step 4: Design the towed buoy forward controller. The deviation e between the actual cable length and the desired cable length... l =L lc -L l As feedback input, the PID control algorithm is used to adjust the cable reeling-in and reeling speed, thereby indirectly controlling the buoy's forward movement by utilizing the cable reeling-in and reeling-out length. The selected PID control algorithm is described as follows:

[0101]

[0102] In the formula, L lc L represents the desired cable length. l K represents the actual cable length. p K i K d These are the proportional, integral, and derivative coefficients of the PID control algorithm. After repeated adjustments, the final selected PID parameters are: K p =0.25, K I =0,K d =0.01.

[0103] Step 5: Combine the lateral vertical controller established in Step 3 with the forward controller established in Step 4, employing an "active + passive" composite control strategy. This involves directly controlling the lateral vertical movement of the towed buoy using the buoy's rudder surface control variables, while indirectly controlling the forward movement of the towed buoy by adjusting the cable length. Design a dual-end coordinated controller for the towed buoy's three-dimensional trajectory to achieve coordinated control of both the lateral vertical and forward directions, thereby enabling precise control of the towed buoy's trajectory.

[0104] like Figure 3The image shows a comparison of the actual and desired buoy tracking results. It can be seen that directly controlling the buoy's lateral vertical movement using its aerodynamic control surfaces allows it to reach the desired position in a very short time. The passive control method, which indirectly controls the buoy's forward movement by using a PID control algorithm to control the cable length, although slower, still enables the towed buoy to track the desired trajectory within 20 seconds with a small tracking error, meeting the expected tracking performance. Therefore, the designed three-dimensional trajectory coordination control method for towed buoys can effectively control the buoy's trajectory movement.

[0105] like Figure 4 The figure shown is a simulation result of the cable extension and retraction length. It can be seen that under the action of PID control, the cable is extended to a length of 7 meters to indirectly control the movement of the towed buoy in the forward direction. Due to the complex movement of the long cable, even if there is slight vibration in the early stage of cable extension and retraction, it eventually tends to stabilize.

[0106] like Figure 5 The figure shown is a simulation result of the buoy control surface control quantity. It can be observed that the buoy control surface control quantity initially vibrates, but can reach a stable state in a very short time, meeting the expected flight control requirements.

[0107] The analysis and simulation verification above fully demonstrate the effectiveness of the three-dimensional trajectory coordination control method for towed buoys proposed in this invention in improving cable-buoy trajectory control.

[0108] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A method for coordinated control of a towed buoy's three-dimensional trajectory, characterized in that: The method specifically includes the following steps: Step 1: Based on the mechanical relationship between the cable and the towed buoy, establish a dynamic model of the cable-buoy system, and convert the six-degree-of-freedom dynamic model of the towed buoy into an affine nonlinear form; Step 2: Treat the unmeasurable tension in the flexible cable as a system disturbance, and construct an extended state observer to assess the system disturbance F. i i = 2, 3, 4 and buoy flight status X i For i = 2, 3, 4, make accurate estimates; Step 3: Design a lateral vertical controller for the towed buoy. Based on the dynamic model of the cable-buoy system obtained in Step 1, introduce the backstepping method to design a lateral vertical controller to realize the active control of the buoy's lateral vertical motion using the buoy rudder surface control quantity. Step 4: Design a forward controller for the towed buoy, using the deviation between the actual cable length and the desired cable length as feedback input, and adjust the cable winding and unwinding speed using a PID control algorithm, thereby indirectly controlling the forward movement of the buoy by utilizing the cable winding and unwinding length. Step 5: Combine the lateral vertical controller established in Step 3 with the forward controller established in Step 4, and design a dual-end coordinated controller for the three-dimensional trajectory of the towed buoy using a combination of active and passive control strategies; control the lateral vertical movement of the buoy by adjusting the buoy rudder surface control quantity, and control the forward movement of the buoy by using the cable extension and retraction length, and finally achieve dual-end coordinated control of the lateral vertical and forward movements of the towed buoy, thereby improving the docking accuracy during the airborne recovery process.

2. The method for coordinated control of a towed buoy's three-dimensional trajectory according to claim 1, characterized in that: The specific steps of step 1 are as follows: First, the cable is assumed to consist of N segments of equal length, mass, and uniform density. Adjacent segments are formed by frictionless nodes, and the mass of each segment is concentrated at the node. The buoy is connected to the last segment. The dynamic model of the cable-buoy system is described as follows: In the formula, r j P is the distance vector from node j-1 to node j; j m is the position vector of the j-th cable segment; j Let m be the mass of the j-th segment of the cable. d G is the mass of the towed buoy. j T j and Q j Let represent the gravity, tension, and external aerodynamic force on the j-th cable segment, respectively; E is the elastic Young's modulus of the cable; A = π(d / 2). 2 d is the cross-sectional area of ​​the cable; d is the diameter of the cable; l0 is the initial length of each cable segment; L j and D j Q represents the lift and resistance on the j-th cable segment; d It is the aerodynamics of the buoy; Next, a six-DOF dynamic model of the towed buoy is constructed using Newton's second law and aerodynamic theory. The following state vector is defined, and the six-DOF dynamic model of the towed buoy is converted into an affine nonlinear form to facilitate the subsequent controller design: In the formula, X i i = 1, 2, 3, 4 represent the flight state information of the trajectory loop, flight path loop, attitude loop, and angular velocity loop, respectively; x, y, z are the positions of the towed buoy; V x V y V z V0 is the normalized velocity; α and β are the airflow angles; φ is the roll angle; p, q, and r are the angular velocities; δ a ,δ e ,δ r F represents the deflection angle of the ailerons, elevator, and rudder. i i = 2, 3, 4 are used to describe unmeasurable cable tension; H i ,i=2,3,4 is the auxiliary function matrix; B i i = 2, 3, 4 is the system input matrix; U act For rudder surface control variables.

3. The method for coordinated control of a towed buoy's three-dimensional trajectory according to claim 1, characterized in that: In step 2, the following extended state observer is designed for the affine nonlinear models of the track loop, attitude loop, and angular velocity loop which are affected by external disturbances: In the formula, X is determined by the extended state observer i Estimate the values ​​of i = 2, 3, 4; The disturbance term F is determined by the extended state observer. i Estimate the parameter L for i = 2, 3, 4. 21 =diag(2ω) 21 ,2ω 22 ), L 31 =diag(2ω) 31 ,2ω 32 ,2ω 33 ), L 41 =diag(2ω) 41 ,2ω 42 ,2ω 43 ), These are the bandwidths of the linearly extended state observers for the track loop, attitude loop, and angular velocity loop, respectively.

4. The method for coordinated control of a towed buoy's three-dimensional trajectory according to claim 1, characterized in that: In step 3, a vertical controller for the towed buoy is designed. The system disturbance estimate obtained in step 2 is used as a feedforward compensation term. The controllers for the trajectory loop, track loop, attitude loop and angular velocity loop are designed in combination with the backstepping method. The precise control of the vertical trajectory of the towed buoy is achieved by directly controlling the deflection of the towed buoy's rudder surface.

5. The method for coordinated control of a towed buoy's three-dimensional trajectory according to claim 4, characterized in that: The specific steps of step 3 are as follows: Step 31: Define X1 as the actual trajectory of the towed buoy, X 1c For the desired trajectory, the trajectory tracking error is e1 = X1 - X 1c Differentiating both sides simultaneously, we get: Therefore, the virtual control law for the trajectory loop can be set as follows: In the formula, the positive definite matrix μ1=diag(μ 11 ,μ 12 ) represents the control parameters of the control law; Step 32: Define the actual speed X2 and the desired speed X 2c The difference is the speed tracking error e2 = X2 - X 2c Differentiating it, we get: This allows us to set the virtual control law for the trajectory loop: In the formula, the positive definite matrix μ2=diag(μ 21 ,μ 22 ) represents the control parameters of the control law; Step 33: Define the tracking error of the attitude loop as e3 = X3 - X 3c Taking its derivative, we get Based on the above equation, its virtual attitude control law can be designed. In the formula, the positive definite matrix μ3=diag(μ 31 ,μ 32 ,μ 33 ) represents the control parameters of the control law; Step 34: Define the angular velocity tracking error as the difference between the actual value and the expected value of the angular velocity, i.e., e4 = X4 - X 4c Taking the derivative of both sides of the tracking error equation, we can obtain: Therefore, the control law required for the final control input can be obtained as follows: In the formula, the positive definite matrix μ4 = diag(μ 41 ,μ 42 ,μ 43 ) represents the control parameters of this control law.

6. The method for coordinated control of a towed buoy's three-dimensional trajectory according to claim 1, characterized in that: In step 4, the deviation e between the actual cable length and the expected cable length is... l =L lc -L l As error feedback, a corresponding PID cable length controller is designed. By adjusting the cable winding and unwinding speed, the cable length can be stably controlled, thereby indirectly controlling the movement of the buoy in the forward direction.

7. The method for coordinated control of a towed buoy's three-dimensional trajectory according to claim 6, characterized in that: The selected PID control algorithm is described as follows: In the formula, L lc L represents the desired cable length. l K represents the actual cable length. p K i K d These are the proportional, integral, and derivative coefficients of the PID control algorithm.

Images