Aircraft and ship formation tracking sliding mode control method based on improved guidance algorithm

Abstract

The invention discloses a ship formation tracking sliding mode control method based on an improved guidance algorithm, and the method comprises the steps: solving an expected heading angle of an unmanned ship through an improved sight guidance algorithm, and obtaining an expected course angle of the unmanned ship; acquiring rudder angle input of the unmanned ship by establishing an unmanned ship path tracking control law based on the observation value of the unmanned ship, and establishing a position and attitude control law of the unmanned ship based on the observation value of the unmanned ship; the control law of the pitch angle channel of the unmanned aerial vehicle, the control law of the roll angle channel of the unmanned aerial vehicle and the control law of the yaw angle channel of the unmanned aerial vehicle are obtained, so that the tracking control of the aircraft-ship formation is realized. According to the method, the ship can be accurately guided under the condition of external interference, and the method has good feasibility and superiority.

Description

Technical Field

[0001] The invention relates to the technical field of unmanned ship and unmanned aerial vehicle motion control and modeling, and in particular to a sliding mode control method for tracking a motor-ship formation based on an improved guidance algorithm. Background Art

[0002] In recent years, with the development of unmanned technology and intelligent system technology, unmanned systems such as unmanned ships, drones, and unmanned underwater vehicles have been widely developed and applied in the field of navigation. Traditional cruise and maritime search and rescue missions rely solely on single or multiple unmanned ships to work together, which is just a single group operation. Due to the narrow field of view of unmanned ships, the search process becomes longer, reducing the efficiency of cruise or search and rescue, and it is impossible to achieve three-dimensional, diversified, and complex water operations. Compared with unmanned ships, drones have the advantages of small size, high speed, and wide observation range, while the endurance and load capacity of unmanned ships are significantly stronger than those of drones.

[0003] Therefore, when unmanned ship-unmanned aerial vehicle system works together to perform tasks such as cruising, monitoring and search and rescue, it can take advantage of both, improve the efficiency of water operations, and has broad application prospects. However, the existing coordinated control of aircraft and ships is easily affected by external interference and it is difficult to achieve accurate guidance. Summary of the invention

[0004] The invention discloses a sliding mode control method for tracking a motor-vessel formation based on an improved guidance algorithm, so as to overcome the above technical problems.

[0005] In order to achieve the above object, the technical solution of the present invention is:

[0006] A sliding mode control method for tracking a motor-vessel formation based on an improved guidance algorithm comprises the following steps:

[0007] S1: Establish a mathematical model of unmanned ship motion with three degrees of freedom and a mathematical model of unmanned aerial vehicle motion with six degrees of freedom considering external interference;

[0008] S2: According to the three-degree-of-freedom unmanned ship motion mathematical model, based on the improved line-of-sight guidance algorithm, obtaining the line-of-sight angle of the guidance algorithm to obtain the desired heading angle of the unmanned ship;

[0009] S3: Based on the sliding mode control algorithm, a control law for unmanned ship path tracking based on the observation value of the unmanned ship is established to obtain the rudder angle input of the unmanned ship according to the expected heading angle of the unmanned ship;

[0010] S4: Based on the sliding mode control algorithm and the six-degree-of-freedom UAV motion mathematical model, a position and attitude control law of the UAV based on the observation value of the UAV is established to obtain the control law of the pitch angle channel of the UAV, the control law of the roll angle channel of the UAV and the control law of the yaw angle channel of the UAV;

[0011] S5: According to the rudder angle input of the unmanned ship, the control law of the pitch angle channel of the unmanned aircraft, the control law of the roll angle channel of the unmanned aircraft and the control law of the yaw angle channel of the unmanned aircraft, the tracking control of the aircraft-ship formation is realized.

[0012] Beneficial effects: The sliding mode control method for tracking a motor-ship formation based on an improved guidance algorithm of the present invention solves the desired heading angle of the unmanned ship by improving the line-of-sight guidance algorithm, and then obtains the desired heading angle of the unmanned ship; and obtains the rudder angle input of the unmanned ship by establishing a control law for the path tracking of the unmanned ship based on the observation value of the unmanned ship, and establishes a position and attitude control law of the unmanned aircraft based on the observation value of the unmanned aircraft, so as to obtain the control law of the pitch angle channel of the unmanned aircraft, the control law of the roll angle channel of the unmanned aircraft, and the control law of the yaw angle channel of the unmanned aircraft, so as to realize the tracking control of the motor-ship formation. The present invention can accurately guide motor-ships in the presence of external interference, and has good feasibility and superiority. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0014] Figure 1 It is a flow chart of the sliding mode control method for tracking a motor-vessel formation based on an improved guidance algorithm of the present invention;

[0015] Figure 2 The structural block diagram of the control system in this embodiment is

[0016] Figure 3 Schematic diagram of the unmanned ship drone in different coordinate systems in this embodiment;

[0017] Figure 4 Schematic diagram of the three-dimensional guidance and tracking of the unmanned ship and unmanned aerial vehicle system in this embodiment;

[0018] Figure 5 This is a diagram showing the path tracking result of the aircraft-ship formation in the three-dimensional coordinate system in this embodiment;

[0019] Figure 6This is a diagram showing the path tracking result of the aircraft-ship formation in the two-dimensional coordinate system in this embodiment;

[0020] Figure 7 is a graph of path tracking performance at different radii in this embodiment;

[0021] Figure 8 Graph showing radius variation results under different conditions in this embodiment;

[0022] Fig. 9 The following is a diagram showing the change of the heading angle and rudder angle of the unmanned ship under different radii in this embodiment;

[0023] Fig.10 This is a comparison diagram of the UAV path tracking control effect with and without the genetic algorithm improved observer in this embodiment;

[0024] Fig.11 This is a schematic diagram of the control input of the first UAV in this embodiment;

[0025] Fig.12 This is a schematic diagram of the control input of the second drone in this embodiment:

[0026] Fig.13 The observation values ​​of the unmanned ship's bow angular velocity and total disturbance obtained by the observer improved by the genetic algorithm in this embodiment;

[0027] Fig.14 The online adjustment result of the parameters of the unmanned ship state observer optimized by the genetic algorithm in this embodiment;

[0028] Fig.15 are the observed values ​​of the pitch angle and total disturbance of the first UAV obtained by the observer improved by the genetic algorithm in this embodiment;

[0029] Fig.16 are the observed values ​​of the yaw angle and total disturbance of the first UAV obtained by the observer improved by the genetic algorithm in this embodiment;

[0030] Fig.17 are the observed values ​​of the roll angle and total disturbance of the first UAV obtained by the observer improved by the genetic algorithm in this embodiment;

[0031] Fig.18 This is the online adjustment result of the parameters of the first UAV pitch angle channel state observer optimized by the genetic algorithm in this embodiment;

[0032] Fig.19 This is the online adjustment result of the parameters of the yaw angle channel state observer of the first UAV optimized by the genetic algorithm in this embodiment.

[0033] Fig. 20This is the online adjustment result of the parameters of the roll angle channel state observer of the first UAV optimized by the genetic algorithm in this embodiment;

[0034] Fig.21 are the observed values ​​of the pitch angle and total disturbance of the second UAV obtained by the observer improved by the genetic algorithm in this embodiment;

[0035] Fig. 22 are the observed values ​​of the yaw angle and total disturbance of the second UAV obtained by the observer improved by the genetic algorithm in this embodiment;

[0036] Fig.23 These are the observed values ​​of the roll angle and total disturbance of the second UAV obtained by the observer improved by the genetic algorithm in this embodiment.

[0037] Fig.24 This is the online adjustment result of the parameters of the second UAV pitch angle channel state observer optimized by the genetic algorithm in this embodiment;

[0038] Fig.25 This is the online adjustment result of the parameters of the yaw angle channel state observer of the second UAV optimized by the genetic algorithm in this embodiment;

[0039] Fig.26 This is the online adjustment result of the parameters of the roll angle channel state observer of the second UAV optimized by the genetic algorithm in this embodiment. DETAILED DESCRIPTION

[0040] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0041] This embodiment introduces a sliding mode control method for tracking a motor-vessel formation based on an improved guidance algorithm. Figure 1 As shown, the following steps are included:

[0042] S1: Establish a mathematical model of unmanned ship motion with three degrees of freedom and a mathematical model of unmanned aerial vehicle motion with six degrees of freedom considering external interference;

[0043] Specifically, in this embodiment, the three-degree-of-freedom unmanned ship motion mathematical model is a leader motion mathematical model, and the six-degree-of-freedom unmanned aerial vehicle motion mathematical model is a follower motion mathematical model; a leader-follower motion model between an unmanned ship and two unmanned aerial vehicles is defined;

[0044] Specifically, the unmanned ship and the drone can communicate through a wireless self-organizing network, with the unmanned ship as the leader and the drone as the follower. By controlling the rotor speed of the drone to make it follow the movement path of the unmanned ship, the cooperative path tracking of the unmanned ship and the drone can be achieved. The control system block diagram is shown in the figure. Figure 2 Specifically, the attached coordinate system is established with the center of gravity of the unmanned ship and the unmanned aerial vehicle as the origin, and the kinematic models of the unmanned ship and the unmanned aerial vehicle are established based on the attached coordinate system, and the position coordinates of the kinematic models of the unmanned ship and the unmanned aerial vehicle are converted from space coordinates to the inertial coordinate system (OX O Y O Z O )like Figure 3 As shown; and the establishment of the hull appendage coordinate system and the inertial coordinate system and the transformation process of the space coordinates are all existing well-known technical means, which are not the invention of this application and will not be repeated here;

[0045] In a specific embodiment, the S1 comprises the following steps:

[0046] S101: Establish a mathematical model of the motion of an unmanned ship with three degrees of freedom, which is expressed as follows:

[0047]

[0048] Where: x L Indicates the horizontal coordinate of the position of the unmanned ship; y L represents the vertical coordinate of the position of the unmanned ship; ψ L Indicates the actual heading angle of the unmanned ship; u L Indicates the forward speed of the unmanned ship; v L Indicates the drift speed of the unmanned ship; r L V represents the bow angular velocity of the unmanned ship; c represents flow velocity; ψ c Indicates flow direction; m L Indicates the mass of the ship; m x Indicates the additional mass of the ship in the x-axis direction; m y represents the additional mass of the ship in the y-axis direction; X H represents the longitudinal force acting on the bare hull; X P represents the longitudinal force acting on the propeller; X W represents the longitudinal force caused by wind disturbance; X R represents the longitudinal force acting on the rudder; X wave represents the longitudinal force caused by wave disturbance; Y H represents the lateral force acting on the bare hull; Y P represents the lateral force acting on the propeller; Y W represents the lateral force caused by wind disturbance; Y Rrepresents the lateral force acting on the rudder; Y wave Indicates the longitudinal force caused by wave disturbance; N H represents the yaw moment acting on the bare hull; N P Indicates the yaw moment acting on the propeller; N R Indicates the yaw moment acting on the rudder; N W Indicates the yaw moment caused by wind disturbance; N wave I represents the yaw moment caused by wave disturbance; zz Indicates the inertia moment of the ship in the z-axis direction; J zz represents the additional inertia moment of the ship in the z-axis direction; t R Indicates the propeller drag reduction coefficient; α H represents the correction factor of the lateral force of the hull induced by steering with respect to the rudder force; x R Indicates the longitudinal distance from the rudder force action center to the ship's center of gravity; x H F represents the longitudinal distance between the point of action of the induced transverse force and the center of gravity of the ship; N represents the positive pressure of the rudder; δ represents the rudder angle input of the unmanned ship; represents the first-order differential;

[0049] Specifically, in order to simplify the design of the controller, this embodiment adopts the improved Nomoto model to design the control law, and its expression is as follows:

[0050]

[0051] Where: b L Indicates the actual gain of the steering gear servo system; T L represents the turning index of the unmanned ship; a represents the coefficient of the high-order nonlinear term of the model; f L represents the total disturbance to the unmanned ship; Indicates the gain of the steering gear servo system; w L represents the unmodeled dynamic part inside the unmanned ship; d L Indicates the external environment interference to the unmanned ship;

[0052] S102: Establish a mathematical model of the six-degree-of-freedom UAV motion, which is expressed as follows:

[0053]

[0054] Where: x i represents the horizontal coordinate of the position of the i-th UAV; y i represents the vertical coordinate of the position of the i-th UAV; z i represents the vertical coordinate of the i-th UAV; φ i represents the rolling angle of the i-th UAV; θ irepresents the actual pitch angle of the i-th UAV; ψ i represents the yaw angle of the i-th UAV; U 1i represents the vertical lift of the i-th UAV; U 2i represents the rolling moment of the i-th UAV; U 3i represents the pitch moment of the i-th UAV; U 4i represents the yaw moment of the i-th UAV; m represents the mass of the UAV; g represents the gravitational acceleration; Both represent the influence coefficient of air resistance on the position coordinates of the i-th UAV; I x Represents the moment of inertia of the quadrotor drone on the x-axis; I y Represents the moment of inertia of the quadrotor drone on the y-axis; I z Represents the moment of inertia of the quadrotor drone on the z-axis; J r represents the rotor inertia of the quadrotor drone; γ represents the algebraic sum of the motor speeds; All represent the influence coefficient of the gyro effect on the Euler angle of the i-th UAV; l represents the wheelbase of the quadrotor UAV; i represents the number of the UAV; represents the second-order differential; f θi , f ψi They represent the disturbance of the pitch angle channel, the roll angle channel and the yaw angle channel of the i-th UAV respectively;

[0055] S2: According to the three-degree-of-freedom unmanned ship motion mathematical model, based on the improved line-of-sight guidance algorithm, obtaining the line-of-sight angle of the guidance algorithm to obtain the desired heading angle of the unmanned ship;

[0056] Specifically, this embodiment designs and improves the line-of-sight guidance algorithm for unmanned ships, changes the acceptance circle radius of the ship at the turning point by adjusting parameters, reduces the tracking error at the turning point, and realizes the path guidance of the unmanned ship; the specific principle diagram is as follows Figure 4 As shown, the following steps are included:

[0057] Preferably, the method for obtaining the desired heading angle of the unmanned ship is as follows:

[0058] S21: With the position of the unmanned ship as the center, a circle for solving the guidance point is established to obtain the coordinates of the guidance point, and then the sight angle of the guidance algorithm can be obtained;

[0059] Preferably, the formula used to obtain the coordinates of the guidance point is as follows:

[0060]

[0061] Where: x los Indicates the horizontal coordinate of the guidance point; y losIndicates the ordinate of the guidance point; x L Indicates the horizontal coordinate of the position of the unmanned ship; y L represents the vertical coordinate of the position of the unmanned ship; R represents the radius of the circle; θ k Indicates the angle between the straight line segment path and the true north direction;

[0062] In order to ensure that x los ,y los If there is a solution, the circle and the expected path have an intersection. The time-varying radius is designed by using the length of the unmanned boat and the vertical distance between the unmanned boat and the path. The expression is:

[0063]

[0064] Where: R represents the radius of the circle; k los1 Represents the weight coefficient of the error term in the circle radius; y Le represents the forward distance along the moving direction of the unmanned ship; k los2 It represents the weight coefficient of the ship length in the circle radius; L represents the length of the unmanned ship; A, B, and C all represent the coefficients of the straight line equation written by the straight line segment path;

[0065] Preferably, the sight angle of the guidance algorithm is obtained as follows:

[0066] Specifically, combined with the position information of the unmanned ship, the target point can be guided and the sight angle can be calculated according to the trigonometric function, and its expression is:

[0067]

[0068] Where: los represents the sight angle obtained by the guidance algorithm;

[0069] S22: Obtaining a desired heading angle of the ship according to the sight angle obtained by the guidance algorithm;

[0070] Specifically, when the ship sails near the turning point, an acceptance circle is designed to enable the ship to turn and track the next path. The expression is:

[0071] Preferably, the desired heading angle of the ship is obtained as follows:

[0072] Combining the above expressions and considering the influence of the drift angle, the expression of the desired heading angle solved by the line-of-sight guidance algorithm is:

[0073]

[0074] Where: Ld represents the expected heading angle of the unmanned ship; β represents the drift angle of the unmanned ship during navigation.

[0075] Preferably, after obtaining the desired heading angle of the ship, an acceptance circle for the ship to achieve steering at the turning point is established to satisfy the following conditions at the position of the unmanned ship:

[0076]

[0077] When the desired heading angle of the ship is updated, S21-S22 are repeatedly executed.

[0078] Where: x w+1 Indicates the horizontal coordinate of the next turning point; y w+1 represents the ordinate of the next turning point; RM represents the radius of the designed acceptance circle; σ represents the complementary angle between the current tracked path and the next tracked path; k los3 ,k los4 Both represent the coefficient of the design acceptance circle radius; L represents the length of the unmanned ship.

[0079] S3: Based on the sliding mode control algorithm, a control law for unmanned ship path tracking based on the observation values ​​of the unmanned ship, that is, the observation values ​​of the bow angular velocity of the unmanned ship and the observation values ​​of the total disturbance is established to obtain the rudder angle input of the unmanned ship according to the desired heading angle of the unmanned ship;

[0080] S5: According to the rudder angle input of the unmanned ship, the pitch angle of the drone, the roll angle of the drone and the yaw angle of the drone, tracking control of the drone-ship formation is achieved.

[0081] To ensure that the system achieves path tracking control;

[0082] Preferably, in S3, the rudder angle input of the unmanned ship is obtained as follows:

[0083] Define the error between the heading angle of the unmanned ship and the expected heading angle:

[0084] ψ Le =ψ Ld -ψ L

[0085] Where: L represents the heading angle of the unmanned ship; ψ Le represents the error between the actual heading angle of the unmanned ship and the expected heading angle; ψ Ld represents the desired heading angle of the unmanned ship;

[0086] The nonlinear sliding surface of the unmanned ship is established, and its expression is:

[0087]

[0088] In order to stabilize the nonlinear sliding surface of the unmanned ship, an exponential reaching law is designed, then:

[0089]

[0090] According to the three-degree-of-freedom unmanned ship motion mathematical model, the control law for unmanned ship path tracking is obtained as follows:

[0091]

[0092] Where: s represents the nonlinear sliding surface of the designed unmanned ship; represents the first-order differential of the nonlinear sliding surface of the designed unmanned ship; ψ L Indicates the actual heading angle of the unmanned ship; They represent the first-order and second-order differentials of the error between the actual heading angle of the unmanned ship and the expected heading angle; k usv1 ,k usv2 ,k usv3 All represent the design parameters of the nonlinear sliding surface of the unmanned ship; δ represents the rudder angle input of the unmanned ship; b Lr Represents the unmanned ship model parameters; Represents the second-order differential of the desired heading angle of the unmanned ship; represents the observed value of the bow angular velocity, obtained by the observer; T L Indicates the turning index of the unmanned ship; represents the observed value of the total disturbance, obtained by the observer;

[0093] S4: Based on the sliding mode control algorithm and the six-degree-of-freedom UAV motion mathematical model, the position and attitude control law of the UAV is established to obtain the control law of the pitch angle channel of the UAV, the control law of the roll angle channel of the UAV and the control law of the yaw angle channel of the UAV based on the observation value of the UAV;

[0094] Preferably, the position and attitude control law of the UAV is obtained as follows:

[0095] S41: Establishing the virtual control law of the UAV:

[0096] First, the method used to establish the virtual control law of the drone in the x direction is as follows:

[0097] Define the path tracking error in the x-direction of the drone and make the control law:

[0098]

[0099] Where: x ei represents the position error of the i-th UAV in the x-axis direction; x di represents the expected position of the i-th UAV in the x-axis direction; x i Represents the horizontal coordinate of the position of the i-th UAV; Represents the first-order differential of the position error of the i-th UAV in the x-axis direction; represents the first-order differential of the expected position of the i-th UAV in the x-axis direction; Represents the first-order differential of the actual position of the i-th UAV in the x-axis direction;

[0100] Design the sliding surface of the UAV in the x direction, and its expression is:

[0101]

[0102] Where: s xi represents the sliding surface in the x-direction of the designed UAV; Represents the first-order differential of the sliding surface in the x-direction of the designed UAV; represents the second-order differential of the position error of the i-th UAV in the x-axis direction; x represents the design parameters of the sliding surface in the x-direction of the UAV;

[0103] Among them, in order to stabilize the sliding surface in the x direction, the hyperbolic tangent reaching law in the x direction is designed, then:

[0104]

[0105] So the virtual control law of the drone in the x direction is obtained as:

[0106]

[0107] Where: k x0 ,k x1 , η x All represent the design parameters of the reaching law of the UAV in the x direction, where k x0 is used to adjust the approach speed of the sliding surface in the x direction; k x1 is the coordinate used to compress the nonlinear sliding surface in the x direction; η x is the amplitude of the nonlinear sliding mode in the x direction; Represents the virtual control law of the UAV in the x-axis direction; represents the second-order differential of the expected position of the i-th UAV in the x-axis direction; m represents the mass of the UAV;

[0108] The method used to obtain the virtual control law of the UAV in the y and z directions is the same as the method used to obtain the virtual control law of the UAV in the x direction, so:

[0109] The virtual control law of the UAV in the y direction is:

[0110]

[0111] The virtual control law of the UAV in the z direction is:

[0112]

[0113] Where: U 1yi represents the virtual control law of the UAV in the y-axis direction; U 1zi Represents the virtual control law of the UAV in the z-axis direction; Represents the second-order differential of the expected position of the i-th UAV in the y-axis direction; Represents the second-order differential of the desired position of the i-th UAV in the z-axis direction; Both represent the influence coefficient of air resistance on the position coordinates of the i-th UAV; Represents the first-order differential of the actual position of the i-th UAV in the y-axis direction; Represents the first-order differential of the position error of the i-th UAV in the x-axis direction; represents the first-order differential of the actual position of the i-th UAV in the z-axis direction; y represents the design parameters of the sliding surface in the y direction of the UAV; k y0 , k y1 , η y All represent the design parameters of the reaching law of the UAV in the y direction, where k y0 is used to adjust the approach speed of the sliding surface in the y direction; k y1 is the coordinate used to compress the nonlinear sliding surface in the y direction; η y is the amplitude of the nonlinear sliding mode in the y direction; s yi represents the sliding surface of the designed UAV in the y direction; g represents the gravitational acceleration; λ z The design parameters of the sliding surface in the z direction of the UAV are represented; represents the first-order differential of the position error of the i-th UAV in the z-axis direction; k z0 , k z1 , η z Both represent the design parameters of the reaching law of the UAV in the z direction; s zi represents the sliding surface of the designed UAV in the z direction;

[0114] S42: obtaining a desired roll angle and a desired pitch angle of the drone according to a virtual control law of the drone and the six-degree-of-freedom drone motion mathematical model;

[0115] Preferably, the desired pitch angle of the drone and the desired roll angle of the drone are obtained as follows:

[0116]

[0117] In the formula, represents the vertical lift of the i-th UAV, m represents the mass of the UAV; φ di The expected roll angle of the i-th UAV; θ diThe expected pitch angle of the i-th UAV; ψ di The expected yaw angle of the i-th UAV; due to the under-actuated nature of the quadcopter, it cannot track six degrees of freedom at the same time. Specifically, the yaw angle in this embodiment is set to 0 degrees, that is, the UAV tracks the surface unmanned ship in the north direction.

[0118] S43: acquiring a control law of a pitch angle channel of the UAV, a control law of a roll angle channel of the UAV, and a control law of a yaw angle channel of the UAV according to the expected pitch angle of the UAV, the expected roll angle of the UAV, and the expected yaw angle of the UAV, respectively;

[0119] Preferably, the control law of the pitch angle channel of the drone, the control law of the roll angle channel of the drone, and the control law of the yaw angle channel of the drone are respectively obtained as follows:

[0120] First, obtain the control law of the pitch angle channel of the drone. The method used is as follows:

[0121] Get the error between the expected pitch angle and the actual pitch angle of the drone:

[0122]

[0123] Design the sliding surface of the UAV pitch angle, and its expression is:

[0124]

[0125] Among them, in order to stabilize the sliding surface of the pitch angle of the UAV, the hyperbolic tangent reaching law of the pitch angle of the UAV is designed, then

[0126]

[0127] So the control law of the pitch angle channel of the UAV is obtained as:

[0128]

[0129] Where: θ ei represents the error between the expected pitch angle and the actual pitch angle of the i-th UAV; Represents the first-order differential of the error between the expected pitch angle and the actual pitch angle of the i-th UAV; Represents the first-order differential of the error between the expected pitch angle and the actual pitch angle of the i-th UAV; θ di represents the expected pitch angle of the i-th UAV; represents the first-order differential of the desired pitch angle of the i-th UAV; θ i represents the actual pitch angle of the i-th UAV; represents the second-order differential of the desired pitch angle of the i-th UAV; represents the first-order differential of the actual pitch angle of the i-th UAV; s θi The sliding surface representing the designed UAV pitch angle; represents the first-order differential component of the sliding surface of the pitch angle of the designed UAV; θ represents the design parameters of the sliding surface; k θ0 , k θ1 , η θ Both represent the design parameters of the reaching law of the pitch angle; I x Represents the moment of inertia of the drone on the x-axis; I y Represents the moment of inertia of the drone on the y-axis; I z Represents the moment of inertia of the drone on the z-axis; J r represents the rotor inertia of the drone; Represents the first-order differential of the actual roll angle of the i-th UAV; represents the first-order differential of the actual yaw angle of the i-th UAV; γ represents the algebraic sum of the speeds of the four rotor motors; l represents the wheelbase of the quadrotor UAV; represents the observed value of the pitch angle channel disturbance of the i-th UAV;

[0130] The method used to obtain the control law of the roll angle channel of the drone and the control law of the yaw angle channel of the drone is the same as the method used to obtain the control law of the pitch angle channel of the drone, so:

[0131] The control law of the roll angle channel of the UAV is:

[0132]

[0133] The control law of the yaw angle channel of the UAV is:

[0134]

[0135] Where: represents the second-order differential of the desired roll angle of the i-th UAV; Both represent the influence coefficient of gyroscopic effect on the Euler angle of the i-th UAV; represents the observed value of the roll angle channel disturbance of the i-th UAV; represents the observed value of the yaw angle channel disturbance of the i-th UAV; λ φ The design parameters of the sliding surface representing the rolling angle of the UAV; λ ψ Design parameters of the sliding surface representing the yaw angle of the UAV; represents the second-order differential of the desired yaw angle of the i-th UAV; k φ0 is used to adjust the approach rate of the sliding surface in the rolling direction; k ψ0 Used to adjust the approach rate of the sliding surface in the yaw direction; η φis the amplitude of the nonlinear sliding mode in the rolling direction; η ψ is the amplitude of the nonlinear sliding mode in the yaw direction; k φ1 Coordinates used to compress the nonlinear sliding surface in the rolling direction; k ψ1 Coordinates used to compress the nonlinear sliding surface in the yaw direction; Represents the first-order differential of the error between the expected roll angle and the actual roll angle of the i-th UAV; represents the first-order differential of the error between the expected yaw angle and the actual yaw angle of the i-th UAV; s φi The sliding surface representing the designed UAV roll angle; s ψi Sliding surface representing the designed UAV yaw angle.

[0136] S5: According to the rudder angle input of the unmanned ship, the control law of the pitch angle channel of the unmanned aircraft, the control law of the roll angle channel of the unmanned aircraft and the control law of the yaw angle channel of the unmanned aircraft, the tracking control of the aircraft-ship formation is realized.

[0137] Preferably, the observation values ​​of the unmanned ship and the observation values ​​of the unmanned aerial vehicle in this embodiment are both obtained through an extended state observer;

[0138] The extended state observer is established as follows:

[0139] Specifically, this embodiment uses a genetic algorithm to improve the unmodeled dynamics part and external disturbance in the extended state observer estimation model, optimize parameter adjustment and improve observation accuracy.

[0140] S41: Establish an extended state observer to observe the interference of unmanned ships and drones. Its expression is:

[0141]

[0142]

[0143] Where: Represents the observed value of the unmanned ship's heading angle; represents the observed value of the unmanned ship's bow angular velocity; represents the observed value of the total disturbance; l L1 ,l L2 ,l L3 Both represent the design parameters of the observer of the unmanned ship; ψ L Indicates the actual heading angle of the unmanned ship; b L represents the actual gain of the steering gear servo system; δ represents the rudder angle input of the unmanned ship; represents the first-order differential of the roll angle of the i-th UAV; T L represents the turning index of the unmanned ship; a represents the coefficient of the high-order nonlinear term of the model; represents the observed value of the first-order differential component of the roll angle of the i-th UAV; represents the observed value of the roll angle of the i-th UAV; represents the first-order differential of the roll angle observation value of the i-th UAV; l φ1 ,l φ2 ,l φ3 Both represent the design parameters of the observer of the UAV roll angle; φ i represents the rolling angle of the i-th UAV; θ i represents the actual pitch angle of the i-th UAV; Represents the first-order differential of the pitch angle of the i-th UAV; represents the observed value of the pitch angle of the i-th UAV; represents the observed value of the first-order differential of the pitch angle of the i-th UAV; ψ i represents the yaw angle of the i-th UAV; represents the first-order differential of the yaw angle of the i-th UAV; represents the first-order differential observation value of the i-th UAV; γ represents the algebraic sum of the motor speed; Both represent the influence coefficient of gyroscopic effect on the Euler angle of the i-th UAV; represents the disturbance observation value of the roll angle channel of the i-th UAV; l θ1 ,l θ2 ,l θ3 All represent the design parameters of the observer of the UAV pitch angle; They represent the pitch angle observation value of the i-th UAV, the first-order differential observation value of the pitch angle, and the first-order differential observation value of the pitch angle channel disturbance observation value respectively; represents the disturbance observation value of the pitch angle channel of the i-th UAV; They represent the first-order differentials of the unmanned ship’s heading angle observation value, the bow angular velocity observation value, and the disturbance observation value respectively; They represent the yaw angle observation value of the i-th UAV, the first-order differential observation value of the yaw angle, and the first-order differential observation value of the yaw angle channel disturbance observation value respectively; represents the observed value of the yaw angle of the i-th UAV; represents the observed value of the first-order differential component of the yaw angle of the i-th UAV; represents the disturbance observation value of the yaw angle channel of the i-th UAV; l ψ1 ,l ψ2 ,l ψ3 The design parameters of the observer representing the yaw angle of the UAV;

[0144] Among them, the design parameters of the observer of the unmanned ship, the design parameters of the observer of the roll angle of the unmanned aircraft, the design parameters of the observer of the pitch angle of the unmanned aircraft, and the design parameters of the observer of the yaw angle of the unmanned aircraft are all obtained through the predicted value expression of the state quantity and the general formula of the state observer;

[0145] Specifically, this embodiment improves the extended state observer by combining a genetic algorithm to achieve online adjustment of the extended state observer parameters.

[0146] The predicted value expression of the state quantity and the general expression of the state observer are:

[0147]

[0148] in,

[0149]

[0150] Using the prediction error of the state variables, the evaluation function of the genetic algorithm is established, and its expression is:

[0151]

[0152] Where: x1(k)=[ψ L φ i θ i ψ i ] T is the ship heading angle ψ L , UAV roll angle φ i , tilt angle θ i and the yaw angle ψ i The vector matrix of is the differential of the vector matrix x1(k); f(k) = [f L f φi f θi f ψi ] T is the external disturbance of the ship L , the disturbance f in the roll angle direction of the UAV φi , the disturbance in the transverse inclination direction f θi and the disturbance in the yaw angle direction f ψi The vector matrix of represents the predicted observation value of x1(k) at time k+1; T p represents the prediction step length; represents the predicted observation value of x2(k) at time k+1; represents the predicted observation value of f(k) at k+1; represents the estimated value of x1(k) at time k; represents the estimated value of x2(k) at time k; represents the estimated value of f(k) at time k; b represents the design parameters of the observer; u represents the control input of the system; g(x) represents the known part of the system, specifically the structure of the known part of the system; l i1 represents the first parameter of the observer; l i2 Represents the second parameter of the observer; l i3 represents the third parameter of the observer; represents the predicted value of the state error of x1(k) at time k+1; x1(k+1) represents the state of x1(k) at time k+1; n p represents the prediction interval; J* represents the evaluation function of the genetic algorithm; represents the prediction error of x1(k); j represents the number of backward prediction steps; k represents the starting time of the prediction; a p ,b p Respectively represent l ip The upper and lower bounds of the value interval; l ip represents the design parameters of the observer.

[0153] Specifically, i1 For example, i1 The interval is divided into 2 n1 -1, select sub-variable library S1(s 11 ,s 12 ,...s 1m ) and convert the subvariable from binary to decimal, the expression is

[0154]

[0155] The same goes for l i2 ,l i3 The selection and conversion of sub-variable libraries and l i1 same.

[0156] By integrating the sub-variables obtained in the above process, we can get the overall variable s with a binary digit of N. m , N = n1 + n2 + n3, and the total database has M variables, then the binary matrix of the database can be obtained

[0157]

[0158] Substitute the decimal value of the sub-variable into the evaluation function J*. Since its function value is greater than or equal to 0, the obtained J* value can be used as the fitness of the variable and the probability of the variable in this library is calculated. Finally, mark each variable in order on the [0,1] interval axis.

[0159]

[0160] In the optimization process, the three methods of selection, crossover and mutation are used together, and the population is updated and iterated continuously to obtain the optimal l i1 ,l i2 ,l i3 In each cycle, the best variable is selected and retained, so that each cycle is based on the optimal value of the previous cycle, realizing online optimization of parameters.

[0161] Where: w 1m Represents the value in decimal form in the subdatabase; v 1m Represents the corresponding binary value; n t ,t=1,2,3 are constant values, which determine the number of parts into which the corresponding interval is divided; s m represents the variables in the total database; p m Represents the probability value corresponding to a variable;

[0162] The simulation results in this embodiment are as follows Figures 5 to 26 As shown, Figure 5 This is the result diagram of the coordinated path tracking of the aircraft-ship formation in a three-dimensional coordinate system. Figure 5 The collaborative trajectory tracking in 3D coordinate system is demonstrated, where the reference trajectory yd is generated by pre-set waypoints. Figure 5 It can be seen that the improved LOS guidance system enables the USV to successfully track the waypoints even under external interference, especially effectively reducing overshoot during turns. Figure 6 The collaborative path tracking result diagram of the above system in a two-dimensional plane shows the collaborative trajectory tracking of USV and UAVs in a more intuitive way. Figure 7 is the path tracking performance graph for different radii; Figure 8 It is the radius change result diagram under different conditions; Fig. 9 This is the change diagram of the heading angle and rudder angle of the unmanned ship under different radii; Fig. 9 It can be clearly seen that different RMs have a significant impact on the path tracking performance. Fig.10 The following is a comparison of the UAV path tracking control effects with and without the genetic algorithm improved extended state observer; Fig.10 It can be clearly seen that both drones are able to effectively track the reference trajectory. However, the drone using GAESO has less deviation in the path tracking process. Fig.11 Schematic diagram of control input for the first drone; Fig.12 Schematic diagram of the control input of the second UAV, indicating that the control forces / torques of both UAVs are reasonable; Fig.13 The observation values ​​of the unmanned ship's bow angular velocity and total disturbance obtained by the observer improved by genetic algorithm indicate that the proposed ESO based on genetic algorithm can effectively estimate the unmanned ship's bow angular velocity and total disturbance; Fig.14 It is the result of online adjustment of parameters of the unmanned ship state observer optimized by genetic algorithm; Fig.15 is the observed value of the pitch angle and total disturbance of the first UAV obtained by the observer improved by genetic algorithm; Fig.16 is the observed value of the yaw angle and total disturbance of the first UAV obtained by using the improved observer of genetic algorithm; Fig.17 is the observed value of the roll angle and total disturbance of the first UAV obtained by using the improved observer of genetic algorithm; Fig.18 It is the result of online adjustment of the parameters of the state observer of the pitch angle channel of the first UAV optimized by using genetic algorithm; Fig.19 It is the result of online adjustment of the parameters of the state observer of the yaw angle channel of the first UAV optimized by using genetic algorithm; Fig. 20 The results of online adjustment of the parameters of the first UAV roll angle channel state observer were optimized using genetic algorithm; Fig.21 is the observed value of the pitch angle and total disturbance of the second UAV obtained by using the improved observer of genetic algorithm; Fig. 22 are the observed values ​​of the yaw angle and total disturbance of the second UAV obtained using the improved observer using the genetic algorithm; Fig.23 is the observed value of the roll angle and total disturbance of the second UAV obtained by using the improved observer of genetic algorithm; Fig.24 It is the result of online adjustment of the parameters of the state observer of the pitch angle channel of the second UAV optimized by using genetic algorithm; Fig.25 It is the result of online adjustment of the parameters of the yaw angle channel state observer of the second UAV optimized by using genetic algorithm; Fig.26 The results of online adjustment of the parameters of the roll angle channel state observer of the second UAV are optimized using genetic algorithm; Figure 15-26 It is shown that the proposed ESO based on genetic algorithm is also effective for different states of the UAV and disturbances on each state component.

[0163] And it can be known from the simulation experiment that the beneficial effects of this embodiment are: the sliding mode control method for tracking a motorboat formation based on the improved guidance algorithm uses the improved guidance algorithm to solve the desired heading angle of the unmanned ship, and the sliding mode control is used to design the control law of the unmanned ship and the unmanned aerial vehicle. The genetic algorithm is combined to improve the state observer to realize the observation of some state quantities and disturbance quantities, reduce the difficulty of parameter adjustment, and improve the observation accuracy. This method has good feasibility and superiority in the presence of external disturbances.

[0164] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A sliding mode control method for tracking a motor-vessel formation based on an improved guidance algorithm, characterized in that: The following steps are involved: S1: Establish a mathematical model of unmanned ship motion with three degrees of freedom and a mathematical model of unmanned aerial vehicle motion with six degrees of freedom considering external interference; S2: According to the three-degree-of-freedom unmanned ship motion mathematical model, based on the improved line-of-sight guidance algorithm, obtaining the line-of-sight angle of the guidance algorithm to obtain the desired heading angle of the unmanned ship; S3: Based on the sliding mode control algorithm, a control law for unmanned ship path tracking based on the observation value of the unmanned ship is established to obtain the rudder angle input of the unmanned ship according to the expected heading angle of the unmanned ship; S4: Based on the sliding mode control algorithm and the six-degree-of-freedom UAV motion mathematical model, a position and attitude control law of the UAV based on the observation value of the UAV is established to obtain the control law of the pitch angle channel of the UAV, the control law of the roll angle channel of the UAV and the control law of the yaw angle channel of the UAV; S5: According to the rudder angle input of the unmanned ship, the control law of the pitch angle channel of the unmanned aircraft, the control law of the roll angle channel of the unmanned aircraft and the control law of the yaw angle channel of the unmanned aircraft, the tracking control of the aircraft-ship formation is realized.

2. The sliding mode control method for tracking a motorship formation based on an improved guidance algorithm according to claim 1 is characterized in that: In S4, the method for establishing the position and attitude control law of the UAV is as follows: S41: Establishing the virtual control law of the UAV: First, establish the virtual control law of the UAV in the x direction. The method used is as follows: Define the path tracking error in the x-direction of the drone and make the control law: Where: x ei represents the position error of the i-th UAV in the x-axis direction; x di represents the expected position of the i-th UAV in the x-axis direction; x i Represents the horizontal coordinate of the position of the i-th UAV; Represents the first-order differential of the position error of the i-th UAV in the x-axis direction; represents the first-order differential of the expected position of the i-th UAV in the x-axis direction; Represents the first-order differential of the actual position of the i-th UAV in the x-axis direction; Design the sliding surface of the UAV in the x direction, and its expression is: Where: s xi represents the sliding surface in the x-direction of the designed UAV; Represents the first-order differential of the sliding surface in the x-direction of the designed UAV; represents the second-order differential of the position error of the i-th UAV in the x-axis direction; x represents the design parameters of the sliding surface in the x-direction of the UAV; in, So the virtual control law of the drone in the x direction is obtained as: Where: k x0 is used to adjust the approach speed of the sliding surface in the x direction; k x1 is the coordinate used to compress the nonlinear sliding surface in the x direction; η x is the amplitude of the nonlinear sliding mode in the x direction; Represents the virtual control law of the UAV in the x-axis direction; represents the second-order differential of the expected position of the i-th UAV in the x-axis direction; m represents the mass of the UAV; The method used to obtain the virtual control law of the UAV in the y and z directions is the same as the method used to obtain the virtual control law of the UAV in the x direction, so: The virtual control law of the UAV in the y direction is: The virtual control law of the UAV in the z direction is: Where: U 1yi represents the virtual control law of the UAV in the y-axis direction; U 1zi Represents the virtual control law of the UAV in the z-axis direction; Represents the second-order differential of the expected position of the i-th UAV in the y-axis direction; Represents the second-order differential of the desired position of the i-th UAV in the z-axis direction; Both represent the influence coefficient of air resistance on the position coordinates of the i-th UAV; Represents the first-order differential of the actual position of the i-th UAV in the y-axis direction; Represents the first-order differential of the position error of the i-th UAV in the x-axis direction; represents the first-order differential of the actual position of the i-th UAV in the z-axis direction; y represents the design parameters of the sliding surface in the y direction of the UAV; k y0 is used to adjust the approach speed of the sliding surface in the y direction; k y1 is the coordinate used to compress the nonlinear sliding surface in the y direction; η y is the amplitude of the nonlinear sliding mode in the y direction; s yi represents the sliding surface of the designed UAV in the y direction; g represents the gravitational acceleration; λ z The design parameters of the sliding surface in the z direction of the UAV are represented; represents the first-order differential of the position error of the i-th UAV in the z-axis direction; k z0 , k z1 , η z Both represent the design parameters of the reaching law of the UAV in the z direction; s zi represents the sliding surface of the designed UAV in the z direction; S42: obtaining a desired roll angle and a desired pitch angle of the drone according to a virtual control law of the drone and the six-degree-of-freedom drone motion mathematical model; The desired pitch angle of the drone and the desired roll angle of the drone are obtained as follows: In the formula, m represents the mass of the UAV; φ di The expected roll angle of the i-th UAV; θ di The expected pitch angle of the i-th UAV; ψ di The expected yaw angle of the i-th UAV; S43: Obtain a control law of a pitch angle channel of the UAV, a control law of a roll angle channel of the UAV, and a control law of a yaw angle channel of the UAV according to a desired pitch angle of the UAV, a desired roll angle of the UAV, and a desired yaw angle of the UAV, respectively.

3. The sliding mode control method for tracking a motorship formation based on an improved guidance algorithm according to claim 2 is characterized in that: In S43, the control law of the pitch angle channel of the drone, the control law of the roll angle channel of the drone, and the control law of the yaw angle channel of the drone are respectively obtained as follows: First, obtain the control law of the pitch angle channel of the drone. The method used is as follows: Get the error between the expected pitch angle and the actual pitch angle of the drone: Design the sliding surface of the UAV pitch angle, and its expression is: in, So the control law of the pitch angle channel of the UAV is obtained as: Where: θ ei represents the error between the expected pitch angle and the actual pitch angle of the i-th UAV; Represents the first-order differential of the error between the expected pitch angle and the actual pitch angle of the i-th UAV; Represents the first-order differential of the error between the expected pitch angle and the actual pitch angle of the i-th UAV; θ di represents the expected pitch angle of the i-th UAV; represents the first-order differential of the desired pitch angle of the i-th UAV; θ i represents the actual pitch angle of the i-th UAV; represents the second-order differential of the desired pitch angle of the i-th UAV; represents the first-order differential of the actual pitch angle of the i-th UAV; s θi The sliding surface representing the designed UAV pitch angle; represents the first-order differential of the sliding surface of the designed UAV pitch angle; λθ represents the design parameter of the sliding surface; k θ0 , k θ1 ,θ θ Both represent the design parameters of the reaching law of the pitch angle; I x Represents the moment of inertia of the drone on the x-axis; I y Represents the moment of inertia of the drone on the y-axis; I z Represents the moment of inertia of the drone on the z-axis; J r represents the rotor inertia of the drone; Represents the first-order differential of the actual roll angle of the i-th UAV; represents the first-order differential of the actual yaw angle of the i-th UAV; γ represents the algebraic sum of the speeds of the four rotor motors; l represents the wheelbase of the quadrotor UAV; represents the observed value of the pitch angle channel disturbance of the i-th UAV; The method used to obtain the control law of the roll angle channel of the drone and the control law of the yaw angle channel of the drone is the same as the method used to obtain the control law of the pitch angle channel of the drone, so: The control law of the roll angle channel of the UAV is: The control law of the yaw angle channel of the UAV is: Where: represents the second-order differential of the desired roll angle of the i-th UAV; Both represent the influence coefficient of gyroscopic effect on the Euler angle of the i-th UAV; represents the observed value of the roll angle channel disturbance of the i-th UAV; represents the observed value of the yaw angle channel disturbance of the i-th UAV; λφ represents the design parameter of the sliding mode surface of the UAV roll angle; λ ψ Design parameters of the sliding surface representing the yaw angle of the UAV; represents the second-order differential of the desired yaw angle of the i-th UAV; k φ0 is used to adjust the approach rate of the sliding surface in the rolling direction; k ψ0 Used to adjust the approach rate of the sliding surface in the yaw direction; η φ is the amplitude of the nonlinear sliding mode in the rolling direction; η ψ is the amplitude of the nonlinear sliding mode in the yaw direction; k φ1 Coordinates used to compress the nonlinear sliding surface in the rolling direction; k ψ1 Coordinates used to compress the nonlinear sliding surface in the yaw direction; Represents the first-order differential of the error between the expected roll angle and the actual roll angle of the i-th UAV; represents the first-order differential of the error between the expected yaw angle and the actual yaw angle of the i-th UAV; s φi The sliding surface representing the designed UAV roll angle; s ψi Sliding surface representing the designed UAV yaw angle.

4. The sliding mode control method for tracking a motorship formation based on an improved guidance algorithm according to claim 1 is characterized in that: The mathematical model of the three-degree-of-freedom unmanned ship motion is established as follows: Where: x L represents the horizontal coordinate of the position of the unmanned ship; yL represents the vertical coordinate of the position of the unmanned ship; ψ L Indicates the actual heading angle of the unmanned ship; u L Indicates the forward speed of the unmanned ship; v L Indicates the drift speed of the unmanned ship; r L V represents the bow angular velocity of the unmanned ship; c Indicates flow velocity; ψ c Indicates flow direction; m L Indicates the mass of the ship; m x Indicates the additional mass of the ship in the x-axis direction; m y represents the additional mass of the ship in the y-axis direction; X H represents the longitudinal force acting on the bare hull; X P represents the longitudinal force acting on the propeller; X W represents the longitudinal force caused by wind disturbance; X R represents the longitudinal force acting on the rudder; X wave represents the longitudinal force caused by wave disturbance; Y H represents the lateral force acting on the bare hull; Y P represents the lateral force acting on the propeller; Y W represents the lateral force caused by wind disturbance; Y R represents the lateral force acting on the rudder; Y wave Indicates the longitudinal force caused by wave disturbance; N H represents the yaw moment acting on the bare hull; N P Indicates the yaw moment acting on the propeller; N R Indicates the yaw moment acting on the rudder; N W Represents the yaw moment caused by wind disturbance; N wave I represents the yaw moment caused by wave disturbance; zz Indicates the inertia moment of the ship in the z-axis direction; J zz represents the additional inertia moment of the ship in the z-axis direction; t R Indicates the propeller resistance reduction coefficient; α H represents the correction factor of the lateral force of the hull induced by steering with respect to the rudder force; x R Indicates the longitudinal distance from the rudder force action center to the ship's center of gravity; x H F represents the longitudinal distance between the point of action of the induced transverse force and the center of gravity of the ship; N represents the positive pressure of the rudder; δ represents the rudder angle input of the unmanned ship; represents the first-order differential; in: Where: b L Indicates the actual gain of the steering gear servo system; T L represents the turning index of the unmanned ship; a represents the coefficient of the high-order nonlinear term of the model; f L represents the total disturbance to the unmanned ship; represents the gain of the steering gear servo system; wL represents the unmodeled dynamic part inside the unmanned ship; d L Indicates the external environment interference to the unmanned ship.

5. The sliding mode control method for tracking a motorship formation based on an improved guidance algorithm according to claim 1, characterized in that: The mathematical model of the six-degree-of-freedom UAV motion is established as follows: Where: x i represents the horizontal coordinate of the position of the i-th UAV; y i represents the vertical coordinate of the position of the i-th UAV; z i represents the vertical coordinate of the i-th UAV; φ i represents the rolling angle of the i-th UAV; θ i represents the actual pitch angle of the i-th UAV; ψ i represents the yaw angle of the i-th UAV; U 1i represents the vertical lift of the i-th UAV; U 2i represents the rolling moment of the i-th UAV; U 3i represents the pitch moment of the i-th UAV; U 4i represents the yaw moment of the i-th UAV; m represents the mass of the drone; represents the vertical lift of the ith UAV; g represents the acceleration due to gravity; Both represent the influence coefficient of air resistance on the position coordinates of the i-th UAV; I x Represents the moment of inertia of the drone on the x-axis; I y Represents the moment of inertia of the drone on the y-axis; I z Represents the moment of inertia of the drone on the z-axis; J r represents the rotor inertia of the drone; γ represents the algebraic sum of the motor speeds; All represent the influence coefficient of the gyro effect on the Euler angle of the i-th UAV; l represents the wheelbase of the quadrotor UAV; i represents the number of the UAV; represents the second-order differential; f θi , f ψi They respectively represent the disturbance of the pitch angle channel, the roll angle channel and the yaw angle channel of the i-th UAV.

6. The sliding mode control method for tracking a motorship formation based on an improved guidance algorithm according to claim 1, characterized in that: In S2, the method for obtaining the desired heading angle of the unmanned ship is as follows: S21: With the position of the unmanned ship as the center, a circle for solving the guidance point is established to obtain the coordinates of the guidance point, and then the sight angle of the guidance algorithm can be obtained; Among them, the method used to obtain the sight angle of the guidance algorithm is as follows: First, obtain the coordinates of the guidance point. The formula used is as follows: Where: x los Indicates the horizontal coordinate of the guidance point; y los Indicates the ordinate of the guidance point; x L Indicates the horizontal coordinate of the position of the unmanned ship; y L represents the vertical coordinate of the position of the unmanned ship; R represents the radius of the circle; θ k Indicates the angle between the straight line segment path and the true north direction; in, Where: R represents the radius of the circle; k los1 Represents the weight coefficient of the error term in the circle radius; y Le represents the forward distance along the moving direction of the unmanned ship; k los2 It represents the weight coefficient of the ship length in the circle radius; L represents the length of the unmanned ship; A, B, and C all represent the coefficients of the straight line equation written by the straight line segment path; Therefore, the sight angle of the guidance algorithm is obtained as follows: Where: los represents the sight angle obtained by the guidance algorithm; S22: Obtaining a desired heading angle of the ship according to the sight angle obtained by the guidance algorithm; The acceptance circle for the ship to achieve steering at the steering point is established as follows: Where: Ld represents the expected heading angle of the unmanned ship; β represents the drift angle of the unmanned ship during navigation.

7. The sliding mode control method for tracking a motorship formation based on an improved guidance algorithm according to claim 6 is characterized in that: An acceptance circle is established for the ship to achieve turning at the next turning point, so as to satisfy at the position of the unmanned ship: When , S21-S22 are repeatedly executed to update the expected heading angle of the ship; Where: x w+1 Indicates the horizontal coordinate of the next turning point; y w+1 represents the ordinate of the next turning point; RM represents the radius of the designed acceptance circle; σ represents the complementary angle between the current tracked path and the next tracked path; k los3 , k los4 Both represent the coefficient of the design acceptance circle radius; L represents the length of the unmanned ship.

8. The sliding mode control method for tracking a motor-vessel formation based on an improved guidance algorithm according to claim 1, characterized in that: In S3, the rudder angle input of the unmanned ship is obtained as follows: Define the error between the heading angle of the unmanned ship and the expected heading angle: ψ Le =ψ Ld -ψ L Where: L represents the heading angle of the unmanned ship; ψ Le represents the error between the actual heading angle of the unmanned ship and the expected heading angle; ψ Ld represents the desired heading angle of the unmanned ship; The nonlinear sliding surface of the unmanned ship is established, and its expression is: in, According to the three-degree-of-freedom unmanned ship motion mathematical model, the rudder angle input of the unmanned ship is obtained as follows: Where: s represents the nonlinear sliding surface of the designed unmanned ship; represents the first-order differential of the nonlinear sliding surface of the designed unmanned ship; ψ L Indicates the actual heading angle of the unmanned ship; They represent the first-order and second-order differentials of the error between the actual heading angle of the unmanned ship and the expected heading angle; k usv1 , k usv2 , k usv3 All represent the design parameters of the nonlinear sliding surface of the unmanned ship; δ represents the rudder angle input of the unmanned ship; b Lr Represents the unmanned ship model parameters; Represents the second-order differential of the desired heading angle of the unmanned ship; represents the observed value of the bow angular velocity; T L Indicates the turning index of the unmanned ship; represents the observed value of the total disturbance.

9. The sliding mode control method for tracking a motorship formation based on an improved guidance algorithm according to claim 1, characterized in that: The observation values ​​of the unmanned ship and the observation values ​​of the unmanned aerial vehicle are both obtained through an extended state observer; The extended state observer is established as follows: Where: Represents the observed value of the unmanned ship's heading angle; represents the observed value of the unmanned ship's bow angular velocity; represents the observed value of the total disturbance; l L1 , l L2 , l L3 Both represent the design parameters of the observer of the unmanned ship; ψ L Indicates the actual heading angle of the unmanned ship; b L represents the actual gain of the steering gear servo system; δ represents the rudder angle input of the unmanned ship; represents the first-order differential of the roll angle of the i-th UAV; T L represents the turning index of the unmanned ship; a represents the coefficient of the high-order nonlinear term of the model; represents the observed value of the first-order differential component of the roll angle of the i-th UAV; represents the observed value of the roll angle of the i-th UAV; represents the first-order differential of the roll angle observation value of the i-th UAV; l φ1 , l φ2 , l φ3 Both represent the design parameters of the observer of the UAV roll angle; φ i represents the rolling angle of the i-th UAV; θ i represents the actual pitch angle of the i-th UAV; Represents the first-order differential of the pitch angle of the i-th UAV; represents the observed value of the pitch angle of the i-th UAV; represents the observed value of the first-order differential component of the pitch angle of the i-th UAV; ψi represents the yaw angle of the i-th UAV; represents the first-order differential of the yaw angle of the i-th UAV; represents the first-order differential observation value of the i-th UAV; γ represents the algebraic sum of the motor speed; Both represent the influence coefficient of gyroscopic effect on the Euler angle of the i-th UAV; represents the disturbance observation value of the roll angle channel of the i-th UAV; l θ1 , l θ2 , l θ3 All represent the design parameters of the observer of the UAV pitch angle; They represent the pitch angle observation value of the i-th UAV, the first-order differential observation value of the pitch angle, and the first-order differential observation value of the pitch angle channel disturbance observation value respectively; represents the disturbance observation value of the pitch angle channel of the i-th UAV; They represent the first-order differentials of the unmanned ship’s heading angle observation value, the bow angular velocity observation value, and the disturbance observation value respectively; They represent the yaw angle observation value of the i-th UAV, the first-order differential observation value of the yaw angle, and the first-order differential observation value of the yaw angle channel disturbance observation value respectively; represents the observed value of the yaw angle of the i-th UAV; represents the observed value of the first-order differential component of the yaw angle of the i-th UAV; represents the disturbance observation value of the yaw angle channel of the i-th UAV; lψ1, lψ2, lψ3 represent the design parameters of the observer of the UAV yaw angle; Among them, the design parameters of the unmanned ship observer, the design parameters of the UAV roll angle observer, the design parameters of the UAV pitch angle observer, and the design parameters of the UAV yaw angle observer are all obtained through the predicted value expression of the state quantity and the general formula of the state observer; The predicted value expression of the state quantity and the general expression of the state observer are: in, Using the prediction error of the state variables, the evaluation function of the genetic algorithm is established, and its expression is: Where: x1(k)=[ψ L φ i θ i ψ i ] T is the ship heading angle ψ L , UAV roll angle φ i , heel angle θ i and the yaw angle ψ i The vector matrix of is the differential of the vector matrix x1(k); F(k)=[f L f φi f θi f ψi ] T is the external disturbance of the ship L , the disturbance f in the rolling angle direction of the UAV φi , the disturbance in the heel angle direction f θi and the disturbance in the yaw angle direction f ψi The vector matrix of represents the predicted observation value of x1(k) at time k+1; T p represents the prediction step length; represents the predicted observation value of x2(k) at time k+1; represents the predicted observation value of f(k) at k+1; represents the estimated value of x1(k) at time k; represents the estimated value of x2(k) at time k; represents the estimated value of f(k) at time k; b represents the design parameters of the observer; u represents the control input of the system; g(x) represents the known structure of the system; l i1 represents the first parameter of the observer; l i2 Represents the second parameter of the observer; l i3 represents the third parameter of the observer; represents the predicted value of the state error of x1(k) at time k+1; x1(k+1) represents the state of x1(k) at time k+1; n p represents the prediction interval; J* represents the evaluation function of the genetic algorithm; represents the prediction error of x1(k); j represents the number of backward prediction steps; k represents the starting time of the prediction; a p , b p Respectively represent l ip The upper and lower bounds of the value interval; l ip represents the design parameters of the observer.

Images